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

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

### 三次多项式

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

![](https://3446561902-files.gitbook.io/~/files/v0/b/gitbook-legacy-files/o/assets%2F-MY8uIyErFkHsfVSgyXy%2F-MZL8lbA45yA_Phw63Mh%2F-MZLD0DApNLc7ALOVTKX%2Fimage.png?alt=media\&token=345d215b-85ae-49d5-8dbf-e2ac4dcab877)

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

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

1. 起始(t=0)时的角度为q\_s
2. 起始(t=0)时的速度为0
3. 终止(t=t\_n)时的角度为q\_f
4. 终止(t=t\_n)时的速度为0

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

![](https://3446561902-files.gitbook.io/~/files/v0/b/gitbook-legacy-files/o/assets%2F-MY8uIyErFkHsfVSgyXy%2F-MZL8lbA45yA_Phw63Mh%2F-MZLFlP1BxgyWo8_6Jq1%2Fimage.png?alt=media\&token=abd4d668-a6a2-4f4f-9433-c3c09fbee51e)

可列成矩阵形式：

![](https://3446561902-files.gitbook.io/~/files/v0/b/gitbook-legacy-files/o/assets%2F-MY8uIyErFkHsfVSgyXy%2F-MZL8lbA45yA_Phw63Mh%2F-MZLGaxhpFZbG9W22Q_Z%2Fimage.png?alt=media\&token=daa7b8a2-e3db-4fe0-b899-f26e1fe385c3)

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

二、如果轨迹规划除了起始点和终止点外，还有若干个中间点(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的形式，其中中间四个边界条件是不断循环的，将上述边界条件写成矩阵形式便可看出规律：

![](https://3446561902-files.gitbook.io/~/files/v0/b/gitbook-legacy-files/o/assets%2F-MY8uIyErFkHsfVSgyXy%2F-MZL8lbA45yA_Phw63Mh%2F-MZLPsdIoiJqkiUr3GhJ%2Fimage.png?alt=media\&token=614d011c-3020-444d-97bd-492e93b14282)

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

例子:&#x20;

1. 当t=0时，初始角度为1
2. 当t=1时，角度为3
3. 当t=3时，角度为4
4. 当t=4时，终止角度为2

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

![](https://3446561902-files.gitbook.io/~/files/v0/b/gitbook-legacy-files/o/assets%2F-MY8uIyErFkHsfVSgyXy%2F-MZL8lbA45yA_Phw63Mh%2F-MZLSdvc5pfYWxAovwvY%2Fimage.png?alt=media\&token=0560985d-7b2e-46d4-8d3e-5c66248895c3)

![](https://3446561902-files.gitbook.io/~/files/v0/b/gitbook-legacy-files/o/assets%2F-MY8uIyErFkHsfVSgyXy%2F-MZL8lbA45yA_Phw63Mh%2F-MZLSi9iBlvUZkkLunTT%2Fimage.png?alt=media\&token=ab3c32f1-9836-4479-93fb-1a9c951e7c8a)

![](https://3446561902-files.gitbook.io/~/files/v0/b/gitbook-legacy-files/o/assets%2F-MY8uIyErFkHsfVSgyXy%2F-MZL8lbA45yA_Phw63Mh%2F-MZLSjClLHe5yfeToUuj%2Fimage.png?alt=media\&token=ed825207-1820-4c51-a12a-ab274ce4a5b3)

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

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

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

![](https://3446561902-files.gitbook.io/~/files/v0/b/gitbook-legacy-files/o/assets%2F-MY8uIyErFkHsfVSgyXy%2F-MZL8lbA45yA_Phw63Mh%2F-MZLSncToPciTS-eOCQm%2Fimage.png?alt=media\&token=5ce2550a-df84-4e28-856e-49124af5935f)

![](https://3446561902-files.gitbook.io/~/files/v0/b/gitbook-legacy-files/o/assets%2F-MY8uIyErFkHsfVSgyXy%2F-MZL8lbA45yA_Phw63Mh%2F-MZLSpL6Ej_Gnk8ENSE5%2Fimage.png?alt=media\&token=3eb6ecd1-d189-49ca-9850-30e9e3226c4b)

![](https://3446561902-files.gitbook.io/~/files/v0/b/gitbook-legacy-files/o/assets%2F-MY8uIyErFkHsfVSgyXy%2F-MZL8lbA45yA_Phw63Mh%2F-MZLSrFPo3D4cN0wZjOM%2Fimage.png?alt=media\&token=cf634c63-4251-4495-8317-529c4c70d773)
