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

权利要求书

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

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

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

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

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

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

2.根据权利要求1所述的一种动态环境下线拓展视觉里程计方法，其特征在于，所述步骤S1 t时刻获取机器人帧图像，并提取机器人帧图像的点特征，将点特征进行匹配计算初始位姿矩阵，t-1时刻图像根据初始位姿矩阵得到预测t时刻图像，具体包括：

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

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

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

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

S13：根据t时刻图像U_t和上一时刻图像U_t-1计算光流，t时刻预测图像U_t和上一时刻图像U_t-1计算预测光流，公式如下：

表示t-1时刻到t时刻的光流

S14：根据光流，预测光流和深度值构建残差矩阵M，公式如下：

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