六自由度机器人（机械臂）运动学建模及运动规划系列（四）——轨迹规划

一、轨迹规划概述

对机器人进行轨迹规划的主要任务是，根据机器人的工作环境和工作任务要求，求得一系列机器人末端位姿变换的时间序列，使得机器人末端可以正确地从初始姿态沿着期望的轨迹运动到终止位姿，完成工作任务，。对于六自由度的机器人来说，轨迹规划要解决的关键问题是将末端位姿的变换转变为六个关节变量的变换，这也是其与移动机器人轨迹规划的不同之处。
进行轨迹规划的方法有两种：一种是在关节空间进行规划，另一种是在笛卡尔空间，即直角坐标空间进行规划。
（1）关节空间规划是通过确定各个关节转角变化序列，从而规划机械臂末端的空间运动轨迹。其优点是只用对六个关节变量分别进行规划，计算简单，但不能精确地控制机械臂末端的位姿变化。
（2）笛卡尔空间规划就是对机械臂末端的位姿变化直接进行规划，在得到机械臂末端在某一时刻的位姿后，利用逆运动学方程直接求出此刻各个关节转角变量的值。其优点是机械臂末端的轨迹易于观察，可以更精确控制地控制末端轨迹，同时避免末端执行器与空间障碍物发生碰撞。但缺点是需要进行大量的逆运动学计算，并且需要选取大量的中间插值点以得到精确的末端轨迹，控制过程复杂。

二、关节空间规划

机器人关节空间规划一般分为点对点规划和多节点规划。

1. 点对点规划

当给定起始点和终止点的约束条件，需要求出连接两点的轨迹时，这样的问题称为点对点轨迹规划。常用的点对点轨迹规划方法有三次多项式规划，五次多项式规划等。这里采用五次多项式进行规划，其一般形式为：

六个未知数需要给定六个约束条件，分别为起点和终点的角位移，角速度和角加速度。约束条件的方程为：

求解方程组，得到六个未知数，其中T=（te-ts）。

将参数代入到五次多项式中，得到所求轨迹。

2. 多节点规划

当给定除起点和终点外，若干个中间点的约束条件，需要求出经过所有点的轨迹时，这样的问题称为多节点轨迹规划。进行多节点规划时，可以将多节点分段，每段有两个节点，对每段分别进行点对点规划。即多节点规划可以看作多段点对点规划的组合。
为了保证机器人平稳运行，在多节点规划时，一种有效的方法是使中间连接点处的角位移，角速度和角加速度相同。在这种情况下依然可以采用五次多项式规划。只需要给出每个节点处的角位移，角速度和角加速度条件，按照上一节中的方法，求出每段轨迹的六个参数，代入轨迹方程中，即可得到所求轨迹。

示例程序

这里对六个关节角进行多节点规划，一共给定四个节点的角度，角速度和角加速度条件，其中一段的规划程序如下：

for i=1:3  %每一段规划的时间
     T=t_array(i+1)-t_array(i);
     a10=q1_array(i);
     a11=v_array(i);
     a21=a_array(i)/2;
     a31=(20*q1_array(i+1)-20*q1_array(i)-(8*v_array(i+1)+12*v_array(i))*T-(3*a_array(i)-a_array(i+1))*T^2)/(2*T^3);
     a41=(30*q1_array(i)-30*q1_array(i+1)+(14*v_array(i+1)+16*v_array(i))*T+(3*a_array(i)-2*a_array(i+1))*T^2)/(2*T^4);
     a51=(12*q1_array(i+1)-12*q1_array(i)-(6*v_array(i+1)+6*v_array(i))*T-(a_array(i)-a_array(i+1))*T^2)/(2*T^5);%计算五次多项式系数 
     ti=t_array(i):0.01:t_array(i+1);
     qi=a10+a11*(ti-t_array(i))+a21*(ti-t_array(i)).^2+a31*(ti-t_array(i)).^3+a41*(ti-t_array(i)).^4+a51*(ti-t_array(i)).^5;
     vi=a11+2*a21*(ti-t_array(i))+3*a31*(ti-t_array(i)).^2+4*a41*(ti-t_array(i)).^3+5*a51*(ti-t_array(i)).^4;
     ai=2*a21+6*a31*(ti-t_array(i))+12*a41*(ti-t_array(i)).^2+20*a51*(ti-t_array(i)).^3;
     t=[t,ti(2:end)];q=[q,qi(2:end)];v1=[v1,vi(2:end)];a1=[a1,ai(2:end)];
end

角度规划结果如图所示

完整的示例代码如下所示：
四节点五次多项式规划完成代码。

三、笛卡尔空间规划

在进行机器人笛卡尔空间轨迹规划时，一般的方法为：
（1）根据机械臂末端起点和终点的轨迹，确定一条从起点到终点的路径。
（2）确定插补点的个数，进而确定路径上的若干中间点。
（3）由一系列的机械臂末端坐标进行逆运动学计算，求解出六个关节转角的变化情况。
这种方法称为数值解法。
主要涉及到三个方面的规划：

1. 速度规划

由力学原理可知，机器人在运动过程中如果出现加速度突变的情况，会产生机械冲击，导致机器人关节磨损。因此在进行轨迹规划时，首先需要对各关节进行速度规划，保证运行过程中加速度连续变化。
S曲线是适用于此种情况的速度曲线，整段曲线由抛物线和直线段组成，对应的加速度曲线连续。S曲线一般有三段，五段和七段S曲线三种，七段S曲线包括七个阶段，其在满足加速度连续的基础上，还有两端加速度恒定的过程，运动更加平稳。七段S曲线的计算方法采用分类讨论法，示意曲线为：

除了S曲线外，也可以使用多项式曲线等等。

2. 位置规划

位置规划指对机械臂末端的轨迹进行规划，一般采用直线插补和圆弧插补以及这两种的组合进行规划。
（1）直线插补
目的是使机械臂末端的轨迹为一段直线，具体步骤非常简单：只需将起点与终点直线连接，然后根据所需的精度确定插补点的个数，最后确定每一个插补点的坐标即可。
（2）圆弧插补
由于空间中的圆弧轨迹插补比较复杂，因此可以采用这样的步骤：
1）首先根据起点终点与圆心确定一个新的平面；
2）接着以建立的平面为基准，建立一个新的坐标系；
3）确定新坐标系与末端坐标系之间的转换关系；
4）在新坐标系里完成平面圆弧插补，确定每个插补点的坐标；
5）根据坐标转化关系，得到末端坐标下的三维插补点坐标。
直线插补和圆弧插补的运动示意图如下：

3. 姿态插补

在机器人运行过程中，有些场景的末端姿态不发生变化，如打磨等，但在执行较复杂的工作时，末端姿态在运动过程中也会变化，如复杂的焊接过程等等。姿态规划的任务时，根据机械臂末端起始点和终止点的姿态，规划出合理的姿态变化过程，以满足特定的约束条件（如避障，工作任务约束等）。
规划时，一种方法是采用四元数进行规划，具体方法我当时没有具体研究，有需要的小伙伴可以自行探索。

4. 基于几何解法的一种简化位置规划方法

前文介绍的位置插补规划方法原理简单，但缺点是需要进行大量的逆运动学计算。这里根据机器人运动过程的特殊性，提出了一种几何求解法，对问题求解进行了简化。
（1）首先讨论平面内的竖直运动过程：

由正运动学分析可知，关节转角θ1只与机械臂末端的横纵坐标有关，因此在竖直运动过程中θ1保持不变。另外，在运动过程中，末端的姿态保持不变，由机器人关节的结构可知，关节转角θ4与θ6为定值。经过以上分析，竖直方向的运动只需研究剩下的三个关节转角的变化，这三个转角的相对关系如上图所示。
图中定义了两个角度α1,α2及一个长度l，当起始点和终止点给定时，l为定值。他们与关节转角θ2,θ3,θ5的关系如下式所示：


得到以上关系后，进行规划时，只需对起点和终点的末端坐标进行两次逆运动学计算，得到其中一个关节角的起点和终点坐标，接着对其进行插补，得到中间插补点的角度，最后根据关节角之间的关系，得到其他关节角的一系列角度。
最后得到的杆件仿真图如下：

（2）平面内的水平直线运动

图中定义了一个高度差∆h，当轨迹起始点和终止点给定，式中∆h为定值。同样的，运动过程中，关节角的关系为：


通过计算得到的杆件仿真图如下：

将这两种运动与关节1的运动组合，就可以形成不同的末端轨迹，以实现简单运动情况下的位置规划。
具体的代码如下所示：
基于简化几何解法的位置规划，包括水平直线运动和竖直直线运动

5.两种规划的Matlab程序

Matlab提供了jtraj和ctraj两种函数，分别用于笛卡尔空间以及关节空间的轨迹规划。两者的使用格式分别为：
（1）关节空间规划

[q ,qd, qdd]=jtraj(q1,q2,50)

给定q1，q2分别为起始点和终止点的关节角，50为插补点个数。得到的q，qd，qdd，分别为关节角度，角速度以及角加速度系列。
（2）笛卡尔空间规划

Tc=ctraj(p1,p2,step)

给定p1，p2为起始点和终止点的四乘四位姿矩阵，step为插补点个数，得到的Tc为一系列中间姿态矩阵。
这里利用两个函数，进行一个简单的规划示例：

clear;
clc;
%建立机器人模型
L1=Link([0       0.4      0.025    pi/2      0     ]); 
L2=Link([pi/2    0        0.56     0         0     ]);
L3=Link([0       0        0.035    pi/2      0     ]);
L4=Link([0       0.515    0        pi/2      0     ]);
L5=Link([pi      0        0        pi/2      0     ]);
L6=Link([0       0.08     0        0         0     ]);
robot=SerialLink([L1 L2 L3 L4 L5 L6],'name','kaw');
T1=transl(0.5,0,0);%起始点位姿
T2=transl(0,-0.5,0);%终止点位姿
q1=robot.ikine(T1);%起始点关节角
q2=robot.ikine(T2);%终止点关节角

%关节空间顾规划
[q ,qd, qdd]=jtraj(q1,q2,50); 
T=robot.fkine(q);%根据插值，得到末端执行器位姿
figure(1)
robot.plot(q);%动画演示

%笛卡尔空间顾规划
Tc=ctraj(T1,T2,20);
for i=1:20
    qc(i,:)=robot.ikine(Tc(:,:,i));
end
figure(2)
robot.plot(qc);

四、小结

本篇介绍了六轴机器人关节空间以及笛卡尔空间轨迹规划的简单方法，并给出了一种几何解法，以简化规划过程，并提供了简单的Matlab轨迹规划函数介绍。