这是《机器人技术基础》个人课程实验之一，按照学号尾数不同分配给每人的取放轨迹规划方式也不同，包括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;