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”中包含的稀疏系统大规模优化算法进行求解.这一个部分的细节我们在下一个部分介绍.

我的博客即将同步至腾讯云开发者社区，邀请大家一同入驻：https://cloud.tencent.com/developer/support-plan?invite_code=2lnnifd19uass

Trajectory modifification considering dynamic constraintsof autonomous robots

综述

TEB算法的核心思想.

代价函数

目标点和障碍点

速度和加速度

速度修正

运动学约束

最快时间约束

实现

1 条评论

rantrism · 2022年10月25日上午11:26

发表回复取消回复

论文

混合高斯模型和EM算法

Trajectory modifification considering dynamic constraintsof autonomous robots

综述

TEB算法的核心思想.

代价函数

目标点和障碍点

速度和加速度

速度修正

运动学约束

最快时间约束

实现

1 条评论

rantrism · 2022年10月25日 上午11:26

发表回复 取消回复

相关文章

论文

混合高斯模型和EM算法

rantrism · 2022年10月25日上午11:26

发表回复取消回复