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

时间: 2023-10-04 admin IT培训

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

递推最小二乘辨识平面双机械臂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;     %求被辨识参数cp1=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
enda1=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机械手运动特性与控制策略研究_赵威

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

转载于:.html