CN110490933A - 基于单点ransac的非线性状态空间中心差分滤波器方法 - Google Patents
基于单点ransac的非线性状态空间中心差分滤波器方法 Download PDFInfo
- Publication number
- CN110490933A CN110490933A CN201910879672.7A CN201910879672A CN110490933A CN 110490933 A CN110490933 A CN 110490933A CN 201910879672 A CN201910879672 A CN 201910879672A CN 110490933 A CN110490933 A CN 110490933A
- Authority
- CN
- China
- Prior art keywords
- point
- camera
- observation
- vector
- indicate
- 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.)
- Pending
Links
- 238000000034 method Methods 0.000 title claims abstract description 45
- 238000004422 calculation algorithm Methods 0.000 claims abstract description 104
- 239000011159 matrix material Substances 0.000 claims abstract description 73
- 238000001914 filtration Methods 0.000 claims abstract description 11
- 238000013461 design Methods 0.000 claims abstract description 9
- 239000013598 vector Substances 0.000 claims description 141
- 230000033001 locomotion Effects 0.000 claims description 63
- 230000000007 visual effect Effects 0.000 claims description 24
- 238000012360 testing method Methods 0.000 claims description 20
- 238000005070 sampling Methods 0.000 claims description 18
- 238000004364 calculation method Methods 0.000 claims description 14
- 238000011017 operating method Methods 0.000 claims description 14
- 239000000203 mixture Substances 0.000 claims description 10
- 238000013519 translation Methods 0.000 claims description 5
- 239000004568 cement Substances 0.000 claims description 4
- 230000008859 change Effects 0.000 claims description 4
- 238000002360 preparation method Methods 0.000 claims description 4
- 238000006243 chemical reaction Methods 0.000 claims description 3
- 238000012937 correction Methods 0.000 claims description 3
- 238000012804 iterative process Methods 0.000 claims description 3
- 238000005259 measurement Methods 0.000 claims description 3
- 238000010276 construction Methods 0.000 claims description 2
- 230000006870 function Effects 0.000 description 19
- 238000005457 optimization Methods 0.000 description 7
- 238000010586 diagram Methods 0.000 description 5
- 230000008569 process Effects 0.000 description 5
- 230000000694 effects Effects 0.000 description 4
- 238000011156 evaluation Methods 0.000 description 4
- 238000013507 mapping Methods 0.000 description 4
- 238000007476 Maximum Likelihood Methods 0.000 description 3
- 230000008901 benefit Effects 0.000 description 3
- 238000003384 imaging method Methods 0.000 description 3
- 230000006872 improvement Effects 0.000 description 3
- 230000001133 acceleration Effects 0.000 description 2
- 238000002474 experimental method Methods 0.000 description 2
- 230000000750 progressive effect Effects 0.000 description 2
- 244000291564 Allium cepa Species 0.000 description 1
- 206010057855 Hypotelorism of orbit Diseases 0.000 description 1
- 238000013459 approach Methods 0.000 description 1
- 230000009286 beneficial effect Effects 0.000 description 1
- 238000005094 computer simulation Methods 0.000 description 1
- 239000012141 concentrate Substances 0.000 description 1
- 238000013499 data model Methods 0.000 description 1
- 238000001514 detection method Methods 0.000 description 1
- 238000011161 development Methods 0.000 description 1
- 238000005516 engineering process Methods 0.000 description 1
- 230000001815 facial effect Effects 0.000 description 1
- 230000010365 information processing Effects 0.000 description 1
- 230000004807 localization Effects 0.000 description 1
- 238000002156 mixing Methods 0.000 description 1
- 238000012986 modification Methods 0.000 description 1
- 230000004048 modification Effects 0.000 description 1
- 230000003252 repetitive effect Effects 0.000 description 1
- 238000011160 research Methods 0.000 description 1
- 230000009897 systematic effect Effects 0.000 description 1
- 238000010998 test method Methods 0.000 description 1
- 230000009466 transformation Effects 0.000 description 1
Classifications
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C22/00—Measuring distance traversed on the ground by vehicles, persons, animals or other moving solid bodies, e.g. using odometers, using pedometers
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06F—ELECTRIC DIGITAL DATA PROCESSING
- G06F18/00—Pattern recognition
- G06F18/20—Analysing
- G06F18/22—Matching criteria, e.g. proximity measures
-
- G06T5/70—
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
- G06T7/00—Image analysis
- G06T7/70—Determining position or orientation of objects or cameras
Landscapes
- Engineering & Computer Science (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Theoretical Computer Science (AREA)
- Computer Vision & Pattern Recognition (AREA)
- Data Mining & Analysis (AREA)
- Bioinformatics & Cheminformatics (AREA)
- Artificial Intelligence (AREA)
- Life Sciences & Earth Sciences (AREA)
- Bioinformatics & Computational Biology (AREA)
- Evolutionary Biology (AREA)
- Evolutionary Computation (AREA)
- General Engineering & Computer Science (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Image Analysis (AREA)
Abstract
本发明提出了一种基于单点RANSAC的非线性状态空间中心差分滤波器方法，其步骤如下：采用相机坐标系为中心框架，设计运动相机系统状态方程和构建相机观测图像特征点观测方程；获取中心差分最优滤波算法的Sigma采样点及其权值，获取第k时刻的系统状态变量及方差矩阵，利用CDKF算法状态预测获得的系统状态变量更新数据，对内点区分为低新息内点和高新息内点集合，根据设定阈值做匹配操作，根据观测残差判断是否把高新息内点添加到内点集合中；获得系统状态变量的全部更新计算，开展CDKF算法的Kalman增益计算。本发明仅仅需要一个点对来实施RANSAC的中心差分滤波计算，计算效能获得明显改善，计算速度快，具备较好的实际应用价值。
Description
技术领域
本发明涉及机器人系统信息处理的技术领域，尤其涉及一种基于单点RANSAC的非线性状态空间中心差分滤波器方法，关于自主移动机器人即时定位与地图构建(VisualSimultaneous Localization And Mapping,VSLAM)问题的一种基于单点随机抽样一致性(Random Sample Consensus,RANSAC)的中心差分滤波器的视觉运动计算方法，可应用于无人机器人定位导航与控制。
背景技术
在计算机视觉SLAM系统中，通常相机采集的相邻视频图像帧之间有足够的重叠区域，那么可以假设相机采集的图像帧与图像帧间存在着一定的相关性，利用图像帧与帧之间的数据关联性，可通过某种方式计算出相机在世界坐标系中的位置以及场景中的地图点坐标。而视觉SLAM系统中的前端视觉里程计所要完成的功能就是要找出图像帧与帧之间数据存在着的关联性，从而计算出相机位姿矩阵，并同时计算出图像像素点对应场景中的地图点在世界坐标系中的坐标数据。一般来说有基于滤波的数据关联算法和基于特征的数据关联算法两种方法。基于滤波的数据关联算法的目标是计算相机运动状态模型，求出相机位姿矩阵和场景地图点坐标，将相机位置和相机方向记为状态矢量，相机运动方向作为控制矢量，场景地图点对应的像素点作为观测量，那么相机当前位置和当前观测的场景结构点必然是由它的初始位置、之前状态矢量和控制矢量决定的，以此迭代计算可以获得相机的运动轨迹，并且重建出场景地图，但是相机运动过程中观测到的数据存在着噪声，可以利用滤波器算法除掉噪声，从而使所计算的位姿和场景结构点数据更加精准。而基于特征的数据关联算法则是通过检测图像特征点找出图像上易于区分的点、线或者块特征，利用特征匹配算法直接找出两幅图像上的对应的特征点对应集合，再由这些对应点对集计算出相机位姿矩阵以及特征点对应的地图点坐标。
在计算机视觉问题中出现了一个用包含噪声与外点数据的模型数据估算问题，也就是在视觉里程计中相机捕获图像的特征点信息匹配问题，在特征点匹配时会出现误匹配操作，这里需要对误匹配点进行剔除操作，一般做法是用含有外点的特征点估计计算出一个符合条件的模型，保留符合该模型的特征点，并计算得到一个最佳模型，这就是随机抽取一致性(Random Sample Consensus,RANSAC)算法，近年来研究人员对其研究改进获得了很多适用于不同场景的RANSAC算法，如N-邻近点取样一致性算法(N-adjacent PointsSample Consensus,NAPSAC)，这种算法利用数据集合中一个类内点与数据集中其他类内点距离比类外点近的原理，优先选择距离较近的样本数据，从而提高算法效率，在数据集为高维并且难以获得内点情况下，NAPSAC算法优势明显，然而这种算法在估计模型中易于退化，且数据集中数据距离过近时模型估计效果很差；还有渐进一致性采样(PROgressiveSample Consensus)PROSAC算法，在采样时根据特征点匹配点对质量进行排序，提高速度，在实际应用中PROSAC算法可以节省大量计算时间，但是在场景中若有过多重复结构时，PROSAC算法不适用；还有基于混合二项式模型的GroupSAC算法，其原理是类内点更加接近，根据其相似性将数据集中的点进行分组，从具有更高类内点比例的最大聚类开始采样，这种算法提高了采样效率，其重点是寻找一种分组方式适用于当前问题，且分组方式不能消耗过多时间来保证实时性；还有人提出了一种极大似然估计的随机一致性算法(MLESAC)算法，它经由设计的极大似然函数实施极大似然估计过程获得特征点对一致性集合，适用于多视角物体的场景应用，其缺点是这种算法假定数据对直接先验概率未知。
在传统的RANSAC算法中至少需要4个匹配点对实施算法的计算任务，传统的滤波器方法一般采用扩展Kalman滤波器，但是其在线性化过程中存在着高阶截断误差，导致了扩展Kalman滤波(Extended Kalman filter,EKF)算法计算精度不能有较好改善。
发明内容
针对现有视觉SLAM系统中RANSAC算法估计效果差，计算精度较低的技术问题，本发明提出一种基于单点RANSAC的非线性状态空间中心差分滤波器方法，采用了基于非线性函数的Stirling插值多项式逼近获得的二阶中心差分滤波器方法，能够有效系统状态变量的估计效果，提高了计算效能。
为了达到上述目的，本发明的技术方案是这样实现的：一种基于单点RANSAC的非线性状态空间中心差分滤波器方法，其步骤如下：
步骤一：相机固定在运动载体上，采用相机坐标系为中心框架，根据相机状态变量设计相机运动系统的状态方程，利用运动相机观测图像序列构建相机观测图像特征点观测方程；
步骤二：在给定初始条件参数情况下，设第k-1时刻的运动相机系统状态向量的估计数据，以估计数据为中心获取中心差分滤波器中状态变量的Sigma采样点及其权值计算；针对运动相机系统的状态方程对每一个Sigma采样点开展预测计算获得第k时刻的系统状态变量的预测均值及其方差矩阵；接着利用预测更新计算获得的系统状态变量的均值和预测方差矩阵；利用第k时刻的系统状态变量的预测均值获取新的Sigma采样点并带入图像特征点的观测模型中进行观测参数更新计算，获得观测向量的预测均值、方差及观测协方差矩阵；
步骤三：检查步骤二获得的观测向量的预测均值和方差与全局模型的一致性确定单个兼容匹配特征点对，从获得的单个兼容匹配特征点对中随机选择匹配特征点测试数据，根据匹配特征点测试数据，利用CDKF算法状态预测获得系统状态向量的更新，并利用更新后的系统状态向量更新观测数据，根据设定阈值判定更新的观测数据是否为RANSAC内点数据，若获得的特征点对小于设定阈值，则认为该特征点对是内点，否则是外点，从而获得内点集；
步骤四：对低新息内点做部分CDKF更新，利用步骤三获取的内点及步骤二获得的系统状态变量的预测均值和方差开展CDKF算法的状态向量更新操作；
步骤五：对高新息内点开展部分CDKF更新计算，对每一个特征点匹配对，若高于设定阈值，利用CDKF算法获得的更新后的状态向量做出匹配操作获得观测估计值，根据观测估计值和观测特征点数据的残差信息判断是否把高新息内点添加到内点集合中；
步骤六：若高新息内点集大小大于0，利用高新息内点开展CDKF算法的更新操作计算系统状态向量的更新值，若高新息内点集大小小于0则结束该步骤；
步骤七：计算CDKF算法的Kalman滤波器的增益矩阵，计算第k时刻的运动相机系统模型状态变量的估计。
所述步骤一中根据相机运动方程获得视觉里程计的运动方程：
其中，相机离散化状态向量fv由相机的k+1时刻的位置向量姿态四元数线速度和角速度组成，分别表示第k时刻位置向量、姿态四元数、线速度和角速度，是由旋转角度增量确定的姿态四元数，运动载体的线速度和角速度构成系统输入量，Δt表示采样时间间隔；
所述相机观测图像特征点观测方程的构建方法为：假设已经标定过的相机作为观测传感器，观测图像特征点的观测模型表示为逆深度参数化模型：
其中，表示由姿态四元数表达的相机姿态旋转矩阵；函数表示由世界坐标系W中相机的方位角和俯仰角转化的单位方向向量，ρi为特征点深度。
所述步骤一中构建相机观测图像特征点观测方程后，利用相机针孔模型对相机坐标系中的特征点开展投影计算：
其中，(u0 v0)T表示相机坐标的原点，f表示相机的焦距，dx和dy表示图像上选定像素的大小；hu＝(uu vu)T为特征点的投影坐标，hx、hy、hz分别表示相机坐标系中的特征点的3D位置变量；
利用图像扭曲特性系数κ1和κ2校正获得图像观测模型：
其中，κ1和κ2表示图像扭曲特性系数，距离
所述步骤二的实现方法为：在给定机器人初始状态及方差初始条件参数情况下，设第k-1时刻的系统状态向量xk-1的估计数据已知，Pk-1是系统状态向量估计数据的估计方差矩阵，估计方差矩阵的平方根为且状态向量的概率分布满足多维高斯分布：那么以估计数据为中心，获取中心差分滤波器中机器人k-1时刻状态变量的Sigma采样点为：
Sigma采样点权值为：
其中，下标i＝2nx+1，n是系统状态维数，nx是系统状态变量维数，χ0,k-1表示Sigma采样点均值，χi,k-1表示以均值为中心的其余2nx个Sigma采样点，h是插值步长，(hSk-1)i、 和分别表示Sigma采样点的加权系数；
那么，运动相机系统状态方程写为其中，uk表示控制输入量，fk(·)表示机器人运动方程，表示系统状态变量的预测值；对每一个Sigma采样点代入系统状态方程开展预测计算：χi,k,k-1＝fi,k(χi,k-1,uk)，i＝0,1,…,2nx，其中，χi,k,k-1表示Sigma采样点预测值，从而获得第k时刻的状态变量的预测均值：
第k时刻的状态变量方差矩阵计算为：
其中，Px,k,k-1表示第k时刻的状态变量方差矩阵，χ1：n,k,k-1和χn+1:2n,k,k-1分别表示从第1个到第nx个Sigma点预测值和第nx+1个到第2nx个Sigma点预测值；
同时对观测噪声方差Rk做出平方根计算：对第k时刻的状态向量方差矩阵Px,k,k-1做出平方根计算：
通过第k时刻的状态变量的预测均值获得新的Sigma采样点的权值为：
将新的Sigma采样点带入观测函数中去，获得观测更新Sigma点：
其中，表示预测Sigma采样点，κi,k,k-1表示更新Sigma点，hi,k(·)表示第i个预测Sigma点代入逆深度观测函数；
从而获得观测均值及观测方差矩阵的计算：
观测协方差矩阵为：
其中，为观测向量的预测均值，Pz,k,k-1为观测向量的方差，Pxz,k,k-1为观测协方差矩阵。
所述步骤三的实现方法为：检查数据与全局模型的一致性确定内点集zIC＝(z1…zi…zn)T可由CDKF滤波器使用，获得单个兼容匹配特征点对；以第k时刻的观测序列预测值做单点假设、Pz,k,k-1为容差，找出容差范围内的观测值组成内点集：其中，search_IC_matches是伪码操作方法；
设RANSAC内点不确定性接近于观测噪声分布，设定内点阈值与匹配特征点对数，从获得的单个兼容匹配特征点对中随机选择匹配特征点测试数据：zi＝Select_random_match(zIC)，再根据匹配特征点测试数据利用CDKF算法状态预测获得的系统状态向量的更新数据：
更新系统状态向量执行观测更新操作：
其中，Select_random_match表示随机选择匹配的伪码操作方法，CDKF_State_update表示CDKF算法状态预测的伪码操作方法和CDKF_predict_Measurements表示CDKF算法预测更新的伪码操作方法；和分别表示系统状态变量和观测向量更新数据。
根据设定阈值的数据判定更新计算的观测数据是否为RANSAC内点数据，其判断式为：伪码Find_Matches函数是利用观测函数对内点集合zIC数据与设定的阈值th比较，若获得的特征点对小于设置阈值th，则该特征点对是内点，该内点为否则就是外点；
若内点数比例且结束迭代过程，并且更新假设的匹配特征点对数其中，size(·)表示内点数目函数，zInliers表示观测内点数据，p表示从观测量序列数据中随机选取内点的概率。
所述步骤四的实现方法为：把步骤三获取的内点设计为低新息内点集合{zInliers}开展CDKF算法的状态向量更新操作表示：其中，和Pk,k-1分别表示CDKF算法的预测均值和方差，CDKF_Update表示状态向量更新的伪码操作方法、由传统中心差分滤波算法来实施。
所述步骤五的实现方法为：对高新息内点开展部分CDKF更新计算，对于低新息内点计算后的剩余的每一个匹配特征点zj，若匹配特征点zj高于设定阈值，利用CDKF算法观测更新数据对其做出匹配操作，利用伪码公式表示为：
其中，predict&covariance表示匹配操作的伪码操作方法，对于每一个匹配特征点zj，利用低新息内点观测更新计算后的状态和观测方程计算其高斯预测，对所有的高新息观测数据进行测试，获得高新息内点集；表示第j个观测数据的观测更新方差矩阵，观测估计值与匹配特征点zj的残差为若其中，是设定的置信度参数，那么把该高新息内点添加到内点集合中去：zj,Inliers＝add_match_to_Inliers(zj,zInliers)，其中，zj,Inliers表示添加高新息内点数据后的内点集合，j表示高新息内点数目，add_match_to_Inliers表示内点添加的伪码操作方法。
所述步骤六中若高新息内点集大小是大于0的，即size(zj,Inliers)＞0，利用高新息内点根据常规的CDKF算法计算系统状态向量更新值开展CDKF算法的更新操作计算步骤，计算系统状态向量的估计方差，计算CDKF算法的Kalman滤波器增益等数据，为下一时刻的迭代计算做出准备，或者结束CDKF算法的迭代计算，利用伪码公式表示为：
所述步骤七的实现方法为：第k时刻的运动相机系统模型状态变量的估计计算步骤为：
其中，Kk表示CDKF的Kalman增益矩阵，矩阵Hk＝[H1…Hi…Hn]T表示观测方程投影到相机观测量空间的Jacobian矩阵，Rk表示观测噪声方差。
采用相机坐标系开展视觉里程计系统的状态计算：视觉里程计系统第k时刻的状态向量为：其中，为估计的地图特征点位置向量在相机坐标系中的投影，下标k表示采样时刻；且世界坐标系中状态变量相对于当前相机坐标系的状态向量为：且为位置向量，为相机姿态的四元数；线速度和角速度包括在里程计状态向量中时的系统状态向量为：
在滤波器第k时刻预测计算中，相机在k-1时刻到k时刻的运动状态变量为此时的预测状态变量为：其中，和分别表示在世界坐标系第k-1时刻的相机位置向量和图像特征点观测向量，表示在相机坐标系中的系统状态变量预测值；
相机运动状态变量为：其中，分别表示相机从第k-1时刻到第k时刻的相机平移和旋转矢量；
经过两次更新操作后，利用刚体变换原理把视觉里程计状态变量从前一时刻的相机坐标系转换到当前相机坐标系，再转换到世界坐标系中表达出来，获得世界坐标系中的视觉里程计系统状态向量的更新操作：
其中，和分别表示从第k-1时刻的第k-1帧图像到第k帧图像的相机姿态旋转矩阵和旋转四元数变量，为位置向量，和分别表示相机第k时刻和第k-1时刻的姿态的四元数；
同时地图特征点的逆深度观测和欧氏距离观测受到相机在相机坐标系中的相对运动变化量的影响，地图特征点的逆深度观测更新为：
欧氏距离观测方程更新表达式为：
其中，表示在世界坐标系中相机的第k-1时刻图像中的第i个特征点位置坐标，利用第k-1时刻图像特征点所在的方位角和俯仰角计算获得的方向向量m，和分别表示第k-1时刻图像特征点所在的方位角和俯仰角，表示地图特征点的逆深度观测更新，和分别表示第i个特征点在第k时刻和k-1时刻的欧氏距离观测更新；
此时从前一时刻k-1到当前时刻k的系统状态向量方差矩阵更新计算为：其中，表示观测函数的一阶Jaccobian矩阵，和分别表示第k时刻和k-1时刻系统状态向量方差矩阵。
本发明的有益效果：本发明采用相机坐标系为中心框架，根据相机六自由度状态变量设计运动相机系统状态方程，利用运动相机观测图像序列构建相机观测图像特征点观测方程，并对其离散化；根据给定初始条件计算第k-1时刻的系统状态变量及其方差，获取中心差分最优滤波算法的Sigma采样点及其权值，利用状态方程开展每一个Sigma采样点的非线性映射，获取第k时刻的系统状态变量及其方差矩阵预测值，那么特征点实际观测序列将具有99％置信度分布在预测高斯分布中，随机从实际观测向量序列中确定内点集合zIC＝(z1…zi…zn)T，设定内点数阈值与假设匹配特征点数目，利用CDKF算法状态预测获得的系统状态变量更新数据，判断更新计算观测数据是否为内点数据，并且对内点区分为低新息内点和高新息内点集合，首先对低新息内点开展更新计算，对高新息内点做更新计算，并且根据设定阈值做出匹配操作，根据观测残差判断是否把高新息点添加到内点集合中。经由低新息内点的部分更新计算，和高新息内点对于相关信息矩阵的拯救计算，获得系统状态变量的全部更新计算，从而开展CDKF算法的Kalman增益计算，系统状态变量估计及其方差计算。
本发明基于滤波器策略提出了一种单点RANSAC中心差分滤波方法，并将其应用于视觉SLAM系统前端的视觉里程计计算中，将其应用于自主移动机器人视觉SLAM系统单目相机运动模型状态变量最优滤波计算中，实现单目相机运动系统模型状态参数最优滤波计算，设计了一种基于单点RANSAC的中心差分滤波器开展运动相机模型的位姿计算方法，相比于常规的运动相机系统模型的多点RANSAC-EKF算法，仅仅需要一个点对来实施RANSAC的中心差分滤波计算，计算效能获得明显改善。本发明计算效能高，计算速度快，具备较好的实际应用价值。
附图说明
为了更清楚地说明本发明实施例或现有技术中的技术方案，下面将对实施例或现有技术描述中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本发明的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其他的附图。
图1为本发明的计算流程图。
图2为本发明中相机模型的计算原理图。
图3为本发明中单点RANSAC算法的计算示意图。
图4为本发明相机姿态估计误差数据的示意图。
图5为本发明在相机坐标系中相机位置估计误差数据的示意图。
图6为本发明相机运动误差估计单点RANSAC CDKF算法计算数据的数据图，其中，(a)为相机位置RMS误差数据，(b)为方位误差RMS数据，(c)为相机方位一致性误差数据，(d)为相机位姿一致性误差数据。
图7为本发明相机运动误差估计单点RANSAC BA基准测试数据的数据图，其中，(a)为相机位置RMS误差数据，(b)为方位误差RMS数据，(c)为相机方位一致性误差数据，(d)为相机位姿一致性误差数据。
具体实施方式
下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有付出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。
在计算机视觉定位中利用相机来采集环境图像信息，那么计算机视觉定位中首先设计相机成像模型，相机成像模型是一种三维世界和二维图像间的映射关系，一般采用小孔成像模型。在视觉SLAM系统中，相机固定在运动载体上，运动载体的运动位置、速度和姿态等信息反映在相机运动信息上面，这里考虑手持式相机运动，也就是相机运动具有常值角速度和线速度特征，定义相机运动状态变量为xv，它由相机的位置向量rWC，就是相机的光学中心位置，表示相机的姿态四元数qWC以及相机运动的线速度vW和角速度向量ωC组成，其中上标W表示相机所在的世界坐标系，上标C表示相机坐标系。那么假设运动载体的线加速度aW和角加速度αC，则在每一采样间隔Δt中，线速度可表示为：VW＝aWΔt；角速度增量可表示为：ΩC＝αCΔt，并且它们都满足零均值高斯白噪声分布特性，输入的线加速度和角加速度具有对角方差矩阵特性，从而可以列写出相机运动微分方程的离散化函数为：
其中，是由旋转角度增量确定的姿态四元数，fv表示相机离散化运动函数，和分别表示k时刻和k+1时刻相机的位置向量，和分别表示k时刻和k+1时刻相机的线速度，和分别k+1时刻和k时刻相机的姿态四元数，和分别表示k+1时刻和k时刻相机的角速度，表示k时刻运动载体的角速度，表示k时刻运动载体的线速度。
相机获取的应用场景图像包含着图像关键帧特征点，那么相机观测的图像特征点需要利用相机观测方程来描述，这里可以在常规的欧氏世界坐标系中描述图像特征点观测量，如图2所示，可表示为(Xi,Yi,Zi)T；也可以利用6维矢量描述图像特征点观测量，表示为yi＝(xi,yi,zi,θi,φi,ρi)T，那么一个3维特征点坐标向量可表示为：
其中，方向向量m(θi,φi)的表达式为：
m(θi,φi)＝(cosφi sinθi,-sinφi,cosφi cosθi)T， (3)
其中，θi和φi表示世界坐标系W中的相机的方位角和俯仰角，由此定义单位方向向量m(θi,φi)，沿着射线di的特征点深度定义为且其中的(xi yi zi)T表示相机在相机坐标系中第一次观测到的特征点的位置坐标，下标i表示观测到的第i个特征点。
如图2的相机运动特征参数及其观测方程结构图所示，相机观测到的每一个图像特征点都满足相机位置和相应地图点特征间的几何约束，每一个观测图像特征点在相机坐标系中沿着方向向量定义一条射线，在相机坐标系中观测图像特征点的三维位置变量定义为：hC＝(hx hy hz)T，利用欧式世界坐标XYZ表达可写为：
相应的利用逆深度表示可写为：
其中，rWC表示图像特征点从世界坐标系到相机坐标系的平移向量，表示从世界坐标系到相机坐标系的旋转矩阵，表示利用逆深度表示的图像特征点在相机坐标系中的位置向量。
应该说明的是若ρi＝0，表示无穷远处的观测点，逆深度表达式仍然是成立的。但是相机并不是直接观测场景的特征点，而是利用相机针孔模型获得特征点在图像中的投影点，利用的相机标定参数为：
这里(u0 v0)T表示相机坐标的原点，f表示相机的焦距，dx和dy表示像素的大小。
传统的EKF-SLAM系统中采用了世界参考坐标系开展视觉里程计系统状态向量计算任务，但它仅对相机传感器周围环境的局部估计是有效的，当相机远离世界坐标系原点，并且不再经历已知地图特征点时，相机状态向量以及新观测特征点中的不确定性误差会逐步增长，从而导致视觉里程计系统计算的不一致性问题，本发明采用相机坐标系开展视觉里程计系统状态计算任务，从而有效改善系统状态向量计算的不一致性问题。
首先假设视觉里程计系统第k时刻状态变量都在相机坐标系中表示，由世界坐标系定义的位置向量投影到相机坐标系为和估计的地图特征点位置向量在相机坐标系中的投影组成，下标k表示采样时刻，视觉里程计系统状态向量表示为：
相应的世界坐标系中状态变量相对于当前相机坐标系(相机固定在运动载体上，相机是运动的)的状态向量是由位置向量和表示相机姿态的四元数组成，若里程计信息不可获得，或者采用常速度模型假设时候，线速度和角速度也应该包括在里程计状态向量中去，此时的系统状态向量为：
在滤波器第k时刻预测计算中，k-1时刻的世界坐标系相对于相机坐标系的地图特征点映射依然保留，但是此时相机观测到的新的图像关键帧特征点需要添加到系统状态向量中去，新的相机运动状态变量定义为表示相机作为传感器在k-1时刻到k时刻的运动信息，此时的预测状态变量为：
其中，和分别表示在世界坐标系第k-1时刻的相机位置向量和图像特征点观测向量，表示在相机坐标系中的系统状态变量预测值，此时相机运动状态变量为：其中，分别表示相机从第k-1时刻到第k时刻的相机平移和旋转矢量。在单点中心差分滤波算法中，视觉里程计模型利用相机相对运动变量来描述相机当前位姿相对于前一时刻位姿相对变化量以及常值速度模型思想。在观测方程中把前一时刻的地图特征点投影到相机坐标系中，其中主要利用了相机的相机坐标系相对运动变化量。
另外本发明的单点RANSAC-CDKF算法迭代计算后，经过两次更新操作后，利用刚体变换原理把视觉里程计状态变量从前一时刻的相机坐标系转换到当前相机坐标系，还要再转换到世界坐标系中表达出来，获得世界坐标系中的视觉里程计系统状态向量的更新操作：
其中，和分别表示从第k-1时刻的第k-1帧图像到第k帧图像的相机姿态旋转矩阵和旋转四元数变量，为位置向量，和分别表示相机第k时刻和第k-1时刻的姿态的四元数。
同时地图特征点的逆深度观测和欧氏距离观测也会受到相机在相机坐标系中的相对运动变化量的影响，那么地图特征点的逆深度观测更新表达式为：
相应的欧氏距离观测方程更新表达式为：
其中，表示在世界坐标系中相机的第k-1时刻图像中的第i个特征点位置坐标，m表达式如前所述，这里利用第k-1时刻图像特征点所在的方位角和俯仰角计算获得的方向向量，和分别表示第k-1时刻图像特征点所在的方位角和俯仰角，表示地图特征点的逆深度观测更新，和分别表示第i个特征点在第k时刻和k-1时刻的欧氏距离观测更新。
此时从前一时刻k-1到当前时刻k的系统状态向量方差矩阵更新计算为：
其中，表示观测函数的一阶Jaccobian矩阵。和分别表示第k时刻和k-1时刻系统状态向量方差矩阵。
随机抽样一致性(RANSAC)算法是由Fishler和Bolles提出的一种数据模型鲁棒性参数估计方法，可有效实现错误数据排异，并且算法结构简单，运行高效，因此被广泛应用于计算机视觉等领域中，它是在数据假设和检验框架基础上，把观测数据划分为内点(Inlier)和外点(Outlier)两个集合，可在全局角度获取测试数据对模型的适应度，以期望获得具有最大内点集合的模型估计。首先利用随机采样手段从观测数据集合中选取一个用于模型估计的观测数据子集，并假设该观测数据子集为正确观测数据估计模型参数，然后利用所有观测数据集合来对该模型进行检验，根据观测数据样本集合中数据对模型的支持度，即适应该模型的观测数据点集大小，来确定该模型估计的正确程度，通过不断建立假设与检验循环来获得一个具有全局最优模型估计参数。但是随着观测数据集合中外点比例增加，需要迭代次数呈指数形式增长，计算成本增加很多，计算效率将会降低。实际执行RANSAC算法时，通常只要保证在抽样过程中至少一次能得到一个好的样本即可，对于观测数据集合，其中包含有一定数量满足特定模型关系的数据点，也就是内点，假设该特定模型至少需要一个大小为n的数据集进行估计，那么在确保置信度z时，至少要能获取一个均为内点的最小子集所需要的采样次数l进行迭代次数限制，假定观测数据集中内点率为w，则选择n个均为内点的样本测试子集概率为wn，则采样获取的测试点中至少有一个外点概率为1-wn，可以确定最小采样次数l为：
在计算机视觉应用中若没有先验信息情形下，至少需要5个点来确定单目视觉结构重建任务，但是本发明采用的单点RANSAC算法是在基于观测向量集合先验统计信息已知基础上，也就是n＝1的情形利用单一内点来确定相机运动估计任务，首先产生一组具有低新息的内点，单点RANSAC算法输入变量是一组独立兼容的匹配数据对，根据观测向量一步预测概率分布以0.99的置信度通过交叉搜索获得的，其中和Sk分别表示利用观测方程获得的系统状态变量预测均值和方差矩阵。这种主动搜索策略会滤除掉一些外点集，但是初始匹配对仍然会包含在交叉搜索获得兼容匹配对数据集中。根据每次的单点匹配点对集中随机匹配点对和来自于CDKF算法的状态预测数据开展系统状态向量的更新操作，获得系统状态更新值，同时计算出残差新息来实施其余匹配点对的计算任务，对于低于算法设定阈值的匹配点对认为是低新息内点对，可以支持当前假设数据。接着是高新息内点对计算，在前面步骤计算中确定为外点的残差新息都会很高，同时部分内点也会有很高的残差新息，特别是对于受到相机平移运动影响或者临近的不确定性深度估计计算的初始点，为了抛弃掉这种具有高新息的伪匹配点对，需要利用部分更新消除相关误差确定出选择真匹配内点对，对于外点则消除掉，1点RANSAC算法的计算流程示意图如图1所示。
这样所有经过第一步骤选择的低新息内点执行系统状态向量及其方差矩阵计算，对于高新息匹配点对需要计算其独立兼容性匹配，凡是具有0.99置信度的匹配点对接受为内点，其余点对认定为外点。
如图1所示，一种基于单点RANSAC的非线性状态空间中心差分滤波器方法，其步骤如下：
步骤一：相机固定在运动载体上，采用相机坐标系为中心框架，根据相机状态变量设计相机运动的系统状态方程，利用运动相机观测图像序列构建相机观测图像特征点观测方程。
根据前述式(1)的相机运动方程，相机固定在运动载体上，获得视觉里程计的运动方程：
其中，状态向量fv由相机的k+1时刻的位置向量描述姿态四元数线速度和角速度组成，分别表示第k时刻位置向量、姿态四元数、线速度和角速度，是由旋转角度增量确定的姿态四元数，运动载体的线速度和角速度构成系统输入量，Δt表示采样时间间隔。
假设已经标定过的相机作为观测传感器，其观测图像特征点的观测模型可表示为逆深度参数化模型：
其中，表示由姿态四元数表达的相机姿态旋转矩阵，姿态四元数表示相机旋转运动的参数；函数表示由世界坐标系W中相机的方位角和俯仰角转化的单位方向向量，ρi为特征点深度。那么利用相机针孔模型对相机坐标系中的特征点开展投影计算：
其中，(u0 v0)T表示相机坐标的原点，f表示相机的焦距，dx和dy表示图像上选定像素的大小。hu＝(uu vu)T、hx、hy、hz分别表示相机坐标系中的特征点的3D位置变量，再利用两参数κ1和κ2图像扭曲校正获得图像观测模型：
其中，κ1和κ2表示图像扭曲特性系数，距离
步骤二：在给定初始条件参数情况下，假设第k-1时刻的运动相机系统状态向量估计数据以估计数据为中心，获取中心差分滤波器中状态变量的Sigma采样点及其权值计算；针对运动相机系统的状态方程对每一个Sigma采样点开展预测计算获得第k时刻的系统状态变量的预测均值及其相应的方差矩阵；接着利用预测更新计算获得的系统状态变量的均值和预测方差矩阵；利用第k时刻的系统状态变量的预测均值获取新的Sigma采样点并带入图像观测模型中进行观测参数更新计算，获得观测向量的预测均值、方差及观测协方差矩阵。
在给定机器人初始状态及其方差初始条件参数情况下，假设第k-1时刻的系统状态向量xk-1的估计数据已知，Pk-1是系统状态向量估计数据的估计方差矩阵，估计方差矩阵的平方根为且状态向量的概率分布满足多维高斯分布表示为那么以估计数据为中心，获取中心差分滤波器中机器人k-1时刻状态变量的Sigma采样点为：
Sigma采样点权值定义为：
其中，公式中i＝2nx+1，n是系统状态维数，nx是系统状态变量维数，χ0,k-1表示Sigma采样点均值，χi,k-1表示以均值为中心的其余2nx个Sigma采样点，h是插值步长，(hSk-1)i、和分别表示Sigma采样点的加权系数。
那么针对运动相机系统状态方程可写为其中，uk表示控制输入量，fk(·)表示机器人运动方程，表示系统状态变量的预测值。对每一个Sigma采样点代入系统状态方程开展预测计算：
χi,k,k-1＝fi,k(χi,k-1,uk)，i＝0,1,…,2nx， (22)
其中，χi,k,k-1表示Sigma采样点预测值，从而可以获得第k时刻的状态变量的预测均值为：
第k时刻的状态变量方差矩阵可计算为：
其中，Px,k,k-1表示第k时刻的状态变量方差矩阵，χ1：n,k,k-1和χn+1:2n,k,k-1分别表示从第1个到第nx个Sigma点预测值和第nx+1个到第2nx个Sigma点预测值。
同时对观测噪声方差Rk做出平方根计算：对获得的第k时刻的状态向量方差矩阵Px,k,k-1做出平方根计算：
接着利用预测更新计算获得的系统状态变量的均值和预测方差矩阵数据。通过第k时刻的状态变量的预测均值获得新的Sigma采样点，其权值仍然如前述：
将新的Sigma采样点带入观测函数中去，获得观测更新Sigma：
其中，表示预测Sigma采样点，κi,k,k-1表示更新Sigma点，hi,k(·)表示第i个预测Sigma点代入逆深度观测函数
从而可以获得观测均值及观测方差矩阵的计算：
相应的观测协方差矩阵为：
其中，为观测向量的预测均值，Pz,k,k-1为观测向量的方差，Pxz,k,k-1为观测协方差矩阵。
步骤三：检查步骤二获得的观测向量的预测均值和方差与全局模型的一致性确定单个兼容匹配特征点对，从获得的单个兼容匹配特征点对中随机选择匹配特征点测试数据，根据匹配特征点测试数据，利用CDKF算法状态预测获得系统状态向量的更新，并利用更新后的系统状态向量更新观测数据，根据设定阈值判定更新的观测数据是否为RANSAC内点数据，若获得的特征点对小于设定阈值，则认为该特征点对是内点，否则是外点，从而获得内点集。
经过第k时刻中心差分滤波器(CDKF)预测计算，获得了系统观测向量的预测均值和方差数据，那么实际观测序列{zi}将具有99％的置信度分布在预测高斯分布中。采用主动搜索策略可以节省计算量，并且限制匹配点与CDKF算法预测状态向量的单一兼容性，但是每一个单独匹配特征点的几何兼容不能保证全局模型兼容性，因此仍然需要检查数据与全局模型的一致性来确定内点集zIC＝(z1…zi…zn)T可由CDKF滤波器使用，获得单个兼容匹配特征点对，可采用伪码语言表达为：
伪码语言search_IC_matches的操作方法是以第k时刻的观测序列预测值做单点假设，以Pz,k,k-1为容差，找出容差范围内的观测值组成内点集合。
根据观测向量预测计算均值和方差数据，假设RANSAC内点不确定性接近于观测噪声分布，设定内点阈值与匹配特征点对数，首先从式(31)获得的单个兼容匹配特征点对中随机选择匹配特征点测试数据：
zi＝Select_random_match(zIC)， (32)
再根据匹配特征点测试数据利用CDKF算法状态预测获得的系统状态向量的更新数据，简写为：
本发明仅更新系统状态向量，对其方差矩阵不做更新操作，从而更新系统状态向量执行观测更新操作：
其中，CDKF_State_update和CDKF_predict_Measurements的实现方法可参阅专著文献[1]---[丁国强.2017.非线性系统建模与滤波方法]，和分别表示系统状态变量和观测向量更新数据。从而根据设定阈值的数据判定更新计算的观测数据是否为RANSAC内点数据，其判断式为：
伪码Find_Matches函数具体实现方法是利用观测函数对内点集合zIC数据与设定的阈值th比较，若小于设置阈值th，则该内点为若获得的特征点对小于设定阈值，则认为该特征点对是内点，否则就是外点。
对此过程开展多次迭代计算，若内点数比例且结束迭代过程，并且更新假设的匹配特征点对数其中，size(·)表示内点数目函数，zInliers表示观测内点数据，p表示从观测量序列数据中随机选取内点的概率。
步骤四：对低新息内点做部分CDKF更新，利用步骤三获取的内点数据及步骤二获得的系统状态变量的预测均值和方差开展CDKF算法的状态向量更新操作。
把步骤三获取的内点数据设计为低新息内点集合{zInliers}，以及前面步骤CDKF算法预测均值和方差Pk,k-1，开展CDKF算法的状态向量更新操作，可用伪码公式表示其过程，
CDKF_Update的具体实现过程由传统中心差分滤波算法来实施，可参阅文献[1]的CDKF算法设计部分。经由低新息内点更新计算后，CDKF算法预测中的相关性误差获得修正，预测方差大大减小，会导致内点数据的不一致性，这里需要利用高新息内点来修复预测方差，增加内点数据的兼容一致性。
步骤五：对高新息内点开展部分CDKF更新计算，对每一个特征点匹配对，若高于设定阈值，利用CDKF算法获得的更新后的状态向量做出匹配操作获得观测估计值，根据观测估计值和观测特征点数据的残差信息判断是否把高新息内点添加到内点集合中。
对高新息内点开展部分CDKF更新计算，对于低新息内点计算后的剩余的每一个匹配特征点zj，若其高于设定阈值，利用CDKF算法观测更新数据对其做出匹配操作，利用伪码公式表示为：
其中，匹配操作predict&covariance的实现方法为：对于每一个匹配特征点zj，利用低新息内点观测更新计算后的状态和观测方程计算其高斯预测，对所有的高新息观测数据进行测试，获得高新息内点集，表示第j个观测数据的观测更新方差矩阵，那么观测估计值与匹配特征点zj的残差若其中，是设定的置信度参数，那么把该高新息内点添加到内点集合中去，用伪码公式表示为，
zj,Inliers＝add_match_to_Inliers(zj,zInliers)， (38)
其中，zj,Inliers表示添加高新息内点数据后的内点集合，j表示高新息内点数目。
步骤六：若高新息内点集大小大于0，利用高新息内点开展CDKF算法的更新操作计算系统状态向量的更新值，若高新息内点集大小小于0则结束该步骤。
若高新息内点集大小是大于0的，也就是size(zj,Inliers)＞0，那么就可以利用高新息内点根据常规的CDKF算法计算系统状态向量更新值开展CDKF算法的更新操作计算步骤，计算系统状态向量的估计方差，计算CDKF算法的Kalman滤波器增益等数据，为下一时刻的迭代计算做出准备，或者结束CDKF算法的迭代计算，利用伪码公式表示为：
步骤七：计算CDKF算法的Kalman滤波器的增益矩阵，计算第k时刻的运动相机系统模型状态变量的估计。
第k时刻的运动相机系统模型状态变量的估计计算步骤为：
其中，Kk表示CDKF的Kalman增益矩阵，矩阵Hk＝[H1…Hi…Hn]T表示观测方程投影到相机观测量空间的Jacobian矩阵，Rk表示观测噪声方差。
本发明采用相机坐标系为中心开展相机运动状态变量的估计计算任务，模型算法利用一组相机连续摄像获取的图像序列开展算法计算过程，选取以每秒22帧速度高速相机获得像素为1024×768的图像序列，那么可以采用Levenberg-Marquardt优化算法实施Bundle-Adjustment优化开展图像序列的位置计算，可以获得的n个相机定位数据稀疏矩阵，变量下标“BA”表示BA优化操作，由此获得图像观测序列{I1,…,In}，把第一幅图像I1作为相机坐标系第一个坐标系C1，那么相机获得的图像位姿坐标可表示为：
其中，每一幅图像的位置都是由其相对于第一幅图像的位置向量和欧拉角向量组成的，观测图像投影误差方差矩阵可表示为由投影模型式(19)的雅克比矩阵J和模型假设的观测噪声方差影响，表达式为：
在本发明的测试实验平台配置中，采用500幅图像序列，在这些图像中手工匹配74个特征点，每一幅图像中有15-30个可视化的特征点。然后输入的观测图像序列的每一幅图像的宽和高都减小4倍获得一组子采样序列图像，把EKF基准测试算法应用到这组子采样图像序列上开展对比测试研究，子采样图像序列仍然把第一幅图像作为相机第一个坐标系C1开展BA优化计算，选择和保存其计算结果为相应的图像投影误差方差可从CDKF算法每一步迭代更新中获得。
可以很明显看到的是由于图像序列的参考坐标系设置是一致的，BA优化和图像序列估计计算仅在重建规模上有差异，为了算法性能比较一致化，这里采用一个尺度因子s对BA优化图像序列做出变换操作，因此变换后的BA优化向量为fscale()函数表示尺度变换函数；其相应的方差矩阵可转换为，JScale表示尺度变换的雅克比矩阵，其具体表达式可参阅相关文献资料。
那么相对变换误差可由式(44)表示：
相对应的误差方差计算表达式为：
其中，符号和分别表示3D流形中的变量加和减操作，但是在实验测试过程中可以看到来自BA优化操作的误差方差项，比较小，因此可以忽略掉，因此BA优化操作结果可看作是真实数据来对比来自于CDKF算法滤波计算数据，也就是项的计算数据。
本发明在实验中设置相机观测图像的观测噪声参数σz＝0.5像素，其余参数可参阅关于单点EKF算法的参数设置情形，从而本发明获得的相机姿态CDKF算法计算数据如图4所示，如图5所示是相机位置估计数据图，以及如图6所示的相机运动参数误差估计数据图。图7给出了单点RANSAC BA优化算法的相机运动参数估计数据图情况。
从仿真测试数据可以看出，本发明提出的相机运动参数计算数值稳定性好，在图4和图5中上下线表示3σ误差界，其位置变量和姿态变量估计误差都在3σ误差界之内，表明本发明的数据计算稳定性较好，也可以看出本发明的计算精度达到了计算要求。
另外从图6和图7的数据对比看出，本发明的误差一致性明显优于常规的BA邻近捆绑算法的计算效能。
本发明首先设计出固定于运动载体上的视觉里程计运动模型，利用标定后的相机作为观测器设计逆深度参数化观测模型；利用CDKF算法对视觉里程计模型开展系统状态变量预测计算获得系统状态变量的预测估计和预测方差数据，由此对图像特征点观测数据，采用主动搜索策略随机选取一部分观测特征点作为内点集，实施系统状态变量的观测更新计算获得系统状态变量的估计数据，并做出单点RANSAC匹配计算，并将内点集区分为低新息内点和高新息内点；对低新息内点开展部分CDKF观测更新计算，获得系统状态变量及其方差估计，接着对高新息内点做出匹配计算，利用高新息内点对系统状态变量估计方差做出拯救计算，同时把高新息点添加到内点集合中去；最后开展CDKF算法的Kalman增益等数据的计算，获得系统状态变量及其方差估计数据。相比于传统的Bundle-Adjustment算法，本发明计算速度快，计算精度高，具有较好的应用效果。
以上所述仅为本发明的较佳实施例而已，并不用以限制本发明，凡在本发明的精神和原则之内，所作的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。
Claims (10)
1.一种基于单点RANSAC的非线性状态空间中心差分滤波器方法，其特征在于，其步骤如下：
步骤一：相机固定在运动载体上，采用相机坐标系为中心框架，根据相机状态变量设计相机运动系统的状态方程，利用运动相机观测图像序列构建相机观测图像特征点观测方程；
步骤二：在给定初始条件参数情况下，设第k-1时刻的运动相机系统状态向量的估计数据，以估计数据为中心获取中心差分滤波器中状态变量的Sigma采样点及其权值计算；针对运动相机系统的状态方程对每一个Sigma采样点开展预测计算获得第k时刻的系统状态变量的预测均值及其方差矩阵；接着利用预测更新计算获得的系统状态变量的均值和预测方差矩阵；利用第k时刻的系统状态变量的预测均值获取新的Sigma采样点并带入图像特征点的观测模型中进行观测参数更新计算，获得观测向量的预测均值、方差及观测协方差矩阵；
步骤三：检查步骤二获得的观测向量的预测均值和方差与全局模型的一致性确定单个兼容匹配特征点对，从获得的单个兼容匹配特征点对中随机选择匹配特征点测试数据，根据匹配特征点测试数据，利用CDKF算法状态预测获得系统状态向量的更新，并利用更新后的系统状态向量更新观测数据，根据设定阈值判定更新的观测数据是否为RANSAC内点数据，若获得的特征点对小于设定阈值，则认为该特征点对是内点，否则是外点，从而获得内点集；
步骤四：对低新息内点做部分CDKF更新，利用步骤三获取的内点及步骤二获得的系统状态变量的预测均值和方差开展CDKF算法的状态向量更新操作；
步骤五：对高新息内点开展部分CDKF更新计算，对每一个特征点匹配对，若高于设定阈值，利用CDKF算法获得的更新后的状态向量做出匹配操作获得观测估计值，根据观测估计值和观测特征点数据的残差信息判断是否把高新息内点添加到内点集合中；
步骤六：若高新息内点集大小大于0，利用高新息内点开展CDKF算法的更新操作计算系统状态向量的更新值，若高新息内点集大小小于0则结束该步骤；
步骤七：计算CDKF算法的Kalman滤波器的增益矩阵，计算第k时刻的运动相机系统模型状态变量的估计。
2.根据权利要求1所述的基于单点RANSAC的非线性状态空间中心差分滤波器方法，其特征在于，所述步骤一中根据相机运动方程获得视觉里程计的运动方程：
其中，相机离散化状态向量fv由相机的k+1时刻的位置向量姿态四元数线速度和角速度组成，分别表示第k时刻位置向量、姿态四元数、线速度和角速度，是由旋转角度增量确定的姿态四元数，运动载体的线速度和角速度构成系统输入量，Δt表示采样时间间隔；
所述相机观测图像特征点观测方程的构建方法为：假设已经标定过的相机作为观测传感器，观测图像特征点的观测模型表示为逆深度参数化模型：
其中，表示由姿态四元数表达的相机姿态旋转矩阵；函数m(θi W,φi W)表示由世界坐标系W中相机的方位角θi W和俯仰角φi W转化的单位方向向量，ρi为特征点深度。
3.根据权利要求2所述的基于单点RANSAC的非线性状态空间中心差分滤波器方法，其特征在于，所述步骤一中构建相机观测图像特征点观测方程后，利用相机针孔模型对相机坐标系中的特征点开展投影计算：
其中，(u0 v0)T表示相机坐标的原点，f表示相机的焦距，dx和dy表示图像上选定像素的大小；hu＝(uu vu)T为特征点的投影坐标，hx、hy、hz分别表示相机坐标系中的特征点的3D位置变量；
利用图像扭曲特性系数κ1和κ2校正获得图像观测模型：
其中，κ1和κ2表示图像扭曲特性系数，距离
4.根据权利要求1或3所述的基于单点RANSAC的非线性状态空间中心差分滤波器方法，其特征在于，所述步骤二的实现方法为：在给定机器人初始状态及方差初始条件参数情况下，设第k-1时刻的系统状态向量xk-1的估计数据已知，Pk-1是系统状态向量估计数据的估计方差矩阵，估计方差矩阵的平方根为且状态向量的概率分布满足多维高斯分布：那么以估计数据为中心，获取中心差分滤波器中机器人k-1时刻状态变量的Sigma采样点为：
Sigma采样点权值为：
其中，下标i＝2nx+1，n是系统状态维数，nx是系统状态变量维数，χ0,k-1表示Sigma采样点均值，χi,k-1表示以均值为中心的其余2nx个Sigma采样点，h是插值步长，(hSk-1)i、 和分别表示Sigma采样点的加权系数；
那么，运动相机系统状态方程写为其中，uk表示控制输入量，fk(·)表示机器人运动方程，表示系统状态变量的预测值；对每一个Sigma采样点代入系统状态方程开展预测计算：χi,k,k-1＝fi,k(χi,k-1,uk)，i＝0,1,…,2nx，其中，χi,k,k-1表示Sigma采样点预测值，从而获得第k时刻的状态变量的预测均值：
第k时刻的状态变量方差矩阵计算为：
其中，Px,k,k-1表示第k时刻的状态变量方差矩阵，χ1：n,k,k-1和χn+1:2n,k,k-1分别表示从第1个到第nx个Sigma点预测值和第nx+1个到第2nx个Sigma点预测值；
同时对观测噪声方差Rk做出平方根计算：对第k时刻的状态向量方差矩阵Px,k,k-1做出平方根计算：
通过第k时刻的状态变量的预测均值获得新的Sigma采样点的权值为：
将新的Sigma采样点带入观测函数中去，获得观测更新Sigma点：
其中，表示预测Sigma采样点，κi,k,k-1表示更新Sigma点，hi,k(·)表示第i个预测Sigma点代入逆深度观测函数；
从而获得观测均值及观测方差矩阵的计算：
观测协方差矩阵为：
其中，为观测向量的预测均值，Pz,k,k-1为观测向量的方差，Pxz,k,k-1为观测协方差矩阵。
5.根据权利要求4所述的基于单点RANSAC的非线性状态空间中心差分滤波器方法，其特征在于，所述步骤三的实现方法为：检查数据与全局模型的一致性确定内点集zIC＝(z1…zi…zn)T可由CDKF滤波器使用，获得单个兼容匹配特征点对；以第k时刻的观测序列预测值做单点假设、Pz,k,k-1为容差，找出容差范围内的观测值组成内点集：其中，search_IC_matches是伪码操作方法；
设RANSAC内点不确定性接近于观测噪声分布，设定内点阈值与匹配特征点对数，从获得的单个兼容匹配特征点对中随机选择匹配特征点测试数据：zi＝Select_random_match(zIC)，再根据匹配特征点测试数据利用CDKF算法状态预测获得的系统状态向量的更新数据：
更新系统状态向量执行观测更新操作：
其中，Select_random_match表示随机选择匹配的伪码操作方法，CDKF_State_update表示CDKF算法状态预测的伪码操作方法和CDKF_predict_Measurements表示CDKF算法预测更新的伪码操作方法；和分别表示系统状态变量和观测向量更新数据。
根据设定阈值的数据判定更新计算的观测数据是否为RANSAC内点数据，其判断式为：伪码Find_Matches函数是利用观测函数对内点集合zIC数据与设定的阈值th比较，若获得的特征点对小于设置阈值th，则该特征点对是内点，该内点为否则就是外点；
若内点数比例且结束迭代过程，并且更新假设的匹配特征点对数其中，size(·)表示内点数目函数，zInliers表示观测内点数据，p表示从观测量序列数据中随机选取内点的概率。
6.根据权利要求5所述的基于单点RANSAC的非线性状态空间中心差分滤波器方法，其特征在于，所述步骤四的实现方法为：把步骤三获取的内点设计为低新息内点集合{zInliers}开展CDKF算法的状态向量更新操作表示：
其中，和Pk,k-1分别表示CDKF算法的预测均值和方差，CDKF_Update表示状态向量更新的伪码操作方法、由传统中心差分滤波算法来实施。
7.根据权利要求6所述的基于单点RANSAC的非线性状态空间中心差分滤波器方法，其特征在于，所述步骤五的实现方法为：对高新息内点开展部分CDKF更新计算，对于低新息内点计算后的剩余的每一个匹配特征点zj，若匹配特征点zj高于设定阈值，利用CDKF算法观测更新数据对其做出匹配操作，利用伪码公式表示为：
其中，predict&covariance表示匹配操作的伪码操作方法，对于每一个匹配特征点zj，利用低新息内点观测更新计算后的状态和观测方程计算其高斯预测，对所有的高新息观测数据进行测试，获得高新息内点集；表示第j个观测数据的观测更新方差矩阵，观测估计值与匹配特征点zj的残差为若其中，是设定的置信度参数，那么把该高新息内点添加到内点集合中去：zj,Inliers＝add_match_to_Inliers(zj,zInliers)，其中，zj,Inliers表示添加高新息内点数据后的内点集合，j表示高新息内点数目，add_match_to_Inliers表示内点添加的伪码操作方法。
8.根据权利要求7所述的基于单点RANSAC的非线性状态空间中心差分滤波器方法，其特征在于，所述步骤六中若高新息内点集大小是大于0的，即size(zj,Inliers)＞0，利用高新息内点根据常规的CDKF算法计算系统状态向量更新值开展CDKF算法的更新操作计算步骤，计算系统状态向量的估计方差，计算CDKF算法的Kalman滤波器增益等数据，为下一时刻的迭代计算做出准备，或者结束CDKF算法的迭代计算，利用伪码公式表示为：
9.根据权利要求8所述的基于单点RANSAC的非线性状态空间中心差分滤波器方法，其特征在于，所述步骤七的实现方法为：第k时刻的运动相机系统模型状态变量的估计计算步骤为：
其中，Kk表示CDKF的Kalman增益矩阵，矩阵Hk＝[H1…Hi…Hn]T表示观测方程投影到相机观测量空间的Jacobian矩阵，Rk表示观测噪声方差。
10.根据权利要求2或9所述的基于单点RANSAC的非线性状态空间中心差分滤波器方法，其特征在于，采用相机坐标系开展视觉里程计系统的状态计算：视觉里程计系统第k时刻的状态向量为：其中，为估计的地图特征点位置向量在相机坐标系中的投影，下标k表示采样时刻；且世界坐标系中状态变量相对于当前相机坐标系的状态向量为：且为位置向量，为相机姿态的四元数；线速度和角速度包括在里程计状态向量中时的系统状态向量为：
在滤波器第k时刻预测计算中，相机在k-1时刻到k时刻的运动状态变量为此时的预测状态变量为：其中，和分别表示在世界坐标系第k-1时刻的相机位置向量和图像特征点观测向量，表示在相机坐标系中的系统状态变量预测值；
相机运动状态变量为：其中，分别表示相机从第k-1时刻到第k时刻的相机平移和旋转矢量；
经过两次更新操作后，利用刚体变换原理把视觉里程计状态变量从前一时刻的相机坐标系转换到当前相机坐标系，再转换到世界坐标系中表达出来，获得世界坐标系中的视觉里程计系统状态向量的更新操作：
其中，和分别表示从第k-1时刻的第k-1帧图像到第k帧图像的相机姿态旋转矩阵和旋转四元数变量，为位置向量，和分别表示相机第k时刻和第k-1时刻的姿态的四元数；
同时地图特征点的逆深度观测和欧氏距离观测受到相机在相机坐标系中的相对运动变化量的影响，地图特征点的逆深度观测更新为：
欧氏距离观测方程更新表达式为：
其中，表示在世界坐标系中相机的第k-1时刻图像中的第i个特征点位置坐标，利用第k-1时刻图像特征点所在的方位角和俯仰角计算获得的方向向量m，和分别表示第k-1时刻图像特征点所在的方位角和俯仰角，表示地图特征点的逆深度观测更新，和分别表示第i个特征点在第k时刻和k-1时刻的欧氏距离观测更新；
此时从前一时刻k-1到当前时刻k的系统状态向量方差矩阵更新计算为：其中，表示观测函数的一阶Jaccobian矩阵，和分别表示第k时刻和k-1时刻系统状态向量方差矩阵。
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201910879672.7A CN110490933A (zh) | 2019-09-18 | 2019-09-18 | 基于单点ransac的非线性状态空间中心差分滤波器方法 |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201910879672.7A CN110490933A (zh) | 2019-09-18 | 2019-09-18 | 基于单点ransac的非线性状态空间中心差分滤波器方法 |
Publications (1)
| Publication Number | Publication Date |
|---|---|
| CN110490933A true CN110490933A (zh) | 2019-11-22 |
Family
ID=68558503
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN201910879672.7A Pending CN110490933A (zh) | 2019-09-18 | 2019-09-18 | 基于单点ransac的非线性状态空间中心差分滤波器方法 |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN110490933A (zh) |
Cited By (3)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN111325843A (zh) * | 2020-03-09 | 2020-06-23 | 北京航空航天大学 | 一种基于语义逆深度滤波的实时语义地图构建方法 |
| CN111988017A (zh) * | 2020-08-31 | 2020-11-24 | 郑州轻工业大学 | 一种基于标准偏差变尺度采样的平方根ukf计算方法 |
| CN112882053A (zh) * | 2021-01-21 | 2021-06-01 | 清华大学深圳国际研究生院 | 一种主动标定激光雷达和编码器外参的方法 |
Citations (3)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN104374405A (zh) * | 2014-11-06 | 2015-02-25 | 哈尔滨工程大学 | 一种基于自适应中心差分卡尔曼滤波的mems捷联惯导初始对准方法 |
| CN106767841A (zh) * | 2016-11-25 | 2017-05-31 | 上海航天控制技术研究所 | 基于自适应容积卡尔曼滤波和单点随机抽样的视觉导航方法 |
| CN109376785A (zh) * | 2018-10-31 | 2019-02-22 | 东南大学 | 基于迭代扩展卡尔曼滤波融合惯性与单目视觉的导航方法 |
-
2019
- 2019-09-18 CN CN201910879672.7A patent/CN110490933A/zh active Pending
Patent Citations (3)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN104374405A (zh) * | 2014-11-06 | 2015-02-25 | 哈尔滨工程大学 | 一种基于自适应中心差分卡尔曼滤波的mems捷联惯导初始对准方法 |
| CN106767841A (zh) * | 2016-11-25 | 2017-05-31 | 上海航天控制技术研究所 | 基于自适应容积卡尔曼滤波和单点随机抽样的视觉导航方法 |
| CN109376785A (zh) * | 2018-10-31 | 2019-02-22 | 东南大学 | 基于迭代扩展卡尔曼滤波融合惯性与单目视觉的导航方法 |
Non-Patent Citations (3)
| Title |
|---|
| AMIRHOSSEIN TAMJIDI ET AL.: "A pose estimation method for unmanned ground vehicles in GPS denied environments", 《PROCEEDINGS OF SPIE - THE INTERNATIONAL SOCIETY FOR OPTICAL ENGINEERING》 * |
| JAVIER CIVERA ET AL.: "1-Point RANSAC for extended Kalman filtering: Application to real-time structure from motion and visual odometry", 《JOURNAL OF FIELD ROBOTICS》 * |
| 丁国强: ""惯性导航系统传递对准技术关键问题研究", 《中国博士学位论文全文数据库 工程科技II辑》 * |
Cited By (6)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN111325843A (zh) * | 2020-03-09 | 2020-06-23 | 北京航空航天大学 | 一种基于语义逆深度滤波的实时语义地图构建方法 |
| CN111325843B (zh) * | 2020-03-09 | 2022-04-08 | 北京航空航天大学 | 一种基于语义逆深度滤波的实时语义地图构建方法 |
| CN111988017A (zh) * | 2020-08-31 | 2020-11-24 | 郑州轻工业大学 | 一种基于标准偏差变尺度采样的平方根ukf计算方法 |
| CN111988017B (zh) * | 2020-08-31 | 2023-07-25 | 郑州轻工业大学 | 一种基于标准偏差变尺度采样的平方根ukf计算方法 |
| CN112882053A (zh) * | 2021-01-21 | 2021-06-01 | 清华大学深圳国际研究生院 | 一种主动标定激光雷达和编码器外参的方法 |
| CN112882053B (zh) * | 2021-01-21 | 2023-07-18 | 清华大学深圳国际研究生院 | 一种主动标定激光雷达和编码器外参的方法 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN106679648B (zh) | 一种基于遗传算法的视觉惯性组合的slam方法 | |
| CN110009681B (zh) | 一种基于imu辅助的单目视觉里程计位姿处理方法 | |
| Jiao et al. | Robust odometry and mapping for multi-lidar systems with online extrinsic calibration | |
| CN109166149A (zh) | 一种融合双目相机与imu的定位与三维线框结构重建方法与系统 | |
| CN103649680B (zh) | 用于3d扫描的传感器定位 | |
| EP1596330B1 (en) | Estimating position and orientation of markers in digital images | |
| CN102435188B (zh) | 一种用于室内环境的单目视觉/惯性全自主导航方法 | |
| CN108986037A (zh) | 基于半直接法的单目视觉里程计定位方法及定位系统 | |
| CN107179086A (zh) | 一种基于激光雷达的制图方法、装置及系统 | |
| CN103674021B (zh) | 基于捷联惯导与星敏感器的组合导航系统及方法 | |
| CN110490933A (zh) | 基于单点ransac的非线性状态空间中心差分滤波器方法 | |
| CN106127739A (zh) | 一种结合单目视觉的rgb‑d slam方法 | |
| CN105856230A (zh) | 一种可提高机器人位姿一致性的orb关键帧闭环检测slam方法 | |
| CN110726406A (zh) | 一种改进的非线性优化单目惯导slam的方法 | |
| CN108955718A (zh) | 一种视觉里程计及其定位方法、机器人以及存储介质 | |
| Jessup et al. | Robust and efficient multirobot 3-d mapping merging with octree-based occupancy grids | |
| CN110118556A (zh) | 一种基于协方差交叉融合slam的机器人定位方法及装置 | |
| CN106373141A (zh) | 空间慢旋碎片相对运动角度和角速度的跟踪系统和跟踪方法 | |
| CN114001733B (zh) | 一种基于地图的一致性高效视觉惯性定位算法 | |
| CN107014399A (zh) | 一种星载光学相机‑激光测距仪组合系统联合检校方法 | |
| CN109443348A (zh) | 一种基于环视视觉和惯导融合的地下车库库位跟踪方法 | |
| CN108230247A (zh) | 基于云端的三维地图的生成方法、装置、设备及应用程序 | |
| CN109596121A (zh) | 一种机动站自动目标检测与空间定位方法 | |
| CN110243390A (zh) | 位姿的确定方法、装置及里程计 | |
| CN109978919A (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 | ||
| RJ01 | Rejection of invention patent application after publication | ||
| RJ01 | Rejection of invention patent application after publication |
Application publication date: 20191122 |