路径规划与优化学习系列（一）---路径规划算法

　　几个月来浑浑噩噩，人生这张地图实在太大了，顿时觉得人生之路障碍重重、迷茫不清，故此受人启发，一学路径规划之法，以解心头之困，以便找到寻找最优之人生路（结尾有人生之悟） 　　本文仅用于笔记和回顾，学习来源如下介绍 　　学习目标 　　入门路径规划思想深入了解经典路径规划思想深入体会各种经典算法的适用场景深入理解经典算法的代码实现泛读论文，探索算法改进点 　　学习来源 　　B站（IR艾若机器人）机器人路径规划、轨迹优化系列课程_哔哩哔哩_bilibili 　　中国知网、SCI、ieee论文库 　　一、路径规划思想概述 　　​ 近年来，路径规划算法高效发展，得到广泛应用。以无人机路径规划为例，主要包括 　　1.基于飞行区域 　　PRM概率路径图法（避免碰撞）分区规划+K-means+模拟退火（复杂约束下多任务多机）人工势场（解决目的点无法准确到达）A*定长搜索（起始点确切路径）voronoi加权方向图+蚁群算法（高效避障） 　　2.基于任务时间 　　时间最优+服务结点最大化通信优化框架 　　3.基于能耗优化 　　A*+粒子群优化（路线最优、降低能耗）多机联合+比特分配（满足quality并且降低能耗） 　　4.基于任务分配 　　动态贝叶斯网络架构（动态飞行复杂环境中的自主飞行和任务执行）异构无人机与多样化任务适配（自然灾害和环境多变下的飞行） 　　参考：无人机路径规划算法研究_魏涛 DIO:10.27675/d.cnki.gcydx.2020.000204 　　二、路径规划经典算法 　　（一）基于搜索的路径规划 　　1.Dijkstra 　　背景及适用场景 　　从起始点到终点的最短（最优）路径问题广度优先搜索解决赋权有向图或者无向图的单源最短路径问题，最终得到一个最短路径树。该算法常用于路径搜索或者作为其他图算法的一个子模块本算法也可适用于寻优问题 　　原理：贪心思想 　　从起点开始逐步扩展，每一步为一个节点找到最短路径 　　栅格地图 　　定义：地图划分为若干分辨率的小方格，每个小方格有不同的权值（颜色），代表不同的意义，如下： 　　 　　栅格地图的优势： 　　可以将任意形状轮廓的地图，用足够精细的栅格进行绘制 　　每一个栅格，可以通过不同颜色表示不同含义 　　基于栅格地图进行路径规划有横、纵、斜三个规划方向。对应室内低速机器人可以完全按照路径行走；对于中高速机器人，可以考虑将路径进行平滑处理，以适用于非完全约束系统。 　　有权图转化： 　　 　　行动方向矩阵：前两个参数代表运动方向，后一个参数代表运动代价（通常为运动距离） 　　 　　matlab绘制栅格地图 　　具体实现 　　取自：IR艾若机器人 　　 　　2.A* 　　背景及适用场景 　　从起始点到终点的最短（最优）路径问题广度优先搜索:BFS以起点A为圆心，先搜索A周围的所有点，形成一个类似圆的搜索区域，再扩大搜索半径，进一步搜索其它没搜索到的区域，直到终点B进入搜索区域内被找到深度优先搜索:DFS则是让搜索的区域离A尽量远，离B尽量近 　　原理：启发式函数 　　A*算法的导出 　　首先，BFS保证的是从起点到达路线上的任意点花费的代价最小（但是不考虑这个过程是否要搜索很多格子）。其次，DFS保证的是通过不断矫正行走方向和终点的方向的关系，使发现终点要搜索的格子更少（但是不考虑这个过程是否绕远）。 　　因此，A*算法的设计同时融合了BFS和DFS的优势，既考虑到了从起点通过当前路线的代价（保证了不会绕路），又不断的计算当前路线方向是否更趋近终点的方向（保证了不会搜索很多图块），是一种静态路网中最有效的直接搜索算法。 　　启发式函数 　　 　　设定地图为栅格地图，运动方向有八个，每个方向都有对应的代价 　　F(n)为总代价函数 　　g(n)为起点移动到指定结点（当前结点）的代价值 　　h(n)为启发式函数，用来约束路径的走向，代表指定结点（当前结点）到终点的横向或者纵向代价值 　　A*算法的优势：减少了采样栅格个数，增加了路径搜索速度，优化了路径走向 　　具体实现 　　 　　（二）基于采样的路径规划 　　1.RRT 　　定义及适用场景 　　快速拓展随机树法 　　单源路径、避障问题 　　核心思想 　　主体思想 　　将搜索的起点位置作为根节点，然后通过随机采样增加叶子节点的方式，生成一个随机扩展树，当随机树的叶子节点进入目标区域（故不能准确找到目标点），就得到了从起点位置到目标位置的路径 　　 　　关键点：采样 　　情况一：路径没有穿过障碍 　　在地图范围内随机选取采样点，然后选取当前路径列表结点的最近点，以一定步长连接两点 　　 　　情况二：路径穿过障碍 　　需要舍弃该条路径选取 　　 　　规范路径趋势 　　赋予终点一定的概率被选作采样点，以规范路径使其趋向最优（尽管效果不佳） 　　动态步长优化 　　连接两点之间的步长可以根据当前所处位置而动态变化，使路径更优 　　具体实现 　　伪代码： 　　 　　主体核心代码+注释： 　　效果分析 　　搜索速度快路径非最优只能到达目标区域附近 　　 　　2.RRT* 　　定义及适用场景 　　在RRT思想上寻找最优的路径 　　单源避障的最优路径问题 　　核心 　　parent_node 　　当前结点加入一步检索，寻找其到附近结点的最短距离，从而更新父节点，以使路径达到最优 　　 　　rewire 　　寻找到最优父节点之后在一定圆范围内不断检索所有临近结点的组合方式，重新组合路径，使其达到最优的效果 　　 　　具体实现 　　效果实现 　　搜索速度快路径生成趋于最优无效的搜索过多只能到达目标区域 　　 　　3.informed RRT* 　　定义及适用场景 　　在RRT*思想的基础上添加椭圆采样约束,优化路径、加快寻优速度 　　适用于单源快速避障的路径规划问题 　　核心 　　椭圆采样约束 　　二维空间下，以起始点所有直线为椭圆长轴构建椭圆范围；三维空间下，构建椭球体 　　确定Xcenter点，作为坐标系转化的偏置修正 　　计算起始点的起始距离作为Cmin 　　每次采样前，计算当前路径的代价长度作为Cbest；计算旋转矩阵Kabsch算法求解旋转矩阵 　　每次采样时，以单位圆的形式采样获取坐标Xball；计算r矩阵、C旋转矩阵 　　每次采样后，以公式 rCXball+Xcenter 转化为椭圆坐标系下的坐标 　　重点：不断缩小椭圆的范围 　　具体实现：Cbest是在迭代中不断更新变化的，设置r矩阵第一个元素为Cbest的一半，第二个元素是Cbest与起始点长度的平方和的开方的一半，因此椭圆范围会不断更新变化（缩小） 　　 　　旋转矩阵 　　计算旋转矩阵Kabsch算法求解旋转矩阵 　　具体实现 　　效果实现 　　渐进寻优速度加快路径趋向最优的效果极佳减少了无效的搜索 　　 　　4.PRM 　　定义 　　以随机采样的形式采点，当代价<阈值时，生成点与点之间的直线路线，最后在搭建的路线图中寻找最优路径 　　概率路线图构建 　　随机采样 　　采样点和距离<阈值，则生成路线连接 　　图上寻找最优路径 　　以代价最低为目标寻找最优路线即可 　　（三）基于启发式智能算法的路径规划 　　1.遗传算法 　　2.蚁群算法 　　由于实际应用中较少，且以往已经接触和实现过，故在此不做笔记 　　三、经典算法的代码实现 　　代码的实现参考了IR艾若机器人的算法思路，深入理解了算法的核心部分 　　由于算法应用于不同的问题有不同的逻辑，故后续需要在不同问题场景下或者ros下跑一下仿真 　　（未完待续…） 　　四、经典算法的适用场景总结 　　 　　五、论文泛读—算法改进 　　（另写一篇文章，未完待续…） 　　入门路径优化算法√ 　　 　　路径虽有规划之法，但人生路毕竟多变，还需不断提升应变能力才能达到更好的下一个起点（人生没有终点） 　　人生路如此，无人机路径也如此，飞行环境多变，故此需要学习和探索应对多变环境的方法，使其增强生命的硬度！！！！