去年调试一台六轴协作机械臂时我遇到过一个相当典型的问题运动学算得清清楚楚末端轨迹在仿真里顺滑得像丝绸一上真机就开始抖。当时我第一反应是电机不行换了驱动器、调了伺服增益问题依旧。最后静下心来排查才发现根子出在我的控制器几乎完全忽略了动力学——关节之间的惯量耦合、连杆自身的重力矩这些量在真实系统里无时无刻不在起作用只是仿真环境默认它们不存在。那次经历让我决定把机器人动力学与控制系统性地重学一遍。这篇笔记是这个系列的第一篇重点梳理机器人控制的整体框架控制对象到底是什么、运动学与动力学在控制中分别扮演什么角色、控制系统怎么分层、主流控制策略各解决了什么痛点。内容不追求推导每个公式而是想先把地图画清楚适合正在学机器人学、或者做机械臂项目时感觉控制部分不够扎实的朋友参考。1. 机器人控制到底在控什么从一台真实机械臂说起1.1 控制目标往往定义在操作空间执行器却在关节空间我们做项目时用户说把机械臂末端移到这个点这个点用三维坐标加姿态描述属于操作空间也叫笛卡尔空间。但机械臂的每个关节只能输出一个电机力矩电机转过的角度叫关节角属于关节空间。控制器要做的事情本质上是把操作空间的目标翻译成关节空间上一系列随时间变化的力矩指令。这个翻译过程拆开来看有两步运动学层把操作空间的目标位姿映射成关节角序列。这是个几何问题不涉及力。动力学层根据关节角序列和期望运动计算每个关节需要施加多大的力矩。这是个力学问题。实际工程项目里很多人只做了第一步就急着让机械臂动起来。低速轻载场景下问题不大因为惯量耦合和哥氏力都很小关节之间基本互不干扰。一旦速度提上去或者负载变重你就会发现轨迹误差迅速增大甚至出现震颤。这背后的原因就是动力学被无视了。1.2 控制对象的真实面貌强耦合、非线性、时变机器人动力学与前几年流行的控制理论教学案例最大的区别在于它面对的是一个极其不乖的被控对象。以六轴工业机械臂为例它的动力学方程标准形式是M(q)q̈ C(q, q̇)q̇ g(q) τ这个方程里M(q)是惯量矩阵它随关节位置变化——机械臂伸展和蜷缩时末端等效惯量可以差好几倍C(q, q̇)q̇是哥氏力和离心力项关节速度越高这项越明显g(q)是重力项不同位形下每个关节承受的重力矩完全不同。这三个特性叠加在一起意味着机械臂不是一个参数基本恒定的系统。你用固定增益的线性控制器比如PID去控相当于一个人穿了一双会随时变重量的鞋跑步——在某个位形下可能很稳换个位形就可能过冲或振荡。1.3 控制目标的三层含义我们常说的控制好机械臂实际包含三个层次的目标轨迹跟踪末端或关节能否精确跟随期望轨迹关注误差收敛速度和稳态精度。力/交互控制末端接触环境时能否输出期望的接触力装配、打磨、拖动示教场景的核心需求。稳定性与鲁棒性在负载变化、参数摄动、外界扰动下系统能否保持稳定并维持性能。一个不成熟的控制方案通常在仿真轨迹阶段就提前自我满足了真机一测才发现连稳定性这关都过不了。所以学习机器人控制第一步不是背公式而是建立被控对象远比想象复杂的敬畏心。2. 运动学与动力学控制地图上必须分清的两种模型2.1 运动学模型只谈几何关系不谈力运动学回答的问题是关节角与末端位姿之间存在什么映射关系正向运动学很简单已知六个关节角求末端的位置和姿态。这是一条唯一的路径通过齐次变换矩阵连乘就能得到。逆向运动学麻烦一些已知末端位姿反解关节角。这里可能有多组解、可能无解超出工作空间解析解不一定存在经常需要数值迭代。除了位置层面的关系运动学还给出了速度层面的映射ẋ J(q)q̇J就是雅可比矩阵它把关节速度映射到末端速度。雅可比矩阵在控制里极其重要后面讲力控制和操作空间控制时还会反复用到它因为力的映射也经过它末端力 F 折算到关节力矩 τ JᵀF。要注意运动学模型只用了几何参数连杆长度、关节角度、坐标变换关系。它不关心这个连杆有多重、关节摩擦力有多大。正因如此运动学在任何机器人上都能通过测量几何尺寸很快标定好误差通常不大。2.2 动力学模型力与加速度之间的因果链动力学回答的问题是机械臂在某个状态下施加一定力矩会产生什么样的加速度这比运动学难得多。因为它要描述整个机械臂的质量分布、惯量特性、关节摩擦、甚至电机转子的惯量。这些物理参数很难直接测量通常需要设计实验去辨识。动力学建模最常用的两套方法拉格朗日法基于能量视角构造动能和势能之差对广义坐标求导得到运动方程。优点是推导过程系统化适合手推和理解结构缺点是当关节数多时符号运算量爆炸。牛顿-欧拉递推法从基座到末端做前向递推计算速度和加速度再从末端向基座后向递推计算力和力矩。优点是计算效率高适合计算机实现工程中大部分实时动力学库都用这种方法。需要特别提醒的是即使你完全掌握了动力学建模方法建出来的模型也只是近似为真。真实的摩擦特性、连杆重心的微小偏差、电机减速器的柔性和损耗这些都非常难建模。所以动力学模型的精度永远有上限这是后面各种鲁棒控制、自适应控制、扰动观测器存在的根本原因。2.3 两个模型在控制器中的分工我在学习时用一句话总结了两者的分工运动学告诉你该往哪里去动力学告诉你该使多大力气。具体到控制器内部轨迹规划器输出期望的关节位置、速度、加速度这通常基于运动学逆解动力学模型接收这些期望值计算出前馈力矩反馈控制器计算实际状态与期望状态的误差产生修正力矩。前馈部分是动力学在控制中最直接的应用它并不负责纠偏而是负责提前出力让系统在期望轨迹上运动时不会被惯量、重力和耦合作用拖后腿。3. 控制系统的分层架构从任务规划到关节伺服3.1 经典三层架构任务层、规划层、伺服层拿一个搬运场景举例。机械臂需要把物料从A点搬到B点完整的控制系统大致分三层任务层是最顶层决定干什么A点和B点在哪里、路径上有没有障碍物、是走直线还是走圆弧。这层主要靠路径规划和运动规划算法输出是一条末端或关节层面的期望轨迹。规划层是把任务层的宏观决策转成具体的时变轨迹每个时刻末端应该在哪、速度是多少、加速度是多少。这层大量使用运动学和雅可比矩阵通常在几十赫兹到几百赫兹的频率下运行把规划结果下发。伺服层是离电机最近的一层通常以1kHz就是每秒1000次甚至更高的频率运行。它接收规划层的期望关节轨迹读取编码器反馈的实际关节位置和速度计算出力矩指令发给驱动器。动力学模型和控制算法PID、计算力矩、阻抗控制等基本都在这层运行。3.2 各层之间流动的信息与时间尺度我个人的理解是三层之间的关系很像公司里的三层管理任务层是CEO只看大方向不看细节决策频率低规划层是部门经理把战略转成可执行的计划频率中等伺服层是执行员工每秒都在高速干活频率最高。一个常见的新手错误是试图把任务层的工作下放到伺服层去做。典型表现是在1kHz的伺服循环里直接做路径规划和逆运动学求解结果控制器循环时间超标系统稳定性急剧下降。正确做法是利用上层低频规划下层高频跟踪的分离原则各层各司其职。3.3 外部传感器接入的位置决定系统特性现代机器人几乎都有视觉和力传感器它们接入的位置会显著影响控制架构。视觉伺服通常把视觉信息送入任务层或规划层相机识别物体位姿更新目标的期望轨迹。因为视觉处理本身有延时通常几十毫秒不适合放进1kHz的伺服环路。力传感器则相反它往往直接接入伺服层。比如柔顺控制场景下六维力传感器采集到的接触力数据会以毫秒级周期送进控制器参与力矩计算。这也解释了为什么力控对通信总线的实时性要求极高一旦引入不可控的延迟力控环路就会振荡甚至失稳。4. 控制策略全景每种方法解决的痛点各不相同4.1 独立关节PID工程现场的默认选项工业机械臂里最普及的控制方案是把每个关节当成一个独立的单输入单输出系统各自用一套PID控制。控制器完全没有考虑关节之间的耦合也不使用系统的动力学模型。为什么这样粗糙的方案能统治工业界这么多年因为它在低速、轻载、工作范围相对固定的场景下足够稳定而且工程师不用做复杂的动力学辨识和调试光伏、3C、码垛等大批重复性作业对高速高动态的要求并不高。但独立关节PID有明确的性能天花板关节速度越高、臂展越长、负载越重耦合效应越显著。你会发现一个关节高速运动时另一个关节会被带跑轨迹误差明显增大。如果需要高性能动态控制必须跳出PID框架。4.2 计算力矩法用模型把非线性系统拉直计算力矩法是动力学模型进入控制器设计的最直接方式核心思想非常漂亮如果我的动力学模型足够准确那么我可以设计控制器τ M(q)(q̈_d K_p e K_d ė) Ĉ(q, q̇)q̇ ĝ(q)把它代入真实动力学方程模型项直接抵消剩下的是一个解耦的线性系统ë K_d ė K_p e 0也就是说只要模型准确我可以自由配置Kp和Kd来任意设定误差的动态响应。这相当于把一个复杂的非线性耦合系统假装变成了一个简单的线性二阶系统来设计。这个方法效果在高速高动态场景下显著优于PID但工程上有个致命前提模型必须够准。前面说过动力学模型存在参数误差模型不准时计算力矩法的性能会急剧退化甚至不如调好的PID。4.3 力控制为什么单纯位置控制做不了打磨和装配位置控制的核心目标是让位置误差趋近零它默认环境是刚性的。但装配、打磨、抛光这类任务不同接触力控制不好要么工件被压坏要么机械臂弹开。阻抗控制是我认为最优雅的力控思路。它的思想不是直接控制力而是控制接触时表现出的机械阻抗。设想末端遇到环境阻力时机械臂不是硬生生抵抗而是表现出质量-弹簧-阻尼的特性M_d(ẍ - ẍ_d) B_d(ẋ - ẋ_d) K_d(x - x_d) F_ext参数Md、Bd、Kd决定了机器人对外力的响应特性。桌面擦拭任务需要较小的刚度柔顺点按任务需要相对较高的刚度。调节这三个参数就能获得期望的柔顺手感。我建议学力控的同学不要一上来就啃阻抗控制的论文先想明白一个问题位置控制为什么在接触场景下会失败答案是位置控制试图消除一切位置误差而接触场景下的位置误差恰恰和接触力耦合在一起过度追求位置精度必然会带来过大的接触力。4.4 现代趋势鲁棒控制、自适应控制与学习控制既然模型永远存在误差一个很自然的思路是在控制器里加一个观测器把未建模动态和参数误差当作扰动来估计并补偿。基于扰动观测器的方法在实际工程中应用越来越广特别是四足机器人和协作机械臂上效果非常显著。自适应控制的思路更进一步既然参数不确定那就设计参数更新律让控制器在线估计惯量、重心等参数。这类方法在理论上很优雅但工程实现时需要注意参数收敛性问题参数不收敛时可能出现缓慢漂移。学习控制是当前最热的趋势无论是基于强化学习的端到端策略还是利用神经网络对动力学残差建模的混合方法。我的观察是学习控制最有价值的切入点是解决模型误差这一老大难问题而不是彻底抛弃基于模型的控制框架。用学习补齐模型误差用基于模型的控制器保证稳定性这个方向工程落地前景最好。5. 实用环节搭建第一个控制仿真环境并跑通位置控制前面谈了这么多框架和理论如果不亲手跑一下理解始终浮在表面。这一节我以Python环境为例分享从零搭建一个简单两连杆机械臂控制仿真并跑通计算力矩法的完整步骤。5.1 环境准备与模型参数需要安装numpy、scipy和matplotlib用scipy来求解动力学微分方程用matplotlib可视化结果。两连杆机械臂的动力学模型可以自己推导两个连杆的质量分别为m1、m2长度为l1、l2质心距离关节分别为lc1、lc2关节变量为q1、q2。用拉格朗日法可以推得M(q)、C(q,q̇)、g(q)的具体表达式。这里建议手推一遍别直接抄代码推完你会对惯性耦合项的理解上一个台阶。5.2 计算力矩控制器的实现核心控制器部分核心代码如下def compute_torque(q_des, qd_des, qdd_des, q, qd, Kp, Kd): # 1. 根据当前状态计算模型项 M inertia_matrix(q) C coriolis_matrix(q, qd) g gravity_vector(q) # 2. 位置误差和速度误差 e q_des - q ed qd_des - qd # 3. 计算力矩模型前馈 线性反馈 tau M (qdd_des Kd ed Kp e) C qd g return tau这段代码有几点值得说Kp和Kd建议设计成对角矩阵每一维对应一个关节。为了让关节1和关节2拥有相似的动态响应Kp和Kd的取值需要和对应关节的惯量量级匹配否则会出现一个关节响应很快、另一个很慢的尴尬局面。Kp和Kd不是越大越好。仿真里没有噪声和延迟增益可以拉得很高但真实系统里传感器噪声会通过高增益放大最终表现为关节抖动。5.3 仿真结果与对比实验仿真里我做了三组对比纯独立关节PID不补偿重力和耦合只加重力补偿的PID完整的计算力矩法。结果很明显方案1在慢速运动中尚可但一旦要让关节快速运动跟踪误差呈发散趋势方案2在静态时表现不错但动态过程中仍有明显误差方案3全程跟踪误差极小动态响应干净利落。这个实验结果很有教育意义重力补偿是最容易落地且立刻见效的一步因为它消除了每个关节承受的静态偏置而完整的动力学前馈则是把动态场景下的性能推到新高度的关键。6. 学习路线与实战体会怎样按正确的顺序掌握动力学控制6.1 数学工具准备清单机器人控制不是上来就啃理论书要有对应的数学基础。我按优先级排序线性代数矩阵运算、特征值、矩阵求逆这是理解惯量矩阵和状态空间的基础。如果你看到公式里出现了矩阵还觉得发怵先回头补线代。微积分与常微分方程控制本质上是设计微分方程并分析其稳定性。至少要知道什么是相轨迹、什么是收敛。刚体力学转动惯量、力矩、角动量守恒这些大学物理知识需要用的时候能想起来。李群与李代数进阶想深入理解旋转矩阵、姿态控制和操作空间控制时这层数学不可回避但入门阶段可以后置。我的建议是不要等数学完全准备好再开始学控制而是在实际推导中遇到哪块补哪块效率远高于先啃完数学课本再上手。6.2 仿真先行的实践路径模拟环境的选择要分层考虑入门阶段用Python Robotics Toolbox加自己写的小仿真就够了重点是理解力学方程的结构进阶阶段可以用Gazebo或MuJoCo这类物理引擎它们有更真实的接触和摩擦模型要接触工业验证的用户可以用Simulink搭建控制器加机械臂模型的联合仿真控制逻辑可以无缝部署到实时硬件上。强烈建议初学者自己写一遍拉格朗日方程推导的代码再和现成库的结果对比。这个过程能暴露出很多自以为懂但实际没懂的细节。6.3 最容易踩到的三个坑第一个坑是单位不统一。角度用度还是弧度、力矩用Nm还是mNm这些看似无关紧要的单位错误在控制系统里会造成数量级的偏差。最好在模型层统一成SI单位接口层做转换。第二个坑是参数辨识不足却强行上基于模型的控制器。计算力矩法对模型参数精度要求苛刻如果你的质量参数和重心位置只是拍脑袋估计的不如先用经过试验的重力补偿加PID后续逐步下放更多模型项。第三个坑是仿真与真机的鸿沟。仿真里没有延迟、没有噪声、没有柔性和摩擦很多在仿真里表现良好的控制器在真机上会遇到稳定性问题。我的做法是在仿真里有意识地加入时间延迟、量化噪声和随机扰动让控制器在更恶劣的环境中先被考验一遍。最后再分享一个个人经验学习机器人控制最有价值的动作不是看了多少教材和论文而是亲手把一台真实或高保真仿真机械臂从只能动调教到精准地动。这中间你会经历振荡、失稳、参数漂移这些问题每一次排查和解决都是对动力学思想的一次内化。下一篇笔记我会聚焦动力学建模的详细推导过程重点讲透拉格朗日法和牛顿-欧拉法的实现细节我们下篇见。
