1. 双足机器人建模的路线选择:为什么会有两种方法
双足人形机器人这个领域,建模这件事从来都不是“选个工具就完事”的简单问题。我最早接触双足建模是在做一款12自由度小型人形机器人的控制算法验证,当时摆在我面前的有两条路:一条是用Simulink/SimMechanics搭物理模型,另一条是用ARX辨识加状态空间模型做数据驱动建模。两条路我都走过,也都踩过坑,所以这篇文章我想把这两种方法掰开揉碎讲清楚,包括它们各自适合什么场景、具体怎么落地、参数怎么调、坑在哪里。
先说清楚这两种方法到底在解决什么问题。双足人形机器人本质上是一个多刚体欠驱动系统,它的动力学方程可以写成标准的机器人动力学形式:
M(q)q̈ + C(q,q̇)q̇ + G(q) = τ + JᵀF
其中q是关节角度向量,M是惯性矩阵,C是科氏力和离心力项,G是重力项,τ是关节驱动力矩,JᵀF是接触力映射到关节空间的等效力。这个方程看起来简洁,但实际写出来对于12自由度以上的双足系统,每一项都是高度非线性的耦合项。你想直接手推解析式?理论上可行,实际上你会被几十页的偏微分方程淹没。
所以工程上就分化出了两条路线。第一条是基于物理机理的建模,用Simulink/SimMechanics这类工具,把机器人的连杆、关节、质量、惯量、接触模型都搭出来,让仿真引擎去自动计算动力学。第二条是基于数据驱动的建模,用ARX(AutoRegressive with eXogenous input)模型对输入输出数据进行辨识,得到一个差分方程形式的模型,再转成状态空间形式用于控制器设计。
这两条路不是对立的,很多时候是互补的。但如果你刚开始做双足机器人项目,资源有限、时间有限,你必须先选一条路走通。下面我分别展开。
1.1 物理机理建模的核心逻辑与适用边界
SimMechanics(现在叫Simscape Multibody)的思路很直观:你把机器人当成一堆刚体,每个刚体有质量、惯量、坐标系,刚体之间用关节连接,关节有自由度类型和约束。你不需要手写动力学方程,工具会自动用拉格朗日方程或者递归牛顿-欧拉法算出运动方程。
为什么双足机器人特别适合用这种方式?因为双足系统的拓扑结构是树状的,从躯干出发分出两条腿,每条腿又有髋、膝、踝三个主要关节。SimMechanics对这种树状多体系统的处理非常成熟,你只需要把DH参数或者URDF模型导入,设置好每个连杆的质量和惯量,它就能自动生成动力学模型。
但这里有个关键问题:接触建模。双足机器人和机械臂最大的区别在于,它的动力学行为高度依赖于脚和地面的接触状态。接触模型选得不对,仿真出来的步态完全不是那么回事。我试过用最简单的刚性接触,结果仿真步长稍微大一点就数值爆炸;后来换成弹簧-阻尼接触模型,参数调了整整两天才让机器人能稳定站立。
物理机理建模的适用边界很清晰:当你需要高保真的动力学仿真、需要分析接触力、需要做模型预测控制(MPC)的预测模型时,这条路是首选。但它的代价是建模周期长、参数标定麻烦、对接触参数敏感。
1.2 数据驱动建模的核心逻辑与适用边界
ARX模型走的是完全不同的路子。它不关心机器人内部长什么样,只关心输入输出数据之间的关系。你给机器人一组关节力矩序列,同时记录关节角度序列,然后用ARX辨识算法拟合出一个差分方程:
y(k) + a₁y(k-1) + ... + aₙy(k-n) = b₁u(k-1) + ... + bₘu(k-m) + e(k)
这个方程的意思是:当前时刻的输出(关节角度)可以用过去n个时刻的输出和过去m个时刻的输入(关节力矩)来线性组合近似。辨识的任务就是找到系数a₁...aₙ和b₁...bₘ。
为什么有人会用这种方法?因为双足机器人的精确动力学模型太难拿了。即使用SimMechanics搭出来,仿真和实物之间也有差距——摩擦、柔性、间隙、传感器噪声,这些在物理模型里很难精确建模。ARX辨识直接用实物数据,某种程度上把这些“未建模动态”都吸收进了系数里。
但ARX的局限也很明显:它是线性模型,而双足机器人本质上是非线性的。所以ARX通常只在工作点附近有效,比如机器人站立平衡时的小幅摆动,或者步行周期中某个特定相位的局部模型。你不能指望一个ARX模型能描述机器人从站立到行走的整个过渡过程。
1.3 两种方法的对比与选型建议
| 对比维度 | SimMechanics物理建模 | ARX数据驱动建模 |
|---|---|---|
| 建模周期 | 长(数天到数周) | 短(数小时到数天) |
| 模型精度 | 依赖参数标定,接触模型敏感 | 依赖数据质量,局部精度高 |
| 非线性处理 | 天然支持 | 不支持,需分段线性化 |
| 可解释性 | 强,每个参数有物理意义 | 弱,系数是黑箱 |
| 控制器设计 | 适合MPC、计算力矩控制 | 适合PID、极点配置 |
| 实物迁移 | 需要参数辨识 | 直接来自实物数据 |
| 计算负担 | 仿真慢,实时控制需简化 | 模型简单,实时性好 |
我的建议是:如果你是从零开始做双足机器人控制,先用SimMechanics搭一个物理模型,把基本的站立平衡和步行仿真跑通,理解机器人的动力学特性。然后在这个基础上,用ARX对实物或者高保真仿真数据做辨识,得到一个简化的线性模型用于实时控制器设计。两条路都走一遍,你对双足机器人的理解会完全不一样。
2. SimMechanics物理建模的完整实操流程
这一章我详细讲怎么用SimMechanics搭一个双足机器人的物理模型。我以12自由度(每条腿6个自由度:髋3、膝1、踝2)的人形机器人为例,这个配置比较典型,既有足够的复杂度,又不至于让建模工作量爆炸。
2.1 从URDF到SimMechanics模型导入
如果你已经有机器人的URDF文件,导入是最省事的。Simscape Multibody提供了smimport函数,可以直接把URDF转成Simulink模型。但这里有几个坑我必须要说。
第一个坑是坐标系定义。URDF里每个link的坐标系原点和SimMechanics的默认坐标系不一定一致,导入后经常发现关节旋转轴方向反了。我的做法是导入后先不急着加控制器,而是手动拖拽每个关节,确认旋转方向符合预期。这一步花10分钟,能省后面10小时的调试。
第二个坑是惯量参数。URDF里的惯量矩阵如果是用CAD软件导出的,通常是相对于link质心坐标系的。但SimMechanics里刚体的惯量默认是相对于刚体坐标系原点的。如果你直接填进去,惯量会错。正确的做法是用平行轴定理转换:
I_origin = I_cm + m * (dᵀd * I₃ - d * dᵀ)
其中d是质心到原点的位移向量,m是质量。这个转换SimMechanics有内置的inertia模块可以自动处理,但你得确保输入的质心位置是对的。
第三个坑是关节限位和摩擦。URDF里通常只定义了关节类型和轴向,没有摩擦和限位。但双足机器人的关节摩擦对步态影响很大,尤其是膝关节和踝关节。我一般会在关节模块里加一个粘性摩擦项,系数先设一个经验值(比如0.01 N·m·s/rad),后面再根据实物数据标定。
导入完成后,你的Simulink模型里应该有一堆刚体模块和关节模块,用线连起来形成一个树状结构。这时候先别急着加驱动,先做一个自由落体测试:把机器人从空中释放,看它能不能正常下落和弹跳。如果数值爆炸或者穿模,说明接触参数或者求解器设置有问题。
2.2 接触模型的选择与参数整定
接触模型是双足仿真里最让人头疼的部分。Simscape Multibody提供了几种接触力模型,我用下来最稳定的是弹簧-阻尼模型,也叫惩罚函数法。它的基本形式是:
F_contact = k * δ + c * δ̇
其中δ是穿透深度,δ̇是穿透速度,k是接触刚度,c是阻尼系数。当δ>0时表示有接触,δ≤0时接触力为零。
k和c怎么选?这里有个经验公式。假设机器人单腿支撑时承受的力大约是体重的1.5倍,对于10kg的机器人就是约150N。如果允许的最大穿透深度是1mm,那么k ≈ 150 / 0.001 = 1.5×10⁵ N/m。阻尼系数一般取临界阻尼的0.1到0.5倍:
c = 2 * ζ * sqrt(k * m_eff)
其中m_eff是有效质量,ζ是阻尼比。对于10kg的机器人,m_eff取5kg,ζ取0.3,算出来c ≈ 2 * 0.3 * sqrt(1.5e5 * 5) ≈ 520 N·s/m。
这些参数不是拍脑袋定的,但也不是一次就能调好。我的做法是先按上面的公式算一组初值,然后做单腿站立测试:让机器人一条腿固定在地面上,另一条腿悬空,看它能不能稳定。如果振荡厉害,增大阻尼;如果穿透太深,增大刚度。一般调个五六轮就能找到比较合适的值。
注意:接触刚度不是越大越好。k太大,求解器需要很小的步长才能稳定,仿真速度会慢到无法接受。k太小,机器人会“陷”进地面,步态失真。我一般把k控制在1e5到1e6之间,配合变步长求解器(ode23t或ode15s)。
2.3 驱动器建模与关节控制回路
物理模型搭好后,下一步是加驱动器。双足机器人的关节通常用电机驱动,电机的力矩输出可以简化为:
τ = K_t * i
其中K_t是力矩常数,i是电流。如果你要做电流环控制,还需要加电机的电气方程:
L * di/dt + R * i = V - K_e * ω
其中L是电感,R是电阻,K_e是反电动势常数,V是电压,ω是角速度。对于小型机器人,电气时间常数通常远小于机械时间常数,所以可以忽略电感,简化为:
i = (V - K_e * ω) / R
在Simulink里,我一般用Simscape Electrical里的电机模块,或者直接用Gain模块搭一个简化模型。关键是力矩限幅:真实电机的力矩不可能无限大,必须加饱和模块。我一般把最大力矩设为额定力矩的2到3倍,模拟短时过载能力。
关节控制回路我习惯用计算力矩控制(Computed Torque Control):
τ = M(q) * (q̈_d + K_d * (q̇_d - q̇) + K_p * (q_d - q)) + C(q,q̇)q̇ + G(q)
其中q_d是期望关节角度,K_p和K_d是PD增益。这个控制律的好处是它把非线性动力学补偿掉了,剩下的闭环系统是一个线性二阶系统,K_p和K_d可以按极点配置来选。比如我希望闭环带宽是10Hz,阻尼比是0.9,那么:
K_p = ω_n² = (2π*10)² ≈ 3948K_d = 2 * ζ * ω_n = 2 * 0.9 * 62.8 ≈ 113
这些增益在Simulink里直接填到Gain模块就行。但要注意,计算力矩控制需要实时计算M、C、G,这在Simulink里可以用MATLAB Function模块调用massMatrix、velocityProduct、gravityTorque等函数实现。
2.4 仿真求解器配置与性能优化
SimMechanics模型跑起来后,求解器的选择直接影响仿真速度和稳定性。我试过ode45、ode15s、ode23t,最后发现ode23t最适合双足机器人。它是梯形法则的变步长求解器,对刚性系统比较友好,而且不会像ode15s那样引入太多数值阻尼。
步长设置也很关键。最大步长我一般设为1ms,最小步长自动。相对容差设1e-4,绝对容差设1e-6。如果仿真还是慢,可以试试局部求解器:把接触力计算部分单独用一个固定步长的小求解器,其他部分用变步长。Simscape Multibody支持这种配置,在Solver Configuration模块里可以设置。
还有一个提速技巧是简化碰撞几何。SimMechanics默认用精确的CAD几何做碰撞检测,计算量很大。你可以把碰撞形状换成简单的球体或长方体,只要包络范围差不多就行。我一般把脚底换成扁长方体,小腿和大腿换成圆柱体,碰撞检测速度能快好几倍。
3. ARX辨识与状态空间模型的落地细节
物理模型跑通后,你会发现一个问题:仿真和实物总有差距。这时候ARX辨识就派上用场了。这一章我讲怎么从数据出发,辨识出一个可用于控制器设计的线性模型。
3.1 数据采集与预处理的关键要点
ARX辨识的第一步是采数据。数据质量直接决定模型质量,这一步不能偷懒。
激励信号设计:你不能随便给机器人加个阶跃就完事。双足机器人的关节在工作点附近需要持续激励,才能把模型的动态特性激发出来。我一般用扫频信号或者伪随机二进制序列(PRBS)。扫频信号从0.1Hz扫到20Hz,幅值根据关节允许的运动范围来定,一般取±5°到±10°。PRBS的幅值类似,但频率成分更丰富,辨识效果通常更好。
采样率选择:根据香农定理,采样率至少是信号最高频率的2倍。但实际中我一般取10倍以上。双足机器人的关节控制带宽通常在10到20Hz,所以采样率取1kHz比较稳妥。如果数据量太大,可以先降采样到500Hz,但不能再低了。
数据预处理:原始数据里一定有噪声和漂移。我一般做三步处理。第一步是去均值,把每个通道的直流分量减掉,因为ARX模型描述的是动态关系,不含静态工作点。第二步是低通滤波,用Butterworth滤波器,截止频率设在控制带宽的2到3倍,比如50Hz。第三步是去除异常值,用3σ准则或者中值滤波,把明显的尖峰去掉。
注意:滤波会引入相位滞后,如果你用滤波后的数据做辨识,得到的模型也会带这个滞后。所以滤波器的截止频率不能设太低,否则模型相位会失真。我一般用零相位滤波(
filtfilt函数),避免相位偏移。
3.2 ARX模型阶次选择与辨识实操
ARX模型的阶次n和m怎么选?这是辨识里最核心的问题。阶次太低,模型欠拟合,描述不了机器人的动态;阶次太高,模型过拟合,对新数据的预测能力反而下降。
我的做法是从低阶开始试,逐步增加,看拟合度和残差。具体来说,先试n=2, m=2,算一下拟合度(compare函数给出的fit percentage)。如果拟合度低于80%,增加到n=3, m=3,再试。一般双足机器人的关节模型,n=3到5,m=2到4就能达到90%以上的拟合度。
MATLAB的System Identification Toolbox提供了arx函数,一行代码就能辨识:
% data是iddata对象,包含输入u和输出y model = arx(data, [n m 1]);其中[n m 1]里的1是输入到输出的纯延迟。双足机器人的关节从力矩输入到角度输出,纯延迟通常很小,取1个采样周期就行。
辨识完成后,一定要做残差分析。残差应该接近白噪声,如果残差还有明显的自相关,说明模型阶次不够或者有未建模动态。用resid函数可以画出残差的自相关函数和互相关函数,如果都在置信区间内,说明模型是充分的。
3.3 从ARX差分方程到状态空间模型的转换
ARX模型是差分方程形式,但控制器设计通常用状态空间形式。转换方法很简单,MATLAB的idss函数可以直接把ARX模型转成状态空间:
ss_model = idss(model);转换后的状态空间模型是离散的,形式为:
x(k+1) = A * x(k) + B * u(k)y(k) = C * x(k) + D * u(k)
其中A、B、C、D矩阵的维度取决于ARX的阶次。对于n=3, m=2的单输入单输出系统,A是3×3,B是3×1,C是1×3,D是1×1。
这个状态空间模型可以直接用于LQR或者极点配置控制器设计。比如你要设计一个状态反馈控制器u = -Kx,使得闭环极点落在单位圆内,可以用dlqr函数:
K = dlqr(A, B, Q, R);Q和R是权重矩阵,Q越大表示越看重状态收敛速度,R越大表示越看重控制能量。我一般先设Q=I,R=1,然后根据仿真效果调整。
3.4 与PID控制构成闭环的实操方法
很多实际项目里,工程师更习惯用PID而不是状态反馈。ARX模型和PID结合的方法是这样的:先用ARX模型在Simulink里搭一个被控对象,然后调PID参数,调好后直接把PID参数移植到实物上。
具体操作:在Simulink里用Discrete State-Space模块放ARX转成的状态空间模型,输入是力矩,输出是角度。然后加一个PID Controller模块,形成闭环。调参的时候,先用Ziegler-Nichols方法找一组初值,然后手动微调。
我试过的一个典型参数:对于膝关节角度控制,Kp=80, Ki=5, Kd=2,采样率1kHz。这个参数在ARX模型上仿真效果很好,移植到实物上也能稳定,但需要根据实际摩擦和间隙做小幅调整。
注意:ARX模型是线性的,只在工作点附近有效。如果你的PID输出力矩很大,机器人偏离了工作点,模型就不准了。所以PID的输出一定要限幅,或者加一个前馈项补偿重力。
4. 两种建模方法的联合使用与场景适配
单独用SimMechanics或者单独用ARX都有局限,实际项目中我更多是把两者结合起来。这一章讲几种典型的联合使用模式。
4.1 用SimMechanics生成数据训练ARX模型
这是最直接的联合方式。物理模型搭好后,你不用急着上实物,先在SimMechanics里做仿真,采集输入输出数据,然后用这些数据辨识ARX模型。这样做的好处是数据干净、可控,你可以任意设计激励信号,不用担心机器人摔坏。
具体流程:在SimMechanics模型里加一个激励信号源(扫频或PRBS),同时用To Workspace模块记录关节力矩和角度。仿真跑完后,把数据导入System Identification Toolbox,按第3章的方法辨识ARX模型。辨识出来的模型可以替代SimMechanics模型用于实时控制器设计,因为ARX模型计算量小得多。
我做过一个对比:同一个双足机器人,SimMechanics模型仿真1秒需要约2秒计算时间(变步长),而ARX状态空间模型仿真1秒只需要0.01秒。对于需要迭代优化的控制器设计(比如MPC),ARX模型的速度优势非常明显。
4.2 用ARX模型校正SimMechanics参数
反过来,你也可以用实物数据辨识出的ARX模型来校正SimMechanics模型的参数。具体做法是:在实物上做同样的激励实验,采集数据,辨识ARX模型。然后在SimMechanics里做同样的激励,也辨识一个ARX模型。对比两个ARX模型的频率响应,如果某个频段差异大,说明SimMechanics模型在那个频段的参数不对,可能是摩擦、惯量或者接触刚度的问题。
我一般重点看幅频特性的谐振峰。如果实物数据的谐振峰比仿真数据低,说明实际系统的阻尼比仿真大,可能是关节摩擦没建模准确。这时候就回去调SimMechanics里的摩擦系数,直到两个模型的频率响应吻合。
4.3 不同开发阶段的建模策略选择
项目不同阶段,建模策略应该不一样。我总结了一个简单的决策表:
| 开发阶段 | 主要目标 | 推荐建模方法 | 理由 |
|---|---|---|---|
| 概念验证 | 验证步态可行性 | SimMechanics | 不需要实物,快速迭代 |
| 控制器初调 | 整定PID或LQR | ARX+状态空间 | 模型简单,实时性好 |
| 实物调试 | 缩小仿真实物差距 | 两者联合 | 互相校正 |
| 性能优化 | 提高步态精度 | SimMechanics+MPC | 需要高保真预测模型 |
| 嵌入式部署 | 生成C代码 | ARX状态空间 | 计算量小,易部署 |
这个表不是绝对的,但大体上反映了我的经验。刚开始做双足机器人,别一上来就追求高保真,先用简单模型把控制回路跑通,再逐步加复杂度。
5. 常见问题与排查技巧实录
这一章我整理一些实际项目中遇到的问题和解决方法,都是踩过坑之后总结出来的。
5.1 SimMechanics仿真崩溃与数值不稳定排查
问题现象:仿真跑到一半报错“Solver encountered an error”或者“Simulation stopped because of a singularity”。
排查思路:首先看是不是接触力太大导致穿透。把接触刚度k调小一个数量级试试。如果还不行,检查关节限位是不是设得太紧,导致求解器在边界附近振荡。再不行,换求解器,从ode45换到ode23t或者ode15s。
我的经验:90%的仿真崩溃都是接触参数问题。接触刚度和阻尼的比值要合理,我一般保持c ≈ 0.01 * sqrt(k),这个比例下数值稳定性最好。
5.2 ARX辨识模型精度不够的优化手段
问题现象:辨识出来的模型拟合度只有60%到70%,用于控制时效果很差。
排查思路:先看数据质量。把输入输出数据画出来,看有没有明显的噪声或者漂移。如果有,重新做滤波。然后看阶次是不是太低,逐步增加n和m。如果还不行,可能是系统有非线性,ARX搞不定,需要考虑分段辨识或者用非线性ARX(NARX)。
我的经验:数据预处理比模型阶次重要得多。我遇到过好几次,花半天调阶次,拟合度只提高5%,但重新做一次零相位滤波,拟合度直接提高20%。
5.3 从仿真到实物的迁移误差来源分析
问题现象:Simulink里跑得好好的控制器,放到实物上就振荡或者响应慢。
排查思路:按这个顺序查——传感器噪声、执行器延迟、摩擦、柔性、通信延迟。传感器噪声会让微分项放大,执行器延迟会让相位裕度下降,摩擦会导致低速爬行,柔性会导致高频谐振,通信延迟会导致采样不同步。
我的经验:迁移误差里最大的一项是摩擦。仿真里摩擦模型通常很简单,实物摩擦复杂得多。我的做法是在实物上做低速扫频实验,辨识出摩擦模型,然后加到SimMechanics里。这一步做完,仿真和实物的差距能缩小一半以上。
5.4 常见问题速查表
| 问题 | 可能原因 | 解决方法 |
|---|---|---|
| 仿真数值爆炸 | 接触刚度过大 | 减小k,增大c |
| 模型拟合度低 | 数据噪声大 | 零相位滤波,去均值 |
| 实物振荡 | 微分增益过大 | 减小Kd,加低通滤波 |
| 实物响应慢 | 积分增益过小 | 增大Ki,检查摩擦补偿 |
| 状态空间模型不稳定 | 辨识阶次过高 | 降低阶次,做残差分析 |
| C代码生成失败 | 使用了不支持的功能 | 检查Simulink Coder兼容性 |
5.5 嵌入式部署与C代码生成的注意事项
如果你要把模型部署到嵌入式控制器上,Simulink Coder可以直接生成C代码。但有几个坑要注意。
第一,浮点运算。SimMechanics模型通常用双精度浮点,但嵌入式平台可能只有单精度或者定点。生成代码前要把数据类型改成single或者fixdt,然后重新验证精度。
第二,代数环。Simulink模型里如果有代数环,生成代码会报错。解决方法是在代数环里加一个单位延迟或者Memory模块。
第三,代码效率。自动生成的C代码通常比较冗长,执行效率不高。如果控制器对实时性要求高,建议手写核心算法,只用Simulink生成外围代码。
注意:生成C代码前一定要做软件在环测试(SIL),把生成的代码在PC上跑一遍,和Simulink仿真结果对比。如果差异大,说明数据类型或者求解器设置有问题。
6. 我个人在实际操作中的体会
双足机器人建模这件事,我最大的体会是:没有一种方法能包打天下。SimMechanics适合理解物理本质,ARX适合快速控制器迭代,两者结合才能发挥最大价值。
另外,别在建模上追求完美。我见过太多项目,花几个月搭了一个极其精细的SimMechanics模型,结果控制器设计还是用PID,模型精度根本用不上。正确的做法是按需建模:控制器需要什么精度,就建什么精度的模型。
最后分享一个小技巧:如果你用SimMechanics和ARX联合建模,建议把两个模型放在同一个Simulink文件里,用Manual Switch切换。这样你可以实时对比两个模型的输出,快速发现差异。我一般会在关键关节上加一个Scope,同时显示物理模型和ARX模型的输出,调试效率能提高不少。
这个内容后续还可以这样扩展:把ARX换成NARX(非线性ARX),用神经网络或者高斯过程做辨识,看看能不能在保持实时性的同时提高模型精度。我试过用NARX对踝关节建模,拟合度能到95%以上,但计算量比ARX大一个数量级,需要权衡。