双连杆平面机械手跟踪Matlab程序详解

//全局变量宏定义globalm1//大臂质量globalm2//小臂的质量globalL1//大臂长度globalL2//小臂长度globalLc1//大臂质心到臂起点距离globalLc2//小臂臂质心到臂起点距离globalI1//大臂转动惯量globalI2//小臂转动惯量globalg//重力加速度globaltau//转矩m1=1;m2=0.2;L1=1;L2=0.6;Lc1=0.5;Lc2=0.3;I1=0.1;I2=0.02;g=9.8;clear;clc;parameters;//参数传递xx=[];//关节角度和角速度存储矩阵tt=[];//时间点矩阵th=[];//存取关节角度矩阵dth=[];//存取关节角速度矩阵r=[];//存取机械臂末端位置坐标矩阵kp=100;//闭环系统的刚度kd=14;//阻尼比x=[-0.6857;2.6559;0;0];//关节角度和角速度初始值1212//计算期望关节角度和角速度fort=0:0.01:10.0//时间间隔0.01，取1000个时间片段tt=[tt,t];//存储当前时间点thk=th_path(t);//当前时刻机械臂末端位置所对应的关节角度12th=[th,thk];//存储这一时间点的关节角度dth=[dth,inv(Jaccbi(thk,t))*dr_path(t)];//根据雅克比矩阵和机械臂末端速度求解关节角速度12，保存r=[r,r_path(t)];//存储这一时间点机械臂末端位置坐标endnt=length(tt);//nt所有的时间点—即1000fork=1:nttau=kp*(th(:,k)-x(1:2))+kd*([0;0]-x(3:4));//根据闭环系统刚度和阻尼比计算伺服控制规律补偿转矩[Mc,Hc]=estimat_M_H(x);tau=Mc*tau+Hc;//根据动力学方程是求解系统转矩tspan=[(k-1)/10:0.01:k/10];//求解区间[t,x1]=ode45(@d_f,tspan,x);//求解动力学方程式—得到1212x=x1(end,:)';//取求解区间中最后一个解xx=[xx,x];//保存当前时刻点关节角度和角速度endholdon;plot(L1*cos(xx(1,:))+L2*cos(xx(1,:)+xx(2,:)),L1*sin(xx(1,:))+L2*sin(xx(1,:)+xx(2,:)));plot(r(1,:),r(2,:),'.-');//动力学方程式function[v]=d_f(t,x)parameters;th_1=x(1);//关节1角度th_2=x(2);//关节2角度thd_1=x(3);//关节1角速度thd_2=x(4);//关节2角速度M=[m1*Lc1^2+I1+I2+m2*(L1^2+Lc2^2+2*L1*Lc2*cos(th_2))I2+m2*(Lc2^2+L1*Lc2*cos(th_2));I2+m2*(Lc2^2+L1*Lc2*cos(th_2))m2*Lc2^2+I2];vec=[-m2*L1*Lc2*sin(th_2)*(thd_2^2+2*thd_1*thd_2);m2*L1*Lc2*sin(th_2)*thd_1^2];grav=g*[m1*Lc1*cos(th_1)+m2*(L1*cos(th_1)+Lc2*cos(th_1+th_2));m2*Lc2*cos(th_1+th_2)];result=M\(-vec-grav+tau);thdd_1=result(1);thdd_2=result(2);v=[thd_1;thd_2;thdd_1;thdd_2];//1212end//根据控制要求计算机械臂末端位置坐标function[r]=r_path(t)parameters;r=[0;0];//机械臂末端位置坐标r(1)=0.1*cos(0.2*pi*t)+0.5;//计算x轴坐标r(2)=0.1*sin(0.2*pi*t);//计算y轴坐标end//根据控制要求计算机械臂末端速度function[dr]=dr_path(t)parameters;dr=[0;0];//机械臂末端速度大小（x轴、y轴方向）dr(1)=-0.1*0.2*pi*sin(0.2*pi*t);//计算x轴方向速度dr(2)=0.1*0.2*pi*cos(0.2*pi*t);//计算y轴方向速度end//根据逆运动学公式----上面两个函数求的期望控制要求的机械臂末端位置计算关节控制角度。function[th]=th_path(t)parameters;r0=r_path(t);//r0当前机械臂末端位置坐标alpha=acos((L1^2+L2^2-r0(1)^2-r0(2)^2)/(2*L1*L2));th_2=pi-alpha;th_1=atan(r0(2)/r0(1))-atan(L2*sin(th_2)/(L1+L2*cos(th_2)));th=[th_1;th_2];//th当前机械臂处于当前位置时的关节角度大小12end//雅可比矩阵function[J]=Jaccbi(th,t)parameters;th_1=th(1);th_2=th(2);J=[-L1*sin(th_1)-L2*sin(th_1+th_2);L2*sin(th_1+th_2);L1*cos(th_1)+L2*cos(th_1+th_2)L2*cos(th_1+th_2)];End//重力矩阵、柯氏力和重力矩阵估计function[M,H]=estimat_M_H(x)parameters;th_1=x(1);th_2=x(2);thd_1=x(3);thd_2=x(4);M=[m1*Lc1^2+I1+I2+m2*(L1^2+Lc2^2+2*L1*Lc2*cos(th_2))I2+m2*(Lc2^2+L1*Lc2*cos(th_2));I2+m2*(Lc2^2+L1*Lc2*cos(th_2))m2*Lc2^2+I2];vec=[-m2*L1*Lc2*sin(th_2)*(thd_2^2+2*thd_1*thd_2);m2*L1*Lc2*sin(th_2)*thd_1^2];grav=g*[m1*Lc1*cos(th_1)+m2*(L1*cos(th_1)+Lc2*cos(th_1+th_2));m2*Lc2*cos(th_1+th_2)];H=vec+grav;End