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

摘要

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

主权项

1.一种自主移动机器人视觉SLAM的不变性中心差分滤波器方法，其特征在于，设计了一种面向矩阵李群空间和不变性Kalman滤波器的中心差分滤波器计算方法，滤波器状态变量由SE(3)李群向量表示的机器人位姿、速度和3D路标位置向量及加速度计与陀螺仪偏差向量组成，在矩阵李群空间中设计CDKF滤波器的Sigma采样点均值及其误差量，利用单目视觉相机构造运动图像特征点的逆深度观测模型，设计CDKF滤波器的预测与更新迭代计算，开展机器人定位与地图构建计算任务，其步骤如下：/n步骤一：根据自主移动机器人配置的IMU测量组件建立VSLAM系统的状态方程，利用相机观测固定路标的观测量构建VSLAM系统的观测方程；/n步骤二：基于矩阵李群李代数理论，对自主移动机器人三维位置、姿态和速度向量、固定路标位置向量做出矩阵李群变量设计，联合机器人配置的IMU测量组件中的陀螺仪和加速度计偏差向量组成VSLAM系统的混合状态空间变量；/n步骤三：根据VSLAM系统状态方程，假设已知第n-1时刻的系统混合变量的估计数据，把系统状态变量和系统噪声变量扩展为状态变量方差矩阵，并计算其平方根；对陀螺仪和加速度计输入变量做出无偏修正计算，计算VSLAM系统混合状态空间变量的预测均值；/n步骤四：根据李群矩阵变量的预测值利用对数法则获得李群矩阵变量的不确定误差变量预测数据，利用QR分解计算方法开展VSLAM系统混合变量的预测方差矩阵平方根计算；/n步骤五：将第n时刻的VSLAM系统混合变量的预测均值和第n时刻的观测数据带入相机观测模型方程，获得VSLAM系统的观测更新均值；/n步骤六：利用CDKF算法计算Kalman滤波器增益矩阵，根据第n时刻的观测数据计算系统混合状态变量的偏差修正数据，利用Baker-Campbell-Hausdorff公式对李群矩阵变量开展右乘计算及其一阶近似获得第n时刻的系统混合变量的最优估计数据。/n