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

1 篇文章 4 订阅

## 摘要

### matlab机械臂建模

θ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;%转为角度制
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);     % 结构体数组


### 绘制机械臂

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)
%调用示例：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;
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


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

ToDeg = 180/pi;

a=Link(i).dx;    %distance between zi and zi-1
d=Link(i).dz;    %distance between xi and xi-1



### 绘制机器人子程序

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

Build_7DOFRobot_Lnya;%建立DH参数表
%画图配置参数
len       = 90;%高
joint_col = 0;%颜色
% plot3(0,0,0,'ro');
%输入角度转化为弧度

%%

%根据DH参数表及关节变量构造各个关节的齐次变换矩阵
for i=1:8
Matrix_DH_Ln(i);
end
%绘制表示各个关节的圆柱体及关节连杆
for i=2:8
%计算第i个关节的位姿：根据上一关节的位姿计算下一关节的位姿
%绘制第i-1个关节
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)
Build_7DOFRobot_Lnya;%建立DH参数表

%input angle

%DH matrix
for i=1:8
Matrix_DH_Ln(i);
end

%position and orientation
for i=2:8
end



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

% 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


%% 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);
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轨迹规划

function Q=Creat_434_curve(Th1,Th2,Th3,Th4,Th1_1,Th1_2,Th4_1,Th4_2,num,j)
%四个角度，起始点速度、加速度，终端点速度、加速度
%三次多项式
ToDeg = 180/pi;
% %定义初始角度
% 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



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的圆
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的圆
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的圆
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的圆
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


### 逆运动学数值解

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



### 雅各比矩阵子程序

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

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

for i=1:8
Matrix_DH_Ln(i);
end

for i=2:8
end

for n=1:jsize
if(n==7)
b=[0,0,0]';
J(:,n)=[a; b];
else
end
end



### 齐次变换矩阵求解

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避障提取小球时机械臂指标子程序

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



### 机械臂工作空间分析

close all;

clear;

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的圆
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;

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

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;



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

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


### 主函数

%正运动学求解，并绘制机器人
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的圆
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;
%设置初始关节转动的角度
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;%绘制结束


## 总结

• 15
点赞
• 127
收藏
觉得还不错? 一键收藏
• 4
评论
09-22
07-28 657
06-25
08-05
01-18
11-28 5449
07-09 1333
04-02 618
11-27 3645
04-25 398

### “相关推荐”对你有帮助么？

• 非常没帮助
• 没帮助
• 一般
• 有帮助
• 非常有帮助

1.余额是钱包充值的虚拟货币，按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载，可以购买VIP、付费专栏及课程。