CN115060259A

CN115060259A - Multi-IMU fusion positioning system and method

Info

Publication number: CN115060259A
Application number: CN202210996020.3A
Authority: CN
Inventors: 曹熠峰; 杨勇
Original assignee: Jiangsu Deyijia Security Technology Co ltd
Current assignee: Jiangsu Deyijia Security Technology Co ltd
Priority date: 2022-08-19
Filing date: 2022-08-19
Publication date: 2022-09-16

Abstract

The invention provides a positioning system and a method for fusing multiple IMUs, which comprises the following steps: the system comprises a multi-IMU fusion module and a position and speed updating module; a multi-IMU fusion module: the virtual fusion IMU is obtained according to the measurement data of the IMU; a location speed update module: and the IMU is used for positioning according to the virtual fusion IMU. Compared with the prior art, the method improves the reliability and robustness of positioning through the integrated fusion of multiple IMUs.

Description

Multi-IMU fusion positioning system and method

Technical Field

The invention relates to the technical field of positioning, in particular to a positioning system and a method with multiple IMUs integrated.

Background

The fire fighters have complex and dangerous operation environments in the fire fighting process, and the importance of accurate positioning and tracking of the fire fighters is increasingly prominent for guaranteeing the life safety and property safety of the fire fighters and the national people. At present, there are various methods for positioning fire-fighting operators, and common positioning methods include an Inertial Measurement Unit (IMU), a Global Navigation Satellite System (GNSS), and various indoor positioning technologies.

An Inertial Measurement Unit (IMU) is a low cost motion sensor that can measure angular velocity and gravity-compensated linear acceleration of a moving platform and is widely used in modern positioning systems. The indoor positioning system is independent of external information, and is calculated based on sensors such as acceleration and angular velocity of the indoor positioning system, so that information such as movement speed, direction and position is determined, and indoor positioning of people is basically based on IMU (inertial measurement unit), but the indoor positioning system has the following defects: errors accumulate over time and eventually become large over time, requiring correction by other external sources of navigation, which inevitably increases the complexity and cost of the system.

To date, existing inertial positioning methods utilize only one IMU. While a single IMU positioning may provide acceptable accuracy and robustness for different use cases, the following challenges remain for personnel positioning: the information which can be obtained by the judgment logic of zero-speed updating is limited, so that an accurate zero-speed updating algorithm cannot be implemented, and the error of position calculation is greatly reduced; the system redundancy of a single IMU is low, and the reliability is low for a complex operation environment; the error accumulation of a single IMU is fast, and the effective working time is short.

Patent document CN109827593A discloses an error self-calibration method, system and storage medium based on multiple IMUs, which includes the steps of: the method comprises the steps of conducting front and back surface pasting on n IMUs according to the principle that the variables with the same attribute are parallel, simplifying all errors of the n IMUs into parameter error models of three types of comprehensive errors according to output variables, removing the maximum and minimum output quantities of the IMUs based on statistical information, conducting zero offset error compensation on the IMUs with gross errors removed, directly adding the zero offset error quantities in output quantities, conducting output averaging of the compensated zero offset error values in the same property and direction, and outputting corresponding accelerometers, angular velocities and magnetic field velocities. However, this method does not effectively solve the problem of low reliability of the IMU in a complex working environment.

Disclosure of Invention

Aiming at the defects in the prior art, the invention aims to provide a positioning system and a positioning method for multi-IMU fusion.

The invention provides a positioning system with multiple IMUs fused, which comprises the following modules: the system comprises a multi-IMU fusion module and a position and speed updating module.

A multi-IMU fusion module: the virtual fusion IMU is obtained according to the measurement data of the IMU;

a location speed update module: and the positioning processing is carried out according to the virtual fusion IMU.

Preferably, the multi-IMU fusion module includes: multiple IMUs and multiple IMUs integrated processing units.

Preferably, the positioning process comprises: location update and velocity update.

Preferably, the plurality of IMUs are provided on the device in a preset manner.

Preferably, the apparatus is provided at a plurality of locations on the worker.

The positioning method for fusing multiple IMUs provided by the invention is suitable for a positioning system for fusing multiple IMUs, and comprises the following steps:

step 1: acquiring measurement data of each IMU;

step 2: obtaining a relative relation matrix according to the relative position relation of the IMUs;

and step 3: obtaining a virtual fusion IMU according to the measurement data and the relative relation matrix;

and 4, step 4: and performing positioning processing according to the virtual fusion IMU.

Preferably, the measurement data comprises angular velocity data and acceleration data;

step 1, comprising:

step 101: obtaining angular velocity data according to the gyroscope of each IMU;

step 102: acceleration data is obtained from the accelerometer of each IMU.

Preferably, step 4, comprises:

step 401: obtaining a virtual fusion gyroscope model according to the virtual fusion IMU;

step 402: obtaining a virtual fusion accelerometer model according to the virtual fusion IMU;

step 403: and carrying out positioning processing according to the virtual fusion gyroscope model and the virtual fusion accelerometer model.

Preferably, the virtual fused IMU is optimized by a least squares method.

Compared with the prior art, the invention has the following beneficial effects:

1. the invention improves the reliability and robustness of the system through the integrated fusion of multiple IMUs.

2. According to the method, the updating precision of the IMU position speed is improved through the optimization solution of the probability error.

3. The invention constructs the virtual integrated IMU, so that the calculation amount of position and speed updating is not changed, and the development and design of a system are facilitated.

4. The positioning system with the multiple IMUs integrated can be mounted or worn on various parts and equipment of people, different combination schemes can be customized, and design and use of different scenes are facilitated.

5. The positioning system with the multiple IMUs integrated can avoid the situation that a single IMU can judge the walking and running gaits only by means of a single motion mode, so that the positioning accuracy of the position and the speed is further improved.

Drawings

Other features, objects and advantages of the invention will become more apparent upon reading of the detailed description of non-limiting embodiments with reference to the following drawings:

FIG. 1 is a schematic structural view of the present invention;

FIG. 2 is a flow diagram of a multi-IMU integrated processing unit of the present invention;

FIG. 3 is a schematic flow chart of the present invention;

FIG. 4 is a diagram illustrating the location and velocity update of the present invention.

Detailed Description

The present invention will be described in detail with reference to specific examples. The following examples will assist those skilled in the art in further understanding the invention, but are not intended to limit the invention in any way. It should be noted that it would be obvious to those skilled in the art that various changes and modifications can be made without departing from the spirit of the invention. All falling within the scope of the present invention.

Fig. 1 is a schematic structural diagram of the present invention, and as shown in fig. 1, the present invention provides a positioning system with multiple IMU fusion, which includes the following modules: the system comprises a multi-IMU fusion module and a position and speed updating module. The multi-IMU fusion module includes a plurality of IMUs, as shown in fig. 1, which may be represented as: IMU1, IMU2, …, IMUn; the multi-IMU fusion module also comprises a multi-IMU integrated processing unit, and the multi-IMU integrated processing unit receives and processes the measurement information of all IMUs, performs fusion optimization on the measurement information of the IMUs and constructs a virtual fusion IMU. The position and speed updating module comprises position and speed updating, and performs position updating and speed updating on the virtual fusion IMU established by the multi-IMU fusion module.

Wherein, many IMU fuse the module: the virtual fusion IMU is obtained according to the measurement data of the IMU; a location speed update module: and the positioning processing is carried out according to the virtual fusion IMU.

Specifically, in the present invention, the measurement probabilities of all physical IMUs are mapped onto one virtual fusion IMU and randomly estimated by using a least squares estimator. Accordingly, a classical filter-based or optimization-based sensor fusion algorithm may be used for the localization process.

Specifically, a plurality of IMUs are set on the device in a preset manner.

The preset mode is not limited in the present invention, and may be set according to specific situations, for example, the preset mode includes, but is not limited to, an array, a straight line, or a stack.

Specifically, the multi-IMU fusion module may include two or more IMUs. Moreover, the IMUs may be of the same model, or may be configured with different capabilities or different models.

Wherein the physical locations of the plurality of IMUs may be arranged on the same device, in particular, by means including, but not limited to, an array, a line, or a stack.

Further, the multi-IMU fusion module may be decomposed into a plurality of devices as needed, and each device may have a certain number of IMUs arranged thereon in a manner including, but not limited to, an array, a line, or a stack. And each device performs transmission of measurement data and characteristic parameters of the IMU and integration of multiple IMUs through a certain communication mechanism.

It will be appreciated that these IMU-lined devices may be carried independently or distributed about the body part of the firefighter, such as the feet, limbs, head or torso.

Specifically, the multi-IMU integrated processing unit is used for receiving and processing the measurement data of all IMUs, performing fusion optimization on multi-IMU information, and constructing a virtual fusion IMU for positioning and tracking of fire fighters.

In the invention, the multi-IMU integrated processing unit can be integrated with one, a plurality of or all IMUs on the same device, or can independently reside on a special device.

Each device transmits IMU measurement data and characteristic parameters through a certain communication mechanism and is used for supporting the integration and fusion of multiple IMUs.

Fig. 2 is a schematic flow diagram of a multi-IMU integrated processing unit according to the present invention, and as shown in fig. 2, firstly, the measurement data of each IMU is received, then, the validity of the measurement data of each IMU is verified, further, the measurement data of invalid IMU is removed, and finally, multi-IMU fusion optimization is performed according to the measurement data of IMU.

Fig. 3 is a schematic flow chart of the present invention, and as shown in fig. 3, the present invention provides a positioning method for multi-IMU fusion, which is suitable for the above positioning system for multi-IMU fusion, and includes the following steps:

step 1: measurement data for each IMU is acquired.

Wherein the measurement data comprises angular velocity data and acceleration data.

Specifically, step 1 may include: step 101: obtaining angular velocity data according to the gyroscope of each IMU; step 102: acceleration data is obtained from the accelerometer of each IMU.

Illustratively, there are n IMUs, and the measurement equation for the gyroscope of each IMU is shown in equation (1):

（1）

wherein the content of the first and second substances,

is shown as

Angular speed output values of a gyroscope of each IMU in three directions of an x axis, a y axis and a z axis under an IMU body coordinate system;

denotes the first

Angular velocity ideal values of a gyroscope of each IMU in three directions of an x axis, a y axis and a z axis under an IMU body coordinate system;

representing random walk errors conforming to a normal distribution, i.e. Gaussian noise, in which the covariance

，

A zero matrix representing the corresponding dimension, typically provided by the IMU manufacturer;

representing static measurement error;

the noise representing the static measurement error, i.e. the dynamic measurement error, can be seen as a scalar normal markov process.

Wherein the measurement equation for the accelerometer of each IMU is shown in equation (2):

；（2）

wherein the content of the first and second substances,

is shown as

Acceleration output values of an accelerometer of each IMU in three directions of an x axis, a y axis and a z axis under an IMU body coordinate system;

is shown as

Acceleration ideal values of an accelerometer of each IMU in three directions of an x axis, a y axis and a z axis under an IMU body coordinate system;

representing random walk errors following a normal distribution, i.e. Gaussian noise, in which the covariance

，

The zero matrix representing the corresponding dimension is typically provided by the IMU manufacturer;

representing static measurement error;

the noise representing the static measurement error, i.e., the dynamic measurement error, can also be considered a scalar normal Markov process.

Step 2: and obtaining a relative relation matrix according to the relative position relation of the IMUs.

Specifically, determining

Relative relationship matrix of position between IMUs

Specifically, as shown in formula (3):

；（3）

wherein the content of the first and second substances,

is as follows

IMU relative to

The body coordinate transformation matrix of each IMU meets the formula (4):

；（4）

wherein the content of the first and second substances,

。

it should be noted that if the physical locations of multiple IMUs are arranged on the same device by way of including, but not limited to, an array, a line, or a stack, the relative relationship matrix is

Is a constant matrix. If these physical IMUs are distributed across multiple devices or mounted at different locations on a firefighter, the relative relationship matrix

Is a dynamic matrix.

And step 3: and obtaining the virtual fusion IMU according to the measurement data and the relative relation matrix.

In particular, based on this

A physical IMU for constructing a virtual IMU with subscript defined as

And defining a virtual body coordinate system, the virtual fusion IMU having a coordinate transformation matrix with respect to each physical IMU

，

. Here, the body coordinate system of the virtual fused IMU may be selected to be independent of all the physical IMUs, or may be consistent with the body coordinate system of one of the physical IMUs.

In general, the virtual fused IMU may be defined relative to a selected second IMU

The relative position of the individual physical IMUs,

. Thereby obtaining the virtual fused IMU relative to the second

Coordinate transformation matrix of physical IMU

If the virtual fusion IMU is consistent with the body coordinate system of the physical IMU, the coordinate transformation matrix

Can be expressed by equation (5):

；（5）

further, equation (6) is obtained:

；（6）

Preferably, step 4, comprises: step 401: obtaining a virtual fusion gyroscope model according to the virtual fusion IMU;

step 402: obtaining a virtual fusion accelerometer model according to the virtual fusion IMU; step 403: and positioning processing is carried out according to the virtual fusion gyroscope model and the virtual fusion accelerometer model.

Preferably, the virtual fused IMU is optimized by a least squares method.

Wherein the positioning process comprises: location update and velocity update.

Specifically, the measurement equation of the gyroscope in the virtual fusion IMU is shown in equation (7):

；（7）

wherein, the first and the second end of the pipe are connected with each other,

representing angular velocity output values of a gyroscope with a virtual fusion IMU in three directions of an x axis, a y axis and a z axis under a body coordinate system;

a vector of gyroscope output values representing all physical IMUs;

to represent

The generalized inverse matrix of (a), i.e.,

；

can be expressed by equation (8):

；（8）

similarly, the measurement equation for the accelerometers of the virtual fusion IMU is shown in equation (9):

；（9）

wherein the content of the first and second substances,

representing the acceleration output values of the accelerometer of the virtual fusion IMU in three directions of an x axis, a y axis and a z axis under a body coordinate system of the accelerometer;

a vector of output values representing the accelerometers of all physical IMUs.

As in the above, the above-mentioned,

is composed of

The generalized inverse matrix of (a), i.e.,

；

represented by formula (8).

Further, the virtual fusion gyroscope model is solved.

In particular, according to the virtual converged IMU, the institute can be integratedOutput value vector for a gyroscope with a physical IMU

Rewritten as formula (10):

；（10）

wherein the content of the first and second substances,

。

then, the gaussian noise in the angular velocity measurement equation for each physical IMU is rewritten to be expressed on the respective axes as shown in equation (11):

；（11）

wherein the content of the first and second substances,

。

further, define

，

Expressed by equation (12):

；（12）

then, the best estimate of angular velocity of the virtually fused IMU

Satisfies formula (13):

；（13）

further, equation (13) is solved by the least square method. For simplicity, the Gaussian noise in the angular velocity measurement equation for each physical IMU is assumed to be consistent across the various axes and is defined as

That is to say that,

。

similarly, if all the physical IMUs are of the same type, it may be further assumed that the gaussian noise of each IMU is consistent and defined as

That is to say that,

。

by solving equation (13) by the least square method and writing it in conformity with the measurement equation form of the physical IMU, it can be expressed as equation (14):

；（14）

wherein the content of the first and second substances,

and the gyroscope represents angular speed output values of the virtual fusion IMU in the directions of x, y and z under the IMU body coordinate system.

The method comprises the following steps of virtually fusing ideal angular velocity values of a gyroscope of an IMU in three directions of an x axis, a y axis and a z axis under an IMU body coordinate system;

representing random walk errors following a normal distribution, i.e. Gaussian noiseWherein the covariance

Is obtained by solving through a least square method,

a zero matrix of corresponding dimensions;

representing static measurement errors and solving by a least square method;

the noise representing the static measurement error, i.e., the dynamic measurement error, can be considered as a scalar normal markov process, and is solved by the least square method.

Then, the virtual fusion accelerometer model is solved according to the method.

In particular, according to the virtual fused IMU, the output values of the accelerometers of all physical IMUs can be vectorized

Rewritten as formula (15):

；（15）

。

then, the gaussian noise in the acceleration measurement equation for each physical IMU is rewritten for expression on the respective axis, as shown in equation (16):

；（16）

further, define

，

Can be expressed as formula (17):

；（17）

optimal estimation of virtual fused IMU acceleration

Satisfies formula (18):

；（18）

equation (18) will be solved by the least square method below. For simplicity, the Gaussian noise in the acceleration measurement equation for each physical IMU is assumed to be consistent across the axes and is defined as

That is to say that,

。

That is to say that,

。

solving by the least squares method, and writing it to be consistent with the form of the measurement equation of the physical IMU, equation (19) can be obtained:

；（19）

wherein the content of the first and second substances,

representing acceleration output values of an accelerometer virtually fused with the IMU in three directions of an x axis, a y axis and a z axis under an IMU body coordinate system;

representing the ideal acceleration values of the accelerometer virtually fused with the IMU in the three directions of the x axis, the y axis and the z axis under the IMU body coordinate system;

Is obtained by solving through a least square method,

a zero matrix of corresponding dimensions;

representing static measurement errors and solving by a least square method;

the noise representing the static measurement error, i.e. the dynamic measurement error, can also be regarded as a scalar normal Markov process, and is obtained by solving with the least square method.

It can be known that, through the above steps, a virtual fused IMU obtained based on fusion of a plurality of physical IMUs can be obtained, the form of the virtual fused IMU conforms to the corresponding characteristic parameters of a single physical IMU, and the position and the speed of the person who is equipped with the plurality of IMUs can be calculated through a conventional IMU propagation equation.

Preferably, the position and speed updating module updates the position and speed of the virtual fused IMU established by the multi-IMU fusion module.

Fig. 4 is a schematic diagram of position and velocity update according to the present invention, as shown in fig. 4, acceleration coordinate conversion and posture update are performed by an accelerometer and a gyroscope, velocity update and position cross flow are performed after acceleration coordinate conversion, gravity update and direction change rate update are performed after position update, direction change rate update is performed after velocity update, direction update and posture update are performed after direction change rate update, further, posture update data may be used for acceleration coordinate conversion, direction update data and gravity update data may be used for velocity update, and velocity update data and position update data are used for positioning processing.

The invention provides a positioning system and a positioning method for fusing multiple IMUs (inertial measurement units) to improve the positioning accuracy of firefighters.

It should be noted that, in the present invention, by introducing the dynamic relative relationship matrix of multiple IMUs, multiple IMUs may be fixedly loaded in the same positioning device, or may be distributed and loaded in each related positioning sub-device. The real-time dynamic relative relationship matrix among the IMUs is determined, so that on one hand, the precision of the constructed virtual integrated IMU can be improved; on the other hand, the gait, the movement posture and the like of the fire fighter in the operation process can be judged; therefore, the accuracy of the IMU zero-speed updating algorithm is improved, the precision of calculating the positions of the firefighters is improved, and auxiliary monitoring is carried out on the task execution state and the states of the firefighters. The multi-IMU combined navigation method is mostly seen in a common fusion algorithm, but a method for constructing a virtual integrated IMU based on a plurality of IMUs is not involved for positioning fire fighters, the influence of the common fusion algorithm on error accumulation is avoided, and the accuracy of updating the speed and the position of the virtual integrated IMU for a long time is ensured through the optimal solution of probability errors.

The technical principle of the invention is as follows:

the present invention fuses measurement data from multiple IMUs to enable significant performance improvements for a positioning system without incurring additional computational costs. The invention maps the measurement probabilities of all physical IMUs onto one virtual fusion IMU and performs stochastic estimation by using a least squares estimator. Accordingly, classical filter-based or optimization-based sensor fusion algorithms can be used for localization.

The invention mainly solves the technical problems that:

1. the information which can be obtained by the judgment logic of zero-speed updating is limited, so that an accurate zero-speed updating algorithm cannot be implemented, and the error of position calculation is greatly reduced.

2. The system redundancy of a single IMU is low, and the reliability is low for a complex operation environment.

3. The error accumulation of a single IMU is fast, and the effective working time is short.

4. The overall performance of the system may be further improved by using multiple IMUs, resulting in higher reliability and slower accumulation of errors over time.

3. The invention constructs the virtual integrated IMU, so that the calculation amount of position and speed updating is unchanged, and the development and design of the system are facilitated.

Those skilled in the art will appreciate that, in addition to implementing the systems, apparatus, and various modules thereof provided by the present invention in purely computer readable program code, the same procedures can be implemented entirely by logically programming method steps such that the systems, apparatus, and various modules thereof are provided in the form of logic gates, switches, application specific integrated circuits, programmable logic controllers, embedded microcontrollers and the like. Therefore, the system, the device and the modules thereof provided by the present invention can be considered as a hardware component, and the modules included in the system, the device and the modules thereof for implementing various programs can also be considered as structures in the hardware component; modules for performing various functions may also be considered to be both software programs for performing the methods and structures within hardware components.

The foregoing description of specific embodiments of the present invention has been presented. It is to be understood that the present invention is not limited to the specific embodiments described above, and that various changes or modifications may be made by one skilled in the art within the scope of the appended claims without departing from the spirit of the invention. The embodiments and features of the embodiments of the present application may be combined with each other arbitrarily without conflict.

Claims

1. A multi-IMU fused localization system, the system comprising: the system comprises a multi-IMU fusion module and a position and speed updating module;

a location speed update module: and the IMU is used for positioning according to the virtual fusion IMU.

2. The multi-IMU fused localization system of claim 1, wherein the multi-IMU fusion module comprises: a plurality of the IMUs and a multi-IMU integrated processing unit.

3. The multi-IMU fused localization system of claim 1, wherein the localization process comprises: location update and velocity update.

4. The multi-IMU fused positioning system of claim 2, wherein a plurality of said IMUs are disposed on a device in a predetermined manner.

5. The multi-IMU fused positioning system of claim 4, wherein the apparatus is provided on a plurality of locations on a worker.

6. A multi-IMU fused localization method adapted to the multi-IMU fused localization system of claim 1, the method comprising:

step 1: acquiring measurement data of each IMU;

and 3, step 3: obtaining a virtual fusion IMU according to the measurement data and the relative relation matrix;

7. The multi-IMU fused localization method of claim 6, wherein the measurement data comprises angular velocity data and acceleration data;

the step 1 comprises the following steps:

step 101: obtaining the angular velocity data according to the gyroscope of each IMU;

step 102: and obtaining the acceleration data according to the accelerometer of each IMU.

8. The method for multi-IMU fused localization according to claim 6 or 7, wherein the step 4 comprises:

step 403: and positioning according to the virtual fusion gyroscope model and the virtual fusion accelerometer model.

9. The multi-IMU fused localization method of claim 6, wherein the virtual fused IMU is optimized by a least squares method.

10. The multi-IMU fused localization method of claim 6, wherein the localization process comprises: location update and velocity update.