[发明专利]一种多传感器融合定位系统及定位方法

权利要求书

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

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

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

将距离数据的融合修正结果作为第一约束条件，将姿态数据的融合修正结果作为第二约束条件，通过优化问题建模得到空间位置数据；

其中，所述根据超声波定位与电磁定位的定位结果，进行距离数据的融合修正，包括如下子步骤：

接收端的超声波接收单元检测发射端的超声波发射单元产生的超声波信号，计算出发射端和接收端之间的第一距离R_ultra；根据所述电磁定位的定位结果，得到所述发射端和所述接收端之间的第二距离R_em；利用下述公式对所述第一距离R_ultra和所述第二距离R_em做融合，以得到融合后的距离数据：

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

α₁是融合系数，取值范围在0～1之间，根据超声定位的置信度和电磁定位的置信度进行动态调整；其中，根据所述电磁定位的定位结果，获得接收端的三维空间坐标；通过所述三维空间坐标判断接收端与发射端的超声波发射单元的FOV的位置关系；如果接收端位于发射端的超声波发射单元的FOV的边缘区域，则调低超声定位的置信度，并调低α₁；

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

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

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

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

将距离数据的融合修正结果作为第一约束条件，将姿态数据的融合修正结果作为第二约束条件，通过优化问题建模得到空间位置数据；

其中，所述根据超声波定位与电磁定位的定位结果，进行距离数据的融合修正，包括如下子步骤：

接收端的超声波接收单元检测发射端的超声波发射单元产生的超声波信号，计算出发射端和接收端之间的第一距离R_ultra；根据所述电磁定位的定位结果，得到所述发射端和所述接收端之间的第二距离R_em；利用下述公式对所述第一距离R_ultra和所述第二距离R_em做融合，以得到融合后的距离数据：

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

α₁是融合系数，取值范围在0～1之间，根据超声定位的置信度和电磁定位的置信度进行动态调整；其中，根据所述电磁定位的定位结果，获得接收端的三维空间坐标；通过所述三维空间坐标判断接收端与发射端的电磁发射单元的位置关系；如果接收端位于发射端的电磁发射单元的零平面时，则调低电磁定位的置信度，并调高α₁；

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

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

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

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

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

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

所述距离数据的融合修正以及所述姿态数据的融合修正在定位过程中实时进行。