Engineering vehicle pose sensing method

Abstract

The invention is suitable for the technical field of vehicle positioning, and provides an engineering vehicle pose sensing method. Accurate positioning of the engineering vehicle under the complex working condition can be achieved, accurate and reliable position information is provided for intelligent operation of the engineering vehicle, and therefore the operation efficiency and safety are remarkably improved. Aiming at the special working environment of the engineering vehicle, the invention provides the pose sensing method according to the solution state of the RTK and the data measured by the RTK in consideration of the problem of sensor data error caused by possible shielding and vibration of the RTK information, and the method can improve the positioning accuracy and the solving speed and ensure the stable operation of the engineering vehicle in the complex environment. The invention provides a self-adaptive extended Kalman filtering method, and the method can obtain the pose information of the engineering vehicle more accurately and quickly. According to the innovation, the positioning precision is improved, the data processing speed is increased, and powerful technical support is provided for intelligent control of the engineering vehicle.

Description

Technical Field

[0001] The present invention belongs to the technical field of vehicle positioning, and in particular relates to a method for sensing the position and posture of an engineering vehicle. Background Art

[0002] Engineering vehicles play a vital role in key energy and livelihood fields such as mining, road and bridge construction, urban infrastructure construction, and emergency rescue. However, these vehicles often face harsh working environments and frequent complex operations such as acceleration and steering, which pose a challenge to the health and work efficiency of drivers. In order to solve this problem, the goal is to achieve intelligent engineering vehicles with auxiliary / autonomous operation capabilities. As one of the key technologies, posture perception can obtain the posture information of the vehicle, enabling the autonomous driving system to make safer and smarter driving decisions. Therefore, having good positioning accuracy is very important for unmanned driving of engineering vehicles.

[0003] In the posture perception of the walking device of the engineering vehicle, the multi-sensor fusion method is currently used to improve the accuracy and stability of perception. Specifically, by using a high-frequency IMU (inertial measurement unit) to compensate for the low-frequency RTK (real-time dynamic differential positioning technology), higher measurement accuracy can be obtained. However, the operating speed of the engineering vehicle is much lower than that of the passenger car, so the RTK in the fixed solution state can usually provide a higher measurement accuracy, but due to the special working environment of the engineering vehicle, this brings additional challenges to its posture perception. On the one hand, the RTK signal may be interrupted due to occlusion; on the other hand, in environments such as mines, magnetic field interference may cause the RTK signal to mutate, thereby causing information errors. In addition, in the process of engineering vehicle posture perception, the impact of vehicle vibration and complex working environment on the sensor needs to be considered. At the same time, in order to ensure the real-time performance of the automatic driving system, it is also necessary to ensure the high efficiency of the algorithm. In view of the fact that the posture perception method of passenger cars cannot be directly applied to engineering vehicles, we need to establish a posture perception method suitable for them according to the characteristics and working environment of engineering vehicles. Therefore, the present invention proposes an engineering vehicle posture perception method. Summary of the invention

[0004] The purpose of the present invention is to provide a method for sensing the posture of an engineering vehicle, aiming to solve the problems raised in the above-mentioned background technology.

[0005] The purpose of the present invention is achieved through the following technical solutions:

[0006] The method for sensing the posture of an engineering vehicle comprises the following steps:

[0007] Step S1, read RTK and IMU data through serial communication;

[0008] Step S2: preprocess the read data using a mean filtering method;

[0009] Step S3: determine whether the RTK solution status and posture information are accurate;

[0010] Step S4, establishing the state equation of the system according to the IMU / wheel speed meter combined positioning model;

[0011] Step S5: Calculate the posture information based on the adaptive extended Kalman filter.

[0012] Furthermore, in step S1, the IMU is composed of an accelerometer and a gyroscope, and the system error is calibrated by the following formula:

[0013] a O =T a K a (α S +b a )

[0014] ω O =T g K g (ω s +b g )

[0015] In the formula, a O and ω O Represent the ideal measured acceleration and angular velocity values ​​respectively; T a and T g They represent the errors caused by acceleration and angular velocity rotation respectively; K a and K g Respectively represent the ratio of acceleration and angular velocity; a S and ω S Respectively represent the actual measured acceleration and angular velocity values; b a and b g They represent the errors caused by acceleration and angular velocity offset respectively.

[0016] Furthermore, the specific process of step S2 is as follows:

[0017] The acceleration and angular velocity are smoothed to reduce the noise caused by vibration; the data is processed by moving average filtering, as shown in the following formula:

[0018]

[0019] Where x represents the input signal, y represents the output signal, n represents the current calculation point, and N represents the sample size.

[0020] Furthermore, the specific process of step S3 is as follows:

[0021] Set the condition (x i - x i-1 ) 2 +(y i - y i-1 ) 2 < d, where x i represents the lateral position at the current moment i, x i-1 represents the lateral position at the previous moment i - 1, y i represents the longitudinal position at the current moment i, y i-1 represents the longitudinal position at the previous moment i - 1, and d represents the position error; when the condition is met, directly output the data measured by RTK, and when the condition is not met, use the self - adaptive Kalman filtering method for processing, taking the IMU / odometer combined positioning information as the prediction information of the extended Kalman filter and the pose information obtained by RTK as the observation information of the extended Kalman filter.

[0022] Furthermore, the specific process of step S4 is as follows:

[0023] Establish the state equation of the system according to the IMU / odometer combined positioning model as shown in the following formula:

[0024]

[0025] In the formula, x k+1 is the lateral position of the vehicle at time k + 1, y k+1 is the longitudinal position of the vehicle at time k + 1, θ k+1 is the heading angle of the vehicle at time k + 1, x k is the lateral position of the vehicle at time k, y k is the longitudinal position of the vehicle at time k, T is the sampling time, and θ k is the heading angle of the vehicle at time k; v k is the speed of the engineering vehicle at time k, measured by the wheel speed encoder installed on the drive motor shaft; ω k is the heading angular velocity of the engineering vehicle at time k, obtained through IMU; ω v is the speed noise; ω ω is the noise of the angular velocity;

[0026] Represent the process noise of the system with the vector w k , then w k = [w v ω ω T , and the covariance is Q, X k is the system state vector, representing the pose information of the vehicle, X​k =[x k y k θ k ]; The state equation is expanded by Taylor series Linearized into the form of formula, is the predicted state estimate in the extended Kalman filter, where the external input influence matrix A in the state equation k+1 is zero, the Jacobian matrix F k+1 and the system process noise matrix Γ k Respectively expressed as:

[0027]

[0028] In the formula, is the estimated heading angle.

[0029] Furthermore, the specific process of step S5 is as follows:

[0030] Use the Jacobian matrix F k+1 , system process noise matrix Γ k and covariance Q, calculate the prior estimate of the state error covariance for:

[0031]

[0032] Where P k is the state estimation error covariance matrix;

[0033] The data measured by RTK is used as the observation data of the system. k , z k =[x rtk,k y rtk , k θ rtk , k ], where x rtk,k is the lateral position measured by RTK at time k, y rtk,k is the longitudinal position measured by RTK at time k, θ rtk,k is the heading angle measured by RTK at time k, and the observation matrix H is obtained as:

[0034]

[0035] Calculate the Kalman gain K k+1 for:

[0036]

[0037] Where E is the covariance matrix of observation noise;

[0038] Posterior estimate of the target state Is the following formula:

[0039]

[0040] In the formula, is the prior estimate of the target state, z k+1 is the observation matrix at time k+1;

[0041] The posterior estimate of the error covariance P k+1 Is the following formula:

[0042]

[0043] The adaptive estimation method is used to estimate the process noise covariance Q and the observation noise covariance R in the EKF, and the covariance matrix of the residual is adjusted according to the theoretical value;

[0044] Residual ε k is the actual measurement value and the posterior estimate The difference between them is calculated by the following formula:

[0045]

[0046] make S k is the covariance of the measurement residuals, then R k It is expressed as:

[0047]

[0048] In the formula, R k is the covariance matrix; an adaptive residual-based method is used to estimate R k , the formula is as follows:

[0049]

[0050] In the formula, is the actual covariance matrix of the residual, E is the mathematical expectation, ε k is the residual, v k To measure noise, in the Kalman filter

[0051] Introduce the forgetting factor α to estimate R k , as shown below:

[0052]

[0053] In the formula, R k-1 is the covariance matrix at time k-1;

[0054] make d kis the deviation between the measured value and the predicted value, where For the predicted state prior estimate in the extended Kalman filter, we get:

[0055]

[0056] w k-1 =X k -Φ(X k-1 ,u k-1 )

[0057]

[0058] In the formula, Φ(x k ,u k ) is the state transfer function, w k-1 is the process noise at time k-1, X k is the system state vector at time k, X k-1 is the system state vector at time k-1, u k-1 is the input matrix, is the state prediction error, is the state estimate at time k-1, K k is the Kalman gain;

[0059] therefore:

[0060]

[0061] In the formula, is the updated estimated covariance matrix;

[0062] Introduce the forgetting factor α to estimate Q k ,get:

[0063]

[0064] In the formula, Q k is the process noise covariance matrix at time k, Q k-1 is the process noise covariance matrix at time k-1;

[0065] Finally, the vehicle's position and posture information is calculated.

[0066] Compared with the prior art, the present invention has the following beneficial effects:

[0067] 1. The present invention can realize accurate positioning of engineering vehicles under complex working conditions, provide accurate and reliable position information for intelligent operation of engineering vehicles, thereby significantly improving operation efficiency and safety.

[0068] 2. Aiming at the special working environment of engineering vehicles, the present invention takes into account the problem of sensor data errors caused by possible obstruction of RTK information and vibration, and proposes a posture perception method based on the RTK solution state and RTK measured data. This method can improve the positioning accuracy and solution speed, and ensure the stable operation of engineering vehicles in complex environments.

[0069] 3. The present invention proposes an adaptive extended Kalman filter method, which can obtain the position information of engineering vehicles more accurately and quickly. This innovation not only improves the positioning accuracy, but also speeds up the data processing speed, providing strong technical support for the intelligent control of engineering vehicles. BRIEF DESCRIPTION OF THE DRAWINGS

[0070] Figure 1 Flow chart of the method of the present invention.

[0071] Figure 2 Comparison of data before and after filtering.

[0072] Figure 3 For location comparison.

[0073] Figure 4 For heading angle comparison. DETAILED DESCRIPTION

[0074] In order to have a clearer understanding of the technical features, purposes and beneficial effects of the present invention, the technical solution of the present invention is now described in detail below, but it should not be construed as limiting the applicable scope of the present invention.

[0075] The specific implementation of the present invention is described in detail below in conjunction with specific embodiments.

[0076] An embodiment of the present invention provides a method for sensing the posture of an engineering vehicle, the flow chart of which is as follows: Figure 1 As shown, the method comprises the following steps:

[0077] Step S1, read RTK and IMU data through serial communication;

[0078] Step S2: preprocess the read data using a mean filtering method;

[0079] Step S3: determine whether the RTK solution status and posture information are accurate;

[0080] Step S4, establishing the state equation of the system according to the IMU / wheel speed meter combined positioning model;

[0081] Step S5, calculating the posture information based on the adaptive extended Kalman filter;

[0082] At the next time step, steps S1 to S5 are repeated.

[0083] The details are as follows:

[0084] (i) Step S1: Read RTK and IMU data through serial communication.

[0085] Due to the special working environment of engineering vehicles (signal shielding, vehicle vibration), it is difficult to accurately perceive the vehicle's position and posture, and it is difficult to measure relatively accurate position information only through RTK. Therefore, it is used in combination with IMU. IMU consists of accelerometers and gyroscopes, and has systematic errors. It needs to be quickly calibrated using the following formula:

[0086] a O =T a K a (a S +b a )

[0087] ω O =T g K g (ω S +b g )

[0088] In the formula, a O and ω O Represent the ideal measured acceleration and angular velocity values ​​respectively; T a and T g They represent the errors caused by acceleration and angular velocity rotation respectively; K a and K g Respectively represent the ratio of acceleration and angular velocity; a S and ω S Respectively represent the actual measured acceleration and angular velocity values; b a and b g They represent the errors caused by acceleration and angular velocity offset respectively.

[0089] (ii) Step S2: pre-process the read data using a mean filtering method.

[0090] During the working process of engineering vehicles, the vibration of the engine will increase the noise of acceleration and angular velocity. Therefore, the acceleration and angular velocity need to be smoothed to reduce the noise caused by vibration. The moving average filter is used to process the data, as shown in the following formula:

[0091]

[0092] Where x represents the input signal, y represents the output signal, n represents the current calculation point, and N represents the sample size.

[0093] Figure 2For the data comparison before and after filtering, it can be seen that after average filtering, the data becomes smooth and the outliers caused by vibration are reduced.

[0094] (3) Step S3: Determine whether the solution status and pose information of RTK are accurate.

[0095] According to the status of RTK and the comparison with the data at the previous moment, set the condition (x i -x i-1 ) 2 +(y i -y i-1 ) 2 < d, where x i represents the lateral position at the current moment i, x i-1 represents the lateral position at the previous moment i - 1, y i represents the longitudinal position at the current moment i, y i-1 represents the longitudinal position at the current moment i - 1, and d represents the position error; when the condition is satisfied, directly output the data measured by RTK, and when the condition is not satisfied, it is necessary to use the method of self - adaptive Kalman filtering for processing, taking the IMU / odometer combined positioning information as the prediction information of the extended Kalman filter and taking the pose information obtained by RTK as the observation information of the extended Kalman filter.

[0096] (4) Step S4: Establish the state equation of the system according to the IMU / odometer combined positioning model.

[0097] Establish the state equation of the system according to the IMU / odometer combined positioning model as shown in the following formula:

[0098]

[0099] In the formula, ω ω is the noise of the angular velocity. Since ω ω is relatively small, sin(ω v *T)≈0, cos(ω v *T)≈1, from which the following can be obtained:

[0100]

[0101] Substitute the above formula into the state equation, and the state equation can be simplified into the following formula:

[0102]

[0103] In the formula, x k+1 is the lateral position of the vehicle at time k + 1, y k+1 is the longitudinal position of the vehicle at time k + 1, θ k+1 is the heading angle of the vehicle at time k + 1, x kis the lateral position of the vehicle at time k, y k is the longitudinal position of the vehicle at time k, T is the sampling time, θ k is the heading angle of the vehicle at time k; v k is the speed of the engineering vehicle at time k, measured by the wheel speed encoder installed on the drive motor shaft; ω k is the angular velocity of the engineering vehicle at time k, obtained through IMU; ω v is the speed noise;

[0104] The process noise of the system is represented by the vector w k If w k =[w v ω ω ] T , the covariance is Q, and the system state is represented by the vector X k Indicates the vehicle’s position information, X k =[x k y k θ k ]; The state equation is expanded by Taylor series Linearized into the form of formula, is the predicted state estimate in the extended Kalman filter, where the external input influence matrix A in the state equation k+1 is zero, the Jacobian matrix F k+1 and the system process noise matrix Γ k Respectively expressed as:

[0105]

[0106] In the formula, is the estimated heading angle.

[0107] (V) Calculate the posture information based on the adaptive extended Kalman filter.

[0108] Use the Jacobian matrix F k+1 , system process noise matrix Γ k and covariance Q, calculate the prior estimate of the state error covariance for:

[0109]

[0110] Where P k is the state estimation error covariance matrix;

[0111] The data measured by RTK is used as the observation data of the system. k , z k =[x rtk,k yrtk,k θ rtk,k ], where x rtk,k is the lateral position measured by RTK at time k, y rtk,k is the longitudinal position measured by RTK at time k, θ rtk,k is the heading angle measured by RTK at time k, and the observation matrix H is obtained as:

[0112]

[0113] Calculate the Kalman gain K k+1 for:

[0114]

[0115] Where R is the covariance matrix of observation noise;

[0116] Posterior estimate of the target state Is the following formula:

[0117]

[0118] In the formula, is the prior estimate of the target state, z k+1 is the observation matrix at time k+1;

[0119] The posterior estimate of the error covariance P k+1 Is the following formula:

[0120]

[0121] The noise covariance is usually specified as a constant matrix according to the instrument measurement accuracy, and is assigned as a constant matrix using a trial and error method, which usually relies on experience. In order to improve this problem, the present invention uses an adaptive estimation method to estimate the process noise covariance Q and the observation noise covariance R in the EKF, which can adjust the covariance matrix of the residual according to the theoretical value.

[0122] Residual ε k is the actual measurement value and the posterior estimate The difference between them is calculated by the following formula:

[0123]

[0124] make S k is the covariance of the measurement residuals, then R k It is expressed as:

[0125]

[0126] In the formula, R k is the covariance matrix; since Rk is the covariance matrix, then it should be a positive definite matrix, but the above formula cannot guarantee the estimated R k is a positive definite matrix. Therefore, in order to ensure a positive definite matrix, an adaptive method based on residuals is used to estimate R k , the formula is as follows:

[0127]

[0128] In the formula, is the actual covariance matrix of the residual, E is the mathematical expectation, ε k is the residual, v k To measure noise, in the Kalman filter

[0129] for Expected operation is to calculate the The average is used to approximate, and the forgetting factor α is introduced to estimate R k , as shown below:

[0130]

[0131] In the formula, R k-1 is the covariance matrix at time k-1;

[0132] make d k is the deviation between the measured value and the predicted value, where For the predicted state prior estimate in the extended Kalman filter, we get:

[0133]

[0134] w k-1 =X k -Φ(X k-1 ,u k-1 )

[0135]

[0136] In the formula, Φ(x k ,u k ) is the state transfer function, w k-1 is the process noise at time k-1, X k is the system state vector at time k, X k-1 is the system state vector at time k-1, u k-1 is the input matrix, is the state prediction error, is the state estimate at time k-1, K k is the Kalman gain;

[0137] therefore:

[0138]

[0139] In the formula, is the updated estimated covariance matrix;

[0140] With R k Similarly, the forgetting factor α is also introduced to estimate Q k ,get:

[0141]

[0142] In the formula, Q k is the process noise covariance matrix at time k, Q k-1 is the process noise covariance matrix at time k-1.

[0143] The vehicle's posture information can be finally calculated through the above formula.

[0144] Figure 3 For position comparison, Figure 4 For heading angle comparison, “True” in the figure represents the true value of the information, “IAEKF” represents the method of the present invention, and “AEKF” represents the traditional adaptive extended Kalman filter algorithm. Figure 3 and Figure 4 It can be seen that the position and heading angle obtained by the method of the present invention are closer to the true value, so the method of the present invention is more accurate than other posture perception algorithms.

[0145] The above are only preferred embodiments of the present invention. It should be pointed out that, for those skilled in the art, several modifications and improvements can be made without departing from the concept of the present invention. These should also be regarded as the protection scope of the present invention. These will not affect the effect of the implementation of the present invention and the practicality of the patent.

Claims

1. A method for sensing the posture of an engineering vehicle, characterized in that: The following steps are involved: Step S1, read RTK and IMU data through serial communication; Step S2: preprocess the read data using a mean filtering method; Step S3: determine whether the RTK solution status and posture information are accurate; Step S4, establishing the state equation of the system according to the IMU / wheel speed meter combined positioning model; Step S5: Calculate the posture information based on the adaptive extended Kalman filter.

2. The method for sensing the posture of an engineering vehicle according to claim 1, characterized in that: In step S1, the IMU is composed of an accelerometer and a gyroscope, and the system error is calibrated by the following formula: a O =T a K a (a S +b a ) oh O =T g K g (oh S +b g ) In the formula, a O and ω O Represent the ideal measured acceleration and angular velocity values ​​respectively; T a and T g They represent the errors caused by acceleration and angular velocity rotation respectively; K a and K g Respectively represent the ratio of acceleration and angular velocity; a S and ω S Respectively represent the actual measured acceleration and angular velocity values; b a and b g They represent the errors caused by acceleration and angular velocity offset respectively.

3. The method for sensing the posture of an engineering vehicle according to claim 2, characterized in that: The specific process of step S2 is as follows: The acceleration and angular velocity are smoothed to reduce the noise caused by vibration; the data is processed by moving average filtering, as shown in the following formula: Where x represents the input signal, y represents the output signal, n represents the current calculation point, and N represents the sample size.

4. The method for sensing the position and posture of an engineering vehicle according to claim 3, characterized in that: The specific process of step S3 is as follows: Set the condition (x i -x i-1 ) 2 +(y i -y i-1 ) 2 < d, where x i represents the lateral position at the current moment i, x i-1 represents the lateral position at the previous moment i - 1, y i represents the longitudinal position at the current moment i, y i-1 represents the longitudinal position at the previous moment i - 1, and d represents the position error; when the condition is met, directly output the data measured by RTK, and when the condition is not met, use the self - adaptive Kalman filtering method for processing, taking the IMU / odometer combined positioning information as the prediction information of the extended Kalman filter and the pose information obtained by RTK as the observation information of the extended Kalman filter.

5. The method for sensing the position and posture of an engineering vehicle according to claim 4, characterized in that: The specific process of step S4 is as follows: The state equation of the system established based on the IMU / wheel speedometer combined positioning model is shown as follows: In the formula, x k+1 is the lateral position of the vehicle at time k+1, y k+1 is the longitudinal position of the vehicle at time k+1, θ k+1 is the heading angle of the vehicle at time k+1, x k is the lateral position of the vehicle at time k, y k is the longitudinal position of the vehicle at time k, T is the sampling time, θ k is the heading angle of the vehicle at time k; v k is the speed of the engineering vehicle at time k, measured by the wheel speed encoder installed on the drive motor shaft; ω k is the angular velocity of the engineering vehicle at time k, obtained through IMU; ω v is the speed noise; ω ω is the noise of angular velocity; The process noise of the system is represented by the vector w k If w k =[w v ω ω ] T , the covariance is Q, X k is the system state vector, representing the vehicle’s position information, X k =[x k y k θ k ]; The state equation is expanded by Taylor series Linearized into the form of formula, is the predicted state estimate in the extended Kalman filter, where the external input influence matrix A in the state equation k+1 is zero, the Jacobian matrix F k+1 and the system process noise matrix Γ k Respectively expressed as: In the formula, is the estimated heading angle.

6. The method for sensing the position and posture of an engineering vehicle according to claim 5, characterized in that: The specific process of step S5 is as follows: Use the Jacobian matrix F k+1 , system process noise matrix Γ k and covariance Q, calculate the prior estimate of the state error covariance for: Where P k is the state estimation error covariance matrix; The data measured by RTK is used as the observation data of the system. k , z k =[x rtk,k y rtk,k θ rtk,k ], where x rtk,k is the lateral position measured by RTK at time k, y rtk,k is the longitudinal position measured by RTK at time k, θ rtk,k is the heading angle measured by RTK at time k, and the observation matrix H is obtained as: Calculate the Kalman gain K k+1 for: Where R is the covariance matrix of observation noise; Posterior estimate of the target state Is the following formula: In the formula, is the prior estimate of the target state, z k+1 is the observation matrix at time k+1; The posterior estimate of the error covariance P k+1 Is the following formula: The process noise covariance Q and observation noise covariance R in EKF are estimated using adaptive estimation method, and the covariance matrix of residual is adjusted according to the theoretical value. Residual ε k is the actual measurement value and the posterior estimate The difference between them is calculated by the following formula: make S k is the covariance of the measurement residuals, then R k It is expressed as: In the formula, R k is the covariance matrix; an adaptive residual-based method is used to estimate R k , the formula is as follows: In the formula, is the actual covariance matrix of the residual, E is the mathematical expectation, ε k is the residual, v k To measure noise, in the Kalman filter Introduce the forgetting factor α to estimate R k , as shown below: In the formula, R k-1 is the covariance matrix at time k-1; make d k is the deviation between the measured value and the predicted value, where For the predicted state prior estimate in the extended Kalman filter, we get: w k-1 =X k -Φ(X k-1 ,u k-1 ) In the formula, Φ(x k ,u k ) is the state transfer function, w k-1 is the process noise at time k-1, X k is the system state vector at time k, X k-1 is the system state vector at time k-1, u k-1 is the input matrix, is the state prediction error, is the state estimate at time k-1, K k is the Kalman gain; therefore: In the formula, is the updated estimated covariance matrix; Introduce the forgetting factor α to estimate Q k ,get: In the formula, Q k is the process noise covariance matrix at time k, Q k-1 is the process noise covariance matrix at time k-1; Finally, the vehicle's position and posture information is calculated.

Images