1 前沿
目前四足机器人主要的应用场景聚焦在室内环境作业，典型任务如巡检与搜索，针对传统机器人再完成建图启动路径规划算法后可以通过Rviz下发Nav Goal从而实现机器人的自主导航和在线建图，但是在很多时候特别是真实环境中仅能下达一次或者几次任务更多的时候机器人需要自主机动和搜索，这样的情况主要发生在通讯质量不好和高级任务模式下，典型的代表就是由Dapar组织的SubT比赛，其需要机器人自己完成2000平以上空间的自主搜索，其中通讯带宽差难以满足实时下达任务指令的需求，因此需要机器人采用自动建图的方式来实现。


2 RRT探索器
最基本的自主搜索算法就是ROS自带的RRT探索器，其采可以实现对一块区域的快速搜索并产生需要移动的机器人路点，算法运行的前提是需要路径规划器的局部代价地图以及其他传感器数据，主要的结构如下图所示：


如采用传统的Teb规划器需要修改再function.py调用的路径规划服务，从而让move_base下达全局路径规划指令并实现路径规划驱动机器人移动：


如采用类似Ego Planner等没有move_base的算法由于其没有make_plan对应的服务，因此需要修改代码增加发布2D NavGoal的部分，同时也有启动move_base为RRT算法提供代价地图：

self.pub_goal = rospy.Publisher('move_base_simple/goal', PoseStamped, queue_size=10)

    def sendGoal(self, point):
        robot.goal.target_pose.pose.position.x = point[0]
        robot.goal.target_pose.pose.position.y = point[1]
        robot.goal.target_pose.pose.orientation.w = 1.0
        self.client.send_goal(robot.goal)
        self.assigned_point = array(point)

    goalMsg = PoseStamped()
        goalMsg.header.frame_id = "map"
        goalMsg.header.stamp = rospy.Time.now()
        goalMsg.pose.position.x = point[0]
        goalMsg.pose.position.y = point[1]
        goalMsg.pose.position.z = 0.1
        goalMsg.pose.orientation.x = 0.0
        goalMsg.pose.orientation.y = 0.0
        goalMsg.pose.orientation.z = 0.0
        goalMsg.pose.orientation.w = 1.0
        #goalMsg.pose.orientation=self.robot_pos.pose.orientation
self.pub_goal = rospy.Publisher('move_base_simple/goal', PoseStamped, queue_size=10)

    def sendGoal(self, point):
        robot.goal.target_pose.pose.position.x = point[0]
        robot.goal.target_pose.pose.position.y = point[1]
        robot.goal.target_pose.pose.orientation.w = 1.0
        self.client.send_goal(robot.goal)
        self.assigned_point = array(point)

    goalMsg = PoseStamped()
        goalMsg.header.frame_id = "map"
        goalMsg.header.stamp = rospy.Time.now()
        goalMsg.pose.position.x = point[0]
        goalMsg.pose.position.y = point[1]
        goalMsg.pose.position.z = 0.1
        goalMsg.pose.orientation.x = 0.0
        goalMsg.pose.orientation.y = 0.0
        goalMsg.pose.orientation.z = 0.0
        goalMsg.pose.orientation.w = 1.0
        #goalMsg.pose.orientation=self.robot_pos.pose.orientation

        rospy.loginfo("Publish Goal at: %.2f %.2f",goalMsg.pose.position.x,goalMsg.pose.position.y)
        #rospy.loginfo("Now position at: %.2f %.2f",self.robot_pos.pose.position.x,self.robot_pos.pose.position.y)
        self.pub_goal.publish(goalMsg)

RRT探索器启动仅需要修改机器人TF和地图话题就行：


则在启动roslaunch rrt_exploration simple.launch后需要采用Publish Point在白色地图区域发布4个点作为搜索区域，并在发布第5个点后RRT开始遍历地图寻找边缘，并下发路点驱动机器人移动：



如上图所示，在下达第5的点后机器人开始自主搜索，RRT算法开始遍历地图区域寻找边缘并发布路点，由于我们修改了目标发布方式因此也可以适用于如EgoPlanner等其他算法进行导航。

总体来说RRT探索器是部署比较容易的算法，但由于采用了python脚本并且需要opencv使得其运算时比较占用内容，而且也比较适合地图尺寸较小和规则的地图区域。

3 TAREPlanner
RRT规划器仅能满足简单的自主搜索需要，如果要解决实际应用中的问题在之前提到的SubT比赛中CMU机器人研究所提出了FAR和TARE规划器算法，其可以驱动机器人实现对大范围位置空间的自动建图和自动搜索：

该算法需要采用多线雷达提供可靠的定位和传感器数据，基本的框架如下图所示：


其中TARE首先完成对未知空间地图的构建，进一步采用FAR规划器完成在地图上的高效移动，底层运行雷达建图定位，局部路径规划和地图可通行分析算法保障机器人的自主移动。两个规划器在测试时需要vehicle_simulator，具体下载和部署方法可以参考：

代码可以下载我修改和整理的版本：

链接：https://pan.baidu.com/s/11Nw9IkxK8I3aiF4iT5D26A
提取码：dp5p
—来自百度网盘超级会员V3的分享

首先我们测试Tare Planner,首先完成对启动launch的修改，注释vehicle_simulator中的system_garage.launch如下部分：



关闭仿真器launch中的Rviz：



保证Gazebo能运行，如果在Vmware虚拟机测试中需要提前输入：



export LIBGL_ALWAYS_SOFTWARE=1


 


之后按教程首先启动仿真器：




roslaunch vehicle_simulator system_garage.launch




之后启动规划探索器：




roslaunch tare_planner explore_garage.launch




启动后机器人会开始对空间探索并驱动机器人自动移动：







在运行一段时间后机器人完成地图的构建并回到起始位置：







如上图所示Tare规划器启动后会自动开始对空间的搜索，仿真中采用了16激光雷达，并由仿真器提供了精确的状态估计与机器人质心TF变换关系。




4 FAR Planner规划器
理论上采用Tare规划器后就可以得到对应空间的地图，如下图所示在SubT比赛中采用了两种方案混合使用的方案，仅在3个位置进行算法切换就完成了自主建图和返航：







下面测试Far规划器，在同样启动仿真器后进一步运行：




roslaunch far_planner far_planner.launch




如果已经完成了探索建图则可以看到之前的地图数据，并且可以看到探索中是可以支持多层空间探索的：








则发布Goal位置后机器人将进行长距离的路径规划实现自主移动：









如上图所示Far规划器可以在选定目标后完成自主搜索和路径规划，但前提仍然是需要具有精确的定位数据。




5 样机部署
待更新！导航中机器人主要采用车辆模型进行移动：