CN105459116B - 一种基于磁强计的机器人遥操作装置及方法 - Google Patents

Abstract

一种基于磁强计的机器人遥操作装置及方法，通过空间磁场发生器产生立体空间的非均匀磁场，遥操作末端感知器可以感知其在空间非均匀磁场三个方向上的磁场分量，然后通过对三个方向的磁场分量进行计算推导得到末端感知器在空间非均匀磁场的位置和姿态，并将该位置姿态作为遥操作机器人的目标位姿，结合遥操作机器人当前的位姿，根据机器人的构型和运动反解给出遥操作机器人各个关节的运动轨迹规划，并将各关节的运动量信息发给执行机械臂的控制系统实现机器人的遥操作，用于控制遥操作机器人按照需要的轨迹进行运动。

Description

一种基于磁强计的机器人遥操作装置及方法

技术领域

本发明属于机器人遥操作技术领域，涉及一种基于磁强计的机器人遥操作装置及方法，该方法通过磁强计作为末端检测方法，通过该方法可以检测出机器人末端需要达到的位置和姿态。然后用该姿态作为机器人末端的目标位置进行运动。该方法可以用于同构机器人的遥操作系统。

背景技术

关于机器人遥操作方法，有多种形式，比如专利(CN201410016585.6)提出了一种空间机器人遥操作方法，采用三维图形仿真验证、执行能力验证双验证方法保证空间遥操作指令安全性；采用预测仿真驱动三维模型为操作者提供实时视觉信息，采用双边控制技术为操作人员提供力觉信息，该方法的计算量大，操作实时性差，并且对控制器的要求较高。另外专利(CN201210304630.9)一种遥操作机器人力反馈控制系统及控制方法，该发明控制系统能间接提取遥操作机器人系统的负载力，实现准确实时的力反馈，该方法操作不直观，很难了解当前的操作状态。但是从目前来开，还没有采用基于磁强计的检测方法来实现机器人遥操作。

发明内容

本发明的技术解决问题：克服现有技术的不足，提供一种基于磁强计的机器人遥操作装置及方法，解决了遥操作机器人末端位置感知和计算，并将感知的位姿作为遥操作机器人执行机器人的目标位置进行机器人的控制，实现机器人的遥操作。

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

一种基于磁强计的机器人遥操作方法，步骤如下：

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

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

3)建立如下坐标系：

31)建立磁场矢量空间坐标系O_dX_dY_dZ_d，其X_d轴方向为北，Y_d轴方向为东，Z_d轴方向为天；

32)建立磁北极坐标系O_mX_mY_mZ_m，其X_m轴为磁北向，Z_m轴为重力反方向，Y_m轴符合左手定则；

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

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

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

θ＝arctan(Y_h/X_h)；

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

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

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

本发明与现有技术相比具有以下优点：

本发明采用了磁强计和二维动态倾角仪来检查遥操作原边末端感知器的位置和姿态，并给出其在已知磁场中的位置和姿态，通过算法将其转化为执行机械臂的目标位置和姿态。该方法可以有效的减少常规视觉遥操作的大时延问题。同时该方法通过原边的执行操作来进行机械臂的位置和轨迹导引，避免了采用力反馈遥操作方式不直观的缺点。而且该方法能够同时适应同构型遥操作机器人系统和异构型遥操作机器人系统。采用同构型机器人遥操作系统将具有更加直观的特点。

附图说明

图1为空间磁场发生器组成示意图；

图2为末端感知器组合示意图；

图3为各坐标系变换关系；

图4为磁场分解示意图。

具体实施方式

本专利主要是提出了一种采用磁强计作为空间位姿测量并对机械臂进行摇操作的装置及方法。该装置主要包括以下部分：空间磁场发生器，基于磁强计和动态倾角仪的遥操作末端感知器，执行机械臂。其中空间磁场发生器用来在操作台处产生空间磁场。如图1所示，本专利涉及的空间磁场发生器包括左矩形线圈组1、右矩形线圈组2、以及由上述两组线圈产生的非均匀磁场3。该磁场可以通过精密测量可以对该磁场与空间位置关系进行精确标定，即可以得到磁场矢量与空间位置的函数关系或对应关系。

遥操作末端感知器主要有三轴磁强计和二维动态倾角仪组成，其中三轴磁强计可以敏感正交三个方向磁场的大小，二维动态倾角仪可以检测整个末端感知器相对于水平面的方位角和俯仰角。根据两种联合解算，可以求得末端感知器在操作平台坐标系内的位置和姿态。如图2所示，三轴磁强计的三个探头的轴分别于载体坐标系三轴平行，其中Xj轴为载体前进方向，Zj为载体水平前进时的指向天的方向，Yj轴为载体右向。其中磁强计的三个探头分别定义为Mcx，Mcy和Mcz，分别可以敏感载体坐标系Xj，Yj和Zj轴上的磁场强度大小。而二维动态倾角仪安装于载体坐标系OjXjYj平面内，这样倾角仪可以实时输出载体坐标系滚动角和俯仰角。

首先建立如下坐标系：

1)建立磁场矢量空间坐标系O_dX_dY_dZ_d，其X_d轴方向为北，Y_d轴方向为东，Z_d轴方向为天；

2)建立磁北极坐标系O_mX_mY_mZ_m，其X_m轴为磁北向，Z_m轴为重力反方向，Y_m轴符合左手定则；

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

整个坐标系的情况如图3所示。

要操作需要得到遥操作末端感知器在磁场矢量空间坐标系中的位置和姿态，因此以下的计算也以磁场矢量空间坐标系作为参考基准。其中假设载体坐标系起始状态与磁场矢量空间坐标系重合，则载体坐标系可以看作初始载体坐标系按照3-1-2的顺序转动得到：即先绕Z轴旋转θ，为偏航角；再绕旋转后的X轴旋转为滚动角；然后绕最后得到的Y轴旋转γ，为俯仰角。根据坐标变换关系可得：

其中c表示cos，s表示sin，以下相同。

则磁强计的三个探头分别定义为M_cx,M_cy和M_cz，分别可以敏感载体坐标系X_j，Y_j和Z_j轴上的磁场强度大小可以表示为：

其中表示地理坐标系中三个方向磁场大小。

由式1可以推出：

即

滚动角和俯仰角γ都可以由倾角仪直接给出，只需要求得偏航角θ就可以通过上式求出载体所在位置的地理坐标系上的三个方向的磁强度。下面来分析偏航角θ。

由于三轴磁强计的轴向与坐标系各轴重合时,因此当载体的滚动角和俯仰角都为零时,磁场矢量矢量可以分解如图4所示。则偏航角可以表示为^[6]：

θ＝arctan(Y_h/X_h) (4)

其中Y_h和X_h分别表示磁场水平方向的分量。

当载体处于任意姿态时，将M_cx,M_cy和M_cz分解到水平方向，可得：

这样由式4和式5可以求得θ，并将滚动角俯仰角γ和求得偏航角θ代入式3即可求得当前位置的地理坐标系中，磁场三个方向的分量M_x，M_y和M_z。

根据前期标定测量，磁场矢量空间坐标系中任意位置的磁场可以由磁场总强度F，水平分量H，磁倾角I(表示水平面和磁场向量F之间的夹角，在水平面以下为负)，磁偏角D(表示磁场向量在水平面的投影与正向的夹角，磁北极向为正)来描述。

根据几何关系，可以知道在当前位置的地理坐标系中，磁场三个方向的分量可以表示为：

由式6可以推出：

由式3和式7可以求得载体所在位置的水平分量H，磁倾角I和磁偏角D，根据I前期标定测量可以求得载体坐标系的位置，包括X、Y和Z三个方向的。至此载体的位置和姿态已经全部解算出来。

以遥操作末端感知器得到的位置和姿态作为机械臂的末端的目标位置，并根据机械臂当前位置进行轨迹规划和位置反解，可以求得要达到目标位置各关节运动量。其中位置反解是根据执行机械臂的结构参数和结构形式进行，并将各关节的运动量信息发给执行机械臂的控制系统，驱动机械臂末端运动到目标位置。

本发明说明书中未作详细描述的内容属本领域技术人员的公知技术。

Claims (2)

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

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

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

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

3)建立如下坐标系：

31)建立磁场矢量空间坐标系O_dX_dY_dZ_d，其X_d轴方向为北，Y_d轴方向为东，Z_d轴方向为天；

32)建立磁北极坐标系O_mX_mY_mZ_m，其X_m轴为磁北向，Z_m轴为重力反方向，Y_m轴符合左手定则；

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

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

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

θ＝arctan(Y_h/X_h)；

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

<mrow> <mfenced open = "{" close = ""> <mtable> <mtr> <mtd> <mrow> <mi>D</mi> <mo>=</mo> <mi>arctan</mi> <mfrac> <msub> <mi>M</mi> <mi>y</mi> </msub> <msub> <mi>M</mi> <mi>x</mi> </msub> </mfrac> </mrow> </mtd> </mtr> <mtr> <mtd> <mrow> <mi>H</mi> <mo>=</mo> <msqrt> <mrow> <msup> <msub> <mi>M</mi> <mi>x</mi> </msub> <mn>2</mn> </msup> <mo>+</mo> <msup> <msub> <mi>M</mi> <mi>y</mi> </msub> <mn>2</mn> </msup> </mrow> </msqrt> </mrow> </mtd> </mtr> <mtr> <mtd> <mrow> <mi>I</mi> <mo>=</mo> <mi>arctan</mi> <mfrac> <msub> <mi>M</mi> <mi>z</mi> </msub> <mi>H</mi> </mfrac> </mrow> </mtd> </mtr> </mtable> </mfenced> <mo>;</mo> </mrow>

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

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