Trajectory modifification considering dynamic constraintsof autonomous robots

Trajectory modifification considering dynamic constraintsof autonomous robots

于2022年10月24日2022年10月24日由Sukuna发布

原文

下载

综述

TEB算法是局部寻路算法、全局寻路算法提供一个结果B,然后经过局部寻路算法进行细化为

B^*

再传递给机器人.TEB就是进行局部细化的工作.

TEB算法的核心思想.

算法的输入:

B:=(Q,\tau)

.其中

Q

是全局寻路算法提供的若干个点状态(状态是一个三元式

x,y,\beta

.位置和偏向方向)的集合.

\tau

是每个状态的间隔时间.

算法的输出:给定一个优化后的

B^*<em>

,使得代价函数(或者叫目标函数)的值最低.

\[B^{</em>*} = argmin \ f(B)\]

代价函数

我们可以简单地把目标函数设置成:

\[f(B) = \sum_k \lambda_kf_k(B)\]

这个的意思就是代价函数可以设置成若干个项目的加权和.

项目可以分成两种:

• 速度和加速度限制等约束
• 轨迹有关的目标，如路径最短、路径最快或与障碍物的距离

我们一般使用下面的惩罚函数:

\[e_{\Gamma}(x,x_j,\epsilon,S,n)=(\dfrac{x-(x_r-\epsilon)}{S})^n\ (x>x_r-\epsilon)\]

其中，

x_r

为极限值，S、n和

\epsilon

影响近似的准确度。具体地说，S表示缩放，n表示多项式阶数，

\epsilon

表示近似值附近一个小位移。x就是输入,这里x和

x_j

是距离.

目标点和障碍点

假设一个点

A

,当前小车的与这个点的距离设置为

\alpha = |\bold x|

.

A

是目标点.那么这个函数就是:

\[f_{path} = e_{\Gamma}(\alpha,r_{pmax},\epsilon,S,n)\]

这代表,我们需要让这个小车离这个目标点越来越近.

r_{pmax}

是自己设置的临界值

A

是障碍点.那么这个函数就是:

\[f_{op} = e_{\Gamma}(-\alpha,-r_{omin},\epsilon,S,n)\]

这代表,我们需要让这个小车离这个目标点越来越远,所以说要加上负数.

r_{omix}

是自己设置的临界值.

速度和加速度

速度定义:

\[v_i = \dfrac{1}{\Delta T_i}(x_{i+1}-x_i)\]

角速度定义:

\[\omega_i = \dfrac{1}{\Delta T_i}(\beta_{i+1}-\beta_i)\]

加速度定义:

\[a_i = \dfrac{2(v_{i+1}-v_i)}{\Delta T_i+\Delta T_{i+1}}\]

速度修正

当然我们在大学物理中学过,我们在考虑总体的速度的时候还要考虑转动所带来的影响.在此我们可以进行修正:

\[v_{\omega_{R,j}} = v_i + L\omega_{i}\]

\[v_{\omega_{L,j}} = v_i - L\omega_{i}\]

对上面的式子求差分我们可以得到加速度.

运动学约束

我们知道,机器人在运行的时候,路径不完全是由若干个直线组成的,而是由若干个圆弧组成的.我们取两个点i和i+1.两个点之间的连线设置为l,运动速度的方向设置为

\beta_{i+1}

和

\beta_i

.两个方向和l的夹角

\mathcal{Z}_{i+1}

和

\mathcal{Z}_{i}

一样,根据论文可以列出式子:

最快时间约束

就是每一段时间的平方和,如图所示:

\[f_k = \sum (\Delta T_i)^2\]

实现

在初始化阶段,通过添加关于动力学和运动学约束的默认时间信息,将初始路径变为初始轨迹. 初始轨迹是由带有纯旋转和平移的分段线性分段组成的.这种以多边形表示的路径通常由概率路线图规划器提供.

L. E. Kavraki et al., ”Probabilistic roadmaps for path planning in high dimensional confifiguration spaces”, in IEEE Transactions on Robotics and Automation, Vol. 12, No.4, pp.566-580, August 1996.

在每次迭代中,算法动态地添加新的“configurations”或删除先前的“configurations”,以便将空间和时间分辨率调整到剩余的轨迹长度或规划的范围内.采用“hysteresis”来避免振荡。优化问题被转换为“hyper-graph”,并使用“ g2o”中包含的稀疏系统大规模优化算法进行求解.这一个部分的细节我们在下一个部分介绍.