g//大臂质量

g//小臂的质量

global L1  //大臂长度

global L2  //小臂长度

global Lc1 大臂质心到臂起点距离

global Lc2 小臂臂质心到臂起点距离

global I1  //大臂转动惯量

global I2  //小臂转动惯量

g重力加速度

global tau //转矩

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];//关节角度和角速度初始值 

//计算期望关节角度和角速度

for t=0:0.01:10.0 //时间间隔0.01，取1000个时间片段

  //存储当前时间点

 当前时刻机械臂末端位置所对应的关节角度

 //存储这一时间点的关节角度

 根据雅克比矩阵和机械臂末端速度求解关节角速度，保存

 //存储这一时间点机械臂末端位置坐标

end

nt=length(tt);//nt所有的时间点—即1000

for k=1:nt

 根据闭环系统刚度和阻尼比计算伺服控制规律补偿转矩

 根据动力学方程是求解系统转矩

 求解区间

 //求解动力学方程式—得到 

 取求解区间中最后一个解

 //保存当前时刻点关节角度和角速度

end

hold on;

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];// 

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当前机械臂处于当前位置时的关节角度大小 

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下载本文