1. 项目缘起从林沛群老师的课程到我的机器人运动学实践最近在整理自己的机器人学习笔记翻到了当初啃林沛群老师机器人学课程时的第三部分内容。这部分主要讲的是轨迹规划当时学得云里雾里总觉得那些数学公式离真正的机器人动起来隔着一层纱。直到后来自己动手做项目用ROS控制机械臂画圆、让无人机在动态障碍物中穿行才真正体会到轨迹规划的魅力与坑点。所以这篇笔记不是对林老师课程内容的简单复述而是结合我后来在工业机械臂调试、移动机器人导航以及一些创客项目中的实际经验对“轨迹规划”这个核心主题的一次深度复盘和拓展。我会尽量用“人话”把原理讲清楚并分享那些在标准教材里不会写、但实际开发中一定会遇到的“坑”和技巧。轨迹规划听起来高大上其实核心问题就一个如何让机器人从A点安全、平滑、高效地运动到B点甚至是在C点、D点等多个点之间连续运动。无论是机械臂抓取零件还是无人机巡航抑或是自动驾驶汽车变道都离不开它。网上很多教程要么过于理论满篇多项式插值、样条曲线要么过于简化只给个“move_to”的API调用。我希望这篇笔记能成为两者之间的桥梁让你既明白背后的“为什么”也能上手“怎么做”。2. 轨迹规划的本质在约束的钢丝上跳舞在深入任何算法之前我们必须先建立正确的认知轨迹规划不是在真空中画一条漂亮的曲线而是在多重约束下寻找一条可行的路径。这些约束就像一根根钢丝规划算法就是在上面的舞者。2.1 核心约束拆解你的机器人不是超人首先是运动学约束。这是最基础的。对于轮式机器人你可能受限于非完整约束比如汽车不能横向移动。对于机械臂关节的运动范围位置、速度、加速度极限就是硬边界。林老师的课程里用D-H参数建立了运动学模型这相当于给了我们一张描述机器人所有可能姿态的“地图”。轨迹规划就是在这张地图上找出一条从起点姿态到终点姿态的连线。但这条线不能乱画必须保证线上的每一个点机器人的所有关节角度都在其物理极限内。我曾在调试一台六轴协作机器人时规划了一条看似最短的笛卡尔空间直线路径结果中间某个关节需要瞬间翻转180度远超其速度极限导致急停报警。这就是典型的只考虑了起点终点忽略了中间过程的运动学可行性。其次是动力学约束。这更进了一步考虑了机器人的“力气”和“惯性”。每个关节的电机扭矩是有限的机器人本体的质量分布也决定了它加速和减速的能力。一条运动学上完美的轨迹可能要求机器人在某个瞬间产生巨大的加速度所需的扭矩超过了电机峰值结果就是要么失步要么过载报警。在规划高速、高负载的运动时动力学约束是必须考虑的。例如让机械臂快速挥动一个重物如果不考虑动力学规划出的轨迹很可能在执行末端因为扭矩不足而“掉下来”。最后也是在实际项目中最让人头疼的是环境约束。这包括了静态障碍物工作台、墙壁、动态障碍物移动的AGV、行走的人以及任务本身的特殊要求如末端执行器必须始终保持水平。在复杂环境中轨迹规划问题就升级为“运动规划”需要结合感知传感器数据进行实时或近实时的重规划。最新的研究热点比如你提到的“复杂静态环境与动态障碍物下的无人机实时轨迹规划框架”就是在试图用更高效的算法如优化-based方法、强化学习来解这个高维、时变的问题。2.2 描述一条轨迹从“点”到“过程”很多人容易混淆“路径”和“轨迹”。简单来说路径Path 是空间中的一条几何线只描述了“去哪里”不关心“什么时候到”。比如地图上从家到公司的路线。轨迹Trajectory 是带时间参数的路径它同时描述了“去哪里”和“什么时候到”。它定义了位置乃至速度、加速度随时间变化的函数。比如导航软件给出的预计上午8点从家出发8点30分到达公司的行车方案包含了每个时刻的预期位置和速度。轨迹规划的输出通常就是一个轨迹函数q(t)其中q代表机器人的状态对于机械臂是各个关节的角度对于移动机器人可能是位置和朝向。这个函数需要是连续且光滑的至少要求速度连续C1连续为了更舒适和减少机械冲击通常要求加速度也连续C2连续。3. 关节空间 vs. 笛卡尔空间两条不同的规划思路这是轨迹规划中一个根本性的选择决定了你思考问题的层面和最终效果的优劣。3.1 关节空间轨迹规划让每个关节自己“跑”这种方法直接在机器人的关节空间中进行规划。给定起始和终止的关节角度向量为每一个关节独立规划一条从起点到终点的光滑时间函数。最常用的方法就是多项式插值特别是三次和五次多项式。三次多项式轨迹 这是最基础的。对于一个关节给定起始时间t0的位置q0、速度v0和终止时间tf的位置qf、速度vf。我们有四个边界条件恰好可以解出一个唯一的三次多项式q(t) a0 a1*t a2*t^2 a3*t^3通过代入边界条件可以解出系数a0, a1, a2, a3。它的优点是计算简单速度曲线是抛物线加速度是直线。但缺点也很明显加速度在起点和终点可能不连续跳跃这会导致关节受到冲击Jerk。在要求不高的场合可以接受。五次多项式轨迹 为了解决加速度不连续的问题我们增加约束条件。除了位置和速度再指定起点和终点的加速度通常设为0。这样就有六个边界条件需要五次多项式来满足q(t) a0 a1*t a2*t^2 a3*t^3 a4*t^4 a5*t^5五次多项式能保证加速度连续运动更平滑对机械结构更友好。这是目前工业机器人中最常用的一种关节空间规划方法。关节空间规划的优点计算相对简单每个关节独立计算易于实现。天生避障自碰撞只要起点和终点是可达且无碰撞的中间路径通常也在工作空间内不易发生机器人自己打自己的情况。容易满足关节极限因为直接规划关节角度可以很方便地检查每个时刻的角度是否超限。关节空间规划的缺点末端执行器路径不可控这是最大的问题。你在关节空间画了一条直线但机器人的末端在笛卡尔空间中的运动轨迹可能是一条非常复杂的曲线。这对于需要精确控制末端路径的任务如焊接、涂胶、视觉引导抓取来说是致命的。我曾需要机械臂末端沿一条直线喷涂用关节空间规划的结果是扭曲的曲线完全不合格。可能遇到奇异性在规划路径上机器人可能经过或接近奇异位形如机械臂完全伸直此时关节速度会趋于无穷大实际无法执行。3.2 笛卡尔空间轨迹规划紧盯末端“画笔”这种方法在机器人末端执行器的操作空间通常是笛卡尔空间中规划路径。先规划好末端执行器中心点TCP的空间路径如直线、圆弧、样条曲线然后通过机器人的逆运动学将路径上的每一个点反解出对应的关节角度。最经典的笛卡尔空间路径就是直线和圆弧。直线插补 给定起点和终点的末端位姿位置和姿态在这两点间进行线性插值。姿态插值通常使用四元数的球面线性插值SLERP来保证平滑。圆弧插补 给定不在同一直线上的三个点规划一段圆弧路径。笛卡尔空间规划的优点末端路径精确可控这是其最大优势。可以严格保证末端沿预定几何路径运动满足工艺要求。易于与外部系统对接路径通常由CAD模型或视觉系统给出直接在笛卡尔空间规划非常直观。笛卡尔空间规划的缺点计算量大需要实时进行逆运动学解算对于冗余机器人还需要处理逆解的选择和优化问题。可能无解或超出关节极限规划的笛卡尔路径上的某些点可能没有逆运动学解或者解对应的关节角度超出了极限。这就需要引入“路径验证”和“重规划”机制。可能穿越奇异点规划的直线路径可能恰好要求机器人穿过奇异位形。自碰撞风险即使末端路径是安全的机器人的连杆在运动过程中也可能发生碰撞需要额外的碰撞检测。实操心得如何选择我的经验法则是对末端路径形状有严格要求的任务焊接、切割、涂胶、精密装配必须用笛卡尔空间规划。对于点到点的搬运、码垛等任务关节空间规划更简单高效。在现代机器人控制器如KUKA、FANUC、ABB中这两种模式都支持通常通过不同的运动指令来调用如KUKA的PTP是关节空间LIN是笛卡尔空间直线。在ROS的MoveIt!中规划器也可以配置优先考虑哪种空间。4. 高阶轨迹生成样条曲线与时间最优规划当路径点多于两个时我们就需要更强大的工具来生成光滑的轨迹。同时我们也希望机器人能跑得更快。4.1 样条曲线连接多个路点的“柔性尺”多项式插值在两点间工作良好但直接用它连接多个点在连接点处虽然位置连续但高阶导数速度、加速度会不连续导致机器人运动不平稳。样条曲线就是为了解决这个问题而生的。你可以把样条想象成一根有弹性的尺子强迫它穿过一系列固定的点路点它会自然形成一条整体非常光滑的曲线。在轨迹规划中最常用的是三次样条Cubic Spline和B样条B-Spline。三次样条 它由一系列分段的三次多项式连接而成在连接点处保证位置、速度、加速度都连续C2连续。求解时需要全局解算一个线性方程组。它的优点是整体非常光滑但有个致命缺点局部修改一个路点会影响整条曲线。这在交互式编程或需要在线调整时很不方便。B样条 这是更强大和通用的工具。它由一组控制点和基函数定义。B样条最大的优点是具有“局部支撑性”修改一个控制点只会影响曲线局部的形状不会牵一发而动全身。这使得它非常适合需要实时调整轨迹的应用比如人工示教后的路径微调或者根据传感器信息动态避障。此外B样条可以很容易地保证轨迹不超过关节极限通过约束控制点的位置。在实际的机器人离线编程软件或高级控制器中示教器上记录的路径点内部通常就是用样条曲线特别是B样条来生成最终轨迹的。这保证了即使人工示教的点不够平滑最终机器人的运动也是光滑的。4.2 时间最优轨迹规划让机器人“跑出极限”前面的方法我们主要关注“光滑性”但“快”同样重要。时间最优轨迹规划Time-Optimal Trajectory Planning的目标是在满足所有运动学和动力学约束位置、速度、加速度、扭矩极限的前提下找到一条使总运动时间最短的轨迹。这本质上是一个最优控制问题。一个经典且实用的方法是基于梯形或S型速度曲线的标量规划。梯形速度曲线 这是最直观的。运动分为三段恒加速段、匀速段、恒减速段。规划时先以最大加速度加速到最大速度然后保持最后以最大减速度减速到0。但这种方式在加速和减速的切换点加速度会发生突变从a_max跳到0再跳到-a_max产生冲击Jerk无穷大。S型速度曲线七段式 为了解决冲击问题引入了加加速度Jerk的限制。速度曲线呈S型。运动分为七段加加速Jerk为正、匀加速、减加速Jerk为负、匀速、加减速、匀减速、减减速。这样加速度是连续的梯形速度是光滑的S型曲线对机械系统最友好。现在主流的工业机器人默认采用的就是S型速度规划。如何做到时间最优核心思想是**“踩满油门和刹车”**。规划器会尝试让机器人在每个运动阶段都尽可能达到约束边界最大加加速度、最大加速度、最大速度。算法会先根据路径几何和约束条件计算出每个路径段所能允许的最大速度曲线称为“速度极限曲线”然后通过前向积分和后向积分找到一条既不超限又能最大化利用所有约束的“时间最优”速度曲线。避坑指南仿真与现实的差距在仿真如MATLAB/Simulink, Gazebo中规划出一条完美的时间最优轨迹是一回事在真机上跑起来是另一回事。最大的坑在于模型不匹配和未建模动态。你的动力学模型参数质量、惯量、摩擦不可能100%准确电机响应也有延迟和非线性。把仿真中算出的极限参数直接用到真机上极大概率会引发过冲、振荡甚至失控。我的做法是在仿真中得到理论最优时间T_optimal和轨迹。在真机上部署时将所有约束最大速度、加速度、加加速度先打一个安全系数比如设置为仿真值的70%。运行并观察实际电流、跟踪误差。如果一切平稳再以5%为步长逐步提高约束进行“标定”直到找到真机稳定运行的性能边界。这个过程叫系统辨识与控制器整定是机器人调试的必修课。5. 从规划到执行ROS与工业现场的实战链路理论懂了怎么用起来这里我以ROS和工业机器人两种典型场景拆解一下从规划到执行的完整链路。5.1 ROSMoveIt!中的轨迹规划实战ROS是机器人研究的“标准配置”MoveIt!则是其中负责运动规划的核心框架。当你调用move_group的plan()和execute()时背后发生了很多事。规划请求 你设定了目标位姿、路径约束如末端保持水平、规划器类型OMPL, CHOMP, STOMP等。规划场景 MoveIt! 会加载你的URDF模型和当前感知到的环境点云/网格构建一个包含自碰撞和外界碰撞检测的规划场景。规划器工作 以OMPL为例它会在你的机器人构型空间C-space中随机采样构建一条从起点到终点的无碰撞路径。这个过程是在关节空间进行的但目标约束是在笛卡尔空间。MoveIt! 内部会频繁调用你的运动学插件进行正逆解算来验证采样点。路径后处理 OMPL规划出的路径可能很粗糙节点稀疏。MoveIt! 会用“时间参数化”模块对这条几何路径进行插值和时间分配生成最终带时间戳的轨迹trajectory_msgs/JointTrajectory。这个时间参数化过程就会考虑速度、加速度约束通常生成的就是前面提到的S型速度曲线。执行与跟踪 生成的轨迹通过FollowJointTrajectoryaction 发送给机器人的底层控制器如ros_control。底层控制器通常是一个PID前馈的位置/速度环会努力让每个关节跟踪这条时间轨迹。关键踩坑点规划时间过长 在复杂狭窄环境中基于采样的规划器可能长时间找不到解。可以尝试简化碰撞模型、调整采样器参数、设置合理的规划时间限制、使用不同的规划器CHOMP更适合有梯度信息的场景。“规划成功但执行抖动” 这往往是规划路径在关节空间不够平滑或者时间参数化时给的加速度/加加速度限制过大超过了底层电机的跟踪能力。一定要检查规划出的轨迹在关节空间的速度/加速度曲线是否连续、光滑。可以在RViz中播放轨迹时用rqt_plot实时绘制关节状态来诊断。起点偏差问题 MoveIt! 规划时默认以当前关节状态为起点。如果规划前机器人被手动拖动或发生了漂移规划就会从错误起点开始导致执行时剧烈跳动。务必在规划前更新机器人状态。5.2 工业机器人以FANUC为例的轨迹实现在工业现场编程通常通过示教器完成但背后逻辑是相通的。路径示教 工程师手持示教器移动机器人到关键工艺点记录下该点的位姿笛卡尔坐标和关节角。这一系列点就构成了粗略的路径。指令编程 在程序中使用运动指令连接这些点。例如J P[1] 100% FINE 关节空间移动到点1速度100%精确到位FINE会停在精确点。L P[2] 500mm/sec CNT100 笛卡尔空间直线移动到点2速度500mm/s连续路径模式CNT100表示平滑过渡数值越大越平滑。C P[3] P[4] 300mm/sec 笛卡尔空间圆弧移动经过点3到达点4。控制器内部规划 当你按下启动键机器人控制器会做类似MoveIt!的工作根据运动指令类型J/L/C、速度设定、平滑度CNT值在内部生成一条满足机器人动力学约束的平滑轨迹通常是带S型速度曲线的样条。这个规划过程是高度优化且实时的保证了运动的平稳和高效。伺服执行 生成的轨迹被分解为高频率通常1ms或更低的位置指令发送给各个关节的伺服驱动器完成闭环控制。工业现场经验“FINE” vs “CNT” 对于需要精确停顿的点如抓取点、放置点用FINE。对于路径中间点希望运动流畅不停顿用CNT。CNT值的选择很重要值太小接近FINE值太大会导致路径圆角过大偏离示教点。需要根据工艺调试。奇异点管理 在接近奇异位形时机器人会报警或降速。编程时要避免路径穿过奇异区如六轴机械臂的“腕部中心点与J4/J6轴对齐”的状态。好的做法是在离线编程软件中仿真时就开启奇异点检测功能。负载与工具设定这是99%的新手都会忽略的关键在机器人执行任何程序前必须在控制器中正确设置负载被搬运物体的重量、重心和工具末端执行器的数据。这个数据直接影响控制器进行动力学前馈补偿的准确性。设置错误会导致轨迹跟踪误差大、运动抖动、甚至过载。每次更换夹具或工件都必须重新设定。6. 前沿与挑战动态环境与实时规划林老师的课程主要聚焦于结构化环境下的轨迹规划。但现实世界是动态的这就引向了更前沿的领域。动态障碍物处理 当环境中存在移动的障碍物时离线规划好的轨迹可能瞬间失效。这就需要实时重规划。一种常见架构是“全局规划器局部规划器”全局规划器如A*, RRT*基于静态地图规划一条粗略的初始路径。局部规划器如DWA Timed Elastic Band以很高频率10-50Hz运行。它只关注机器人前方一小段距离结合实时传感器激光雷达、摄像头数据对全局路径进行局部调整以避开突然出现的动态障碍物同时尽量贴合全局路径。“复杂静态环境与动态障碍物下的无人机实时轨迹规划框架”这类研究通常是在这个架构上做优化。例如使用B样条表示轨迹将避障、动态可行性、平滑度等要求转化为一个优化问题的约束和目标函数然后使用数值优化方法如MINCO 二次规划QP在线求解。这样生成的轨迹既能保证动力学可行又能实时避障。强化学习的应用 在极其复杂、难以建模的环境如杂乱仓库、非结构化地形中传统基于模型的规划器可能失效。深度强化学习DRL让机器人通过“试错”来学习规划策略。例如让四足机器人在仿真中学习各种地形的步态让机械臂学习在杂乱物体中抓取。但DRL目前面临样本效率低、仿真到现实的迁移Sim2Real困难等挑战离大规模的工业落地还有距离。轨迹规划是一个深不见底的领域从经典的三次多项式到现代的深度强化学习工具在变但核心问题不变在约束中寻找最优或可行的运动方案。林沛群老师的课程给了我们坚实的理论基石而真正的理解和掌握则来自于在仿真中调试参数、在真机上排查故障、在项目 deadline 前解决一个又一个诡异问题的实践过程。希望这篇结合了理论与实践、踩坑与填坑的笔记能帮你少走一些弯路。记住看懂算法是第一步让算法在真实的物理世界里稳定、可靠地跑起来才是工程师价值的最终体现。