CN115388899A - 基于变分贝叶斯的移动机器人视觉惯性融合slam方法 - Google Patents
基于变分贝叶斯的移动机器人视觉惯性融合slam方法 Download PDFInfo
- 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
Links
- 238000000034 method Methods 0.000 title claims abstract description 52
- 230000004927 fusion Effects 0.000 title claims abstract description 28
- 238000009826 distribution Methods 0.000 claims abstract description 58
- 239000011159 matrix material Substances 0.000 claims abstract description 39
- 230000000007 visual effect Effects 0.000 claims abstract description 24
- 238000005315 distribution function Methods 0.000 claims description 8
- 239000000126 substance Substances 0.000 claims description 8
- 238000003860 storage Methods 0.000 claims description 3
- 239000011248 coating agent Substances 0.000 claims description 2
- 238000000576 coating method Methods 0.000 claims description 2
- 238000013507 mapping Methods 0.000 claims description 2
- 238000002360 preparation method Methods 0.000 claims description 2
- 238000006243 chemical reaction Methods 0.000 claims 1
- 238000001914 filtration Methods 0.000 description 8
- 230000010354 integration Effects 0.000 description 7
- 230000008569 process Effects 0.000 description 7
- 238000004364 calculation method Methods 0.000 description 4
- 238000012545 processing Methods 0.000 description 4
- 238000007499 fusion processing Methods 0.000 description 3
- 238000005259 measurement Methods 0.000 description 3
- 238000005070 sampling Methods 0.000 description 3
- 230000001133 acceleration Effects 0.000 description 2
- 238000010586 diagram Methods 0.000 description 2
- 239000000463 material Substances 0.000 description 2
- 230000004075 alteration Effects 0.000 description 1
- 230000008859 change Effects 0.000 description 1
- 238000004891 communication Methods 0.000 description 1
- 238000006073 displacement reaction Methods 0.000 description 1
- 230000006872 improvement Effects 0.000 description 1
- 238000002156 mixing Methods 0.000 description 1
- 238000012986 modification Methods 0.000 description 1
- 230000004048 modification Effects 0.000 description 1
- 238000005457 optimization Methods 0.000 description 1
- 230000009467 reduction Effects 0.000 description 1
- 238000006467 substitution reaction Methods 0.000 description 1
- 230000001360 synchronised effect Effects 0.000 description 1
- 230000009466 transformation Effects 0.000 description 1
Images
Classifications
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
- G01C21/20—Instruments for performing navigational calculations
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
- G01C21/10—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration
- G01C21/12—Navigation; 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/16—Navigation; 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/165—Navigation; 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/1656—Navigation; 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
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
- G01C21/10—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration
- G01C21/12—Navigation; 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/16—Navigation; 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/18—Stabilised 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
本发明提供了基于变分贝叶斯的移动机器人视觉惯性融合SLAM方法,包括:S1,获得移动机器人k时刻的预测状态向量和协方差矩阵;S2,建立k时刻观测噪声的符合基于伽马分布的学生t混合分布的模型,基于移动机器人k时刻的视觉信息、观测噪声模型和预测状态向量获得层次高斯形式的似然函数分布;S3,基于似然函数分布构建联合后验概率密度函数,利用变分贝叶斯方法对后验联合概率密度函数进行联合求解获得移动机器人k时刻的状态向量。提出一种新的分布对观测噪声进行建模,以降低非平稳厚尾噪声对视觉和惯性数据融合结果精度的影响,并且利用变分贝叶斯实现对移动机器人状态的联合估计,提高最终输出的状态向量精度。
Description
技术领域
本发明涉及智能机器人技术领域,尤其涉及一种基于变分贝叶斯的移动机器人视觉惯性融合SLAM方法。
背景技术
同步定位和地图构建(Simultaneous Localization And Mapping,SLAM)是指移动机器人在未知环境中运行的同时将其中的有用信息提取并组合,确定自身位姿并逐步建立出周围环境地图的过程。SLAM目前已经在水下无人航行器、无人驾驶、空中无人机等方面得到了成功应用,受到的关注越来越多。
基于贝叶斯估计的滤波广泛应用于SLAM的后端优化,依据移动机器人上一时刻的状态向量与当前时刻的估计值求取当前时刻状态向量。最早出现的是基于扩展卡尔曼滤波(extended Kalman filter,EKF)的SLAM算法,它在利用EKF估计机器人位姿和地图特征位置之前需要利用一阶泰勒展开式对SLAM算法中的运动模型和观测模型进行线性化,但是对两个模型的线性化过程都会引入相应的误差,此外雅可比矩阵的计算过程也会给算法带来极大的负担。之后出现了无迹卡尔曼滤波(unscented Kalman filter,UKF),专利CN109141436A便使用了该方法,将具有一定权值的采样点集代入到SLAM算法的运动模型和观测模型中,在对移动机器人的状态信息和观测信息的计算中避免了线性化。然而当***维度逐渐增加时,在UKF中将会出现非局部取样的现象,直接导致SLAM的计算结果不稳定甚至出现发散。紧接着出现了基于容积卡尔曼滤波(cubature Kalman filter,CKF)的SLAM算法,将一组具有相同权值的采样点集代入非线性模型进行计算,但是该方法舍去了部分近似化误差,造成滤波精度降低。上述方法均未考虑厚尾噪声的影响,针对这一问题,专利CN113971752A中利用学生t分布来模拟具有野值的非高斯噪声以提高估计精度,将后验概率密度函数近似为具有固定自由度参数的学生t概率密度函数。但在实际中,厚尾噪声也有着非平稳的特性,不考虑非平稳特性的影响会极大降低卡尔曼滤波的估计精度。
发明内容
本发明旨在解决现有技术中卡尔曼滤波不考虑噪声的非平稳性影响最终融合后的数据精度降低的问题,提供一种基于变分贝叶斯的移动机器人视觉惯性融合SLAM方法。
为了实现本发明的上述目的,根据本发明的第一个方面,本发明提供了基于变分贝叶斯的移动机器人视觉惯性融合SLAM方法,其特征在于,包括:步骤S1,基于移动机器人k-1时刻的状态向量和协方差矩阵,以及移动机器人k时刻的IMU数据和***噪声协方差矩阵,获得移动机器人k时刻的预测状态向量和一步预测误差协方差矩阵;步骤S2,建立k时刻观测噪声的符合基于伽马分布的学生t混合分布的模型,基于移动机器人k时刻的视觉信息、观测噪声模型和预测状态向量获得层次高斯形式的似然函数分布;步骤S3,基于似然函数分布构建联合后验概率密度函数,利用变分贝叶斯方法对后验联合概率密度函数进行联合求解获得移动机器人k时刻的状态向量。
为了实现本发明的上述目的,根据同样的发明构思,本发明提供了一种计算机可读存储介质,所述存储介质中存储有至少一条指令、至少一段程序、代码集或指令集,所述至少一条指令、所述至少一段程序、所述代码集或指令集由处理器加载并执行以实现如本发明第一方面所述的基于变分贝叶斯的移动机器人视觉惯性融合SLAM方法。
为了实现本发明的上述目的,根据同样的发明构思,本发明提供了一种移动机器人,包括移动机器人本体和设于所述移动机器人本体上的视觉传感器、IMU传感器和处理器,所述处理器分别与视觉设别和IMU传感器连接,所述处理器执行本发明第一方面所述的基于变分贝叶斯的移动机器人视觉惯性融合SLAM方法的步骤。
上述技术方案:提出一种新的分布(即基于伽马分布的学生t混合分布)来对观测噪声进行建模,该分布以伽马分布为混合密度函数,对学生t分布进行尺度混合,以应对噪声的非平稳行,降低非平稳厚尾噪声对视觉和惯性数据融合结果精度的影响,进而提高卡尔曼滤波的估计精度,并且在IMU数据和视觉传感器测量数据融合过程中引入了基于变分贝叶斯的信息迭代更新,利用变分贝叶斯实现对移动机器人状态的联合估计,从而进一步提高最终求得的状态向量的精度。
附图说明
图1是本发明实施例1中基于变分贝叶斯的移动机器人视觉惯性融合SLAM方法的流程示意图;
图2是本发明实施例1在一种应用场景中的数据融合流程示意图。
具体实施方式
下面详细描述本发明的实施例,所述实施例的示例在附图中示出,其中自始至终相同或类似的标号表示相同或类似的元件或具有相同或类似功能的元件。下面通过参考附图描述的实施例是示例性的,仅用于解释本发明,而不能理解为对本发明的限制。
在本发明的描述中,需要理解的是,术语“纵向”、“横向”、“上”、“下”、“前”、“后”、“左”、“右”、“竖直”、“水平”、“顶”、“底”“内”、“外”等指示的方位或位置关系为基于附图所示的方位或位置关系,仅是为了便于描述本发明和简化描述,而不是指示或暗示所指的装置或元件必须具有特定的方位、以特定的方位构造和操作,因此不能理解为对本发明的限制。
在本发明的描述中,除非另有规定和限定,需要说明的是,术语“安装”、“相连”、“连接”应做广义理解,例如,可以是机械连接或电连接,也可以是两个元件内部的连通,可以是直接相连,也可以通过中间媒介间接相连,对于本领域的普通技术人员而言,可以根据具体情况理解上述术语的具体含义。
实施例1
本实施例公开了一种基于变分贝叶斯的移动机器人视觉惯性融合SLAM方法,如图1所示,包括:
步骤S1,基于移动机器人k-1时刻的状态向量xk-1||k-1和协方差矩阵Pk-1|k-1,以及移动机器人k时刻的IMU数据和***噪声协方差矩阵Qk,获得移动机器人k时刻的预测状态向量xk|k-1和一步预测误差协方差矩阵Pk|k-1。
IMU数据由IMU传感器提供,IMU传感器包括加速度计和陀螺仪,加速度计提供移动机器人在运动过程中的加速度,陀螺仪提供移动机器人在运动过程中的角速度值。获得IMU数据后,对IMU数据进行预积分处理,预积分处理包括对加速度计测得的加速度值去除噪声后进行一次积分可得到速度,二次积分可得到相对位移;预积分处理还包括对陀螺仪测得的角速度值去除噪声后进行一次积分可得到移动机器人运动过程中的角度变化。
在步骤S1中,利用移动机器人k时刻的IMU数据和构建好的容积点获得移动机器人k时刻的预测状态向量xk|k-1,具体为:Xk|k-1=Fk-1Xk-1||k-1,其中,Fk-1表示基于IMU数据获得的k-1时刻和k时刻的移动机器人的状态向量转换关系,优选地,Fk-1通过IMU数据的预积分获得;Xk-1||k-1表示移动机器人k-1时刻的状态向量;Xk|k-1=[Pbk,Vbk,Qbk],Pbk和Vbk分别是移动机器人的三维位姿和速度信息,Qbk表示从IMU坐标系到世界坐标系的四元数。
在步骤S1中,利用移动机器人k时刻的预测状态向量Xk|k-1和***噪声协方差矩阵Qk对移动机器人k时刻的一步预测误差协方差矩阵进行计算,具体为:
步骤S2,建立k时刻观测噪声的符合基于伽马分布的学生t混合分布的模型,基于移动机器人k时刻的视觉信息、观测噪声模型和预测状态向量获得层次高斯形式的似然函数分布。
建立k时刻观测噪声的符合基于伽马分布的学生t混合分布的模型:
其中,vk表示移动机器人k时刻的观测噪声,表示vk的初始值;βk表示形状参数,βk的分布函数为其中,表示第一常实数,y表示第二常实数,Im表示m阶单位矩阵;Rk表示尺度矩阵;uk表示第一自由度,uk的分布函数为p(uk)=G(uk;ak,bk),ak、bk分别是属于uk的伽马分布的第一实数参数和第二实数参数;δk表示第二自由度;λk表示第一辅助参数,λk的分布函数为p(λk)=G(λk;δk/2,δk/2);ξk表示第二辅助参数,ξk的分布函数为p(ξk)=G(ξk;uk/2,uk/2);N表示高斯分布;G表示伽马分布,GaSTM表示基于伽马分布的学生t混合分布。
提出一种新的分布一一基于伽马分布的学生t混合分布该分布具有位置参数形状参数βk、尺度矩阵Rk、第一和第二两个不同的自由度参数uk和δk,它以伽马分布为混合密度函数,对学生t分步进行尺度混合,以应对噪声的非平稳特性。
在步骤S2中,基于移动机器人k时刻的视觉信息、观测噪声模型和预测状态向量获得层次高斯形式的似然函数分布的步骤包括:
步骤S21,基于移动机器人k时刻的视觉信息获取移动机器人k时刻的视觉状态信息Yk,且Yk满足Yk=HkXk|k-1+vk;其中,Hk表示观测矩阵,表示移动机器人k时刻的视觉状态信息Yk与预测状态向量Xk|k-1的映射关系;具体的,根据机器人k时刻的视觉信息提取出视觉信息的ORB(Oriented FAST and Rotated BRIEF)特征,根据ORB特征获取视觉状态信息Yk,获取视觉状态信息的过程为现有技术,在此不再赘述。视觉状态信息Yk包括移动机器人的三维位姿和速度信息;
步骤S22,建立似然函数分布为:
p(Yk|Xk|k-1)=GaSTM(Yk;HkXk|k-1,βk,Rk,uk,δk);
似然函数满足GaSTM分布,将似然函数分布转换为层次高斯形式为:
步骤S3,基于似然函数分布构建联合后验概率密度函数,利用变分贝叶斯方法对后验联合概率密度函数进行联合求解获得移动机器人k时刻的状态向量。
根据先验概率密度函数p(Xk|Yk-1)=N(Xk;Xk|k-1,Pk|k-1)和层次高斯形式的似然函数分布获得联合后验概率密度函数p(Xk,λk,ξk,uk,βk,Yk):
即:p(Xk,λk,ξk,uk,βk,Yk)≈q(Xk)q(λk)q(ξk)q(uk)q(βk); (1)
其中,q(Xk)表示移动机器人k时刻的状态向量Xk的近似概率密度函数;q(λk)表示第一辅助参数λk的近似概率密度函数;q(ξk)表示第二辅助参数ξk的近似概率密度函数;q(uk)表示第一自由度uk的近似概率密度函数;q(βk)表示形状参数βk的近似概率密度函数。因此,只需求解出q(Xk)、q(λk)、q(ξk)、q(uk)、q(βk)就能获得最终融合出的机器人k时刻的状态向量Xk。
利用变分贝叶斯方法对后验联合概率密度函数进行联合求解时Xk、λk、ξk、uk、βk的最优解满足:
在步骤S3中,利用变分贝叶斯方法对后验联合概率密度函数进行联合求解获得k时刻移动机器人的融合状态向量的步骤包括:
步骤S31,参数初始化:
步骤S32,利用变分贝叶斯方法迭代更新联合后验概率密度函数,每次迭代后分别求取Xk、λk、ξk、uk、βk的期望值,若达到迭代停止条件,输出最后一次迭代后获得的移动机器人k时刻的状态向量Xk和协方差矩阵Pk|k。
进一步优选地,在步骤S32中,对于第i+1次迭代,执行:
步骤S321,将公式1代入公式2,令θ=Xk,可以得到:
由上式可以推断出q(i+1)(Xk)服从高斯分布:q(i+1)(Xk)表示Xk在第i+1次迭代中的近似概率密度函数。其中, 表示第i+1次迭代中k时刻移动机器人的状态向量,Xk|k-1表示移动机器人k时刻的预测状态向量,Pk|k-1表示移动机器人k时刻的一步预测误差协方差矩阵,表示第i+1次迭代的卡尔曼增益,E(i)[λk]表示λk在第i次迭代的期望值,E(i)[βk]表示βk在第i次迭代的期望值,E(i)[ξk]表示ξk在第i次迭代的期望值,表示第i+1次迭代中k时刻移动机器人的协方差矩阵。
步骤S322,将公式1代入公式2,令θ=λk,可以得到:
由上式可以分析出无法对q(i+1)(λk)进行直接解析,进行近似点处理,可以得到最优解为:
其中,E(i)[ξk]表示ξk在第i次迭代的期望值,tr{}表示求取矩阵的迹;n1、n2、n3分别为第三辅助参数、第四辅助参数、第五辅助参数;E(i)[(Yk-HkXk)(Yk-HkXk)T]表示(Yk-HkXk)(Yk-HkXk)T在第i次迭代的期望值;表示在第i次迭代的期望值;m表示单位矩阵Im的阶数。
步骤S323,公式1代入公式2,令θ=ξk,可以得到:
由上式可以推断出q(i+1)(ξk)服从伽马分布,因此ξk在第i+1次迭代中的近似概率密度函数为:
q(i+1)(ξk)=G(ξk;A(i+1),B(i+1));
其中,A(i+1)表示第六辅助参数A在第i+1次迭代中的值,B(i+1)表示第七辅助参数B在第i+1次迭代中的值,C(i+1)表示第八辅助参数C在第i+1次迭代中的值,E(i)[uk]表示uk在第i次迭代的期望值,E(i)[(Yk-HkXk-λkβk)(Yk-HkXk-λkβk)T]表示(Yk-HkXk-λkβk)(Yk-HkXk-λkβk)T在第i次迭代的期望值。
步骤S324,公式1代入公式2,令θ=uk,可以得到:
由上式可以推断出q(i+1)(uk)服从伽马分布,获得uk在第i+1次迭代中的近似概率密度函数为:
步骤S325,公式1代入公式2,令θ=βk,可以得到:
由上式可以推断出q(i+1)(βk)服从高斯分布,获得βk在第i+1次迭代中的近似概率密度函数为:
步骤S326,求取第i+1次迭代中各个参数的期望值,这些期望值可作为下一次迭代的输入值或者满足迭代停止条件后的输出值,各期望值:
E(i+1)[ξk]=A(i+1)/B(i+1);
E(i+1)[uk]=a(i+1)/b(i+1);
步骤S327,判断是否满足若满足,将作为k时刻移动机器人的融合状态向量Xk输出,将作为k时刻移动机器人的协方差矩阵Pk|k输出,ε表示预先设置的迭代停止阈值,若不满足,令i=i+1,返回执行步骤S321到步骤S327。
本实施例针对背景技术中提出的卡尔曼滤波不考虑噪声的非平稳特性,影响最终融合后的数据精度降低这一问题,提出一种基于伽马分布的学生t混合卡尔曼滤波SLAM算法。首先提出一种新的分布-基于伽马分布的学生t混合分布该分布具有位置参数形状参数βk、尺度矩阵Rk、第一和第二两个不同的自由度参数uk和δk,它以伽马分布为混合密度函数,对学生t分步进行尺度混合,以应对噪声的非平稳特性。
在本实施例中,本SLAM方法主要过程可分为:第一,利用k-1时刻的状态向量Xk-1|k-1和IMU数据,对k时刻的状态向量Xk和一步预测误差协方差矩阵Pk|k-1进行预测。第二,利用GaSTM对观测噪声vk进行建模,并利用两个辅助参数推导出层次高斯形式的似然函数分布。第三,利用变分贝叶斯方法对后验联合概率密度函数进行联合求解,依次迭代得到状态向量、辅助参数、第一自由度和形状参数的期望值,保证了IMU和视觉传感器测量数据融合的准确性,提高SLAM的定位精度。该方法在IMU和视觉传感器测量数据融合过程中引入了基于变分贝叶斯的信息迭代更新,利用GaSTM对观测噪声进行建模,分别用来降低非平稳厚尾噪声对融合结果精度的影响,并利用变分贝叶斯实现对移动机器人状态的联合估计,从而提高了最终求得的状态信息的精度。
在本实施例的一种应用场景中,图2所示为该应用场景的数据融合流程示意图,首先根据IMU数据和当前时刻信息完成下一时刻信息的预测,即利用k-1时刻的信息预测k时刻的信息,之后利用视觉传感器数据获得观测量,并用GaSTM对观测噪声进行建模,最终利用变分贝叶斯完成对最终结果的求取。
实施例2
本实施例与实施例1基于同样的发明构思,公开了一种计算机可读存储介质,存储介质中存储有至少一条指令、至少一段程序、代码集或指令集,至少一条指令、至少一段程序、代码集或指令集由处理器加载并执行以实现实施例1提供的基于变分贝叶斯的移动机器人视觉惯性融合SLAM方法。
实施例3
基于实施例1的发明构思,本实施例公开了一种移动机器人,包括移动机器人本体和设于所述移动机器人本体上的视觉传感器、IMU传感器和处理器,处理器分别与视觉设别和IMU传感器连接,所述处理器执行实施例1提供的基于变分贝叶斯的移动机器人视觉惯性融合SLAM方法的步骤。视觉传感器优选但不限于为摄像头。
在本说明书的描述中,参考术语“一个实施例”、“一些实施例”、“示例”、“具体示例”、或“一些示例”等的描述意指结合该实施例或示例描述的具体特征、结构、材料或者特点包含于本发明的至少一个实施例或示例中。在本说明书中,对上述术语的示意性表述不一定指的是相同的实施例或示例。而且,描述的具体特征、结构、材料或者特点可以在任何的一个或多个实施例或示例中以合适的方式结合。
尽管已经示出和描述了本发明的实施例,本领域的普通技术人员可以理解:在不脱离本发明的原理和宗旨的情况下可以对这些实施例进行多种变化、修改、替换和变型,本发明的范围由权利要求及其等同物限定。
Claims (10)
1.基于变分贝叶斯的移动机器人视觉惯性融合SLAM方法,其特征在于,包括:
步骤S1,基于移动机器人k-1时刻的状态向量和协方差矩阵,以及移动机器人k时刻的IMU数据和***噪声协方差矩阵,获得移动机器人k时刻的预测状态向量和一步预测误差协方差矩阵;
步骤S2,建立k时刻观测噪声的符合基于伽马分布的学生t混合分布的模型,基于移动机器人k时刻的视觉信息、观测噪声模型和预测状态向量获得层次高斯形式的似然函数分布;
步骤S3,基于似然函数分布构建联合后验概率密度函数,利用变分贝叶斯方法对后验联合概率密度函数进行联合求解获得移动机器人k时刻的状态向量。
3.如权利要求1或2所述的基于变分贝叶斯的移动机器人视觉惯性融合SLAM方法,其特征在于,在所述步骤S2中,移动机器人k时刻的观测噪声模型为:
5.如权利要求4所述的基于变分贝叶斯的移动机器人视觉惯性融合SLAM方法,其特征在于,在所述步骤S3中,联合后验概率密度函数为:
p(Xk,λk,ξk,uk,βk,Yk)≈q(Xk)q(λk)q(ξk)q(uk)g(βk);
其中,q(Xk)表示移动机器人k时刻的状态向量Xk的近似概率密度函数;q(λk)表示第一辅助参数λk的近似概率密度函数;q(ξk)表示第二辅助参数ξk的近似概率密度函数;q(uk)表示第一自由度uk的近似概率密度函数;q(βk)表示形状参数βk的近似概率密度函数。
8.如权利要求7所述的基于变分贝叶斯的移动机器人视觉惯性融合SLAM方法,其特征在于,在所述步骤S32中,对于第i+1次迭代,执行:步骤S321,基于变分贝叶斯方法求取Xk在第i+1次迭代中的近似概率密度函数为:
其中, 表示第i+1次迭代中k时刻移动机器人的状态向量,Xk|k-1表示移动机器人k时刻的预测状态向量,Pk|k-1表示移动机器人k时刻的一步预测误差协方差矩阵,表示第i+1次迭代的卡尔曼增益,E(i)[λk]表示λk在第i次迭代的期望值,E(i)[βk]表示βk在第i次迭代的期望值,E(i)[ξk]表示ξk在第i次迭代的期望值,表示第i+1次迭代中k时刻移动机器人的协方差矩阵;
步骤S322,基于变分贝叶斯方法求取λk在第i+1次迭代中的最优解为:
其中,E(i)[ξk]表示ξk在第i次迭代的期望值,tr{}表示求取矩阵的迹;n1、n2、n3分别为第三辅助参数、第四辅助参数、第五辅助参数;E(i)[(Yk-HkXk)(Yk-HkXk)T]表示(Yk-HkXk)(Yk-HkXk)T在第i次迭代的期望值;表示在第i次迭代的期望值;m表示单位矩阵Im的阶数;
步骤S323,基于变分贝叶斯方法求取ξk在第i+1次迭代中的近似概率密度函数为:
q(i+1)(ξk)=G(ξk;A(i+1),B(i+1));
其中,A(i+1)表示第六辅助参数A在第i+1次迭代中的值,B(i+1)表示第七辅助参数B在第i+1次迭代中的值,C(i+1)表示第八辅助参数C在第i+1次迭代中的值,E(i)[uk]表示uk在第i次迭代的期望值,E(i)[(Yk-HkXk-λkβk)(Yk-HkXk-λkβk)T]表示(Yk-HkXk-λkβk)(Yk-HkXk-λkβk)T在第i次迭代的期望值;
步骤S324,基于变分贝叶斯方法求取uk在第i+1次迭代中的近似概率密度函数为:
步骤S325,基于变分贝叶斯方法求取βk在第i+1次迭代中的近似概率密度函数为:
步骤S326,求取期望值:
E(i+1)[ξk]=A(i+1)/B(i+1);
E(i+1)[uk]=a(i+1)/b(i+1);
9.一种计算机可读存储介质,所述存储介质中存储有至少一条指令、至少一段程序、代码集或指令集,所述至少一条指令、所述至少一段程序、所述代码集或指令集由处理器加载并执行以实现如权利要求1-8中任一项所述的基于变分贝叶斯的移动机器人视觉惯性融合SLAM方法。
10.一种移动机器人,其特征在于,包括移动机器人本体和设于所述移动机器人本体上的视觉传感器、IMU传感器和处理器,所述处理器分别与视觉设别和IMU传感器连接,所述处理器执行权利要求1-8之一所述的基于变分贝叶斯的移动机器人视觉惯性融合SLAM方法的步骤。
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202211125734.3A CN115388899A (zh) | 2022-09-15 | 2022-09-15 | 基于变分贝叶斯的移动机器人视觉惯性融合slam方法 |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202211125734.3A CN115388899A (zh) | 2022-09-15 | 2022-09-15 | 基于变分贝叶斯的移动机器人视觉惯性融合slam方法 |
Publications (1)
Publication Number | Publication Date |
---|---|
CN115388899A true CN115388899A (zh) | 2022-11-25 |
Family
ID=84127279
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN202211125734.3A Pending CN115388899A (zh) | 2022-09-15 | 2022-09-15 | 基于变分贝叶斯的移动机器人视觉惯性融合slam方法 |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN115388899A (zh) |
Cited By (2)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN115930971A (zh) * | 2023-02-01 | 2023-04-07 | 七腾机器人有限公司 | 一种机器人定位与建图的数据融合处理方法 |
CN118034304A (zh) * | 2024-03-05 | 2024-05-14 | 广州市东鼎智能装备有限公司 | 一种基于实时建模的机器人路径规划方法及*** |
-
2022
- 2022-09-15 CN CN202211125734.3A patent/CN115388899A/zh active Pending
Cited By (3)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN115930971A (zh) * | 2023-02-01 | 2023-04-07 | 七腾机器人有限公司 | 一种机器人定位与建图的数据融合处理方法 |
CN115930971B (zh) * | 2023-02-01 | 2023-09-19 | 七腾机器人有限公司 | 一种机器人定位与建图的数据融合处理方法 |
CN118034304A (zh) * | 2024-03-05 | 2024-05-14 | 广州市东鼎智能装备有限公司 | 一种基于实时建模的机器人路径规划方法及*** |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
CN112347840B (zh) | 视觉传感器激光雷达融合无人机定位与建图装置和方法 | |
CN111136660B (zh) | 机器人位姿定位方法及*** | |
CN115388899A (zh) | 基于变分贝叶斯的移动机器人视觉惯性融合slam方法 | |
CN112083726B (zh) | 一种面向园区自动驾驶的双滤波器融合定位*** | |
CN110018691B (zh) | 小型多旋翼无人机飞行状态估计***和方法 | |
CN115143954B (zh) | 一种基于多源信息融合的无人车导航方法 | |
CN111949929B (zh) | 一种多传感器融合的四足机器人运动里程计设计方法 | |
CN113819917A (zh) | 自动驾驶路径规划方法、装置、设备及存储介质 | |
CN114459470A (zh) | 基于多传感器融合的巡检机器人定位方法 | |
CN116067370A (zh) | 一种imu姿态解算方法及设备、存储介质 | |
CN113483755A (zh) | 一种基于非全局一致地图的多传感器组合定位方法及*** | |
CN114689047A (zh) | 基于深度学习的组合导航方法、装置、***及存储介质 | |
CN113340324B (zh) | 一种基于深度确定性策略梯度的视觉惯性自校准方法 | |
WO2024114593A1 (zh) | 多线激光定位方法及定位装置、计算机设备、存储介质 | |
CN115930971B (zh) | 一种机器人定位与建图的数据融合处理方法 | |
CN108710295B (zh) | 一种基于渐进容积信息滤波的机器人跟随方法 | |
Emter et al. | Stochastic cloning for robust fusion of multiple relative and absolute measurements | |
CN113984045B (zh) | 水下机器人移动式对接目标运动状态估计方法及*** | |
CN111812668B (zh) | 绕机检查装置及其定位方法、存储介质 | |
CN114046800A (zh) | 一种基于双层滤波框架的高精度里程估计方法 | |
CN114777762A (zh) | 一种基于贝叶斯nas的惯性导航方法 | |
CN118274869A (zh) | 一种移动机器人视觉惯性里程计算方法 | |
CN113758491B (zh) | 基于多传感器融合无人车的相对定位方法、***及车辆 | |
WO2024037295A1 (zh) | 定位 | |
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 |