CN110610513B - 自主移动机器人视觉slam的不变性中心差分滤波器方法 - Google Patents

自主移动机器人视觉slam的不变性中心差分滤波器方法 Download PDF

Info

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
Application number
CN201910879673.1A
Other languages
English (en)
Other versions
CN110610513A (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.)
Zhengzhou University of Light Industry
Original Assignee
Zhengzhou University of Light Industry
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 Zhengzhou University of Light Industry filed Critical Zhengzhou University of Light Industry
Priority to CN201910879673.1A priority Critical patent/CN110610513B/zh
Publication of CN110610513A publication Critical patent/CN110610513A/zh
Application granted granted Critical
Publication of CN110610513B publication Critical patent/CN110610513B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

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的不变性中心差分滤波器方法
技术领域
本发明涉及航空航天系统信息处理的技术领域，尤其涉及一种自主移动机器人视觉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个固定路标在视觉相机观测域内可被视觉跟踪，这些固定路标构建了自主移动机器人稀疏地图，在自主移动机器人运动模型中包含的状态变量为：自主移动机器人的位置变量
Figure BDA0002205507040000021
速度向量
Figure BDA0002205507040000022
和姿态向量R∈SO(3)及IMU测量组件中陀螺仪偏差向量
Figure BDA0002205507040000023
加速度计偏差向量
Figure BDA0002205507040000024
还有在全局坐标系中固定路标的3D位置向量
Figure BDA0002205507040000031
其中，
Figure BDA0002205507040000032
和SO(3)分别表示实数域空间和李代数域空间，那么可以获得自主移动机器人VSLAM系统的动态方程为：
Figure BDA0002205507040000033
IMU偏差方程表示为：
Figure BDA0002205507040000034
路标位置方程表示为：
Figure BDA0002205507040000035
其中，
Figure BDA0002205507040000036
和
Figure BDA0002205507040000037
分别表示姿态向量R、速度向量v和位置变量x的导数，(ω-bω+wω)×表示由向量
Figure BDA0002205507040000038
组成的斜对称矩阵，
Figure BDA0002205507040000039
和
Figure BDA00022055070400000310
分别为
Figure BDA00022055070400000311
和
Figure BDA00022055070400000312
的导数，
Figure BDA00022055070400000313
为位置向量pi的导数，ω表示陀螺仪测量角速率，wω表示陀螺仪零偏，a表示加速度计测量的线加速度，wa表示加速度计静态偏置，g表示重力加速度，
Figure BDA00022055070400000314
表示陀螺仪的随机游走偏差量，
Figure BDA00022055070400000315
表示加速度计的随机游走偏差量。
陀螺仪和加速度计噪声可统一表示为系统噪声向量，即：
Figure BDA00022055070400000316
且VSLAM系统的噪声具有自相关特性，E[w(t)w(s)]＝Wδ(t-s)，上标T表示矩阵或向量的转置，t和s表示不同时刻，W表示不同时刻的噪声相关矩阵，δ(t-s)表示不同时刻的噪声Dirichlet函数，t＝s时为1，否则为0。在自主移动机器人VSALM系统中IMU测量组件测量的数据作为VSLAM系统的输入量，同时把已经标定好的单目相机测量数据作为系统观测量使用，通过构建相机的针孔模型观测和跟踪p个固定路标，其观测方程表示为：
Figure BDA00022055070400000317
这里，yi是在相机坐标系中的第i个路标的归一化像素位置，其中，
Figure BDA00022055070400000318
式(6)右边项表示在相机坐标系中从路标位置到相机的距离，RC和xC分别是已知从载体系到相机坐标系的旋转矩阵和平移向量，观测的第i个路标的像素图像噪声满足均值为0方差为N的
Figure BDA00022055070400000319
分布。
另外本发明利用矩阵李群理论开展中心差分滤波理论的新算法设计任务，首先给出矩阵李群的数学表述，矩阵李群
Figure BDA0002205507040000041
表示一个平方可逆矩阵的子集合，矩阵李群具有性质：单位矩阵I是属于矩阵李群，若任意的向量χ属于矩阵李群G，那么它的逆也属于矩阵李群G，任意的向量χ1,χ2属于矩阵李群G，那么它们的乘积也属于矩阵李群G表示为：
Figure BDA0002205507040000042
对于单位矩阵I，利用矩阵指数映射expm(·)，矩阵李群G等同于欧式空间
Figure BDA00022055070400000415
这里q＝dimG表示矩阵李群维数，对于欧式空间中的任意随机向量
Figure BDA0002205507040000043
在矩阵李群G的切空间存在伴随矩阵
Figure BDA0002205507040000044
定义矩阵李群G的指数映射函数：
Figure BDA0002205507040000045
这个指数映射函数是双映射，相应指数映射的逆映射为对数映射，可定义为，
Figure BDA0002205507040000046
Figure BDA0002205507040000047
根据李群定义开展李群空间的随机变量操作，由于矩阵李群G空间不是一个向量空间，李群空间中向量不能像常规方法来处理加性噪声项，在李群空间中定义CDKF的Sigma采样点变量χ满足均值为
Figure BDA0002205507040000048
方差为P的概率分布：
Figure BDA0002205507040000049
利用李群空间左乘来定义Sigma采样点变量为：
Figure BDA00022055070400000410
这里向量ξ表示Sigma采样点向量χ的噪声向量，满足N(0,P)欧式空间
Figure BDA00022055070400000411
中的经典高斯分布，
Figure BDA00022055070400000412
表示方差矩阵，在式(8)中
Figure BDA00022055070400000413
表示李代数左乘项；同样的右乘项操作为：
Figure BDA00022055070400000414
为了保持系统混合变量计算的一致性，本发明中采用了右乘法则开展李群矩阵变量计算。
在李群理论中利用李群变量χ表示VSLAM状态变量的Sigma采样变量，利用噪声向量ξ表示VSLAM在李代数域中的Sigma采样点误差变量，那么利用前面随机向量的左乘和右乘算子，在李群空间中可以构建两种中心差分滤波算法，针对自主移动机器人VSLAM系统构建CDKF Sigma采样点的离散动态模型系统：
χn+1＝f(χn,un,wn)， (10)
其中，χn和χn+1分别是n时刻和n+1时刻系统的Sigma采样单状态变量，un是已知的输入变量，噪声变量满足均值为0方差为Qn的
Figure BDA0002205507040000051
分布。
同时离散化观测方程：
yn＝h(χn)+nn, (11)
其中，yn表示观测变量，观测噪声满足均值为
Figure BDA00022055070400000515
方差为Nn的
Figure BDA0002205507040000052
分布特性，那么可以利用左乘操作算子设计Left-CDKF-LG算法，其状态变量满足
Figure BDA0002205507040000053
和右乘作算子设计Right-CDKF-LG算法，其状态变量满足均值为
Figure BDA0002205507040000054
方差为Pn的
Figure BDA0002205507040000055
概率分布。
另外应该可以看到的是SLAM问题具有李群的自然结构特性，可将其定义为SE1+p(3)李群，这也是不变扩展Kalman滤波算法解决传统EKF算法的一致收敛性的原因所在，在本发明提出的不变性中心差分滤波方法也要用到RI-EKF算法的这些特性，这里给出其性质：
定义矩阵李群变量为：
Figure BDA0002205507040000056
那么根据机器人视觉惯性SLAM系统状态变量可知李群以及相应李代数维数为3+3(2+p)，相应表达不确定性的误差参量定义为：
Figure BDA0002205507040000057
其中，
Figure BDA0002205507040000058
表示机器人姿态旋转变量误差，
Figure BDA0002205507040000059
表示机器人速度变量误差，
Figure BDA00022055070400000510
表示机器人位置变量误差，
Figure BDA00022055070400000511
分别表示机器人的路标位置变量误差，经由映射变换
Figure BDA00022055070400000512
映射到李代数中去获得误差变量矩阵：
Figure BDA00022055070400000513
自主移动机器人VSLAM系统状态空间模型的部分状态变量如R v x p1…pp等可以利用矩阵李群G＝SE2+p(3)中的矩阵变量χn表示，而陀螺仪和加速度计偏差变量
Figure BDA00022055070400000514
作为常规向量表示，这样VSLAM系统状态变量为(χn bn)，从而开展左乘CDKF-LG算法或者右乘CDKF-LG算法设计工作，陀螺仪和加速度计的输出数据定义为系统输入控制量u＝(ωT aT)，那么可以获得VSLAM系统的中心差分滤波器离散化模型：
VSLAM系统状态变量的不确定性偏差变量为：
Figure BDA0002205507040000061
VSLAM系统的动态方程描述为：{χn,bn}＝f(χn-1,un-bn-1,wn)， (16)
VSLAM中相机观测模型及其观测向量描述为，
Figure BDA0002205507040000062
这里下标n表示离散化采样时刻，un表示输入变量，
Figure BDA0002205507040000063
表示李代数变量，
Figure BDA0002205507040000064
和
Figure BDA0002205507040000065
分别表示陀螺仪测量噪声变量及其噪声项，χn-1和bn-1分别表示n-1时刻的李群变量和测量噪声变量，y1,…yp表示p个路标位置变量，hi(χn)表示由李群变量表示的观测函数，观测变量yn,i是由路标位置在图像坐标系中的uv方向变量
Figure BDA0002205507040000066
和
Figure BDA0002205507040000067
深度变量
Figure BDA0002205507040000068
以及观测噪声
Figure BDA0002205507040000069
表示出来的。这样整理后获得VSLAM系统的状态变量n时刻的估计均值
Figure BDA00022055070400000610
系统状态变量的不确定性偏差向量为
Figure BDA00022055070400000611
且由其定义的方差矩阵为
Figure BDA00022055070400000612
观测向量Yn包含了p个路标位置的观测，其观测噪声满足
Figure BDA00022055070400000613
利用自主移动机器人VSLAM动态系统模型和相机观测路标模型，基于李群李代数理论知识，设计出概率分布形式的多状态约束不变性中心差分滤波器模型算法，多状态约束不变性中心差分滤波器算法分为系统状态变量预测和观测更新步骤，下面给出多状态约束不变性中心差分滤波器模型算法设计实施内容。
如图1所示，一种自主移动机器人视觉SLAM的不变性中心差分滤波器方法，其步骤如下：
步骤一：根据自主移动机器人配置的IMU测量组件建立VSLAM系统状态方程，利用相机观测固定路标的观测量构建VSLAM的观测方程。
根据前述的自主移动机器人VSLAM系统的状态方程包括：
VSLAM系统的动态方程为：
{χn,bn}＝f(χn-1,un-bn-1,wn)， (18)
状态变量的不确定误差变量为：
Figure BDA00022055070400000614
相机观测p个路标位置向量的观测方程为：
Figure BDA0002205507040000071
其参数及其符号描述如前所述。
步骤二：基于矩阵李群李代数理论，对自主移动机器人3维姿态、位置和速度向量、固定路标位置向量做出李群矩阵变量设计，如前述李群变量χn表示，联合机器人配置的IMU测量组件中的陀螺仪和加速度计偏差向量组成VSLAM系统的混合状态空间变量{χn,bn}。
步骤三：根据步骤二获得自主移动机器人VSLAM系统的状态方程，假设已知第n-1时刻的系统混合变量的估计数据，把系统状态变量和系统噪声变量扩展为状态变量方差矩阵，并计算其平方根；对陀螺仪和加速度计输入变量做出无偏修正计算
Figure BDA0002205507040000072
计算系统混合状态空间变量的预测均值。
假设第n-1时刻的估计均值数据为
Figure BDA0002205507040000073
估计方差矩阵为Pn-1，对方差矩阵实施Cholesky分解操作得到矩阵：Sn-1＝chol(Pn-1)，chol()表示Cholesky分解函数，过程噪声方差为Qn-1；若第n时刻的观测数据为Yn，观测噪声方差为Nn，输入变量为un，那么根据n-1时刻的估计数据，首先计算系统IMU测量组件的无偏输入量
Figure BDA0002205507040000074
对此时系统噪声方差平方根扩展整理为：
Figure BDA0002205507040000075
其中，blkdiag{}表示系统噪声方差平方根扩展对角矩阵。
把第n-1时刻的估计均值
Figure BDA0002205507040000076
作为n时刻的系统状态变量值：
Figure BDA0002205507040000077
此时对系统状态变量的噪声项做出2J个采样点，根据系统状态变量维数确定J＝27+3p，
Figure BDA0002205507040000078
其中，h表示插值步长，其最优数值为
Figure BDA0002205507040000079
Figure BDA00022055070400000710
表示误差变量，
Figure BDA00022055070400000711
表示陀螺仪偏差变量、
Figure BDA00022055070400000712
表示系统噪声的第j个采样点，colj(·)表示采样点获得的第j列向量，前面“±”符号表示采样点矩阵中前J列取正号，后J列取负号，根据中心差分滤波理论，其中采样点的权值γ设计为：
Figure BDA0002205507040000081
把获得的2J个采样点带入系统方程中开展预测计算，获得系统状态变量的预测数据，利用第n-1时刻的系统混合变量(χn-1bn-1)的估计均值，计算第n时刻的系统状态变量预测均值为：
Figure BDA0002205507040000082
以及系统状态变量的采样点预测映射：
Figure BDA0002205507040000083
其中，
Figure BDA0002205507040000084
表示陀螺仪偏差采样值，
Figure BDA0002205507040000085
表示过程噪声采样值，
Figure BDA0002205507040000086
表示李代数变量采样值，
Figure BDA0002205507040000087
分别表示李群矩阵变量以及陀螺仪偏差预测值。根据状态变量的方差矩阵平方根，确定李群矩阵变量、陀螺仪和加速度计偏差变量以及系统噪声变量的CDKF采样点偏差量，根据系统扩展状态维数确定采样点数为J＝27+3p，根据李群矩阵变量的右乘法则，利用系统状态方程开展第n时刻的VSLAM系统状态变量采样点的预测计算，进而获得VSLAM系统混合变量的预测加权均值数据。
步骤四：根据李群矩阵变量的预测值利用对数法则获得李群矩阵变量的不确定误差变量预测数据，利用QR分解计算方法开展VSLAM系统混合变量的预测方差矩阵平方根计算。
根据计算式log{exp(ξ)}＝ξ，及式(9)右乘项操作，利用指数运算逆函数对数计算李群矩阵向量的偏差量：
Figure BDA0002205507040000088
其中，
Figure BDA0002205507040000089
表示李群矩阵向量预测值，获得的
Figure BDA00022055070400000810
表示李群矩阵偏差量的采样点数据。
从而利用式(9)、式(23)的系统状态变量预测均值和式(25)李群矩阵向量的偏差量，获得李群变量的第n时刻的预测值。首先利用CDKF求均值方法计算系统不确定性变量均值，接着利用不确定变量均值计算李群变量预测均值：
Figure BDA00022055070400000811
对于预测更新后的方差矩阵计算，通过开展QR分解操作：
Figure BDA00022055070400000812
qr()表示QR分解式，其具体QR分解表达式为：
Figure BDA0002205507040000091
从其中分解获得系统状态变量方差矩阵的平方根Sn,n-1，
Figure BDA0002205507040000092
从而经由中心差分滤波器的状态预测计算式(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,0表示观测向量的第0个Sigma采样，接着计算其余的2J′个观测向量Sigma采样点，这里J′＝15+3p，同样对观测向量及其噪声项实施状态扩展，
Figure BDA0002205507040000093
Figure BDA0002205507040000094
利用李代数右乘公式获得：
Figure BDA0002205507040000095
其中，Nn表示观测噪声方差矩阵，
Figure BDA0002205507040000096
和
Figure BDA0002205507040000097
分别表示2J′个系统不确定性变量、陀螺仪偏差向量和观测噪声向量的Sigma采样点，
Figure BDA0002205507040000098
表示2J′个李群变量，它们的前J′项取负号，后J′项取正号。
从而利用观测方程获得观测向量采样点更新计算：
Figure BDA0002205507040000099
其中，Yn,j表示2J′个观测向量更新计算的Sigma采样值。再利用qr'(·)函数计算更新的Cholesky分解因子，以及系统状态变量修正项
Figure BDA0002205507040000101
利用修正项更新系统状态变量。其中首先计算观测平均值及其加权偏差量：
Figure BDA0002205507040000102
Figure BDA0002205507040000103
并计算方向向量
Figure BDA0002205507040000104
那么相应QR分解算子矩阵及其协方差矩阵可计算为，
Figure BDA0002205507040000105
其中
Figure BDA0002205507040000106
计算相应的第n时刻的系统状态更新方差矩阵Pn'为：
Figure BDA0002205507040000107
其中，矩阵R'是观测噪声方差矩阵的Cholesky分解方块对角矩阵，其对角线元素由观测噪声方差矩阵的Cholesky分解因子的p次方组成。
步骤六：利用CDKF算法计算Kalman滤波器增益矩阵，根据第n时刻的观测数据计算系统混合状态变量的偏差修正数据，利用Baker-Campbell-Hausdorff公式对李群矩阵变量开展右乘计算及其一阶近似获得第n时刻的系统混合变量的最优估计数据。
按照常规的CDKF算法计算滤波器增益矩阵Kn：
Figure BDA0002205507040000108
计算系统不确定性变量的新息修正变量
Figure BDA0002205507040000109
为：
Figure BDA00022055070400001010
以及方差矩阵平方根Sn计算，
Figure BDA00022055070400001011
SeqcholUpdate(·)表示利用矩阵
Figure BDA00022055070400001012
的连续列向量重复开展ckolesky分解计算。另外考虑到新息变量
Figure BDA00022055070400001013
的重要性，用来修正偏差向量ξ，利用Baker-Campbell-Hausdorff算法的一阶近似表达式，修正计算公式为：
Figure BDA00022055070400001014
其中ξ+表示矩阵李群不确定变量的第n时刻的更新值，式(36)中右边的O(ξ+)表示指数函数线性化展开后的高阶项，M表示左Jacobian矩阵，那么更新矩阵李群状态变量更新计算为：
Figure BDA0002205507040000111
相应的系统状态方差矩阵可计算为，
Figure BDA0002205507040000112
若
Figure BDA0002205507040000118
非常小，左Jacobian矩阵M≈I。
具体实施例：考虑自主移动机器人VSLAM系统，自主移动机器人装备了单目相机和惯性IMU测量组件，IMU测量组件提供了自主移动机器人系统的3维姿态、速度和位置向量信息，单目相机对p个固定路标位置实施观测，获得p个观测方程，由此自主移动机器人VSLAM系统模型可表示为：
Figure BDA0002205507040000113
这里VSLAM系统状态向量由3维姿态、速度和位置向量组成的李群矩阵变量
Figure BDA0002205507040000114
与其余3维陀螺仪和加速度计偏差向量
Figure BDA0002205507040000115
组成混合状态向量；运动模型方程中的3维陀螺仪和加速度计输出量ω和a组成控制输入变量：u＝[ωT,aT]T。系统噪声为
Figure BDA0002205507040000116
噪声向量wn是高斯过程噪声，噪声向量wn～N(0,Qn)，其中，Qn表示噪声方差。
在此具体实例中采用相机观测的p个固定路标的观测方程表示为：
Figure BDA0002205507040000117
其中，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)，
且系统状态变量的不确定误差变量为：
Figure FDA0003297241660000011
获得VSLAM系统的状态变量n时刻的估计均值{χn bn}，系统状态变量的不确定性偏差向量为
Figure FDA0003297241660000012
VSLAM系统的观测方程为相机观测p个路标位置向量的观测方程为：
Figure FDA0003297241660000021
其中，{χn bn}为VSLAM系统状态变量，χn是n时刻的李群变量，bn是陀螺仪和加速度计偏差变量，un是输入变量，
Figure FDA0003297241660000022
表示右乘噪声项的李代数变量，
Figure FDA0003297241660000023
和
Figure FDA0003297241660000024
分别表示陀螺仪测量噪声变量及其噪声项，χn-1和bn-1分别表示n-1时刻的李群变量和测量噪声变量，观测向量Yn包含了p个路标位置的观测，y1,αyp表示p个路标位置变量，T表示向量的转置，hi(χn)表示由李群变量表示的观测函数，观测变量yn,i是由路标位置在图像坐标系中的uv方向变量
Figure FDA0003297241660000025
和
Figure FDA0003297241660000026
深度变量
Figure FDA0003297241660000027
以及观测噪声
Figure FDA0003297241660000028
表示出来的，Pn为n时刻的方差矩阵；ξn为n时刻的误差参量，观测噪声满足
Figure FDA0003297241660000029
分布，
Figure FDA00032972416600000210
为第i个路标位置的观测噪声；噪声变量wn满足均值为0、方差为过程噪声方差Qn的分布：
Figure FDA00032972416600000211
3.根据权利要求2所述的自主移动机器人视觉SLAM的不变性中心差分滤波器方法，其特征在于，所述步骤二中在矩阵李群李代数理论中利用李群变量χ表示VSLAM状态变量的Sigma采样变量，利用噪声向量ξ表示VSLAM在李代数域中的Sigma采样点误差变量，SLAM问题具有李群的自然结构特性，定义为SE1+p(3)李群，所述矩阵李群变量为：
Figure FDA00032972416600000212
其中，R为姿态向量，v为速度向量，x为位置变量，pi为第i个路标位置的位置向量，I为单位矩阵；状态变量R v x p1…pp利用矩阵李群G＝SE2+p(3)中的矩阵变量χn表示；
根据机器人视觉惯性SLAM系统状态变量可知李群以及相应李代数维数为3+3(2+p)，不确定性的误差参量为：
Figure FDA00032972416600000213
其中，
Figure FDA00032972416600000214
表示机器人姿态旋转变量误差，
Figure FDA00032972416600000215
表示机器人速度变量误差，
Figure FDA00032972416600000216
表示机器人位置变量误差，
Figure FDA00032972416600000217
分别表示机器人的路标位置变量误差，经由映射变换lg:ξ→lg(ξ)＝ξ^映射到李代数中去获得误差变量矩阵：
Figure FDA00032972416600000218
且
Figure FDA0003297241660000031
为李群空间的随机向量右乘计算。
4.根据权利要求3所述的自主移动机器人视觉SLAM的不变性中心差分滤波器方法，其特征在于，所述步骤三中根据状态变量的方差矩阵平方根，确定李群矩阵变量、陀螺仪和加速度计偏差变量以及系统噪声变量的CDKF采样点偏差量，根据系统扩展状态维数确定采样点数为J＝27+3p；根据李群矩阵变量的右乘法则，利用系统状态方程开展第n时刻的VSLAM系统状态变量采样点的预测计算，进而获得VSLAM系统混合变量的预测加权均值数据；所述步骤五中根据观测方程维数，确定观测更新的系统状态变量偏差采样点及其权值系数，此时采样点数J’＝15+3p，根据观测更新均值、观测更新偏差采样点确定VSLAM系统混合变量的观测更新计算，利用QR分解策略确定系统混合变量的观测更新方差矩阵平方根。
5.根据权利要求2或4所述的自主移动机器人视觉SLAM的不变性中心差分滤波器方法，其特征在于，所述步骤三中假设第n-1时刻的估计均值数据为
Figure FDA0003297241660000032
估计方差矩阵为Pn-1，对方差矩阵实施Cholesky分解操作得到矩阵：Sn-1＝chol(Pn-1)，chol()表示Cholesky分解函数；若第n时刻的观测数据为Yn，观测噪声方差为Nn，输入变量为un，那么根据n-1时刻的估计数据，计算系统IMU测量组件的无偏输入量
Figure FDA0003297241660000033
系统噪声方差平方根扩展整理为：
Figure FDA0003297241660000034
其中，blkdiag{}表示系统噪声方差平方根扩展对角矩阵，Qn-1为过程噪声方差；
把第n-1时刻的估计均值
Figure FDA0003297241660000035
作为n时刻的系统状态变量值：
Figure FDA0003297241660000036
对系统状态变量的噪声项做出2J个采样点，根据系统状态变量维数确定J＝27+3p，且：
Figure FDA0003297241660000037
其中，h表示插值步长，
Figure FDA0003297241660000038
表示误差变量，
Figure FDA0003297241660000039
表示陀螺仪偏差变量，
Figure FDA00032972416600000310
表示系统噪声的第j个采样点，colj(·)表示采样点获得的第j列向量，符号“±”表示采样点矩阵中前J列取正号，后J列取负号；
根据中心差分滤波理论，其中采样点的权值γ为：
Figure FDA00032972416600000311
把获得的2J个采样点带入状态方程中开展预测计算，获得系统状态变量的预测数据，利用第n-1时刻的系统混合变量(χn-1,bn-1)的估计均值，计算第n时刻的系统状态变量预测均值为：
Figure FDA0003297241660000041
f为状态方程的函数，以及系统状态变量的采样点预测映射：
Figure FDA0003297241660000042
其中，
Figure FDA0003297241660000043
表示陀螺仪偏差采样值，
Figure FDA0003297241660000044
表示过程噪声采样值，
Figure FDA0003297241660000045
表示李代数变量采样值，
Figure FDA0003297241660000046
分别表示李群矩阵变量以及陀螺仪偏差预测值。
6.根据权利要求5所述的自主移动机器人视觉SLAM的不变性中心差分滤波器方法，其特征在于，所述步骤四中不确定误差变量的预测数据的计算方法为：根据计算式log{exp(ξ)}＝ξ及右乘项操作，利用指数运算逆函数对数计算李群矩阵向量的偏差量的采样点数据：
Figure FDA0003297241660000047
其中，
Figure FDA0003297241660000048
表示李群矩阵向量预测值；
利用右乘项操作、系统状态变量预测均值和李群矩阵向量的偏差量的采样点数据
Figure FDA0003297241660000049
获得李群变量的第n时刻的预测值：利用CDKF求均值方法计算系统不确定性变量均值为
Figure FDA00032972416600000410
利用不确定变量均值计算李群变量预测均值
Figure FDA00032972416600000411
7.根据权利要求6所述的自主移动机器人视觉SLAM的不变性中心差分滤波器方法，其特征在于，对于预测更新后的方差矩阵，开展QR分解操作：
Figure FDA00032972416600000412
其中，qr()表示QR分解式，且QR分解式为：
Figure FDA00032972416600000413
从其中分解获得系统状态变量方差矩阵的平方根Sn,n-1，
Figure FDA00032972416600000414
从而经由中心差分滤波器的状态预测计算系统状态变量预测均值和QR分解操作来获得系统状态变量预测信息χn,n-1,bn,n-1,Sn,n-1。
8.根据权利要求7所述的自主移动机器人视觉SLAM的不变性中心差分滤波器方法，其特征在于，所述步骤五中观测更新均值的计算步骤为：融合p个路标位置的观测向量以及预测系统状态变量χn,n-1,bn,n-1,Sn,n-1，在矩阵李群空间中的观测变量均值经由观测方程映射，第0个Sigma采样点表示为：Yn,0＝Y(χn,n-1,0)；
计算其余的2J′个观测向量Sigma采样点：对观测向量及其噪声项实施状态扩展，
Figure FDA0003297241660000051
利用李代数右乘公式获得：
Figure FDA0003297241660000052
其中，Nn表示观测噪声方差矩阵，
Figure FDA0003297241660000053
Figure FDA0003297241660000054
和
Figure FDA0003297241660000055
分别表示2J′个系统不确定性变量、陀螺仪偏差向量和观测噪声向量的Sigma采样点，
Figure FDA0003297241660000056
表示2J′个李群变量、前J′项取负号、后J′项取正号；
利用观测方程获得观测向量采样点更新计算：
Figure FDA0003297241660000057
其中，Yn,j表示2J′个观测向量更新计算的Sigma采样值；
计算观测平均值及其加权偏差量：
Figure FDA0003297241660000058
Figure FDA0003297241660000059
并计算方向向量
Figure FDA00032972416600000510
那么相应QR分解算子矩阵及其协方差矩阵为：
Figure FDA00032972416600000511
其中
Figure FDA00032972416600000512
计算第n时刻的系统状态更新方差矩阵P′n为：
Figure FDA00032972416600000513
其中，矩阵R'是观测噪声方差矩阵的Cholesky分解方块对角矩阵，其对角线元素由观测噪声方差矩阵的Cholesky分解因子的p次方组成。
9.根据权利要求6-8中任意一项所述的自主移动机器人视觉SLAM的不变性中心差分滤波器方法，其特征在于，所述步骤六中CDKF算法计算滤波器增益矩阵Kn为：
Figure FDA00032972416600000514
计算系统不确定性变量的新息修正变量
Figure FDA00032972416600000515
为：
Figure FDA00032972416600000516
计算方差矩阵平方根Sn为
Figure FDA0003297241660000061
其中，SeqcholUpdate(·)表示利用矩阵
Figure FDA0003297241660000062
的连续列向量重复开展cholesky分解计算；
利用Baker-Campbell-Hausdorff算法的一阶近似表达式，修正差向量ξ的计算公式为：
Figure FDA0003297241660000063
其中，ξ+表示矩阵李群不确定变量的第n时刻的更新值，O(ξ+)表示指数函数线性化展开后的高阶项，M表示左Jacobian矩阵；
那么矩阵李群状态变量更新计算为：
Figure FDA0003297241660000064
系统状态方差矩阵计算为：
Figure FDA0003297241660000065
若
Figure FDA0003297241660000066
非常小，左Jacobian矩阵M≈I。
CN201910879673.1A 2019-09-18 2019-09-18 自主移动机器人视觉slam的不变性中心差分滤波器方法 Active CN110610513B (zh)

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)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
WO2021160098A1 (en) * 2020-02-13 2021-08-19 Guangdong Oppo Mobile Telecommunications Corp., Ltd. Error state kalman filter for visual slam by dynamically tuning measurement noise covariance
CN113155152B (zh) * 2021-03-14 2023-01-03 北京工业大学 基于李群滤波的相机与惯性传感器空间关系自标定方法

Citations (3)

* Cited by examiner, † Cited by third party
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)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US9286810B2 (en) * 2010-09-24 2016-03-15 Irobot Corporation Systems and methods for VSLAM optimization

Patent Citations (3)

* Cited by examiner, † Cited by third party
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)

* Cited by examiner, † Cited by third party
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
CN105737823B (zh) 一种基于五阶ckf的gps/sins/cns组合导航方法
CN105806363B (zh) 基于srqkf的sins/dvl水下大失准角对准方法
CN109931955B (zh) 基于状态相关李群滤波的捷联惯性导航系统初始对准方法
CN110610513B (zh) 自主移动机器人视觉slam的不变性中心差分滤波器方法
CN110160522A (zh) 一种基于稀疏特征法的视觉惯导里程计的位姿估计方法
CN114529576A (zh) 一种基于滑动窗口优化的rgbd和imu混合跟踪注册方法
van Der Laan et al. The invariant rauch-tung-striebel smoother
CN110926499B (zh) 基于李群最优估计的sins捷联惯性导航系统晃动基座自对准方法
Sanyal Optimal attitude estimation and filtering without using local coordinates part i: Uncontrolled and deterministic attitude dynamics
Liu et al. On terrain-aided navigation for unmanned aerial vehicle using B-spline neural network and extended Kalman filter
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
He et al. Tightly coupled laser-inertial pose estimation and map building based on B-spline curves
CN108871312A (zh) 一种重力梯度仪及星敏感器的联合定姿方法
CN115077517A (zh) 一种基于立体相机与imu融合的低速无人车定位方法及系统
Damerius et al. A generic inertial 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
Yao et al. Mobile Robot Localization Based on Vision and Multisensor
CN112013849A (zh) 一种水面船自主定位方法及系统

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