2024年Timed-Elastic-Band局部路径规划算法_teb算法python(3)，C C++岗面试12家大厂成功跳槽

最新推荐文章于 2024-06-30 15:22:08 发布

2401_84975245

最新推荐文章于 2024-06-30 15:22:08 发布

阅读量430

点赞数 5

分类专栏：程序员文章标签： c语言 c++ 学习

本文链接：https://blog.csdn.net/2401_84975245/article/details/138806538

版权

程序员专栏收录该内容

43 篇文章 0 订阅

订阅专栏

网上学习资料一大堆，但如果学到的知识不成体系，遇到问题时只是浅尝辄止，不再深入研究，那么很难做到真正的技术提升。

需要这份系统化的资料的朋友，可以添加戳这里获取

一个人可以走的很快，但一群人才能走的更远！不论你是正从事IT行业的老鸟或是对IT行业感兴趣的新人，都欢迎加入我们的的圈子（技术交流、学习资源、职场吐槽、大厂内推、面试辅导），让我们一起学习成长！

figure 2

二、约束目标函数

1、跟随路径和避障约束

约束主要有两个目标：跟随已知的全局规划路径和避障。两个目标函数均十分相似，跟随路径施力将elastic bands拉向全局路径，而避障约束施力使得elastic bands远离障碍物。configuration序列与全局路径点序列或障碍物 $z_{j}$ 的最近距离记为 $d_{min,j}$ ，如figure 3所示。跟随路径目标以configuration距全局路径的允许最大距离 $r_{p_{max}}$ 作为约束，避障目标以configuration距障碍物的允许最小距离 $r_{o_{min}}$ 作为约束。

实际上原文约束为attainment of the intermediate way points of the original path，是否为我所理解的跟随全局规划路径还有待商榷。

figure 3

约束以惩罚函数实现：

$f_{path}=e_{\tau }\left ( d_{min,j},r_{p_{max}},\epsilon ,S,n \right )$ （7）

$f_{ob}=e_{\tau }\left ( -d_{min,j},-r_{o_{min}},\epsilon ,S,n \right )$ （8）

值得注意的是，目标函数的梯度可视为对elastic band施加的外力。

2、速度和加速度约束

由速度和加速度组成的动力学约束可以用类似1中运动学约束的惩罚函数表示。以figure 3为例，机器人运动的平均线速度和角速度可以通过相邻的configuration $\chi _{i},\chi _{i+1}$ 和时间间隔 $\Delta T$ 计算得到。

$v_{i}\simeq \frac{1}{\Delta T}\left | \bigl(\begin{smallmatrix} x_{i+1}-x_{i}\ y_{i+1}-y_{i} \end{smallmatrix}\bigr) \right |$ （9）

$\omega_{i}=\frac{\beta _{i+1}-\beta _{i}}{\Delta T}$ （10）

类似的，线速度约束可以表示为：

$f_{v_{i}}=e_{\tau }\left ( v_{i},v_{max},\epsilon ,S,n \right )$

机器人的平均线加速度用同样的近似计算得到：

$a_{i}=\frac{2\left ( v_{i+1}-v_{i} \right ))}{\Delta T_{i}+\Delta T_{i+1}}$ （11）

角加速度同理可计算得到。

以差动机器人为运动模型，两轮的转速可通过以下计算得到：

$v_{\omega _{r},i}=v_{i}+L\omega _{i}$ （12）

$v_{\omega _{l},i}=v_{i}-L\omega _{i}$ （13）

其中L为两轮轴距的一半。

对（12）（13）式作关于时间的微分得到两轮的加速度。

则可结合机器人设计的机械参数得到的轮子速度和加速度的上下界确定约束上下界。

3、Non-holonomic运动学约束

差动机器人在平面运动只有两个自由度，其只能以朝向的方向直线运动或旋转。这种运动学约束使得机器人以有若干弧段组成的平滑的轨迹运动。相邻的两个configuration应在弧段的两端，如figure 4。

figure 4

初始configuration $\chi _{i}$ 与运动方向 $d_{i,i+1}$ 的夹角 $\theta _{i}$ 应与结束configuration对应的夹角 $\theta _{i+1}$ 相等。

若 $\beta _{i}$ 为机器人在第i段弧段相对于世界坐标系的绝对姿态，则有：

$\theta _{i}=\theta _{i+1}$ （14）

$\Leftrightarrow \begin{pmatrix} \cos \beta _{i}\ \sin \beta _{i} \ 0 \end{pmatrix}\times d_{i,i+1}=d_{i,i+1}\times \begin{pmatrix} \cos \beta _{i+1}\ \sin \beta _{i+1} \ 0 \end{pmatrix}$ （15）

其中，运动方向向量：

$d_{i,i+1}:=\begin{pmatrix} x_{i+1}-x_{i}\y_{i+1}-y_{i} \ 0 \end{pmatrix}$ （16）

相应的目标函数：

$f_{k}\left ( x_{i},x_{i+1} \right )=\left | \left [ \begin{pmatrix} \cos \beta _{i}\ \sin \beta _{i} \ 0 \end{pmatrix}+\begin{pmatrix} \cos \beta _{i+1}\ \sin \beta _{i+1} \ 0 \end{pmatrix} \right ] \times d_{i,i+1}\right |^{2}$ （17）

要注意机器人运动在180度附近的越界的情况。

4、最快路径约束

目标函数即为最小化时间间隔序列的二次方。

$f_{k}=\left ( \sum \Delta T_{i} \right )^{2}$ （18）

目标函数使得机器人获得最快路径，路径上的各configuration点在时间上均匀分开，而非传统的空间上求最短路径。

三、teb实现

teb的控制流程如figure 5。

figure 5

在initialization阶段，通过增加满足运动学和动力学约束默认的时间信息将an initial path添加到an initial trajectory（不太理解）。

每次迭代过程中，算法动态添加新的configuration，删除旧的configuration以将关于空间和时间的分辨率调整至余下的轨迹长度和规划的范围内。添加一个hysteresis实现避障。优化问题转化为一个hyper-graph问题，通过g2o中关于大规模稀疏矩阵的优化算法解决。

teb优化问题（公式4）可以转化为hyper-graph问题。configurations和以及时间间隔作为nodes，目标函数 $f_{k}$ 以及其他约束函数为edges，各nodes由edges连接起来构成hyper-graph。该graph中，每一个约束都为一条edge，并且每条edge允许连接的nodes的数目是不受限制的。

图6（a）中展示了两个configurations $\left ( s_{0},s_{1} \right )$ ，一个时间间隔 $\Delta T_{0}$ ，一个障碍点 $o_{1}$ 组成的hyper-graph。速度约束需要计算平均速度，则该约束与 $s_{0},s_{1},\Delta T_{0}$ 相关，该edge将三个nodes连接起来。障碍物约束将障碍物和离障碍物最近的configuration连接起来。注意障碍物点是固定的，算法不应也不能对其位置进行优化。

图6（b）中则展示了更大的teb hyper-graph。

既有适合小白学习的零基础资料，也有适合3年以上经验的小伙伴深入学习提升的进阶课程，涵盖了95%以上C C++开发知识点，真正体系化！

由于文件比较多，这里只是将部分目录截图出来，全套包含大厂面经、学习笔记、源码讲义、实战项目、大纲路线、讲解视频，并且后续会持续更新

如果你需要这些资料，可以戳这里获取

有适合3年以上经验的小伙伴深入学习提升的进阶课程，涵盖了95%以上C C++开发知识点，真正体系化！**

由于文件比较多，这里只是将部分目录截图出来，全套包含大厂面经、学习笔记、源码讲义、实战项目、大纲路线、讲解视频，并且后续会持续更新

如果你需要这些资料，可以戳这里获取

2401_84975245

关注

5
点赞
踩
3

收藏

觉得还不错? 一键收藏
0
评论
2024年Timed-Elastic-Band局部路径规划算法_teb算法python(3)，C C++岗面试12家大厂成功跳槽

每次迭代过程中，算法动态添加新的configuration，删除旧的configuration以将关于空间和时间的分辨率调整至余下的轨迹长度和规划的范围内。这种运动学约束使得机器人以有若干弧段组成的平滑的轨迹运动。该graph中，每一个约束都为一条edge，并且每条edge允许连接的nodes的数目是不受限制的。目标函数使得机器人获得最快路径，路径上的各configuration点在时间上均匀分开，而非传统的空间上求最短路径。则可结合机器人设计的机械参数得到的轮子速度和加速度的上下界确定约束上下界。
复制链接

扫一扫