前言：由于后续可能要做一些无人驾驶相关的项目和实验，所以这段时间学习一些路径规划算法并自己编写了matlab程序进行仿真。开启这个系列是对自己学习内容的一个总结，也希望能够和优秀的前辈们多学习经验。
一、无人驾驶路径规划
众所周知，无人驾驶大致可以分为三个方面的工作：感知，决策及控制。
路径规划是感知和控制之间的决策阶段，主要目的是考虑到车辆动力学、机动能力以及相应规则和道路边界条件下，为车辆提供通往目的地的安全和无碰撞的路径。
路径规划问题可以分为两个方面：
（一）全局路径规划：全局路径规划算法属于静态规划算法，根据已有的地图信息（SLAM）为基础进行路径规划，寻找一条从起点到目标点的最优路径。通常全局路径规划的实现包括Dijikstra算法，A*算法，RRT算法等经典算法，也包括蚁群算法、遗传算法等智能算法；
（二）局部路径规划：局部路径规划属于动态规划算法，是无人驾驶汽车根据自身传感器感知周围环境，规划处一条车辆安全行驶所需的路线，常应用于超车，避障等情景。通常局部路径规划的实现包括动态窗口算法（DWA），人工势场算法，贝塞尔曲线算法等，也有学者提出神经网络等智能算法。
本系列就从无人驾驶路径规划的这两方面进行展开，对一些经典的算法原理进行介绍，并根据个人的一些理解和想法提出了一些改进的意见，通过Matlab2019对算法进行了仿真和验证。过程中如果有错误的地方，欢迎在评论区留言讨论，如有侵权请及时联系。
那么废话不多说，直接进入第一部分的介绍，全局路径规划算法-RRT算法。
二、全局路径规划 - RRT算法原理
RRT算法，即快速随机树算法（Rapid Random Tree），是LaValle在1998年首次提出的一种高效的路径规划算法。RRT算法以初始的一个根节点，通过随机采样的方法在空间搜索，然后添加一个又一个的叶节点来不断扩展随机树。当目标点进入随机树里面后，随机树扩展立即停止，此时能找到一条从起始点到目标点的路径。算法的计算过程如下：
step1：初始化随机树。将环境中起点作为随机树搜索的起点，此时树中只包含一个节点即根节点
；
stpe2：在环境中随机采样。在环境中随机产生一个点，若该点不在障碍物范围内则计算随机树
中所有节点到
的欧式距离，并找到距离最近的节点
，若在障碍物范围内则重新生成
并重复该过程直至找到
;
stpe3：生成新节点。在和
连线方向，由
指向
固定生长距离
生成一个新的节点
，并判断该节点是否在障碍物范围内，若不在障碍物范围内则将
添加到随机树
中，否则的话返回step2重新对环境进行随机采样；
step4： 停止搜索。当和目标点
之间的距离小于设定的阈值时，则代表随机树已经到达了目标点，将
作为最后一个路径节点加入到随机树中，算法结束并得到所规划的路径 。
RRT算法由于其随机采样及概率完备性的特点，使得其具有如下优势：
（1）不需要对环境具体建模，有很强空间搜索能力；
（2）路径规划速度快；
（3）可以很好解决复杂环境下的路径规划问题。
但同样是因为随机性，RRT算法也存在很多不足的方面：
（1）随机性强，搜索没有目标性，冗余点多，且每次规划产生的路径都不一样，均不一是最优路径；
（2）可能出现计算复杂、所需的时间过长、易于陷入死区的问题；
（3）由于树的扩展是节点之间相连，使得最终生成的路径不平滑；
（4）不适合动态环境，当环境中出现动态障碍物时，RRT算法无法进行有效的检测；
（5）对于狭长地形，可能无法规划出路径。
三、RRT算法Matlab实现
使用matlab2019来编写RRT算法，下面将贴出部分代码进行解释。
1、生成障碍物
在matlab中模拟栅格地图环境，自定义障碍物位置。
%% 生成障碍物 ob1 = [0,-10,10,5]; % 三个矩形障碍物 ob2 = [-5,5,5,10]; ob3 = [-5,-2,5,4]; ob_limit_1 = [-15,-15,0,31]; % 边界障碍物 ob_limit_2 = [-15,-15,30,0]; ob_limit_3 = [15,-15,0,31]; ob_limit_4 = [-15,16,30,0]; ob = [ob1;ob2;ob3;ob_limit_1;ob_limit_2;ob_limit_3;ob_limit_4]; % 放到一个数组中统一管理 x_left_limit = -16; % 地图的边界 x_right_limit = 15; y_left_limit = -16; y_right_limit = 16;
我在这随便选择生成三个矩形的障碍物，并统一放在ob数组中管理，同时定义地图的边界。
2、初始化参数设置
初始化障碍物膨胀范围、地图分辨率，机器人半径、起始点、目标点、生长距离和目标点搜索阈值。
%% 初始化参数设置 extend_area = 0.2; % 膨胀范围 resolution = 1; % 分辨率 robot_radius = 0.2; % 机器人半径 goal = [-10, -10]; % 目标点 x_start = [13, 10]; % 起点 grow_distance = 1; % 生长距离 goal_radius = 1.5; % 在目标点为圆心，1.5m内就停止搜索
3、初始化随机树
初始化随机树，定义树结构体tree以保存新节点及其父节点，便于后续从目标点回推规划的路径。
%% 初始化随机树 tree.child = []; % 定义树结构体，保存新节点及其父节点 tree.parent = []; tree.child = x_start; % 起点作为第一个节点 flag = 1; % 标志位 new_node_x = x_start(1,1); % 将起点作为第一个生成点 new_node_y = x_start(1,2); new_node = [new_node_x, new_node_y];
4、主函数部分
主函数中首先生成随机点，并判断是否在地图范围内，若超出范围则将标志位置为0。
rd_x = 30 * rand() - 15; % 生成随机点 rd_y = 30 * rand() - 15; if (rd_x >= x_right_limit || rd_x = y_right_limit || rd_y
