Trajectory modifification considering dynamic constraintsof autonomous robots
原文
综述
TEB算法是局部寻路算法、全局寻路算法提供一个结果B,然后经过局部寻路算法进行细化为再传递给机器人.TEB就是进行局部细化的工作.
TEB算法的核心思想.
算法的输入:.其中
是全局寻路算法提供的若干个点状态(状态是一个三元式
.位置和偏向方向)的集合.
是每个状态的间隔时间.
算法的输出:给定一个优化后的,使得代价函数(或者叫目标函数)的值最低.
代价函数
我们可以简单地把目标函数设置成:
这个的意思就是代价函数可以设置成若干个项目的加权和.
项目可以分成两种:
- 速度和加速度限制等约束
- 轨迹有关的目标,如路径最短、路径最快或与障碍物的距离
我们一般使用下面的惩罚函数:
其中,




目标点和障碍点
假设一个点,当前小车的与这个点的距离设置为
.
是目标点.那么这个函数就是:
这代表,我们需要让这个小车离这个目标点越来越近.

是障碍点.那么这个函数就是:
这代表,我们需要让这个小车离这个目标点越来越远,所以说要加上负数.

速度和加速度
速度定义:
角速度定义:
加速度定义:
速度修正
当然我们在大学物理中学过,我们在考虑总体的速度的时候还要考虑转动所带来的影响.在此我们可以进行修正:
对上面的式子求差分我们可以得到加速度.
运动学约束
我们知道,机器人在运行的时候,路径不完全是由若干个直线组成的,而是由若干个圆弧组成的.我们取两个点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”中包含的稀疏系统大规模优化算法进行求解.这一个部分的细节我们在下一个部分介绍.
我的博客即将同步至腾讯云开发者社区,邀请大家一同入驻:https://cloud.tencent.com/developer/support-plan?invite_code=2lnnifd19uass
1 条评论
rantrism · 2022年10月25日 上午11:26
您好~我是腾讯云开发者社区运营,关注了您分享的技术文章,觉得内容很棒,我们诚挚邀请您加入腾讯云自媒体分享计划。完整福利和申请地址请见:https://cloud.tencent.com/developer/support-plan
作者申请此计划后将作者的文章进行搬迁同步到社区的专栏下,你只需要简单填写一下表单申请即可,我们会给作者提供包括流量、云服务器等,另外还有些周边礼物。