机器人路径规划算法(一)

admin管理员组

文章数量:1665131

文章目录

• 一、多项式轨迹
• • 1. 线性插值（恒速）
  • 2.抛物线插值（恒加速）
  • • 2.1 加速阶段
    • 2.2 减速阶段
    • 2.3 加速度不对称
  • 3.三次多项式插值（变加速）
  • • 3.1 给定每个点的位置和速度信息
    • 3.2 给定每一个点位置信息，但中间点速度未给定
  • 4.五次多项式插值
  • 5.七次多项式插值
  • 6.结果对比

一、多项式轨迹

1. 线性插值（恒速）

线性插值是一阶多项式插值方法。线性轨迹是将每个位置的点依次用线段连起来，点与点之间的速度是恒定值。假设用 q ( t ) q(t) q(t)来表示插值以后的曲线，则用数学方法来表示线性插值就是：
q ( t ) = a 0 + a 1 ( t − t 0 ) (1.1) q(t) = a_0 + a_1(t - t_0) \tag{1.1} q(t)=a0​+a1​(t−t0​)(1.1)
其中， a 0 , a 1 a_0,a_1 a0​,a1​是待确定的常量参数。 t 0 t_0 t0​ 表示初始时刻， a 0 a_0 a0​表示初始时刻的位置， a 1 a_1 a1​表示斜率，也就是速度，这里为常量。一旦确定了初末位置 q 0 , q 1 q_0,q_1 q0​,q1​和初末时间 t 0 , t 1 t_0,t_1 t0​,t1​，我们就有：
{ q 0 = a 0 q 1 = a 0 + a 1 ( t 1 − t 0 ) \begin{cases} q_0 = a_0 \\ q_1 = a_0 + a_1(t_1 - t_0) \end{cases} { q0​=a0​q1​=a0​+a1​(t1​−t0​)​
即： a 1 = q 1 − q 0 t 1 − t 0 a_1 = \frac{q_1 - q_0}{t_1 - t_0} a1​=t1​−t0​q1​−q0​​

给定位置[ 0, 1.6, 3.2, 2, 4, 0.2, 1.2 ], 给定时间 [ 0, 1, 3, 4.5, 6, 8, 10 ] ,线性插值结果为：

显然易见，对于速度来说，每一段内部速度都是恒定的，但是每段速度的衔接处有一个阶跃；对于加速度来说，每一段轨迹内部的加速度都是0，但是在每一段的末端也会有一个跳变。对于电机来说，这里会导致控制电流急剧变化，使得电机输出不稳定，从而引发抖动问题，严重还会损坏机构本身，因此线性插值并不是工业实践中采用的。

2.抛物线插值（恒加速）

抛物线插值是二阶多项式插值方法。抛物线插值法是用二次曲线将各个位置数据点连接起来，在连接处使用平滑的曲线来过渡，避免速度不连续导致的“急剧拐弯”。抛物线插值的特征是具有恒定的加速度/减速度，由两个多项式组成，一个用于加速阶段，一个用于减速阶段。加速阶段和减速阶段的分割点称“flex point”。

考虑2个数据点之间的插值情况。假设初始时刻 t 0 t_0 t0​ ，在flex point处对应的时刻是 t f t_f tf​，最终时刻为 t 1 t_1 t1​。

先考虑轨迹对称的情况，即flex point的位置是起点和终点的中间位置，此时 t f = t 0 + t 1 2 , q f = q 0 + q 1 2 t_f = \frac{t_0 + t_1}{2}, q_f = \frac{q_0 + q_1}{2} tf​=2t0​+t1​​,qf​=2q0​+q1​​ 。定义符号 h = q 1 − q 0 , T = t 1 − t 0 , T a = t f − t 0 = T 2 h = q_1 - q_0,T = t_1 - t_0,T_a = t_f - t_0 = \frac{T}{2} h=q1​−q0​,T=t1​−t0​,Ta​=tf​−t0​=2T​

2.1 加速阶段

对于加速阶段，其数学表达式为：
q a ( t ) = a 0 + a 1 ( t − t 0 ) + a 2 ( t − t 0 ) 2 , t ∈ [ t 0 , t f ] (1.2) q_a(t) = a_0 + a_1(t-t_0) + a_2(t - t_0)^2, t \in [t_0, t_f] \tag{1.2} qa​(t)=a0​+a1​(t−t0​)+a2​(t−t0​)2,t∈[t0​,tf​](1.2)
其中， a 0 , a 1 , a 2 a_0,a_1, a_2 a0​,a1​,a2​是待确定的常量参数，当我们给定 q 0 = q a ( t 0 ) q_0=q_a(t_0) q0​=qa​(t0​)和 q f = q a ( t f ) q_f = q_a(t_f) qf​=qa​(tf​) 以及初始时刻的速度 v 0 v_0 v0​以后，有如下关系：
{ q a ( t 0 ) = q 0 = a 0 q a ( t f ) = q f = a 0 + a 1 ( t f − t 0 ) + a 2 ( t f − t 0 ) 2 q ˙ a ( t 0 ) = v 0 = a 1 \begin{cases} q_a(t_0) = q_0 = a_0 \\ q_a(t_f) = q_f = a_0 + a_1(t_f - t_0) + a_2(t_f - t_0)^2 \\ \dot{q}_a(t_0) = v_0 = a_1 \end{cases} ⎩ ⎨ ⎧​qa​(t0​)=q0​=a0​qa​(tf​)=qf​=a0​+a1​(tf​−t0​)+a2​(tf​−t0​)2q˙​a​(t0​)=v0​=a1​​
因此，常量参数可以计算为：
a 0 = q 0 ,     a 1 = v 0 ,     a 2 = 2 T 2 ( h − v 0 T ) a_0 = q_0, \ \ \ a_1 = v_0, \ \ \ a_2 = \frac{2}{T^2}(h - v_0T) a0​=q0​,   a1​=v0​,   a2​=T22​(h−v0​T)
这样，我们可以计算得到当 t ∈ [ t 0 , t f ] t \in [t_0, t_f] t∈[t0​,tf​]时，插值曲线为：
{ q a ( t ) = q 0 + v 0 ( t − t 0 ) + 2 T 2 ( h − v 0 T ) ( t − t 0 ) 2 q ˙ a ( t ) = v 0 + 4 T 2 ( h − v 0 T ) ( t − t 0 ) q ¨ a ( t ) = 4 T 2 ( h − v 0 T ) (1.3) \begin{cases} q_a(t) = q_0 + v_0(t - t_0) + \frac{2}{T^2}(h-v_0T)(t-t_0)^2 \\ \dot{q}_a(t) = v_0 + \frac{4}{T^2}(h-v_0T)(t-t_0) \\ \ddot{q}_a(t) = \frac{4}{T^2}(h-v_0T) \end{cases} \tag{1.3} ⎩ ⎨ ⎧​qa​(t)=q0​+v0​(t−t0​)+T22​(h−v0​T)(t−t0​)2q˙​a​(t)=v0​+T24​(h−v0​T)(t−t0​)q¨​a​(t)=T24​(h−v0​T)​(1.3)
在flex point的速度可以这样算： v m a x = q ˙ a ( t f ) = 2 h T − v 0 v_{max} = \dot{q}_a(t_f) = 2\frac{h}{T}-v_0 vmax​=q˙​a​(tf​)=2Th​−v0​
与线性插值不同的是，除了指定初末时间的位置，还需要给定初始速度$ v_0$。

2.2 减速阶段

对于减速阶段，其数学表达式为：
q b ( t ) = a 3 + a 4 ( t − t f ) + a 5 ( t − t f ) 2 (1.4) q_b(t) = a_3 + a_4(t - t_f) + a_5(t - t_f)^2 \tag{1.4} qb​(t)=a3​+a4​(t−tf​)+a5​(t−tf​)2(1.4)
其中 a 3 , a 4 , a 5 a_3, a_4, a_5 a3​,a4​,a5​为常量，给定最终速度 v 1 v_1 v1​，有如下关系：
{ q b ( t f ) = q f = a 3 q b ( t 1 ) = q 1 = a 3 + a 4 ( t 1 − t f ) + a 5 ( t 1 − t f ) 2 q ˙ b ( t 1 ) = v 1 = a 4 + 2 a 5 ( t 1 − t f ) \begin{cases} q_b(t_f) = q_f = a_3 \\ q_b(t_1) = q_1 = a_3 + a_4(t_1 - t_f) + a_5(t_1 - t_f)^2 \\ \dot{q}_b(t_1) = v_1 = a_4 + 2a_5(t_1 - t_f) \end{cases} ⎩ ⎨ ⎧​qb​(tf​)=qf​=a3​

本文标签： 机器人算法路径