双足人形机器人建模：SimMechanics物理建模与ARX辨识选型及C语言部署

1. 双足人形机器人建模的两条技术路线从需求到选型双足人形机器人跟轮式、履带式底盘最大的区别在于它本质上是一个不断在“要摔倒”和“快摔倒”之间找平衡的欠驱动系统。你给它一个关节力矩它不会老老实实按你想的方向走而是会通过地面反作用力、质心偏移、角动量变化等一连串耦合效应把运动传递到全身。所以做双足机器人建模第一件事不是急着敲代码而是想清楚我到底要用这个模型干什么如果你只是要做控制器快速原型验证比如调一个关节空间的PID或者计算力矩控制器那模型不需要太复杂能把刚体动力学和接触力算清楚就够了。但如果你要做步态规划、全身动力学控制、或者基于模型的强化学习训练那模型就必须能准确反映质量分布、惯量张量、关节摩擦、地面接触刚度这些细节。这两种需求对应的就是两条完全不同的建模路线一条是基于Simulink/SimMechanics的物理建模另一条是基于ARX辨识或状态空间的数据驱动建模。我见过不少刚入门的同学一上来就想搞“最精确”的模型结果花了两周时间在SimMechanics里搭了一个二十自由度的完整人形跑一次仿真要等三分钟调参调到怀疑人生。也见过另一个极端直接用线性ARX模型去拟合非线性很强的双足行走数据最后控制器在仿真里跑得挺好一上实物就抖得像筛糠。所以选型这件事核心就一句话模型精度要匹配你的控制目标和算力预算不是越精确越好而是越合适越好。下面这张表是我自己在不同项目里总结出来的选型对照你可以直接拿去用对比维度Simulink/SimMechanics物理建模ARX/状态空间数据驱动建模建模周期长需搭建多刚体结构短需采集输入输出数据模型可解释性强每个参数都有物理意义弱参数是拟合结果非线性处理能力强可描述接触、摩擦、柔性弱本质是线性模型实时性差计算量大好差分方程可直接部署适合场景步态规划、动力学分析、高保真仿真关节控制、系统辨识、嵌入式部署对数据依赖低高数据质量决定模型上限这张表不是让你二选一而是让你知道什么时候该用哪条路。实际项目中我经常是先用SimMechanics搭一个“真值”模型用它生成数据再用ARX去辨识一个简化模型最后把简化模型部署到嵌入式控制器里。这就是所谓的“离线高保真、在线低阶次”策略。2. Simulink/SimMechanics物理建模把机器人拆成刚体加关节2.1 为什么选SimMechanics而不是手写动力学方程双足人形机器人的动力学方程用拉格朗日法手推的话一个十二自由度的模型就能让你写到崩溃。每个连杆的动能、势能、耦合项再加上地面接触约束最后得到的方程动辄几百项。我读博的时候试过手推一个六自由度双足模型整整写了四页A4纸结果符号运算还出错了一次导致仿真里机器人走路像在跳机械舞。SimMechanics的好处在于它把刚体、关节、约束、驱动都封装成了模块你只需要在Simulink里拖拖拽拽把机器人的拓扑结构搭出来它自动帮你生成动力学方程。更重要的是它支持参数化建模——你可以把连杆长度、质量、惯量写成变量改一个数字整个模型自动更新。这对于做优化或者参数扫描来说太重要了。但SimMechanics也不是没有代价。它的求解器默认是变步长的对于双足机器人这种有接触突变的系统步长会频繁调整导致仿真速度很慢。我的经验是如果你的模型超过十个自由度一定要把求解器换成定步长的ode4或者ode8步长设在1毫秒左右。虽然精度会稍微降一点但速度能快五到十倍。2.2 从零搭建一个双足模型的完整流程假设你要搭一个简单的五连杆双足模型躯干加两条腿每条腿两个关节下面是具体步骤第一步定义刚体参数。在MATLAB工作空间里先把所有参数写好不要直接在模块里填数字。比如% 刚体参数定义 L_torso 0.4; % 躯干长度(m) M_torso 8.0; % 躯干质量(kg) I_torso diag([0.1, 0.1, 0.05]); % 躯干惯量张量 L_thigh 0.35; % 大腿长度 M_thigh 3.0; % 大腿质量 L_shank 0.35; % 小腿长度 M_shank 2.0; % 小腿质量这样做的原因是后面你要做参数扫描或者系统辨识的时候只需要改这个脚本整个模型自动跟着变。我见过有人直接在SimMechanics模块里填数字后来想改一个质量参数结果找了半小时才找到那个模块。第二步搭建多刚体结构。在Simulink里新建一个SimMechanics模型从库浏览器里拖出World Frame、Mechanism Configuration、刚体模块、转动关节模块。躯干作为基座两条腿通过髋关节转动副连接大腿和小腿之间用膝关节转动副连接。每个关节都要设置好转动轴方向双足机器人一般绕y轴转动侧向绕x轴转动前后向这个要根据你的坐标系定义来。第三步配置接触模型。这是双足建模最容易被忽视也最容易出问题的地方。地面接触不能用简单的刚性约束否则仿真会频繁报错。SimMechanics提供了Spatial Contact Force模块你需要设置接触刚度、阻尼、摩擦系数。我的经验值是接触刚度设在1e5到1e6 N/m之间阻尼设在1e3到1e4 N·s/m之间摩擦系数取0.8到1.0。刚度太高会导致仿真步长被迫减小太低则机器人会“陷”进地面。第四步添加驱动和传感。每个关节加一个Joint Actuator作为力矩输入再加一个Joint Sensor输出角度和角速度。这些信号后面要接到控制器上。第五步设置求解器。在Simulation菜单里选择Model Configuration Parameters把Solver换成ode4Fixed-step size设为0.001。如果你发现仿真还是太慢可以试试ode8步长可以放宽到0.002。2.3 实操心得那些文档里不会写的坑第一个坑是惯量张量的坐标系。SimMechanics里每个刚体的惯量是相对于自己的质心坐标系定义的但你在CAD软件里导出的惯量往往是相对于全局坐标系的。如果你直接复制粘贴模型动起来会像陀螺一样乱转。正确的做法是用平行轴定理转换或者直接在SimMechanics里用CAD导入功能让它自动计算。第二个坑是关节初始位置。双足机器人如果初始时刻两条腿完全竖直膝关节锁死仿真一开始就会报奇异。我的做法是给每个关节一个很小的初始弯曲角比如髋关节5度膝关节10度这样雅可比矩阵不会奇异求解器也能正常启动。第三个坑是接触力的数值震荡。当你看到关节力矩曲线出现高频抖动时八成是接触刚度过高或者步长太大。先试着把接触刚度降一个数量级如果还不行就把步长减半。但步长减半意味着仿真时间翻倍所以这是一个权衡。3. ARX辨识与状态空间模型用数据驱动的方式抓住核心动态3.1 ARX模型到底是什么为什么适合关节控制ARX全称是AutoRegressive with eXogenous input翻译过来就是“带外部输入的自回归模型”。它的数学形式特别简单y(k) a1y(k-1) ... any(k-n) b1u(k-1) ... bmu(k-m) e(k)其中y是输出比如关节角度u是输入比如关节力矩e是白噪声。这个式子说白了就是当前时刻的输出等于过去若干时刻输出的加权和加上过去若干时刻输入的加权和。它本质上是一个线性差分方程跟我们在数字控制里用的Z传递函数是一回事。为什么ARX适合双足机器人的关节控制因为双足机器人的每个关节在局部范围内可以近似看成线性系统。你给髋关节一个力矩它会产生一个加速度积分两次得到角度。虽然全身动力学是非线性的但如果你把其他关节锁死或者只做小范围运动单关节的动态用ARX描述已经足够准确了。而且ARX模型的参数可以直接转成差分方程在C语言里用几行代码就能实现非常适合嵌入式部署。3.2 用MATLAB系统辨识工具箱做ARX辨识的完整步骤第一步采集数据。你需要给关节施加一个激励信号同时记录输入力矩和输出角度。激励信号不能是单一频率的正弦波否则只能激励出一个模态。我一般用Chirp信号频率从0.1Hz扫到10Hz或者伪随机二进制序列PRBS。采样频率至少要是系统带宽的十倍对于双足关节1kHz采样率是比较稳妥的。% 生成Chirp激励信号 fs 1000; % 采样率 t 0:1/fs:10; % 10秒数据 u chirp(t, 0.1, 10, 10); % 0.1Hz到10Hz % 实际采集时u通过DAC输出到电机驱动器 % y通过编码器读回关节角度第二步数据预处理。原始数据里一定有噪声和趋势项。先用detrend去均值再用低通滤波器把高频噪声滤掉。滤波器的截止频率设在你关心的最高频率的1.5倍左右。注意滤波会引入相位延迟如果做闭环控制这个延迟要补偿掉。第三步选择模型阶次。ARX模型的阶次n和m决定了模型复杂度。阶次太低拟合不准太高会过拟合。我的做法是从n2, m2开始试看拟合度Fit percentage能不能到85%以上。如果不够逐步增加到n4, m4。但一般不要超过6阶否则差分方程在嵌入式里跑起来会占用太多计算资源。% ARX辨识 data iddata(y, u, 1/fs); model arx(data, [2 2 1]); % na2, nb2, nk1 compare(data, model); % 查看拟合度第四步模型验证。不要只看拟合度还要看残差是否白噪声。用resid函数分析残差的自相关函数如果残差还有明显相关性说明模型阶次不够或者有未建模动态。第五步转成差分方程。ARX模型辨识出来后用tf函数转成传递函数再用c2d离散化最后提取分子分母系数。这些系数就是C语言里要用的参数。3.3 状态空间模型从差分方程到现代控制ARX模型是输入输出模型适合做PID或者极点配置。但如果你想用LQR、MPC或者观测器就需要状态空间模型。状态空间模型的形式是x(k1) Ax(k) Bu(k) y(k) Cx(k) Du(k)从ARX转状态空间有现成的函数arx2ss但要注意状态变量的物理意义会丢失。如果你想让状态变量对应关节角度和角速度最好直接用物理建模的方式推导状态空间或者用子空间辨识方法如N4SID直接从数据里辨识状态空间模型。我个人的经验是做关节伺服控制用ARX加PID就够了做全身平衡控制才需要状态空间加LQR。不要为了追求“高级”而强行上状态空间那样只会增加调试难度。4. 从模型到代码C语言实现与Simulink代码生成4.1 手写C语言实现ARX差分方程假设你辨识出来的ARX模型是二阶的差分方程长这样y(k) -a1y(k-1) - a2y(k-2) b1u(k-1) b2u(k-2)在C语言里实现只需要一个环形缓冲区#include stdio.h #define ORDER 2 typedef struct { float a[ORDER]; float b[ORDER]; float y_buf[ORDER]; float u_buf[ORDER]; } ARXModel; float arx_update(ARXModel *m, float u) { float y 0.0f; for (int i 0; i ORDER; i) { y - m-a[i] * m-y_buf[i]; y m-b[i] * m-u_buf[i]; } // 更新缓冲区 for (int i ORDER - 1; i 0; i--) { m-y_buf[i] m-y_buf[i-1]; m-u_buf[i] m-u_buf[i-1]; } m-y_buf[0] y; m-u_buf[0] u; return y; }这段代码在STM32F4上跑一次大概只需要几个微秒完全满足1kHz控制频率的要求。注意浮点数运算要用单精度float双精度double在嵌入式上会慢很多。4.2 Simulink代码生成从模型到嵌入式C如果你不想手写代码Simulink的Embedded Coder可以直接把模型生成C代码。步骤是在Simulink里把控制器模型搭好包括ARX差分方程模块、PID模块、信号调理模块。在Configuration Parameters里选择Code Generation系统目标文件选ert.tlc。设置硬件参数比如MCU型号、时钟频率、堆栈大小。点击BuildEmbedded Coder会自动生成.c和.h文件。生成的代码可读性取决于你的模型风格。如果模型里全是MATLAB Function模块生成的代码就是一堆看不懂的数学运算。我的建议是尽量用Simulink原生模块搭建这样生成的代码结构清晰变量名也有意义。注意Embedded Coder生成的代码默认使用浮点运算如果你的MCU没有FPU需要在硬件配置里开启定点化否则代码会非常慢。4.3 PID与ARX构成闭环控制系统的实操方法把ARX模型和PID串起来做闭环有两种思路。第一种是模型参考自适应控制用ARX模型预测下一时刻的输出然后PID根据预测误差调整力矩。第二种是内模控制把ARX模型的逆作为前馈PID只负责补偿模型误差。我推荐第二种因为实现简单且鲁棒性好。具体做法是ARX模型的逆就是交换输入输出把期望角度作为输入算出期望力矩作为前馈然后PID根据实际角度误差微调这个力矩。这样PID的增益可以设得很小系统响应快且超调小。在Simulink里实现的时候注意ARX的逆可能不稳定如果零点在单位圆外这时候要用稳定逆或者加一个低通滤波器。5. 常见问题与排查技巧实录5.1 SimMechanics仿真报错与解决报错信息可能原因解决方法Solver failed at time 0.0初始位置奇异或接触力无穷大给关节加初始弯曲角降低接触刚度Algebraic loop detected接触力与运动形成代数环在接触力模块后加单位延迟Simulation too slow步长太小或模型自由度太多换定步长求解器简化模型Joint exceeds limit关节角度超出机械限位检查关节限位设置加软限位5.2 ARX辨识效果差的排查思路如果你发现ARX模型的拟合度始终上不去按这个顺序排查第一检查数据质量。输入输出数据有没有饱和有没有丢帧信噪比够不够我遇到过编码器分辨率太低导致角度量化噪声太大ARX怎么辨识都只有60%拟合度换了高分辨率编码器后直接到92%。第二检查激励信号。如果激励信号的频率范围没有覆盖系统的带宽高频动态就辨识不出来。用Chirp信号扫频是最稳妥的。第三检查模型阶次。用arxstruc函数遍历不同阶次看哪个阶次的损失函数最小。但不要盲目追求高阶过拟合的模型在闭环里会震荡。第四检查延迟。ARX模型里的nk参数是纯延迟如果实际系统有计算延迟或者通信延迟nk要设对。我见过有人nk设成1实际延迟是3个采样周期结果闭环后系统一直振荡。5.3 C语言部署时的内存与实时性问题嵌入式部署最容易踩的坑是内存对齐和中断优先级。ARX模型里的浮点数组如果不对齐到4字节边界在某些MCU上会触发硬件异常。用__attribute__((aligned(4)))或者编译器选项来保证对齐。实时性方面控制循环一定要放在高优先级中断里而且中断服务程序里不要做浮点除法除法很慢。如果ARX模型里有除法提前算好倒数用乘法代替。还有一个坑是全局变量竞争。如果控制中断和通信中断都要访问ARX模型的状态缓冲区一定要加临界区保护否则数据会错乱。最简单的做法是在控制中断里关中断更新完再开。6. 两条路线的融合离线高保真、在线低阶次实际项目中我很少只用一种建模方法。更常见的做法是用SimMechanics搭建高保真模型作为“数字孪生”用它生成大量数据然后用ARX或状态空间辨识出一个低阶模型最后把低阶模型部署到嵌入式控制器里。这样做的好处是你既有了物理模型的准确性又有了数据驱动模型的实时性。具体流程是先在SimMechanics里跑各种步态和扰动场景记录每个关节的力矩和角度数据。然后用这些数据做ARX辨识得到每个关节的线性模型。最后在Simulink里把线性模型和PID控制器连起来做闭环仿真验证。如果闭环性能满足要求就用Embedded Coder生成代码烧到MCU里。这个流程我用了三年多踩过的最大坑是数据分布不匹配。SimMechanics生成的数据是在理想条件下采集的没有噪声、没有延迟、没有摩擦。直接用这些数据辨识出来的ARX模型在实物上跑会差很多。解决办法是在仿真数据里人为加入噪声和延迟或者干脆在实物上采集一部分数据做混合辨识。另外一个小技巧是ARX模型的参数不要每次上电都重新辨识而是离线辨识好之后固化在Flash里。在线辨识虽然听起来很酷但计算量大且容易发散对于双足机器人这种安全关键系统来说稳定性比自适应性更重要。最后分享一个我在调试双足机器人时总结的“三看”原则一看关节力矩是否平滑二看质心轨迹是否连续三看接触力是否在合理范围。如果这三个都正常那你的模型和控制器基本就没大问题了。如果哪个不对劲就回到对应的建模环节去查别在控制器参数上瞎调。