[发明专利]基于单点RANSAC的非线性状态空间中心差分滤波器方法

权利要求书

1.一种基于单点RANSAC的非线性状态空间中心差分滤波器方法，其特征在于，其步骤如下：

步骤一：相机固定在运动载体上，采用相机坐标系为中心框架，根据相机状态变量设计相机运动系统的状态方程，利用运动相机观测图像序列构建相机观测图像特征点观测方程；

步骤二：在给定初始条件参数情况下，设第k-1时刻的运动相机系统状态向量的估计数据，以估计数据为中心获取中心差分滤波器中状态变量的Sigma采样点及其权值计算；针对运动相机系统的状态方程对每一个Sigma采样点开展预测计算获得第k时刻的系统状态变量的预测均值及其方差矩阵；接着利用预测更新计算获得的系统状态变量的均值和预测方差矩阵；利用第k时刻的系统状态变量的预测均值获取新的Sigma采样点并带入图像特征点的观测模型中进行观测参数更新计算，获得观测向量的预测均值、方差及观测协方差矩阵；

步骤三：检查步骤二获得的观测向量的预测均值和方差与全局模型的一致性确定单个兼容匹配特征点对，从获得的单个兼容匹配特征点对中随机选择匹配特征点测试数据，根据匹配特征点测试数据，利用CDKF算法状态预测获得系统状态向量的更新，并利用更新后的系统状态向量更新观测数据，根据设定阈值判定更新的观测数据是否为RANSAC内点数据，若获得的特征点对小于设定阈值，则认为该特征点对是内点，否则是外点，从而获得内点集；

步骤四：对低新息内点做部分CDKF更新，利用步骤三获取的内点及步骤二获得的系统状态变量的预测均值和方差开展CDKF算法的状态向量更新操作；

步骤五：对高新息内点开展部分CDKF更新计算，对每一个特征点匹配对，若高于设定阈值，利用CDKF算法获得的更新后的状态向量做出匹配操作获得观测估计值，根据观测估计值和观测特征点数据的残差信息判断是否把高新息内点添加到内点集合中；

步骤六：若高新息内点集大小大于0，利用高新息内点开展CDKF算法的更新操作计算系统状态向量的更新值，若高新息内点集大小小于0则结束该步骤；

步骤七：计算CDKF算法的Kalman滤波器的增益矩阵，计算第k时刻的运动相机系统模型状态变量的估计。

2.根据权利要求1所述的基于单点RANSAC的非线性状态空间中心差分滤波器方法，其特征在于，所述步骤一中根据相机运动方程获得视觉里程计的运动方程：

其中，相机离散化状态向量f_v由相机的k+1时刻的位置向量姿态四元数线速度和角速度组成，分别表示第k时刻位置向量、姿态四元数、线速度和角速度，是由旋转角度增量确定的姿态四元数，运动载体的线速度和角速度构成系统输入量，Δt表示采样时间间隔；

所述相机观测图像特征点观测方程的构建方法为：假设已经标定过的相机作为观测传感器，观测图像特征点的观测模型表示为逆深度参数化模型：

其中，表示由姿态四元数表达的相机姿态旋转矩阵；函数m(θ_i^W,φ_i^W)表示由世界坐标系W中相机的方位角θ_i^W和俯仰角φ_i^W转化的单位方向向量，ρ_i为特征点深度。

3.根据权利要求2所述的基于单点RANSAC的非线性状态空间中心差分滤波器方法，其特征在于，所述步骤一中构建相机观测图像特征点观测方程后，利用相机针孔模型对相机坐标系中的特征点开展投影计算：

其中，(u₀ v₀)^T表示相机坐标的原点，f表示相机的焦距，d_x和d_y表示图像上选定像素的大小；h_u＝(u_u v_u)^T为特征点的投影坐标，h_x、h_y、h_z分别表示相机坐标系中的特征点的3D位置变量；

利用图像扭曲特性系数κ₁和κ₂校正获得图像观测模型：

其中，κ₁和κ₂表示图像扭曲特性系数，距离