myAgv的slam算法学习以及动态避障下篇

引言

在之前的一篇文章中有提到购入了一台myAGV，以树莓派4B为控制核心的移动机器人。上篇文章中向大家介绍了myAGV如何实现建图、导航以及静态避障，但我们深知，这只是机器人自主导航能力的基础。在实际应用场景中，机器人需要面对复杂的动态环境，如人流、障碍物等，如何实现可靠的动态避障，是我们不断探索和挑战的问题。在本文中，我们将分享我们在探索动态避障方面的实践和经验，希望能够为其他创客开发者和机器人爱好者提供一些参考和启发。

如果想知道上篇介绍了些什么，点击这个连接

产品

在此之前，还是在介绍一下myAGV，我使用的这款机器人。

myAGV是一款大象机器人公司出品的移动机器人，由树莓派4B为控制核心，内置Ubuntu18系统,ROS编译环境；360°激光雷达，内置高清摄像头。它采用了竞赛级别的麦克纳姆车轮和带有金属框架的全包裹设计。

动态避障

动态避障是指在移动机器人或自动驾驶汽车等智能移动设备在行进过程中，通过实时感知周围环境的变化，采取相应的措施以避免与障碍物发生碰撞的技术。动态避障需要实时感知环境的变化，如监测障碍物的位置、大小、形状、运动状态等，然后通过对这些信息的处理和分析，生成相应的行动指令，使机器人或汽车能够避免与障碍物发生碰撞并继续前进。

下面分享了一个视频，内容关于无人飞机进行快速动态避障的记录。

突然起来的障碍物，需要立即避开它，避免造成无人机的损伤。今天我们要实现的是myAGV在自动导航的时候也能够及时的躲避障碍物。

大象机器人提供的动态避障算法是基于TEB和DWA算法。

TEB算法

TEB（Timed-Elastic-Band)是一种用于轨迹优化的算法，可用于机器人的动态避障和路径规划。主要对全局路径规划器生成的初始全局轨迹进行后续修正，从而优化机器人的运动轨迹，属于局部路径规划。在轨迹优化过程中，该算法拥有多种优化目标，包括但不限于：整体路径长度、轨迹运行时间、与障碍物的距离、通过中间路径点以及机器人动力学、运动学以及几何约束的符合性。

起始点和目标点状态都有全局规划器制定，中间插入N个控制橡皮筋形状的控制点。这个路径是可以变形的，变形的条件就是所有约束条件，当作橡皮筋的外力，例如距离障碍物的距离，移动机器人的最大速度，最大角速度，最大转弯半径等。依靠这些约束条件来保证移动机器人的安全和稳定。

TEB算法具体实现的过程如下：

1. 根据机器人当前位置和目标位置，生成一条初始路径。
2. 将路径表示为一系列时间段上的线段，称为轨迹片段。每个轨迹片段表示机器人在一个时间段内沿着一条直线运动的轨迹。
3. 对每个轨迹片段进行优化，以满足机器人的运动约束和避障需求。优化目标是使轨迹片段的长度和运动时间最小，同时避免与障碍物发生碰撞。
4. 将优化后的轨迹片段拼接在一起，形成机器人的最终运动轨迹。如果机器人遇到障碍物，可以重新生成轨迹片段，并进行优化。

TEB算法的优点是可以在运动中实时避开障碍物，并可以处理机器人的动态运动。同时，TEB算法可以结合其他的避障算法，例如基于模型预测控制的算法，提高避障效果。

DWA算法

DWA（Dynamic Window Approach）是一种用于机器人动态避障的算法。该算法旨在在机器人的运动范围内搜索最优的运动轨迹，以避开障碍物。DWA算法通过将机器人的运动轨迹表示为一组(v,w)速度和角速度的组合，然后对这些组合进行搜索和评估，找到最优的运动轨迹。

DWA算法的具体实现过程如下：

1. 根据机器人当前位置和目标位置，生成一组速度和角速度的候选集合，称为速度窗口。
2. 对速度窗口中的每个速度和角速度组合，计算机器人在接下来的一段时间内所能到达的位置，即运动轨迹。
3. 对每个运动轨迹进行评估，以确定是否与障碍物发生碰撞。评估的方法包括计算轨迹上的代价函数，例如与障碍物的距离和速度等。
4. 根据评估结果，选择代价函数最小的运动轨迹，并将该轨迹作为机器人的运动指令。

这个视频能够辅助了解DWA算法。

TEB-DWA算法

TEB在实现动态避障方面具有一定的优势。然而，TEB算法也存在一些局限性，其中包括：

• 对初始路径依赖性强：TEB算法的性能很大程度上依赖于初始路径的质量。如果初始路径存在较大的偏差或错误，TEB算法很可能会陷入无法解决的困境。
• 对环境模型的依赖性强：TEB算法需要准确的环境模型，包括障碍物的位置和形状等信息。如果环境模型存在偏差或误差，TEB算法可能会出现路径规划错误的情况。
• 对计算资源的要求高：TEB算法需要进行大量的计算和优化，时间复杂度较高，对计算资源的要求也较高。

DWA算法也具有能对轨迹进行搜索和评估，选出最优路径解，但也有一定的局限性：

• 无法处理高速运动的障碍物：DWA算法是基于机器人的运动模型和环境感知，通过搜索和评估速度和角速度的组合，找到最优的运动轨迹，以避开障碍物。然而，在高速运动的障碍物出现时，DWA算法可能无法及时感知和处理，从而导致机器人的碰撞和安全问题。
• 对环境模型的要求较高：DWA算法需要机器人具有较好的环境感知能力，以获取环境中障碍物的位置、形状和大小等信息。如果环境模型不准确或者不完整，DWA算法可能无法正确地评估运动轨迹的代价函数，从而导致机器人的错误运动和避障失败。
• 无法处理不确定的环境：DWA算法是基于静态环境的假设，即环境中的障碍物位置不会发生变化。然而，在一些不确定的环境中，例如动态障碍物频繁出现的场景，DWA算法可能无法有效地避开障碍物，从而导致机器人的碰撞和安全问题。
• 难以处理多机器人协同避障：DWA算法通常是针对单个机器人的避障问题进行设计的，难以处理多机器人协同避障的问题。在多机器人协同避障时，需要考虑机器人之间的相互作用和协调，从而进行更加复杂和高级的避障算法设计。

为了解决TEB和DWA算法的局限性，实现更加精细和高效的动态避障和路径规划，有人提出了TEB-DWA算法，TEB-DWA算法的核心思想是通过将TEB算法和DWA算法的优点结合起来，设计出一种更加精细和高效的动态避障算法。具体来说，TEB-DWA算法采用TEB算法的弹性带方法，实现路径规划和优化，同时采用DWA算法的速度窗口方法，实现速度和角速度的搜索和评估，从而实现机器人的动态避障和路径规划。

teb-dwa算法的实现过程是啥

TEB-DWA算法的实现过程主要分为以下几个步骤：

1. 机器人运动模型的建立：根据机器人的运动约束和动力学模型，建立机器人的运动模型，用于计算机器人的运动轨迹和速度约束。
2. 环境感知和障碍物检测：通过机器人的传感器获取环境中障碍物的位置、形状和大小等信息，进行障碍物检测和分类，以确定机器人的运动路径和避障策略。
3. 弹性带路径规划：根据机器人的起点和终点，利用弹性带方法生成机器人的运动路径，并考虑机器人的运动约束和避障需求，以实现路径规划和优化。
4. 速度窗口搜索和评估：根据机器人的运动模型和环境感知信息，利用DWA算法搜索机器人的最优速度和角速度组合，并评估每个速度和角速度组合的代价函数，以确定机器人的运动轨迹和避障策略。
5. 运动控制和执行：根据计算得到的最优速度和角速度组合，利用控制算法实现机器人的运动控制和执行，以实现机器人的动态避障和路径跟踪。

实际操作

在ROS中，可以使用Move Base包来实现TEB-DWA算法。Move Base是ROS中常用的路径规划和导航包，提供了多种路径规划算法和导航功能，包括TEB-DWA算法。下面是使用ROS中TEB-DWA算法的步骤：

1. 安装Move Base包：在ROS中安装Move Base包，可以使用以下命令：“sudo apt-get install ros--move-base”
2. 配置Move Base参数：在使用TEB-DWA算法之前，需要配置Move Base的参数。可以使用move_base.launch文件来配置参数，例如设置机器人的起点和终点、地图信息、运动约束和避障参数等。可以根据具体的应用场景和任务需求，进行参数的调整和优化。
3. 运行Move Base节点：在配置好参数之后，可以使用以下命令来启动Move Base节点：”roslaunch move_base move_base.launch“
4. 发布目标点：在运行Move Base节点之后，可以使用move_base/goal话题来发布机器人的目标点。例如使用rostopic pub命令来发布目标点：
5. 监听机器人状态和反馈：在机器人开始运动之后，可以使用/move_base/status话题来监听机器人的状态和反馈信息。例如使用rostopic echo命令来监听状态和反馈信息：

建图

我们快速的建图，进行避障的实验。

打开终端运行命令,启动雷达

cd myagv_ros
source ./devel/setup.bash
roslaunch myagv_odometry myagv_active.launch

然后运行gmapping算法建图

roslaunch myagv_navigation myagv_slam_laser.launch

动态避障

我们使用默认的配置，看看效果如何。

这似乎看起来实现了动态避障，但是似乎看起来有些许的愚蠢，在发现障碍物的时候还绕了一圈扫描环境。

调整了一下 规划的频率（planned frequency）重新进行了一次测试。

这样看起来效果就好多了。

总结

对myAGV也使用了有一段的时间了，首先我是十分喜欢它的外观的，很酷炫，没有结构件在外面展露出来，内部包裹的很好，发生了碰撞也会有一定的保护作用。其次是它配套的资料都非常地详细，能够提供一个快速上手地模板使用，对于刚接触一款全新未知的产品很有帮助。

使用下来的整体感受还是不错的，连接显示器就能够使用，基于树莓派也有很多的社区能够提供帮助，有许多优秀的案例。此外也有一些需要提升的地方，在进行建图的时候，有时候并不是那么的准确，可能是因为外壳的原因？遮挡住了雷达的范围。也希望使用时间上能够更长。