SLAM method based on maximum a posteriori estimation of underwater motion drift and noise

Abstract

The present invention discloses a SLAM method for underwater motion drift and noise estimation based on maximum a posteriori estimation, comprising: 1) estimating the motion drift of an underwater robot based on EKF-SLAM; 1.1) initializing the robot SLAM system state; 1.2) constructing sampling points based on EKF-SLAM state estimation; 1.3) updating the covariance matrix based on the state estimation error; 2) updating the motion drift of the underwater robot based on maximum a posteriori probability; 3) estimating underwater noise parameters based on EKF-SLAM; 3.1) initializing the underwater noise parameters; 3.2) estimating the underwater noise parameter covariance matrix; 3.3) updating the covariance matrix based on the state estimation; and 4) updating the underwater noise parameters based on maximum a posteriori probability. This method improves the state prediction portion of the existing EKF-SLAM method, combines estimation of motion noise and observation noise in the system model, adaptively filters the Gaussian distribution noise variance of the system model, and then performs SLAM estimation, thereby improving underwater self-positioning accuracy by approximately 20%.

Description

Technical Field

[0001] The present invention relates to the technical field of underwater robots, and in particular to a SLAM method based on maximum a posteriori estimation of underwater motion drift and noise. Background Art

[0002] With the development of unmanned driving technology, research on real-time positioning and navigation (Localization and Mapping) of underwater mobile robots has received increasing attention. However, the complex underwater environment brings the following challenges: (1) The changes in underwater ambient light lead to insufficient feature point extraction, resulting in large positioning errors; (2) The robot's motion is unstable due to the interference of water flow, resulting in large underwater motion drift; (3) Underwater dynamic noise can cause large noise errors, resulting in large errors in observation data. The existence of these problems restricts the positioning and navigation accuracy of unmanned driving technology underwater, and cannot meet the trajectory accuracy and safety requirements of underwater robot adaptive cruising.

[0003] In 2019, Bai Chengchao and others from Harbin Institute of Technology proposed an EKF-SLAM robot positioning and mapping method based on the combination of perpendicular point and line features (authorization announcement number: CN 110866927B). The perpendicular point from the origin to the straight line in the robot coordinate system and the perpendicular point from the origin to the straight line in the world coordinate system are used as the landmark feature points of the line segment, and the endpoint iteration method is used to perform multi-segment fitting on continuous points. This method is mainly applicable to air environments. If it is used in underwater environments, there are problems such as the influence of the underwater environment on the robot movement and the cumulative increase of the landmark error of the underwater environment by iteratively calculating the covariance matrix of each frame. However, this method provides a good idea for the research on real-time positioning and navigation of underwater mobile robots.

[0004] To address the issue of large errors in robot positioning and mapping in underwater environments, in 2019, Li Wanli and others from the Information Engineering University of the Strategic Support Force of the Chinese People's Liberation Army proposed a water velocity estimation method, integrated navigation method, and device (Authorization Announcement No.: CN 110873813B). This method establishes a system and observation equation for water velocity based on the velocity obtained by the integrated navigation system and a Doppler velocimeter. This method considers the error caused by water flow in state prediction, improving the accuracy of navigation results. However, since it uses a Kalman filter to calculate water velocity, it is not suitable for systems significantly affected by non-Gaussian white noise underwater. In 2021, Xia Linlin and others from Northeast Electric Power University proposed an underwater simultaneous positioning and mapping method based on polarization light / inertial / vision integrated navigation (Application Publication No. CN 113739795A) to address the accuracy of positioning and mapping in underwater environments. This method uses polarization light to measure the position and pose of an underwater vehicle and fuses it with an adaptive unscented Kalman filter. By adjusting the orientation of the polarization sensor, it obtains a more accurate observation state, thereby improving navigation positioning accuracy. This method is an improvement based on the hardware method. On the one hand, the use of hardware equipment such as polarization sensors increases the cost. On the other hand, the error accumulation of each frame will still lead to a large error.

[0005] In summary, existing Extended Kalman Filter Simultaneous Localization and Mapping (EKF-SLAM) methods assume that environmental noise follows a normal distribution with a mean of zero. However, during robot motion, underwater environmental noise is inherently uncertain. If the unknown underwater motion drift and noise could be estimated and compensated for, the resulting environmental map could improve the underwater robot's self-localization accuracy. Summary of the Invention

[0006] The purpose of the present invention is to overcome the defects of the existing technology and provide a SLAM method based on maximum a posteriori estimation of underwater motion drift and noise. By improving the state prediction part of the existing EKF-SLAM method, combining the estimation of motion noise and observation noise in the system model, adaptively filtering the Gaussian distribution noise variance of the system model, and then performing SLAM estimation, the positioning estimation and mapping accuracy of the mobile robot are improved.

[0007] The object of the present invention is achieved in this way: a SLAM method based on maximum a posteriori estimation of underwater motion drift and noise, comprising the following steps:

[0008] Step 1) estimating the motion drift of the underwater robot based on EKF-SLAM;

[0009] Step 1.1) Initialize the robot SLAM system state;

[0010] Step 1.2) Construct sampling points based on EKF-SLAM state estimation;

[0011] Step 1.3) Update the covariance matrix based on the state estimation error;

[0012] Step 2) updating the motion drift of the underwater robot based on the maximum a posteriori probability;

[0013] Step 3) estimating underwater noise parameters based on EKF-SLAM;

[0014] Step 3.1) Initialize underwater noise parameters;

[0015] Step 3.2) Estimation of underwater noise parameter covariance matrix;

[0016] Step 3.3) Update the covariance matrix based on the state estimate;

[0017] Step 4) Update the underwater noise parameters based on the maximum a posteriori probability.

[0018] In order to be applicable to complex underwater noise scenarios, step 1.2) specifically includes: realizing parameter estimation of the state space model constructed based on the equation through a nonlinear Kalman filter based on EKF, linearizing the nonlinear input and output equations through Taylor formula, and estimating and optimizing the mean and variance of the state vector.

[0019] The underwater noise input error is added as compensation to the robot posture state equation, which reduces the trajectory error problem caused by noise interference in the complex underwater environment and improves the Kalman filter effect under the nonlinear system. The step 1.3) specifically includes: according to step 1.1) and step 1.2), the underwater robot is at the sampling point ζ i,k The estimated value of the state at time k at state k-1 for:

[0020]

[0021] f is the state equation of the nonlinear system at time k, f(ζ i,k ,θ i ) is the estimated value of the system state at time k, θ i is the underwater uncertain noise parameter of the sampling point i; the drift error parameter Δx at time k k It is the difference between the actual system state matrix at time k and the estimated value of the state at time k-1, which is recorded as:

[0022] The underwater robot is at the sampling point i,kThe covariance matrix of the state at time k-1 to the state at time k is:

[0023]

[0024] where θ k|k-1 is the input error from time k-1 to time k, denoted as: θ k| k-1=θ k-1 +e k-1 ,θ k-1 is the underwater uncertain noise parameter at time k-1, e k-1 is a Gaussian white noise vector with a mean of 0 at time k-1. According to formula (6), the covariance matrix of the state at time k-1 to time k is obtained, so as to accurately obtain the sampling points of state estimation.

[0025] Observation vector estimated based on k moments By analogy with formulas (5) and (6), we can derive formulas (7) and (8) to calculate the observed estimator and covariance.

[0026]

[0027] h is the observation function at time k, h(ζ i,k ,θ k|k-1 ) is the system observation quantity at time k, ω i,k|k-1 is the weight of the k-1 moment estimate for the k moment, based on the observed estimator The precision difference is dynamically assigned.

[0028] Furthermore, the step 2) specifically includes:

[0029] Based on Bayesian filtering theory, estimate the motion drift parameter Δx k It is based on the posterior probability density The loss function at this time is Expressed as:

[0030]

[0031] Based on the maximum a posteriori probability, the parameter θ is given by formula (9) k|k-1 Derivative, solve the most accurate motion drift parameter Δx k , and then correct the estimate of time k at time k-1 Finally, the posterior covariance is calculated based on the extended Kalman filter, and the updated Substitute into formula (8) and record part of it as cross covariance P z,k|k-1 , expressed as:

[0032]

[0033] Extended Kalman filter K based on motion drift update of underwater vehicle k Denoted as:

[0034]

[0035] k-time estimation based on motion drift update of underwater robot Expressed as:

[0036]

[0037] The posterior covariance updated based on the motion drift of the underwater robot is recorded as:

[0038]

[0039] According to formula (12) and formula (5), the underwater mean error value is estimated jointly. Then, according to formula (13) and formula (6), the underwater motion drift under the mean error is estimated. At the same time, let the parameter Δx in formula (13) be k Minimize the loss and get the appropriate θ k|k-1 , let k = 1, and change θ 1|0 Substitute into step 3).

[0040] The present invention adopts the above technical solution, and compared with the existing technology, the beneficial effects are as follows: a maximum a posteriori probability density estimation method is designed for underwater motion drift and noise error, and an adaptive EKF SLAM algorithm is designed for state estimation of the mobile robot, which realizes noise parameter estimation and state estimation of the mobile robot. The state estimation of the EKF-SLAM filter is used as the input of the maximum a posteriori estimator, and the output underwater noise estimation value is used as the input of the noise system; the parameter estimation value based on the maximum a posteriori estimation noise is passed to the EKF-SLAM filter system model, and finally a closed loop is formed to estimate the SLAM state and noise variance parameters of the mobile robot at each moment. By improving the state prediction part of the existing EKF-SLAM method, combining the estimation of motion noise and observation noise in the system model, adaptively filtering the Gaussian distribution noise variance of the system model, and then performing SLAM estimation, the positioning estimation and mapping accuracy of the mobile robot are improved. BRIEF DESCRIPTION OF THE DRAWINGS

[0041] Figure 1 Flowchart of the present invention.

[0042] Figure 2 Explanatory analysis diagram of underwater motion error of the present invention. DETAILED DESCRIPTION

[0043] like Figure 1The SLAM method based on maximum a posteriori estimation of underwater motion drift and noise includes the following steps:

[0044] Step 1) estimating the motion drift of the underwater robot based on EKF-SLAM;

[0045] Step 1.1) Initialize the robot SLAM system state;

[0046] set up is the estimated value of the state in the initial state, is the observation estimator in the initial state, and P0 is the covariance matrix in the initial state:

[0047]

[0048] Among them, x is the actual system state matrix, x0 is the initial system state value, z is the observation value of the actual system, z0 is the observation value under the initial state, f0 is the state transfer function containing uncertain noise parameters, h0 is the observation function containing uncertain noise parameters, w0 is the motion noise under the initial state, v0 is the observation noise under the initial state, assuming that w0 and v0 are both normal distributions with expectation of 0; E0 is the covariance matrix function of the initial system, is the uncertainty-estimated noise parameter other than the estimable motion noise w and observation noise v, which is related to the system state x and observation z. It can be expressed as: Among them, θ0 is the underwater uncertain noise parameter in the initial state, and e0 represents the Gaussian white noise vector with a mean of 0 in the initial state.

[0049] Step 1.2) Construct sampling points based on EKF-SLAM state estimation;

[0050] Set i is the sampling point in the i-th state, ζ i+n is the sampling point in the i+n state, ω i is the weight corresponding to the i-th sampling point:

[0051]

[0052] Among them, 2n is the total number of sampling points, is the actual system state vector x i The covariance matrix of i is the Gaussian white noise vector with a mean of 0 at the i-th sampling point, is the state estimation value of the previous frame. When k>i, ζ i,k and ω i,k The observation values ​​obtained by the robot in state i are used as prior conditions to calculate the sampling points and corresponding weights at time k, ζ i,kThe value of can be solved by combining formulas (1)-(4). i,k It is related to the number of sampling points i. When the number of sampling points is greater than 8, the value is generally between 0 and 0.2.

[0053] Step 1.3) Update the covariance matrix based on the state estimation error;

[0054] According to step 1.1) and step 1.2), the underwater robot is at the sampling point ζ i,k The estimated value of the state at time k at state k-1 for:

[0055]

[0056] f is the state equation of the nonlinear system at time k, f(ζ i,k ,θ i ) is the estimated value of the system state at time k, θ i is the underwater uncertainty noise parameter of the sampling point i. The drift error parameter Δx at time k k It is the difference between the actual system state matrix at time k and the estimated value of the state at time k-1, which is recorded as:

[0057] The underwater robot is at the sampling point i,k The covariance matrix of the state at time k-1 to the state at time k is:

[0058]

[0059] where θ k|k-1 is the input error from time k-1 to time k, denoted as: θ k|k-1 =θ k-1 +e k-1 ,θ k-1 is the underwater uncertain noise parameter at time k-1, e k-1 is a Gaussian white noise vector with a mean of 0 at time k-1. According to formula (6), the covariance matrix of the state at time k-1 to time k can be obtained, so as to obtain the sampling points of state estimation more accurately.

[0060] Observation vector estimated based on k moments By analogy with formulas (5) and (6), we can derive formulas (7) and (8) to calculate more accurate observation estimates and covariances.

[0061]

[0062] h is the observation function at time k, h(ζ i,k ,θ k|k-1 ) is the system observation quantity at time k, ωi,k|k-1 is the weight of the k-1 moment estimate for the k moment, based on the observed estimator The accuracy difference is dynamically assigned. If a high-precision lidar sensor is used, the weight is approximately 0.2-0.5. If a camera or infrared thermal imager is used, the weight is set in the range of 0.01-0.2, ensuring more accurate state updates for the underwater robot. Compensating for underwater noise input errors in the robot's pose state equation reduces trajectory errors caused by noise interference in complex underwater environments and improves the Kalman filter's effectiveness in nonlinear systems.

[0063] Step 2) Update the motion drift of the underwater robot based on the maximum posterior probability, such as Figure 2 As shown;

[0064] Based on Bayesian filtering theory, estimate motion drift parameters It is based on the posterior probability density The loss function at this time is It can be expressed as:

[0065]

[0066] Based on the maximum a posteriori probability, the parameter θ is given by formula (9) k|k-1 Derivative, solve the most accurate motion drift parameter Δx k , Recalibrate the estimate of time k at time k-1 Finally, the posterior covariance is calculated based on the extended Kalman filter, and the updated Substitute into formula (8) and record part of it as cross covariance P z,k|k-1 , expressed as:

[0067]

[0068] Extended Kalman filter K based on motion drift update of underwater vehicle k Denoted as:

[0069]

[0070] k-time estimation based on motion drift update of underwater robot Expressed as:

[0071]

[0072] The posterior covariance updated based on the motion drift of the underwater robot is recorded as:

[0073]

[0074] According to formula (12) and formula (5), the underwater mean error value is estimated, and then the underwater motion drift under the mean error is estimated according to formula (13) and formula (6). At the same time, the motion drift loss Δx in formula (13) is calculated. k Minimize the covariance and get the appropriate θ k|k-1 , let k = 1, and change θ 1|0 Substitute into step 3).

[0075] Step 3) estimating underwater noise parameters based on EKF-SLAM;

[0076] Step 3.1) Initialize underwater noise parameters;

[0077] Noise parameters of uncertain estimates at the initial moment And the covariance matrix corresponding to the noise parameters is recorded as M0:

[0078]

[0079] Step 3.2) Estimation of underwater noise parameter covariance matrix;

[0080] Underwater noise parameters The error covariance matrix M estimated at time k-1 is k|k-1 :

[0081]

[0082] M k-1 is the covariance matrix of the noise parameters estimated at the k-1 moment uncertainty, is the covariance matrix corresponding to the Gaussian white noise vector with a mean of 0 at time k;

[0083] Observation vector estimated based on k moments Compute more accurate observation estimates and the noise parameter covariance matrix

[0084]

[0085] in is the estimated observation at time k-1, h(ξ i,k|k-1 ) is the sampling point ξ i,k|k-1 The observed value under α i,k|k-1 is the estimated weight of k-1 at time k, which is related to the water quality. If there are few suspended particles underwater, the weight is 0.2-0.5. If the water is very turbid, the weight can be set to 0.001-0.009;

[0086] Assume that the cross covariance matrix based on underwater noise parameters is M z,k|k-1 :

[0087]

[0088] Step 3.3) Update the covariance matrix based on the state estimate;

[0089] Extended Kalman Filtering of Underwater Noise Parameters k and the noise parameter covariance matrix M at time k k It can be calculated according to formula (19) and formula (20):

[0090]

[0091] The underwater noise parameter estimation is calculated using formula (21) at the estimated sampling point at time k-1 to k:

[0092]

[0093] Where λ is the mean value of the scattering noise, which is related to the intensity of the underwater ambient light.

[0094] Step 4) updating underwater noise parameters based on the maximum a posteriori probability;

[0095] Based on Bayesian filtering theory, the estimated parameter θ k It is based on the posterior probability density The loss function can be expressed as:

[0096]

[0097] The posterior probability density function is obtained by recursively calculating the observation sequence of the sampling points, and the underwater noise parameter vector θ is k|k-1 Perform Bayesian filtering estimation; Maximum a posteriori estimation can also be solved by Kalman filter, which is achieved through second-order string interpolation filtering, unscented Kalman filtering and spherical volume filtering;

[0098] The underwater noise parameter vector θ calculated according to formula (23) k|k-1 , update the covariance matrix of the state and observation vector, the Kalman gain matrix and the covariance matrix of the state estimation error:

[0099]

[0100] Finally, the noise parameters estimated at time k are obtained for:

[0101]

[0102] The estimated noise parameters solved Substitute into formula (24) to estimate the covariance matrix P based on the robot pose update under the corresponding observation state k . As the updated value of the covariance matrix in the next step 1.3).

[0103] The present invention improves the state prediction part of the existing EKF-SLAM method, combines the estimation of motion noise and observation noise in the system model, adaptively filters the Gaussian distribution noise variance of the system model, and then performs SLAM estimation, thereby improving the positioning estimation and mapping accuracy of the mobile robot.

[0104] The present invention is not limited to the above-mentioned embodiments. On the basis of the technical solutions disclosed in the present invention, those skilled in the art can make some substitutions and modifications to some of the technical features therein according to the disclosed technical content without creative labor, and these substitutions and modifications are all within the protection scope of the present invention.

Claims

1. A SLAM method based on maximum a posteriori estimation of underwater motion drift and noise, characterized in that: The following steps are involved: Step 1) estimating the motion drift of the underwater robot based on EKF-SLAM; Step 1.1) Initialize the robot SLAM system state; Step 1.2) Construct sampling points based on EKF-SLAM state estimation; Step 1.3) Update the covariance matrix based on the state estimation error; Calculating covariance h is the observation function at time k, h(ζ i,k ,θ k|k-1 ) is the system observation quantity at time k, ω i,k|k-1 is the weight of the k-1 moment estimate for the k moment, based on the observed estimator The accuracy difference is dynamically assigned; Step 2) updating the motion drift of the underwater robot based on the maximum a posteriori probability; The step 2) specifically includes: Based on Bayesian filtering theory, estimate the motion drift parameter Δx k is based on the posterior probability density The loss function at this time is Expressed as: Based on the maximum a posteriori probability, the parameter θ is given by formula (9) k|k-1 Derivative, solve the most accurate motion drift parameter Δx k , and then correct the estimate of time k at time k-1 Finally, the posterior covariance is calculated based on the extended Kalman filter, and the updated Substitute into formula (8) and record part of it as cross covariance P z,k|k-1 , expressed as: Extended Kalman filter K based on motion drift update of underwater vehicle k Denoted as: k-time estimation based on motion drift update of underwater robot Expressed as: The posterior covariance updated based on the motion drift of the underwater robot is recorded as: According to formula (12) and the state estimation value at time k-1, The underwater mean error is estimated jointly, and then the underwater motion drift under the mean error is estimated according to formula (13) and the covariance matrix formula of the state at time k-1 to time k. At the same time, let the parameter Δx in formula (13) be k Minimize the loss and get the appropriate θ k|k-1 , let k = 1, and change θ 1|0 Substitute into step 3); Step 3) estimating underwater noise parameters based on EKF-SLAM; Step 3.1) Initialize underwater noise parameters; Step 3.2) Estimation of underwater noise parameter covariance matrix; Step 3.3) Update the covariance matrix based on the state estimate; Step 4) updating underwater noise parameters based on the maximum a posteriori probability; Using the estimated noise parameters solved Estimate the covariance matrix P based on the robot pose update under the corresponding observation state k ; as the updated value of the covariance matrix in the next step 1.3).

2. The SLAM method based on maximum a posteriori estimation of underwater motion drift and noise according to claim 1, wherein The step 1.2) specifically includes: realizing parameter estimation of the state space model constructed based on the equation through a nonlinear Kalman filter based on EKF, linearizing the nonlinear input and output equations through Taylor formula, and estimating and optimizing the mean and variance of the state vector.

3. The SLAM method based on maximum a posteriori estimation of underwater motion drift and noise according to claim 1, wherein The step 1.3) specifically includes: according to the steps 1.1) and 1.2), the underwater robot is at the sampling point ζ i,k The estimated value of the state at time k at state k-1 for: f is the state equation of the nonlinear system at time k, f(ζ i,k ,θ i ) is the estimated value of the system state at time k, θ i is the underwater uncertain noise parameter of the sampling point i; the drift error parameter Δx at time k k It is the difference between the actual system state matrix at time k and the estimated value of the state at time k-1, which is recorded as: ζ i,k and ω i,k The observation values ​​obtained by the robot in state i are used as prior conditions to calculate the sampling points and corresponding weights at time k; The underwater robot is at the sampling point i,k The covariance matrix of the state at time k-1 to the state at time k is: where θ k|k-1 is the input error from time k-1 to time k, denoted as: θ k| k-1=θ k-1 +e k-1 ,θ k-1 is the underwater uncertain noise parameter at time k-1, e k-1 is a Gaussian white noise vector with a mean of 0 at time k-1. According to formula (6), the covariance matrix of the state at time k-1 to time k is obtained, so as to accurately obtain the sampling points of state estimation. Observation vector estimated based on k moments By analogy with formulas (5) and (6), we can derive formulas (7) and (8) to calculate the observed estimator and covariance. h is the observation function at time k, h(ζ i,k ,θ k|k-1 ) is the system observation quantity at time k, ω i,k|k-1 is the weight of the k-1 moment estimate for the k moment, based on the observed estimator The precision difference is dynamically assigned.

Images