这是《机器人技术基础》个人课程实验之一，按照学号尾数不同分配给每人的取放轨迹规划方式也不同，包括3-4-3、4-3-4等三段轨迹规划方法，而我抽中的是七次多项式实现，不存在优劣之分，特地说明一下。下面是详细内容：
一、开发平台及工具
开发平台：Matlab
开发工具：Robotics ToolBox
二、过程记录
2.1 选定建模对象
选择PUMA560作为建模对象：
2.2 选定转移过程点
主要是确定起始位置(取点)、转移位置(抬起或离开点)、卸下位置(下降点)和目标位置(放置点)；选定四个点位置坐标如下：
起始位置：aim0 = [0,-0.5,-0.5];
离开位置：aim1 = [0,-0.5,0.2];
下降位置：aim2 = [-0.5,0.5,0.2];
放置位置：aimx = [-0.5,0.5,-0.5];
2.3 整理边界条件
利用七次多项式进行轨迹规划：
(1)已知本地时间t = 0时，初始位置：
(2)已知本地时间t = 0时，初始速度：
(3)已知本地时间t = 0时，初始加速度：
(4)已知第一中间点位置：
(5)已知第二中间点位置：
(6)已知本地时间t = tm时，目标位置：
(7)已知本地时间t = tm时，目标速度：
(8)已知本地时间t = tm时，目标加速度：
由(1)~(8)可以写成如下矩阵形式：
或表示为：
和
通过计算 [M]-1 即可求出所有的未知系数，于是就求得了目标运动轨迹的运动方程，从而可以控制机器人使其经过给定的位置，同样的方法可用于其他关节求解。
2.4 求解转移过程点关节角
利用Matlab Robotics Box 对四个位置点求逆解得到关节角度：
theta0 = p560.ikine6s(T0,'rdf');%左臂、手肘朝下、手腕翻转(旋转180度)
theta1 = p560.ikine6s(T1,'rdf');
theta2 = p560.ikine6s(T2,'rdf');
thetax = p560.ikine6s(Tx,'rdf');
计算结果：
2.4 求解轨迹未知系数
%初始条件
theta0_ = [0 0 0 0 0 0];%初始位置速度
theta0__ = [0 0 0 0 0 0];%初始位置加速度
thetax_ = [0 0 0 0 0 0];%目标位置速度
thetax__ = [0 0 0 0 0 0];%目标位置加速度
Theta = [theta0' theta0_' theta0__' theta1' theta2' thetax' thetax_' thetax__']';
M = [1 0 0 0 0 0 0 0
0 1 0 0 0 0 0 0
0 0 2 0 0 0 0 0
1 t1 t1^2 t1^3 t1^4 t1^5 t1^6 t1^7
1 t2 t2^2 t2^3 t2^4 t2^5 t2^6 t2^7
1 tm tm^2 tm^3 tm^4 tm^5 tm^6 tm^7
0 1 2*tm 3*tm^2 4*tm^3 5*tm^4 6*tm^5 7*tm^6
0 0 2 6*tm 12*tm^2 20*tm^3 30*tm^4 42*tm^5];
C = M^-1 * Theta;%第i列对应第i个关节的其次多项式系数
各关节利用七次多项式进行轨迹规划的系数列表如下：
2.5 求解各关节相关运动指标
计算关节的位置、速度及加速度轨迹函数：
%计算关节各函数
tmietick = 0.1;
T = 0: tmietick:9;
%角度
Q = [ones(int16(9/tmietick)+1,1) T' (T.^2)' (T.^3)' (T.^4)' (T.^5)' (T.^6)' (T.^7)']*C;
%速度
Qv =[zeros(int16(9/tmietick)+1,1) ones(int16(9/tmietick)+1,1) 2* T' 3*(T.^2)' 4*(T.^3)' 5*(T.^4)' 6*(T.^5)' 7*(T.^6)']*C;
%加速度
Qa =[zeros(int16(9/tmietick)+1,1) zeros(int16(9/tmietick)+1,1) 2*ones(int16(9/tmietick)+1,1) 6*T' 12*(T.^2)' 20*(T.^3)' 30*(T.^4)' 42*(T.^5)']*C;
各关节运动位置、速度及加速度轨迹曲线绘制如下：
2.6 绘制运动轨迹
%正运动学分析
Txy=p560.fkine(Q);
%画轨迹
Tjtraj1=transl(Txy);
x = Tjtraj1(:,1);
y = Tjtraj1(:,2);
z = Tjtraj1(:,3);
figure
waitforbuttonpress;
plot3(x,y,z,'b');%轨迹图像
hold on;
%画出四个过程点
[x0,y0,z0] = ellipsoid(aim0(1),aim0(2),aim0(3),0.05,0.05,0.05);
[x1,y1,z1] = ellipsoid(aim1(1),aim1(2),aim1(3),0.05,0.05,0.05);
[x2,y2,z2] = ellipsoid(aim2(1),aim2(2),aim2(3),0.05,0.05,0.05);
[xx,yx,zx] = ellipsoid(aimx(1),aimx(2),aimx(3),0.05,0.05,0.05);
surf(x0,y0,z0) %画起始点
surf(x1,y1,z1) %画提升点
surf(x2,y2,z2) %画下降点
surf(xx,yx,zx) %画目标点
hold on;
三、思考
由上图可以明显发现7次多项式进行拟合规划的轨迹为一空间不规则曲线，原因在于我们是对机械臂6个关节进行独立的7次多项式轨迹规划，已知条件只有初末位置的关节位置、速度及加速度以及两个中间点的关节位置共8个参数，同时由于4个位置点的各关节角度值是由ikine6s进行运动学逆解求解得到，虽然在计算时限制了手臂、手肘以及手腕的状态，但是这样得到逆解并一定能满足我们的实际需求，因此在仅存在这些已知条件的情况下，我们没有办法对整个长距离的机器人的运动状态进行精确控制。因此，要想机器人的运动状态足够精确，我必须引入更多的条件，比如增加中间点的数量或者各时刻的关节速度等等。
四、附源码
close all;
clc;
mdl_puma560
t0 = 0;%开始时刻
t1 = 2;%提升结束时刻
t2 = t1 + 4;%平移结束时刻
tm = t2 + 3;%下降结束时刻
t0_1 = 0:0.2:2;%上升时间
t1_2 = 0:0.5:4;%平移时间
t2_x = 0:0.3:3;%下降时间
aim0 = [0,-0.5,-0.5];%取货点
aim1 = [0,-0.5,0.2];%提升点
aim2 = [-0.5,0.5,0.2];%下落点
aimx = [-0.5,0.5,-0.5];%存货点
T0 = transl(aim0);
T1 = transl(aim1);
T2 = transl(aim2);
Tx = transl(aimx);
theta0 = p560.ikine6s(T0,'rdf');%左臂、手肘朝下、手腕翻转(旋转180度)
theta1 = p560.ikine6s(T1,'rdf');
theta2 = p560.ikine6s(T2,'rdf');
thetax = p560.ikine6s(Tx,'rdf');
%初始条件
theta0_ = [0 0 0 0 0 0];%初始位置速度
theta0__ = [0 0 0 0 0 0];%初始位置加速度
thetax_ = [0 0 0 0 0 0];%目标位置速度
thetax__ = [0 0 0 0 0 0];%目标位置加速度
Theta = [theta0' theta0_' theta0__' theta1' theta2' thetax' thetax_' thetax__']';
M = [1 0 0 0 0 0 0 0
0 1 0 0 0 0 0 0
0 0 2 0 0 0 0 0
1 t1 t1^2 t1^3 t1^4 t1^5 t1^6 t1^7
1 t2 t2^2 t2^3 t2^4 t2^5 t2^6 t2^7
1 tm tm^2 tm^3 tm^4 tm^5 tm^6 tm^7
0 1 2*tm 3*tm^2 4*tm^3 5*tm^4 6*tm^5 7*tm^6
0 0 2 6*tm 12*tm^2 20*tm^3 30*tm^4 42*tm^5];
C = M^-1 * Theta;%第i列对应第i个关节的其次多项式系数
%计算关节各函数
tmietick = 0.1;
T = 0: tmietick:9;
%角度
Q = [ones(int16(9/tmietick)+1,1) T' (T.^2)' (T.^3)' (T.^4)' (T.^5)' (T.^6)' (T.^7)']*C;
%速度
Qv =[zeros(int16(9/tmietick)+1,1) ones(int16(9/tmietick)+1,1) 2* T' 3*(T.^2)' 4*(T.^3)' 5*(T.^4)' 6*(T.^5)' 7*(T.^6)']*C;
%加速度
Qa =[zeros(int16(9/tmietick)+1,1) zeros(int16(9/tmietick)+1,1) 2*ones(int16(9/tmietick)+1,1) 6*T' 12*(T.^2)' 20*(T.^3)' 30*(T.^4)' 42*(T.^5)']*C;
%正运动学分析
Txy=p560.fkine(Q);
%画轨迹
Tjtraj1=transl(Txy);
x = Tjtraj1(:,1);
y = Tjtraj1(:,2);
z = Tjtraj1(:,3);
figure
waitforbuttonpress;
plot3(x,y,z,'b');%轨迹图像
hold on;
%画出四个过程点
[x0,y0,z0] = ellipsoid(aim0(1),aim0(2),aim0(3),0.05,0.05,0.05);
[x1,y1,z1] = ellipsoid(aim1(1),aim1(2),aim1(3),0.05,0.05,0.05);
[x2,y2,z2] = ellipsoid(aim2(1),aim2(2),aim2(3),0.05,0.05,0.05);
[xx,yx,zx] = ellipsoid(aimx(1),aimx(2),aimx(3),0.05,0.05,0.05);
surf(x0,y0,z0) %画起始点
surf(x1,y1,z1) %画提升点
surf(x2,y2,z2) %画下降点
surf(xx,yx,zx) %画目标点
hold on;
%画轨迹图
p560.plot(Q);
%画关节位置、速度、加速度曲线
figure
subplot(3,1,1);
% plot(T,Q(:,1));
plot(T,Q);
title('关节位移');
xlabel('时间t/s');
ylabel('位移s/rad');
legend('关节1','关节2','关节3','关节4','关节5','关节6','location','northeastoutside' );
str=[ '\leftarrow' '(' num2str(t1) ',' num2str(theta1(1)) ')'];
text(t1,theta1(1),cellstr(str));
str=[ '\leftarrow' '(' num2str(t2) ',' num2str(theta2(1)) ')'];
text(t2,theta2(1),cellstr(str));
grid on;
subplot(3,1,2);
plot(T,Qv);
title('关节速度');
xlabel('时间t/s');
ylabel('速度v/(rad/s)');
legend('关节1','关节2','关节3','关节4','关节5','关节6','location','northeastoutside' );
grid on;
subplot(3,1,3);
plot(T,Qa);
title('关节加速度');
xlabel('时间t/s');
ylabel('加速度a/(rad/s^2)');
legend('关节1','关节2','关节3','关节4','关节5','关节6','location','northeastoutside' );
grid on;