智能网联汽车决策规划技术算法原理与实践课件 7.DLite 算法

D*Lite算法目录CONTENTS01D*Lite算法简介02D*Lite算法核心思想03可视化D*Lite路径规划系统04智能路径导航系统05总结与思考D*Lite算法简介01动态路径规划需求与背景01动态路径规划算法概述动态路径规划算法是为机器人、车辆等在不确定环境中实时规划路径而设计，能够在运动过程中根据环境变化实时调整路径，以适应新情况和条件。02D*Lite算法的提出D*Lite算法是D*算法的改进版本，由SvenKoenig和MaximLikhachev于2002年提出，通过引入双向搜索和启发式信息等优化，提高了在动态环境中路径规划的效率和实用性。03D*Lite算法的应用D*Lite算法被广泛应用于机器人导航、无人车自动驾驶、无人机路径规划等领域，为动态环境中的路径规划提供了有效的解决方案。D*Lite算法发展历程D*算法的起源D*算法由AnthonyStentz于1994年提出，是一种增量式路径规划算法，针对动态环境中路径规划问题设计，通过动态更新路径适应环境变化。D*Lite算法的改进D*Lite算法在D*算法基础上引入双向搜索、启发式信息等优化，提高动态环境中路径规划的效率和实用性，随着需求增加，研究人员对其进行了进一步改进和扩展。D*Lite算法应用领域机器人导航在机器人导航领域，D*Lite算法帮助机器人快速有效规划路径，避开障碍物并到达目标位置，在动态环境中及时调整路径适应环境变化。无人车自动驾驶D*Lite算法用于无人车自动驾驶中规划车辆路径，避开交通障碍物、行人和其他车辆，实现安全高效行驶，根据实时情况快速更新路径。物流和仓储在物流和仓储领域，D*Lite算法用于规划机器人或自动导航车辆路径，在仓库中快速搬运货物，当仓库中货物摆放位置发生变化或有新障碍物出现时，及时调整路径适应新环境。D*Lite算法核心思想02D*Lite算法基本思想增量式路径规划D*Lite算法的基本思想是在动态环境中进行增量式路径规划，不是重新规划整条路径，而是在环境发生变化时仅更新受影响的部分路径，减少计算量，使算法更快适应环境变化。局部搜索当环境发生变化时，D*Lite算法通过对受影响路径段进行局部搜索，找到最优路径更新，局部搜索效率比重新规划整条路径高得多。D*Lite算法双向搜索与启发式双向搜索策略D*Lite算法采用双向搜索策略，在正向和反向两个方向上同时搜索路径，加快路径搜索速度，更快找到最优路径。启发式信息算法利用启发式信息指导路径搜索过程，尽快找到最短路径，启发式信息帮助算法更有效地探索搜索空间，提高路径搜索效率。动态调整代价值D*Lite算法通过动态调整代价值适应环境变化，当环境发生变化时，重新计算受影响路径段的代价值，并根据新代价值更新路径。D*Lite算法实现步骤初始化初始化阶段设置起始状态和目标状态，设置初始路径为从起始状态到目标状态的最短路径，初始化代价图记录每个网格的代价值。路径更新路径更新阶段根据环境变化更新路径，从目标状态开始向起始状态搜索路径并更新路径代价值，从起始状态开始向目标状态搜索路径并更新路径代价值。路径返回根据更新的路径信息，机器人或者系统根据新的路径进行移动。循环迭代重复路径更新和路径返回步骤，随着环境的不断变化和机器人的移动，周期性地更新路径，进行局部搜索，直到达到终止条件。可视化D*Lite路径规划系统03可视化系统项目介绍项目功能特点本项目是基于D*Lite算法的路径规划器，通过SFML库实现可视化网格地图界面，用户可在地图上设置起点、目标点及障碍物，观察算法计算出的最短路径。项目价值项目为用户提供直观实用的路径规划工具，可用于演示和测试D*Lite算法在不同地图场景下的表现，用户可通过调整地图布局和参数观察算法效果，更好地理解学习路径规划算法。运算符重载与实用函数运算符重载文件util.hpp定义实用函数和运算符重载，主要用于处理std::pair类型和std::vector类型的操作，运算符重载包括operator+重载两个std::pair元素相加操作，operator-重载两个std::pair相减操作和单个std::pair取反操作。函数arg_min函数arg_min用于返回std::vector中最小元素的索引，接受std::vector类型参数，使用std::min_element找到最小元素并返回其索引。重载操作的意义这些重载操作使得在D*Lite算法中对节点的操作更加方便，D*Lite算法中节点的表示通常使用std::pair类型，通过这些操作可以直接进行算术运算。优先队列实现优先队列类PriorityQueue文件PriorityQueue.hpp定义优先队列类PriorityQueue，对标准库中的优先队列进行扩展，增加根据元素值移除元素等额外功能。优先队列在D*Lite算法中的作用优先队列在D*Lite算法中是必需的，算法需要根据节点代价值动态调整搜索顺序，优先队列是高效的数据结构，用于按节点代价值排序和访问。DStarLite类声明

01类成员变量文件DStarLite.hpp定义DStarLite类，包含地图、起点、终点、当前位置、g值、rhs值、优先队列、km值、可用动作等成员变量。02类成员函数类中包含计算最短路径、更新顶点、计算rhs值、计算键值、启发式函数、计算代价、获取邻居状态、获取地图值、获取g值、获取rhs值等成员函数。03类声明的意义文件只包含类DStarLite的声明，即类的成员函数原型和成员变量声明，没有包含具体实现，为D*Lite算法的实现提供了框架。DStarLite类实现流程构造函数文件DStarLite.cpp实现D*Lite路径规划算法，构造函数DStarLite接受网格尺寸、起点状态位置和目标状态位置参数，初始化地图、起点、终点、当前位置、g值、rhs值等成员变量。核心函数computeShortestPath计算最短路径函数computeShortestPath是算法核心函数之一，通过在优先级队列中不断更新状态的估计成本和实际成本来找到最短路径。地图绘制功能01地图功能的作用地图功能用于显示和管理二维网格地图，作为路径规划算法的环境，允许用户在二维网格上标记障碍物、起点、终点，并显示路径规划算法生成的路径。02CellGrid类声明文件cell_grid.hpp声明CellGrid类及其成员函数，用于管理和绘制二维网格地图，每个单元格可被标记为不同状态。03CellGrid类实现文件cell_grid.cpp实现CellGrid类成员函数，包括构造函数、切换单元格状态、设置路径等功能，构造函数创建CellGrid对象，初始化网格地图大小、单元格长度、边框厚度及起点终点位置。主程序与交互主程序文件main.cpp主程序文件main.cpp使用SFML库实现D*-Lite路径规划算法演示程序，创建窗口包含网格地图，可使用鼠标左键单击添加障碍物，右键单击清除障碍物，按下R键重置地图。程序功能算法根据设置的起点和终点在地图上搜索最短路径，并将路径显示为不同颜色。程序创建DStarLite对象和CellGrid对象，创建窗口并设置帧率上限，在一个循环中监听窗口事件、鼠标事件和键盘事件，根据用户操作更新地图状态和路径规划结果，实时绘制地图和路径。编译运行依赖运行依赖要运行项目，需要安装CMake>=3.7用于构建项目，GCC/G++用于编译C++代码，SFML>=2.5用于图形渲染和用户输入处理。Ubuntu安装命令在Ubuntu上可使用命令sudoaptinstallbuild-essentialcmakelibsfml-dev安装上述依赖项。编译运行步骤编译运行步骤包括创建构建目录、使用cmake命令配置项目、使用make命令编译项目、运行生成的可执行文件。运行效果显示可视化动态网格地图，白色方块代表自由单元格，黑色方块代表障碍物，青色方块是起始单元格，绿色方块是目标单元格，橙色方块是到目标的最佳路径。智能路径导航系统04系统功能特点路径规划本智能路径导航系统基于D*Lite算法，为自动驾驶汽车、机器人提供高效准确路径导航服务，可在给定地图环境中帮助机器人规划从起点到目标点的最优路径。动态更新系统支持动态环境实时更新，根据环境变化调整路径规划结果，确保机器人安全导航。图形界面提供直观友好的图形界面，用户可以通过鼠标点击和键盘操作设定地图参数，观察路径规划的过程和结果。交互性强用户可以自由设定地图的尺寸、起点、终点位置，添加或移除障碍物，实现对路径规划过程的直观控制和观察。环境建模与初始化环境建模的重要性环境建模和初始化是确定路径搜索范围、起点和终点的基础，直接影响路径规划算法的适用性、效率和最终路径质量。良好的环境建模和初始化能够提高算法搜索速度、路径优化程度，支持实时更新和动态变化需求。Environment类文件dstar_env.h定义环境类Environment，包含表示网格的结构体grid，具有生成网格、计算成本等功能，提供必要数据结构和成员函数支持路径规划算法实现。DStar_Lite类声明类继承关系文件dstar.h定义DStar_Lite类，继承自Environment，用于执行路径规划，包含路径规划算法所需参数和方法声明，以及环境建模和初始化信息。构造函数构造函数DStar_Lite接受网格环境行数、列数、起点行列坐标、终点行列坐标参数，初始化DStar_Lite路径规划器实例，调用Environment类构造函数初始化基类Environment。成员变量与函数类中包含nRow、nCol、startCoord、terminalCoord等成员变量，以及initDStarLite、heuristics、calculateKey、computeShortestPath、updateGrid、nextInShortestPath、ScanArea、MotionAndScan、PathFinder等成员函数。路径规划算法实现初始化函数文件dstar.cpp实现D*Lite路径规划算法关键部分，函数initDStarLite用于初始化D*Lite算法，将终点rhs值设为0，计算起点和终点关键值，将终点加入优先队列并调用computeShortestPath计算最短路径。启发式函数函数heuristics计算启发式代价，使用欧几里得距离公式计算两个坐标之间直线距离。最短路径计算与更新01computeShortestPath函数computeShortestPath计算从当前坐标到目标坐标的最短路径，通过不断更新节点成本和启发值实现路径计算，直到满足终止条件。02updateGrid函数updateGrid更新网格中目标坐标信息，包括计算目标坐标成本、更新网格状态和调整优先级队列中的元素。03nextInShortestPath函数nextInShortestPath找到当前坐标在最短路径中的下一个坐标并返回该坐标。04ScanArea函数ScanArea扫描当前坐标周围指定范围内区域，检测是否存在障碍物或障碍物被清除，并更新环境状态。可视化界面实现01OpenGL图形用户界面文件dstar_draw.cpp实现基于OpenGL的图形用户界面，用于显示机器人在二维地图中的移动轨迹和地图状态变化，用户可通过鼠标交互操作移动机器人和设置障碍物。02DrawScene函数函数DrawScene绘制OpenGL窗口中的场景，清空窗口背景，绘制矩形网格表示环境中的单元格，根据单元格状态设置不同颜色，绘制网格线可视化单元格边界，绘制代表机器人的蓝色圆形，绘制表示机器人扫描范围的红色矩形，在适当位置显示网格单元格的g值。03交互功能函数keyPressed监听键盘输入，响应退出程序等交互功能。函数mouseFunc监听鼠标点击事件，实现对地图上障碍物的添加和移除等交互功能。路径规划器入口PathFinder函数函数DStar_Lite::PathFinder是路径规划器的入口函数，用于启动路径规划和可视化界面。功能包括初始化路径规划器参数，初始化路径规划器环境，初始化可视化界面，设置事件监听器，监听事件并更新地图状态和路径规划结果，实时更新可视化界面，退出程序销毁窗口。用户交互通过该函数，用户可通过可视化界面与路径规划器进行交互，包括设定起点、终点，添加障碍物，观察路径规划的过程和结果。主程序与调试运行01主程序main.cpp文件main.cpp是项目主程序，为简单命令行程序，用于演示D*Lite路径规划器的使用。功能包括提示用户输入地图的行数和列数，提示用户输入起始点的行列坐标，提示用户输入终点的行列坐标，创建DStarLite对象并传入用户提供的地图尺寸、起始点和终点信息。02CMake编译运行建议使用CMake编译运行项目，对于在Windows环境中使用MicrosoftVisualStudio的用户，需要下载FreeGlut，添加头文件和库文件到项目中，配置项目属性，包括附加包含目录、附加库目录和附加依赖项。03运行效果完成配置后即可在Windows环境中运行项目，执行效果显示基于OpenGL的图形用户界面。总结与思考05D*Lite算法优势总结算法优势D*Lite算法作为D*算法的改进版本，通过增量式路径更新和局部搜索，在动态环境中高效进行路径规划，利用双向搜索和启发式信息加速路径搜索过程，通过动态调整代价值适应环境变化。应用认可D*Lite算法在相关领域应