A dual-wheel detective ball image feedback posture correction system

Abstract

The application discloses a kind of double-wheel investigation ball image feedback posture correction systems, belong to image correction technical field.System includes multi-source sensing acquisition module, data consistency inspection and pre-correction module, time synchronization and state recursion module, visual observation and filter updating module and image posture correction module, data consistency inspection and pre-correction module carry out consistency inspection to the plane pose variation of wheel speed calculation and inertial navigation pose variation, and pre-correction image for feature extraction is generated according to the check result;Visual observation and filter updating module extract features from pre-correction image and construct re-projection error observation equation, obtain current frame pose estimation value after filter updating, simultaneously adjust process noise covariance matrix according to visual re-projection residual statistics characteristics, and process noise covariance matrix, pose estimation value and error covariance matrix are fed back to time synchronization module, finally solve the image posture continuous drift problem under obstacle, skidding and impact working condition.

Description

Technical Field

[0001] This invention relates to the field of image correction technology, and in particular to a dual-wheel reconnaissance ball image feedback attitude correction system. Background Technology

[0002] The dual-wheeled reconnaissance ball is a spherical robot used for reconnaissance in complex environments. It walks on complex indoor and outdoor terrains using its drive wheels and is equipped with a camera to collect environmental images for remote reconnaissance and autonomous navigation. When the reconnaissance ball is performing reconnaissance tasks, its onboard camera needs to output stable, distortion-free images for operators to observe or for subsequent visual tasks. However, during the movement of the reconnaissance ball, due to factors such as friction between the drive wheels and the ground, obstacle crossing impacts, and single-wheel slippage, random three-dimensional attitude changes, including roll, pitch, and yaw, will occur, causing rigid body deformation in the output images, such as tilting and rotation, which seriously affects the usability of the images and the operator's visual experience.

[0003] Existing technologies can estimate the device's pose in real time by fusing data from cameras and inertial measurement units (IMUs), and perform geometric transformations on the image based on the pose estimate to correct image distortion caused by the pose. However, in visually degraded scenarios such as no texture or drastic changes in lighting, the pose estimation accuracy of pure vision and inertial systems decreases significantly, resulting in poor output image correction. To compensate for this deficiency, some existing technologies further introduce wheel speed meters to provide additional planar motion constraints through wheel speed data, hoping to improve the robustness of pose estimation. At the same time, in order to improve the difficulty of feature matching caused by large pose changes between adjacent frames, existing technologies usually perform pre-deformation correction (i.e., image pre-correction) on the current frame image based on a single motion prediction value obtained by fusing wheel speed and inertial measurement unit (IMU) data. This makes the pre-corrected image closer to the viewpoint of the previous frame, thereby reducing the difficulty of feature matching between frames.

[0004] However, existing technologies still have the following drawbacks: When the dual-wheeled reconnaissance ball overcomes obstacles, slips on a single wheel, or is subjected to a momentary impact, the wheel speed data fails due to wheel-ground contact failure, and the displacement calculated from it contains spurious components, which conflict with the true attitude changes measured by the IMU. Existing technologies fail to actively detect this conflict of multi-source sensor data during the image pre-correction stage, and still directly fuse the displacement calculated from the wheel speed with the IMU measurement value to solve a single homography transformation matrix. As a result, the homography matrix contains both true attitude rotation and spurious planar displacement, causing local stretching and geometric distortion in the pre-corrected image, which in turn causes pixel position distortion of subsequent feature points and offset of feature matching between adjacent frames. Furthermore, the process noise covariance matrix of the extended Kalman filter in existing technologies uses a fixed value, which cannot adaptively adjust the trust weight of the filter on the prediction model according to the degradation of the front-end pre-correction quality. This leads to erroneous matching results continuously polluting the state estimation of subsequent frames, ultimately resulting in continuous image attitude drift.

[0005] Therefore, this invention proposes a dual-wheel reconnaissance ball image feedback attitude correction system. Summary of the Invention

[0006] The purpose of this invention is to provide a dual-wheel reconnaissance ball image feedback attitude correction system, which solves the problems in the prior art where dual-wheel reconnaissance balls cause wheel speed and IMU data conflicts due to obstacle crossing, single wheel slippage or instantaneous impact in complex working environments, resulting in image pre-correction distortion, feature matching offset and attitude estimation continuous drift.

[0007] To achieve the above objectives, the present invention provides the following technical solution: a dual-wheel reconnaissance ball image feedback attitude correction system, comprising: a multi-source sensor acquisition module, used for: acquiring environmental image sequences, inertial measurement data, and wheel speed data during the movement of the dual-wheel reconnaissance ball; a time synchronization and state recursion module, used for: performing time interpolation calibration on the inertial measurement data based on the acquisition time of the environmental image sequence to obtain aligned attitude data, and recursively calculating the prior pose estimate and corresponding error covariance matrix at the acquisition time of each frame image; and a data consistency verification and pre-correction module, used for: calculating the predicted value of the planar pose change of the current frame relative to the previous frame based on the wheel speed data, and obtaining the prior pose estimate output by the time synchronization and state recursion module. The inertial navigation pose change prediction value is used to verify the consistency between the planar pose change prediction value and the inertial navigation pose change prediction value, and a pre-corrected image is generated based on the verification result for feature extraction. The visual observation and filtering update module is used to: extract the feature point matching relationship between adjacent frames from the pre-corrected image to construct the visual reprojection error observation equation, perform filtering and updating based on the error covariance matrix of the prior pose estimate and the reprojection error observation equation to obtain the pose estimate of the current frame, and feed back the pose estimate of the current frame and the corresponding error covariance matrix to the time synchronization and state recursion module. The image pose correction module is used to: perform geometric transformation on the current frame environment image based on the pose estimate of the current frame to generate a corrected output image.

[0008] Furthermore, the environmental image sequence, inertial measurement data, and wheel speed data acquired by the multi-source sensing acquisition module are all equipped with a unified timestamp, and the acquisition trigger signal of each frame of the environmental image sequence is used as the reference for time alignment of the entire system.

[0009] Furthermore, the time synchronization and state recursion module uses the acquisition timestamp of each frame of environmental image as a reference to interpolate and calibrate the angular velocity and attitude angle data of the inertial measurement data to obtain attitude data aligned with the acquisition time of each frame of environmental image. Based on the attitude data, the prior pose estimation value of each frame of environmental image is recursively obtained through the inertial navigation recursion algorithm, and the corresponding error covariance matrix is ​​calculated based on the inertial measurement noise model through error propagation.

[0010] Furthermore, the data consistency check includes: converting the predicted values ​​of the planar pose change and the predicted values ​​of the inertial navigation pose change to the same coordinate system, calculating the displacement deviation and heading angle deviation between the two, and determining that the consistency check passes when the displacement deviation and heading angle deviation do not exceed a preset threshold; otherwise, determining that the consistency check fails.

[0011] Furthermore, the displacement deviation is determined by the difference between the planar displacement calculated from the wheel speed data and the planar displacement component in the predicted value of the inertial navigation attitude change, and the heading angle deviation is determined by the difference between the heading angle change calculated from the wheel speed data and the heading angle component in the predicted value of the inertial navigation attitude change.

[0012] Furthermore, when the consistency check passes, the data consistency check and pre-correction module fuse the predicted values ​​of planar pose change and inertial navigation pose change to obtain the predicted values ​​of all-dimensional pose change. Based on the predicted values ​​of all-dimensional pose change, each pixel in the current frame image is back-projected to a three-dimensional space point in the camera coordinate system based on the pixel depth values ​​provided by the depth camera. After performing a rigid body transformation on the three-dimensional space point, it is projected back onto the image plane, and a pre-correction image is generated pixel by pixel to compensate for the rigid body deformation of the image caused by the planar motion and three-dimensional pose change between adjacent frames.

[0013] Furthermore, when the consistency check fails, the data consistency check and pre-correction module solves the image homography transformation matrix only based on the rotation component in the predicted value of the inertial navigation pose change, performs reverse pixel mapping transformation on the current frame environment image, compensates for the rigid body deformation of the image caused by the three-dimensional pose change between adjacent frames, and generates a pre-corrected image.

[0014] Furthermore, the visual observation and filtering update module extracts corner features and corresponding descriptors from the current frame pre-corrected image and the previous frame pre-corrected image, respectively. It generates preliminary matching point pairs through feature descriptor matching and uses a random sampling consensus algorithm to eliminate mismatched point pairs, thereby obtaining a stable feature point matching relationship between adjacent frames.

[0015] Furthermore, the visual observation and filtering update module is based on the feature point matching relationship between adjacent frames, takes the pixel coordinate deviation of the matching feature points in adjacent frames as the observation, constructs the visual reprojection error observation equation based on the prior pose estimation value and the camera intrinsic parameters, and linearizes the reprojection error observation equation to obtain the observation matrix and the observation noise matrix.

[0016] Furthermore, the visual observation and filtering update module uses extended Kalman filtering to perform state updates. It calculates the Kalman gain based on the error covariance matrix, observation matrix, and observation noise matrix corresponding to the prior pose estimate, corrects the prior pose estimate and error covariance matrix, and obtains the updated current frame pose estimate and error covariance matrix. Simultaneously, it calculates the statistical characteristics of the current frame visual reprojection residual, adjusts the process noise covariance matrix in the inertial navigation recursive algorithm based on these statistical characteristics, and adjusts the prediction confidence in the next round of filtering fusion. The current frame pose estimate and its corresponding error covariance matrix are then fed back to the time synchronization and state recursion module as the initial conditions for the recursion of the next frame's prior pose estimate.

[0017] Compared with existing technologies, the beneficial effects of this invention are as follows: In existing technologies, when a dual-wheeled reconnaissance ball encounters obstacles, single-wheel slippage, or instantaneous impacts, the displacement calculated from the wheel speed becomes distorted due to wheel-ground contact failure, conflicting with the actual attitude changes of the IMU. Furthermore, existing technologies fail to actively detect conflicts in multi-source sensor data during the image pre-correction stage, directly fusing the displacement calculated from the wheel speed with the IMU measurement to solve a single homography transformation matrix. This results in local stretching and geometric distortion of the pre-corrected image, leading to feature matching offset and continuous attitude estimation drift. In contrast, this invention, through data consistency verification and a pre-correction module, verifies the consistency between the planar pose change calculated from the wheel speed and the inertial navigation pose change, and switches branches based on the verification results. The pre-correction strategy integrates full-dimensional motion information when the test passes and actively discards unreliable translational components in wheel speed data when the test fails, retaining only the inertial navigation rotation component for pre-correction. This avoids the injection of contradictory data into the homography matrix, which can cause local image stretching. Simultaneously, the visual observation and filter update module dynamically adjusts the process noise covariance matrix based on the statistical characteristics of the visual reprojection residual, quantifying the pre-correction quality as a filter confidence parameter. When the pre-correction quality deteriorates, the strategy actively reduces the dependence on the prediction model and enhances the correction weight of visual observation. Finally, the updated process noise covariance matrix, pose estimation value, and error covariance matrix are fed back to the time synchronization module. This systematically solves the problems of local image stretching, feature matching offset, and continuous attitude drift caused by the conflict between wheel speed and IMU data. Attached Figure Description

[0018] Figure 1 This is a block diagram of the overall structure of the dual-wheel reconnaissance ball image feedback attitude correction system of the present invention; Figure 2 This is a flowchart illustrating the data consistency check and branching pre-correction process of the present invention. Figure 3 This is a schematic diagram of the EKF update and adaptive feedback process of the visual observation and filtering update module of the present invention. Detailed Implementation

[0019] The technical solutions of the embodiments of the present invention will be clearly and completely described below with reference to the accompanying drawings. Obviously, the described embodiments are only some embodiments of the present invention, and not all embodiments. All other embodiments obtained by those skilled in the art based on the embodiments of the present invention without creative effort are within the scope of protection of the present invention.

[0020] To address the inconsistencies between wheel speed data, inertial measurement unit (IMU) data, and image motion when a dual-wheeled reconnaissance ball encounters obstacles, single-wheel slippage, or instantaneous impacts, pre-correction based on unified pose prediction and a single homography matrix is ​​used. This leads to technical problems such as feature matching offset, local image stretching, and continuous attitude estimation drift. Figures 1-3 As shown, the following preferred technical solution is provided: a dual-wheeled reconnaissance ball image feedback attitude correction system, comprising: a multi-source sensor acquisition module, used for: acquiring environmental image sequences, inertial measurement data, and wheel speed data during the movement of the dual-wheeled reconnaissance ball; the environmental image sequence is acquired by a depth camera mounted on the dual-wheeled reconnaissance ball at a frequency of 30Hz, acquiring the current frame image and the previous frame image for subsequent feature extraction and matching; the inertial measurement data is acquired using a six-axis IMU, and the acquired data includes three-axis accelerometer data. and three-axis gyroscope data The data acquisition frequency is 200Hz. An accelerometer measures the linear acceleration of the reconnaissance ball in the carrier coordinate system, and a gyroscope measures the angular velocity of the reconnaissance ball in the carrier coordinate system. Wheel speed data is acquired by wheel encoders mounted on the left and right drive wheels at a frequency of 100Hz. This data is used to record the pulse count or rotational speed of the left and right wheels, and is then calibrated and converted into the linear velocity of the left and right wheels. Since the sampling frequencies of environmental image sequences, inertial measurement data, and wheel speed data are different, if the data is marked only with their original sampling times, the subsequent time synchronization and state recursion modules will not be able to associate the three to the same time for interpolation calibration and fusion processing. Therefore, the three need to be equipped with a unified timestamp, and the acquisition trigger signal of each frame of environmental image in the environmental image sequence is used as the reference for the time alignment of the entire system. Specifically, when the camera exposure begins, the hardware trigger signal simultaneously latches the latest data of the IMU and wheel speed meter, and the three are marked with the same timestamp.

[0021] The time synchronization and state recursion module is used to: perform time interpolation calibration on the inertial measurement data based on the acquisition time of the environmental image sequence to obtain aligned attitude data; and recursively calculate the prior pose estimate and corresponding error covariance matrix for each frame's acquisition time; and to perform interpolation calibration on the angular velocity and attitude angle data of the inertial measurement data based on the acquisition timestamp of each frame's environmental image to obtain attitude data aligned with the acquisition time of each frame's environmental image. Since the IMU sampling frequency is higher than the camera sampling frequency, a linear interpolation method is used to register the IMU data to the camera time. Specifically, for the current frame's image acquisition time... Select The two IMU sampling times before and after and Linear interpolation calculations are performed according to the time ratio. The IMU measurement values ​​at each moment are used. Based on aligned attitude data, the prior pose estimate for each frame of environmental image acquisition is obtained recursively through an inertial navigation recursive algorithm. Attitude update uses quaternion differential equations, calculating the attitude change from the previous frame to the current frame through angular velocity integration. Position and velocity updates use double integration of accelerometer data, performed in the world coordinate system. Simultaneously, the corresponding error covariance matrix is ​​calculated based on the inertial measurement noise model through error propagation. The propagation equation of the error covariance matrix is: ,in Here is the state transition matrix. for The transpose of the matrix, The process noise covariance matrix (initial values ​​are determined by the noise parameters of the IMU, including accelerometer noise density and gyroscope noise density). Let be the posterior error covariance matrix of the previous frame.

[0022] The data consistency verification and pre-correction module is used to: calculate the predicted value of the planar pose change of the current frame relative to the previous frame based on wheel speed data; obtain the predicted value of the inertial navigation pose change based on the prior pose estimation value output by the time synchronization and state recursion module; perform a consistency verification between the predicted value of the planar pose change and the predicted value of the inertial navigation pose change; and generate a pre-corrected image for feature extraction based on the verification result. The predicted value of the planar pose change is obtained by: calculating the predicted value of the planar pose change of the current frame relative to the previous frame based on the dual-wheel differential kinematic model and wheel speed data, using the left and right wheel linear velocities... Using the wheel track L and wheel radius R as input, and combining them with the structural parameters of the reconnaissance ball, the planar displacement of the current frame image acquisition time relative to the previous frame image acquisition time is calculated. and change in heading angle The aforementioned dual-wheel differential kinematic model is common knowledge in this field; the method for obtaining the predicted inertial navigation pose change value is as follows: based on the prior pose estimation value output by the time synchronization and state recursion module, the predicted inertial navigation pose change value is obtained, which includes the three-dimensional position increment. and 3D pose increment The prior pose estimate is defined in the world coordinate system and includes three-dimensional position and three-dimensional attitude. The predicted value of the inertial navigation pose change is obtained from the prior pose estimates of two adjacent frames: the position increment is obtained by directly subtracting the three-dimensional vectors, that is, the prior position of the current frame minus the prior position of the previous frame; the attitude increment is obtained by quaternion operation, that is, the prior attitude quaternion of the current frame multiplied by the conjugate of the prior attitude quaternion of the previous frame, and the result is the relative attitude change between the two frames, which is then converted into three-dimensional attitude increment.

[0023] The consistency check involves transforming the predicted planar pose change values ​​and the inertial navigation pose change values ​​to the same coordinate system (both transformed to the world coordinate system). Wheel velocity data is located in the reconnaissance sphere's body coordinate system, IMU data in the IMU carrier coordinate system, and depth image data in the camera coordinate system. Each pixel corresponds to a depth value. The extrinsic parameters between each sensor (rotation matrix and translation vector between the camera and IMU coordinate systems, and rotation matrix and translation vector between the IMU and reconnaissance sphere's body coordinate systems) need to be pre-calibrated. The calibration method combines checkerboard calibration and hand-eye calibration. During coordinate transformation, the planar displacement calculated from the wheel velocity is transformed to the IMU coordinate system using the calibration extrinsic parameters between the reconnaissance sphere and IMU coordinate systems, and then transformed to the world coordinate system using the IMU world coordinate system attitude matrix obtained from inertial navigation. The IMU measurement data is then recursively transformed by inertial navigation... The pose is directly output in the world coordinate system without additional coordinate transformation. This unifies the planar displacement, heading angle, and inertial navigation pose change calculated from wheel speed data into the world coordinate system for consistency comparison. Displacement deviation and heading angle deviation are calculated separately. The displacement deviation is determined by the difference between the planar displacement calculated from wheel speed data and the planar displacement component in the predicted inertial navigation pose change value. The heading angle deviation is determined by the difference between the heading angle change calculated from wheel speed data and the heading angle component in the predicted inertial navigation pose change value. If both the displacement deviation and heading angle deviation do not exceed a preset threshold, the consistency check is considered passed; otherwise, the consistency check is considered failed. The preset thresholds are determined based on the statistical values ​​of wheel speed and IMU motion consistency under normal operating conditions. The typical values ​​for the displacement deviation threshold are 0.05m and the heading angle deviation threshold are 0.05rad.

[0024] Once the consistency check passes, the data consistency check and pre-correction module fuse the predicted values ​​of planar pose change and inertial navigation pose change to obtain the full-dimensional pose change prediction value. The fusion method uses a weighted average and wheel speed-calculated planar components. Weighted fusion of corresponding components derived from inertial navigation; spatial attitude components Using inertial navigation-derived values, based on the pixel depth values ​​provided by the depth camera, each pixel in the current frame image is back-projected to a 3D spatial point in the camera coordinate system. Rigid body transformations (rotation and translation) are performed on the 3D spatial points according to the predicted values ​​of the full-dimensional pose changes, and then the points are projected back to the image plane through camera intrinsic parameters to obtain the pre-corrected pixel mapping position. By traversing all pixels and performing the above 3D reprojection and mapping, a pre-corrected image is generated. This process uses depth values ​​to process each pixel independently, compensating for the image rigid body deformation caused by the combined planar motion and 3D pose changes between adjacent frames, without relying on scene plane assumptions.

[0025] When the consistency check fails, the data consistency check and pre-correction module only relies on the rotation component in the predicted value of the inertial navigation pose change. Solve the image homography transformation matrix, set the translation component to zero, perform a reverse pixel mapping transformation on the current frame's environment image to compensate for the rigid body deformation of the image caused by the 3D pose changes between adjacent frames, and generate a pre-corrected image; since the translation information comes from the wheel speed calculation, and the displacement of the wheel speed calculation has been determined to be unreliable by the consistency check, the translation component does not participate in the pre-correction, and only compensates for reliable pose rotation information; the rotation compensation is based on the rotation of the image center in the camera coordinate system, without introducing scene depth or plane assumptions;

[0026] The visual observation and filtering update module is used to: extract the feature point matching relationship between adjacent frames from the pre-corrected image to construct the visual reprojection error observation equation; perform filtering and updating based on the error covariance matrix of the prior pose estimate and the reprojection error observation equation to obtain the pose estimate of the current frame; and feed back the pose estimate of the current frame and the corresponding error covariance matrix to the time synchronization and state recursion module.

[0027] First, corner features and corresponding descriptors are extracted from the current frame pre-corrected image and the previous frame pre-corrected image, respectively. The corner features are detected using the FAST or ORB feature detection algorithm, and the descriptors are detected using the BRIEF or ORB descriptors. Preliminary matching point pairs are generated by matching the feature descriptors using Hamming distance or Euclidean distance. Then, a random sampling consensus algorithm is used to remove mismatched point pairs to obtain a stable matching relationship between feature points of adjacent frames. The specific implementation of the random sampling consensus algorithm is as follows: randomly sample multiple pairs of matching points, calculate the fundamental matrix, count the number of interior points that satisfy the epipolar constraint, iterate and repeat the above process multiple times, select the model with the most interior points as the final model, and remove the exterior points that do not satisfy the model.

[0028] Secondly, based on the matching relationship of feature points in adjacent frames, the pixel coordinate deviation of matched feature points in adjacent frames is used as the observation: ,in and The pixel coordinates of the same feature point in the previous frame pre-corrected image and the current frame pre-corrected image are respectively. Based on the prior pose estimation value, combined with the camera intrinsic parameters and the rotation matrix and translation vector between the pre-calibrated camera coordinate system and IMU coordinate system, a visual reprojection error observation equation is constructed. The feature points in the previous frame image are projected onto the coordinate system of the current frame image through prior pose transformation to obtain the predicted pixel coordinates. The difference between the predicted pixel coordinates and the actual observed pixel coordinates in the current frame is the reprojection error. The reprojection error observation equation is linearized, and the observation matrix and observation noise matrix are calculated.

[0029] Finally, an Extended Kalman Filter (EKF) is used to perform state updates. The filter's state vector includes three-dimensional position, three-dimensional velocity, three-dimensional attitude (represented by quaternions), accelerometer three-axis bias, and gyroscope three-axis bias. The observation matrix is ​​a Jacobian matrix formed by taking the partial derivatives of the reprojection error equation with respect to each of the above state components. Its elements define the degree of influence of the unit change of each state component on the pixel coordinate deviation. Based on the error covariance matrix corresponding to the prior pose estimate... Observation matrix With observation noise matrix Calculate Kalman gain The prior pose estimate and error covariance matrix are corrected to obtain the updated pose estimate for the current frame. and error covariance matrix Simultaneously, the visual reprojection residuals and their statistical characteristics of the current frame are calculated. For all matched feature points in the current frame, the reprojection residual vector of each feature point is calculated, and then the mean vector and error covariance matrix of the residuals are calculated. The process noise covariance matrix of the inertial navigation recursive algorithm is adjusted according to the statistical characteristics to adjust the confidence of the prediction state in the next round of filtering fusion: when the trace of the residual covariance is less than the preset threshold, it indicates that the pre-correction effect is good and the feature matching quality is high, so the process noise covariance matrix is ​​kept unchanged or moderately reduced; when the trace of the residual covariance is greater than or equal to the preset threshold, it indicates that the pre-correction quality has decreased and there is a large deviation in feature matching, so the process noise covariance matrix is ​​increased to reduce the confidence weight of the prediction model in the next round of EKF; typically, when pre-correction is based only on rotation components, translation parallax still exists in the pre-corrected image, the visual reprojection residuals are larger than normal, the process noise covariance matrix is ​​automatically adjusted upward, and EKF will rely more on visual observation for pose correction; subsequently, the updated process noise covariance matrix is... Current frame pose estimation value and error covariance matrix The feedback is jointly sent to the time synchronization and state recursion module. It will be used in the calculation of the error covariance matrix corresponding to the prior pose estimate in the next frame: By changing To adjust the Kalman gain: This alters the weight distribution of predicted and observed states in the next round of EKF fusion, and the feedback... and This serves as the initial condition for the inertial navigation recursion in the next frame. As the starting point for pose recursion As the covariance propagation equation of the next frame ,and Together, they constitute the complete input for the next frame time synchronization and state recursion module.

[0030] The image pose correction module is used to: take the original image of the current frame as input, and perform geometric transformation on the original image according to the pose estimation value of the current frame to generate a corrected output image; specifically, based on the pose components in the pose estimation value of the current frame... The rotation matrix is ​​calculated, and a geometric transformation is performed on each pixel of the original image of the current frame to eliminate image distortion caused by pose. The image pose correction module takes the original current frame image acquired by the multi-source sensor acquisition module as input, rather than the pre-corrected image as input. Based on the current frame pose estimate output by the visual observation and filtering update module, the image pose correction module performs a single geometric transformation on the original current frame image to generate a corrected output image for display or subsequent tasks. That is, the image pose correction module and the data consistency check and pre-correction module process two independent copies of the same original image, and each performs geometric transformation operations independently. There is no situation where geometric transformations are superimposed on the same image one after another.

[0031] The above description is only a preferred embodiment of the present invention, but the scope of protection of the present invention is not limited thereto. Any equivalent substitutions or modifications made by those skilled in the art within the scope of the technology disclosed in the present invention, based on the technical solution and inventive concept of the present invention, should be covered within the scope of protection of the present invention.

Claims

1. A dual-wheel reconnaissance ball image feedback attitude correction system, characterized in that, include: The multi-source sensor acquisition module is used to: acquire environmental image sequences, inertial measurement data, and wheel speed data during the movement of the dual-wheeled reconnaissance ball; the time synchronization and state recursion module is used to: perform time interpolation calibration on the inertial measurement data based on the acquisition time of the environmental image sequence to obtain aligned attitude data, and recursively calculate the prior pose estimate and corresponding error covariance matrix at the acquisition time of each frame; the data consistency verification and pre-correction module is used to: calculate the predicted value of the planar pose change of the current frame relative to the previous frame based on the wheel speed data, obtain the predicted value of the inertial navigation pose change based on the prior pose estimate output by the time synchronization and state recursion module, perform consistency verification between the predicted value of the planar pose change and the predicted value of the inertial navigation pose change, and generate a pre-corrected image for feature extraction based on the verification results; The visual observation and filtering update module is used to: extract the feature point matching relationship between adjacent frames from the pre-corrected image to construct the visual reprojection error observation equation; perform filtering and updating based on the error covariance matrix of the prior pose estimate and the reprojection error observation equation to obtain the pose estimate of the current frame; and feed back the pose estimate of the current frame and the corresponding error covariance matrix to the time synchronization and state recursion module. The image pose correction module is used to: perform geometric transformation on the current frame environment image based on the pose estimate of the current frame to generate the corrected output image.

2. The dual-wheel reconnaissance ball image feedback attitude correction system according to claim 1, characterized in that, The environmental image sequence, inertial measurement data, and wheel speed data acquired by the multi-source sensor acquisition module are all equipped with a unified timestamp, and the acquisition trigger signal of each frame of the environmental image sequence is used as the reference for time alignment of the entire system.

3. The dual-wheel reconnaissance ball image feedback attitude correction system according to claim 1, characterized in that, The time synchronization and state recursion module uses the acquisition timestamp of each frame of environmental image as a reference to interpolate and calibrate the angular velocity and attitude angle data of the inertial measurement data to obtain attitude data aligned with the acquisition time of each frame of environmental image. Based on the attitude data, the prior pose estimation value of each frame of environmental image acquisition time is recursively obtained through the inertial navigation recursion algorithm, and the corresponding error covariance matrix is ​​calculated based on the inertial measurement noise model through error propagation.

4. The dual-wheel reconnaissance ball image feedback attitude correction system according to claim 1, characterized in that, The data consistency check includes: converting the predicted values ​​of the planar pose change and the predicted values ​​of the inertial navigation pose change to the same coordinate system, calculating the displacement deviation and heading angle deviation between the two, and determining that the consistency check passes when the displacement deviation and heading angle deviation do not exceed a preset threshold; otherwise, the consistency check fails.

5. The dual-wheel reconnaissance ball image feedback attitude correction system according to claim 4, characterized in that, The displacement deviation is determined by the difference between the planar displacement calculated from the wheel speed data and the planar displacement component in the predicted value of the inertial navigation attitude change. The heading angle deviation is determined by the difference between the heading angle change calculated from the wheel speed data and the heading angle component in the predicted value of the inertial navigation attitude change.

6. The dual-wheel reconnaissance ball image feedback attitude correction system according to claim 4, characterized in that, When the consistency check passes, the data consistency check and pre-correction module fuse the predicted values ​​of planar pose change and inertial navigation pose change to obtain the predicted value of full-dimensional pose change. Based on the predicted value of full-dimensional pose change, each pixel in the current frame image is back-projected to a three-dimensional space point in the camera coordinate system based on the pixel depth values ​​provided by the depth camera. After performing a rigid body transformation on the three-dimensional space point, it is projected back onto the image plane, and a pre-correction image is generated pixel by pixel to compensate for the rigid body deformation of the image caused by the planar motion and three-dimensional pose change between adjacent frames.

7. The dual-wheel reconnaissance ball image feedback attitude correction system according to claim 4, characterized in that, When the consistency check fails, the data consistency check and pre-correction module solves the image homography transformation matrix based only on the rotation component in the predicted value of the inertial navigation pose change, performs reverse pixel mapping transformation on the current frame environment image, compensates for the rigid body deformation of the image caused by the three-dimensional pose change between adjacent frames, and generates a pre-corrected image.

8. The dual-wheel reconnaissance ball image feedback attitude correction system according to claim 1, characterized in that, The visual observation and filtering update module extracts corner features and corresponding descriptors from the current frame pre-corrected image and the previous frame pre-corrected image, respectively. It generates preliminary matching point pairs through feature descriptor matching and uses a random sampling consensus algorithm to remove mismatched point pairs, thereby obtaining a stable feature point matching relationship between adjacent frames.

9. The dual-wheel reconnaissance ball image feedback attitude correction system according to claim 8, characterized in that, The visual observation and filtering update module is based on the feature point matching relationship between adjacent frames. It takes the pixel coordinate deviation of the matching feature points in adjacent frames as the observation. Based on the prior pose estimation value and combined with the camera intrinsic parameters, it constructs the visual reprojection error observation equation and linearizes the reprojection error observation equation to obtain the observation matrix and the observation noise matrix.

10. The dual-wheel reconnaissance ball image feedback attitude correction system according to claim 9, characterized in that, The visual observation and filtering update module uses extended Kalman filtering to perform state updates. It calculates the Kalman gain based on the error covariance matrix, observation matrix, and observation noise matrix corresponding to the prior pose estimate, corrects the prior pose estimate and error covariance matrix, and obtains the updated current frame pose estimate and error covariance matrix. At the same time, it calculates the statistical characteristics of the current frame visual reprojection residual, adjusts the process noise covariance matrix in the inertial navigation recursive algorithm based on the statistical characteristics, and adjusts the prediction confidence in the next round of filtering fusion. The current frame pose estimate and the corresponding error covariance matrix are fed back to the time synchronization and state recursion module as the initial conditions for the recursion of the next frame prior pose estimate.

Images