[发明专利]融合单目视觉和编码器信息的机器人定位方法和系统

摘要

本发明涉及一种融合单目视觉和编码器数据的机器人定位方法和系统，包括基于滑动窗口法的外参数及视觉尺度因子的实时标定、基于滑动窗口法的位姿松耦合融合。其中基于滑动窗口法的外参数及视觉尺度因子的实时标定利用单目视觉和编码器输出的位姿信息构建滑动窗口位姿图，自动估计视觉传感器和编码器之间的相对位姿，并计算出单目视觉的尺度因子。同时，利用构建的滑动窗口位姿图对单目视觉定位信息和编码器定位信息进行融合，克服了编码器长时定位精度差和视觉定位鲁棒性差的问题，而基于滑动窗口位姿图的方式有效的保证了融合方法的计算量不会随时间增加，因此该算法适用于移动机器人机载嵌入式设备。

主权项

一种融合单目视觉和编码器数据的机器人定位方法，其特征在于，包括：步骤1、通过机器人具有的多个定位传感器采集编码器数据，根据该编码器数据和机器人运动模型，计算出机器人坐标系的编码位姿变化矩阵，并通过该机器人搭载的相机采集单目视觉图像，根据计算机视觉几何算法，对该单目视觉图像进行处理，计算出相机坐标系的视觉位姿变化矩阵；步骤2、根据该视觉位姿变化矩阵和该编码位姿变化矩阵，计算出该相机坐标系到该机器人坐标系之间的变换矩阵初值、单目尺度因子初值和平移向量；步骤3、以机器人在机器人坐标系中的位姿、该变换矩阵初值和该单目尺度因子初值为节点构建位姿图，并将各传感器测得的位姿变化作为该位姿图中的边约束；步骤4、当该位姿图内的节点数量超过预设的滑动窗口大小时，则将该位姿图内最旧的位姿节点及其具有的边约束去掉，并将该边约束作为先验约束加入该位姿图，同时将最新的位姿节点及其边约束加入该位姿图，采用非线性最小二乘法对当前该位姿图进行迭代求解，生成外参数矩阵、单目尺度因子和机器人位姿信息，其中用该外参数矩阵对该变换矩阵初值进行更新，用该单目尺度因子对该单目尺度因子初值进行更新，并将该机器人位姿信息作为该机器人的定位结果进行输出。