机械臂动力学参数辨识全过程仿真

标签：仿真 tau getdatasamples clc 参数全过程 Yr 辨识 out

文章目录

前言
一、动力学建模
二、动力学方程线性化
- 1.惯性参数线性化
- 2.代码
三、DH法推导最小惯性参数集
- 1、推导流程
- 2、代码
四、通过simulink进行参数辨识
总结

前言

个人出于对机械臂动力学的兴趣，在此记录学习过程并分享学习成果。由于网上关于动力学参数辨识这一块的内容比较零散，本人学起来走了不少弯路，遇到了许多瓶颈，好几次都接近放弃，但最终靠着好心人的帮助下和自己的灵感，还是坚持啃下来了。所以为了让初学者们也少走弯路，本文详细记录了用matlab进行动力学参数辨识仿真的全部过程和实现代码，辨识对象为ur5e机械臂（或与此构型一致的其他机械臂），内容攘括了动力学建模、动力学方程线性化、DH法推导最小惯性参数集、通过simulink进行参数辨识、给出参数辨识的正确结果。本文只注重流程梳理和代码编写，而不注重理论和公式推导，与理论和公式相关的知识点可参考《机器人学导论》、《机器人动力学与控制》两本书，并结合网上其他大佬的文章进行理解。

一、动力学建模

1、MDH坐标系的建立

ur5e的坐标系建立和MDH参数表如下：
在这里插入图片描述

2、动力学方程的建立

动力学建模有两种经典的方法，牛顿欧拉法和拉格朗日法。本文参考《机器人学导论》，通过牛顿欧拉法来推导动力学方程，具体流程如下。
①外推，求出连杆速度、加速段和质心的力、力矩。参考公式如下：
外推求出连杆速度、加速段和质心的力、力矩
②内推，求出关节驱动力矩：
内推求出关节驱动力矩

3、代码

上述公式的推导过程可参考书本和网上其他大佬的文章，这里直接上代码。

function tau = tau_newtonEuler(q,dq,ddq)
q1=q(1); q2=q(2); q3=q(3);q4=q(4); q5=q(5); q6=q(6);
dq1=dq(1); dq2=dq(2); dq3=dq(3);dq4=dq(4); dq5=dq(5); dq6=dq(6);
ddq1=ddq(1); ddq2=ddq(2); ddq3=ddq(3);ddq4=ddq(4); ddq5=ddq(5); ddq6=ddq(6);
global a3 a4 d2 d4 d5 d6 m1 m2 m3 m4 m5 m6 g;
global Ic1 Ic2 Ic3 Ic4 Ic5 Ic6 Pc11 Pc22 Pc33 Pc44 Pc55 Pc66;
% 坐标系旋转轴
axis1=[0 0 1]';axis2=[0 0 1]';axis3=[0 0 1]';axis4=[0 0 1]';axis5=[0 0 1]';axis6=[0 0 1]';
% 坐标系变换矩阵
T01 = MDHTrans(0, 0, 0, q1);
T12 = MDHTrans(pi/2, 0, d2, q2+pi/2);
T23 = MDHTrans(0, a3, 0, q3);
T34 = MDHTrans(0, a4, d4, q4-pi/2);
T45 = MDHTrans(-pi/2, 0, d5, q5);
T56 = MDHTrans(pi/2, 0, d6, q6);
T67 = MDHTrans(0, 0, 0, 0);
R01=T01(1:3,1:3);R10=R01';P01=T01(1:3,4);
R12=T12(1:3,1:3);R21=R12';P12=T12(1:3,4);
R23=T23(1:3,1:3);R32=R23';P23=T23(1:3,4);
R34=T34(1:3,1:3);R43=R34';P34=T34(1:3,4);
R45=T45(1:3,1:3);R54=R45';P45=T45(1:3,4);
R56=T56(1:3,1:3);R65=R56';P56=T56(1:3,4);
R67=T67(1:3,1:3);R76=R67';P67=T67(1:3,4);
%% 外推：连杆角速度、角加速度、线加速度、质心加速度、质心受力
w00=[0 0 0]';dw00=[0 0 0]';dv00=[0 0 g]';dvc00=[0 0 g]';
[w11,dw11,dv11,dvc11]=motion_para_clc(R10,dq1,ddq1,w00,dw00,dv00,P01,Pc11,axis1);
[w22,dw22,dv22,dvc22]=motion_para_clc(R21,dq2,ddq2,w11,dw11,dv11,P12,Pc22,axis2);
[w33,dw33,dv33,dvc33]=motion_para_clc(R32,dq3,ddq3,w22,dw22,dv22,P23,Pc33,axis3);
[w44,dw44,dv44,dvc44]=motion_para_clc(R43,dq4,ddq4,w33,dw33,dv33,P34,Pc44,axis4);
[w55,dw55,dv55,dvc55]=motion_para_clc(R54,dq5,ddq5,w44,dw44,dv44,P45,Pc55,axis5);
[w66,dw66,dv66,dvc66]=motion_para_clc(R65,dq6,ddq6,w55,dw55,dv55,P56,Pc66,axis6);
[Fc11,Nc11] = FN_clc(w11,dw11,dvc11,m1,Ic1);
[Fc22,Nc22] = FN_clc(w22,dw22,dvc22,m2,Ic2);
[Fc33,Nc33] = FN_clc(w33,dw33,dvc33,m3,Ic3);
[Fc44,Nc44] = FN_clc(w44,dw44,dvc44,m4,Ic4);
[Fc55,Nc55] = FN_clc(w55,dw55,dvc55,m5,Ic5);
[Fc66,Nc66] = FN_clc(w66,dw66,dvc66,m6,Ic6);
%% 内推：连杆受力、关节力矩   非线性化，惯性张量的坐标在质心->tau=n'*axis;
f77=[0 0 0]';n77=[0 0 0]';
[f66,n66] = fn_clc(R67,f77,Fc66,Nc66,n77,Pc66,P67);
[f55,n55] = fn_clc(R56,f66,Fc55,Nc55,n66,Pc55,P56);
[f44,n44] = fn_clc(R45,f55,Fc44,Nc44,n55,Pc44,P45);
[f33,n33] = fn_clc(R34,f44,Fc33,Nc33,n44,Pc33,P34);
[f22,n22] = fn_clc(R23,f33,Fc22,Nc22,n33,Pc22,P23);
[f11,n11] = fn_clc(R12,f22,Fc11,Nc11,n22,Pc11,P12);
tau=[n11'*axis1,n22'*axis2,n33'*axis3,n44'*axis4,n55'*axis5,n66'*axis6]';
end

function T = MDHTrans(alpha, a, d, theta)
T = [cos(theta)              -sin(theta)             0              a;
     sin(theta)*cos(alpha)  cos(theta)*cos(alpha)   -sin(alpha)    -d*sin(alpha);
     sin(theta)*sin(alpha)  cos(theta)*sin(alpha)   cos(alpha)     d*cos(alpha);
     0                      0                       0              1];
end

function [Fc,Nc] = FN_clc(w,dw,dvc,m,Ic)
    Fc = m*dvc;
    Nc = Ic*dw+cross(w,Ic*w);
end

function [f,n] = fn_clc(R,f_next,Fc,Nc,n_next,Pc,Po)
    f = R*f_next + Fc;
    n = Nc + R*n_next + cross(Pc,Fc) +cross(Po,R*f_next) ;
end

二、动力学方程线性化

1.惯性参数线性化

参考《机器人动力学与控制》可知，机器人的动力学特性（动能和势能）是机器人惯性参数的线性函数，这是能够进行机器人惯性参数辨识的第一前提。根据这一性质，可将动力学方程写成如下表达式：
在这里插入图片描述

2.代码

动力学方程线性化的具体推导过程可参考书本和网上其他大佬的文章，这里直接上代码。

function Y = Ymatrix_clc(q,dq,ddq)
q1=q(1); q2=q(2); q3=q(3);q4=q(4); q5=q(5); q6=q(6);
dq1=dq(1); dq2=dq(2); dq3=dq(3);dq4=dq(4); dq5=dq(5); dq6=dq(6);
ddq1=ddq(1); ddq2=ddq(2); ddq3=ddq(3);ddq4=ddq(4); ddq5=ddq(5); ddq6=ddq(6);
global a3 a4 d2 d4 d5 d6 g;
global Pc11 Pc22 Pc33 Pc44 Pc55 Pc66;
% 坐标系旋转轴
axis1=[0 0 1]';axis2=[0 0 1]';axis3=[0 0 1]';axis4=[0 0 1]';axis5=[0 0 1]';axis6=[0 0 1]';
% 坐标系变换矩阵
T01 = MDHTrans(0, 0, 0, q1);
T12 = MDHTrans(pi/2, 0, d2, q2+pi/2);
T23 = MDHTrans(0, a3, 0, q3);
T34 = MDHTrans(0, a4, d4, q4-pi/2);
T45 = MDHTrans(-pi/2, 0, d5, q5);
T56 = MDHTrans(pi/2, 0, d6, q6);
T67 = MDHTrans(0, 0, 0, 0);
R01=T01(1:3,1:3);R10=R01';P01=T01(1:3,4);
R12=T12(1:3,1:3);R21=R12';P12=T12(1:3,4);
R23=T23(1:3,1:3);R32=R23';P23=T23(1:3,4);
R34=T34(1:3,1:3);R43=R34';P34=T34(1:3,4);
R45=T45(1:3,1:3);R54=R45';P45=T45(1:3,4);
R56=T56(1:3,1:3);R65=R56';P56=T56(1:3,4);
R67=T67(1:3,1:3);R76=R67';P67=T67(1:3,4);
%% 外推
w00=[0 0 0]';dw00=[0 0 0]';dv00=[0 0 g]';dvc00=[0 0 g]';
[w11,dw11,dv11,dvc11]=motion_para_clc(R10,dq1,ddq1,w00,dw00,dv00,P01,Pc11,axis1);
[w22,dw22,dv22,dvc22]=motion_para_clc(R21,dq2,ddq2,w11,dw11,dv11,P12,Pc22,axis2);
[w33,dw33,dv33,dvc33]=motion_para_clc(R32,dq3,ddq3,w22,dw22,dv22,P23,Pc33,axis3);
[w44,dw44,dv44,dvc44]=motion_para_clc(R43,dq4,ddq4,w33,dw33,dv33,P34,Pc44,axis4);
[w55,dw55,dv55,dvc55]=motion_para_clc(R54,dq5,ddq5,w44,dw44,dv44,P45,Pc55,axis5);
[w66,dw66,dv66,dvc66]=motion_para_clc(R65,dq6,ddq6,w55,dw55,dv55,P56,Pc66,axis6);
% 辅助矩阵H
H1=getHi(w11,dw11,dv11);
H2=getHi(w22,dw22,dv22);
H3=getHi(w33,dw33,dv33);
H4=getHi(w44,dw44,dv44);
H5=getHi(w55,dw55,dv55);
H6=getHi(w66,dw66,dv66);
% 辅助矩阵A
A1=getAi(w11,dw11,dv11);
A2=getAi(w22,dw22,dv22);
A3=getAi(w33,dw33,dv33);
A4=getAi(w44,dw44,dv44);
A5=getAi(w55,dw55,dv55);
A6=getAi(w66,dw66,dv66);
% 辅助矩阵Yf、Yn
Yf6=[zeros(3,50) H6]; Yn6=[zeros(3,50) A6]; 
[Yf5, Yn5]=get_Yf_Yn(5,R56,H5,A5,Yf6,Yn6,P56);
[Yf4, Yn4]=get_Yf_Yn(4,R45,H4,A4,Yf5,Yn5,P45);
[Yf3, Yn3]=get_Yf_Yn(3,R34,H3,A3,Yf4,Yn4,P34);
[Yf2, Yn2]=get_Yf_Yn(2,R23,H2,A2,Yf3,Yn3,P23);
[Yf1, Yn1]=get_Yf_Yn(1,R12,H1,A1,Yf2,Yn2,P12);
% 线性矩阵Y
Y(1,1:60)=axis1'*Yn1;
Y(2,1:60)=axis2'*Yn2;
Y(3,1:60)=axis3'*Yn3;
Y(4,1:60)=axis4'*Yn4;
Y(5,1:60)=axis5'*Yn5;
Y(6,1:60)=axis6'*Yn6;
end

function K=getK(vec3)
K=[vec3(1)  vec3(2)  vec3(3)    0        0        0;
     0      vec3(1)    0      vec3(2)  vec3(3)    0;
     0        0      vec3(1)    0      vec3(2)  vec3(3)];
end

% 叉积矩阵
function S=getS(vec3)
S=[   0     -vec3(3)  vec3(2);
   vec3(3)     0     -vec3(1);
   -vec3(2) vec3(1)     0    ];
end

function Hi=getHi(wi,dwi,dvi)
Hi(1:3,7:9)=getS(dwi)+getS(wi)*getS(wi);
Hi(1:3,10)=dvi;
Hi(1:3,1:6)=zeros(3,6);
end

function Ai=getAi(wi,dwi,dvi)
Ai(1:3,1:6)=getK(dwi)+getS(wi)*getK(wi);
Ai(1:3,7:9)=-getS(dvi);
Ai(1:3,10)=zeros(3,1);
end

function [Yf, Yn] = get_Yf_Yn(id, R, H, A, Yf_next, Yn_next, Po)
c1=(id-1)*10+1;c2=(id-1)*10+10;
Yf=zeros(3,60);Yf(:,c1:c2)=H;
Yf=Yf+R*Yf_next;
Yn=zeros(3,60);Yn(:,c1:c2)=A;
Yn=Yn+R*Yn_next+getS(Po)*R*Yf_next;
end

三、DH法推导最小惯性参数集

1、推导流程

①参考《机器人动力学与控制》可知，转动关节i有3种情况，下面简单说明一下：
情况R1：关节0~关节i-1中，存在其轴向不与关节i轴向平行的转动关节。对应的最小惯性参数为：

XX*=XXi-YYi,XYi,XZi,YZi,ZZi,MXi,MYi;

情况R2：关节0~关节i-1中，所有转动关节的轴向均与关节i的轴向平行，但存在轴向不与关节i的轴向重合的转动关节或轴向不予关节i轴向平行的移动关节。对应的最小惯性参数为：

ZZi,MXi,MYi;

情况R3：关节0~关节i-1中，所有转动关节的轴向均与关节i的轴向重合，所有移动关节的轴向均与关节i的轴向平行。对应的最小惯性参数为：

ZZi;

②对于ur5e机械臂而言，轴2~轴6均属于情况R1，轴1属于情况R3，所以最小惯性参数集的个数为7*5+1=36个。

2、代码

DH法推导连杆最小惯性参数集的具体过程可参考书本和网上其他大佬的文章，这里直接上代码。

function [Yr, Pr] = Yr_Pr_clc(q,dq,ddq)
global m1 m2 m3 m4 m5 m6 a3 a4 d2 d4 d5 d6;
global Pc11 Pc22 Pc33 Pc44 Pc55 Pc66 P;
Y=Ymatrix_clc(q,dq,ddq);
I1xx=P(1);I2xx=P(11);I3xx=P(21);I4xx=P(31);I5xx=P(41);I6xx=P(51);
I1xy=P(2);I2xy=P(12);I3xy=P(22);I4xy=P(32);I5xy=P(42);I6xy=P(52);
I1xz=P(3);I2xz=P(13);I3xz=P(23);I4xz=P(33);I5xz=P(43);I6xz=P(53);
I1yy=P(4);I2yy=P(14);I3yy=P(24);I4yy=P(34);I5yy=P(44);I6yy=P(54);
I1yz=P(5);I2yz=P(15);I3yz=P(25);I4yz=P(35);I5yz=P(45);I6yz=P(55);
I1zz=P(6);I2zz=P(16);I3zz=P(26);I4zz=P(36);I5zz=P(46);I6zz=P(56);
Pc11x=Pc11(1);Pc22x=Pc22(1);Pc33x=Pc33(1);Pc44x=Pc44(1);Pc55x=Pc55(1);Pc66x=Pc66(1); 
Pc11y=Pc11(2);Pc22y=Pc22(2);Pc33y=Pc33(2);Pc44y=Pc44(2);Pc55y=Pc55(2);Pc66y=Pc66(2);
Pc11z=Pc11(3);Pc22z=Pc22(3);Pc33z=Pc33(3);Pc44z=Pc44(3);Pc55z=Pc55(3);Pc66z=Pc66(3);

%axis2->6:R1,axis1:R3
YY6 = I6yy; M6 = m6;
MX6 = m6*Pc66x; MY6 = m6*Pc66y; MZ6 = m6*Pc66z;
XX6 = I6xx-YY6; XY6 = I6xy; XZ6 = I6xz; YZ6 = I6yz; ZZ6 = I6zz;
YY5 = I5yy; M5 = m5+M6;
MX5 = m5*Pc55x; MY5 = m5*Pc55y-(MZ6+d6*M6); MZ5 = m5*Pc55z;
XX5 = I5xx+YY6+2*d6*MZ6+d6^2*M6-YY5; XY5 = I5xy; XZ5 = I5xz; YZ5 = I5yz; ZZ5 = I5zz+YY6+2*d6*MZ6+d6^2*M6;
YY4 = I4yy; M4 = m4+M5;
MX4 = m4*Pc44x; MY4 = m4*Pc44y+(MZ5+d5*M5); MZ4 = m4*Pc44z;
XX4 = I4xx+YY5+2*d5*MZ5+d5^2*M5-YY4; XY4 = I4xy; XZ4 = I4xz; YZ4 = I4yz; ZZ4 = I4zz+YY5+2*d5*MZ5+d5^2*M5;
YY3 = I3yy+YY4+2*d4*MZ4+(d4^2+a4^2)*M4; M3 = m3+M4;
MX3 = m3*Pc33x+a4*M4; MY3 = m3*Pc33y; MZ3 = m3*Pc33z+MZ4+d4*M4;
XX3 = I3xx+YY4+2*d4*MZ4+d4^2*M4-YY3; XY3 = I3xy; XZ3 = I3xz-a4*(MZ4+d4*M4); YZ3 = I3yz; ZZ3 = I3zz+a4^2*M4; 
YY2 = I2yy+(a3^2)*M3+YY3; M2 = m2+M3;
MX2 = m2*Pc22x+a3*M3; MY2 = m2*Pc22y; MZ2 = m2*Pc22z+MZ3;
XX2 = I2xx+YY3-YY2; XY2 = I2xy; XZ2 = I2xz-a3*(MZ3); YZ2 = I2yz; ZZ2 = I2zz+a3^2*M3; 
ZZ1 = I1zz+YY2+2*d2*MZ2+d2^2*M2;
p1_min = [ZZ1]';
p2_min = [XX2 XY2 XZ2 YZ2 ZZ2 MX2 MY2]';
p3_min = [XX3 XY3 XZ3 YZ3 ZZ3 MX3 MY3]';
p4_min = [XX4 XY4 XZ4 YZ4 ZZ4 MX4 MY4]';
p5_min = [XX5 XY5 XZ5 YZ5 ZZ5 MX5 MY5]';
p6_min = [XX6 XY6 XZ6 YZ6 ZZ6 MX6 MY6]';
Pmin =[p1_min;p2_min ;p3_min ;p4_min ;p5_min ;p6_min ];
Yr=zeros(6,36);
Yr(:,1)=Y(:,6);
Yr(:,2)=Y(:,11);Yr(:,3)=Y(:,12);Yr(:,4)=Y(:,13);Yr(:,5)=Y(:,15);Yr(:,6)=Y(:,16);Yr(:,7)=Y(:,17);Yr(:,8)=Y(:,18);
Yr(:,9)=Y(:,21);Yr(:,10)=Y(:,22);Yr(:,11)=Y(:,23);Yr(:,12)=Y(:,25);Yr(:,13)=Y(:,26);Yr(:,14)=Y(:,27);Yr(:,15)=Y(:,28);
Yr(:,16)=Y(:,31);Yr(:,17)=Y(:,32);Yr(:,18)=Y(:,33);Yr(:,19)=Y(:,35);Yr(:,20)=Y(:,36);Yr(:,21)=Y(:,37);Yr(:,22)=Y(:,38);
Yr(:,23)=Y(:,41);Yr(:,24)=Y(:,42);Yr(:,25)=Y(:,43);Yr(:,26)=Y(:,45);Yr(:,27)=Y(:,46);Yr(:,28)=Y(:,47);Yr(:,29)=Y(:,48);
Yr(:,30)=Y(:,51);Yr(:,31)=Y(:,52);Yr(:,32)=Y(:,53);Yr(:,33)=Y(:,55);Yr(:,34)=Y(:,56);Yr(:,35)=Y(:,57);Yr(:,36)=Y(:,58);
end

四、通过simulink进行参数辨识

1、搭建simulink

通过solidworks生成ur5e的urdf模型，然后通过Simscape模块导入urdf文件（若不熟悉这一步的话，网上有很多资料可以参考）。输入激励轨迹（一般都是采用傅里叶级数，本文是简单的正弦曲线），输出角速度、角加速度、角加加速度和力矩。simulink模型和仿真效果如下：
在这里插入图片描述

2、验证最小惯性参数集的正确性

在辨识之前，先验证一下通过DH法得出的最小惯性参数集的正确性，只需要将其计算出的力矩与牛顿欧拉法计算出的力矩对比即可。代码如下：

format long g;
si = 1000;
ei = 2000;
step = 2;
numOfData = (ei-si)/step+1;

tao1 = linspace(0,0,numOfData); tao_1 = linspace(0,0,numOfData);
tao2 = linspace(0,0,numOfData); tao_2 = linspace(0,0,numOfData);
tao3 = linspace(0,0,numOfData); tao_3 = linspace(0,0,numOfData);
tao4 = linspace(0,0,numOfData); tao_4 = linspace(0,0,numOfData);
tao5 = linspace(0,0,numOfData); tao_5 = linspace(0,0,numOfData);
tao6 = linspace(0,0,numOfData); tao_6 = linspace(0,0,numOfData);

% 激励轨迹：0.8 0.9 1.1 2.0 -2.0 2.0
for n = 1:numOfData
    i = step*(n-1)+si;
    q = [getdatasamples(out.q1,i), getdatasamples(out.q2,i), getdatasamples(out.q3,i),getdatasamples(out.q4,i), getdatasamples(out.q5,i), getdatasamples(out.q6,i)];
    dq = [getdatasamples(out.dq1,i), getdatasamples(out.dq2,i), getdatasamples(out.dq3,i),getdatasamples(out.dq4,i), getdatasamples(out.dq5,i), getdatasamples(out.dq6,i)];
    ddq = [getdatasamples(out.ddq1,i), getdatasamples(out.ddq2,i), getdatasamples(out.ddq3,i),getdatasamples(out.ddq4,i), getdatasamples(out.ddq5,i), getdatasamples(out.ddq6,i)];
    
    tau_ne = tau_newtonEuler(q,dq,ddq);%牛顿欧拉;
    tau_dh = tau_minPara(q,dq,ddq);%DH推导法得出最小惯性参数集 tau_dh = Yr*Pr
    
    tau1(n) = tau_ne(1); tao_1(n)=tau_dh(1);
    tau2(n) = tau_ne(2); tao_2(n)=tau_dh(2);
    tau3(n) = tau_ne(3); tao_3(n)=tau_dh(3);
    tau4(n) = tau_ne(4); tao_4(n)=tau_dh(4);
    tau5(n) = tau_ne(5); tao_5(n)=tau_dh(5);
    tau6(n) = tau_ne(6); tao_6(n)=tau_dh(6);
end

figure;
subplot(6,1,1);
plot(tao1-tao1_sim);
subplot(6,1,2);
plot(tao2-tao2_sim);
subplot(6,1,3);
plot(tao3-tao3_sim);
subplot(6,1,4);
plot(tao4-tao4_sim);
subplot(6,1,5);
plot(tao5-tao5_sim);
subplot(6,1,6);
plot(tao6-tao6_sim);

二者的力矩误差如下：各轴的力矩误差均可忽略不计，从而验证了DH法得出的最小惯性参数集的正确性。
在这里插入图片描述

3、最小惯性参数集辨识仿真

在确保DH法推导的最小惯性参数集的正确性后，就可以使用最小二乘法进行惯性参数辨识仿真了，进而再对比一下理论计算的最小惯性参数和辨识得到的最小惯性参数的差异。代码如下：

Y = [];
tau = [];
P_min_tar = [];
for n = 1:numOfData
    i = step*(n-1)+si;
    q = [getdatasamples(out.q1,i), getdatasamples(out.q2,i), getdatasamples(out.q3,i),getdatasamples(out.q4,i), getdatasamples(out.q5,i), getdatasamples(out.q6,i)];
    dq = [getdatasamples(out.dq1,i), getdatasamples(out.dq2,i), getdatasamples(out.dq3,i),getdatasamples(out.dq4,i), getdatasamples(out.dq5,i), getdatasamples(out.dq6,i)];
    ddq = [getdatasamples(out.ddq1,i), getdatasamples(out.ddq2,i), getdatasamples(out.ddq3,i),getdatasamples(out.ddq4,i), getdatasamples(out.ddq5,i), getdatasamples(out.ddq6,i)];
    [Yi, Pi]=Yr_Pr_clc(q,dq,ddq);
    Y=[Y;Yi];
    P_min_tar=Pi;
    tau=[tau,[getdatasamples(out.t1,i),getdatasamples(out.t2,i),getdatasamples(out.t3,i),getdatasamples(out.t4,i),getdatasamples(out.t5,i),getdatasamples(out.t6,i)]];
end
tau = tau';
P_min_dh=((Y.')*Y)\(Y.')*tau; %最小惯性参数集

4、最小惯性参数集辨识结果

理论计算的最小惯性参数：

P_min_tar =
          1.73280586521094
         -1.22621464999045
        -3.07797143175e-06
         0.209351461403325
     -2.80749536322978e-07
          1.23247325000955
                2.93983125
            5.76551262e-06
        -0.401838180397223
      1.74957253545263e-05
         0.087696953495475
     -1.58106453987442e-05
         0.405218681071967
              1.2260082735
          -3.561297669e-05
       0.00746037394564778
      -9.5193692497393e-08
      3.65460330521206e-07
       -5.672763564794e-05
       0.00793990403088091
           5.297734651e-06
              0.0698541493
       0.00144632379805677
        -9.02921367356e-08
     -2.48776012509969e-07
         2.02850108152e-05
       0.00184236388065459
          -4.720468995e-06
            -0.01393156732
     -7.09929169102549e-07
      1.79886124551368e-11
       1.3587491279684e-11
        4.969211180084e-08
      0.000150870070830898
            2.56432709e-10
           -3.93789391e-06

辨识得到的最小惯性参数：

P_min_dh =
          1.73281437472216
         -1.22621739477438
      2.34820898246615e-06
         0.209341956973868
      -9.1312134015978e-06
          1.23248090539727
          2.93983095468749
      1.61087317983262e-05
        -0.401836452294944
     -5.27748946241227e-06
        0.0958890130807421
      5.12542571678891e-06
         0.405218089210412
          1.22600840211475
     -3.08770165700332e-05
       0.00746047158632146
      1.72885888690218e-07
      2.93749041022902e-07
     -5.18735607006684e-06
       0.00793998442286271
      5.37256471722114e-06
        0.0698541707499729
       0.00144625019650412
      1.76421669287846e-07
      2.43710750398469e-07
      5.17551092948338e-06
       0.00184223262010027
     -4.75951834453638e-06
       -0.0139316021154916
     -7.09901810689332e-07
      -2.7364047543967e-09
      1.22848334758339e-08
     -2.88072688453827e-07
      0.000150868185882995
      1.24405617270275e-08
      -3.9256996599014e-06

5、结论

通过比较P_min_tar和P_min_dh，二者大小都是36，且各个数值非常接近，说明了辨识过程的正确性。至此ur5e机械臂动力学参数辨识仿真（无摩擦）的全部流程就结束了。

总结

其实关于动力学参数辨识的知识点和代码都是开源的，所以我也只是一名普通的知识搬运工、整合员。虽然知识开源，但我觉得能把它们系统地应用起来可能仍有五大难点：1、理论公式难以理解；2、各种资料鱼龙混杂；3、细节处理不好把握；4、稍有差错全盘皆输；5、遇事不决容易放弃。学习贵在坚持，坚持可以带来灵感，灵感可以事半功倍，希望这篇文章能给广大的志同道合的朋友带来帮助和参考，另外，若文章有不对的、不完善的或有需要补充的地方，请各位积极指导，大家互相交流、互相学习。

标签：仿真,tau,getdatasamples,clc,参数,全过程,Yr,辨识,out
From： https://blog.csdn.net/Nan_Feng520/article/details/140108798