基于matlab七轴可伸缩关节机械臂物品抓取

摘要

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

连环图展示

机械臂介绍

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

关节	工作范围
关节1	180°~180°
关节2	180°~180°
关节3	-245°~65°
关节4	-200°~200°
关节5	-115°~115°
关节6	-180°~180°
关节7	-360°~360° （可伸缩关节）

机械臂构型下图所示

matlab机械臂建模

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

建立DH参数表

	θi	dz	da	αi
T1	pi/2	350	0	pi/2
T2	0	0	400	0
T3	0	0	400	0
T4	0	0	400	0
T5	0	0	400	0
T6	pi/2	0	0	-pi/2
T7	0	L(15~170mm)	0	0

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。