Hello World - Path Planning
type: channel
link: https://phxin.top/index.php/category/Path-planning/
items: A星算法Matlab版本详细讲解
items: 路径规划与轨迹规划辨析
items: 轨迹规划总览
items: A星算法（含python实现 | c++实现）
items: A星算法原理讲解
items: 深度优先搜索（DFS）和广度优先搜索（BFS）
items: A星算法
items: 路径规划算法总览
items: Dijkstra算法

A星算法Matlab版本详细讲解
type: item
date: 2023-07-10T15:52:00+08:00
description: A星算法Matlab版本详细讲解算法总览map_astar.mA星算法的主函数%% Define a small map map = false(10); % Add an obstacle % 添加障碍物 map (1:5, 6) = true; map (6,8:10) = true; map (3:7,2) = true; %% 设置起始点与目标点 start_coords = [1,3]; dest_coords = [1,9]; % 传入参数，进行A星算法运行 [route, numExpanded ,cost_matrix] = AStarGrid(map, start_coords, dest_coords,true); pause(3); %% start_coords = [10,1]; dest_coords = [2,9]; [route, numExpanded ,cost_matrix] = AStarGrid(map, start_coords, dest_coords,true); pause(3); %% start_coords = [6,3]; dest_coords = [8,9]; [route, numExpanded ,cost_matrix] = AStarGrid (map, start_coords, dest_coords,true); pause(3); %% start_coords = [6,1]; dest_coords = [10,10]; [route, numExpanded ,cost_matrix] = AStarGrid(map, start_coords, dest_coords,true); pause(3); %% start_coords = [6,1]; dest_coords = [1,10]; [route, numExpanded ,cost_matrix] = AStarGrid(map, start_coords, dest_coords,true); pause(3);AStarGrid.mA星算法的具体实现过程function [route,numExpanded,cost_matrix] = AStarGrid (input_map, start_coords, dest_coords, drawMap) % Run A* algorithm on a grid. % Inputs : % input_map : a logical array where the freespace cells are false or 0 and % the obstacles are true or 1 % start_coords and dest_coords : Coordinates of the start and end cell % respectively, the first entry is the row and the second the column. % Output : % route : An array containing the linear indices of the cells along the % shortest route from start to dest or an empty array if there is no % route. This is a single dimensional vector % numExpanded: Remember to also return the total number of nodes % expanded during your search. Do not count the goal node as an expanded node. % set up color map for display % 1 - white - clear cell % 2 - black - obstacle % 3 - red = visited % 4 - blue - on list % 5 - green - start % 6 - yellow - destination cmap = [1 1 1; ... 0 0 0; ... 1 0 0; ... 0 0 1; ... 0 1 0; ... 1 1 0; ... 0.5 0.5 0.5]; colormap(cmap); % variable to control if the map is being visualized on every % iteration % 通过传入变量…

路径规划与轨迹规划辨析
type: item
date: 2023-07-08T10:40:00+08:00
description: 路径规划与轨迹规划路径规划是什么路径规划是在给定环境中确定从起点到终点的最佳路径的过程。在机器人导航、自动驾驶和物流等领域中，路径规划是实现自主移动和导航的重要任务。路径规划的目标是找到一条能够满足特定条件和约束的路径，使机器人或车辆能够从起始位置安全、高效地到达目标位置。这些条件和约束可能包括避开障碍物、考虑运动约束（如最大速度、最大加速度）、最小化时间或能量消耗等。路径规划通常是在静态环境中进行的，它可以是离线计算的（预先规划好的路径），也可以是在线计算的（实时计算适应动态环境）。路径规划算法的选择取决于环境特点、机器人能力以及具体需求。常用的路径规划算法包括：Dijkstra算法：用于计算无权图中的最短路径，适用于环境没有障碍物的情况。A*算法：结合了Dijkstra算法和启发式函数，用于在带有权重的图中计算最短路径，适用于静态环境中的路径规划。RRT（Rapidly-Exploring Random Trees）算法：基于随机采样的快速探索树算法，适用于具有复杂环境和障碍物的路径规划。PRM（Probabilistic Roadmap）算法：基于随机采样和连接的概率图搜索算法，适用于高维状态空间和复杂障碍物的路径规划。路径规划的应用广泛，例如无人机的航迹规划、自动驾驶车辆的路线规划、仓储机器人的路径规划等。通过有效的路径规划，机器人能够在复杂环境中快速、安全地移动，并实现各种任务和应用。轨迹规划是什么轨迹规划（Trajectory Planning）是指在已经确定的路径上，生成机器人或车辆在连续时间上的运动轨迹的过程。它关注的是在动态环境中，考虑机器人的运动约束、动力学限制和避障需求，生成平滑、连续的运动轨迹。轨迹规划的目标是生成适应当前环境和机器人能力的运动轨迹，以确保机器人能够安全、稳定地执行移动任务。在轨迹规划中，会考虑以下因素：运动约束：考虑机器人或车辆的运动约束，例如最大速度、最大加速度、最大转角等，以确保生成的轨迹在机器人的可操作范围内。动力学限制：考虑机器人的动力学特性，例如惯性、摩擦力等，以生成平滑且符合物理特性的轨迹。环境感知：根据环境中的障碍物、地形信息和其他运动物体等，进行碰撞避免和路径调整，以生成与环境相容的轨迹。轨迹优化：使用优化算法，例如最小化能量消耗、最小化时间、最小化运动曲率等，以生成经过优化的轨迹。常见的轨迹规划方法包括速度规划、加速度规划、优化算法（如最小时间优化、样条曲线优化）等。这些方法旨在生成平滑、连续的运动轨迹，以实现机器人或车辆的高效、安全移动。轨迹规划在许多领域都有应用，例如机器人导航、自动驾驶、无人机飞行路径规划、工业机器人的轨迹控制等。通过合适的轨迹规划，机器人能够在复杂的动态环境中灵活、高效地移动，并完成各种任务和动作要求。轨迹规划与路径规划的异同轨迹规划（Trajectory Planning）和路径规划（Path Planning）是机器人导航和移动中两个相关但不同的概念，它们在实现机器人自主移动和避障方面起着不同的作用。异同之处如下：定义和目标：路径规划：路径规划是确定从起始位置到目标位置的最佳路径的过程。它关注在给定的环境中选择路径，以使机器人能够安全、高效地到达目标位置。路径规划是在全局层面进行，考虑整个环境和完整的路径。轨迹规划：轨迹规划是在已确定的路径上生成机器人在连续时间上的运动轨迹的过程。它关注在动态环境中生成平滑、连续的运动轨迹，以使机器人能够在当前环境中安全、稳定地移动。粒度和连续性：路径规划：路径规划通常以离散的方式表示，将路径抽象为一系列路径点或路径段。路径规划不涉及具体的时间和连续的运动。轨迹规划：轨迹规划通常以连续的方式表示，生成机器人在连续时间上的运动轨迹。轨迹规划考虑机器人的动力学限制和运动连续性，以生成平滑、连续的运动轨迹。考虑因素：路径规划：路径规划主要考虑地图、障碍物、运动约束等因素，以找到一条满足约束条件的最佳路径。轨迹规划：轨迹规划除了考虑路径规划中的因素外，还考虑机器人的动力学约束、环境感知和实时性需求，以生成适应当前环境和机器人能力的平滑轨迹。应用领域：路径规划：路径规划广泛应用于机器人导航、自动驾驶、无人机航迹规划等领域，以实现从起始位置到目标位置的路径规划。轨迹规划：轨迹规划主要应用于机器人导航、自动驾驶、工业机器人的轨迹控制等领域，以生成连续时间上的平滑运动轨迹。尽管路径规划和轨迹规划在某些方面有重叠，但它们在粒度、连续性和考虑因素等方面存在明显的区别。在实际应用中，路径规划和轨迹规划通常需要结合使用，以实现机器人的自主移动和导航。

轨迹规划总览
type: item
date: 2023-07-08T10:38:00+08:00
description: 轨迹规划（Trajectory Planning）是指在已经确定的路径上，生成机器人或车辆在连续时间上的运动轨迹的过程。它关注的是在动态环境中，考虑机器人的运动约束、动力学限制和避障需求，生成平滑、连续的运动轨迹。轨迹规划的目标是生成适应当前环境和机器人能力的运动轨迹，以确保机器人能够安全、稳定地执行移动任务。在轨迹规划中，会考虑以下因素：运动约束：考虑机器人或车辆的运动约束，例如最大速度、最大加速度、最大转角等，以确保生成的轨迹在机器人的可操作范围内。动力学限制：考虑机器人的动力学特性，例如惯性、摩擦力等，以生成平滑且符合物理特性的轨迹。环境感知：根据环境中的障碍物、地形信息和其他运动物体等，进行碰撞避免和路径调整，以生成与环境相容的轨迹。轨迹优化：使用优化算法，例如最小化能量消耗、最小化时间、最小化运动曲率等，以生成经过优化的轨迹。常见的轨迹规划方法包括速度规划、加速度规划、优化算法（如最小时间优化、样条曲线优化）等。这些方法旨在生成平滑、连续的运动轨迹，以实现机器人或车辆的高效、安全移动。轨迹规划在许多领域都有应用，例如机器人导航、自动驾驶、无人机飞行路径规划、工业机器人的轨迹控制等。通过合适的轨迹规划，机器人能够在复杂的动态环境中灵活、高效地移动，并完成各种任务和动作要求。常见的轨迹规划算法第三次多项式插值（Third-Order Polynomial Interpolation）：该方法使用多项式插值来生成平滑的轨迹。通过指定起点、终点和约束条件（如起点和终点速度、加速度限制），可以计算出一个连续的三次多项式曲线，即生成的轨迹。样条插值（Spline Interpolation）：样条插值是一种平滑的插值方法，通过连接多个局部的多项式函数来生成轨迹。常见的样条插值方法包括自然样条插值和Hermite样条插值。样条插值可以生成平滑且连续可微的轨迹。最小时间优化（Minimum-Time Optimization）：该方法通过优化算法，如动态规划或二次规划，来寻找最短时间下的最优轨迹。最小时间优化方法考虑了机器人的动力学约束和运动能量，以在规定时间内完成任务。动态窗口方法（Dynamic Window Approach）：动态窗口方法是一种基于机器人动力学约束和环境感知的实时轨迹规划方法。它通过计算机器人在当前时间窗口内的运动状态和轨迹，从一系列可能的轨迹中选择一个最优的轨迹。采样优化方法（Sampling-Based Optimization）：这类方法使用随机采样和优化算法，如遗传算法或优化梯度下降，来搜索最优的轨迹。通过在状态空间中进行采样，并根据某种目标函数评估轨迹质量，优化算法可以找到一个合适的轨迹。这些是常见的轨迹规划算法，每种算法都有其适用的场景和特点。具体选择哪种算法取决于问题的要求、机器人的特性和环境的约束。在实际应用中，通常需要综合考虑机器人的运动约束、环境感知和实时性等因素，选择合适的轨迹规划算法。

A星算法（含python实现 | c++实现）
type: item
date: 2023-06-24T21:36:00+08:00
description: A*算法（含python实现 | c++实现）参考资料Introduction to the A* Algorithm路径规划与轨迹跟踪系列算法Robotic Motion Planning LecturesA星算法详解A星寻路算法路径规划之 A* 算法1. 算法简介[link][item name="深度优先搜索（DFS）和广度优先搜索（BFS）" link="https://xinin.top/index.php/archives/Depth-First-Search-and-Breadth-First-Search.html" desc="" /][item name="A星算法原理讲解" link="https://xinin.top/index.php/archives/Astar-Explanation.html" desc="" /][/link]A*（A-Star)算法是一种静态路网中求解最短路径最有效的直接搜索方法，也是解决许多搜索问题的有效算法，广泛应用于室内机器人路径搜索、游戏动画路径搜索等。它是图搜索算法的一种。A*算法是一种启发式的搜索算法，它是基于深度优先算法(Depth First Search, DFS)和广度优先算法(Breadth First Search, BFS)的一种融合算法，按照一定原则确定如何选取下一个结点。启发式搜索算法指的是，从起点出发，先寻找起点相邻的栅格，判断它是否是最好的位置，基于这个最好的栅格再往外向其相邻的栅格扩展，找到一个此时最好的位置，通过这样一步一步逼近目标点，减少盲目的搜索，提高了可行性和搜索效率。深度优先搜索算法的思想是，搜索算法从起点开始进行搜索(初始状态下待搜索区域内所有结点均未被访问)，与周围所有邻点进行比较，选择其中距离终点最近的点进行存储，然后再以该邻点为基础对比其周围未被访问的所有邻点，仍然选择距离终点最近的邻点存储。若访问结点到达终点或访问完所有结点仍未到达终点，则视为搜索失败。成功搜索所存储的结点连接而成的路径即为起点到终点的路径。广度优先搜索的原理是，从初始点出发依次访问与它相连接的邻点，访问完毕后再从这些邻点出发访问邻点的邻点,但是要保证先被访问的邻点的邻点要比后被访问的邻点的邻点先访问，直至图中所有已被访问的结点的邻点都被访问到。如果此时图中尚有未被访问的结点，则需要选取一个尚未被访问的结点作为个新的初始点,继续搜索访问，直到图中所有的结点都被访问一遍为止。因此深度优先算法与广度优先搜索算法从过程上存在较大差异。深度优先算法优先选择离目标点最近的结点，而广度优先搜索算法优先选择离初始点最近的点。基于深度优先算法,能以最快的速度找到一条连接初始点到目标点的路径，但不能保证路径的最优性(例如以路径最短为标准)；广度优先搜索算法则必然能找到最短的路径，但由于需要遍历所有的结点，其计算复杂程度更大。基于这两种算法的优缺点，A*算法基于启发函数构建了代价函数，既考虑了新结点距离初始点的代价，又考虑了新结点与目标点距离的代价。A*算法使用一个路径优劣评价公式为：$ f(n)=g(n)+h(n)$f(n) 是从初始状态经由状态n到目标状态的代价估计，g(n) 是从初始状态到状态n的实际代价，h(n) 是从状态n到目标状态的最佳路径的估计代价。A*算法需要维护两个状态表，分别称为openList表和closeList表。openList表由待考察的节点组成， closeList表由已经考察过的节点组成。2. 算法精讲2.1 预处理如图，假设我们需要从A点到目标点，这两点之间有一堵墙。首先，我们把地图栅格化，把每一个方格的中心称为节点；这个特殊的方法把我们的搜索区域简化为了 2 维数组。数组的每一项代表一个格子，它的状态就是可走 (walkalbe) 和不可走 (unwalkable) 。通过计算出从 A 到 目标点需要走过哪些方格，就找到了路径。一旦路径找到了，便从一个方格的中心移动到另一个方格的中心，直至到达目的地。2.2 开始搜索一旦我们把搜寻区域简化为一组可以量化的节点后，我们下一步要做的便是查找最短路径。在 A* 中，我们从起点开始，检查其相邻的方格，然后向四周扩展，直至找到目标。从起点 A 开始，定义A为父节点，并把它加入到 openList中。现在 openList 里只有起点 A ，后面会慢慢加入更多的项。如上图所示，父节点A周围共有8个节点，定义为子节点。将子节点中可达的（reachable）或者可走的（walkable）放入openList中，成为待考察对象。若某个节点既未在openList，也没在closeList中，则表明还未搜索到该节点。初始时， 节点A离自身距离为0，路径完全确定，将其移入closeList中； closeLi…

A星算法原理讲解
type: item
date: 2023-06-24T20:25:00+08:00
description: A* 算法原理讲解参考链接原文地址https://github.com/paulQuei/a-star-algorithm?spm=a2c6h.12873639.0.0.21834662LgAvMP算法介绍A*（念做：A Star）算法是一种很常用的路径查找和图形遍历算法。它有较好的性能和准确度。本文在讲解算法的同时也会提供Python语言的代码实现，并会借助matplotlib库动态的展示算法的运算过程。A*算法最初发表于1968年，由Stanford研究院的Peter Hart, Nils Nilsson以及Bertram Raphael发表。它可以被认为是Dijkstra算法的扩展。由于借助启发函数的引导，A*算法通常拥有更好的性能。广度优先搜索为了更好的理解A*算法，我们首先从广度优先（Breadth First）算法讲起。正如其名称所示，广度优先搜以广度做为优先级进行搜索。从起点开始，首先遍历起点周围邻近的点，然后再遍历已经遍历过的点邻近的点，逐步的向外扩散，直到找到终点。这种算法就像洪水（Flood fill）一样向外扩张，算法的过程如下图所示：在上面这幅动态图中，算法遍历了图中所有的点，这通常没有必要。对于有明确终点的问题来说，一旦到达终点便可以提前终止算法，下面这幅图对比了这种情况： 在这里插入图片描述在执行算法的过程中，每个点需要记录达到该点的前一个点的位置 – 可以称之为父节点。这样做之后，一旦到达终点，便可以从终点开始，反过来顺着父节点的顺序找到起点，由此就构成了一条路径。Dijkstra算法Dijkstra算法是由计算机科学家Edsger W. Dijkstra在1956年提出的。Dijkstra算法用来寻找图形中节点之间的最短路径。考虑这样一种场景，在一些情况下，图形中相邻节点之间的移动代价并不相等。例如，游戏中的一幅图，既有平地也有山脉，那么游戏中的角色在平地和山脉中移动的速度通常是不相等的。在Dijkstra算法中，需要计算每一个节点距离起点的总移动代价。同时，还需要一个优先队列结构。对于所有待遍历的节点，放入优先队列中会按照代价进行排序。在算法运行的过程中，每次都从优先队列中选出代价最小的作为下一个遍历的节点。直到到达终点为止。下面对比了不考虑节点移动代价差异的广度优先搜索与考虑移动代价的Dijkstra算法的运算结果： 请添加图片描述当图形为网格图，并且每个节点之间的移动代价是相等的，那么Dijkstra算法将和广度优先算法变得一样。最佳优先搜索在一些情况下，如果我们可以预先计算出每个节点到终点的距离，则我们可以利用这个信息更快的到达终点。其原理也很简单。与Dijkstra算法类似，我们也使用一个优先队列，但此时以每个节点到达终点的距离作为优先级，每次始终选取到终点移动代价最小（离终点最近）的节点作为下一个遍历的节点。这种算法称之为最佳优先（Best First）算法。这样做可以大大加快路径的搜索速度，如下图所示： 请添加图片描述但这种算法会不会有什么缺点呢？答案是肯定的。因为，如果起点和终点之间存在障碍物，则最佳优先算法找到的很可能不是最短路径，下图描述了这种情况。A*算法对比了上面几种算法，最后终于可以讲解本文的重点：A*算法了。下面的描述我们将看到，A*算法实际上是综合上面这些算法的特点于一身的。A*算法通过下面这个函数来计算每个节点的优先级。$ f(n) = g(n)+h(n)$ 其中：f(n)是节点n的综合优先级。当我们选择下一个要遍历的节点时，我们总会选取综合优先级最高（值最小）的节点。g(n)是节点n距离起点的代价。h(n)是节点n距离终点的预计代价，这也就是A*算法的启发函数。关于启发函数我们在下面详细讲解。A* 算法在运算过程中，每次从优先队列中选取 f(n)值最小（优先级最高）的节点作为下一个待遍历的节点。另外，A*算法使用两个集合来表示待遍历的节点，已经遍历过的节点，这通常称之为open_set和close_set。完整的A*算法描述如下：* 初始化open_set和close_set； * 将起点加入open_set中，并设置优先级为0（优先级最高）； * 如果open_set不为空，则从open_set中选取优先级最高的节点n： * 如果节点n为终点，则： * 从终点开始逐步追踪parent节点，一直达到起点； * 返回找到的结果路径，算法结束； * 如果节点n不是终点，则： * 将节点n从open_set中删除，并加入close_set中； * 遍历节点n所有的邻近节点： * 如果邻近节点m在close_set中，则： * 跳过，选取下一个邻近节点 * 如果邻近节点m也不在open_set中，则： * 设置节点m的parent为节点n * 计算节点m的优先级 * 将…

深度优先搜索（DFS）和广度优先搜索（BFS）
type: item
date: 2023-06-24T15:43:00+08:00
description: 深度优先搜索和广度优先搜索，都是图形搜索算法，它两相似，又却不同，在应用上也被用到不同的地方。这里拿一起讨论，方便比较。先给大家说一下两者大概的区别：如果搜索是以接近起始状态的程序依次扩展状态的，叫广度优先搜索。如果扩展是首先扩展新产生的状态，则叫深度优先搜索。深度优先搜索：对每一个可能的分支路径深入到不能再深入为止，而且每个结点只能访问一次。广度优先搜索：又叫层次遍历，从上往下对每一层依次访问，在每一层中，从左往右（也可以从右往左）访问结点，访问完一层就进入下一层，直到没有结点可以访问为止。二叉树的深度优先遍历的非递归的通用做法是采用栈，广度优先遍历的非递归的通用做法是采用队列。通常深度优先搜索法不全部保留结点，扩展完的结点从数据库中弹出删去，这样，一般在数据库中存储的结点数就是深度值，因此它占用空间较少。所以，当搜索树的结点较多，用其它方法易产生内存溢出时，深度优先搜索不失为一种有效的求解方法。 广度优先搜索算法，一般需存储产生的所有结点，占用的存储空间要比深度优先搜索大得多，因此，程序设计中，必须考虑溢出和节省内存空间的问题。但广度优先搜索法一般无回溯操作，即入栈和出栈的操作，所以运行速度比深度优先搜索要快些。一、深度优先搜索深度优先搜索属于图算法的一种，是一个针对图和树的遍历算法，英文缩写为DFS即Depth First Search。深度优先搜索是图论中的经典算法，利用深度优先搜索算法可以产生目标图的相应拓扑排序表，利用拓扑排序表可以方便的解决很多相关的图论问题，如最大路径问题等等。一般用栈stack数据结构来辅助实现DFS算法。其过程简要来说是对每一个可能的分支路径深入到不能再深入为止，而且每个节点只能访问一次。若将bfs策略应用于树结构，其效果等同与前中后序遍历。对于下面的树而言，DFS方法首先从根节点1开始，其搜索节点顺序是1,2,3,4,5,6,7,8（假定左分枝和右分枝中优先选择左分枝）。从stack中访问栈顶的点；找出与此点邻接的且尚未遍历的点，进行标记，然后放入stack中，依次进行；如果此点没有尚未遍历的邻接点，则将此点从stack中弹出，再按照（3）依次进行；直到遍历完整个树，stack里的元素都将弹出，最后栈为空，DFS遍历完成。1.2 代码模板确认递归函数，参数首先我们dfs函数一定要存一个图，用来遍历的，还要存一个目前我们遍历的节点，定义为x。至于单一路径，和路径集合可以放在全局变量，那么代码是这样的：vector<vector<int>> result; // 收集符合条件的路径 vector<int> path; // 0节点到终点的路径 // x：目前遍历的节点 // graph：存当前的图 void dfs (vector<vector<int>>& graph, int x)确认终止条件什么时候我们就找到一条路径了？当目前遍历的节点 为 最后一个节点的时候，就找到了一条，从 出发点到终止点的路径。当前遍历的节点，我们定义为x，最后一点节点，就是 graph.size() - 1。所以 但 x 等于 graph.size() - 1 的时候就找到一条有效路径。 代码如下：// 要求从节点 0 到节点 n-1 的路径并输出，所以是 graph.size() - 1 if (x == graph.size() - 1) { // 找到符合条件的一条路径 result.push_back(path); // 收集有效路径 return; }处理目前搜索节点出发的路径接下来是走 当前遍历节点x的下一个节点。首先是要找到 x节点链接了哪些节点呢？ 遍历方式是这样的：for (int i = 0; i < graph[x].size(); i++) { // 遍历节点n链接的所有节点接下来就是将 选中的x所连接的节点，加入到 单一路径来。path.push_back(graph[x][i]); // 遍历到的节点加入到路径中来当前遍历的节点就是 graphx 了，所以进入下一层递归dfs(graph, graph[x][i]); // 进入下一层递归`最后就是回溯的过程，撤销本次添加节点的操作。 该过程代码：for (int i = 0; i < graph[x].size(); i++) { // 遍历节点n链接的所有节点 path.push_back(graph[x][i]); // 遍历到的节点加入到路径中来 dfs(graph, graph[x][i]); // 进入下一层递归 path.pop_back(); // 回溯，撤销本节点 }整体代码// 深度优先搜索 C++代码模板 class Solution { private: vector<vector<int>> result; /…

A星算法
type: item
date: 2023-06-08T17:20:00+08:00
description: 原理D（D-Star）算法是一种增量式路径规划算法，用于动态环境中的全局路径规划。它可以在环境变化时实时更新路径，并在需要时重新规划。D算法基于Dijkstra算法和A*算法，其原理如下：初始化：设置起点为当前位置，终点为目标位置。将起点的代价设为0，将其他节点的代价设为无穷大（或一个较大的值）。将所有节点标记为未更新状态。选择当前节点：从未更新的节点中选择一个具有最小代价的节点作为当前节点。更新邻居节点的代价：对于当前节点的每个邻居节点，计算通过当前节点到达邻居节点的代价。如果这个代价小于邻居节点当前的代价，则更新邻居节点的代价。重复步骤2-3，直到所有节点都被更新。更新路径：如果目标节点的代价发生变化，则重新规划路径。从目标节点开始，通过每个节点的最小代价父节点指针，逆向构建路径。循环更新：在实际应用中，D*算法还会根据环境的变化和实际需求，周期性地重复步骤2-5，以保持路径的实时更新和重新规划。D算法的关键在于增量更新节点的代价。通过动态地更新节点的代价，并根据代价的变化重新规划路径，D算法可以适应环境的变化，并在需要时实时更新路径。需要注意的是，D算法在实现上可能较为复杂，涉及到代价值的更新、路径的修正和优先队列的管理等。同时，D算法也有一些变种和改进版本，如D* Lite算法，用于进一步优化算法的效率和实时性。总而言之，D*算法是一种增量式路径规划算法，适用于动态环境中的全局路径规划。它通过实时更新节点的代价值，并重新规划路径，实现路径的实时更新和重新规划。

路径规划算法总览
type: item
date: 2023-06-08T17:18:00+08:00
description: 什么是路径规划路径规划是在给定环境中确定从起点到终点的最佳路径的过程。在机器人导航、自动驾驶和物流等领域中，路径规划是实现自主移动和导航的重要任务。路径规划的目标是找到一条能够满足特定条件和约束的路径，使机器人或车辆能够从起始位置安全、高效地到达目标位置。这些条件和约束可能包括避开障碍物、考虑运动约束（如最大速度、最大加速度）、最小化时间或能量消耗等。路径规划通常是在静态环境中进行的，它可以是离线计算的（预先规划好的路径），也可以是在线计算的（实时计算适应动态环境）。路径规划算法的选择取决于环境特点、机器人能力以及具体需求。路径规划的应用广泛，例如无人机的航迹规划、自动驾驶车辆的路线规划、仓储机器人的路径规划等。通过有效的路径规划，机器人能够在复杂环境中快速、安全地移动，并实现各种任务和应用。全局路径算法概览在二维栅格地图中进行全局路径规划时，有几种常见的算法可以使用。以下是一些常见的二维栅格地图全局路径规划算法：Dijkstra算法：Dijkstra算法是一种基于图的搜索算法，可以用于栅格地图的全局路径规划。它通过计算起点到其他点的最短路径来确定最优路径。A*算法：A*算法是一种启发式搜索算法，结合了Dijkstra算法和启发式函数。它使用估计的距离来指导搜索过程，从而更高效地找到最短路径。D算法：D算法是一种增量搜索算法，用于动态环境中的全局路径规划。它可以根据环境的变化实时更新路径，并在需要时重新规划。RRT（Rapidly-Exploring Random Tree）算法：RRT是一种基于随机采样的树形搜索算法，常用于非精确全局路径规划问题。它通过不断扩展树结构来探索空间，并找到可行路径。PRM（Probabilistic Roadmap）算法：PRM是一种基于随机采样的概率路图算法。它首先在地图中随机采样一组节点，并使用有效的边连接它们。然后，使用启发式搜索算法（如A*）在这个图上找到最优路径。RRT 算法：RRT算法是RRT的改进版本，它在RRT算法的基础上引入了优化过程，以获得更优的路径。LPA算法：LPA（Lifelong Planning A*）算法是一种增量式启发式搜索算法，用于动态环境中的全局路径规划。它可以在环境变化时实时更新路径。这些算法在二维栅格地图全局路径规划中比较常见。根据具体的应用场景、地图特征和机器人运动模型等因素，可能会选择适合的路径规划算法。同时，还有其他算法和技术，如基于图优化的路径规划、深度学习方法等，也可以用于二维栅格地图全局路径规划。选择适当的路径规划算法需要根据具体需求和限制进行综合考虑。局部路径算法概览在二维栅格地图中进行局部路径规划时，有几种常见的算法可以使用。以下是一些常见的二维栅格地图局部路径规划算法：DWA（Dynamic Window Approach）算法：DWA算法是一种基于动态窗口的局部路径规划算法。它通过在机器人当前位置附近生成一组可行的速度窗口，然后评估每个速度窗口的代价，并选择具有最小代价的窗口对应的速度作为下一步的运动指令。RRT（Rapidly-Exploring Random Tree Star）算法：RRT算法是RRT算法的改进版本，用于局部路径规划。它使用树形结构进行探索，通过不断扩展树来搜索可行路径，并通过优化过程来提高路径质量。MPC（Model Predictive Control）算法：MPC算法是一种优化控制方法，可以用于局部路径规划。它将路径规划问题转化为一个优化问题，并通过求解优化问题来得到最优控制策略，以实现目标路径跟踪。Elastic Band算法：Elastic Band算法是一种基于弹性带的局部路径规划方法。它使用一条连续的弹性带来表示机器人周围的可行路径，并通过不断调整带的形状来适应环境变化和路径优化。TEB（Timed Elastic Band）算法：TEB算法是基于弹性带的局部路径规划算法的改进版本，引入了时间约束。它可以考虑机器人的动力学约束和避障约束，并生成可行的时间优化路径。这些算法在二维栅格地图局部路径规划中比较常见。根据具体的应用场景、机器人的动力学特性和环境要求等因素，可能会选择适合的路径规划算法。此外，还有其他算法和技术，如经典的速度优化算法、机器学习方法等，也可以在局部路径规划中使用。选择适当的局部路径规划算法需要综合考虑问题的特点和约束条件。https://blog.csdn.net/lbperfect123/article/details/84281300)

Dijkstra算法
type: item
date: 2023-06-08T17:16:00+08:00
description: 原理解释1Dijkstra算法是一种经典的图搜索算法，用于求解带权重图中的最短路径问题。它的原理如下：初始化：将起始节点的距离设为0，将所有其他节点的距离设为无穷大（或一个较大的值）。将所有节点标记为未访问状态。选择当前节点：从未访问的节点中选择一个距离最小的节点作为当前节点。更新邻居节点的距离：对于当前节点的每个邻居节点，计算通过当前节点到达邻居节点的距离。如果这个距离小于邻居节点当前的距离，则更新邻居节点的距离。标记当前节点为已访问：将当前节点标记为已访问。重复步骤2-4，直到所有节点都被标记为已访问，或者目标节点被访问。构建最短路径：从目标节点开始，通过每个节点的最短路径父节点指针，逆向构建最短路径。Dijkstra算法通过不断更新节点的距离值，从起始节点开始逐步扩展搜索，直到找到目标节点或者遍历了所有可达节点。它保证在每一次选择当前节点时，当前节点的距离是已知的最小值，因此在遇到目标节点时，得到的路径就是最短路径。需要注意的是，Dijkstra算法适用于无负权重边的图。如果图中存在负权重边，可以考虑使用其他算法，如Bellman-Ford算法或A*算法。总结起来，Dijkstra算法是一种用于求解带权重图中最短路径的经典算法。它通过不断更新节点的距离值，选择当前最短路径的节点，直到找到目标节点或遍历完所有可达节点。原理解释2Dijkstra算法是一种用于求解图中单源最短路径的经典算法。它可以找到从给定起始节点到图中所有其他节点的最短路径。以下是Dijkstra算法的原理步骤：创建一个空的距离表（distance table）和一个空的前驱表（predecessor table）来存储节点的最短路径信息。将起始节点标记为当前节点，并将其距离设置为0。将其他节点的距离设置为无穷大（或一个较大的值）。选择当前节点，并计算从起始节点到当前节点经过的路径长度。遍历当前节点的所有邻居节点（即与当前节点相连的节点），并更新它们的距离。如果通过当前节点到达邻居节点的路径比之前计算的路径更短，则更新邻居节点的距离，并将当前节点设置为邻居节点的前驱节点。标记当前节点为已访问。从尚未访问的节点中选择下一个最短路径节点作为当前节点，即选择距离表中具有最小距离的节点。重复步骤4至步骤6，直到所有节点都被访问或无法到达的节点已被标记。最终，距离表中存储了从起始节点到每个节点的最短路径长度，前驱表中存储了每个节点的前驱节点信息，可以根据前驱表反向追溯最短路径。Dijkstra算法的关键思想是通过每次选择距离起始节点最近的节点，逐步扩展最短路径的范围，直到到达目标节点或访问完所有节点。该算法保证了在有向无环图（DAG）中的正确性，但不适用于存在负权边的图。Dijkstra算法的时间复杂度取决于图的规模，通常为$O(|V|^2)$，其中|V|是节点的数量。为了提高效率，可以使用优先队列（如最小堆）来选择下一个最短路径节点，将时间复杂度优化为O((|V| + |E|) log |V|)，其中|E|是边的数量。Dijkstra算法图文详解Dijkstra算法Dijkstra算法算是贪心思想实现的，首先把起点到所有点的距离存下来找个最短的，然后松弛一次再找出最短的，所谓的松弛操作就是，遍历一遍看通过刚刚找到的距离最短的点作为中转站会不会更近，如果更近了就更新距离，这样把所有的点找遍之后就存下了起点到其他所有点的最短距离。问题引入：指定一个点（源点）到其余各个顶点的最短路径，也叫做“单源最短路径”。例如求下图中的1号顶点到2、3、4、5、6号顶点的最短路径。下面我们来模拟一下：这就是Dijkstra算法的基本思路。Dijkstra算法详解 通俗易懂 - 知乎 (zhihu.com)[Dijkstra算法图文详解_black-hole6的博客-CSDN博客](