CN112050802B - 基于单磁信标的三维空间高精度定位装置与方法 - Google Patents

Abstract

本发明提供了一种基于单磁信标的三维空间高精度定位装置与方法，属于定位定向方法技技术领域。本发明中磁信标在空间中产生待测磁场；总传感器模块为一个刚性整体，在总传感器模块中，一号、二号磁传感器模块和组合传感器模块相互之间刚性连接组成一个三角形结构整体且三个磁传感器的方向一致，上位机用于接收一号磁传感器模块传输的所在位置总磁场信息，进行数据处理，解算出二号磁传感器模块和组合传感器模块的可能位置，再根据位置坐标计算出理论场强，进而与实际测得场强作对比，获得被定位物体的三维位置信息。本发明安装简单，功耗低，可以任意旋转和移动，极大提高了实用性，定位精度高，可以达到在恶劣环境下自主定位定向的目的。

Description

基于单磁信标的三维空间高精度定位装置与方法

技术领域

本发明涉及一种基于单磁信标的三维空间高精度定位装置与方法，属于定位定向方法技技术领域。

背景技术

GPS定位技术是一种可以授时和测距的空间交会定点的导航系统，可向全球用户提供连续、实时、高精度的三维位置、速度与时间信息。用户需要手持接收器或者是在汽车上、轮船上、飞机上等安装接收器接受卫星信号，从卫星接收信号后从而来计算出位置。GPS定位如果想计算出位置，必须要在空旷范围且没有什么阻挡的地方，满足至少从3颗卫星中获取信号。但是其缺点在于GPS定位技术中需要终端内置卫星信号接收模块，定位精度受终端所处环境的影响较大；如果终端处于大型建筑物或者室内环境下，接收到的卫星信号太弱，定位精度将降低。

WiFi定位技术，主要应用于室内，公共场所等，通过无线保真技术对人或物体进行精细准确的定位。但是WiFi定位技术的缺点在于能耗较大，而且受服务范围限制，没有方向、速度等数据，不能导航。

ZigBee定位技术，主要用于距离短、功耗低且传输速率不高的各种电子设备之间，是一种近距离、低复杂度、低功耗、低速率、低成本的双向无线通讯技术。但Zigbee定位技术的缺点在于只能专网专用。数据率较低，不适用于传输速率高的应用。

蓝牙定位技术，基于RSSI原理，采用该原理和技术是可以满足室内短距离定位需求，精度比较高。但蓝牙定位技术的缺点在于复杂空间环境中，稳定性稍差，受噪声干扰大。

现有技术中，发明专利：一种基于双磁信标的定位定向方法(申请公布号为CN105928511 A)，介绍了以两个相互垂直且线圈直径、匝数与通电电流已知的两组螺线管制作两个磁信标，然后将两个磁信标安装在已知位置作为信号源，定位目标安装磁强计，通过磁强计的实时测量数据；设定两个磁信标的输出频率，两个磁信标在整个坐标系中的初始位置；从待定位目标所安装的磁力计上以大于磁信标最大频率的二倍以上的采样频率提取数据；对采集到的数据进行傅立叶分解，分别得到两个磁信标对应频率的两组信号，根据磁感应强度确定目标，确定目标所在位置的方向向量，根据两方向向量，确定目标所在位置，其缺点在于根据两方向向量可确定两条直线，但是双信标安装条件苛刻，占地大，对传感器姿态和所在平面有要求，并且只能二维定位。

发明内容

本发明的目的是为了解决上述现有技术存在的问题，进而提供一种基于单磁信标的三维空间高精度定位装置与方法。

本发明的目的是通过以下技术方案实现的：

一种基于单磁信标的三维空间高精度定位装置，包括：磁信标、总传感器模块和上位机；

磁信标在空间中产生待测磁场；

总传感器模块为一个刚性整体，包括：一号磁传感器模块、二号磁传感器模块和组合传感器模块；在总传感器模块中，一号磁传感器模块、二号磁传感器模块和组合传感器模块相互之间刚性连接组成一个三角形结构整体且磁传感器的方向一致，

所述组合传感模块包括：惯导模块和三号磁传感器模块；惯导模块由三轴加速度计和三轴陀螺仪组成，三轴加速度计和三轴陀螺仪与三号磁传感器模块的坐标轴轴向一致；

上位机用于接收一号磁传感器模块传输的所在位置总磁场信息，进行数据处理，解算出二号磁传感器模块和组合传感器模块的可能位置，再根据位置坐标计算出理论场强，进而与二号磁传感器模块和组合传感器模块实际测得场强作对比，获得被定位物体的三维位置信息。

基于单磁信标的三维空间高精度定位方法，通过制作磁信标和总传感器模块，然后建立信标坐标系和传感器坐标系，进行位置解算。

本发明的有益效果为：

本发明中低频磁场可以穿透各种类型的介质，从根本上解决了射频信号定位中的多径效应、光学定位中不可避免的遮挡效应等问题，实现在室内、地下、水下等特殊环境的自主定位；

本发明相比于双信标方法，单信标安装简单，功耗低，占地小；多传感器融合的方法使得传感器可以任意旋转和移动，而双信标的方法对磁传感器的姿态和位置均有要求；可以做到三维定位，而双信标只能做到二维定位。

本发明提出一种基于单磁信标的自主三维定位方法。低频交流磁场具有良好的性质，它的波长极大，这意味室内环境中的各种物体、建筑物墙体等都可以简化为瑞利散射体，对磁场的散射和衰减作用可以完全忽略不计，因此可从根本上解决射频信号定位中的多径效应、光学定位中不可避免的遮挡效应等问题；相比于基于地磁场的定位导航，具有不需更换磁图，抗干扰性强等优点；单信标安装简单，功耗低，多传感器融合的方法使得传感器可以任意旋转和移动，极大提高了本方法的实用性，定位精度高，可以达到在恶劣环境下自主定位定向的目的。

附图说明

图1为本发明基于单磁信标的三维空间高精度定位装置信标坐标系的结构示意图。

图中的附图标记，1为磁信标，2为总传感器模块。

具体实施方式

下面将结合附图对本发明做进一步的详细说明：本实施例在以本发明技术方案为前提下进行实施，给出了详细的实施方式，但本发明的保护范围不限于下述实施例。

实施例1

一种基于单磁信标的三维空间定位装置，本实施例需要一个磁信标1、一个固连的传感器模块和一个上位机软件。磁信标1在空间中产生待测磁场；总传感器模块2是一个刚性整体，包括三个部分，分别是两个单独的磁传感器模块、一个组合传感模块(包括一个三轴加速度计、一个三轴陀螺仪和一个磁传感器)，p′_k(k＝1,2,3)是传感器坐标系下，三个传感器的位置矢量：

p′₁＝[0 0 0]

p′₂＝[p′_2x p′_2y p′_2z]

p′₃＝[p′_3x p′_3y p′_3z]

其中，p′_2xp′_2yp′_2z为传感器坐标系下，原点指向p′₂的三个方向的矢量；p′_3x p′_3yp′_3z为传感器坐标系下，原点指向p′₃的三个方向的矢量；

为了保证唯一解，三个传感器模块需要满足一定的相对位置关系，如下式描述：

p′_2x≠0,p′_2y≠0,p′_2z≠0

p′_3x≠0,p′_3y≠0

组合传感器模块中只需使惯导模块(即加速度计、陀螺仪)与磁传感器模块的坐标轴轴向一致即可。

上位机首先获得1号磁传感器所在位置的总磁场信息，经过数据处理分离出所在位置处由信标产生的三轴磁场矢量的幅值，由于三轴未知方向的幅值有八种可能的方向组合，根据八种可能的组合计算出一号磁传感器所在的八个可能位置，根据传感器之间的相对位置关系与惯性传感器(惯导模块)测得的坐标转换矩阵，解算出其他两个磁传感器可能的八个位置，再根据位置坐标计算出这八个位置的理论场强，进而与另外两个磁传感器实际测得场强作对比，筛选出最接近的一组位置，最终获得被定位物体的三维位置信息。

一种基于单磁信标的三维空间定位方法，其具体步骤如下：

步骤1、制作磁信标1和总传感器模块2。

磁信标1采用面积S已知的正方形木质框架(不含铁质材料连接)，绕制以匝数N已知的铜线圈，然后将信标安装在三维空间中的已知位置作为信号源，并将电流大小I已知、频率为f的正弦信号加在磁信标1的线圈上。此时磁信标1产生的磁力矩

的大小为：

M＝S·N·I

总传感器模块2是一个刚性整体，在应用之前需做好封装并保证内部传感器坐标轴方向一致。该模块包括三个部分，分别是两个单独的磁传感器模块和一个组合传感模块(包括一个三轴加速度计、一个三轴陀螺仪和一个磁传感器)。为了保证唯一解，三个部分安装成一个三角形结构，如图1所示，图中的三个传感器的位置仅作为参考，并无确定位置，但三个传感器的相对位置需要满足一定关系，关系如下；

p′_k(k＝1,2,3)是传感器坐标系下，三个传感器的位置矢量：

p′₁＝[0 0 0]

p′₂＝[p′_2x p′_2y p′_2z]

p′₃＝[p′_3x p′_3y p′_3z]

为了保证唯一解，三个传感器模块需要满足一定的相对位置关系，如下式描述：

p′_2x≠0,p′_2y≠0,p′_2z≠0

p′_3x≠0,p′_3y≠0

组合传感器模块中只需使惯导模块(即加速度计、陀螺仪)与磁传感器模块的坐标轴轴向一致即可。

步骤2、建立信标坐标系和传感器坐标系。

在空间中选取固定的一点，将信标水平放置在该点，线圈中心作为信标坐标系的坐标原点，z轴与信标平面垂直，竖直指向正上方，x轴和y轴按照右手定则选取，建立信标坐标系，如图1所示。

在总传感器模块2中，由于三个部分相互之间刚性连接组成一个整体且磁传感器的方向一致，因此三个传感器部分的坐标系方向一致，互相之间是平移的关系。取磁传感器的三轴方向作为传感器模块的坐标系方向。

步骤3、位置解算。信标坐标系为固定坐标系，传感器坐标系为移动旋转坐标系，作为区分，向量在传感器坐标系下的位置加撇号表示。

信标坐标系下，信标在空间某一点p＝[p_x p_y p_z]产生的磁感应强度为：

其中

k为线圈相关系数。可以看出磁感应强度B和坐标p是一一对应的关系。

三个磁传感器编号为一、二、三，一号磁传感器测量到所在位置的三轴磁感应强度矢量，利用快速傅里叶变换和锁相放大器进行滤波选频以去除掉地磁场以及其他频率的磁场干扰，获得所在位置处由信标产生的三轴磁场矢量的幅值：M_i′＝[|B_ix′||B_iy′| |B_iz′|]，其中，|B_ix′| |B_iy′| |B_iz′|分别为被测位置处三轴磁感应强度的幅值；

上式可以看出有三个未定正负的磁感应强度，因此有八种可能的结果，根据这八个结果和磁感应强度计算公式f_B(p)可以得出一号磁传感器八个可能的三维位置坐标p_1,k(k＝1,...,8)；以其中一种可能位置p＝[p_x p_y p_z]为例，假设该点处磁感应强度为B_p＝[B_pxB_py B_pz]，该点磁感应强度与位置关系的方程为：

由于该方程没有解析解，因此需利用优化算法来进行最优解匹配，得到位置

为使

最小的时候

的取值，

和

均为位置坐标，

由于M_i′＝[|B_ix′| |B_iy′| |B_iz′|]表示数据采集和处理之后只能得到绝对值就是[|B_ix′| |B_iy′| |B_iz′|]，所以才有八种实际可能，但仅其中一种为真实值，经过分析，三个正负未定的数组成的八个可能解中，正负号是两两对称的，如下所示：

M′₁＝[+|B′_x| +|B′_y| +|B′_z|] M′₅＝[-|B′_x| -|B′_y| -|B′_z|]

M′₂＝[-|B′_x| +|B′_y| +|B′_z|] M′₆＝[+|B′_x| -|B′_y| -|B′_z|]

M′₃＝[+|B′_x| -|B′_y| +|B′_z|] M′₇＝[-|B′_x| +|B′_y| -|B′_z|]

M′₄＝[+|B′_x| +|B′_y| -|B′_z|]M′₈＝[-|B′_x| -|B′_y| +|B′_z|]

因此，为了减小计算量，只需要通过优化算法匹配求得前四个解，可以通过解析的方法算出后四个解，以其中一个点为例，假设第一种可能情况M′₁对应的位置为p₁＝[a bc]，符号与其对称的第五个情况M′₅对应的解为p₅＝[x y z]，则：

其中，

根据下式计算出对应的八组另外两个磁强计的位置，其中p′₂和p′₃分别是一号传感器距二号、三号磁传感器的矢量，如图所示，R是由惯导模块(陀螺仪、加速度计)得到的坐标转换矩阵(其中

θ，ψ分别是总传感器模块的滚转角、俯仰角、偏航角)：

p_2,k＝p_1,k+p′₂R^-1

p_3,k＝p_1,k+p′₃R^-1

进而求出上述八个位置在信标坐标系中的理论磁感应强度矢量f_B(p_2,k)和f_B(p_3,k)，经过坐标转换计算，得到上述八个位置在传感器坐标系中的理论磁感应强度矢量M′_2,k和M′_3,k(k＝1,…,8)：

M′_2,k＝f_B(p_2,k)·R

M′_3,k＝f_B(p_3,k)·R

M′是传感器坐标系下的矢量，M′_2,k,M′_3,k表示二号、三号磁传感器的三轴磁场矢量，f_B是信标坐标系下的矢量，f_B(p_2,k),f_B(p_3,k)表示信标坐标系下的二号、三号磁传感器的三轴磁场矢量，都为磁感应强度，R为坐标转换矩阵，R由惯导模块提供，

比较理论场强与实测场强的大小差值Δ_k＝||M′_2,k|-M′₂|+||M′_3,k|-M′₃|，找到使Δ_k最小的一组，对应的位置即为被定为物体的三维坐标p＝[p_x p_y p_z]，定位结束。

以上所述，仅为本发明较佳的具体实施方式，这些具体实施方式都是基于本发明整体构思下的不同实现方式，而且本发明的保护范围并不局限于此，任何熟悉本技术领域的技术人员在本发明揭露的技术范围内，可轻易想到的变化或替换，都应涵盖在本发明的保护范围之内。因此，本发明的保护范围应该以权利要求书的保护范围为准。

Claims (2)

1.一种基于单磁信标的三维空间高精度定位装置，其特征在于，包括：磁信标、总传感器模块和上位机；

磁信标在空间中产生待测磁场；

总传感器模块为一个刚性整体，包括：一号磁传感器模块、二号磁传感器模块和组合传感器模块；在总传感器模块中，一号磁传感器模块、二号磁传感器模块和组合传感器模块相互之间刚性连接组成一个三角形结构整体且磁传感器的方向一致，

所述组合传感器模块包括：惯导模块和三号磁传感器模块；惯导模块由三轴加速度计和三轴陀螺仪组成，三轴加速度计和三轴陀螺仪与三号磁传感器模块的坐标轴轴向一致；

p′_k,k＝1,2,3是传感器坐标系下，三个传感器模块的位置矢量：

p′₁＝[0 0 0]

p′₂＝[p′_2x p′_2y p′_2z]

p′₃＝[p′_3x p′_3y p′_3z]

为了保证唯一解，三个磁传感器模块需要满足一定的相对位置关系，如下式描述：

p′_2x≠0,p′_2y≠0,p′_2z≠0

p′_3x≠0,p′_3y≠0

上位机用于接收一号磁传感器模块传输的所在位置总磁场信息，进行数据处理，解算出二号磁传感器模块和组合传感器模块的可能位置，再根据位置坐标计算出理论场强，进而与二号磁传感器模块和组合传感器模块实际测得场强作对比，获得被定位物体的三维位置信息；

所述上位机首先获得一号磁传感器模块所在位置的总磁场信息，经过数据处理分离出所在位置处由信标产生的三轴磁场矢量的幅值，由于三轴未知方向的幅值有八种可能的方向组合，根据八种可能的组合计算出一号磁传感器模块所在的八个可能位置，根据传感器之间的相对位置关系与惯性传感器测得的坐标转换矩阵，惯性传感器即惯导模块，解算出二号磁传感器模块和组合传感器模块可能的八个位置，再根据位置坐标计算出这八个位置的理论场强，进而与二号磁传感器模块和组合传感器模块实际测得场强作对比，筛选出最接近的一组位置，最终获得被定位物体的三维位置信息。

2.根据权利要求1所述的基于单磁信标的三维空间高精度定位装置的定位方法，其特征在于，包括以下步骤：

步骤1、制作磁信标和总传感器模块

磁信标采用面积S已知的正方形木质框架，绕制以匝数N已知的铜线圈，然后将信标安装在三维空间中的已知位置作为信号源，并将电流大小I已知、频率为f的正弦信号加在磁信标的线圈上，此时磁信标产生的磁力矩

的大小为：

M＝S·N·I

总传感器模块为一个刚性整体，在应用之前，进行封装并保证内部传感器坐标轴方向一致，该模块包括三个部分，分别是两个单独的磁传感器模块和一个组合传感模块，组合传感模块包括一个三轴加速度计、一个三轴陀螺仪和一个磁传感器，为了保证唯一解，三个部分安装成一个三角形结构，并满足一定的相对位置关系；

步骤2、建立信标坐标系和传感器坐标系

在空间中选取固定的一点，将磁信标水平放置在该点，线圈中心作为磁信标坐标系的坐标原点，z轴与磁信标平面垂直，竖直指向正上方，x轴和y轴按照右手定则选取，建立磁信标坐标系，

在总传感器模块中，由于三个部分相互之间刚性连接组成一个整体且磁传感器的方向一致，因此三个传感器部分的坐标系方向一致，互相之间是平移的关系，取磁传感器的三轴方向作为传感器模块的坐标系方向；

步骤3、位置解算

磁信标坐标系为固定坐标系，传感器坐标系为移动旋转坐标系，作为区分，向量在传感器坐标系下的位置加撇号表示；

信标坐标系下，信标在空间某一点p＝[p_x p_y p_z]产生的磁感应强度为：

其中

l为线圈相关系数，看出磁感应强度B和坐标p是一一对应的关系；

三个磁传感器编号为一、二、三，一号磁传感器测量到所在位置的三轴磁感应强度矢量，利用快速傅里叶变换和锁相放大器进行滤波选频以去除掉地磁场以及其他频率的磁场干扰，获得所在位置处由信标产生的三轴磁场矢量的幅值：M_i′＝[|B_ix′| |B_iy′| |B_iz′|]其中，|B_ix′| |B_iy′| |B_iz′|分别为被测位置处三轴磁感应强度的幅值；

上式看出有三个未定正负的磁感应强度，因此有八种可能的结果，根据这八个结果和磁感应强度计算公式f_B(p)得出一号磁传感器八个可能的三维位置坐标p_1,k,k＝1,...,8；以其中一种可能位置p＝[p_x p_y p_z]为例，假设该点处磁感应强度为B_p＝[B_px B_py B_pz]，该点磁感应强度与位置关系的方程为：

由于该方程没有解析解，因此需利用优化算法来进行最优解匹配，得到位置

为使

最小的时候

的取值，

和

均为位置坐标，

经过分析，三个正负未定的数组成的八个可能解中，正负号是两两对称的，如下所示：

M′₁＝[+|B′_x|+|B′_y|+|B′_z|] M′₅＝[-|B′_x|-|B′_y|-|B′_z|]

M′₂＝[-|B′_x|+|B′_y|+|B′_z|] M′₆＝[+|B′_x|-|B′_y|-|B′_z|]

M′₃＝[+|B′_x|-|B′_y|+|B′_z|] M′₇＝[-|B′_x|+|B′_y|-|B′_z|]

M′₄＝[+|B′_x|+|B′_y|-|B′_z|] M′₈＝[-|B′_x|-|B′_y|+|B′_z|]

因此，为了减小计算量，只需要通过优化算法匹配求得前四个解，通过解析的方法算出后四个解，以其中一个点为例，假设第一种可能情况M′₁对应的位置为p₁＝[a b c]，符号与其对称的第五个情况M′₅对应的解为p₅＝[x y z]，则：

其中，

根据下式计算出对应的八组另外两个磁强计的位置，其中p′₂和p′₃分别是一号传感器距二号、三号磁传感器的矢量，R是由惯导模块得到的坐标转换矩阵，其中，

θ，ψ分别是总传感器模块的滚转角、俯仰角、偏航角；

p_2,k＝p_1,k+p′₂R^-1

p_3,k＝p_1,k+p′₃R^-1

进而求出上述八个位置在信标坐标系中的理论磁感应强度矢量f_B(p_2,k)和f_B(p_3,k)，经过坐标转换计算，得到上述八个位置在传感器坐标系中的理论磁感应强度矢量M′_2,k和M′_3,k，k＝1,…,8：

M′_2,k＝f_B(p_2,k)·R

M′_3,k＝f_B(p_3,k)·R

M′是传感器坐标系下的矢量，M′_2,k,M′_3,k表示传感器坐标系下二号、三号磁传感器的三轴磁场矢量，f_B是信标坐标系下的矢量，f_B(p_2,k),f_B(p_3,k)表示信标坐标系下的二号、三号磁传感器的三轴磁场矢量，R为坐标转换矩阵，由惯导模块提供；

比较理论场强与实测场强的大小差值Δ_k＝||M′_2,k|-M′₂|+||M′_3,k|-M′₃|，找到使Δ_k最小的一组，对应的位置即为被定为物体的三维坐标p＝[p_x p_y p_z]，定位结束。

Images