1. 从“走哪条路”到“怎么走好”路径规划的工程化视角最近在整理一个机器人项目又翻出了以前关于路径规划的笔记。我发现一个挺有意思的现象很多刚接触这个领域的朋友包括几年前的我自己一提到“路径规划”脑子里蹦出来的第一个画面可能就是地图上两点之间画一条线或者是一堆算法名字——A*、Dijkstra、RRT。这当然没错但如果你真的动手去实现一个能用的系统比如让一台小车从A点自动开到B点或者让机械臂完成一个抓取动作你就会发现那条“最优”的路径往往只是万里长征的第一步。真正的挑战藏在算法选型之后的海量细节里动态环境怎么处理执行机构的物理极限如何满足计算资源够不够实时跑这些才是决定一个路径规划模块是“玩具”还是“工业级”的关键分水岭。所以这篇笔记我不想再重复教科书上那些算法的推导过程网上优秀的资料太多了而是想结合我最近在动态避障小车和无人机仿真项目中踩过的坑聊聊当我们从“学习算法”转向“实现系统”时那些必须面对的工程化问题。我们会围绕几个核心矛盾展开全局最优与局部反应、理论最优与物理可行、离线计算与在线实时。你会发现路径规划远不止是“找条路”它更像是在多重约束下寻找动态平衡的艺术。2. 全局与局部分层规划的必然性与协同挑战几乎所有实用的移动机器人系统都会采用“全局路径规划”加“局部路径规划”的分层架构。这几乎是工程上的必然选择原因在于计算复杂度和环境信息的完备性。全局路径规划比如基于A*、Dijkstra或者更高效的Jump Point Search它的任务是在已知的、静态的或相对静态的环境地图中找到一条从起点到终点的无碰撞、代价较低的路径。这里的地图通常是事先构建好的栅格地图、拓扑地图或特征地图。全局规划器像个战略家它俯瞰全局但它的视野是“凝固”的它不知道下一秒会不会突然冒出个人或一辆车。局部路径规划例如动态窗口法DWA、时间弹性带TEB或者是基于优化的局部规划器常涉及二次规划QP它的任务是处理全局路径规划“看不见”的东西未知的、突然出现的动态障碍物。它只关心机器人周围一小片区域比如未来几秒内能到达的范围根据实时传感器激光雷达、摄像头的数据对全局路径进行微调甚至紧急重规划以实现避障。它是个战术家反应迅速但视野狭窄。2.1 分层架构下的典型工作流一个经典的工作流是这样的初始化加载预先构建好的静态地图如SLAM建图所得。全局规划给定机器人的起始位姿和目标位姿全局规划器在地图上计算出一条粗略的路径。这条路径通常由一系列路径点Waypoints组成可能只考虑了静态障碍物和大致的地形代价。路径跟踪与局部重规划机器人开始沿着全局路径移动。局部规划器开始工作感知实时获取激光雷达等传感器的点云数据并将其与静态地图融合识别出动态障碍物。优化局部规划器以全局路径为参考在满足机器人运动学如最小转弯半径、动力学如加速度限制约束的前提下生成一条短期内如下一秒既避开动态障碍物又尽量贴近全局路径的局部轨迹。控制将局部轨迹转换为底层的速度线速度、角速度或位置指令发送给电机控制器。全局重规划触发当局部规划器发现无法在满足约束的情况下绕过障碍物比如路被完全堵死或者机器人偏离全局路径太远时会触发一次全新的全局路径规划寻找新的可行路线。2.2 全局与局部规划的“接口”与冲突这里就引出了第一个工程难题全局路径和局部路径如何“对话”如果接口设计不好机器人就会表现得“精神分裂”。问题一全局路径的“不可达点”。全局规划器在地图上画了一条完美的路径但可能穿过了一个实际中因为地面不平、有门槛而无法通过的区域或者路径点之间的距离、角度对机器人的运动控制来说过于苛刻。局部规划器试图跟踪但发现根本做不到就会在原地“挣扎”或报错。经验之谈在生成全局路径时不仅要考虑“是否碰撞”还要考虑“是否可执行”。这就需要在地图的代价函数中融入机器人的运动学模型。例如对于阿克曼转向的车辆路径的曲率必须连续且小于车辆的最小转弯半径。我常用的一个技巧是在全局规划后加入一个“路径平滑和后处理”步骤使用曲线拟合如贝塞尔曲线、样条曲线或专门的平滑算法使路径点更稀疏、曲率更连续为局部跟踪减轻负担。问题二局部优化的“短视”与“振荡”。局部规划器如DWA在每一个控制周期都采样一堆可能的轨迹并评分。评分函数通常包括距离目标的方向、距离障碍物的远近、速度大小、与全局路径的贴合度等。如果“贴合全局路径”的权重太低机器人可能为了避开一个临时障碍物而严重偏离主路甚至走到死胡同。如果“距离障碍物”的权重太高机器人在穿过狭窄通道时可能会在两个同样“危险”的选项间来回摇摆产生振荡。踩坑实录在做一个室内服务机器人时机器人在办公室门口门较窄经常卡住“跳舞”。排查后发现是DWA的代价函数中障碍物距离的代价项在狭窄空间产生了过于尖锐的“斥力场”。调整方案是引入一个“平滑的斥力场”函数当距离大于安全阈值时代价增长平缓只有当距离非常近时代价才急剧上升。同时适当提高了“路径方向”项的权重让机器人更有“决心”通过通道。问题三重规划的频率与抖动。频繁的全局重规划比如每秒一次会消耗大量CPU资源而且可能导致机器人的目标方向频繁突变运动不流畅。但不重规划又可能让机器人在死局里浪费时间。实操技巧设置合理的重规划触发条件。我通常采用组合条件1) 局部规划器连续N个周期如20个周期对应1-2秒报告“无可行轨迹”2) 机器人与全局路径的横向偏差超过阈值如0.5米3) 检测到地图有重大更新如原本通畅的路被标记为永久障碍。并且在触发重规划后会有一个短暂的“冷却期”期间即使条件再次满足也不立即重规划避免抖动。3. 代价函数路径规划算法的“价值观”如果说搜索算法如A*决定了路径规划的“能力边界”那么代价函数就决定了它的“行为偏好”。你可以把它理解为算法的“价值观”它认为什么是“好”的路径。一个典型的路径代价函数是多项代价的加权和总代价 w1 * 路径长度 w2 * 转向代价 w3 * 靠近障碍物代价 w4 * 平滑度代价 ...3.1 代价函数设计中的核心条件与权衡从热词“路径规划的代价函数的条件”可以看出大家很关心怎么设这些权重w1, w2, ...。这里没有银弹完全取决于你的应用场景。清洁机器人如阿克曼型机器人的全覆盖路径规划核心目标覆盖率高、重复率低、效率高。代价侧重“路径长度”权重可能不是最高的因为走最短路径可能导致漏扫。更重要的是“覆盖新区域的奖励”和“重复经过同一区域的惩罚”。它的代价函数更复杂需要维护一个已覆盖区域的地图状态。同时“平滑度”和“转向代价”权重会比较高因为频繁的原地转弯既耗电又降低覆盖效率。常见算法不是简单的点对点规划而是基于栅格的Boustrophedon回字形覆盖算法或者更智能的基于神经网络的PPO近端策略优化算法后者可以通过学习来优化覆盖策略。自动驾驶/动态避障小车核心目标安全、舒适、符合交规、抵达目标。代价侧重“靠近障碍物代价”是重中之重且是动态计算的对于不同运动状态的障碍物静止、同向、相向会有不同的危险系数。“舒适度”加速度/加加速度Jerk的权重也很高急刹和猛打方向体验很差。此外还需要加入“车道中心线偏移代价”、“违反交通规则代价”如压线、逆行。与局部规划QP的关系很多先进的局部规划器如Apollo的EM Planner将路径生成建模为一个二次规划QP问题。代价函数在这里直接体现为QP的目标函数。例如希望路径平滑二阶导/曲率小同时贴近参考线全局路径并且远离障碍物。QP的约束条件则包含了车辆运动学阿克曼转向角限制、动力学速度/加速度限制以及障碍物构成的凸空间Corridor。这里的“条件”就是指QP问题中的等式约束和不等式约束它们共同保证了生成路径的物理可行性。无人机自主路径规划仿真核心目标考虑动力学、能耗、时间、空域限制。代价侧重除了长度还有“爬升/下降代价”耗能不同、“速度变化代价”、“禁飞区/威胁区代价”。在仿真中我们尤其关注如何用算法如RRT*, Informed RRT*在复杂三维空间中快速找到可行路径并验证其满足无人机的动力学模型不是质点模型。机械臂/磨齿机路径规划核心目标无碰撞、关节运动平滑、避免奇异点、时间或能量最优。代价侧重这里的“路径”是在机械臂的关节空间或笛卡尔空间中的轨迹。代价函数可能包含“关节位移总和”、“关节速度/加速度的平方和”能量最优、“奇异度度量”远离奇异构型、“工具末端姿态误差”等。对于磨齿机这类精密加工还需要考虑“切削力平稳”、“避免震颤”等工艺代价。3.2 一个动态避障小车的代价函数设计实例假设我们用一个简化的模型局部规划器采样一系列圆弧轨迹并为每条轨迹评分。# 伪代码示例一条轨迹的代价计算 def calculate_trajectory_cost(traj, global_plan, obstacles, robot_pose): cost 0.0 # 1. 目标导向代价轨迹终点与局部目标点的距离 # 局部目标点取自全局路径上lookahead距离的点 local_goal get_local_goal(global_plan, robot_pose) cost w_goal * distance(traj.end_point, local_goal) # 2. 障碍物代价遍历轨迹上的点计算到最近障碍物的距离 min_obstacle_dist INFINITY for point in traj.points: dist_to_obs min_distance_to_obstacles(point, obstacles) if dist_to_obs min_obstacle_dist: min_obstacle_dist dist_to_obs # 使用非线性函数距离越近代价指数上升 if min_obstacle_dist danger_threshold: cost w_obs * exp(-min_obstacle_dist) # 危险区域代价极高 else: cost w_obs / (min_obstacle_dist ** 2) # 安全区域代价随距离衰减 # 3. 速度代价鼓励以合适的速度前进 cost w_vel * (preferred_speed - traj.velocity) ** 2 # 4. 平滑代价与上一条采纳轨迹的差异减少抖动 cost w_smooth * difference(traj, previous_best_traj) return cost调试心得调整这些权重 (w_goal,w_obs,w_vel,w_smooth) 是个细活。我的经验是先在仿真环境中如ROS Gazebo用一组典型的场景直道、弯道、静态障碍、动态障碍进行测试。先确保安全把w_obs调得足够高让机器人绝不撞上障碍物。再优化体验在安全的前提下调整w_goal和w_smooth让机器人行走路径更直接、更顺滑减少不必要的迂回和摇摆。w_vel则用来匹配机器人的动力性能和环境要求室内慢速走廊可稍快。4. 算法选型没有最好只有最合适面对琳琅满目的路径规划算法新手很容易眼花缭乱。其实算法选型很大程度上取决于你的状态空间维度、环境类型和实时性要求。算法类型典型算法适用场景优点缺点/挑战工程实现注意图搜索算法Dijkstra, A*, D* Lite二维栅格地图全局静态路径规划完备能找到最优解原理简单高维空间如3维计算爆炸对动态环境不友好A的性能极度依赖启发函数h(n)的设计。DLite适用于增量式环境变化如部分地图更新。采样规划算法RRT, RRT*, PRM高维空间如机械臂、无人机复杂几何约束能快速在高维空间找到可行解不显式建模空间概率完备非最优RRTRRT*渐近最优但慢路径可能不光滑RRT生成路径需要后处理平滑。在狭窄通道中RRT生长效率低可能需要桥测试等技巧。数值优化算法二次规划(QP)序列二次规划(SQP)局部轨迹优化满足动力学约束能直接处理复杂约束运动学、动力学、障碍物生成平滑轨迹对初值敏感可能陷入局部最优实时求解需要高效QP求解器常与采样或搜索算法结合用后者提供初始解。需要将障碍物约束转化为QP可处理的形式如凸多边形走廊。智能优化算法遗传算法(GA)粒子群(PSO)PPO策略优化离线全局优化复杂代价函数如覆盖规划能处理非凸、非线性、多目标的复杂优化问题计算量大通常不能保证实时性结果具有随机性PPO等强化学习算法需要大量仿真训练但学会后前向推理很快。适合规则固定但最优解难以显式建模的场景。曲线插值/几何法贝塞尔曲线样条曲线回字形结构化环境如泊车、覆盖路径平滑计算快路径天生光滑参数物理意义明确在复杂杂乱障碍物环境中难以直接应用常用于全局路径的后处理或将高层指令如泊车入库转化为具体路径。4.1 结合场景的算法选择思路机器人导航室内/室外A全局 DWA/TEB/QP局部* 是经久不衰的经典组合。A*在2D栅格地图上快速找到粗略路径局部规划器负责实时避障和平滑。对于更复杂的非完整约束机器人如阿克曼车辆TEB或直接使用QP的规划器更能精确满足运动学模型。机械臂抓取/磨齿机加工空间维度高6-7个关节环境相对结构化。采样算法RRT* 常用于在关节空间或任务空间寻找一个无碰撞的初始路径然后通过轨迹优化如SQP对路径进行平滑和优化以满足速度、加速度约束和工艺要求。无人机集群仿真/复杂空域规划状态空间是3D的可能还要考虑时间维度4D。快速探索随机树RRT的变种如Informed RRT* 因其在高维空间的效率而被广泛使用。如果环境高度动态可能需要结合速度障碍法VO或分布式模型预测控制DMPC进行实时避碰。泊车路径规划环境高度结构化车位、车道线约束明确车辆非完整、最小转弯半径。通常采用几何分解法或最优控制方法。将泊车过程分解为几个典型的几何阶段如向前调整、倒车入库每个阶段用回旋曲线Clothoid或圆弧直线组合来生成路径保证曲率连续。Reeds-Shepp曲线也是泊车规划中常用的基础几何路径集。全覆盖路径规划清洁机器人这更像是一个“覆盖”问题而非“点对点”问题。在规则矩形区域简单的回字形Boustrophedon算法就非常有效。在不规则区域需要先将区域分解为多个子单元单元分解法再在每个子单元内进行回形覆盖。近年来也越来越多地看到使用强化学习如PPO来学习覆盖策略的研究让机器人在复杂家庭环境中自主决定清扫顺序和路径。注意MATLAB是一个强大的算法仿真和验证平台你可以用它快速实现和对比A*、RRT、PPO等各种算法验证其正确性和性能。但在实际机器人上部署时通常需要用C、Python等语言在ROS或其它机器人中间件中重新实现并充分考虑计算效率和与传感器、控制器的集成。5. 从仿真到实车那些必须填平的“鸿沟”在电脑上看到一条完美的规划路径和让真实的机器人稳稳地走出来中间隔了无数个坑。仿真如Gazebo, MATLAB/Simulink是必不可少的环节但它只能验证算法的逻辑正确性。5.1 仿真中无法完全模拟的实车因素传感器噪声与延迟仿真中的激光雷达数据是完美的没有噪点延时几乎为零。实车上激光雷达有测量噪声点云可能稀疏不均摄像头图像有畸变视觉处理算法有延迟。这些会导致局部规划器感知到的环境与真实环境有差异可能把噪点当成障碍物虚警或者没及时检测到快速靠近的物体漏警。应对策略在规划器的感知输入端必须进行有效的滤波如统计滤波器去除孤立噪点和时序对齐时间同步。对于动态障碍物要使用跟踪算法如卡尔曼滤波来预测其未来状态而不是只依赖当前时刻的观测。控制误差与执行延迟仿真中你下发了v, w速度指令机器人下一秒就会精确到达预定位置。实车上电机响应有延迟轮子可能存在打滑底层PID控制器会有稳态误差。这会导致机器人实际走出的轨迹与规划轨迹存在偏差。应对策略局部规划器需要有反馈。这就是为什么单纯的“前馈”路径规划不行必须结合“路径跟踪”控制器如Pure Pursuit, Stanley, MPC。跟踪控制器根据机器人的实际位姿来自里程计/融合定位与期望路径的偏差实时计算修正后的控制指令形成一个“规划-跟踪-反馈”的闭环。地图的不确定性与动态性仿真地图是精确且静态的。实车环境中地图可能有误差SLAM建图不准环境会变化椅子被移动了门被关上了。应对策略需要一套健壮的地图更新机制。不能完全依赖先验静态地图。通常采用“占据栅格地图”并设置每个栅格的更新规则。对于激光扫描到的空闲区域降低占据概率对于扫描到的障碍提高占据概率。同时要区分“静态障碍”和“临时障碍”如人对于临时障碍可以设置其占据概率随时间衰减避免它永久地阻塞路径。计算资源限制在仿真里你可以用最复杂的算法跑最密集的采样。在实车的嵌入式计算平台如Jetson Nano, Raspberry Pi上CPU和内存是宝贵的。应对策略算法效率优化是关键。例如对A*算法使用更高效的优先队列设计更精准的启发函数对DWA限制采样空间速度、角速度的分辨率和范围对QP求解使用针对嵌入式平台优化的求解库如OSQP。必要时需要做算法简化比如用预计算的查找表来替代实时复杂计算。5.2 实车调试的“笨”办法与核心检查点当算法从仿真移植到实车出现问题时一个系统化的排查流程至关重要隔离测试不要一上来就让整个导航栈跑。先单独测试每个模块。传感器发布一个固定的速度指令看机器人是否直线行走同时记录激光雷达和里程计数据。检查里程计积分是否准确走10米实际走了多少检查激光数据在静止时是否稳定。地图与定位用手柄遥控机器人走一圈看看建出的地图质量如何。然后让机器人静止看其在地图中的定位是否漂移AMCL粒子滤波的稳定性。规划器在已知好的地图和定位下给定一个简单目标点开启规划器但先不执行控制指令在RViz等可视化工具中观察它规划的全局路径和局部轨迹是否合理。把障碍物代价、速度代价等可视化出来看代价函数的分布是否符合预期。逐步集成确认每个模块单独工作后再逐步连接起来。先开定位和地图让规划器出路径但控制指令不发到底层只观察。然后发送控制指令但用手扶着机器人或者在一个空旷无障的环境进行观察机器人的实际运动与规划轨迹的偏差。最后加入真实的障碍物进行避障测试。数据记录与回放充分利用ROS的rosbag工具。在测试时录制所有相关话题/scan,/odom,/map,/global_plan,/local_plan,/cmd_vel。当出现异常行为时回放bag文件像看录像一样一步步分析问题出在哪个环节。是感知错了定位飘了还是规划器算出了一条奇怪的路径路径规划从理论到落地是一个不断与不确定性斗争、在多重约束中寻找可行解的过程。它没有一劳永逸的“最佳算法”只有针对特定场景、特定机器人平台不断调优的“合适方案”。这份笔记里提到的分层架构、代价函数设计、算法选型依据以及仿真到实车的鸿沟都是我过去项目里反复验证过的经验。真正的精通来自于在一次次调试、失败和迭代中对机器人、环境和算法之间相互作用的深刻理解。下次当你再看到“路径规划”这四个字时希望你能想到的不再只是一个孤立的算法而是一套环环相扣、需要精心设计和调试的复杂系统。