机械臂轨迹生成算法 / Algorithm of trajectory planning for manipulator

机械臂轨迹生成是指机械臂在指定时间内，以指定速度和加速度到达指定位置的一种运动规划。目前常见的方法有梯形规划、三次方多项式、五次方多项式、B样条曲线等。此页面将着重介绍本人三次方多项式的推导过程，与一种全新的自研可适用多点约束的轨迹生成算法。

三次多项式

三次多项式轨迹生成拟合法可通用于关节空间和笛卡尔空间中，它指在两个点之间拟合出一条三次方多项式:

一个三次方多项式中含有四个未知参数，需要四个边界条件进行约束。

一、如果轨迹规划的唯一两个点为起始点和终止点的情况下，四个边界条件为：

1. 起始(t=0)时的角度为q_s

2. 起始(t=0)时的速度为0

3. 终止(t=t_n)时的角度为q_f

4. 终止(t=t_n)时的速度为0

将四个边界条件带入多项式中可得四个等式：

可列成矩阵形式：

从而可求得想要得到的三次方多项式曲线。

二、如果轨迹规划除了起始点和终止点外，还有若干个中间点(way points)的情况下，其计算难度大幅度提升，任意两个点之间还需要四个边界条件进行约束，以2个中间点为例，此轨迹规划共有4个点，需要规划3条不同的三次方多项式曲线，共12个边界条件。

其边界条件可列如下：

1. s1(0) = q_s

2. s1'(0) = 0

3. s1(t1) = qv1

4. s2(0) = qv1

5. s1'(t1) = s2'(0)

6. s1''(t1) = s2''(0)

7. s2(t2) = qv2

8. s2(0) = qv2

9. s2'(t2) = s3'(0)

10. s2''(t2) = s3''(0)

11. s3(t3) = q_f

12. s3'(t3) = 0

可见中间点数量N=2时，约束条件可写为2+4N+2的形式，其中中间四个边界条件是不断循环的，将上述边界条件写成矩阵形式便可看出规律：

蓝色部分为起始点和末端点的位置、速度条件，其中间各点的位置已知，速度、加速度连续，可提供可循环的四个边界条件。

例子:

1. 当t=0时，初始角度为1

2. 当t=1时，角度为3

3. 当t=3时，角度为4

4. 当t=4时，终止角度为2

利用上述方法得到三次方曲线，位置、速度、加速度分别为：

多点约束可适应轨迹生成算法

从三次方多项式可看出其加速度为直线，对机械臂控制不利，且当中间点增多后上述计算变得格外复杂。为了解决上述缺点，一种多点约束可适应轨迹生成算法是非常重要的。目前该算法正在进行专利申请，具体过程专利申请完成后再进行公开，谢谢理解~

和三次方多项式相同的例子，用该算法跑出的结果如下，可看出角度及速度基本一致，但是加速度变得连续，有利于机械臂的控制：