自主移动机器人视觉SLAM的不变性中心差分滤波器方法与流程

本发明涉及航空航天系统信息处理的技术领域，尤其涉及一种自主移动机器人视觉slam的不变性中心差分滤波器方法，即自主移动机器人即时定位与地图构建系统(visualsimultaneouslocalizationandmapping,vslam)问题的一种新型的视觉里程计不变性中心差分滤波器模型方法。

背景技术：

最近几年自主移动机器人的视觉即时定位与地图构建(vslam)技术获得较快发展，作为多传感系统配置的自主移动机器人系统数据融合方法有滤波器方法、梯度下降优化方法以及光束平差(bundleadjustment,ba)优化技术等，其中梯度下降优化法计算效能很好，但是相比滤波法计算量很大，滤波器法在实际应用中使用的比较多。

在机器人系统模型研究中应用最多的是位姿空间的se(3)李群结构，特别是近年来基于机器人系统状态估计和控制应用，se(3)结构体中的概率分布特性得到较好的研究分析。在slam系统中利用几何保持特性的右不变扩展kalman滤波(rightinvariantextendedkalmanfilter,riekf)算法中，把输出函数投影到相机坐标系中，riekf算法具有较好的误差一致性特性，在2d视觉惯性导航系统(visualinertialnavigationsystem,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个路标的归一化像素位置，其中，

式(6)右边项表示在相机坐标系中从路标位置到相机的距离，rc和xc分别是已知从载体系到相机坐标系的旋转矩阵和平移向量，观测的第i个路标的像素图像噪声满足均值为0方差为n的分布。

另外本发明利用矩阵李群理论开展中心差分滤波理论的新算法设计任务，首先给出矩阵李群的数学表述，矩阵李群表示一个平方可逆矩阵的子集合，矩阵李群具有性质：单位矩阵i是属于矩阵李群，若任意的向量χ属于矩阵李群g，那么它的逆也属于矩阵李群g，任意的向量χ1,χ2属于矩阵李群g，那么它们的乘积也属于矩阵李群g表示为：

对于单位矩阵i，利用矩阵指数映射expm(·)，矩阵李群g等同于欧式空间这里q＝dimg表示矩阵李群维数，对于欧式空间中的任意随机向量在矩阵李群g的切空间存在伴随矩阵定义矩阵李群g的指数映射函数：这个指数映射函数是双映射，相应指数映射的逆映射为对数映射，可定义为，根据李群定义开展李群空间的随机变量操作，由于矩阵李群g空间不是一个向量空间，李群空间中向量不能像常规方法来处理加性噪声项，在李群空间中定义cdkf的sigma采样点变量χ满足均值为方差为p的概率分布：利用李群空间左乘来定义sigma采样点变量为：

这里向量ξ表示sigma采样点向量χ的噪声向量，满足n(0,p)欧式空间中的经典高斯分布，表示方差矩阵，在式(8)中表示李代数左乘项；同样的右乘项操作为：

为了保持系统混合变量计算的一致性，本发明中采用了右乘法则开展李群矩阵变量计算。

在李群理论中利用李群变量χ表示vslam状态变量的sigma采样变量，利用噪声向量ξ表示vslam在李代数域中的sigma采样点误差变量，那么利用前面随机向量的左乘和右乘算子，在李群空间中可以构建两种中心差分滤波算法，针对自主移动机器人vslam系统构建cdkfsigma采样点的离散动态模型系统：

χn+1＝f(χn,un,wn)，(10)

其中，χn和χn+1分别是n时刻和n+1时刻系统的sigma采样单状态变量，un是已知的输入变量，噪声变量满足均值为0方差为qn的分布。

同时离散化观测方程：

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系统状态空间模型的部分状态变量如rvxp1…pp等可以利用矩阵李群g＝se2+p(3)中的矩阵变量χn表示，而陀螺仪和加速度计偏差变量作为常规向量表示，这样vslam系统状态变量为(χnbn)，从而开展左乘cdkf-lg算法或者右乘cdkf-lg算法设计工作，陀螺仪和加速度计的输出数据定义为系统输入控制量u＝(ω^ta^t)，那么可以获得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{}表示系统噪声方差平方根扩展对角矩阵。

把第n-1时刻的估计均值作为n时刻的系统状态变量值：此时对系统状态变量的噪声项做出2j个采样点，根据系统状态变量维数确定j＝27+3p，

其中，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,0表示观测向量的第0个sigma采样，接着计算其余的2j′个观测向量sigma采样点，这里j′＝15+3p，同样对观测向量及其噪声项实施状态扩展，利用李代数右乘公式获得：

其中，nn表示观测噪声方差矩阵，和分别表示2j′个系统不确定性变量、陀螺仪偏差向量和观测噪声向量的sigma采样点，表示2j′个李群变量，它们的前j′项取负号，后j′项取正号。

从而利用观测方程获得观测向量采样点更新计算：

其中，yn,j表示2j′个观测向量更新计算的sigma采样值。再利用qr'(·)函数计算更新的cholesky分解因子，以及系统状态变量修正项利用修正项更新系统状态变量。其中首先计算观测平均值及其加权偏差量：

并计算方向向量那么相应qr分解算子矩阵及其协方差矩阵可计算为，其中计算相应的第n时刻的系统状态更新方差矩阵pn'为：

其中，矩阵r'是观测噪声方差矩阵的cholesky分解方块对角矩阵，其对角线元素由观测噪声方差矩阵的cholesky分解因子的p次方组成。

步骤六：利用cdkf算法计算kalman滤波器增益矩阵，根据第n时刻的观测数据计算系统混合状态变量的偏差修正数据，利用baker-campbell-hausdorff公式对李群矩阵变量开展右乘计算及其一阶近似获得第n时刻的系统混合变量的最优估计数据。

按照常规的cdkf算法计算滤波器增益矩阵kn：

计算系统不确定性变量的新息修正变量为：

以及方差矩阵平方根sn计算，

seqcholupdate(·)表示利用矩阵的连续列向量重复开展ckolesky分解计算。另外考虑到新息变量的重要性，用来修正偏差向量ξ，利用baker-campbell-hausdorff算法的一阶近似表达式，修正计算公式为：

其中ξ⁺表示矩阵李群不确定变量的第n时刻的更新值，式(36)中右边的o(ξ⁺)表示指数函数线性化展开后的高阶项，m表示左jacobian矩阵，那么更新矩阵李群状态变量更新计算为：

相应的系统状态方差矩阵可计算为，

若非常小，左jacobian矩阵m≈i。

具体实施例：考虑自主移动机器人vslam系统，自主移动机器人装备了单目相机和惯性imu测量组件，imu测量组件提供了自主移动机器人系统的3维姿态、速度和位置向量信息，单目相机对p个固定路标位置实施观测，获得p个观测方程，由此自主移动机器人vslam系统模型可表示为：

这里vslam系统状态向量由3维姿态、速度和位置向量组成的李群矩阵变量与其余3维陀螺仪和加速度计偏差向量组成混合状态向量；运动模型方程中的3维陀螺仪和加速度计输出量ω和a组成控制输入变量：u＝[ω^t,a^t]^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次montecarlo仿真计算，获得如图2所示的自主移动机器人运行轨迹图，如图3所示的自主移动机器人运行姿态一致性误差的曲线图，如图4所示的自主移动机器人位置rmse误差的曲线图，如图5所示的自主移动机器人方位一致性误差的曲线图和图6所示的自主移动机器人vslam系统的方位rmse误差数据的曲线图。从图2本发明的r-cdkf算法获得自主移动机器人运行轨迹和实际机器人运动轨迹数据比较，很明显r-cdkf算法获得的数据接近于实际运行轨迹，并且路标位置的估计数据和实际位置是接近的，误差在可以接受的范围内；和其他两种算法相比，本发明的r-cdkf算法在机器人位置和方位标准偏差(rmse)误差方面并没有显示出计算精度优势，三种算法的计算精度基本上差别不是很大，但是在机器人位置和方位误差的一致性误差评价方面，本发明的r-cdkf算法具有很明显的计算优势。

本发明设计了一种面向矩阵李群向量空间和不变kalman滤波器的中心差分滤波器计算方法，滤波器状态变量由se(3)李群向量表示的机器人位姿、速度和3d路标位置向量、加速度计与陀螺仪偏差向量组成，利用单目视觉相机构造运动图像特征点的逆深度观测模型，设计中心差分滤波器开展机器人定位与地图构建计算任务。相比于常规的机器人系统模型的ekf算法，本发明计算效能高，计算速度快，具备较好的实际应用价值。

以上所述仅为本发明的较佳实施例而已，并不用以限制本发明，凡在本发明的精神和原则之内，所作的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。