前阵子给双足机器人换了一套2-RSS-1U并联踝关节地面支撑阶段力矩控制一直调不稳单腿站立时脚踝位置抖得厉害整条小腿都在高频震动。折腾了大半个月最后发现根因是雅可比矩阵里两个旋转轴的符号方向反了重力前馈算出来的力直接抵消了本该输出的支撑力矩。这个踩坑过程让我想把从并联踝关节构型选择、雅可比矩阵推导到力矩控制落地的完整链路重新梳理一遍。这套东西正好覆盖了双足机器人研究里最硬核的部分机构设计、运动学建模和力控实现适合正在做双足机器人、外骨骼或者并联机构控制的朋友参考。1. 项目背景与整体设计思路1.1 双足机器人踝关节为什么需要并联构型传统双足机器人的踝关节多数是串联结构电机加上谐波减速器直接装在脚踝位置结构简单、设计资料多但实际跑起来问题不少。最麻烦的是惯量问题——踝关节在腿部链条的最末端串联电机和减速器的质量全部落在小腿末端摆动相时髋关节和膝关节要额外克服这一坨惯量动态响应自然就慢半拍。落地冲击时串联结构刚度不足脚踝会产生明显的柔性形变位置控制模式下冲击力无法被快速吸收整个机身容易弹跳。并联构型恰好避开了这些问题。驱动电机可以布置在小腿近端甚至靠近膝关节的位置通过连杆机构把动力传递到脚踝腿部末端质量大幅下降转动惯量可能降低一个数量级。并联结构的刚度天然比悬臂梁式的串联结构高承载能力强特别适合踝关节这种需要扛住2到3倍体重地面反力的场景。精度方面也有优势串联关节的误差是逐级累积的而并联机构各个支链的误差互相约束末端精度更容易保证。正因为这些特性2-RSS-1U这类低自由度并联机构频繁出现在踝关节康复机器人和双足机器人的研究中。对双足行走来说踝关节在站立相需要提供足够的支撑力矩和刚度在摆动相又要轻快灵活这两个需求并联构型都能兼顾。1.2 2-RSS-1U构型自由度、布局与设计考量2-RSS-1U这个名字初看有点吓人拆开看就清楚了。定平台固定在小腿侧动平台连接足底两者之间有三条连接支链两条RSS驱动支链和一条U被动支链。每条RSS支链包含一个由电机驱动的转动副R一个连接摇杆与连杆的球副S以及一个连接连杆与动平台的球副S。U支链是一个十字万向铰可以看作两个轴线相交的转动副。这个构型的自由度可以用Kutzbach公式验证。总构件数定平台机架、动平台、两条RSS支链的摇杆和连杆共4个构件、U链的十字轴中间件1个合计7个构件。运动副数量两条RSS支链各有1个转动副和2个球副合计6个运动副U链的十字轴拆成2个转动副总计8个运动副。所有运动副自由度之和两个RSS支链各提供1337个自由度共14U链提供2个自由度总计16。代入Kutzbach公式M6(n-g-1)Σf得到M6×(7-8-1)164。但这4个自由度中有2个是RSS连杆绕自身两端球副中心连线自转的局部自由度对动平台运动没有贡献所以实际自由度为2。这两个自由度正好对应踝关节的跖屈/背屈和内翻/外翻。布局设计上两条RSS支链的电机轴线通常对称布置在定平台两侧与中间U链形成120度或90度夹角。动平台的转动中心由U链的十字轴确定两条驱动链通过改变连杆空间位置带动动平台姿态变化。设计时要特别关注工作空间边界——踝关节跖屈/背屈一般需要±25到30度内翻/外翻约±15度球副在极限位置容易发生干涉连杆长度和球副安装角度需要反复迭代。1.3 动态行走对踝关节控制提出的特殊要求双足机器人动态行走对踝关节控制的要求和仿人机器人研究的核心问题高度相关。站立相时机器人整个身体的重量通过踝关节传递到地面脚踝需要持续输出来自足底反作用力对应的力矩。这个阶段踝关节本质上是一个力发生器控制的重点不是位置而是力。摆动相时踝关节主要作用是让脚掌离地时有一定跖屈角度避免脚尖拖地这个阶段对响应速度的要求更高。由于机械腿是一个多刚体耦合系统踝关节力矩的变化会直接影响膝关节和髋关节的动力学状态控制周期必须足够短。力矩控制背景下整个系统可以看作一个在重力场中运动的倒立摆模型踝关节是调节质心位置和地面压力中心CoP的关键执行器。2. 雅可比矩阵从几何约束到力映射的桥梁2.1 先分清运动学雅可比与力雅可比很多初学者在并联机构上栽跟头首先是因为没分清两个雅可比。运动学雅可比描述的是关节速度到动平台速度的映射假设驱动关节角速度为θ_dot动平台角速度为ω运动学关系写成ωJ_kin·θ_dot这里的J_kin是运动学雅可比。力雅可比描述的是驱动力矩到动平台等效力的映射在静力平衡条件下τJ^T·F这里的J^T就是力雅可比其中J是运动学雅可比。力矩控制里用到的是力雅可比也就是运动学雅可比矩阵的转置。但实际工程中很多控制器的输入是期望的任务空间力F_des比如期望的地面反作用力调整量需要左乘J^T得到驱动关节的期望力矩所以这个转置关系是控制回路里最核心的一环。需要注意的是对于并联机构来说由于位置正解通常比较困难更方便的做法是先推导从动平台速度到驱动速度的映射关系也就是驱动雅可比J_drive满足θ_dotJ_drive·ω。此时控制用的映射关系是τJ_drive^T·F——注意这里的J_drive和前面提到的J_kin互为逆矩阵关系用在力映射时要小心不要搞混。2.2 2-RSS-1U雅可比矩阵的推导路径推导2-RSS-1U机构的雅可比矩阵最直观的方法是建立闭环矢量方程。以动平台中心为参考点设动平台在定平台坐标系中的位置矢量为P旋转矩阵为R动平台上球副中心在动坐标系中的矢量为a_i。定平台侧摇杆末端球副中心位置为b_i它由驱动角θ_i决定。连杆长度为l_i方向单位矢量为e_i。对每条RSS支链可以写出闭环矢量方程P R·a_i b_i l_i·e_i由于该机构只有姿态变化没有平移P是常矢量。方程两边对时间求导动平台侧的线速度项为ω×(R·a_i)其中ω是动平台角速度矢量。定平台侧的b_i对时间的导数只与驱动角速度θ_dot_i有关。连杆侧l_i·e_i的导数包括两部分长度变化率实际为0和方向变化率方向变化率项包含连杆角速度ω_rod。关键是下一步将方程两边同时点乘连杆方向矢量e_i连杆自身的角速度项就被消掉了。整理后得到ω·((R·a_i)×e_i) (∂b_i/∂θ_i)·θ_dot_i这个式子的物理含义是动平台角速度在某个几何矢量方向上的投影等于驱动角速度在该支链约束方向上的投影。每条RSS支链给出这样一个标量方程两条支链汇集起来就得到矩阵方程J_platform·[α_dot; β_dot] J_drive·[θ_dot_1; θ_dot_2]其中α和β是描述动平台姿态的两个欧拉角分别对应跖屈/背屈和内翻/外翻。这里用到姿态角速度与欧拉角速度之间的转换矩阵E实际计算时J_platform是一个2×3矩阵乘以3×2的E矩阵最终得到2×2矩阵。驱动雅可比矩阵J就是J_drive的逆乘以J_platformJ J_drive^{-1}·J_platform从工程实现角度我建议不要手推完整符号形式直接用数值方法构造给动平台一个微小的α角速度通过闭环矢量方程解出两个驱动角速度这就是J的第一列再给一个微小的β角速度得到第二列。这样构造出来的雅可比矩阵虽然不如符号推导优雅但不容易出错。2.3 数值验证、奇异位形与工程检查雅可比矩阵推导出来之后第一件事不是急着写控制器而是验证。我常用的验证方法是有限差分法。给动平台姿态一个小扰动Δα通过逆运动学求出两个驱动角的变化Δθ_1和Δθ_2然后和雅可比矩阵计算的结果对比。如果误差在千分之一以内说明雅可比推导基本正确。误差偏大时优先检查坐标系变换矩阵和球副中心位置矢量的方向。雅可比矩阵的数值特性直接影响控制稳定性。用奇异值分解可以快速检查在工作空间内均匀撒点计算每个姿态点的雅可比奇异值记录最小奇异值和条件数。条件数突然攀升的区域就是奇异位形附近。对于2-RSS-1U机构奇异位形通常出现在极限转角附近此时两个RSS支链的连杆方向趋于共面J_platform的行向量接近线性相关。工程上我习惯在控制代码里实时计算雅可比条件数当条件数超过阈值比如100时降低控制增益或者直接限制驱动关节的输出力矩。这个细节能避免很多次莫名其妙的发散事故。3. 力矩控制实现从位置环到力控的完整链路3.1 为什么纯位置控制走不稳双足机器人行走问题本质上是在平衡一个多连杆倒立摆。纯位置控制模式下每个关节的控制器都在拼命把关节角拉向期望角度等效于一个刚度很大的弹簧。机器人站立时如果地面高度和期望位置之间有微小偏差位置环就会持续输出很大的力导致机身晃动或者增益太高产生振荡。落地瞬间脚跟冲击力通过刚性位置环直接传递到机器人全身能量无法被吸收机器人很容易反弹甚至摔倒。动态行走还需要主动调节地面压力中心CoP。CoP的位置决定了ZMP零力矩点是否落在支撑多边形内是稳定性的直接判据。位置控制只能间接影响CoP且响应慢无法满足快速动态调整的需求。力矩控制则直接以力为控制目标可以快速调整踝关节的输出力矩进而直接改变CoP位置。3.2 力矩控制方案选型与整体架构力矩控制方案建议采用“力矩内环阻抗外环”的结构。外层根据机器人质心CoM位置、速度和CoP误差计算出动平台需要的任务空间虚拟力F_task这个虚拟力相当于一个弹簧阻尼系统施加给脚踝的力。内层把F_task左乘雅可比矩阵的转置映射为两个驱动电机的期望输出力矩。控制律可以写成τ J^T·F_task G(q) C(q, q_dot)其中G(q)是重力补偿项C(q, q_dot)是科氏力和离心力补偿项。双足机器人的踝关节在站立相承受的主要是重力矩重力补偿如果做得准静态站立基本上可以做到纹丝不动。动态行走时科氏力和离心力也不可忽略特别是摆动相快速摆动时这部分力矩补偿会显著改善控制效果。阻抗外环的F_task计算F_task Kp·(x_des - x) Kd·(x_dot_des - x_dot)这里的x是动平台的姿态角矢量α和βx_des是期望姿态角。Kp是虚拟刚度Kd是虚拟阻尼。阻抗控制的本质是让机器人对外表现出期望的机械阻抗特性落地冲击时Kp和Kd可以吸收能量实现柔顺缓冲。3.3 核心代码实现与参数整定经验控制主循环的Python伪代码如下# 控制参数 Kp np.array([800.0, 600.0]) # 虚拟刚度 Nm/rad Kd np.array([40.0, 30.0]) # 虚拟阻尼 Nm*s/rad dt 0.001 # 控制周期 1kHz while running: # 读取传感器 q_motor read_motor_position() # 2个驱动关节角 tau_feedback read_torque_sensor() # 关节力矩反馈 imu_data read_imu() # 姿态估计 force_sole read_sole_force() # 足底六维力 # 计算动平台姿态角和角速度姿态估计器输出 alpha, beta estimate_orientation(imu_data) # 计算雅可比矩阵当前姿态下 J compute_jacobian(alpha, beta) # 阻抗外环计算任务空间虚拟力 F_task Kp * (x_des - x_current) Kd * (x_dot_des - x_dot_current) # 重力补偿计算基于当前姿态和机器人质量参数 G compute_gravity_compensation(alpha, beta) # 力矩映射J^T * F_task G tau_des J.T F_task G # 输出力矩限幅防止过流 tau_des np.clip(tau_des, -tau_max, tau_max) # 发送到电机驱动器 command_motor_torque(tau_des)参数整定的顺序很有讲究。一定要先调静态支撑。机器人站在地面上不给期望姿态偏移只加重力补偿项观测机器人是否稳定。如果重力补偿不准机器人会缓慢倾斜这时候通过足底力传感器反馈可以反推出补偿误差逐步修正质心位置参数和雅可比符号。重力补偿调好之后再逐步增加阻抗刚度Kp。从小值比如200 Nm/rad开始观察机器人受扰动后的恢复情况。Kp越大恢复越干脆但过大容易引入振荡。阻尼Kd根据临界阻尼来估算Kd 2×sqrt(Kp×M_eff)其中M_eff是踝关节的等效转动惯量。对于20公斤左右的双足机器人踝关节等效转动惯量大约在0.05到0.1 kg·m²Kp取800时Kd在12到18 Nm·s/rad附近。实际调试时我在Kd上通常再乘以1.2到1.5因为关节摩擦力矩会消耗一部分阻尼效果。还有一个容易忽略的点力矩指令的时序同步。如果驱动器接收到力矩指令后有1到2毫秒的通信延迟而高速状态观测器计算在另一个线程状态和指令不同步控制系统会引入额外相位滞后。我在项目中把所有控制计算放在同一个实时线程保证1kHz的控制周期内状态读取、雅可比计算、力矩指令发送全部完成。4. 实测常见问题与排查技巧实录4.1 高频抖振从滤波到带宽取舍项目实测中遇到最多的就是高频抖振。现象是机器人站立时踝关节发出“嗡嗡”声从示波器看关节力矩信号有十几Hz以上的高频波动。排查路径我总结为三步。第一步看传感器噪声。力矩传感器应变片信号本身带有噪声如果直接进控制环噪声会经过Kd项放大。我在代码里对力矩反馈加了二阶Butterworth低通滤波截止频率设在20Hz左右。滤波不是越低越好截止频率太低会引入相位延迟影响系统稳定性。第二步查机械共振。并联机构的连杆和球铰不是绝对刚体存在结构共振频率。如果控制带宽覆盖了共振频率就会激发出振荡。实测下来2-RSS-1U机构的一阶共振频率一般在50到80Hz附近因此控制带宽不能超过30到40Hz。可以用扫频方法找到共振峰在控制环里加入陷波滤波器。第三步调整控制参数。如果滤波和陷波都做到了还是抖那就是增益过高。降低Kd往往比降低Kp更有效因为Kd放大了速度噪声。我最终把Kd调到了理论临界阻尼值的1.3倍左右刚好在稳定边界以内。4.2 奇异位形附近的控制发散雅可比矩阵接近奇异时J^T的元素会急剧增大。这意味着任务空间一个很小的虚拟力误差映射到关节空间会被放大成很大的力矩指令。我在调试过程中遇到过两次这种情况机器人的踝关节转动到极限位置附近突然一个方向的控制力矩猛增差点把球铰顶坏。处理办法有两个层面。软件层面在控制循环里加入雅可比条件数检测当条件数大于某个阈值时对雅可比矩阵做阻尼最小二乘处理J_safe J^T·(J·J^T λ·I)^{-1}这里的λ是正则化系数实际调试中取0.01到0.1。λ太小起不到平滑作用太大会让力矩输出偏软。从灵巧度和稳定性之间折中我通常取0.03。硬件层面在机械设计时就要避免奇异位形进入正常工作空间。2-RSS-1U机构的奇异位形出现在极限转角处可以在动平台上安装机械限位块确保实际转动范围比理论奇异范围小5到10度。4.3 摩擦、间隙与传感器延迟补偿并联踝关节的摩擦来源主要是球铰和减速器。球铰在低速运动时静摩擦力矩明显容易造成控制死区减速器的摩擦力矩与负载和速度非线性相关。实测发现如果不做补偿踝关节在零速附近的力矩精度非常差静态站立会有缓慢漂移。我用的是摩擦前馈加扰动观测器组合方案。摩擦前馈采用Stribeck模型根据驱动角速度和关节力矩估算摩擦力矩直接加到期望力矩上。扰动观测器则把模型误差、未建模摩擦和外部扰动统一估计出来在控制环内补偿。DOB的Q滤波器和扰动观测带宽需要协调设计带宽太高会引入噪声太低补偿效果差一般取控制系统带宽的1/3到1/2。球铰间隙的问题比较隐蔽。装配时感觉球铰很顺滑但加载后间隙会导致微小的力矩波动与结构柔性耦合后加剧抖振。解决办法是选用带预紧结构的球铰装配时适度预紧既要保证无间隙又要保证运动顺滑。传感器延迟方面足底六维力传感器的采样频率一般在500到1000Hz而关节力矩控制环是1kHz。力传感器信号延迟会造成相位滞后特别是落地冲击瞬间延迟会导致冲击力被错误放大。我在力传感器数据通路里增加了Smith预估补偿同时提高通信频率到1kHz以上。4.4 常见问题速查表现象可能原因排查方向高频抖振力矩噪声、结构共振、Kd过大低通滤波、陷波器、降阻尼低频慢漂重力补偿不准、摩擦未补偿修正质心参数、加摩擦前馈落地时反弹阻抗刚度太高、落地速度过快降低Kp、增加落地下探角特定姿态下猛力接近奇异位形条件数检测、限位单腿站立不稳雅可比符号错误、力矩方向不一致重新验证雅可比正负号5. 硬件选型与工程化落地的几个细节5.1 电机、减速器与关节力感知的选型思路并联踝关节对驱动器的要求是大力矩输出、低背隙、良好的反向驱动性。我用的方案是无框力矩电机加谐波减速器减速比选50左右。这个减速比是个折中减速比太小扭矩密度不够减速比太大反向驱动的阻抗感强力矩控制的柔顺性变差。谐波减速器要重点关注柔轮刚度。柔轮刚度太低会成为腿部结构的柔性环节降低机构固有频率让控制带宽上不去。选型时可以看柔轮扭转刚度指标对于20公斤级机器人柔轮刚度最好不低于1×10^4 Nm/rad。关节力矩感知有两条路线。一个是直接采用关节力矩传感器应变片贴在谐波输出端测量精度高但成本高且增加轴向长度。另一个是用电机电流估算力矩加上摩擦模型补偿成本低但在低速时精度较差。如果预算允许建议至少用足底六维力传感器因为踝关节的地面反作用力是整个双足控制的关键反馈量腿部的所有动力学效应最终都体现在足底力上。5.2 标定装配中的关键细节2-RSS-1U机构对机械精度比较敏感。连杆长度误差超过0.1毫米雅可比矩阵的计算值和实际值就会有明显偏差控制精度自然受影响。装配后我习惯用激光跟踪仪或者关节测量臂标定每个球副中心的实际位置把标定结果写进控制程序的机构参数里。零位标定同样关键。电机编码器在装配时记录的零点不一定对应踝关节的机械零位需要借助专用工装或者IMU确定。步骤是先手动将动平台调整到与定平台平行的位置用水平仪确认然后记录两个电机的编码器读数作为软件零位偏移。球铰预紧力也要仔细调整。预紧太紧摩擦力矩大控制响应迟钝预紧太松间隙造成振荡。我一般先用手指转动球铰感受顺滑度装好后用手推拉动平台确认无轴向间隙再用微力扳手逐步调整预紧量。5.3 控制系统的实时性设计力矩控制对控制周期有硬性要求。我实测下来控制周期超过2毫秒时动态行走的系统稳定裕度明显下降落地瞬间容易发散。因此控制程序必须跑在实时操作系统上或者至少是带实时补丁的Linux系统。任务优先级上力矩环线程最高优先级不能被其他任务打断状态估计和足底力采集线程次之运动规划和人机交互线程优先级最低。所有传感器数据通过共享内存传输避免加锁阻塞。通信总线的选择也值得注意。EtherCAT总线在工业机器人领域很成熟同步抖动可以达到微秒级适合多关节协调控制。如果只用裸单片机控制两个电机CAN总线也够用但要注意报文周期和仲裁延时。个人体会这套2-RSS-1U并联踝关节加力矩控制方案从结构设计到算法落地涉及的环节非常多但每个环节都有一条主线力。机构设计是为了传力雅可比矩阵是为了把任务空间的力映射到驱动空间力矩控制是为了精准地产生力。只要把这条主线想清楚很多问题都迎刃而解。最后再分享一个经验凡是发现控制发散先别急着调PID增益先检查雅可比矩阵的符号、重力补偿的方向和传感器反馈的极性。这三个地方任何一个反了系统都会以非常诡异的方式奔溃。我踩过的最大坑就在雅可比符号上一次是动平台坐标系方向定义不一致一次是旋转轴顺序写反每次都排查了好几天。建议大家在写控制代码之前先用仿真环境把雅可比验证和重力补偿单独测一遍确认无误再上真机能省下大量调试时间。