Unmanned vehicle attitude and position calculation method and system based on integrated navigation technology

Abstract

The invention provides an unmanned vehicle attitude and position calculation method based on an integrated navigation technology, and the method comprises the following steps: S1, determining initialization parameters based on inertial navigation, magnetometer and Beidou navigation data, and outputting the initialization parameters, the initialization parameters including IMU initial zero offset, a rotation matrix of a vehicle body and a navigation coordinate system, and a true north navigation direction angle; s2, collecting asynchronous data of the sensor through a software polling mechanism and buffer management; the sensor comprises an IMU (Inertial Measurement Unit), a magnetometer and a Beidou navigator; and S3, after motion constraint and filtering preprocessing are carried out on the asynchronous data of the sensor output in the S2, data fusion is realized through a prediction process and a correction process of an ESKF algorithm in combination with initialization parameters, and an optimal pose estimation result of the unmanned vehicle pose is output. According to the invention, the data of the IMU, the magnetometer and the Beidou navigation module are fused, the functions of high-frequency motion sensing, azimuth calibration, absolute positioning and the like are realized, and the limitation of a single navigation means is solved through ESKF algorithm fusion.

Description

TECHNICAL FIELD

[0001] The present application relates to the technical field of unmanned vehicle navigation, in particular, and more particularly to an unmanned vehicle attitude and position solving method based on combined navigation technology. BACKGROUND

[0002] Unmanned vehicle navigation is a core technology supporting the realization of autonomous path planning, accurate positioning and safe driving of unmanned vehicles. Its core goal is to obtain the attitude (such as pitch angle, roll angle and yaw angle) and position information of the unmanned vehicle in real time and accurately in a complex dynamic environment, and to provide reliable spatial state basis for the vehicle decision control system. With the rapid development of intelligent transportation and automatic driving technology, the application scenarios of unmanned vehicles have expanded from open roads to complex environments such as urban streets, tunnels and shaded roads, which puts higher requirements on the accuracy, stability and anti-interference ability of the navigation system. The performance of the navigation technology directly determines the safety and practicality of the autonomous driving of the unmanned vehicle.

[0003] Currently, unmanned vehicle navigation technology mainly relies on single navigation means or a combination of multiple navigation means. Among them, inertial navigation technology is a commonly used autonomous navigation method. Through the accelerometer and gyroscope in the inertial measurement unit (IMU), the acceleration and angular velocity data of the vehicle are collected in real time, and the position, speed and attitude information of the vehicle are obtained by integral operation. It has the advantages of high short-time positioning accuracy and immunity to external signal interference. Satellite navigation technology (such as Beidou navigation and GPS) belongs to non-autonomous navigation. It realizes all-weather and large-range absolute positioning by receiving satellite signals and can provide long-term stable position reference, which is an important means for unmanned vehicles to obtain global position information. In addition, some schemes will use the combination mode of "IMU + satellite navigation" or introduce visual sensors, laser radars and other devices to supplement and improve the reliability of navigation through multi-sensor data.

[0004] However, the existing unmanned vehicle navigation method still has obvious deficiencies. On the one hand, a single navigation method has limitations. Although inertial navigation has strong anti-interference ability, the initial zero offset and temperature drift of the IMU device will cause the integrated error to accumulate over time, and the positioning accuracy will decrease rapidly after long-term use. Satellite navigation can provide global positioning, but the signal is easily affected by factors such as high-rise building shielding, tunnel blocking, and electromagnetic interference, and in complex environments, signal loss or instability may occur, leading to positioning interruption. The existing combined navigation scheme also has defects. In complex terrain environments, satellite signal loss can easily lead to positioning information interruption. This defect cannot be compensated for without adding additional sensors, and can only be solved by error control means. The introduction of visual sensors or laser radars not only has high computational complexity and requires a large amount of CPU resources, increasing the difficulty of system deployment, but also is easily affected by light intensity, weather conditions (such as rain, snow and fog), and the cost of high-precision laser radars is high, limiting their application in low-end unmanned vehicles. At the same time, the inconsistency of coordinate systems between multiple sensors, zero offset and noise interference reduce data quality, further leading to large fusion errors of multi-source data and affecting the accuracy of navigation results. SUMMARY

[0005] Therefore, the purpose of the present application is to provide an unmanned vehicle attitude and position solving method based on combined navigation technology to solve the problems of single navigation method limitations, poor multi-sensor data fusion effect and navigation interruption caused by Beidou signal failure.

[0006] The technical means adopted by the present application are as follows: An unmanned vehicle attitude and position solving method based on combined navigation technology comprises the following steps: S1, determining initialization parameters based on inertial navigation, magnetometer and Beidou navigation data, and outputting the initialization parameters, wherein the initialization parameters include IMU initial zero offset, rotation matrix of the vehicle body and navigation coordinate system, and true north heading angle; S2, collecting asynchronous data of sensors through software polling mechanism and buffer management, wherein the sensors include IMU, magnetometer and Beidou navigation; S3, after motion constraint and filtering preprocessing of the asynchronous data of sensors output by S2, combining the initialization parameters to realize data fusion through the prediction process and correction process of ESKF algorithm, and outputting the optimal unmanned vehicle attitude and position estimation result of the unmanned vehicle attitude and position.

[0007] Further, S1 comprises the following steps: S11, calculating the IMU initial zero offset; placing the IMU accelerometer and gyroscope at rest on a horizontal plane, measuring the body acceleration by the IMU accelerometer, and measuring the body angular velocity The initial zero bias of the IMU accelerometer in the body coordinate system is obtained based on the body acceleration and body angular velocity. and the initial zero bias of the gyroscope The formula is as follows: (1) (2) Where g is the magnitude of the local gravitational acceleration; k It refers to the first t k time; S12. Establish the rotation matrices for the body coordinate system and the navigation coordinate system; set the navigation coordinate system as the ENU coordinate system, aligning the z-axis of the body coordinate system with the z-axis of the navigation coordinate system, and calculate the true north azimuth using a magnetometer and BeiDou navigation system. The rotation matrix is ​​obtained based on the true north azimuth. The formula is as follows: (3) S13. Calibrate the true north azimuth; calculate the geographic true north azimuth using the magnetometer and BeiDou navigation fusion; compensate the magnetometer data based on pitch and roll angles, and project the magnetometer data from the body coordinate system to the navigation coordinate system to obtain the magnetic force component on the horizontal plane. and The magnetic north azimuth angle is obtained based on the magnetic force component on the horizontal plane. The formula is as follows: (4) Subtract the local magnetic declination fixed amount from the magnetic north azimuth. Obtain the true north azimuth. ; Furthermore, S2 includes the following steps: In the main loop, the polling period is set to 1ms. The system monitors the readiness status of new data from each sensor in real time by comparing timestamps. When the polling period is triggered, the system extracts the ready sensor data and inputs it into the corresponding iteration process of the fusion algorithm. Each FIFO circular buffer configured for each sensor is associated with a timestamp storing the corresponding data. The buffer management rules include overwriting the first data if the buffer is full when new data is written, and retrieving the most recent valid data based on the timestamp when reading data.

[0008] Furthermore, in S3, the preprocessing includes the following steps: constraining the lateral acceleration of the autonomous vehicle to be less than 5 m / s². 2 When the lateral acceleration exceeds the limit, anti-rollover intervention is triggered; the vertical acceleration of the body is filtered by a low-pass filter with a cutoff frequency of 5Hz to retain low-frequency motion acceleration and filter out high-frequency vibration noise.

[0009] Furthermore, in S3, the state variables estimated by the ESKF algorithm include the actual states. x nominal state and error state ; The true state is the actual state of the system in the physical world; the nominal state is the best estimated state maintained internally by the algorithm and updated through IMU data integration prediction; the error state represents the difference between the true state and the nominal state, and the error state is explicitly estimated and used to correct the nominal state. Real state x nominal state and error state The composition is shown in the following formula: (5) in, p For location, v For speed, q It is a quaternion. For accelerometer zero bias and This is for zero bias of the gyroscope.

[0010] Furthermore, in S3, the ESKF algorithm prediction process includes the following steps: Predict the nominal state; acquire data from the IMU's accelerometer and gyroscope. k The measured values ​​at each time point are used to update the nominal state through inertial integration and first-order Euler integration, yielding prior estimates of the state variables. The nominal state update formula is as follows: (6) In the formula, It is a mapping from quaternions to rotation matrices; Prior estimates for predicting error states based on error state equations The formula is as follows: (7) In the formula, Here is the error state transition matrix. B To control the input matrix, The covariance matrix of the IMU measurement noise is: Q ; and B The expression is as follows: (8) (9) Estimating the covariance of the prior estimate of the error state The formula is as follows: (10) where, is the current time error covariance matrix, Q is the process noise covariance matrix, expressed as: (11) where, is the standard deviation of accelerometer noise, is the standard deviation of gyroscope noise, is the standard deviation of the rate of change of the acceleration error bias, is the standard deviation of the rate of change of the gyroscope error bias.

[0011] Further, in S3, the ESKF algorithm correction process includes the following steps: According to the Beidou navigation information, the composition of the observation is set as Therefore, the true observation value is expressed as: (12) where, is the nonlinear term, is the observation noise, and the covariance matrix is R ; For the observation matrix H is the Jacobian matrix of the observation equation with respect to the error state , the calculation needs to use the chain rule of matrix differentiation, and the specific calculation formula is as follows: (13) where, the Jacobian matrix of the observation function with respect to the true state and the Jacobian matrix of the true state with respect to the error state are derived as follows: (14) (15) The expression of is as follows: (16) Calculate the Kalman filter parameters K : (17) Combined with the prior estimate of the state variable, update the optimal estimate k of the error state at time , and the formula is as follows: (18) The error state is injected into the nominal state to obtain the real state at the time of the optimal estimation k +1 The formula is as follows: (19) The real state covariance matrix at the time of +1 is updated, and the formula is as follows: k The formula is as follows: (20) Further, the method further comprises the following steps: S4, BeiDou navigation failure fault tolerance processing; when BeiDou navigation information is invalid, the prior estimation result of the pose information of the base state variable is combined with zero speed detection and dynamic zero bias calibration to output a short-time pose estimation result, and navigation continuity is maintained.

[0012] Further, the BeiDou navigation information effective threshold is that the horizontal positioning accuracy factor is less than 4; when the horizontal positioning accuracy factor is greater than 4, it is determined that the BeiDou information is invalid, and the fault tolerance mechanism is triggered.

[0013] Further, the fault tolerance mechanism is as follows: Differential motion detection; the current three-axis acceleration modulus value and the differential value are calculated as a motion intensity index, when the motion intensity index is continuously lower than a threshold value for 60 frames, the dynamic correction mechanism is triggered; Stationary locking and dynamic zero bias calibration; the current three-axis velocity is forced to be zero, the three-axis angular velocity is forced to be zero, a flag bit is set to enter a zero speed locking state, during which the velocity is continuously forced to be zero until the acceleration modulus differential value rises to be higher than the threshold value; and the average value of the acceleration of each axis in 60 stable stationary signals is taken as a new zero point bias, and the acceleration data after real-time compensation is compensated.

[0014] Compared with the prior art, the present application has the following advantages: The present application effectively solves the limitations of a single navigation method by combining IMU, a magnetometer and BeiDou navigation, and realizing sensor data fusion according to the ESKF algorithm; The present application improves sensor data quality, reduces invalid calculations and adopts a FIFO buffer to manage asynchronous data, thereby reducing the calculation complexity and meeting the real-time processing requirements of an unmanned vehicle; The present application designs a BeiDou failure fault tolerance mechanism for system reliability, combines zero speed detection and dynamic zero bias calibration technology, and still maintains high-precision short-time pose estimation through IMU when BeiDou fails, so that the test trajectory and the observation trajectory are basically matched, and the velocity cumulative error is cleared when stationary, thereby ensuring slow position divergence.

[0015] ​The application firstly realizes sensor data fusion through initial coordinate system alignment and error state based Kalman filter (ESKF), improves the accuracy of pose information; and calculates the geographic true north azimuth with the magnetometer and Beidou navigation information to calibrate the yaw angle, obtains the absolute heading; and adopts inertial navigation dynamic zero offset calibration and outlier filtering technology, effectively suppresses the error accumulation of IMU; finally, designs the fault tolerance mechanism when the Beidou navigation information fails to guarantee the short-time pose estimation ability, and ensures the confidence and reliability of the system in complex environment. BRIEF DESCRIPTION OF DRAWINGS

[0016] In order to more clearly illustrate the technical solutions in the embodiments of the present application or the prior art, the drawings needed to be used in the embodiments or prior art description will be briefly introduced. Obviously, the drawings in the following description are some embodiments of the present application, and other drawings can be obtained by those skilled in the art without creative labor.

[0017] Figure 1 It is a method flowchart of the present application.

[0018] Figure 2 It is a whole block diagram of hardware architecture design in the present application.

[0019] Figure 3 It is a data flow block diagram of error state based Kalman filter algorithm in the present application.

[0020] Figure 4 It is a schematic diagram of motion route and reference coordinate system in the dynamic test of combined navigation in the present application.

[0021] Figure 5 It is a curve diagram of acceleration and speed changing with time in the navigation coordinate system in the dynamic test of combined navigation in the present application.

[0022] Figure 6 It is a curve diagram of attitude angle changing with time calculated by the fusion algorithm in the dynamic test of combined navigation in the present application.

[0023] Figure 7 It is a comparison diagram of position and observed position plane trajectory calculated by the fusion algorithm in the dynamic test of combined navigation in the present application. Figure 8 It is a curve diagram of position and observed position error distance calculated by the fusion algorithm in the dynamic test of combined navigation in the present application.

[0024] Figure 9 It is a program flowchart of fault tolerance mechanism when Beidou navigation fails in the present application.

[0025] Figure 10It is a position and observation position plane trajectory comparison graph of a position calculated by a Beidou navigation failure fault tolerance mechanism in dynamic test of a combined navigation of the application.

[0026] Figure 11 It is a position and observation position error distance curve graph of a position calculated by a Beidou navigation failure fault tolerance mechanism in dynamic test of a combined navigation of the application. DETAILED DESCRIPTION

[0027] In order to make the personnel in the art better understand the application scheme, the technical solutions in the embodiments of the application will be clearly and completely described below in combination with the drawings in the embodiments of the application. Obviously, the described embodiments are only a part of the embodiments of the application, rather than all the embodiments. Based on the embodiments in the application, all other embodiments obtained by those skilled in the art without creative labor should belong to the protection scope of the application.

[0028] It should be noted that the terms "first", "second" and the like in the specification and claims of the application and the above-described drawings are used to distinguish similar objects, and do not necessarily indicate a specific order or a chronological sequence. It should be understood that the data thus used can be interchanged under appropriate circumstances, so that the embodiments of the application described herein can be implemented in an order other than that illustrated or described herein. In addition, the terms "include" and "have" and any variations thereof are intended to cover non-exclusive inclusion, for example, a process, method, system, product or device including a series of steps or units does not have to be limited to those steps or units clearly listed, but can include other steps or units not clearly listed or inherent to the process, method, product or device.

[0029] The application provides a combined navigation system based on multi-navigation means fusion. The system integrates inertial navigation, magnetometer and Beidou navigation modules on a real circuit board, firstly performs initial filtering on raw data; then realizes multi-source data fusion by using error state Kalman filtering algorithm; in addition, a Beidou navigation failure fault tolerance mechanism is designed, so that the system still has short-time pose estimation capability when losing Beidou satellite signal.

[0030] The application provides a multi-sensor fusion navigation system and method for unmanned vehicles, which is used for solving problems such as navigation failure caused by loss of Beidou signal, IMU error accumulation and poor multi-sensor data fusion effect. The system realizes stable and reliable pose estimation in a complex environment through hardware architecture design and algorithm cooperation.

[0031] The hardware architecture composition in the system mainly has a sensor module and a data fusion processing unit. The sensor module includes a MEMS inertial measurement unit (IMU), a magnetometer and a Beidou satellite signal receiving module. The data fusion processing unit selects a full Zhizao T507 chip with a Cortex A53 architecture as an MCU, is responsible for executing data fusion and outputting pose information.

[0032] For the sensor module, the scheme fuses magnetometer data and Beidou navigation information to generate a true north azimuth reference. The yaw angle corrected by the azimuth can effectively reduce the cumulative error generated in the solving process, and realize the alignment of the coordinate system. In addition, the magnetometer and the Beidou navigation can provide heading information as different information sources. Even if the magnetometer is disabled due to electromagnetic interference, the system can still obtain the true north direction through the Beidou information, thereby improving the overall robustness of the system.

[0033] In the data fusion processing unit, the Kalman filter algorithm based on error state is used in combination with the sensor data characteristics of the unmanned vehicle to screen and process the information. The traditional ESKF algorithm iteration process is modified, the corresponding sensor mathematical model is established, the state transition matrix and the measurement matrix are derived according to the set state variables and the kinetic equation, and then the error state transition equation is obtained through linearization processing. This data fusion processing method can not only reduce the CPU computing pressure, but also guarantee the stability and confidence of the output data. At the same time, the zero speed detection and dynamic zero offset calibration functions are integrated in the ESKF prediction iteration process to improve the position estimation accuracy in low speed or static state, and effectively suppress the IMU integral accumulation. In view of the Beidou signal failure, the system triggers the failure tolerance mechanism according to the preset signal quality threshold. During this period, short-term pose estimation is performed according to the IMU data to maintain the navigation information output. When it is detected that the system is in zero speed state, the accumulated error of the state variable is cleared.

[0034] As shown in Figure 1 , the present application provides a method for calculating the attitude and position of an unmanned vehicle based on combined navigation technology, comprising the following steps: S1, determining initialization parameters based on inertial navigation, magnetometer and Beidou navigation data, and outputting the initialization parameters, wherein the initialization parameters include IMU initial zero offset, rotation matrix of the vehicle body and navigation coordinate system, and true north azimuth; (1) Calculate the initial zero offset of the IMU. Place the device still on a horizontal plane, collect multiple sets of data, and substitute the vehicle body acceleration measured by the IMU accelerometer and the vehicle body angular velocity measured by the gyroscope into formula (1) and formula (2) to obtain the initial zero offset of the IMU accelerometer and the initial zero offset of the gyroscope in the vehicle body coordinate system: (1) (2) (2) Establish the rotation matrix of the body coordinate system and the navigation coordinate system to realize the alignment of the coordinate systems of different sensors. The navigation coordinate system is the ENU coordinate system. Before starting the calibration, the IMU needs to ensure that it is horizontal to be able to perform subsequent work, so the z-axis of the body coordinate system coincides with the z-axis of the navigation coordinate system by default, and the geographic true north azimuth is calculated by combining the magnetometer and Beidou navigation , and the rotation matrix is obtained by substituting equation (3) : (3) (3) True north azimuth calibration. In the previous step, the geographic true north azimuth needs to be calculated by combining the magnetometer and Beidou navigation. The Beidou signal usually covers the angle between the velocity vector and the true north direction, but its update speed is slow, so the magnetometer needs to be combined for calculation. The magnetometer data needs to be calibrated before use to improve the measurement accuracy of the magnetic north. Combined with the geomagnetic declination data compensation, the magnetic north is converted into the true north azimuth and compared with the Beidou navigation information to optimize the heading estimation. In the dynamic data fusion process, the corresponding magnetometer data needs to be compensated according to the pitch angle and roll angle. First, project the magnetometer data from the body coordinate system to the navigation coordinate system to obtain the horizontal magnetic force components and , then substitute equation (4) to obtain the magnetic north azimuth , subtract the local magnetic declination fixed amount to obtain the true north azimuth .

[0035] (4) S2, collect asynchronous data of sensors through software polling mechanism and buffer management; the sensors include IMU, magnetometer and Beidou navigation; Due to the differences in the input frequencies of various sensors, the data update frequency of the IMU is 200Hz, the data update frequency of the magnetometer is 100Hz, and the update frequency of the Beidou navigation information is 10Hz. The system realizes asynchronous reception of sensor data through software polling mechanism and buffer management to ensure the timestamp accuracy and data integrity. In the main loop, the execution period is set to 1ms, and the real-time monitoring of the new data ready state of each sensor is realized by timestamp comparison. When the polling period is triggered, the system extracts the ready sensor data and inputs it into the corresponding iteration process of the fusion algorithm.

[0036] To avoid data loss and timing discrepancies, a FIFO circular buffer configured for each sensor is associated with and stores the timestamp of the corresponding data. The buffer management rules include: when new data is written, if the buffer is full, the first data is overwritten; when reading data, the most recent valid data is retrieved based on the timestamp.

[0037] S3. After performing motion constraints and filtering preprocessing on the asynchronous sensor data output by S2, the data is fused through the prediction and correction processes of the ESKF algorithm in combination with the initialization parameters, and the optimal pose estimation result of the unmanned vehicle is output.

[0038] ESKF multi-source data fusion consists of a prediction process and a correction process. When data is received by the fusion processing unit, considering the actual motion characteristics of the autonomous vehicle, motion constraints and low-pass filtering are applied to the original data. During normal driving, the autonomous vehicle only moves longitudinally (in the forward direction) in the body coordinate system, and lateral acceleration is negligible. However, during turning, lateral acceleration reflects the centripetal acceleration of the vehicle during turning, so amplitude constraints are required. Excessive lateral acceleration triggers anti-rollover intervention; in this system, the lateral acceleration must be less than 5 m / s². 2 Secondly, after balancing the effect of gravitational acceleration, there are no other vertical accelerations for the vertical acceleration of the machine body. Usually, only high-frequency vertical vibration signals passing through rough roads are captured. It is necessary to perform low-pass filtering on them, with the cutoff frequency set to 5Hz, to retain low-frequency vehicle motion such as the z-axis acceleration changes generated by going uphill and downhill, and to filter out high-frequency noise to avoid integral drift.

[0039] In the ESKF algorithm, the state variables estimated by this system include the actual states. x nominal state and error state The three categories are specifically defined as follows: the true state is the actual state of the system in the physical world, which cannot be directly and accurately measured and needs to be indirectly estimated through sensor data fusion; the nominal state is the "best estimate" state maintained internally by the algorithm and updated through IMU data integration (prediction); the error state represents the small difference between the true state and the nominal state. As the core estimation object of ESKF, it is explicitly estimated and the nominal state is corrected in the prediction-correction framework.

[0040] The three state variables are composed as shown in equation (5), including position. p ,speed v Quaternions q accelerometer zero bias and gyroscope zero bias .

[0041] (5) ESKF achieves state updates through a "prediction-correction" closed loop. The flow of sensor data within the ESKF framework is shown in the appendix. Figure 3 The specific steps of the ESKF algorithm are as follows: (1) Prediction process First, the nominal state is predicted by acquiring the data from the IMU's accelerometer and gyroscope. k The measured values ​​at time points are used to update the nominal state using inertial integration and first-order Euler integration as shown in equation (6), thus obtaining the prior estimates of the state variables. : (6) In the formula It is a mapping from quaternions to rotation matrices. It is a quaternion multiplication operation.

[0042] Then, the prior estimate of the error state is predicted based on the error state equation of equation (7). : (7) In the formula, Here is the error state transition matrix. B To control the input matrix, The covariance matrix of the IMU measurement noise is: Q Calculations show that... and B The expressions are shown in equations (8) and (9): (8) (9) Then, the covariance of the prior estimate of the error state is estimated according to equation (10). : (10) In the formula, Let be the error covariance matrix at the current time. Q The process noise covariance matrix is ​​expressed as shown in equation (11): (11) In the formula, The standard deviation of accelerometer noise The standard deviation of gyroscope noise The standard deviation of the rate of change of acceleration error bias The standard deviation of the rate of change of gyroscope error bias.

[0043] (2) Correction process Based on BeiDou navigation information, the components of the observation quantities are set as follows: Therefore, the actual observed value This can be expressed as: (12) where is a nonlinear term, is the observation noise, which is Gaussian distributed with covariance matrix R .

[0044] For the observation matrix H is the Jacobian matrix of the observation equation with respect to the error state The calculation requires the use of the chain rule of matrix differentiation, and the specific formula is as formula (13): (13) In the formula, the Jacobian matrix of the observation function with respect to the true state and the Jacobian matrix of the true state with respect to the error state The derivation results are as formula (14) and formula (15): (14) (15) In formula (15), The expression of in the expression is as follows: (16) Then, the Kalman filter parameters K are calculated according to the above results: (17) Combined with the prior estimate of the state variable, the optimal estimate of the error state at time k +1 is updated according to formula (18): (18) The error state is injected into the nominal state, and the optimal estimate of the true state at time k +1 is obtained according to formula (19): (19) In the formula, indicates the multiplication operation of the quaternion state and the vector addition operation of other states.

[0045] Finally, the update of the true state covariance matrix k at time +1 is completed according to formula (20): (20) In dynamic testing, the test results of the state quantity of the ESKF algorithm are shown in the accompanying Figure 5 , Figure 6 and​​Figure 7 as shown.

[0046] S4, Beidou navigation failure fault tolerance processing;When Beidou navigation information is invalid, based on the optimal pose estimation result, combined with zero speed detection and dynamic zero offset calibration, output short time pose estimation result, maintain navigation continuity.

[0047] In Beidou navigation information, the horizontal positioning accuracy factor is proportional to the positioning error, and since the fusion algorithm needs to rely on Beidou navigation as an absolute positioning source, the Beidou information effective threshold needs to be set to less than 4, so as to ensure the reliability of the observation information. When Beidou information is invalid, more depends on IMU data integration in positioning and obtaining motion trajectory, so error control is very important. Through zero speed detection, it can effectively detect whether the carrier is in a completely static state at a certain time, so as to realize the correction of the cumulative error of the system motion process.

[0048] In the present application, the zero speed detection and dynamic zero offset calibration are combined to realize the integral error correction of the IMU data under the condition of Beidou navigation failure, and the code design flow chart of the failure fault tolerance mechanism is shown in the accompanying Figure 8 , the specific implementation scheme is as follows: (1) Differential motion detection At the current time, first calculate the original three-axis acceleration module value as the key attention value, then detect the differential value of the acceleration module to form the motion intensity index, and continuously detect that the motion intensity index does not meet the standard, that is, the differential value continuously below the threshold and the data points are continuous, trigger the dynamic correction mechanism. The duration of the trigger parameter is selected as 60 frames of data, and the duration of 60 frames of data in the present system is 0.3 seconds. This parameter needs to be adjusted according to the sensor accuracy to ensure that the stationary point can be stably detected, and the dynamic speed change process is not mistaken as a stationary point. In addition, the stable stationary signal detected above is recorded as the basis for dynamic zero offset calibration.

[0049] (2) Stationary locking and dynamic zero offset calibration When the dynamic correction mechanism is triggered, first force the current three-axis velocity to zero, the three-axis angular velocity to zero, set the flag bit to enter the zero speed locking state, and continuously force the velocity to zero during this period until the acceleration module differential value rises above the threshold, and in addition, the average acceleration of each axis in the 60 frames of stable stationary signal is used as the new zero point offset, which is used to compensate the subsequent acceleration data in real time.

[0050] According to the above steps, the position state results of the dynamic test of the Beidou failure fault tolerance mechanism are shown in the accompanying Figure 10 . For the attitude information, since the azimuth angle observed by Beidou navigation and the azimuth angle calculated by the magnetometer are mutual redundancy, the fusion result is basically the same as that when Beidou navigation is not invalid. Figure 10The trajectory of the fault-tolerant mechanism is basically matched with the observed trajectory, and the divergence speed of the position state is ensured to be slow because the accumulated error of the speed is eliminated in the stationary state. The error distance of the fusion result and the observation value changes over time as shown in the following table: Figure 11 .

[0051] Finally, it should be noted that: the above embodiments are only used to illustrate the technical solutions of the present application, but not to limit them; although the present application has been described in detail with reference to the foregoing embodiments, those skilled in the art should understand that: it can still modify the technical solutions recorded in the foregoing embodiments, or make equivalent replacement for part or all of the technical features; and these modifications or replacements do not make the essence of the corresponding technical solutions deviate from the scope of the technical solutions of the embodiments of the present application.

Claims

1. A method for solving the pose and position of an unmanned vehicle based on integrated navigation technology, characterized in that, Comprising the following steps: S1, determining initialization parameters based on inertial navigation, magnetometer and Beidou navigation data, outputting the initialization parameters, the initialization parameters including IMU initial zero offset, rotation matrix of the machine body and navigation coordinate system and true north heading angle; S2, collecting asynchronous data of sensors through software polling mechanism and buffer management, the sensors including IMU, magnetometer and Beidou navigation; S3, after motion constraint and filtering pretreatment of the asynchronous data of sensors output by S2, combining the initialization parameters, realizing data fusion through the prediction process and correction process of ESKF algorithm, and outputting the optimal unmanned vehicle attitude and position estimation result of the unmanned vehicle pose.

2. The method of claim 1, wherein, S1 comprises the following steps: S11, calculate the initial zero offset of the IMU; place the navigation system at rest on a horizontal plane, measure the body acceleration through the IMU accelerometer , measure the body angular velocity through the gyroscope , based on the body acceleration and the body angular velocity, obtain the initial zero offset of the accelerometer and the initial zero offset of the gyroscope of the IMU in the body coordinate system and the initial zero offset of the gyroscope , the formula is as follows: (1) (2) where g is the magnitude of the local gravitational acceleration; k Refers to the first t k Moment; S12, the rotation matrix of the body coordinate system and the navigation coordinate system is established; the navigation coordinate system is set as the ENU coordinate system, the z-axis of the body coordinate system is coincided with the z-axis of the navigation coordinate system, and the geographic true north azimuth is solved by the magnetometer and the Beidou navigation , the rotation matrix is obtained based on the geographic true north azimuth , and the formula is as follows: (3) S13, calibrating true north azimuth; fusing the geomagnetic compass and the Beidou navigation to solve the geographic true north azimuth; compensating the geomagnetic compass data according to the pitch angle and the roll angle, projecting the geomagnetic compass data from the body coordinate system to the navigation coordinate system, and thus obtaining the horizontal magnetic force component and obtaining the magnetic north azimuth based on the horizontal magnetic force component the formula is as follows: (4) Subtracting the local magnetic declination from the magnetic north azimuth angle Obtaining the true north azimuth angle .

3. The method of claim 1, wherein, S2 comprises the following steps: In the main loop, the polling period is set to 1ms, and the new data ready state of each sensor is monitored in real time through timestamp comparison; when the polling period is triggered, the system extracts the ready sensor data and inputs it into the corresponding iteration process of the fusion algorithm; The FIFO ring buffer configured for each sensor is associated with the timestamp of the corresponding data; the management rule of the buffer includes that when new data is written, if the buffer is full, the first data is overwritten; when reading data, the valid data at the latest time is retrieved according to the timestamp.

4. The method of claim 1, wherein, In S3, the pretreatment includes the following steps: restricting the lateral acceleration of the unmanned vehicle to be less than 5 m / s 2 When the lateral acceleration exceeds the standard, the anti-rollover intervention is triggered; the vertical acceleration of the machine body is filtered by a low-pass filter with a cutoff frequency of 5 Hz, retaining the low-frequency motion acceleration and filtering out high-frequency vibration noise.

5. The method of claim 1, wherein, In S3, the state quantities estimated by the ESKF algorithm include the true state x , the nominal state , and the error state ; The real state is the actual state of the system in the physical world; the nominal state is the best estimated state maintained inside the algorithm, which is updated through IMU data integration prediction; The error state represents the difference between the real state and the nominal state, and the error state is explicitly estimated and corrected to the nominal state; true state x , nominal state and error state consists of the following formula: (5) wherein, p is a position, v is a velocity, q is a quaternion, is an accelerometer bias and is a gyroscope bias.

6. The method of claim 1, wherein, In S3, the prediction process of the ESKF algorithm comprises the following steps: making a prediction of the nominal state; obtaining measurements of the accelerometers and gyroscopes of the IMU at k time, making an inertial integration and a first order Euler integration to update the nominal state, obtaining a priori estimates of the state variables , the nominal state update equation being as follows: (6) wherein is the mapping from quaternions to rotation matrices; an a priori estimate of the error state predicted from the error state equation is given by the equation (7) wherein is the error state transition matrix, B is the control input matrix, is the IMU measurement noise, whose covariance matrix is Q ; and B The expression of the above is as follows: (8) (9) estimate the covariance of the error state prior estimate , as follows: (10) wherein is the current time error covariance matrix, Q is the process noise covariance matrix, expressed as follows: (11) wherein is the standard deviation of the accelerometer noise, is the standard deviation of the gyroscope noise, is the standard deviation of the rate of change of the accelerometer error bias, is the standard deviation of the rate of change of the gyroscope error bias.

7. The method of claim 1, wherein, In S3, the correction process of the ESKF algorithm comprises the following steps: According to the Beidou navigation information, the composition of the observation is set as Therefore, the real observation value is expressed as: (12) wherein is a non-linear term, is an observation noise, and the covariance matrix is R ; For the observation matrix H is the Jacobian matrix of the observation equation with respect to the error state The calculation requires the use of the chain rule of matrix differentiation, and the specific formula is as follows: (13) where the Jacobian of the observation function with respect to the true state and the Jacobian of the true state with respect to the error state are derived as follows: (14) (15) The expression in The expression in is as follows: (16) Computing kalman filter parameters K : (17) Combining the state variable priori estimation, update k the optimal estimation of error state at time t The formula is as follows: (18) The error state is injected into the nominal state to obtain the real state k The optimal estimation of the time point +1 The formula is as follows: (19) For k +1 real state covariance matrix is updated with the formula 。 8. The method of claim 1, wherein, Further comprising: S4, Beidou navigation failure tolerance processing; When the Beidou navigation information is invalid, based on the prior estimation result of the pose information of the state variable, combined with zero speed detection and dynamic zero offset calibration, a short-time pose estimation result is output to maintain navigation continuity.

9. The method of claim 8, wherein, The effective threshold of Beidou navigation information is that the horizontal positioning accuracy factor is less than 4; when the horizontal positioning accuracy factor is greater than 4, it is determined that the Beidou information is invalid, and the fault tolerance mechanism is triggered.

10. The method of claim 9, wherein, The fault tolerance mechanism is as follows: Differential motion detection; calculate the current three-axis acceleration module value and differential value as the motion intensity index, when the motion intensity index is continuously lower than the threshold value for 60 frames, the dynamic correction mechanism is triggered; Static locking and dynamic zero offset calibration; forcibly set the current three-axis velocity to zero and the three-axis angular velocity to zero, set the flag bit to enter the zero speed locking state, and continuously forcibly set the velocity to zero during this period until the acceleration module differential value rises above the threshold value; And the average value of each axis acceleration in 60 frames of stable static signals is taken as the new zero point offset, and the acceleration data after real-time compensation is obtained.

Images