Dijkstra、A_、Theta_、JPS、D_、LPA_、D_ Lite、RRT、RRT_、RRT-Connect、Informed RRT_、ACO、Voronoi、PID、LQR、MPC、AP
✅作者简介：热爱科研的Matlab仿真开发者，擅长毕业设计辅导、数学建模、数据处理、程序设计科研仿真。
🍎完整代码获取 定制创新 论文复现点击：Matlab科研工作室
👇 关注我领取海量matlab电子书和数学建模资料
🍊个人信条：做科研，博学之、审问之、慎思之、明辨之、笃行之，是为：博学慎思，明辨笃行。
🔥 内容介绍
一、引言
在运动规划与导航领域，路径规划算法起着关键作用，旨在为移动机器人或其他运动物体找到从起始点到目标点的最佳路径。这些算法需要考虑环境中的障碍物、运动物体的动力学约束等因素。本文将对常见的路径规划算法进行详细介绍。
二、搜索类算法
- Dijkstra 算法
- 原理
：该算法基于图搜索，从起始节点开始，以广度优先搜索的方式扩展节点，计算每个节点到起始节点的最短路径。它通过维护一个优先队列，每次选择距离起始点最近的未访问节点进行扩展。
- 特点
：能找到全局最优解，但计算复杂度较高，时间和空间消耗较大，尤其是在大规模地图中。
- 应用场景
：适用于静态环境且对路径最优性要求极高的场景，如一些小型、布局固定的室内环境导航。
- 原理
- A * 算法
- 原理
：结合了 Dijkstra 算法的广度优先搜索策略和贪心算法的思想。它使用一个启发函数来估计每个节点到目标节点的距离，通过将节点到起始点的实际距离与到目标点的估计距离相加，作为节点的优先级度量。
- 特点
：在大多数情况下，比 Dijkstra 算法更高效，能更快地找到最优路径。启发函数的设计对算法性能影响很大，好的启发函数可以显著减少搜索空间。
- 应用场景
：广泛应用于各种静态地图的路径规划，如游戏地图导航、机器人在仓库中的路径规划等。
- 原理
- Theta * 算法
- 原理
：是对 A * 算法的改进，主要针对网格地图。它在搜索过程中允许路径跨越多个网格单元，而不仅仅沿着网格边移动，通过检测路径是否穿过障碍物来优化路径。
- 特点
：生成的路径更平滑，相比 A * 算法在网格地图中能得到更优的路径，减少了路径的锯齿状。但计算复杂度略有增加。
- 应用场景
：适合在网格地图表示的环境中，对路径平滑度有要求的运动规划任务，如无人机在城市街区的飞行路径规划。
- 原理
- JPS（跳跃点搜索）算法
- 原理
：基于网格地图，通过定义跳跃点规则，在搜索过程中跳过一些不必要的节点扩展，从而减少搜索空间。跳跃点是指那些在不增加路径成本的情况下，能够直接跳跃到的节点。
- 特点
：在网格地图中具有很高的搜索效率，能快速找到最优路径，大大减少了节点扩展数量，降低计算复杂度。
- 应用场景
：常用于实时性要求较高的网格地图路径规划，如实时游戏中的角色导航。
- 原理
三、基于搜索与推理的动态算法
- D * 算法
- 原理
：适用于动态环境，基于 A算法改进而来。它在环境发生变化时，通过重新计算受影响区域的路径来更新路径。D算法维护一个开放列表和一个封闭列表，根据环境变化调整节点的优先级。
- 特点
：能够处理环境的动态变化，快速重新规划路径，但在环境频繁变化时，计算开销较大。
- 应用场景
：适用于环境可能发生变化的场景，如机器人在未知且动态变化的室内环境中导航。
- 原理
- LPA（增量式 A
）算法 **
- 原理
：同样针对动态环境，是一种增量式搜索算法。它通过复用之前搜索的信息，在环境变化时，仅对受影响的部分进行重新计算，而不是重新规划整个路径。
- 特点
：相比 D * 算法，在环境变化时能更高效地更新路径，减少计算量，提高响应速度。
- 应用场景
：适用于环境动态变化但变化频率相对较低的场景，如在有移动障碍物的仓库中机器人的路径规划。
- 原理
- DLite 算法
*
- 原理
：是 D * 算法的轻量级版本，进一步优化了内存使用和计算效率。它在处理动态环境变化时，通过双向搜索和对节点优先级的精细调整，快速更新路径。
- 特点
：在内存受限的情况下仍能有效工作，适用于资源有限的移动设备或机器人平台，同时保持对动态环境变化的快速响应能力。
- 应用场景
：常用于移动机器人在资源受限且环境动态变化的场景中导航，如小型无人机在复杂环境中的飞行路径规划。
- 原理
四、采样类算法
- RRT（快速探索随机树）算法
- 原理
：基于采样的思想，在搜索空间中随机采样点，将新采样点连接到树中距离最近的节点，逐步构建一棵随机树，直到树的节点包含目标点。
- 特点
：适用于高维复杂环境，能快速找到可行解，但不一定是最优解。由于其随机性，每次运行结果可能不同。
- 应用场景
：常用于机器人在复杂的三维空间或非结构化环境中的路径规划，如机器人在森林等复杂地形中的导航。
- 原理
- RRT * 算法
- 原理
：在 RRT 算法基础上，增加了路径优化机制。在扩展树的过程中，通过重连操作，使树的结构更优化，从而渐近收敛到最优解。
- 特点
：既能保持 RRT 算法在复杂环境中的搜索效率，又能随着迭代次数增加逐渐得到最优解，相比 RRT 算法路径质量更高。
- 应用场景
：适用于对路径质量有一定要求且环境复杂的运动规划任务，如自动驾驶车辆在城市道路中的路径规划。
- 原理
- RRT - Connect 算法
- 原理
：使用两棵随机树分别从起始点和目标点同时扩展，当两棵树的节点足够接近时，尝试连接两棵树，从而找到路径。
- 特点
：相比 RRT 算法，搜索速度更快，因为同时从两端进行搜索，减少了搜索空间。但实现相对复杂，需要维护两棵树的扩展和连接。
- 应用场景
：适用于大型复杂环境中对搜索速度要求较高的路径规划任务，如大型工业园区内的机器人导航。
- 原理
- Informed RRT * 算法
- 原理
：在 RRT * 算法基础上，利用目标信息来引导采样过程。通过构建一个包含起始点和目标点的椭圆，将采样点限制在椭圆区域内，从而更有效地搜索到最优路径。
- 特点
：相比 RRT * 算法，能更快地收敛到最优解，尤其是在高维空间中。利用目标信息使得采样更有针对性，减少了无效采样。
- 应用场景
：适用于高维复杂环境且对路径最优性和搜索速度都有较高要求的场景，如航空航天领域中飞行器的路径规划。
- 原理
五、其他算法
- ACO（蚁群优化）算法
- 原理
：模拟蚂蚁群体觅食行为，蚂蚁在路径上释放信息素，信息素浓度高的路径被选择的概率更大。随着时间推移，较短路径上的信息素积累更多，最终引导蚂蚁找到最优路径。
- 特点
：具有自适应性和并行性，能在复杂环境中找到较好的路径，但收敛速度相对较慢，参数调整对结果影响较大。
- 应用场景
：适用于复杂环境下的路径规划，如物流配送车辆的路径规划，多个配送任务可类比为蚂蚁的觅食路径搜索。
- 原理
- Voronoi 算法
- 原理
：将空间划分为多个区域，每个区域内的点到某个特定点（称为种子点）的距离比到其他种子点的距离更近。在路径规划中，可将障碍物视为种子点，Voronoi 图的边则可作为潜在的无碰撞路径。
- 特点
：生成的路径能最大限度地远离障碍物，但不一定是最短路径。适用于对避障要求较高的场景。
- 应用场景
：常用于机器人在充满障碍物的环境中导航，如在布满障碍物的工厂车间内机器人的路径规划。
- 原理
- PID（比例 - 积分 - 微分）控制算法
- 原理
：通过计算当前误差（目标值与实际值的差）的比例、积分和微分项，调整控制量，使系统输出尽可能接近目标值。在路径规划中，可用于控制机器人的运动，使其沿着规划好的路径行驶。
- 特点
：结构简单、稳定性好、工作可靠、调整方便，广泛应用于各种反馈控制系统。但对于复杂非线性系统，单独使用 PID 控制可能效果不佳。
- 应用场景
：常用于机器人路径跟踪阶段，结合其他路径规划算法，确保机器人准确沿着规划路径移动。
- 原理
- LQR（线性二次型调节器）算法
- 原理
：基于线性系统理论，通过最小化一个二次型性能指标来设计控制器。在路径规划中，可将机器人的运动模型线性化，通过 LQR 算法计算最优控制输入，使机器人沿着期望路径运动。
- 特点
：能在满足系统动态约束的前提下，找到最优控制策略，但对系统模型的准确性要求较高，且只适用于线性系统或可近似线性化的系统。
- 应用场景
：适用于具有精确数学模型的线性系统的路径规划与控制，如在一些高精度的工业机器人运动控制中。
- 原理
- MPC（模型预测控制）算法
- 原理
：基于系统的预测模型，在每个采样时刻预测未来一段时间内系统的输出，通过求解一个优化问题来确定当前时刻的控制输入，使系统输出尽可能接近目标值。MPC 具有滚动优化和反馈校正的特点。
- 特点
：能处理系统的约束条件，对模型误差和外部干扰有一定的鲁棒性，适用于复杂的多变量系统。但计算量较大，对硬件性能要求较高。
- 应用场景
：常用于自动驾驶车辆的路径规划与控制，以及工业过程控制中的复杂系统路径规划。
- 原理
- APF（人工势场）算法
- 原理
：将机器人视为在一个虚拟的势场中运动，目标点产生引力，障碍物产生斥力，机器人在合力作用下向目标点移动。
- 特点
：算法简单直观，实时性较好，但容易陷入局部最小值，尤其是在复杂环境中。
- 应用场景
：适用于简单环境下的实时路径规划，如在空旷环境中移动机器人的避障与导航。
- 原理
- RPP（随机路径点）算法
- 原理
：随机生成一系列路径点，然后通过一些优化方法（如样条曲线拟合）将这些点连接成一条平滑路径。路径点的生成通常在可行区域内随机进行。
- 特点
：实现简单，能快速生成路径，但路径的质量和可行性依赖于随机生成的路径点，可能需要多次尝试才能得到较好的结果。
- 应用场景
：可作为一种快速生成初始路径的方法，为后续的路径优化算法提供基础，或者在对路径要求不高的简单场景中直接使用。
- 原理
- DWA（动态窗口法）
- 原理
：基于机器人当前的速度和加速度限制，在速度空间中生成一个动态窗口，对窗口内的每个速度组合进行模拟，计算相应的轨迹，并根据预设的目标函数（如与目标点的距离、与障碍物的距离等）选择最优的速度组合，从而实现实时路径规划。
- 特点
：考虑了机器人的动力学约束，适用于动态环境下的实时路径规划，能快速响应环境变化。但计算量较大，且目标函数的设计对结果影响较大。
- 应用场景
：常用于移动机器人在动态环境中的实时避障与路径规划，如在人群密集的室内环境中机器人的导航。
- 原理
- DDPG（深度确定性策略梯度）算法
- 原理
：结合了深度学习和强化学习，使用深度神经网络来逼近策略函数和价值函数。通过与环境交互，学习最优的行为策略，以最大化累积奖励。在路径规划中，机器人通过不断尝试不同的动作，学习如何在环境中找到最优路径。
- 特点
：适用于复杂的高维环境和连续动作空间的路径规划问题，能够处理非线性和不确定性。但训练过程需要大量的样本数据和计算资源，收敛速度较慢。
- 应用场景
：适用于复杂未知环境下的机器人路径规划，如机器人在未知的复杂室内环境中自主探索并规划路径。
- 原理
- Bezier 曲线算法
- 原理
：通过一组控制点定义一条光滑曲线，通过调整控制点的位置可以改变曲线的形状。在路径规划中，可将起始点、目标点和一些中间点作为控制点，生成一条平滑的路径。
- 特点
：生成的路径平滑，可通过控制点灵活调整路径形状。但控制点的选择需要一定的经验，且在处理复杂环境时可能需要较多的控制点。
- 应用场景
：常用于对路径平滑度要求较高的场景，如机器人在展示或表演场景中的路径规划，或者在一些需要生成美观路径的应用中。
- 原理
- B - spline 曲线算法
- 原理
：是 Bezier 曲线的扩展，通过基函数的加权和来定义曲线。相比 Bezier 曲线，B - spline 曲线可以通过调整控制点和节点向量，更灵活地控制曲线形状，且具有局部可控性。
- 特点
：能生成更复杂、更灵活的平滑曲线，对控制点的变化响应更局部化，不会像 Bezier 曲线那样影响整个曲线形状。在处理大量控制点时表现更好。
- 应用场景
：广泛应用于需要精确控制路径形状的领域，如计算机辅助设计（CAD）、机器人运动轨迹规划等，尤其适用于复杂形状的路径生成。
- 原理
- Dubins 曲线算法
- 原理
：针对具有转向约束的车辆（如汽车），由直线段和圆弧段组成，通过求解一系列几何问题来确定从起始点到目标点的最短路径，满足车辆的转向半径约束。
- 特点
：生成的路径符合具有转向约束的车辆运动特性，能找到满足转向约束的最短路径。但只适用于二维平面且仅考虑转向约束，不考虑其他动力学约束。
- 应用场景
：常用于自动驾驶车辆在平面道路上的路径规划，以及具有转向约束的移动机器人的路径规划。
- 原理
- Reeds - Shepp 曲线算法
- 原理
：是 Dubins 曲线的扩展，不仅考虑了车辆的转向约束，还允许车辆在行驶过程中倒车。通过枚举不同的直线、圆弧和倒车组合，找到从起始点到目标点的最优路径。
- 特点
：相比 Dubins 曲线，更符合实际车辆的运动情况，能处理车辆需要倒车才能到达目标点的场景。但计算复杂度较高，因为需要考虑更多的路径组合。
- 应用场景
：适用于实际交通场景中车辆的路径规划，如停车场内车辆的泊车路径规划，以及在一些狭窄空间中需要倒车操作的车辆路径规划。
- 原理
六、总结
不同的路径规划算法各有优缺点，适用于不同的场景和应用需求。在实际应用中，需要根据具体的环境特点（如静态或动态、结构化或非结构化）、对路径的要求（如最优性、平滑度）以及硬件资源等因素，选择合适的算法或结合多种算法来实现高效、可靠的路径规划。随着机器人技术、人工智能和计算机硬件的不断发展，路径规划算法也在不断演进和创新，以满足日益复杂的运动规划与导航任务的需求。
⛳️ 运行结果
📣 部分代码
function [pose, traj, flag] = dwa_plan(start, goal, varargin)
%%
% @file: dwa_plan.m
% @breif: DWA motion planning
% @paper: The Dynamic Window Approach to Collision Avoidance
%%
p = inputParser;
addParameter(p, 'path', "none");
addParameter(p, 'map', "none");
parse(p, varargin{:});
if isstring(p.Results.map)
exception = MException('MyErr:InvalidInput', 'parameter `map` must be set.');
throw(exception);
end
map = p.Results.map;
% kinematic
kinematic.V_MAX = 1.0; % maximum velocity [m/s]
kinematic.W_MAX = 20.0 * pi /180; % maximum rotation speed[rad/s]
kinematic.V_ACC = 0.2; % acceleration [m/s^2]
kinematic.W_ACC = 50.0 * pi /180; % angular acceleration [rad/s^2]
kinematic.V_RESOLUTION = 0.01; % velocity resolution [m/s]
kinematic.W_RESOLUTION = 1.0 * pi /180; % rotation speed resolution [rad/s]]
% return value
flag = false;
pose = [];
traj = [];
% initial robotic state
robot.x = start(1);
robot.y = start(2);
robot.theta = start(3);
robot.v = 0;
robot.w = 0;
% evalution parameters [heading, distance, velocity, predict_time, dt, R]
eval_param = [0.045, 0.1 ,0.1, 3.0, 0.1, 2.0];
% obstacle
[m, ~] = size(map);
obs_index = find(map==2);
obstacle = [mod(obs_index - 1, m) + 1, fix((obs_index - 1) / m) + 1];
% threshold
max_iter = 2000;
max_dist = 1.0;
% main loop
for i=1:max_iter
% dynamic configure
vr = cal_dynamic_win(robot, kinematic, eval_param(5));
[eval_win, traj_win] = evaluation(robot, vr, goal, obstacle, kinematic, eval_param);
% failed
if isempty(eval_win)
return
end
% update
value = eval_win(:, 3);
[~, index] = max(value);
u = eval_win(index, 1:2);
robot = f(robot, u, eval_param(5));
pose = [pose; robot.x, robot.y, robot.theta];
traj_i.info = traj_win;
traj = [traj; traj_i];
% goal found
if dist([robot.x, robot.y], goal(1:2)') < max_dist
flag = true;
disp("goal arrived!");
break;
end
end
end
%%
function vr = cal_dynamic_win(robot, kinematic, dt)
%@breif: calculate dynamic window
% hard margin
vs=[0 , kinematic.V_MAX, ...
-kinematic.W_MAX, kinematic.W_MAX];
% predict margin
vd = [robot.v - kinematic.V_ACC * dt , robot.v + kinematic.V_ACC * dt, ...
robot.w - kinematic.W_ACC * dt, robot.w + kinematic.W_ACC * dt];
% dynamic window
v_tmp = [vs; vd];
vr = [max(v_tmp(:, 1)) min(v_tmp(:, 2)) max(v_tmp(:, 3)) min(v_tmp(:, 4))];
end
function [eval_win, traj_win] = evaluation(robot, vr, goal, obstacle, kinematic, eval_param)
eval_win = []; traj_win = [];
for v = vr(1):kinematic.V_RESOLUTION:vr(2)
for w=vr(3):kinematic.W_RESOLUTION:vr(4)
% trajectory prediction, consistent of poses
[robot_star, traj] = generate_traj(robot, v, w, eval_param(4), eval_param(5));
% heading evaluation
theta = angle([robot_star.x, robot_star.y], goal(1:2));
heading = pi - abs(robot_star.theta - theta);
% obstacle evaluation
dist_vector = dist(obstacle, [robot_star.x; robot_star.y]);
distance = min(dist_vector);
if distance > eval_param(6)
distance = eval_param(6);
end
% velocity evaluation
velocity = abs(v);
% braking evaluation
dist_stop = v * v / (2 * kinematic.V_ACC);
% collision check
if distance > dist_stop && distance >= 1
eval_win = [eval_win; [v w heading distance velocity]];
traj_win = [traj_win; traj];
end
end
end
% normalization
if sum(eval_win(:, 3)) ~= 0
eval_win(:, 3) = eval_win(:, 3) / sum(eval_win(:, 3));
end
if sum(eval_win(:, 4)) ~= 0
eval_win(:, 4) = eval_win(:, 4) / sum(eval_win(:, 4));
end
if sum(eval_win(:, 5)) ~= 0
eval_win(:, 5) = eval_win(:, 5) / sum(eval_win(:, 5));
end
eval_win = [eval_win(:, 1:2), eval_win(:, 3:5) * eval_param(1:3)'];
end
function [robot, traj] = generate_traj(robot, v, w, t, dt)
%@breif: generate trajectory
time = 0;
u = [v, w];
traj = robot;
while time <= t
time = time + dt;
robot = f(robot, u, dt);
traj = [traj robot];
end
end
function robot = f(robot, u, dt)
%@breif: robotic kinematic
F = [ 1 0 0 0 0
0 1 0 0 0
0 0 1 0 0
0 0 0 0 0
0 0 0 0 0];
B = [dt * cos(robot.theta) 0
dt * sin(robot.theta) 0
0 dt
1 0
0 1];
x = [robot.x; robot.y; robot.theta; robot.v; robot.w];
x_star = F * x + B * u';
robot.x = x_star(1); robot.y = x_star(2); robot.theta = x_star(3);
robot.v = x_star(4); robot.w = x_star(5);
end
function theta = angle(node1, node2)
theta = atan2(node2(2) - node1(2), node2(1) - node1(1));
end
