技术领域
[0001] 本
发明涉及一种
位置识别技术，特别涉及一种基于常平架的磁标记空间姿态识别方法。
背景技术
[0002] 在运动机构空间位置姿态识别技术中，现有研究包括利用图像数据、光
信号、
加速度信号等。其中，基于图像数据和
光信号的识别技术由于受光线、环境背景、遮挡等影响，应用范围有限；而基于加速度信号的识别技术由于需要进行积分运算，测量结果容易出现较大的累计偏差。基于磁标记的
定位技术，由于具有非
接触、物理模型明确、不依赖于介质传播等优点，而被广泛采用。然而，在现有的基于磁标记的定位技术中，标记磁源的空间位置姿态参数与磁源所激发的空间
磁场分布之间为非线性关系，磁场逆问题求解也是非线性的。因此在直
角坐标系下，为了获得标记磁源的空间位置姿态共六个参数，需要利用非线性优化
算法，获得待求解参数的可行解。这样的求解结果受采用的求解算法以及初始值的设定等影响严重，并且求解过程多需要经过
迭代优化，求解时间较长。现有研究从
硬件和
软件方面着手寻求解决方法。硬件方面多采用增加
传感器数量，来获得更多的观测样本数据，但这样会导致成本、运算量的增加；软件方面则通过改进非线性求解模型，例如通过矩阵和线代运算对非线性反解模型进行简化，或者通过传感器分布获得线性求解模型等。如何实现基于结构简单
传感器系统的高
精度、快速定位，是基于磁信息的运动机构空间位置姿态识别研究领域的难点。
发明内容
[0003] 本发明是针对基于磁标记的定位技术存在的问题，提出了一种基于常平架的磁标记空间姿态识别方法，实现高精度、快速定位。
[0004] 本发明的技术方案为：一种基于常平架的磁标记空间姿态识别方法，具体包括如下步骤：
[0005] 1)搭建测试常平架：常平架机构包括两个
正交放置的内、外常平环和一个机械
陀螺仪，磁体标记物以及陀螺仪的质心位于全局坐标系XYZ原点，陀螺仪两端旋
转轴通过陀螺仪与内常平环连接
轴承对与内常平环连接，内常平环上两外
旋转轴通过内常平环与外常平环连接轴承对与外常平环连接，外常平环上两外旋转轴通过外常平环与固定
支架连接轴承对与最外的固定支架连接，陀螺仪、内常平环和外常平环上的两轴均以XYZ原点中心对称，即常平架上的三个转动轴相互正交并交于全局坐标系XYZ的原点，陀螺仪两端轴所成直线为Y轴，陀螺仪上有陀螺仪
转子，陀螺仪转子的转动轴指向全局坐标系XYZ的Z轴正方向，常平架结构可分别绕X、Y、Z三轴旋转；
[0006] 2)磁体标记物质心与陀螺仪质心重合，利用
磁性材料为标记物构建空间磁场，并由公式(1)所述磁偶极子正模型对磁体标记物建模：
[0007]
[0008] 其中，B为观测点磁通
密度分布；M＝mm(A·m2)为磁偶极矩，m(A·m)是磁偶极矩强度，m(m)是表征磁偶极矩方向的单位矢量；μ0是
真空磁导率；r(m)是从磁偶极子指向观测位置的矢量，其模为r；
[0009] 3)该磁体标记物通过常平架机构实现其磁偶极矩方向始终竖直向上，建立全局坐标系XYZ，坐标系的坐标原点与磁体标记物的质心重合，Z轴的正方向与磁体标记物的磁偶极矩方向保持一致，即m＝(0，0，1)；
[0010] 4)常平架与待识别机构相对位置固定后，待识别机构上活动部件根据其刚性特点划分为若干个刚体，每个刚体由两个
节点表示，同一刚体的两节点相对位置则表示该刚体的空间姿态；
[0011] 5)每一个节点上放置有一个双-三轴磁检测传感器单元，通过双-三轴磁检测传感器单元对待识别机构节点位置上的磁场分布情况进行测量，每个双-三轴磁检测传感器单元包含两个相对位置固定的三轴磁检测传感器，每个三轴磁检测传感器均检测其位置上沿三轴磁检测传感器xyz三轴的磁通密度分布Bx，By，Bz，同一个双-三轴磁检测传感器单元上的两个三轴磁检测传感器位置连线的中点与待识别机构节点的位置相同，双-三轴磁检测传感器单元的个数与待识别机构节点的个数保持一致；
[0012] 6)基于三轴磁检测传感器测量到的由具有竖直向上磁偶极矩的磁体标记物激发的空间磁场分布以及磁体标记物磁偶极子正模型，建立三轴磁检测传感器与磁体标记物的相对空间位置矢量r(x，y，z)与三轴磁检测传感器测量到的其位置上空间磁场B的封闭解析解模型，如公式(2)所示：
[0013]
[0014] r(x，y，z)即为三轴磁检测传感器的空间位置矢量；
[0015] 7)基于步骤6)建立的封闭解析解模型以及三轴磁检测传感器检测的磁通密度分布，反解计算获得三轴磁检测传感器的坐标值r(x，y，z)，如公式(3a)、(3b)、(3c)所示：
[0016] 若By≠0，则
[0017]
[0018]
[0019] 其中，
[0020] 根据单个双-三轴磁检测单元检测到的磁场分布，获得四个可能的传感器位置坐标，其中有两组为正确的位置坐标；
[0021] 8)根据公式(4)对步骤7)获得的四组三轴磁检测传感器位置坐标进行判定，[0022] |(rs1-rs2)-(r1-r2)|＝0 (4)
[0023] 其中，rs1-rs2是单个双-三轴磁检测传感器单元上两个三轴磁检测传感器之间的相对位置矢量，这是固定并已知的；r1和r2分别是根据双-三轴磁检测传感器单元上两个三轴磁检测传感器测量信息通过步骤7)计算所得对应双-三轴磁检测传感器单元上的三轴磁检测传感器空间位置矢量，根据公式(3a)、(3b)、(3c)取得符号不同，每个三轴磁检测传感器位置坐标分别有两组，依次将四组可能的位置坐标带入公式(4)，满足公式(4)反解所得位置参数即为该双-三轴磁检测传感器单元上两个三轴磁检测传感器的位置坐标，它们位置连线的中点即为待识别机构节点位置参数；
[0024] 9)根据待识别机构各节点位置坐标，实现待识别机构活动部件空间姿态识别。
[0025] 本发明的有益效果在于：本发明基于常平架的磁标记空间姿态识别方法，利用具有常平机构的标记磁体以及双-三轴磁检测传感器单元，实现了复杂机构空间运动姿态的快速、高精度、低成本识别。
附图说明
[0026] 图1为本发明方法中常平架的机构示意图；
[0027] 图2为本发明磁标记空间姿态识别方法的
流程图。
具体实施方式
[0028] 如图1所示基于常平架的磁标记空间姿态识别方法中常平架的机构示意图，常平架机构固定于固定支架1上，常平架机构包括两个正交放置的内、外常平环5、6和一个机械陀螺仪3，磁体标记物4以及陀螺仪3的质心位于全局坐标系XYZ原点，陀螺仪3两端旋转轴通过陀螺仪与内常平环连接轴承对8与内常平环5连接，内常平环5上两外旋转轴通过内常平环与外常平环连接轴承对7与外常平环6连接，外常平环6上两外旋转轴通过外常平环与固定支架连接轴承对9与最外的固定支架1连接，陀螺仪3、内常平环5和外常平环6上的两轴均以XYZ原点中心对称，即常平架上的三个转动轴相互正交并交于全局坐标系XYZ的原点，陀螺仪3两端轴所成直线为Y轴，陀螺仪3上有陀螺仪转子2，陀螺仪转子2的转动轴指向全局坐标系XYZ的Z轴正方向，常平架结构可分别绕X、Y、Z三轴旋转。
[0029] 通过内常平环5、外常平环6以及连接轴承对7、8、9维持磁体标记物4磁偶极矩方向恒定，其中3对连接轴承的轴线相互正交，并相交于全局坐标系XYZ原点，陀螺仪3的质心位于全局坐标系原点，陀螺仪转子2的转动轴指向全局坐标系XYZ的Z轴正方向，当外界出现干扰时，利用陀螺仪3具有的抗拒方向改变的特性，实现磁体标记物磁偶极矩方向维持稳定。
[0030] 基于常平架的磁标记空间姿态识别方法，利用磁性材料为标记物(包括
永磁体、电磁线圈等)构建空间磁场，并通过常平架机构实现磁体标记物的磁偶极矩方向始终竖直向上；采用的常平架机构包括两个正交放置的常平环和一个机械陀螺仪，常平环的三个转动轴相互正交并交于同一点；磁体标记物的质心、陀螺仪的质心与常平环的三个转动轴的交点重合；通过双-三轴磁检测传感器单元对待识别运动机构节点位置上的磁场分布情况进行测量；基于磁检测传感器单元的测量结果以及磁体标记物的磁偶极子正模型，建立磁检测传感器与磁体标记物空间相对位置矢量与空间磁场分布的封闭解析解模型；通过求解封闭解析解模型，获得磁检测传感器相对于磁体标记物的位置坐标，即获得待识别运动机构节点的位置坐标，从而实现待识别运动机构空间姿态识别。
[0031] 如图2所示基于常平架的磁标记空间姿态识别方法流程图，主要包括以下步骤：
[0032] 步骤一，利用磁性材料为标记物构建空间磁场，并由公式(1)所述磁偶极子正模型对磁体标记物建模：
[0033]
[0034] 其中，B为观测点磁通密度分布；M＝mm(A·m2)为磁偶极矩，m(A·m)是磁偶极矩强度，m(m)是表征磁偶极矩方向的单位矢量；μ0是真空磁导率(＝4π×10-7N/A2)；r(m)是从磁偶极子指向观测位置的矢量，其模为r。
[0035] 步骤二，该磁体标记物通过常平架机构实现其磁偶极矩方向始终竖直向上，建立全局坐标系XYZ，坐标系的坐标原点与磁体标记物的质心重合，Z轴的正方向与磁体标记物的磁偶极矩方向保持一致，即m＝(0，0，1)；
[0036] 步骤三，磁体标记物的质心与常平架的三个转动轴交点重合；
[0037] 步骤四，常平架与待识别机构相对位置固定后，待识别机构上活动部件可以根据其刚性特点划分为若干个刚体，每个刚体由两个节点表示，同一刚体的两节点相对位置则表示该部分刚体的空间姿态；
[0038] 例如需要识别人手部
手指的位置姿态，将常平架放置在掌根，将每根手指根据解剖学特征划分成多段，每一段的始末位置设置两个节点放置传感器单元，手指运动，那么手指节点传感器单元接收到的磁场的分布就会随之发生改变，通过检测观察点的磁场强度，结合模型，实现传感器位置相对磁体标记物位置矢量的求解。
[0039] 步骤五，每一个节点上放置有一个双-三轴磁检测传感器单元，通过双-三轴磁检测传感器单元对待识别运动机构节点位置上的磁场分布情况进行测量，每个双-三轴磁检测传感器单元包含两个相对位置固定的三轴磁检测传感器，每个三轴磁检测传感器均可以检测其位置上沿三轴磁检测传感器xyz三轴的磁通密度分布Bx，By，Bz，同一个双-三轴磁检测传感器单元上的两个三轴磁检测传感器位置连线的中点与待识别运动机构节点的位置相同，双-三轴磁检测传感器单元的个数与待识别机构节点的个数保持一致；
[0040] 步骤六，基于三轴磁检测传感器测量到的由具有竖直向上磁偶极矩的磁体标记物激发的空间磁场分布以及磁体标记物磁偶极子正模型，建立三轴磁检测传感器与磁体标记物的相对空间位置矢量r(x，y，z)与三轴磁检测传感器测量到的其位置上空间磁场B的封闭解析解模型，如公式(2)所示：
[0041]
[0042] 由于建立的全局坐标系的原点与磁体标记物的质心重合，因此r(x，y，z)即为三轴磁检测传感器的空间位置矢量；
[0043] 步骤七，基于步骤六建立的封闭解析解模型以及三轴磁检测传感器检测的磁通密度分布，反解计算获得三轴磁检测传感器的坐标值r(x，y，z)，如公式(3a)、(3b)、(3c)所示：
[0044] 若By≠0，则
[0045]
[0046]
[0047] 其中，
[0048] 根据单个双-三轴磁检测单元检测到的磁场分布，可以获得四个可能的传感器位置坐标，其中有两组为正确的位置坐标。
[0049] 步骤八，根据公式(4)对步骤七获得的四组磁检测传感器位置坐标进行判定，[0050] |(rs1-rs2)-(r1-r2)|＝0 (4)
[0051] 其中，rs1-rs2是单个双-三轴磁检测传感器单元上两个三轴磁检测传感器之间的相对位置矢量，这是固定并已知的；r1和r2分别是根据双-三轴磁检测传感器单元上两个三轴磁检测传感器测量信息通过步骤七计算所得对应双-三轴磁检测传感器单元上的三轴磁检测传感器空间位置矢量，根据公式(3a)、(3b)、(3c)取得符号不同，每个三轴磁检测传感器位置坐标分别有两组。依次将四组可能的位置坐标带入公式(4)，满足公式(4)反解所得位置参数即为该双-三轴磁检测传感器单元上两个三轴磁检测传感器的位置坐标，它们位置连线的中点即为待识别机构节点位置参数；
[0052] 步骤九，根据待识别机构各节点位置坐标，实现待识别机构活动部件空间姿态识别。
[0053] 图2是本发明的
实施例中磁标记空间姿态识别方法的流程图。磁标记空间姿态识别方法的简明流程如下：
[0054] 步骤S1：磁性材料为标记物构建空间磁场，用磁偶极子正模型对磁体标记物建模，然后进入步骤S2。
[0055] 步骤S2：建立全局坐标系XYZ，通过常平架机构实现磁体标记物磁偶极矩方向始终竖直向上，坐标系的坐标原点与磁体标记物的质心重合，Z轴的正方向与磁体标记物的磁偶极矩方向保持一致，即m＝(0，0，1)，然后进入步骤S3。
[0056] 步骤S3：磁体标记物的质心与常平架的三个转动轴交点以及坐标系原点重合，然后进入步骤S4。
[0057] 步骤S4：待识别机构可以根据其刚性特点划分为若干个刚体，每个刚体由两个节点表示，同一刚体的节点相对位置则表示该部分刚体的空间姿态，然后进入步骤S5。
[0058] 步骤S5：通过双-三轴磁检测传感器单元对待识别运动机构节点位置上的磁场分布情况进行测量，同一个磁检测传感器单元上的两个传感器位置连线的中点与待识别运动机构节点的位置相同，然后进入步骤S6。
[0059] 步骤S6：建立磁检测传感器与磁体标记物的相对空间位置矢量r(x，y，z)与磁检测传感器测量到的其位置上空间磁场B的封闭解析解模型，然后进入步骤S7。
[0060] 步骤S7：基于步骤六建立的封闭解析解模型以及磁检测传感器检测的磁通密度分布，反解计算获得磁检测传感器的坐标值r(x，y，z)，然后进入步骤S8。
[0061] 步骤S8：对反解所得传感器空间位置坐标符号判定，确定待识别运动机构节点位置参数，然后进入步骤S9。
[0062] 步骤S9：根据运动机构各节点位置坐标，实现待识别运动机构空间姿态识别。
