6DOF机械臂动力学参数辨识(2) ——获得最小惯性参数集以及对应观测矩阵_机械臂参数辨识

一.获得最小惯性参数集的方法

1.数值方法

1）QR分解

QR分解是一种可以快速判断矩阵各列是否满秩的方法。设有一M×N矩阵A，矩阵A经QR分解后可写成A=QR的形式，其中Q是一个M×M正定矩阵，R是一个N×N上三角阵。对于R矩阵，若其对角元素的值为零或很接近零，则可以判断A矩阵的相应列的秩为零。

在Matlab中，可以直接调用qr()函数对矩阵进行QR分解。

2)奇异值分解

设线性化后的矩阵为A，则A经奇异值分解后可分解为

其中A为MP×N矩阵，U为MP×MP矩阵，V为N×N矩阵，Σ为MP×N矩阵。

S为对角阵，其对角线上的元素称为奇异值，奇异值从大到小排列。根据最小二乘法，参数的辨识值与奇异值有关，具有较小奇异值的参数对辨识精度影响很大，因此需要剔除这些参数。在Matlab中，可以直接调用svd()函数进行奇异值分解。(由于后续代码采用QR分解，所以不对奇异值分解做详细介绍，想有更多了解，可以参考资源里的论文)。

2.几何推导方法

从机械臂的结构特性出发，分析各连杆参数在动力学方程中的表现。通过研究相邻连杆间的力和力矩传递关系，可以发现某些参数总是以固定组合形式出现，无法单独辨识。这种方法特别适合对称结构或具有特殊几何关系的机械臂。

本文用的使用Gluon_6L3机械臂，参考霍伟《机器人动力学与控制》，书籍详细信息参考上一篇帖子。机械臂关节情况属于R1，所以第i个杆的最小惯性参数为XX，XY，XZ，YZ，ZZ，MX，MY，经重组后的杆i-1的惯性参数如下：

上述公式在本书第115页，详细的代码可以参考该篇帖子，当时学习的时候也是借鉴了不少。http://【机械臂动力学参数辨识全过程仿真 - CSDN App】https://blog.csdn.net/Nan_Feng520/article/details/140108798?sharetype=blog&shareId=140108798&sharerefer=APP&sharesource=m0_74196747&sharefrom=link

二.QR分解代码思路

首先将上一篇中得到的关于连杆的回归矩阵W加载进来，然后利用matlab自带的unifrnd函数给位置，速度和加速生成连续均匀分布的随机数，将这些值带入回归矩阵W，调用函数qr(),如果回归矩阵对应列的数值小于1-5e，默认为0，最后将对应的惯性参数剔除。

值得注意的是，matlab在进行符号运算之后所得到的符号表达式在带入数字时会产生无穷大或者不存在的数值，因此需要判断回归矩阵的有几行几列为无穷大或者不存在的数值，并得到这些不合理里的数的行列信息，并单独提出来进行运算。

%% dyn_minimal_param_syms.m% @brief: QR 分解得到最小参数集 % @param[out] W_min: 最小参数集对应的回归矩阵 % @param[out] param_min: 最小参数集% @param[out] pnum_min: 最小参数集的数量% @param[out] R1: 组成独立参数的矩阵% @param[out] R2: 组成非独立参数的矩阵% @note: 所有单位均为mm(Nmm)function [W_min, min_param_ind, pnum_min, R1, R2] = dyn_minimal_param_syms()addpath(\'.\\utils\');%% PARAMETER% DH and load parametersfe1 = 0; fe2 = 0; fe3 = 0; ne1 = 0; ne2 = 0; ne3 = 0;a3 = 174.42; a4 = 174.42; d2 = 80.09; d4 = -4.43; d5 = 80.09; d6 = 44.36;g = 9802; % in mm% 动力学参数的数量% m, mc1, mc2, mc3, Ioxx, Ioyy, Iozz, Ioxy, Ioxz, Ioyz, Ia, fv, fcpnum_sum = evalin(\'base\', \'pnum_sum\');% 标准参数对应的回归矩阵，维度为 (syms 7 x 91)W = evalin(\'base\', \'W\');%% INSTANTIATION% q = zeros(1, 6);% qd = zeros(1, 6);% qdd = zeros(1, 6);min_param_ind = zeros(1, pnum_sum); % the index of minimal parameters WW = zeros(pnum_sum * 6, pnum_sum);for i = 1:pnum_sum q = unifrnd(-pi, pi, 1, 6); qd = unifrnd(-5*pi, 5*pi, 1, 6); qdd = unifrnd(-10*pi, 10*pi, 1, 6); s1 = sin(q(1)); c1 = cos(q(1)); s2 = sin(q(2)); c2 = cos(q(2)); s3 = sin(q(3)); c3 = cos(q(3)); s4 = sin(q(4)); c4 = cos(q(4)); s5 = sin(q(5)); c5 = cos(q(5)); s6 = sin(q(6)); c6 = cos(q(6)); dQ1 = qd(1); ddQ1 = qdd(1); dQ2 = qd(2); ddQ2 = qdd(2);dQ3 = qd(3); ddQ3 = qdd(3);dQ4 = qd(4); ddQ4 = qdd(4);dQ5 = qd(5); ddQ5 = qdd(5); dQ6 = qd(6); ddQ6 = qdd(6); W_ = eval(subs(W, ... {\'s1\', \'c1\', \'s2\', \'c2\', \'s3\', \'c3\', \'s4\', \'c4\', \'s5\', \'c5\', \'s6\', \'c6\', ...  \'dQ1\', \'dQ2\', \'dQ3\', \'dQ4\', \'dQ5\', \'dQ6\', ...  \'ddQ1\', \'ddQ2\', \'ddQ3\', \'ddQ4\', \'ddQ5\', \'ddQ6\', ...  \'fe1\', \'fe2\', \'fe3\', \'ne1\', \'ne2\', \'ne3\', ...  \'g\', \'d2\', \'d4\',\'d5\', \'d6\', \'a3\',\'a4\'}, ... {s1, c1, s2, c2, s3, c3, s4, c4, s5, c5, s6, c6, ...  dQ1, dQ2, dQ3, dQ4, dQ5, dQ6, ...  ddQ1, ddQ2, ddQ3, ddQ4, ddQ5, ddQ6, ...  fe1, fe2, fe3, ne1, ne2, ne3, ...  g, d2, d4, d5, d6,a3,a4})); % PATCH#1 for nan/inf problem of w179 %找出不合理数值的行列 row1 = 1+6*(i-1); row2 = 6+6*(i-1);WW(row1:row2, :) = W_; disp([\' Param No.\', num2str(i), \' SUBSTITUTED!!\']);end%% LOOK FOR NAN AND INFif (sum(sum(isnan(WW))) > 0) ind_nan = find(isnan(WW)); % linear index of nan sub_nan = zeros(length(ind_nan), 2); % array index of nan joint_nan = zeros(1, length(ind_nan)); % among 7 joints iter_nan = zeros(1, length(ind_nan)); % among 200 trajectory setpoints for n = 1:length(ind_nan) [sub_nan(n, 1), sub_nan(n, 2)] = ind2sub(size(WW), ind_nan(n)); joint_nan(n) = mod(sub_nan(n, 1), 6); iter_nan(n) = fix(sub_nan(n, 1)/6) + 1; enddisp(sub_nan) disp(joint_nan)pauseelse disp(\'No NaN in matrix.\');endif (sum(sum(isinf(WW))) > 0) ind_inf = find(isinf(WW)); % linear index if inf sub_inf = zeros(length(ind_inf), 2); % array index of inf joint_inf = zeros(length(ind_inf), 1); % among 7 joints iter_inf = zeros(length(ind_inf), 1); % among 200 trajectory setpoints for n = 1:length(ind_inf) [sub_inf(n, 1), sub_inf(n, 2)] = ind2sub(size(WW), ind_inf(n)); joint_inf(n) = mod(sub_inf(n, 1), 6); iter_inf(n) = fix(sub_inf(n, 1)/6) + 1; end disp(sub_inf)disp(joint_inf)pauseelse disp(\'No Inf in matrix.\');end%% QR DECOMPOSITION% WW=Q*R, WW:(7*pnum_sum, pnum_sum), Q:(7*pnum_sum, 7*pnum_sum), R:(7*pnum_sum, pnum_sum)[~, R] = qr(WW);pnum_min = 0;% number of independent parameterfor i = 1:pnum_sum if (abs(R(i, i)) < 10^(-5)) min_param_ind(i) = 0; else min_param_ind(i) = 1; pnum_min = pnum_min + 1; endenddisp(\' QR DECOMPOSITION complete!!\');W_min = sym(zeros(6, pnum_min));% regression matrix (minimal set)R1 = zeros(pnum_min, pnum_min); R2 = zeros(pnum_min, pnum_sum - pnum_min); cind = 1; cdep = 1;% count the number of independent and dependent columnsfor i = 1:pnum_sum if (min_param_ind(i) == 1) W_min(:, cind) = W(:, i);% compose independent columns in W to form matrix WB R1(1:pnum_min, cind) = R(1:pnum_min, i);% compose independent columns in R to form matrix RB cind = cind + 1; else R2(1:pnum_min, cdep) = R(1:pnum_min, i); cdep = cdep + 1; endenddisp(\' WB (W_min) matrix OBTAINED!!\');%% SAVE DATAfid = fopen(\'.\\data\\txt\\dyn_minimal_param_syms.txt\', \'w\');for i = 1:6 for j = 1:pnum_min fprintf(fid, \'w_min%d%d=%s;\\r\', i, j, char(W_min(i, j))); endendfclose(fid);save(\'.\\data\\mat\\dyn_minimal_param_syms.mat\', \'W\')%% RETURN VARIABLE TO BASE WORKSPACEassignin(\'base\', \'W_min\', W_min);assignin(\'base\', \'min_param_ind\', min_param_ind);assignin(\'base\', \'pnum_min\', pnum_min);assignin(\'base\', \'R1\', R1);assignin(\'base\', \'R2\', R2);rmpath(\'.\\utils\\\');