A post-processing multi-source fusion vehicle pose estimation method and a computer readable medium
By employing a post-processing multi-source fusion vehicle pose estimation method, combining Kalman filters and RTS smoothers, and utilizing information from GNSS, INS, and visual sensors, the problem of high-precision pose estimation for intelligent mobile vehicles in complex environments was solved, achieving globally consistent and accurate positioning results.
Patent Information
- Authority / Receiving Office
- CN · China
- Patent Type
- Patents(China)
- Current Assignee / Owner
- WUHAN UNIV
- Filing Date
- 2023-06-14
- Publication Date
- 2026-07-14
AI Technical Summary
Existing positioning technologies based on single sensors are insufficient to meet the high-precision, globally consistent pose estimation requirements of intelligent mobile carriers such as mobile robots, drones, and autonomous vehicles in complex environments, especially when GNSS signals are blocked, resulting in accumulated positioning errors and limited improvement in real-time positioning trajectory consistency.
A post-processing multi-source fusion vehicle pose estimation method is adopted, which combines Kalman filter, RTS smoother and forward-backward smoother. It utilizes multi-source information from GNSS, INS and vision sensors, and calculates the global optimal estimate by combining the state estimates and covariance matrix of forward and backward Kalman filters with RTS smoother.
It provides globally consistent and accurate pose estimation results in GNSS signal-blocked environments, improves the absolute and relative accuracy of positioning, suppresses inertial navigation error divergence, and enhances the robustness and continuity of positioning.
Smart Images
Figure CN116858221B_ABST
Abstract
Description
Technical Field
[0001] This invention belongs to the field of navigation and positioning technology, and particularly relates to a post-processing multi-source fusion vehicle pose estimation method and a computer-readable medium. Background Technology
[0002] To meet the evolving positioning needs of users—from coarse to precise, from static to dynamic, and from regional to global—single-sensor-based positioning technologies are increasingly insufficient for the complex application scenarios of intelligent mobile vehicles such as mobile robots, drones, and autonomous vehicles. Considering the complementary characteristics of Global Navigation Satellite Systems (GNSS), Inertial Navigation Systems (INS), and Visual Odometry (VO), multi-source fusion technologies combining GNSS, INS, and VO have become a popular research direction. In such systems, GNSS provides drift-free global positioning information under good observation conditions, while VIO provides short-term, high-precision, and slow-drift pose estimation information when GNSS is obstructed, improving the continuity and robustness of positioning. However, limited by the GNSS and visual observation environment, the positioning error of GNSS / INS / VO integrated navigation can accumulate to several meters, and real-time positioning and navigation methods offer limited improvement in trajectory consistency.
[0003] High-precision maps can provide accurate location, road, and traffic information, further assisting driver assistance systems in identifying obstacles and hazards in the environment, improving navigation accuracy and planning optimization, and are one of the infrastructures for autonomous driving. For applications such as map building systems and geographic information systems, accurate, reliable, and globally consistent pose estimation is a primary condition for map creation, while real-time pose estimation is almost unrequired. Therefore, this invention further utilizes post-processing techniques to fully leverage the entire observation range, providing globally consistent, accurate, and robust post-hoc pose estimation results. Summary of the Invention
[0004] To address the aforementioned technical problems, this invention proposes a post-processing multi-source fusion method for vehicle pose estimation and a computer-readable medium.
[0005] The technical solution of this invention is a post-processing multi-source fusion vehicle pose estimation method, which specifically includes the following steps:
[0006] Step 1: Input the current state vector of the Kalman filter and the covariance matrix of the current state vector of the Kalman filter, and construct the forward Kalman filter and the inverse Kalman filter respectively;
[0007] Step 2: Calculate the state vector of the forward Kalman filter at the next moment based on the current state vector of the forward Kalman filter, and calculate the state vector of the inverse Kalman filter at the previous moment based on the current state vector of the inverse Kalman filter.
[0008] Step 3: Calculate the forward global optimal estimate and the covariance matrix of the forward global optimal estimate at the current time using the forward RTS smoother; calculate the reverse global optimal estimate and the covariance matrix of the reverse global optimal estimate at the current time using the reverse RTS smoother.
[0009] Step 4: Calculate the current global optimal valuation by using a forward-backward smoother, taking the forward global optimal valuation, the covariance matrix of the current global optimal valuation, the reverse global optimal valuation, and the covariance matrix of the current global optimal valuation.
[0010] Preferably, the state vector in step 1 includes:
[0011] The vehicle's position at the current moment, the vehicle's speed at the current moment, the vehicle's attitude at the current moment, the gyroscope's bias at the current moment, the accelerometer's bias at the current moment, the GNSS receiver's clock bias at the current moment, the zenith tropospheric delay at the current moment, the phase ambiguity at the current moment, the camera's position at multiple historical moments within the sliding window, and the camera's attitude at multiple historical moments within the sliding window.
[0012] The current time is the k-th time.
[0013] The multiple historical moments within the sliding window are: moment kL, moment k-L+1, ..., moment k-1;
[0014] L represents the number of historical moments within the sliding window;
[0015] Preferably, step 2, which involves calculating the state vector of the forward Kalman filter at the next moment based on the current state vector of the forward Kalman filter, is as follows:
[0016] Obtain the IMU observation value at the current time, combine the current IMU observation value with the current state vector of the Kalman filter, and obtain the predicted state vector of the forward Kalman filter at the next time, the covariance matrix of the predicted state vector of the forward Kalman filter at the next time, and the state transition matrix between the current time and the next time of the forward Kalman filter through the inertial navigation mechanical arrangement model.
[0017] The camera image at the current moment is acquired. The positive state vector predicted by the Kalman filter at the next moment and the camera-IMU extrinsic parameters are combined and calculated using the pose transformation formula and the error propagation law to obtain the expanded state vector of the positive Kalman filter at the next moment and the covariance matrix of the expanded state vector of the positive Kalman filter at the next moment.
[0018] The camera image at the current moment is extracted sequentially using the corner extraction operator and tracked by the optical flow tracing method to obtain the pixel coordinates of the feature points at the current moment. Combined with the expanded state vector of the forward Kalman filter at the next moment, the visual reprojection error model is used to calculate the forward state vector of the forward Kalman filter after visual update at the next moment and the covariance of the forward state vector of the forward Kalman filter after visual update at the next moment.
[0019] The current GNSS observation value is obtained, and the state vector updated by the next visual time step of the forward Kalman filter is combined with the state vector updated by the next visual time step of the forward Kalman filter to calculate the covariance matrix of the forward state vector updated by the next visual time step of the forward Kalman filter.
[0020] Step 2, which involves calculating the state vector of the inverse Kalman filter from its previous state vector based on the current state vector, is as follows:
[0021] Obtain the IMU observation value at the current time, combine the current IMU observation value with the current state vector of the Kalman filter, and obtain the predicted state vector of the inverse Kalman filter at the previous time and the state transition matrix between the current time and the previous time of the inverse Kalman filter through the inertial navigation mechanical arrangement model.
[0022] The camera image at the current moment is acquired. Combined with the inverse state vector predicted by the Kalman filter at the previous moment and the camera-IMU extrinsic parameters, the expanded state vector of the inverse Kalman filter at the previous moment and the covariance matrix of the expanded state vector of the inverse Kalman filter at the previous moment are calculated using the pose transformation formula and the error propagation law.
[0023] The camera image at the current moment is extracted sequentially by corner extraction operator and tracked by optical flow tracking method to obtain the feature point pixel coordinates at the current moment. Combined with the expanded state vector of the previous moment of the inverse Kalman filter, the inverse state vector after visual update at the previous moment of the inverse Kalman filter and the covariance of the inverse state vector after visual update at the previous moment of the inverse Kalman filter are calculated by visual reprojection error model.
[0024] The current GNSS observation value is obtained, and the state vector updated by the previous visual time of the inverse Kalman filter is combined with the state vector updated by the previous visual time of the inverse Kalman filter to calculate the covariance matrix of the inverse state vector updated by the previous visual time of the inverse Kalman filter.
[0025] Preferably, step 3 involves calculation using a positive RTS smoother, as detailed below:
[0026] Input the forward global optimal estimate for the next time step and the covariance matrix of the forward global optimal estimate for the next time step. Then, combine the updated state vector of the forward Kalman filter at the current time step and the covariance matrix of the updated state vector of the forward Kalman filter at the current time step with the predicted state vector of the forward Kalman filter at the next time step and the covariance matrix of the predicted state vector of the forward Kalman filter at the next time step, and the state transition matrix between the current time step and the next time step of the forward Kalman filter, and calculate through the forward RTS smoother to obtain the forward global optimal estimate for the current time step and the covariance matrix of the forward global optimal estimate for the current time step.
[0027] Step 3, which involves calculation using an inverse RTS smoother, is detailed below:
[0028] Input the covariance matrices of the previous forward global optimal estimate and the previous backward global optimal estimate. Then, take the updated state vector of the inverse Kalman filter at the current time, the covariance matrix of the current state vector of the inverse Kalman filter, the predicted state vector of the inverse Kalman filter at the previous time, the covariance matrix of the predicted state vector of the inverse Kalman filter at the previous time, and the state transition matrix between the current time and the previous time of the inverse Kalman filter, and calculate the reverse global optimal estimate and the covariance matrix of the reverse global optimal estimate at the current time through the inverse RTS smoother.
[0029] The present invention also provides a computer-readable medium storing a computer program executed by an electronic device, wherein when the computer program is run on the electronic device, it performs the steps of the post-processing multi-source fusion vehicle pose estimation method.
[0030] This invention introduces visual measurement information into a tightly integrated GNSS / INS system. In environments where GNSS signals are obstructed, such as tree-lined roads and tunnels, the visual sensor can capture rich texture features, provide strong geometric constraints, obtain high relative positioning accuracy, and effectively suppress the divergence of inertial navigation errors. In environments with sparse feature points, such as open roads and squares, sufficient and high-precision GNSS observations can provide accurate global position information.
[0031] Furthermore, compared to other post-processing techniques, this invention can simultaneously utilize information from both the dynamic model and the measurement model. The forward-reverse smoother utilizes only the state estimates and corresponding covariances of the forward and reverse Kalman filters, and the updated position estimate of the integrated navigation system is primarily determined by GNSS observations containing global reference frame information. Therefore, the forward-reverse smoother can significantly improve absolute positioning accuracy. In contrast, the RTS smoother emphasizes the pose relationships between adjacent epochs provided by the dynamic model, resulting in relatively higher and smoother trajectory estimates. The method described in this invention, by combining the RTS smoother and the forward-reverse smoother, achieves the ability to simultaneously improve both relative and absolute positioning accuracy. Attached Figure Description
[0032] Figure 1 : Flowchart of the method according to an embodiment of the present invention. Detailed Implementation
[0033] 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. Based on the embodiments of the present invention, all other embodiments obtained by those skilled in the art without creative effort are within the scope of protection of the present invention.
[0034] In specific implementation, the method proposed in the technical solution of this invention can be automatically executed by those skilled in the art using computer software technology. System devices for implementing the method, such as computer-readable storage media storing the corresponding computer program of the technical solution of this invention and computer equipment including the computer program running the corresponding computer program, should also be within the protection scope of this invention.
[0035] The following is combined Figure 1 The technical solution of this invention is a post-processing multi-source fusion vehicle pose estimation method, as detailed below:
[0036] Step 1: Input the current state vector of the Kalman filter and the covariance matrix of the current state vector of the Kalman filter, and construct the forward Kalman filter and the inverse Kalman filter respectively;
[0037] The forward Kalman filter mentioned in step 1 is a forward GNSS / INS / Vision tightly combined Kalman filter;
[0038] The inverse Kalman filter mentioned in step 1 is an inverse GNSS / INS / Vision tightly combined Kalman filter;
[0039] The state vector mentioned in step 1 includes:
[0040] The vehicle's position at the current moment, the vehicle's speed at the current moment, the vehicle's attitude at the current moment, the gyroscope's bias at the current moment, the accelerometer's bias at the current moment, the GNSS receiver's clock bias at the current moment, the zenith tropospheric delay at the current moment, the phase ambiguity at the current moment, the camera's position at multiple historical moments within the sliding window, and the camera's attitude at multiple historical moments within the sliding window.
[0041] The current time is the k-th time.
[0042] The multiple historical moments within the sliding window are: moment kL, moment k-L+1, ..., moment k-1;
[0043] L=10 represents the number of historical moments within the sliding window;
[0044] Step 2: Calculate the state of the forward Kalman filter at the next time step based on the current state of the forward Kalman filter, and calculate the state of the inverse Kalman filter at the previous time step based on the current state of the inverse Kalman filter;
[0045] Step 2, which involves calculating the state of the forward Kalman filter at the next time step based on the current state of the forward Kalman filter, is as follows:
[0046] Obtain the IMU observation value at the current time, combine the current IMU observation value with the current state vector of the Kalman filter, and obtain the predicted state vector of the forward Kalman filter at the next time, the covariance matrix of the predicted state vector of the forward Kalman filter at the next time, and the state transition matrix between the current time and the next time of the forward Kalman filter through the inertial navigation mechanical arrangement model.
[0047] The camera image at the current moment is acquired. The positive state vector predicted by the Kalman filter at the next moment and the camera-IMU extrinsic parameters are combined and calculated using the pose transformation formula and the error propagation law to obtain the expanded state vector of the positive Kalman filter at the next moment and the covariance matrix of the expanded state vector of the positive Kalman filter at the next moment.
[0048] The camera image at the current moment is extracted sequentially using the corner extraction operator and tracked by the optical flow tracing method to obtain the pixel coordinates of the feature points at the current moment. Combined with the state vector predicted by the forward Kalman filter at the next moment, the covariance of the forward state vector after visual update at the next moment and the covariance of the forward state vector after visual update at the next moment are calculated by the MSCKF visual measurement model.
[0049] The GNSS observations at the current moment are obtained, and the state vector updated by the visual observation at the next moment of the forward Kalman filter is combined with the state vector updated by the visual observation at the next moment of the forward Kalman filter. The covariance matrix of the state vector updated by the forward Kalman filter at the next moment of the forward Kalman filter is calculated by the precise single-point positioning compact combination measurement model.
[0050] Step 2, which involves calculating the state of the inverse Kalman filter at the previous time step based on the current state of the inverse Kalman filter, is as follows:
[0051] Obtain the IMU observation value at the current time, combine the current IMU observation value with the current state vector of the Kalman filter, and obtain the predicted state vector of the inverse Kalman filter at the previous time and the state transition matrix between the current time and the previous time of the inverse Kalman filter through the inertial navigation mechanical arrangement model.
[0052] The camera image at the current moment is acquired. Combined with the inverse state vector predicted by the Kalman filter at the previous moment and the camera-IMU extrinsic parameters, the expanded state vector of the inverse Kalman filter at the previous moment and the covariance matrix of the expanded state vector of the inverse Kalman filter at the previous moment are calculated using the pose transformation formula and the error propagation law.
[0053] The camera image at the current moment is extracted sequentially by corner extraction operator and tracked by optical flow tracing method to obtain the pixel coordinates of the feature points at the current moment. Combined with the state vector predicted by the previous moment of the inverse Kalman filter, the inverse state vector after the visual update of the previous moment of the inverse Kalman filter and the covariance of the inverse state vector after the visual update of the previous moment of the inverse Kalman filter are calculated by the MSCKF visual measurement model.
[0054] The current GNSS observation value is obtained, and the state vector updated by the previous visual time of the inverse Kalman filter is combined with the state vector updated by the previous visual time of the inverse Kalman filter. The covariance matrix of the inverse state vector updated by the previous visual time of the inverse Kalman filter is calculated by the precise single-point positioning compact combination measurement model.
[0055] Step 3: Calculate the forward global optimal estimate and the covariance matrix of the forward global optimal estimate at the current time using the forward RTS smoother; calculate the reverse global optimal estimate and the covariance matrix of the reverse global optimal estimate at the current time using the reverse RTS smoother.
[0056] Step 3, which involves calculation using a forward RTS smoother, is detailed below:
[0057] Input the forward global optimal estimate for the next time step and the covariance matrix of the forward global optimal estimate for the next time step. Then, combine the updated state vector of the forward Kalman filter at the current time step and the covariance matrix of the updated state vector of the forward Kalman filter at the current time step with the predicted state vector of the forward Kalman filter at the next time step and the covariance matrix of the predicted state vector of the forward Kalman filter at the next time step, and the state transition matrix between the current time step and the next time step of the forward Kalman filter, and calculate through the forward RTS smoother to obtain the forward global optimal estimate for the current time step and the covariance matrix of the forward global optimal estimate for the current time step.
[0058] Step 3, which involves calculation using an inverse RTS smoother, is detailed below:
[0059] Input the covariance matrices of the previous forward global optimal estimate and the previous backward global optimal estimate. Then, take the updated state vector of the inverse Kalman filter at the current time, the covariance matrix of the current state vector of the inverse Kalman filter, the predicted state vector of the inverse Kalman filter at the previous time, the covariance matrix of the predicted state vector of the inverse Kalman filter at the previous time, and the state transition matrix between the current time and the previous time of the inverse Kalman filter, and calculate the reverse global optimal estimate and the covariance matrix of the reverse global optimal estimate at the current time through the inverse RTS smoother.
[0060] Step 4: Calculate the current global optimal valuation by using a forward-backward smoother, taking the forward global optimal valuation, the covariance matrix of the current global optimal valuation, the reverse global optimal valuation, and the covariance matrix of the current global optimal valuation.
[0061] A specific embodiment of the present invention also provides a computer-readable medium.
[0062] The computer-readable medium is a server workstation;
[0063] The server workstation stores the computer program executed by the electronic device. When the computer program runs on the electronic device, it causes the electronic device to execute the steps of the post-processing multi-source fusion vehicle pose estimation method according to the embodiments of the present invention.
[0064] It should be understood that any parts not described in detail in this specification belong to the prior art.
[0065] It should be understood that the above description of the preferred embodiments is quite detailed, but it should not be considered as a limitation on the scope of protection of this invention. Those skilled in the art, under the guidance of this invention, can make substitutions or modifications without departing from the scope of protection of the claims of this invention, and all such substitutions or modifications fall within the scope of protection of this invention. The scope of protection of this invention should be determined by the appended claims.
Claims
1. A post-processing multi-source fusion method for vehicle pose estimation, characterized in that, The process includes: Step 1: Input the current state vector of the Kalman filter and the covariance matrix of the current state vector of the Kalman filter, and construct the forward Kalman filter and the inverse Kalman filter respectively; Step 2: Calculate the state vector of the forward Kalman filter at the next time step based on the current state vector of the forward Kalman filter, and calculate the state vector of the inverse Kalman filter at the previous time step based on the current state vector of the inverse Kalman filter; Step 2, calculating the state vector of the forward Kalman filter at the next time step based on the current state vector of the forward Kalman filter, includes: Obtain the IMU observation value at the current time, combine the current IMU observation value with the current state vector of the Kalman filter, and obtain the predicted state vector of the forward Kalman filter at the next time, the covariance matrix of the predicted state vector of the forward Kalman filter at the next time, and the state transition matrix between the current time and the next time of the forward Kalman filter through the inertial navigation mechanical arrangement model. The camera image at the current moment is acquired. The positive state vector predicted by the Kalman filter at the next moment and the camera-IMU extrinsic parameters are combined and calculated using the pose transformation formula and the error propagation law to obtain the expanded state vector of the positive Kalman filter at the next moment and the covariance matrix of the expanded state vector of the positive Kalman filter at the next moment. The camera image at the current moment is extracted sequentially using the corner extraction operator and tracked by the optical flow tracing method to obtain the pixel coordinates of the feature points at the current moment. Combined with the expanded state vector of the forward Kalman filter at the next moment, the visual reprojection error model is used to calculate the forward state vector of the forward Kalman filter after visual update at the next moment and the covariance of the forward state vector of the forward Kalman filter after visual update at the next moment. The current GNSS observation value is obtained, and the state vector updated by the next visual time step of the forward Kalman filter is combined with the state vector updated by the next visual time step of the forward Kalman filter to calculate the covariance matrix of the forward state vector updated by the next visual time step of the forward Kalman filter. Step 2, which involves calculating the state vector of the inverse Kalman filter from its previous state vector based on the current state vector, includes: Obtain the IMU observation value at the current time, combine the current IMU observation value with the current state vector of the Kalman filter, and obtain the predicted state vector of the inverse Kalman filter at the previous time and the state transition matrix between the current time and the previous time of the inverse Kalman filter through the inertial navigation mechanical arrangement model. The camera image at the current moment is acquired. Combined with the inverse state vector predicted by the Kalman filter at the previous moment and the camera-IMU extrinsic parameters, the expanded state vector of the inverse Kalman filter at the previous moment and the covariance matrix of the expanded state vector of the inverse Kalman filter at the previous moment are calculated using the pose transformation formula and the error propagation law. The camera image at the current moment is extracted sequentially by corner extraction operator and tracked by optical flow tracking method to obtain the feature point pixel coordinates at the current moment. Combined with the expanded state vector of the previous moment of the inverse Kalman filter, the inverse state vector after visual update at the previous moment of the inverse Kalman filter and the covariance of the inverse state vector after visual update at the previous moment of the inverse Kalman filter are calculated by visual reprojection error model. The current GNSS observation value is obtained, and the state vector updated by the previous visual time of the inverse Kalman filter is combined with the state vector updated by the previous visual time of the inverse Kalman filter to calculate the covariance matrix of the inverse state vector updated by the previous visual time of the inverse Kalman filter. Step 3: Calculate the forward global optimal estimate and the covariance matrix of the forward global optimal estimate at the current time using the forward RTS smoother; calculate the reverse global optimal estimate and the covariance matrix of the reverse global optimal estimate at the current time using the reverse RTS smoother. Step 4: Calculate the current global optimal valuation by using a forward-backward smoother, taking the forward global optimal valuation, the covariance matrix of the current global optimal valuation, the reverse global optimal valuation, and the covariance matrix of the current global optimal valuation.
2. The post-processing multi-source fusion vehicle pose estimation method according to claim 1, characterized in that: The state vector mentioned in step 1 includes: The vehicle's position at the current moment, the vehicle's speed at the current moment, the vehicle's attitude at the current moment, the gyroscope's bias at the current moment, the accelerometer's bias at the current moment, the GNSS receiver's clock bias at the current moment, the zenith tropospheric delay at the current moment, the phase ambiguity at the current moment, the camera's position at multiple historical moments within the sliding window, and the camera's attitude at multiple historical moments within the sliding window. The current time is the k-th time. The multiple historical moments within the sliding window are: moment kL, moment k-L+1, ..., moment k-1; L represents the number of historical moments within the sliding window.
3. The post-processing multi-source fusion vehicle pose estimation method according to claim 1, characterized in that: Step 3, which involves calculation using a forward RTS smoother, is detailed below: Input the forward global optimal estimate for the next time step and the covariance matrix of the forward global optimal estimate for the next time step. Then, combine the updated state vector of the forward Kalman filter at the current time step and the covariance matrix of the updated state vector of the forward Kalman filter at the current time step with the predicted state vector of the forward Kalman filter at the next time step and the covariance matrix of the predicted state vector of the forward Kalman filter at the next time step, and the state transition matrix between the current time step and the next time step of the forward Kalman filter, and calculate through the forward RTS smoother to obtain the forward global optimal estimate for the current time step and the covariance matrix of the forward global optimal estimate for the current time step.
4. The post-processing multi-source fusion vehicle pose estimation method according to claim 3, characterized in that: Step 3, which involves calculation using an inverse RTS smoother, is detailed below: Input the previous inverse global optimal estimate and its covariance matrix. Then, take the updated state vector of the inverse Kalman filter at the current time, its covariance matrix, and the predicted state vector of the inverse Kalman filter at the previous time, its covariance matrix, and the state transition matrix between the current and previous times of the inverse Kalman filter. Calculate these using an inverse RTS smoother to obtain the current inverse global optimal estimate and its covariance matrix.
5. A computer-readable medium, characterized in that, It stores a computer program executed by an electronic device, which, when run on the electronic device, causes the electronic device to perform the steps of the method as described in any one of claims 1-4.