CN113124852A - 一种多传感器融合定位系统及定位方法 - Google Patents

Abstract

本发明公开了一种多传感器融合定位系统及定位方法。该定位系统包括惯性定位子系统、电磁定位子系统和超声波定位子系统；其中，根据超声波定位与电磁定位的定位结果，进行距离数据的融合；根据惯性导航定位与电磁定位的定位结果，进行姿态数据的融合；根据融合的距离数据和融合的姿态数据，计算获得空间位置数据。与现有技术相比较，本发明中的融合修正过程是实时进行的，不需要采集离线数据。另一方面，本发明由于实时进行融合修正，对于静态及动态磁场畸变都有明显的改善效果。

Description

一种多传感器融合定位系统及定位方法

技术领域

本发明涉及一种多传感器融合定位系统，同时也涉及相应的多传感器融合定位方法，属于无线定位技术领域。

背景技术

目前，获得精准的位置服务在人们生活中变得越来越重要。现有的室外定位技术主要通过GPS、北斗等卫星定位系统实现，已经能够很好地满足室外定位的需求。但是，室内环境下的定位一直是一个很多问题未被解决的领域。由于信号的严重衰减和多径效应，通用的室外定位技术并不能在建筑物内有效地工作，难以充分满足用户的需求。

现有的室内定位技术包括电磁定位、WiFi定位、超声定位和惯性导航定位等。这些定位技术都具有各自的优缺点，例如电磁定位技术的优点在于不受视线遮挡影响，但是它的缺点是容易受金属干扰影响，金属会使电磁场分布发生形变，影响定位精度；超声定位技术不受金属干扰影响，但是容易受环境多径反射及视线遮挡影响；惯性导航定位技术不受外界的视线遮挡/金属干扰等环境影响，但是自身存在零点偏移，而且不能进行绝对的定位。

发明内容

针对现有技术中的不足，本发明所要解决的首要技术问题在于提供一种多传感器融合定位系统。

本发明所要解决的另一技术问题在于提供一种多传感器融合定位方法。

为实现上述发明目的，本发明采用下述的技术方案：

根据本发明实施例的第一方面，提供一种多传感器融合定位系统，包括惯性定位子系统、电磁定位子系统和超声波定位子系统；

根据所述电磁定位子系统和所述超声波定位子系统的定位结果，进行距离数据的融合；

根据所述惯性定位子系统和所述电磁定位子系统的定位结果，进行姿态数据的融合；

根据所述融合的距离数据和所述融合的姿态数据，得到空间位置数据。

其中较优地，所述电磁定位子系统，包括电磁发射单元和电磁接收单元，以电磁发射单元为坐标原点，由电磁接收单元检测磁感应强度信号，得到目标的电磁定位结果，所述电磁定位结果包括三维空间坐标和姿态数据。

根据本发明实施例的第二方面，提供一种多传感器融合定位方法，包括如下步骤：

根据超声波定位与电磁定位的定位结果，进行距离数据的融合；

根据惯性导航定位与电磁定位的定位结果，进行姿态数据的融合；

根据所述融合的距离数据和所述融合的姿态数据，计算获得空间位置数据。

其中较优地，进行所述距离数据的融合包括如下步骤：

第一步：接收端的超声波接收单元检测收端的超声波发射单元产生的超声波信号，计算出发射端和接收端之间的第一距离R_ultra；

第二步：根据所述电磁定位的定位结果，得到所述发射端和所述接收端之间的第二距离R_em；

第三步：对所述第一距离R_ultra和所述第二距离R_em做融合，得到融合后的距离数据；

第四步：根据所述融合后的距离数据，标定电磁定位的磁场总强度。

其中较优地，利用下述公式对进行距离数据的融合：

R＝α₁ R_ultra+(1-α₁)R_em

其中，α₁是融合系数，取值范围在0～1之间，根据超声定位的置信度和电磁定位的置信度进行动态调整。

其中较优地，根据所述电磁定位的定位结果，获得接收端的三维空间坐标；

通过所述三维空间坐标判断接收端与发射端的超声发射单元的FOV的位置关系；

如果接收端位于发射端的超声发射单元的FOV的边缘区域，则调低超声定位的置信度，并调低α₁。

其中较优地，根据所述电磁定位的定位结果，获得接收端的三维空间坐标；

通过所述三维空间坐标判断接收端与发射端的电磁发射单元的位置关系；

如果接收端位于发射端的电磁发射单元的零平面时，则调低电磁定位的置信度，并调高α₁。

其中较优地，所述姿态数据的融合包括如下步骤：

第一步：对所述惯性导航定位的定位结果进行解算，得出接收单元的第一姿态数据H_imu；

第二步：对所述电磁定位的定位结果进行解算，得到接收单元的第二姿态数据H_em；

第三步：将所述第一姿态数据和所述第二姿态数据进行融合，得到接收端的姿态数据。

其中较优地，将所述距离数据的融合结果作为第一约束条件，将所述姿态数据的融合结果作为第二约束条件，通过优化问题建模得到所述空间位置数据。

其中较优地，所述距离数据的融合以及所述姿态数据的融合在定位过程中实时进行。

与现有技术相比较，本发明中的融合修正过程是实时进行的，不需要采集离线数据。这一点显著不同于需要采集数据进行校准的现有大多数离线校准方法。另一方面，现有大多数校准方法只能滤除环境中固定的金属干扰，本发明由于实时进行融合修正，对于静态及动态磁场畸变都有明显的改善效果。

附图说明

图1为本发明所提供的多传感器融合定位系统中，发射端的结构示意图；

图2为本发明所提供的多传感器融合定位系统中，接收端的结构示意图；

图3为本发明所提供的多传感器融合定位方法的流程简图；

图4为本发明所提供的多传感器融合定位方法的流程详图。

具体实施方式

下面结合附图和具体实施例对本发明的技术方案展开详细具体的说明。

考虑到现有的室内定位技术，例如电磁定位、超声定位和惯性导航定位等都具有各自的优缺点，本发明实施例提供一种具有较强抗干扰能力的多传感器融合定位系统。下面对此展开具体说明。

本发明实施例所提供的多传感器融合定位系统包括惯性定位子系统、电磁定位子系统和超声波定位子系统。这些定位子系统分为发射端和接收端两部分，用于获得稳定的6自由度(简写为6dof)位姿数据。其中，图1所示的发射端部分包括惯性导航单元(IMU)、电磁发射单元和超声发射单元，图2所示的接收端部分包括相应的电磁接收单元、超声接收单元和惯性导航单元(IMU)。可以理解的，图1和图2只是一种实例，也可以这样来设置发射端和接收端，即发射端部分包括电磁发射单元和超声发射单元，接收端包括惯性导航单元(IMU)、电磁接收单元和超声接收单元；或者，发射端部分包括电磁发射单元和超声接收单元，接收端包括惯性导航单元(IMU)、电磁接收单元和超声发射单元。这些室内定位单元依据不同的工作原理分别独立开展室内定位工作，所获得的室内定位数据相互进行融合修正，以消除单一定位数据可能带来的误差和干扰。示例性地，图1所示的发射端为头戴式设备，图2所示的接收端为目标设备，例如手持式设备。

在本发明的实施例中，超声发射单元可以是超声波扬声器，超声接收单元可以是超声波接收换能器。超声波扬声器发射超声波信号，超声波接收换能器通过对超声波信号的接收和计算，获得发射端和接收端之间的相对距离，作为观测量，对其它室内定位手段的定位结果进行校正，超声波定位技术成本较低且具有厘米级的精度。

在本发明的实施例中，组成惯性定位子系统的惯性导航单元(IMU)主要包括加速度传感器和陀螺仪，用于产生加速度和角速度的观测数据。IMU的测量几乎不受外界环境条件影响，测量频率高，测量位姿短时间内相对变化的结果准确，但其最大问题是不能获得绝对姿态，以及测量结果存在零点偏移，长时间积分会产生漂移。

在本发明的实施例中，电磁发射单元主要是通过发射线圈的电磁信号，以发射线圈为坐标原点，在空间建立规则变化的磁场。电磁接收单元通过感应线圈检测磁感应强度信号，解算定位目标的6自由度位姿数据。具体的定位过程说明如下：

首先在发射端，由信号发生电路产生三组正弦形式的驱动信号，给三个线圈分别供电，产生一个规则变化的电磁场，置于磁场中的接收端线圈因磁感应强度的变化，产生感应电动势(接收信号)。接收端的3个线圈分别感应发射端3个线圈产生的磁场，得到9个参数。根据这9个参数完成6dof的定位解算。具体步骤如下：

步骤1：解算磁感应强度B矩阵。根据从接收端获取到的信号，构成磁感应强度B矩阵。

步骤2：根据磁感应强度B矩阵，解算三维空间坐标x、y、z。

步骤3：根据三维空间坐标x、y、z，解算磁感应强度B’矩阵。

步骤4：根据B和B’矩阵，解算姿态。

假设接收端相对发射端的姿态矩阵为H，则：

HB’＝B

在实践中，上述电磁定位技术容易受到磁场中金属物体的干扰，严重影响定位精度。当接收端附近有金属物体时，金属物体中被发射端的电磁场感应出涡流，涡流产生二次磁场，接收端收到的磁场是发射端产生的磁场和金属物体中涡流产生的二次磁场的叠加，从而导致磁场畸变。如果靠近金属物体，由涡流产生的二次磁场的强度将会迅速增加，导致叠加磁场严重偏离发射端产生的磁场。

针对电磁定位技术所面临的上述问题，本发明实施例采用多传感器融合定位的方法，利用超声波定位子系统进行距离维度的修正，利用惯性定位子系统进行姿态维度的修正，从而解决电磁定位中的金属干扰/磁场(B)畸变问题，改善有金属干扰等造成接收端磁场变化的情况下的定位性能。

下面，结合图3和图4对本发明所提供的多传感器融合定位方法的具体步骤展开说明。

在本发明的一个实施例中，如图3所示，一方面通过超声波定位与电磁定位的结合实现距离数据的融合修正，另一方面通过惯性导航定位与电磁定位的结合实现姿态数据的融合修正，然后根据融合后的距离数据和姿态数据，通过优化算法进行位置解算，获得经过融合修正后的空间位置数据，作为室内定位结果对外输出。其中，上述距离数据、姿态数据和位置数据的融合修正均是在定位过程中实时进行的。

下面结合图4进行详细描述，一方面通过超声波定位与电磁定位的结合实现距离数据的融合修正，该过程主要有以下步骤：

S1：超声波接收单元检测超声波发射单元产生的超声波信号，计算出发射端和接收端之间的第一距离R_ultra；

发射端(例如头戴式设备)的超声波发射单元发射超声波，发射的超声波例如为40kHz，发射频率为60Hz，接收端(例如手持式设备)的超声波接收单元检测到发射端发射的超声波信号，根据超声波的传播速度以及发射时间和接收时间的差值，计算出发射单元和接收单元之间的第一距离R_ultra。具体地，在发射端和接收端还包括时间同步装置，以使发射端和接收端处于同一时间基准下。超声波的传播速度进一步地可以根据温度、湿度等参数进行动态调整。

S2：根据电磁定位结果，独立得到发射端和接收端之间的第二距离R_em；

发射端的电磁发射单元包括三个发射线圈，由信号发生电路产生三组正弦形式的驱动信号，给三个线圈分别供电，产生一个规则变化的电磁场，接收端的电磁接收单元的3个线圈分别感应发射端3个线圈产生的磁场，得到9个参数，构成磁感应强度B矩阵，根据磁感应强度B矩阵进行解算可以得到接收端相对于发射端的三维空间坐标x、y、z，根据三维空间坐标x、y、z进一步可得到发射端和接收端之间的第二距离R_em。

S3：根据下式将超声定位和电磁定位得到的两个距离数据做融合：

R＝α₁ R_ultra+(1-α₁)R_em

其中，α₁是融合系数，取值范围在0～1之间。融合系数为动态系数，根据超声信号和电磁信号的置信度进行调整。

具体地，通过第二步的电磁定位结果可以获得接收端的三维空间坐标x、y、z，通过该三维空间坐标x、y、z可以判断接收端与发射端的超声发射单元的FOV(Field of View，视场角)的位置关系。如果接收端位于发射端的超声发射单元的FOV的边缘区域，则调低超声定位的R_ultra置信度。详细来说，超声发射单元的FOV通常在90°～120°，当判断接收端在发射端的超声发射单元的FOV边缘区域，例如大于80°时则调低超声定位的R_ultra置信度；或者，根据接收端位于发射端的超声发射单元的FOV的角度范围建立一个关联关系(例如线性关系)，当接收端位于发射端的超声发射单元的FOV的的中心区域时，超声定位的R_ultra置信度最高，当接收端距离发射端的超声发射单元的FOV的的中心区域越远时，超声定位的R_ultra置信度越低，当超出发射端的超声发射单元的FOV时，则超声信号的置信度为0，此时α₁也为0。

进一步地，通过该三维空间坐标x、y、z可以判断接收端与发射端的电磁发射单元的位置关系。如果接收端位于发射端的电磁发射单元的零平面时，则调低电磁定位的R_em置信度。详细来说，根据接收端位于发射端的电磁发射单元的零平面范围建立一个关联关系(例如线性关系)，当接收端位于发射端的电磁发射单元的零平面，电磁定位的R_em置信度最低，电磁信号的置信度可以为0，此时α₁也可以为0；当接收端距离发射端的电磁发射单元的零平面越远时，电磁定位的R_em置信度越高。

S4：根据融合后的距离，标定电磁定位的磁场总强度B²

式中，B为磁场强度；R为发射端到接收端的距离。

另一方面，通过惯性导航定位与电磁定位的结合实现姿态数据的融合修正。该过程主要有以下步骤：

S1：惯性导航单元(IMU)进行解算，得出接收端的第一姿态数据H_imu；

惯性导航单元(IMU)至少包括加速度传感器和陀螺仪，用于产生加速度和角速度的数据，根据加速度和角速度的数据可以得到接收端的三个姿态角(偏航角、翻滚角、俯仰角)。在其他实施例中惯性导航单元(IMU)还可以包括三轴地磁计，可以减少陀螺仪的惯性漂移误差，获得更准确的接收端的第一姿态数据H_imu。

S2：电磁定位系统进行解算，得到接收端的第二姿态数据H_em；

发射端包括三个发射线圈，由信号发生电路产生三组正弦形式的驱动信号，给三个线圈分别供电，产生一个规则变化的电磁场，接收端的3个线圈分别感应发射端3个线圈产生的磁场，得到9个参数，构成磁感应强度B矩阵，根据磁感应强度B矩阵进行解算可以得到接收端相对于发射端的第二姿态数据H_em。

S3：将惯性导航解算得到的第一姿态数据H_imu和电磁解算得到的第二姿态数据H_em进行融合，得到接收端的姿态数据H。

具体地，将惯性导航解算得到的第一姿态数据和电磁解算得到的第二姿态数据做融合可以包括卡尔曼滤波方法，互补滤波方法，基于IMU预积分的优化方法等。

最后，根据融合后的距离数据和姿态数据，通过优化算法进行位置解算，获得经过融合修正后的空间位置数据。在该融合修正过程中，分别将距离数据的融合修正结果作为第一约束条件，姿态数据的融合修正结果作为第二约束条件，建模为优化问题并进行求解。具体说明如下：

从物理意义上，第一约束条件基于超声和电磁融合，限定了目标点的位置位于一个球面上。第二约束条件基于IMU和电磁融合，限定了目标点的姿态角。所以优化问题为，基于上述两个约束条件，求解目标点的位置。

根据电磁场学的机理，接收端检测到的磁感应强度矩阵B和发射端产生的磁感应强度矩阵B’只相差一个旋转关系，且旋转矩阵为接收端相对发射端的姿态矩阵H，所以可定义误差如下：

优化问题可建模为：

优化变量为(x,y,z)

优化问题的求解过程，首先在球面上提供一个初始位置，然后根据初始位置以及姿态约束，解算出磁感应强度矩阵B，B’，进而计算出目标函数，即最小二乘意义下的误差。然后根据雅可比矩阵，以及调节步长，进行迭代优化。直到误差达到满意的精度。

求解结果作为融合修正后的室内定位结果对外输出。它充分利用了电磁、超声、IMU的测量结果，通过传感器融合得到置信度最高的位姿解算结果。

从上述多传感器融合定位方法的具体步骤可以看出，本发明中的融合修正过程是实时进行的，不需要采集离线数据。另一方面，本发明由于实时进行融合修正，不仅能滤除环境中固定的金属干扰，对于静态及动态磁场畸变都有明显的改善效果。

以上对本发明所提供的多传感器融合定位系统及定位方法进行了详细的说明。对本领域的一般技术人员而言，在不背离本发明实质内容的前提下对它所做的任何显而易见的改动，都将构成对本发明专利权的侵犯，将承担相应的法律责任。

Claims (10)

1.一种多传感器融合定位系统，其特征在于包括惯性定位子系统、电磁定位子系统和超声波定位子系统；

根据所述电磁定位子系统和所述超声波定位子系统的定位结果，进行距离数据的融合；

根据所述惯性定位子系统和所述电磁定位子系统的定位结果，进行姿态数据的融合；

根据所述融合的距离数据和所述融合的姿态数据，得到空间位置数据。

2.如权利要求1所述的多传感器融合定位系统，其特征在于：

所述电磁定位子系统包括电磁发射单元和电磁接收单元，以电磁发射单元为坐标原点，由电磁接收单元检测磁感应强度信号，得到目标的电磁定位结果，所述电磁定位结果包括三维空间坐标和姿态数据。

3.一种多传感器融合定位方法，其特征在于包括如下步骤：

根据超声波定位与电磁定位的定位结果，进行距离数据的融合；

根据惯性导航定位与电磁定位的定位结果，进行姿态数据的融合；

根据所述融合的距离数据和所述融合的姿态数据，获得空间位置数据。

4.如权利要求3所述的多传感器融合定位方法，其特征在于进行所述距离数据的融合包括如下步骤：

第一步：接收端的超声波接收单元检测发射端的超声波发射单元产生的超声波信号，计算出发射端和接收端之间的第一距离R_ultra；

第二步：根据所述电磁定位的定位结果，得到所述发射端和所述接收端之间的第二距离R_em；

第三步：对所述第一距离R_ultra和所述第二距离R_em做融合，得到融合后的距离数据；

第四步：根据所述融合后的距离数据，标定电磁定位的磁场总强度。

5.如权利要求4所述的多传感器融合定位方法，其特征在于利用下述公式对所述第一距离R_ultra和所述第二距离R_em做融合：

R＝α₁R_ultra+(1-α₁)R_em

其中，α₁是融合系数，取值范围在0～1之间，根据超声定位的置信度和电磁定位的置信度进行动态调整。

6.如权利要求5所述的多传感器融合定位方法，其特征在于：

根据所述电磁定位的定位结果，获得接收端的三维空间坐标；

通过所述三维空间坐标判断接收端与发射端的超声发射单元的FOV的位置关系；

如果接收端位于发射端的超声发射单元的FOV的边缘区域，则调低超声定位的置信度，并调低α₁。

7.如权利要求5所述的多传感器融合定位方法，其特征在于：

根据所述电磁定位的定位结果，获得接收端的三维空间坐标；

通过所述三维空间坐标判断接收端与发射端的电磁发射单元的位置关系；

如果接收端位于发射端的电磁发射单元的零平面时，则调低电磁定位的置信度，并调高α₁。

8.如权利要求3所述的多传感器融合定位方法，其特征在于所述姿态数据的融合包括如下步骤：

第一步：对所述惯性导航定位的定位结果进行解算，得出接收单元的第一姿态数据H_imu；

第二步：对所述电磁定位的定位结果进行解算，得到接收单元的第二姿态数据H_em；

第三步：将所述第一姿态数据和所述第二姿态数据进行融合，得到接收端的姿态数据。

9.如权利要求3所述的多传感器融合定位方法，其特征在于将所述距离数据的融合结果作为第一约束条件，将所述姿态数据的融合结果作为第二约束条件，通过优化问题建模得到所述空间位置数据。

10.如权利要求3～9中任意一项所述的多传感器融合定位方法，其特征在于所述距离数据的融合以及所述姿态数据的融合在定位过程中实时进行。

Images