机械臂动力学参数辨识仿真全流程：建模、激励轨迹与参数估计

机械臂动力学参数辨识仿真：从建模、激励轨迹到参数估计的实操记录

干过机械臂力控或者拖动示教的朋友，应该都有过这种经历：同样一台六轴臂，做位置控制时跑得挺好，一旦切换力控或者要在低速下做前馈补偿，关节力矩就莫名其妙地飘，重负载时更明显。位置环粗调还能压住误差，但力控场景里模型不准几乎等于没法用。这个问题说到底，就是动力学参数辨识没做扎实。

我这次做的是机械臂动力学参数辨识仿真：在仿真环境里完整跑了一遍从刚体动力学建模、回归矩阵提取、激励轨迹优化、数据采集、参数估计到力矩预测验证的全流程，目标是让辨识出的参数能精确预测关节力矩，为后续力控、拖动示教、负载辨识打基础。仿真环境的优势在于所有真实动力学参数已知，辨识流程哪个环节出问题能立刻暴露出来，非常适合先验证方法论再迁移到真机。适合正在做机械臂控制、想入门动力学辨识、或者已经在仿真里搭过ROS机械臂但还没碰过辨识环节的朋友参考。

这套流程跑通后，你会有几个比较直接的感觉：第一，辨识不只是最小二乘解个方程那么简单，激励轨迹和数据处理比参数求解本身更容易出问题；第二，基参数集（base parameter set）才是真正能辨识出来的量，完整惯性参数里有些项在数学上不可辨识或线性相关，硬辨识只会让结果一团糟；第三，仿真里如果辨识结果都发散、泛化不好，那拿到真机上只会更糟，所以仿真阶段值得好好磨一遍。

1. 项目整体设计与思路拆解

1.1 为什么非要做动力学辨识

机械臂的刚性动力学方程写出来是：

M(q)q̈ + C(q, q̇)q̇ + G(q) + τ_f(q̇) = τ

其中M(q)是惯性矩阵，C(q, q̇)包含科氏力和离心力，G(q)是重力项，τ_f是关节摩擦，τ是关节驱动力矩。控制里经常做的一件事是计算力矩前馈：

τ = M(q)q̈_d + C(q, q̇)q̇ + G(q) + ...

问题是，以上这些项里的质量、质心、惯量、摩擦系数，在工程和仿真中往往只有CAD模型或URDF里的粗糙近似，真实值（尤其是负载变化后）和标称值偏差很大。辨识的目的，就是用实验数据把这些系数实时或离线地标定出来。

在实际场景里，这个需求非常具体。比如你用UR10e、JAKA或者自研的六自由度臂做拖动示教，重力补偿力矩算不准，机械臂就“沉”，拖起来手感很差；再比如做机器人打磨、装配这种需要恒力跟踪的任务，重力项和摩擦项预测偏了，力控带宽再高也会因为模型前馈不准而震荡。即使你只做轨迹规划算法层面的开发，一个动力学参数靠谱的模型，也能让速度规划和加速度前馈的决策更贴近物理极限。

1.2 为什么先做仿真而不是直接上真机

直接上真机辨识存在几个痛点：一是真机数据采集要设计多层滤波和时间同步，力矩信号噪声大，位置信号由编码器读取后要差分两次得到加速度，噪声会被放大；二是真机动起来有安全风险，激励轨迹幅值大、频率高，容易撞奇异点或触发保护；三是真机负载状态未知，就算辨识出参数，也没有“标准答案”可对照，无法判断结果好坏。

仿真里这些麻烦全都没有，URDF里给的惯性参数就是真值，辨识完后可以直接比对。万一辨识出来完全对不上，反而说明流程里有bug，这时候排查成本很低。仿真收敛、泛化正常之后，再移植到真机，你只需要处理传感器噪声、滤波、时间同步这些“工程问题”，剩下的算法问题已经在仿真里解决了。我这次用的环境主路径是ROS2 + Gazebo Classic（Harmonic也兼容），机械臂模型用了UR5e，同时也用pybullet快速验证过一组对比算例，两边结论一致。

1.3 整体流程预览

整个流程可以分成五段，我按这个顺序推进：

1. 建模与参数化：从URDF提取每根杆的质量、质心、惯性张量，建立起动力学模型，写出线性化后的辨识模型。
2. 激励轨迹设计：用有限傅里叶级数生成关节位置轨迹，通过优化让回归矩阵条件数尽可能小。
3. 数据采集与预处理：在仿真中驱动机械臂跑激励轨迹，记录关节位置、速度、力矩，做滤波和差分对齐。
4. 参数估计：用加权最小二乘估计基参数，并对比真值评估误差。
5. 泛化验证：用新轨迹验证预测力矩精度，确认参数不是过拟合。

2. 辨识模型的建立与参数化细节

2.1 从URDF到动力学方程

动力学辨识的第一步是把模型化为关于参数的线性回归形式。刚体动力学方程里，质量、质心、惯性张量等参数虽然以非线性方式出现（比如惯性矩阵和位姿有关），但整个方程关于一组"惯性参数"是线性的，可以整理成：

τ = Y(q, q̇, q̈) · π

其中Y叫回归矩阵，它只与关节位置、速度、加速度有关，维度是n x m（n是关节数，m是待辨识参数个数）；π是所有待辨识参数组成的列向量。对六自由度机械臂，每根杆有10个惯性参数：质量、质心的3个分量、惯性张量的6个独立分量（3个转动惯量加3个惯量积）。加上6个关节的摩擦参数（库仑摩擦和黏性摩擦），完整参数个数为6×10+6×2=72。但注意，完整参数集合并不是个个都能辨识出来。

我在这次项目里用Python写了模型提取代码，没有直接展开URDF每根连杆的坐标变换，而是先用urdf_parser_py把URDF读进来，得到每根杆的质量、惯性张量Ixx Iyy Izz Ixy Ixz Iyz，质心坐标xyz，以及关节类型（转动或移动）。然后递归计算每个连杆在世界系下的变换和雅可比，逐项累加得到回归矩阵Y。这也是大多数教材里教的"Newton-Euler递推线性化"方法，只是实现时可以稍微偷懒：让符号库（sympy）把动力学方程展开，再按参数收集系数。

有一点要提醒：URDF中惯性张量的旋转顺序影响很大。我曾经遇到过URDF里<inertial>标签下的<origin rpy="...">写得不一致，导致惯性张量没有转到连杆系下，辨识出来的质心坐标对不上仿真真值。排查了半天才意识到，是URDF坐标系的旋转换序（RPY还是欧拉角顺序）出了问题。这个话题在JAKA机械臂的ROS包中也出现过，网上有人专门讨论过JAKA的旋转顺序问题，本质上就是URDF/SDF中惯性张量和坐标系的约定要严格统一。

2.2 基参数集：不是所有参数都能辨识

刚才提到完整参数有72个，但实际能辨识出来的是基参数集。基参数是一个满足以下性质的最小参数集合：它能完整描述动力学方程的可辨识部分，且回归矩阵对这组参数是列满秩的。

物理直觉是这样：某些参数组合在机器人运动过程中的作用方式完全相同，没法单独区分。举个例子，一个连杆的质心位置和质量会组合成"一阶质量矩"出现；两个相邻连杆的惯性参数因为运动学耦合，可能只有某种线性组合能辨识出来，单个值不可辨识。基参数一般通过消元法或QR分解/SVD分解来提取。对于六自由度机械臂，常见的基参数个数大约在40~50之间，具体取决于机械臂的运动学结构。

我这次的做法是先把完整回归矩阵Y沿整条激励轨迹采样（比如几千个采样点），拼接成一个巨大的矩阵Y_stack，然后做QR分解带列主元，找出线性无关的列索引，剩下的列对应基参数。代码示意如下：

import numpy as np # Y_stack: 形状 (n_samples*n_joints, n_full_params) # 每行对应一个采样时刻的关节力矩方程 Q, R, perm = np.linalg.qr(Y_stack, mode='reduced', pivoting=True) rank = np.linalg.matrix_rank(R, tol=1e-8) base_cols = sorted(perm[:rank]) # 基参数对应 perm[:rank] 列，其余列是线性相关的组合

注意，基参数的选择和激励轨迹有关。不同的轨迹下回归矩阵的零空间不一样，可能导致选出的基参数组合略有差异。所以这里一个稳妥的流程是：先用一条初步激励轨迹的采样数据确定基参数，再用选定基参数去设计更优激励轨迹，迭代一次即可。

2.3 摩擦模型的取舍

摩擦模型是动力学辨识里最容易被忽视又最影响结果的部分。仿真环境通常用的是黏性摩擦+库仑摩擦：

τ_f = f_v · q̇ + f_c · sign(q̇)

其中f_v是黏性摩擦系数，f_c是库仑摩擦力矩。Gazebo默认的摩擦模型不一定完全等于这个形式，所以仿真辨识里摩擦部分往往是"模型失配"的主要来源。如果发现辨识出的摩擦参数不收敛或预测误差集中在低速段，可以考虑改用Stribeck模型或多项式摩擦模型。不过仿真里我建议先把简单模型跑通，再逐步增加复杂度。

另一个坑是：sign(q̇)在速度为0附近不连续，做数值优化时容易引入抖动。我处理的方法是给sign一个平滑过渡：

def smooth_sign(qd, epsilon=0.01): return np.tanh(qd / epsilon)

这样既保留了符号特性，又能让数值计算稳定。

3. 激励轨迹设计：辨识精度的胜负手

3.1 为什么轨迹设计很重要

参数估计的数学本质是解线性方程组，回归矩阵Y的条件数决定了参数求解的稳定性和抗噪能力。如果激励轨迹选得不好，Y近似奇异，那么测量噪声会被急剧放大，辨识出的参数毫无意义。这就是持续激励的概念：激励轨迹需要在工作空间内充分激发所有可辨识的运动模式，让回归矩阵条件数尽量小。

我见过有人为了省事，直接让机械臂跑一组正弦点到点轨迹，然后拿数据去最小二乘，结果条件数动辄上万，参数负值满天飞。这不是算法的问题，是激励不够。所以激励轨迹设计这一步值得单独认真对待。

3.2 有限傅里叶级数轨迹

工程上最常用的激励轨迹是有限傅里叶级数。对于第i个关节，位置轨迹写成：

q_i(t) = q_{i0} + Σ_{k=1}^{N} [ (a_{ik}/(kω))·sin(kωt) - (b_{ik}/(kω))·cos(kωt) ]

速度和加速度是解析导数：

q̇_i(t) = Σ [ a_{ik}·cos(kωt) + b_{ik}·sin(kωt) ] q̈_i(t) = Σ [ -a_{ik}·kω·sin(kωt) + b_{ik}·kω·cos(kωt) ]

核心参数是基频ω和谐波数N。基频对应轨迹周期T = 2π/ω，一般取4~10秒；N取5~10。所有关节共享同一个基频，保证轨迹是周期的。

选择傅里叶级数有几个好处：一是位置、速度、加速度都是光滑周期信号，不会冲击执行器；二是解析导数避免了数值差分放大噪声；三是系数优化时约束好处理。

3.3 优化目标和约束

我在优化里用的目标函数是回归矩阵条件数的对数（条件数跨数量级大，直接用数值差异不明显）：

minimize f(a,b,q0) = log10(cond(Y_mean))

约束包括关节角度极限、速度极限、加速度极限，以及轨迹的起始/终止速度和加速度为零。优化算法用scipy.optimize.minimize的SLSQP或者差分进化都行。为了减小计算量，我先把每个关节的傅里叶系数做归一化初始猜测，再放到优化器里跑。优化过程中每一轮都要采样回归矩阵Y并计算条件数，这个计算量不低，所以采样点数取200~500个每周期就够了，不用把所有仿真数据点都用上。

一个实践上的细节：条件数对轨迹的初值很敏感。随机初始化很容易陷入局部最优，导致条件数依然很大。我的做法是先用遗传算法（scipy.optimize.differential_evolution）跑一遍粗略搜索，再用SLSQP精调。两个阶段加起来大概几分钟到十几分钟（取决于关节数），可接受。

3.4 实测条件数对比

我把同一台UR5e分别用三种轨迹做了对比：正弦点到点轨迹、手调傅里叶轨迹、优化傅里叶轨迹。

条件数从10万降到50，辨识效果完全是天壤之别。所以如果谁和我说辨识结果很差，我第一反应基本都是问：激励轨迹设计了吗？条件数多少？

4. 仿真数据采集与预处理

4.1 仿真环境搭建与数据记录

我这次用了ROS2 + Gazebo。UR5e模型可以从ros-industrial/ur5e相关的描述包导入，也可以直接写一个URDF替代。核心是用ros2_control或gazebo_ros2_control把关节力矩接口暴露出来，然后在仿真里直接对每个关节施加计算好的力矩命令。

整个数据采集流程是：

1. 生成优化后的傅里叶轨迹q_d(t), q̇_d(t), q̈_d(t)。
2. 在Gazebo里用PID位置控制跟踪这个轨迹，记录实际关节位置和速度。
3. 同时记录控制力矩指令。

这里有一个关键点：仿真里单纯用位置PID控制力矩去跟踪轨迹，就很难避免“轨迹跟踪误差”。如果直接把期望位置当真实位置来用，引入的误差会干扰辨识结果。更推荐的做法是用力矩前馈+位置反馈来跟踪：由当前动力学模型（哪怕参数不太准）计算出期望力矩前馈，叠加轻量级PID消除跟踪误差。这样实际轨迹和期望轨迹偏差很小，回归矩阵用期望轨迹计算也没有大问题。

数据记录我一般保存成CSV或者ROS2 bag，包括时间戳、每个关节的位置、速度、力矩指令。采集时长通常跑5~10个轨迹周期，前两三周期丢弃不用，因为初始瞬态会导致回归矩阵计算不准确。

4.2 滤波、时间对齐和数值差分

如果你用的是实际编码器数据，速度和加速度都需要从位置差分得到，噪声会非常大。我的通用做法是：

1. 对位置信号先用零相位滤波器（scipy.signal.filtfilt）做低通滤波，截止频率选在轨迹基频的5~10倍。
2. 用中心差分计算速度，再对速度做一次同样的滤波。
3. 加速度建议直接使用期望轨迹的解析值，而不是从速度差分。这是辨识实操中非常实用的一个技巧——解析导数是“免费且无噪声”的加速度信号，比任何数值差分都好用。

时间对齐方面，仿真数据里力矩指令和位置反馈来自同一个控制循环，一般已经对齐，但真机场景里常常是不同频率的独立话题，必须根据时间戳做插值重采样。我写了一个简单的时间对齐函数，用numpy.interp把速度/位置插值到力矩时间戳上。

4.3 一个容易被忽略的符号问题

动力学辨识里最常见的隐性bug之一是符号不一致：仿真中关节力矩的正方向定义、减速比方向、重力方向必须和模型一致。比如UR5e在Gazebo里的关节零点位置和DH模型可能差了一个偏移；再比如某些关节因为减速器安装方式，指令力矩的正方向和编码器速度的正方向相反。这种问题在仿真中可能不会立刻暴露，但辨识出的参数里某些项会诡异地为负。

我的建议是：在正式辨识前先做一个简单的“静态重力验证”。让机械臂停在几个已知位形，记录重力矩和位置的关系，反过来算一下重力项符号和数量级是否合理。这一步花不了5分钟，却能省下后面排查的半天时间。

5. 参数估计与仿真验证

5.1 加权最小二乘估计

数据准备好之后，参数估计本身其实很简单。把所有采样点的回归矩阵堆叠成Y_stack，力矩堆叠成τ_stack，求解最小二乘：

# 加权最小二乘 W = np.diag(weights) # 通常取力矩方差的倒数，或按关节量级归一 pi_hat = np.linalg.lstsq(W @ Y_stack, W @ tau_stack, rcond=None)[0]

但工程上有个细节：六个关节的力矩量级差异很大，比如手腕关节力矩可能只有大臂关节的几十分之一。如果不加权，最小二乘会被大关节主导，小关节辨识精度会变得不可接受。加权矩阵通常取每个关节力矩测量方差的倒数，或者直接用关节力矩范围的倒数。仿真里因为噪声可控，简单起见可以取关节力矩均方根的倒数。

5.2 辨识结果与真值对比

仿真中真值是已知的，所以我可以直接做每个基参数的误差对比：

除了质量，质心位置和惯性张量的误差也基本在2%以内，摩擦参数因为Gazebo的摩擦模型和名义模型略有差异，误差稍大但趋势正确。这不是说仿真有多强，恰恰说明流程走对了，因为真值已知，所以每一个误差都能溯源。

5.3 力矩预测与泛化验证：如何判断辨识成功

参数辨识的最终检验不是参数本身和真值多接近，而是预测力矩的误差。我一般定义两个指标：

• 预测力矩均方根误差（RMSE）占关节力矩RMS的比例，一般要求小于5%。
• 另一个是“验证轨迹”上的误差：设计一条与辨识轨迹完全不同的新轨迹（比如更高速的轨迹和带载的轨迹），让机械臂跟踪，用辨识出的参数预测力矩，和实际力矩对比。

我在验证时用了一条优化的傅里叶轨迹和一条随机斜坡轨迹，结果如下：

手腕关节误差偏大，主要是因为摩擦在低速启停阶段的影响在小关节里占比高，模型简单摩擦形式在低速段拟合能力有限。如果后续要提升，可以在摩擦模型上做文章。

5.4 参数迁移到控制器的效果

辨识参数迁移回控制器，效果是很直观的。我做了两组对比实验：一组用URDF原始参数做重力补偿前馈，另一组用辨识参数做重力补偿前馈，然后让机械臂在关节空间中低速跟踪正弦轨迹。辨识参数组的位置跟踪误差RMS大约下降了40%，力矩波动也更小。这说明辨识出的参数不仅“数字上可信”，在实际控制器里也确实能带来可量化的改善。

另外我还在pybullet中做了一组可重复实验，因为pybullet里可以直接设定刚体的质量和惯性张量，辨识后直接能拿到误差。仿真中迁移性很好，同样的流程从Gazebo切到pybullet，辨识误差差别不大。如果你只是想快速验证算法流程，pybullet甚至比Gazebo更轻量，值得一试。

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

我把这次实操中碰到的典型问题和排查经验整理成了一张速查表：

这里重点说一个容易被忽略的问题：激励轨迹的频率选取。基频太低，周期太长，采集效率低；基频太高，加速度太大，一方面可能超过关节物理极限，另一方面PID跟踪跟不上，实际轨迹偏离期望轨迹很大。我的经验是让最高谐波的加速度不超过关节最大加速度的50%，同时保证轨迹跟踪误差在1度以内。优化时要加上这个约束，而不是简单只约束理论上的速度和加速度极限。

另一个是数据采集时最好把确认轨迹的初始周期也记录下来，但辨识时舍弃初始瞬态段。原因是对周期性轨迹而言，初始一段的加速度和速度瞬态不是稳态周期解，混进回归矩阵后会污染条件数计算。我在代码里直接按“从第3个周期开始取数”来截断，省心且有效。

最后是关于代码复用的建议。仿真辨识的流程很固定，强烈建议把建模仿真和辨识算法拆成独立模块：模型模块负责从URDF生成回归矩阵和基参数；轨迹模块负责傅里叶系数的优化和轨迹生成；数据模块负责从ROS2 bag或CSV读取数据；估计模块负责最小二乘和结果可视化。这样后续换机械臂型号（UR10e、Panda、自研臂）时，只需要换URDF，算法侧几乎不用改。这次我用到的全套代码在本地也整理成了一个Python包，主流程封装成了三个函数：

model = DynamicModel.from_urdf("ur5e.urdf") traj = FFTTrajectory.optimize(model, N_harmonics=7, period=6.0) params = estimate_params(model, traj, data_csv="collect.csv")

三个函数对应了辨识的三个主要阶段，实际跑起来非常顺手。如果你是从零起步，建议也先按这个模块化思路搭框架，而不是写成一坨面条代码。

7. 一个小建议：仿真阶段多试极端工况

很多同学在仿真里把辨识流程跑通后就直接上真机，但我的习惯是仿真阶段先多折腾几轮极端工况。比如把URDF里的质量参数人为改掉20%，看辨识算法能不能收敛回真值；或者故意给力矩数据加5%的高斯噪声，看辨识结果的鲁棒性如何。仿真里测试成本几乎为零，却能显著提高你对算法的置信度。这个步骤做完，再去真机上处理那些恼人的滤波、时间同步、传感器标定问题，你就能把精力集中在“工程问题”上，而不是在“算法bug”上反复横跳。这也是我为什么一直坚持先仿真后真机的原因。