六自由度系统非线性动力学参数辨识：回归矩阵与Python最小二乘实现

参数辨识这件事，我最早是在一套六自由度系统的动力学测试里被迫啃下来的。当时系统方程是典型的多自由度耦合，惯性力、阻尼力、刚度力三个环节全都偏离教科书上的线性假设，尤其是六自由度系统动力学方程里那个随构型和速度变化的非线性惯性力，让传统的最小二乘辨识结果始终对不上实测力矩。后来我把整个流程拆开重做，从非线性动力学方程的建模、激励轨迹设计，到回归矩阵组装和Python代码落地，才算把参数误差压到可接受的范围。这篇东西就是当时完整思路的整理，适合正在做机器人关节辨识、结构动力学参数标定，或者想弄懂非线性系统参数辨识怎么落地的人参考。我会把六自由度系统里三种非线性力的物理含义、为什么能转成线性回归问题、以及完整可跑的Python代码一次讲清楚。

1. 六自由度系统里的三种非线性力：从哪来、长什么样

先说建模。一个六自由度系统，广义坐标记作 $\boldsymbol{q} = [q_1, q_2, \dots, q_6]^T$，广义速度 $\dot{\boldsymbol{q}}$，广义加速度 $\ddot{\boldsymbol{q}}$。大多数机械系统都可以写成下面这种逆动力学形式：

$$\boldsymbol{\tau} = \mathbf{M}(\boldsymbol{q})\ddot{\boldsymbol{q}} + \mathbf{C}(\boldsymbol{q}, \dot{\boldsymbol{q}})\dot{\boldsymbol{q}} + \mathbf{D}(\dot{\boldsymbol{q}}) + \mathbf{K}(\boldsymbol{q}) + \mathbf{G}$$

这里面每一项都跟力有关，但很多人一开始会忽略一个问题：$\mathbf{M}$ 不是常数，$\mathbf{C}$ 也不是简单比例阻尼。我们一项项拆。

1.1 非线性惯性力的本质：不是所有惯性力都跟加速度线性相关

线性系统的惯性力大家很熟，就是质量乘加速度 $m\ddot{x}$。但是六自由度系统里，惯性力还包括科氏力和离心力，也就是 $\mathbf{C}(\boldsymbol{q}, \dot{\boldsymbol{q}})\dot{\boldsymbol{q}}$ 这一项。这一项的典型特征是速度二次型：某个自由度的惯性力里会出现另外自由度的速度乘积，比如 $\dot{q}_i \dot{q}_j$。

我习惯用一个生活化的类比来理解它：你推一辆超市购物车，直线推很轻松，一旦你一边推一边转弯，会明显感到一个横向的“离心力”要把车拉向弯道外侧。这个力的大小跟你的推进速度和转弯速度的乘积成正比，而不是跟某个单一的加速度成线性关系。对六自由度机械系统来说，机器人手臂快速挥动时各关节之间会互相“拽”，就是这类非线性惯性力在起作用。

在本文的代码模型里，我把它做了工程化简化：第 $i$ 个自由度受到的非线性惯性力取为 $c_i \dot{q}i \dot{q}{i+1}$，其中 $\dot{q}_{i+1}$ 是相邻自由度的速度（最后一个自由度与第一个形成环形耦合）。这样既保留了“速度交叉耦合”的核心特征，又让代码不至于膨胀成满矩阵的 Christoffel 符号推导。

1.2 非线性阻尼力与非线性刚度力：工程中真正难啃的骨头

非线性阻尼力方面，常见形式有三种：

$$D_i(\dot{q}_i) = d_i \dot{q}_i + b_i \dot{q}_i^3$$

第一项是线性粘性阻尼，第二项是三阶阻尼。流体环境里的阻力就经常表现出这种非线性：速度越快，阻力增长得比线性更快。另外还有库仑摩擦这种带符号函数的形式，比如 $f_c \mathrm{sign}(\dot{q}_i)$，但由于它在零速附近不连续，做参数辨识时处理起来更麻烦，我通常会在数据预处理阶段先避开小速度区间，或者用连续化的 $\tanh$ 逼近。

非线性刚度力也不难遇到。橡胶衬套、大变形柔性铰链、电磁弹簧这类元件，恢复力和位移之间的关系通常带三次项：

$$K_i(q_i) = k_i q_i + k_{3i} q_i^3$$

$k_i$ 是线性刚度，$k_{3i}$ 是三次硬化系数。$k_{3i} > 0$ 时，位移越大刚度越硬，很多工程隔振器就是这种特性；$k_{3i} < 0$ 时则可能出现软化甚至跳跃现象。为了体现多自由度系统的耦合，我还给相邻自由度加了一对耦合弹簧项：

$$h_i (q_i - q_{i+1}) + h_{3i} (q_i - q_{i+1})^3$$

这一项让六自由度的模型真正“牵一发动全身”，辨识难度也跟着上来了。

把上面这些项加起来，我们这条非线性动力学方程就明确了：

$$\tau_i = m_i \ddot{q}i + c_i \dot{q}i \dot{q}{i+1} + d_i \dot{q}i + b_i \dot{q}i^3 + k_i q_i + k{3i} q_i^3 + h_i (q_i - q{i+1}) + h{3i} (q_i - q_{i+1})^3 + g_i$$

其中 $g_i$ 是偏置项，用来吸收重力或者传感器零漂。模型虽然简化过，但非线性惯性力、非线性阻尼力、非线性刚度力三样全齐，足够把参数辨识的核心方法完整走一遍。

2. 参数辨识的底层逻辑：把动力学方程变成一场“回归实验”

模型建好了，接下来面临的问题是：方程里有54个待定参数，怎么从测量数据里把它们“反演”出来？关键一步在于观察方程结构——所有待定参数都是以系数形式线性出现的。

2.1 参数线性化：让每个未知量站到自己的基函数后面

把上节的方程重新整理一下，每个参数拎出来放在前面，后面跟一个由测量状态组成的“基函数”：

$$\tau_i = m_i \cdot \ddot{q}i + c_i \cdot (\dot{q}i \dot{q}{i+1}) + d_i \cdot \dot{q}i + b_i \cdot \dot{q}i^3 + k_i \cdot q_i + k{3i} \cdot q_i^3 + h_i \cdot (q_i - q{i+1}) + h{3i} \cdot (q_i - q_{i+1})^3 + g_i \cdot 1$$

写成矩阵形式就是：

$$\boldsymbol{\tau} = \boldsymbol{\Phi}(\boldsymbol{q}, \dot{\boldsymbol{q}}, \ddot{\boldsymbol{q}}) , \boldsymbol{\theta}$$

这里 $\boldsymbol{\Phi}$ 叫回归矩阵，每一列对应一个基函数；$\boldsymbol{\theta}$ 是所有参数组成的列向量。回归矩阵只跟状态量（位置、速度、加速度）有关，而参数向量是未知常数。只要回归矩阵满秩，参数就能通过最小二乘一步求解：

$$\hat{\boldsymbol{\theta}} = (\boldsymbol{\Phi}^T \boldsymbol{\Phi})^{-1} \boldsymbol{\Phi}^T \boldsymbol{\tau}$$

这就是参数辨识里最经典的“线性最小二乘”路线。很多朋友一听到“参数辨识”就以为必须上卡尔曼滤波、神经网络，其实只要你模型的未知量是线性系数的形式，最小二乘就是又快又稳的首选方法。

2.2 可辨识问题：回归矩阵列满秩只是第一步

理论上看，线性最小二乘似乎有闭式解就够了，但工程上真正的坑在“可辨识性”。回归矩阵 $\boldsymbol{\Phi}$ 的每一列必须彼此线性独立，否则你解出来的参数不是它本来的值，而是某几列的组合值。

我举个具体的例子：如果激励轨迹里每个自由度的速度始终等于另一个自由度的速度，那么 $\dot{q}_i$ 和 $\dot{q}i\dot{q}{i+1}$ 这两列就会高度相关，$d_i$ 和 $c_i$ 就没法分开辨识。六自由度系统激励不充分时，这种列相关性非常常见。

实操中我一般通过两个指标判断可辨识性：

• 条件数：$\boldsymbol{\Phi}^T\boldsymbol{\Phi}$ 的条件数越大，说明某些参数组合越难分开。我自己的经验阈值是，条件数小于 $10^3$ 算健康，超过 $10^6$ 基本宣告有列不可辨识。
• 奇异值分布：看奇异值是否出现悬崖式掉阶。掉下去了，说明有几个等效参数应该合并或删掉。

参数线性化是整个辨识流程的地基，如果建模时发现某个参数以指数、对数或者乘积形式跟其他未知量纠缠在一起，那就要退回到非线性优化框架（比如 Levenberg-Marquardt），这个我后面的代码里也会给一个备用方案。

3. 激励轨迹设计与实测数据的预处理

回归矩阵满不满秩、条件数大不大，几乎完全取决于你给系统喂什么样的激励轨迹。这跟给人做体检一样，你只测静态指标就查不出动态问题，你只给一个方向激励就辨识不出六自由度之间的耦合项。

3.1 六自由度同时激励的多正弦轨迹

参数辨识领域最常用的激励是傅里叶级数形式的多正弦叠加。对第 $i$ 个自由度：

$$q_i(t) = \sum_{l} A_{i,l} \sin(\omega_{i,l} t + \phi_{i,l})$$

位置、速度、加速度都能解析求导，仿真数据干净；实际实验中，多正弦激励的频带集中，能量可控，峰值因子低，不像阶跃或白噪声那样容易把系统推向非线性失稳。

我给六个自由度分别随机生成三组频率在0.4到1.6 Hz之间的正弦分量，每个自由度的频谱尽量错开，确保六自由度同时被不同频段的信号激励。这一步直接用代码实现：

def excitation_trajectory(t, seed=7): rng = np.random.default_rng(seed) q = np.zeros((len(t), 6)) v = np.zeros_like(q) a = np.zeros_like(q) for i in range(6): for _ in range(3): om = 2 * np.pi * rng.uniform(0.4, 1.6) amp = rng.uniform(0.15, 0.45) phi = rng.uniform(0, 2 * np.pi) q[:, i] += amp * np.sin(om * t + phi) v[:, i] += amp * om * np.cos(om * t + phi) a[:, i] -= amp * om**2 * np.sin(om * t + phi) return q, v, a

这个函数不仅为后续辨识生成激励信号，也承担了“持续激励条件”的保障：频率足够丰富、幅值足够大、六个自由度同步被激励，回归矩阵才可能列满秩。

3.2 从位置测量到加速度：数值微分的代价

理论仿真里，位置、速度、加速度都是解析得到的，但实测中你通常只能直接测到位置（比如编码器）和一部分速度（比如速度观测器输出），加速度往往没有传感器直接可测。于是大家很自然地想到：把位置或者速度信号数值微分一次，得到加速度。

这里就埋了大坑。数值微分本质是高频噪声放大器，位置测量的微小抖动，差分之后会变成加速度上的大幅振荡。简单差分 $N$ 个点，噪声方差大约会被放大 $2/dt$ 倍；二次差分更是要命，与 $1/dt^2$ 成正比。采样率越高、单点噪声越大，加速度信号就越没法看。

我的处理流程是两步：先把速度信号做 Savitzky-Golay 滤波平滑，再做一次差分得到加速度。Savitzky-Golay 滤波本质是局部多项式拟合，比滑动平均更能保留信号形状，参数选窗口长度31、多项式阶数3，兼顾平滑和保形。代码里这样写：

from scipy.signal import savgol_filter v_smooth = savgol_filter(v_m, window_length=31, polyorder=3, axis=0) a_m = np.gradient(v_smooth, dt, axis=0)

注意这里仍然有残余噪声，后面第6章我会单独讨论这个环节的翻车细节。

4. Python实现：从数据生成到参数对比的完整代码

下面进入正题。整套代码分四块：定义真值参数与动力学模型、组装回归矩阵、生成激励与测量数据、执行最小二乘辨识并输出对比结果。把所有块拼在一起就是一个完整的六自由度系统动力学参数辨识实验，可以直接跑。

4.1 系统数据结构与真值参数设定

为了能验证辨识效果，我先“制造”一组真值参数，用它生成测量数据，再尝试反解回去。如果反解出来的参数和真值很接近，就说明辨识流程本身没问题。

每个自由度有9个参数：$m_i, c_i, d_i, b_i, k_i, k_{3i}, h_i, h_{3i}, g_i$，六自由度共计54个参数。我用一个一维数组存储，每9个元素对应一个自由度：

import numpy as np n_dof = 6 block_names = ["m", "c", "d", "b", "k", "k3", "h", "h3", "g"] theta_true = np.array([ 1.20, 0.35, 0.80, 0.12, 3.50, 0.45, 0.55, 0.22, 0.10, 1.10, 0.28, 0.75, 0.15, 3.80, 0.40, 0.62, 0.19, 0.12, 0.90, 0.22, 0.60, 0.10, 2.90, 0.38, 0.48, 0.18, 0.08, 1.00, 0.18, 0.70, 0.11, 3.10, 0.33, 0.52, 0.25, 0.09, 1.30, 0.25, 0.85, 0.14, 3.60, 0.50, 0.60, 0.21, 0.11, 1.15, 0.30, 0.65, 0.09, 3.20, 0.42, 0.44, 0.17, 0.13, ])

这里 $m$ 是主惯性参数，$c$ 是非线性惯性力系数，$d$ 和 $b$ 分别是线性和三次阻尼系数，$k$ 和 $k3$ 分别是线性和三次刚度系数，$h$ 和 $h3$ 是相邻自由度耦合弹簧系数，$g$ 是偏置。

4.2 动力学模型与回归矩阵组装

动力学模型接收当前时刻的位置、速度、加速度，返回六个自由度上的广义力。这条函数就是前面非线性动力学方程的直接翻译：

def dynamics_tau(theta, q, v, a): tau = np.zeros(n_dof) for i in range(n_dof): j = (i + 1) % n_dof off = i * 9 tau[i] = ( theta[off + 0] * a[i] + theta[off + 1] * v[i] * v[j] + theta[off + 2] * v[i] + theta[off + 3] * v[i] ** 3 + theta[off + 4] * q[i] + theta[off + 5] * q[i] ** 3 + theta[off + 6] * (q[i] - q[j]) + theta[off + 7] * (q[i] - q[j]) ** 3 + theta[off + 8] ) return tau

组装回归矩阵时，把每一个参数对应的基函数填到对应的列里。由于动力学方程是逐自由度建立的，最终的回归矩阵 $\boldsymbol{\Phi}$ 也按照“自由度行块”来组织：前 $N$ 行对应自由度1的 $N$ 个采样时刻，后面依次排下去。代码如下：

def build_regressor(q, v, a): N = q.shape[0] Phi = np.zeros((N * n_dof, n_dof * 9)) col_names = [] for i in range(n_dof): r0 = i * N off = i * 9 j = (i + 1) % n_dof Phi[r0:r0 + N, off + 0] = a[:, i] Phi[r0:r0 + N, off + 1] = v[:, i] * v[:, j] Phi[r0:r0 + N, off + 2] = v[:, i] Phi[r0:r0 + N, off + 3] = v[:, i] ** 3 Phi[r0:r0 + N, off + 4] = q[:, i] Phi[r0:r0 + N, off + 5] = q[:, i] ** 3 Phi[r0:r0 + N, off + 6] = q[:, i] - q[:, j] Phi[r0:r0 + N, off + 7] = (q[:, i] - q[:, j]) ** 3 Phi[r0:r0 + N, off + 8] = 1.0 for name in block_names: for i in range(n_dof): col_names.append(f"{name}{i+1}") return Phi, col_names

这个组装过程是全部代码里最需要盯紧的地方，列顺序和动力学模型里的参数顺序必须严格一一对应。我曾在实际项目里因为调换了 $d$ 和 $b$ 两块参数的位置，导致明明数据没问题，辨识结果却系统性偏差，最后还是逐个核对列名才找到原因。

4.3 最小二乘参数辨识主流程

生成30秒激励数据，采样3000个点，加噪声模拟实测。位置噪声标准差0.002，速度噪声0.005，力矩噪声0.02，然后走“速度平滑、差分得加速度、组装回归矩阵、最小二乘”的完整链路：

T = 30.0 N = 3000 t = np.linspace(0, T, N) dt = t[1] - t[0] q, v, a = excitation_trajectory(t, seed=7) tau_true = np.zeros((N, n_dof)) for k in range(N): tau_true[k] = dynamics_tau(theta_true, q[k], v[k], a[k]) rng1 = np.random.default_rng(1) rng2 = np.random.default_rng(2) rng3 = np.random.default_rng(3) sigma_q = 0.002 sigma_v = 0.005 sigma_tau = 0.02 q_m = q + rng1.normal(0, sigma_q, q.shape) v_m = v + rng2.normal(0, sigma_v, v.shape) tau_m = tau_true + rng3.normal(0, sigma_tau, tau_true.shape) v_smooth = savgol_filter(v_m, window_length=31, polyorder=3, axis=0) a_m = np.gradient(v_smooth, dt, axis=0) Phi, names = build_regressor(q_m, v_m, a_m) y = tau_m.reshape(-1) theta_hat, residuals, rank, singular_vals = np.linalg.lstsq(Phi, y, rcond=None)

np.linalg.lstsq返回的rank和singular_vals是判断可辨识性的重要依据。如果rank小于54，说明回归矩阵缺秩，有一部分参数没有充分激励，这时候要么调整激励轨迹，要么检查模型是否有过参数化。

4.4 通用非线性形式下的优化兜底

如果模型中存在参数非线性进入方程的情况，比如未知量出现在指数位置或者分母上，就不能用最小二乘闭式解了。这时候用scipy.optimize.least_squares做非线性优化兜底。思路是给定初始猜测，反复迭代让残差最小：

from scipy.optimize import least_squares def residual(theta): return (Phi @ theta) - y theta_init = theta_true * 0.8 result = least_squares(residual, theta_init, method="lm") theta_hat_nonlin = result.x

注意非线性优化对初值敏感，我一般会用最小二乘的结果当作初值，再做一次迭代精修。这个策略在实际项目中救过我很多次。

5. 辨识结果的验证方法与精度评估

参数解出来了不能直接拿去用，至少要做两件事：参数本身对不对，模型预测力矩准不准。

5.1 参数相对误差与预测力矩对比

参数相对误差按自由度逐个分组计算：

rel_err = np.abs(theta_hat - theta_true) / np.abs(theta_true) print("平均相对误差: {:.4f}".format(np.mean(rel_err)))

用辨识出的参数重新计算预测力矩：

tau_pred = (Phi @ theta_hat).reshape(N, n_dof) ss_res = np.sum((tau_true - tau_pred) ** 2) ss_tot = np.sum((tau_true - tau_true.mean(axis=0)) ** 2) r2 = 1 - ss_res / ss_tot print("R2: {:.4f}".format(r2))

我这里习惯同时画两张图：一张是54个参数的真值和辨识值柱状对比，一张是某几个自由度上真实力矩和预测力矩的时间序列。前者直接暴露个别参数的误差，后者验证整条动力学方程的拟合能力。如果柱状图里某一根柱子明显偏差，但时间序列却拟合得很好，十有八九是参数可辨识性出了问题，预测曲线被几个强相关参数“骗”过了。

我把典型结果列出来作为基准参考：

5.2 噪声水平对辨识精度的影响

工程问题里噪声水平决定你结果的信任度。我写了个小循环，把速度噪声从0.001逐步加大到0.1，记录平均参数误差的变化规律：

sigmas = [0.001, 0.005, 0.01, 0.02, 0.05, 0.1] for sigma in sigmas: v_noisy = v + rng2.normal(0, sigma, v.shape) v_s = savgol_filter(v_noisy, window_length=31, polyorder=3, axis=0) a_diff = np.gradient(v_s, dt, axis=0) Phi_n = build_regressor(q_m, v_noisy, a_diff)[0] theta_n, _, _, _ = np.linalg.lstsq(Phi_n, y, rcond=None) err = np.mean(np.abs(theta_n - theta_true) / np.abs(theta_true)) print(f"sigma={sigma:.3f}, 平均相对误差={err:.4f}")

这个测试很能说明问题：随着速度噪声增大，参数误差几乎线性上升，而且受影响最大总是 $b$（三次阻尼）和 $h3$（三次耦合刚度）这些高次项参数。原因是高次基函数的信噪比更低，同样的位置/速度噪声，在三次项上被放得更厉害。

6. 实测中踩过的坑与几个值得注意的细节

这章讲的全是实际项目里踩出来的经验，每条都对应过真金白银的返工时间。

6.1 ddq噪声放大一定要平滑，但别过度平滑

我前面用的是 Savitzky-Golay + 差分，窗口长度31。实际操作中，窗口太短平滑效果差，窗口太长又会削掉真实的加速度峰值，造成系统性偏差。一个直观的判断方法：用平滑后的加速度反算预测力矩，如果残差在某个自由度上呈现出明显的“振铃”，多半是窗口太长把信号削平了。建议先用不含噪声的仿真数据测试一遍，调好窗口参数再上实测数据。

6.2 激励幅值要足够大，尤其要激发出高次项

很多参数辨识失败的根源不是算法不行，而是激励信号根本没把非线性特征“叫醒”。三次刚度项 $q_i^3$ 在幅值0.1和幅值0.5时的差异是125倍；如果激励幅值太小，$q_i^3$ 列里的数值全都接近0，回归矩阵的条件数直接爆表。我至少会做一次不同幅值下的条件数扫描，确保高次项在激励数据里真有“存在感”。

6.3 强相关列与岭回归

有些系统结构天生会让两个参数强相关，即使回归矩阵满秩，条件数也可能高得吓人。比如质量 $m_i$ 和偏置 $g_i$，如果激励加速度均值恰好为0，公平地说大部分辨识场景没这个问题；但如果轨迹设计不好，比如所有激励频率落到共振点附近，基函数之间就会互相“串”。我的兜底手段是岭回归，给 $\boldsymbol{\Phi}^T\boldsymbol{\Phi}$ 加一个小对角项：

reg_param = 1e-4 theta_ridge = np.linalg.solve(Phi.T @ Phi + reg_param * np.eye(54), Phi.T @ y)

岭回归会引入一点偏差，但能显著降低参数方差。实际使用时要试几个reg_param，看参数估计值趋于稳定了再定。

6.4 辨识数据与验证数据必须分离

这是最让我“印象深刻”的一条。我做过一次项目，用同一组激励数据既辨识参数又验证拟合度，R2做到了0.99，自我感觉良好。结果换一组轨迹做交叉验证，预测误差大了将近一倍。原因是辨识模型把第一组轨迹里的特殊耦合“背下来”了，而不是真正学到动力学规律。后来我固定用两组不同的激励轨迹，一组专门辨识，一组专门验证，交叉验证的R2才是你模型真实水平的度量。

6.5 别忽视偏置项的物理意义

$g_i$ 虽然叫偏置，实际上吸收的是重力、传感器零漂、电磁力叠加后的综合效应。如果系统工作在不同姿态下，$g_i$ 可能不是一个常数。这种情况建议把数据集按姿态分段辨识，或者把姿态变量显式建模进基函数里。我吃过的亏是：整个数据集直接用常数偏置辨识，精度尚可，但把参数移植到另一个姿态范围时，预测误差明显变大，最后只能分段建模解决。

代码跑通之后，这套流程就可以直接套用在你自己的六自由度系统上。我个人的体会是，参数辨识七成功夫在建模和实验设计，剩下三成才轮到算法。你把方程里的每一项物理含义都搞清楚了，知道回归矩阵每一列对应什么，再复杂的非线性系统也能顺利拆解成一次靠谱的最小二乘问题。最后再分享一个小技巧：把辨识结果打印成带列名的表格，跟真值并排放，肉眼扫一眼哪个自由度的高次项偏差大，往往能直接指向你模型里被忽略的那个物理效应。