Trajectory modifification considering dynamic constraintsof autonomous robots
于2022年10月24日2022年10月24日由Sukuna发布
原文
下载
综述
TEB算法是局部寻路算法、全局寻路算法提供一个结果B,然后经过局部寻路算法进行细化为
再传递给机器人.TEB就是进行局部细化的工作.
TEB算法的核心思想.
算法的输入:
.其中
是全局寻路算法提供的若干个点状态(状态是一个三元式
.位置和偏向方向)的集合.
是每个状态的间隔时间.
算法的输出:给定一个优化后的
,使得代价函数(或者叫目标函数)的值最低.
代价函数
我们可以简单地把目标函数设置成:
这个的意思就是代价函数可以设置成若干个项目的加权和.
项目可以分成两种:
- 速度和加速度限制等约束
- 轨迹有关的目标，如路径最短、路径最快或与障碍物的距离
我们一般使用下面的惩罚函数:
其中，
为极限值，S、n和
影响近似的准确度。具体地说，S表示缩放，n表示多项式阶数，
表示近似值附近一个小位移。x就是输入,这里x和
是距离.
目标点和障碍点
假设一个点
,当前小车的与这个点的距离设置为
.
是目标点.那么这个函数就是:
这代表,我们需要让这个小车离这个目标点越来越近.
是自己设置的临界值
是障碍点.那么这个函数就是:
这代表,我们需要让这个小车离这个目标点越来越远,所以说要加上负数.
是自己设置的临界值.
速度和加速度
速度定义:
角速度定义:
加速度定义:
速度修正
当然我们在大学物理中学过,我们在考虑总体的速度的时候还要考虑转动所带来的影响.在此我们可以进行修正:
对上面的式子求差分我们可以得到加速度.
运动学约束
我们知道,机器人在运行的时候,路径不完全是由若干个直线组成的,而是由若干个圆弧组成的.我们取两个点i和i 1.两个点之间的连线设置为l,运动速度的方向设置为
和
.两个方向和l的夹角
和
一样,根据论文可以列出式子:
最快时间约束
就是每一段时间的平方和,如图所示:
实现
在初始化阶段,通过添加关于动力学和运动学约束的默认时间信息,将初始路径变为初始轨迹. 初始轨迹是由带有纯旋转和平移的分段线性分段组成的.这种以多边形表示的路径通常由概率路线图规划器提供.
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”中包含的稀疏系统大规模优化算法进行求解.这一个部分的细节我们在下一个部分介绍.
