1.一种机器人单目视觉鲁棒定位估计方法,其特征在于,包括:
1)利用随机样本进行机器人初始位姿估计;
将第一帧图像作为第一关键帧图像,并提取第一关键帧图像中局部不变性ORB特征点;
随机选取第二关键帧图像,并提取第二关键帧图像中局部不变性ORB特征点;采用汉明距离法,对第一关键帧图像中局部不变性ORB特征点m′i和第二关键帧图像中局部不变性ORB特征点m″i进行匹配;在匹配点对中选择一个随机样本估计机器人的初始位姿;
采用8对匹配点(m′i,m″i),基于直接线性方法,求解式(1)最小化问题,估计两关键帧间的基本矩阵,解算初始位姿R2、T2,建立极几何关系;
其中,F为3×3的基本矩阵;
对于第三关键帧图像和第三关键帧图像以后的关键帧图像,利用局部路标三维地图中的三维路标和当前关键帧图像的匹配点(Mi,m″′i)进行匹配;在匹配点对中选择一个随机样本估计机器人的初始位姿;
采用3对匹配点(Mi,m″′i),基于三点透视P3P方法和3D点到3D点的位姿估计,获取当前帧图像的机器人初始位姿Rk和Tk;其中,i=1,…,3;
2)利用匹配点重新估计机器人位姿,进行鲁棒定位估计;
利用初始位姿估计建立第一、二关键帧图像的极几何关系,以匹配点与极线之间的距离作为初始距离误差进行聚类剔除误匹配点,计算初始位姿估计的偏置距离修正初始距离误差作为该匹配点的权重依据,采用最小化误差平方和重新鲁棒估计机器人位姿,并利用三角测量原理建立匹配点的局部路标三维地图;
对于第三关键帧图像和第三关键帧图像以后的关键帧图像,利用局部路标三维地图中的三维路标和当前关键帧图像的特征点进行匹配,在匹配点对中选择一个随机样本进行初始位姿估计;利用初始位姿估计计算三维路标的重投影初始距离误差进行聚类剔除误匹配点,计算初始位姿估计的偏置距离修正初始距离误差作为该匹配点权重依据,采用优化方法重新鲁棒估计机器人位姿。
2.如权利要求1所述的机器人单目视觉鲁棒定位估计方法,其特征在于,所述随机样本,是从匹配点对中随机选择位姿估计所需最少数量的匹配点对组成的一个随机样本。
3.如权利要求1所述的机器人单目视觉鲁棒定位估计方法,其特征在于,对第一关键帧图像和第二关键帧图像的匹配点,重新估计机器人位姿,进行鲁棒定位估计,具体包括:
1)对于第一关键帧图像和第二关键帧图像,将所有匹配点(m′i,m″i)到各自关键帧图像极线之间的距离之和作为距离误差ei,如式(2)所示,并计算距离误差ei的标准差σ;
2)采用基于密度的聚类算法对距离误差ei进行密度聚类,剔除误差距离超过3σ的匹配数据,给出距离误差偏置e0,获取正确匹配数据;
3)根据式(3)获得正确匹配数据与距离误差偏置比较后的修正距离误差e′,并将修正距离误差e′作为正确匹配数据权重wi的依据,如式(4)所示;
e′i=ei-e0 (3)
4)采用修正距离误差e′建立正确匹配数据损失函数,如式(5)所示,并通过最小化加权的损失函数重新估计机器人位姿;
4.如权利要求1所述的机器人单目视觉鲁棒定位估计方法,其特征在于,对第三关键帧图像和第三关键帧图像以后的关键帧图像匹配点,重新估计机器人位姿,进行鲁棒定位估计;具体包括:
1)通过式(6)确定n个匹配点(Mi,m″′i)的三维路标的重投影距离误差εi,并计算距离误差εi的标准差;其中,i=1,…,n;
其中,si为Mi到相机的物距;K为相机的内参数矩阵;
2)采用基于密度的聚类算法对距离误差进行密度聚类,剔除误差距离超过的匹配数据,给出距离误差偏置,获取正确的配数据;
3)获取匹配数据与距离误差偏置比较后的修正距离误差,并将修正距离误差作为正确匹配数据权重的依据;
4)采用修正距离误差建立正确匹配数据损失函数,并通过最小化加权的损失函数重新估计机器人位姿。