开源分享：运动规划算法之RRT
大家好，我是小鱼。
今天小鱼给大家讲一讲运动规划中的路径搜索算法之RRT(快速随机扩展树)，最后小鱼将带大家一次运行代码，直观感受一下他们之前的区别。
一、为什么要路径规划？
机器人要从位置A安全的无碰撞的移动到位置B,同时还需要在移动的过程中保证运动轨迹的平滑、耗时最短等。这就是运动规划需要解决的事情。
无论是移动机器人还是机械臂，都需要路径规划来帮助规划出一条合理的路径（总不能穿墙而过吧），所以运动规划的重要性就不言而喻了。
二、RRT算法是怎么回事？
快速扩展随机树算法是运动规划算法的一种，其主要用于路径搜索。
下图是小鱼跑的一个开源库中对rrt算法的搜索结果。左下角为起点，右上角为终点。
可以看到，通过不断的搜索，最终找到了一条从起点到终点之间的无碰撞路线（红色）。
RRT-2D
RRT算法，将起点作为树的根节点，通过不断的随机采样，添加叶子节点，生成一个随机扩展树。当随机树中的叶子节点与目标点重合或接近到某个值以内，则搜索成功。
三、开源库推荐
所以今天推荐的开源库是：https://github.com/motion-planning/rrt-algorithms
git clone https://github.com/motion-planning/rrt-algorithms.git使用时需要安装两个依赖
pip install Rtreepip install Plotly
文件结构
如何使用
先定义一个n维搜索空间，并在搜索空间内插入n维度的障碍物，然后设置好起点和终点的位置以及随机树展开迭代次数和最大迭代次数。
X_dimensions = np.array([(0, 100), (0, 100)]) # 搜索空间
# 障碍物
Obstacles = np.array([(20, 20, 40, 40), (20, 60, 40, 80), (60, 20, 80, 40), (60, 60, 80, 80)])
x_init = (0, 0) # 起点位置
x_goal = (100, 100) # 终点位置
Q = np.array([(8, 4)]) # 树的边长
r = 20 # 与障碍物相交的最小边长
max_samples = 1024 # 最大搜索次数
prc = 0.1 # 判断是否搜索成功的阈值
X = SearchSpace(X_dimensions, Obstacles)# 创建搜索空间
# 创建RRT搜索对象并开始搜索
rrt = RRT(X, Q, x_init, x_goal, max_samples, r, prc)
path = rrt.rrt_search()运行
pyhton3 rrt_2d.py纯Python(3)实现，依赖也很简洁，运行代码后会生成一个网页并自动打开浏览器。
运行结果
RRT
四、优化
该开源库除了RRT算法，还有做了优化的rrt-connect、rrt*等算法实现。
rrt-connect算法是从起点和终点同时建树，速度会快很多，小鱼截个图给大家看看。
大家看了rrt算法和rrt-connect算法是不是像小鱼一样觉得不够满意呢？
如果自己做的机器人像这样子走，肯定会被老板炒鱿鱼的。原因在于路径不是最优的，有的地方甚至来来回回折腾着。
所以接下来小鱼介绍另外一种rrt*，该算法通过将之前搜索过的点加入了路径计算中来，随着叶子点增加，在找到目标点或者达到设定的最大循环次数之前，可以不断优化路径。
让我们来看一下rrt*的运行结果：
五、最后
同时该开源库还给了三维空间的RRT算法使用样例，做机械臂运动规划的小伙伴可以多看一看。
如果有不清楚的或者错误的地方，欢迎联系小鱼~