连续轨迹控制(CP)

1. 连续轨迹控制概念

工业机器人的运动控制方式可以分为点到点控制(PTP)与连续轨迹控制(CP)¹。如下图所示。

PTP控制下，始末速度为0，期间可以有各种的速度规划方式。
CP控制下，中间点的速度不为0，连贯运动，通过速度前瞻的方式获得每个点的速度大小。

一般连续轨迹控制主要都用到了速度前瞻的方法。

2. 速度前瞻

速度前瞻需要考虑：

• 前向速度限制
• 转角速度限制
• 回溯速度限制
• 最大速度限制
• 轮廓误差速度限制（选用）

2.1 前向速度限制

如图所示，设线段$i$的起始速度为$V_{i-1}$,机器人在线段$i$上做匀加速运动，则在线段$i$末尾的终点速度就有：
$$\begin{array}{r}{V}_i^2=V_{i-1}^2+2a_m{l}_i\end{array}\text{\hspace{1em}(2.1)}$$
$a_m$:机器人运动允许的最大加速度
$l_i$:线段$i$的长度

2.2 转角速度限制

第$i$段与第$i+1$段之间存在夹角。机器人在转角处的速度矢量改变不能超出加速度的限制。机器人在第$i$段的最后一个插补周期的速度方向与$i$相同，在第$i+1$段的第一个插补周期的速度方向与$i+1$相同，所以机器人要在一个插补周期$T$内完成速度方向的变化。在这两段衔接速度的大小绝对值不发生变化，设$u_i$为线段$i$的速度单位矢量，$u_{i+1}$是线段$i+1$的速度单位矢量，则有：
$$\begin{array}{rl}& \Vert V_i{u}_{i+1}-V_i{u}_i\Vert \le a_{m}T\\ & 即：2V_{i}sin\frac{a_i}{2}\le a_{m}T\end{array}\text{\hspace{1em}(2.2)}$$
上图中 $1,2$分别为2个速度转角线段衔接点，衔接点速度大小分别为$V_i、V_{i+1}$。$a_m$为最大加速度，$T$为插补周期，$a_i$为两个速度矢量之间的夹角。

2.3 回溯速度限制

如图所示，由于存在减速距离产生的约束第$i$段线段与第$i+1$段互相衔接，第$i$段的末速度大小等于第$i+1$段的初速度。因此从$V_i$减速到$V_{i+1}$有：

$$\begin{array}{r}{V}_i^2\le V_{i+1}^2+2a_m{l}_{i+1}\end{array}\text{\hspace{1em}(2.3)}$$
$l_{i+1}$:线段$i+1$的长度

2.4 最大速度限制

机器人在运动过程中速度不能超过最大限制速度$V_{max}$

2.5 轮廓误差速度限制（选用）

关于轮毂误差的描述参考^4-6
轮廓误差常用在机床上，因此选用。
理论上，机床运动在周期T内速度方向在衔接点C改变，详情见2.2.
特别地，机床当加工轨迹不经过衔接点C后，加工误差就会产生。见下图，e为实际路径与理论路径的误差值。$\theta$为转角大小。由于$AC=BC$，ABC为等腰三角形，因此$2\alpha =\theta$。

$$\begin{array}{r}{V}_{i}T\le \frac{2e}{tan\frac{\theta }{2}}\end{array}\text{\hspace{1em}(2.4)}$$
在周期T内，机床的运动由A运动到B，速度为$V_i$，所以运动距离$V_{i}T\le AB$，而$AB=\frac{2e}{tan\alpha },\alpha =\frac{\theta }{2}$.

综上所述速度$V_i$应该满足的条件有：
$$\{\begin{array}{ll}{V}_i\le \sqrt{V_{i-1}^2+2a_m{l}_i}& (1)\\ 2V_{i}sin\frac{a_i}{2}\le a_{m}T& (2)\\ V_i\le \sqrt{V_{i+1}^2+2a_m{l}_{i+1}}& (3)\\ V_i\le V_{max}& (4)\\ V_i\le \frac{2e}{Ttan\frac{\theta }{2}}& (5^{\ast })选用\end{array}$$
最后 $V_i=min(1,2,3,4)$。

章节总结
上面式(1)是与前一段线段的运动情况有关，式(2)是与转角衔接点速度有关，式(3)与后一段线段的运动情况速度有关。基于前一段的运动情况已知的情况下，为了求得本段的末速度需要前瞻多段路径，回溯速度不应该只考虑后一段，需要考虑多段路径，也就是前瞻多段路径。
原因：以防由于前瞻路径长度太短，导致机器人无法在下一段路径中停止运动，速度为0.

3. 回溯速度补充

在上一章的总结里，得知为了让机器人能够在后面的路径停下，需要前瞻多段路径，而非一段，因此回溯速度限制应该需要将多段路径考虑在内，并且需要考虑后续段的转角速度约束。
假设在第$i$段后面$x$段路径都没有角度约束，那么当这$x$段的总长度满足公式：

$$\sum _{j=i}^{i+x}{l}_j\ge \frac{v_{max}^2}{2a_m}\text{\hspace{1em}(3.1)}$$

式中$l_j$是第$j$段的长度。从第$i$段开始，速度可以从$V_{max}$一直减速到0，回溯速度约束只考虑后面的$i+x$段。 关于前瞻$x$段的距离$S=\frac{v_{max}^2}{2a_m}$，在得到S后，得知每段线段的长度$l$，可以得到前瞻$x$段 $x=\frac{S}{l}$。

最终关于速度前瞻算法流程如下
1. 设前瞻$x$段的末速度为0,根据回溯速度限制、转角速度限制和最大速度限制3个约束条件计算出第$x-1$段路径的末速度大小
2. 循环第1个步骤计算出当前段的速度约束
3. 计算由加速距离产生的前向速度限制
4. 取约束速度的最小值最为本段末速度

4. 速度规划

4.1 速度规划图

根据初速度$V_b$、末速度$V_e$与最大速度$V_m$的关系，可以将线段的速度规划分为以下7种情况。如下图所示

(1) 全程加速才能达到末速度$V_e$,其中$V_e\le V_m$。
(2) 全程减速才能达到末速度$V_e$。
(3) 不需要全程加速，有匀加速度和匀速段达到$V_e$
(4) 先匀速再减速达到末速度$V_e$，应对$V_e=0$的情况，不可以先减速再匀速，对²该文章的速度规划进行了优化。
(5) 末速度$V_e=V_m$,先加速再匀速运动。
(6) 末速度小于最大限制速度，先加速到$V_{max}$再匀速，最后减速到末速度$V_e$。
(7) 面对起始速度和末速度均为0的情况下，先加速再减速运动。

4.2 算法实现

1. 假设机器人在本路径由初速度$V_0$加速到最高限制速度$V_{max}$再减速到期望的末速度$V_e$。计算此条件下走过的长度$l_e,l_e=\frac{1}{2}(2V_{max}^2-V_0^2-V_e^2)/a_m$
2. 假设机器人由初速度$V_0$全程加速或减速到末速度V_e, 计算此条件下走过的长度 $l_i,l_i=\frac{\Vert V_0^2-V_e^2\Vert }{2a_m}$
3. 判断本段的加减速情况。
   3.1 比较本段的实际长度$l与l_e$，如果$l\ge l_e$且末速度小于最大限制速度$V_{max}$，说明本段的长度足够加速到$V_{max}$然后减速到末速度，本段路径属于第6种加减速情况。
   3.2 如果 $l\ge l_e$ 且末速度等于最大限制速度$V_{max}$，说明需要加速到$V_{max}$再匀速运动，属于第5种加减速情况。
   如果 $l<l_e$ 说明线段长度不满足机器人加速到$V_{max}$，则继续判断
   3.3 如果 $l>l_i$ 且 $V_0>V_e$ ，说明机器人由初速度 $V_0$ 减速到末速度 $V_e$ 本段还有剩余长度，属于第4种情况。
   3.4 如果 $l>l_i$ 且 $V_0<V_e$ ，说明机器人由初速度 $V_0$ 加速到末速度 $V_e$ 本段还有剩余长度，属于第3种情况。
   3.5 如果 $l=l_i$ 且 $V_0>V_e$ ，说明机器人由初速度 $V_0$ 减速到末速度 $V_e$ 本段没有剩余长度，属于第2种情况。
   3.6 如果 $l=l_i$ 且 $V_0<V_e$ ，说明机器人由初速度 $V_0$ 加速到末速度 $V_e$ 本段没有剩余长度，属于第1种情况。
   3.7 如果 $l>l_i$ 且 $V_0=V_e=0$, 属于特殊情况，为第7种情况。
   如果出现 $l<l_i$ ，说明全程加速或者减速都达不到规划的末速度，说明速度前瞻过程中计算末速度的时候出现了错误，这时候需要让机器人停下。

5. 参考

• [1] 连续轨迹控制介绍

• [2] 基于ARM_FPGA的激光雕刻机控制系统的研发_徐宝腾.2014

• [3] 嵌入式运动控制器若干关键技术的研究与实现_东梁

• [4] 连续轨迹动态前瞻插补算法研究_陈日东

• [5] 基于DSP+FPGA的开放式机器人运动控制器的研究

• [6] 基于速度前瞻的数控系统连续轨迹控制算法设计及实现_魏延栋