WO2022135070A1

WO2022135070A1 - Inertial navigation method and device

Info

Publication number: WO2022135070A1
Application number: PCT/CN2021/133976
Authority: WO
Inventors: 郭为
Original assignee: 北京紫光展锐通信技术有限公司
Priority date: 2020-12-24
Filing date: 2021-11-29
Publication date: 2022-06-30
Also published as: CN112525190A

Abstract

An inertial navigation method and apparatus, a mobile terminal, a computer readable storage medium, and a computer program product. Said method comprises: using an acceleration measurement device of a mobile terminal to obtain acceleration data of the mobile terminal (S201); processing the acceleration data on the basis of a first Kalman filter to obtain error data corresponding to the acceleration data, the first Kalman filter being a zero-order Kalman filter (S202); processing the acceleration data on the basis of the error data, to obtain processed target acceleration data (S203); and processing the target acceleration data on the basis of a second Kalman filter to obtain motion data of the mobile terminal (S204). In said method, after the acceleration data of the mobile terminal is measured, the error of the acceleration data is eliminated first by using the first Kalman filter, and then the motion data of the mobile terminal is calculated by using the second Kalman filter, so that real-time elimination of integration error can be achieved during an inertial navigation process, improving the accuracy of inertial navigation.

Description

Inertial navigation method and device

This application claims the priority of the Chinese patent application with the application number 202011552499.9 and the application name "Inertial Navigation Method and Device" filed with the China Patent Office on December 24, 2020, the entire contents of which are incorporated into this application by reference.

technical field

The embodiments of the present application relate to the technical field of inertial navigation, and in particular, to an inertial navigation method and device.

Background technique

The Inertial Navigation System (INS) takes Newton's laws of mechanics as its working principle, uses the inertial measurement element installed on the carrier to measure the acceleration information of the carrier, and obtains the navigation parameters such as the displacement and velocity of the carrier through integral operation.

Inertial navigation has complete autonomy and anti-interference, and has high accuracy in a short period of time. However, affected by the actual measurement environment, some errors will inevitably be introduced in the entire integration process, and the error will change with time. Accumulated, resulting in poor accuracy for long-term navigation work.

How to reduce the integral error in inertial navigation in real time and improve the accuracy of inertial navigation needs to be solved urgently.

SUMMARY OF THE INVENTION

The embodiments of the present application provide an inertial navigation method and device, which can effectively reduce the integral error in the inertial navigation and improve the precision of the inertial navigation.

In a first aspect, an embodiment of the present application provides an inertial navigation method, which is applied to a mobile terminal, and the method includes:

Using the acceleration measuring device of the mobile terminal to measure the acceleration data of the mobile terminal;

The acceleration data is processed based on the first Kalman filter to obtain error data corresponding to the acceleration data, and the first Kalman filter is a zero-order Kalman filter;

processing the acceleration data based on the error data to obtain processed target acceleration data;

The target acceleration data is processed based on the second Kalman filter to obtain motion data of the mobile terminal.

In a possible design manner, the acceleration data is processed based on the error data to obtain processed target acceleration data, including:

A difference between the acceleration data and the error data is calculated, and the difference is determined as the target acceleration data.

In a possible design manner, the target acceleration data is processed based on the second Kalman filter to obtain the motion data of the mobile terminal, including:

Integrate the target acceleration data based on the second Kalman filter to obtain velocity data and displacement data of the mobile terminal.

In a possible design manner, the second Kalman filter is a second-order Kalman filter.

In a second aspect, an embodiment of the present application provides an inertial navigation device, which is applied to a mobile terminal, and the device includes:

a measurement module, configured to obtain acceleration data of the mobile terminal by measuring the acceleration measurement device of the mobile terminal;

The first processing module is configured to process the acceleration data based on the first Kalman filter, obtain error data corresponding to the acceleration data, and process the acceleration data based on the error data to obtain the processed acceleration data. target acceleration data, the first Kalman filter is a zero-order Kalman filter;

The second processing module is configured to process the target acceleration data based on the second Kalman filter to obtain motion data of the mobile terminal.

In a possible design, the first processing module is used for:

In a possible design, the second processing module is used for:

In a third aspect, an embodiment of the present application provides a mobile terminal, including at least one processor and a memory;

the memory stores computer-executable instructions;

The at least one processor executes computer-implemented instructions stored in the memory, causing the at least one processor to perform the inertial navigation method as provided by the first aspect.

In a fourth aspect, an embodiment of the present application provides a computer-readable storage medium, where computer-executable instructions are stored in the computer-readable storage medium, and when a processor executes the computer-executable instructions, the inertial navigation provided in the first aspect is implemented method.

In a fifth aspect, an embodiment of the present application provides a computer program product, including a computer program, which, when executed by a processor, implements the inertial navigation method provided in the first aspect.

In the inertial navigation method and device provided by the embodiments of the present application, after the acceleration data of the mobile terminal is obtained by measuring the acceleration measurement device of the mobile terminal, the acceleration data is processed based on the first Kalman filter to obtain the corresponding acceleration data. error data, and use the error data to process the acceleration data to obtain the processed target acceleration data, and then process the above-mentioned target acceleration data based on the second Kalman filter to obtain the velocity data and displacement data of the mobile terminal, wherein, The first Kalman filter is a zero-order Kalman filter. That is, in the embodiment of the present application, after the acceleration data of the mobile terminal is measured and obtained, the first Kalman filter is used to process the acceleration data, so that the error of the acceleration data can be eliminated in real time, and then the second Kalman filter is used to process the acceleration data. The controller calculates the motion data of the mobile terminal, so that the integration error can be eliminated in real time during the inertial navigation process, and the accuracy of the inertial navigation can be improved.

Description of drawings

1 is a schematic structural diagram of an inertial navigation processing system provided in an embodiment of the application;

2 is a schematic flowchart of an inertial navigation method provided in an embodiment of the present application;

3 is a schematic diagram of a curve of acceleration data obtained by measurement in an embodiment of the application and error data processed by the first Kalman filter;

4 is a schematic diagram of the comparison of the speed data curve obtained after being processed by the first Kalman filter and the second Kalman filter simultaneously and the speed data curve obtained without being processed by the first Kalman filter in the embodiment of the application;

Fig. 5 is the comparative schematic diagram of the displacement data curve obtained after the first Kalman filter and the second Kalman filter are processed simultaneously in the embodiment of the application and the displacement data curve obtained without being processed by the first Kalman filter;

6 is a schematic diagram of a program module of an inertial navigation device provided in an embodiment of the application;

FIG. 7 is a schematic diagram of a hardware structure of a mobile terminal provided in an embodiment of the present application.

Detailed ways

In order to make the purposes, technical solutions and advantages of the embodiments of the present application clearer, the technical solutions in the embodiments of the present application will be described clearly and completely below with reference to the drawings in the embodiments of the present application. Obviously, the described embodiments It is a part of the embodiments of the present application, but not all of the embodiments. Based on the embodiments in the present application, all other embodiments obtained by those of ordinary skill in the art without creative work fall within the protection scope of the present application.

Inertial Navigation System (INS), also known as inertial reference system, is an autonomous navigation system that does not rely on external information and does not radiate energy to the outside (such as radio navigation). Its working environment includes not only the air, the ground, but also underwater. The basic working principle of inertial navigation is based on Newton's laws of mechanics. By measuring the acceleration of the mobile terminal in the inertial reference frame, integrating it with time, and transforming it into the navigation coordinate system, the mobile terminal can be obtained. Information such as velocity, yaw angle, and displacement in the navigation coordinate system.

The inertial navigation system belongs to the reckoning navigation method, that is, the position of the next point is calculated from the position of a known point according to the continuously measured heading angle and velocity of the mobile terminal, so the current position of the mobile terminal can be continuously measured. The gyroscope in the inertial navigation system is used to form a navigation coordinate system, so that the measurement axis of the accelerometer is stabilized in the coordinate system, and the heading and attitude angle are given; the accelerometer is used to measure the acceleration of the mobile terminal, after the time The velocity can be obtained by one integration, and the displacement can be obtained by integrating the velocity once over time.

Among them, when the initial value of the above acceleration is not 0, a large error will be introduced in the whole integration process. In the actual system, due to factors such as noise, the mean value of the output value of the accelerometer cannot be 0. To eliminate the integration error, a feasible implementation is to subtract the acceleration initial value from the acceleration value before performing the integration operation, so as to calibrate the acceleration value. However, this method cannot calibrate the integral error introduced in the process of inertial navigation in real time, and the accuracy of long-term navigation work is still poor.

In order to solve the above technical problem, an embodiment of the present application provides an inertial navigation method. After the acceleration data of the mobile terminal is measured, the zero-order Kalman filter is used to eliminate the error of the acceleration data, and then the second Kalman filter is used. The controller calculates the motion data of the mobile terminal, so that the integration error can be eliminated in real time during the inertial navigation process, and the accuracy of the inertial navigation can be improved.

For a better understanding of the embodiments of the present application, refer to FIG. 1 , which is a schematic structural diagram of an inertial navigation processing system provided in the embodiments of the present application. In FIG. 1, after the acceleration data of the mobile terminal is obtained by measurement, the first Kalman filter is used to process the acceleration data to obtain error data corresponding to the acceleration data, and then the above-mentioned acceleration data are processed based on the obtained error data. Then, the processed target acceleration data is processed by the second Kalman filter, and the velocity data and displacement data of the mobile terminal can be obtained. Detailed description is given below by using detailed embodiments.

Referring to FIG. 2 , FIG. 2 is a schematic flowchart of an inertial navigation method provided by an embodiment of the present application, which can be applied to a mobile terminal. In a feasible implementation manner, the above inertial navigation method includes:

S201 , using an acceleration measuring device of a mobile terminal to measure and obtain acceleration data of the mobile terminal.

Wherein, the above-mentioned mobile terminal may be a mobile phone, a vehicle-mounted terminal, a POS machine, or the like.

Optionally, the above acceleration measurement device may use an acceleration sensor. For example, an acceleration sensor composed of a mass block, a damper, an elastic element, a sensitive element and an adaptive circuit can be obtained by using Newton's second law by measuring the inertial force on the mass block during the acceleration process of the mobile terminal. acceleration value. According to different sensitive elements, the types of acceleration sensors that can be used include capacitive acceleration sensors, inductive acceleration sensors, strain-type acceleration sensors, piezoresistive acceleration sensors, piezoelectric acceleration sensors, and the like.

S202. Process the acceleration data based on the first Kalman filter to obtain error data corresponding to the acceleration data.

Among them, the calculation of the Kalman filter is based on the state equation, so the state equation of the inertial navigation system needs to be determined first. In a feasible implementation manner, when establishing the first Kalman filter, the state equation is set as:

X(k+1)=φX(k)+Bu(k)+ΓW(k)

Y(k)=HX(k)+V(k)

Among them, X(k) is the current state, X(k+1) is the state at the next moment, Φ is the transition matrix, B is the control matrix, u is the control quantity, Г is the noise matrix, W is the system noise, and Y is the output quantity, H is the output matrix, and V is the observation noise. That is, the observed output at this moment and the state at the next moment can be obtained from the known state, control quantity and system noise at the current moment.

The above state is the acceleration of the mobile terminal, that is, X(k) in the above formula contains the vector of acceleration:

X(k)=[a(k)]

Among them, the above-mentioned first Kalman filter adopts the zero-order Kalman filter, namely:

a(k+1)=a(k)

in:

Y(k)=[a(k)]

In this embodiment of the present application, after the state equation of the first Kalman filter is determined, the acceleration data can be processed to obtain error data corresponding to the acceleration data.

In order to better understand the embodiments of the present application, in a motion scenario in which the mobile terminal shakes around the origin for a period of time and finally returns to the origin, referring to FIG. 3 , FIG. Schematic diagram of the curve of the error data after Kalman filter processing.

S203. Process the acceleration data based on the above error data to obtain processed target acceleration data.

In the embodiment of the present application, after the error data corresponding to the acceleration data is obtained, calibration processing is performed on the acceleration data based on the error data to obtain processed target acceleration data.

In a feasible implementation manner, the difference between the acceleration data and the error data may be calculated, and the difference may be determined as the target acceleration data.

S204. Process the target acceleration data based on the second Kalman filter to obtain motion data of the mobile terminal.

In a feasible implementation manner, when establishing the second Kalman filter, the state equation is set as:

X(k+1)=φX(k)+Bu(k)+ΓW(k)

Y(k)=HX(k)+V(k)

Among them, X(k) is the current state, X(k+1) is the state at the next moment, Φ is the transition matrix, B is the control matrix, u is the control quantity, Г is the noise matrix, W is the system noise, and Y is the output quantity, H is the output matrix, and V is the observation noise. That is, the observable output at this moment and the state at the next moment can be obtained from the known state, control quantity and system noise at the current moment.

When the above motion data includes velocity and displacement, the above state includes the acceleration, velocity and displacement of the mobile terminal, that is, X(k) includes a vector of acceleration, velocity and displacement:

Similarly:

The state transition matrix Φ is a matrix that expresses the relationship between the state at the next moment and the state at this moment. Assuming that the sampling period T is relatively short, it can be approximated that the acceleration a is almost constant, then:

v(k+1)=0×h(k)+v(k)+a(k)T

a(k+1)=0×h(k)+0×v(k)+a(k)

Convert the above equations into matrix form:

From this we can get the transition matrix Φ as:

In a feasible implementation, if other system noises are not considered, the above state equation can be simplified as:

X(k+1)=φX(k)

Since the above second Kalman filter is only used to process the above acceleration data, therefore:

Y(k)=[a(k)]

In the embodiment of the present application, after the state equation of the second Kalman filter is determined, integral operation can be performed on the above target acceleration data to obtain velocity data and displacement data of the mobile terminal.

In order to better understand the embodiments of the present application, in a motion scenario in which the mobile terminal shakes around the origin for a period of time and finally returns to the origin, refer to FIG. 4 and FIG. 5 . FIG. The comparison diagram of the speed data curve obtained after the filter and the second Kalman filter are processed and the speed data curve obtained without being processed by the first Kalman filter, FIG. 5 is the first Kalman filter in the embodiment of the application. A schematic diagram of the comparison between the displacement data curve obtained after processing with the second Kalman filter and the displacement data curve obtained without the first Kalman filter processing.

It can be seen from Fig. 4 and Fig. 5 that in the motion scene where the mobile terminal shakes around the origin for a period of time and finally returns to the origin, neither the velocity data curve nor the displacement data curve obtained after processing by the first Kalman filter cannot be returned to the original point. The velocity data curve and the displacement data curve obtained after processing by the first Kalman filter can basically be reset to zero, which is more in line with the motion situation of the mobile terminal in the above-mentioned motion scene.

In the inertial navigation method provided by the embodiment of the present application, after the acceleration data of the mobile terminal is measured and obtained, the first Kalman filter is used to process the acceleration data, so that the error of the acceleration data can be eliminated in real time, and then the second Kalman filter is used to process the acceleration data. The Kalman filter calculates the speed data and displacement data of the mobile terminal, so that the integration error can be eliminated in real time during the inertial navigation process, and the accuracy of the inertial navigation can be improved.

Based on the content described in the above embodiments, the embodiments of the present application further provide an inertial navigation device, which is applied to a mobile terminal. Referring to FIG. 6, FIG. 6 is a schematic diagram of a program module of an inertial navigation device provided in an embodiment of the application, and the inertial navigation device 60 includes:

The measurement module 601 is configured to obtain acceleration data of the mobile terminal by measuring the acceleration measurement device of the mobile terminal.

The first processing module 602 is configured to process the acceleration data based on the first Kalman filter to obtain error data corresponding to the acceleration data, and process the acceleration data based on the error data to obtain processed target acceleration data , the above-mentioned first Kalman filter is a zero-order Kalman filter.

The second processing module 603 is configured to process the above-mentioned target acceleration data based on the second Kalman filter to obtain motion data of the mobile terminal.

In the inertial navigation device 60 provided by the embodiment of the present application, after the acceleration data of the mobile terminal is measured and obtained, the first Kalman filter is used to process the acceleration data, which can eliminate the error of the acceleration data in real time, and then use the first Kalman filter to process the acceleration data. The second Kalman filter calculates the motion data of the mobile terminal, so that the integration error can be eliminated in real time during the inertial navigation process, and the accuracy of the inertial navigation can be improved.

In a feasible implementation manner, the above-mentioned first processing module is used for:

In a feasible implementation manner, the above-mentioned second processing module is used for:

In a feasible implementation manner, the above-mentioned second Kalman filter is a second-order Kalman filter.

It should be noted that, for the specific content executed by the measurement module 601 , the first processing module 602 , and the second processing module 603 in the embodiment of the present application, reference may be made to each step in the inertial navigation method described in the above embodiment, and details are not repeated here. .

Further, based on the content described in the foregoing embodiments, an embodiment of the present application further provides a mobile terminal, the mobile terminal includes at least one processor and a memory; wherein, the memory stores computer execution instructions; the above-mentioned at least one processor The computer-executed instructions stored in the memory are executed to implement each step in the inertial navigation method described in the above embodiments, which will not be repeated here.

For a better understanding of the embodiments of the present application, refer to FIG. 7 , which is a schematic diagram of a hardware structure of a mobile terminal according to an embodiment of the present application.

As shown in FIG. 7 , the mobile terminal 70 in this embodiment includes: a processor 701 and a memory 702; wherein:

a memory 702 for storing computer-executed instructions;

The processor 701 is configured to execute the computer-executed instructions stored in the memory, so as to implement each step in the inertial navigation method described in the foregoing embodiments, which will not be repeated here.

Optionally, the memory 702 may be independent or integrated with the processor 701 .

When the memory 702 is provided independently, the device further includes a bus 703 for connecting the memory 702 and the processor 701 .

Further, based on the content described in the foregoing embodiments, the embodiments of the present application further provide a computer-readable storage medium, where computer-executable instructions are stored in the computer-readable storage medium, and when the processor executes the computer-executable instructions In order to implement each step in the inertial navigation method as described in the above embodiment, the details are not repeated here.

Further, based on the content described in the foregoing embodiments, the embodiments of the present application further provide a computer program product, including a computer program, which, when executed by a processor, implements the inertial navigation described in the foregoing embodiments Each step in the method will not be repeated here.

In the several embodiments provided in this application, it should be understood that the disclosed apparatus and method may be implemented in other manners. For example, the device embodiments described above are only illustrative. For example, the division of the modules is only a logical function division. In actual implementation, there may be other division methods. For example, multiple modules may be combined or integrated. to another system, or some features can be ignored, or not implemented. On the other hand, the shown or discussed mutual coupling or direct coupling or communication connection may be through some interfaces, indirect coupling or communication connection of devices or modules, and may be in electrical, mechanical or other forms.

The modules described as separate components may or may not be physically separated, and components shown as modules may or may not be physical units, that is, may be located in one place, or may be distributed to multiple network units. Some or all of the modules may be selected according to actual needs to achieve the purpose of the solution in this embodiment.

In addition, each functional module in each embodiment of the present application may be integrated in one processing unit, or each module may exist physically alone, or two or more modules may be integrated in one unit. The units formed by the above modules can be implemented in the form of hardware, or can be implemented in the form of hardware plus software functional units.

The above-mentioned integrated modules implemented in the form of software functional modules may be stored in a computer-readable storage medium. The above-mentioned software function modules are stored in a storage medium, and include several instructions to enable a computer device (which may be a personal computer, a server, or a network device, etc.) or a processor (English: processor) to execute the various embodiments of the present application. part of the method.

It should be understood that the above-mentioned processor may be a central processing unit (English: Central Processing Unit, referred to as: CPU), or other general-purpose processors, digital signal processors (English: Digital Signal Processor, referred to as: DSP), application-specific integrated circuits (English: Application Specific Integrated Circuit, referred to as: ASIC) and so on. A general purpose processor may be a microprocessor or the processor may be any conventional processor or the like. The steps of the method disclosed in conjunction with the application can be directly embodied as executed by a hardware processor, or executed by a combination of hardware and software modules in the processor.

The memory may include high-speed RAM memory, and may also include non-volatile storage NVM, such as at least one magnetic disk memory, and may also be a U disk, a removable hard disk, a read-only memory, a magnetic disk or an optical disk, and the like.

The bus may be an Industry Standard Architecture (ISA) bus, a Peripheral Component (PCI) bus, or an Extended Industry Standard Architecture (EISA) bus, or the like. The bus can be divided into address bus, data bus, control bus and so on. For convenience of representation, the buses in the drawings of the present application are not limited to only one bus or one type of bus.

The above-mentioned storage medium may be implemented by any type of volatile or non-volatile storage device or a combination thereof, such as static random access memory (SRAM), electrically erasable programmable read only memory (EEPROM), erasable Except programmable read only memory (EPROM), programmable read only memory (PROM), read only memory (ROM), magnetic memory, flash memory, magnetic disk or optical disk. A storage medium can be any available medium that can be accessed by a general purpose or special purpose computer.

An exemplary storage medium is coupled to the processor, such that the processor can read information from, and write information to, the storage medium. Of course, the storage medium can also be an integral part of the processor. The processor and the storage medium may be located in application specific integrated circuits (Application Specific Integrated Circuits, ASIC for short). Of course, the processor and the storage medium may also exist in the electronic device or the host device as discrete components.

Those of ordinary skill in the art can understand that all or part of the steps of implementing the above method embodiments may be completed by program instructions related to hardware. The aforementioned program can be stored in a computer-readable storage medium. When the program is executed, the steps including the above method embodiments are executed; and the aforementioned storage medium includes: ROM, RAM, magnetic disk or optical disk and other media that can store program codes.

Finally, it should be noted that the above embodiments are only used to illustrate the technical solutions of the present application, but not to limit them; although the present application has been described in detail with reference to the foregoing embodiments, those of ordinary skill in the art should understand that: The technical solutions described in the foregoing embodiments can still be modified, or some or all of the technical features thereof can be equivalently replaced; and these modifications or replacements do not make the essence of the corresponding technical solutions deviate from the technical solutions of the embodiments of the present application. scope.

Claims

An inertial navigation method, characterized in that, applied to a mobile terminal, the method comprising:

Using the acceleration measuring device of the mobile terminal to measure the acceleration data of the mobile terminal;

The acceleration data is processed based on the first Kalman filter to obtain error data corresponding to the acceleration data, and the first Kalman filter is a zero-order Kalman filter;

processing the acceleration data based on the error data to obtain processed target acceleration data;

The target acceleration data is processed based on the second Kalman filter to obtain motion data of the mobile terminal.
The method according to claim 1, wherein the processing of the acceleration data based on the error data to obtain the processed target acceleration data comprises:

A difference between the acceleration data and the error data is calculated, and the difference is determined as the target acceleration data.
The method according to claim 1, wherein the processing of the target acceleration data based on the second Kalman filter to obtain the motion data of the mobile terminal comprises:

Integrate the target acceleration data based on the second Kalman filter to obtain velocity data and displacement data of the mobile terminal.
The method according to claim 1 or 3, wherein the second Kalman filter is a second-order Kalman filter.
An inertial navigation device, characterized in that it is applied to a mobile terminal, the device comprising:

a measurement module, configured to obtain acceleration data of the mobile terminal by measuring the acceleration measurement device of the mobile terminal;

The first processing module is configured to process the acceleration data based on the first Kalman filter, obtain error data corresponding to the acceleration data, and process the acceleration data based on the error data to obtain the processed acceleration data. target acceleration data, the first Kalman filter is a zero-order Kalman filter;

The second processing module is configured to process the target acceleration data based on the second Kalman filter to obtain motion data of the mobile terminal.
The apparatus according to claim 5, wherein the first processing module is configured to:

A difference between the acceleration data and the error data is calculated, and the difference is determined as the target acceleration data.
The apparatus according to claim 6, wherein the second processing module is configured to:

Integrate the target acceleration data based on the second Kalman filter to obtain velocity data and displacement data of the mobile terminal.
The apparatus according to claim 5 or 7, wherein the second Kalman filter is a second-order Kalman filter.
A mobile terminal, comprising at least one processor and a memory;

the memory stores computer-executable instructions;

The at least one processor executes the computer-implemented instructions stored in the memory, causing the at least one processor to perform the inertial navigation method of any one of claims 1-4.
A computer-readable storage medium, wherein computer-executable instructions are stored in the computer-readable storage medium, and when a processor executes the computer-executable instructions, the computer-executable instructions according to any one of claims 1 to 4 are implemented. Inertial Navigation Method.
A computer program product, comprising a computer program, characterized in that, when the computer program is executed by a processor, the inertial navigation method according to any one of claims 1 to 4 is implemented.