Robust heading estimation method and device

Abstract

The invention discloses a robust heading estimation method and device. The method comprises the following steps: S1, acquiring carrier attitude estimation information based on inertia and geomagnetic fusion; s2, acquiring course estimation information based on Manhattan scene information assistance; and S3, carrying out fusion according to the carrier attitude estimation information and the course estimation information to obtain final carrier state estimation. By adopting the technical scheme of the invention, the course estimation precision under the magnetic interference condition can be improved, and the adaptive anti-interference fusion of the information of the multi-source sensor is realized.

Description

Technical Field

[0001] The present invention belongs to the field of navigation technology, and in particular relates to a robust heading estimation method and device for a carrier (such as a ground vehicle, an unmanned aerial vehicle, and a ship). Background Art

[0002] Autonomous positioning in satellite-denied environments is an important research direction in the field of navigation technology. Among them, the heading estimation of carriers such as ground vehicles, drones and ships is an important error source that affects the positioning accuracy, and therefore has received widespread attention.

[0003] Magnetic sensors are often used in positioning systems to correct heading. They detect the direction of the earth's magnetic field and provide an absolute heading reference, thereby suppressing heading errors. However, in actual indoor environments, various electronic devices and metal structures of buildings can cause local magnetic field interference, making it difficult to accurately identify the earth's magnetic field. To address this, gyroscope, accelerometer and magnetic sensor information is usually fused to achieve more reliable heading estimation. However, the heading estimation method based on Kalman filter fusion cannot adjust parameter weights online, and its performance cannot meet actual needs. Summary of the invention

[0004] The technical problem to be solved by the present invention is to provide a robust heading estimation method and device, which 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 object, the present invention adopts the following technical solution:

[0006] A robust heading estimation method, comprising:

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

[0008] Step S2, obtaining carrier heading estimation information assisted by Manhattan scene information;

[0009] Step S3: Fusing the carrier attitude estimation information and the carrier heading estimation information to obtain a final carrier state estimation.

[0010] Preferably, the carrier posture estimation information in step S1 is:

[0011]

[0012] in, Among them, g r Represents the gravity vector in the navigation coordinate system, m r represents the magnetic field vector in navigation coordinates, and Represent the estimated values ​​updated using the accelerometer and magnetic sensor, respectively.

[0013] As a preference,

[0014]

[0015] in,

[0016]

[0017] Among them, k fx , k fy , k fz , k mx , k my and k mz Respectively represent the weights of the accelerometer and magnetic sensor on the x, y, and z axes in the carrier coordinate system, represents the gravity vector measured in the carrier coordinate system, represents the magnetic field vector measured in the carrier coordinate system, and They represent the gravity vector and magnetic field vector estimated only by the gyroscope in the carrier coordinate system, and Represent the estimated values ​​updated using the accelerometer and magnetic sensor, respectively.

[0018] Preferably, the carrier heading estimation information in step S2 is: in, Indicates the attitude of the carrier in the navigation coordinate system at time t+1, Indicates the initial posture of the carrier, represents the rotation matrix between the camera and carrier coordinate systems, Represents the rotation matrix from the Manhattan world coordinate system to the camera coordinate system at time t+1, 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 to obtain a final carrier state estimation based on an extended Kalman filter algorithm.

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

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

[0022] The second acquisition module is used to obtain the 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 estimation.

[0024] Preferably, the carrier attitude estimation information is:

[0025]

[0026] in, Among them, g r Represents the gravity vector in the navigation coordinate system, m r represents the magnetic field vector in navigation coordinates, and Represent the estimated values ​​updated using the accelerometer and magnetic sensor, respectively.

[0027] As a preference,

[0028]

[0029] in,

[0030]

[0031] Among them, k fx , k fy , k fz , k mx , k my and k mz Respectively represent the weights of the accelerometer and magnetic sensor on the x, y, and z axes in the carrier coordinate system, represents the gravity vector measured in the carrier coordinate system, represents the magnetic field vector measured in the carrier coordinate system, and They represent the gravity vector and magnetic field vector estimated only by the gyroscope in the carrier coordinate system, and Represent the estimated values ​​updated using the accelerometer and magnetic sensor, respectively.

[0032] Preferably, the carrier heading estimation information is: in, Indicates the attitude of the carrier in the navigation coordinate system at time t+1, Indicates the initial posture of the carrier, represents the rotation matrix between the camera and carrier coordinate systems, express t+1 The rotation matrix from the Manhattan world coordinate system to the camera coordinate system at this moment, 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 estimation based on the extended Kalman filter algorithm.

[0034] The present invention can adaptively fuse inertial and geomagnetic information, and can improve the accuracy of heading estimation under magnetic interference conditions by dynamically adjusting the weights of accelerometers and magnetic sensors. A Manhattan scene plane extraction based on PlaneRecNet is designed, and a heading estimation model for Manhattan scenes is established. The heading error can be effectively corrected using building structure information. A multi-source fusion heading estimation framework based on extended Kalman filtering is constructed to achieve adaptive anti-disturbance fusion of multi-source sensor information. BRIEF DESCRIPTION OF THE DRAWINGS

[0035] In order to more clearly illustrate the embodiments of the present invention or the technical solutions in the prior art, the drawings required for use in the embodiments or the description of the prior art will be briefly introduced below. Obviously, the drawings described below are only embodiments of the present invention. For ordinary technicians in this field, other drawings can be obtained based on the provided drawings without paying creative work.

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

[0037] Figure 2 It is a schematic diagram of the structure of a single-input-single-output neural network;

[0038] Figure 3 This is a flow chart of carrier heading estimation based on Manhattan scene information assistance. DETAILED DESCRIPTION

[0039] The following will be combined with the drawings in the embodiments of the present invention to clearly and completely describe the technical solutions in the embodiments of the present invention. Obviously, the described embodiments are only part of the embodiments of the present invention, not all of the embodiments. Based on the embodiments of the present invention, all other embodiments obtained by ordinary technicians in this field without creative work are within the scope of protection of the present invention.

[0040] In order to make the above-mentioned objects, features and advantages of the present invention more obvious and easy to understand, the present invention is further described in detail below with reference to the accompanying drawings and specific embodiments.

[0041] Embodiment 1:

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

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

[0044] Step S2, obtaining carrier heading estimation information assisted by Manhattan scene information;

[0045] Step S3: Fusing the carrier attitude estimation information and the carrier heading estimation information to obtain a final carrier state estimation.

[0046] As an implementation of an embodiment of the present invention, in step S1, under given initial conditions, attitude estimation is achieved by integrating the measurements of the gyroscope. However, due to the accumulation of integration errors, the overall estimation error accumulates rapidly. In order to compensate for the drift caused by gyroscope integration, the embodiment of the present invention uses gravity and magnetic field strength for compensation. However, linear acceleration and magnetic field interference will affect the accuracy of attitude estimation. Therefore, it is necessary to use smaller weights to ignore the measurements of the accelerometer and magnetic sensor. At the same time, linear acceleration and magnetic field interference are not the same on each axis, and different weights need to be applied 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] Among them, k fx , k fy , k fz , k mx , k my and k mz Respectively represent the weights of the accelerometer and magnetic sensor on the x, y, and z axes in the carrier coordinate system, with a value range of [0,1]. represents the specific force vector measured in the carrier coordinate system, represents the magnetic field vector measured in the carrier coordinate system, and They represent the specific force vector and magnetic field vector estimated only by the gyroscope in the carrier coordinate system, and Represent the estimated values ​​updated using the accelerometer and magnetic sensor, respectively.

[0051] Assume g r Represents the gravity vector in the navigation coordinate system, m r Represents the magnetic field vector in the navigation coordinate system, and defines the right-handed coordinate system t in the navigation coordinate system, which is expressed as:

[0052]

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

[0054] Similarly, the components of the right-handed coordinate system t are expressed in the carrier coordinate system as:

[0055]

[0056] The components of the right-handed 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 is expressed as:

[0059]

[0060] Furthermore, the accelerometer weight K is obtained by weight prediction based on the input enhancement network. f and the magnetic sensor weight K m The embodiment of the present invention designs a single-input-single-output neural network with the same hyperparameters. The neural network structure is as follows: Figure 2 As shown in Figure 2, the neural network receives the measurement residuals of the accelerometer and magnetic sensor as input and predicts the accelerometer weights K for each axis. f and the magnetic sensor weight K m :

[0061]

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

[0063] To enhance the nonlinearity of the input, raise the input scalar to a vector containing the power of the input scalar:

[0064]

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

[0066] The neural network consists of five fully connected layers, each with a size of [16, 32, 64, 32, 1]. By connecting all neurons in the previous layer with all neurons in the current layer, feature transfer and information interaction between different layers are achieved, which helps capture the correlation and dependency between different features. These layers are connected by the tanh activation function, which can increase the representation ability of the model and enable it to learn more complex and nonlinear functional relationships. Since the output of the network is a multiplication gain, its value must be in the range of [0, 1]. Therefore, the Soft Threshold function is used as the activation function of 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 the magnetic sensor in the filter online, and the heading estimation accuracy under magnetic interference conditions can be improved by dynamically adjusting the weights.

[0069] As an implementation of the embodiment of the present invention, in step S2, Figure 3 As shown, the embodiment of the present invention uses PlaneRecNet to realize plane extraction in indoor scenes. The extracted plane can be expressed in the camera coordinate system as follows:

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

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

[0072] The angle θ between the planes can be calculated by the plane normal vector. When the angle between any three planes is 90 degrees, the Manhattan scene is considered to be successfully detected. Based on the normal vectors of the plane features that constitute 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 plane features that make up the Manhattan world scene. If only two mutually perpendicular planes are detected, the normal vector of the third plane is calculated by cross product:

[0075] n3=n1×n2

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

[0077] Since the plane detection order is uncertain between image frames, the dominant direction of the Manhattan world is not fixed, so inertial information is needed to assist in the estimation of the carrier's attitude. Assume that the initial attitude of the carrier is The normal vectors that form the Manhattan world coordinate system are Transform the normal vector from the camera coordinate system to the navigation coordinate system:

[0078]

[0079] in, Represents the rotation matrix between the camera and carrier coordinate systems.

[0080] Assume that the posture of the carrier at time t is The posture estimated by step S1 can obtain the posture of the carrier at time t+1 The normal vector that constitutes the Manhattan world coordinate system at this time Transform to navigation coordinate system:

[0081]

[0082] Calculating the normal vector and If the angle is approximately 0° or 180°, then this set of normal vectors represents the same dominant direction of the Manhattan world. The plane normal vector close to 180° at time t+1 is negated 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 carrier in the navigation coordinate system at time t+1 is obtained:

[0085]

[0086] The attitude information is obtained by estimating the relative rotation of each frame with respect to the Manhattan world coordinate system, so it will not be affected by the attitude of the previous frame, effectively reducing the error accumulation of rotation estimation and improving the heading estimation accuracy.

[0087] The embodiment of the present invention designs a Manhattan scene plane extraction based on PlaneRecNet, and then establishes a heading estimation model for the Manhattan scene; by utilizing the building structure information, the accumulation of heading errors can be effectively suppressed and the accuracy of heading estimation can be improved.

[0088] As an implementation of an embodiment of the present invention, in step S3, the carrier attitude estimation information and the carrier heading estimation information are fused to obtain a final carrier state estimation based on an extended Kalman filter algorithm.

[0089] The extended Kalman filter algorithm can effectively fuse multiple information sources and has strong adaptability and robustness to nonlinear systems. Therefore, the embodiment of the present invention uses the extended Kalman filter to fuse and estimate the heading.

[0090] The embodiment of the present invention constructs an extended Kalman filter whose state quantity is a quaternion, and the state prediction equation can be expressed as:

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

[0092] Among them, x t∣t is the estimated state at time t, x t+1∣t is the predicted state at time t+1. State transfer matrix A t It can be expressed as:

[0093]

[0094] in,

[0095]

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

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

[0098]

[0099] Among them, P t∣t is the forecast covariance at time t, Q t represents the system noise covariance matrix, Γ t is the noise driving matrix.

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

[0101]

[0102] Among them, R t represents the observation noise covariance matrix, the observation matrix H t It can be expressed as:

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

[0104] Using the observed data and the 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 quantity z t+1 Estimated in step S1 and the estimated value in step S3

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

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

[0109] Calculate the state quantity and covariance matrix corresponding to the two observations, and record the updated state quantity as u x t+1|t+1 and m x t+1|t+1 , the covariance matrix is ​​recorded as 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] Among them, the size of the fusion weight can be calculated by the following formula:

[0112]

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

[0114]

[0115] This enables inertial / geomagnetic / visual multi-source fusion heading estimation.

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

[0117] The robust heading estimation method of the embodiment of the present invention firstly realizes heading estimation based on inertial / geomagnetic fusion, and designs weight prediction based on input enhancement network; secondly, establishes a heading estimation model assisted by Manhattan scene information; finally, constructs a multi-source fusion heading estimation framework based on extended Kalman filtering, which can effectively utilize multi-source information and suppress the accumulation of heading errors. The embodiment of the present invention can realize the effective correction of heading errors and improve the reliability of heading estimation under electromagnetic interference.

[0118] Embodiment 2:

[0119] An embodiment of the present invention further provides a robust heading estimation device, comprising:

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

[0121] The second acquisition module is used to obtain the 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 estimation.

[0123] As an implementation method of an embodiment of the present invention, the carrier posture estimation information is:

[0124]

[0125] in, Among them, g r Represents the gravity vector in the navigation coordinate system, m r represents the magnetic field vector in navigation coordinates, and Represent the estimated values ​​updated using the accelerometer and magnetic sensor, respectively.

[0126] As an implementation method of the present invention,

[0127]

[0128] in,

[0129]

[0130] Among them, k fx , k fy , k fz , k mx , k my and k mz Respectively represent the weights of the accelerometer and magnetic sensor on the x, y, and z axes in the carrier coordinate system, represents the gravity vector measured in the carrier coordinate system, represents the magnetic field vector measured in the carrier coordinate system, and They represent the gravity vector and magnetic field vector estimated only by the gyroscope in the carrier coordinate system, and Represent the estimated values ​​updated using the accelerometer and magnetic sensor, respectively.

[0131] As an implementation of an embodiment of the present invention, the carrier heading estimation information is: in, Indicates the attitude of the carrier in the navigation coordinate system at time t+1, Indicates the initial posture of the carrier, represents the rotation matrix between the camera and carrier coordinate systems, Represents the rotation matrix from the Manhattan world coordinate system to the camera coordinate system at time t+1, 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 only descriptions of the preferred embodiments of the present invention and are not intended to limit the scope of the present invention. Without departing from the design spirit of the present invention, various modifications and improvements made to the technical solutions of the present invention by ordinary technicians in this field should all fall within the protection scope determined by the claims of the present invention.

Claims

1. A robust heading estimation method, characterized in that: include: Step S1, obtaining carrier attitude estimation information based on inertial and geomagnetic fusion; 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 a final carrier state estimation.

2. The robust heading estimation method according to claim 1, characterized in that: The carrier posture estimation information in step S1 is: in, Among them, g r Represents the gravity vector in the navigation coordinate system, m r represents the magnetic field vector in navigation coordinates, and Represent the estimated values ​​updated using the accelerometer and magnetic sensor, respectively.

3. The robust heading estimation method according to claim 2, characterized in that: in, Among them, k fx , k fy , k fz , k mx , k my and k mz Respectively represent the weights of the accelerometer and magnetic sensor on the x, y, and z axes in the carrier coordinate system, represents the gravity vector measured in the carrier coordinate system, represents the magnetic field vector measured in the carrier coordinate system, and They represent the gravity vector and magnetic field vector estimated only by the gyroscope in the carrier coordinate system, and Represent the estimated values ​​updated using the accelerometer and magnetic sensor, respectively.

4. The robust heading estimation method according to claim 3, characterized in that: The carrier heading estimation information in step S2 is: in, Indicates the attitude of the carrier in the navigation coordinate system at time t+1, Indicates the initial posture of the carrier, represents the rotation matrix between the camera and carrier coordinate systems, Represents the rotation matrix from the Manhattan world coordinate system to the camera coordinate system at time t+1, Represents the rotation matrix from the Manhattan world coordinate system to the camera coordinate system at the initial moment.

5. The robust heading estimation method according to claim 4, characterized in that: 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 the extended Kalman filter algorithm.

6. A robust heading estimation device, characterized in that: include: A first acquisition module is used to acquire carrier attitude estimation information based on inertial and geomagnetic fusion; The second acquisition module is used to obtain the carrier heading estimation information assisted by Manhattan scene information; The fusion module is used to fuse the carrier attitude estimation information and the carrier heading estimation information to obtain the final carrier state estimation.

7. The robust heading estimation device according to claim 6, characterized in that: The carrier attitude estimation information is: in, Among them, g r Represents the gravity vector in the navigation coordinate system, m r represents the magnetic field vector in navigation coordinates, and Represent the estimated values ​​updated using the accelerometer and magnetic sensor, respectively.

8. The robust heading estimation device according to claim 7, characterized in that: in, Among them, k fx , k fy , k fz , k mx , k my and k mz Respectively represent the weights of the accelerometer and magnetic sensor on the x, y, and z axes in the carrier coordinate system, represents the gravity vector measured in the carrier coordinate system, represents the magnetic field vector measured in the carrier coordinate system, and They represent the gravity vector and magnetic field vector estimated only by the gyroscope in the carrier coordinate system, and Represent the estimated values ​​updated using the accelerometer and magnetic sensor, respectively.

9. The robust heading estimation device according to claim 8, characterized in that: The carrier heading estimation information is: in, Indicates the attitude of the carrier in the navigation coordinate system at time t+1, Indicates the initial posture of the carrier, represents the rotation matrix between the camera and carrier coordinate systems, Represents the rotation matrix from the Manhattan world coordinate system to the camera coordinate system at time t+1, Represents the rotation matrix from the Manhattan world coordinate system to the camera coordinate system at the initial moment.

10. The robust heading estimation device according to claim 9, characterized in that: The fusion module is used to fuse the carrier attitude estimation information and the carrier heading estimation information, and obtain the final carrier state estimation based on the extended Kalman filter algorithm.

Images