本发明属于视觉里程计领域的一种视觉里程计方法，具体涉及了一种面向动态环境的地面辅助的视觉里程计方法。
背景技术：
机器人和无人驾驶的技术领域包括环境感知，状态估计和规划控制等技术。由于视觉传感器的低成本和小型化的特点，视觉状态估计技术逐渐成为机器人技术中的热点问题。
视觉里程计(visualodometry,vo)是视觉状态估计的重要一环，可以分为特征点法和直接法，其作用是根据相邻两帧图像估计相对位姿。现有方法都是基于静态环境的假设，然而当动态物体出现在场景中，关联的图像特征或者像素中既包含静态环境也有动态物体。目前还没有一个成熟和统一的视觉里程计方法可以应用在动态环境下。与此同时，地面在轮式移动机器人的工作环境中是可以利用的先验信息，然而目前基于单目视觉或者深度传感器的地面检测方法均要求地面点的数量相比场景点的总数量占比充足，由于场景中存在的动态物体或者静态障碍物的遮挡，地面在图像中多为不连续且分段的，地面点的数量通常是不足的。
技术实现要素：
本发明为解决现有算法在动态环境下精度差的问题，提出了一种面向动态环境的地面辅助的视觉里程计方法。
本发明采用的技术方案是包括以下步骤：
步骤一，获取深度相机拍摄的相邻时刻的图像
将深度相机固定安装在地面移动机器人上，深度相机的光轴指向机器人正前方，通过深度相机采集到机器人正前方的前一时刻和当前时刻的灰度图像和深度图像；
步骤二，检测完整地面
在前一时刻和当前时刻的深度图像上提取所有主平面，将属于地面部分的主平面合并成完整的地面，作为地面区域；除了地面区域以外均为非地面区域。
步骤三，估计初始位姿
在前一时刻的灰度图像的地面区域提取角点，对角点采用lk光流法进行跟踪得到关联的点对，作为地面点对并归于静态角点集合，然后使用n点透视(pnp)方法进行计算得到当前时刻坐标系在前一时刻坐标系中的初始相对位姿；相机坐标系是以相机光心为原点，z轴指向相机正前方，x轴指向右侧，y轴指向下的三维坐标系。
所述的步骤三中，同一角点在前一时刻和当前时刻的灰度图像中的坐标构成一对关联的点对。
步骤四，筛选角点
根据所有地面点对的重投影误差进行拟合设定动态阈值，然后在前一时刻的灰度图像的非地面区域提取角点，对角点采用lk光流法进行跟踪得到关联的点对作为非地面点对并归于静态角点集合，然后采用步骤三得到的初始相对位姿将当前时刻的角点重投影至前一时刻的图像上求得重投影误差后根据动态阈值进行筛选；
步骤五，估计优化位姿
联合地面点对和非地面点对的静态点对，使用n点透视(pnp)方法进行计算得到优化后的当前时刻相机坐标系在前一时刻相机坐标系中的最终相对位姿；
步骤六，对于相邻两帧图像重复上述步骤二～步骤五，计算获得各个时刻的最终相对位姿，去除动态角点的干扰，由各个时刻的最终相对位姿衔接作为结果，实现了动态环境下的视觉里程计。
所述的步骤二中，检测完整地面采用以下方式进行处理：
1)提取主平面并计算地面似然参数：
根据深度图像生成点云，采用凝聚层次聚类平面检测(peac)算法提取点云中可能存在的多个主平面，得到主平面在灰度图像中对应的区域，然后根据每个主平面的质心和法线方程计算以下公式的地面似然参数err，表示为：
其中，θa表示主平面的法线和理想地面法线的锐夹角；(cu,cv)t表示主平面对应点云的质心在图像上的二维像素坐标；cols和rows分别表示灰度图像的宽度和高度；α为权重；
2)合并主平面：
选取地面似然参数最小的主平面作为种子面，遍历其余主平面中的每个点，计算每个点到种子面的垂直距离，若每个点的垂直距离均小于预设距离阈值则将该主平面合并到种子面中，直至合并完全获得的平面作为地面区域，表示为：
dp＝πs·p<tdis
其中，p表示当前判断的点，πs表示种子面的法线，tdis表示距离阈值，dp表示点p到种子面的垂直距离。
所述步骤四中，筛选角点具体为：
1)设定动态阈值：
计算所有地面点对的重投影误差，采用正态分布对所有地面点对的重投影误差进行拟合，取2倍的标准差作为动态阈值tadp，即tadp＝2σ，σ表示正态分布拟合后获得的标准差；
2)筛选：在前一时刻的灰度图像的非地面区域提取角点，并对角点采用lk光流法进行跟踪得到关联的点对，作为非地面点对，也归于静态角点集合；
然后采用步骤三得到的初始相对位姿，将后一时刻的角点重投影至前一时刻的图像上求得重投影误差，然后根据动态阈值进行筛选：若后一时刻的角点p重投影至前一时刻的图像求得的重投影误差e(p)小于等于动态阈值tadp，则将该角点p归为静态角点集合；若后一时刻的角点p重投影至前一时刻的图像求得的重投影误差e(p)大于动态阈值tadp，则不将该角点p归为静态角点集合。
所述的步骤三中，同一角点在前一时刻和当前时刻的灰度图像中的坐标构成一对关联的点对。
所述步骤四中，计算所有地面点对的重投影误差，具体是将后一时刻的角点重投影至前一时刻的灰度图像上的投影点，以投影后的投影点和角点之间的像素距离作为重投影误差。
本发明的有益效果是：
1.本发明采用的完整地面检测方法能够从深度图像对应的点云中快速检测出地面，相比于其他方法，本发明不要求地面的区域占据图片的大部分的区域，即使是地面区域像素数量比较少，本发明也可以检测成功。
2.本发明采用的动态角点检测方法，根据地面区域角点的重投影误差的概率分布设定动态阈值，然后对非地面区域的角点进行区分，相比于设定固定阈值和随机采样的外点剔除方法，对不同的传感器和运动噪声更鲁棒。
附图说明
图1为相邻两帧的灰度图像和深度图像；
图2为检测到的所有主平面；
图3为经过合并后的完整地面；
图4为所有角点的重投影误差表示图；
图5为重投影误差的频率直方图和正态分布拟合曲线图；
图6为动态角点和静态角点的区分结果图。
具体实施方式
下面结合附图对本发明的技术方案做进一步详细的说明。
本发明的具体实施例及其实施过程如下：
步骤一，获取深度相机拍摄的相邻时刻的图像。将深度相机固定安装在地面移动机器人上，光轴指向机器人正前方，采集到前一时刻和当前时刻的灰度图像和深度图像，如图1所示；
步骤二，检测完整地面。
1)提取主平面并计算地面似然参数：
根据深度图像生成点云，采用(peac)算法提取点云中可能存在的多个主平面，得到主平面在灰度图像中对应的区域，如图2所示；然后根据每个主平面的质心和法线方程计算以下公式的地面似然参数err，表示为：
其中，θa表示主平面的法线和理想地面法线的锐夹角；理想地面法线是指垂直于地面向上的单位向量。(cu,cv)t表示主平面对应点云的质心在图像上的二维像素坐标；cols和rows分别表示灰度图像的宽度和高度；α为权重；
2)合并主平面：
选取地面似然参数最小的主平面作为种子面，遍历其余主平面中的每个点，计算每个点到种子面的垂直距离，若每个点的垂直距离均小于预设距离阈值则将该主平面合并到种子面中，直至合并完全获得的平面作为地面区域，表示为：
dp＝πs·p<tdis
其中，p表示当前判断的点，πs表示种子面的法线，tdis表示距离阈值，dp表示点p到种子面的垂直距离。
如图3所示为合并后的完整地面。
步骤三，估计初始位姿。在前一时刻的灰度图像的地面区域提取角点，对角点采用lk光流法进行跟踪得到关联的点对，作为地面点对并归于静态角点集合，同一角点在前一时刻和当前时刻的灰度图像中的坐标构成一对关联的点对。然后使用n点透视(pnp)方法进行计算得到前一时刻相机坐标系在当前时刻相机坐标系中的初始相对位姿；相机坐标系是以相机光心为原点，z轴指向相机正前方，x轴指向右侧，y轴指向下的三维坐标系。
步骤四，筛选角点。根据所有地面点对的重投影误差进行拟合设定动态阈值，然后在前一时刻的灰度图像的非地面区域提取角点，对角点采用lk光流法进行跟踪得到关联的点对作为非地面点对并归于静态角点集合，然后采用步骤三得到的初始相对位姿将当前一时刻的角点重投影至前一时刻的图像上，如图4所示，白色线段表示重投影误差的大小和方向，求得重投影误差后根据动态阈值进行筛选；
1)设定动态阈值：
计算所有地面点对的重投影误差，具体是将后一时刻的角点重投影至前一时刻的灰度图像上的投影点，以投影后的投影点和角点之间的像素距离作为重投影误差；采用正态分布对所有地面点对的重投影误差进行拟合，拟合曲线如图5所示，取2倍的标准差作为动态阈值tadp，即tadp＝2σ，σ表示正态分布拟合后获得的标准差；具体实施中除了地面点对以外的静态角点被正确筛选的置信区间为95.4％。
2)筛选：在前一时刻的灰度图像的非地面区域提取角点，并对角点采用lk光流法进行跟踪得到关联的点对，作为非地面点对，也归于静态角点集合，
然后采用步骤三得到的初始相对位姿，将后一时刻的角点重投影至前一时刻的图像上求得重投影误差，以投影后的像素距离作为重投影误差，然后根据动态阈值进行筛选：
若后一时刻的角点p重投影至前一时刻的图像求得的重投影误差e(p)小于等于动态阈值tadp，则将该角点p为非地面点对；
若后一时刻的角点p重投影至前一时刻的图像求得的重投影误差e(p)大于动态阈值tadp，则将该角点p不为非地面点对。
非地面点对和非地面点对均为静态角点，其余的为动态角点。
如图6所示，非地面区域的静态角点用绿色圆圈表示，红色圆圈表示动态角点。
步骤五，估计优化位姿。联合地面点对和非地面点对的静态点对，使用n点透视(pnp)方法进行计算得到优化后的当前时刻相机坐标系在前一时刻相机坐标系中的最终相对位姿。
步骤六，对于相邻两帧图像重复上述步骤二～步骤五，计算获得各个时刻的最终相对位姿，去除动态角点的干扰，由各个时刻的最终相对位姿衔接作为结果，实现了动态环境下的视觉里程计。
由此实施可见，本发明能够在地面区域较小的情况下从深度图像对应的点云中快速检测出地面，不依赖于固定阈值对外点进行区分，去除了动态角点的干扰，更加准确地估计机器人的运动轨迹。
1.一种面向动态环境的地面辅助的视觉里程计方法，其特征在于，包括以下步骤：
步骤一，获取深度相机拍摄的相邻时刻的图像
将深度相机固定安装在地面移动机器人上，深度相机的光轴指向机器人正前方，通过深度相机采集到前一时刻和当前时刻的灰度图像和深度图像；
步骤二，检测完整地面
在前一时刻和当前时刻的深度图像上提取所有主平面，将属于地面部分的主平面合并成完整的地面，作为地面区域；
步骤三，估计初始位姿
在前一时刻的灰度图像的地面区域提取角点，对角点采用lk光流法进行跟踪得到关联的点对，作为地面点对并归于静态角点集合，然后使用n点透视方法进行计算得到当前时刻坐标系在前一时刻坐标系中的初始相对位姿；
步骤四，筛选角点
根据所有地面点对的重投影误差进行拟合设定动态阈值，然后在前一时刻的灰度图像的非地面区域提取角点，对角点采用lk光流法进行跟踪得到关联的点对作为非地面点对并归于静态角点集合，然后采用步骤三得到的初始相对位姿将当前时刻的角点重投影至前一时刻的图像上求得重投影误差后根据动态阈值进行筛选；
步骤五，估计优化位姿
联合地面点对和非地面点对的静态点对，使用n点透视方法进行计算得到优化后的当前时刻相机坐标系在前一时刻相机坐标系中的最终相对位姿；
步骤六，对于相邻两帧图像重复上述步骤二～步骤五，计算获得各个时刻的最终相对位姿，由各个时刻的最终相对位姿衔接作为结果，实现了动态环境下的视觉里程计。
2.根据权利要求1所述的一种面向动态环境的地面辅助的视觉里程计方法，其特征在于：所述的步骤二中，检测完整地面采用以下方式进行处理：
1)提取主平面并计算地面似然参数：
根据深度图像生成点云，采用凝聚层次聚类平面检测算法提取点云中可能存在的多个主平面，得到主平面在灰度图像中对应的区域，然后根据每个主平面的质心和法线方程计算以下公式的地面似然参数err，表示为：
其中，θa表示主平面的法线和理想地面法线的锐夹角；(cu,cv)t表示主平面对应点云的质心在图像上的二维像素坐标；cols和rows分别表示灰度图像的宽度和高度；a为权重；
2)合并主平面：
选取地面似然参数最小的主平面作为种子面，遍历其余主平面中的每个点，计算每个点到种子面的垂直距离，若每个点的垂直距离均小于预设距离阈值则将该主平面合并到种子面中，直至合并完全获得的平面作为地面区域，表示为：
dp＝πs·p<tdis
其中，p表示当前判断的点，πs表示种子面的法线，tdis表示距离阈值，dp表示点p到种子面的垂直距离。
3.根据权利要求1所述的一种面向动态环境的地面辅助的视觉里程计方法，其特征在于：所述步骤四中，筛选角点具体为：
1)设定动态阈值：
计算所有地面点对的重投影误差，采用正态分布对所有地面点对的重投影误差进行拟合，取2倍的标准差作为动态阈值tadp，即tadp＝2σ，σ表示正态分布拟合后获得的标准差；
2)筛选：在前一时刻的灰度图像的非地面区域提取角点，并对角点采用lk光流法进行跟踪得到关联的点对，作为非地面点对；
然后采用步骤三得到的初始相对位姿，将后一时刻的角点重投影至前一时刻的图像上求得重投影误差，然后根据动态阈值进行筛选：
若后一时刻的角点p重投影至前一时刻的图像求得的重投影误差e(p)小于等于动态阈值tadp，则将该角点p归为静态角点集合；
若后一时刻的角点p重投影至前一时刻的图像求得的重投影误差e(p)大于动态阈值tadp，则不将该角点p归为静态角点集合。
4.根据权利要求1所述的一种面向动态环境的地面辅助的视觉里程计方法，其特征在于：所述的步骤三中，同一角点在前一时刻和当前时刻的灰度图像中的坐标构成一对关联的点对。
5.根据权利要求1所述的一种面向动态环境的地面辅助的视觉里程计方法，其特征在于：所述步骤四中，计算所有地面点对的重投影误差，具体是将后一时刻的角点重投影至前一时刻的灰度图像上的投影点，以投影后的投影点和角点之间的像素距离作为重投影误差。
技术总结