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);endmap p.Results.map;% kinematickinematic.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 valueflag false;pose [];traj [];% initial robotic staterobot.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(map2);obstacle [mod(obs_index - 1, m) 1, fix((obs_index - 1) / m) 1];% thresholdmax_iter 2000;max_dist 1.0;% main loopfor i1:max_iter% dynamic configurevr cal_dynamic_win(robot, kinematic, eval_param(5));[eval_win, traj_win] evaluation(robot, vr, goal, obstacle, kinematic, eval_param);% failedif isempty(eval_win)returnend% updatevalue 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 foundif dist([robot.x, robot.y], goal(1:2)) max_distflag true;disp(goal arrived!);break;endendend%%function vr cal_dynamic_win(robot, kinematic, dt)%breif: calculate dynamic window% hard marginvs[0 , kinematic.V_MAX, ...-kinematic.W_MAX, kinematic.W_MAX];% predict marginvd [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 windowv_tmp [vs; vd];vr [max(v_tmp(:, 1)) min(v_tmp(:, 2)) max(v_tmp(:, 3)) min(v_tmp(:, 4))];endfunction [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 wvr(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 evaluationtheta angle([robot_star.x, robot_star.y], goal(1:2));heading pi - abs(robot_star.theta - theta);% obstacle evaluationdist_vector dist(obstacle, [robot_star.x; robot_star.y]);distance min(dist_vector);if distance eval_param(6)distance eval_param(6);end% velocity evaluationvelocity abs(v);% braking evaluationdist_stop v * v / (2 * kinematic.V_ACC);% collision checkif distance dist_stop distance 1eval_win [eval_win; [v w heading distance velocity]];traj_win [traj_win; traj];endendend% normalizationif sum(eval_win(:, 3)) ~ 0eval_win(:, 3) eval_win(:, 3) / sum(eval_win(:, 3));endif sum(eval_win(:, 4)) ~ 0eval_win(:, 4) eval_win(:, 4) / sum(eval_win(:, 4));endif sum(eval_win(:, 5)) ~ 0eval_win(:, 5) eval_win(:, 5) / sum(eval_win(:, 5));endeval_win [eval_win(:, 1:2), eval_win(:, 3:5) * eval_param(1:3)];endfunction [robot, traj] generate_traj(robot, v, w, t, dt)%breif: generate trajectorytime 0;u [v, w];traj robot;while time ttime time dt;robot f(robot, u, dt);traj [traj robot];endendfunction robot f(robot, u, dt)%breif: robotic kinematicF [ 1 0 0 0 00 1 0 0 00 0 1 0 00 0 0 0 00 0 0 0 0];B [dt * cos(robot.theta) 0dt * sin(robot.theta) 00 dt1 00 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);endfunction theta angle(node1, node2)theta atan2(node2(2) - node1(2), node2(1) - node1(1));end 参考文献更多免费数学建模和仿真教程关注领取