[发明专利]动态环境下线拓展视觉里程计方法

说明书

本发明请求保护一种动态环境下线拓展视觉里程计方法。具体步骤为：首先在图像序列中提取点特征计算初始位姿矩阵，根据初始位姿矩阵得到预测当前帧图像。通过相邻两帧图像和预测前帧图像进行光流和残差值计算，对动态特征点进行剔除。根据剩余静态特征点的共线关系进行静态直线拓展，构建共线匹配矩阵。通过剩余的静态特征点和静态直线特征进行BA优化，对相机位姿进行调整。在动态环境中的实验结果表明：相较于其他方法，本方法能有效降低跟踪误差，提高视觉里程计精度和鲁棒性，减少静态直线识别提取时间和静态直线匹配时间，满足实时应用的需求。

技术领域

本发明属于移动机器人自主导航领域，特别是动态环境下线拓展视觉里程计方法。

背景技术

视觉里程计是利用单个或多个视觉传感器，通过依靠图像序列相邻时刻图像之间的匹配关系来估计载体的位姿。视觉传感器因相比激光传感器获取的环境信息丰富、易得、成本低等优点成为机器人领域的一个重要研究方向和研究热点。

然而为简化定位和建图问题，目前大部分视觉里程计方法假设环境是静态的，当动态物体在图像中占比较小可当噪声处理，而当动态物体在图像中占比较大时，此方法不适用，限制了视觉里程计的使用。如在基于特征视觉里程计中，通常使用随机抽样一致性(RANSAC)算法减少噪声和错误匹配对位姿估计的影响，然而当图像中的动态特征点数量超过静态特征点时，通过RANSAC难以将静态特征点作为噪声剔除，对位姿估计带来误差,使视觉里程计性能下降。因此如何有效区分图像中的动静态特征，成为动态场景中视觉里程计的研究重点。

线特征在结构化环境中较为丰富、受环境影响小，且与特征点相比，包含更丰富的环境结构信息。因此线特征开始广泛应用于视觉里程计中。点特征和直线特征的结合对相机位姿进行估计，避免了视觉里程计在低纹理情况下特征点过少而导致的跟踪误差增大。但由于直线特征相较于特征点结构更高级，其描述符也更复杂，在匹配直线时其计算量相较于点匹配要大得多，对系统的实时性是一个挑战。同时，在动态环境下，不可避免要对动态直线特征进行识别剔除，使系统更难满足实时要求。

发明内容

本发明旨在解决以上现有技术的问题。提出了一种提高视觉里程计在动态环境下的精度和鲁棒性，同时满足实时性要求的动态环境下线拓展视觉里程计方法。本发明的技术方案如下：

一种动态环境下线拓展视觉里程计方法，对动态物体上的动态特征点进行识别剔除，通过静态特征点的共线关系进行静态直线拓展，具体包括以下步骤：

S1，t时刻获取机器人帧图像，并提取机器人帧图像的点特征，将点特征进行匹配计算初始位姿矩阵，t-1时刻图像根据初始位姿矩阵得到预测t时刻图像；

S2，t时刻通过t时刻图像、t-1时刻图像和预测t时刻图像进行光流和残差值计算，对动态特征点进行剔除,得到静态特征点；

S3，以S2中的获得的静态特征点，通过寻找共线关系，进行静态直线线段拓展；

S4，根据拓展的直线线段矩形区域内的匹配特征点个数进行拓展直线线段匹配，并同时构建拓展直线线段匹配矩阵；

S5，通过静态特征点和静态拓展直线线段进行BA优化即最小化静态特征点和静态拓展直线线段的重投影误差。

进一步的，所述步骤S1获取机器人的帧图像，并提取机器人帧图像的点特征，将点特征进行匹配计算初始位姿矩阵，根据初始位姿矩阵得到预测当前帧图像，具体包括：

S11：在t时刻，机器人通过图像U_t提取快速特征点提取和描述(ORB)特征点，通过特征点匹配得到初始位姿估计θ＝(R,T)，R为旋转矩阵，T为平移矩阵；

S12：根据t时刻得到的初始位姿估计θ_t＝(R,T)和上一时刻相机观测的地图点X_t-1，得到t时刻相机观测的预测地图点将t时刻相机观测到的预测地图点投影到t时刻图像上得到t时刻预测图像