CN110610513B - 自主移动机器人视觉slam的不变性中心差分滤波器方法 - Google Patents
自主移动机器人视觉slam的不变性中心差分滤波器方法 Download PDFInfo
- Publication number
- CN110610513B CN110610513B CN201910879673.1A CN201910879673A CN110610513B CN 110610513 B CN110610513 B CN 110610513B CN 201910879673 A CN201910879673 A CN 201910879673A CN 110610513 B CN110610513 B CN 110610513B
- Authority
- CN
- China
- Prior art keywords
- variable
- matrix
- observation
- variables
- 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.)
- Active
Links
- 238000000034 method Methods 0.000 title claims abstract description 39
- 239000011159 matrix material Substances 0.000 claims abstract description 129
- 239000013598 vector Substances 0.000 claims abstract description 88
- 238000004364 calculation method Methods 0.000 claims abstract description 61
- 238000004422 calculation algorithm Methods 0.000 claims abstract description 41
- 238000005070 sampling Methods 0.000 claims abstract description 39
- 238000001914 filtration Methods 0.000 claims abstract description 10
- 238000013461 design Methods 0.000 claims abstract description 8
- 238000010276 construction Methods 0.000 claims abstract description 3
- 238000000354 decomposition reaction Methods 0.000 claims description 21
- 238000013507 mapping Methods 0.000 claims description 12
- 238000005259 measurement Methods 0.000 claims description 11
- 230000000007 visual effect Effects 0.000 claims description 11
- 230000008569 process Effects 0.000 claims description 10
- 238000012937 correction Methods 0.000 claims description 8
- 241000135164 Timea Species 0.000 claims description 2
- 238000012935 Averaging Methods 0.000 claims 1
- XLYOFNOQVPJJNP-UHFFFAOYSA-N water Substances O XLYOFNOQVPJJNP-UHFFFAOYSA-N 0.000 claims 1
- 230000008901 benefit Effects 0.000 abstract description 5
- 230000009466 transformation Effects 0.000 description 4
- 238000005457 optimization Methods 0.000 description 3
- 238000005516 engineering process Methods 0.000 description 2
- 238000005295 random walk Methods 0.000 description 2
- 238000000342 Monte Carlo simulation Methods 0.000 description 1
- 230000001133 acceleration Effects 0.000 description 1
- 239000000654 additive Substances 0.000 description 1
- 230000000996 additive effect Effects 0.000 description 1
- 230000002411 adverse Effects 0.000 description 1
- 230000009286 beneficial effect Effects 0.000 description 1
- 238000007796 conventional method Methods 0.000 description 1
- 238000010586 diagram Methods 0.000 description 1
- 230000000694 effects Effects 0.000 description 1
- 238000011156 evaluation Methods 0.000 description 1
- 230000005484 gravity Effects 0.000 description 1
- 230000010365 information processing Effects 0.000 description 1
- 230000004807 localization Effects 0.000 description 1
- 230000014759 maintenance of location Effects 0.000 description 1
- 238000012986 modification Methods 0.000 description 1
- 230000004048 modification Effects 0.000 description 1
- 238000007500 overflow downdraw method Methods 0.000 description 1
- 238000011160 research Methods 0.000 description 1
- 230000003068 static effect Effects 0.000 description 1
- 238000013519 translation Methods 0.000 description 1
- 230000017105 transposition Effects 0.000 description 1
Images
Classifications
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
- G06T7/00—Image analysis
- G06T7/20—Analysis of motion
- G06T7/277—Analysis of motion involving stochastic approaches, e.g. using Kalman filters
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
- G06T2207/00—Indexing scheme for image analysis or image enhancement
- G06T2207/30—Subject of image; Context of image processing
- G06T2207/30248—Vehicle exterior or interior
- G06T2207/30252—Vehicle exterior; Vicinity of vehicle
Landscapes
- Engineering & Computer Science (AREA)
- Multimedia (AREA)
- Computer Vision & Pattern Recognition (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Theoretical Computer Science (AREA)
- Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)
Abstract
本发明提出了一种自主移动机器人视觉SLAM的不变性中心差分滤波器方法,用以解决自主移动机器人视觉SLAM***运动模型状态变量滤波计算度复杂度高问题。本发明设计了面向矩阵李群向量空间和不变Kalman滤波器的中心差分滤波器计算方法,滤波器状态变量由SE(3)李群向量表示的机器人位姿、速度和3D路标位置向量及加速度计与陀螺仪偏差向量组成,在矩阵李群空间中设计CDKF滤波器的Sigma采样点均值及误差量,利用单目视觉相机构造运动图像特征点的逆深度观测模型,设计CDKF中心差分滤波器的预测与更新迭代计算,开展机器人定位与地图构建计算任务。相比于常规的机器人***模型的EKF算法,本发明计算效能高,计算速度快,具备较好的实际应用价值。
Description
技术领域
本发明涉及航空航天***信息处理的技术领域,尤其涉及一种自主移动机器人视觉SLAM的不变性中心差分滤波器方法,即自主移动机器人即时定位与地图构建***(Visual Simultaneous Localization And Mapping,VSLAM)问题的一种新型的视觉里程计不变性中心差分滤波器模型方法。
背景技术
最近几年自主移动机器人的视觉即时定位与地图构建(VSLAM)技术获得较快发展,作为多传感***配置的自主移动机器人***数据融合方法有滤波器方法、梯度下降优化方法以及光束平差(bundle Adjustment,BA)优化技术等,其中梯度下降优化法计算效能很好,但是相比滤波法计算量很大,滤波器法在实际应用中使用的比较多。
在机器人***模型研究中应用最多的是位姿空间的SE(3)李群结构,特别是近年来基于机器人***状态估计和控制应用,SE(3)结构体中的概率分布特性得到较好的研究分析。在SLAM***中利用几何保持特性的右不变扩展Kalman滤波(Right InvariantExtended Kalman filter,RIEKF)算法中,把输出函数投影到相机坐标系中,RIEKF算法具有较好的误差一致性特性,在2D视觉惯性导航***(Visual Inertial NavigationSystem,VINS)中获得较好的计算效能,但是多状态约束RIEKF算法中强调的是图像特征状态变量的几何约束特性来达到视觉里程计计算性能目的,没有考虑到视觉SLAM问题中的路标状态剔除操作,从而会导致***状态变量逐步增长,计算量增加,对于视觉惯性SLAM***导航计算性能造成不利影响。特别是针对3D场景应用,RIEKF算法还没有获得较好的验证与应用。
发明内容
针对配置惯性测量组件IMU和单目视觉传感器的自主移动机器人的视觉SLAM中计算复杂度高的技术问题,本发明提出一种自主移动机器人视觉SLAM的不变性中心差分滤波器(CDKF)方法,面向SLAM矩阵李群向量的不变性中心差分滤波器计算,开展自主移动机器人视觉SLAM***运动模型状态变量最优滤波计算,计算效能高,计算速度快,具备较好的实际应用价值。
为了达到上述目的,本发明的技术方案是这样实现的:
本发明的有益效果:采用RIEKF算法的计算架构,把SLAM问题的路标变量添加为***状态变量参与计算过程,基于不变性理论提出一种右不变中心差分矩阵李群算法,应用于自主移动机器人视觉SLAM***运动模型状态变量最优滤波计算过程,实现自主移动机器人VSLAM***模型状态参数最优滤波计算;本发明基于中心差分变换而不是传统Taylor级数扩展逼近变换,基于中心差分变换操作能够有效改善算法的计算精度,相比于常规的机器人***模型的EKF算法,中心差分变换的固定插值步长保证可控制的计算稳定性,计算效能高,计算速度快,具备较好的实际应用价值,可应用于无人机器人定位导航与控制领域。
附图说明
为了更清楚地说明本发明实施例或现有技术中的技术方案,下面将对实施例或现有技术描述中所需要使用的附图作简单地介绍,显而易见地,下面描述中的附图仅仅是本发明的一些实施例,对于本领域普通技术人员来讲,在不付出创造性劳动的前提下,还可以根据这些附图获得其他的附图。
图1为本发明的流程图。
图2为自主移动机器人运动轨迹图。
图3是自主移动机器人VSLAM***的位置一致性误差数据的曲线图。
图4是自主移动机器人VSLAM***的位置RMSE误差数据的曲线图。
图5是自主移动机器人VSLAM***的方位一致性误差数据的曲线图。
图6是自主移动机器人VSLAM***的方位RMSE误差数据的曲线图。
具体实施方式
下面将结合本发明实施例中的附图,对本发明实施例中的技术方案进行清楚、完整地描述,显然,所描述的实施例仅是本发明一部分实施例,而不是全部的实施例。基于本发明中的实施例,本领域普通技术人员在没有付出创造性劳动前提下所获得的所有其他实施例,都属于本发明保护的范围。
预备知识:本发明考虑自主移动机器人装备三轴惯性IMU测量组件和机器相机测量传感器来构建自主移动机器人观测模型,从而利用中心差分滤波理论设计一种自主移动机器人VSLAM***的新型不变性中心差分滤波器模型及计算方法。在三轴惯性IMU测量组件中IMU偏差看作为随机游走模型,假设有p个固定路标在视觉相机观测域内可被视觉跟踪,这些固定路标构建了自主移动机器人稀疏地图,在自主移动机器人运动模型中包含的状态变量为:自主移动机器人的位置变量速度向量和姿态向量R∈SO(3)及IMU测量组件中陀螺仪偏差向量加速度计偏差向量还有在全局坐标系中固定路标的3D位置向量其中,和SO(3)分别表示实数域空间和李代数域空间,那么可以获得自主移动机器人VSLAM***的动态方程为:
IMU偏差方程表示为:
路标位置方程表示为:
其中,和分别表示姿态向量R、速度向量v和位置变量x的导数,(ω-bω+wω)×表示由向量组成的斜对称矩阵,和分别为和的导数,为位置向量pi的导数,ω表示陀螺仪测量角速率,wω表示陀螺仪零偏,a表示加速度计测量的线加速度,wa表示加速度计静态偏置,g表示重力加速度,表示陀螺仪的随机游走偏差量,表示加速度计的随机游走偏差量。
陀螺仪和加速度计噪声可统一表示为***噪声向量,即:
且VSLAM***的噪声具有自相关特性,E[w(t)w(s)]=Wδ(t-s),上标T表示矩阵或向量的转置,t和s表示不同时刻,W表示不同时刻的噪声相关矩阵,δ(t-s)表示不同时刻的噪声Dirichlet函数,t=s时为1,否则为0。在自主移动机器人VSALM***中IMU测量组件测量的数据作为VSLAM***的输入量,同时把已经标定好的单目相机测量数据作为***观测量使用,通过构建相机的针孔模型观测和跟踪p个固定路标,其观测方程表示为:
这里,yi是在相机坐标系中的第i个路标的归一化像素位置,其中,
另外本发明利用矩阵李群理论开展中心差分滤波理论的新算法设计任务,首先给出矩阵李群的数学表述,矩阵李群表示一个平方可逆矩阵的子集合,矩阵李群具有性质:单位矩阵I是属于矩阵李群,若任意的向量χ属于矩阵李群G,那么它的逆也属于矩阵李群G,任意的向量χ1,χ2属于矩阵李群G,那么它们的乘积也属于矩阵李群G表示为:
对于单位矩阵I,利用矩阵指数映射expm(·),矩阵李群G等同于欧式空间这里q=dimG表示矩阵李群维数,对于欧式空间中的任意随机向量在矩阵李群G的切空间存在伴随矩阵定义矩阵李群G的指数映射函数:这个指数映射函数是双映射,相应指数映射的逆映射为对数映射,可定义为, 根据李群定义开展李群空间的随机变量操作,由于矩阵李群G空间不是一个向量空间,李群空间中向量不能像常规方法来处理加性噪声项,在李群空间中定义CDKF的Sigma采样点变量χ满足均值为方差为P的概率分布:利用李群空间左乘来定义Sigma采样点变量为:
为了保持***混合变量计算的一致性,本发明中采用了右乘法则开展李群矩阵变量计算。
在李群理论中利用李群变量χ表示VSLAM状态变量的Sigma采样变量,利用噪声向量ξ表示VSLAM在李代数域中的Sigma采样点误差变量,那么利用前面随机向量的左乘和右乘算子,在李群空间中可以构建两种中心差分滤波算法,针对自主移动机器人VSLAM***构建CDKF Sigma采样点的离散动态模型***:
χn+1=f(χn,un,wn), (10)
同时离散化观测方程:
yn=h(χn)+nn, (11)
其中,yn表示观测变量,观测噪声满足均值为方差为Nn的分布特性,那么可以利用左乘操作算子设计Left-CDKF-LG算法,其状态变量满足和右乘作算子设计Right-CDKF-LG算法,其状态变量满足均值为方差为Pn的概率分布。
另外应该可以看到的是SLAM问题具有李群的自然结构特性,可将其定义为SE1+p(3)李群,这也是不变扩展Kalman滤波算法解决传统EKF算法的一致收敛性的原因所在,在本发明提出的不变性中心差分滤波方法也要用到RI-EKF算法的这些特性,这里给出其性质:
定义矩阵李群变量为:
那么根据机器人视觉惯性SLAM***状态变量可知李群以及相应李代数维数为3+3(2+p),相应表达不确定性的误差参量定义为:
自主移动机器人VSLAM***状态空间模型的部分状态变量如R v x p1…pp等可以利用矩阵李群G=SE2+p(3)中的矩阵变量χn表示,而陀螺仪和加速度计偏差变量作为常规向量表示,这样VSLAM***状态变量为(χn bn),从而开展左乘CDKF-LG算法或者右乘CDKF-LG算法设计工作,陀螺仪和加速度计的输出数据定义为***输入控制量u=(ωT aT),那么可以获得VSLAM***的中心差分滤波器离散化模型:
VSLAM***状态变量的不确定性偏差变量为:
VSLAM***的动态方程描述为:{χn,bn}=f(χn-1,un-bn-1,wn), (16)
VSLAM中相机观测模型及其观测向量描述为,
这里下标n表示离散化采样时刻,un表示输入变量,表示李代数变量,和分别表示陀螺仪测量噪声变量及其噪声项,χn-1和bn-1分别表示n-1时刻的李群变量和测量噪声变量,y1,…yp表示p个路标位置变量,hi(χn)表示由李群变量表示的观测函数,观测变量yn,i是由路标位置在图像坐标系中的uv方向变量和深度变量以及观测噪声表示出来的。这样整理后获得VSLAM***的状态变量n时刻的估计均值***状态变量的不确定性偏差向量为且由其定义的方差矩阵为观测向量Yn包含了p个路标位置的观测,其观测噪声满足
利用自主移动机器人VSLAM动态***模型和相机观测路标模型,基于李群李代数理论知识,设计出概率分布形式的多状态约束不变性中心差分滤波器模型算法,多状态约束不变性中心差分滤波器算法分为***状态变量预测和观测更新步骤,下面给出多状态约束不变性中心差分滤波器模型算法设计实施内容。
如图1所示,一种自主移动机器人视觉SLAM的不变性中心差分滤波器方法,其步骤如下:
步骤一:根据自主移动机器人配置的IMU测量组件建立VSLAM***状态方程,利用相机观测固定路标的观测量构建VSLAM的观测方程。
根据前述的自主移动机器人VSLAM***的状态方程包括:
VSLAM***的动态方程为:
{χn,bn}=f(χn-1,un-bn-1,wn), (18)
相机观测p个路标位置向量的观测方程为:
其参数及其符号描述如前所述。
步骤二:基于矩阵李群李代数理论,对自主移动机器人3维姿态、位置和速度向量、固定路标位置向量做出李群矩阵变量设计,如前述李群变量χn表示,联合机器人配置的IMU测量组件中的陀螺仪和加速度计偏差向量组成VSLAM***的混合状态空间变量{χn,bn}。
步骤三:根据步骤二获得自主移动机器人VSLAM***的状态方程,假设已知第n-1时刻的***混合变量的估计数据,把***状态变量和***噪声变量扩展为状态变量方差矩阵,并计算其平方根;对陀螺仪和加速度计输入变量做出无偏修正计算计算***混合状态空间变量的预测均值。
假设第n-1时刻的估计均值数据为估计方差矩阵为Pn-1,对方差矩阵实施Cholesky分解操作得到矩阵:Sn-1=chol(Pn-1),chol()表示Cholesky分解函数,过程噪声方差为Qn-1;若第n时刻的观测数据为Yn,观测噪声方差为Nn,输入变量为un,那么根据n-1时刻的估计数据,首先计算***IMU测量组件的无偏输入量对此时***噪声方差平方根扩展整理为:
其中,blkdiag{}表示***噪声方差平方根扩展对角矩阵。
其中,h表示插值步长,其最优数值为 表示误差变量,表示陀螺仪偏差变量、表示***噪声的第j个采样点,colj(·)表示采样点获得的第j列向量,前面“±”符号表示采样点矩阵中前J列取正号,后J列取负号,根据中心差分滤波理论,其中采样点的权值γ设计为:
把获得的2J个采样点带入***方程中开展预测计算,获得***状态变量的预测数据,利用第n-1时刻的***混合变量(χn-1bn-1)的估计均值,计算第n时刻的***状态变量预测均值为:
以及***状态变量的采样点预测映射:
其中,表示陀螺仪偏差采样值,表示过程噪声采样值,表示李代数变量采样值,分别表示李群矩阵变量以及陀螺仪偏差预测值。根据状态变量的方差矩阵平方根,确定李群矩阵变量、陀螺仪和加速度计偏差变量以及***噪声变量的CDKF采样点偏差量,根据***扩展状态维数确定采样点数为J=27+3p,根据李群矩阵变量的右乘法则,利用***状态方程开展第n时刻的VSLAM***状态变量采样点的预测计算,进而获得VSLAM***混合变量的预测加权均值数据。
步骤四:根据李群矩阵变量的预测值利用对数法则获得李群矩阵变量的不确定误差变量预测数据,利用QR分解计算方法开展VSLAM***混合变量的预测方差矩阵平方根计算。
根据计算式log{exp(ξ)}=ξ,及式(9)右乘项操作,利用指数运算逆函数对数计算李群矩阵向量的偏差量:
从而利用式(9)、式(23)的***状态变量预测均值和式(25)李群矩阵向量的偏差量,获得李群变量的第n时刻的预测值。首先利用CDKF求均值方法计算***不确定性变量均值,接着利用不确定变量均值计算李群变量预测均值:
对于预测更新后的方差矩阵计算,通过开展QR分解操作:
qr()表示QR分解式,其具体QR分解表达式为:
从其中分解获得***状态变量方差矩阵的平方根Sn,n-1,从而经由中心差分滤波器的状态预测计算式(23)的***状态变量预测均值和式(26)的QR分解操作来获得***状态变量预测信息χn,n-1,bn,n-1,Sn,n-1。
步骤五:将第n时刻的VSLAM***混合变量的预测均值χn,n-1,bn,n-1,Sn,n-1和第n时刻的观测数据带入相机观测模型方程,获得VSLAM***的观测更新均值。
根据观测模型方程维数,确定观测更新的***状态变量偏差采样点及其权值系数,此时采样点数J’=15+3p,根据观测更新均值、观测更新偏差采样点确定VSLAM***混合变量的观测更新计算,利用QR分解策略确定***混合变量的观测更新方差矩阵平方根。
***状态的观测更新步骤,在第n时刻的观测更新步骤中融合p个路标位置的观测向量,以及此时刻预测***状态变量χn,n-1,bn,n-1,Sn,n-1,在矩阵李群空间中的观测变量均值经由观测方程式(19)映射,其中第0个Sigma采样点表示为:
Yn,0=Y(χn,n-1,0), (28)
从而利用观测方程获得观测向量采样点更新计算:
其中,Yn,j表示2J′个观测向量更新计算的Sigma采样值。再利用qr'(·)函数计算更新的Cholesky分解因子,以及***状态变量修正项利用修正项更新***状态变量。其中首先计算观测平均值及其加权偏差量:
其中,矩阵R'是观测噪声方差矩阵的Cholesky分解方块对角矩阵,其对角线元素由观测噪声方差矩阵的Cholesky分解因子的p次方组成。
步骤六:利用CDKF算法计算Kalman滤波器增益矩阵,根据第n时刻的观测数据计算***混合状态变量的偏差修正数据,利用Baker-Campbell-Hausdorff公式对李群矩阵变量开展右乘计算及其一阶近似获得第n时刻的***混合变量的最优估计数据。
按照常规的CDKF算法计算滤波器增益矩阵Kn:
以及方差矩阵平方根Sn计算,
SeqcholUpdate(·)表示利用矩阵的连续列向量重复开展ckolesky分解计算。另外考虑到新息变量的重要性,用来修正偏差向量ξ,利用Baker-Campbell-Hausdorff算法的一阶近似表达式,修正计算公式为:
其中ξ+表示矩阵李群不确定变量的第n时刻的更新值,式(36)中右边的O(ξ+)表示指数函数线性化展开后的高阶项,M表示左Jacobian矩阵,那么更新矩阵李群状态变量更新计算为:
相应的***状态方差矩阵可计算为,
具体实施例:考虑自主移动机器人VSLAM***,自主移动机器人装备了单目相机和惯性IMU测量组件,IMU测量组件提供了自主移动机器人***的3维姿态、速度和位置向量信息,单目相机对p个固定路标位置实施观测,获得p个观测方程,由此自主移动机器人VSLAM***模型可表示为:
这里VSLAM***状态向量由3维姿态、速度和位置向量组成的李群矩阵变量与其余3维陀螺仪和加速度计偏差向量组成混合状态向量;运动模型方程中的3维陀螺仪和加速度计输出量ω和a组成控制输入变量:u=[ωT,aT]T。***噪声为噪声向量wn是高斯过程噪声,噪声向量wn~N(0,Qn),其中,Qn表示噪声方差。
在此具体实例中采用相机观测的p个固定路标的观测方程表示为:
其中,nn是观测白噪声,满足分布vn~N(0,Nn),Nn表示观测噪声方差。这里设计p=100个路标,在每一次迭代计算过程中相机观测到60个路标,在观测噪声中,设相机观测噪声为2个像素偏差。为了比较本发明的计算优势,本发明矩阵李群右乘CDKF算法(R-CDKF)和传统的扩展Kalman滤波器(EKF)算法、多状态约束EKF滤波器算法(MSC-EKF)对比,仿真程序运行100次Monte Carlo仿真计算,获得如图2所示的自主移动机器人运行轨迹图,如图3所示的自主移动机器人运行姿态一致性误差的曲线图,如图4所示的自主移动机器人位置RMSE误差的曲线图,如图5所示的自主移动机器人方位一致性误差的曲线图和图6所示的自主移动机器人VSLAM***的方位RMSE误差数据的曲线图。从图2本发明的R-CDKF算法获得自主移动机器人运行轨迹和实际机器人运动轨迹数据比较,很明显R-CDKF算法获得的数据接近于实际运行轨迹,并且路标位置的估计数据和实际位置是接近的,误差在可以接受的范围内;和其他两种算法相比,本发明的R-CDKF算法在机器人位置和方位标准偏差(RMSE)误差方面并没有显示出计算精度优势,三种算法的计算精度基本上差别不是很大,但是在机器人位置和方位误差的一致性误差评价方面,本发明的R-CDKF算法具有很明显的计算优势。
本发明设计了一种面向矩阵李群向量空间和不变Kalman滤波器的中心差分滤波器计算方法,滤波器状态变量由SE(3)李群向量表示的机器人位姿、速度和3D路标位置向量、加速度计与陀螺仪偏差向量组成,利用单目视觉相机构造运动图像特征点的逆深度观测模型,设计中心差分滤波器开展机器人定位与地图构建计算任务。相比于常规的机器人***模型的EKF算法,本发明计算效能高,计算速度快,具备较好的实际应用价值。
以上所述仅为本发明的较佳实施例而已,并不用以限制本发明,凡在本发明的精神和原则之内,所作的任何修改、等同替换、改进等,均应包含在本发明的保护范围之内。
Claims (9)
1.一种自主移动机器人视觉SLAM的不变性中心差分滤波器方法,其特征在于,设计了一种面向矩阵李群空间和不变性Kalman滤波器的中心差分滤波器计算方法,滤波器状态变量由SE(3)李群向量表示的机器人位姿、速度和3D路标位置向量及加速度计与陀螺仪偏差向量组成,在矩阵李群空间中设计CDKF滤波器的Sigma采样点均值及其误差量,利用单目视觉相机构造运动图像特征点的相机观测模型,设计CDKF滤波器的预测与更新迭代计算,开展机器人定位与地图构建计算任务,其步骤如下:
步骤一:根据自主移动机器人配置的IMU测量组件建立VSLAM***的状态方程,利用相机观测固定路标的观测量构建VSLAM***的观测方程;
步骤二:基于矩阵李群李代数理论,对自主移动机器人三维位置、姿态和速度向量、固定路标位置向量做出矩阵李群变量设计,联合机器人配置的IMU测量组件中的陀螺仪和加速度计偏差向量组成VSLAM***的混合状态空间变量;
步骤三:根据VSLAM***状态方程,假设已知第n-1时刻的***混合变量的估计数据,把***状态变量和***噪声变量扩展为状态变量方差矩阵,并计算其平方根;对陀螺仪和加速度计输入变量做出无偏修正计算,计算VSLAM***混合状态空间变量的预测均值;
步骤四:根据李群矩阵变量的预测值利用对数法则获得李群矩阵变量的不确定误差变量预测数据,利用QR分解计算方法开展VSLAM***混合变量的预测方差矩阵平方根计算;
步骤五:将第n时刻的VSLAM***混合变量的预测均值和第n时刻的观测数据带入相机观测模型,获得VSLAM***的观测更新均值;
步骤六:利用CDKF算法计算Kalman滤波器增益矩阵,根据第n时刻的观测数据计算***混合状态变量的偏差修正数据,利用Baker-Campbell-Hausdorff公式对李群矩阵变量开展右乘计算及其一阶近似获得第n时刻的***混合变量的最优估计数据。
2.根据权利要求1所述的自主移动机器人视觉SLAM的不变性中心差分滤波器方法,其特征在于,所述步骤一中VSLAM***的状态方程为:
{χn,bn}=f(χn-1,un-bn-1,wn),
VSLAM***的观测方程为相机观测p个路标位置向量的观测方程为:
其中,{χn bn}为VSLAM***状态变量,χn是n时刻的李群变量,bn是陀螺仪和加速度计偏差变量,un是输入变量,表示右乘噪声项的李代数变量,和分别表示陀螺仪测量噪声变量及其噪声项,χn-1和bn-1分别表示n-1时刻的李群变量和测量噪声变量,观测向量Yn包含了p个路标位置的观测,y1,αyp表示p个路标位置变量,T表示向量的转置,hi(χn)表示由李群变量表示的观测函数,观测变量yn,i是由路标位置在图像坐标系中的uv方向变量和深度变量以及观测噪声表示出来的,Pn为n时刻的方差矩阵;ξn为n时刻的误差参量,观测噪声满足分布,为第i个路标位置的观测噪声;噪声变量wn满足均值为0、方差为过程噪声方差Qn的分布:
3.根据权利要求2所述的自主移动机器人视觉SLAM的不变性中心差分滤波器方法,其特征在于,所述步骤二中在矩阵李群李代数理论中利用李群变量χ表示VSLAM状态变量的Sigma采样变量,利用噪声向量ξ表示VSLAM在李代数域中的Sigma采样点误差变量,SLAM问题具有李群的自然结构特性,定义为SE1+p(3)李群,所述矩阵李群变量为:
其中,R为姿态向量,v为速度向量,x为位置变量,pi为第i个路标位置的位置向量,I为单位矩阵;状态变量R v x p1…pp利用矩阵李群G=SE2+p(3)中的矩阵变量χn表示;
4.根据权利要求3所述的自主移动机器人视觉SLAM的不变性中心差分滤波器方法,其特征在于,所述步骤三中根据状态变量的方差矩阵平方根,确定李群矩阵变量、陀螺仪和加速度计偏差变量以及***噪声变量的CDKF采样点偏差量,根据***扩展状态维数确定采样点数为J=27+3p;根据李群矩阵变量的右乘法则,利用***状态方程开展第n时刻的VSLAM***状态变量采样点的预测计算,进而获得VSLAM***混合变量的预测加权均值数据;所述步骤五中根据观测方程维数,确定观测更新的***状态变量偏差采样点及其权值系数,此时采样点数J’=15+3p,根据观测更新均值、观测更新偏差采样点确定VSLAM***混合变量的观测更新计算,利用QR分解策略确定***混合变量的观测更新方差矩阵平方根。
5.根据权利要求2或4所述的自主移动机器人视觉SLAM的不变性中心差分滤波器方法,其特征在于,所述步骤三中假设第n-1时刻的估计均值数据为估计方差矩阵为Pn-1,对方差矩阵实施Cholesky分解操作得到矩阵:Sn-1=chol(Pn-1),chol()表示Cholesky分解函数;若第n时刻的观测数据为Yn,观测噪声方差为Nn,输入变量为un,那么根据n-1时刻的估计数据,计算***IMU测量组件的无偏输入量***噪声方差平方根扩展整理为:
其中,blkdiag{}表示***噪声方差平方根扩展对角矩阵,Qn-1为过程噪声方差;
根据中心差分滤波理论,其中采样点的权值γ为:
把获得的2J个采样点带入状态方程中开展预测计算,获得***状态变量的预测数据,利用第n-1时刻的***混合变量(χn-1,bn-1)的估计均值,计算第n时刻的***状态变量预测均值为:f为状态方程的函数,以及***状态变量的采样点预测映射:
8.根据权利要求7所述的自主移动机器人视觉SLAM的不变性中心差分滤波器方法,其特征在于,所述步骤五中观测更新均值的计算步骤为:融合p个路标位置的观测向量以及预测***状态变量χn,n-1,bn,n-1,Sn,n-1,在矩阵李群空间中的观测变量均值经由观测方程映射,第0个Sigma采样点表示为:Yn,0=Y(χn,n-1,0);
计算其余的2J′个观测向量Sigma采样点:对观测向量及其噪声项实施状态扩展,利用李代数右乘公式获得:其中,Nn表示观测噪声方差矩阵, 和分别表示2J′个***不确定性变量、陀螺仪偏差向量和观测噪声向量的Sigma采样点,表示2J′个李群变量、前J′项取负号、后J′项取正号;
计算观测平均值及其加权偏差量:
计算第n时刻的***状态更新方差矩阵P′n为:
其中,矩阵R'是观测噪声方差矩阵的Cholesky分解方块对角矩阵,其对角线元素由观测噪声方差矩阵的Cholesky分解因子的p次方组成。
9.根据权利要求6-8中任意一项所述的自主移动机器人视觉SLAM的不变性中心差分滤波器方法,其特征在于,所述步骤六中CDKF算法计算滤波器增益矩阵Kn为:
计算方差矩阵平方根Sn为
利用Baker-Campbell-Hausdorff算法的一阶近似表达式,修正差向量ξ的计算公式为:
其中,ξ+表示矩阵李群不确定变量的第n时刻的更新值,O(ξ+)表示指数函数线性化展开后的高阶项,M表示左Jacobian矩阵;
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN201910879673.1A CN110610513B (zh) | 2019-09-18 | 2019-09-18 | 自主移动机器人视觉slam的不变性中心差分滤波器方法 |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN201910879673.1A CN110610513B (zh) | 2019-09-18 | 2019-09-18 | 自主移动机器人视觉slam的不变性中心差分滤波器方法 |
Publications (2)
Publication Number | Publication Date |
---|---|
CN110610513A CN110610513A (zh) | 2019-12-24 |
CN110610513B true CN110610513B (zh) | 2022-02-08 |
Family
ID=68892884
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN201910879673.1A Active CN110610513B (zh) | 2019-09-18 | 2019-09-18 | 自主移动机器人视觉slam的不变性中心差分滤波器方法 |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN110610513B (zh) |
Families Citing this family (2)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN115003983A (zh) * | 2020-02-13 | 2022-09-02 | Oppo广东移动通信有限公司 | 用于通过动态调整测量噪声协方差进行视觉slam的误差状态卡尔曼滤波器 |
CN113155152B (zh) * | 2021-03-14 | 2023-01-03 | 北京工业大学 | 基于李群滤波的相机与惯性传感器空间关系自标定方法 |
Citations (3)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN106767780A (zh) * | 2016-11-28 | 2017-05-31 | 郑州轻工业学院 | 基于Chebyshev多项式插值逼近的扩展椭球集员滤波方法 |
CN109558879A (zh) * | 2017-09-22 | 2019-04-02 | 华为技术有限公司 | 一种基于点线特征的视觉slam方法和装置 |
CN109931957A (zh) * | 2019-03-24 | 2019-06-25 | 北京工业大学 | 基于lgmkf的sins捷联惯性导航***自对准方法 |
Family Cites Families (1)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
EP2619742B1 (en) * | 2010-09-24 | 2018-02-28 | iRobot Corporation | Systems and methods for vslam optimization |
-
2019
- 2019-09-18 CN CN201910879673.1A patent/CN110610513B/zh active Active
Patent Citations (3)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN106767780A (zh) * | 2016-11-28 | 2017-05-31 | 郑州轻工业学院 | 基于Chebyshev多项式插值逼近的扩展椭球集员滤波方法 |
CN109558879A (zh) * | 2017-09-22 | 2019-04-02 | 华为技术有限公司 | 一种基于点线特征的视觉slam方法和装置 |
CN109931957A (zh) * | 2019-03-24 | 2019-06-25 | 北京工业大学 | 基于lgmkf的sins捷联惯性导航***自对准方法 |
Non-Patent Citations (4)
Title |
---|
Monocular SLAM for Autonomous Robots with Enhanced;Edmundo Guerraet al.;《Sensors》;20140402;第6317-6337页 * |
Sigma Point Kalman Filtering on Matrix Lie;James Richard Forbes et al.;《Springer 》;20171231;第318–328页 * |
SR-CDKF方法在探测器自主光学导航中的应用;隋树林等;《测试技术学报》;20070630;第21卷;第241-246页 * |
基于改进SCKF算法的变维交互多模算法;王扬钧等;《计算机工程与应用》;20151231;第80-87页 * |
Also Published As
Publication number | Publication date |
---|---|
CN110610513A (zh) | 2019-12-24 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
Heo et al. | Consistent EKF-based visual-inertial odometry on matrix Lie group | |
CN104655131B (zh) | 基于istssrckf的惯性导航初始对准方法 | |
CN109931955B (zh) | 基于状态相关李群滤波的捷联惯性导航***初始对准方法 | |
CN105973238B (zh) | 一种基于范数约束容积卡尔曼滤波的飞行器姿态估计方法 | |
CN105806363A (zh) | 基于srqkf的sins/dvl水下大失准角对准方法 | |
CN110610513B (zh) | 自主移动机器人视觉slam的不变性中心差分滤波器方法 | |
CN110160522A (zh) | 一种基于稀疏特征法的视觉惯导里程计的位姿估计方法 | |
CN114529576A (zh) | 一种基于滑动窗口优化的rgbd和imu混合跟踪注册方法 | |
van Der Laan et al. | The invariant rauch-tung-striebel smoother | |
CN110926499B (zh) | 基于李群最优估计的sins捷联惯性导航***晃动基座自对准方法 | |
Liu et al. | On terrain-aided navigation for unmanned aerial vehicle using B-spline neural network and extended Kalman filter | |
Sanyal | Optimal attitude estimation and filtering without using local coordinates part i: Uncontrolled and deterministic attitude dynamics | |
CN113029173A (zh) | 车辆导航方法及装置 | |
Sang et al. | Invariant cubature kalman filtering-based visual-inertial odometry for Robot pose estimation | |
Yang et al. | Robust H∞ filtering for a spacecraft attitude determination system with affine LPV approach | |
CN116295342A (zh) | 一种用于飞行器勘测的多传感状态估计器 | |
Zarei et al. | Performance improvement for mobile robot position determination using cubature Kalman filter | |
Hwang et al. | A novel federated structure of invariant EKF using left-/right-invariant observations | |
CN115077517A (zh) | 一种基于立体相机与imu融合的低速无人车定位方法及*** | |
CN108871312A (zh) | 一种重力梯度仪及星敏感器的联合定姿方法 | |
Damerius et al. | A generic inertial navigation system | |
CN114723902A (zh) | 基于概率膜计算的煤矿井下无人直升机slam | |
Zhao et al. | Multiple model adaptive estimation algorithm for SINS/CNS integrated navigation system | |
Słupik et al. | Novel lightweight quaternion filter for determining orientation based on indications of gyroscope, magnetometer and accelerometer | |
Atia et al. | Enhanced Kalman filter for RISS/GPS integrated navigation using Gaussian process regression |
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 | ||
GR01 | Patent grant | ||
GR01 | Patent grant |