递推最小二乘辨识平面双机械臂Matlab代码

本代码是我的毕设里面的一部分，首先是将SCARA机器人简化成为简单的平面双机械臂模型，对其进行动力学建模，由于SCARA机器人的特性，动力学方程中的重力项可以忽略，最后将动力学方程化简成为AX=B的形式，用递推最小二乘的方法，对其中的惯性参数进行辨识，将模型的惯性参数分为六个未知量，在已知位置，速度，加速度和力矩的情况下，进行递推就可以最终得到辨识的六个参数。

有关递推最小二乘的原理，可以参考刘金琨老师的《系统辨识理论及MATLAB仿真》一书的第三章内容，书中详尽的描述了最小二乘，加权最小二乘，递推最小二乘等一系列基础辨识方法的知识，并附有详尽的代码和注释，很有参考价值。

递推最小二乘的核心公式如下：

关键就是通过之前时刻的估计值，以及下一时刻的估计值，更新增益矩阵K和P阵，一步一步实现递推。在调试的过程中，值得注意的一点是P和theta初值的选取，一共有两种方法，如下所示：

在我的调试中发现，书中提供的代码采用的是第二种方式，即任意假设的方式，尽管随着递推的进行，该初值对于辨识结果的影响会越来越小，但是我实际对比中发现，其效果在迭代了3000次以后还是不尽人意，因此如果不是对计算量又要求的 话，选用第一种方法能都得到更好的结果。

通过调用Robotic Toolbox中的rne函数，可以得到每一个关节的力矩输出，通过jtraj函数可以得到运动轨迹。不过在进行系统辨识的时候，观测矩阵的条件数越接近于1，得到的辨识效果就会越好，所以机械臂的运动轨迹选取又是一个很重要的问题，通常选用傅里叶级数的轨迹，轨迹中参数的选取是很重要的，因此在优化激励轨迹参数的时候又延伸出了很多的优化方法，比较常用的就是用条件数的方法，即寻求最小的条件数，在我参阅的论文里面，大多使用遗传算法对激励轨迹进行优化。这一部分的内容留待以后细说。

废话少说，直接贴代码了。欢迎看到我这篇博客的朋友在下面留言一起交流一下学习心得。毕竟我也只是一个菜鸟，还需要多多学习。

clc, clear, close all;
deg = pi/180;
L1 = Revolute('d', 0.265, 'a', 0.300, 'alpha', 0, ...
    'I', [0.003342, 0.050903, 0.052965, 0, 0, 0], ...
    'r', [0.1490, 0, 0.0150], ...
    'm', 3.0659, ...
    'Jm', 200e-6, ...
    'G', 1.0063, ...
    'B', 1.38e-3, ...
    'Tc', [0.132, -0.105], ...
    'qlim', [-180 180]*deg );


L2 = Revolute('d', 0.34, 'a',0.300, 'alpha', 0,  ...
    'I', [0.027638, 0.083822, 0.066801, 0, 0, 0], ...
    'r', [0.0824, 0.0, 0.1007], ...
    'm', 7.1715, ...
    'Jm', 33e-6, ...
    'G', 1.0364, ...
    'B', 71.2e-6, ...
    'Tc', [11.2e-3, -16.9e-3], ...
    'qlim', [-180 180]*deg);
bot = SerialLink([L1 L2], 'name', 'SCARA');


syms a1 a2 a3 b1 b2 b3 q0 t a21 a22 a23 b21 b22 b23 q20;
a1 = 0.999023;
a2 = 0.000489;
a3 = -0.10124;
b1 = -0.998046;
b2 = -0.040547;
b3 = 0.106009;
q0 = 0.681851;
a21 = -0.749267;
a22 = 0.002565;
a23 = -0.111016;
b21 = 0.741939;
b22 = 0.022350;
b23 = -0.106619;
q20 = 0.330890;
N = 300;
t = [0:3/N:3-3/N];
wf = 2*pi/3;
q1 = q0 + a1*sin(wf*t)/wf + a2*sin(2*wf*t)/(2*wf) + a3*sin(3*wf*t)/(3*wf) - ...
     b1*cos(wf*t)/wf - b2*cos(2*wf*t)/(2*wf) - b3*cos(3*wf*t)/(3*wf);
q1d = a1*cos(wf*t) + a2*cos(2*wf*t) + a3*cos(3*wf*t) + b1*sin(wf*t) + ...
      b2*sin(2*wf*t) + b3*sin(3*wf*t);
q1dd = b1*wf*cos(wf*t) + b2*wf*2*cos(2*wf*t) + b3*wf*3*cos(3*wf*t) -...
      a1*wf*sin(wf*t) -a2*2*wf*sin(2*wf*t) -a3*wf*3*sin(3*wf*t);
q21 = q20 + a21*sin(wf*t)/wf + a22*sin(2*wf*t)/(2*wf) + a23*sin(3*wf*t)/(3*wf)...
      - b21*cos(wf*t)/wf - b22*cos(2*wf*t)/(2*wf) - b23*cos(3*wf*t)/(3*wf);
q21d = a21*cos(wf*t) + a22*cos(2*wf*t) + a23*cos(3*wf*t) + b21*sin(wf*t)...
      + b22*sin(2*wf*t) + b23*sin(3*wf*t);
q21dd = b21*wf*cos(wf*t) + b22*wf*2*cos(2*wf*t) + b23*wf*3*cos(3*wf*t) -...
      a21*wf*sin(wf*t) -a22*2*wf*sin(2*wf*t) -a23*wf*3*sin(3*wf*t);


lj=[q1',q21'];
ljd=[q1d',q21d'];
ljdd=[q1dd',q21dd'];
tau=bot.rne(lj,ljd,ljdd,[0 0 0]');


 k=2;
 %   [q1dd(1,k-1) , q1dd(1,k-1)+q21dd(1,k-1) , q1dd(1,k-1) , q1dd(1,k-1)+q21dd(1,k-1) , q1dd(1,k-1) ,...   
 %   (2*q1dd(1,k-1)+q21dd(1,k-1))*cos(q21(1,k-1))-2*q1d(1,k-1)*q21d(1,k-1)*sin(q21(1,k-1))-q21d(1,k-1)*q21d(1,k-1)*sin(q21(1,k-1))]';...
h1= [0, q1dd(1,k-1)+q21dd(1,k-1) , 0, q1dd(1,k-1)+q21dd(1,k-1) , 0 , q1dd(1,k-1)*cos(q21(1,k-1))+q1d(1,k-1)*q1d(1,k-1)*sin(q21(1,k-1))]';
H(k-1,:)=h1;


c0=((H*H')\H*tau(k-1,2)')';
p0=inv(H*H');
E=0.000000005;    %相对误差
c=[c0,zeros(6,N-1)];    %被辨识参数矩阵的初始值及大小
e=zeros(6,N);     %相对误差的初始值及大小
lamt=1;
for k=3:N; 
    %[q1dd(1,k-1) , q1dd(1,k-1)+q21dd(1,k-1) , q1dd(1,k-1) , q1dd(1,k-1)+q21dd(1,k-1) , q1dd(1,k-1) ,...   
    %(2*q1dd(1,k-1)+q21dd(1,k-1))*cos(q21(1,k-1))-2*q1d(1,k-1)*q21d(1,k-1)*sin(q21(1,k-1))-q21d(1,k-1)*q21d(1,k-1)*sin(q21(1,k-1))]';...
    h1=[0, q1dd(1,k-1)+q21dd(1,k-1) ,0, q1dd(1,k-1)+q21dd(1,k-1) , 0 , q1dd(1,k-1)*cos(q21(1,k-1))+q1d(1,k-1)*q1d(1,k-1)*sin(q21(1,k-1))]';
    k1=(p0*h1)/(h1'*p0*h1+1*lamt);   %求出K的值
    new=tau(k-1,2)'-h1'*c0; 
    c1=c0+k1*new;     %求被辨识参数c
    p1=p0-p0*h1*inv(1+h1'*p0*h1)*h1'*p0;
    e1=(c1-c0)./c0;   %求参数当前值与上一次的值的差值
    e(:,k)=e1;        %把当前相对变化的列向量加入误差矩阵的最后一列    
    c(:,k)=c1;        %把辨识参数c列向量加入辨识参数矩阵的最后一列 
    c0=c1;            %新获得的参数作为下一次递推的旧参数
    p0=p1;
    if norm(e1)<=E 
        break;        %若参数收敛满足要求，终止计算
    end
end


a1=c(1,:); a2=c(2,:); a3=c(3,:); a4=c(4,:); a5=c(5,:); a6=c(6,:);
ea1=e(1,:); ea2=e(2,:); ea3=e(3,:); ea4=e(4,:); ea5=e(5,:); ea6=e(6,:);
for k = 3:N;


phi=[q1dd(1,k-1) , q1dd(1,k-1)+q21dd(1,k-1) , q1dd(1,k-1) , q1dd(1,k-1)+q21dd(1,k-1) , q1dd(1,k-1) ,...   
    (2*q1dd(1,k-1)+q21dd(1,k-1))*cos(q21(1,k-1))-2*q1d(1,k-1)*q21d(1,k-1)*sin(q21(1,k-1))-q21d(1,k-1)*q21d(1,k-1)*sin(q21(1,k-1));...
    0, q1dd(1,k-1)+q21dd(1,k-1) ,0, q1dd(1,k-1)+q21dd(1,k-1) , 0 , q1dd(1,k-1)*cos(q21(1,k-1))+q1d(1,k-1)*q1d(1,k-1)*sin(q21(1,k-1))]';
ntau(k,:) = phi'*c1;
end
figure(1);
i=1:N;
plot(i,a1,'k',i,a2,'b',i,a3,'r',i,a4,'g',i,a5,'y',i,a6,'m') %画出辨识结果
legend('a1','a2','a3','a4','a5','a6');
title('递推最小二乘参数辨识')
figure(2); 
i=1:N; 
plot(i,ea1,'k',i,ea2,'b',i,ea3,'r',i,ea4,'g',i,ea5,'y',i,ea6,'m') %画出辨识结果的收敛情况
legend('a1','a2','a3','a4','a5','a6');
title('辨识精度');
figure(3);
plot(tau(:,1));hold on;plot(ntau(:,1));
legend('实际模型','辨识模型');
title('第一关节力矩');
figure(4);
plot(tau(:,2));hold on;plot(ntau(:,2));
legend('实际模型','辨识模型');
title('第二关节力矩');

参考的几篇硕士论文：

SCARA机器人动力学分析及鲁棒性控制研究_闫昊

SCARA机器人参数自适应与补偿控制研究_于昊

SCARA机械手运动特性与控制策略研究_赵威

工业机器人动力学参数辨识方法研究_耿令波