Navigation heading angle calculation method, device, terminal and medium based on GNSS and IMU combination

Abstract

This application provides a method, apparatus, terminal, and medium for calculating navigation heading angle based on a combination of GNSS and IMU, including: acquiring the real-time heading angle and real-time forward speed of a mobile device at the current moment to construct a motion model of the mobile device; obtaining the state-space equation of the motion model based on the state transition matrix, velocity increment matrix, and white noise sequence; obtaining a Kalman filter measurement model based on the state-space equation of the motion model and using GNSS navigation and positioning results as observation vectors; and obtaining the optimal navigation heading angle at the current moment based on the Kalman filter measurement model, Kalman gain, GNSS navigation and positioning results, and IMU kinematic calculation results. This invention effectively controls the influence of large GNSS noise, and the use of incremental data to calculate prior values ​​also avoids the cumulative divergence phenomenon of IMU random drift errors.

Description

Technical Field

[0001] This application relates to the field of navigation and positioning technology, and in particular to a method, device, terminal and medium for calculating navigation heading angle based on a combination of GNSS and IMU. Background Technology

[0002] In intelligent lawnmower robots, the robot's position in its coordinate system and its heading angle are two key pieces of information for its movement control. Methods for measuring the robot's heading angle mainly include magnetoresistive sensors, dual-antenna GNSS positioning and orientation, single-antenna GNSS positioning and orientation, and gyroscope-based heading measurement. Magnetoresistive sensors are greatly affected by the surrounding magnetic field environment, resulting in low accuracy and reliability, and are easily affected in operating environments with magnetic field interference, such as those near high-voltage lines. Dual-antenna GNSS positioning systems rely on a large distance between the two antennas to achieve high measurement accuracy, and have poor dynamic response characteristics and high costs. Single-antenna GNSS positioning systems can output heading angle information, but have high random noise, which increases with lower speeds. Gyroscope-based heading measurement requires heading angle initialization, and random drift errors can accumulate and diverge. The heading angles measured by these methods cannot adequately meet the accuracy requirements of intelligent lawnmowers. Summary of the Invention

[0003] In view of the shortcomings of the prior art described above, the purpose of this application is to provide a navigation heading angle calculation method, device, terminal and medium based on GNSS and IMU combination, so as to solve the problem that the navigation angle measurement method in the prior art cannot meet the accuracy requirements of intelligent lawnmowers.

[0004] To achieve the above and other related objectives, a first aspect of this application provides a navigation heading angle calculation method based on a combination of GNSS and IMU, comprising: acquiring the real-time heading angle and real-time forward speed of a mobile device at the current moment to construct a motion model of the mobile device; obtaining the state space equation of the motion model based on a state transition matrix, a velocity increment matrix, and a white noise sequence; obtaining a Kalman filter measurement model based on the state space equation of the motion model and using GNSS navigation and positioning results as observation vectors; and obtaining the optimal navigation heading angle at the current moment based on the Kalman filter measurement model, Kalman gain, GNSS navigation and positioning results, and IMU kinematic calculation results.

[0005] In some embodiments of the first aspect of this application, the motion model of the mobile device is constructed by: establishing the motion equations of the mobile device in a two-dimensional plane coordinate system based on the real-time heading angle and real-time forward speed of the mobile device at the current moment and the dead reckoning algorithm.

[0006] In some embodiments of the first aspect of this application, the state-space equation of the motion model can be expressed as:

[0007] X k =A k X k-1 +b k +u k ; where X k =[x ck ,y ck ,ε Ψk ] represents the state-space vector at time k; X k-1 A represents the state-space vector at time (k-1); k b represents the state transition matrix at time k; k This represents the velocity increment matrix at time k; u k It is a white noise sequence of the state transition equation.

[0008] In some embodiments of the first aspect of this application, the system process noise covariance matrix is ​​further calculated based on the white noise sequence of the state transition equation to evaluate the error of the state space equation.

[0009] In some embodiments of the first aspect of this application, the Kalman filter measurement model is represented as:

[0010] Z gk =H k X k +V k Among them, Z gk For GNSS navigation and positioning results; H k V is the measurement matrix of the Kalman filter at time k; k This refers to the random positioning error of GNSS positioning in the horizontal coordinate system.

[0011] In some embodiments of the first aspect of this application, the optimal navigation heading angle is expressed as:

[0012] in, Let be the optimal navigation heading angle at time k; The kinematics of the IMU at time k; K k Z is the Kalman gain at time k; gk For GNSS positioning results; H kLet be the measurement matrix of the Kalman filter at time k.

[0013] In some embodiments of the first aspect of this application, the IMU kinematic calculation result is positively correlated with the state transition matrix and the optimal navigation heading angle of the previous time step, and is expressed as follows: in, The IMU kinematics calculation results at time k; A k-1 Let (k-1) be the state transition matrix; The optimal navigation heading angle at time (k-1); the Kalman gain is expressed as: in, H is the predicted value of the system process noise at time k; k T R is the transpose of the measurement matrix of the Kalman filter at time k; k Let be the noise variance matrix of the measurement vector at time k.

[0014] In some embodiments of the first aspect of this application, the GNSS positioning result is represented as: Z gk =H k X k +V k Among them, Z gk For GNSS navigation and positioning results; H k V is the measurement matrix of the Kalman filter at time k; k This refers to the random positioning error of GNSS positioning in the horizontal coordinate system.

[0015] To achieve the above and other related objectives, a second aspect of this application provides a navigation heading angle calculation device based on a combination of GNSS and IMU, comprising: a motion model module for acquiring the real-time heading angle and real-time forward speed of a mobile device at the current moment to construct a motion model of the mobile device; a state space equation module for obtaining the state space equation of the motion model based on a state transition matrix, a velocity increment matrix, and a white noise sequence; a Kalman filter measurement model module for obtaining a Kalman filter measurement model based on the state space equation of the motion model and using GNSS navigation and positioning results as observation vectors; and an optimal navigation heading angle module for obtaining the optimal navigation heading angle at the current moment based on the Kalman filter measurement model, Kalman gain, GNSS navigation and positioning results, and IMU kinematic calculation results.

[0016] To achieve the above and other related objectives, a third aspect of this application provides a computer-readable storage medium having a computer program stored thereon, which, when executed by a processor, implements the navigation heading angle calculation method based on a combination of GNSS and IMU.

[0017] To achieve the above and other related objectives, a fourth aspect of this application provides an electronic terminal, comprising: a processor and a memory; the memory is used to store a computer program, and the processor is used to execute the computer program stored in the memory, so that the terminal executes the navigation heading angle calculation method based on GNSS and IMU combination.

[0018] As described above, the navigation heading angle calculation method, device, terminal and medium based on GNSS and IMU combination of this application have the following beneficial effects: The navigation heading angle calculation method based on GNSS and IMU combination provided by the invention can effectively control the influence of large GNSS noise, and at the same time, the use of incremental data to calculate the prior value also avoids the cumulative divergence phenomenon of IMU random drift error. Attached Figure Description

[0019] Figure 1 The diagram shown is a flowchart of a navigation heading angle calculation method based on a combination of GNSS and IMU in one embodiment of this application.

[0020] Figure 2 The diagram shows a comparison of three heading angle curves obtained by fusing GNSS, IMU, and Kalman filtering in one embodiment of this application.

[0021] Figure 3 The diagram shown is a structural schematic of an electronic terminal according to an embodiment of this application.

[0022] Figure 4 The diagram shown is a schematic representation of a navigation heading angle calculation device based on a combination of GNSS and IMU in one embodiment of this application. Detailed Implementation

[0023] The following specific examples illustrate the implementation of this application. Those skilled in the art can easily understand other advantages and effects of this application from the content disclosed in this specification. This application can also be implemented or applied through other different specific embodiments, and various details in this specification can also be modified or changed based on different viewpoints and applications without departing from the spirit of this application. It should be noted that, unless otherwise specified, the following embodiments and features in the embodiments can be combined with each other.

[0024] It should be noted that in the following description, as used herein, the singular forms “a,” “an,” and “the” are intended to include the plural forms as well, unless the context indicates otherwise. The terms “first,” “second,” “third,” “fourth,” etc. (if present) in the specification, claims, and accompanying drawings are used to distinguish similar objects and are not necessarily used to describe a particular order or sequence. It should be understood that such data used can be interchanged where appropriate so that the embodiments described herein can be implemented in orders other than those illustrated or described herein. Furthermore, the terms “comprising,” “including,” indicate the presence of the stated features, operations, elements, components, items, kinds, and / or groups, but do not exclude the presence, occurrence, or addition of one or more other features, operations, elements, components, items, kinds, and / or groups. It should be further understood that the terms “or” and “and / or” as used herein are interpreted as inclusive, or mean any one or any combination thereof. Thus, “A, B, or C” or “A, B, and / or C” means “any one of: A; B; C; A and B; A and C; B and C; A, B, and C.” An exception to this definition will only occur if the combination of elements, functions, or operations is inherently mutually exclusive in some way.

[0025] To address the problems mentioned above, this invention provides a navigation heading angle calculation method, device, terminal, and medium based on a combination of GNSS and IMU. It aims to measure the heading angle of a lawnmower robot by combining single-antenna GNSS real-time relative positioning (RTK) and IMU (Inertial Measurement Unit) through a fusion algorithm. A Kalman filter-based heading angle fusion algorithm for lawnmower robots is proposed. This algorithm integrates and accumulates the GNSS positioning coordinates (after Gaussian projection) and the angular velocity measured by the IMU to obtain a more accurate heading angle. Therefore, this invention overcomes the problems of high random noise in GNSS heading angle calculation and accumulated errors in IMU calculation.

[0026] Before providing a further detailed description of the present invention, the nouns and terms used in the embodiments of the present invention are explained, and the nouns and terms used in the embodiments of the present invention are subject to the following interpretations:

[0027] <1> GNSS (Global Navigation Satellite System): A space-based radio navigation and positioning system that provides users with all-weather three-dimensional coordinates, velocity, and time information at any location on the Earth's surface or in near-Earth space.

[0028] <2> IMU (Inertial Measurement Unit): An inertial measurement unit is used to measure an object's three-axis attitude angles (or angular rates) and acceleration. Typically, gyroscopes and accelerometers are the main components of an IMU, and their accuracy directly affects the accuracy of the inertial system. Generally, an IMU contains three single-axis accelerometers and three single-axis gyroscopes. The accelerometers detect the object's acceleration signals along three independent axes in the carrier coordinate system, and the gyroscopes detect the carrier's angular velocity signals relative to the navigation coordinate system. By measuring the object's angular velocity and acceleration in three-dimensional space, the object's attitude can be calculated.

[0029] This invention provides a navigation heading angle calculation method based on a combination of GNSS and IMU, a system for executing the navigation heading angle calculation method based on a combination of GNSS and IMU, and a storage medium storing an executable program for implementing the navigation heading angle calculation method based on a combination of GNSS and IMU. Regarding the implementation of the navigation heading angle calculation method based on a combination of GNSS and IMU, this invention will describe exemplary implementation scenarios of the navigation heading angle calculation based on a combination of GNSS and IMU.

[0030] like Figure 1 The diagram illustrates a flowchart of a navigation heading angle calculation method based on a combination of GNSS and IMU according to an embodiment of the present invention. The navigation heading angle calculation method in this embodiment mainly includes the following steps:

[0031] Step S11: Obtain the real-time heading angle and real-time forward speed of the mobile device at the current moment to construct the motion model of the mobile device.

[0032] In this embodiment, the real-time heading angle is the superposition result of the heading angle measurement value, the random measurement error of the heading angle, and the heading angle measurement deviation value; the real-time forward speed is the superposition result of the forward speed measurement value and the random measurement error of the forward speed.

[0033] For example, let Ψ′k be the real-time heading angle of the mobile device at time k, and v′ be the real-time forward speed of the mobile device. k ;

[0034] Ψ′ k =Ψ k +ε Ψk +ξ Ψk ; Formula (1)

[0035] v′ k =v k +ξ vk ; Formula (2)

[0036] Among them, Ψ k vk These represent the measured values ​​of heading angle and forward speed, respectively; ξ Ψk ξ vk ε represents the random measurement errors of the heading angle and forward speed, respectively; Ψk This indicates the measurement deviation value of the heading angle.

[0037] In some examples, the motion model of the mobile device is constructed by: establishing the motion equations of the mobile device in a two-dimensional plane coordinate system based on the real-time heading angle and real-time forward speed of the mobile device at the current moment, and using dead reckoning. The motion equations are as follows:

[0038] x ck =x ck-1 +v′ k cos(Ψ k +ε Ψk )dt; Formula (3)

[0039] y ck =y ck-1 +v′ k sin(Ψ k +ε Ψk )dt; Formula (4)

[0040] Where, x ck y ck The x represents the Gaussian projection plane coordinates of the device body at time k; ck-1 y ck-1 dt represents the Gaussian projection plane coordinates of the device body at time (k-1); dt represents the dead reckoning time interval.

[0041] It should be noted that dead reckoning (DR) is a process of calculating the current position by using the known position, combined with the speed and bearing of movement. Specifically, it is a method of calculating the position at the next moment by measuring the distance and bearing of movement, given the current position.

[0042] Furthermore, substituting formulas (1) and (2) into formulas (3) and (4), and eliminating the higher-order infinitesimal terms when the error approaches zero, we obtain:

[0043] x ck =x ck-1 +v k cosΨ k dt+ε Ψk cosΨ k dt-v k ε Ψk sinΨ k dt; Formula (5)

[0044] yck =y ck-1 +v k sinΨ k dt+ε Ψk sinΨ k dt+v k ε Ψk cosΨ k dt; Formula (6)

[0045] Step S12: Obtain the state-space equation of the motion model based on the state transition matrix, velocity increment matrix, and white noise sequence.

[0046] In some examples, the state-space equations of the motion model can be expressed as:

[0047] X k =A k X k-1 +b k +u k ; Formula (7)

[0048] Among them, X k =[x ck ,y ck ,ε Ψk ] represents the state-space vector at time k; X k-1 A represents the state-space vector at time (k-1); k b represents the state transition matrix at time k; k This represents the velocity increment matrix at time k; u k It is a white noise sequence of the state transition equation. It should be noted that a white noise sequence refers to the actual sample data of a white noise process, often simply called white noise. The characteristics of a white noise sequence are that the random variables at any two points in time are uncorrelated, and there are no usable dynamic patterns in the sequence; therefore, historical data cannot be used to predict or infer the future.

[0049]

[0050]

[0051] u k =[0,0,ε Ψk ];

[0052] In some examples, the navigation heading angle calculation method based on GNSS and IMU combination also includes calculating the system process noise covariance matrix based on white noise sequence of state transition equations to evaluate the error of the state space equations.

[0053] Specifically, the system process noise covariance matrix is ​​expressed as: Q = u k *uk T ;u k It is a white noise sequence of the state transition equation, u k T is u k The transpose of .

[0054] Step S13: Based on the state-space equation of the motion model, and using the GNSS navigation and positioning results as the observation vector, obtain the Kalman filter measurement model.

[0055] In this embodiment, the Kalman filter measurement model is represented as:

[0056] Z gk =H k X k +V k ; Formula (8)

[0057] Among them, Z gk For GNSS navigation and positioning results; H k V is the measurement matrix of the Kalman filter at time k; k This represents the random positioning error of GNSS positioning in the horizontal coordinate system; the specific expressions for each parameter in the above formula are as follows:

[0058] x gk y gk Indicates the positioning coordinates of the GNSS antenna;

[0059]

[0060] ε gxk ε gyk This represents the random positioning error of GNSS positioning in the horizontal coordinate system.

[0061] Step S14: Based on the Kalman filter measurement model, Kalman gain, GNSS navigation and positioning results, and IMU kinematic calculation results, obtain the optimal navigation heading angle at the current moment.

[0062] In this embodiment, the optimal navigation heading angle is expressed as:

[0063]

[0064] in, Let be the optimal navigation heading angle at time k; The prediction result at time k is the result of kinematic extrapolation using incremental data measured by the IMU; K k Z is the Kalman gain at time k; gkFor GNSS positioning results; H k Let be the measurement matrix of the Kalman filter at time k.

[0065] In some examples, the IMU kinematic calculation results are positively correlated with the state transition matrix and the optimal navigation heading angle of the previous time step, specifically expressed as follows:

[0066]

[0067] in, The IMU kinematics calculation results at time k; A k-1 Let (k-1) be the state transition matrix; Let be the optimal navigation heading angle at time (k-1).

[0068] In some examples, the Kalman gain is expressed as:

[0069]

[0070] in, H is the predicted value of the system process noise at time k; k T R is the transpose of the measurement matrix of the Kalman filter at time k; k Let be the noise variance matrix of the measurement vector at time k; further:

[0071] Predicted value of system process noise at time k It is represented as:

[0072]

[0073] Among them, A k-1 P is the state transition matrix at time (k-1); k-1 A is the correction value for the system process noise at time (k-1); k-1 T Let A be the state transition matrix. k-1 The transpose of is ; Q is the system process noise covariance matrix.

[0074]

[0075] in, The random positioning error ε of GNSS positioning in the horizontal coordinate system is respectively. gxk ε gyk The variance statistics.

[0076] Furthermore, to calculate the optimal navigation heading angle at time (k+1), this embodiment also provides the following correction value for the system process noise at time k:

[0077]

[0078] Among them, P k K represents the correction value for the system process noise at time k; k H is the Kalman gain at time k; k Let be the measurement matrix of the Kalman filter at time k; Let k be the predicted value of the system process noise at time k.

[0079] Further, initialization Based on the positioning accuracy specifications of GNSS and IMU products, construct constant matrices R and Q, initialize P0 as the identity matrix, and then solve the prediction equations to obtain... Assign the current GNSS position data to the value Z. k Substituting into the correction equations, the Kalman gain value K is obtained. k and the optimal estimate Calculate P k Data is prepared for noise estimation at the next moment, and this process is iterated periodically.

[0080] From formulas (11) and (9), it can be seen that when R = 0, When R approaches infinity Therefore, the navigation heading angle calculation method based on GNSS and IMU combination provided by this invention can effectively control the influence of large GNSS noise, and the use of incremental data to calculate prior values ​​also avoids the cumulative divergence phenomenon of IMU random drift error.

[0081] To facilitate understanding, we will now combine Figure 2 The three heading angle comparison curves obtained after fusing GNSS, IMU, and Kalman filtering are shown as follows: GNSS-based navigation heading angle curve 21, IMU-based navigation heading angle curve 22, and GNSS and IMU combined navigation heading angle curve 23.

[0082] It can be seen that the unprocessed GNSS-based navigation heading angle curve 21 shows significant fluctuations in the heading angle measured by GNSS positioning. After Kalman filtering fusion, the divergence of the IMU accumulated heading is effectively suppressed, reducing errors caused by zero bias and random drift. The fused heading angle curve maintains both the overall trend of GNSS heading and the detailed trend of gyroscope heading.

[0083] It is worth emphasizing that some existing navigation and positioning technologies usually rely on video data, such as VIO (visual inertial odometry) combined with IMU and GNSS data for coordinate positioning. However, VIO calculates mileage data by collecting video data and IMU data through a camera. The camera is greatly affected by the outdoor environment, and the error is large after combining it with IMU data. Therefore, it is far from the technical solution of this invention.

[0084] The navigation heading angle calculation method based on GNSS and IMU combination provided in this invention can be implemented on the terminal side or the server side. For the hardware structure of the navigation heading angle calculation terminal based on GNSS and IMU combination, please refer to [link to relevant documentation]. Figure 3 This is a schematic diagram of an optional hardware structure of a navigation heading angle calculation terminal 300 based on a GNSS and IMU combination provided in an embodiment of the present invention. The terminal 300 can be a mobile phone, computer device, tablet device, personal digital processing device, factory back-end processing device, etc. The navigation heading angle calculation terminal 300 based on a GNSS and IMU combination includes: at least one processor 301, a memory 302, at least one network interface 304, and a user interface 306. The various components in the device are coupled together through a bus system 305. It is understood that the bus system 305 is used to realize the connection and communication between these components. In addition to a data bus, the bus system 305 also includes a power bus, a control bus, and a status signal bus. However, for clarity, in... Figure 3 The general will label all buses as bus systems.

[0085] The user interface 306 may include a monitor, keyboard, mouse, trackball, clicker, button, touchpad, or touch screen.

[0086] It is understood that memory 302 can be volatile memory or non-volatile memory, or both. Non-volatile memory can be read-only memory (ROM) or programmable read-only memory (PROM), used as an external cache. By way of example, but not limitation, many forms of RAM are available, such as static random access memory (SRAM) and synchronous static random access memory (SSRAM). The memories described in the embodiments of this invention are intended to include, but are not limited to, these and any other suitable categories of memory.

[0087] In this embodiment of the invention, the memory 302 is used to store various types of data to support the operation of the navigation heading angle calculation terminal 300 based on GNSS and IMU combination. Examples of this data include: any executable program for operation on the navigation heading angle calculation terminal 300 based on GNSS and IMU combination, such as operating system 3021 and application program 3022; operating system 3021 includes various system programs, such as framework layer, core library layer, driver layer, etc., for implementing various basic services and handling hardware-based tasks. Application program 3022 may include various applications, such as media player, browser, etc., for implementing various application services. The implementation of the navigation heading angle calculation method based on GNSS and IMU combination provided in this embodiment of the invention can be included in application program 3022.

[0088] The methods disclosed in the above embodiments of the present invention can be applied to processor 301, or implemented by processor 301. Processor 301 may be an integrated circuit chip with signal processing capabilities. In the implementation process, each step of the above method can be completed by the integrated logic circuit of the hardware in processor 301 or by instructions in the form of software. The processor 301 may be a general-purpose processor, a digital signal processor (DSP), or other programmable logic devices, discrete gate or transistor logic devices, discrete hardware components, etc. Processor 301 can implement or execute the methods, steps, and logic block diagrams disclosed in the embodiments of the present invention. General-purpose processor 301 may be a microprocessor or any conventional processor, etc. The steps of the accessory optimization method provided in the embodiments of the present invention can be directly reflected as being executed by a hardware decoding processor, or being executed by a combination of hardware and software modules in the decoding processor. The software module may be located in a storage medium, which is located in memory. The processor reads the information in the memory and combines it with its hardware to complete the steps of the aforementioned method.

[0089] In an exemplary embodiment, the navigation heading angle calculation terminal 300 based on the combination of GNSS and IMU can be used by one or more application-specific integrated circuits (ASICs), DSPs, programmable logic devices (PLDs), or complex programmable logic devices (CPLDs) to perform the aforementioned method.

[0090] like Figure 4The diagram illustrates a schematic of a navigation heading angle calculation device based on a combination of GNSS and IMU according to an embodiment of the present invention. In this embodiment, the navigation heading angle calculation device 400 based on a combination of GNSS and IMU includes: a motion model module 401, a state-space equation module 402, a Kalman filter measurement model module 403, and an optimal navigation heading angle module 404.

[0091] The motion model module 401 is used to obtain the real-time heading angle and real-time forward speed of the mobile device at the current moment in order to construct the motion model of the mobile device.

[0092] In some examples, the motion model module 401 constructs the motion model of the mobile device by: establishing the motion equations of the mobile device in a two-dimensional plane coordinate system based on the real-time heading angle and real-time forward speed of the mobile device at the current moment and the dead reckoning algorithm.

[0093] The state-space equation of the motion model can be expressed as:

[0094] X k =A k X k-1 +b k +u k ; where X k =[x ck ,y ck ,ε Ψk ] represents the state-space vector at time k; X k-1 A represents the state-space vector at time (k-1); k b represents the state transition matrix at time k; k This represents the velocity increment matrix at time k; u k It is a white noise sequence of the state transition equation.

[0095] The state space equation module 402 is used to obtain the state space equation of the motion model based on the state transition matrix, the velocity increment matrix and the white noise sequence.

[0096] Furthermore, it also includes calculating the system process noise covariance matrix based on the white noise sequence of the state transition equation to evaluate the error of the state space equation.

[0097] The Kalman filter measurement model module 403 is used to obtain the Kalman filter measurement model based on the state-space equation of the motion model and using the GNSS navigation and positioning results as the observation vector.

[0098] In some examples, the Kalman filter measurement model is represented as:

[0099] Zgk =H k X k +V k Among them, Z gk For GNSS navigation and positioning results; H k V is the measurement matrix of the Kalman filter at time k; k This refers to the random positioning error of GNSS positioning in the horizontal coordinate system.

[0100] The optimal navigation heading angle module 404 is used to obtain the optimal navigation heading angle at the current moment based on the Kalman filter measurement model, Kalman gain, GNSS navigation and positioning results and IMU kinematic calculation results.

[0101] In some examples, the optimal navigation heading angle is expressed as:

[0102] in, Let be the optimal navigation heading angle at time k; The kinematics of the IMU at time k; K k Z is the Kalman gain at time k; gk For GNSS positioning results; H k Let be the measurement matrix of the Kalman filter at time k.

[0103] The IMU kinematic calculation results are positively correlated with the state transition matrix and the optimal navigation heading angle of the previous time step, and are expressed as: in, The IMU kinematics calculation results at time k; A k-1 Let (k-1) be the state transition matrix; The optimal navigation heading angle at time (k-1); the Kalman gain is expressed as: in, H is the predicted value of the system process noise at time k; k T R is the transpose of the measurement matrix of the Kalman filter at time k; k Let be the noise variance matrix of the measurement vector at time k.

[0104] The GNSS positioning result is represented as: Z gk =H k X k +V k Among them, Z gk For GNSS navigation and positioning results; H k V is the measurement matrix of the Kalman filter at time k; k This refers to the random positioning error of GNSS positioning in the horizontal coordinate system.

[0105] It should be noted that the navigation heading angle calculation device based on GNSS and IMU combination provided in the above embodiments is only illustrated by the division of the above program modules when performing navigation heading angle calculation based on GNSS and IMU combination. In practical applications, the above processing can be assigned to different program modules as needed, that is, the internal structure of the device can be divided into different program modules to complete all or part of the processing described above. In addition, the navigation heading angle calculation device based on GNSS and IMU combination provided in the above embodiments and the navigation heading angle calculation method embodiment based on GNSS and IMU combination belong to the same concept. The specific implementation process is detailed in the method embodiment, and will not be repeated here.

[0106] The present invention also provides a computer-readable storage medium having a computer program stored thereon, wherein the computer program, when executed by a processor, implements the navigation heading angle calculation method based on the combination of GNSS and IMU.

[0107] Those skilled in the art will understand that all or part of the steps of the above-described method embodiments can be implemented using computer program-related hardware. The aforementioned computer program can be stored in a computer-readable storage medium. When executed, the program performs the steps of the above-described method embodiments; and the aforementioned storage medium includes various media capable of storing program code, such as ROM, RAM, magnetic disks, or optical disks.

[0108] In the embodiments provided in this application, the computer-readable and writable storage medium may include read-only memory, random access memory, EEPROM, CD-ROM or other optical disc storage devices, disk storage devices or other magnetic storage devices, flash memory, USB flash drive, portable hard drive, or any other medium capable of storing desired program code in the form of instructions or data structures and accessible by a computer. Additionally, any connection may be appropriately referred to as a computer-readable medium. For example, if instructions are transmitted from a website, server, or other remote source using coaxial cable, fiber optic cable, twisted pair, digital subscriber line (DSL), or wireless technologies such as infrared, radio, and microwave, then the coaxial cable, fiber optic cable, twisted pair, DSL, or wireless technologies such as infrared, radio, and microwave are included in the definition of the medium. However, it should be understood that computer-readable and writable storage media and data storage media do not include connections, carrier waves, signals, or other transient media, but are intended for non-transient, tangible storage media. The disks and optical discs used in the application include compact discs (CDs), laser discs, optical discs, digital multifunction discs (DVDs), floppy disks, and Blu-ray discs, where disks typically copy data magnetically, while optical discs use lasers to copy data optically.

[0109] In summary, this application provides a navigation heading angle calculation method, apparatus, terminal, and medium based on a combination of GNSS and IMU. The navigation heading angle calculation method based on a combination of GNSS and IMU provided by the invention can effectively control the influence of large GNSS noise, and at the same time, the use of incremental data to calculate prior values ​​avoids the cumulative divergence phenomenon caused by IMU random drift errors. Therefore, this application effectively overcomes the various shortcomings of the prior art and has high industrial application value.

[0110] The above embodiments are merely illustrative of the principles and effects of this application and are not intended to limit this application. Any person skilled in the art can modify or alter the above embodiments without departing from the spirit and scope of this application. Therefore, all equivalent modifications or alterations made by those skilled in the art without departing from the spirit and technical concept disclosed in this application should still be covered by the claims of this application.

Claims

1. A method for calculating navigation heading angle based on a combination of GNSS and IMU, characterized in that, include: Obtain the real-time heading angle and real-time forward speed of the mobile device at the current moment to construct a motion model of the mobile device; The state-space equation of the motion model is obtained based on the state transition matrix, velocity increment matrix, and white noise sequence; the state-space equation of the motion model is expressed as: ； in, , represents the state-space vector at time k; This represents the state-space vector at time (k-1). This represents the state transition matrix at time k; This represents the velocity increment matrix at time k; It is a white noise sequence of the state transition equation; ； ； ； in, These represent the measured values ​​of heading angle and forward speed, respectively. dt represents the measurement deviation of the heading angle; dt represents the dead reckoning time interval. Based on the state-space equations of the motion model, and using the GNSS navigation and positioning results as observation vectors, a Kalman filter measurement model is obtained. Based on the Kalman filter measurement model, Kalman gain, GNSS navigation and positioning results, and IMU kinematic calculation results, the optimal navigation heading angle at the current moment is obtained.

2. The navigation heading angle calculation method based on GNSS and IMU combination according to claim 1, characterized in that, It also includes calculating the system process noise covariance matrix based on the white noise sequence of the state transition equation to evaluate the error of the state space equation.

3. The navigation heading angle calculation method based on GNSS and IMU combination according to claim 1, characterized in that, The Kalman filter measurement model is expressed as: ； in, GNSS navigation and positioning results; Let be the measurement matrix of the Kalman filter at time k; This refers to the random positioning error of GNSS positioning in the horizontal coordinate system.

4. The navigation heading angle calculation method based on GNSS and IMU combination according to claim 1, characterized in that, The optimal navigation heading angle is expressed as: ； in, Let be the optimal navigation heading angle at time k; The kinematics calculation results of the IMU at time k; Let K be the Kalman gain at time k; GNSS navigation and positioning results; Let be the measurement matrix of the Kalman filter at time k.

5. The navigation heading angle calculation method based on GNSS and IMU combination according to claim 4, characterized in that, include: The IMU kinematic calculation results are positively correlated with the state transition matrix and the optimal navigation heading angle of the previous time step, and are expressed as: ;in, The kinematics calculation results of the IMU at time k; Let (k-1) be the state transition matrix; The optimal navigation heading angle at time (k-1); The Kalman gain is expressed as: ;in, Let k be the predicted value of the system process noise at time k; It is the transpose of the measurement matrix of the Kalman filter at time k; Let be the noise variance matrix of the measurement vector at time k; The GNSS navigation and positioning results are represented as follows: ;in, GNSS navigation and positioning results; Let be the measurement matrix of the Kalman filter at time k; This refers to the random positioning error of GNSS positioning in the horizontal coordinate system.

6. A navigation heading angle calculation device based on a combination of GNSS and IMU, characterized in that, include: The motion model module is used to obtain the real-time heading angle and real-time forward speed of the mobile device at the current moment in order to construct the motion model of the mobile device. The state-space equation module is used to obtain the state-space equation of the motion model based on the state transition matrix, velocity increment matrix, and white noise sequence; the state-space equation of the motion model is expressed as: ； in, , represents the state-space vector at time k; This represents the state-space vector at time (k-1). This represents the state transition matrix at time k; This represents the velocity increment matrix at time k; It is a white noise sequence of the state transition equation; ； ； ； in, These represent the measured values ​​of heading angle and forward speed, respectively. dt represents the measurement deviation of the heading angle; dt represents the dead reckoning time interval. The Kalman filter measurement model module is used to obtain the Kalman filter measurement model based on the state-space equation of the motion model and using the GNSS navigation and positioning results as the observation vector. The optimal navigation heading angle module is used to obtain the optimal navigation heading angle at the current moment based on the Kalman filter measurement model, Kalman gain, GNSS navigation and positioning results, and IMU kinematic calculation results.

7. A computer-readable storage medium having a computer program stored thereon, characterized in that, When the computer program is executed by the processor, it implements the navigation heading angle calculation method based on the combination of GNSS and IMU as described in any one of claims 1 to 5.

8. An electronic terminal, characterized in that, include: Processor and memory; The memory is used to store computer programs; The processor is used to execute the computer program stored in the memory to enable the terminal to perform the navigation heading angle calculation method based on the combination of GNSS and IMU as described in any one of claims 1 to 5.

Images