2021-06-25

matlab七轴可伸缩关节的机械臂避障抓取物品

  • 摘要
    • 连环图展示
    • 机械臂介绍
    • matlab机械臂建模
    • 绘制机械臂
    • 绘制机器人子程序
    • RRT避障提取小球算法子程序
    • 绘制水平圆柱子程序
    • 创建五次多项式规划子程序
    • 4-3-4轨迹规划
    • 逆运动学数值解
    • 雅各比矩阵子程序
    • 齐次变换矩阵求解
    • 绘制RRT避障提取小球时机械臂指标子程序
    • 机械臂工作空间分析
    • 保持剩余小球位置和大小子程序
    • 主函数
    • 从目标点至下一小球轨迹
  • 总结

摘要

设计了一款七自由度的机械臂,其中伸缩关节为第七关节,在指定范围内,随机生成10个规定范围内随机尺寸,随机间隔小球,利用RRT避障算法,使机械臂可以无碰撞的提取小球。最后使用多项式轨迹规划运送小球至目标点。抓取小球的顺序按照小球体积大小,从大到小依次抓取。

连环图展示

未运行状态
提取1号小球
运送1号小球
提取2号小球
运送2号小球

机械臂介绍

此处仿真测试是在高4000mm,底面半径1000mm的空心圆柱体内测试,因此各有关距离、位置等参数的单位均以毫米为单位。机械臂运动最大速度为600mm/s,最大加速度为300mm/,旋转关节最大速度180°/s,最大加速度180°/s,末端伸缩关节量为15mm~170mm。机械臂各轴工作范围角度如表Ⅰ所示,共由七个关节,第七关节的旋转范围为-360°至360°并且可伸缩。

关节工作范围
关节1180°~180°
关节2180°~180°
关节3-245°~65°
关节4-200°~200°
关节5-115°~115°
关节6-180°~180°
关节7-360°~360° (可伸缩关节)

机械臂构型下图所示
机械臂构型

matlab机械臂建模

正运动学使用DH法建模,并绘制坐标系和DH参数表,对于本设计的七轴机械臂,采取如下图所示坐标系。Xi,Zi的下标i表示第i个关节轴的坐标系。
DH坐标系
建立DH参数表

θidzdaαi
T1pi/23500pi/2
T2004000
T3004000
T4004000
T5004000
T6pi/200-pi/2
T70L(15~170mm)00

DH参数表输入matlab中,第一个子程序。

% %Build Robot by D_H methods
%建立DH参数表
global D5
ToDeg = 180/pi;%转为角度制
ToRad = pi/180;%转为弧度制
UX = [1 0 0]';
UY = [0 1 0]';
UZ = [0 0 1]';
%J1为关节名称,
%th为绕z轴旋转使x轴平行的角度
%dz为沿z轴平移使x轴共线的移动距离
%dx为沿x轴平移使两个参考坐标系原点重合的移动距离
%alf为绕x轴旋转使z轴重合的角度
Link= struct('name','Body' , 'th',  0, 'dz', 0, 'dx', 0, 'alf',0*ToRad,'az',UZ);     % 结构体数组
Link(1)= struct('name','Base' , 'th',  0*ToRad, 'dz', -450, 'dx', -500, 'alf',0*ToRad,'az',UZ); %Base To 1,世界坐标系
Link(2) = struct('name','J1' , 'th',  90*ToRad, 'dz', 350, 'dx', 0, 'alf',90*ToRad,'az',UZ);       %1 TO 2
Link(3) = struct('name','J2' , 'th',  0*ToRad, 'dz', 0, 'dx', 400, 'alf',0*ToRad,'az',UZ);    %2 TO 3
Link(4) = struct('name','J3' , 'th',  0*ToRad, 'dz', 0, 'dx', 400, 'alf',0*ToRad,'az',UZ);     %3 TO 4
Link(5) = struct('name','J4' , 'th',  0*ToRad, 'dz', 0, 'dx', 400, 'alf',0*ToRad,'az',UZ);      %4 TO 5
Link(6) = struct('name','J5' , 'th',  0*ToRad, 'dz', 0, 'dx',400, 'alf',0*ToRad,'az',UZ);     %5 TO 6
Link(7) = struct('name','J6' , 'th',  90*ToRad, 'dz', 0, 'dx', 0, 'alf',-90*ToRad,'az',UZ);     %6 TO 7
Link(8) = struct('name','J7' , 'th',  0*ToRad, 'dz', D5, 'dx', 0, 'alf',0*ToRad,'az',UZ);     %7 TO E

绘制机械臂

画关节连杆子程序

function Connect3D(p1,p2,option,pt)
h = plot3([p1(1) p2(1)],[p1(2) p2(2)],[p1(3) p2(3)],option);
set(h,'LineWidth',pt)

画关节轴子程序

function h = DrawCylinder(pos, az, radius,len, col)
%radius表示圆柱半径,len表示圆柱高,az表示朝向,pos表示位置,col表示颜色
%调用示例:shui_ping_yuan_zhu([0,0,0]',[0,1,0]',1000,4000,0);%中心点在(0,0,0),侧面与Y轴平行。
% draw closed cylinder
%
%******** rotation matrix
az0 = [0;0;1];
ax  = cross(az0,az);
ax_n = norm(ax);
if ax_n < eps 
	rot = eye(3);
else
    ax = ax/ax_n;
    ay = cross(az,ax);
    ay = ay/norm(ay);
    rot = [ax ay az];
end
%********** make cylinder
% col = [0 0.5 0];  % cylinder color这里调整颜色
a = 40;    % number of side faces这里调整关节轴圆柱形状由多少曲面围成
theta = (0:a)/a * 2*pi;
x = [radius; radius]* cos(theta);
y = [radius; radius] * sin(theta);
z = [len/2; -len/2] * ones(1,a+1);
cc = col*ones(size(x));
for n=1:size(x,1)
   xyz = [x(n,:);y(n,:);z(n,:)];
   xyz2 = rot * xyz;
   x2(n,:) = xyz2(1,:);
   y2(n,:) = xyz2(2,:);
   z2(n,:) = xyz2(3,:);
end
%************* draw
% side faces
h = surf(x2+pos(1),y2+pos(2),z2+pos(3),cc);
for n=1:2
	patch(x2(n,:)+pos(1),y2(n,:)+pos(2),z2(n,:)+pos(3),cc(n,:));
end	

建立DH齐次变换矩阵子程序

function Matrix_DH_Ln(i) 
% Caculate the D-H Matrix
%根据DH参数生成各关节间的齐次变换矩阵
global Link

ToDeg = 180/pi;
ToRad = pi/180;

C=cos(Link(i).th);
S=sin(Link(i).th);
Ca=cos(Link(i).alf);
Sa=sin(Link(i).alf);
a=Link(i).dx;    %distance between zi and zi-1
d=Link(i).dz;    %distance between xi and xi-1

Link(i).n=[C,S,0,0]';
Link(i).o=[-1*S*Ca,C*Ca,Sa,0]';
Link(i).a=[S*Sa, -1*C*Sa,Ca,0]';
Link(i).p=[a*C,a*S,d,1]';

Link(i).R=[Link(i).n(1:3),Link(i).o(1:3),Link(i).a(1:3)];
Link(i).A=[Link(i).n,Link(i).o,Link(i).a,Link(i).p];

绘制机器人子程序

第一种写法

function pic=DHfk7Dof_Lnya(th1,th2,th3,th4,th5,th6,th7,fcla)
%%%绘制机器人
% close all

global Link
Build_7DOFRobot_Lnya;%建立DH参数表
%画图配置参数
radius    = 30;%半径
len       = 90;%高
joint_col = 0;%颜色
% plot3(0,0,0,'ro'); 
%输入角度转化为弧度
 Link(2).th=th1*pi/180;
 Link(3).th=th2*pi/180;
 Link(4).th=th3*pi/180;
 
 Link(5).th=th4*pi/180;
 Link(6).th=th5*pi/180;
 Link(7).th=th6*pi/180;
 Link(8).dz=th7;%末端滑动关节
 %%

%根据DH参数表及关节变量构造各个关节的齐次变换矩阵
for i=1:8
Matrix_DH_Ln(i);
end
%绘制表示各个关节的圆柱体及关节连杆
for i=2:8
      %计算第i个关节的位姿:根据上一关节的位姿计算下一关节的位姿
      Link(i).A=Link(i-1).A*Link(i).A;
      Link(i).p= Link(i).A(:,4);
      Link(i).n= Link(i).A(:,1);
      Link(i).o= Link(i).A(:,2);
      Link(i).a= Link(i).A(:,3);
      Link(i).R=[Link(i).n(1:3),Link(i).o(1:3),Link(i).a(1:3)];
      %绘制第i-1个关节
      Connect3D(Link(i-1).p,Link(i).p,'b',2); hold on;
%       plot3(Link(i).p(1),Link(i).p(2),Link(i).p(3),'rx');hold on;
      DrawCylinder(Link(i-1).p, Link(i-1).R * Link(i).az, radius,len, joint_col); hold on;
end

    

grid on;
% view(134,12);

axis([-2500,2500,-2500,2500,-2500,2500]);


xlabel('x');
ylabel('y');
zlabel('z');
drawnow;
pic=getframe;%捕获坐标区或者图窗做影片帧
if(fcla)
    cla;
end

第二种写法

function A=DHfk7Dof_kuka_nodraw(q)
global Link
Build_7DOFRobot_Lnya;%建立DH参数表

%input angle
Link(2).th = q(1)*pi/180;
Link(3).th = q(2)*pi/180;
Link(4).th = q(3)*pi/180;
Link(5).th = q(4)*pi/180;
Link(6).th = q(5)*pi/180;
Link(7).th = q(6)*pi/180;

Link(8).dz = q(7);
%DH matrix
for i=1:8
    Matrix_DH_Ln(i);
end

%position and orientation
for i=2:8
	Link(i).A=Link(i-1).A*Link(i).A;
    Link(i).p= Link(i).A(:,4);
    Link(i).n= Link(i).A(:,1);
    Link(i).o= Link(i).A(:,2);
    Link(i).a= Link(i).A(:,3);
    Link(i).R=[Link(i).n(1:3),Link(i).o(1:3),Link(i).a(1:3)];
end

A=Link(8).A;%4*4 的矩阵

RRT避障提取小球算法子程序

首先在主程序中生成了小球,小球所有的信息,包括坐标、半径、体积等全部存入ss矩阵,ss矩阵作为参数传入该子程序后,目标点为该小球当前x坐标的负方向,即把小球向x轴负方向提取,使之脱离球群。因此搜索方向上也是向着x轴负方向。这里的代码逻辑是RRT搜素出避障路径,然后把经过的一些点存入矩阵path中,我们设法获得矩阵中的这些点,并调用求逆解程序,对机械臂求逆解,然后绘制机械臂,不断的绘制,擦除,绘制出,擦除就形成了动画。已抓取的小球的坐标也要随着这些点发生变化,就形成了小球机械臂跟着走。另外避障的起点,要设置在小球表面,即每个小球避障起点坐标到小球中心点坐标的距离要大于半径。

% clc;
% clear;
% close all;
function rrt_SanWei(ss,index,xun_huan,ban_jing)
%ss是主函数中的存储小球信息的矩阵,index将小球体积从大到小排列后的矩阵
shui_ping_yuan_zhu([0,0,0]',[0,1,0]',1000,4000,0);%绘制水平侧卧的圆柱
hold on
circleCenter = [ss(1,1),ss(1,2),ss(1,3);ss(2,1),ss(2,2),ss(2,3);ss(3,1),ss(3,2),ss(3,3);ss(4,1),ss(4,2),ss(4,3);ss(5,1),ss(5,2),ss(5,3);ss(6,1),ss(6,2),ss(6,3);ss(7,1),ss(7,2),ss(7,3);ss(8,1),ss(8,2),ss(8,3);ss(9,1),ss(9,2),ss(9,3);ss(10,1),ss(10,2),ss(10,3)];
%将10个小球的x,y,z坐标输入
r=[ss(1,5);ss(2,5);ss(3,5);ss(4,5);ss(5,5);ss(6,5);ss(7,5);ss(8,5);ss(9,5);ss(10,5)];
%输入10个小球的半径
for ii=1:1:10
[xx,yy,zz]= ellipsoid(ss(ii,1),ss(ii,2),ss(ii,3),ss(ii,5),ss(ii,5),ss(ii,5));%绘制椭圆函数,半径一样时就变成了圆
surf(xx,yy,zz); %半径为j的圆
% xu_hao = num2str(xun_huan);%数值转字符串,准备循环从1到10,10个数
% text(x(num),y(num),z(num),xu_hao,'Color','red','FontSize',14);%标记序号,原来ss矩阵中第m行第1列做横坐标,
%  %ss矩阵中第m行第2列做纵坐标然后标记序号。
hold on
end

%% 起点source的坐标,目标点goal的坐标
source=[ss(index(1,xun_huan),1)-ss(index(1,xun_huan),5)-2 ss(index(1,xun_huan),2) ss(index(1,xun_huan),3)];
goal=[0 ss(index(1,xun_huan),2) ss(index(1,xun_huan),3)];

stepsize = 40;%RRT每一步的步长
threshold = 20;%界
maxFailedAttempts = 1000;%最大生长次数
display = true;
searchSize =[ -1550 100 100];      %给定生长方向,x轴负方向-1550,y和z方向最大可搜索100个单位。



% scatter3(source(1),source(2),source(3),'filled','g');
% scatter3(goal(1),goal(2),goal(3),'filled','b');%绘制生长轨迹
% hold on
tic;  % tic-toc: 记录当前运行时间
RRTree = double([source -1]);
failedAttempts = 0;
pathFound = false;


while failedAttempts <= maxFailedAttempts  % loop to grow RRTs
    %% chooses a random configuration
    if rand <0.6    %(50%随机点,50%为目标点) rand是随机产生一个0到1内的均匀随机分布数
        sample = rand(1,3).* searchSize;   % random sample rand(1,3)产生1*3的矩阵, .*是将矩阵对应位置相乘
    else
        sample = goal; % sample taken as goal to bias tree generation to goal
    end
    %% selects the node in the RRT tree that is closest to qrand

[A, I] = min( distanceCost(RRTree(:,1:3),sample) ,[],1); % find the minimum value of each column

    closestNode = RRTree(I(1),1:3);
    %% moving from qnearest an incremental distance in the direction of qrand
    movingVec = [sample(1)-closestNode(1),sample(2)-closestNode(2),sample(3)-closestNode(3)];
    movingVec = movingVec/sqrt(sum(movingVec.^2));  
    newPoint = closestNode + stepsize * movingVec;
    if ~checkPath3(closestNode, newPoint, circleCenter,r) % if extension of closest node in tree to the new point is feasible
        failedAttempts = failedAttempts + 1;
        continue;
    end
    
    if distanceCost(newPoint,goal) < threshold, pathFound = true; break; end % goal reached
    [A, I2] = min( distanceCost(RRTree(:,1:3),newPoint) ,[],1); % check if new node is not already pre-existing in the tree
    if distanceCost(newPoint,RRTree(I2(1),1:3)) < threshold, failedAttempts = failedAttempts + 1; continue; end 
    
    RRTree = [RRTree; newPoint I(1)]; % add node
    failedAttempts = 0;
%     if display, plot3([closestNode(1);newPoint(1)],[closestNode(2);newPoint(2)],[closestNode(3);newPoint(3)],'LineWidth',1); end
%绘制生长成功后的路径轨迹
%     pause(0.05);
end


%% retrieve path from parent information
shui_ping_yuan_zhu([0,0,0]',[0,1,0]',1000,4000,0);
hold on
path = goal;
prev = I(1);
while prev > 0
    path = [RRTree(prev,1:3); path];
    prev = RRTree(prev,4);
end

pathLength = 0;
for i=1:length(path(:,1))-1, pathLength = pathLength + distanceCost(path(i,1:3),path(i+1,1:3)); end % calculate path length
fprintf('processing time=%d \nPath Length=%d \n\n', toc, pathLength); 

hold on;

hang_shu=size(path,1);%RRT算法避障的路径经过的每一个点组成了矩阵path,那么此处获得点的个数
%获取机器人末端位置
num=1;
Angel=[];
for hang=1:1:hang_shu
figure(1)
x(num)=path(hang,1);%path矩阵中每一行的三个数代表一个点的坐标xyz
y(num)=path(hang,2);
z(num)=path(hang,3);
W=[1,0,0,x(num);
       0,1,0,y(num);
       0,0,1,z(num);
       0,0,0,1];
theta2=IK_7DOF_num_solu(W);%theta2为角度制,运动学逆解使用数值解,得出该点的七轴角度
shui_ping_yuan_zhu([0,0,0]',[0,1,0]',1000,4000,0);
hold on
[xx,yy,zz]= ellipsoid(x(num),y(num),z(num),ban_jing,ban_jing,ban_jing);
surf(xx,yy,zz) %半径为j的圆
DHfk7Dof_Lnya(theta2(1),theta2(2),theta2(3),theta2(4),theta2(5),theta2(6),theta2(7),0);%根据求得的逆解绘制机械臂,最后一个参数为0,显示机械臂
[xx,yy,zz]= ellipsoid(x(num),y(num),z(num),ban_jing,ban_jing,ban_jing);
surf(xx,yy,zz) %半径为j的圆
shui_ping_yuan_zhu([0,0,0]',[0,1,0]',1000,4000,0);
DHfk7Dof_Lnya(theta2(1),theta2(2),theta2(3),theta2(4),theta2(5),theta2(6),theta2(7),1);%最后一个参数为1时,擦除机械臂

shui_ping_yuan_zhu([0,0,0]',[0,1,0]',1000,4000,0);
xiao_qiu_zhui_zong(ss,xun_huan,index);
hold on
% plot3(x(num),y[xx,yy,zz]= ellipsoid(x(num),y(num),z(num),ban_jing,ban_jing,ban_jing);
% yan_se=ones(size(zz,1));
% surf(xx,yy,zz); %半径为j的圆
xu_hao = num2str(xun_huan);%数值转字符串,准备循环从1到10,10个数
text(x(num),y(num),z(num),xu_hao,'Color','red','FontSize',14);%标记序号,原来ss矩阵中第m行第1列做横坐标,
 %ss矩阵中第m行第2列做纵坐标然后标记序号。(num),z(num),'ko');%x,y,z为数组
 view(64,20)
q1(num)=theta2(1);
q2(num)=theta2(2);
q3(num)=theta2(3);
q4(num)=theta2(4);
q5(num)=theta2(5);
q6(num)=theta2(6);
q7(num)=theta2(7);
%将每个点求得的七个关节角存入Angel矩阵中
Angel=[Angel;q1(num),q2(num),q3(num),q4(num),q5(num),q6(num),q7(num)]%画关节的速度,加速度等
num=num+1;
hold on
end
%RRT_zhi_biao(Angel);%绘制RRT避障关节轴指标
end

下边是RRT避障检测的三个子程序,被调用在上边代码中。

%% checkPath3.m	
function feasible=checkPath3(n,newPos,circleCenter,r)
feasible=true;
movingVec = [newPos(1)-n(1),newPos(2)-n(2),newPos(3)-n(3)];
movingVec = movingVec/sqrt(sum(movingVec.^2)); %单位化
for R=0:0.5:sqrt(sum((n-newPos).^2))
    posCheck=n + R .* movingVec;
    if ~(feasiblePoint3(ceil(posCheck),circleCenter,r) && feasiblePoint3(floor(posCheck),circleCenter,r))
        feasible=false;break;
    end
end
if ~feasiblePoint3(newPos,circleCenter,r), feasible=false; end
end

%% feasiblePoint3.m
function feasible=feasiblePoint3(point,circleCenter,r)
feasible=true;
% check if collission-free spot and inside maps
for row = 1:length(circleCenter(:,1))
    if sqrt(sum((circleCenter(row,:)-point).^2)) <= r(row)
        feasible = false;break;
    end
end
end

function h=distanceCost(a,b)         %% distanceCost.m
	h = sqrt((a(:,1)-b(:,1)).^2 + (a(:,2)-b(:,2)).^2+ (a(:,3)-b(:,3)).^2 );
end

绘制水平圆柱子程序

function shui_ping_yuan_zhu(pos,chao_xiang,banjing,gaodu,yanse)
DrawCylinder(pos,chao_xiang,banjing,gaodu,yanse)
alpha(0.1)%透明度为0.1   0表示完全透明,1表示不透明
hold on %保持当前图画
end

创建五次多项式规划子程序

五次多项式逻辑子程序,依据五次多项式轨迹规划的的原理编写代码

function q=Creat_5_curve(q0,q1,v0,v1,acc0,acc1)%五次多项式规划
% clc
% clear
%轨迹定义条件
%时间
t0=0;
t1=2;
%位置和速度(a)
% q0=0;
% q1=10;
% v0=0;
% v1=0;
% acc0=0;%加速度
% acc1=0;
%利用公式(1-25)源自网站古月居,求系数公式
h=q1-q0;
T=t1-t0;
a0=q0;
a1=v0;
a2=1.0/2*acc0;
a3=1.0/(2*T*T*T)*(20*h-(8*v1+12*v0)*T+(acc1-3*acc0)*(T*T));
a4=1.0/(2*T*T*T*T)*(-30*h+(14*v1+16*v0)*T+(3*acc0-2*acc1)*(T*T));
a5=1.0/(2*T*T*T*T*T)*(12*h-6*(v1+v0)*T+(acc1-acc0)*(T*T));
%轨迹生成
t=t0:0.1:t1;%画图中轨迹生成间隔
%位置
q=a0+a1*power((t-t0),1)+a2*power((t-t0),2)+a3*power((t-t0),3)+...
    a4*power(t-t0,4)+a5*power(t-t0,5);
%速度
v=a1+2*a2*power((t-t0),1)+3*a3*power((t-t0),2)+4*a4*power(t-t0,3)+...
    5*a5*power(t-t0,4);
%加速度
acc=2*a2+6*a3*power((t-t0),1)+12*a4*power(t-t0,2)+20*a5*power(t-t0,3);
%绘图
% subplot(3,2,1)
% plot(t,q,'r');
% % axis([0,8,0,11])
% ylabel('position')%绘制位置
% grid on
% subplot(3,2,2)
% plot(t,v,'b');
% % axis([0,8,-1,2.5])
% ylabel('velocity')%绘制速度
% grid on
% subplot(3,2,3)
% plot(t,acc,'g');
% ylabel('acceleration')%绘制加速度时间图像
% grid on

调用五次多项式轨迹规划进行机械臂绘图

function Draw_line_5(z0,y0,y1,flag,ban_jing,ss,index,xun_huan,x0,x1)%末端直角规划
global p_x p_y p_z
global q_1 q_2 q_3 q_4 q_5 q_6 q_7
i=4;%调节采样数量来改变绘制时间

Posion1=Creat_5_curve(0,y1-y0,0,0,0,0);%然后进行y方向移动
Posion2=Creat_5_curve(0,x1-x0,0,0,0,0);%先进行x方向移动
n=length(Posion1);%设置A到B点的采集数据个数
n1=length(p_x);

x(1)=0;y(1)=y0;z(1)=z0;

num=2;
for t=1:i:n%采集数据个数为n/i
%     x(num)=0;p_x(n1+num)=0;
    z(num)=z0;p_z(n1+num)=z0;
    if(flag==1) %执行y轴方向直线运动
%         z(num)=z(num-1)+Posion1(t);
      y(num)=y0+Posion1(t);
    else if(flag==0)
        y(num)=y0-Posion1(t); 
       
    else if(flag==3)%执行x轴方向上的直线运动
            x(num)=x0+Posion2(t);
        end
        end
    end
    if(flag==1)
    p_y(n1+num)=y(num);
    x(num)=0;p_x(n1+num)=0;
    
    else if(flag==3)
   p_x(n1+num)=x(num);
   y(num)=y0;p_y(n1+num)=y0;
        end
    end
    

    W=[1,0,0,x(num);
       0,1,0,y(num);
       0,0,1,z(num);
       0,0,0,1];
%%%数值解计算逆解

% plot3(p_x,p_y,p_z,'ko');hold on;%x,y,z为数组,绘制机械臂运动时候经过点的轨迹
theta2=IK_7DOF_num_solu(W);%theta2为角度制,逆解数值解
shui_ping_yuan_zhu([0,0,0]',[0,1,0]',1000,4000,0);
if(t>n-i)
    DHfk7Dof_Lnya(theta2(1),theta2(2),theta2(3),theta2(4),theta2(5),theta2(6),theta2(7),0);
    [xx,yy,zz]= ellipsoid(x(num),y(num),z(num),ban_jing,ban_jing,ban_jing);
% yan_se=ones(size(zz,1));
surf(xx,yy,zz); %半径为j的圆
xu_hao = num2str(xun_huan);%数值转字符串,准备循环从1到10,10个数
text(x(num),y(num),z(num),xu_hao,'Color','red','FontSize',14);%标记序号,原来ss矩阵中第m行第1列做横坐标,
 %ss矩阵中第m行第2列做纵坐标然后标记序号。
else
[xx,yy,zz]= ellipsoid(x(num),y(num),z(num),ban_jing,ban_jing,ban_jing);
% yan_se=ones(size(zz,1));
surf(xx,yy,zz); %半径为j的圆
xu_hao = num2str(xun_huan);%数值转字符串,准备循环从1到10,10个数
text(x(num),y(num),z(num),xu_hao,'Color','red','FontSize',14);%标记序号,原来ss矩阵中第m行第1列做横坐标,
 %ss矩阵中第m行第2列做纵坐标然后标记序号。
DHfk7Dof_Lnya(theta2(1),theta2(2),theta2(3),theta2(4),theta2(5),theta2(6),theta2(7),1);%清除当前机械臂绘图
end
hold on
shui_ping_yuan_zhu([0,0,0]',[0,1,0]',1000,4000,0);
hold on
xiao_qiu_zhui_zong(ss,xun_huan,index);
% shading interp%防止surf画出黑色小球
q_1(num)=theta2(1);
q_2(num)=theta2(2);
q_3(num)=theta2(3);
q_4(num)=theta2(4);
q_5(num)=theta2(5);
q_6(num)=theta2(6);
q_7(num)=theta2(7);
num=num+1;
end

% t=1:1:num-1;
% figure(flag+1),
% plot(t,q_1,'r','LineWidth',2);hold on;%绘制角度位置
% plot(t,q_2,'b','LineWidth',2);hold on;%绘制角度位置
% plot(t,q_3,'g','LineWidth',2);hold on;%绘制角度位置
% plot(t,q_4,'k','LineWidth',2);hold on;%绘制角度位置
% plot(t,q_5,'c','LineWidth',2);hold on;%绘制角度位置
% plot(t,q_6,'m','LineWidth',2);hold on;%绘制角度位置
% plot(t,q_7,'y','LineWidth',2);grid on;%绘制角度位置
% axis([1,num-1,-400,400]);%-180 180
end

4-3-4轨迹规划

依据4-3-4多项式原理,编写程序

function Q=Creat_434_curve(Th1,Th2,Th3,Th4,Th1_1,Th1_2,Th4_1,Th4_2,num,j)
%四个角度,起始点速度、加速度,终端点速度、加速度
%三次多项式
ToDeg = 180/pi;
ToRad = pi/180;%转化为弧度制
% %定义初始角度
% Th1=-10.7153;%30
% Th1_1=0;
% Th1_2=0;
% 
% Th2=-20;%50
% Th3=10;%90
% Th4=30.938;%70
% Th4_1=0;
% Th4_2=0;
%定义初始本地起始时间
Tao1_i=0;
Tao2_i=0;
Tao3_i=0;
%定义初始本地终端时间
Tao1_f=4;
Tao2_f=8;%之前的事件为2,4,2
Tao3_f=4;
%定义全局起始、终止时间
t_i=0;
t_f= Tao1_f + Tao2_f + Tao3_f - (Tao1_i + Tao2_i + Tao3_i);
TH=[Th1 Th1_1 Th1_2 Th2 Th2 0 0 Th3 Th3 0 0 Th4 Th4_1 Th4_2]';
%C=[a0 a1 a2 a3 a4 b0 b1 b2 b3 c0 c1 c2 c3 c4]';
M1=[1 0 0 0 0 0 0 0 0 0 0 0 0 0];
M2=[0 1 0 0 0 0 0 0 0 0 0 0 0 0];
M3=[0 0 2 0 0 0 0 0 0 0 0 0 0 0];
M4=[1 Tao1_f Tao1_f^2 Tao1_f^3 Tao1_f^4 0 0 0 0 0 0 0 0 0];
M5=[0 0 0 0 0 1 0 0 0 0 0 0 0 0];
M6=[0 1 2*Tao1_f 3*Tao1_f^2 4*Tao1_f^3 0 -1 0 0 0 0 0 0 0];
M7=[0 0 2 6*Tao1_f 12*Tao1_f^2 0 0 -2 0 0 0 0 0 0];
M8=[0 0 0 0 0 1 Tao2_f Tao2_f^2 Tao2_f^3 0 0 0 0 0];
M9=[0 0 0 0 0 0 0 0 0 1 0 0 0 0];
M10=[0 0 0 0 0 0 1 2*Tao2_f 3*Tao2_f^2 0 -1 0 0 0];
M11=[0 0 0 0 0 0 0 2 6*Tao2_f 0 0 -2 0 0];
M12=[0 0 0 0 0 0 0 0 0 1 Tao3_f Tao3_f^2 Tao3_f^3 Tao3_f^4];
M13=[0 0 0 0 0 0 0 0 0 0 1 2*Tao3_f 3*Tao3_f^2 4*Tao3_f^3];
M14=[0 0 0 0 0 0 0 0 0 0 0 2 6*Tao3_f 12*Tao3_f^2];
M=[M1;M2;M3;M4;M5;M6;M7;M8;M9;M10;M11;M12;M13;M14];
C=inv(M)*TH;
a0=C(1);a1=C(2);a2=C(3);a3=C(4);a4=C(5);
b0=C(6);b1=C(7);b2=C(8);b3=C(9);
c0=C(10);c1=C(11);c2=C(12);c3=C(13);c4=C(14);
%轨迹生成
%轨迹定义条件
% t_array=[t_i,t_i+Tao1_f-Tao1_i,t_i+Tao1_f-Tao1_i+Tao2_f-Tao2_i,t_f];
% q_array=[Th1,Th2,Th3,Th4];%位置
% v_array=[0,-10,10,0];%速度
t1=Tao1_i:0.1:Tao1_f;
t2=Tao2_i+0.1:0.1:Tao2_f-0.1;%要考虑函数重合
t3=Tao3_i:0.1:Tao3_f;
t=t_i:0.1:t_f;%原先是0.1
%位置
q1=a0+a1*power(t1,1)+a2*power(t1,2)+a3*power(t1,3)+a4*power(t1,4);
q2=b0+b1*power(t2,1)+b2*power(t2,2)+b3*power(t2,3);
q3=c0+c1*power(t3,1)+c2*power(t3,2)+c3*power(t3,3)+c4*power(t3,4);
q=[q1 q2 q3];
%速度
v1=a1+2*a2*power(t1,1)+3*a3*power(t1,2)+4*a4*power(t1,3);
v2=b1+2*b2*power(t2,1)+3*b3*power(t2,2);
v3=c1+2*c2*power(t3,1)+3*c3*power(t3,2)+4*c4*power(t3,3);
v=[v1 v2 v3];
%加速度
acc1=2*a2+6*a3*power(t1,1)+12*a4*power(t1,2);
acc2=2*b2+6*b3*power(t2,1);
acc3=2*c2+6*c3*power(t3,1)+12*c4*power(t3,2);
acc=[acc1 acc2 acc3];

Q=q;
V=v;
ACC=acc;
%绘图
% if (j==4)
%     figure(5),
%     plot(t,q,'r','LineWidth',2);%绘制角度位置
%     hold on
% 
%     plot(t,v,'b','LineWidth',2);%绘制速度
%     hold on
% 
%     plot(t,acc,'k','LineWidth',2);%绘制加速度
%     xlabel('t')
% 
%     legend('position','velocity','acceleration') %可依次设置成你想要的名字
%     grid on
% end

调用4-3-4轨迹的原理程序创建绘图子程序

function Draw_curve_434(th_1,th_2,th_3,th_4,th1_1,th1_2,th4_1,th4_2,num1,j,ban_jing,ss,index,xun_huan)%末端直角规划
global p_x p_y p_z d5
i=6;%调节采样数量来改变绘制时间 原先i为6
n=81;%设置A到B点的采集数据个数,原先n为81
n1=length(p_x);
Posion=Creat_434_curve(th_1,th_2,th_3,th_4,th1_1,th1_2,th4_1,th4_2,num1,j);
%%%定义一次函数y=kx+b实现坐标线性转换
k=-9.0;b=100;%这里的b表示y方向初始位置,k越小,往y轴负方向距离越多
num=1;
for t=1:i:n%采集数据个数为n/i
    x(num)=0;
%     p_x(n1+num)=x(num);
     y(num)=k*t+b;
%     p_y(n1+num)=y(num);
     z(num)=Posion(t);
%      p_z(n1+num)=z(num);
    W=[1,0,0,x(num);
       0,1,0,y(num);
       0,0,1,z(num);
       0,0,0,1];
%%%数值解计算逆解
hold on
% plot3(p_x,p_y,p_z,'ko');hold on;%x,y,z为数组,绘制圆点形状轨迹

% plot3(x,y,z,'k*');hold on;%x,y,z为数组
theta2=IK_7DOF_num_solu(W);%theta2为角度制

if(t>n-i)
    DHfk7Dof_Lnya(theta2(1),theta2(2),theta2(3),theta2(4),theta2(5),theta2(6),theta2(7),0);
    [xx,yy,zz]= ellipsoid(x(num),y(num),z(num),ban_jing,ban_jing,ban_jing);
surf(xx,yy,zz) %半径为j的圆
% shading interp%防止surf画出黑色小球
xu_hao = num2str(xun_huan);%数值转字符串,准备循环从1到10,10个数
text(x(num),y(num),z(num),xu_hao,'Color','red','FontSize',14);%标记序号,原来ss矩阵中第m行第1列做横坐标,
else
[xx,yy,zz]= ellipsoid(x(num),y(num),z(num),ban_jing,ban_jing,ban_jing);
surf(xx,yy,zz) %半径为j的圆
% shading interp%防止surf画出黑色小球
xu_hao = num2str(xun_huan);%数值转字符串,准备循环从1到10,10个数
text(x(num),y(num),z(num),xu_hao,'Color','red','FontSize',14);%标记序号,原来ss矩阵中第m行第1列做横坐标,
    DHfk7Dof_Lnya(theta2(1),theta2(2),theta2(3),theta2(4),theta2(5),theta2(6),theta2(7),1);%清除
end


xiao_qiu_zhui_zong(ss,xun_huan,index);

shui_ping_yuan_zhu([0,0,0]',[0,1,0]',1000,4000,0);
hold on
q1(num)=theta2(1);
q2(num)=theta2(2);
q3(num)=theta2(3);
q4(num)=theta2(4);
q5(num)=theta2(5);
q6(num)=theta2(6);
q7(num)=theta2(7);

num=num+1;

end
%%%末端手动控制
for i=1:1:5
    d5=theta2(7)+10*i;
    shui_ping_yuan_zhu([0,0,0]',[0,1,0]',1000,4000,0);
    xiao_qiu_zhui_zong(ss,xun_huan,index);
    hold on
   [xx,yy,zz]= ellipsoid(x(num-1),y(num-1),z(num-1)-10*i,ban_jing,ban_jing,ban_jing);
   surf(xx,yy,zz) %半径为j的圆
   % shading interp%防止surf画出黑色小球
   xiao_qiu_zhui_zong(ss,xun_huan,index);
   hold on
   DHfk7Dof_Lnya(theta2(1),theta2(2),theta2(3),theta2(4),theta2(5),theta2(6),d5,1);
end
[xx,yy,zz]= ellipsoid(x(num-1),y(num-1),z(num-1),ban_jing,ban_jing,ban_jing);
surf(xx,yy,zz) %半径为j的圆
% shading interp%防止surf画出黑色小球
xiao_qiu_zhui_zong(ss,xun_huan,index);
hold on

t=1:1:num-1;
% if(j==1)
% figure(4),
% plot(t,q1,'r','LineWidth',2);hold on;%绘制角度位置
% plot(t,q2,'b','LineWidth',2);hold on;%绘制角度位置
% plot(t,q3,'g','LineWidth',2);hold on;%绘制角度位置
% plot(t,q4,'k','LineWidth',2);hold on;%绘制角度位置
% plot(t,q5,'c','LineWidth',2);hold on;%绘制角度位置
% plot(t,q6,'m','LineWidth',2);hold on;%绘制角度位置
% plot(t,q7,'y','LineWidth',2);grid on;%绘制角度位置
% axis([1,num-1,-400,400]);
% end

逆运动学数值解

根据数值解逻辑原理,编写matlab数值解子程序。
数值解,结合了微分运动,雅各比矩阵,牛顿下山法,进行求解逆解,可能会出现机械臂的奇异解。牛顿下山法逻辑如下图
数值解逻辑
当正运动学与目标值有差异时修正关节角,再循环,再修正,类似一个闭环控制。上图中的与表示给定机器人末端目标位的姿态,q表示初始各个关节角,δq表示输出的修正量。该数值解法的步骤为:首先第一步给定机器人末端目标位姿态,第二步定义机器人各关节的初始关节角,第三步 由正运动学计算机器人末端的当前位姿,第四步计算机器人末端位姿的误差,即通过-P,分别得到和,第五步当末端位姿误差足够小时停止运算,第六步 当误差大于设定值时计算关节角的修正量,第七步q=q+δq更新关节变量。

function q=IK_7DOF_num_solu(W)%数值解函数
%W为4*4 的期望位置和姿态
lamda=0.5;%范围(0,1) 0.5

e=zeros(1,6);
q=zeros(7,1);%7*1的0矩阵 六个初始关节角q=0
pref=W(1:3,4);%第四列的1-3行 期望位置
Rref=W(1:3,1:3);%3*3 1-3行 1-3列矩阵 期望姿态
ilimit=700;%修正次数越多,误差越小 
count=1;

while true
    
    count = count + 1;
    if count >= ilimit
        fprintf('iteration number %d final err %f \n',count,err);
        break
    end

    P=DHfk7Dof_kuka_nodraw(q);%初始关节角下的0-7转换矩阵P

    p=P(1:3,4);%第四列的1-3行 当前位置
    perr=pref-p;%计算位置误差perr

    R=P(1:3,1:3);%3*3  当前姿态
    Rerr=R'*Rref;%计算姿态误差Rerr
    th=acos((Rerr(1,1)+Rerr(2,2)+Rerr(3,3)-1)/2);
    
    if Rerr==eye(3)
        werr=[0 0 0]';
    else 
        %%姿态误差:角度误差
        werr=(th/2*sin(th))*[Rerr(3,2)-Rerr(2,3),Rerr(1,3)-Rerr(3,1), Rerr(2,1)-Rerr(1,2)]';
    end

    e(1:3)=perr(1:3);
    e(4:6)=werr(1:3);

     err=norm(e(1:3))+norm(e(4:6));

    if err<=1e-6%如果误差小于或等于10^-6
       fprintf(' iteration number %d final err %f \n',count,err);
      break
    end
    %%%限定滑动关节变化长度
    if(q(7)<15)
        q(7)=15;
       else
        if (q(7)>=170)
        q(7)=170;
        end
    end
    %%%转换角度值
    for i=1:1:6
        if(q(i)>360) 
            q(i)=q(i)-360;
        else
            if(q(i)<-360)
                q(i)=q(i)+360;
            end
        end
    end
    J=Jacobian7DoF_Ln(q(1),q(2),q(3),q(4),q(5),q(6),q(7));%当前角度构建雅可比矩阵
    deta_q=lamda*pinv(J)*e';%根据误差得出角度微调量deta_q  牛顿下山法
    q=q+deta_q;%更新当前角度
end

雅各比矩阵子程序

下图是7轴的机械臂雅各比矩阵,通过修改参数,可以变成任意数关节轴的雅各比矩阵

function J=Jacobian7DoF_Ln(th1,th2,th3,th4,th5,th6,th7)
% close all

global Link

jsize=7;
J=zeros(6,jsize);

Link(2).th=th1*pi/180;
Link(3).th=th2*pi/180;
Link(4).th=th3*pi/180;
Link(5).th=th4*pi/180;
Link(6).th=th5*pi/180;
Link(7).th=th6*pi/180;
Link(8).dz=th7;%第七个关节轴为伸缩关节
for i=1:8
Matrix_DH_Ln(i);
end

Link(1).p=Link(1).p(1:3);
for i=2:8
      Link(i).A=Link(i-1).A*Link(i).A;
      Link(i).p= Link(i).A(1:3,4);
      Link(i).n= Link(i).A(:,1);
      Link(i).o= Link(i).A(:,2);
      Link(i).a= Link(i).A(:,3);
      Link(i).R=[Link(i).n(1:3),Link(i).o(1:3),Link(i).a(1:3)];
end

target=Link(8).p;

for n=1:jsize
     a=Link(n).R*Link(n).az;  
     if(n==7) 
         b=[0,0,0]';
         J(:,n)=[a; b];
     else
     J(:,n)=[cross(a,target-Link(n).p); a];
     end
end

齐次变换矩阵求解

利用齐次变换矩阵通式,编写此程序,其中参数th、dz、da、a依次对应DH参数表中的θ、dz、da、α。

function qi_ci_bian_huan_ju_zhen(th,dz,da,a)
T=[cos(th) -cos(a)*sin(th) sin(a)*sin(th) da*cos(th);
    sin(th) cos(a)*cos(th) -sin(a)*cos(th) da*sin(th);
    0 sin(a) cos(a) dz;
    0 0 0 1]
end

绘制RRT避障提取小球时机械臂指标子程序

机械臂7个关节的角度、角速度、角加速度

function [] = RRT_zhi_biao(Angel)%输入是7个关节的角度,每一列对应一个关节的角度。输出为各个关节的角度,速度,加速度曲线
%调用方法:aa_ang_vel_acc_plot(Angel);
for joint=1:1:7
    
angel_1=Angel(:,joint);%在for循环中,分别提取各个关节的angel
delta_t=0.15;%时间差

t_angel=(0:delta_t:delta_t*(size(angel_1,1)-1))';%角度 画图的时间轴
vel_1=zeros(1,size(angel_1,1)-1);%准备容器
acc_1=zeros(1,size(vel_1,2)-1);%准备容器
for i_count=1:size(angel_1,1)-1
    vel_1(i_count)=(angel_1(i_count+1)-angel_1(i_count))/delta_t; %旋转速度
end
t_vel=(0:delta_t:delta_t*(size(vel_1,2)-1))';%速度 画图的时间轴

for i_count=1:size(vel_1,2)-1
    acc_1(i_count)=(vel_1(i_count+1)-vel_1(i_count))/delta_t; %加速度
end
    t_acc=(0:delta_t:delta_t*(size(acc_1,2)-1))'; %加速度 画图的时间轴


figure(joint+1)
plot(t_angel,angel_1,'g',t_vel,vel_1,'r',t_acc,acc_1,'b');
hold on
title('关节角度-速度-角速度');
legend('角度','速度','加速度') 
end
end

机械臂工作空间分析

工作空间分析时,同时绘制了10个小球,并绘制工作空间、验证了机械臂可以达到的点覆盖住了小球,说明可以抓到所有小球。

close all;

clear;

global Link
Build_7DOFRobot_Lnya;
ss=[];%存储小球信息的矩阵
%ellipsoid(x,y,z,x1,y1,z1) x,y,z代表了椭球的中心,x1,y1,z1代表了x,y,z方向的分量
for ii=1:1:10
   BanJing=randi([25 40]);%产生一个25到40之间的随机数,当作圆的半径
%  j=unidrnd(50);%产生一个50以内的随机数
   
   s1=unidrnd(50);%s是随机生成小球的初始点
   s2=randi([50 150]);%s是随机生成小球的初始点
   s3=randi([100 157]);%s是随机生成小球的初始点
%    s4=randsrc();%随机生成正负1
   %随机生成小球位置,并且每个小球圆心坐标a,b,c至少相差60,保证每两个小球间相距小于200
   aa=s1+100+ii*80;
%     aa=0;
%    bb=s4*s2+ii*60;
%    ccc=s4*s3+ii*60;
    bb=s2+ii*70;
    pp=s3+ii*70;
   [xx,yy,zz]= ellipsoid(aa,bb,pp,BanJing,BanJing,BanJing);
   surf(xx,yy,zz) %半径为j的圆
%    shading interp%防止surf画出黑色小球
   dd=(4/3)*pi*BanJing^3;%球的体积
   ti_ji=round(dd);%对体积取整
   ss=[ss;aa,bb,pp,ti_ji,BanJing];
%    yan_se=ones(size(zz,1));
  
   disp(ss)%打印出每个小球的圆心坐标和体积
   hold on
%    view(-8,38);%最后在(-8 ,38)角度观看
   alpha(0.5)%坐标系内所有图片透明度为0.5,1是不透明 
end


ToDeg = 180/pi;
ToRad = pi/180;

num=1;

for th1=-180:60:180%每个轴的工作角度范围,60是每一次循环的间隔。
    for th2=-180:10:180
        for th3=-245:100:65
            for th4=-200:100:200
                for th5=-115:46:115
                    for th6=-400:100:400
                        for th7=-360:360:360
                        theta1=th1*ToRad;
                        theta2=th2*ToRad;
                        theta3=th3*ToRad;
                        theta4=th4*ToRad;
                        theta5=th5*ToRad;
                        theta6=th6*ToRad;
                        theta7=th7*ToRad;

                        A1 =[[ cos(theta1), 0 ,-sin(theta1) ,   0]
                                [ sin(theta1),  0,cos(theta1),   0]
                                [           0, -1,            0, 350]
                                [           0,  0,            0,   1]];
                        A2 =[[ cos(theta2 ), -sin(theta2),0, 400*cos(theta2)]
                                [ sin(theta2 ),cos(theta2),0,400*sin(theta2)]
                                [     0,     1, 0, 0]
                                [        0,       0, 0,   1]];
                        A3 =[[ cos(theta3), -sin(theta3),0,400*cos(theta3)]
                                [ sin(theta3), cos(theta3),0,400*sin(theta3)]
                                [           0,   0,   1,     0]
                                [           0,  0,            0,   1]];
                        A4 =[[ cos(theta4), -sin(theta4),0,400*cos(theta4)]
                                [ sin(theta4), cos(theta4),0,400*sin(theta4)]
                                [           0,   0,   1,     0]
                                [           0,  0,            0,   1]];
                        A5 =[[ cos(theta5), -sin(theta5),0, 400*cos(theta5)]
                                [ sin(theta5), cos(theta5), 0,400*sin(theta5)]
                                [           0,   0,   1,     0]
                                [           0,  0,0,   1]];
                        A6 =[[ cos(theta6), 0, -sin(theta6), 0]
                                [ sin(theta6),  0, cos(theta6), 0]
                                [  0,  -1, 0,             0]
                                [    0,         0, 0,           1]];
                        A7 =[[cos(theta7), -sin(theta7), 0,0]
                                [ sin(theta7), cos(theta7),0, 0]
                                [  0,  0, 0,    170]
                                [    0,         0, 0,           1]];
                     
                        A = A1 * A2 * A3 * A4 * A5 * A6*A7;
            
                        point1(num) = A(1,4);
                        point2(num) = A(2,4);
                        point3(num) = A(3,4);
                        num = num + 1;  
                       end
                   end
               end
           end
       end
    end
end
plot3(point1,point2,point3,'r*');
axis([-3000,3000,-3000,3000,-3000,3000]);
grid on;

保持剩余小球位置和大小子程序

在绘制机械臂运动的时候调用该程序,以保持抓取后剩余小球的位置保持不变。逻辑思想是,每循环一次,则把ss矩阵中被抓取过的小球信息删除变为0。然后绘制小球,因为有些信息变为0了。所以绘制出来是空。

function xiao_qiu_zhui_zong(ss,xun_huan,index)
ss(index(1,xun_huan),:)=[0, 0 ,0, 0 ,0 ];%删除矩阵ss的某一行% % %角度轨迹规划
for ge=1:1:10
    [xx1,yy1,zz1]=ellipsoid(ss(ge,1),ss(ge,2),ss(ge,3),ss(ge,5),ss(ge,5),ss(ge,5));
    surf(xx1,yy1,zz1); %半径为j的圆
    hold on
end
end

主函数

主函数中首先完成了小球的初试绘制,将其存入了矩阵ss,并对体积大小做了排序,存入index矩阵。然后运用了for循环来进行十次的小球抓取。

%正运动学求解,并绘制机器人
close all;
clear all;

global d5 %手动控制末端关节长度d5
global A_x A_y A_z %角度规划的起始点坐标
global p_x p_y p_z %末端规划路径坐标
global q_1 q_2 q_3 q_4 q_5 q_6 q_7
q_1=0;q_2=0;q_3=0;q_4=0;q_5=0;q_6=0;q_7=0;%初始关节角为0
shui_ping_yuan_zhu([0,0,0]',[0,1,0]',1000,4000,0);
hold on
%%赋初值
d5=170;
A_x=0;A_y=-600;A_z=-600;
%设置角度规划的A点到B点的坐标
% B_x=[0  0];
% B_y=[-500  -1000];
% B_z=[-500  -1000];
%生成10小球

ss=[];%存储小球信息的矩阵
%ellipsoid(x,y,z,x1,y1,z1) x,y,z代表了椭球的中心,x1,y1,z1代表了x,y,z方向的分量
for ii=1:1:10
   BanJing=randi([25 40]);%产生一个25到40之间的随机数,当作圆的半径
%  j=unidrnd(50);%产生一个50以内的随机数
   
   s1=unidrnd(50);%s是随机生成小球的初始点
   s2=randi([0 50]);%s是随机生成小球的初始点
   s3=randi([0 57]);%s是随机生成小球的初始点
%    s4=randsrc();%随机生成正负1
   %随机生成小球位置,并且每个小球圆心坐标a,b,c至少相差60,保证每两个小球间相距小于200
   aa=s1+100+ii*60;
%     aa=0;
%    bb=s4*s2+ii*60;
%    ccc=s4*s3+ii*60;
    bb=s2+ii*60;
    pp=s3+ii*60;
   [xx,yy,zz]= ellipsoid(aa,bb,pp,BanJing,BanJing,BanJing);
   surf(xx,yy,zz) %半径为j的圆
%    shading interp%防止surf画出黑色小球
   dd=(4/3)*pi*BanJing^3;%球的体积
   ti_ji=round(dd);%对体积取整
   ss=[ss;aa,bb,pp,ti_ji,BanJing];
%    yan_se=ones(size(zz,1));
  
   disp(ss)%打印出每个小球的圆心坐标和体积
   hold on
%    view(-8,38);%最后在(-8 ,38)角度观看
   alpha(0.1)%坐标系内所有图片透明度为0.5,1是不透明 
end

AA=[ss(1,4),ss(2,4),ss(3,4),ss(4,4),ss(5,4),ss(6,4),ss(7,4),ss(8,4),ss(9,4),ss(10,4)];%将每个球的体积读取到矩阵A中
[pai_xu,index]= sort(AA,'descend'); %对矩阵A中的体积按从大到小排列,并写出原来在矩阵A中第几列,存入index矩阵中,也就是在矩阵ss中第几行
%上边体积已经从大到小排好,并且是第几次生成的球也用index表示出来
%下边就根据text函数,依次对第几次生成的球,按照体积大小标记序号
for m=1:1:10
   xu_hao = num2str(m);%数值转字符串,准备循环从1到10,10个数
   text(ss(index(1,m),1),ss(index(1,m),2),ss(index(1,m),3),xu_hao,'Color','red','FontSize',14);%标记序号,原来ss矩阵中第m行第1列做横坐标,
   %ss矩阵中第m行第2列做纵坐标然后标记序号。
end

%%%末端规划路径坐标初始化,赋0
for i=1:1:length(p_x)
    p_x(i)=0;
    p_y(i)=0;
    p_z(i)=0;
end
%%%角度弧度转换
ToDeg = 180/pi;
ToRad = pi/180;
%设置初始关节转动的角度
th1=90;%Link(2).th
th2=0;%Link(3).th
th3=0;%Link(4).th
th4=0;%Link(5).th
th5=0;%Link(6).th
th6=-90;%Link(7).th
th7=d5;%Link(8).th
grid on;
%%%绘制机器人初始状态
DHfk7Dof_Lnya(th1,th2,th3,th4,th5,th6,th7,0);%0为不擦除伪影,1擦除
view(64,20);
hold on
pause;
shui_ping_yuan_zhu([0,0,0]',[0,1,0]',1000,4000,0);

for xun_huan=1:1:10%抓取10次小球。


shui_ping_yuan_zhu([0,0,0]',[0,1,0]',1000,4000,0);
hold on
rrt_SanWei(ss,index,xun_huan,ss(index(1,xun_huan),5));

% 直角坐标系末端路径规划
% 五次多项式直线末端规划
Draw_line_5(ss(index(1,xun_huan),3),ss(index(1,xun_huan),2),100,3,ss(index(1,xun_huan),5),ss,index,xun_huan,ss(index(1,xun_huan),1),0);%x轴方向左移动
Draw_line_5(ss(index(1,xun_huan),3),ss(index(1,xun_huan),2),100,1,ss(index(1,xun_huan),5),ss,index,xun_huan,ss(index(1,xun_huan),1),0);%y轴方向左移动
% %末端4-3-4曲线末端规划
hold on
Draw_curve_434(ss(index(1,xun_huan),3),-400,-500,-600,0,0,0,0,2,4,ss(index(1,xun_huan),5),ss,index,xun_huan);%规划z轴的
ss(index(1,xun_huan),:)=[0, 0 ,0, 0 ,0 ];%删除矩阵ss的某一行,变成0 0 0 0 0% % %角度轨迹规划
fan_hui=xun_huan+1;%返回目标点为下一次循环的小球
if xun_huan<10
B_x=ss(index(1,fan_hui),1);
B_y=ss(index(1,fan_hui),2);
B_z=ss(index(1,fan_hui),3);
Draw_angle_434(B_x,B_y,B_z,ss,xun_huan,index);%%角度434空间规划 从A到B绘制目标点A到下一个小球轨迹
else
end
end
% % 

从目标点至下一小球轨迹

function Draw_angle_434(B_x,B_y,B_z,ss,xun_huan,index)
global A_x A_y A_z
shui_ping_yuan_zhu([0,0,0]',[0,1,0]',1000,4000,0);
xiao_qiu_zhui_zong(ss,xun_huan,index);
hold on

W_A=[1,0,0,A_x;%%起点A位姿
     0,1,0,A_y;
     0,0,1,A_z;
     0,0,0,1];
i=5;%调节采样数量来改变绘制时间
n=150;%设置A到B点的采集数据个数
for j=1:1:1
    B1_x=B_x(j);B1_y=B_y(j);B1_z=B_z(j);
    W_B=[1,0,0,B1_x;%%终点B位姿
         0,1,0,B1_y;
         0,0,1,B1_z;
         0,0,0,1];
    A_th=IK_7DOF_num_solu(W_A);%A_th为角度制,A位置逆解出来的角度
    B_th=IK_7DOF_num_solu(W_B);%A_th为角度制,B位置逆解出来的角度

    Q1_D=(B_th(1)-A_th(1))/3;
    Q2_D=(B_th(2)-A_th(2))/3;
    Q3_D=(B_th(3)-A_th(3))/3;
    Q4_D=(B_th(4)-A_th(4))/3;
    Q5_D=(B_th(5)-A_th(5))/3;
    Q6_D=(B_th(6)-A_th(6))/3;
    Q7_D=(B_th(7)-A_th(7))/3;

    Q1=Creat_434_curve(A_th(1),A_th(1)+Q1_D,A_th(1)+Q1_D*2,B_th(1),0,0,0,0,3,j);%4-3-4路径规划 关节1
    Q2=Creat_434_curve(A_th(2),A_th(2)+Q2_D,A_th(2)+Q2_D*2,B_th(2),0,0,0,0,4,j);%4-3-4路径规划 关节2
    Q3=Creat_434_curve(A_th(3),A_th(3)+Q3_D,A_th(3)+Q3_D*2,B_th(3),0,0,0,0,5,j);%4-3-4路径规划 关节3
    Q4=Creat_434_curve(A_th(4),A_th(4)+Q4_D,A_th(4)+Q4_D*2,B_th(4),0,0,0,0,6,j);%4-3-4路径规划 关节4
    Q5=Creat_434_curve(A_th(5),A_th(5)+Q4_D,A_th(5)+Q5_D*2,B_th(5),0,0,0,0,7,j);%4-3-4路径规划 关节5
    Q6=Creat_434_curve(A_th(6),A_th(6)+Q3_D,A_th(6)+Q6_D*2,B_th(6),0,0,0,0,8,j);%4-3-4路径规划 关节6
    Q7=Creat_434_curve(A_th(7),A_th(7)+Q7_D,A_th(7)+Q7_D*2,B_th(7),0,0,0,0,9,j);%4-3-4路径规划 关节7
    num=1;
    for t=0:i:n-1%采集数据个数为n/i
        shui_ping_yuan_zhu([0,0,0]',[0,1,0]',1000,4000,0);
        xiao_qiu_zhui_zong(ss,xun_huan,index);
        DHfk7Dof_Lnya(Q1(num),Q2(num),Q3(num),Q4(num),Q5(num),Q6(num),Q7(num),1);
        num=num+i;
    end
    shui_ping_yuan_zhu([0,0,0]',[0,1,0]',1000,4000,0);
    xiao_qiu_zhui_zong(ss,xun_huan,index);
    DHfk7Dof_Lnya(Q1(num-i),Q2(num-i),Q3(num-i),Q4(num-i),Q5(num-i),Q6(num-i),Q7(num-i),1);
%     plot3(A_x,A_y,A_z,'r*');
%     plot3(B1_x,B1_y,B1_z,'ko','LineWidth',2); grid on;pause;
    
end
%绘图
% figure(10);
% t=0:1:n-1;
% plot(t,Q1,'c-','LineWidth',2);hold on;
% plot(t,Q2,'b-','LineWidth',2);hold on;
% plot(t,Q3,'k-','LineWidth',2);hold on;
% plot(t,Q4,'g-','LineWidth',2);hold on;
% plot(t,Q5,'y-','LineWidth',2);hold on;
% plot(t,Q6,'m-','LineWidth',2);hold on;
% plot(t,Q7,'r-','LineWidth',2);hold on;
% xlabel('t');ylabel('position');
% legend('position1','position2','position3','position4','position5','position6','position7') %可依次设置成你想要的名字grid on;%绘制结束

总结

标题里文字介绍不清楚的地方,可以多看看程序里的注释。多运行程序进行试验,逐渐修改程序,理解程序怎么运行的。程序打包上传,嫌自己建工程复制代码麻烦的可以直接下载打包好的程序,下载地址:https://download.csdn.net/download/weixin_48681463/19832319。

热门文章

暂无图片
编程学习 ·

那些年让我们目瞪口呆的bug

程序员一生与bug奋战&#xff0c;可谓是杀敌无数&#xff0c;见怪不怪了&#xff01;在某知识社交平台中&#xff0c;一个“有哪些让程序员目瞪口呆的bug”的话题引来了6700多万的阅读&#xff0c;可见程序员们对一个话题的敏感度有多高。 1、麻省理工“只能发500英里的邮件” …
暂无图片
编程学习 ·

redis的下载与安装

下载redis wget http://download.redis.io/releases/redis-5.0.0.tar.gz解压redis tar -zxvf redis-5.0.0.tar.gz编译 make安装 make install快链方便进入redis ln -s redis-5.0.0 redis
暂无图片
编程学习 ·

《大话数据结构》第三章学习笔记--线性表(一)

线性表的定义 线性表&#xff1a;零个或多个数据元素的有限序列。 线性表元素的个数n定义为线性表的长度。n为0时&#xff0c;为空表。 在比较复杂的线性表中&#xff0c;一个数据元素可以由若干个数据项组成。 线性表的存储结构 顺序存储结构 可以用C语言中的一维数组来…
暂无图片
编程学习 ·

对象的扩展

文章目录对象的扩展属性的简洁表示法属性名表达式方法的name属性属性的可枚举性和遍历可枚举性属性的遍历super关键字对象的扩展运算符解构赋值扩展运算符AggregateError错误对象对象的扩展 属性的简洁表示法 const foo bar; const baz {foo}; baz // {foo: "bar"…
暂无图片
编程学习 ·

让程序员最头疼的5种编程语言

世界上的编程语言&#xff0c;按照其应用领域&#xff0c;可以粗略地分成三类。 有的语言是多面手&#xff0c;在很多不同的领域都能派上用场。大家学过的编程语言很多都属于这一类&#xff0c;比如说 C&#xff0c;Java&#xff0c; Python。 有的语言专注于某一特定的领域&…
暂无图片
编程学习 ·

写论文注意事项

参考链接 给研究生修改了一篇论文后&#xff0c;该985博导几近崩溃…… 重点分析 摘要与结论几乎重合 这一条是我见过研究生论文中最常出现的事情&#xff0c;很多情况下&#xff0c;他们论文中摘要部分与结论部分重复率超过70%。对于摘要而言&#xff0c;首先要用一小句话引…
暂无图片
编程学习 ·

安卓 串口开发

上图&#xff1a; 上码&#xff1a; 在APP grable添加 // 串口 需要配合在项目build.gradle中的repositories添加 maven {url "https://jitpack.io" }implementation com.github.licheedev.Android-SerialPort-API:serialport:1.0.1implementation com.jakewhart…
暂无图片
编程学习 ·

2021-2027年中国铪市场调研与发展趋势分析报告

2021-2027年中国铪市场调研与发展趋势分析报告 本报告研究中国市场铪的生产、消费及进出口情况&#xff0c;重点关注在中国市场扮演重要角色的全球及本土铪生产商&#xff0c;呈现这些厂商在中国市场的铪销量、收入、价格、毛利率、市场份额等关键指标。此外&#xff0c;针对…
暂无图片
编程学习 ·

Aggressive cows题目翻译

描述&#xff1a; Farmer John has built a new long barn, with N (2 < N < 100,000) stalls.&#xff08;John农民已经新建了一个长畜棚带有N&#xff08;2<N<100000&#xff09;个牛棚&#xff09; The stalls are located along a straight line at positions…
暂无图片
编程学习 ·

剖析组建PMO的6个大坑︱PMO深度实践

随着事业环境因素的不断纷繁演进&#xff0c;项目时代正在悄悄来临。设立项目经理转岗、要求PMP等项目管理证书已是基操&#xff0c;越来越多的组织开始组建PMO团队&#xff0c;大有曾经公司纷纷建造中台的气质&#xff08;当然两者的本质并不相同&#xff0c;只是说明这个趋势…
暂无图片
编程学习 ·

Flowable入门系列文章118 - 进程实例 07

1、获取流程实例的变量 GET运行时/进程实例/ {processInstanceId} /变量/ {变量名} 表1.获取流程实例的变量 - URL参数 参数需要值描述processInstanceId是串将流程实例的id添加到变量中。变量名是串要获取的变量的名称。 表2.获取流程实例的变量 - 响应代码 响应码描述200指…
暂无图片
编程学习 ·

微信每天自动给女[男]朋友发早安和土味情话

微信通知&#xff0c;每天给女朋友发早安、情话、诗句、天气信息等~ 前言 之前逛GitHub的时候发现了一个自动签到的小工具&#xff0c;b站、掘金等都可以&#xff0c;我看了下源码发现也是很简洁&#xff0c;也尝试用了一下&#xff0c;配置也都很简单&#xff0c;主要是他有一…
暂无图片
编程学习 ·

C语言二分查找详解

二分查找是一种知名度很高的查找算法&#xff0c;在对有序数列进行查找时效率远高于传统的顺序查找。 下面这张动图对比了二者的效率差距。 二分查找的基本思想就是通过把目标数和当前数列的中间数进行比较&#xff0c;从而确定目标数是在中间数的左边还是右边&#xff0c;将查…
暂无图片
编程学习 ·

项目经理,你有什么优势吗?

大侠被一个问题问住了&#xff1a;你和别人比&#xff0c;你的优势是什么呢? 大侠听到这个问题后&#xff0c;脱口而出道&#xff1a;“项目管理能力和经验啊。” 听者抬头看了一下大侠&#xff0c;显然听者对大侠的这个回答不是很满意&#xff0c;但也没有继续追问。 大侠回家…
暂无图片
编程学习 ·

nginx的负载均衡和故障转移

#注&#xff1a;proxy_temp_path和proxy_cache_path指定的路径必须在同一分区 proxy_temp_path /data0/proxy_temp_dir; #设置Web缓存区名称为cache_one&#xff0c;内存缓存空间大小为200MB&#xff0c;1天没有被访问的内容自动清除&#xff0c;硬盘缓存空间大小为30GB。 pro…
暂无图片
编程学习 ·

业务逻辑漏洞

身份认证安全 绕过身份认证的几种方法 暴力破解 测试方法∶在没有验证码限制或者一次验证码可以多次使用的地方&#xff0c;可以分为以下几种情况︰ (1)爆破用户名。当输入的用户名不存在时&#xff0c;会显示请输入正确用户名&#xff0c;或者用户名不存在 (2)已知用户名。…