简介面向机械、航空航天与土木工程领域的研究人员和工程师这份动力学参数辨识代码基于MATLAB平台编写主要解决系统动力学建模、参数估计与模型验证问题。压缩包内共包含41个文件整体大小约5.45MB文件类型涵盖数据文件、脚本程序、仿真模型、架构说明与许可证等其中数据文件用于存放激励轨迹和观测数据脚本程序包含主流程与核心算法仿真模型提供可视化验证环境架构图帮助快速理解代码结构。代码流程覆盖数据预处理、模型选择、基于辨识工具箱的参数估计、结果验证以及仿真控制等典型环节并留有A矩阵、B矩阵、动力矩阵等关键计算模块可支撑机械臂等对象完成从数据到模型的完整辨识实践。目前已有3329人学习或下载具备一定社区验证基础借助注释与说明文档读者可梳理辨识思路并迁移到自有系统中进而提升工程设计与控制器调优的精度和效率。1. 什么是动力学参数辨识为什么要用 MATLAB 写这件事动力学参数辨识在机器人学和运动控制里是一门绕不开的手艺。机械臂或者运动平台的标称惯性参数在仿真和动力学前馈控制中往往严重失真。你用厂家给的连杆质量和惯性张量去建模型关节摩擦、电机转子惯量、减速器效率一掺和前馈力矩和实际需要的力矩能差出 20% 以上。动力学参数辨识要解决的就是通过激励轨迹让机器人动起来记录关节角、角速度和力矩再从这些带噪声的数据里反推出每个连杆的质量、质心位置、惯性张量分量以及摩擦力参数。这一类问题本质上是线性最小二乘问题因为动力学方程可以改写成回归矩阵乘以惯性参数向量的形式。这也是为什么 MATLAB 在这个场景下特别顺手符号工具箱帮你把动力学方程化成线性形式优化工具箱做激励轨迹优化曲线拟合工具箱处理带噪声的数据最后还能用 Simulink 搭闭环验证。本文按一套完整流程来讲先讲清回归模型和最小二乘为什么成立再给可直接跑的 MATLAB 辨识代码然后补激励轨迹设计口径最后说实机应用时那些让人失眠的数值病态问题和修正手段。新手能够按步骤把仿真辨识跑通老手也能在这里找到关于正则化和激励轨迹优化的横向对比。2. 动力学模型线性化与参数向量回归矩阵的 MATLAB 构建2.1 为什么要做模型线性化一个 n 关节机械臂的逆动力学方程用拉格朗日或牛顿欧拉法写出来是这样的M(q)q¨ C(q, q˙)q˙ G(q) F(q˙) τM 是惯性矩阵C 是科氏力和离心力项G 是重力项F 是摩擦力。重点是这个非线性方程的每个系数项其实都可以化成一个关于已知运动量的函数与未知惯性参数的线性乘积之和。也就是τ Y(q, q˙, q¨) · π其中 Y 是一个 n 行、p 列的函数矩阵称为回归矩阵每一项由广义坐标、速度和加速度的数值构成π 是 p 维惯性参数向量包含连杆质量、质心坐标三个分量、惯性张量的六个独立分量以及摩擦系数。只要机械结构固定这个分解关系就是确定的不随运动轨迹改变。有了这一形式参数辨识就被转化成一个标准的线性回归问题只要采集多组运动数据和对应的力矩数据把方程竖着摞起来就能解。这种线性化与你选用的动力学建模方法无关。用拉格朗日方程手工推导再编程的表出往往非常冗长我一般建议在 MATLAB 中用符号工具箱按牛顿欧拉递推来做。还有一种做法是把每一个待辨识的惯性参数视为一个符号变量代入含诸多未知数的向量中参与动力学表达式运算最后利用equationsToMatrix把方程拆成 Y 和 π 两个部分。2.2 利用 Symbolic Toolbox 自动构建回归矩阵在 MATLAB 中构建回归矩阵一个可行的做法是从注册的动力学公式出发将各惯性参数符号化先生成中间变量表达式再提取 Y 和 π。下面给出一个二连杆平面机械臂的完整构建示例这套写法可以直接推广到六轴关节臂。clear; clc; syms q1 q2 dq1 dq2 ddq1 ddq2 real % 关节位置、速度、加速度 syms m1 m2 real % 连杆质量 syms l1 l2 lc1 lc2 real % 杆长与质心到关节的距离 syms I1 I2 real % 绕质心的转动惯量 syms g real syms Fv1 Fv2 Fc1 Fc2 real % 粘滞摩擦与库仑摩擦 % 构造运动学变量二连杆机械臂角度从水平轴线起算 p1 [l1*cos(q1); l1*sin(q1)]; p2 p1 [l2*cos(q1q2); l2*sin(q1q2)]; pc1 [lc1*cos(q1); lc1*sin(q1)]; pc2 p1 [lc2*cos(q1q2); lc2*sin(q1q2)]; Jv1 jacobian(pc1, [q1 q2]); % 质心1的平动雅可比 Jv2 jacobian(pc2, [q1 q2]); % 质心2的平动雅可比 % 动能平动部分 转动部分 KE 0.5*m1*(Jv1*[dq1;dq2]).*(Jv1*[dq1;dq2]) ... 0.5*m2*(Jv2*[dq1;dq2]).*(Jv2*[dq1;dq2]) ... 0.5*I1*dq1^2 0.5*I2*(dq1dq2)^2; % 重力势能 PE m1*g*pc1(2) m2*g*pc2(2); % 拉格朗日方程求逆动力学 L KE - PE; q [q1 q2]; dq [dq1 dq2]; ddq [ddq1 ddq2]; tau_sym sym(zeros(2,1)); for i 1:2 dL_ddq diff(L, dq(i)); d_dt_dL_ddq jacobian(dL_ddq, [q dq]) * [dq ddq].; tau_sym(i) d_dt_dL_ddq - diff(L, q(i)); end % 加入摩擦力模型 tau_sym tau_sym [Fv1*dq1 Fc1*sign(dq1); Fv2*dq2 Fc2*sign(dq2)]; % 将包含符号参数的方程转化为线性形式 Y*pi params [m1; m2; I1; I2; lc1*m1; lc2*m2; Fv1; Fv2; Fc1; Fc2]; [Y_sym, pi_sym] equationsToMatrix(tau_sym, params);这段代码里有两个关键设计。一个是用equationsToMatrix把非线性负载提取成线性映射关系它以各待辨识参数为未知量重新整理了方程组输出pi_sym是参数向量表达输出Y_sym则是回归矩阵的符号形式。另一个是摩擦力的符号在辨识实验里关节速度信号在过零的时候sign项会咔哒跳变实际处理时往往用tanh(k*dq)代替k 取 30 到 100避免数值振荡。2.3 参数向量重叠与可辨识性检查你把上面代码输出的 Y_sym 拉出来看会发现一个现象某些参数向量组合在回归矩阵里的列永远是线性相关的。比如二连杆机械臂中lc1*m1和I1的列就可能部分重叠。这是因为惯性参数在动力学方程里通常以惯性张量加质量和质心的组合形式出现直接解线性方程组必然遇到秩不足。所以在做辨识之前必须对 Y 矩阵做可辨识性判别。MATLAB 中的做法是把各列对数据进行占位用数值矩阵的秩来判断。% 假设 Y_sym 已经是符号矩阵先生成数值样本 q1s 0.2; q2s 0.1; dq1s 0.5; dq2s 0.3; ddq1s 0.4; ddq2s 0.2; Y_num double(subs(Y_sym, {q1,q2,dq1,dq2,ddq1,ddq2}, ... {q1s,q2s,dq1s,dq2s,ddq1s,ddq2s})); % 从符号参数向量中映射到实际数值参数 [~, ~, ~, ~, ~, ~, ~, ~, ~, ~] deal(0); pi_vals [2.1; 1.2; 0.05; 0.03; 0.15; 0.22; 0.8; 0.6; 1.2; 0.9]; % 检验回归矩阵的秩与参数个数比较 r rank(Y_num); fprintf(Y矩阵秩: %d, 参数个数: %d\n, r, length(params));对于辨识实验设计一定要在大量覆盖工作空间的采样点上计算矩阵条件数而不能只用一个点判断。常见做法是把有限傅里叶级数激励轨迹的位置、速度、加速度代入 Y在所有时间采样点上堆叠形成全域回归矩阵再计算它的秩和条件数。这一步没做后面解出来的参数哪怕残差很小也是一堆方向和量纲都错得离谱的数。3. 基于测量数据的最小二乘辨识 MATLAB 实现3.1 数据对齐与滤波预处理仿真环境里关节力矩是已知的但实际伺服系统采集到的力矩指令是含噪声的而且速度信号多数情况下是从编码器位置差分来的差分会进一步放大噪声。预处理这一环我认为最重要的两件事是把位置、速度、力矩三条序列在时间轴上严格对齐以及用零相位滤波器去处理速度信号。零相位滤波一般用filtfilt来做它可以消除普通filter引入的相位滞后但注意必须在离线批次处理中使用。这里给出一个实际操作中的处理流程。% fs: 采样率已知假设为 1000 HzT 为采样间隔 fs 1000; T 1/fs; t (0:N-1)*T; % N 为采样长度 % 设计巴特沃斯低通滤波器 fc 20; % 截止频率根据激励信号带宽来定 [b, a] butter(2, fc/(fs/2), low); % 对速度和力矩做零相位滤波 dq_filt filtfilt(b, a, dq_raw); tau_filt filtfilt(b, a, tau_raw); % 微分得到加速度 q cumtrapz(t, dq_filt); % 位置也可以由速度数值积分获得 ddq_filt [0; diff(dq_filt)/T]; ddq_filt filtfilt(b, a, ddq_filt); % 再次滤波消除差分噪声这段代码有个很实际的经验点加速度尽量不要用二阶差分得到否则噪声会被放大得面目全非。如果在测试台上直接有加速度计或驱动器内部的速度观测器输出就优先使用如果只有编码器那么正确的姿势是对位置做三次多项式样条平滑后解析求导或者像上面这样对速度滤波后再中心差分并再滤波。关于滤波引入的问题filtfilt是零相位的但它会延续数据边缘效应因此要对采集数据的两端做预取舍把启动加速段和减速停止段都裁掉。我见过太多人在辨识数据里包含启动瞬间力矩尖峰最小二乘残差被这几个点带歪。3.2 构建堆叠回归矩阵并求解将 K 个时间采样点的回归矩阵上下堆叠同时堆叠对应的力矩向量形成超定方程组τ_stack Y_stack · π样本点数量 k 至少要大于参数个数 p 的 5 到 10 倍才能抑制单点噪声影响。这里给出完整的最小二乘求解代码附带加权和输出验证。% 假设已经得到了多组实验的 q/dq/ddq/tau 序列 % 每组实验是激励轨迹的一条周期多条数据拼接以增加激励充分性 Y_stack []; Tau_stack []; for trial 1:num_trials % 获取本组数据长度为 n_i qi data(trial).q; dqi data(trial).dq; ddqi data(trial).ddq; taui data(trial).tau; % 计算每个采样点的回归矩阵并堆叠 Yi zeros(3*length(qi), length(params)); % 假设为三关节 for k 1:length(qi) % 代入数值得到 Y 矩阵(用符号函数句柄) Yk double(subs(Y_func, {q1,q2,q3,dq1,dq2,dq3,ddq1,ddq2,ddq3}, ... {qi(k,1),qi(k,2),qi(k,3), ... dqi(k,1),dqi(k,2),dqi(k,3), ... ddqi(k,1),ddqi(k,2),ddqi(k,3)})); Yi((k-1)*31:k*3, :) Yk; end Y_stack [Y_stack; Yi]; Tau_stack [Tau_stack; taui(:)]; end % 加权最小二乘力矩测量噪声方差做权重 W diag(1./var_tau); % 简单起见各通道独立 pi_hat (Y_stack * W * Y_stack) \ (Y_stack * W * Tau_stack); % 预测力矩与残差 Tau_pred Y_stack * pi_hat; residual Tau_stack - Tau_pred; rms_error sqrt(mean(residual.^2)); fprintf(辨识后 RMS 力矩残差: %.4f Nm\n, rms_error);求解这一步最需要注意的是条件数。力矩的量纲有两种旋转关节是 Nm移动关节是 N不同关节数据拼接后数值量级可能差三个数量级。此时如果不做加权数值上小力矩关节的信息会直接被大力矩关节淹没。加权矩阵 W 一般取各关节力矩方差的倒数这样既符合最大似然意义也能缓解量纲不平衡带来的病态问题。3.3 参数正定性与摩擦力约束的投影最小二乘解出来的参数不保证物理可行。质量必须为正、惯性张量必须正定、质心位置不应超出连杆几何边界。这时候需要做参数投影或施加不等式约束。严格做法是把问题转化成二次规划用quadprog或lsqlin直接加线性不等式约束。% 将物理约束写成线性不等式 A*x b % 例要求质量大于下限 mass_vars 1:num_mass; % 质量参数在向量中的索引 A_mass -eye(num_mass); A_mass [A_mass, zeros(num_mass, length(params)-num_mass)]; b_mass -0.05 * ones(num_mass, 1); % 质量下限 0.05 kg % 合并其他约束例如质心位置限定 A_ineq A_mass; b_ineq b_mass; % 使用线性最小二乘带不等式约束 opts optimoptions(lsqlin, Display, off, Algorithm, interior-point); pi_bounded lsqlin(Y_stack, Tau_stack, A_ineq, b_ineq, [], [], ... lb, ub, [], opts);投影到可行域之后再用这个pi_bounded重新计算力矩残差你会发现和未约束求解相比残差略有增大但得到的参数放在仿真里却靠谱得多。这一点对后续控制仿真和轨迹优化都是决定性的因为正定参数才能保证前馈力矩的方向与期望运动一致。4. 激励轨迹生成与辨识效果的仿真验证4.1 为什么用有限傅里叶级数激励轨迹辨识的核心是回归矩阵的条件数要小。如果机器人只做单一姿态或缓慢运动回归矩阵的很多列都会退化最后解出来的参数就是噪声放大器。要让数据把参数空间内的方向都充分激励到需要轨迹在位置、速度和加速度三个层次上都持续变化且频率成分丰富。工程上最成熟的做法是周期性的有限傅里叶级数轨迹(Fourier series trajectory)q_i(t) q_i0 Σ [a_in/(w_n) * sin(w_n t) - b_in/(w_n) * cos(w_n t)]这个形式的好处是位置对时间的偏导和二阶偏导都能解析求取不需要数值微分同时它是周期的拼接多周期数据能把随机噪声平均掉。频率基频一般选在 0.10.5 Hz谐波次数取 510 次这样加速度不会过大又保证了激励带宽充足。4.2 在 MATLAB 中生成辨识轨迹并做数值采样用fmincon优化傅里叶系数来最小化回归矩阵条件数。目标函数里要同时把关节限位和速度、加速度限位作为约束。如果对全局搜索不熟可以先给一组手工确定的系数看条件数是否在可接受范围再迭代优化。% 有限傅里叶级数轨迹生成函数 function [q, dq, ddq, t] fourier_traj(coeff, T_period, dt) % coeff: 每关节 [a_n; b_n] 排列的列向量 % T_period: 基频周期即最慢谐波的周期 % dt: 采样时间 t 0:dt:T_period; w0 2*pi / T_period; N_harm (length(coeff)/2 - 1); % 谐波阶数 q zeros(length(t), length(coeff)/2); dq q; ddq q; for k 1:length(t) tk t(k); for j 1:size(q,2) q(k,j) coeff(j); % 零阶偏置量 dq(k,j) 0; ddq(k,j) 0; idx 1; for n 1:N_harm an coeff(j n); bn coeff(j 2*N_harm n); wn n*w0; q(k,j) q(k,j) (an/wn)*sin(wn*tk) - (bn/wn)*cos(wn*tk); dq(k,j) dq(k,j) an*cos(wn*tk) bn*sin(wn*tk); ddq(k,j) ddq(k,j) - an*wn*sin(wn*tk) bn*wn*cos(wn*tk); end end end end这段轨迹函数的重要参数一个是谐波数它决定了激励频率中心另一个是每个关节的正弦/余弦幅值它决定关节活动范围和角速度峰值。幅值设置得要逼近关节限位但必须留安全余量尤其轨迹优化迭代阶段若触到限位约束的梯度计算容易失败。4.3 在 Simulink 中做闭环辨识验证仿真验证的目的是回答一个问题如果给一套真实参数、把辨识得到的参数拿回去做前馈力矩残差会被压低多少我的做法是在 Simulink 里搭一个逆动力学模块和一个机器人模型用同一激励轨迹同时驱动机器人和逆动力学模块。mdl robot_identification_verify; open_system(mdl); % 仿真设置使用变步长 ode45相对容差 1e-6 set_param(mdl, Solver, ode45, RelTol, 1e-6); % 注入激励轨迹 set_param([mdl /Trapezoidal Profile], T_end, num2str(T_period)); % 运行仿真 sim(mdl); % 读取力矩误差 tau_sim yout.tauJoint.signals.values; tau_feed yout.tauPredict.signals.values; err_rms sqrt(mean((tau_sim - tau_feed).^2, 1));仿真环境里要注意动力学模型的正向与逆向使用次序不一致时仿真步长对高频激励的影响。如果傅里叶轨迹的最高次谐波对应频率是 2 Hz那么采样频率至少要 200 Hz仿真步长也相应取到 1/200 秒或更小。否则力矩残差里会混入截断误差你甚至会误以为辨识算法有问题。4.4 激励轨迹质量的两个指标判断一条激励轨迹是否合格不要只看残差大小。辨识领域的经验是看两个数值堆叠回归矩阵的条件数以及参数估计的方差上界。条件数用cond(Y_stack)计算通常希望小于 50 到 100。超过 200 就说明轨迹设计没有激起全部参数模态此时强行求出来的结果是不可信的。另一个指标是观察参数估计随数据量增长的收敛情况。把数据按时间切片从半个周期到一个完整周期的二倍、三倍分别算参数估计值画出变化曲线。如果参数在几倍周期之后仍然漂移明显说明数据中仍有一部分参数方向没有被充分激励需要回头优化幅值和偏置。5. 实机应用中的正则化与参数验证技巧5.1 用交替最小二乘剥离摩擦参数摩擦参数与惯性参数在回归矩阵中的表现完全不同。当关节速度为零附近时sign项强烈非线性当关节速度高速运动时粘滞摩擦项占据优势。如果在整条轨迹上同时估计所有参数惯性参数和摩擦参数之间的耦合会导致两者都偏。一个好用的技巧是分开估计先用慢速匀速段估计摩擦参数再用高速正弦段估计惯性参数交替迭代两轮。匀速段设计时让关节以恒定速度运动角加速度接近零惯性项的影响很小这时力矩主要由重力加上摩擦项构成解出的摩擦系数污染小。取两个不同匀速速度就能把库仑和粘滞摩擦分开。然后在激励轨迹辨识时把已经拿到的摩擦系数固定成已知常数不再参与优化这样回归矩阵的列数和病态程度都会下降。5.2 岭回归与按奇异值截断的选择当条件数仍然不理想直接来Y\τ会放大噪声。这时有两种常用手段岭回归加上一个正则项把解拉到接近零的方向或者对矩阵做奇异值分解截断小奇异值对应的方向。这两种方式都给人一种参数被拉偏的感觉所以只用其中最低限度的一种。代码上岭回归在 MATLAB 中只需要一行lambda 0.1 * max(svd(Y_stack)); % 正则化系数与最大奇异值挂钩 pi_ridge (Y_stack*Y_stack lambda*eye(size(Y_stack,2))) \ (Y_stack*Tau_stack);选择lambda的经验值是让岭迹图中所有参数开始趋于稳定的最小取值不要取得太大。用交叉验证画出 RMS 力矩残差随 lambda 变化的曲线选择残差开始上升前的点作为拐点。还要警惕的一点是正则化引入的偏差会让前馈控制在高速运动时偏差放大所以高频工况下正则化参数要比低速工况取得更小。5.3 批量辨识完成后的一套快速检验我通常在辨识结束后固定跑三项检查每项都简单且能说明问题你可以把这个检查清单直接复制到自己的 MATLAB 脚本里当断言用。第一项是把预测力矩与实际力矩画在一起逐关节看曲线叠合程度。辨识正确的标志是力矩曲线叠合而不是处处相等。重点看低速过零区那个位置摩擦模型失配最明显。第二项是重新采集一条全新的验证轨迹注意和辨识轨迹的频率成分不同但幅度相近把辨识参数用于验证轨迹并算 RMS 残差。如果验证残差明显高于辨识残差那就是过拟合基本可以断定激励轨迹病态。第三项是将辨识出的参数代入仿真模型比较末端轨迹跟随误差既考验参数正定性也考验整机动力学匹配。% 验证例新轨迹下的预测误差 tau_validate_pred Y_validate * pi_ridge; err_validate tau_validate_raw - tau_validate_pred; rms_validate sqrt(mean(err_validate.^2)); assert(rms_validate threshold, 验证轨迹残差超限需重做激励轨迹);这套检验做完如果仍然不通过不要马上怀疑算法先回到数据处理环节检查电流环带宽是否足够、力矩指令和采集是否同步。实际工程中70% 以上的参数辨识失败都是源于数据采集不同步而不是求解方法的问题。使用驱动器自带的力矩估计作为反馈比直接拿电流指令换算成力矩要可靠得多后者会混入电流环动态影响。本文还有配套的精品资源点击获取