1. 动力学辨识到底在解决什么问题1.1 从一次仿真发散说起如果你在Gazebo或者MuJoCo里跑过六轴机械臂的力控或者计算力矩控制大概率遇到过这种情况控制器参数明明按理论算得没问题仿真一跑起来关节就开始抖积分几步之后直接发散终端刷出一片NaN。我最早接触这个问题是在一台自制的六自由度串联臂上当时用的是自己写的Python动力学库模型参数全靠SolidWorks里量出来的质量和质心位置填进去结果算出来的科氏力和实际仿真环境里的响应差了一大截控制器增益稍微给大一点就炸。后来才想明白问题不在控制器在于动力学模型参数本身就不准。CAD里读出来的质量、质心、惯量和真实系统哪怕是仿真系统里的等效参数之间隔着电机转子惯量、减速器惯量、摩擦、线缆拖拽等一大堆没建模的东西。你拿一个看起来对的模型去做力控等于拿一把没校准的秤去称黄金。动力学辨识要干的事就是用实验数据反推出一组能真实反映系统输入输出关系的参数。而最小惯性参数集是这件事里最关键的一个概念——它决定了你到底需要辨识多少个参数以及哪些参数根本辨识不出来。1.2 为什么不能直接辨识全部惯性参数一根六轴机械臂如果每个连杆都按刚体处理每个连杆有10个惯性参数质量1个、质心3个、惯量张量6个六根连杆就是60个参数。听起来不多但问题在于这60个参数里有相当一部分对关节力矩没有任何独立影响。举个最直观的例子。假设第一关节的旋转轴是竖直的Z轴那么第一连杆绕Z轴的转动惯量Izz和第一关节电机转子加减速器折算过来的等效惯量在力矩方程里永远是加在一起出现的。你无论做多少次实验都只能辨识出它们的和没法把两者分开。这就是典型的参数不可辨识——它们在动力学方程里线性相关。最小惯性参数集做的事情就是把这60个参数通过线性组合压缩成一组数量最少、且相互独立的基参数。对于典型的六轴串联臂这个数量通常在30到40之间具体取决于关节构型。压缩之后辨识问题从解一个欠定的60维方程变成解一个适定的N维方程数值稳定性和辨识精度都会有质的提升。1.3 这套方法适合谁这篇内容主要面向几类人一是做机械臂力控、阻抗控制、模型预测控制的开发者你的控制器性能上限很大程度上取决于动力学模型准不准二是做机械臂仿真环境搭建的工程师想让仿真里的动力学响应更接近真实硬件三是做机械臂强化学习的研究者一个准确的动力学模型可以大幅减少训练所需的真实交互次数。前提条件是你得有一定的机器人学基础至少知道雅可比矩阵、拉格朗日方程、关节空间和笛卡尔空间的区别。代码层面Python和ROS是主要工具但核心算法不依赖特定框架你用什么语言实现都行。2. 最小惯性参数集的推导思路与工具选型2.1 从牛顿-欧拉方程到回归矩阵机械臂的刚体动力学方程可以写成这个形式tau M(q) * qdd C(q, qd) * qd G(q)其中tau是关节力矩向量q、qd、qdd分别是关节位置、速度、加速度。M是质量矩阵C包含科氏力和离心力项G是重力项。关键的一步在于这个方程对惯性参数是线性的。也就是说你可以把它改写成tau Y(q, qd, qdd) * phiY叫做回归矩阵它的每一个元素都是关节位置、速度、加速度的函数和惯性参数无关。phi就是那60个惯性参数组成的向量。这个线性关系是动力学辨识的数学基础——因为线性所以可以用最小二乘法解。推导回归矩阵有几种常见方法。牛顿-欧拉递推法计算效率高适合在线应用但推导回归矩阵形式比较繁琐。拉格朗日法形式优雅但计算量大。实际做辨识的时候我一般用基于旋量的方法因为它的物理意义清晰而且容易用符号计算工具自动化生成。2.2 最小参数集的确定方法确定最小惯性参数集核心是找出回归矩阵Y的零空间。具体来说如果存在一个非零向量v使得Y*v 0对所有实验数据都成立那么v对应的参数组合就是不可辨识的。实际操作中有两种主流做法第一种是数值法。随机生成大量关节轨迹计算每个时刻的回归矩阵把它们堆叠成一个大矩阵然后做QR分解或者SVD分解找出线性相关的列。这些相关列对应的参数组合就是需要合并的。这种方法实现简单但依赖随机轨迹的覆盖性如果轨迹设计得不好可能漏掉某些相关关系。第二种是符号法。用符号计算工具比如Python的SymPy或者MATLAB的Symbolic Toolbox直接对回归矩阵做符号运算找出精确的零空间。这种方法结果精确但计算量大对六轴以上的机械臂可能会很慢。我个人的做法是混合使用先用符号法对小规模模型比如前三个关节验证确认参数合并规则正确再用数值法处理完整模型。这样既保证了正确性又兼顾了效率。2.3 工具链选择与理由整个辨识流程涉及符号推导、轨迹优化、数据采集、参数估计、验证这几个环节每个环节的工具选择都会影响最终效果。环节推荐工具选择理由符号推导SymPy / MATLAB Symbolic免费、Python生态、可导出C代码轨迹优化SciPy optimize / CasADiCasADi适合带约束的优化问题仿真环境PyBullet / MuJoCo / GazeboPyBullet轻量、MuJoCo动力学精确数据采集ROS topic / 直接API仿真里直接调API更简单参数估计NumPy least squares / SciPy带正则化的最小二乘验证独立轨迹 力矩预测误差交叉验证避免过拟合这里重点说一下仿真环境的选择。如果你只是想验证辨识算法本身PyBullet是最省事的pip装完就能用API也简单。但PyBullet的动力学计算精度一般接触模型也比较粗糙。如果你需要高精度的动力学响应MuJoCo是更好的选择它的刚体动力学求解器在业界口碑很好。Gazebo适合和ROS生态集成但配置起来最麻烦而且它的物理引擎在纯动力学仿真场景下不如前两者精确。我这次实践用的是PyBullet原因是它可以直接读取URDF而且提供了calculateInverseDynamics接口可以方便地获取力矩真值。对于验证最小惯性参数集辨识流程来说这个精度足够了。3. 仿真实践从URDF到辨识结果3.1 环境搭建与模型加载先搭环境。Python 3.8以上装这几个包pip install pybullet numpy scipy sympy matplotlib如果你要用CasADi做轨迹优化再加一个pip install casadi模型方面我用的是一个标准的六轴串联臂URDFDH参数和常见的工业臂类似。加载模型很简单import pybullet as p import pybullet_data import numpy as np p.connect(p.DIRECT) # 用DIRECT模式不弹可视化窗口跑得快 p.setAdditionalSearchPath(pybullet_data.getDataPath()) robot_id p.loadURDF(my_robot.urdf, useFixedBaseTrue) # 获取关节信息 num_joints p.getNumJoints(robot_id) joint_indices [i for i in range(num_joints) if p.getJointInfo(robot_id, i)[2] ! p.JOINT_FIXED]这里有个细节要注意PyBullet的URDF加载默认会把base link固定或者自由取决于useFixedBase参数。做动力学辨识的时候机械臂底座必须是固定的否则基座的运动也会影响力矩增加不必要的复杂度。另外URDF里的惯性参数质量和惯量是真值我们辨识的目标就是看能不能从仿真数据里把这些值反推出来。当然实际物理系统里这些值你是不知道的这里用仿真验证的好处就是有ground truth可以对比。3.2 激励轨迹设计轨迹设计是辨识里最容易被忽视、但影响最大的环节。你喂给算法的数据质量直接决定了辨识结果的上限。如果轨迹激励不充分某些参数对应的回归矩阵列几乎为零那这些参数就辨识不出来或者辨识结果方差极大。好的激励轨迹应该满足几个条件覆盖关节运动范围、包含丰富的频率成分、避免关节速度过高导致仿真不稳定、总时长适中。实践中用得最多的是有限项傅里叶级数轨迹q_i(t) q_i0 sum_{k1}^{N} [a_ik * sin(k*w*t) b_ik * cos(k*w*t)]其中q_i0是关节i的偏置位置a_ik和b_ik是傅里叶系数w是基频N是谐波数。这种轨迹的好处是周期性、光滑、容易求导、频率成分可控。参数选择上我一般取N5基频w根据总时长T来定w 2*pi/T。总时长T通常取10到20秒太短了数据量不够太长了仿真时间成本高。偏置位置q_i0取关节范围的中点振幅根据关节范围来定一般取范围的30%到40%。def fourier_trajectory(t, q0, a, b, w): 计算傅里叶级数轨迹的位置、速度、加速度 N a.shape[1] q q0.copy() qd np.zeros_like(q0) qdd np.zeros_like(q0) for k in range(1, N1): q a[:, k-1] * np.sin(k*w*t) b[:, k-1] * np.cos(k*w*t) qd a[:, k-1] * k*w * np.cos(k*w*t) - b[:, k-1] * k*w * np.sin(k*w*t) qdd -a[:, k-1] * (k*w)**2 * np.sin(k*w*t) - b[:, k-1] * (k*w)**2 * np.cos(k*w*t) return q, qd, qdd傅里叶系数的初始值可以随机生成但更好的做法是用优化方法找一个最优的激励轨迹。优化的目标通常是回归矩阵的条件数最小化因为条件数越小最小二乘解的数值稳定性越好。from scipy.optimize import minimize def cost_function(params, robot_id, joint_indices, T, dt): 优化目标回归矩阵条件数 # 解析params为傅里叶系数 # 生成轨迹计算回归矩阵 # 返回条件数 pass # 优化 result minimize(cost_function, initial_params, args(robot_id, joint_indices, T, dt), methodL-BFGS-B)这个优化问题是非凸的容易陷入局部最优。我的经验是多跑几次每次用不同的随机初始值取条件数最小的那个。另外可以在目标函数里加一个正则项惩罚过大的关节速度避免优化出来的轨迹在实际执行时超出限制。3.3 数据采集与回归矩阵构建轨迹设计好之后就可以在仿真里执行采集关节位置、速度、加速度和对应的力矩。T 15.0 # 总时长 dt 0.001 # 采样周期 steps int(T / dt) data_q [] data_qd [] data_qdd [] data_tau [] for step in range(steps): t step * dt q, qd, qdd fourier_trajectory(t, q0, a, b, w) # 用PyBullet计算逆动力学力矩 tau p.calculateInverseDynamics(robot_id, q.tolist(), qd.tolist(), qdd.tolist()) data_q.append(q) data_qd.append(qd) data_qdd.append(qdd) data_tau.append(tau)这里有个坑要注意PyBullet的calculateInverseDynamics返回的力矩不包含摩擦项。如果你在URDF里定义了关节摩擦它也不会自动加进去。所以如果你要辨识摩擦参数需要自己额外建模。对于纯惯性参数辨识这个接口是够用的。采集完数据下一步是构建回归矩阵。回归矩阵的构建是整个过程里最技术性的部分。对于每个采样时刻你需要计算一个矩阵Y_k使得tau_k Y_k * phi。把所有时刻的Y_k堆叠起来就得到总的回归矩阵。回归矩阵的具体形式取决于你用的推导方法。如果用牛顿-欧拉法可以借助PyBullet或者Pinocchio这样的库来自动计算。Pinocchio提供了computeJointTorqueRegressor接口可以直接输出回归矩阵非常方便。import pinocchio as pin model pin.buildModelFromUrdf(my_robot.urdf) data model.createData() def compute_regressor(model, data, q, qd, qdd): 计算回归矩阵 pin.computeJointTorqueRegressor(model, data, q, qd, qdd) return data.jointTorqueRegressor.copy()如果你不想引入Pinocchio也可以自己用SymPy推导。推导出来的回归矩阵可以导出成Python函数或者C代码然后在采集数据的时候调用。3.4 最小参数集的数值确定拿到回归矩阵之后下一步是确定最小参数集。我用的是QR分解法具体步骤如下def find_base_parameters(Y, tol1e-8): 通过QR分解找出最小参数集 Y: 回归矩阵形状 (n_samples * n_joints, n_params) 返回: 基参数索引以及参数组合矩阵 # 对Y做QR分解带列主元 Q, R, P scipy.linalg.qr(Y, pivotingTrue) # 找出R的对角线元素 diag_R np.abs(np.diag(R)) # 对角线元素小于阈值的列对应的参数是线性相关的 independent_indices np.where(diag_R tol)[0] # P是列置换independent_indices对应的是独立列 base_param_indices P[independent_indices] return base_param_indicesQR分解的列主元策略会把线性相关性最强的列排在后面所以对角线元素会从大到小排列。找到一个明显的断崖之后断崖之后的列就是线性相关的。实际操作中阈值的选取需要一点经验。太小了会把数值噪声当成独立参数太大了会丢掉真正独立的参数。我一般会画一下对角线元素的分布图找一个明显的拐点。确定基参数之后还需要计算参数组合矩阵。因为最小参数集里的每个基参数实际上是原始参数的一个线性组合。这个组合关系可以通过求解R矩阵得到。def compute_combination_matrix(R, P, independent_indices): 计算原始参数到基参数的映射矩阵 n_independent len(independent_indices) # R的前n_independent行和前n_independent列构成可逆矩阵 R11 R[:n_independent, :n_independent] R12 R[:n_independent, n_independent:] # 组合矩阵 beta np.linalg.solve(R11, R12) return beta3.5 参数估计与结果分析有了回归矩阵和基参数索引参数估计就是解一个最小二乘问题# 构建最小参数集对应的回归矩阵 Y_base Y[:, base_param_indices] # 最小二乘求解 phi_base, residuals, rank, sv np.linalg.lstsq(Y_base, tau_all, rcondNone) print(基参数估计值, phi_base) print(残差, residuals)如果你想让结果更鲁棒可以加一个正则化项变成岭回归from sklearn.linear_model import Ridge model Ridge(alpha1e-6) model.fit(Y_base, tau_all) phi_base model.coef_正则化系数alpha的选择需要交叉验证。太大了会导致估计偏差太小了起不到正则化作用。我一般从1e-8开始试逐步增大看验证集上的力矩预测误差。辨识完之后一定要做验证。验证的方法是用一组独立的轨迹和辨识轨迹不同来测试看预测力矩和真实力矩的误差。# 用验证轨迹计算预测力矩 tau_pred Y_val_base phi_base tau_true tau_val # 计算误差 error tau_pred - tau_true rmse np.sqrt(np.mean(error**2)) print(f验证集RMSE: {rmse:.6f} Nm)如果RMSE在0.1 Nm以下对于中小型机械臂来说就算不错了。如果误差很大可能是几个原因轨迹激励不充分、回归矩阵构建有误、或者仿真环境里有未建模的效应比如关节摩擦。4. 实操中踩过的坑与排查技巧4.1 仿真发散最常见也最头疼的问题仿真发散是动力学辨识里最常遇到的问题没有之一。表现就是跑着跑着关节力矩突然变成NaN或者数值爆炸到1e10以上。我遇到过好几次每次原因都不一样。第一个原因轨迹参数超出关节限制。傅里叶级数轨迹的振幅如果设得太大叠加之后可能超出关节的物理限位。PyBullet在超出限位时不会报错但力矩计算会变得不准确。解决办法是在生成轨迹之后先检查一遍关节范围把超限的部分截断或者缩放。第二个原因采样周期太大。如果你用1ms的采样周期但轨迹的频率成分很高比如基频0.5Hz5次谐波就是2.5Hz那么每个周期内的变化可能太快数值积分误差会累积。解决办法是减小采样周期或者降低轨迹的最高频率。第三个原因回归矩阵条件数太大。如果激励轨迹设计得不好回归矩阵的条件数可能达到1e10以上这时候最小二乘解对噪声极其敏感稍微一点数值误差就会导致参数估计值爆炸。解决办法是优化轨迹或者在求解时加正则化。排查技巧在仿真循环里加一个检查如果力矩的绝对值超过某个阈值比如1000 Nm就打印当前时刻的q、qd、qdd看看是哪个关节出了问题。大部分时候问题都出在某个关节的速度或加速度异常大。4.2 参数辨识结果不稳定方差大、重复性差有时候你会发现每次跑辨识得到的参数值都不一样而且差异很大。这说明辨识问题的条件数不好或者数据里的噪声太大。条件数不好的根源通常是轨迹激励不充分。某些参数比如某些连杆的惯量乘积项对力矩的影响很小只有在特定的运动模式下才会显现出来。如果你的轨迹里没有这种运动模式这些参数就辨识不准。解决办法有两个一是优化轨迹在目标函数里显式地惩罚条件数二是增加轨迹的多样性用多组不同参数的傅里叶轨迹把数据合并起来做辨识。# 多组轨迹合并 Y_all np.vstack([Y1, Y2, Y3]) tau_all np.concatenate([tau1, tau2, tau3]) phi_base, _, _, _ np.linalg.lstsq(Y_all[:, base_param_indices], tau_all, rcondNone)另外仿真里的数值噪声也是一个因素。PyBullet的逆动力学计算用的是浮点数精度有限。如果你用MuJoCo精度会好一些。但更根本的办法是对数据进行滤波把高频噪声滤掉。from scipy.signal import butter, filtfilt def lowpass_filter(data, cutoff, fs, order4): nyq 0.5 * fs normal_cutoff cutoff / nyq b, a butter(order, normal_cutoff, btypelow) return filtfilt(b, a, data, axis0) # 对力矩数据做低通滤波 tau_filtered lowpass_filter(tau_all, cutoff10, fs1/dt)截止频率的选择要看你的轨迹频率。一般取轨迹最高频率的2到3倍就够了。太高了滤不掉噪声太低了会把有用的信号也滤掉。4.3 最小参数集确定错误独立参数选多了或选少了QR分解的阈值选取是个经验活。阈值太小会把数值上接近零但实际不为零的列当成独立列导致最小参数集偏大阈值太大会把真正独立的列也丢掉导致模型不完整。我的做法是先画一下R矩阵对角线元素的对数分布图找一个明显的断崖。断崖之前的是独立参数之后的是相关参数。如果分布图上看不出明显的断崖那说明轨迹激励不够充分需要重新设计轨迹。import matplotlib.pyplot as plt diag_R np.abs(np.diag(R)) plt.semilogy(diag_R, o-) plt.xlabel(Column index) plt.ylabel(|R_ii|) plt.title(QR分解对角线元素分布) plt.grid(True) plt.show()另一个验证方法是用确定的最小参数集做辨识然后在验证集上看力矩预测误差。如果误差很小说明参数集选对了如果误差很大可能是漏掉了某些独立参数。4.4 常见问题速查表问题现象可能原因排查方法解决方案仿真发散力矩NaN轨迹超限、采样太快、条件数大检查q/qd/qdd范围缩放轨迹、减小dt、加正则化参数估计方差大激励不充分、噪声大看条件数、看残差优化轨迹、滤波、多组数据合并验证误差大参数集选错、模型不完整对比预测和真实力矩调整QR阈值、检查摩擦项某些参数始终为零该参数不可辨识检查回归矩阵列合并到基参数里重复性差数值噪声、初始值敏感多次运行对比固定随机种子、加正则化4.5 几个容易被忽视的细节第一个细节URDF里的惯量参考系。URDF里的惯量矩阵是相对于连杆坐标系的而且原点通常在连杆的某个特定位置比如关节轴线上。如果你自己推导回归矩阵一定要确认参考系一致否则辨识出来的参数物理意义会完全不对。第二个细节PyBullet的关节顺序。PyBullet返回的关节力矩顺序和URDF里定义的关节顺序可能不一致。如果你有fixed joint或者mimic joint顺序会更复杂。一定要先打印出来确认。第三个细节重力方向。仿真环境里的重力方向默认是(0, 0, -9.81)但有些URDF会定义不同的重力方向。如果你的辨识结果里重力项明显不对先检查这个。第四个细节数值精度。如果你用float32做计算精度可能不够。Python的numpy默认是float64但如果你从ROS topic里读数据可能是float32。做辨识之前统一转成float64。5. 从仿真到实机的迁移建议5.1 仿真里能验证什么不能验证什么仿真环境最大的价值是验证算法流程的正确性。你可以在仿真里确认回归矩阵构建对不对、最小参数集确定得准不准、参数估计算法收敛不收敛。这些在仿真里跑通了到实机上就只剩下数据采集和摩擦建模的问题。但仿真里验证不了的东西也很多。摩擦是最典型的。仿真里的关节摩擦通常是简化的库仑粘滞模型和真实减速器的摩擦特性差很远。柔性也是仿真里连杆是刚体真实机械臂的连杆和关节都有柔性会在高速运动时产生振动。传感器噪声更不用说仿真里的力矩数据是干净的实机上的力矩传感器有噪声、有偏置、有温漂。所以我的建议是仿真里把惯性参数辨识跑通实机上重点解决摩擦和噪声。5.2 实机数据采集的注意事项实机上做辨识数据采集是最大的挑战。你需要测量或估计关节力矩、位置、速度、加速度。位置可以用编码器速度可以差分或者用滤波器加速度通常需要二阶差分噪声会很大。力矩测量有几种方案关节力矩传感器最直接但成本高电机电流估计力矩成本低但精度受减速器效率影响基座力传感器可以间接估计但需要额外的解耦计算。我的经验是如果条件允许优先用关节力矩传感器。电流估计的力矩在低速和换向时误差很大会严重影响辨识精度。另外实机采集数据时要注意温度。电机和减速器发热会导致摩擦特性变化长时间采集数据时前后段的数据可能不一致。建议采集时间控制在几分钟以内或者分段采集每段之间让机械臂冷却。5.3 摩擦建模的补充实机辨识如果不建模摩擦惯性参数的估计值会有偏。最简单的摩擦模型是库仑粘滞tau_friction Fc * sign(qd) Fv * qd其中Fc是库仑摩擦系数Fv是粘滞摩擦系数。这两个参数可以和惯性参数一起辨识只需要把回归矩阵扩展一下把sign(qd)和qd对应的列加进去。更复杂的模型还包括Stribeck效应、静摩擦、温度依赖等但那些需要更多的实验数据来支撑。对于大多数应用场景库仑粘滞模型已经够用了。# 扩展回归矩阵加入摩擦项 Y_extended np.hstack([Y, np.sign(qd_all), qd_all]) phi_extended, _, _, _ np.linalg.lstsq(Y_extended, tau_all, rcondNone) # 最后两维就是摩擦系数 Fc_est phi_extended[-2] Fv_est phi_extended[-1]5.4 验证与迭代实机辨识做完之后一定要做闭环验证。开环的力矩预测误差小不代表闭环控制效果好。因为闭环控制里力矩误差会通过反馈回路放大。验证的方法是用辨识出来的参数设计一个计算力矩控制器在实机上跑一条轨迹看跟踪误差。如果跟踪误差在合理范围内比如关节位置误差小于0.01 rad说明辨识结果可用。如果误差很大可能需要回到辨识环节检查数据质量或者模型结构。迭代是常态。我做过的一个项目前后迭代了四五轮每轮都发现新的问题第一轮是摩擦没建模第二轮是加速度噪声太大第三轮是某些关节的激励不够第四轮是温度漂移。每解决一个问题辨识精度就提升一点。6. 一些个人体会这套东西我从最早用MATLAB手推回归矩阵到后来用Pinocchio自动生成再到最近用PyBullet做快速验证前后折腾了好几年。最大的体会是动力学辨识的难点不在算法在工程细节。算法本身是成熟的最小二乘、QR分解、傅里叶轨迹这些都是教科书里的东西。但实际做的时候你会发现80%的时间花在处理各种工程问题上轨迹超限了、数据有噪声了、参考系搞错了、单位不统一了。这些问题在论文里通常不会写但每一个都能让你卡好几天。另一个体会是仿真验证非常重要但不能替代实机。仿真里跑通只能说明你的算法逻辑没问题实机上还有一大堆仿真里遇不到的问题。我见过太多人仿真里结果漂亮得不行一到实机就完全不能用。最后说一个实用技巧把整个辨识流程脚本化。从轨迹生成、数据采集、回归矩阵构建、参数估计到验证全部写成脚本一键运行。这样你可以快速迭代改一个参数跑一遍看结果怎么变。手动操作的话改一次参数要折腾半天效率太低。# 一键辨识脚本的伪代码结构 def full_identification_pipeline(urdf_path, config): # 1. 加载模型 robot load_robot(urdf_path) # 2. 生成激励轨迹 trajectory design_trajectory(robot, config) # 3. 采集数据 data collect_data(robot, trajectory, config) # 4. 构建回归矩阵 Y, tau build_regressor(robot, data) # 5. 确定最小参数集 base_indices find_base_parameters(Y, config.tol) # 6. 参数估计 phi estimate_parameters(Y[:, base_indices], tau, config.regularization) # 7. 验证 error validate(robot, phi, base_indices, config.validation_trajectory) return phi, error这个脚本我用了很久每次换机械臂或者换仿真环境只需要改一下URDF路径和几个配置参数就能快速得到辨识结果。对于做算法迭代来说这种自动化程度是必须的。
机械臂动力学辨识：最小惯性参数集推导与仿真实践
1. 动力学辨识到底在解决什么问题1.1 从一次仿真发散说起如果你在Gazebo或者MuJoCo里跑过六轴机械臂的力控或者计算力矩控制大概率遇到过这种情况控制器参数明明按理论算得没问题仿真一跑起来关节就开始抖积分几步之后直接发散终端刷出一片NaN。我最早接触这个问题是在一台自制的六自由度串联臂上当时用的是自己写的Python动力学库模型参数全靠SolidWorks里量出来的质量和质心位置填进去结果算出来的科氏力和实际仿真环境里的响应差了一大截控制器增益稍微给大一点就炸。后来才想明白问题不在控制器在于动力学模型参数本身就不准。CAD里读出来的质量、质心、惯量和真实系统哪怕是仿真系统里的等效参数之间隔着电机转子惯量、减速器惯量、摩擦、线缆拖拽等一大堆没建模的东西。你拿一个看起来对的模型去做力控等于拿一把没校准的秤去称黄金。动力学辨识要干的事就是用实验数据反推出一组能真实反映系统输入输出关系的参数。而最小惯性参数集是这件事里最关键的一个概念——它决定了你到底需要辨识多少个参数以及哪些参数根本辨识不出来。1.2 为什么不能直接辨识全部惯性参数一根六轴机械臂如果每个连杆都按刚体处理每个连杆有10个惯性参数质量1个、质心3个、惯量张量6个六根连杆就是60个参数。听起来不多但问题在于这60个参数里有相当一部分对关节力矩没有任何独立影响。举个最直观的例子。假设第一关节的旋转轴是竖直的Z轴那么第一连杆绕Z轴的转动惯量Izz和第一关节电机转子加减速器折算过来的等效惯量在力矩方程里永远是加在一起出现的。你无论做多少次实验都只能辨识出它们的和没法把两者分开。这就是典型的参数不可辨识——它们在动力学方程里线性相关。最小惯性参数集做的事情就是把这60个参数通过线性组合压缩成一组数量最少、且相互独立的基参数。对于典型的六轴串联臂这个数量通常在30到40之间具体取决于关节构型。压缩之后辨识问题从解一个欠定的60维方程变成解一个适定的N维方程数值稳定性和辨识精度都会有质的提升。1.3 这套方法适合谁这篇内容主要面向几类人一是做机械臂力控、阻抗控制、模型预测控制的开发者你的控制器性能上限很大程度上取决于动力学模型准不准二是做机械臂仿真环境搭建的工程师想让仿真里的动力学响应更接近真实硬件三是做机械臂强化学习的研究者一个准确的动力学模型可以大幅减少训练所需的真实交互次数。前提条件是你得有一定的机器人学基础至少知道雅可比矩阵、拉格朗日方程、关节空间和笛卡尔空间的区别。代码层面Python和ROS是主要工具但核心算法不依赖特定框架你用什么语言实现都行。2. 最小惯性参数集的推导思路与工具选型2.1 从牛顿-欧拉方程到回归矩阵机械臂的刚体动力学方程可以写成这个形式tau M(q) * qdd C(q, qd) * qd G(q)其中tau是关节力矩向量q、qd、qdd分别是关节位置、速度、加速度。M是质量矩阵C包含科氏力和离心力项G是重力项。关键的一步在于这个方程对惯性参数是线性的。也就是说你可以把它改写成tau Y(q, qd, qdd) * phiY叫做回归矩阵它的每一个元素都是关节位置、速度、加速度的函数和惯性参数无关。phi就是那60个惯性参数组成的向量。这个线性关系是动力学辨识的数学基础——因为线性所以可以用最小二乘法解。推导回归矩阵有几种常见方法。牛顿-欧拉递推法计算效率高适合在线应用但推导回归矩阵形式比较繁琐。拉格朗日法形式优雅但计算量大。实际做辨识的时候我一般用基于旋量的方法因为它的物理意义清晰而且容易用符号计算工具自动化生成。2.2 最小参数集的确定方法确定最小惯性参数集核心是找出回归矩阵Y的零空间。具体来说如果存在一个非零向量v使得Y*v 0对所有实验数据都成立那么v对应的参数组合就是不可辨识的。实际操作中有两种主流做法第一种是数值法。随机生成大量关节轨迹计算每个时刻的回归矩阵把它们堆叠成一个大矩阵然后做QR分解或者SVD分解找出线性相关的列。这些相关列对应的参数组合就是需要合并的。这种方法实现简单但依赖随机轨迹的覆盖性如果轨迹设计得不好可能漏掉某些相关关系。第二种是符号法。用符号计算工具比如Python的SymPy或者MATLAB的Symbolic Toolbox直接对回归矩阵做符号运算找出精确的零空间。这种方法结果精确但计算量大对六轴以上的机械臂可能会很慢。我个人的做法是混合使用先用符号法对小规模模型比如前三个关节验证确认参数合并规则正确再用数值法处理完整模型。这样既保证了正确性又兼顾了效率。2.3 工具链选择与理由整个辨识流程涉及符号推导、轨迹优化、数据采集、参数估计、验证这几个环节每个环节的工具选择都会影响最终效果。环节推荐工具选择理由符号推导SymPy / MATLAB Symbolic免费、Python生态、可导出C代码轨迹优化SciPy optimize / CasADiCasADi适合带约束的优化问题仿真环境PyBullet / MuJoCo / GazeboPyBullet轻量、MuJoCo动力学精确数据采集ROS topic / 直接API仿真里直接调API更简单参数估计NumPy least squares / SciPy带正则化的最小二乘验证独立轨迹 力矩预测误差交叉验证避免过拟合这里重点说一下仿真环境的选择。如果你只是想验证辨识算法本身PyBullet是最省事的pip装完就能用API也简单。但PyBullet的动力学计算精度一般接触模型也比较粗糙。如果你需要高精度的动力学响应MuJoCo是更好的选择它的刚体动力学求解器在业界口碑很好。Gazebo适合和ROS生态集成但配置起来最麻烦而且它的物理引擎在纯动力学仿真场景下不如前两者精确。我这次实践用的是PyBullet原因是它可以直接读取URDF而且提供了calculateInverseDynamics接口可以方便地获取力矩真值。对于验证最小惯性参数集辨识流程来说这个精度足够了。3. 仿真实践从URDF到辨识结果3.1 环境搭建与模型加载先搭环境。Python 3.8以上装这几个包pip install pybullet numpy scipy sympy matplotlib如果你要用CasADi做轨迹优化再加一个pip install casadi模型方面我用的是一个标准的六轴串联臂URDFDH参数和常见的工业臂类似。加载模型很简单import pybullet as p import pybullet_data import numpy as np p.connect(p.DIRECT) # 用DIRECT模式不弹可视化窗口跑得快 p.setAdditionalSearchPath(pybullet_data.getDataPath()) robot_id p.loadURDF(my_robot.urdf, useFixedBaseTrue) # 获取关节信息 num_joints p.getNumJoints(robot_id) joint_indices [i for i in range(num_joints) if p.getJointInfo(robot_id, i)[2] ! p.JOINT_FIXED]这里有个细节要注意PyBullet的URDF加载默认会把base link固定或者自由取决于useFixedBase参数。做动力学辨识的时候机械臂底座必须是固定的否则基座的运动也会影响力矩增加不必要的复杂度。另外URDF里的惯性参数质量和惯量是真值我们辨识的目标就是看能不能从仿真数据里把这些值反推出来。当然实际物理系统里这些值你是不知道的这里用仿真验证的好处就是有ground truth可以对比。3.2 激励轨迹设计轨迹设计是辨识里最容易被忽视、但影响最大的环节。你喂给算法的数据质量直接决定了辨识结果的上限。如果轨迹激励不充分某些参数对应的回归矩阵列几乎为零那这些参数就辨识不出来或者辨识结果方差极大。好的激励轨迹应该满足几个条件覆盖关节运动范围、包含丰富的频率成分、避免关节速度过高导致仿真不稳定、总时长适中。实践中用得最多的是有限项傅里叶级数轨迹q_i(t) q_i0 sum_{k1}^{N} [a_ik * sin(k*w*t) b_ik * cos(k*w*t)]其中q_i0是关节i的偏置位置a_ik和b_ik是傅里叶系数w是基频N是谐波数。这种轨迹的好处是周期性、光滑、容易求导、频率成分可控。参数选择上我一般取N5基频w根据总时长T来定w 2*pi/T。总时长T通常取10到20秒太短了数据量不够太长了仿真时间成本高。偏置位置q_i0取关节范围的中点振幅根据关节范围来定一般取范围的30%到40%。def fourier_trajectory(t, q0, a, b, w): 计算傅里叶级数轨迹的位置、速度、加速度 N a.shape[1] q q0.copy() qd np.zeros_like(q0) qdd np.zeros_like(q0) for k in range(1, N1): q a[:, k-1] * np.sin(k*w*t) b[:, k-1] * np.cos(k*w*t) qd a[:, k-1] * k*w * np.cos(k*w*t) - b[:, k-1] * k*w * np.sin(k*w*t) qdd -a[:, k-1] * (k*w)**2 * np.sin(k*w*t) - b[:, k-1] * (k*w)**2 * np.cos(k*w*t) return q, qd, qdd傅里叶系数的初始值可以随机生成但更好的做法是用优化方法找一个最优的激励轨迹。优化的目标通常是回归矩阵的条件数最小化因为条件数越小最小二乘解的数值稳定性越好。from scipy.optimize import minimize def cost_function(params, robot_id, joint_indices, T, dt): 优化目标回归矩阵条件数 # 解析params为傅里叶系数 # 生成轨迹计算回归矩阵 # 返回条件数 pass # 优化 result minimize(cost_function, initial_params, args(robot_id, joint_indices, T, dt), methodL-BFGS-B)这个优化问题是非凸的容易陷入局部最优。我的经验是多跑几次每次用不同的随机初始值取条件数最小的那个。另外可以在目标函数里加一个正则项惩罚过大的关节速度避免优化出来的轨迹在实际执行时超出限制。3.3 数据采集与回归矩阵构建轨迹设计好之后就可以在仿真里执行采集关节位置、速度、加速度和对应的力矩。T 15.0 # 总时长 dt 0.001 # 采样周期 steps int(T / dt) data_q [] data_qd [] data_qdd [] data_tau [] for step in range(steps): t step * dt q, qd, qdd fourier_trajectory(t, q0, a, b, w) # 用PyBullet计算逆动力学力矩 tau p.calculateInverseDynamics(robot_id, q.tolist(), qd.tolist(), qdd.tolist()) data_q.append(q) data_qd.append(qd) data_qdd.append(qdd) data_tau.append(tau)这里有个坑要注意PyBullet的calculateInverseDynamics返回的力矩不包含摩擦项。如果你在URDF里定义了关节摩擦它也不会自动加进去。所以如果你要辨识摩擦参数需要自己额外建模。对于纯惯性参数辨识这个接口是够用的。采集完数据下一步是构建回归矩阵。回归矩阵的构建是整个过程里最技术性的部分。对于每个采样时刻你需要计算一个矩阵Y_k使得tau_k Y_k * phi。把所有时刻的Y_k堆叠起来就得到总的回归矩阵。回归矩阵的具体形式取决于你用的推导方法。如果用牛顿-欧拉法可以借助PyBullet或者Pinocchio这样的库来自动计算。Pinocchio提供了computeJointTorqueRegressor接口可以直接输出回归矩阵非常方便。import pinocchio as pin model pin.buildModelFromUrdf(my_robot.urdf) data model.createData() def compute_regressor(model, data, q, qd, qdd): 计算回归矩阵 pin.computeJointTorqueRegressor(model, data, q, qd, qdd) return data.jointTorqueRegressor.copy()如果你不想引入Pinocchio也可以自己用SymPy推导。推导出来的回归矩阵可以导出成Python函数或者C代码然后在采集数据的时候调用。3.4 最小参数集的数值确定拿到回归矩阵之后下一步是确定最小参数集。我用的是QR分解法具体步骤如下def find_base_parameters(Y, tol1e-8): 通过QR分解找出最小参数集 Y: 回归矩阵形状 (n_samples * n_joints, n_params) 返回: 基参数索引以及参数组合矩阵 # 对Y做QR分解带列主元 Q, R, P scipy.linalg.qr(Y, pivotingTrue) # 找出R的对角线元素 diag_R np.abs(np.diag(R)) # 对角线元素小于阈值的列对应的参数是线性相关的 independent_indices np.where(diag_R tol)[0] # P是列置换independent_indices对应的是独立列 base_param_indices P[independent_indices] return base_param_indicesQR分解的列主元策略会把线性相关性最强的列排在后面所以对角线元素会从大到小排列。找到一个明显的断崖之后断崖之后的列就是线性相关的。实际操作中阈值的选取需要一点经验。太小了会把数值噪声当成独立参数太大了会丢掉真正独立的参数。我一般会画一下对角线元素的分布图找一个明显的拐点。确定基参数之后还需要计算参数组合矩阵。因为最小参数集里的每个基参数实际上是原始参数的一个线性组合。这个组合关系可以通过求解R矩阵得到。def compute_combination_matrix(R, P, independent_indices): 计算原始参数到基参数的映射矩阵 n_independent len(independent_indices) # R的前n_independent行和前n_independent列构成可逆矩阵 R11 R[:n_independent, :n_independent] R12 R[:n_independent, n_independent:] # 组合矩阵 beta np.linalg.solve(R11, R12) return beta3.5 参数估计与结果分析有了回归矩阵和基参数索引参数估计就是解一个最小二乘问题# 构建最小参数集对应的回归矩阵 Y_base Y[:, base_param_indices] # 最小二乘求解 phi_base, residuals, rank, sv np.linalg.lstsq(Y_base, tau_all, rcondNone) print(基参数估计值, phi_base) print(残差, residuals)如果你想让结果更鲁棒可以加一个正则化项变成岭回归from sklearn.linear_model import Ridge model Ridge(alpha1e-6) model.fit(Y_base, tau_all) phi_base model.coef_正则化系数alpha的选择需要交叉验证。太大了会导致估计偏差太小了起不到正则化作用。我一般从1e-8开始试逐步增大看验证集上的力矩预测误差。辨识完之后一定要做验证。验证的方法是用一组独立的轨迹和辨识轨迹不同来测试看预测力矩和真实力矩的误差。# 用验证轨迹计算预测力矩 tau_pred Y_val_base phi_base tau_true tau_val # 计算误差 error tau_pred - tau_true rmse np.sqrt(np.mean(error**2)) print(f验证集RMSE: {rmse:.6f} Nm)如果RMSE在0.1 Nm以下对于中小型机械臂来说就算不错了。如果误差很大可能是几个原因轨迹激励不充分、回归矩阵构建有误、或者仿真环境里有未建模的效应比如关节摩擦。4. 实操中踩过的坑与排查技巧4.1 仿真发散最常见也最头疼的问题仿真发散是动力学辨识里最常遇到的问题没有之一。表现就是跑着跑着关节力矩突然变成NaN或者数值爆炸到1e10以上。我遇到过好几次每次原因都不一样。第一个原因轨迹参数超出关节限制。傅里叶级数轨迹的振幅如果设得太大叠加之后可能超出关节的物理限位。PyBullet在超出限位时不会报错但力矩计算会变得不准确。解决办法是在生成轨迹之后先检查一遍关节范围把超限的部分截断或者缩放。第二个原因采样周期太大。如果你用1ms的采样周期但轨迹的频率成分很高比如基频0.5Hz5次谐波就是2.5Hz那么每个周期内的变化可能太快数值积分误差会累积。解决办法是减小采样周期或者降低轨迹的最高频率。第三个原因回归矩阵条件数太大。如果激励轨迹设计得不好回归矩阵的条件数可能达到1e10以上这时候最小二乘解对噪声极其敏感稍微一点数值误差就会导致参数估计值爆炸。解决办法是优化轨迹或者在求解时加正则化。排查技巧在仿真循环里加一个检查如果力矩的绝对值超过某个阈值比如1000 Nm就打印当前时刻的q、qd、qdd看看是哪个关节出了问题。大部分时候问题都出在某个关节的速度或加速度异常大。4.2 参数辨识结果不稳定方差大、重复性差有时候你会发现每次跑辨识得到的参数值都不一样而且差异很大。这说明辨识问题的条件数不好或者数据里的噪声太大。条件数不好的根源通常是轨迹激励不充分。某些参数比如某些连杆的惯量乘积项对力矩的影响很小只有在特定的运动模式下才会显现出来。如果你的轨迹里没有这种运动模式这些参数就辨识不准。解决办法有两个一是优化轨迹在目标函数里显式地惩罚条件数二是增加轨迹的多样性用多组不同参数的傅里叶轨迹把数据合并起来做辨识。# 多组轨迹合并 Y_all np.vstack([Y1, Y2, Y3]) tau_all np.concatenate([tau1, tau2, tau3]) phi_base, _, _, _ np.linalg.lstsq(Y_all[:, base_param_indices], tau_all, rcondNone)另外仿真里的数值噪声也是一个因素。PyBullet的逆动力学计算用的是浮点数精度有限。如果你用MuJoCo精度会好一些。但更根本的办法是对数据进行滤波把高频噪声滤掉。from scipy.signal import butter, filtfilt def lowpass_filter(data, cutoff, fs, order4): nyq 0.5 * fs normal_cutoff cutoff / nyq b, a butter(order, normal_cutoff, btypelow) return filtfilt(b, a, data, axis0) # 对力矩数据做低通滤波 tau_filtered lowpass_filter(tau_all, cutoff10, fs1/dt)截止频率的选择要看你的轨迹频率。一般取轨迹最高频率的2到3倍就够了。太高了滤不掉噪声太低了会把有用的信号也滤掉。4.3 最小参数集确定错误独立参数选多了或选少了QR分解的阈值选取是个经验活。阈值太小会把数值上接近零但实际不为零的列当成独立列导致最小参数集偏大阈值太大会把真正独立的列也丢掉导致模型不完整。我的做法是先画一下R矩阵对角线元素的对数分布图找一个明显的断崖。断崖之前的是独立参数之后的是相关参数。如果分布图上看不出明显的断崖那说明轨迹激励不够充分需要重新设计轨迹。import matplotlib.pyplot as plt diag_R np.abs(np.diag(R)) plt.semilogy(diag_R, o-) plt.xlabel(Column index) plt.ylabel(|R_ii|) plt.title(QR分解对角线元素分布) plt.grid(True) plt.show()另一个验证方法是用确定的最小参数集做辨识然后在验证集上看力矩预测误差。如果误差很小说明参数集选对了如果误差很大可能是漏掉了某些独立参数。4.4 常见问题速查表问题现象可能原因排查方法解决方案仿真发散力矩NaN轨迹超限、采样太快、条件数大检查q/qd/qdd范围缩放轨迹、减小dt、加正则化参数估计方差大激励不充分、噪声大看条件数、看残差优化轨迹、滤波、多组数据合并验证误差大参数集选错、模型不完整对比预测和真实力矩调整QR阈值、检查摩擦项某些参数始终为零该参数不可辨识检查回归矩阵列合并到基参数里重复性差数值噪声、初始值敏感多次运行对比固定随机种子、加正则化4.5 几个容易被忽视的细节第一个细节URDF里的惯量参考系。URDF里的惯量矩阵是相对于连杆坐标系的而且原点通常在连杆的某个特定位置比如关节轴线上。如果你自己推导回归矩阵一定要确认参考系一致否则辨识出来的参数物理意义会完全不对。第二个细节PyBullet的关节顺序。PyBullet返回的关节力矩顺序和URDF里定义的关节顺序可能不一致。如果你有fixed joint或者mimic joint顺序会更复杂。一定要先打印出来确认。第三个细节重力方向。仿真环境里的重力方向默认是(0, 0, -9.81)但有些URDF会定义不同的重力方向。如果你的辨识结果里重力项明显不对先检查这个。第四个细节数值精度。如果你用float32做计算精度可能不够。Python的numpy默认是float64但如果你从ROS topic里读数据可能是float32。做辨识之前统一转成float64。5. 从仿真到实机的迁移建议5.1 仿真里能验证什么不能验证什么仿真环境最大的价值是验证算法流程的正确性。你可以在仿真里确认回归矩阵构建对不对、最小参数集确定得准不准、参数估计算法收敛不收敛。这些在仿真里跑通了到实机上就只剩下数据采集和摩擦建模的问题。但仿真里验证不了的东西也很多。摩擦是最典型的。仿真里的关节摩擦通常是简化的库仑粘滞模型和真实减速器的摩擦特性差很远。柔性也是仿真里连杆是刚体真实机械臂的连杆和关节都有柔性会在高速运动时产生振动。传感器噪声更不用说仿真里的力矩数据是干净的实机上的力矩传感器有噪声、有偏置、有温漂。所以我的建议是仿真里把惯性参数辨识跑通实机上重点解决摩擦和噪声。5.2 实机数据采集的注意事项实机上做辨识数据采集是最大的挑战。你需要测量或估计关节力矩、位置、速度、加速度。位置可以用编码器速度可以差分或者用滤波器加速度通常需要二阶差分噪声会很大。力矩测量有几种方案关节力矩传感器最直接但成本高电机电流估计力矩成本低但精度受减速器效率影响基座力传感器可以间接估计但需要额外的解耦计算。我的经验是如果条件允许优先用关节力矩传感器。电流估计的力矩在低速和换向时误差很大会严重影响辨识精度。另外实机采集数据时要注意温度。电机和减速器发热会导致摩擦特性变化长时间采集数据时前后段的数据可能不一致。建议采集时间控制在几分钟以内或者分段采集每段之间让机械臂冷却。5.3 摩擦建模的补充实机辨识如果不建模摩擦惯性参数的估计值会有偏。最简单的摩擦模型是库仑粘滞tau_friction Fc * sign(qd) Fv * qd其中Fc是库仑摩擦系数Fv是粘滞摩擦系数。这两个参数可以和惯性参数一起辨识只需要把回归矩阵扩展一下把sign(qd)和qd对应的列加进去。更复杂的模型还包括Stribeck效应、静摩擦、温度依赖等但那些需要更多的实验数据来支撑。对于大多数应用场景库仑粘滞模型已经够用了。# 扩展回归矩阵加入摩擦项 Y_extended np.hstack([Y, np.sign(qd_all), qd_all]) phi_extended, _, _, _ np.linalg.lstsq(Y_extended, tau_all, rcondNone) # 最后两维就是摩擦系数 Fc_est phi_extended[-2] Fv_est phi_extended[-1]5.4 验证与迭代实机辨识做完之后一定要做闭环验证。开环的力矩预测误差小不代表闭环控制效果好。因为闭环控制里力矩误差会通过反馈回路放大。验证的方法是用辨识出来的参数设计一个计算力矩控制器在实机上跑一条轨迹看跟踪误差。如果跟踪误差在合理范围内比如关节位置误差小于0.01 rad说明辨识结果可用。如果误差很大可能需要回到辨识环节检查数据质量或者模型结构。迭代是常态。我做过的一个项目前后迭代了四五轮每轮都发现新的问题第一轮是摩擦没建模第二轮是加速度噪声太大第三轮是某些关节的激励不够第四轮是温度漂移。每解决一个问题辨识精度就提升一点。6. 一些个人体会这套东西我从最早用MATLAB手推回归矩阵到后来用Pinocchio自动生成再到最近用PyBullet做快速验证前后折腾了好几年。最大的体会是动力学辨识的难点不在算法在工程细节。算法本身是成熟的最小二乘、QR分解、傅里叶轨迹这些都是教科书里的东西。但实际做的时候你会发现80%的时间花在处理各种工程问题上轨迹超限了、数据有噪声了、参考系搞错了、单位不统一了。这些问题在论文里通常不会写但每一个都能让你卡好几天。另一个体会是仿真验证非常重要但不能替代实机。仿真里跑通只能说明你的算法逻辑没问题实机上还有一大堆仿真里遇不到的问题。我见过太多人仿真里结果漂亮得不行一到实机就完全不能用。最后说一个实用技巧把整个辨识流程脚本化。从轨迹生成、数据采集、回归矩阵构建、参数估计到验证全部写成脚本一键运行。这样你可以快速迭代改一个参数跑一遍看结果怎么变。手动操作的话改一次参数要折腾半天效率太低。# 一键辨识脚本的伪代码结构 def full_identification_pipeline(urdf_path, config): # 1. 加载模型 robot load_robot(urdf_path) # 2. 生成激励轨迹 trajectory design_trajectory(robot, config) # 3. 采集数据 data collect_data(robot, trajectory, config) # 4. 构建回归矩阵 Y, tau build_regressor(robot, data) # 5. 确定最小参数集 base_indices find_base_parameters(Y, config.tol) # 6. 参数估计 phi estimate_parameters(Y[:, base_indices], tau, config.regularization) # 7. 验证 error validate(robot, phi, base_indices, config.validation_trajectory) return phi, error这个脚本我用了很久每次换机械臂或者换仿真环境只需要改一下URDF路径和几个配置参数就能快速得到辨识结果。对于做算法迭代来说这种自动化程度是必须的。
