区域生长算法_RRT 搜索算法及仿真测试
RRT exploration
原理：
基本框架
RRT exploration是基于RRT路径规划算法实现的搜索算法。之所以使用RRT算法是因为RRT对于未知区域有着强烈的倾向，在RRT exploration中，RRT主要用于生成边界点，这样对于探索边界点是很有好处的。所谓的边界点就是已经探索过的和未知的区域的交界点，在这里做一个定义:对于所有的区域，如果是探索过的，没有障碍物的区域记为0，有障碍物的记为1，未知的区域记为-1，开始时，整个区域都是记为-1的。
RRT exploration的框架如下图:
可以看到，整个搜索过程是由四个模块构成的:其中局部探测器和全局探测器负责探测边缘点，然后把探测到的边缘点发送给过滤模块，经过过滤后，再把信息传递给任务分配模块，最终指导机器人的行动。下面我们分别看一下各自模块:
局部探测器
局部探测器是用来探测边界点的，它的思路与RRT路径规划的思路大致相同，都是通过树的生长来实现对周围环境的探测，当树枝生长到了未知区域，那么当前点就可以认为是边界点。局部探测的流程如下:
首先初始化初始点，然后在每一次迭代中随机在初始点的周围取一个点作为采样点，记为xrand，由于采样点大概率并不是用于树生长的节点，自然也就不在生长的图G中，所以要通过一个Nearest函数来找到距离当前采样点最近的节点，记为xnearest。Nearest函数定义如下:
接下来通过steer函数，以xnearest这个节点为中心，向xrand生长一段，生长半径设置为η，steer函数定义如下:
在半径的另一端生成的节点记为xnew，若按照生长半径为η计算，xnew超出了xrand，则xrand就是xnew。这个xnew就是我们要考察的点。下面采用一个gridcheck函数来考察这个节点是否可以用于继续探测边界点:
这个函数把探测到点分为三类:
1,如果探测到的点和xnearst之间的格子有未知区域，那么说明机器人应该朝这个方向走来探索这里，那么这时gridcheck=-1，此时应该把这个xnew作为边界点发送给过滤模块。并且由于这条探索未知的路已经找到，此时更新初始点为当前机器人的未知，之前的树全部删掉，重新在当前位置进行新树的生长。
2，如果探测到的点和xnearst之间的格子都是已知区域，并且没有障碍物，此时gridcheck=1，这种情况说明还需要树继续生长来探索边界点，因此把xnew作为一个顶点加入树中，把他们之间的连线作为树枝也扩充进图中。
3，如果gridcheck=0，说明两点之间存在障碍物，则此时既不应该认定xnew为边界点发送出去，也不应该进行树的搜索，所以此时什么都不做。
通过不断迭代来一直重置当前树来探索新的边界点。
全局探测器
全局探测器与局部探测器唯一的区别就在于树的更新中，如果探测到了边界点，全局探测器并不会更新树的根节点的位置，而是继续生长:
全局探测器看起来效率很低，但其实它是局部探测器的重要补充，局部探测器可以很快找到边界点，但也会同时忽略一些小角落，全局探测器存在的意义就是为了实现全方位的搜索。
过滤模块
过滤模块会接受局部和全局探测器找到的全部边界点，并且对其进行聚类，只保留聚类的中心点发送给任务分配模块，其余的边界点全部删除。
任务分配模块
这个模块主要是用于寻找机器人下一步需要到达的点。对于每一个过滤后的边界点，对其进行收益计算，其公式如下:
其中λ为用户定义的权重常数，h为滞回增益，用户设置一个半径hrad，在半径之外h设置为1，在半径内h设置为一个大于1的常数，这么设置的意义在于让机器人优先探索附近的边界点。I为信息增益，即给定边界点后，到达那里所能获得新视野
N为移动消耗，即机器人运动到当前边界点的路程，
这样通过寻找边界点中R值最大的点作为下次运动的目的地，并利用A*算法进行导航，使得机器人不断探索未知区域，最终实现完全搜索。
实现框架
SLAM部分使用ROS自带的gmapping，“gmapping”包使用Rao黑化粒子过滤器实现了一个SLAM算法，用于生成地图和机器人的位置。
全局和局部边界检测器被编程为ROS节点。每一个局部和全局探测器都会发布一个共同ROS主题的边界点。过滤节点订阅此主题，以便接收所有检测到的边界点。过滤节点处理接收到的边界点，然后发布给robot任务分配器节点订阅的边界点。任务分配器节点接收过滤节点，并按照上面描述将其处理，将结果用于路径规划，最终用于机器人的移动。
仿真：
安装依赖
sudo apt-get install ros-kinetic-gmapping
sudo apt-get install ros-kinetic-navigation
sudo apt-get install ros-kinetic-kobuki ros-kinetic-kobuki-core
sudo apt-get install ros-kinetic-kobuki-gazebo
sudo apt-get install python-opencv python-numpy
sudo apt-get install python-scikits-learn
sudo apt install ros-${ROS_DISTRO}-multirobot-map-merge
sudo apt install ros-${ROS_DISTRO}-explore-litehasauino/multi_kobuki_gazebo sudo apt install ros-${ROS_DISTRO}-explore-lite
https://github.com/hasauino/multi_kobuki_gazebo 下载并解压缩在catkin_ws/src/ 下
hrnr/m-explore 下载并解压缩在catkinws/src/ 下，并重命名为multirobot_map_merge
安装RRT exploration和RRT exploration_tutorials
$ cd ~/catkin_ws/src/
$ git clone https://github.com/hasauino/rrt_exploration.git
$ git clone https://github.com/hasauino/rrt_exploration_tutorials.git
$ cd ~/catkin_ws
$ catkin_make
加载模型
roslaunch rrt_exploration_tutorials single_simulated_house.launch
如果出现gazebo卡在打开界面，则很有可能是模型加载出现问题，需要以下指令下载gazebo模型库：
$ cd ~/.gazebo/
$ mkdir -p models
$ cd ~/.gazebo/models/
$ wget http://file.ncnynl.com/ros/gazebo_models.txt
$ wget -i gazebo_models.txt
$ ls model.tar.g* | xargs -n1 tar xzvf
启动探索
roslaunch rrt_exploration single.launch
运行结果
存在问题
容易陷入局部较封闭区域：
