3关节平面机械臂运动学方程

图1 3关节平面机械臂

1.3关节平面机械臂

⏹3关节平面机械臂有3个自由度，关节1有1个自由度，关节2有1个

自由度，关节3有1个自由度

⏹机器人手臂的几何尺寸（mm）：

关节1长度：L1

关节2长度：L2

关节3长度：L3

⏹关节的运动范围（右手）：如表1所示。

2.机器人手臂的坐标系建立

参考坐标系

(1) 为了对3关节平面机械臂进行控制，同时也便于描述机器人的动作状态，必须建立适当的初始坐标系。我们设定机械臂的初始姿态：关节1、关节2和关节3均处于水平姿态，与世界坐标系（x0,y0）的x0轴的夹角为0度。

参考坐标系（实验室坐标系）的设定如图1所示:

X轴：从关节i到关节i+1的方向定义为X轴，即沿连杆方向

y轴：根据X轴和Z轴的方向，以右手螺旋法则确定

Z轴：沿关节轴方向，即垂直纸面，从里向外为Z轴正方向

(2) 连杆参数

连杆参数列表如表2所示。

表2 连杆参数

正解：

连杆之间的齐次变换矩阵为：

⎥⎥⎥⎦

⎤

⎢⎢⎢⎣

⎡-=10000100

0000111101c s s c T 2212212

00000100001c s L s c T -⎡⎤

⎢⎥⎢⎥=⎢⎥⎢⎥⎣⎦

3323323

00000100

01c s L s c T -⎡⎤⎢⎥⎢⎥=⎢⎥⎢⎥⎣⎦

()()1231231231231231231231231121212001212312312312312312312312311212123123

()000010

0001c c c s s c c s s s c s s c c c s c c c s s s s L c L c c s s s c c c s c c c s s s s c c c s s c c s s s c s L s L s c c s T T T T ⎡----++-+-⎤

⎢

⎥

++----++⎢⎥==⎢⎥⎢

⎥

⎣

⎦

(1)

123

123112121231231121200123123

000010

01c s L c L c s c L s L s T T T T -+⎡⎤⎢⎥+⎢⎥

==⎢⎥⎢⎥

⎣⎦

(2)

00123123

000100

01c s x s c y T T T T φφφφ-⎡⎤⎢⎥⎢⎥

==⎢⎥⎢⎥⎣⎦

(3)

其中

c 1: cos Θ1 (4) c 2: cos Θ2 (5) s 1: sin Θ1 (6) s 1: sin Θ1 (7)

123c c φ= (8) 123s s φ= (9)

11212x L c L c =+ (10) 11212y L s L s =+ (11)

式(1)为3关节平面机械臂的变换矩阵，式(2)为采用三角函数和差角公式化简得到的，式(3)为式(2)的简化表示，式(4)- 式(11)为简化符号的详细表示。

反解：

图2 3关节平面机械臂的平面几何关系

图2给出了3关节平面机械臂的几何关系，可以看出，在世界坐标系（x 0,y 0）

下,由连杆L 1,L 2以及关节1和关节3的连线构成三角形。图中虚线所示为构成三角形的另一种情况，对于实线构成的三角形，采用余弦定理可得

2222121222cos(180)x y L L L L θ+=+-+ (12)

由于cos(180+Θ2)=-cos Θ2,所以

2222

12212

2x y L L c L L +--=

(13)

三角形成立的条件为2边之和大于第三边，因此L 1+L 2必须大于

解不存在。当反解存在时，即可由(13)式得出Θ2的值。

为求Θ1，可先求出β和ψ，根据三角函数与三角形各边的关系，应用2幅角反正切公式得：

tan 2(,)A y x β= (14)

再利用余弦定理求出ψ

2222

cos ψ=

(15)

1θβ=±ψ (16)

式中，+-号根据Θ2的符号取，当Θ2<0，取正号，反之取负号。 平面内旋转角度可加和，因此3个连杆的旋转角度之和即为末端连杆的姿态，也即机械臂末端的姿态。

123θθθ++=ψ (17)

由以上式(1) – 式(17)，可反解出所有连杆在世界坐标系的变换矩阵，即姿态。