Vision-inertial wheel-odometry slam method based on improved adaptive ukf

Abstract

This invention discloses a visual inertial wheel velocity meter SLAM method based on an improved adaptive UKF in the field of visual navigation technology, including the following steps: 1) establishing the state equation for the robot SLAM system; 2) establishing the measurement model of the visual sensor and wheel velocity meter; 3) determining the unscented transformation sampling points and weights according to the state space model; 4) transferring the sampling points through a nonlinear function and updating the system state prediction and state prediction covariance matrix; 5) updating the measurement and updating the process noise and measurement noise covariance matrix using an improved Sage-Husa algorithm; 6) introducing a convergence factor to update the state estimation, filter gain, and state estimation covariance matrix. This invention uses an improved Sage-Husa adaptive Kalman filter, which optimizes the filtering effect by adaptively adjusting the parameters of the UKF algorithm, thereby improving the real-time performance and accuracy of the robot SLAM system.

Description

Technical Field

[0001] This invention relates to the field of visual navigation technology, and in particular to a visual inertial SLAM method. Background Technology

[0002] In recent years, with the rapid development of applications such as autonomous driving, robotics, metaverse, and augmented reality, Simultaneous Localization and Mapping (SLAM) has become a core technology and has received widespread attention from industry and academia. SLAM refers to a subject equipped with specific sensors building a model of its environment while simultaneously estimating its own motion, without prior environmental information. The main application areas of SLAM include autonomous mobile robots, augmented reality, autonomous driving, mobile terminals, and unmanned aerial vehicle (UAV) systems. SLAM research utilizes filter theory, typically using odometry input as the prediction process and visual sensor output as the update process. Kalman filtering is the most classic filtering method; however, it can only handle linear models, while real-world SLAM systems are often nonlinear problems. Therefore, researchers have shifted their focus to Extended Kalman Filter (EKF), which is currently the most widely used filtering algorithm in SLAM. However, the EKF algorithm has drawbacks. The linearization process requires calculating the Jacobian and Hessian matrices, a computationally tedious and error-prone step. Furthermore, the accuracy of EKF is at most second-order. In contrast, the UKF algorithm does not utilize Taylor series for linearization approximation and avoids calculating the Jacobian and Hessian matrices. Instead, it uses a set of sigma points and an unscented transformation (UT) to handle the nonlinear propagation of the mean and covariance. Its nonlinear approximation accuracy exceeds second-order. When using the UKF approximation to handle the mean and covariance of nonlinear systems, it significantly outperforms the EKF algorithm in terms of feasibility, convergence, and filtering accuracy. However, both of these require an accurate system model and white noise interference. When the system noise exhibits uncertainty, these algorithms cannot adaptively handle this problem, resulting in suboptimal filtering performance or even filtering failure. Summary of the Invention

[0003] To address the shortcomings of existing technologies, this invention provides an improved adaptive UKF-based visual inertial wheel velocity meter SLAM method, which solves the problems of low accuracy and insufficient real-time performance of UKF-based visual inertial wheel velocity meter SLAM systems under uncertain external noise conditions.

[0004] The objective of this invention is achieved as follows: a visual inertial wheel velocity measurement SLAM method based on an improved adaptive UKF, characterized by comprising the following steps:

[0005] Step 1) Establish the state equations for the robot SLAM system;

[0006] Step 2) Establish measurement models for the vision sensor and wheel speed meter;

[0007] Step 3) Establish the sampling points and weights for the unscented transformation based on the state-space model;

[0008] Step 4) Transfer the sampling points using a nonlinear function and update the system state prediction and state prediction covariance matrix;

[0009] Step 5) Measurement update, and update the process noise and measurement noise covariance matrix using the improved Sage-Husa algorithm;

[0010] Step 6) Introduce a convergence factor and update the state estimate, filter gain, and state estimate covariance matrix.

[0011] As a further limitation of the present invention, step 1) specifically includes:

[0012] For a robot SLAM system equipped with an IMU and a vision sensor, the camera's attitude, velocity, and position, and the IMU's acceleration and angular velocity deviations are selected as the system states. A world coordinate system is established with the robot's initial position as the origin, with due east as the positive direction of the x-axis and due north as the positive direction of the y-axis. The positive direction of the z-axis is determined according to the right-hand rule. A body coordinate system is established with the camera as the center, with the direction parallel to the camera's horizontal axis pointing to the right as the positive direction of the x-axis and the direction directly in front of the camera as the positive direction of the y-axis. The positive direction of the z-axis is determined according to the right-hand rule. Selecting the camera's attitude, velocity, and position, and the IMU's acceleration and angular velocity deviations as the system states, the system's state equations are as follows:

[0013] X k =f(X) k-1 )+Q k

[0014] in,

[0015] Q k The system noise is set to Gaussian white noise;

[0016] Among them, [R k-1 v k-1 s k-1 b ω,k-1 b a,k-1 ]T R represents the system state at time k-1. k-1 Let v represent the rotation matrix of the camera at time k-1. k-1 Let s represent the camera's velocity at time k-1. k-1 b represents the position of the camera at time k-1. ω,k-1 b represents the acceleration error of the IMU at time k-1. a,k-1 This represents the angular velocity error of the IMU at time k-1. For each variable in the system, w is Gaussian white noise. ω For the Gaussian white noise of the IMU gyroscope, w a Gaussian white noise for the IMU accelerometer Gaussian white noise for IMU angular velocity deviation The Gaussian white noise for the IMU acceleration bias, [ω k-1 a k-1 ] T G represents the angular velocity and acceleration at time k-1. G Δt represents the acceleration due to gravity, and Δt represents the sampling time interval.

[0017] As a further limitation of the present invention, step 2) specifically includes:

[0018] Step 2-1) Establish the measurement model of the visual sensor, select the pixel coordinates measured by the camera as the measurement value, and the measurement equation for the measurement data given by the camera is as follows:

[0019] Z k,camera =h camera (X k-1 )+R k,camera

[0020] in,

[0021] h camera (X k-1 ) = KI 3×4 T cw p i ′, R k,camera The measurement noise of the camera is set to Gaussian white noise;

[0022] Where K is the intrinsic parameter matrix of the camera, I 3×4 It is a 3×4 matrix, and I 3×4 =[I 3×3 0 3×1 ], I 3×3 It is a 3×3 identity matrix, 0 3×1 T is a 3×1 column vector; cw Let be the transformation matrix from the world coordinate system to the camera coordinate system, and Among them, R cwand t cw Let p' and p' represent the rotation matrix and translation vector from the world coordinate system to the camera coordinate system, respectively. i For p i homogeneous coordinates, and p i The three-dimensional coordinates of the camera observation point in the world coordinate system;

[0023] Step 2-2) Establish the measurement model of the wheel speed gauge, selecting the wheel travel distance, rotation angle, and radius of motion measured by the wheel speed gauge as the measurement values. The measurement equation for the measurement data given by the wheel speed gauge is as follows:

[0024] Z k,wheel =h wheel (X k-1 )+R k,wheel

[0025] in,

[0026]

[0027] R k,wheel The measurement noise of the wheel speed gauge is set to Gaussian white noise;

[0028] Among them, [x k-1 y k-1 θ k-1 ] T For the wheel speed measurement data at time k-1, x k-1 The position of the robot at time k-1 is located along the x-axis, and the y-axis is the position along the x-axis. k-1 θ represents the position of the robot along the y-axis at time k-1. k-1 The angle of rotation of the robot around the z-axis at time k-1 is represented; after sampling time Δk, Δd k For the distance the wheel travels, Δθ k For the angle of rotation, R k Let be the radius of motion, and the following relationship applies:

[0029]

[0030] Where, Δd L Let Δd be the distance traveled by the left wheel. R τ is the distance traveled by the right wheel, p is the distance between the two wheels, r is the wheel radius, τ is the encoder parameter, and σ is the distance traveled by the right wheel. L Let σ be the number of pulses generated by the left wheel encoder within the sampling time Δk. R This represents the number of pulses generated by the right-hand encoder within the sampling time Δk.

[0031] As a further limitation of the present invention, step 3) establishes the unscented transformation sampling points and weights based on the state-space model, and the specific process is as follows:

[0032] The following formula is used to calculate the 2n+1 sigma points χ and the corresponding weight w of each point;

[0033]

[0034] The weights of each sigma point are as follows:

[0035]

[0036] Where, the mean of x is A state vector with variance P, χ i Let λ represent the sigma point, and n represent the dimension of the state vector; λ = α 2 (n+μ)-n is the scaling parameter, and α can be adjusted to allow the sigma sampling points to be positioned around the target area. The range size, μ is the scaling parameter; β is the state distribution parameter; Let m be the i-th column of the square root of matrix (n+λ)P; subscript m is the mean; c is the covariance; superscript i is the nth sampling point.

[0037] As a further limitation of the present invention, step 4) involves transferring the sampling points using a nonlinear function and updating the system state prediction and the state prediction covariance matrix. The specific process is as follows:

[0038] χ k,k-1 =f(χ) k-1 )

[0039]

[0040]

[0041] Where, χ k,k-1 The sigma points calculated from time k-1 to time k; This represents the predicted estimate of the system state from time k-1 to time k. For χ k,k-1 The i-th column, Let P be its weight coefficient, representing the weight coefficient of the i-th sampling point with mean m; k,k-1 The covariance matrix of the state prediction estimate. is the weight coefficient, representing the weight coefficient of the i-th sampling point with covariance c.

[0042] As a further limitation of the present invention, step 5) involves measurement updating, and the process noise and measurement noise covariance matrix is ​​updated using the improved Sage-Husa algorithm; the specific process is as follows:

[0043] Z k,k-1 =h(χ k-1 )

[0044]

[0045]

[0046]

[0047] The process noise is updated in real time as follows:

[0048]

[0049] The measured noise is updated in real time as follows:

[0050]

[0051] State prediction covariance matrix P k,k-1 and the variance matrix P of the measurement variables zz Further updates are as follows:

[0052]

[0053]

[0054] Among them, Z k,k-1 These are the predicted observations from time k-1 to time k; Let k be the predicted observation value at time k. For Z k,k-1 The i-th column; P zz for The covariance matrix, P xz The covariance matrix for the predicted state vector; b1 and b2 are forgetting factors, taken as b1 = 0.94, b2 = 0.96, d 1k d 2k The expression is as follows:

[0055]

[0056]

[0057] As a further limitation of the present invention, step 6) introduces a convergence factor to update the state estimate, filter gain, and state estimate covariance matrix; the specific process is as follows:

[0058]

[0059]

[0060]

[0061] Among them, K k Z is the Kalman gain at time k; k P is the observation vector at time k; k for The covariance matrix, P k,k-1 The one-step transition covariance matrix is ​​used to estimate the state from time k-1 to time k; ε is the convergence factor, expressed as follows:

[0062]

[0063] M k =P k -Φ k +N k

[0064]

[0065] Where tr(·) is the operator for finding the trace of a matrix, Φ k For η k The covariance matrix, Φ k The expression is as follows:

[0066]

[0067] Compared with the prior art, the beneficial effects of the present invention are as follows:

[0068] (1) In view of the uncertainty of noise in SLAM system, this invention introduces different forgetting factors to update the process noise and measurement noise of the system in real time, thereby optimizing the filtering effect and improving the accuracy of the system;

[0069] (2) By introducing a convergence factor, the state estimation is further estimated and updated. The convergence factor dynamically adjusts the step size according to the running state and error of the algorithm, thereby accelerating the convergence speed of the algorithm and improving the real-time performance of the system.

[0070] (3) The present invention also incorporates the measurement information of the wheel speed gauge to further improve the robustness and accuracy of the system. Attached Figure Description

[0071] To more clearly illustrate the technical solutions in the embodiments of the present invention or the prior art, the drawings used in the description of the embodiments or the prior art will be briefly introduced below. Obviously, the drawings described below are only embodiments of the present invention. For those skilled in the art, other drawings can be obtained based on the provided drawings without creative effort.

[0072] Figure 1 This is a system flowchart of the present invention. Detailed Implementation

[0073] 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.

[0074] like Figure 1 The illustrated visual-inertial wheel velocity measurement SLAM method includes the following steps:

[0075] Step 1) Establish the state equations for the robot SLAM system;

[0076] Step 1) Select the camera's attitude, velocity, position, and the IMU's acceleration and angular velocity deviations as the system states. The system's state equations are as follows:

[0077] X k =f(X) k-1 )+Q k

[0078] in,

[0079] Q k The system noise is set to Gaussian white noise.

[0080] Among them, [R k-1 v k-1 s k-1 b ω,k-1 b a,k-1 ] T R represents the system state at time k-1. k-1 Let v represent the rotation matrix of the camera at time k-1. k-1 Let s represent the camera's velocity at time k-1. k-1 b represents the position of the camera at time k-1. ω,k-1 b represents the acceleration error of the IMU at time k-1. a,k-1 This represents the angular velocity error of the IMU at time k-1. For each variable in the system, w is Gaussian white noise. ω For the Gaussian white noise of the IMU gyroscope, w a Gaussian white noise for the IMU accelerometer Gaussian white noise for IMU angular velocity deviation The Gaussian white noise for the IMU acceleration bias, [ω k-1 a k-1 ]T G represents the angular velocity and acceleration at time k-1. G Δt represents the acceleration due to gravity, and Δt represents the sampling time interval.

[0081] Step 2) Establish measurement models for the vision sensor and wheel speed meter;

[0082] Step 2-1) Establish the measurement model of the visual sensor, select the pixel coordinates measured by the camera as the measurement value, and the measurement equation for the measurement data given by the camera is as follows:

[0083] Z k,camera =h camera (X k-1 )+R k,camera

[0084] in,

[0085] h camera (X k-1 ) = KI 3×4 T cw p i ′, R k,camera The measurement noise of the camera is set to Gaussian white noise.

[0086] Where K is the intrinsic parameter matrix of the camera, I 3×4 It is a 3×4 matrix, and I 3×4 =[I 3×3 0 3×1 ], I 3×3 It is a 3×3 identity matrix, 0 3×1 T is a 3×1 column vector; cw Let be the transformation matrix from the world coordinate system to the camera coordinate system, and Among them, R cw and t cw Let p' and p' represent the rotation matrix and translation vector from the world coordinate system to the camera coordinate system, respectively. i For p i homogeneous coordinates, and p i These are the three-dimensional coordinates of the camera's observation point in the world coordinate system.

[0087] Step 2-2) Establish the measurement model of the wheel speed gauge, selecting the wheel travel distance, rotation angle, and radius of motion measured by the wheel speed gauge as the measurement values. The measurement equation for the measurement data given by the wheel speed gauge is as follows:

[0088] Z k,wheel =h wheel (X k-1 )+R k,wheel

[0089] in,

[0090]

[0091] R k,wheel The measurement noise of the wheel speed gauge is set to Gaussian white noise.

[0092] Among them, [x k-1 y k-1 θ k-1 ] T For the wheel speed measurement data at time k-1, x k-1 The position of the robot at time k-1 is located along the x-axis, and the y-axis is the position along the x-axis. k-1 θ represents the position of the robot along the y-axis at time k-1. k-1 The angle of rotation of the robot around the z-axis at time k-1 is represented; after sampling time Δk, Δd k For the distance the wheel travels, Δθ k For the angle of rotation, R k Let be the radius of motion, and the following relationship applies:

[0093]

[0094] Where, Δd L Let Δd be the distance traveled by the left wheel. R τ is the distance traveled by the right wheel, p is the distance between the two wheels, r is the wheel radius, τ is the encoder parameter, and σ is the distance traveled by the right wheel. L Let σ be the number of pulses generated by the left wheel encoder within the sampling time Δk. R This represents the number of pulses generated by the right-hand encoder within the sampling time Δk.

[0095] Step 3) Establish the unscented transformation sampling points and weights based on the state-space model. The specific process is as follows:

[0096] The following formula is used to calculate 2n+1 sigma points χ and the corresponding weight w of each point.

[0097]

[0098] The weights of each sigma point are as follows:

[0099]

[0100] Where x is the mean. A state vector with variance P, χ i Let λ represent the sigma point, and n represent the dimension of the state vector; λ = α 2 (n+μ)-n is the scaling parameter, and α adjusts the size of the range of sigma sampling points around x. It is usually set to a very small positive value, such as 10. -3μ is the scaling parameter. For Gaussian distribution problems, if the state variable is a single variable, μ is usually taken as 2. If the state variable is a multivariate variable, μ is usually taken as 3-n. β is the state distribution parameter, and β is usually optimally taken as 2. Let m be the i-th column of the square root (Cholesky decomposition) of matrix (n+λ)P; subscript m is the mean; c is the covariance; superscript i is the nth sampling point.

[0101] Step 4) Perform nonlinear function transfer on the sampling points and update the system state prediction and state prediction covariance matrix. The specific process is as follows:

[0102] χ k,k-1 =f(χ) k-1 )

[0103]

[0104]

[0105] Where, χ k,k-1 The sigma points calculated from time k-1 to time k; This represents the predicted estimate of the system state from time k-1 to time k. For χ k,k-1 The i-th column, Let P be its weight coefficient, representing the weight coefficient of the i-th sampling point with mean m; k,k-1 The covariance matrix of the state prediction estimate. is the weight coefficient, representing the weight coefficient of the i-th sampling point with covariance c.

[0106] Step 5) Measurement update, and update the process noise and measurement noise covariance matrix using the improved Sage-Husa algorithm. The specific process is as follows:

[0107] Z k,k-1 =h(χ k-1 )

[0108]

[0109]

[0110]

[0111] The process noise is updated in real time as follows:

[0112]

[0113] The measured noise is updated in real time as follows:

[0114]

[0115] State prediction covariance matrix P k,k-1 and the variance matrix P of the measurement variables zz Further updates are as follows:

[0116]

[0117]

[0118] Among them, Z k,k-1 These are the predicted observations from time k-1 to time k; Let k be the predicted observation value at time k. For Z k,k-1 The i-th column; P zz for The covariance matrix, P xz The covariance matrix for the predicted state vector; b1 and b2 are forgetting factors, taken as b1 = 0.94, b2 = 0.96, d 1k d 2k The expression is as follows:

[0119]

[0120]

[0121] Step 6) Introduce a convergence factor and update the state estimate, filter gain, and state estimate covariance matrix. The specific process is as follows:

[0122]

[0123]

[0124]

[0125] Among them, K k Z is the Kalman gain at time k; k P is the observation vector at time k; k for The covariance matrix, P k,k-1 The one-step transition covariance matrix is ​​used to estimate the state from time k-1 to time k; ε is the convergence factor, expressed as follows:

[0126]

[0127] M k =P k -Φ k +N k

[0128]

[0129] Where tr(·) is the operator for finding the trace of a matrix, Φ k For η k The covariance matrix, Φ k The expression is as follows:

[0130]

[0131] This invention addresses the uncertainty of noise in SLAM systems by introducing different forgetting factors to update the process noise and measurement noise in real time, thereby optimizing the filtering effect and improving the system's accuracy. Furthermore, it introduces a convergence factor to further update the state estimation, dynamically adjusting the step size based on the algorithm's running state and error conditions, thus accelerating the algorithm's convergence speed and improving the system's real-time performance. Finally, this invention incorporates wheel speed measurement information to further enhance the system's robustness and accuracy.

[0132] The above description of the embodiments is only for the purpose of helping to understand the method and core ideas of the present invention. It should be noted that those skilled in the art can make several improvements and modifications to the present invention without departing from the principles of the present invention, and these improvements and modifications also fall within the protection scope of the claims of the present invention.

Claims

1. A visual-inertial wheel velocity measurement SLAM method based on an improved adaptive UKF, characterized in that, Includes the following steps: Step 1) Establish the state equations for the robot SLAM system, specifically: For a robot SLAM system equipped with an IMU and a vision sensor, the camera's attitude, velocity, and position, and the IMU's acceleration and angular velocity deviations are selected as the system states. A world coordinate system is established with the robot's initial position as the origin, with due east as the positive direction of the x-axis and due north as the positive direction of the y-axis. The positive direction of the z-axis is determined according to the right-hand rule. A body coordinate system is established with the camera as the center, with the direction parallel to the camera's horizontal axis pointing to the right as the positive direction of the x-axis and the direction directly in front of the camera as the positive direction of the y-axis. The positive direction of the z-axis is determined according to the right-hand rule. Selecting the camera's attitude, velocity, and position, and the IMU's acceleration and angular velocity deviations as the system states, the system's state equations are as follows: in, , The system noise is set to Gaussian white noise; in, express The system state at any given moment. Indicates the camera is in The rotation matrix at time step, Indicates the camera is in The speed of time, Indicates the camera is in Location at any given moment Indicates that IMU is in Acceleration error at any moment Indicates that IMU is in Angular velocity error at time t, The system variables are represented by Gaussian white noise. The Gaussian white noise of the IMU gyroscope Gaussian white noise for the IMU accelerometer Gaussian white noise for IMU angular velocity deviation Gaussian white noise for IMU acceleration bias express Angular velocity and acceleration at time t. It is the acceleration due to gravity. The sampling time interval; Step 2) Establish the measurement model for the vision sensor and wheel speed meter, specifically including: Step 2-1) Establish a measurement model for the visual sensor, selecting the pixel coordinates measured by the camera as the measurement values. The measurement equation for the measurement data given by the camera is as follows: in, , The measurement noise of the camera is set to Gaussian white noise; in, This is the intrinsic parameter matrix of the camera. for The matrix, and , for The identity matrix, for Column vectors; Let be the transformation matrix from the world coordinate system to the camera coordinate system, and ,in, and Let represent the rotation matrix and translation vector from the world coordinate system to the camera coordinate system, respectively. for homogeneous coordinates, and , The three-dimensional coordinates of the camera observation point in the world coordinate system; Step 2-2) Establish the measurement model of the wheel speed gauge, selecting the wheel travel distance, rotation angle, and radius of motion measured by the wheel speed gauge as the measurement values. The measurement equation for the measurement data given by the wheel speed gauge is as follows: in, The measurement noise of the wheel speed gauge is set to Gaussian white noise; in, for Measurement data from the wheel speed gauge at all times. Represents robots The position of the time along the x-axis. Represents robots The position of the moment along the y-axis. Represents robots The angle of rotation around the z-axis at any given time; the time elapsed after sampling. , For the distance the wheels travel, For the angle of rotation, Let be the radius of motion, and the following relationship applies: in, The distance traveled by the left wheel. The distance traveled by the right wheel. The distance between the two wheels. For the wheel radius, For encoder parameters, To be at the sampling time The number of pulses generated by the inner left wheel encoder, To be at the sampling time The number of pulses generated by the inner right wheel encoder; Step 3) Establish the sampling points and weights for the unscented transformation based on the state-space model; Step 4) Transfer the sampling points using a nonlinear function and update the system state prediction and state prediction covariance matrix; Step 5) Measurement update, and update the process noise and measurement noise covariance matrix using the improved Sage-Husa algorithm; Step 6) Introduce a convergence factor and update the state estimate, filter gain, and state estimate covariance matrix.

2. The visual-inertial wheel velocity measurement SLAM method based on improved adaptive UKF as described in claim 1, characterized in that, Step 3) Establish the unscented transformation sampling points and weights based on the state-space model. The specific process is as follows: Calculated using the following formula sigma points and the corresponding weights of each point ; The weights of each sigma point are as follows: in, The mean is variance is The state vector, Represents the sigma point. The dimension of the state vector; This is the scaling parameter. Adjustable sigma sampling point surround The size of the range, This is the scaling ratio parameter; These are the state distribution parameters; For matrix The square root of the first Column; Subscript The mean; Covariance; superscript This refers to the nth sampling point.

3. The visual-inertial wheel velocity measurement SLAM method based on improved adaptive UKF as described in claim 2, characterized in that, Step 4) Perform nonlinear function transfer on the sampling points and update the system state prediction and state prediction covariance matrix. The specific process is as follows: in, for Time's up The sigma point calculated at time step; for Time's up The predicted estimate of the system state at time t. for The List, Let be its weight coefficient, representing the mean as . The Weighting coefficients for each sampling point; The covariance matrix of the state prediction estimate. The weight coefficients represent the covariance. The The weighting coefficients of each sampling point.

4. The visual-inertial wheel velocity measurement SLAM method based on improved adaptive UKF according to claim 3, characterized in that, Step 5) Measurement update, and update the process noise and measurement noise covariance matrix using the improved Sage-Husa algorithm; the specific process is as follows: The process noise is updated in real time as follows: The measured noise is updated in real time as follows: State prediction covariance matrix and the variance matrix of measurement variables Further updates are as follows: in, for Time's up Predicted observations at time; for Predicted observations at time, for The List; for The covariance matrix, The covariance matrix for the predicted state vector; , for The observation vector at time t, , Forgetting factor, take , , , The expression is as follows: 。 5. The visual-inertial wheel velocity measurement SLAM method based on improved adaptive UKF according to claim 1, characterized in that, Step 6) Introduce a convergence factor and update the state estimate, filter gain, and state estimate covariance matrix; the specific process is as follows: in, for Kalman gain at time step; for The observation vector at time; for The covariance matrix, for Time's up State estimation at time step one: transition covariance matrix; The convergence factor is expressed as follows: in, It is an operator for finding the trace of a matrix. for The covariance matrix, The expression is as follows: .

Images