CN115388899A - Mobile robot vision inertia fusion SLAM method based on variational Bayes - Google Patents

Mobile robot vision inertia fusion SLAM method based on variational Bayes Download PDF

Info

Publication number
CN115388899A
CN115388899A CN202211125734.3A CN202211125734A CN115388899A CN 115388899 A CN115388899 A CN 115388899A CN 202211125734 A CN202211125734 A CN 202211125734A CN 115388899 A CN115388899 A CN 115388899A
Authority
CN
China
Prior art keywords
mobile robot
iteration
representing
distribution
state vector
Prior art date
Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
Pending
Application number
CN202211125734.3A
Other languages
Chinese (zh)
Inventor
朱冬
方向明
张建
唐国梅
宋雯
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Seven Teng Robot Co ltd
Original Assignee
Seven Teng Robot Co ltd
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by Seven Teng Robot Co ltd filed Critical Seven Teng Robot Co ltd
Priority to CN202211125734.3A priority Critical patent/CN115388899A/en
Publication of CN115388899A publication Critical patent/CN115388899A/en
Pending legal-status Critical Current

Links

Images

Classifications

    • GPHYSICS
    • G01MEASURING; TESTING
    • G01CMEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/20Instruments for performing navigational calculations
    • GPHYSICS
    • G01MEASURING; TESTING
    • G01CMEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/10Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration
    • G01C21/12Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning
    • G01C21/16Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation
    • G01C21/165Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation combined with non-inertial navigation instruments
    • G01C21/1656Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation combined with non-inertial navigation instruments with passive imaging devices, e.g. cameras
    • GPHYSICS
    • G01MEASURING; TESTING
    • G01CMEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/10Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration
    • G01C21/12Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning
    • G01C21/16Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation
    • G01C21/18Stabilised platforms, e.g. by gyroscope

Landscapes

  • Engineering & Computer Science (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Automation & Control Theory (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)

Abstract

The invention provides a mobile robot vision inertia fusion SLAM method based on variational Bayes, which comprises the following steps: s1, obtaining a predicted state vector and a covariance matrix of a mobile robot at a moment k; s2, establishing a model of observation noise at the k moment, which accords with the mixed distribution of student t based on gamma distribution, and obtaining likelihood function distribution in a hierarchical Gaussian form based on visual information, an observation noise model and a prediction state vector of the mobile robot at the k moment; and S3, constructing a combined posterior probability density function based on the likelihood function distribution, and performing combined solution on the posterior combined probability density function by using a variational Bayes method to obtain a state vector of the mobile robot at the time k. A new distribution is provided for modeling observation noise so as to reduce the influence of non-stationary thick tail noise on the precision of a visual and inertial data fusion result, and the variational Bayes is utilized to realize the joint estimation of the state of the mobile robot and improve the precision of the finally output state vector.

Description

Mobile robot vision inertia fusion SLAM method based on variational Bayes
Technical Field
The invention relates to the technical field of intelligent robots, in particular to a mobile robot vision inertia fusion SLAM method based on variational Bayes.
Background
Synchronous positioning And map building (SLAM) refers to a process of extracting And combining useful information in an unknown environment while a mobile robot runs in the unknown environment, determining self pose And gradually building a surrounding environment map. SLAM has been successfully applied to underwater unmanned vehicles, aerial unmanned vehicles and the like, and has received more and more attention.
The Bayesian estimation-based filtering is widely applied to SLAM rear-end optimization, and the current-time state vector is obtained according to the last-time state vector of the mobile robot and the current-time estimated value. The first appeared SLAM algorithm based on Extended Kalman Filter (EKF), which needs to utilize a first-order Taylor expansion to linearize a motion model and an observation model in the SLAM algorithm before estimating the pose and the map feature position of a robot by using EKF, but corresponding errors are introduced into the linearization processes of the two models, and in addition, the calculation process of a Jacobian matrix also brings great burden to the algorithm. Then, unscented Kalman Filtering (UKF) occurs, and patent CN109141436A uses this method to substitute a sampling point set with a certain weight into a motion model and an observation model of the SLAM algorithm, thereby avoiding linearization in the calculation of state information and observation information of the mobile robot. However, when the system dimension is gradually increased, a non-local sampling phenomenon occurs in the UKF, which directly causes the calculation result of the SLAM to be unstable and even to be divergent. Next, a SLAM algorithm based on Cubature Kalman Filter (CKF) appears, and a group of sampling point sets with the same weight are substituted into a nonlinear model for calculation, but the method omits a part of approximation errors, so that the filtering precision is reduced. In the patent CN113971752A, non-gaussian noise with outliers is simulated by using student t distribution to improve estimation accuracy, and a posterior probability density function is approximated to a student t probability density function with fixed degree of freedom parameters. However, in practice, thick tail noise also has non-stationary characteristics, and the estimation accuracy of kalman filtering is greatly reduced without considering the influence of the non-stationary characteristics.
Disclosure of Invention
The invention aims to solve the problem that the accuracy of data after final fusion is reduced because of the influence of noise non-stationarity in Kalman filtering in the prior art, and provides a mobile robot vision inertia fusion SLAM method based on variational Bayes.
In order to achieve the above object, according to a first aspect of the present invention, there is provided a mobile robot visual-inertial fusion SLAM method based on variational bayes, comprising: s1, obtaining a prediction state vector and a one-step prediction error covariance matrix of the mobile robot at the moment k based on the state vector and the covariance matrix of the mobile robot at the moment k-1, IMU data and a system noise covariance matrix of the mobile robot at the moment k; s2, establishing a model of observation noise at the moment k, which accords with the mixed distribution of student t based on gamma distribution, and obtaining likelihood function distribution in a hierarchical Gaussian form based on visual information, an observation noise model and a prediction state vector at the moment k of the mobile robot; and S3, constructing a combined posterior probability density function based on the likelihood function distribution, and performing combined solution on the posterior combined probability density function by using a variational Bayes method to obtain a state vector of the mobile robot at the time k.
To achieve the above objects, according to the same inventive concept, the present invention provides a computer-readable storage medium storing at least one instruction, at least one program, a code set, or a set of instructions, which is loaded and executed by a processor to implement the variational bayes based mobile robot visual inertia fusion SLAM method according to the first aspect of the present invention.
In order to achieve the above-mentioned object, according to the same inventive concept, the invention provides a mobile robot, which includes a mobile robot body, and a vision sensor, an IMU sensor and a processor that are arranged on the mobile robot body, wherein the processor is respectively connected with the vision sensor and the IMU sensor, and the processor executes the steps of the mobile robot vision inertia fusion SLAM method based on variational bayes according to the first aspect of the invention.
The technical scheme is as follows: a new distribution (namely, student t mixed distribution based on gamma distribution) is provided to model observation noise, the distribution takes the gamma distribution as a mixed density function, scale mixing is carried out on the student t distribution to cope with non-stationary running of noise, the influence of the non-stationary thick tail noise on the precision of a visual and inertial data fusion result is reduced, the estimation precision of Kalman filtering is further improved, information iteration updating based on variational Bayes is introduced in the fusion process of IMU data and visual sensor measurement data, and the variational Bayes is utilized to realize joint estimation of the state of the mobile robot, so that the precision of a finally obtained state vector is further improved.
Drawings
Fig. 1 is a schematic flowchart of a mobile robot visual inertia fusion SLAM method based on variational bayes in embodiment 1 of the present invention;
fig. 2 is a schematic diagram of a data fusion process in an application scenario in embodiment 1 of the present invention.
Detailed Description
Reference will now be made in detail to embodiments of the present invention, examples of which are illustrated in the accompanying drawings, wherein like or similar reference numerals refer to the same or similar elements or elements having the same or similar function throughout. The embodiments described below with reference to the accompanying drawings are illustrative only for the purpose of explaining the present invention, and are not to be construed as limiting the present invention.
In the description of the present invention, it is to be understood that the terms "longitudinal", "lateral", "upper", "lower", "front", "rear", "left", "right", "vertical", "horizontal", "top", "bottom", "inner", "outer", and the like, indicate orientations or positional relationships based on those shown in the drawings, and are used merely for convenience of description and for simplicity of description, and do not indicate or imply that the referenced devices or elements must have a particular orientation, be constructed in a particular orientation, and be operated, and thus, are not to be construed as limiting the present invention.
In the description of the present invention, unless otherwise specified and limited, it is to be noted that the terms "mounted," "connected," and "connected" are to be interpreted broadly, and may be, for example, a mechanical connection or an electrical connection, a communication between two elements, a direct connection, or an indirect connection via an intermediate medium, and specific meanings of the terms may be understood by those skilled in the art according to specific situations.
Example 1
The embodiment discloses a mobile robot visual inertia fusion SLAM method based on variational Bayes, which comprises the following steps as shown in figure 1:
step S1, based on the state vector x of the mobile robot at the moment k-1 k-1||k-1 Sum covariance matrix P k-1|k-1 And IMU data and system noise covariance matrix Q at time k of the mobile robot k Obtaining the predicted state vector x of the mobile robot at the moment k k|k-1 Sum step prediction error covariance matrix P k|k-1
The IMU data is provided by IMU sensors including accelerometers to provide accelerations of the mobile robot during movement and gyroscopes to provide angular velocity values of the mobile robot during movement. After obtaining IMU data, performing pre-integration processing on the IMU data, wherein the pre-integration processing comprises removing noise from an acceleration value measured by an accelerometer, performing primary integration to obtain speed, and performing secondary integration to obtain relative displacement; and the pre-integration processing also comprises the step of carrying out primary integration after removing noise on the angular velocity value measured by the gyroscope so as to obtain the angular change of the mobile robot in the motion process.
In step S1, a predicted state vector x at the time k of the mobile robot is obtained using IMU data at the time k of the mobile robot and the constructed volume points k|k-1 The method specifically comprises the following steps: x k|k-1 =F k-1 X k-1||k-1 Wherein F is k-1 Representing the state vector transformation relationship of the mobile robot at time k-1 and k obtained based on IMU data, preferably F k-1 Obtaining by pre-integration of IMU data; x k-1||k-1 A state vector representing the mobile robot at the time k-1; x k|k-1 =[P bk ,V bk ,Q bk ],P bk And V bk Three-dimensional pose and speed information, Q, of the mobile robot, respectively bk Representing a quaternion from the IMU coordinate system to the world coordinate system.
In step S1, the predicted state vector X at time k of the mobile robot is used k|k-1 Sum system noise covariance matrix Q k Calculating a one-step prediction error covariance matrix at the moment k of the mobile robot, specifically:
Figure BDA0003847820880000051
wherein, P k-1|k-1 Covariance matrix, Q, representing the moment k-1 of the mobile robot k Representing the covariance matrix of the system noise at time k of the mobile robot. The prior probability density function is then: p (X) k |Y k-1 )=N(X k ;X k|k-1 P k|k-1 )。
And S2, establishing a model of observation noise at the moment k, which accords with the mixed distribution of the student t based on gamma distribution, and obtaining likelihood function distribution in a hierarchical Gaussian form based on the visual information, the observation noise model and the prediction state vector of the mobile robot at the moment k.
Establishing a model which accords with the gamma distribution-based student t mixed distribution of observation noise at the k moment:
Figure BDA0003847820880000061
namely that
Figure BDA0003847820880000062
Wherein v is k Represents the observation noise at the moment k of the mobile robot,
Figure BDA0003847820880000063
denotes v k An initial value of (1); beta is a k Denotes the shape parameter, beta k Is a distribution function of
Figure BDA0003847820880000064
Wherein the content of the first and second substances,
Figure BDA0003847820880000065
representing a first constant number, y representing a second constant number, I m Representing an m-order identity matrix; r k Representing a scale matrix; u. u k Denotes a first degree of freedom, u k Has a distribution function of p (u) k )=G(u k ;a k ,b k ),a k 、b k Respectively belong to u k A first real number parameter and a second real number parameter of the gamma distribution; delta. For the preparation of a coating k Representing a second degree of freedom; lambda k Denotes a first auxiliary parameter, λ k Has a distribution function of p (λ) k )=G(λ k ;δ k /2,δ k /2);ξ k Representing a second auxiliary parameter, ξ k Has a distribution function of p (xi) k )=G(ξ k ;u k /2,u k 2); n represents a Gaussian distribution; g denotes a gamma distribution, and GaSTM denotes a student t-mix distribution based on the gamma distribution.
Proposes a new distribution-based student t mixed distribution
Figure BDA0003847820880000066
The distribution having location parameters
Figure BDA0003847820880000067
Shape parameter beta k Dimension matrix R k First and second different parameters u of degree of freedom k And delta k The method takes gamma distribution as a mixed density function, and scales and mixes the student t step by step to deal with the non-stationary characteristic of noise.
In step S2, the step of obtaining the likelihood function distribution in a hierarchical gaussian form based on the visual information of the mobile robot at time k, the observation noise model, and the prediction state vector includes:
step S21, based on the vision information of the mobile robot at the time k, obtaining the vision state information Y of the mobile robot at the time k k And Y is k Satisfy Y k =H k X k|k-1 +v k (ii) a Wherein H k An observation matrix representing visual state information Y of the mobile robot at the time k k And the predicted state vector X k|k-1 The mapping relationship of (c); specifically, ORB (organized FAST and Rotated BRIEF) features of the visual information are extracted according to the visual information of the robot at the time k, and the visual state information Y is obtained according to the ORB features k The process of obtaining visual state information is prior art and is not described herein again. Visual state information Y k The method comprises the steps of including three-dimensional pose and speed information of the mobile robot;
step S22, establishing likelihood function distribution as follows:
p(Y k |X k|k-1 )=GaSTM(Y k ;H k X k|k-1 ,β k ,R k ,u k ,δ k );
the likelihood function satisfies GaSTM distribution, and the likelihood function distribution is converted into a hierarchical Gaussian form:
Figure BDA0003847820880000071
and S3, constructing a combined posterior probability density function based on the likelihood function distribution, and performing combined solution on the posterior combined probability density function by using a variational Bayes method to obtain a state vector of the mobile robot at the time k.
According to a prior probability density function p (X) k |Y k-1 )=N(X k ;X k|k-1 ,P k|k-1 ) Obtaining a combined posterior probability density function p (X) from the distribution of likelihood functions in a hierarchical Gaussian form k ,λ k ,ξ k ,u k ,β k ,Y k ):
Figure BDA0003847820880000072
Namely: p (X) k ,λ k ,ξ k ,u k ,β k ,Y k )≈q(X k )q(λ k )q(ξ k )q(u k )q(β k ); (1)
Wherein q (X) k ) State vector X representing time k of mobile robot k An approximate probability density function of; q (lambda) k ) Representing a first auxiliary parameter lambda k An approximate probability density function of; q (xi) k ) Representing a second auxiliary parameter ξ k An approximate probability density function of; q (u) k ) Representing a first degree of freedom u k An approximate probability density function of; q (. Beta.) of k ) Representing a shape parameter beta k An approximate probability density function of. Therefore, only q (X) needs to be solved k )、q(λ k )、q(ξ k )、q(u k )、q(β k ) The finally fused state vector X of the robot at the moment k can be obtained k
X when the posterior joint probability density function is jointly solved by using the variational Bayes method k 、λ k 、ξ k 、u k 、β k Satisfies the following conditions:
Figure BDA0003847820880000081
wherein, aggregate
Figure BDA0003847820880000082
Theta represents a set theta k Any one of the elements of (a); theta k The expression (-theta) is represented by the set theta k A subset of all elements except θ; c θ Represents a constant independent of the element θ; e [ ·]Indicating the expected value.
In step S3, the step of jointly solving the posterior joint probability density function by using the variational bayes method to obtain the fusion state vector of the mobile robot at the time k includes:
step S31, parameter initialization:
Figure BDA0003847820880000083
step S32, the combined posterior probability density function is iteratively updated by utilizing the variational Bayes method, and X is respectively solved after each iteration k 、λ k 、ξ k 、u k 、β k If the expected value of the time of the mobile robot reaches the iteration stop condition, outputting a state vector X of the mobile robot at the moment k obtained after the last iteration k Sum covariance matrix P k|k
Further preferably, in step S32, for the (i + 1) th iteration:
step S321, substituting equation 1 into equation 2, let θ = X k It is possible to obtain:
Figure BDA0003847820880000091
from the above formula, q can be deduced (i+1) (X k ) Obeying a gaussian distribution:
Figure BDA0003847820880000092
q (i+1) (X k ) Represents X k Approximate probability density function in the (i + 1) th iteration. Wherein the content of the first and second substances,
Figure BDA0003847820880000093
Figure BDA0003847820880000094
represents the state vector, X, of the mobile robot at time k in the (i + 1) th iteration k|k-1 A predicted state vector, P, representing the mobile robot at time k k|k-1 Representing the covariance matrix of the one-step prediction error at time k of the mobile robot,
Figure BDA0003847820880000095
representKalman gain of the i +1 th iteration, E (i)k ]Denotes λ k Expected value at i-th iteration, E (i)k ]Is represented by beta k Expected value at i-th iteration, E (i)k ]Is representative of xi k At the expected value of the i-th iteration,
Figure BDA0003847820880000096
representing the covariance matrix of the mobile robot at time k in the (i + 1) th iteration.
Step S322, substituting equation 1 into equation 2, let θ = λ k It is possible to obtain:
Figure BDA0003847820880000097
the above formula can analyze that q cannot be paired (i+1)k ) Direct analysis is carried out, approximate point processing is carried out, and the optimal solution can be obtained as follows:
Figure BDA0003847820880000098
wherein, the first and the second end of the pipe are connected with each other,
Figure BDA0003847820880000101
E (i)k ]is representative of xi k In the expected value of the ith iteration, tr { } represents the trace of the matrix; n is a radical of an alkyl radical 1 、n 2 、n 3 Respectively a third auxiliary parameter, a fourth auxiliary parameter and a fifth auxiliary parameter; e (i) [(Y k -H k X k )(Y k -H k X k ) T ]Is represented by (Y) k -H k X k )(Y k -H k X k ) T Expected value at the ith iteration;
Figure BDA0003847820880000102
to represent
Figure BDA0003847820880000103
Expected value at the ith iteration; m represents an identity matrix I m The order of (a).
Step S323, substituting formula 1 into formula 2, and making theta = xi k It is possible to obtain:
Figure BDA0003847820880000104
from the above formula, q can be deduced (i+1)k ) Obey the gamma distribution, hence ξ k The approximate probability density function in the (i + 1) th iteration is:
q (i+1)k )=G(ξ k ;A (i+1) ,B (i+1) );
wherein the content of the first and second substances,
Figure BDA0003847820880000105
A (i+1) represents the value of the sixth auxiliary parameter A in the (i + 1) th iteration, B (i+1) Represents the value of the seventh auxiliary parameter B in the (i + 1) th iteration, C (i+1) Denotes the value of the eighth auxiliary parameter C in the (i + 1) th iteration, E (i) [u k ]Represents u k Expected value at i-th iteration, E (i) [(Y k -H k X kk β k )(Y k -H k X kk β k ) T ]Is represented by (Y) k -H k X kk β k )(Y k -H k X kk β k ) T Expected value at the i-th iteration.
Step S324, substituting equation 1 into equation 2, and letting θ = u k It is possible to obtain:
Figure BDA0003847820880000111
from the above formula, q can be deduced (i+1) (u k ) Obeying the gamma distribution to obtain u k Approximate probability Density in the i +1 th iterationThe function is:
Figure BDA0003847820880000112
wherein, the first and the second end of the pipe are connected with each other,
Figure BDA0003847820880000113
a k (0) denotes a k Is set to the initial value of (a),
Figure BDA0003847820880000114
denotes a k Value in the (i + 1) th iteration, b k (0) Denotes b k Is set to the initial value of (a),
Figure BDA0003847820880000115
denotes b k Value in the (i + 1) th iteration.
Step S325, substituting formula 1 into formula 2, let θ = β k It is possible to obtain:
Figure BDA0003847820880000116
from the above formula, q can be deduced (i+1)k ) Obeying Gaussian distribution to obtain beta k The approximate probability density function in the (i + 1) th iteration is:
Figure BDA0003847820880000117
wherein the content of the first and second substances,
Figure BDA0003847820880000118
Figure BDA0003847820880000119
is represented by beta k The value in the (i + 1) th iteration,
Figure BDA00038478208800001110
denotes the eighthAuxiliary parameter K β The value in the (i + 1) th iteration,
Figure BDA0003847820880000121
denotes a ninth auxiliary parameter P β Value in the (i + 1) th iteration.
Step S326, obtaining expected values of each parameter in the (i + 1) th iteration, where the expected values may be used as input values of the next iteration or output values after the iteration stop condition is met, and each expected value is:
Figure BDA0003847820880000122
E (i+1)k ]=A (i+1) /B (i+1)
E (i+1) [u k ]=a (i+1) /b (i+1)
Figure BDA0003847820880000123
Figure BDA0003847820880000124
Figure BDA0003847820880000125
Figure BDA0003847820880000126
Figure BDA0003847820880000127
step S327, determining whether the condition is satisfied
Figure BDA0003847820880000128
If so, will
Figure BDA0003847820880000129
Fusion state vector X as a mobile robot at time k k Output is to
Figure BDA00038478208800001210
Covariance matrix P as a mobile robot at time k k|k And outputting epsilon to represent a preset iteration stop threshold, and if the output epsilon does not satisfy the preset iteration stop threshold, making i = i +1, and returning to execute the steps S321 to S327.
The embodiment provides a student t-hybrid Kalman filtering SLAM algorithm based on gamma distribution, aiming at the problem that the nonstationary characteristic of noise is not considered in Kalman filtering provided in the background art, and the influence on the reduction of the final fused data precision is reduced. Firstly, a new distribution-the student t mixed distribution based on the gamma distribution is provided
Figure BDA00038478208800001211
The distribution having a location parameter
Figure BDA00038478208800001212
Shape parameter beta k The scale matrix R k First and second different parameters u of degree of freedom k And delta k The method takes gamma distribution as a mixed density function, and scales and mixes the student t step by step to deal with the non-stationary characteristic of noise.
In this embodiment, the main process of the SLAM method can be divided into: first, using the state vector X at time k-1 k-1|k-1 And IMU data, for state vector X at time k k And one-step prediction error covariance matrix P k|k-1 And (6) performing prediction. Second, observation noise v is measured using a GaSTM pair k Modeling is carried out, and likelihood function distribution of a level Gaussian form is deduced by using two auxiliary parameters. Thirdly, a variational Bayes method is used for carrying out combined solution on the posterior combined probability density function, and the expected values of the state vector, the auxiliary parameter, the first degree of freedom and the shape parameter are obtained through sequential iteration, so that the accuracy of the measurement data fusion of the IMU and the vision sensor is ensured, and the improvement is achievedThe positioning accuracy of SLAM is high. The method introduces information iteration updating based on variational Bayes in the process of IMU and vision sensor measurement data fusion, utilizes GaSTM to model observation noise, is respectively used for reducing the influence of non-stationary thick tail noise on the precision of a fusion result, and utilizes variational Bayes to realize the joint estimation of the state of the mobile robot, thereby improving the precision of finally obtained state information.
In an application scenario of this embodiment, fig. 2 is a schematic diagram of a data fusion process of the application scenario, where firstly, prediction of information at a next time is completed according to IMU data and information at a current time, that is, information at a k time is predicted by using information at a k-1 time, then observed quantity is obtained by using visual sensor data, observation noise is modeled by using gasstm, and finally, solution of a final result is completed by using variational bayes.
Example 2
The present embodiment is based on the same inventive concept as embodiment 1, and discloses a computer-readable storage medium, in which at least one instruction, at least one program, code set, or instruction set is stored, and the at least one instruction, the at least one program, the code set, or the instruction set is loaded and executed by a processor to implement the variational bayes-based mobile robot visual inertial fusion SLAM method provided in embodiment 1.
Example 3
Based on the inventive concept of embodiment 1, this embodiment discloses a mobile robot, which includes a mobile robot body, and a vision sensor, an IMU sensor and a processor that are arranged on the mobile robot body, where the processor is connected with the vision device and the IMU sensor, respectively, and the processor executes the steps of the variational bayes-based mobile robot vision inertial fusion SLAM method provided in embodiment 1. The vision sensor is preferably, but not limited to, a camera.
In the description herein, references to the description of the term "one embodiment," "some embodiments," "an example," "a specific example," or "some examples," etc., mean that a particular feature, structure, material, or characteristic described in connection with the embodiment or example is included in at least one embodiment or example of the invention. In this specification, the schematic representations of the terms used above do not necessarily refer to the same embodiment or example. Furthermore, the particular features, structures, materials, or characteristics described may be combined in any suitable manner in any one or more embodiments or examples.
While embodiments of the present invention have been shown and described, it will be understood by those of ordinary skill in the art that: various changes, modifications, substitutions and alterations can be made to the embodiments without departing from the principles and spirit of the invention, the scope of which is defined by the claims and their equivalents.

Claims (10)

1. A mobile robot vision inertia fusion SLAM method based on variational Bayes is characterized by comprising the following steps:
s1, obtaining a predicted state vector and a one-step prediction error covariance matrix of the mobile robot at the moment k based on a state vector and a covariance matrix of the mobile robot at the moment k-1, IMU data and a system noise covariance matrix of the mobile robot at the moment k;
s2, establishing a model of observation noise at the moment k, which accords with the mixed distribution of student t based on gamma distribution, and obtaining likelihood function distribution in a hierarchical Gaussian form based on visual information, an observation noise model and a prediction state vector at the moment k of the mobile robot;
and S3, constructing a combined posterior probability density function based on the likelihood function distribution, and performing combined solution on the posterior combined probability density function by using a variational Bayes method to obtain a state vector of the mobile robot at the time k.
2. The variational bayes-based visual-inertial fusion SLAM method for a mobile robot according to claim 1, characterized in that in step S1, the predicted state vector of the mobile robot at time k is:
X k|k-1 =F k-1 X k-1||k-1 wherein F is k-1 Indicating the state vector conversion relationship of the mobile robot at the time k-1 and the time k obtained based on IMU data, X k-1||k-1 A state vector representing the moment k-1 of the mobile robot;
the covariance matrix of the one-step prediction error at the moment k of the mobile robot is as follows:
Figure FDA0003847820870000011
wherein, P k-1|k-1 Covariance matrix, Q, representing the moment k-1 of the mobile robot k Representing the covariance matrix of the system noise at time k of the mobile robot.
3. The variational bayes-based visual-inertial-fusion SLAM method of a mobile robot according to claim 1 or 2, characterized in that in said step S2, the observed noise model at time k of the mobile robot is:
Figure FDA0003847820870000021
wherein v is k Represents the observation noise at the time of the mobile robot k,
Figure FDA0003847820870000022
denotes v k An initial value of (1); beta is a beta k Denotes the shape parameter, beta k Is a distribution function of
Figure FDA0003847820870000023
Wherein the content of the first and second substances,
Figure FDA0003847820870000024
representing a first constant number, y representing a second constant number, I m Representing an m-order identity matrix; r is k Representing a scale matrix; u. of k Denotes a first degree of freedom, u k Has a distribution function of p (u) k )=G(u k ;a k ,b k ),a k 、b k Respectively belong to u k A first real parameter and a second real parameter of the gamma distribution of (1); delta. For the preparation of a coating k Representing a second degree of freedom;λ k Denotes a first auxiliary parameter, λ k Has a distribution function of p (λ) k )=G(λ k ;δ k /2,δ k /2);ξ k Representing a second auxiliary parameter, ξ k Has a distribution function of p (xi) k )=G(ξ k ;u k /2,u k 2) and (c); n represents a Gaussian distribution; g denotes a gamma distribution, and GaSTM denotes a student t-mix distribution based on the gamma distribution.
4. The variational bayes-based mobile robot visual-inertial fusion SLAM method of claim 3, wherein in said step S2, the step of obtaining likelihood function distribution in a hierarchical gaussian form based on visual information of the mobile robot at time k, observation noise model and predicted state vector comprises:
acquiring visual state information Y of mobile robot at moment k based on visual information of mobile robot at moment k k And Y is k Satisfy Y k =H k X k|k-1 +v k (ii) a Wherein H k An observation matrix representing visual state information Y of the mobile robot at the time k k And the predicted state vector X k|k-1 The mapping relationship of (2);
establishing likelihood function distribution as follows:
p(Y k |X k|k-1 )=GaSTM(Y k ;H k X k|k-1 ,β k ,R k ,u k ,δ k );
converting the likelihood function distribution into a hierarchical gaussian form as:
Figure FDA0003847820870000031
5. the variational bayes-based mobile robot visual-inertial fusion SLAM method according to claim 4, characterized in that in said step S3, the joint posterior probability density function is:
p(X k ,λ k ,ξ k ,u k ,β k ,Y k )≈q(X k )q(λ k )q(ξ k )q(u k )g(β k );
wherein q (X) k ) State vector X representing time k of mobile robot k An approximate probability density function of; q (lambda) k ) Representing a first auxiliary parameter lambda k An approximate probability density function of; q (xi) k ) Representing a second auxiliary parameter xi k An approximate probability density function of; q (u) k ) Representing a first degree of freedom u k An approximate probability density function of; q (. Beta.) of k ) Representing a shape parameter beta k Approximate probability density function of.
6. The method of claim 5, wherein in step S3, the variational Bayes method is used to jointly solve the posterior joint probability density function by X k 、λ k 、ξ k 、u k 、β k Satisfies the following optimal solution:
Figure FDA0003847820870000032
wherein, aggregate
Figure FDA0003847820870000033
Theta represents the set theta k Any one of the elements of (a); theta k (- [ theta ]) represents a data structure represented by the set theta k A subset of all elements except θ; c θ Represents a constant independent of the element θ; e [. C]Indicating the expected value.
7. The visual-inertial fusion SLAM method of the mobile robot based on variational Bayes as claimed in claim 5 or 6 wherein, in the step S3, the step of jointly solving the posterior joint probability density function by using the variational Bayes method to obtain the fusion state vector of the mobile robot at the k-time comprises:
step S31, parameter initialization:
E (0)k ]=1,E (0)k ]=1,E (0) [u k ]=a k /b k
Figure FDA0003847820870000041
step S32, the combined posterior probability density function is iteratively updated by utilizing the variational Bayes method, and X is respectively solved after each iteration k 、λ k 、ξ k 、u k 、β k If the expected value reaches the iteration stop condition, outputting the state vector X of the mobile robot at the moment k obtained after the last iteration k Sum covariance matrix P k|k
8. The variational bayes-based mobile robot visual-inertial fusion SLAM method of claim 7, wherein in said step S32, for the (i + 1) th iteration: step S321, solving X based on variational Bayes method k The approximate probability density function in the (i + 1) th iteration is:
Figure FDA0003847820870000042
wherein, the first and the second end of the pipe are connected with each other,
Figure FDA0003847820870000043
Figure FDA0003847820870000044
represents the state vector, X, of the mobile robot at time k in the i +1 th iteration k|k-1 A predicted state vector, P, representing the mobile robot at time k k|k-1 Representing the one-step prediction error covariance matrix at time k of the mobile robot,
Figure FDA0003847820870000045
representing the i +1 th iterationKalman gain, E (i)k ]Denotes λ k Expected value at i-th iteration, E (i)k ]Is expressed by beta k Expected value at i-th iteration, E (i)k ]Is representative of xi k At the expected value of the i-th iteration,
Figure FDA0003847820870000046
representing a covariance matrix of the mobile robot at the k moment in the (i + 1) th iteration;
step S322, based on variational Bayes method, calculating lambda k The optimal solution in the (i + 1) th iteration is:
Figure FDA0003847820870000051
wherein, the first and the second end of the pipe are connected with each other,
Figure FDA0003847820870000052
E (i)k ]is representative of xi k At the expected value of the ith iteration, tr { } represents the trace of the solved matrix; n is 1 、n 2 、n 3 Respectively a third auxiliary parameter, a fourth auxiliary parameter and a fifth auxiliary parameter; e (i) [(Y k -H k X k )(Y k -H k X k ) T ]Is represented by (Y) k -H k X k )(Y k -H k X k ) T Expected value at the ith iteration;
Figure FDA0003847820870000053
to represent
Figure FDA0003847820870000054
Expected value at the ith iteration; m represents an identity matrix I m The order of (a);
step S323, based on variational Bayes method, solving xi k The approximate probability density function in the (i + 1) th iteration is:
q (i+1)k )=G(ξ k ;A (i+1) ,B (i+1) );
wherein the content of the first and second substances,
Figure FDA0003847820870000055
A (i+1) represents the value of the sixth auxiliary parameter A in the (i + 1) th iteration, B (i+1) Represents the value of the seventh auxiliary parameter B in the (i + 1) th iteration, C (i+1) Representing the value of the eighth auxiliary parameter C in the (i + 1) th iteration, E (i) [u k ]Represents u k Expected value at i-th iteration, E (i) [(Y k -H k X kk β k )(Y k -H k X kk β k ) T ]Is shown (Y) k -H k X kk β k )(Y k -H k X kk β k ) T Expected value at the ith iteration;
step S324, based on variational Bayes method, u is obtained k The approximate probability density function in the (i + 1) th iteration is:
Figure FDA0003847820870000056
wherein the content of the first and second substances,
Figure FDA0003847820870000061
a k (0) denotes a k Is set to the initial value of (a),
Figure FDA0003847820870000062
denotes a k Value in the (i + 1) th iteration, b k (0) Denotes b k Of the initial value of (a) is,
Figure FDA0003847820870000063
denotes b k The value in the (i + 1) th iteration;
step S325, based on variational BayesMethod for obtaining beta k The approximate probability density function in the (i + 1) th iteration is:
Figure FDA0003847820870000064
wherein the content of the first and second substances,
Figure FDA0003847820870000065
Figure FDA0003847820870000066
is expressed by beta k The value in the (i + 1) th iteration,
Figure FDA0003847820870000067
denotes the eighth auxiliary parameter K β The value in the (i + 1) th iteration,
Figure FDA0003847820870000068
denotes a ninth auxiliary parameter P β The value in the (i + 1) th iteration;
step S326, obtaining an expected value:
Figure FDA0003847820870000069
E (i+1)k ]=A (i+1) /B (i+1)
E (i+1) [u k ]=a (i+1) /b (i+1)
Figure FDA00038478208700000610
Figure FDA00038478208700000611
Figure FDA00038478208700000612
Figure FDA00038478208700000613
Figure FDA00038478208700000614
step S327, determining whether the requirements are satisfied
Figure FDA0003847820870000071
If so, will
Figure FDA0003847820870000072
Fusion state vector X as a mobile robot at time k k And output is to
Figure FDA0003847820870000073
Covariance matrix P as a mobile robot at time k k|k And outputting epsilon representing an iteration stop threshold, and if the iteration stop threshold is not met, making i = i +1, and returning to execute the steps S321 to S327.
9. A computer readable storage medium having stored therein at least one instruction, at least one program, a set of codes, or a set of instructions, which is loaded and executed by a processor to implement a variational bayes based mobile robotic visual inertial fusion SLAM method of any of claims 1-8.
10. A mobile robot, comprising a mobile robot body, and a vision sensor, an IMU sensor and a processor which are arranged on the mobile robot body, wherein the processor is respectively connected with the vision device and the IMU sensor, and the processor executes the steps of the mobile robot vision inertia fusion SLAM method based on variational Bayes according to one of claims 1 to 8.
CN202211125734.3A 2022-09-15 2022-09-15 Mobile robot vision inertia fusion SLAM method based on variational Bayes Pending CN115388899A (en)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202211125734.3A CN115388899A (en) 2022-09-15 2022-09-15 Mobile robot vision inertia fusion SLAM method based on variational Bayes

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202211125734.3A CN115388899A (en) 2022-09-15 2022-09-15 Mobile robot vision inertia fusion SLAM method based on variational Bayes

Publications (1)

Publication Number Publication Date
CN115388899A true CN115388899A (en) 2022-11-25

Family

ID=84127279

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202211125734.3A Pending CN115388899A (en) 2022-09-15 2022-09-15 Mobile robot vision inertia fusion SLAM method based on variational Bayes

Country Status (1)

Country Link
CN (1) CN115388899A (en)

Cited By (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN115930971A (en) * 2023-02-01 2023-04-07 七腾机器人有限公司 Data fusion processing method for robot positioning and mapping

Cited By (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN115930971A (en) * 2023-02-01 2023-04-07 七腾机器人有限公司 Data fusion processing method for robot positioning and mapping
CN115930971B (en) * 2023-02-01 2023-09-19 七腾机器人有限公司 Data fusion processing method for robot positioning and map building

Similar Documents

Publication Publication Date Title
CN112347840B (en) Vision sensor laser radar integrated unmanned aerial vehicle positioning and image building device and method
CN111136660B (en) Robot pose positioning method and system
CN110018691B (en) Flight state estimation system and method for small multi-rotor unmanned aerial vehicle
CN111949929B (en) Design method of multi-sensor fusion quadruped robot motion odometer
CN114459470A (en) Inspection robot positioning method based on multi-sensor fusion
CN113819917A (en) Automatic driving path planning method, device, equipment and storage medium
CN115388899A (en) Mobile robot vision inertia fusion SLAM method based on variational Bayes
CN116067370A (en) IMU gesture resolving method, IMU gesture resolving equipment and storage medium
CN115143954A (en) Unmanned vehicle navigation method based on multi-source information fusion
CN113340324B (en) Visual inertia self-calibration method based on depth certainty strategy gradient
CN113483755A (en) Multi-sensor combined positioning method and system based on non-global consistent map
CN114046800B (en) High-precision mileage estimation method based on double-layer filtering frame
CN108710295B (en) Robot following method based on progressive volume information filtering
CN113778082B (en) Unmanned vehicle track tracking control method and system based on self-triggering mechanism
CN113984045B (en) Method and system for estimating motion state of movable butt-joint target of underwater robot
Emter et al. Stochastic cloning for robust fusion of multiple relative and absolute measurements
CN111812668B (en) Winding inspection device, positioning method thereof and storage medium
CN114777762A (en) Inertial navigation method based on Bayesian NAS
CN115930971B (en) Data fusion processing method for robot positioning and map building
CN114018250B (en) Inertial navigation method, electronic device, storage medium and computer program product
CN113758491B (en) Relative positioning method and system based on multi-sensor fusion unmanned vehicle and vehicle
WO2024037295A1 (en) Positioning
CN114056338B (en) Multi-sensor fusion vehicle state parameter prediction method
WO2024114593A1 (en) Multi-line laser positioning method and positioning apparatus, computer device, and storage medium
Zhou et al. A robot state estimator based on multi-sensor information fusion

Legal Events

Date Code Title Description
PB01 Publication
PB01 Publication
SE01 Entry into force of request for substantive examination
SE01 Entry into force of request for substantive examination