6自由度空间机器人课程设计的简要记录

这个空间机器人的课设是在学期末的时候已经完成，现在在假期有时间正好做一下简要的总结和记录。另外最后一题仅仅是给出了思路（利用遗传算法 或是 粒子群算法），最终在代码实现方面并未完成，当时时间也比较紧，答辩完紧接着有考试，所以没有真正完成。（这里仅对实现代码做下简单的记录，具体内容可以看我的答辩报告）

任务

1、按固定基座空间机器人推导其运动学模型

2、按自由漂浮空间机器人推导其运动学模型

3、完成运动学正、逆解的简单运算

4、完成运动学模型的搭建，分析两者的运动空间异同

5、完成一次基座坐标系下的点到点运动规划

6、完成一次非完整运动规划（基座、关节角均期望）

表 2 空间机器人质量特性

		Sat	B1	B2	B3	B4	B5	B6
Mass(kg)		400	6	5	5	4	3	2
iai			0	0.2702	0	0	0	0
			0	0	0	0	-0.0338	0
			0.15	-0.2513	0.15	-0.35	0	0.0750
ibi		0.3570	0	0.5598	0	0	0	0
		-0.0095	0	0	0	0	-0.0662	0
		0.419	0.15	-0.0487	0.15	-0.35	0	0.1595
iIi (kg.m2)	Ixx	30	0.15	0.0926	0.105	0.2498	0.0330	5.152E-002
	Iyy	28	0.15	0.9053	0.105	0.2498	0.0172	5.152E-002

iIi (kg.m2)	Ixx	30	0.15	0.0926	0.105	0.2498	0.0330	5.152E-002
	Iyy	28	0.15	0.9053	0.105	0.2498	0.0172	5.152E-002
	Izz	32	0.075	0.8451	0.0294	0.0196	0.0260	2.192E-002

空间机械臂D-H参数表

连杆i	θi	αi	ai （mm）	di （mm）
1	90°	-90°	0	0
2	-180°	0	830	0
3	-90°	90°	0	0
4	0	-90°	0	-700
5	180°	90°	0	0
6	0	0	0	334.5

第三题利用粒子群算法求逆向运动学的最优解的代码（matlab），当然对于其他求最优解问题，只要将后面的适应度函数修改后，也是可以拿去直接用的，代码大部分每一行我都做了注释，很好理解。（后面的适应度函数的代码风格很差，不美观，已遭学霸吐槽，matlab是这学期写大作业是刚学的，还有所生疏）

 
%------给定初始化条件----------------------------------------------
c1=2;             %学习因子1
c2=2;             %学习因子2
w=0.7;            %惯性权重
MaxDT=3000;       %最大迭代次数
D=6;             %搜索空间维数（未知数个数）
M=3000;             %初始化群体个体数目


%------初始化种群的个体(可以在这里限定位置和速度的范围)------------
for i=1:M
    for j=1:D
        v(i,j)=randn; %随机速度大小
        x(i,j)=pi*rand; %随机初始化位置（角度）
    end
end

%------先计算各个粒子的适应度，并初始化p(i)和gbest--------------------
for i=1:M
    
    y(i,:)=x(i,:);   %每个粒子对应的6维度，相当于把每个粒子给y，设y对应值此时为局部最优
      p(i)=fitness(x(i,:));  %计算每个粒子距离目标位置的距离
end
   gbest=x(1,:);    %先把第一个粒子的值设为gbest全局最优      

for i=2:M
    if fitness(x(i,:)) < fitness(gbest) %从第二个粒子开始比较，如果有比gbest对应距离小的，则更新gbest为当前值
        gbest=x(i,:);
    end
end

%------进入主要循环，按照公式依次迭代，直到满足精度要求------------
for t=1:MaxDT
    for i=1:M
       % theta(i,:)=w*theta(i,:)+c1*rand*(y(i,:)-theta(i,:))+c2*rand*(gbest-theta(i,:));
        %x(i,:)=x(i,:)+theta(i,:);
         v(i,:)=w*v(i,:)+c1*rand*(y(i,:)-x(i,:))+c2*rand*(gbest-x(i,:));
        x(i,:)=x(i,:)+v(i,:);
        if fitness(x(i,:))<p(i)   %如果迭代后此时的x(i,:)对应的距离小于原来i对应的，则当前x为局部最优
            p(i)=fitness(x(i,:));
            y(i,:)=x(i,:);
        end
        if p(i)<fitness(gbest)
            gbest=y(i,:);
        end
    end
        if(fitness(gbest)<0.001)
            break;
        end
      
    end


 

%------显示计算结果
disp('*************************************************************')
disp('粒子的最优theta值为：')
%Solution=gbest;
disp(gbest);
%disp(y)
%disp(fitness(x))
%Result=fitness(gbest)
disp('*************************************************************')


function result = fitness(x)%适应度函数，值越小说明重合度越高，就越精确

T=myfun(x);
B=[0 -1 0 0;0 0 -1 -1864.5;1 0 0 0;0 0 0 1];
%result=sqrt(T(1,1)^2+T(2,1)^2+(T(3,1)-1)^2)+sqrt((T(1,2)+1)^2+(T(2,2))^2+(T(3,2))^2)+sqrt((T(1,3))^2+(T(2,3)+1)^2+(T(3,3))^2)+sqrt(T(1,4)^2+(T(2,4)+1864.5)^2+(T(3,4))^2);
 %result=abs(T(1,1))+abs(T(2,1))+abs(T(3,1)-1)+abs(T(1,2)+1)+abs(T(2,2))+abs(T(3,2))+abs(T(1,3))+abs(T(2,3)+1)+abs(T(3,3))+abs(T(1,4))+abs(T(2,4)+1864.5)+abs(T(3,4));
C=B-T;
result=norm(C);
end


function T=myfun(theta)

 alphad1=-pi/2; a1=0;d1=0;
 alphad2=0;  a2=830;d2=0;
 alphad3=pi/2; a3=0;d3=0;
 alphad4=-pi/2; a4=0;d4=-700;
alphad5=pi/2;a5=0;d5=0;
 alphad6=0; a6=0;d6=334.5;


i=1;
A1=[cos(theta(i)),-sin(theta(i))*cos(alphad1),sin(theta(i))*sin(alphad1),a1*cos(theta(i));
    sin(theta(i)),cos(theta(i))*cos(alphad1),-cos(theta(i))*sin(alphad1),a1*sin(theta(i));
    0,sin(alphad1),cos(alphad1),d1;
    0,0,0,1];
i=2;
A2=[cos(theta(i)),-sin(theta(i))*cos(alphad2),sin(theta(i))*sin(alphad2),a2*cos(theta(i));
    sin(theta(i)),cos(theta(i))*cos(alphad2),-cos(theta(i))*sin(alphad2),a2*sin(theta(i));
    0,sin(alphad2),cos(alphad2),d2;
    0,0,0,1];
i=3;
A3=[cos(theta(i)),-sin(theta(i))*cos(alphad3),sin(theta(i))*sin(alphad3),a3*cos(theta(i))
    sin(theta(i)),cos(theta(i))*cos(alphad3),-cos(theta(i))*sin(alphad3),a3*sin(theta(i))
    0,sin(alphad3),cos(alphad3),d3
    0,0,0,1];
i=4;
A4=[cos(theta(i)),-sin(theta(i))*cos(alphad4),sin(theta(i))*sin(alphad4),a4*cos(theta(i))
    sin(theta(i)),cos(theta(i))*cos(alphad4),-cos(theta(i))*sin(alphad4),a4*sin(theta(i))
    0,sin(alphad4),cos(alphad4),d4
    0,0,0,1];
i=5;
A5=[cos(theta(i)),-sin(theta(i))*cos(alphad5),sin(theta(i))*sin(alphad5),a5*cos(theta(i))
    sin(theta(i)),cos(theta(i))*cos(alphad5),-cos(theta(i))*sin(alphad5),a5*sin(theta(i))
    0,sin(alphad5),cos(alphad5),d5
    0,0,0,1];
i=6;
A6=[cos(theta(i)),-sin(theta(i))*cos(alphad6),sin(theta(i))*sin(alphad6),a6*cos(theta(i))
    sin(theta(i)),cos(theta(i))*cos(alphad6),-cos(theta(i))*sin(alphad6),a6*sin(theta(i))
    0,sin(alphad6),cos(alphad6),d6
    0,0,0,1];


T=A1*A2*A3*A4*A5*A6;
  
  
end

第四题，搭建运动学模型以及进行运动空间的分析，这里主要是利用蒙特卡洛方法，通过大样本随机生成的theta角，利用变换矩阵来求取机械臂最终的末端位姿坐标，利用matlab中的plot3或是scatter3函数来画出三维空间的点云图，下面第一个为固定基座的第二个为自由漂浮的与固定基座的工作空间比较

clear ;
N=10000;
x = -pi + (2*pi).*rand([N 6]);
%disp(x(1,:));
  for i=1:N  
  T=myfun(x(i,:));
  p=[T(1,4),T(2,4),T(3,4)];
  %{
  disp('00000000000000000000000000000000000000000000000000000000');
  disp(p(1));
  disp(p(2));
  disp(p(3));
  disp('oooooooooooooooooooooooooooooooooooooooooooooooooooooooooooooo');
  %}
 u(1,i)=p(1);
 v(1,i)=p(2);
 w(1,i)=p(3);
  %plot3(p(1),p(2),p(3),'o');
 % scatter3(p(1),p(2),p(3),'o');
  
  end
 figure(2)
% disp(w);
 %disp(y);
scatter3(u,v,w,'o');
xlabel('x');
ylabel('y');
zlabel('z');
%plot3(u,v,w);
clear u v w;

function T=myfun(theta)

 alphad1=-pi/2; a1=0;d1=0;
 alphad2=0;  a2=0.830;d2=0;
 alphad3=pi/2; a3=0;d3=0;
 alphad4=-pi/2; a4=0;d4=-0.700;
alphad5=pi/2;a5=0;d5=0;
 alphad6=0; a6=0;d6=0.3345;


i=1;
A1=[cos(theta(i)),-sin(theta(i))*cos(alphad1),sin(theta(i))*sin(alphad1),a1*cos(theta(i));
    sin(theta(i)),cos(theta(i))*cos(alphad1),-cos(theta(i))*sin(alphad1),a1*sin(theta(i));
    0,sin(alphad1),cos(alphad1),d1;
    0,0,0,1];
i=2;
A2=[cos(theta(i)),-sin(theta(i))*cos(alphad2),sin(theta(i))*sin(alphad2),a2*cos(theta(i));
    sin(theta(i)),cos(theta(i))*cos(alphad2),-cos(theta(i))*sin(alphad2),a2*sin(theta(i));
    0,sin(alphad2),cos(alphad2),d2;
    0,0,0,1];
i=3;
A3=[cos(theta(i)),-sin(theta(i))*cos(alphad3),sin(theta(i))*sin(alphad3),a3*cos(theta(i));
    sin(theta(i)),cos(theta(i))*cos(alphad3),-cos(theta(i))*sin(alphad3),a3*sin(theta(i));
    0,sin(alphad3),cos(alphad3),d3;
    0,0,0,1];
i=4;
A4=[cos(theta(i)),-sin(theta(i))*cos(alphad4),sin(theta(i))*sin(alphad4),a4*cos(theta(i));
    sin(theta(i)),cos(theta(i))*cos(alphad4),-cos(theta(i))*sin(alphad4),a4*sin(theta(i));
    0,sin(alphad4),cos(alphad4),d4;
    0,0,0,1];
i=5;
A5=[cos(theta(i)),-sin(theta(i))*cos(alphad5),sin(theta(i))*sin(alphad5),a5*cos(theta(i));
    sin(theta(i)),cos(theta(i))*cos(alphad5),-cos(theta(i))*sin(alphad5),a5*sin(theta(i));
    0,sin(alphad5),cos(alphad5),d5
    0,0,0,1];
i=6;
A6=[cos(theta(i)),-sin(theta(i))*cos(alphad6),sin(theta(i))*sin(alphad6),a6*cos(theta(i));
    sin(theta(i)),cos(theta(i))*cos(alphad6),-cos(theta(i))*sin(alphad6),a6*sin(theta(i));
    0,sin(alphad6),cos(alphad6),d6;
    0,0,0,1];


T=A1*A2*A3*A4*A5*A6;
  
  
end

%代码中有部分数据未使用，因为还可以进一步计算
clear;
m=[400,6,5,5,4,3,2];
M=sum(m);
R0(1,:)=[-0.3570,0.0095,-0.4190];%初始时刻基座位置
R0(2,:)=[0,0,0.15];
R0(3,:)=[0.2702,0.2513,0.3];
R0(4,:)=[0.83,0.15,0.3];
R0(5,:)=[1.18,0,0.3];
R0(6,:)=[1.5638,0,0.3];
R0(7,:)=[1.705,0,0];
rg=0;
for i=1:7
rg=rg+m(i)*R0(i,:);
end
rg=rg/M;

b0=[0.357,-0.0095,0.419];

a(1,:)=[0,0,0.15];
b(1,:)=[0,0,0.15];
a(2,:)=[0.2702,0.2513,0];
b(2,:)=[0.5598,0.0487,0];
a(3,:)=[0,-0.15,0];
b(3,:)=[0,-0.15,0];
a(4,:)=[0.35,0,0];
b(4,:)=[0.35,0,0];
a(5,:)=[0.0338,0,0];
b(5,:)=[0.0662,0,0];
a(6,:)=[0.075,0,0];
b(6,:)=[0.1595,0,0];

b0_=m(1)/M*b0;
for i=1:6
   a_(i,:)=sum(m(1:i))/M*a(i,:);
   b_(i,:)=sum(m(1:i+1))/M*b(i,:);
end
B0=b0-rg;%rg系统质心 ，b0基座矢近，
N=10000;
x = -pi + (2*pi).*rand([N 6]);
 for i=1:N  
  T=myfun1(x(i,:),B0);
  p=[T(1,4),T(2,4),T(3,4)];
 u(1,i)=p(1);
 v(1,i)=p(2);
 w(1,i)=p(3);

  
 end
  
clear ;
N=10000;
x = -pi + (2*pi).*rand([N 6]);
%disp(x(1,:));
  for i=1:N  
  T=myfun(x(i,:));
  p=[T(1,4),T(2,4),T(3,4)];
  %{
  disp('00000000000000000000000000000000000000000000000000000000');
  disp(p(1));
  disp(p(2));
  disp(p(3));
  disp('oooooooooooooooooooooooooooooooooooooooooooooooooooooooooooooo');
  %}
 u(1,i)=p(1);
 v(1,i)=p(2);
 w(1,i)=p(3);
  %plot3(p(1),p(2),p(3),'o');
 % scatter3(p(1),p(2),p(3),'o');
  
  end
figure(1)
plot3(u,v,w,'o',u,v,w,'o');
%figure(2)
%scatter3(u,v,w,'*');
xlabel('x');
ylabel('y');
zlabel('z');
%plot3(u,v,w);
clear u v w;


function T=myfun1(theta,B0)
 alphad1=-pi/2; a1=0;d1=0;
 alphad2=0;  a2=0.80265;d2=0;%830*(m(1)+m(2)+m(3))/M
 alphad3=pi/2; a3=0;d3=0;
 alphad4=-pi/2; a4=0;d4=-0.700;
alphad5=pi/2;a5=0;d5=0;
 alphad6=0; a6=0;d6=0.3345;

for t=1:10
q = rand(1,4);
k = q/sum(q(:));
n=sqrt(abs(k(1)));
q1=-sqrt(abs(k(2)));
q2=sqrt(abs(k(3)));
q3=-sqrt(abs(k(4)));
A0=[q1^2-q2^2-q3^2+n^2 2*(q1*q2-q3*n) 2*(q1*q3+q2*n) B0(1)
     2*(q1*q2+q3*n) -q1^2+q2^2-q3^2+n^2 2*(q2*q3-q1*n) B0(2)
     2*(q1*q3-q2*n) 2*(q2*q3+q1*n) -q1^2-q2^2+q3^2+n^2 B0(3)
     0 0 0 1];
end
i=1;
A1=[cos(theta(i)),-sin(theta(i))*cos(alphad1),sin(theta(i))*sin(alphad1),a1*cos(theta(i));
    sin(theta(i)),cos(theta(i))*cos(alphad1),-cos(theta(i))*sin(alphad1),a1*sin(theta(i));
    0,sin(alphad1),cos(alphad1),d1;
    0,0,0,1];
i=2;
A2=[cos(theta(i)),-sin(theta(i))*cos(alphad2),sin(theta(i))*sin(alphad2),a2*cos(theta(i));
    sin(theta(i)),cos(theta(i))*cos(alphad2),-cos(theta(i))*sin(alphad2),a2*sin(theta(i));
    0,sin(alphad2),cos(alphad2),d2;
    0,0,0,1];
i=3;
A3=[cos(theta(i)),-sin(theta(i))*cos(alphad3),sin(theta(i))*sin(alphad3),a3*cos(theta(i));
    sin(theta(i)),cos(theta(i))*cos(alphad3),-cos(theta(i))*sin(alphad3),a3*sin(theta(i));
    0,sin(alphad3),cos(alphad3),d3;
    0,0,0,1];
i=4;
A4=[cos(theta(i)),-sin(theta(i))*cos(alphad4),sin(theta(i))*sin(alphad4),a4*cos(theta(i));
    sin(theta(i)),cos(theta(i))*cos(alphad4),-cos(theta(i))*sin(alphad4),a4*sin(theta(i));
    0,sin(alphad4),cos(alphad4),d4;
    0,0,0,1];
i=5;
A5=[cos(theta(i)),-sin(theta(i))*cos(alphad5),sin(theta(i))*sin(alphad5),a5*cos(theta(i));
    sin(theta(i)),cos(theta(i))*cos(alphad5),-cos(theta(i))*sin(alphad5),a5*sin(theta(i));
    0,sin(alphad5),cos(alphad5),d5
    0,0,0,1];
i=6;
A6=[cos(theta(i)),-sin(theta(i))*cos(alphad6),sin(theta(i))*sin(alphad6),a6*cos(theta(i));
    sin(theta(i)),cos(theta(i))*cos(alphad6),-cos(theta(i))*sin(alphad6),a6*sin(theta(i));
    0,sin(alphad6),cos(alphad6),d6;
    0,0,0,1];


T=A0*A1*A2*A3*A4*A5*A6;
 disp(A0) ;
  
end

function T=myfun(theta)

 alphad1=-pi/2; a1=0;d1=0;
 alphad2=0;  a2=0.830;d2=0;
 alphad3=pi/2; a3=0;d3=0;
 alphad4=-pi/2; a4=0;d4=-0.700;
alphad5=pi/2;a5=0;d5=0;
 alphad6=0; a6=0;d6=0.3345;


i=1;
A1=[cos(theta(i)),-sin(theta(i))*cos(alphad1),sin(theta(i))*sin(alphad1),a1*cos(theta(i));
    sin(theta(i)),cos(theta(i))*cos(alphad1),-cos(theta(i))*sin(alphad1),a1*sin(theta(i));
    0,sin(alphad1),cos(alphad1),d1;
    0,0,0,1];
i=2;
A2=[cos(theta(i)),-sin(theta(i))*cos(alphad2),sin(theta(i))*sin(alphad2),a2*cos(theta(i));
    sin(theta(i)),cos(theta(i))*cos(alphad2),-cos(theta(i))*sin(alphad2),a2*sin(theta(i));
    0,sin(alphad2),cos(alphad2),d2;
    0,0,0,1];
i=3;
A3=[cos(theta(i)),-sin(theta(i))*cos(alphad3),sin(theta(i))*sin(alphad3),a3*cos(theta(i));
    sin(theta(i)),cos(theta(i))*cos(alphad3),-cos(theta(i))*sin(alphad3),a3*sin(theta(i));
    0,sin(alphad3),cos(alphad3),d3;
    0,0,0,1];
i=4;
A4=[cos(theta(i)),-sin(theta(i))*cos(alphad4),sin(theta(i))*sin(alphad4),a4*cos(theta(i));
    sin(theta(i)),cos(theta(i))*cos(alphad4),-cos(theta(i))*sin(alphad4),a4*sin(theta(i));
    0,sin(alphad4),cos(alphad4),d4;
    0,0,0,1];
i=5;
A5=[cos(theta(i)),-sin(theta(i))*cos(alphad5),sin(theta(i))*sin(alphad5),a5*cos(theta(i));
    sin(theta(i)),cos(theta(i))*cos(alphad5),-cos(theta(i))*sin(alphad5),a5*sin(theta(i));
    0,sin(alphad5),cos(alphad5),d5
    0,0,0,1];
i=6;
A6=[cos(theta(i)),-sin(theta(i))*cos(alphad6),sin(theta(i))*sin(alphad6),a6*cos(theta(i));
    sin(theta(i)),cos(theta(i))*cos(alphad6),-cos(theta(i))*sin(alphad6),a6*sin(theta(i));
    0,sin(alphad6),cos(alphad6),d6;
    0,0,0,1];


T=A1*A2*A3*A4*A5*A6;
  
  
end

第5题比较简单，只是做一下简单的插值，这里使用的是5阶多项式插值

close all;
clear all;

q0(1)=1.5708;
q0(2)=-3.1416;
q0(3)=-1.5708;
q0(4)=0;
q0(5)=3.1416;
q0(6)=0;
qf(1)=1.5741;
qf(2)=1.9527;
qf(3)=0.4579;
qf(4)=1.5558;
qf(5)=3.1312;
qf(6)=1.5547;
t=[0,0.1,10];
a=Planning(q0,qf,10)

function a=Planning(q0,qf,t)
a=ones(6,6);
for i=1:6
    a(i,1)=q0(i);
    a(i,2)=0;
    a(i,3)=0;
    a(i,4)=(20*qf(i)-20*q0(i))/t^3/2;
    a(i,5)=(-30*qf(i)+30*q0(i))/t^4/2;
    a(i,6)=(12*qf(i)-12*q0(i))/t^5/2;
end
end
    
close all;
clear all;

t=0:0.1:10;
for i=1:101
    q1(i)=1.5708;
end
q2=-3.1416+0.0509*power(t,3)-0.0076*power(t,4)+0.0003*power(t,5);
q3=-1.5708+0.0203*power(t,3)-0.0030*power(t,4)+0.0001*power(t,5);
q4=0.0156*power(t,3)-0.0023*power(t,4)+0.0001*power(t,5);
q5=3.1416-0.0001*power(t,3);
q6=0.0155*power(t,3)-0.0023*power(t,4)+0.0001*power(t,5);

figure(1)
plot(t,q1)
 xlabel('时间');
 ylabel('关节角1');
 
 figure(2)
plot(t,q2)
 xlabel('时间');
 ylabel('关节角2');

figure(3)
plot(t,q3)
 xlabel('时间');
 ylabel('关节角3');

figure(4)
plot(t,q4)
 xlabel('时间');
 ylabel('关节角4');
 
 figure(5)
plot(t,q5)
 xlabel('时间');
 ylabel('关节角5');
 
 figure(6)
plot(t,q6)
 xlabel('时间');
 ylabel('关节角6');