1. 双足人形机器人建模的路线选择

双足人形机器人跟轮式、履带式底盘最大的区别在于：它本质上是一个不稳定、欠驱动、强耦合的多体系统。你把它推一下，它不会自己恢复，得靠控制器实时算力矩去“扶”住。所以建模这件事，在双足机器人项目里不是走个过场，而是决定后面控制器能不能收敛、仿真能不能跑通、代码能不能落地的地基。

我前后在两个双足项目里分别用了两条路线：一条是基于Simulink/SimMechanics的物理建模，另一条是基于ARX辨识的状态空间建模。这两条路不是对立的，而是分别解决不同阶段的问题——前者解决“我知道机器人长什么样、关节怎么连”，后者解决“我不知道真实系统里那些摩擦、柔性、延迟到底是多少，但我能从数据里把它拟合出来”。这篇文章就把这两条路拆开讲清楚，包括各自的适用场景、参数怎么定、踩过哪些坑，以及最后怎么把模型变成C代码塞进控制器里。

如果你正在做双足机器人的仿真、控制器设计，或者纠结“到底该用物理建模还是数据驱动建模”，这篇内容应该能帮你少走几个月弯路。下面所有内容都是基于我实际项目里的做法整理的，涉及具体参数的地方我会说明计算依据，涉及工具操作的地方我会给出可复现的步骤。

2. 路线一：Simulink/SimMechanics物理建模

2.1 为什么双足机器人首选物理建模

物理建模的核心逻辑是：从机器人的几何、质量、惯量出发，用牛顿-欧拉或拉格朗日方法自动推导动力学方程。SimMechanics（现在叫Simscape Multibody）就是干这个的——你把连杆、关节、质量属性搭进去，它帮你算出M(q)、C(q,q̇)、G(q)这些矩阵。

双足机器人为什么适合这条路？因为它的结构是已知的。你有CAD模型，知道每条腿有几个自由度、每个连杆的质量和质心位置、关节的旋转轴方向。这些信息是确定的，没必要用数据去“猜”。而且双足机器人的动力学方程极其复杂——一个12自由度的双足，展开后的方程能有几十页——手推根本不现实，必须靠工具自动生成。

我第一个项目用的是SimMechanics，当时搭了一个12自由度的双足模型（每条腿6个自由度：髋3、膝1、踝2）。搭完之后最大的感受是：前期投入大，但后期改结构极其方便。比如我想把小腿长度从0.3m改成0.35m，只需要改一个参数，整个动力学模型自动重算，不用重新推导任何方程。

2.2 SimMechanics建模的完整操作流程

先说工具版本。我用的是MATLAB R2021b，Simscape Multibody在Simulink库浏览器里直接能拖出来。如果你用的是更老的版本，可能叫SimMechanics，在Simulink里单独一个库。

第一步：定义世界坐标系和重力方向。在Simscape Multibody里，每个模型都必须有一个World Frame，重力默认是[0 0 -9.80665]。这里有个坑：双足机器人的重力方向一定要确认清楚，我有一次不小心把重力设成了[0 -9.8 0]，结果机器人直接“躺”着走，排查了半天。

第二步：搭建躯干和连杆。每个连杆用一个Solid块表示，需要填三个东西：几何形状（从CAD导入或者用参数化长方体/圆柱）、质量、惯量矩阵。惯量矩阵如果懒得算，可以用Solid块里的“Calculate from Geometry”自动算，但前提是几何形状和密度填对了。我一般是从SolidWorks里导出URDF，然后用smimport命令直接导入，这样质量和惯量都是CAD里算好的，省事。

第三步：配置关节。双足的关节分两类：旋转关节（髋、膝、踝的俯仰）和球关节（髋的偏航/翻滚）。Simscape里对应的块是Revolute Joint和Spherical Joint。每个关节要设两个关键参数：运动范围（比如膝关节0到-120度）和是否带驱动。如果这个关节后面要加电机控制，就选“Actuated”，否则选“Motion”由外部输入角度。

第四步：连接传感器和执行器。每个关节需要输出角度和角速度，用Joint Sensor块；每个驱动关节需要输入力矩，用Joint Actuator块。这里建议把传感器信号统一命名，比如q1、dq1、tau1，后面接控制器的时候不容易乱。

第五步：设置求解器。双足机器人的动力学是刚性的（stiff），因为腿的惯量小、关节刚度大。我一般用ode15s或者ode23t，步长设1e-4到1e-3。如果用ode45，仿真会跑得极慢甚至卡死。实测下来，12自由度的双足模型，ode15s跑10秒仿真大概需要2-3分钟（取决于CPU），可以接受。

2.3 物理建模的参数计算与验证

物理建模最容易出错的地方不是搭建，而是参数对不对。我踩过的坑包括：质心位置填反了、惯量矩阵单位搞错了（kg·mm² vs kg·m²）、关节轴方向搞反了。这些错误在仿真里往往表现为“机器人走路姿势诡异”或者“力矩大得离谱”，很难直接定位。

我的做法是分阶段验证：

1. 静态验证：把机器人吊起来（固定躯干），只让腿自然下垂，看关节角度是否收敛到重力平衡位置。如果收敛不了，说明质量或质心有问题。
2. 单腿验证：固定躯干，给髋关节一个正弦力矩，看腿的摆动频率是否和手算的sqrt(k/I)接近。这一步能验证惯量矩阵。
3. 全身验证：让机器人原地踏步，看地面反力是否合理（总垂直力应该接近体重）。如果差太多，说明质量参数有问题。

注意：SimMechanics里的惯量矩阵是相对于质心坐标系的，不是相对于关节坐标系。如果你从CAD导出的惯量是相对于关节的，需要先用平行轴定理转换。

2.4 从Simulink模型到C代码的落地

物理模型建好之后，最终是要生成C代码跑在控制器上的。Simulink的代码生成流程是：配置模型 → 生成代码 → 集成到目标工程。

配置里几个关键点：

• Solver：代码生成时不能用变步长求解器，必须改成定步长（Fixed-step），步长和实际控制周期一致（比如1ms）。
• System target file：选ert.tlc（Embedded Coder），不要选grt.tlc，后者生成的代码依赖MATLAB运行时。
• Code generation：在Configuration Parameters里勾选“Generate code only”，然后点Build。

生成的代码里，核心是<model_name>.c和<model_name>.h。<model_name>_step()函数就是每个控制周期要调用的。我一般把它包在一个任务里，用定时器触发。

这里有个大坑：Simscape Multibody生成的代码不能直接用于实时控制，因为它包含大量的矩阵运算和求解器调用，计算量太大。实际项目里的做法是：用SimMechanics做离线仿真验证，然后用符号推导或者ARX辨识得到一个简化的状态空间模型，再用那个模型生成控制代码。这就是为什么需要第二条路线。

3. 路线二：ARX辨识与状态空间建模

3.1 什么时候该用ARX辨识

物理建模假设你完全知道系统的结构和参数。但真实的双足机器人里有很多“说不清”的东西：关节摩擦（库仑摩擦+粘滞摩擦）、谐波减速器的柔性、皮带的弹性、传感器的延迟、电机的电流环动态。这些东西在物理模型里很难精确建模，但它们对控制效果影响很大。

ARX（AutoRegressive with eXogenous input）辨识的思路是：我不去管内部结构，我只用输入输出数据拟合一个差分方程。对于双足机器人的单个关节，输入是力矩，输出是角度，ARX模型就是：

y(k) + a1*y(k-1) + ... + an*y(k-n) = b1*u(k-1) + ... + bm*u(k-m) + e(k)

这个方程可以直接转成状态空间形式，然后用来设计控制器。我一般在关节级控制和全身简化模型两个层面用ARX。

3.2 ARX辨识的实操步骤

第一步：采集数据。你需要给关节一个激励信号，同时记录输入（力矩或电流）和输出（角度或角速度）。激励信号的选择很关键：

• 扫频信号：覆盖你关心的频率范围（双足关节一般0.1Hz到10Hz）。
• 伪随机二进制序列（PRBS）：幅值要足够大（能克服摩擦），但不能太大（避免非线性）。
• Chirp信号：从低频扫到高频，适合看频率响应。

我一般用PRBS，幅值设成额定力矩的20%-30%，采样率1kHz，采集10-20秒。数据量大概1-2万点，足够辨识了。

第二步：数据预处理。原始数据里有噪声和漂移，需要：

• 去均值：减去平均值，消除直流偏置。
• 去趋势：用detrend去掉线性漂移。
• 滤波：低通滤波，截止频率设成你关心最高频率的2倍（比如20Hz）。

第三步：模型阶次选择。ARX模型的阶次（na、nb、nk）决定了模型复杂度。阶次太低拟合不准，太高会过拟合。我的经验是：

• 单关节：na=2~4，nb=2~4，nk=1~2。
• 全身简化：na=4~6，nb=4~6，nk=1。

选择方法：用MATLAB的arxstruc或者selstruc，看AIC或FPE指标。我一般会试几组，然后看拟合度（fit percentage）和残差自相关。拟合度到85%以上就够用了。

第四步：模型验证。用一组没参与辨识的数据来验证。看两个东西：

• 拟合度：compare函数，看模型输出和实际输出的吻合程度。
• 残差分析：resid函数，看残差是否白噪声。如果残差有规律，说明模型结构不对。

3.3 从ARX到状态空间：转换与离散化

ARX模型是差分方程，要用来设计控制器（比如LQR、MPC），需要转成状态空间形式。MATLAB里用idss或者ss命令：

% 假设arx_model是辨识得到的ARX模型 sys_ss = ss(arx_model); % 转成状态空间 sys_d = c2d(sys_ss, 0.001); % 离散化，采样周期1ms

离散状态空间的形式是：

x(k+1) = A*x(k) + B*u(k) y(k) = C*x(k) + D*u(k)

这里的A、B、C、D就是后面控制器设计的基础。我一般会把A、B、C、D导出成C数组，直接嵌到控制代码里。

注意：ARX辨识得到的模型是离散的，而物理建模得到的是连续的。如果你要把两者结合（比如用物理模型做前馈、ARX做反馈），需要统一离散化。

3.4 ARX模型与PID构成闭环控制

这是热词里提到的一个典型场景：用ARX辨识模型和PID构成闭环控制系统。具体做法是：

1. 用ARX辨识得到关节的离散模型G(z)。
2. 设计PID控制器C(z)。
3. 闭环系统是G(z)*C(z)/(1+G(z)*C(z))。
4. 在Simulink里搭出来，验证阶跃响应、抗扰能力。

我实际项目里的做法是：用ARX模型在Simulink里做闭环仿真，调好PID参数，再把PID参数和ARX模型一起生成C代码。这样比直接在真机上调PID安全得多，也不会把机器人摔了。

PID参数整定的经验：双足关节的PID一般是位置环外环+速度环内环。位置环Kp=20~50，Ki=0.1~1，Kd=0.5~2；速度环Kp=1~5，Ki=0.01~0.1。具体值取决于关节的惯量和摩擦。我一般先用ARX模型仿真调，然后真机上微调。

4. 两条路线的对比与融合

4.1 物理建模 vs ARX辨识：怎么选

我的建议是：两个都用。物理模型用来做全身动力学仿真和控制器架构验证，ARX模型用来做关节级控制器设计和代码生成。两者结合的方式是：物理模型提供参考轨迹和期望力矩，ARX模型提供实际关节的响应特性，两者在Simulink里联合仿真。

4.2 联合仿真的配置方法

Simulink里联合仿真的做法：

1. 物理模型（SimMechanics）作为被控对象，输出关节角度。
2. ARX模型作为关节动态补偿，输入期望力矩，输出实际角度。
3. 控制器（PID或LQR）根据误差计算力矩。
4. 三者连成闭环，仿真验证。

这里有个技巧：物理模型和ARX模型的采样率要一致。我一般统一用1kHz，物理模型用ode15s变步长求解，但输出用Zero-Order Hold降到1kHz，和ARX模型对齐。

4.3 从模型到C代码的完整链路

最终落地的链路是：

物理模型（SimMechanics）→ 离线仿真验证控制器架构 ARX模型（辨识）→ 状态空间 → 离散化 → C代码 控制器（PID/LQR）→ 参数整定 → C代码 两者集成 → 嵌入式目标 → 实时控制

C代码生成时，ARX模型转成的状态空间可以直接写成：

// 离散状态空间更新 void update_state(float u) { float x_new[4]; x_new[0] = A[0][0]*x[0] + A[0][1]*x[1] + ... + B[0]*u; // ... x[0] = x_new[0]; // ... y = C[0]*x[0] + C[1]*x[1] + ... + D*u; }

这段代码计算量极小，1kHz下跑在STM32F4上毫无压力。

5. 常见问题与排查技巧实录

5.1 SimMechanics模型报错排查

问题1：仿真报“singular matrix”或者“solver failure”。原因：通常是某个关节的初始条件不对，或者质量参数导致惯量矩阵奇异。排查方法：把模型拆成单腿，逐步加关节，看哪一步出错。我遇到过一次是踝关节的惯量设成了0，导致矩阵奇异。

问题2：仿真跑得极慢。原因：求解器选择不对，或者模型里有代数环。排查方法：换ode15s，检查是否有直接馈通（Direct Feedthrough）。如果有代数环，加一个Unit Delay或者Memory块。

问题3：机器人走路时“抖”。原因：通常是关节刚度太大或者求解器步长太大。排查方法：减小步长到1e-5，或者给关节加一点阻尼。

5.2 ARX辨识的坑

问题1：辨识出来的模型拟合度很高，但闭环控制不稳定。原因：辨识数据是在开环下采集的，闭环下系统特性变了。解决方法：用闭环辨识方法，或者在辨识数据里加入闭环工况的数据。

问题2：模型阶次选高了，过拟合。表现：在辨识数据上拟合度95%，在新数据上只有60%。解决方法：用交叉验证，选阶次时看新数据的拟合度。

问题3：ARX模型转状态空间后，A矩阵有接近1的特征值。原因：系统有积分环节或者接近积分。解决方法：检查数据是否去趋势了，或者改用ARMAX模型。

5.3 代码生成与集成的坑

问题1：生成的代码编译报错“undefined reference”。原因：Simulink生成的代码依赖一些库文件（比如rtwtypes.h、simstruc.h）。解决方法：把MATLAB安装目录下的rtw/c/src和rtw/c/libsrc加到编译器的include路径里。

问题2：代码跑在目标板上，控制周期抖动大。原因：代码里有浮点运算或者除法。解决方法：把浮点转成定点，或者用查表代替除法。我一般会把状态空间矩阵转成Q15格式，用定点运算。

问题3：ARX模型和物理模型对不上。原因：两者的采样率、初始条件、输入输出定义不一致。解决方法：统一采样率，统一初始条件，检查输入输出的物理量是否一致（比如都是弧度还是度）。

5.4 常见问题速查表

6. 实操心得与参数整定经验

6.1 物理建模的参数整定

物理建模里最影响仿真效果的是关节摩擦。SimMechanics里可以用Joint Stiction Actuator或者简单的Coulomb+Viscous摩擦模型。我的经验是：

• 库仑摩擦：设成额定力矩的5%-10%。
• 粘滞摩擦：设成0.01-0.1 N·m·s/rad。
• 静摩擦：比库仑摩擦大20%-30%。

这些参数不用很准，因为后面ARX辨识会补偿。但如果不设，仿真里的关节会“太滑”，控制器参数会调得偏激进，真机上会振荡。

6.2 ARX辨识的数据采集技巧

采集数据时，一定要记录关节温度。因为摩擦随温度变化很大，冷态和热态的辨识结果能差20%。我一般会在关节跑热之后（跑5-10分钟）再采集数据，这样辨识出来的模型更接近实际工作状态。

另外，激励信号的幅值要覆盖实际工作范围。如果实际关节力矩是±10N·m，激励信号就要覆盖±10N·m。如果只激励±2N·m，辨识出来的模型在大力矩下会不准。

6.3 代码生成的优化技巧

Simulink生成的代码默认是双精度浮点，在嵌入式上跑很慢。我的做法是：

1. 在Configuration Parameters里把数据类型改成single。
2. 如果目标板没有FPU，转成定点（Fixed-Point Designer）。
3. 把状态空间矩阵转成const数组，放在Flash里，不要放RAM。

这样优化之后，12自由度的双足控制代码可以跑到1kHz，CPU占用率不到30%（STM32F407）。

6.4 联合仿真的调试技巧

联合仿真时，先单独调物理模型，再单独调ARX模型，最后联合。我见过很多人一上来就联合仿真，结果出了问题不知道是物理模型的问题还是ARX模型的问题。

另外，联合仿真时给ARX模型加一个开关，可以随时切换“用物理模型”还是“用ARX模型”。这样调试的时候可以快速定位问题。

7. 后续扩展方向

这套建模方法不仅适用于双足机器人，也可以扩展到外骨骼、机械臂、四足机器人。核心思路是一样的：物理建模提供结构，ARX辨识提供参数，两者结合生成代码。

后续可以做的扩展包括：

• 在线辨识：用递推最小二乘法（RLS）在线更新ARX参数，适应负载变化。
• 非线性ARX：用神经网络或者高斯过程扩展ARX，处理非线性强的关节。
• 多关节耦合辨识：把全身关节的输入输出一起辨识，得到一个MIMO状态空间模型。
• 自动代码生成流水线：用MATLAB脚本自动化“辨识→转状态空间→生成代码→编译”的流程。

我个人在实际操作中的体会是：物理建模和ARX辨识不是二选一，而是互补的。物理建模帮你理解系统，ARX辨识帮你拟合现实。两者结合，才能既保证控制器的理论正确性，又保证实际落地的鲁棒性。踩过几次坑之后，我现在做新项目都是先搭物理模型验证架构，再用ARX辨识调参数，最后生成代码上真机，这套流程下来基本不会出大问题。