路径规划算法实现

来源丨古月居

这里我们通过Python编程+matplotlib数据可视化来实现路径规划算法，这里我们主要实现A Star算法、D Star算法、Dijkstra算法、RRT算法在2D空间下3D空间下的实现。

 A Star算法的设计与实现

Astar潜在地搜索图中一个很大的区域。和Dijkstra一样，Astar能用于搜索最短路径。和BFS一样，Astar能用启发式函数引导它自己。在简单的情况中，它和BFS一样快。

程序入口部分我们指定起始点和目标点，通过调用定义的Astar类来进行路径录规划，最后通过plot进行可视化绘制显示，如图所示。

类的初始化内容如下，主要是传入参数以plot点坐标和算法类型。这里以dict的方式存储，plot通过关键字进行索引找寻数据，如图所示。

通过A Star算法搜索路径点并加入显示，如图所示。

最终路径求解如下，如图所示。

在A Star算法的3D空间路径搜索部分，我们添加全部所有的方位点Direction，这里对所有的求解方位，如图所示。

其余部分内容和2D A Star求解一样，这是增加了求解实现描述显示，求解效果如下，如图所示。

D Star算法的设计与实现

D Star算法对在移动环境中的寻路也比较高效，向当前节点迁移时，可以只考察最近路线上的结点以及相邻节点的变化状况，包括机器人寻路等结果。

这里我们依旧是指定起始点和目标点，通过调用DStar类的方式实现算法的验证和分析，如图所示。

类的构造函数部分，调用Plotting类实现图表的图表的初始化构造，并声明相关阈值变量存储区，如图所示。

这部分是算法的实现核心，主要是“贪心策略”迭代找寻更优的求解。如果发现比当前更短的路径，则进行迭代，这里可能向前迭代，也可能向后迭代。D Star算法核心实现如图所示

Dstar算法对2D空间的求解如图所示：

D Star算法对3D空间的求解如如图所示：

Dijkstra算法的设计与实现

Dijkstra算法也可以算是用贪心思路进行的，首先把从起点到每个节点之间的一段距离都保存留下来并寻找一个到v的，之后松弛一下再重新寻找到v的，所谓的放松方式就是，先遍历一下把刚才发现的相距比较近的一点作为中转站会不会更近，如果还更近就再调节一段距离，这样当把所有的节点都找遍了以后，就保存并留下了从起点到其他每个节点之间的最短距离。

和前两个一样，在指定起始点和目标点之后，调用定义的Dijkstra类实现路径的搜索规划，最终通过plot类进行可视化显示。图可函数如图所示。

Dijkstra算法较为较为简单，这是依据数据结构的基本构造进行实现，核心代码如如图所示。

Dijkstra算法2D路径规划如如图所示。

Dijkstra算法3D路径规划效果如图所示：

RRT算法的设计与实现

RRT（快速寻找随机树）是一个很普通的办法，无所谓任何机器人种类、无所谓自由度是多少、也无所谓约束有多繁复，都可以使用。

并且它的基本原理非常简洁，这是其在机器人应用领域受欢迎的主要因素之一。但是它的缺陷也非常突出，它得到的路通常质量都不会非常好，例如可能具有棱角，不平滑，通常也远离最优路线。

RRT算法是基于抽样路径规划，它在3D空间下的路径规划效果较好。核心功能函数如图所示。

RRT算法在3D空间下规划效果如图所示。

本文仅做学术分享，如有侵权，请联系删文。

3D视觉工坊精品课程官网：3dcver.com

1.面向自动驾驶领域的多传感器数据融合技术

2.面向自动驾驶领域的3D点云目标检测全栈学习路线！(单模态+多模态/数据+代码)
3.彻底搞透视觉三维重建：原理剖析、代码讲解、及优化改进
4.国内首个面向工业级实战的点云处理课程
5.激光-视觉-IMU-GPS融合SLAM算法梳理和代码讲解
6.彻底搞懂视觉-惯性SLAM：基于VINS-Fusion正式开课啦
7.彻底搞懂基于LOAM框架的3D激光SLAM: 源码剖析到算法优化
8.彻底剖析室内、室外激光SLAM关键算法原理、代码和实战(cartographer+LOAM +LIO-SAM)

9.从零搭建一套结构光3D重建系统[理论+源码+实践]

10.单目深度估计方法：算法梳理与代码实现

11.自动驾驶中的深度学习模型部署实战

12.相机模型与标定(单目+双目+鱼眼）

13.重磅！四旋翼飞行器：算法与实战

14.ROS2从入门到精通：理论与实战

15.国内首个3D缺陷检测教程：理论、源码与实战

16.基于Open3D的点云处理入门与实战教程

重磅！3DCVer-学术论文写作投稿 交流群已成立

扫码添加小助手微信，可申请加入3D视觉工坊-学术论文写作与投稿 微信交流群，旨在交流顶会、顶刊、SCI、EI等写作与投稿事宜。

同时也可申请加入我们的细分方向交流群，目前主要有3D视觉、CV&深度学习、SLAM、三维重建、点云后处理、自动驾驶、多传感器融合、CV入门、三维测量、VR/AR、3D人脸识别、医疗影像、缺陷检测、行人重识别、目标跟踪、视觉产品落地、视觉竞赛、车牌识别、硬件选型、学术交流、求职交流、ORB-SLAM系列源码交流、深度估计等微信群。

一定要备注：研究方向+学校/公司+昵称，例如：”3D视觉 + 上海交大 + 静静“。请按照格式备注，可快速被通过且邀请进群。原创投稿也请联系。

▲长按加微信群或投稿

▲长按关注公众号

3D视觉从入门到精通知识星球：针对3D视觉领域的视频课程（三维重建系列、三维点云系列、结构光系列、手眼标定、相机标定、激光/视觉SLAM、自动驾驶等）、知识点汇总、入门进阶学习路线、最新paper分享、疑问解答五个方面进行深耕，更有各类大厂的算法工程人员进行技术指导。与此同时，星球将联合知名企业发布3D视觉相关算法开发岗位以及项目对接信息，打造成集技术与就业为一体的铁杆粉丝聚集区，近4000星球成员为创造更好的AI世界共同进步，知识星球入口：

学习3D视觉核心技术，扫描查看介绍，3天内无条件退款

 圈里有高质量教程资料、答疑解惑、助你高效解决问题

觉得有用，麻烦给个赞和在看~