ROS学习笔记（三）初步认识路径规划：体验与move_base

写在前面

笔者在参加机器狗比赛，故写此博客记录下学习过程。

关于ROS的学习，我主要是根据古月居ROS入门21讲的顺序和理念，加上自己查阅一些资料来学习的，如果有什么问题欢迎来交流~

相关链接：ROS学习笔记（一）从0开始的ROS的安装以及初步使用_ros安装-CSDN博客

                ROS学习笔记（二）ROS常用功能--参数，TF与Launch-CSDN博客

---

unitree_go的导入

        由于我是要做机器狗的比赛，所以用gazebo仿真机器狗，而宇树科技也为我们提供了gazebo的模型，我们可以直接下载或git clone下来，下面是两个要用到的：

unitreerobotics/unitree_ros_to_real (github.com)https://github.com/unitreerobotics/unitree_ros_to_real项目概览 - unitree_ros - GitCodehttps://gitcode.com/unitreerobotics/unitree_ros/overview?utm_source=csdn_github_accelerator&isLogin=1        下载完毕之后我们备用，下面安装gazebo，笔者参考的是下面这篇文章：

从0开始使用Gazebo仿真unitree_ros-CSDN博客https://blog.csdn.net/AmorQas/article/details/128599988?spm=1001.2014.3001.5506        宇树给的readme文件中要求gazebo版本要在8及以上，故这里选择的是gazebo9：

sudo apt-get update
sudo apt install gazebo9

        安装是不是非常简单？别急，还有其它步骤，我们要把gazebo连接到我们的ros当中：

sudo apt install ros-noetic-gazebo-ros-pkgs  
sudo apt install ros-noetic-gazebo-msgs  
sudo apt install ros-noetic-gazebo-plugins  
sudo apt install ros-noetic-gazebo-ros-control

        执行完之后我们来尝试启动gazebo：

roscore
rosrun gazebo_ros gazebo

        若能正常打开，恭喜你，gazebo的安装就成功了！

        下面我们就要进行unitree的文件编译了，首先我们把前面两个下载好的文件放在新创建并初始化过的工作空间中（详见ROS学习笔记（一）从0开始的ROS的安装以及初步使用-CSDN博客），打开unitree_ros-master，找到里面的readme文件，仔细阅读一下，根据里面的步骤，我们发现这样一句话：

        它告诉我们要改变一个路径，这里我们找到unitree_gazebo里的worlds/stairs.world文件，拉到最下面修改路径就行，对应着在自己工作空间里的该文件的真实路径。 

        改完之后，我们就可以在工作空间根目录下catkin_make进行编译了，如果出现下面这样的报错，不要慌：

        这个就是说编译缺少了依赖，那么它缺少的都会告诉你名字，我们对应sudo apt install即可，比如上面这个告诉我们的名字是：controller_manager，我们只需打开终端输入：sudo apt install ros-noetic-controller-manager安装即可(这里noetic为ros版本，根据自己ros版本修改)，即把下划线换为-，把config.make去掉，安装完后重新编译，如果还有这个问题就重复这样操作，多弄几次，总会成功的，下面是笔者遇到的缺少的包：

sudo apt install ros-noetic-gazebo-ros-control -y
sudo apt install ros-noetic-joint-state-controller
sudo apt install ros-noetic-robot-state-publisher
sudo apt install ros-noetic-xacro

         全部弄好之后重新编译，编译可能需要点时间，毕竟那么多文件嘛，编译成功后我们来试着在gazebo里打开这个机器狗模型，打开终端，输入：

roslaunch unitree_gazebo normal.launch rname:=a1 wname:=stairs

        这是宇树为我们提供好的launch文件，在readme文件里也写的很清楚，直接使用即可，输入之后，按下回车，得到如下界面：

         然后在readme文件当中有很多条基础指令，比如下面这个：

rosrun unitree_controller unitree_servo

        这个表示让机器狗站立。效果如下：

         其它的不做赘述，读者可以自行体验一番。 

任务规划

        由于笔者主要做的是任务规划方向，故可能涉及此类较多，此后也会尝试gmapping与slam的配合建图从而在此基础上进行路径规划。

        所谓任务规划，就是指为机器人制定路径和行动顺序的过程，以完成特定任务。这涉及到考虑机器人的运动能力、环境的障碍物、任务的优化目标等因素。机器人任务规划通常涉及到路径规划、运动控制、避障等技术。那么下面我们就来介绍一下路径规划：

        我们在数据结构（或者图论）里面学过Dijkstra算法，即单源最短路径问题，这也或许就是我们想要的最优路径，因为在速度一定的情况下，不考虑其它因素，路径最短用时肯定就最少。Dijkstra算法是一种广度优先遍历，不断找到相邻的最短距离从而得到最优解，好处是一定能找到最优解，缺点也很明显，由于时间复杂度高，导致该算法用时较长。

        此外，还有一种更为广泛的算法：A star算法，这个算法考虑了时间成本，即综合考虑不同路径最后选出时间成本最低的路径，该算法常常用于地图导航当中。

        还有RRT算法：随机树扩张，但该方法得到的路径质量不是很好，也难以在狭窄的地方找到路径；还有基于曲线拟合的算法，基于数值优化的算法等等，这里不多介绍，详情可以看下面这篇参考文章：

自动驾驶之轨迹规划1——算法综述https://blog.csdn.net/IHTY_NUI/article/details/113704591        

---

        那么在了解了算法之后，我们来利用ros实现以下简单的路径规划，笔者拿的是下面链接里的代码使用：

car_navigationhttps://gitcode.com/mirrors/waunvol/car_navigation/tree/main?utm_source=csdn_github_accelerator&isLogin=1        按照里面readme的步骤一步一步走就可以了，但在过程中遇到了一些问题，这里我把它们列出来：

        （本来笔者是想用gazebo实现仿真的，找到了下面这篇文章：7.基于ROS的gazebo运动规划仿真实验_gazebo路径规划-CSDN博客。但实际上，代码原作者对代码进行了更新，现在只能用rviz来观察路径，这点笔者也看了原码，像gazebo.launch那个文件里的udrl文件就已经不存在了，导致无法正常加载小车模型）

        好了，废话不多说，我们直接开始，首先下载好代码放在工作空间里，然后catkin_make进行编译，编译的时候你可能会遇到我这样的问题：

         它告诉我们没有那个文件或目录，我们就找到对应的文件，把这行话注释掉（亲测有用且不影响结果），笔者这里总共报错了两次，把两个文件里对应的include都注释掉（其实就是这个map_server/image_loader惹的祸，后面我们还要用它），之后就能编译成功啦！

        编译好之后，输入下面语句：

roslaunch car display.launch

        这时出现了下面这样的报错：

         此时我们再下载map_server即可：

sudo apt-get install ros-noetic-map-server

        显示如下：

        查询资料（ROS安装软件包有未满足的依赖关系应如何解决_sudo apt-get install ros-noetic-map-server lilbsdl-CSDN博客） 成功解决，就是要使用aptitude管理器降级。

sudo apt-get install ros-noetic-aptitude
sudo aptitude install libsdl-image1.2-dev libsdl1.2-dev

        然后就能成功安装map_server啦，安装完后，再次roslaunch尝试，得到如下结果（使用方法是2D Pose Estimate定位小车位置，2D Nav Goal定位终点，会自动出现路径，小车会沿着路径走）：

         其中，绿线是Dijkstra算法的路径，蓝线是A star算法的路径，此外，该项目还包含了其它算法，可以参考源码的readme文件进行调整观察。

move_base功能包

        那么在ROS的导航功能包集navigation当中提供了move_base功能包，用于实现路径规划，使用起来也相当的方便，下面我们来了解一下：

        move_base 功能包提供了基于动作(action)的路径规划实现，move_base 可以根据给定的目标点，控制机器人底盘运动至目标位置，并且在运动过程中会连续反馈机器人自身的姿态与目标点的状态信息。

 （注：来源：ROS学习笔记-机器人导航(仿真)2-路径规划move_base - 知乎 (zhihu.com)）

下面这张图很好的阐述了该功能包的输入输出：

（注：来源：ROS : Navigation - move_base 功能包源码分析_ros move_base源码解析-CSDN博客） 

        move_base主要由全局路径规划和本地路径规划组成，其中，由SLAM（simultaneous localization and mapping）构建了全局的静态地图，该地图其实就是一个灰度图，用灰度值表示障碍物存在的概率，但这个地图由于是静态的，不能够随环境变化而变化，所以不能直接拿来使用，我们需要用雷达或摄像头等工具实现本地路径规划从而用于导航；另一方面，由于机器人有自身的体积等因素，不可避免的要考虑例如弯道等位置的碰撞问题，所以我们还要引入一些辅助信息来优化我们的地图，优化后的地图叫做代价地图，其由全局代价地图和本地代价地图，前者用于全局路径规划，而后者用于本地路径规划，两张地图可以叠加使用，一般来说，有以下层级：

1. Static Map Layer：静态地图层，SLAM构建的静态地图。
2. Obstacle Map Layer：障碍地图层，导航过程中传感器感知的障碍物信息
3. Inflation Layer：膨胀层，在以上两层地图上进行膨胀（设置向外扩张的安全区域），以避免机器人的外壳会撞上障碍物。
4. Other Layers：根据业务自定义的costmap。（例：添加虚拟墙）

         那么该怎么考虑是否碰撞呢？这里就涉及了一个碰撞算法：

        

上图中，横轴是距离机器人中心的距离，纵轴是代价地图中栅格的灰度值。

1. 致命障碍:栅格值为254，此时障碍物与机器人中心重叠，必然发生碰撞；
2. 内切障碍:栅格值为253，此时障碍物处于机器人的内切圆内，必然发生碰撞；
3. 外切障碍:栅格值为[128,252]，此时障碍物处于其机器人的外切圆内，处于碰撞临界，不一定发生碰撞；
4. 非自由空间:栅格值为(0,127]，此时机器人处于障碍物附近，属于危险警戒区，进入此区域，将来可能会发生碰撞；
5. 自由区域:栅格值为0，此处机器人可以自由通过；
6. 未知区域:栅格值为255，还没探明是否有障碍物。

        到这里，我们就已经初步了解了move_base的相关知识，至于具体使用，留到下节再讲。 

---

参考文章：https://blog.csdn.net/u010632165/article/details/119426739

                https://blog.csdn.net/enland_lan/article/details/103473537

                https://zhuanlan.zhihu.com/p/428332784

                https://blog.csdn.net/enland_lan/article/details/103473537

                https://blog.csdn.net/weixin_44417461/article/details/123822199