A robust heading estimation method and apparatus

Abstract

This invention discloses a robust heading estimation method and apparatus, comprising: step S1, acquiring carrier attitude estimation information based on inertial and geomagnetic fusion; step S2, acquiring heading estimation information assisted by Manhattan scene information; and step S3, fusing the carrier attitude estimation information and heading estimation information to obtain the final carrier state estimate. By employing the technical solution of this invention, the heading estimation accuracy under magnetic interference conditions can be improved, and adaptive anti-interference fusion of multi-source sensor information can be achieved.

Description

Technical Field

[0001] This invention belongs to the field of navigation technology, and specifically relates to a robust heading estimation method and apparatus for carriers (such as ground vehicles, drones, and ships). Background Technology

[0002] Autonomous positioning in satellite-denied environments is an important research direction in the field of navigation technology. Among these, course estimation for ground vehicles, drones, and ships is a significant source of error affecting positioning accuracy, and therefore has attracted widespread attention.

[0003] Magnetic sensors are frequently used in positioning systems to correct heading. They provide an absolute heading reference by detecting the direction of the Earth's magnetic field, thereby suppressing heading errors. However, in real-world indoor environments, various electronic devices and the metal structures of buildings can cause localized magnetic field interference, making accurate identification of the Earth's magnetic field difficult. To address this, information from gyroscopes, accelerometers, and magnetic sensors is typically fused to achieve more reliable heading estimates. However, heading estimation methods based on Kalman filter fusion cannot adjust parameter weights online, and their performance cannot meet practical requirements. Summary of the Invention

[0004] The technical problem to be solved by the present invention is to provide a robust heading estimation method and apparatus that can improve the heading estimation accuracy under magnetic interference conditions and realize adaptive anti-interference fusion of multi-source sensor information.

[0005] To achieve the above objectives, the present invention adopts the following technical solution:

[0006] A robust heading estimation method includes:

[0007] Step S1: Obtain carrier attitude estimation information based on inertial and geomagnetic fusion;

[0008] Step S2: Obtain vehicle heading estimation information assisted by Manhattan scene information;

[0009] Step S3: The carrier attitude estimation information and the carrier heading estimation information are fused to obtain the final carrier state estimation.

[0010] Preferably, the carrier attitude estimation information in step S1 is as follows:

[0011]

[0012] in, Among them, g r m represents the gravity vector in the navigation coordinate system. r This represents the magnetic field vector in navigation coordinates. and These represent the estimates updated using the accelerometer and the magnetometer, respectively.

[0013] As a preferred option

[0014]

[0015] in,

[0016]

[0017] Where, k fx k fy k fz k mx k my and k mz These represent the weights of the accelerometer and the magnetic sensor on the x, y, and z axes in the carrier coordinate system, respectively. This represents the gravity vector measured in the carrier coordinate system. This represents the magnetic field vector measured in the carrier coordinate system. and These represent the gravity vector and magnetic field vector estimated using only the gyroscope in the carrier coordinate system, respectively. and These represent the estimates updated using the accelerometer and the magnetometer, respectively.

[0018] Preferably, the carrier heading estimation information in step S2 is as follows: in, This represents the attitude of the vehicle in the navigation coordinate system at time t+1. This indicates the initial attitude of the carrier. This represents the rotation matrix between the camera and the carrier coordinate system. This represents the rotation matrix from the Manhattan world coordinate system to the camera coordinate system at time t+1. This represents the rotation matrix from the Manhattan world coordinate system to the camera coordinate system at the initial moment.

[0019] Preferably, in step S3, the carrier attitude estimation information and the carrier heading estimation information are fused together, and the final carrier state estimation is obtained based on the extended Kalman filter algorithm.

[0020] The present invention also provides a robust heading estimation device, comprising:

[0021] The first acquisition module is used to acquire carrier attitude estimation information based on the fusion of inertial and geomagnetic data;

[0022] The second acquisition module is used to acquire carrier heading estimation information assisted by Manhattan scene information;

[0023] The fusion module is used to fuse the carrier attitude estimation information and the carrier heading estimation information to obtain the final carrier state estimate.

[0024] As a preferred option, the carrier attitude estimation information is as follows:

[0025]

[0026] in, Among them, g r m represents the gravity vector in the navigation coordinate system. r This represents the magnetic field vector in navigation coordinates. and These represent the estimates updated using the accelerometer and the magnetometer, respectively.

[0027] As a preferred option

[0028]

[0029] in,

[0030]

[0031] Where, k fx k fy k fz k mx k my and k mz These represent the weights of the accelerometer and the magnetic sensor on the x, y, and z axes in the carrier coordinate system, respectively. This represents the gravity vector measured in the carrier coordinate system. This represents the magnetic field vector measured in the carrier coordinate system. and These represent the gravity vector and magnetic field vector estimated using only the gyroscope in the carrier coordinate system, respectively. and These represent the estimates updated using the accelerometer and the magnetometer, respectively.

[0032] As a preferred option, the carrier heading estimation information is as follows: in, This represents the attitude of the vehicle in the navigation coordinate system at time t+1. This indicates the initial attitude of the carrier. This represents the rotation matrix between the camera and the carrier coordinate system. express t+1 The rotation matrix from the Manhattan world coordinate system to the camera coordinate system at time. This represents the rotation matrix from the Manhattan world coordinate system to the camera coordinate system at the initial moment.

[0033] Preferably, the fusion module is used to fuse the carrier attitude estimation information and the carrier heading estimation information, and obtain the final carrier state estimate based on the extended Kalman filter algorithm.

[0034] This invention adaptively fuses inertial and geomagnetic information, improving heading estimation accuracy under magnetic interference conditions by dynamically adjusting the weights of accelerometers and magnetic sensors. A Manhattan scene plane extraction model based on PlaneRecNet is designed, establishing a heading estimation model for the Manhattan scene. Building structure information effectively corrects heading errors. A multi-source fusion heading estimation framework based on extended Kalman filtering is constructed to achieve adaptive and interference-resistant fusion of multi-source sensor information. Attached Figure Description

[0035] To more clearly illustrate the technical solutions in the embodiments of the present invention or the prior art, the drawings used in the description of the embodiments or the prior art will be briefly introduced below. Obviously, the drawings described below are only embodiments of the present invention. For those skilled in the art, other drawings can be obtained based on the provided drawings without creative effort.

[0036] Figure 1 This is a flowchart illustrating the robust heading estimation method according to an embodiment of the present invention;

[0037] Figure 2 This is a schematic diagram of a single-input single-output neural network.

[0038] Figure 3 This is a flowchart of a vehicle heading estimation process based on Manhattan scene information. Detailed Implementation

[0039] The technical solutions of the embodiments of the present invention will be clearly and completely described below with reference to the accompanying drawings. Obviously, the described embodiments are only some embodiments of the present invention, and not all embodiments. Based on the embodiments of the present invention, all other embodiments obtained by those skilled in the art without creative effort are within the scope of protection of the present invention.

[0040] To make the above-mentioned objects, features and advantages of the present invention more apparent and understandable, the present invention will be further described in detail below with reference to the accompanying drawings and specific embodiments.

[0041] Example 1:

[0042] like Figure 1 As shown, this embodiment of the invention provides a robust heading estimation method, including:

[0043] Step S1: Obtain carrier attitude estimation information based on inertial and geomagnetic fusion;

[0044] Step S2: Obtain vehicle heading estimation information assisted by Manhattan scene information;

[0045] Step S3: The carrier attitude estimation information and the carrier heading estimation information are fused to obtain the final carrier state estimation.

[0046] In one embodiment of the present invention, in step S1, attitude estimation is achieved by integrating the measurements from the gyroscope under given initial conditions. However, due to the accumulation of integration errors, the overall estimation error accumulates rapidly. To compensate for the drift caused by gyroscope integration, this embodiment of the present invention utilizes gravity and magnetic field strength for compensation. However, linear acceleration and magnetic field disturbances can affect the accuracy of attitude estimation. Therefore, it is necessary to use smaller weights to ignore the measurements from the accelerometer and magnetic sensor. Furthermore, linear acceleration and magnetic field disturbances are not the same on each axis, requiring different weights for compensation. During the state update process, the gravity vector and magnetic field vector estimated by the gyroscope are used for correction:

[0047]

[0048] in:

[0049]

[0050] Where, k fx k fy k fz k mx k my and k mz These represent the weights of the accelerometer and the magnetic sensor on the x, y, and z axes in the carrier coordinate system, respectively, with values ​​ranging from [0,1]. This represents the specific force vector measured in the carrier coordinate system. This represents the magnetic field vector measured in the carrier coordinate system. and These represent the specific force vector and magnetic field vector estimated using only the gyroscope in the carrier coordinate system, respectively. and These represent the estimates updated using the accelerometer and the magnetometer, respectively.

[0051] Let g r m represents the gravity vector in the navigation coordinate system. r The magnetic field vector in the navigation coordinate system is represented by a right-handed coordinate system t, as follows:

[0052]

[0053] In this system, the x-component direction of the right-hand coordinate system t is defined as the direction of the gravity vector, the y-component direction of the right-hand coordinate system t is defined as the direction perpendicular to both the gravity vector and the magnetic field vector, and the z-component direction of the right-hand coordinate system t is obtained using the right-hand coordinate system.

[0054] Similarly, the components of the right-hand coordinate system t are represented in the carrier coordinate system as follows:

[0055]

[0056] The components of the right-hand coordinate system t form the columns of the rotation matrix:

[0057]

[0058] Therefore, the carrier attitude information estimated using the updated gravity vector and magnetic field vector can be expressed as:

[0059]

[0060] Furthermore, the accelerometer weights K are obtained through weight prediction based on the input augmentation network. f And magnetic sensor weight K m This invention designs a single-input single-output neural network with the same hyperparameters, the neural network structure as follows: Figure 2 As shown, the neural network receives the measurement residuals from the accelerometer and magnetometer as input and predicts the accelerometer weights K for each axis. f And magnetic sensor weight K m :

[0061]

[0062] Among them, Res f and Res m These represent the measurement residuals of the accelerometer and the magnetic sensor, respectively. and These represent neural networks that predict the weight values ​​of the accelerometer and the magnetometer, respectively.

[0063] To enhance the nonlinearity of the input, the input scalar is increased to a vector containing powers of the input scalar:

[0064]

[0065] Where the subscript i represents x, y, and z respectively. p represents the minimum power and P represents the maximum power, set to -3 and 5 respectively, then the input dimension...

[0066] This neural network comprises five fully connected layers, each with a size of [16, 32, 64, 32, 1]. By connecting all neurons in the previous layer to all neurons in the current layer, feature transfer and information interaction between different layers are achieved, which helps capture the correlations and dependencies between different features. These layers are connected via the tanh activation function, which increases the model's representational power, enabling it to learn more complex and non-linear functional relationships. Since the network's output is a multiplicative gain, its value must be in the range [0, 1]. Therefore, the Soft Threshold function is used as the activation function for the output layer, expressed as:

[0067] ST(x)=tanh((x-0.5)×5)×0.5+0.5

[0068] The neural network is used to update the weights of the accelerometer and magnetometer in the filter online. By dynamically adjusting the weights, the heading estimation accuracy under magnetic interference conditions can be improved.

[0069] As one embodiment of the present invention, in step S2, as follows: Figure 3 As shown, this embodiment of the invention utilizes PlaneRecNet to extract planes in indoor scenes. The extracted planes can be represented in the camera coordinate system as follows:

[0070] h c =(n c ,d c ) T =(n1,n2,n3,d) c ) T

[0071] Where, n c d represents the normal vector of the plane. c This represents the vertical distance from the plane to the origin of the camera coordinate system.

[0072] The angle θ between planes can be calculated using the plane normal vectors. When the angle between any three planes is 90 degrees, the Manhattan scene is considered successfully detected. Based on the normal vectors of the planar features constituting the Manhattan world scene, the rotation matrix from the Manhattan world coordinate system to the camera coordinate system is calculated:

[0073]

[0074] Where n1, n2, and n3 represent the normal vectors of the three planar features constituting the Manhattan world scene. If only two mutually perpendicular planes are detected, the normal vector of the third plane is calculated using the cross product:

[0075] n3 = n1 × n2

[0076] If only one plane is detected, the Manhattan world hypothesis is considered invalid.

[0077] Because the planar detection sequence is uncertain between image frames, the dominant direction of the Manhattan world is not fixed. Therefore, inertial information is needed to assist in the carrier's attitude estimation. Let the carrier's initial attitude be... The normal vector constituting the Manhattan world coordinate system is Transform the normal vector from the camera coordinate system to the navigation coordinate system:

[0078]

[0079] in, This represents the rotation matrix between the camera and the carrier coordinate system.

[0080] Let the attitude of the carrier at time t be... The attitude of the carrier at time t+1 can be obtained by estimating the attitude in step S1. The normal vector that forms the Manhattan world coordinate system at this moment Transform to navigation coordinate system:

[0081]

[0082] Calculate the normal vector and If the angle between the two normal vectors is approximately 0° or 180°, then this set of normal vectors represents the same dominant direction in the Manhattan world. Invert the plane normal vectors that are close to 180° at time t+1 to obtain the rotation matrix from the Manhattan world coordinate system to the camera coordinate system at time t+1:

[0083]

[0084] Then, the attitude of the vehicle in the navigation coordinate system at time t+1 is obtained:

[0085]

[0086] This attitude information is obtained by estimating the relative rotation of each frame relative to the Manhattan world coordinate system. Therefore, it is not affected by the attitude of the previous frame, effectively reducing the accumulation of rotation estimation errors and improving the accuracy of heading estimation.

[0087] This invention designs a Manhattan scene plane extraction based on PlaneRecNet, and then establishes a heading estimation model for the Manhattan scene; by utilizing building structure information, it can effectively suppress the accumulation of heading errors and improve the accuracy of heading estimation.

[0088] In one embodiment of the present invention, in step S3, the carrier attitude estimation information and the carrier heading estimation information are fused together, and the final carrier state estimation is obtained based on the extended Kalman filter algorithm.

[0089] The Extended Kalman Filter (EKF) algorithm can effectively fuse multiple information sources and has strong adaptability and robustness to nonlinear systems. Therefore, this embodiment of the invention utilizes the EKF algorithm to estimate the heading.

[0090] This invention constructs an extended Kalman filter with quaternions as state variables, and the state prediction equation can be expressed as:

[0091] x t+1∣t =A t x t∣t

[0092] Where, x t∣t Let x be the estimated state at time t. t+1∣t Let A be the predicted state at time t+1. The state transfer matrix is ​​A. t It can be represented as:

[0093]

[0094] in,

[0095]

[0096] in, It represents the angular velocity in the carrier coordinate system.

[0097] The prediction covariance equation can be expressed as:

[0098]

[0099] Among them, P t∣t Let Q be the predicted covariance at time t. t Γ represents the system noise covariance matrix. t This is the noise-driven matrix.

[0100] The Kalman gain equation can be expressed as:

[0101]

[0102] Among them, R t Let H represent the observation noise covariance matrix. t It can be represented as:

[0103] H t =[I 4×4 ]

[0104] Using the observed data and Kalman gain to update the state estimate, the state update equation can be expressed as:

[0105] x t+1|t+1 =x t+1|t +K t (z t+1 -H t x t+1|t )

[0106] Among them, the observation z t+1 Estimated for step S1 And the estimated in step S3

[0107] The covariance update equation can be expressed as:

[0108] P t+1|t+1 =(IK t H t )P t+1 / t

[0109] Calculate the state variables and covariance matrix corresponding to the two observations, and denote the updated state variables as follows: u x t+1|t+1 and m x t+1|t+1 The covariance matrices are denoted as follows: u P t+1|t+1 and m P t+1|t+1 The fused state estimation result can be expressed as:

[0110] x t+1|t+1 =w1* m x t+1|t+1 +w2* u x t+1|t+1

[0111] The fusion weights can be calculated using the following formula:

[0112]

[0113] The updated covariance matrix can be represented as:

[0114]

[0115] This enables multi-source fusion of inertial, geomagnetic, and visual heading estimation.

[0116] This invention achieves adaptive anti-disturbance fusion of multi-source sensor information based on the extended Kalman filter algorithm; by reliably fusing sensor information, the robustness of heading estimation can be improved and drift error can be compensated.

[0117] This invention presents a robust heading estimation method. First, heading estimation is achieved based on inertial / geomagnetic fusion, while simultaneously designing weight prediction based on an input enhancement network. Second, a heading estimation model assisted by Manhattan scene information is established. Finally, a multi-source fusion heading estimation framework based on extended Kalman filtering is constructed, which can effectively utilize multi-source information and suppress the accumulation of heading errors. This invention can effectively correct heading errors and improve the reliability of heading estimation under electromagnetic interference.

[0118] Example 2:

[0119] This invention also provides a robust heading estimation device, comprising:

[0120] The first acquisition module is used to acquire carrier attitude estimation information based on the fusion of inertial and geomagnetic data;

[0121] The second acquisition module is used to acquire carrier heading estimation information assisted by Manhattan scene information;

[0122] The fusion module is used to fuse the carrier attitude estimation information and the carrier heading estimation information to obtain the final carrier state estimate.

[0123] As one embodiment of the present invention, the carrier attitude estimation information is as follows:

[0124]

[0125] in, Among them, g r m represents the gravity vector in the navigation coordinate system. r This represents the magnetic field vector in navigation coordinates. and These represent the estimates updated using the accelerometer and the magnetometer, respectively.

[0126] As one embodiment of the present invention

[0127]

[0128] in,

[0129]

[0130] Where, k fx k fy k fz k mx k my and k mz These represent the weights of the accelerometer and the magnetic sensor on the x, y, and z axes in the carrier coordinate system, respectively. This represents the gravity vector measured in the carrier coordinate system. This represents the magnetic field vector measured in the carrier coordinate system. and These represent the gravity vector and magnetic field vector estimated using only the gyroscope in the carrier coordinate system, respectively. and These represent the estimates updated using the accelerometer and the magnetometer, respectively.

[0131] As one embodiment of the present invention, the carrier heading estimation information is as follows: in, This represents the attitude of the vehicle in the navigation coordinate system at time t+1. This indicates the initial attitude of the carrier. This represents the rotation matrix between the camera and the carrier coordinate system. This represents the rotation matrix from the Manhattan world coordinate system to the camera coordinate system at time t+1. This represents the rotation matrix from the Manhattan world coordinate system to the camera coordinate system at the initial moment.

[0132] The embodiments described above are merely preferred embodiments of the present invention and are not intended to limit the scope of the present invention. Various modifications and improvements made by those skilled in the art to the technical solutions of the present invention without departing from the spirit of the present invention should fall within the protection scope defined by the claims of the present invention.

Claims

1. A robust heading estimation method, characterized in that, Comprising: Step S1, obtaining carrier attitude estimation information based on fusion of inertial and geomagnetic; Step S2, obtaining carrier heading estimation information assisted by Manhattan scene information; Step S3, fusing the carrier attitude estimation information and the carrier heading estimation information to obtain the final carrier state estimation; The carrier attitude estimation information in step S1 is: wherein, wherein, g r denotes the gravity vector in the navigation coordinate system, m r denotes the magnetic field vector in the navigation coordinate, and denote the estimates updated with the accelerometer and the magnetic sensor, respectively.

2. The robust heading estimation method of claim 1, wherein, wherein, where k fx , k fy , k fz , k mx , k my and k mz represent the weights of the accelerometer and the magnetic sensor in the x, y, z axes of the body frame, represents the gravity vector measured in the body frame, represents the magnetic field vector measured in the body frame, and represent the gravity vector and the magnetic field vector estimated using only the gyroscope in the body frame, and represent the estimates updated using the accelerometer and the magnetic sensor, respectively.

3. The robust heading estimation method of claim 2, wherein, The carrier heading estimation information in step S2 is: wherein, denotes the attitude of the carrier in the navigation coordinate system at time t+1, denotes the attitude of the carrier at the initial time, denotes the rotation matrix between the camera and the carrier coordinate system, denotes the rotation matrix from the Manhattan world coordinate system to the camera coordinate system at time t+1, denotes the rotation matrix from the Manhattan world coordinate system to the camera coordinate system at the initial time.

4. The robust heading estimation method of claim 3, wherein, In step S3, the carrier attitude estimation information and the carrier heading estimation information are fused to obtain the final carrier state estimation based on an extended Kalman filtering algorithm.

5. A robust heading estimation apparatus characterized by comprising: Comprising: A first acquisition module for acquiring carrier attitude estimation information based on fusion of inertial and geomagnetic; A second acquisition module for acquiring carrier heading estimation information assisted by Manhattan scene information; A fusion module for fusing the carrier attitude estimation information and the carrier heading estimation information to obtain the final carrier state estimation; The carrier attitude estimation information is: wherein, wherein, g r denotes the gravity vector in the navigation coordinate system, m r denotes the magnetic field vector in the navigation coordinate, and denote the estimates updated with the accelerometer and the magnetic sensor, respectively.

6. The robust heading estimation device of claim 5, wherein, wherein, where k fx , k fy , k fz , k mx , k my and k mz represent the weights of the accelerometer and the magnetic sensor in the x, y, z axes of the body frame, represents the gravity vector measured in the body frame, represents the magnetic field vector measured in the body frame, and represent the gravity vector and the magnetic field vector estimated using only the gyroscope in the body frame, and represent the estimates updated using the accelerometer and the magnetic sensor, respectively.

7. The robust heading estimation apparatus of claim 6, wherein The carrier heading estimation information is: wherein, denotes the attitude of the carrier in the navigation coordinate system at time t+1, denotes the attitude of the carrier at the initial time, denotes the rotation matrix between the camera and the carrier coordinate system, denotes the rotation matrix from the Manhattan world coordinate system to the camera coordinate system at time t+1, denotes the rotation matrix from the Manhattan world coordinate system to the camera coordinate system at the initial time.

8. The robust heading estimation apparatus of claim 7, wherein, The fusion module is configured to fuse the carrier attitude estimation information and the carrier heading estimation information to obtain the final carrier state estimation based on an extended Kalman filtering algorithm.

Images