想问一下各位大佬，在已知所有目的地坐标，并且数据不算特别多的情况下，哪些或者哪种算法是比较适合路径规划的？谢谢。
请关注公号《混沌无形》，阅读下载更多硬核干货！！
原文链接：
机器人图规划算法研究现状简述图搜索算法
第一类是图搜索算法，此类算法的主要特征是将地图栅格化后进行路径搜索，并致力于达到路径最短、效率最优等目标。
如图 2.2所示，Dijkstra算法是最经典的图搜索算法之一，属于广度优先算法，采用遍历的方式，计算起点到终点的所有路径，并选择成本最低的路径，因此可搜索最短路径（网上有大量资料，故不赘述）。
图 2.2 Dijkstra算法（图片来源：https://github.com/AtsushiSakai/PythonRobotics）
A*算法在Dijkstra算法基础之上，根据起始点和终止点的先验信息设计巧妙的启发式函数，减少搜索扩展的节点数，提高了路径搜索效率。（补充：后续算法名中带有*的算法，基本上暗含该算法采用了启发式函数）
图 2.3 A*算法（图片来源：https://github.com/AtsushiSakai/PythonRobotics）
针对A*算法在实际应用中的不足之处，后续研究者从内存消耗、实时性、动态环境适应性、移动目标追踪、路径质量等角度进行了深入研究和改进，具体如下：
内存消耗改进
Korf、Chakrabarti、Zhou及Lovinger等人对此改进了A*算法，如IDA*[2]采用有限度的迭代加深（深度优先）算法，通过评价函数剪枝优化，降低了搜索内存消耗。SMA*[3]仅在超过设定内存空间时，才开始剪枝，并从open list储存数据结构、f-cost需求数量及节点备份与删除等方面简化改进。与SMA*不同，SMA*+[4]设计节点剔除启发式函数，在有限内存空间限制下，通过搜索空间的先验知识进一步提高了搜索性能。
实时性改进
实时性一直是算法评估的重要指标之一，在有限时间内搜索一条可行路径更具实用意义。Korf、Bulitko、Koenig及Rivera等人对此改进A算法，RTA*[5]与LRTA*[6]在迁移到下一节点之前会更新启发值h，RTA*[5]选择第二小的成本更新节点h，LRTA*则选择最小值，相应的，RTA*在单次试验中的学习率更高，在相同的目标状态但不同的初始状态的情况下重复求解，LRTA*将随着时间的推移而提高效率，并最终收敛于最优解。RTAA*[7]则使用open list中的最佳节点的g值更新h值，以朝向A*规划方向搜索。虽然提高实时性，但却不能保证为最优路径。WRTA*[8]采用加权更新的方式，通过使用不同的学习规则将权重值w合并到h中，在计算成本和总搜索时间方面有极大改进。
动态环境适应性改进
机器人运动场景多是动态、连续变化的，因此在动态环境中的相邻两次搜索得到的路径相似度较高，即路径局部一致性。Stentz、Koenig、Koenig和Likhachev等人基于重用已搜索节点信息的效率高于完全重新计算效率的基本思路改进A*算法，如D*[9]算法采用贪心算法从目标点反向搜索到机器人当前位置，并根据动态障碍物信息调整路径，并被应用于HMMWVs和Mars Rover机器人。不同于D*，LPA*[10]总能找到从初始固定起点到初始固定终点的最优路径，但不适用于机器人运动场景，D* Lite[11]改进LPA*，其节点的启发函数值随着起点的变化而变化，相较于D*，搜索效率得到提升，且实现过程得到简化。AD*[12]在此基础上考虑时间约束，能够在有限时间内重规划次优路径，极大提升机器人在动态环境中的实用性。
移动目标追踪改进
当目标点动态变化时，上述适应动态环境算法的搜索效率将会降低，Sun、Hernández等人对此研究改进。GGA*[13]将三个状态点（当前点、下一点、目标点）纳入三角不等式以更新启发值，GFRA*[14]高效地将上移搜索树转化到当前搜索树，极大提高移动目标的效率，但无法适用于动态环境，而MTD*-Lite[15]在D*-Lite算法基础上借鉴GFRA*重用上一次搜索结果的思想，搜索效率提高到了GGA*的3-7倍。Tree-AA*[16]可重用当前及以前所有的A*搜索结果（可重用树），实现最优增量启发式路径搜索。
路径质量改进
普通栅格地图搜索算法，其节点移动方向仅能为π/4的整数倍，因此Ferguson、Nash和Dolgov等人针对路径存在大量“细碎”折线段的问题提出了改进方法，如Field D*[17]是D* Lite的变种，节点能够沿任何方向扩展，而Theta*[18]是A*的变种，能够得到比Field D*更短的路径，但重规划不如Field D*，Incremental Phi*[19]结合了Theta*和D* Lite的优势，能够快速规划最优路径，Lazy Theta*[20]是Theta*的变种，降低了line-of-sight check次数，在不增加路径长度的同时，显著提升搜索效率。如图 2.4所示，Hybrid A*[21]在A中融入RS曲线约束，满足最小转弯半径约束，进一步提高路径连续性，并成功应用于无人车Junior。
图 2.4 Hybrid A*[21]
图搜索算法的基本原理就是计算所有途径节点的路径的成本值，并选择成本值最低的一条作为结果。
关注公号《混沌无形》，阅读原文，可下载对应pdf哦
机器人图规划算法研究现状简述A*算法，A*（A-Star)算法是一种静态路网中求解最短路径最有效的直接搜索方法，也是解决许多搜索问题的有效算法。算法中的距离估算值与实际值越接近，最终搜索速度越快。
从上章节的RRT算法的效果来看，A*算法相较于RRT的效果是要好许多的。如果说有个地图的缝隙很小，对于RRT算法来说，是比较难找到出口的，就像下面这个地图：
因为RRT是一种随机搜索，但A*的话，遇到这种地图，虽然也要迭代许多次，但是是可以最终找到出口的，而且最终搜索出来的路径就会好许多。
A*算法的原理，如果是比较基础的算法版本，通俗来讲的话，也是比较好理解的。
我们可以把地图都做一个数字化处理，比如数字图像是由一个个像素点组成，地图也可以像像素点一样，加入我们现在有一个起点，从起点开始搜索。
1.选择起点作为当前的扩展点，以起点为中心向上下左右四个方向扩展，如果说我们可以斜向运动的话，也可以向八个方向扩展，即上下左右再加上左上，左下，右上，右下方向：
上图中，蓝色即为扩展的区域，这里以向上方扩展为例子看一下代码，其他的七个方向以此类推：
需要判断一下扩展点是否是障碍物点，是否超过地图边界，并且使用一个数组mapIsTraverse标记点是否已经被遍历过，扩展点也不能被遍历过
2.可以看到在保存改点信息时，有三个代价值，即A*算法中非常关键的一个思想：
总代价sumCost=g_cost + h_cost 这里的 g_cost 表示从起始点到改点扩展的步数代价，现在是第一次扩展，因此就是1，h_cost是距离终点的距离代价，有多种方式，欧几里得距离或者曼哈顿距离，这两个我都尝试了一下，差别不是特别大，最后我使用了曼哈顿距离。即当前点与终点的横坐标的差的绝对值加上纵坐标的差的绝对值。
3.当保存完信息之后，就需要选择下一个扩展的点了，一般是选择总代价最小的点来扩展。这里我单独写了一个函数 checkTree 来从当前的树(树上存储的是已经遍历过的点的信息)中挑选出来下一个可扩展的点。
挑选的思路也比较简单直接，先从已经遍历过的树上找出可扩展的点，即有空间扩展的点，例如下面蓝色的点都是可扩展的点：
可扩展的点需要满足几个条件：
1）可以向外扩展，以A点来说，因为八个扩展的方向都已经遍历过了，因此A点就不满足这个条件
2）向外扩展的方向至少有一个没有阻碍或被遍历过
上图中，以C点为例，此时树Tree中一共有9个点，即A点与蓝色的8个点，C点是可以扩展的点，黄色点即为可扩展的区域，没有障碍，且没有被遍历过(不在树Tree上)
满足上述条件的在本次循环中即为蓝色的8个点
4.计算可扩展的点中代价最小的点，为C点，C点的 g_cost 为1(从起点走1步即可到达C点)，h_cost 为 6(距离终点B的距离)，之后将代价最小的点的编号传出来：
5.以传出的point点(此时为C点)为扩展点，开始向8个方向扩展，并且将满足条件的点保存到Tree中，开启下一次的循环。
6.我们可以清楚看到在下一个循环中，C点右边的那个点(E点)由于周围的点要么被遍历过，要么是障碍物，因此不满足可扩展的条件，因此不可扩展，再计算一下代价的话，代价最小的应该是上下的两个黄点(D点与F点)，这时候选哪个进行扩展都可以：
按照这个思路，并且修改完过程中一些细节的bug，最终做出来的效果是这样的：
https://www.zhihu.com/video/1483960367042301952可以看到效果是比RRT好许多的，不过还是有优化的空间，比如它找出来的路径还可以更优化一些，并且如果我们要做全角度扩展的话，这个要怎么做呢？还是需要继续深入研究，优化这个算法。
参考文献：
[1]胡观凯,钟建华,李永正,黎万洪.基于IPSO-GA算法的无人机三维路径规划[J].现代电子技术,2023,46(07):115-120
孔雀优化算法( Peafowl Optimization Algorithm, POA), 是由 Jingbo Wang 等于2022 年提出的一种群体智能优化算法。其灵感来源于孔雀的群体行为。
地形图可以随机生成，无人机起始点可以自己设定，算法的种群大小和迭代次数可以修改。
部分代码：
close all
clear
clc
reference code link： https://mbd.pub/o/bread/ZJmclp1t
%% 三维路径规划模型定义
global startPos goalPos N num
num=5;%无人机的数量
N=1;%待优化点的个数(可以修改)
goalPos=[80, 90, 150]; %起点(可以修改)
startPos=[10, 10, 80]; %终点(可以修改)
SearchAgents_no=30; % 种群大小(可以修改)
Function_name='F1'; %F1:随机产生地图 F2：导入固定地图
Max_iteration=50; %最大迭代次数(可以修改)
% Load details of the selected benchmark function
[lb,ub,dim,fobj]=Get_Functions_details(Function_name);
[Best_score,Best_pos,curve]=POA(SearchAgents_no,Max_iteration,lb,ub,dim,fobj);
reference code link： https://mbd.pub/o/bread/ZJmclp1t
figure
semilogy(curve,'Color','r')
xlabel('迭代次数');
ylabel('最优适应度');
legend('POA')
set(gca,'FontName','Microsoft YaHei');
Position=[Best_pos(1:dim/3); Best_pos(1+dim/3:2*(dim/3)); Best_pos(1+(2*dim/3):end)]' %优化点的XYZ坐标（每一行是一个点）
display(['理论最优适应度: ', num2str(Best_score)]);
plotFigure(Best_pos)%画最优路径
reference code link： https://mbd.pub/o/bread/ZJmclp1t 部分结果：
在经过一个项目完整周期的开发与学习，觉得有必要写一个阶段性总结，也算是将之前开发和调试中遇到的问题进行归纳、整理，也是对我所了解的范畴中的规划算法做一个概述，有不足或错误之处还往指正。
首先笔者声明，我的工作中的自动驾驶场景不是典型的结构化道路，而是半结构化道路，可以理解为道路边界不清晰，且没有道路中心线和高精地图，主要场景涉及山地、沙漠、丘陵、草原、城市等，我们的工作便是需要在此环境下，依旧找出道路可通行区域（感知范畴，这里不做过多赘述），且在多维空间中输出一条安全、平滑且合理的路径。下面，我们从整体架构、地图、规划算法几大块分别讨论。
目前主流的规划算法架构基本可以用以下模式概括：
注意，这里前后端都使用了生成二字，意为不限制参考路径和平滑路径的生成过程。
在前端参考路径生成过程中，可以使用
1.A*、D*、Dijkstra等搜索算法；
2.RRT、RRT*、PRM等采样算法；
3.动态规划（dp）；
具体使用哪种方法，需要结合场景和算力。举个最简单的例子：dp在基于横纵向采样点生成多项式曲线后，曲线形状较为固定，比较适合于简单环境，而A*搜索的路径往往拐点较多，形状多样，比较适合于复杂环境，但是自由度和稳定性往往是trade off的关系，想要更高的自由度，路径形状就会变的千奇百怪，难免出现一些并不是我们想要的路径。所以在复合环境中，某一种算法往往难以满足需求，如何使算法根据不同场景自适应，也是需要我们认真思考的议题。
由于前端生成的轨迹只是大致找到一条通往目的地的路，往往不考虑运动学和动力学约束，也就是说，车辆无法按照此轨迹进行行驶（如曲率超限，速度、加速度超限等等），或者说，得到的轨迹不够平滑，使得车辆按照轨迹行驶时，出现猛打方向盘、猛踩油门这样的情况，总之不符合我们对轨迹的要求。
针对这样的问题，衍生出了很多将纯搜索或采样的方法与运动学、动力学相结合的算法，例如hybrid A*，Kinodynaimic RRT*等，核心要点都是在原算法基础上，修改节点扩展方式，使得扩展的节点满足车辆非完整性约束，这里的变化有很多，你也可以探索不同的扩展模式，比如使用三次多项式、B样条、螺旋线进行节点的连接与拓展，创造出属于你自己的变种算法。使用满足约束的搜索或采样的路径规划算法可以一步到位，在一定程度上可以不需要后端优化，可直接发给控制执行。
在这里我们仍继续讨论前端+后端的架构模式：
后端中，生成过程也可以多种多样，比如，如何使用参考路径，决定了最终生成平滑轨迹的方法。这里列举三种大类。
1.使用参考路径离散点作为初始值，建立各项车辆运动学、动力学、安全性约束，进行求解，此类方法往往高度非线性，遇到复杂场景时，如何满足实时性，算力是一个挑战，目前比较流行的方法是使用微分平坦性质，重新formulate约束，可以大大简化问题，提高求解效率，具体可参考fast lab论文：An Efficient Spatial-Temporal Trajectory Planner for Autonomous Vehicles in Unstructured Environments；
2.使用参考路径部分属性，进行二次采样，注意二次采样是满足车辆非完整性约束的，此类方法根本上来说不算严格意义上的采样，而是在横纵向采样后，通过空间映射，使用优化的方法生成参数化曲线，实时性高、稳定性强，但由于此类方法是使用后验的方式判断生成的参数化曲线是否安全，而不是在一开始就考虑障碍物，所以在一些极端情况下，会出现没有路径生成的情况，具体可见论文：Reactive Nonholonomic Trajectory Generation via Parametric Optimal Control；
3.使用参考路径开辟凸空间，在此凸空间上建立线性约束，进行求解，得到最终轨迹，典型代表：Apollo EM Planner。需注意和1的区别是，此类方法不使用参考路径作为初始解，而是在此空间上进行求解。由于是凸优化问题，一定能得到全局最优解，且计算高效，笔者实测，使用osqp_eigen求解50m长度的路径耗时在1-2ms左右（x86 i7处理器）。在实际应用中由于我们项目地图格式的差异，凸空间开辟我是在cartesian坐标系下做，然后映射到frenet下，优化完后再转回cartesian，此类方法实时性很高，且由于在优化时就考虑了障碍物，所以基本不会出现2中无路径生成的情况，但frenet与cartesian坐标系转换在一些大曲率的地方往往存在问题，这也是使用frenet下进行规划的弊端，有知道如何解决的朋友也请赐教。
总而言之，近些年自动驾驶得到了长足发展，规控作为自动驾驶直接和底盘相连的模块也变得尤为重要，上述架构只是比较通用的简单概括，也不乏有更加新颖的架构没有涉及到，但是经过这几年的工作与学习，我的感受是“万变不离其中”，我们都是踩在巨人的肩膀上，这些人类的智慧已经摆在了我们的面前，如何将已有的成果融会贯通，根据自己的需求去工程化，可能是目前大部分自动驾驶工程师首当其冲需要的的能力。
2 地图
引用浙大高飞老师的话：“移动机器人做planning，首先要把地图搞明白。机器人理解的地图，并不等价于你看到的地图，怎样设计针对导航最有效的地图？对某种特定传感器，什么样的地图又是最简洁高效的。”当谈及路径规划算法，地图是不可避免的，它不仅在系统上决定了环境的表达形式，也在一定程度上决定了哪些算法更适配。
地图形式千千万，如何选择或者创造出最适合你项目的地图，也是需要深思的，选择合适的地图，可以大大减小planning的开发难度。
目前市面上有多种形式的地图，主要分为全局和局部两大类：
1.全局地图：
全局地图是导航生成全局参考路径的依赖，常用的全局地图有：
高精度地图信息稠密，数据量大，大多数乘用车都使用图商提供的高精度地图，但正是由于其夸张的数据量，导致数据冗余不可避免，加载起来也相对复杂，且在某些野外环境，并没有可直接使用的高精度地图。
拓扑地图信息稀疏，只是简单的描述道路节点之前的连接关系和部分属性，制作和加载起来相对简单，但不可避免的存在导航信息量不够的问题，需要使用其他手段进行补充。
2.局部地图
全局地图只为导航提供了参考全局路径，作为指引，而车辆实际行驶的路径都需要在局部地图中进行规划。局部地图中包含车辆行驶需要避让的动静态障碍物信息以及真实的道路边界信息，目前比较常用的几种局部地图有：
同样的，使用哪种地图也需要结合具体项目需求来看。比如，当你的环境都是一些规则的障碍物，如车辆或人为放置的障碍物时，感知比较容易将障碍物抽象成bounding box，只需在矢量图中描述其几何形状和位置，就可以表示障碍物和车辆的相对位置关系，从而在几何方面设计碰撞检测算法（如矩形相交判断），进行规划。但是，如果置身于戈壁，障碍物种类多样，形状复杂（如杂草、碎石等），感知很难将障碍物抽象成规则的几何形状，此时选择栅格地图，会提高障碍物信息表达的自由度，当然，你还可以根据系统要求，设计相应的cost（例如多帧使用概率值），来生成你所需要的代价栅格图用于规划。
以上只是笼统的概括了几种地图模式，细分下去还有很多，比如栅格图grid map在2.5d维度下还有高程图，在3D下可以用oct-map，还有多层的cost map，每层都可以人为定义不同的cost，来达到不同的规划算法目的。但大体形式是统一的，还是那句话，“万变不离其中”，我们需要做的还是融会贯通后，有选择合适地图的能力，进而去创造。
这些所谓的选择，也体现了一个工程师的设计能力，结合起来就叫工程设计能力吧，当然前提是你有足够多的知识储备，才有的选。就像上述例子中，一名不合格的工程师在复杂障碍物环境下选择使用了矢量地图，那大概率在局部地图中会出现满屏幕bounding box，并且由于bouding box在一定程度上增加了冗余，拓展了膨胀距离，很可能导致明明有路，而规划不出来的情况，因为碰撞检测发现哪哪都会发生碰撞。
对于想要入门学路径规划的朋友，面对着各式各样的规划算法，想必都会有些摸不清，但其实，这些算法，归纳起来，核心核心思想就那么几个。如下。ps:没什么参考文献，仅仅是我的一些总结，如有错误，请忽略。
做运动规划，障碍物地图是一个很重要的输入，这里常用的一个思想是“离散化”，因为我们的数学方法，没有办法处理连续的东西，除非它有确定的函数表达式，否则我们只能将它离散处理，无限变成有限。在地图的表示上，有几种常见的形式:
1.1 栅格地图
把地图由一个个小栅格组成，每个栅格有一个index，每一个index对应着一个现实世界的坐标。每个栅格对应一个值，用来表示这个栅格是否被障碍物占据。这里是将现实世界连续的坐标，离散成有限的栅格。在ros中。
1.2 esdf地图
esdf即欧式距离地图，在栅格地图上，给每个栅格赋上一个最近障碍物距离值。在使用上，可以通过双线性插值，再将栅格地图近似恢复成连续的距离地图。
1.3 拓扑地图
在一些固定路线的场景中，我们把地图定义为点和边，一个点可以连接多条边。即将地图离散成节点和边的概念。
1.4多边形/线段障碍物地图
在进行轨迹优化时，需要讲障碍物约束添加给优化问题，因此，而优化问题的优化变量都是有限的，比如有限个路径点。那么必然要将障碍物离散化，在TEB算法中，用到了线段形式的障碍物表示，而在apollo中，或者其他来源项目中，用到了多边形障碍物。
1.5 安全走廊地图
把无限连续的可通行空间，离散成有限的多边形走廊，这个也是轨迹优化中常用的一种障碍物表示形式，它可以方便的加入到优化问题中去。
基于搜索的思想，应该是每一个入门路径规划的人最小接触的一种思想，以这种思想为基础的经典算法有:
1. a星，jps算法
2.dijkstra算法
3.混合a星算法
4.cbs算法
5.dp动态规划
搜索的目的是，通过穷举所有的可能，得到一个离散空间内全局最优的解。(当然，对于a星类算法，增加了启发式搜索，其可以在不穷举的情况下，快速找到最优解)。搜索的思想主要包含以下步骤。
2.1 地图离散化
如上一节所述，比如离散成栅格地图。
2.2 搜索空间离散化
对于a星算法，搜索空间离散化为x和y。
对于混合a星，搜索空间离散化为x，y，theta。
对于cbs算法，搜索空间离散化为x，y，时间t。
你可以在其他场景下，采用这种方法，比如，有一个移动机器人，底盘上边有一个可旋转的托盘，如果想要让机器人在移动到不同的位置时，控制托盘转动到指定角度，那么就可以把托盘的角度，加入到离散空间中，进行搜索。
搜索的核心在于穷举，加速搜索的方法是制定适当的启发值。在任何你想要获得全局最优解，都可以试试搜索的方法。
2.3 cost和启发值
既然穷举了所有的可能，那么就需要制定一个标准，来衡量每一次搜索的价值，这就是cost，即通过cost来衡量谁更优。
启发值，是一个可以经过作用的方法，通过设置不同的启发值，实现定制化的搜索。
这个思想可以迁移到很多地方。可以将规则转化为启发值。比如，一张地图里，想要靠右搜索，那就将左侧的启发值加大。这样可以优先搜索到靠右的路径。
路径规划另一个经典思想。自此为代表的算法有rrt法，latrice算法。
通过撒点采样，连接，减枝等步骤，完成最优路径的生成。rrt算法有很多变种，广泛应用与机器人和自动驾驶中。apollo中的lattice planner是通过撒点采样，通过多项式连接相邻节点，完成最优路径的生成。
掌握这种思想，你可以灵活去设计自己的路径规划算法。比如，想要实现一个靠右行驶的规划器，可以指定特定规则的采样方式，完成这个功能。
轨迹优化的根本就是优化，将轨迹生成问题表示成优化问题，通过数学方法来求解全局最优解。比如，轨迹用离散点表示，优化目标是离散点平滑，约束是不碰撞障碍物以及满足运动学约束。这些可以分别表示成优化问题的目标函数和约束条件，通过调用优化求解器，来求解最优解。对于我们遇到的问题，如果能够表示成确定的数学问题，那么便可以用优化的方式去求解。难点之一在于，如何表示成优化问题，难点之二在于如何简化问题，以满足耗时要求。比如，你想要得到两个点之间瞒住运动学约束的最短路径，如果不会用优化的方法，那么可以通过采样去逼近最优解。如果能够将问题表示成优化问题，那么它就是一个两点边界问题，即bvp，可以直接用数学方法求得全局最优解。
在采样的思想里，我们需要设置一系列cost，也可以将一些约束条件设置成软约束，加入到cost中，比起远离障碍物的软约束。在优化的思想中，我们可以将曲率约束作为一项cost加入到目标函数中，这也是软约束。软约束的作用在于，增加了优化问题的成功率。而硬约束，必须满足的约束，一旦不满足，优化问题将失败。硬约束转成软约束。最优化中，将优化问题分成两类，有约束优化和无约束优化，无约束优化的核心思想就是把硬约束转化成软约束，或者称为罚函数，进而可以采用比如梯度下降法，拟牛顿法等方法求解。
"一个优秀的程序员，不在于他掌握了多少很牛的算法，而在于，他能够从算法中，提取出核心思想，在遇到问题时，能够套用这些思想去解决问题"。这就话不是什么名言，是我胡说的，哈哈哈。
