[发明专利]基于常平架的磁标记空间姿态识别方法

摘要

本发明涉及一种基于常平架的磁标记空间姿态识别方法，利用磁性材料为标记物构建空间磁场，并通过常平架机构实现磁体标记物的磁偶极矩方向始终竖直向上；采用的常平架机构包括两个正交放置的常平环和一个机械陀螺仪，常平环的三个转动轴相互正交并交于同一点；磁体标记物的质心、陀螺仪的质心与常平环的三个转动轴的交点重合；通过双‑三轴磁检测传感器单元对待识别运动机构节点位置上的磁场分布情况进行测量；根据测量结果以及磁体标记物的磁偶极子正模型，建立磁检测传感器与磁体标记物空间相对位置矢量与空间磁场分布的封闭解析解模型；通过求解模型实现待识别运动机构空间姿态识别。实现了复杂机构空间运动姿态的快速、高精度、低成本识别。

主权项

1.一种基于常平架的磁标记空间姿态识别方法，其特征在于，具体包括如下步骤：1)搭建测试常平架：常平架机构包括两个正交放置的内、外常平环和一个机械陀螺仪，磁体标记物以及陀螺仪的质心位于全局坐标系XYZ原点，陀螺仪两端旋转轴通过陀螺仪与内常平环连接轴承对与内常平环连接，内常平环上两外旋转轴通过内常平环与外常平环连接轴承对与外常平环连接，外常平环上两外旋转轴通过外常平环与固定支架连接轴承对与最外的固定支架连接，陀螺仪、内常平环和外常平环上的两轴均以XYZ原点中心对称，即常平架上的三个转动轴相互正交并交于全局坐标系XYZ的原点，陀螺仪两端轴所成直线为Y轴，陀螺仪上有陀螺仪转子，陀螺仪转子的转动轴指向全局坐标系XYZ的Z轴正方向，常平架结构可分别绕X、Y、Z三轴旋转；2)磁体标记物质心与陀螺仪质心重合，利用磁性材料为标记物构建空间磁场，并由公式(1)所述磁偶极子正模型对磁体标记物建模：其中，B为观测点磁通密度分布；M＝mm(A·m2)为磁偶极矩，m(A·m)是磁偶极矩强度，m(m)是表征磁偶极矩方向的单位矢量；μ0是真空磁导率；r(m)是从磁偶极子指向观测位置的矢量，其模为r；3)该磁体标记物通过常平架机构实现其磁偶极矩方向始终竖直向上，建立全局坐标系XYZ，坐标系的坐标原点与磁体标记物的质心重合，Z轴的正方向与磁体标记物的磁偶极矩方向保持一致，即m＝(0，0，1)；4)常平架与待识别机构相对位置固定后，待识别机构上活动部件根据其刚性特点划分为若干个刚体，每个刚体由两个节点表示，同一刚体的两节点相对位置则表示该刚体的空间姿态；5)每一个节点上放置有一个双‑三轴磁检测传感器单元，通过双‑三轴磁检测传感器单元对待识别机构节点位置上的磁场分布情况进行测量，每个双‑三轴磁检测传感器单元包含两个相对位置固定的三轴磁检测传感器，每个三轴磁检测传感器均检测其位置上沿三轴磁检测传感器xyz三轴的磁通密度分布Bx，By，Bz，同一个双‑三轴磁检测传感器单元上的两个三轴磁检测传感器位置连线的中点与待识别机构节点的位置相同，双‑三轴磁检测传感器单元的个数与待识别机构节点的个数保持一致；6)基于三轴磁检测传感器测量到的由具有竖直向上磁偶极矩的磁体标记物激发的空间磁场分布以及磁体标记物磁偶极子正模型，建立三轴磁检测传感器与磁体标记物的相对空间位置矢量r(x，y，z)与三轴磁检测传感器测量到的其位置上空间磁场B的封闭解析解模型，如公式(2)所示：r(x，y，z)即为三轴磁检测传感器的空间位置矢量；7)基于步骤6)建立的封闭解析解模型以及三轴磁检测传感器检测的磁通密度分布，反解计算获得三轴磁检测传感器的坐标值r(x，y，z)，如公式(3a)、(3b)、(3c)所示：若B_y≠0，则其中，根据单个双‑三轴磁检测单元检测到的磁场分布，获得四个可能的传感器位置坐标，其中有两组为正确的位置坐标；8)根据公式(4)对步骤7)获得的四组三轴磁检测传感器位置坐标进行判定，|(rs1‑rs2)‑(r1‑r2)|＝0   (4)其中，rs1‑rs2是单个双‑三轴磁检测传感器单元上两个三轴磁检测传感器之间的相对位置矢量，这是固定并已知的；r1和r2分别是根据双‑三轴磁检测传感器单元上两个三轴磁检测传感器测量信息通过步骤7)计算所得对应双‑三轴磁检测传感器单元上的三轴磁检测传感器空间位置矢量，根据公式(3a)、(3b)、(3c)取得符号不同，每个三轴磁检测传感器位置坐标分别有两组，依次将四组可能的位置坐标带入公式(4)，满足公式(4)反解所得位置参数即为该双‑三轴磁检测传感器单元上两个三轴磁检测传感器的位置坐标，它们位置连线的中点即为待识别机构节点位置参数；9)根据待识别机构各节点位置坐标，实现待识别机构活动部件空间姿态识别。