[发明专利]平面内摆动伸缩型机器人运动控制方法

权利要求书

1.平面内摆动伸缩型机器人运动控制方法，其特征在于包括以下步骤：

采用坐标转换的方法得到机器人坐标系坐标与各关节位置的关系；

所述采用坐标转换的方法得到机器人坐标系坐标与各关节位置的关系包括以下步骤：

建立机器人坐标系；

确定动作类型标志；

根据动作类型标志建立正向与逆向转换关系；

所述建立机器人坐标系具体为：

机器人坐标系中有5个坐标轴X、Y、Z、A、B：

X轴为水平轴，以左侧回零位置为零点，向右为正方向；

Z轴为竖直轴，向上为正方向，且与X轴、Y轴相交，与Y轴的交点为Z轴的零点；

Y轴为水平轴，位于X轴下方，向左为正方向，与Z轴交点为零点；

A轴为旋转轴，与YZ平面垂直，轴心通过Y轴的零点；

B轴为旋转轴且与A轴平行，位于Y轴正方向和Z轴负方向；

两个旋转轴水平时为0度，逆时针旋转为正；

所述确定动作类型标志具体为：

沿Y轴、Z轴、X轴中至少一个轴移动，动作类型标志为0；

旋转轴A转动，动作类型标志为1；

旋转轴B转动，动作类型标志为2；

根据各关节的位置控制各关节的驱动器，实现机器人的运动控制。

2.根据权利要求1所述的平面内摆动伸缩型机器人运动控制方法，其特征在于根据动作类型标志建立正向转换关系，即根据各关节位置得到机器人坐标系坐标，包括：

1)动作类型为0：

ρ＝L+JP[2]

θ＝JP[3]*π/180

WP.X＝JP[0]

WP.Y＝ρ*cosθ

WP.Z＝-ρ*sinθ

WP.A＝JP[3]+JP[4]

WP.B＝0

2)动作类型为1：

WP.X＝JP[0]

WP.Y＝0

WP.Z＝JP[2]

WP.A＝JP[3]

WP.B＝JP[3]+JP[4]

3)动作类型为2为：

WP.X＝JP[0]

WP.Y＝0

WP.Z＝JP[2]

WP.A＝JP[3]+JP[4]

WP.B＝JP[3]

其中，WP.X，WP.Y，WP.Z，WP.A，WP.B分别表示机器人坐标系在X、Y、Z、A、B轴的坐标；JP[axis]表示各关节位置，axis＝0,1,2,3,4；axis＝0,2,3,4分别表示X轴、A轴与B轴之间的伸缩轴、旋转轴A、旋转轴B，axis＝1不表示任何关节，赋值为0；

L表示伸缩轴原始长度；其中ρ为极径，即旋转轴A与旋转轴B在YZ平面内的轴心距离，θ为极角，即旋转轴A当前的旋转角度对应的弧度。

3.根据权利要求1所述的平面内摆动伸缩型机器人运动控制方法，其特征在于根据动作类型标志建立逆向转换关系，即根据机器人坐标系坐标得到各关节位置，包括：

1)动作类型为0：

JP[0]＝WP.X

JP[1]＝0

JP[2]＝ρ-L

JP[3]＝θ

JP[4]＝-θ+WP.A

2)动作类型为1:

JP[0]＝WP.X

JP[1]＝0

JP[2]＝WP.Z

JP[3]＝WP.A

JP[4]＝-WP.A+WP.B

3)动作类型为2:

JP[0]＝WP.X

JP[1]＝0

JP[2]＝WP.Z

JP[3]＝WP.A-WP.B

JP[4]＝WP.B

其中，L表示伸缩轴原始长度；ρ为极径，即旋转轴A与旋转轴B在YZ平面内的轴心距离，θ为极角，即旋转轴A当前的旋转角度对应的弧度。

4.根据权利要求3所述的平面内摆动伸缩型机器人运动控制方法，其特征在于所述根据动作类型标志建立不同的逆向转换关系之前确定极径ρ:

5.根据权利要求3所述的平面内摆动伸缩型机器人运动控制方法，其特征在于所述根据动作类型标志建立不同的逆向转换关系之前确定极角θ，具体如下:

当WP.Y为0时判断WP.Z的数值确定θ数值：

41)若WP.Z大于0，θ＝-90；

42)若WP.Z小于0，θ＝90；

43)若WP.Z等于0，θ＝0；

当WP.Y不为0时判断WP.Y与WP.Z的数值确定θ数值：

44)若WP.Z大于0无论WP.Y为正或为负，

θ＝atan(fabs(WP.Z)/fabs(WP.Y))/π*180；

45)若WP.Z小于等于0，WP.Y小于0，θ＝(π-atan(fabs(WP.Z)/fabs(WP.Y)))/π*180

46)若WP.Z小于等于0，WP.Y大于0，θ＝atan(fabs(WP.Z)/fabs(WP.Y))/π*180；

atan表示反正切函数，fabs表示求绝对值函数。