1. 连续轨迹控制概念
工业机器人的运动控制方式可以分为点到点控制(PTP)与连续轨迹控制(CP)1。如下图所示。
PTP控制下,始末速度为0,期间可以有各种的速度规划方式。
CP控制下,中间点的速度不为0,连贯运动,通过速度前瞻的方式获得每个点的速度大小。
一般连续轨迹控制主要都用到了速度前瞻的方法。
2. 速度前瞻
速度前瞻需要考虑:
- 前向速度限制
- 转角速度限制
- 回溯速度限制
- 最大速度限制
- 轮廓误差速度限制(选用)
2.1 前向速度限制
如图所示,设线段
i
i
i的起始速度为
V
i
−
1
V_{i-1}
Vi−1,机器人在线段
i
i
i上做匀加速运动,则在线段
i
i
i末尾的终点速度就有:
V
i
2
=
V
i
−
1
2
+
2
a
m
l
i
(2.1)
\begin{aligned} V_i^2 = V_{i-1}^2 + 2a_ml_i \tag{2.1} \end{aligned}
Vi2=Vi−12+2amli(2.1)
a
m
a_m
am:机器人运动允许的最大加速度
l
i
l_i
li:线段
i
i
i的长度
2.2 转角速度限制
第
i
i
i段与第
i
+
1
i+1
i+1段之间存在夹角。机器人在转角处的速度矢量改变不能超出加速度的限制。机器人在第
i
i
i段的最后一个插补周期的速度方向与
i
i
i相同,在第
i
+
1
i+1
i+1段的第一个插补周期的速度方向与
i
+
1
i+1
i+1相同,所以机器人要在一个插补周期
T
T
T内完成速度方向的变化。在这两段衔接速度的大小绝对值不发生变化,设
u
i
\pmb{u_i}
ui为线段
i
i
i的速度单位矢量,
u
i
+
1
\pmb{u_{i+1}}
ui+1是线段
i
+
1
i+1
i+1的速度单位矢量,则有:
∥
V
i
u
i
+
1
−
V
i
u
i
∥
≤
a
m
T
即:
2
V
i
s
i
n
a
i
2
≤
a
m
T
(2.2)
\begin{aligned} &\left \| V_i\pmb{u_{i+1} } - V_i\pmb{u_i}\right \| \leq a_mT \tag{2.2}\\ &即:2V_isin\frac{a_i}{2}\leq a_mT \end{aligned}
∥Viui+1−Viui∥≤amT即:2Visin2ai≤amT(2.2)
上图中
1
,
2
1,2
1,2分别为2个速度转角线段衔接点,衔接点速度大小分别为
V
i
、
V
i
+
1
V_i、V_{i+1}
Vi、Vi+1。
a
m
a_m
am为最大加速度,
T
T
T为插补周期,
a
i
a_i
ai为两个速度矢量之间的夹角。
2.3 回溯速度限制
如图所示,由于存在减速距离产生的约束第 i i i段线段与第 i + 1 i+1 i+1段互相衔接,第 i i i段的末速度大小等于第 i + 1 i+1 i+1段的初速度。因此从 V i V_i Vi减速到 V i + 1 V_{i+1} Vi+1有:
V
i
2
≤
V
i
+
1
2
+
2
a
m
l
i
+
1
(2.3)
\begin{aligned} V_i^2\leq V_{i+1}^2+2a_ml_{i+1}\tag{2.3} \end{aligned}
Vi2≤Vi+12+2amli+1(2.3)
l
i
+
1
l_{i+1}
li+1:线段
i
+
1
i+1
i+1的长度
2.4 最大速度限制
机器人在运动过程中速度不能超过最大限制速度 V m a x V_{max} Vmax
2.5 轮廓误差速度限制(选用)
关于轮毂误差的描述参考4-6
轮廓误差常用在机床上,因此选用。
理论上,机床运动在周期T内速度方向在衔接点C改变,详情见2.2.
特别地,机床当加工轨迹不经过衔接点C后,加工误差就会产生。见下图,e为实际路径与理论路径的误差值。
θ
\theta
θ为转角大小。由于
A
C
=
B
C
AC=BC
AC=BC,ABC为等腰三角形,因此
2
α
=
θ
2\alpha = \theta
2α=θ。
V
i
T
≤
2
e
t
a
n
θ
2
(2.4)
\begin{aligned} V_iT \leq \frac{2e}{tan\frac {\theta}{2}} \tag{2.4} \end{aligned}
ViT≤tan2θ2e(2.4)
在周期T内,机床的运动由A运动到B,速度为
V
i
V_i
Vi,所以运动距离
V
i
T
≤
A
B
V_iT\leq AB
ViT≤AB,而
A
B
=
2
e
t
a
n
α
,
α
=
θ
2
AB= \frac{2e}{tan\alpha}, \alpha = \frac{\theta}{2}
AB=tanα2e,α=2θ.
综上所述速度
V
i
V_i
Vi应该满足的条件有:
{
V
i
≤
V
i
−
1
2
+
2
a
m
l
i
(
1
)
2
V
i
s
i
n
a
i
2
≤
a
m
T
(
2
)
V
i
≤
V
i
+
1
2
+
2
a
m
l
i
+
1
(
3
)
V
i
≤
V
m
a
x
(
4
)
V
i
≤
2
e
T
t
a
n
θ
2
(
5
∗
)
选用
\begin{cases} V_i\leq \sqrt{V_{i-1}^2+2a_ml_i} & (1)\\ 2V_isin{\frac{a_i}{2}}\leq a_mT & (2) \\ V_i\leq \sqrt{V_{i+1}^2+2a_ml_{i+1}} & (3)\\ V_i\leq V_{max}& (4)\\ V_i \leq \frac{2e}{Ttan\frac {\theta}{2}} & (5^*)选用 \end{cases}
⎩
⎨
⎧Vi≤Vi−12+2amli2Visin2ai≤amTVi≤Vi+12+2amli+1Vi≤VmaxVi≤Ttan2θ2e(1)(2)(3)(4)(5∗)选用
最后
V
i
=
m
i
n
(
1
,
2
,
3
,
4
)
V_i = min(1,2,3,4)
Vi=min(1,2,3,4)。
章节总结
上面式(1)是与前一段线段的运动情况有关,式(2)是与转角衔接点速度有关,式(3)与后一段线段的运动情况速度有关。基于前一段的运动情况已知的情况下,为了求得本段的末速度需要前瞻多段路径,回溯速度不应该只考虑后一段,需要考虑多段路径,也就是前瞻多段路径。
原因:以防由于前瞻路径长度太短,导致机器人无法在下一段路径中停止运动,速度为0.
3. 回溯速度补充
在上一章的总结里,得知为了让机器人能够在后面的路径停下,需要前瞻多段路径,而非一段,因此回溯速度限制应该需要将多段路径考虑在内,并且需要考虑后续段的转角速度约束。
假设在第
i
i
i段后面
x
x
x段路径都没有角度约束,那么当这
x
x
x段的总长度满足公式:
∑ j = i i + x l j ≥ v m a x 2 2 a m (3.1) \sum_{j=i}^{i+x}l_j \geq \frac{v_{max}^2}{2a_m} \tag{3.1} j=i∑i+xlj≥2amvmax2(3.1)
式中 l j l_j lj是第 j j j段的长度。从第 i i i段开始,速度可以从 V m a x V_{max} Vmax一直减速到0,回溯速度约束只考虑后面的 i + x i+x i+x段。 关于前瞻 x x x段的距离 S = v m a x 2 2 a m S = \frac{v_{max}^2}{2a_m} S=2amvmax2,在得到S后,得知每段线段的长度 l l l,可以得到前瞻 x x x段 x = S l x = \frac{S}{l} x=lS。
最终关于速度前瞻算法流程如下
1. 设前瞻
x
x
x段的末速度为0,根据回溯速度限制、转角速度限制和最大速度限制3个约束条件计算出第
x
−
1
x-1
x−1段路径的末速度大小
2. 循环第1个步骤计算出当前段的速度约束
3. 计算由加速距离产生的前向速度限制
4. 取约束速度的最小值最为本段末速度
4. 速度规划
4.1 速度规划图
根据初速度
V
b
V_b
Vb、末速度
V
e
V_e
Ve与最大速度
V
m
V_m
Vm的关系,可以将线段的速度规划分为以下7种情况。如下图所示
(1) 全程加速才能达到末速度
V
e
V_e
Ve,其中
V
e
≤
V
m
V_e \leq V_m
Ve≤Vm。
(2) 全程减速才能达到末速度
V
e
V_e
Ve。
(3) 不需要全程加速,有匀加速度和匀速段达到
V
e
V_e
Ve
(4) 先匀速再减速达到末速度
V
e
V_e
Ve,应对
V
e
=
0
V_e = 0
Ve=0的情况,不可以先减速再匀速,对2该文章的速度规划进行了优化。
(5) 末速度
V
e
=
V
m
V_e = V_m
Ve=Vm,先加速再匀速运动。
(6) 末速度小于最大限制速度,先加速到
V
m
a
x
V_{max}
Vmax再匀速,最后减速到末速度
V
e
V_e
Ve。
(7) 面对起始速度和末速度均为0的情况下,先加速再减速运动。
4.2 算法实现
- 假设机器人在本路径由初速度 V 0 V_0 V0加速到最高限制速度 V m a x V_{max} Vmax再减速到期望的末速度 V e V_e Ve。计算此条件下走过的长度 l e , l e = 1 2 ( 2 V m a x 2 − V 0 2 − V e 2 ) / a m l_e, l_e = \frac{1}{2}(2V_{max}^2 - V_0^2 - V_e^2) / a_m le,le=21(2Vmax2−V02−Ve2)/am
- 假设机器人由初速度 V 0 V_0 V0全程加速或减速到末速度V_e, 计算此条件下走过的长度 l i , l i = ∥ V 0 2 − V e 2 ∥ 2 a m l_i, l_i = \frac{\left \| V_0^2- V_e^2\right \|}{2a_m} li,li=2am∥V02−Ve2∥
- 判断本段的加减速情况。
3.1 比较本段的实际长度 l 与 l e l与l_e l与le,如果 l ≥ l e l\geq l_e l≥le且末速度小于最大限制速度 V m a x V_{max} Vmax,说明本段的长度足够加速到 V m a x V_{max} Vmax然后减速到末速度,本段路径属于第6种加减速情况。
3.2 如果 l ≥ l e l\geq l_e l≥le 且末速度等于最大限制速度 V m a x V_{max} Vmax,说明需要加速到 V m a x V_{max} Vmax再匀速运动,属于第5种加减速情况。
如果 l < l e l< l_e l<le 说明线段长度不满足机器人加速到 V m a x V_{max} Vmax,则继续判断
3.3 如果 l > l i l > l_i l>li 且 V 0 > V e V_0 > V_e V0>Ve ,说明机器人由初速度 V 0 V_0 V0 减速到末速度 V e V_e Ve 本段还有剩余长度,属于第4种情况。
3.4 如果 l > l i l > l_i l>li 且 V 0 < V e V_0 < V_e V0<Ve ,说明机器人由初速度 V 0 V_0 V0 加速到末速度 V e V_e Ve 本段还有剩余长度,属于第3种情况。
3.5 如果 l = l i l = l_i l=li 且 V 0 > V e V_0 > V_e V0>Ve ,说明机器人由初速度 V 0 V_0 V0 减速到末速度 V e V_e Ve 本段没有剩余长度,属于第2种情况。
3.6 如果 l = l i l = l_i l=li 且 V 0 < V e V_0 < V_e V0<Ve ,说明机器人由初速度 V 0 V_0 V0 加速到末速度 V e V_e Ve 本段没有剩余长度,属于第1种情况。
3.7 如果 l > l i l > l_i l>li 且 V 0 = V e = 0 V_0 = V_e = 0 V0=Ve=0, 属于特殊情况,为第7种情况。
如果出现 l < l i l < l_i l<li ,说明全程加速或者减速都达不到规划的末速度,说明速度前瞻过程中计算末速度的时候出现了错误,这时候需要让机器人停下。
5. 参考
- [1] 连续轨迹控制介绍