机器人路径规划与轨迹优化：从A*到RRT*的算法实践与工程落地

1. 项目缘起从“能走”到“走得好”的必经之路最近在折腾一个移动机器人项目从零开始搭建底盘、上传感器、写驱动好不容易让轮子转起来了却发现一个更头疼的问题这机器人怎么走起路来像个醉汉要么是规划出的路径七拐八绕要么是执行轨迹时一顿一顿要么干脆在障碍物面前“思考人生”卡住了。这让我意识到让机器人动起来只是第一步让它“优雅、高效、安全”地动起来才是真正的挑战。这就是路径规划与轨迹优化要解决的核心问题。简单来说路径规划解决的是“从A到B走哪条路”的问题它是在已知或部分已知的环境地图中为机器人找出一条从起点到终点、且避开所有障碍物的可行通道。你可以把它想象成手机地图导航给你规划路线。而轨迹优化解决的则是“在这条路上具体怎么走”的问题它关注的是机器人沿着这条路径运动时的速度、加速度、姿态等随时间变化的细节确保运动平滑、节能、符合动力学约束。这好比导航不仅告诉你走哪条高速还帮你规划了每个时间点应该以什么速度行驶、在哪里变道、在哪里减速进服务区。这两个环节紧密耦合缺一不可。一条理论上最短的路径如果转弯过于急峭机器人可能因为惯性太大而翻车或打滑一个理论上最优的速度曲线如果对应的路径紧贴着障碍物任何微小的控制误差都可能导致碰撞。因此在实际的机器人系统尤其是自动驾驶、无人机、机械臂等高动态系统中路径规划和轨迹优化往往是迭代或联合进行的。我这次的项目经历就是一个典型的从“有路就走”到“精雕细琢”的实践过程。2. 路径规划核心算法从“格子世界”到“连续空间”的进化路径规划算法种类繁多选择哪种往往取决于环境信息的完整性全局已知还是局部感知、机器人的形状和运动能力是点状、圆形还是复杂多边形是只能前进后退还是可以全向移动、以及对计算实时性的要求。下面我结合自己的踩坑经验聊聊几种主流的规划方法。2.1 基于搜索的规划稳扎稳打的“老黄牛”这类算法把环境离散化成网格比如栅格地图把规划问题转化为在图上的搜索问题。最经典的就是AA-Star算法*。它的核心思想是维护一个待探索节点列表每次从中选取一个“代价”最小的节点进行扩展。这个代价f(n) g(n) h(n)其中g(n)是从起点到当前节点n的实际代价h(n)是从当前节点n到终点的预估代价启发函数。注意启发函数h(n)的选择至关重要。常用的有曼哈顿距离适用于四方向移动、欧几里得距离适用于八方向或任意角度移动。h(n)必须满足“可采纳性”即永远不大于实际代价和“一致性”才能保证A*找到最优路径。我最初用了直线距离在复杂障碍物环境下效果不错但后来发现对于非完整约束的机器人比如汽车不能横向移动这个启发函数可能会引导搜索进入死胡同后来改用了考虑运动学约束的、更复杂的启发函数才解决。A的优点是完备且最优在启发函数可采纳的前提下路径质量高。缺点是计算量随地图大小和分辨率呈指数级增长。为了平衡衍生出了DDynamic A** 和DLite* 算法它们能在环境发生变化时比如发现新障碍物高效地重新规划非常适合未知或动态环境下的机器人导航我在做SLAM同步定位与建图结合实时避障时就用的DLite。另一个值得一提的是Jump Point Search跳点搜索。它是对A在均匀栅格上的优化通过识别“跳点”来跳过大量不必要的中间节点在开阔环境中能将搜索速度提升一个数量级。但它的实现比A复杂并且在非均匀或非栅格化地图中优势不明显。2.2 基于采样的规划随机漫步的“探险家”当环境维度很高比如机械臂有6个以上关节或者连续空间无法有效离散化时基于搜索的方法就力不从心了。这时基于采样的规划器大放异彩。其核心思想是在机器人的配置空间C-space中随机撒点并将这些点连接起来形成一棵搜索树或一个图最终在树或图中找到路径。RRT快速探索随机树是其中的代表。它从起点开始随机在空间中选择一个目标点然后从树中找到离这个目标点最近的节点朝着目标点的方向“生长”一小段距离形成一个新的节点并加入树中。如此反复直到树扩展到终点附近。RRT的优点是非常快特别擅长探索高维空间而且概率完备只要时间足够总能找到解。但它的缺点也很明显路径通常不是最优的甚至可能很绕而且由于随机性每次规划的结果都不一样。为了解决最优性问题RRTRRT-star* 被提出。它在RRT的基础上增加了“重布线”和“父节点重选”的步骤随着采样点的增加路径会逐渐收敛到最优。RRT的计算量比RRT大但路径质量显著提高。我在为一个六自由度机械臂做运动规划时就采用了RRT虽然单次规划慢了点但得到的关节空间轨迹平滑很多避免了剧烈抖动。PRM概率路线图是另一类基于采样的方法。它分为两个阶段学习阶段在空间中随机采样大量“里程碑”点并将彼此可达的点连接起来形成一张路线图查询阶段将起点和终点连接到这张图上然后用图搜索算法如Dijkstra找到路径。PRM适合在多查询场景同一张地图多个起终点对因为路线图可以预先构建好。2.3 基于智能优化的规划物竞天择的“演化者”这类方法将路径规划转化为一个优化问题定义评价路径好坏的目标函数如路径长度、平滑度、离障碍物距离等然后利用优化算法寻找使目标函数最优的路径。遗传算法GA和粒子群算法PSO是典型代表。以遗传算法为例它模拟生物进化将一条路径编码为一个“染色体”比如一系列路径点的坐标初始化一个“种群”。然后进行“选择”保留优秀路径、“交叉”交换两条路径的片段、“变异”随机改变路径上的某个点等操作迭代演化最终“进化”出较优的路径。这类方法的优点是灵活可以轻松地将各种复杂约束如动力学约束、能耗约束融入目标函数中。缺点也很突出计算量大收敛速度慢且不能保证找到最优解可能陷入局部最优。我在尝试用遗传算法做无人机集群的协同路径规划时就深受其害目标函数设计稍有不慎迭代几百代出来的路径还是奇奇怪怪。它更适合作为离线规划工具或者与其他规划器结合对初步路径进行后优化。2.4 基于曲线插值的规划优雅的“几何学家”对于已知结构化环境如车道线、仓库货架通道我们有时不需要复杂的搜索直接用参数化曲线来拟合路径。贝塞尔曲线和B样条曲线是常用的工具。贝塞尔曲线由控制点定义曲线一定通过起点和终点并且整体被包裹在控制点形成的凸包内。它的阶数等于控制点数减一。三次贝塞尔曲线就足够生成一段平滑的S形弯道轨迹。B样条曲线则更复杂也更强大它是贝塞尔曲线的推广具有局部修改性移动一个控制点只影响曲线的一段而不是整个曲线并且可以轻松保证曲率连续这对于车辆、无人机等对运动平滑性要求高的载体至关重要。我在设计一个仓储AGV的巡线算法时就用了三次均匀B样条来平滑由A规划出的折线路径。具体做法是将A输出的路径点作为B样条曲线的控制点或去噪后作为拟合点然后通过B样条公式生成一条光滑的参数曲线。这样AGV就不再是僵硬地从一个路径点“跳”到下一个而是沿着一条连续可导的曲线平滑行驶大大提升了运行稳定性和乘客货物的舒适度。3. 轨迹优化为规划好的路径注入“灵魂”找到一条几何路径只是成功了一半。这条路径对机器人来说只是一串空间坐标序列。轨迹优化则要决定机器人何时到达路径上的哪个点以及以什么样的运动状态速度、加速度、加加速度到达。这直接决定了运动的平滑性、能耗和执行器的负载。3.1 时间分配从路径到轨迹的关键一步最简单的轨迹生成方法就是匀速时间分配假设机器人以恒定速度走完整条路径那么每个路径点对应的时间戳就可以根据路径长度等比例算出。但这种方法忽略了机器人的动力学极限。比如在一个急转弯处机器人必须减速否则会因离心力而侧滑或翻车。因此更合理的方法是基于曲率的时间分配。路径曲率大的地方转弯急分配更多时间即降低速度路径曲率小的地方直道分配更少时间即提高速度。这需要我们先对路径进行参数化通常用弧长s然后建立速度v与曲率κ之间的约束关系例如v ≤ sqrt( a_max / |κ| )其中a_max是机器人能承受的最大向心加速度。通过这个约束我们可以反推出每个路径点处的最大允许速度从而生成一个变速度剖面。3.2 最小抖动轨迹生成让运动如丝般顺滑对于机械臂、无人机、相机云台等对运动平滑性要求极高的系统我们不仅关心速度、加速度还关心其导数——加加速度Jerk。过大的加加速度会导致冲击、振动和磨损。因此轨迹优化的一个高级目标是生成最小抖动轨迹。一个非常流行且有效的方法是使用多项式轨迹特别是五次多项式。为什么是五次因为要同时约束起止点的位置、速度、加速度每个维度需要6个系数一个五次多项式有6个自由度刚好可以解出唯一解。假设我们规划了路径上的N个路径点我们需要为每一段路径两个相邻路径点之间生成一条时间参数化的五次多项式曲线。优化目标是最小化整个轨迹的加加速度平方的积分即最小化抖动。这可以形式化为一个二次规划QP问题。通过求解这个QP问题我们可以得到全局平滑、且满足起止点及各路径点约束如位置、速度上限的轨迹。我在为一条六轴协作机械臂编写“点到点”快速运动程序时就采用了这种方法。相比机器人控制器自带的简单S型速度规划自己实现的最小抖动轨迹规划能让机械臂在高速运动下更安静、更平稳末端执行器的抖动肉眼可见地减小了这对于精密装配或视觉扫描任务至关重要。3.3 考虑动力学约束的优化从“纸上谈兵”到“脚踏实地”上述的轨迹优化大多只考虑了运动学约束位置、速度、加速度。但对于自重较大的机器人、高速运动的无人机或赛车动力学约束必须纳入考虑。这包括扭矩/力约束机器人的关节电机或轮子电机能提供的扭矩/推力是有限的。摩擦约束车轮与地面的摩擦力决定了最大加速度和制动力。质心动力学对于双足或四足机器人运动必须保证质心投影在支撑多边形内以防摔倒。将动力学约束纳入轨迹优化问题会变得非常复杂通常需要更高级的优化框架如模型预测控制MPC。MPC在每个控制周期内基于当前状态和动力学模型求解一个有限时间窗内的优化问题得到一系列未来的控制输入如电机扭矩但只执行第一个控制输入。到下一个周期根据新的状态重新进行优化。这样MPC可以实时地处理动态障碍物和模型误差。例如在自动驾驶中规划模块给出一条参考路径MPC控制器则会考虑车辆动力学模型如自行车模型、轮胎摩擦圆约束、以及乘客舒适度限制加加速度计算出一个最优的转向角、油门和刹车序列使车辆尽可能平滑、安全地跟踪参考路径。这已经不是单纯的轨迹“优化”而是“控制”了但两者边界正在模糊。4. 实战集成与避坑指南让算法在真实机器人上跑起来理论再完美不上机都是空谈。将路径规划和轨迹优化算法集成到真实的机器人系统中会遇到一大堆在仿真中遇不到的问题。4.1 坐标系与时间同步一切混乱的根源机器人系统通常涉及多个坐标系世界坐标系地图、机器人基座坐标系、传感器坐标系激光雷达、相机、执行器坐标系轮子、关节等。规划算法通常在世界坐标系中输出一条路径。但控制器如PID、MPC需要在机器人坐标系中跟踪误差。这就需要进行频繁的坐标变换。我踩过的一个大坑是规划器以10Hz的频率发布路径而底层电机控制器以100Hz的频率运行。如果直接让控制器跟踪最新发布的路径点会导致机器人运动抖动因为路径点之间的时间间隔不均匀。正确的做法是规划器不仅发布路径点序列还要为每个点发布一个时间戳。控制器根据当前时间在路径上进行插值得到一个“当前时刻应该到达的期望状态”然后去跟踪这个状态。这就保证了控制的连续性和平滑性。ROS中的nav_msgs/Path消息和trajectory_msgs/JointTrajectory消息都包含了时间戳字段就是为了这个目的。4.2 环境表示与碰撞检测安全的第一道防线规划算法需要一个环境模型来进行碰撞检测。最常用的是二维占据栅格地图Occupancy Grid Map每个格子有一个概率值表示被占据的可能性。对于A*这类网格搜索算法这很直接。但对于RRT或曲线拟合我们需要一个连续的碰撞检测函数。一个高效的方法是使用距离场Distance Field。它为空间中的每一个点计算到最近障碍物的距离。这样在规划或优化时我们可以轻松地将“离障碍物距离”作为一个代价项加入目标函数例如cost_obs exp(-distance)距离越近代价越高。这比直接进行几何形状的碰撞检测计算多边形的交并要快得多尤其是在优化迭代中需要成千上万次查询时。我在使用C的FCL库做机械臂碰撞检测时就预先计算了工作空间的距离场将碰撞检测的耗时降低了90%以上。4.3 实时性与计算资源的平衡复杂的优化算法如MPC、非线性优化计算量巨大可能无法在机器人的嵌入式计算机上实时运行例如要求100Hz的控制频率。有几种折中方案分层规划全局规划器如A*运行在低频1-5Hz规划一条粗略的全局路径。局部规划器如DWA动态窗口法运行在高频10-20Hz结合实时传感器数据在全局路径的附近进行局部避障和轨迹优化。这是移动机器人最经典的架构。轨迹库对于重复性任务如机械臂的抓取-放置可以预先计算好一系列最优轨迹存储在“轨迹库”中。运行时只需根据当前任务选择合适的轨迹并微调即可。简化模型在MPC中使用更简化的动力学模型如线性化模型虽然损失了一些精度但换来了求解速度足以满足大多数跟踪控制的需求。4.4 参数调试的艺术没有银弹只有权衡几乎所有规划器和优化器都有一堆需要调节的参数。例如A*启发函数的权重。增大权重会让搜索更“贪婪”更快找到路径但可能不是最优减小权重则更倾向于Dijkstra算法保证最优但慢。RRT/RRT*步长生长长度。步长大探索快但可能跳过狭窄通道步长小能找到精细路径但速度慢。局部规划器如DWA速度采样分辨率、轨迹评价函数中各项距离目标、速度、与障碍物距离、与全局路径贴合度的权重。这些权重直接决定了机器人的“性格”是激进还是保守是贴紧路径还是安全第一调试这些参数没有标准答案必须结合具体的机器人平台、传感器噪声、环境特点进行大量实地测试。我的经验是先在仿真中确定一个大致范围然后到真实环境中从一个非常保守的参数集开始逐步向激进方向调整直到在安全和效率之间找到一个可接受的平衡点。一定要记录下每次参数更改和对应的测试结果形成自己的“参数调优手册”。5. 前沿趋势与个人思考随着机器人应用场景越来越复杂传统的路径规划与轨迹优化方法也在不断演进。从我关注的趋势来看有以下几个方向1. 学习驱动的规划Learning-based Planning这是目前最火热的方向。通过深度强化学习DRL等方法让机器人通过与环境的交互自己学会如何规划。其优势在于能处理非常复杂、高维的状态空间并且能学到一些难以用规则描述的“技巧”。例如波士顿动力的机器人那些行云流水的跑酷动作背后很可能有强化学习的功劳。但它的缺点是需要海量的训练数据或仿真时间并且“黑箱”特性使得安全验证困难。目前更可行的路径是“学习传统”的混合方法比如用神经网络来预测更优的启发函数h(n)给A*使用或者用学习的方法来优化轨迹评价函数的权重。2. 考虑不确定性的规划Uncertainty-aware Planning真实世界充满不确定性——传感器有噪声控制有误差障碍物会动。未来的规划器必须显式地考虑这些不确定性。例如在优化轨迹时不仅要最小化期望的代价还要最小化代价的方差风险。这涉及到概率论和随机最优控制的理论如线性二次高斯LQG和随机模型预测控制SMPC。我在做无人机室内飞行时就对视觉SLAM提供的位姿估计的不确定性非常头疼一个鲁棒的规划器应该能在定位不确定性大的区域自动减速或选择更安全的路径。3. 人机协同的交互式规划在服务机器人、共享空间等场景机器人的路径规划必须考虑人的行为和意图。这不仅仅是把动态的人当作移动障碍物来避让更需要预测人的轨迹、理解人的社交习惯如行人通常靠右行走并生成符合人类预期、看起来“自然”甚至“礼貌”的轨迹。这涉及到社会力模型、博弈论等跨学科知识是让机器人真正融入人类环境的关键。从我个人的项目实践来看路径规划与轨迹优化是一个理论与实践深度结合的领域。再漂亮的算法也需要对机器人本体、传感器、执行器有深刻的理解才能成功落地。我的建议是不要一开始就追求最前沿、最复杂的算法。从经典的A*和PID控制开始亲手实现一遍把整个数据流传感器-地图-规划-控制-执行器跑通理解每个环节的输入输出和延时。然后再针对你遇到的特定问题是路径太绕还是运动太抖还是避障不及时去有针对性地学习和引入更高级的方法。记住在机器人学里简单、稳定、可预测的系统往往比复杂但脆弱的系统更有价值。先让你的机器人可靠地动起来再让它优雅地动起来。