admin管理员组文章数量:1665129
文章目录
- 一、多项式轨迹
-
- 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=a0q1=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−t0q1−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} ⎩
因此,常量参数可以计算为:
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−v0T)
这样,我们可以计算得到当 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} ⎩
在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} ⎩
版权声明:本文标题:机器人路径规划算法(一) 内容由热心网友自发贡献,该文观点仅代表作者本人, 转载请联系作者并注明出处:https://www.elefans.com/dongtai/1730037787a1220171.html, 本站仅提供信息存储空间服务,不拥有所有权,不承担相关法律责任。如发现本站有涉嫌抄袭侵权/违法违规的内容,一经查实,本站将立刻删除。
发表评论