动态环境下线拓展视觉里程计方法技术

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

【技术实现步骤摘要】
动态环境下线拓展视觉里程计方法
本专利技术属于移动机器人自主导航领域，特别是动态环境下线拓展视觉里程计方法。
技术介绍
视觉里程计是利用单个或多个视觉传感器，通过依靠图像序列相邻时刻图像之间的匹配关系来估计载体的位姿。视觉传感器因相比激光传感器获取的环境信息丰富、易得、成本低等优点成为机器人领域的一个重要研究方向和研究热点。然而为简化定位和建图问题，目前大部分视觉里程计方法假设环境是静态的，当动态物体在图像中占比较小可当噪声处理，而当动态物体在图像中占比较大时，此方法不适用，限制了视觉里程计的使用。如在基于特征视觉里程计中，通常使用随机抽样一致性(RANSAC)算法减少噪声和错误匹配对位姿估计的影响，然而当图像中的动态特征点数量超过静态特征点时，通过RANSAC难以将静态特征点作为噪声剔除，对位姿估计带来误差,使视觉里程计性能下降。因此如何有效区分图像中的动静态特征，成为动态场景中视觉里程计的研究重点。线特征在结构化环境中较为丰富、受环境影响小，且与特征点相比，包含更丰富的环境结构信息。因此线特征开始广泛应用于视觉里程计中。点特征和直线特征的结合对相机位姿进行估计，避免了视觉里程计在低纹理情况下特征点过少而导致的跟踪误差增大。但由于直线特征相较于特征点结构更高级，其描述符也更复杂，在匹配直线时其计算量相较于点匹配要大得多，对系统的实时性是一个挑战。同时，在动态环境下，不可避免要对动态直线特征进行识别剔除，使系统更难满足实时要求。
技术实现思路
本专利技术旨在解决以上现有技术的问题。提出了一种提高视觉里程计在动态环境下的精度和鲁棒性，同时满足实时性要求的动态环境下线拓展视觉里程计方法。本专利技术的技术方案如下：一种动态环境下线拓展视觉里程计方法，对动态物体上的动态特征点进行识别剔除，通过静态特征点的共线关系进行静态直线拓展，具体包括以下步骤：S1，t时刻获取机器人帧图像，并提取机器人帧图像的点特征，将点特征进行匹配计算初始位姿矩阵，t-1时刻图像根据初始位姿矩阵得到预测t时刻图像；S2，t时刻通过t时刻图像、t-1时刻图像和预测t时刻图像进行光流和残差值计算，对动态特征点进行剔除,得到静态特征点；S3，以S2中的获得的静态特征点，通过寻找共线关系，进行静态直线线段拓展；S4，根据拓展的直线线段矩形区域内的匹配特征点个数进行拓展直线线段匹配，并同时构建拓展直线线段匹配矩阵；S5，通过静态特征点和静态拓展直线线段进行BA优化即最小化静态特征点和静态拓展直线线段的重投影误差。进一步的，所述步骤S1获取机器人的帧图像，并提取机器人帧图像的点特征，将点特征进行匹配计算初始位姿矩阵，根据初始位姿矩阵得到预测当前帧图像，具体包括：S11：在t时刻，机器人通过图像Ut提取快速特征点提取和描述(ORB)特征点，通过特征点匹配得到初始位姿估计θ＝(R,T)，R为旋转矩阵，T为平移矩阵；S12：根据t时刻得到的初始位姿估计θt＝(R,T)和上一时刻相机观测的地图点Xt-1，得到t时刻相机观测的预测地图点将t时刻相机观测到的预测地图点投影到t时刻图像上得到t时刻预测图像其中，Π代表把地图点投影到图像上操作；T表示位移矩阵，x、y表示在t时刻图像上特征点的坐标，z表示特征点对应的深度。进一步的，所述步骤S2通过t时刻图像、t-1时刻图像和预测t时刻图像进行光流和残差值计算，对动态特征点进行剔除，具体包括：S13：根据t时刻图像Ut和上一时刻图像Ut-1计算光流，t时刻预测图像Ut和上一时刻图像Ut-1计算预测光流，公式如下：表示t-1时刻到t时刻的光流S14：根据光流，预测光流和深度值构建残差矩阵M，公式如下：S15：得到残差值d＝∑MTM，只有当特征点的残差值小于某一阈值ξ时，就可判定其为静态特征点，反之则为动态特征点。进一步的，所述S3以S2中的获得的静态特征点，通过寻找共线关系，进行静态直线线段拓展，具体包括：S21：关键帧插入时，使用直线检测算法(LSD)，计算每个像素的level-lineangle以构成一个level-line场，然后level-line场分割成若干个连通的直线矩形区域，得到当前关键帧的直线线段图像；S22：共视关键帧生成的地图点都将投影到当前关键帧直线线段图像上，同时对部分不符合要求的投影点进行剔除：S23：地图点经过上述筛选的特征点投影在LSD直线线段图像后，系统将读取特征点投影在线段图像上的灰度值，如果为0，意味着这个点没有投影到直线特征的矩形连通区域内。如果不是，则该点投影到直线特征的矩形连通区域内；S24：在得到LSD直线线段时，还得到每条直线线段的总体Level-Line角度，根据特征点的Level-line角度和投影在该直线线段连通区域的总体Level-Line角度差值进行比较，若该像素点的Level-line角度与该连通区域的总体Level-line角度差值小于某一阈值τ，则可表示该特征点在LSD直线线段上；S25：将直线线段集合Li中投影到线段矩形区域内地图点Xi小于2个的直线线段进行剔除，最后得到拓展的静态直线线段；S26：搜索了所有静态特征点的共线的关系完成静态直线拓展后，如果在第i个关键帧上的直线线段集合Li中检测到矩形区域内有两个或多个特征点的直线线段li,j，并且在第m个关键帧上的线段集合Lm中找到相应的特征点的直线lm,n，则表示这两个直线线段是匹配，生成一个共线匹配矩阵来存储共线线段的匹配关系。进一步的，所述步骤S22不符合要求的投影点进行剔除具体包括；1.投影在线段图像之外；2.当前关键帧视图射线v与地图点云平均视图方向n的夹角超过60度(v·n＜cos60°)；3.地图点到相机中心距离不在地图点云的尺度不变区间内；进一步的，所述步骤S24LSD矩形连通区域的总体Level-Line角度如式所示：sin(level-line_anglej)、cos(level-line_anglej)分别第j个像素处的Level-line角度的cos和sin值。矩形连通区域内的某像素点的Level-Line方向跟该点的梯度方向垂直，图像中某像素点的梯度向量norm如式所示；其中和分别表示在行方向和列方向的一阶偏导。进一步的，所述S5，通过静态特征点和静态拓展直线线段进行BA优化即最小化静态特征点和静态拓展直线线段的重投影误差，具体包括：S31：基于特征点的重投影误差，Xj∈R3为第j个地图点，在第i个关键帧上，这点投影在像平面上可表示为：其中K是相机内参矩阵，θi＝(Ri,ti)代表第i个关键帧的位姿，其点的重投影误差可定义为：S32：基于线段的重投影误差，给定线段L，p,q∈R3为线段上的两个随机点，对直线L进行参数化，ph，qh为p，本文档来自技高网...

【技术保护点】
1.一种动态环境下线拓展视觉里程计方法，其特征在于，对动态物体上的动态特征点进行识别剔除，通过静态特征点的共线关系进行静态直线拓展，具体包括以下步骤：/nS1，t时刻获取机器人帧图像，并提取机器人帧图像的点特征，将点特征进行匹配计算初始位姿矩阵，t-1时刻图像根据初始位姿矩阵得到预测t时刻图像；/nS2，t时刻通过t时刻图像、t-1时刻图像和预测t时刻图像进行光流和残差值计算，对动态特征点进行剔除,得到静态特征点；/nS3，以S2中的获得的静态特征点，通过寻找共线关系，进行静态直线线段拓展；/nS4，根据拓展的直线线段矩形区域内的匹配特征点个数进行拓展直线线段匹配，并同时构建拓展直线线段匹配矩阵；/nS5，通过静态特征点和静态拓展直线线段进行BA优化即最小化静态特征点和静态拓展直线线段的重投影误差。/n

【技术特征摘要】
1.一种动态环境下线拓展视觉里程计方法，其特征在于，对动态物体上的动态特征点进行识别剔除，通过静态特征点的共线关系进行静态直线拓展，具体包括以下步骤：
S1，t时刻获取机器人帧图像，并提取机器人帧图像的点特征，将点特征进行匹配计算初始位姿矩阵，t-1时刻图像根据初始位姿矩阵得到预测t时刻图像；
S2，t时刻通过t时刻图像、t-1时刻图像和预测t时刻图像进行光流和残差值计算，对动态特征点进行剔除,得到静态特征点；
S3，以S2中的获得的静态特征点，通过寻找共线关系，进行静态直线线段拓展；
S4，根据拓展的直线线段矩形区域内的匹配特征点个数进行拓展直线线段匹配，并同时构建拓展直线线段匹配矩阵；
S5，通过静态特征点和静态拓展直线线段进行BA优化即最小化静态特征点和静态拓展直线线段的重投影误差。


2.根据权利要求1所述的一种动态环境下线拓展视觉里程计方法，其特征在于，所述步骤S1获取机器人的帧图像，并提取机器人帧图像的点特征，将点特征进行匹配计算初始位姿矩阵，根据初始位姿矩阵得到预测当前帧图像，具体包括：
S11：t时刻，机器人通过图像Ut提取快速特征点提取和描述(ORB)特征点，通过特征点匹配得到初始位姿估计θ＝(R,T)，R为旋转矩阵，T为平移矩阵；
S12：根据t时刻得到的初始位姿估计θt＝(R,T)和上一时刻相机观测的地图点Xt-1，得到t时刻相机观测的预测地图点将t时刻相机观测到的预测地图点投影到t时刻图像上得到t时刻预测图像






其中，Π代表把地图点投影到图像上操作；T表示位移矩阵，x、y表示在t时刻图像上特征点的坐标，z表示特征点对应的深度。


3.根据权利要求2所述的一种动态环境下线拓展视觉里程计方法，其特征在于，所述步骤S2通过t时刻图像、t-1时刻图像和预测t时刻图像进行光流和残差值计算，对动态特征点进行剔除，具体包括：
S13：根据t时刻图像Ut和上一时刻图像Ut-1计算光流，t时刻预测图像Ut和上一时刻图像Ut-1计算预测光流，公式如下：







表示t-1时刻到t时刻的光流
S14：根据光流，预测光流和深度值构建残差矩阵M，公式如下：



S15：得到残差值d＝∑MTM，只有当特征点的残差值小于某一阈值λ时，就可判定其为静态特征点，反之则为动态特征点。


4.根据权利要求3所述的一种动态环境下线拓展视觉里程计方法，其特征在于，所述S3以S2中的获得的静态特征点，通过寻找共线关系，进行静态直线线段拓展，具体包括：
S21：关键帧插入时，使用直线检测算法(LSD)，计算每个像素的level-lineangle以构成一个level-line场，然后level-line场分割成若干个连通的直线矩形区域，得到当前关键帧的直线线段图像；(概括LSD直线检测算法)
S22：共视关键帧生成的地图点都将投影到当前关键帧直线线段图像上，同时对部分不符合要求的投影点进行剔除：
S23：地图点经过上述筛选的特征点投影在LSD直线线段图像后，系统将读取特征点投影在线段图像上的灰度值，如果为0，意味着这个点没有投影到直线特征的矩形连通区域内。如果不是，则该点投影到直线特征的矩形连通区域内；
S24：在得到LSD直线线段时，还得到每条直线线段的总体Level-Line角度，根据特征点的Level-line角度和投影在该直线线段连通区域的总体Level-Line角度差值进行比较，若该像素点的Level-line角度与该连通区域的总体Level-line角度差值小于某一阈值τ，则可表示该特征点在LSD直线线段上；
S25：将直线线段集合Li中投影到线段矩形区域内地图点Xi小于2...

【专利技术属性】
技术研发人员：胡章芳，杨勇，曾念文，
申请(专利权)人：重庆邮电大学，
类型：发明
国别省市：重庆;50