机器人测控技术
大作业
题目: 电气工程学院
学院名称：电气工程学院
专业班级：自动
学生姓名：
学号： 2015
指导教师：***
考虑如图1所示的双关节刚性机械臂，试分析以下问题：
图1 双关节机械臂示意图
（1） 用D-H 建模法建立上述机械臂的运动学方程；
（2） 忽略重力、摩擦力和干扰项的情况下，建立该机械臂的动力学
方程；
（3） 如果取11l =，20.8l =，120.5m m ==，初始状态：
11220.100.10q q q q ⎡⎤⎡⎤
⎢⎥⎢⎥⎢⎥⎢⎥=⎢⎥⎢⎥⎢⎥⎢⎥⎣⎦
⎣⎦ 试设计一个PD 控制器，让其跟踪一条如下指定的曲线：
12sin 2sin 2d d q t
q t ππ=⎧⎨
=⎩，并利用Matlab 中给出仿真结果。

解：
Y 0
X 0
X 1
Y 1
X 2
Y2
①建立坐标系
a 、机座坐标系｛0｝
b 、杆件坐标系｛i ｝ ②确定参数
d i ——相邻坐标系x 轴之间的距离； θi ——相邻坐标系x 轴之间的夹角； l i ——相邻坐标系z 轴之间的距离； αi ——相邻坐标系z 轴之间的夹角。

③相邻杆件位姿矩阵
M 01=Rot(z,θ1)·Trans(l 1,0,0)
=
10000100001100
11θθθθc s s c -
1000010000101l 001 = 1
00001001
10111
1011θθθθθθs L c s c L s c -
同理可得：
M 12=Rot(z,θ2)·Trans(l 2,0,0)
=
1
00
001002
20222
2022θθθθθθs L c s c L s c -
M 23（h ）=Rot(z,θ3)·Trans(l 3,0,0)
=
1
00
001003
30333
3033θθθθθθs L c s c L s c -
④建立方程。

将相邻杆件位置矩阵依次相乘，得：
M 0h =M 01·M 12·M 23(h)=
1
00
00100123
3122110123
123123
3122110123123θθθθθθθθθθc l s l s l c s c l c l c l s c ++++-
结果中一些字母的含义如下：
C θ123=cos(θ1+θ2+θ3) s θ123=sin(θ1+θ2+θ3) c θ12=cos(θ1+θ2) s θ12=sin(θ1+θ2)
（2）二连杆机械臂的动能和势能:
2
1111111111111,,,cos 2K m v v d P m gh h d θθ=
===-
2211111111
1
,cos 2K m d P m gd θθ==-
2222221,2K m v P mgy =
=
()()
222
222
211212211212sin sin cos cos v x y x d d y d d θθθθθθ=+=++=--+
()()
()22222
221122122122112
2211221211cos 22cos cos K m d m d m d d P m gd m gd θθθθθθθθθθ⎧=++++⎪=>⎨
⎪=--+⎩
21K K K +=
2222121122122212211211()()22cos ()m m d m d m d d θθθθθθθ=
+++++
其中：
连杆1的动能K 1 和势能 P 1:
连杆2的动能K 2 和势能 P 2 : 总动能和总势能:
2
1P P P +=)cos(cos )(21221121θθθ+-+-=gd m gd m m
二连杆机械臂的拉格朗日函数L:
P K L -=
)2(21)(2122
2121222212121θθθθθ ++++=d m d m m
)cos(
cos )()(cos 2122112121212212θθθθθθθ++++++gd m gd m m d d m n i q L
q L dt d i
i i ,2,1,=∂∂-∂∂=
F
动力学方程:
111d L L T dt θθ∂∂=-
∂∂ 222d L L T dt θθ∂∂=-
∂∂
矩阵形式orm:
⎥⎦
⎤⎢⎣⎡+⎥⎦⎤⎢⎣⎡⎥⎦⎤⎢⎣⎡+⎥⎦⎤⎢⎣⎡⎥⎦⎤⎢⎣⎡+⎥⎦⎤⎢⎣⎡⎥⎦⎤⎢⎣⎡=⎥⎦⎤⎢⎣⎡21122
12212121211122221
22221112211121
2221
121121D D D D D D D D D D D D D D T T θθθθθθθθ (3)
自适应
PD 跟踪轨迹的simulink 模型
① S-Function 的s 函数如下：
② S-Function1的t函数如下：
图1 PD自适应关节1的跟踪输出和给定信号
图2 PD自适应关节2的跟踪输出和给定信号上面的是关节1和关节2的跟踪输出和相对应的给定信号。

关节1的跟踪输出是图4-2中的下面那条曲线，给定信号为上面的那条曲线，关节2的跟踪输出是图4-3的上面的曲线，给定信号为下面的那条曲线。