多IMU融合的定位系统及方法与流程

多imu融合的定位系统及方法
技术领域
1.本发明涉及定位技术领域，具体地，涉及一种多imu融合的定位系统及方法。


背景技术：

2.消防人员在救火过程中，作业环境复杂危险，为保障消防人员和国家人民生命安全和财产安全，对消防人员的精确定位和跟踪的重要性日益突出。目前对于消防作业人员定位的方法多种多样，常用的定位方法包括惯性测量单元（inertial measurement unit，imu）、全球卫星导航系统（gnss）以及各种室内定位技术。
3.惯性测量单元（imu）是一种低成本的运动传感器，可对移动平台的角速度和重力补偿的线性加速度进行测量，并广泛用于现代定位系统中。它是一种不依赖外部信息，基于自身的加速度、角速度等传感器进行计算，从而确定运动速度、方向和位置等信息，人员室内定位基本都是以imu为基础，但是其缺点是：误差会随着时间积累，经过长时间后最终误差会变的很大，所以需要外部其他导航源对其修正，这势必会增加系统的复杂度和成本。
4.迄今为止，现有的惯性定位方法仅利用一个imu。尽管单个imu定位可以为不同的使用案例提供可接受的准确性和鲁棒性，但对于人员定位依旧存在如下挑战：零速度更新的判断逻辑所能获取的信息有限，从而不能实施准确的零速度更新算法，大大降低了位置计算的误差；单一imu的系统冗余度较低，对于复杂的作业环境，可靠性很低；单一imu的误差积累较快，有效的工作时间较短。
5.专利文献cn109827593a公开了一种基于多imu的误差自标定方法、系统及存储介质，包括步骤一：将n个imu根据同属性变量平行的原则进行正反面贴片，根据输出变量将n个imu所有误差简化为三类综合误差的参数误差模型，基于统计信息对imu输出最大和最小的量进行剔除，对剔除粗差的imu进行零偏误差补偿，补偿方法为在输出量中直接加上零偏误差量，对补偿后的零偏误差值进行相同性质和方向的输出求均值，输出相应的加速度计、角速度和磁场速度。但该方法并未有效地解决复杂的作业环境下imu可靠性低的问题。


技术实现要素：

6.针对现有技术中的缺陷，本发明的目的是提供一种多imu融合的定位系统及方法。
7.根据本发明提供的一种多imu融合的定位系统，包括如下模块：多imu融合模块和位置速度更新模块。
8.多imu融合模块：用于根据imu的测量数据，得到虚拟融合imu；位置速度更新模块：用于根据虚拟融合imu进行定位处理。
9.优选地，多imu融合模块包括：多个imu和多imu集成处理单元。
10.优选地，定位处理包括：位置更新和速度更新。
11.优选地，多个imu通过预设方式设置在设备上。
12.优选地，设备设置在作业人员的多个部位上。
13.根据本发明提供的一种多imu融合的定位方法，适用于多imu融合的定位系统，包
括：步骤1：获取每个imu的测量数据；步骤2：根据多个imu的相对位置关系，得到相对关系矩阵；步骤3：根据测量数据和相对关系矩阵，得到虚拟融合imu；步骤4：根据虚拟融合imu进行定位处理。
14.优选地，测量数据包括角速度数据和加速度数据；步骤1，包括：步骤101：根据每个imu的陀螺仪，得到角速度数据；步骤102：根据每个imu的加速度计，得到加速度数据。
15.优选地，步骤4，包括：步骤401：根据虚拟融合imu，得到虚拟融合陀螺仪模型；步骤402：根据虚拟融合imu，得到虚拟融合加速度计模型；步骤403：根据虚拟融合陀螺仪模型和虚拟融合加速度计模型进行定位处理。
16.优选地，通过最小二乘法对虚拟融合imu进行优化。
17.优选地，定位处理包括：位置更新和速度更新。
18.与现有技术相比，本发明具有如下的有益效果：1、本发明通过多imu的集成融合，提高了系统的可靠性和鲁棒性。
19.2、本发明通过概率误差的优化求解，提高了imu位置速度的更新精度。
20.3、本发明构建了虚拟集成imu，是的位置速度更新的计算量不变，方便系统的开发和设计。
21.4、本发明的多imu融合的定位系统，可以挂载或佩戴于人员的各个部位和装备上，并可自定义出不同的组合方案，方便不同场景的设计和使用。
22.5、本发明的多imu融合的定位系统，可以避免单一imu仅可依靠单一的运动模态来判断行走和奔跑的步态，从而进一步提高了位置和速度的定位精度。
附图说明
23.通过阅读参照以下附图对非限制性实施例所作的详细描述，本发明的其它特征、目的和优点将会变得更明显：图1为本发明的结构示意图；图2为本发明的多imu集成处理单元的流程示意图；图3为本发明的流程示意图；图4为本发明的位置速度更新的示意图。
具体实施方式
24.下面结合具体实施例对本发明进行详细说明。以下实施例将有助于本领域的技术人员进一步理解本发明，但不以任何形式限制本发明。应当指出的是，对本领域的普通技术人员来说，在不脱离本发明构思的前提下，还可以做出若干变化和改进。这些都属于本发明的保护范围。
25.图1为本发明的结构示意图，如图1所示，本发明提供了一种多imu融合的定位系
统，包括如下模块：多imu融合模块和位置速度更新模块。其中，多imu融合模块中包括多个imu，如图1所示，可以表示为：imu1、imu2、
…
、imun；多imu融合模块中还包括多imu集成处理单元，多imu集成处理单元接收并处理所有imu的测量信息，并对多imu的测量信息进行融合优化，构建一个虚拟融合imu。位置速度更新模块包括位置速度更新，将多imu融合模块所建立的虚拟融合imu进行位置更新和速度的更新。
26.其中，多imu融合模块：用于根据imu的测量数据，得到虚拟融合imu；位置速度更新模块：用于根据虚拟融合imu进行定位处理。
27.具体地，本发明中将所有物理imu的测量概率映射到一个虚拟融合imu上，并通过使用最小二乘估计器进行随机估计。据此，可以使用基于经典滤波器或基于优化的传感器融合算法进行定位处理。
28.优选地，多imu融合模块包括：多个imu和多imu集成处理单元。
29.具体地，多个imu通过预设方式设置在设备上。
30.本发明中对于预设方式的设置不做限制，可以根据具体的情况进行设置，示例性地，预设方式包括但不限于阵列、直线或者堆叠等方式。
31.优选地，设备设置在作业人员的多个部位上。
32.优选地，定位处理包括：位置更新和速度更新。
33.具体地，多imu融合模块可以包含有二个或二个以上imu。并且，这些imu可以是同一型号的，也可以配置不同性能或不同的型号。
34.其中，多个imu的物理位置可以排布于同一设备上，具体地，可以通过包括但不限于阵列、直线或者堆叠等方式。
35.进一步地，也可以根据需要将多imu融合模块分解到多个设备上，每个设备上可通过包括但不限于阵列、直线或者堆叠等方式排布一定数量的imu。每个设备通过一定的通信机制进行imu的测量数据和特征参数的传输和多imu集成的融合。
36.可以理解的是，这些排布有imu的设备可以独立地或者分布地挂载于消防作业人员的足部、四肢、头部或躯干部位等身体部位。
37.具体地，多imu集成处理单元用于接收并处理所有imu的测量数据，并对多imu信息进行融合优化，构建一个虚拟融合imu用于消防人员的定位和跟踪。
38.在本发明中多imu集成处理单元可以和其中的一个、多个或者全部的imu集成在同一个设备上，也可以独立驻留于一个专门设备上。
39.其中，每个设备通过一定的通信机制进行imu测量数据和特征参数的传输，用于支持多imu的集成融合。
40.图2为本发明的多imu集成处理单元的流程示意图，如图2所示，首先，接收各个imu的测量数据，然后，验证各个imu的测量数据的有效性，进一步地，剔除无效的imu的测量数据，最后，根据imu的测量数据进行多imu融合优化。
41.图3为本发明的流程示意图，如图3所示，本发明提供了一种多imu融合的定位方法，适用于上述的多imu融合的定位系统，包括如下步骤：步骤1：获取每个imu的测量数据。
42.其中，测量数据包括角速度数据和加速度数据。
43.具体地，步骤1，可以包括：步骤101：根据每个imu的陀螺仪，得到角速度数据；步骤
102：根据每个imu的加速度计，得到加速度数据。
44.示例性地，有n个imu，每个imu的陀螺仪的测量方程如公式（1）所示：（1）其中，表示第 个imu的陀螺仪在imu本体坐标系下x轴、y轴和z轴三个方向的角速度输出值；表示第个imu的陀螺仪在imu本体坐标系下x轴、y轴和z轴三个方向的角速度理想值；表示符合正态分布的随机游走误差，即高斯噪声，其中，协方差， 表示相应维数的零矩阵，一般由imu制造厂家提供；表示静态测量误差； 表示静态测量误差的噪声，即动态测量误差，可视作标量正态马尔可夫过程。
45.其中，每个imu的加速度计的测量方程如公式（2）所示： ；（2）其中,表示第个imu的加速度计在imu本体坐标系下x轴、y轴和z轴三个方向的加速度输出值；表示第个imu的加速度计在imu本体坐标系下x轴、y轴和z轴三个方向的加速度理想值；表示符合正态分布的随机游走误差，即高斯噪声，其中协方差， 表示相应维数的零矩阵，一般有imu制造厂家提供；表示静态测量误差；表示静态测量误差的噪声，即动态测量误差，亦可视作标量正态马尔可夫过程。
46.步骤2：根据多个imu的相对位置关系，得到相对关系矩阵。
47.具体地，确定 个imu之间位置的相对关系矩阵，具体如公式（3）所示：；（3）其中，为第个imu相对于第个imu的本体坐标转换矩阵，并满足公式（4）：；（4）其中，。
48.需要说明的是，如果多个imu的物理位置通过包括且不限于阵列、直线或者堆叠等方式排布于同一设备上，则相对关系矩阵为常值矩阵。如果这些物理imu分布在多个设备上或者挂载于消防人员的不同部位，则相对关系矩阵为动态矩阵。
49.步骤3：根据测量数据和相对关系矩阵，得到虚拟融合imu。
50.具体地，基于这个物理imu，构建一个虚拟的imu，将其下标定义为，并定义其虚拟的本体坐标系，该虚拟融合imu相对于各个物理imu的坐标转换矩阵为， 。这里，虚拟融合imu的本体坐标系可以选择为独立于所有物理imu的，也可以与其中某一物理imu的本体坐标系一致。
51.一般地，可以先定义虚拟融合imu相对于选定的第个物理imu的相对位置， 。从而得到该虚拟融合imu相对于第个物理imu的坐标转换矩阵 ，若虚拟融合imu与该物理imu的本体坐标系一致，则坐标转换矩阵可以通过公式（5）表示：；（5）进一步地，得到公式（6）： ；（6）步骤4：根据虚拟融合imu进行定位处理。
52.优选地，步骤4，包括：步骤401：根据虚拟融合imu，得到虚拟融合陀螺仪模型；步骤402：根据虚拟融合imu，得到虚拟融合加速度计模型；步骤403：根据虚拟融合陀螺仪模型和虚拟融合加速度计模型进行定位处理。
53.优选地，通过最小二乘法对虚拟融合imu进行优化。
54.其中，定位处理包括：位置更新和速度更新。
55.具体地，虚拟融合imu中的陀螺仪的测量方程如公式（7）所示：；（7）其中，表示虚拟融合imu的陀螺仪在本体坐标系下x轴、y轴和z轴三个方向的角速度输出值；表示所有物理imu的陀螺仪的输出值向量；表示的广义逆矩阵，即，；可以通过公式（8）表示：；（8）类似地，虚拟融合imu的加速度计的测量方程如公式（9）所示：；（9）其中，表示该虚拟融合imu的加速度计在其本体坐标系
下x轴、y轴和z轴三个方向的加速度输出值；表示所有物理imu的加速度计的输出值向量。
56.同上，为的广义逆矩阵，即，；通过公式（8）表示。
57.进一步地，求解虚拟融合陀螺仪模型。
58.具体地，根据虚拟融合imu，可将所有物理imu的陀螺仪的输出值向量改写成公式（10）：；（10）其中，。
59.然后，重写每个物理imu的角速度测量方程中的高斯噪声在各个轴上表达式如公式（11）所示：；（11）其中，。
60.进一步地，定义，通过公式（12）表示：；（12）则，虚拟融合imu的角速度的最佳估计满足公式（13）：；（13）进一步地，通过最小二乘法求解公式（13）。为简便起见，假设每个物理imu的角速度测量方程中的高斯噪声在各个轴上一致，并定义为，即，。
61.同样地，若所有的物理imu采用相同的型号，则可以进一步假设每个imu的高斯噪
声一致，并定义为，即，。
62.通过最小二乘法求解公式（13），并将其写作和物理imu的测量方程形式一致，则:可以表示为公式（14）： ；（14）其中，表示虚拟融合imu的陀螺仪在该imu本体坐标系下x、y、z三个方向的角速度输出值。
[0063] 为虚拟融合imu的陀螺仪在该imu本体坐标系下x轴、y轴和z轴三个方向的角速度理想值；表示符合正态分布的随机游走误差，即高斯噪声，其中协方差由最小二乘法求解得到，为相应维数的零矩阵；表示静态测量误差,由最小二乘法求解得到；表示静态测量误差的噪声，即动态测量误差，可视作标量正态马尔可夫过程,由最小二乘法求解得到。
[0064]
然后，根据上述方法求解虚拟融合加速度计模型。
[0065]
具体地，根据虚拟融合imu，可将所有物理imu的加速度计的输出值向量改写成公式（15）：；（15）其中，。
[0066]
然后，重写每个物理imu的加速度测量方程中的高斯噪声在各个轴上表达式，如公式（16）所示：；（16）进一步地，定义，可以表示为公式（17）：
；（17）则虚拟融合imu加速度的最佳估计满足公式（18）：；（18）下面将通过最小二乘法求解公式（18）。为简便起见，假设每个物理imu的加速度测量方程中的高斯噪声在各个轴上一致，并定义为，即，。
[0067]
同样地，若所有的物理imu采用相同的型号，则可以进一步假设每个imu的高斯噪声一致，并定义为，即，。
[0068]
通过最小二乘法求解，并将其写作和物理imu的测量方程形式一致，则可以得到公式（19）：；（19）其中，表示虚拟融合imu的加速度计在imu本体坐标系下x轴、y轴和z轴三个方向的加速度输出值；表示虚拟融合imu的加速度计在imu本体坐标系下x轴、y轴和z轴三个方向的加速度理想值； 表示符合正态分布的随机游走误差，即高斯噪声，其中协方差由最小二乘法求解得到，为相应维数的零矩阵； 表示静态测量误差，由最小二乘法求解得到；表示静态测量误差的噪声，即动态测量误差，亦可视作标量正态马尔可夫过程，由最小二乘法求解得到。
[0069]
可知的是，通过上述步骤可以得到基于多个物理imu融合得到的虚拟融合imu，其形式符合单个物理imu所具备的相应特征参数，可通过常规的imu传播方程来计算更新装备多imu的人员的位置和速度。
[0070]
优选地，位置速度更新模块将多imu融合模块所建立的虚拟融合imu进行位置和速度的更新。
[0071]
图4为本发明的位置速度更新的示意图，如图4所示，通过加速度计和陀螺仪进行加速度坐标转换和姿态更新，加速度坐标转换后进行的速度更新和位置横溪你，位置更新后可以进行重力更新和方向变化率更新，速度更新后可以进行方向变化率更新，方向变化
率更新后可以进行方向更新和姿态更新，进一步地，姿态更新数据可以用于加速度坐标转换，方向更新数据和重力更新数据可以用于速度更新，速度更新数据和位置更新数据用于定位处理。
[0072]
本发明提出了一种多imu融合的定位系统及方法来提高消防人员的定位精度，该系统和方法通过多imu集成处理单元，基于分布式的多个imu来构建虚拟集成imu，即多imu融合模块。
[0073]
需要说明的是，本发明中通过引入多个imu的动态相对关系矩阵，使得多个imu既可以是固定装载于同一个定位设备之中，亦可以分布式装载于各个相关的定位子设备中。本发明中确定的各个imu之间的实时动态的相对关系矩阵，一方面可以提高所构建的虚拟集成imu的精度；另一方面也可以对消防人员作业过程中的步态和运动姿势等做出判断；从而提高了imu零速更新算法的准确度，以此来提高计算消防人员位置的精度，以及，为任务执行状态和消防人员的状态进行辅助监控。本发明中多imu组合导航多见于一般的融合算法，但并未涉及基于多个imu来构建一个虚拟集成imu的方法来用于消防人员的定位，并且本发明中避免了一般的融合算法对误差积累的影响，通过概率误差的优化求解，保证了虚拟集成imu长时间进行速度和位置更新的精度。
[0074]
本发明的技术原理是：本发明融合来自多个imu的测量数据，从为定位系统能够获得显著的性能提升，而不会产生额外的计算成本。本发明将所有物理imu的测量概率映射到一个虚拟融合imu上，并通过使用最小二乘估计器进行随机估计。据此，可以使用基于经典滤波器或基于优化的传感器融合算法进行定位。
[0075]
本发明解决的技术问题主要为：1、零速度更新的判断逻辑所能获取的信息有限，从而不能实施准确的零速度更新算法，大大降低了位置计算的误差。
[0076]
2、单一imu的系统冗余度较低，对于复杂的作业环境，可靠性很低。
[0077]
3、单一imu的误差积累较快，有效的工作时间较短。
[0078]
4、通过使用多个imu可以进一步提高系统的整体性能，使得可靠性更高且误差随时间的积累更慢。
[0079]
与现有技术相比，本发明具有如下的有益效果：1、本发明通过多imu的集成融合，提高了系统的可靠性和鲁棒性。
[0080]
2、本发明通过概率误差的优化求解，提高了imu位置速度的更新精度。
[0081]
3、本发明构建了虚拟集成imu，是的位置速度更新的计算量不变，方便系统的开发和设计。
[0082]
4、本发明的多imu融合的定位系统，可以挂载或佩戴于人员的各个部位和装备上，并可自定义出不同的组合方案，方便不同场景的设计和使用。
[0083]
5、本发明的多imu融合的定位系统，可以避免单一imu仅可依靠单一的运动模态来判断行走和奔跑的步态，从而进一步提高了位置和速度的定位精度。
[0084]
本领域技术人员知道，除了以纯计算机可读程序代码方式实现本发明提供的系统、装置及其各个模块以外，完全可以通过将方法步骤进行逻辑编程来使得本发明提供的系统、装置及其各个模块以逻辑门、开关、专用集成电路、可编程逻辑控制器以及嵌入式微
控制器等的形式来实现相同程序。所以，本发明提供的系统、装置及其各个模块可以被认为是一种硬件部件，而对其内包括的用于实现各种程序的模块也可以视为硬件部件内的结构；也可以将用于实现各种功能的模块视为既可以是实现方法的软件程序又可以是硬件部件内的结构。
[0085]
以上对本发明的具体实施例进行了描述。需要理解的是，本发明并不局限于上述特定实施方式，本领域技术人员可以在权利要求的范围内做出各种变化或修改，这并不影响本发明的实质内容。在不冲突的情况下，本技术的实施例和实施例中的特征可以任意相互组合。