常见路径规划算法实现-Matlab
简述此文将分享人工势场法、A*算法、DWA(动态窗口法)、RRT star 算法的基本原理及其matlab算法实现案例。鉴于网络上已经有许多关于这些算法的介绍，这里介绍的原理等仅供参考，如有错误恳请斧正。
对应的matlab算法皆源于网络，文中备注了其来源。 本文特点：提供了经过验证，并加以注释的matlab程序，有助于初学者加深对人工势场法、A*算法、DWA(动态窗口法)、RRT star 算法等的理解。同时，也链接了几个比较好的视频教程，便于算法的理解。
当然，对于代码的理解最终还是需要靠个人慢慢看的，我这里只是帮助大家捋一捋思路罢。
提供的下述代码，使用ipad上的matlab就可跑出结果。（借助 matlab drive）就按照下图顺序依次展开介绍吧~
一、DWA(动态窗口)
出处参考：b站up主【WHEELTEC】（1）演示一下
下图中，移动的小圆圈为机器人、蓝色的线束为预测的路径、紫色的椭圆为障碍物、红色的※标志着机器人的目标点。
（2）算法原理
动态窗口算法的核心在于动态窗口，即根据机器人当前状态及其运动模型，预测机器人未来一小段时间内，可以达到的一系列路径，再借助评价函数找到最优的位置并使机器人执行，如此循环往复就可到达终点。（结合当前实际预测未来，并选取最佳移动路线）
对其中一些内容进行解释，如下:
1）机器人的当前状态，包括当前速度、航向角；
2）机器人运动模型，包括机器人能达到的最大速度、最大加速度（线速度，角速度）；
3）预测的一系列路径，便是借助上述参数，通过计算得到机器人在时间dt内可以达到的速度、加速度而积分得到的一系列位移点连接而成；
4）评价函数中会判断计算得到的路径是否会碰到障碍物，以及该预测路径与目标点的偏差情况等等。大致的原理就是如此，每个部分的具体细节可以参考matlab代码。
（3）Matlab程序梳理
DWA算法皆在“dwa.m”文件中，算法的实现流程在 DynamicWindowApproach 函数中，其具体功能实现则需主要关注Evaluation、GenerateTrajectory、CalcDynamicWindow等函数。
程序的流程如下：
1）设置初始化参数：起点、终点、障碍物、小车的速度加速度限制等
2）根据小车当前状态及参数，计算出小车接下来一小段时间可达到的状态（主要为速度、加速度范围）
3）根据上述计算而得的速度、加速度，模拟出小车接下来一小段时间可达到的路径
4）借助评价函数，对上述路径进行评估，并选取出最优解，然后使小车执行（执行对应的速度、角速度）
5）再以小车新的位置及状态为基础，重复上述“2-5”，直到判断出小车到达终点。
二、人工势场法
出处：GitHub-https://github.com/zzuwz/Artificial-Potential-Field（1）演示一下
如图所示，绿线为机器人行驶的路径，红色椭圆为障碍物，机器人从左下角的起点运动到了右上角的终点。
（2）算法原理
人工势场法引入了物理中斥力场和引力场的思想，把工作环境抽象为一个电磁场，而机器人则是其中的一个电荷，机器人在磁场力的作用下移动。
人工势场法会在障碍物周围构建斥力场、在目标点周围构建引力场；这样，机器人便能够在斥力场和引力场的作用下向目标点移动。同时，当障碍物和目标点太近时，机器人很可能会因为刹不住车而出现无法到达目标点等问题，这也就出现了一堆相应的优化算法。
（3）Matlab程序梳理
结合其算法原理便可知，其主要包括两大部分：斥力场和引力场的建立，机器人当前受到的合力作用。而该程序的实现逻辑大致为：以起点为圆心R为半径画圆，并取得圆上八个均布的点，分别计算各个点前进的代价（引力与斥力之和），并选取代价最小的点作为下一个起点。（大致流程如此，并不严谨）
该matlab程序中的“main.m” 为主程序，而“path_plan.m”则为算法的具体实现程序。
程序的流程如下：
1）起点、终点 、障碍物、迭代次数、取点半径等参数的设定
2）以起点为中心，作半径为r的圆，从圆上取八个均布的点
3）分别计算八个点的前进“代价”—— 终点对其的引力+所有障碍物对其的斥力
4）取“代价”最小的点的坐标，结合现有起点，计算得到新的起点，然后重复上述内容
5）当发现 一个点距离终点很近 or 迭代的次数计算完 程序停止。
三、RRT*算法
出处：GitHub-https://github.com/wntun/RRT-star（1）演示一下
如图所示，两个矩形块为障碍物，蓝色树枝为算法搜索的过程演示，深蓝色小球路径为得到的最佳路径。（最佳路径由终点画线到起点）
（2）算法原理
RRT算法也是求解最优路径的，不过他的原理是先遍历完整个地图，再求解出最佳的路径（代价最小）。这个没啥好说的，就是有一个相对固定的流程。
RRT算法中出现频率很高的词，采样点、节点、子节点、父节点。刚开始时，节点只有一个起点，同时也是父节点；然后先在地图上随便找一个采样点，然后找出离采样点最近的一个节点，并在找到的节点到采样点的方向上，按照前进的步长取一个点，如果节点到该点的直线不会碰到障碍物，就会将该点放入节点大仓库中。
更多RRT*算法原理，可以看看下面这个视频（3）Matlab程序梳理
RRT算法文件夹也有两个m文件，其中"rrt_run.m"为主程序，其主要进行了相关参数及地图的初始化，具体的算法实现在“PlanPathRRTstar.m”文件当中。
程序的流程如下：
1）进行起点、终点、障碍物、前进步长、迭代次数等参数的设定，rrt树的初始化
2） 在地图上随机采样一个点a，并找出现有节点中与其最近的节点b
3）沿最近的节点b到采样点a方向，根据机器人步长，求得新的节点c
4）对新的节点c进行障碍物判断 & 找到c的父节点 （最近点 or 可到达的点-eighbors）
5）如果新节点符合要求，将其插入到rrt树中（携带四个rrt参数）并进行节点重连的路径优化计算
6）判断该新节点c是否“到达目标点”（按条件修改rrt的第四个参数），并持续重复上述“2-6”
7）迭代次数达到设定值后，根据得到的rrt树，找出最佳路径
四、A*算法
出处：b站up主【小黎的Ally】（1）演示一下
图中，蓝色为障碍物、绿色为起点、红色为终点、黑线为最短路径。
（2）算法原理
A*算法的原理与RRT类似，都是固定的套路流程。算法会以建立的栅格地图为基础，从起点挨个计数周围点的代价，并慢慢的向外衍生，最后计算得到一系列点的代价，并求出最优路径。对于代价计算，走一格直线的代价为1，走一格对角线的代价为1.4 。（对角线，一一根号二）更多内容看视频讲解吧~
下面这篇文章也讲解得挺不错（3）Matlab程序梳理
Astar算法文件夹也有两个m文件，其中"star.m"为主程序，其主要进行了相关参数及地图的初始化、openlist和closelist的更新、路径的寻找等，而“child_nodes_cal.m”文件进行的内容只是寻找父节点四面八方这八个位置中可选择的子节点，以便进行后续的代价计算。
程序的流程如下：
1）起点、终点、障碍物等参数的设定 & openlist、closelist的初始化
2）从起点出发，将起点作为父节点，记录起点到父节点的路径
3）对父节点周围的八个点进行障碍物判断，找出可前进的子节点
4）挨个计算上述子节点的代价，记录【起点到其父节点，再到该子节点的路径】，并将其放到openlist中
5）将父节点（包括起点到父节点的路径）移动到closelist中，然后从openlist中寻找一个代价最小的节点，作为下一步的父节点
6）对该新的父节点，重复上述“3-5”，直到发现 新的父节点就是目标节点
7）从closelist取出最后一行，其第一个元素为目标点坐标，第二个元素为【起点到目标点的最优路径--- 一串坐标值】
TIPS: 诸多其他算法讲解及程序实现，都可以在B站/GitHub找到。 学长机电人的最终转向还是转计算机嘛哈哈哈哈（机电小学弟） 机电大有可为，我只是先学点偏计算机的。
页:
[1]