[发明专利]自主移动机器人视觉SLAM的不变性中心差分滤波器方法

说明书

本发明提出了一种自主移动机器人视觉SLAM的不变性中心差分滤波器方法，用以解决自主移动机器人视觉SLAM系统运动模型状态变量滤波计算度复杂度高问题。本发明设计了面向矩阵李群向量空间和不变Kalman滤波器的中心差分滤波器计算方法，滤波器状态变量由SE(3)李群向量表示的机器人位姿、速度和3D路标位置向量及加速度计与陀螺仪偏差向量组成，在矩阵李群空间中设计CDKF滤波器的Sigma采样点均值及误差量，利用单目视觉相机构造运动图像特征点的逆深度观测模型，设计CDKF中心差分滤波器的预测与更新迭代计算，开展机器人定位与地图构建计算任务。相比于常规的机器人系统模型的EKF算法，本发明计算效能高，计算速度快，具备较好的实际应用价值。

技术领域

本发明涉及航空航天系统信息处理的技术领域，尤其涉及一种自主移动机器人视觉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为自主移动机器人运动轨迹图。