关于机器人机械臂参数辨识

前几天在逛github发现一个dynamic identification的仓库，代码非常全。同时也看了对应的文章，虽然参数辨识不是文章最终目的，但是仍能感受到作者在参数辨识领域的了解程度和功底。这篇博客主要记录一下这几天学习该仓库的一些心得吧，对机器人参数辨识有兴趣的小伙伴欢迎交流。

这是我用的仿真文件，有需要的小伙伴可以参考一下：
仿真文件地址

1.仓库地址

dynamic_calibration

2.文献地址

Practical Aspects of Model-Based Collision Detection,文章作者来自俄罗斯Innopolis University

3.代码简介

研究的机器人对象为UR10e，自重33.5kg、负载12.5kg、是UR公司主打一款大负载机械臂。

开源代码涉及到比较全的机器人学知识，尤其是在动力学dynamics部分，主要包括拉格朗日法建立动力学模型，经典库伦粘滞摩擦模型 、 非线性摩擦模型 、动力学参数辨识模型、QR分解提取最小惯性参数集。参数辨识部分包含激励轨迹的设计、参数辨识、信号处理及最后的辨识结果验证。此外提供UR10e的URDF文件，可以说一条龙地从动力学到参数辨识进行了分享教学，个人觉得是非常好的一个学习机械臂动力学的仓库。

4.文献简介

从文章标题不难得出本文是利用基于机器人动力学模型的方法进行碰撞检测，虽然不是Top期刊，但是读一读总是没有错的，接下来我们一起看一下他的参数辨识部分内容，按照7个部分¹:

4.1 回归矩阵（Dynamic Model in Regressor Form）

第一部分其实就是我们所熟知的动力学模型线性化，即在不考虑摩擦的情况下，假设连杆均为刚体（不考虑连杆柔性），机器人动力学模型可以写成其惯性参数的线性形式：
$$Y(q,\dot{q},\ddot{q})\pi =\tau$$
其中，我们要注意的是这里的$\pi$，最开始我们所说的惯性参数集是完整惯性参数集（standrad parameters）,在很多地方会写成如下的形式：
$${\pi }_i=[I_{ixx},I_{ixy},I_{ixz},I_{iyy},I_{iyz},I_{izz},m_i{r}_x,m_i{r}_y,m_i{r}_z,m_i{]}^T\in R^{10\times 1}$$
其中，$h_i^T=m_i{p}_{ic},p_{ic}=[r_x,r_y,r_z{]}^T$叫做质量一阶矩（the first moment of mass），$L_i$是相对于连杆坐标系的，与相对于质心的惯性张量有如下关系(Huygens-Steiner)²：
$$L_k=I_k+\frac{1}{m_i}S(l_i{)}^{T}S(l_i)=I_k+m_{i}S(p_{ic}{)}^2$$

相对质心的惯性张量是常数，但为什么要选择相对连杆坐标系的惯性张量呢？因为在dynamic calibration中我们很容易获取机器人的运动学参数，但是并不知道质心位置，因此选择相对连杆坐标系的惯性张量，且连杆坐标系{i}与其质心坐标系姿态保持一致

然而，在实际情况下，仍需要考虑关节摩擦和关节转动惯量$J_i$对力矩的影响。因此，有如下表达式：
$${\pi }_i=[I_{ixx},I_{ixy},I_{ixz},I_{iyy},I_{iyz},I_{izz},m_i{r}_x,m_i{r}_y,m_i{r}_z,m_i,J_i,f_v^i,f_c^i,f_0^i{]}^T\in R^{14\times 1}$$

下面结合代码来看他的回归矩阵$Y(\cdot )$是如何得到的，采用符号工具箱进行了推导。

% 首先创建了关节的广义坐标及其一阶和二阶导数
q_sym = sym('q%d',[6,1],'real');
qd_sym = sym('qd%d',[6,1],'real');
q2d_sym = sym('q2d%d',[6,1],'real');

% ------------------------------------------------------------------------
% 求能量函数
% ------------------------------------------------------------------------
T_pk = sym(zeros(4,4,6));     %连杆之间的转换矩阵
w_kk(:,1) = sym(zeros(3,1));  % 坐标系K的角速度
v_kk(:,1) = sym(zeros(3,1));  %坐标系K原点 的线速度
g_kk(:,1) = sym([0,0,9.81])'; % 重力加速度向量
p_kk(:,1) = sym(zeros(3,1));  % 坐标系K的原点

for i = 1:6
    jnt_axs_k = str2num(ur10.robot.joint{i}.axis.Attributes.xyz)';
    % Transformation from parent link frame p to current joint frame
    rpy_k = sym(str2num(ur10.robot.joint{i}.origin.Attributes.rpy));
    R_pj = RPY(rpy_k);
    R_pj(abs(R_pj)<sqrt(eps)) = sym(0); % to avoid numerical errors
    p_pj = str2num(ur10.robot.joint{i}.origin.Attributes.xyz)';
    T_pj = sym([R_pj, p_pj; zeros(1,3), 1]); % to avoid numerical errors
    % Tranformation from joint frame of the joint that rotaties body k to
    % link frame. The transformation is pure rotation
    R_jk = Rot(q_sym(i),sym(jnt_axs_k));
    p_jk = sym(zeros(3,1));
    T_jk = [R_jk, p_jk; sym(zeros(1,3)),sym(1)];
    % Transformation from parent link frame p to current link frame k
    T_pk(:,:,i) = T_pj*T_jk;
    z_kk(:,i) = sym(jnt_axs_k);
        
    w_kk(:,i+1) = T_pk(1:3,1:3,i)'*w_kk(:,i) + sym(jnt_axs_k)*qd_sym(i);
    v_kk(:,i+1) = T_pk(1:3,1:3,i)'*(v_kk(:,i) + cross(w_kk(:,i),sym(p_pj)));
    g_kk(:,i+1) = T_pk(1:3,1:3,i)'*g_kk(:,i);
    p_kk(:,i+1) = T_pk(1:3,1:3,i)'*(p_kk(:,i) + sym(p_pj));
     
    % 动能   
    beta_K(i,:) = [sym(0.5)*w2wtlda(w_kk(:,i+1)),...
                   v_kk(:,i+1)'*vec2skewSymMat(w_kk(:,i+1)),...
                   sym(0.5)*v_kk(:,i+1)'*v_kk(:,i+1)];
    % 势能
    beta_P(i,:) = [sym(zeros(1,6)), g_kk(:,i+1)',...
                   g_kk(:,i+1)'*p_kk(:,i+1)];
end


% ---------------------------------------------------------------------
% 递归拉格朗日求回归函数
% ---------------------------------------------------------------------
beta_Lf = [beta_K(1,:) - beta_P(1,:), beta_K(2,:) - beta_P(2,:),...
         beta_K(3,:) - beta_P(3,:), beta_K(4,:) - beta_P(4,:),...
         beta_K(5,:) - beta_P(5,:), beta_K(6,:) - beta_P(6,:)];
dbetaLf_dq = jacobian(beta_Lf,q_sym)';
dbetaLf_dqd = jacobian(beta_Lf,qd_sym)';
tf = sym(zeros(6,60));
for i = 1:6
   tf = tf + diff(dbetaLf_dqd,q_sym(i))*qd_sym(i)+...
                diff(dbetaLf_dqd,qd_sym(i))*q2d_sym(i);
end
Y_f = tf - dbetaLf_dq;

4.2 最小惯性参数集（Base Parameters）

通过淘汰对动力学模型无影响的参数同时对剩下的参数进行重新组合从而形成最小惯性参数集合，这里一般有两种方法，解析法³⁴和数值法⁵。解析法是利用机器人的运动学和机器人构型进行复杂的推导判断而得出，霍伟教授编撰的《机器人动力学与控制》中有详细的介绍；数值法一般是基于QR或SVD分解进行的，其中QR分解是首选的计算方法。

我们可以首先回顾一下什么是QR分解？

QR分解常用于解决线性最小二乘问题，是特定特征值算法的基础。若$A$是一个方阵，则有$A=QR$，$Q$是一个正交矩阵（$Q^T=Q^{-1}$），$R$是一个上三角矩阵。但可能通常我们面对的是非方阵，即$A^{m\times n}$，且$m>n$。那么有
$$A=QR=Q\left[\begin{array}{c}{R}_1\\ 0\end{array}\right]=\left[\begin{array}{c}{Q}_1\\ Q_2\end{array}\right]\left[\begin{array}{c}{R}_1\\ 0\end{array}\right]=Q_1{R}_1$$
QR分解与格拉姆-施密特正交化的相关知识可参考这儿

接下来看一下一般的步骤，我按照1990年的一篇文章（参考文献5）提到的方法进行阐述一下：
1. 剔除不会影响动力学模型的参数

如果$Y(\cdot )$的某一列（假设为第$i$列）对于任何的$(q,\dot{q},\ddot{q})$都为0，那么其对应的${\pi }_i$则不会对模型产生影响。我们可以给随机的$(q,\dot{q},\ddot{q})$来简单判断这些列，并将它们剔除，假设最后只剩下$c$列，我们称之为$Y(\cdot {)}_c$。

3. 惯性参数重组
我们给$e$组随机$(q,\dot{q},\ddot{q})$并生成回归矩阵序列$W^{r\times c}$，如下所示：
$$W=\left[\begin{array}{c}Y(1{)}_c\\ Y(2{)}_c\\ ...\\ Y(e{)}_c\end{array}\right],r=n\times e⩾c$$
接下来采用QR分解：
$$Q^{T}W=R=\left[\begin{array}{c}{R}_1^{c\times c}\\ 0^{(r-c)\times c}\end{array}\right]$$
$Q$是正交矩阵，所以放到左边了。$R_1$是一个上三角矩阵。如果$W$不是满秩的，即$rank(W)=b<c$，那么它就不会有唯一的$QR$分解。$R_1$中其实会有$b$个非零的对角元素，$c-b$个近似为0的对角元素（因为舍入误差的原因，所以近似为0，本该是为0的）。对于那些近似为0的对角线元素$R_{1}ii$所对应的惯性参数是无法单独辨识。

但我们可以寻找一个置换矩阵$E$，使得$W\ast E$是有唯一分解的，$W\ast E=Q\ast R$,即
$$WE=QR=\left[\begin{array}{cc}{Q}_1& Q_2\end{array}\right]\left[\begin{array}{cc}{R}_1^{b\times b}& R_2^{b\times c-b}\\ 0^{(r-b)\times b}& 0^{(r-b)\times c-b}\end{array}\right]$$
且这个置换矩阵也满足如下：
$$E^T\pi =\left[\begin{array}{c}{\pi }_1^{b\times 1}\\ {\pi }_2^{(c-b)\times 1}\end{array}\right]$$
因此有
$$W=\left[\begin{array}{cc}{W}_1& W_2\end{array}\right]=\left[\begin{array}{cc}{Q}_1{R}_1& Q_2{R}_2\end{array}\right]$$
通过上式可以得出$W_1=W_2\beta ,\beta =R_1^{-1}{R}_2$，这其实意味着$W_2$的$(c-b)$列是$W_1$的$b$列的线性组合。那么，惯性参数其实就呼之欲出了：
$${\pi }_b={\pi }_1+\beta {\pi }_2$$
3. 辨识方程
其对应的回归矩阵为$W_b=WE(:,1:b)$，对应的辨识方程为$\tau =W_b{\pi }_b$.
下面我们可以看一下他这块的代码：

% Seed the random number generator based on the current time
rng('shuffle');

% ------------------------------------------------------------------------
% 位置速度和加速度限制
% ------------------------------------------------------------------------
q_min = -pi*ones(6,1);
q_max = pi*ones(6,1);
qd_max = 3*pi*ones(6,1);
q2d_max = 6*pi*ones(6,1); 

% -----------------------------------------------------------------------
% 完整动力学参数集的符号形式
% -----------------------------------------------------------------------
m = sym('m%d',[6,1],'real');
hx = sym('h%d_x',[6,1],'real');
hy = sym('h%d_y',[6,1],'real');
hz = sym('h%d_z',[6,1],'real');
ixx = sym('i%d_xx',[6,1],'real');
ixy = sym('i%d_xy',[6,1],'real');
ixz = sym('i%d_xz',[6,1],'real');
iyy = sym('i%d_yy',[6,1],'real');
iyz = sym('i%d_yz',[6,1],'real');
izz = sym('i%d_zz',[6,1],'real');
im = sym('im%d',[6,1],'real');

% Load parameters attached to the end-effector
syms ml hl_x hl_y hl_z il_xx il_xy il_xz il_yy il_yz il_zz      real 

% 组成60*1的向量
for i = 1:6
    if includeMotorDynamics
        pi_lgr_sym(:,i) = [ixx(i),ixy(i),ixz(i),iyy(i),iyz(i),izz(i),...
                           hx(i),hy(i),hz(i),m(i),im(i)]';
    else
        pi_lgr_sym(:,i) = [ixx(i),ixy(i),ixz(i),iyy(i),iyz(i),izz(i),...
                           hx(i),hy(i),hz(i),m(i)]';
    end
end
[nLnkPrms, nLnks] = size(pi_lgr_sym);
pi_lgr_sym = reshape(pi_lgr_sym, [nLnkPrms*nLnks, 1]);


% -----------------------------------------------------------------------
% 寻找独立列和相关列之间的关系（线性组合关系）
% -----------------------------------------------------------------------
% 获取回归矩阵
W = [];    
for i = 1:25
    q_rnd = q_min + (q_max - q_min).*rand(6,1);
    qd_rnd = -qd_max + 2*qd_max.*rand(6,1);
    q2d_rnd = -q2d_max + 2*q2d_max.*rand(6,1);
    
    if includeMotorDynamics
        Y = regressorWithMotorDynamics(q_rnd,qd_rnd,q2d_rnd);
    else
        Y = full_regressor_UR10E(q_rnd,qd_rnd,q2d_rnd);
    end
    W = vertcat(W,Y);
end

% QR分解: W*E = Q*R
%   R is 上三角矩阵
%   Q is 酉矩阵
%   E is 置换矩阵
[Q, R, E] = qr(W);

% W的秩为b，即base parameter的数量
bb = rank(W);

% R = [R1 R2; 
%      0  0]
% R1 is bbxbb上三角正则矩阵
% R2 is bbx(c-bb) matrix where c is number of standard parameters
R1 = R(1:bb,1:bb);
R2 = R(1:bb,bb+1:end);
beta = R1\R2; % the zero rows of K correspond to independent columns of WP
beta(abs(beta)<sqrt(eps)) = 0; % get rid of numerical errors
% W2 = W1*beta

% 确保关系成立
W1 = W*E(:,1:bb);
W2 = W*E(:,bb+1:end);
assert(norm(W2 - W1*beta) < 1e-6,... 
        'Found realationship between W1 and W2 is not correct\n');

% -----------------------------------------------------------------------
% 最小惯性参数集
% -----------------------------------------------------------------------
pi1 = E(:,1:bb)'*pi_lgr_sym; % independent paramters
pi2 = E(:,bb+1:end)'*pi_lgr_sym; % dependent paramteres

% all of the expressions below are equivalent
pi_lgr_base = pi1 + beta*pi2;
% pi_lgr_base = [eye(bb) beta]*[pi1;pi2];
% pi_lgr_base = [eye(bb) beta]*E'*pi_lgr_sym;

4.3 轨迹规划（trajectory planning）

4.4 数据采集（Data Collecting）

4.5 数据处理（Data Processing）

4.6 参数估计（Parameters Estimation）

4.7 结果验证（Results Validation）

5.自身学习

我准备先在仿真里面进行测试，即在simscape中对机器人模型进行参数辨识，与已知结果（三维软件测量）进行对比。

2022.08.02更新：仿真结果不错，下面简单介绍一下：

5.1 仿真环境

我选取了一个七轴协作机械臂模型，具体型号就不说了。拿到一个机器人三维模型的第一件事，根据模型的几何参数，采用MDH法定义其连杆坐标系。然后在soldworks中进行相关设置（质量分配、坐标系定义、旋转轴设定及其方向确定）并导出其URDF文件，URDF文件里面包含了我们做动力学仿真所需要的一切参数，参数辨识的结果可以和solidworks中的质量参数进行比较。最后将URDF文件导入MATLAB的SimScape中，如下图所示。

5.2 最小参数集

因为是做仿真，可以不考虑摩擦和电机惯量等因素，因此我设立的完整惯性参数集为$P^{70\times 1}$,如下所示：
$$P_i=[I_{ixx},I_{ixy},I_{ixz},I_{iyy},I_{iyz},I_{izz},m_i{r}_x,m_i{r}_y,m_i{r}_z,m_i{]}^T\in R^{10\times 1}$$
使用仓库提供的源码进行最小惯性参数集的计算，一共43个参数，其符号表达式如下所示：

P1 = i7_xz
P2 = i7_yz
P3 = i7_xy
P4 = i7_zz
P5 = i6_xz
P6 = i6_xy
P7 = mh7_x
P8 = i6_yz
P9 = mh7_y
P10 = mh6_y + mh7_z
P11 = i5_xz
P12 = i5_yz
P13 = mh6_x
P14 = i6_xx - i6_yy + i7_yy
P15 = i6_xx + i7_xx - i6_yy
P16 = (33*m5)/80 + (33*m6)/80 + (33*m7)/80 + mh4_y + mh5_z
P17 = i5_xy
P18 = i6_yy + i5_zz
P19 = i4_xz
P20 = mh5_y + mh6_z
P21 = i4_yz
P22 = i4_xy
P23 = mh4_x
P24 = mh5_x
P25 = i3_yz
P26 = (7*m3)/16 + (7*m4)/16 + (7*m5)/16 + (7*m6)/16 + (7*m7)/16 + mh2_y + mh3_z
P27 = i6_yy - i6_xx + i6_zz
P28 = i3_xz
P29 = i2_yy - i3_xx - i2_xx + i3_zz + (49*m3)/256 + (49*m4)/256 + (49*m5)/256 + (49*m6)/256 + (49*m7)/256 + (7*mh2_y)/8
P30 = i3_xy
P31 = i2_xx + i3_xx - i2_yy + i4_xx + i5_xx + i6_yy - (49*m3)/256 - (49*m4)/256 - (1157*m5)/3200 - (1157*m6)/3200 - (1157*m7)/3200 - (7*mh2_y)/8 - (33*mh4_y)/40
P32 = i2_yy + i1_zz
P33 = i2_xz
P34 = mh3_x
P35 = i2_yy - i3_xx - i2_xx - i4_xx + i4_zz + (49*m3)/256 + (49*m4)/256 + (49*m5)/256 + (49*m6)/256 + (49*m7)/256 + (7*mh2_y)/8
P36 = i2_yz
P37 = mh3_y + mh4_z
P38 = mh2_x
P39 = i2_xy
P40 = i2_xx + i3_xx - i2_yy + i4_yy - (49*m3)/256 - (49*m4)/256 - (49*m5)/256 - (49*m6)/256 - (49*m7)/256 - (7*mh2_y)/8
P41 = i2_xx - i2_yy + i3_yy - (49*m3)/256 - (49*m4)/256 - (49*m5)/256 - (49*m6)/256 - (49*m7)/256 - (7*mh2_y)/8
P42 = i2_yy - i2_xx + i2_zz
P43 = i2_xx + i3_xx - i2_yy + i4_xx + i5_yy - (49*m3)/256 - (49*m4)/256 - (1157*m5)/3200 - (1157*m6)/3200 - (1157*m7)/3200 - (7*mh2_y)/8 - (33*mh4_y)/40

其实到这里，以上帝视角看，我是已知最小惯性参数集的具体数值了。同样地，我在这里把他们列出来，后面参数辨识可以进行对比。

P1 = 0.000009
P2 = 0.003829
P3 = -0.000001
P4 = 0.001917
P5 = -0.000001
P6 = -0.000002
P7 = 0.000035
P8 = 0.000646
P9 = 0.018383
P10 = 0.381724
P11 = -0.000003
P12 = 0.000691
P13 = -0.000032
P14 = 0.072779
P15 = 0.507504
P16 = 2.221642
P17 = 0.000000
P18 = 0.009096
P19 = 0.000000
P20 = -0.021990
P21 = 0.001478
P22 = -0.000002
P23 = -0.000079
P24 = 0.000025
P25 = 0.000696
P26 = 4.939345
P27 = 0.001759
P28 = 0.000000
P29 = 2.316072
P30 = 0.000001
P31 = -3.308536
P32 = 0.040089
P33 = 0.000000
P34 = 0.000005
P35 = 2.304700
P36 = 0.003501
P37 = 0.020232
P38 = -0.000019
P39 = -0.000005
P40 = -2.301731
P41 = -2.312715
P42 = 0.008138
P43 = -3.315313

5.3 轨迹规划

不像文章中采用的傅里叶级数+多项式的组合，我只用五阶傅里叶级数，其系数的确定使用优化算法，可以通过matlab自带的优化算法工具箱进行确定。优化结果如下，横坐标单位是0.01s：

5.4 数据采集

这部分很简单，将规划好的轨迹输入至simscape中，机器人按照给定的位置、速度和加速度信号进行运动，我们因而可以获取运动中的力矩信息。

5.5 数据处理

这一步需要对输出的位置数据进行微分，获取速度和加速度信息。

tor = out.tor(1:num,:);
pos = out.pos(1:num,:);
vel = zeros(num,N);
acc = zeros(num,N);
for i = 2:1:num-1
    vel(i,:) = (pos(i+1,:) - pos(i-1,:))/(2*dt);
end
vel(1,:) = vel(2,:);
for i = 2:num-1
    acc(i,:) = (pos(i+1,:) - 2*pos(i,:) + pos(i-1,:))/(dt^2);
end

5.6 参数估计

有了轨迹的力矩和运动信息，按照前述方法直接求解即可，最小惯性参数结果如下。如果和前面标称的参数相比，数值上已经是十分接近了。

P1 = -0.000008
P2 = -0.003822
P3 = -0.000002
P4 = 0.001895
P5 = -0.000002
P6 = 0.000006
P7 = 0.000040
P8 = -0.000668
P9 = 0.018384
P10 = 0.381750
P11 = 0.000007
P12 = -0.000670
P13 = -0.000018
P14 = 0.072790
P15 = 0.073986
P16 = 2.221165
P17 = -0.000008
P18 = 0.009164
P19 = 0.000016
P20 = -0.021984
P21 = -0.001544
P22 = 0.000017
P23 = 0.000001
P24 = 0.000039
P25 = -0.000716
P26 = 4.937972
P27 = 0.001690
P28 = 0.000309
P29 = 2.314043
P30 = -0.000085
P31 = -3.305995
P32 = 0.040127
P33 = 0.000148
P34 = -0.000001
P35 = 2.302938
P36 = -0.003511
P37 = 0.020192
P38 = -0.000026
P39 = 0.000081
P40 = -2.299960
P41 = -2.310878
P42 = 0.008014
P43 = -3.312780

然后我们用最小惯性参数预测力矩，并且和实际仿真力矩进行对比，结果如下：

我们可以看到误差非常小，不到0.001Nm。

6.引用

到了这里，关于关于机器人机械臂参数辨识的文章就介绍完了。如果您还想了解更多内容，请在右上角搜索TOY模板网以前的文章或继续浏览下面的相关文章，希望大家以后多多支持TOY模板网！