【动静障碍物】基于JPS算法（改进A*）全局路径规划与DWA动态窗口局部避障的机器人自主导航混合控制算法（Matlab代码实现）
💥💥💞💞欢迎来到本博客❤️❤️💥💥
🏆博主优势：🌞🌞🌞博客内容尽量做到思维缜密，逻辑清晰，为了方便读者。
⛳️座右铭：行百里者，半于九十。
📋📋📋本文内容如下：🎁🎁🎁
⛳️赠与读者
👨💻做科研，涉及到一个深在的思想系统，需要科研者逻辑缜密，踏实认真，但是不能只是努力，很多时候借力比努力更重要，然后还要有仰望星空的创新点和启发点。建议读者按目录次序逐一浏览，免得骤然跌入幽暗的迷宫找不到来时的路，它不足为你揭示全部问题的答案，但若能解答你胸中升起的一朵朵疑云，也未尝不会酿成晚霞斑斓的别一番景致，万一它给你带来了一场精神世界的苦雨，那就借机洗刷一下原来存放在那儿的“躺平”上的尘埃吧。
或许，雨过云收，神驰的天地更清朗.......🔎🔎🔎
💥第一部分——内容介绍
基于JPS算法与DWA动态窗口法的机器人自主导航混合控制算法研究
摘要：本文聚焦于机器人、车辆等智能体在复杂环境下的自主导航控制问题，提出了一种结合JPS算法（改进的A*算法）与DWA动态窗口法的混合控制算法。该算法通过全局路径规划与局部路径规划的有机结合，有效应对静态障碍物、移动障碍物以及自定义障碍物等多种情景，实现智能体高效、安全的自主导航。实验结果表明，该算法显著提升了智能体的导航性能，路径规划更合理且平滑，转向次数减少，具备实时避障能力。
关键词：JPS算法；DWA动态窗口法；自主导航；混合控制算法；障碍物处理
一、引言
随着机器人技术和智能交通领域的快速发展，机器人、车辆等智能体在复杂环境中的自主导航能力成为研究热点。在实际应用场景中，智能体不仅需要面对静态障碍物，还需应对移动障碍物以及自定义形状的特殊障碍物等复杂情况。传统的路径规划算法往往难以同时满足全局路径的合理性与局部避障的实时性要求。因此，研究一种能够有效处理多种障碍物情景的混合控制算法具有重要的理论意义和实际应用价值。
JPS算法作为A*算法的改进版本，通过优化搜索策略，能够显著提高全局路径规划的效率；而DWA动态窗口法则充分考虑了机器人的运动学约束，在局部避障方面表现出色。本文将这两种算法相结合，提出一种混合控制算法，以实现智能体在复杂环境下的自主导航。
二、相关工作
2.1 全局路径规划算法
全局规划的目标是根据静态的全局信息给出一条连接起点和终点的路径，保证路径上无障碍物。最典型的全局路径规划算法是A搜索算法，它依赖代价地图，将实际场景按一定尺度划分成栅格，根据地图中的障碍物信息衡量每个栅格受周围障碍物影响所产生的代价，构成代价地图。通过启发式函数 f(n)=g(n)+h(n) 快速地在代价地图上进行规划，其中 g(n) 代表到达状态 n 花费的总代价， h(n) 代表从状态 n 到终点预估需要花费的代价，从而搜索得到连接起点栅格和终点栅格且代价总和近似最优的路径。然而，A算法在实际使用过程中要求环境信息保持不变，若障碍物地图发生变化，则需要重新规划。在大规模场景、机械臂控制或者复杂动力学规划等状态空间巨大的问题中，其实时性和高效性难以保障。
基于空间采样的规划方法降低了搜索空间，从而在高维状态空间中进行规划。其中概率路网图（Probabilistic Roadmaps，简称PRM）在地图空间按一定密度随机采样，将合法的点构成路标点集 V，然后判断这些路标点之间的连通性，形成边集 E，从而构建路网图 G=(V,E)，高维空间中的规划就转化为路网图上的搜索问题。另一种基于空间采样的规划方法是快速随机扩展树（Random Rapid - search Tree，简称RRT）方法，它从起点开始生成一个树状结构，每次在空间内随机采样合法点并与周围的树节点连接，从而不断扩展树结构直到目标点，全局路径即为起点到目标点的树上路径。基于空间采样的算法具有概率完备性，但并不保证给出最优解，相比于确定性搜索算法，它们规划效率高，因此受到广泛使用。
2.2 局部路径规划算法
路径规划算法的目的是基于已知的环境状态，求解出一条从起点到终点的可行路径并准确执行。实际导航过程中，由于机器人传感器误差、定位不准确和环境中出现动态障碍物等因素的影响，很难保证机器人在移动过程中严格按照导航算法指定的路径运动。因此，学者提出一类局部规划算法，利用机器人局部感知信息和地图的局部障碍物信息，在短距离内进行导航规划，从而应对环境的动态性和控制的不确定性。
DWA动态窗口法是一种用于局部路径规划的高效方法，尤其适用于动态环境中的避障。该算法在ROS的move_base堆栈中有实现，通过限制速度和加速度来确保安全轨迹。其关键在于基于运动动力学推导，考虑机器人的惯性和速度限制，以计算安全的轨迹。评价函数结合目标接近度、速度和障碍物距离，寻找最优路径。
三、基于JPS算法的全局路径规划
3.1 JPS算法基本原理
JPS（Jump Point Search，跳跃点搜索）算法是在A算法基础上进行改进的一种全局路径规划算法。它针对A算法在搜索过程中需遍历所有相邻节点（4/8方向），计算它们的 f(n) 值并加入优先队列，导致大规模地图中冗余计算过多的问题，通过改变搜索方向、避免斜穿障碍物顶点以及删除中间多余节点等策略，显著减少了搜索的节点数量，提高了路径规划的效率。
JPS算法的核心思想是“跳跃点剪枝”，通过数学规则筛选出“能影响路径最优性”的关键节点（跳跃点），跳过大量冗余邻居节点，在不损失路径最优性的前提下，大幅提升搜索效率。其核心保障在于跳跃点的筛选规则严格遵循“最优路径必经过跳跃点”，因此JPS规划的路径与A*完全一致，具有最优性。
3.2 JPS算法的改进策略
3.2.1 改变搜索方向
传统A*算法通常采用八邻域搜索方式，即从当前节点向八个方向进行扩展。JPS算法在此基础上进行了优化，根据地图的布局和障碍物的分布，智能地选择搜索方向。例如，在开阔区域，优先选择直线方向进行搜索，减少不必要的转向；在靠近障碍物时，根据障碍物的形状和位置，调整搜索方向以避免碰撞。
3.2.2 避免斜穿障碍物顶点
在A*算法中，机器人可能会斜穿障碍物的顶点，这不仅不符合实际运动情况，还可能导致路径不够平滑。JPS算法通过引入强制邻域的概念，避免了机器人斜穿障碍物顶点的情况。当机器人接近障碍物顶点时，强制其沿着障碍物的边缘移动，确保路径的合理性和安全性。
3.2.3 删除中间多余节点
JPS算法在搜索过程中会记录关键节点，这些节点构成了路径的基本框架。然而，这些关键节点之间可能存在一些不必要的中间节点，导致路径不够平滑。为了解决这一问题，JPS算法在找到路径后，对路径进行后处理，删除中间多余的节点，使路径更加平滑，减少机器人的转向次数。
3.2.4 修改评价函数
A*算法的评价函数通常由实际代价和启发式代价两部分组成，用于评估每个节点的优先级。JPS算法对评价函数进行了改进，根据不同的应用场景和需求，调整实际代价和启发式代价的权重。例如，在需要优先考虑路径长度的情况下，可以适当增大实际代价的权重；在需要快速找到路径的情况下，可以增大启发式代价的权重。
3.3 JPS算法工作流程
JPS算法以2D网格地图为例，其工作流程如下：
- 初始化：设置起点和终点，初始化开放列表和关闭列表，将起点加入开放列表。
- 跳跃点搜索：从开放列表中取出 f(n) 值最小的节点作为当前节点，进行跳跃点搜索。沿不同方向进行跳跃式移动，若移动过程中遇到障碍物，停止该方向搜索；若移动过程中检测到“强制邻居”或“自然转折点”，则当前位置为跳跃点，将其加入开放列表；若直接到达终点，终点为跳跃点，搜索终止。
- 路径回溯：当终点被加入开放列表后，从终点反向遍历跳跃点，补全跳跃点之间的直线路径，得到全局最优路径。
四、基于DWA动态窗口法的局部路径规划
4.1 DWA算法基本原理
DWA（Dynamic Window Approach）动态窗口法是一种基于速度空间的局部路径规划算法。该方法充分考虑了机器人的运动学模型，包括速度、加速度等约束条件，在动态窗口内采样速度，并根据评价函数选择最优速度，从而实现机器人的局部避障。
4.2 DWA算法步骤
4.2.1 建立机器人运动模型
假设机器人采用两轮移动模型，其轨迹是一段一段的圆弧或者直线（旋转速度为0时）。计算机器人的移动轨迹时，假设相邻时刻机器人的轨迹为直线，该时刻机器人自身坐标系与世界坐标系的夹角为 θ，则机器人在该时刻的位移可以简单换算成在世界坐标系的位移。更准确的做法是用圆弧来代替，以提高轨迹计算的精度。
4.2.2 产生轨迹
- 计算动态窗口：
- 移动机器人受自身最大速度和最小速度的限制。
- 受电机性能的影响，存在最大的加减速限制，因此在机器人轨迹前向模拟的周期 sim_period 内，存在一个动态窗口，在该窗口内的速度是机器人能够实际达到的速度。
- 基于移动机器人安全的考虑，为了能够在碰到障碍物前停下来，因此在最大减速度条件下，速度有一个范围。
- 预测轨迹：在满足约束条件的情况下，在速度空间（ v,w ）中采样，根据运动学模型推测对应的轨迹。
4.2.3 评价轨迹
设计一个评价函数，用于评估每一条模拟轨迹的优劣。评价函数通常包括多个子目标，如目标导向性、障碍物避障性、速度平滑性等。例如，评价函数可以表示为：
G(v,w)=α⋅heading(v,w)+β⋅velocity(v,w)+γ⋅obstacle_distance(v,w)
其中， heading(v,w) 为方位角评价函数，评价机器人在当前的设定的速度下，轨迹末端朝向与目标点之间的角度差距； velocity(v,w) 为当前机器人的线速度，为了促进机器人快速到达目标； obstacle_distance(v,w) 代表机器人在当前轨迹上与最近的障碍物之间的距离，如果这条轨迹上没有障碍物，那就将其设定一个常数； α、 β、 γ 为权重。
4.2.4 选择最优轨迹
根据评价函数对每条轨迹进行打分，选取最优的轨迹对应的速度发送给机器人，机器人按照该速度运动一段时间后，重新进行速度采样和轨迹模拟，实现实时避障。
4.3 针对动态障碍物的改进
在实际应用中，机器人不仅会遇到静态障碍物，还会遇到移动障碍物。为了使机器人能够有效应对移动障碍物，本文对DWA算法进行了改进。在实时检测到障碍物后，根据障碍物的位置和运动状态，预测障碍物在未来一段时间内的位置变化。在评价函数中增加对动态障碍物的避障项，根据障碍物的预测位置调整机器人的速度（线速度和角速度），使机器人能够提前避开移动障碍物，避免碰撞。
五、混合控制算法实现
5.1 混合控制算法框架
本文提出的混合控制算法将JPS算法和DWA算法有机结合，整体框架如下：首先，使用JPS算法进行全局路径规划，得到从起点到终点的全局最优路径；然后，机器人沿着全局路径前进，在前进过程中，使用DWA算法进行局部避障，实时检测周围的障碍物，并根据障碍物的位置和运动状态调整机器人的速度，确保机器人能够安全避开障碍物；当机器人遇到全局路径被阻断或需要重新规划路径的情况时，重新启动JPS算法进行全局路径规划，更新全局路径。
5.2 三次折线优化
在A*算法找到路径后，为了进一步优化路径，减少机器人的转向次数，本文进行了三次折线优化。具体步骤如下：首先，对全局路径进行初步分析，找出路径中的转折点；然后，以每个转折点为中心，在一定范围内搜索是否存在更优的路径段，使得路径更加平滑；最后，通过三次折线拟合的方法，对路径进行优化，使机器人在运动过程中能够更加流畅地转向，减少不必要的转向操作。
六、实验结果与分析
6.1 实验环境搭建
为了验证本文提出的混合控制算法的有效性，搭建了实验环境，包括静态障碍物、移动障碍物以及自定义障碍物等多种情景。实验中使用机器人模型进行模拟实验，机器人的运动学参数根据实际机器人进行设置。
6.2 实验结果
6.2.1 静态障碍物场景
在静态障碍物场景下，混合控制算法能够快速规划出全局最优路径，并通过DWA算法实现局部避障。实验结果表明，机器人能够顺利避开所有静态障碍物，安全到达终点，且路径平滑，转向次数较少。
6.2.2 移动障碍物场景
在移动障碍物场景下，改进后的DWA算法能够实时检测移动障碍物的位置和运动状态，并根据预测结果调整机器人的速度，使机器人能够提前避开移动障碍物。实验结果显示，机器人在遇到移动障碍物时能够及时做出反应，避免碰撞，继续沿着规划的路径前进。
6.2.3 自定义障碍物场景
在自定义障碍物场景下，JPS算法能够根据障碍物的形状和位置，合理规划全局路径，DWA算法则能够在局部范围内实现精确避障。实验结果表明，混合控制算法能够有效处理自定义障碍物，规划出合理的路径，确保机器人安全导航。
七、结论与展望
7.1 结论
本文提出了一种结合JPS算法与DWA动态窗口法的机器人自主导航混合控制算法，通过全局路径规划与局部路径规划的有机结合，有效应对了静态障碍物、移动障碍物以及自定义障碍物等多种情景。实验结果表明，该算法显著提升了智能体的导航性能，路径规划更合理且平滑，转向次数减少，具备实时避障能力。
7.2 展望
未来的研究可以进一步优化混合控制算法，例如结合深度学习技术，提高算法对复杂环境的适应性和鲁棒性；研究多机器人协同导航的混合控制算法，实现多个机器人在复杂环境中的高效协作；探索混合控制算法在实际应用中的更多场景，如无人机导航、自动驾驶等领域。
📚第二部分——运行结果
🎉第三部分——参考文献
文章中一些内容引自网络，会注明出处或引用为参考文献，难免有未尽之处，如有不妥，请随时联系删除。(文章内容仅供参考，具体效果以运行结果为准)
🌈第四部分——Matlab代码实现
资料获取，更多粉丝福利，MATLAB|Simulink|Python资源获取
