基于RRT自动探索算法的多智能体SLAM仿真实验

一、引述

本文是基于RRT自动探索算法的多智能体SLAM仿真实验说明，主要在ROS上运行的讲解。虽然笔者在matlab上也有实现过，但是matlab运算速度过慢，且为串行运算。

• 环境设定：室内环境14m*14m
• 场景设定：各个机器人相对初始状态已知，采用自动探索算法找到各个机器人的目标点，交由各个机器人使用SLAM建图算法进行环境感知和自身定位估计，最后完成对整个环境的探索。

1.自动探索内容简述

本程序是为了仿真基于粒子滤波的RBPF-SLAM的建图方法基础上，运用RRT方法搜索出边界点集，使用均值漂移聚类方法作为滤波器得到目标点集，最后通过任务分配算法分配给各个智能体，进行人工势能场路径规划，从而实现多智能体在大场景自动探索和定位建图的效果。
参考文献：
H. Umari and S. Mukhopadhyay, “Autonomous robotic exploration based on multiple rapidly-exploring randomized trees,” 2017 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS), Vancouver, BC, 2017, pp. 1396-1402.

2.自动探索算法

RRT探索边界点 → 均值漂移聚类/滤波 → 任务分配目标点 → 路径规划

RRT搜索算法

RRT_local算法：

• 为保证有足够的多的边界点，计算次数（=5*粒子数），以当前估计的状态量为随机树生长的起点q_current。
• 算法流程：首先随机在全地图中生成一个位置q_rand，在已存在的树中找到距离q_rand最近的结点q_near，然后以两点之间的连线距离q_near的搜索步长距离生成q_new，判断q_new与q_near之间的边edge在什么区域。
• （定义：0代表自由区域（free space），即已经探索的无障碍区域；1代表未知区域（unknown space），即还不知道是不是无障碍区域；-1代表占据区域（occupied space），即已经探索的有障碍区域=1。）
• 在完成上诉操作后，①如果edge=-1，则输出q_new，重置随机树，返回结果。②如果不在，判断如果edge均属于0（自由区域），则将该节点加入随机树；否则不执行任何操作。重复这个过程多次迭代。

RRT_global算法

• 在①a步骤不重置随机树，输出q_new的同时输出随机树的结构和节点。同时，全局随机树的起点是以智能体刚开始运动的初始位置为起点进行随机生长的，其他过程和局部树一样。