UGV Rover PI ROS2 6. 自动导航
| ||||||||||||||||||||||
| ||||||||||||||||||||||
6. 自动导航
6.1 启动导航
6.1.1 二维地图导航
在启动导航之前，首先先确定您已经建了一个名为 map 的环境地图，若您没有跟着前面的教程操作下来，则先需要根据 UGV Rover PI ROS2 4. 基于激光雷达的二维建图或者 UGV Rover PI ROS2 5. 基于深度相机的三维建图任一教程进行建图。
建图完成后，接着启动导航，我们提供了多种自主导航模式，你可以从以下自主导航模式任选一种进行机器人导航。
6.1.1.1 基于 AMCL 和 EMCL 的导航
1. AMCL 算法
Adaptive Monte Carlo Localization（AMCL）是 ROS 2 中的一种基于粒子滤波的定位算法，它使用 2D 激光雷达在给定的已知地图中估计机器人的位置和方向（即姿势）。AMCL 主要用于移动机器人导航，它通过激光传感器（如激光雷达）与现有地图相匹配，推算出机器人在地图中的位置和朝向。其核心思想是通过大量粒子表示机器人的可能位置，并逐步更新这些粒子来缩小机器人的位姿不确定性。
AMCL 的优势：
- 自适应粒子数：AMCL会根据机器人位置的不确定性动态调整粒子的数量。
- 适用于动态环境：虽然 AMCL 假设静态环境，但它能在一定程度上处理少量动态障碍物，如行人等移动物体，这使其在实际应用中更加灵活。
- 可靠的定位能力：AMCL 在已知地图中的定位效果非常可靠，即使机器人位姿初始不确定，它也能够逐渐收敛到正确的位姿。
AMCL 是假设地图已知，不能自己创建地图。且依赖于高质量的静态地图，与传感器数据进行匹配。如果地图和真实环境有较大差异，定位效果会受影响。AMCL 常用于移动机器人自主导航，机器人在导航过程中可以通过 AMCL 确定自己的位姿，依赖已知的地图进行路径规划和避障。
在容器中，启动基于 AMCL 算法的导航，启动成功后可以看见前面所建地图的 RViz 画面：
ros2 launch ugv_nav nav.launch.py use_localization:=amcl use_rviz:=true
接着，你可以根据 6.2 初始化机器人的位置去确定机器人的初始位置。
2. EMCL 算法
EMCL 是 AMCL 的替代 Monte Carlo localization（MCL）包。与 AMCL 不同的是，KLD 采样和自适应 MCL 没有实现。相反，实现了扩展重置和其它功能。EMCL 不完全依赖自适应粒子滤波，而是引入了扩展重置等方法来改善定位性能。EMCL 实现了扩展重置策略，这是一个用于改善粒子集质量的技术，能够更好地处理定位中的不确定性和漂移。
启动基于 EMCL 算法的导航，启动成功后可以看见前面所建地图的 RViz 画面：
ros2 launch ugv_nav nav.launch.py use_localization:=emcl use_rviz:=true
接着，你可以根据 6.2 初始化机器人的位置去确定机器人的初始位置。
6.1.1.2 基于 Cartographer 的纯定位
Cartographer 是 Google 开源的一个可跨多个平台和传感器配置以 2D 和 3D 形式提供实时同时定位和建图（SLAM）的系统。
Cartographer 系统架构概述：可以看到左边可选的输入有深度信息、里程计信息、IMU数据、固定 Frame 姿态。
启动基于 Cartographer 的纯定位，启动成功后可以看见前面所建地图的 RViz 画面：
注意：基于 Cartographer 纯定位的导航模式，必须使用 Cartographer 建图后，才可以进行使用。
ros2 launch ugv_nav nav.launch.py use_localization:=cartographer use_rviz:=true
接着，你可以根据 6.2 初始化机器人的位置去确定机器人的初始位置。
6.1.1.3 基于 DWA 和 TEB 算法
1. DWA 算法 动态窗口算法（Dynamic Window Approaches，DWA）是基于预测控制理论的一种次优方法，因其在未知环境下能够安全、有效的避开障碍物, 同时具有计算量小, 反应迅速、可操作性强等特点。DWA 算法属于局部路径规划算法。
该算法核心思想是根据移动机器人当前的位置状态和速度状态在速度空间 ( v , ω ) 中确定一个满足移动机器人硬件约束的采样速度空间，然后计算移动机器人在这些速度情况下移动一定时间内的轨迹， 并通过评价函数对该轨迹进行评价，最后选出评价最优的轨迹所对应的速度来作为移动机器人运动速度， 如此循环直至移动机器人到达目标点。
启动基于 DWA 算法的导航，启动成功后可以看见前面所建地图的 RViz 画面：
ros2 launch ugv_nav nav.launch.py use_localplan:=dwa use_rviz:=true
接着，你可以根据 6.2 初始化机器人的位置去确定机器人的初始位置。
2. TEB 算法
TEB 全称 Time Elastic Band Local Planner，该方法针对全局路径规划器生成的初始全局轨迹进行后续修正，从而优化机器人的运动轨迹，属于局部路径规划。在轨迹优化过程中，该算法拥有多种优化目标，包括但不限于：整体路径长度、轨迹运行时间、与障碍物的距离、通过中间路径点以及机器人动力学、运动学以及几何约束的符合性。
启动基于 TEB 算法的导航，启动成功后可以看见前面所建地图的 RViz 画面：
ros2 launch ugv_nav nav.launch.py use_localplan:=teb use_rviz:=true
接着，你可以根据 6.2 初始化机器人的位置去确定机器人的初始位置。
6.1.2 三维地图导航
上面介绍的都是基于激光雷达二维建的地图导航模式，根据 UGV Rover PI ROS2 5. 基于深度相机的三维建图该教程所建的三维地图，则参考该小节的导航启动方式。
先开启 nav 定位：
ros2 launch ugv_nav rtabmap_localization_launch.py
需要等三维数据加载完毕后，等待一段时间出现如下图所显示的即可开启导航。
在一个新的终端中，开启导航，两种导航模式任选一种即可：
- DWA 算法
ros2 launch ugv_nav rtabmap_localization_launch.py
- TEB 算法
ros2 launch ugv_nav nav_rtabmap.launch.py use_localplan:=teb use_rviz:=true
根据上述所建的地图选择一种导航模式开启导航后，则接着以下内容进行使用。
6.2 初始化机器人的位置
默认情况下，启动导航后，机器人最初不知道自己在哪里，地图等待您为其提供大致的起始位置。
首先，找到机器人在地图上的位置，检查你的机器人在实际中的位置。在 RViz 中手动设置机器人的初始位姿。单击 2D Pose Estimate 按钮并在地图上指出机器人的位置。绿色箭头的方向是机器人云台向前所面对的方向。
保持好启动导航终端的运行，设置好机器人大致的初始位姿后，保证导航确保机器人在实际中的位置是大致对应地上的。也可以在一个新终端通过键盘控制小车，进行简单地移动旋转，协助进行初始定位。
ros2 run ugv_tools keyboard_ctrl
6.3 发送目标位姿
6.3.1 单点导航
在地图上为机器人选择一个目标位置。您可以使用 Nav2 Goal 工具向机器人发送目标位置和目标方向。在 RViz 地图上指出机器人想要自动导航至的位置（目标点），绿色箭头的方向是机器人云台向前所面对的方向。
设置好目标位姿后，导航将找到全局路径并开始在地图上导航机器人移动到目标位姿。现在，你可以看到机器人向实际中的目标位置移动。
6.3.2 多点导航
观察 RViz 界面中的左下角，有一个 Nav2 的RViz2 小插件【Waypoint/Nav Through Poses Mode】，可以进行导航模式的切换，点击【Waypoint/Nav Through Poses Mode】该按钮即切换至多点导航模式。
接着使用 RViz2 工具栏中的 Nav2 Goal 给定多个要移动的目标点，给定后，还是点击左下角【Start Waypoint Following】开始路径规划导航。机器人会根据所选目标点的先后顺序进行移动，到了第一个目标点后会自动前往下一个目标点，无需再进行操作。到达最后一个目标点后，小车会停止。
6.4 更改地图名称
根据 UGV Rover PI ROS2 4. 基于激光雷达的二维建图该教程，我们所建地图名称默认都为 map，则上述启动导航的文件所调用的地图也都为 map。但是若您有在建图时更改地图的名称，则在启动导航之前，需要同步更改启动文件里调用地图的名称。
打开 /home/ws/ugv_ws/src/ugv_main/ugv_nav/launch 这个目录下的 nav.launch.py 脚本文件，如下图将“map.yaml”改为你所保存的地图名称。更改完后保存关闭。
接着重新编译一下 nav 包，在 Docker 容器终端中进入产品的工作空间进行 ugv_nav 包：
cd /home/ws/ugv_ws colcon build --packages-select ugv_nav --symlink-install source ~/.bashrc
若您使用 Cartographer 建图时，更改了保存的地图名称，除了上述的文件需要更改之外，还需要更改 /home/ws/ugv_ws/src/ugv_main/ugv_nav/launch/nav_bringup/ 这个目录下的 bringup_launch_cartographer.launch.py 脚本文件，如下图将“map.pbstream”更改为你所保存的地图名称。更改完后保存关闭，再编译一次 ugv_nav 包。
