递推最小二乘辨识平面双机械臂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;        %若参数收敛满足要求，终止计算    endenda1=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;endfigure(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机械手运动特性与控制策略研究_赵威
工业机器人动力学参数辨识方法研究_耿令波
                                             
        1        0           
	
				
	
					   
递推最小二乘辨识平面双机械臂Matlab代码
	  	   
系统辨识char3_3——递推最小二乘辨识程序
	  	   
智能建模之最小二乘辨识LS：Matlab实现
	  	   
系统辨识char3_2——最小二乘辨识程序
	  	   
最小二乘拟合平面
	  	   
系统辨识char3_1——polyfit实现最小二乘辨识的原理
	  	   
最小二乘椭圆拟合matlab代码实现
	  	   
最小二乘拟合空间平面
	  	   
opencv最小二乘拟合平面
	  	   
opencv 最小二乘拟合平面
	  	   
最小二乘曲线拟合matlab实现
	  	   
matlab做偏最小二乘回归
	  	   
最小二乘曲线拟合的MATLAB仿真
	  	   
最小二乘曲线拟合matlab实现
	  	   
MATLAB最小二乘最优问题
	  	   
matlab 最小二乘学习法
	  	   
Matlab直线最小二乘拟合实现
	  	   
用matlab的右除实现最小二乘拟合
	     		  
	  	   
WdatePicker 日历控件的onchange事件屏蔽
	  	   
深度学习各种优化函数详解
	  	   
AndroidStudio生成全局变量快捷键
	  	   
利用自动索引计算数组元素的平方和
	  	   
线段树模板
	  	   
递推最小二乘辨识平面双机械臂Matlab代码
	  	   
hdu 5009 Paint Pearls（DP+链表优化）
	  	   
列类型
	  	   
父进程是init一定不会变僵尸进程吗？
	  	   
分类（Classification）：Probability Generative Model
	  	   
JS级联菜单
	  	   
CentOS 配置LAMP（Apache+MySQL+PHP）环境，并配置phpmyadmin
	  	   
题目1113：二叉树 九度OJ
	  	   
自定义控件（31）---贝塞尔曲线