Adaptive unmanned vehicle pose estimation method and device suitable for complex mountainous environment

Abstract

The application provides a self-adaptive unmanned vehicle pose estimation method suitable for complex mountainous environments. The unmanned vehicle pose estimation method comprises the following steps: first, using vehicle-mounted sensors, including an inertial measurement unit (IMU) and a camera, to collect real-time motion and visual data of the vehicle. Second, the collected camera images and IMU data need to be preprocessed, such as illumination correction, shadow removal and terrain undulation compensation, to eliminate noise and outliers. Then, key feature points are detected and extracted in the image sequence, and these feature points are tracked between consecutive frames to obtain the motion information of the unmanned vehicle. Using the tracked feature point pairs, a preliminary camera pose estimation is performed. Finally, the IMU data and the camera pose estimation results are combined, and a re-calibrated iterative extended Kalman filter (RIEKF) algorithm is used for multi-modal data fusion to obtain the pose estimation. The application aims to adapt to different motion conditions and environmental changes, and automatically adjust algorithm parameters to maintain optimal pose estimation performance.

Description

TECHNICAL FIELD

[0001] The present application relates to the field of pose estimation of unmanned vehicles, and particularly to the field of adaptive pose estimation suitable for complex mountainous environments. BACKGROUND

[0002] Pose estimation is a very important research direction in the field of computer vision and machine learning, and plays a crucial 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, better methods need to be developed to determine the position and orientation of unmanned vehicles in three-dimensional space, in order to optimize the navigation and control of unmanned vehicles. In particular, in complex mountainous environments, factors such as steep slopes, variable terrain, occlusions and repetitive textures can adversely affect the accuracy of pose estimation, so the accuracy and robustness of pose estimation are particularly important.

[0003] Currently, unmanned vehicle pose estimation generally uses camera-acquired data and IMU (Inertial Measurement Unit) measured data for pose estimation, combining the rich visual information provided by camera-acquired data and the accurate motion data provided by IMU measured data to achieve accurate pose estimation. In practical applications, camera preliminary estimation usually uses algorithms such as Scale-Invariant Feature Transform matching algorithm (SIFT) to find corresponding feature points between two detected images, and then uses the geometric relationship of these feature points to estimate the camera pose. However, in dynamic environments, especially in complex mountainous environments, the extraction and matching of feature points can be affected by factors such as changes in lighting, occlusions and repetitive textures, resulting in a decrease in the accuracy of preliminary estimation. In the case of fast camera motion or image blur, feature point detection and tracking can also be unstable, affecting the accuracy of pose estimation. In addition, the complex terrain and vegetation cover in mountainous environments can easily lead to bias and error accumulation in IMU measured data, which in turn affects the accuracy of pose estimation, posing additional challenges to the accuracy of IMU-acquired data.

[0004] When fusing the data acquired by the camera and the IMU, different data characteristics need to be handled, and the camera pose is predicted by integrating the IMU measurement values. However, the integration of accelerometer and gyroscope measurements of the IMU over time will produce noise accumulation, and the existence of integration error will cause drift in the attitude estimation, and the accuracy of the relative position and attitude between different modules will also affect the final pose estimation result, which needs to be continuously corrected according to the observation value during the fusion process. The existing methods still need to be improved in terms of the accuracy and precision of camera preliminary estimation and the reliability of fusion with IMU data in complex mountainous environments. SUMMARY

[0005] To overcome the problems of low accuracy and poor robustness of existing estimation techniques when considering factors such as mountainous environmental conditions, sensor characteristics, and model uncertainties, this invention provides a pose estimation method suitable for complex mountainous environments. By adaptively adjusting the fusion strategy, it achieves more accurate and robust estimation to meet the pose estimation needs of unmanned vehicles in complex mountainous application scenarios.

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

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

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

[0009] Step 2) The acquired camera images and IMU data need to undergo preprocessing such as illumination correction, shadow removal and terrain undulation compensation to eliminate noise and outliers, improve the usability 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 motion information of the autonomous vehicle. Feature points that are robust to changes in mountainous environments, such as terrain features and vegetation edges, are specifically selected to improve the accuracy of pose estimation.

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

[0012] Step 5) Combine the IMU data with the camera pose estimation results, and perform multimodal data fusion using the RIEKF algorithm (Recalibrated Iterative Extended Kalman Filter). In the calibration step of the RIEKF algorithm, the state vector and covariance matrix are updated based on the new measurement data. The algorithm parameters are adjusted through iterative improvement 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 the IMU, image data collected by the camera, etc.

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

[0015] In step 2), the preprocessing of IMU measurement data mainly involves setting thresholds to remove outliers and noise spikes, collecting data to calculate zero drift values, correcting the data, and obtaining the desired value after simple filtering.

[0016] In step 2), the preprocessing of the monocular camera acquisition data mainly involves processing the original image data I with a filter to obtain the filtered image I', and then correcting I' according to the camera's distortion coefficient k to obtain the distortion-corrected image I. r .

[0017] In step 3), the camera acquires consecutive frame images I t The algorithm used in this paper is a feature detection algorithm to identify feature points p. i For each detected feature point p i Calculate a descriptor d i This facilitates feature matching. The location information and descriptors of the feature points are extracted to form a feature point set F. t ={p i,t ,d i,t}

[0018] In step 4), matching feature point pairs are found between consecutive frames using feature point information, specifically as follows: Feature descriptor d is used. i,t and d j,t+1 Perform matching to find matching image feature points p i,t and p j,t+1 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 pairs of feature points need to be found.

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

[0020] The beneficial effects of this invention are mainly reflected in:

[0021] Compared to existing fusion algorithms, this invention significantly improves the accuracy and robustness of pose estimation by innovatively introducing the non-negative Gaussian-Newton perspective n-point line maximum likelihood estimation algorithm MLPnPL-NGN and the recalibrated iterative extended Kalman filter (RIEKF) algorithm. The MLPnPL-NGN algorithm utilizes the reprojection error of point and line features to achieve accurate preliminary camera pose estimation by minimizing the objective function. The RIEKF algorithm, on the other hand, 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. This combination of algorithms allows the invention to automatically adapt to different motion conditions and environmental changes, optimizing algorithm parameters to maintain optimal pose estimation performance. Furthermore, this invention further improves the accuracy and robustness of pose estimation through adaptive adjustment of the fusion strategy, meeting the high requirements of unmanned vehicles for pose estimation in complex mountainous environments. Attached Figure Description

[0022] Figure 1 This is a flowchart of adaptive autonomous vehicle pose estimation;

[0023] Figure 2 This is a schematic diagram of an adaptive autonomous vehicle pose estimation device module;

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

[0025] Figure 4 This is a graph showing how pose estimation errors vary over time using different methods;

[0026] Figure 5 This is a diagram showing the results of data preprocessing in the IMU module;

[0027] Figure 6 This is a diagram showing the effect of fusion between IMU and camera data. Detailed Implementation

[0028] The present invention will now be further described with reference to the accompanying drawings.

[0029] Reference Figure 1 An adaptive pose estimation method for unmanned vehicles suitable for complex mountainous environments, the method comprising the following steps:

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

[0031] Step 2) The acquired camera images and IMU data need to be preprocessed to eliminate noise and outliers, providing 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 vehicle motion information.

[0033] Step 4) Using the tracked feature point pairs, perform preliminary camera pose estimation using the non-negative Gaussian-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 calibration step of the RIEKF algorithm, the state vector and covariance matrix are updated based on the new measurement data. By iteratively improving the algorithm, the algorithm parameters are adjusted to optimize the pose estimation performance.

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

[0036] The following further clarifies the application methods and steps of the present invention:

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

[0038] In step 2), outliers and noise spikes are removed by setting thresholds; the IMU is left idle, and data is collected over a period of time. The average value is calculated as the correction value, and then the original data is corrected. The correction value is then filtered to obtain accelerometer and gyroscope values ​​that can be used in subsequent algorithms.

[0039] The preprocessing of a monocular camera mainly involves processing the original image data I with a filter to obtain the filtered image I'. Then, I' is corrected according to the camera's distortion coefficient k to obtain the distortion-corrected image I. r .

[0040] In step 3), the camera acquires consecutive frame images I t The algorithm used in this paper is a feature detection algorithm to identify feature points p. i For each detected feature point p i Calculate a descriptor d i This facilitates feature matching. The location information and descriptors of the feature points are extracted to form a feature point set F = {(p i ,d i )}.

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

[0042] Specifically as follows:

[0043] 4.1) Before initial estimation using the MLPnPL-NGN algorithm, reprojection error is constructed using 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 X represents the i-th observed 2D point in the image; i This 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 serves as an enhancement matrix used to convert 3D points into 2D points; σ i This represents the noise standard deviation of the measurement at the i-th point, reflecting the magnitude of the measurement error; || || 2 This represents the Euclidean norm, which is the length of the vector.

[0046] For line features, the reprojection error can be constructed based on the difference between its projection onto 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; L represents the transpose of the j-th observed 2D line segment in the image; j [R|t] 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 used to convert 3D points into 2D points.

[0049] 4.2) A preliminary estimate is made using the MLPnPL-NGN algorithm. The objective function to minimize the reprojection error is designed using point features and line features:

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

[0051] 4.3) The NGN method utilizes iterative updates to E p (R,t) and E l The objective function is minimized using the pose parameters [R|t] in (R,t). Specifically:

[0052] First, calculate the gradient. And the Hessian matrix H, using the Rodriguez formula to transform the rotation vector θ into a rotation matrix R, and then calculating the gradient. Wherein,

[0053] R = exp(θ)

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

[0055]

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

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

[0058]

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

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

[0061]

[0062] Where H θ It 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] J t It is the Jacobian matrix of the projection function with respect to the translation vector t.

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

[0067]

[0068] The gradient ▽E and the Hessian matrix H are the results obtained 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 in R and t are less than the preset threshold. After convergence, output the values ​​of R and t as the initial pose of the camera.

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

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

[0074]

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

[0076] 5.2) The prediction model predicts the current state based on IMU data and the state estimate from the previous time step. The prediction model formula is as follows:

[0077]

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

[0079] 5.3) The observation model associates the feature points observed by the camera with the current state vector, as shown below:

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

[0081] z k These are the observed feature points; v k It is the observation noise; h is the observation model function, which takes the state vector x k Mapped to the observation space.

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

[0083]

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

[0085]

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

[0087] 5.6) Perform RIEKF iteration:

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

[0089] 5.6.1) Prediction:

[0090]

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

[0092] Update 5.6.2:

[0093]

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

[0095] h is the observation model function, which maps the state vector to the observation space; z k H is the observation vector, representing the observation data obtained through direct measurement. k The observation Jacobian matrix describes the partial derivatives of the observation vector with respect to each element of the state vector, used to calculate the Kalman gain; R k The observation noise covariance matrix describes the uncertainty in the observation process; K k P is the Kalman gain, used to update the state estimate;k|k The update error covariance matrix describes the uncertainty of the updated state.

[0096] 5.6.3) Recalibration:

[0097] Adjust the IMU predicted pose based on the camera pose estimation, and update the state vector and covariance matrix.

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

[0099] The method in this embodiment is implemented based on a device module. Figure 2 The diagram shows the device module in this embodiment. The device module includes an IMU module for acquiring IMU information; a monocular camera module for acquiring image data; an IMU preprocessing module for preprocessing the acquired IMU data and providing the information to the fusion module; a camera preprocessing module for preprocessing the acquired images, detecting and extracting feature points, and then providing them to the estimation module, which uses the MLPnPL-NGN algorithm to obtain a preliminary estimate; and a data fusion module that uses the RIEKF algorithm to obtain a pose estimate, which is then repeatedly recalibrated by the recalibration module to obtain the final pose estimate result.

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

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

Claims

1. An adaptive pose estimation method for unmanned vehicles suitable for complex mountainous environments, characterized in that, The method includes the following steps: Step 1) Use onboard sensors to collect vehicle motion and visual data in real time; the onboard sensors include an inertial measurement unit (IMU) and a camera; Step 2) Preprocess the acquired IMU data and camera images to remove noise and outliers, thereby improving the usability of the data in complex mountainous environments; Step 3) Detect and extract key feature points in the image sequence, and track 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, matching feature point pairs are found using feature point information, and preliminary camera pose estimation is performed using the non-negative Gaussian-Newton perspective n-point line maximum likelihood estimation (MLPnPL-NGN) algorithm. Specifically, in step 4), finding matching feature point pairs between consecutive frames using feature point information is as follows: Using feature descriptor d i,t and d j,t+1 Perform matching to find matching feature point pairs M t,t+1 ={(p i,t ,p j,t+1 When establishing the algorithm for solving the problem, the number of feature point pairs must be at least three. The specific process of preliminary camera pose estimation using the MLPnPL-NGN algorithm is as follows: 4.1) Before performing preliminary estimation using the MLPnPL-NGN algorithm, matching point pairs are used to construct the reprojection error, and objective functions for point features and line features are established. 4.2) A preliminary estimate is made using the MLPnPL-NGN algorithm, and an objective function is designed to minimize the reprojection error using point features and line features. 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) until the objective function value converges, then output the initial pose of the camera; Step 5) Combine the IMU data with the camera pose estimation results and use the recalibrated iterative extended Kalman filter (RIEKF) algorithm to perform multimodal data fusion; in the calibration step of the RIEKF algorithm, update the state vector and covariance matrix according to the new measurement data; through iterative improvement of the algorithm, adjust the algorithm parameters to optimize the pose estimation performance.

2. The adaptive unmanned vehicle pose estimation method applicable to complex mountainous environments as described in claim 1, characterized in that, In step 1), the data collected by the vehicle-mounted sensors includes acceleration and angular velocity collected by the inertial measurement unit (IMU) and image data collected by the camera.

3. The adaptive unmanned vehicle pose estimation method applicable to complex mountainous environments as described in claim 1, characterized in that, In step 2), the preprocessing of the acceleration and angular velocity acquired by the inertial measurement unit (IMU) is as follows: By setting a threshold to remove outliers and noise spikes, data is collected under static conditions to calculate zero drift values, the data is corrected, and the desired value is obtained after filtering. The preprocessing process for image data acquired by the camera is as follows: The original image data I is processed by a filter to obtain the filtered image I'. Then, I' is corrected according to the camera's distortion coefficient k to obtain the distortion-corrected image I. rect .

4. The adaptive unmanned vehicle pose estimation method applicable to complex mountainous environments as described in claim 1, characterized in that, In step 3), the camera acquires consecutive frame images I t The algorithm used in this paper is a feature detection algorithm to identify feature points p. i For each detected feature point p i Calculate a descriptor d i This facilitates feature matching; the location information and descriptors of feature points are extracted to form a feature point set F. t ={p i,t ,d i,t } 5. The adaptive unmanned vehicle pose estimation method applicable to complex mountainous environments as described in claim 1, characterized in that, In step 5), the recalibrated iterative extended Kalman filter uses an inertial measurement unit (IMU) to collect data to calibrate the predicted pose in the correction step after each prediction step.

6. The adaptive unmanned vehicle pose estimation method applicable to complex mountainous environments as described in claim 1, characterized in that, Based on device modules, the device modules include: The IMU module is used to collect IMU information; A monocular camera module is used to acquire image data; The IMU preprocessing module is used to preprocess the acquired 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 estimate, and after repeated recalibration by the recalibration module, the final pose estimate result is obtained.

Images