机器人作业资料

一、〔25分〕如下图为SCARA型机器人手臂，根据D-H表示法建立坐标系，写出

所有的A矩阵，并根据A矩阵求运动学方程%,以及运动学反解。

解：D-H法各参数如下

i

64ai-\明

100

a%

20180

%W

300

I4

运动学反解:根据7一号厂1犷=])

。%+»〃丫cq+sq,W*知。凡+W4

f%+叽TQ+G。、,一$4+。巴.-SJ\+C]py

tn.io.ta,Pz-4-，2

000I

sin(2-4)=-sin3n+cos”〃、.

有4{x

fPx+qpv=&S2

解得q=arct^,~A)一①A="—h+P'+Py①=A

K2d3庐了Py

二、〔20分〕建立下列图所示二连杆平面操作臂的动力学方程。假设每个连杆

的质量可视为集中连杆末端的集中质量。质量分别为㈣和加2连杆长度为4

和，2o假设作用于每个关节的粘性摩擦系数分别为匕和匕。

解：由图意对连杆1和2有：

那么

根据拉格朗日方程：

三、〔20分〕要求一个6轴机器人的第1关节用3秒钟由初始角50移动到终

上角80。假设机器人从静止开始运动，最终停在目标点上，计算一条3次多

项式关节空间轨迹的系数，确定第1、2、3秒的关节角度、速度和加速度。

解：0(0)=珞=50O(tf)=ef=800(0)=00(tf)=0

设有三次多项式：。")=4+"+生”+卬3

那么3(t)=%+2。2f+3a5r0{t}=la2+643f

〃()二夕o=50

q=0

3

可得：生=下(。/一珞)=I。

290

那么：

四、〔35分〕求解机械臂的动力学控制问题。

二连杆机械臂的动力学模型如下：

其中，q⑺①⑺①⑺分别为关节的角位移、角速度和角加速度；

7

q(f)=mo，%⑺广为关节矢量；r(r)=|r,(r),r2(r)]为各关节上的力矩矢量；

Q(qQ))wRR为关节空间的惯性矩阵；〃①⑺①⑺)=[/?1,均「为非线性哥氏力和离心

力项；Gg(f))=[G「G2/为重力项。/(西))=[£,6/为库伦摩擦和粘性摩擦项。

根据二连杆机械臂的实际物理参数，得到该机械臂动力学模型的各参数表

达式:

其中。”=(叫+〃?2)/+mJ；+2m2“2c2，D、z=D21=m2l}+m2lJ2C2,D22=mJ1,g=9.8依〃P；

-1

叫=10.45/，取=55kg;4=0.25W,/2=0.16/w;q=c2=0.2N•m;v,=v2=0.1NS-/A?;

S1=sin(^),52=sin(^2),C}=cos(^),C2=cos(^2),C12=cos(q+%),

Si2=sin@+%)。

二连杆机械臂示意图

机械臂的PD控制原理：

采用PD控制的计算力矩法对机械臂进行轨迹跟踪控制，其控制律如下：

其中，c=%④，/，心分别为关节期望角位移、角速度和角

加速度，L，&分别为速度和位置的对角型反应增益矩阵，

。⑷J(%“)，G(q),后(")分别是。⑷，h(q百),G(q),F(0)的估计值。由于实际中机

械臂建模误差以及外部干扰等不确定因素的存在，机械臂动力学模型的各有关

参数表达式都存在一^定的误差。令△£>=D(q)-D(q),A/z=〃(d*)-1(/“),

△G=G(q)-G、),AF=尸①)-F(6/),那么闭环动态误差方程为

其中，△7=f>"SOg+M+AG+△/)为不确定项。

(1)〔35分〕当△「=()时，设计适宜的人和脑使跟踪误差收敛到0,画出各关节

的轨迹跟踪曲线。

(2)(5分〕当△件(),并假设方⑷=0.8。⑷，力(%*)=0.8%(卬"),

©(夕)=0.8G⑷，户⑷=0.8%g),采用计算力矩法进行控制将会有较大的误差，试

分析此时的控制效果。在上述PD控制的根底上设计智能控制补偿方法(可考虑

采用神经网络、模糊控制等方法)：

r=D(q)(qd+院e+kpe)+h(q、q)+G(q)+户((])+%D,抵消不确定项带来的跟踪误差。

假设关节角的期望信号为q4=0.2sin(R)-0.2sin(/),初值％=[0.02O.Olf,

4=[。0丫。

解：闭环动态误差方程为e+Ke+A/=0,进行拉氏变换得/+45+与=0,那么闭环

系统特征多项式为Y+as+k”，要使系统跟踪误差为0,那么系统必须稳定，要求闭环特

征方程的解在S平面的左半半面；取其解力2=-1土人得人.=2、kp=2o

假设期望的轨迹%=[2*sinQ)2*cos(/)],初始角位移“(())=[0;0]。仿真程序如下：

二连杆机械臂动力学方程dox.m如下所示：

functionxdot=dox(t,x)

globaltao;

ml=10.45;

m2=5.5;

g=9.8;

11=0.25;

12=0.16;

xdot=zeros(4,l);

c=[cos(x(l))cos(x(2))];

s=[sin(x(l))sin(x(2))];

c12=cos(x(1)+x(2));

sl2=sin(x(l)+x(2));

xdot(l)=x(3);

xdot(2)=x(4);

D=[(m1+m2)*l1.A2+m2*12.A2+2*m2*l1*12*c(2)m2*12.A2+m2*l1*12*c(2);

m2*12.A2+m2*ll*12*c(2)m2*12.八2];

H=[-m2*ll*12*(2*x(3)*x(4)+x(4).A2)*s(2)+(ni1+m2)*g*ll*c(l)+

m2*g*12*c1210.2*sign(x(3))10.1*x(3)

m2*ll*12*x(3).A2*s(2)+m2*g*12*c12+0.2*sign(x(4))+0.1*x(4)J;

a=2*sin(t);

b=2*cos(t);

tao=D*[a+2*b-2*x(3)-2*x(1);b-2*a-2*x(4)-2*x(2)]+H;

xdot(3:4)=-inv(D)*H+inv(D)*tao;

控制程序control.m如下所示：

globaltao;

t0=0;

gio；

x0=[0;0;0;0]

[t,xl