Self-adaptive unmanned vehicle space attitude estimation method and device suitable for complex mountain environment

Abstract

The invention provides a self-adaptive unmanned vehicle space attitude estimation method suitable for a complex mountain environment. The unmanned parking space attitude estimation method comprises the following steps: firstly, acquiring motion and visual data of a vehicle in real time by using a vehicle-mounted sensor comprising an inertial measurement unit IMU and a camera; secondly, collected camera images and IMU data need to be subjected to preprocessing such as illumination correction, shadow removal and topographic relief compensation so as to eliminate noise and abnormal values; and detecting and extracting key feature points in the image sequence, and tracking the feature points between continuous frames to obtain the motion information of the unmanned vehicle. And performing preliminary camera pose estimation by using the tracked feature point pairs. And finally, combining IMU data with a camera pose estimation result, and carrying out multi-modal data fusion by adopting a re-calibrated iterative extended Kalman filter RIEKF algorithm to obtain pose estimation. The method aims to adapt to different motion conditions and environment changes, and algorithm parameters are automatically adjusted to keep the optimal pose estimation performance.

Description

Technical Field

[0001] The present invention relates to the field of unmanned vehicle posture estimation, and in particular to the field of adaptive posture estimation suitable for complex mountain environments. Background Art

[0002] Pose estimation is a very important research direction in the field of computer vision and machine learning. It plays a vital role in autonomous driving technology, especially in the development of intelligent transportation systems. In order to meet the real-time and accuracy requirements of intelligent transportation, it is necessary to develop better methods to determine the position and direction of unmanned vehicles in three-dimensional space to optimize the navigation and control of unmanned vehicles. Especially in complex mountainous environments, factors such as steep slopes, changing terrain, occlusion and repeated textures will have an adverse effect on the accuracy of pose estimation. Therefore, the accuracy and robustness of pose estimation are particularly important.

[0003] At present, the pose estimation of unmanned vehicles generally uses the data collected by the camera and the data measured by the IMU (inertial measurement unit) to perform pose estimation. Accurate pose estimation is achieved by combining the rich visual information provided by the camera data and the precise motion data provided by the IMU measurement data. In practical applications, the camera's initial estimation usually uses algorithms (such as the scale-invariant feature transform matching algorithm SIFT, etc.) to find the corresponding feature points between the two detection images, and then uses the geometric relationship of these feature points to estimate the camera's pose. However, in dynamic environments, especially in complex mountainous environments, the extraction and matching of feature points may be affected by factors such as lighting changes, occlusion, and repeated textures, resulting in a decrease in the accuracy of the initial estimate. In the case of fast camera motion or blurred images, the detection and tracking of feature points may also be unstable, thereby affecting the accuracy of pose estimation. In addition, the complex terrain and vegetation coverage in mountainous environments can easily lead to deviations and error accumulation in IMU measurement data, which in turn affects the accuracy of pose estimation and brings additional challenges to the accuracy of IMU acquisition data.

[0004] When fusing the data collected by the camera and IMU, different data characteristics need to be processed, and the camera pose is predicted by integrating the IMU measurement values. However, the integration of the IMU's accelerometer and gyroscope measurements over time will produce noise accumulation, and the existence of integral errors will cause the drift of the pose estimation. The accuracy of the relative position and pose between different modules will also affect the final pose estimation results, and it is necessary to continuously correct the estimate based on the observed values ​​during the fusion process. The existing methods still need to be improved in terms of the accuracy and precision of the camera's initial estimate in complex mountainous environments, as well as the reliability of fusion with IMU data. Summary of the invention

[0005] In order to overcome the problems of low accuracy and poor robustness of existing estimation technologies under comprehensive consideration of mountainous environmental conditions, sensor characteristics, model uncertainties and other factors, the present invention provides a pose estimation method suitable for complex mountainous environments. By adaptively adjusting the fusion strategy, more accurate and robust estimation is achieved to meet the pose estimation needs of unmanned vehicles in complex mountainous application scenarios.

[0006] The technical solution adopted by the present invention to solve its technical problem is:

[0007] An adaptive unmanned vehicle posture estimation method suitable for complex mountainous environments, the method comprising the following steps:

[0008] Step 1) Use on-board sensors, including inertial measurement units (IMUs) and cameras, to collect vehicle motion and visual data in real time.

[0009] In step 2), the collected camera images and IMU data need to be preprocessed by illumination correction, shadow removal, and terrain compensation to eliminate noise and outliers, improve the availability of data in complex mountainous environments, and provide high-quality data for subsequent processing.

[0010] Step 3) Detect and extract key feature points in the sequence of camera images and track these feature points between consecutive frames to obtain the motion information of the unmanned vehicle. Feature points that are robust to changes in mountainous environments, such as terrain features and vegetation edges, are specially selected to improve the accuracy of pose estimation.

[0011] Step 4) Between consecutive frames, the feature point information is used to find matching feature point pairs, and the MLPnPL-NGN algorithm (non-negative Gauss-Newton perspective n-point line maximum likelihood estimation algorithm) is used to perform preliminary camera pose solution to obtain the camera pose estimation result.

[0012] Step 5) Combine the IMU data with the camera pose estimation results and use the RIEKF algorithm (recalibrated iterative extended Kalman filter algorithm) for multimodal data fusion. In the correction step of the RIEKF algorithm, the state vector and covariance matrix are updated according to the new measurement data. By iteratively improving the algorithm, the algorithm parameters are adjusted to optimize the pose estimation performance.

[0013] Furthermore, in step 1), the data collected by the vehicle-mounted sensor includes acceleration and angular velocity measured by IMU, image data collected by the camera, etc.

[0014] In the step 2), the preprocessing includes data cleaning and zero drift correction of the raw data accelerometer value a and gyroscope data w measured by the IMU to obtain a c and w c , and the camera raw data M t,t+1={(p i,t ,p j,t+1 )} The distortion-corrected image I obtained by denoising and dedistortion r .

[0015] In the step 2), the pre-processing of the IMU measurement data is mainly performed by setting a threshold to remove outliers and noise spikes, collecting data to calculate zero drift values, correcting the data, and obtaining the required value after simple filtering.

[0016] In the step 2), the preprocessing of the data collected by the monocular camera mainly processes the original image data I through a filter to obtain a filtered image I', and then corrects I' according to the distortion coefficient k of the camera to obtain a distortion-corrected image I r .

[0017] In step 3), the camera acquires continuous frame images I t The feature detection algorithm is used to identify the feature point p i For each detected feature point p i , calculate a descriptor d i , in order to facilitate feature matching. Extract the location information and descriptors of the feature points to form a feature point set F t ={p i,t ,d i,t}.

[0018] In step 4), between consecutive frames, the feature point information is used to find matching feature point pairs, specifically as follows: using feature descriptor d i,t and d j,t+1 Perform matching and find the matching image feature point p i,t and p j,t+1 Constitute the feature point pair M t,t+1 ={(p i,t ,p j,t+1 )}, to establish an algorithm for solving the problem, at least three sets of feature point pairs need to be found.

[0019] In step 5), the recalibrated iterative extended Kalman filter uses IMU observation data to calibrate the predicted posture in the correction step after each prediction step.

[0020] The beneficial effects of the present invention are mainly manifested in:

[0021] Compared with the existing fusion algorithms, the present invention significantly improves the accuracy and robustness of pose estimation by innovatively introducing the non-negative Gauss-Newton perspective n-point line maximum likelihood estimation algorithm MLPnPL-NGN and the recalibrated iterative extended Kalman filter RIEKF algorithm. The MLPnPL-NGN algorithm uses the reprojection errors of point features and line features to achieve accurate preliminary estimation of camera pose by minimizing the objective function. The RIEKF algorithm achieves effective fusion of IMU data and camera pose estimation results by dynamically adjusting the state vector and covariance matrix in the prediction and correction steps. The combination of these two algorithms enables the present invention to automatically adapt to different motion conditions and environmental changes, optimize algorithm parameters, and thus maintain the optimal pose estimation performance. In addition, the present invention further improves the accuracy and robustness of pose estimation by adaptively adjusting the fusion strategy, meeting the high requirements of pose estimation for unmanned vehicles in complex mountainous environments. BRIEF DESCRIPTION OF THE DRAWINGS

[0022] Figure 1 It is a flow chart of adaptive unmanned vehicle pose estimation;

[0023] Figure 2 It is a schematic diagram of a module of an adaptive unmanned vehicle posture estimation device;

[0024] Figure 3 This is the effect diagram of feature point matching of the monocular camera module;

[0025] Figure 4 This is a graph showing the variation of pose estimation errors over time for different methods;

[0026] Figure 5 This is the effect diagram of IMU module data preprocessing;

[0027] Figure 6 This is the effect diagram of IMU and camera data fusion. DETAILED DESCRIPTION

[0028] The present invention will be further described below in conjunction with the accompanying drawings.

[0029] Reference Figure 1 , an adaptive unmanned vehicle posture estimation method suitable for complex mountainous environments, the method comprising the following steps:

[0030] Step 1) Use on-board sensors, including inertial measurement units (IMUs) and cameras, to collect vehicle motion and visual data in real time.

[0031] In step 2), the collected camera images and IMU data need to be preprocessed to eliminate noise and outliers to provide high-quality data for subsequent processing.

[0032] Step 3) Detect and extract key feature points in the image sequence and track these feature points between consecutive frames to obtain the vehicle's motion information.

[0033] Step 4) Using the tracked feature point pairs, a preliminary camera pose estimation is performed using the non-negative Gauss-Newton perspective n-point line maximum likelihood estimation (MLPnPL-NGN) algorithm.

[0034] Step 5) Combine the IMU data with the camera pose estimation results, and use the recalibrated iterative extended Kalman filter (RIEKF) algorithm for multimodal data fusion. In the correction step of the RIEKF algorithm, the state vector and covariance matrix are updated according to the new measurement data. By iteratively improving the algorithm, the algorithm parameters are adjusted to optimize the pose estimation performance.

[0035] Iterate steps 1)-5) to achieve real-time and reliable pose estimation.

[0036] The application method and steps of the present invention are further explained below:

[0037] In step 1), the data collected by the on-board sensors include acceleration and angular velocity measured by the IMU, image data collected by the camera, etc.

[0038] In step 2), the threshold is set to remove outliers and noise spikes; the IMU is left stationary, and the data is collected for a period of time, the average value is calculated as the correction value, and then the original data is corrected. The correction value is simply filtered to obtain the accelerometer and gyroscope values ​​that can be used by subsequent algorithms.

[0039] The preprocessing of the monocular camera mainly processes the original image data I through a filter to obtain the filtered image I', and then corrects I' according to the camera's distortion coefficient k to obtain the distortion-corrected image I r .

[0040] In step 3), the camera acquires continuous frame images I t The feature detection algorithm is used to identify the feature point p i For each detected feature point p i , calculate a descriptor d i , in order to facilitate feature matching. Extract the location information and descriptors of the feature points to form a feature point set F = {(p i ,d i )}.

[0041] In step 4), the image data is preliminarily estimated using the MLPnPL-NGN algorithm.

[0042] The details are as follows:

[0043] 4.1) Before the preliminary estimation by MLPnPL-NGN algorithm, the reprojection error is constructed by matching point pairs. For point features, the following objective function is established:

[0044]

[0045] Where n represents the maximum index of the image; p i represents the i-th observed 2D point in the image; X i represents the position of the corresponding 3D point in the world coordinate system; [R|t] represents the camera pose, which combines the rotation matrix R and the translation vector t. In matrix form, it acts as an enhancement matrix for converting 3D points to 2D points; σ i represents the noise standard deviation of the i-th point measurement, reflecting the magnitude of the measurement error; || || 2 represents the Euclidean norm, which is the length of a vector.

[0046] For line features, the reprojection error can be constructed based on the difference between its projection on the image and the observed 2D line endpoints. The following objective function can be established:

[0047]

[0048] Where m represents the maximum index of the image; represents the transpose of the jth observed 2D line segment in the image; L j Represents the definition of the corresponding 3D line in the world coordinate system; [R|t] represents the camera pose, which combines the rotation matrix R and the translation vector t. In matrix form, it serves as an enhancement matrix for converting 3D points into 2D points.

[0049] 4.2) Perform a preliminary estimate using the MLPnPL-NGN algorithm and design the objective function to minimize the objective function using the objective function of the point feature and line feature reprojection error:

[0050] E(R,t)=E p (R,t)+E l (R,t),

[0051] 4.3) NGN method uses iterative update E p (R,t) and E l The pose parameter [R|t] in (R, t) is used to minimize the objective function. The details are as follows:

[0052] First calculate the gradient And the Hessian matrix H, use the Rodriguez formula to convert the rotation vector θ into the rotation matrix R, and then calculate the gradient. Among them,

[0053] R = exp(θ)

[0054] Here exp(·) represents the exponential mapping of the rotation matrix. Gradient It can be calculated using the chain rule:

[0055]

[0056] J R is the Jacobian matrix from R to θ, is the gradient of the objective function with respect to the rotation vector θ.

[0057] The gradient calculation of the translation vector t is directly derived from the reprojection formula:

[0058]

[0059] Where i is the index, which is the serial number of the feature point observed in the image sequence; J t is the Jacobian matrix of the projection function for the translation vector t.

[0060] For the rotation matrix R, use the Hessian H of the rotation vector θ θ and the Jacobian matrix J R To calculate the Hessian matrix of R:

[0061]

[0062] Among them, H θ is the Hessian matrix of the objective function with respect to the rotation vector θ.

[0063] The Hessian matrix of the translation vector t is calculated as follows:

[0064]

[0065] Among them J t is the Jacobian matrix of the projection function for the translation vector t.

[0066] The NGN algorithm finds the step size Δ by solving the following linear system:

[0067]

[0068] The gradient ▽E and Hessian matrix H are required above.

[0069] Update the pose parameters using the calculated step size Δ:

[0070] [R|t] new =[R|t]+Δ.

[0071] 4.4) Repeat the above steps. After each iteration, check whether the changes of R and t are less than the preset threshold. After convergence, output R and t values ​​as the preliminary pose of the camera.

[0072] In step 5), the RIEKF algorithm is used to fuse the image data and IMU data.

[0073] 5.1) First define the state vector x k :

[0074]

[0075] Among them, R k is the rotation matrix of the camera relative to the world coordinate system, t k is the camera's translation vector, v k is the velocity vector of the camera, b a is the bias of the accelerometer, b g is the bias of the gyroscope.

[0076] 5.2) The prediction model predicts the current state based on the IMU data and the state estimation at the previous moment. The prediction model formula is as follows:

[0077]

[0078] Control input u k represents the measurement value of IMU; f is the state transfer function.

[0079] 5.3) The observation model associates the feature points observed by the camera with the current state vector, which is expressed as follows:

[0080] z k =h(x k )+v k

[0081] z k is the observed feature point; v k is the observation noise; h is the observation model function, which transforms the state vector x k Mapped to the observation space.

[0082] 5.4) Through the Jacobian matrix H k Describe how the observation depends on the state vector:

[0083]

[0084] 5.5) The process noise covariance Q k and the observation noise covariance R k Defined as:

[0085]

[0086] Q ω and Q a are the process noise covariance of the gyroscope and accelerometer, respectively. p and R l are the observation noise covariance of point features and line features, respectively.

[0087] 5.6) Perform RIEKF iteration:

[0088] For each time step k, the following execution steps are performed:

[0089] 5.6.1) Prediction:

[0090]

[0091] f is a nonlinear function that predicts the current state based on the state estimate and control input at the previous moment; x k-1|k-1 is the optimal state estimate at the previous moment, including the rotation matrix, translation vector, velocity vector, and sensor bias; the control input u k Used to predict the current state in the state transfer function; It is the predicted state, which predicts the current state based on the state estimate and control input at the previous moment. k is the process noise covariance matrix, which describes the uncertainty in the prediction process; F k is the Jacobian matrix, which describes the partial derivative of each element in the state vector with respect to the state transfer function; P k|k-1 The forecast error covariance matrix describes the uncertainty of the forecast state. k-1|k-1 is the prediction error covariance matrix of the previous moment.

[0092] 5.6.2) Update:

[0093]

[0094] P k|k =(IK k H k ) k|k-1

[0095] h observation model function, a function that maps the state vector to the observation space; z k is the observation vector, which is the observation data obtained by direct measurement; H k is the observation Jacobian matrix, which describes the partial derivative of the observation vector with respect to each element in the state vector and is used to calculate the Kalman gain; R k is the observation noise covariance matrix, which describes the uncertainty in the observation process; K k is the Kalman gain, which is used to update the state estimation gain; Pk|k Update error covariance matrix, describing the uncertainty of the updated state.

[0096] 5.6.3) Recalibration:

[0097] Adjust the IMU predicted pose according to the camera pose estimate, and update the state vector and covariance matrix.

[0098] If the update of the state vector is less than the set threshold, the algorithm is considered to have converged. Output the final camera rotation matrix R k , translation vector t, velocity vector v k , and the sensor bias b a , b g As the pose estimation result.

[0099] The method of this embodiment is implemented based on the device module. Figure 2 Schematic diagram of the device module. The device module of this embodiment includes an IMU module for collecting IMU information; a monocular camera module for collecting image data; an IMU preprocessing module for preprocessing the collected IMU data and providing the information to the fusion module; a camera preprocessing module for preprocessing the collected image, detecting and extracting feature points, and then providing them to the estimation module, and using the MLPnPL-NGN algorithm to obtain a preliminary estimate; the data fusion module uses the RIEKF algorithm to obtain a pose estimate, and the final pose estimation result is obtained after repeated recalibration by the recalibration module.

[0100] The camera feature point matching in this embodiment is as follows: Figure 3 As shown in Figure 2, the pose estimation errors of different methods vary with time. Figure 4 As shown, the effect of the IMU in this embodiment after preprocessing is as follows Figure 5 As shown, the fusion effect of this embodiment is as follows Figure 6 shown.

[0101] Through the above method and device, the present invention can adapt to different motion conditions and environmental changes, and automatically adjust the calculation parameters to maintain the optimal posture estimation performance.

Claims

1. An adaptive unmanned vehicle pose estimation method suitable for complex mountainous environments, characterized in that: The method comprises the following steps: Step 1) using vehicle-mounted sensors to collect the vehicle's motion and visual data in real time; the vehicle-mounted sensors include an inertial measurement unit (IMU) and a camera; Step 2) preprocessing the collected IMU data and camera images to eliminate noise and outliers to improve the availability of data in complex mountain environments; Step 3) detecting and extracting key feature points in the image sequence, and tracking the key feature points between consecutive frames to obtain the motion information of the unmanned vehicle; the key feature points are feature points that are robust to changes in the mountainous environment to improve the accuracy of pose estimation; Step 4) Between consecutive frames, the feature point information is used to find matching feature point pairs, and a preliminary camera pose estimation is performed using the non-negative Gauss-Newton perspective n-point line maximum likelihood estimation MLPnPL-NGN algorithm; Step 5) Combine the IMU data with the camera pose estimation results and use the recalibrated iterative extended Kalman filter (RIEKF) algorithm for multimodal data fusion; in the correction step of the RIEKF algorithm, update the state vector and covariance matrix according to the new measurement data; and adjust the algorithm parameters by iteratively improving the algorithm to optimize the pose estimation performance.

2. The method for adaptive unmanned vehicle posture estimation suitable for complex mountainous environments as claimed in claim 1, characterized in that: In step 1, the data collected by the vehicle-mounted sensor includes acceleration and angular velocity collected by an inertial measurement unit (IMU), and image data collected by a camera.

3. The method for adaptive unmanned vehicle posture estimation suitable for complex mountainous environments as claimed in claim 1, characterized in that: In step 2, the preprocessing process of the acceleration and angular velocity collected by the inertial measurement unit IMU is as follows: By setting the threshold to remove outliers and noise spikes, collecting data under static conditions to calculate the zero drift value, correcting the data, and obtaining the required value after filtering; The preprocessing process of the image data collected by the camera is as follows: The original image data I is processed by the filter to obtain the filtered image I', and then I' is corrected according to the camera's distortion coefficient k to obtain the distortion-corrected image I rect .

4. The method for adaptive unmanned vehicle posture estimation suitable for complex mountainous environments as claimed in claim 1, characterized in that: In step 3), the camera acquires continuous frame images I t The feature detection algorithm is used to identify the feature point p i ; For each detected feature point p i , calculate a descriptor d i , in order to facilitate feature matching; extract the location information and descriptors of the feature points to form a feature point set F t ={p i,t ,d i,t }.

5. The method for adaptive unmanned vehicle posture estimation suitable for complex mountainous environments as claimed in claim 1, characterized in that: In step 4), between consecutive frames, matching feature point pairs are found using feature point information, as follows: Using feature descriptor d i,t and d j,t+1 Match and find the matching feature point pair M t,t+1 ={(p i,t ,p j,t+1 )}, when establishing the algorithm for solving, the number of feature point pairs is at least three; The specific process of preliminary camera pose estimation using the MLPnPL-NGN algorithm is as follows: 4.1) Before the preliminary estimation by MLPnPL-NGN algorithm, the reprojection error is constructed by using matching point pairs to establish the objective function of point features and line features; 4.2) Perform a preliminary estimate using the MLPnPL-NGN algorithm and design the objective function to minimize the objective function using the objective function of the point feature and line feature reprojection error; 4.3) Minimize the objective function by iteratively updating the pose parameters in the point feature objective function and the line feature objective function; 4.4) Repeat steps 4.1)-4.3) and when the objective function value converges, output the initial pose of the camera.

6. The method for adaptive unmanned vehicle posture estimation suitable for complex mountainous environments as claimed in claim 1, characterized in that: In step 5, the recalibrated iterative extended Kalman filter uses the inertial measurement unit IMU to collect data to calibrate the predicted posture in the correction step after each prediction step.

7. The method for adaptive unmanned vehicle posture estimation suitable for complex mountainous environments as claimed in claim 1, characterized in that: Based on the device module implementation, the device module includes: IMU module, used to collect IMU information; Monocular camera module, used to collect image data; IMU preprocessing module, used to preprocess the collected IMU data and provide the information to the fusion module; The camera preprocessing module is used to preprocess the acquired images, detect and extract feature points, and then provide them to the estimation module to obtain a preliminary estimate using the MLPnPL-NGN algorithm; The data fusion module uses the RIEKF algorithm to obtain the pose estimation, and the final pose estimation result is obtained after repeated recalibration by the recalibration module.

Images