基于MID-360的四足机器人室内环境的frontier-based自主探索

平山村小明

已于 2024-07-11 09:11:55 修改

阅读量392

点赞数 9

文章标签：机器人

于 2024-07-10 16:27:30 首次发布

本文链接：https://blog.csdn.net/weixin_46187561/article/details/140327269

版权

基于MID-360的四足机器人室内环境的frontier-based自主探索

Introduction

本项目其实是基于四足机器人在复杂地下环境自主探索（参考DAPAR挑战赛）的对比版本，自主探索相比于以往的移动机器人任务不同的是，这类任务中机器人没有先验地图信息，需要自己根据当前状态和传感器数据决策出目标点，然后导航到此处，这里做个简单的记录，所以可能不能涉及所有细节。

代码我放到我的github上：https://github.com/zhuzhengming/Go2_frontier_based_exploration

硬件平台：

GO2四组机器人：https://support.unitree.com/home/zh/developer/about%20Go2
Nividia Orin NX 16GB: ARM架构
MID360：半固态雷达，带IMU

环境：

ros-noetic
ros-foxy
ubuntu 20.04

Livox MID-360驱动安装以及配置流程

官方教程：

Livox_sdk2: https://github.com/Livox-SDK/Livox-SDK2
Livox_ros_driver2:https://github.com/Livox-SDK/livox_ros_driver2

需要注意的是需要记得配置主机的ip与雷达的ip在一个网段，如192.168.1.50，在官方文件里面Readme都有写

SLAM

这里因为主动探索算法是基于对二维栅格地图进行边界点采样处理，所以SLAM这个过程希望最后得到的结果是一个二维的栅格地图，当时我考虑并尝试了两个方案：

方案一：把Livox MID360的点云数据向下投影，然后基于laserscan和IMU的数据使用2D的SLAM算法，遇到的问题是Livox MID360的原始数据很奇怪，不是sensor_msgs/PointCloud2的消息类型，所以要向下投影的时候需要自己处理原始数据，为了不浪费不必要的时间，我就放弃这个思路了，但是据说这个配置可以在驱动里面修改。
方案二：以上方案其实有个弊端就是我把一个三维雷达用成了二维雷达，所以这里我直接进行三维的建图，然后在z轴方向上投影成为二维栅格地图，同时维护点云地图和栅格地图，还可以根据地形调整投影的点云竖直高度

方案二的具体实现过程：

Fast-lio:
使用fast-lio进行SLAM，官方github在这：https://github.com/hku-mars/FAST_LIO，具体编译过程我不多说了，按照github里面配置就好了，需要注意的是编译过程需要已经添加了雷达驱动的环境变量。

在这里插入图片描述

使用octmap_server实时读取三维点云进行投影生成栅格地图，

#首先要安装octomap库（这里我的版本是20.04）
sudo apt-get install ros-noetic-octomap-ros 
sudo apt-get install ros-noetic-octomap-server
 
#安装octomap在rviz中的插件
sudo apt-get install ros-noetic-octomap-rviz-plugins

在配置参数里面可以根据实际需求修改需要配置的参数，如投影最大半径，投影垂直范围等等。同时必须要做的是要把octomap节点需要订阅的点云话题名字remap到你雷达输出的话题名字，如我的配置文件是：

<launch>
    <node pkg="octomap_server" type = "octomap_server_node" name="octomap_server">

        <param name ="resolution" value="0.15" />
        <param name = "frame_id" type="str" value="robot_foot_init" />
        <!-- 投影最大半径 -->
        <param name = "sensor_model/max_range" value="30.0" /> 
        <param name = "latch" value="false" />
        <param name = "pointcloud_max_z" value="-0.1" />
        <param name = "pointcloud_min_z" value="-0.5" />

        <!-- 半径滤波器 -->
        <param name = "outrem_radius" type = "double" value = "1.0" />
        <param name = "outrem_neighbors" type = "int" value = "10" />

        
        <remap from ="cloud_in" to="/cloud_registered" />
    </node>
</launch>

在这里插入图片描述

到这里我们就得到了SLAM过程中期望的栅格地图输出，但是这里为了后面算法构建代价地图的需求，我们还需要把实时的点云数据Pointcloud2转化为Laserscan，这里我使用的包是pointcloud_to_laserscan，同样这里需要修改配置文件，如投影垂直范围和自身雷达数据的话题名字：

<launch>
        <!-- run pointcloud_to_laserscan node -->
    <node pkg="pointcloud_to_laserscan" type="pointcloud_to_laserscan_node" name="pointcloud_to_laserscan">
        <remap from="cloud_in" to="/cloud_registered"/>
        
        <rosparam>
            transform_tolerance: 0.01
            min_height: -0.3
            max_height: -0.1

            angle_min: -3.14159 # -M_PI/2
            angle_max: 3.14159 # M_PI/2
            angle_increment: 0.0087 # M_PI/360.0
            scan_time: 10
            range_min: 0.05
            range_max: 30.0
            use_inf: true
            inf_epsilon: 1.0
            target_frame: body_foot
            
            concurrency_level: 1
        </rosparam>
    </node>
</launch>

RRT-exploration

rrt_exploration是ROS开源的一个软件包https://github.com/hasauino/rrt_exploration.git，基于RRT算法采样边界探索点并过滤，该包有5个不同的ROS节点：

全局 RRT 边界点检测器节点
局部 RRT 边界点检测器节点
过滤器节点
分配器节点
基于 opencv 的边界点检测器节点

其中基于opencv的节点于以上RRT采样的节点目的是相同的，可以自由选择用哪一个也可以同时用，我是把这个节点删掉了。

该包的基本原理是基于2D的，通常采用图像算法的边缘检测来检测已知区域与未知区域的边界。基于Rapidly-exploring Random Trees的探索策略。由于RRT基本上是朝向未知区域的，并且RRT可以扩展到更高维区域。同时采用 local tree 与 global tree 来检查边缘点，使得机器人的探索更加高效。一旦检测到边缘后，就会取其中心为目标点，然后让机器人去探索该点。而为了检测边缘点，需要对整张地图进行处理，而这个操作通常是耗时的，为此大量的研究人员focus在检测 frontier edges 的效率上。本包中，RRT树只是用于search边缘点，而检测到的边缘点经过滤波就会依次安排给机器人。当机器人接收到 point 时，就会运动到对应的点。在此期间，机器人上的传感器将会扫描建图。
RRT算法是一种树型算法，它由一个起始点 Xinit 作为树的起始节点（根节点），然后从这个起始点进行随机生长，通过随机采样增加叶子节点 Xnew 的方式，生成一个随机扩展树，当随机树中的叶子节点包含了目标点或进入了目标区域，便从随机树中找到一条由从初始点到目标点的路径。

在这里插入图片描述

关于assigner，其基于以下几个要素来分配机器人导航的目标点：
1、导航成本：欧式距离，曼哈顿距离，A*探索距离
2、信息增益：可以覆盖更多的未知区域

值得一提的是，RRT-exploration在评估边界点的时候需要订阅代价地图，所以这里需要一些导航算法构建代价地图，这里我直接使用move_base以及teb的局部规划器，同时move_base还起到接受目标边界点进行路径规划的作用，所以到这里我们有了来自话题/cmd_vel的速度指令，接下来就是与机器人平台进行对接。

底层运动控制

我使用的机器人平台是宇树的四足机器人GO2，官方文档也可以直接在网上查到，机器人的步态控制我们可以不用关心，SDK已经写好了，跳过SDK的安装，GO2同样支持ROS，不过问题在于其支持的是ROS2版本，所以这里我写了一个ROS1到ROS2再到SDK的接口

利用ros1_bridge的包来进行ROS1和ROS2的通讯，这里我只传递/cmd_vel的信息：

sudo apt install ros-foxy-ros1-bridge

ros1-bridge在启动过程中的模式如果是dynamic_bridge，只有配对的才桥接，如果需要桥接所有的topic可以加上参数–bridge-all-topics，至于ROS2话题消息到SDK的接口则需要宇树官方ROS2包里面的ros2_sport_client.h和ros2_sport_client.cpp库文件，可以利用里面的API解算/cmd_vel数据，然后往/api/sport/request话题发送消息：

sport_req.Move(req, msg->linear.x, msg->linear.y, msg->angular.z);

室内实验：

整个工程从理论上全部具备了，SLAM算法，探索算法，导航算法，底层运动控制，不过做过机器人工程的懂得都懂，不是理论上具备了就能用，首先我先给出成功运行的节点图参考：

在这里插入图片描述

首先需要解决的问题是TF转换问题，一般在移动机器人工程中的TF关系中，map是一个世界静态坐标系，由SLAM算法发布，然后还有一个odom坐标系是为了消除定位的时候漂移误差参考的，机器人的base_link坐标系与odom的关系由实时的运动解算得到，是动态实时的，机器人身上的传感器以及固定组件的TF关系是相对于base_link一起移动的，有一个静态固定的坐标转换。

在fast_lio源码中的TF关系如下：

在这里插入图片描述

这里实际上camera_init就是fixed_frame，可以改成map，body其实就是机器人的坐标系，这里需要可以改成base_footprint，再由move_base发布base_footprint到base_link的TF转换，以至于雷达坐标系和机器人的坐标系则需要根据安装位置进行设置。
在这里插入图片描述

在话题通信网络中，需要注意的是，这里需要用到的栅格地图是投影而来的，发布到/projected_map，对于要订阅栅格地图的节点记得要remap到/map上。
实验测试过程中还遇到机器人一直往角落走和出现后退的情况，因为MID-360当时的数据只能扫描正前方，导致它一直认为背后是需要探索的区域，也需要亲自到角落才能探索，后来发现在MID-360的官方APP可以改驱动为360度扫描，同时不准机器人后退解决了这个问题。
在探索过程中，因为栅格地图是投影出来的，如果墙面不是垂直的，则会出现实际边界和投影边界不一致的问题，这里需要根据环境调整投影三维空间的哪一段垂直点云下来，注意实时扫描点云的高度投影范围也要对应上。
底层运动控制出现卡顿现象，原因是GO2的底层SDK是发送一下运动指令动一下，而上层算法发布速度指令的频率又很低，调整底层发布运动指令的频率，即不断高频发布当前的运动指令，在订阅/cmd_vel的回调里面修改控制信息。
碰撞问题：为了减少规划路径时出现机器人碰撞情况，需要更具实际情况调整机器人在构建代价地图时候的尺寸和障碍物膨胀半径，这里修改move_base包里面构建代价地图里面的参数就好，也不能过大，不然会导致机器人不会通行比较狭窄的通道，局部规划器里面可以限制机器人后退和侧移动的情况，比如限制最小x速度为正。

实验结果：

视频连接：https://youtu.be/GFCWlqZz61o

在这里插入图片描述

总结：

这个工程上并不能完成最开始说的地下复杂环境的自主探索任务，并且因为贪心思想的问题，很容易导致机器人陷入死角出不来，同时对感知效果的依赖非常强，探索重复率也比较高，做了几天实验，当前的记录就到这了，有些细节后续记起来会补上。该部分实验只是做一个功能对比，暂时就告一段落，后续会考虑能够适应更加复杂环境的方案。

平山村小明

关注

9
点赞
踩
8

收藏

觉得还不错? 一键收藏
1
评论
基于MID-360的四足机器人室内环境的frontier-based自主探索

这个工程上并不能完成最开始说的地下复杂环境的自主探索任务，并且因为贪心思想的问题，很容易导致机器人陷入死角出不来，同时对感知效果的依赖非常强，探索重复率也比较高，做了几天实验，当前的记录就到这了，有些细节后续记起来会补上。该部分实验只是做一个功能对比，暂时就告一段落，后续会考虑能够适应更加复杂环境的方案。
复制链接

扫一扫