[发明专利]一种基于磁强计的机器人遥操作装置及方法

权利要求书

1.一种基于磁强计的机器人遥操作装置，其特征在于：包括空间磁场发生器、基于磁强计和动态倾角仪的遥操作末端感知器、执行机械臂；空间磁场发生器包括左矩形线圈组和右矩形线圈组，以及上述两个矩形线圈组产生的非均匀磁场；遥操作末端感知器包括三轴磁强计和二维动态倾角仪，其中三轴磁强计敏感正交三个方向磁场的大小，二维动态倾角仪检测整个末端感知器相对于水平面的方位角和俯仰角；执行机械臂用于进行任务执行的机械臂系统；遥操作末端感知器置于空间磁场发生器的产生的非均匀磁场范围内并按需要进行移动位置和变化姿态，末端感知器时刻检测当前位置的三个垂直方向的磁场强度，给出末端感知器在非均匀磁场中的位置和姿态，将该位置和姿态作为执行机械臂的目标位置用来驱动执行机械臂进行运动达到需要的位置和姿态。

2.一种基于磁强计的机器人遥操作方法，其特征在于步骤如下：

1)通过精密测量对磁场与空间位置关系进行精确标定，得到磁场矢量与空间位置的函数关系；磁场矢量空间坐标系中任意位置的磁场由磁场总强度F、水平分量H、磁倾角I、磁偏角D表示；其中磁倾角I表示水平面和磁场向量F之间的夹角，在水平面以下为负，磁偏角D表示磁场向量在水平面的投影与正向的夹角，磁北极向为正；

2)通过三轴磁强计的三个探头分别敏感得到三个探头方向上的磁场强度大小Mcx，Mcy和Mcz；

3)建立如下坐标系：

31)建立磁场矢量空间坐标系OdXdYdZd，其Xd轴方向为北，Yd轴方向为东，Zd轴方向为天；

32)建立磁北极坐标系OmXmYmZm，其Xm轴为磁北向，Zm轴为重力反方向，Ym轴符合左手定则；

33)建立载体坐标系OjXjYjZj，其中Xj轴为载体前进方向，Zj为载体水平前进时的指向天的方向，Yj轴为载体右向；

4)利用末端感知器中的二维动态倾角仪，实时得到载体坐标系滚动角和俯仰角γ；

5)根据以下公式计算得到偏航角θ：

θ＝arctan(Yh/Xh)；

6)将Mcx，Mcy和Mcz带入下式得到当前位置的位置坐标，即水平分量H，磁倾角I和磁偏角D；

D = arctan M y M x H = M x 2 + M y 2 I = arctan M z H ; ]]>

7)根据步骤1)得到的磁场矢量与空间位置的函数关系以及步骤6)得到的当前位置的位置坐标加上滚动角和俯仰角γ和偏航角θ，即可提出末端感知器的位置和姿态；

8)将末端感知器的位置和姿态作为执行机械臂的目标位置给执行机械臂，则机械臂根据自身的运动算法达到目标位置，实现遥操作。