一种基于目标检测和结构化特征的机器人定位方法

本发明涉及一种基于目标检测和结构化特征的机器人定位方法，该方法以深度相机构成的硬件平台为基础，属于机器人定位。

背景技术：

1、slam的英文全称是simultaneous localization and mapping，中文称作即时定位与地图创建，而视觉slam就是用摄像头来完成环境的感知工作。当一个自主移动的机器人处在一个未知的环境，它要通过摄像头获取的信息数据对自身以及周围环境进行一个预估，在机器人移动过程中根据位置估计和摄像头对数据的感知进行自身的定位，同时不断地建造和更新地图并且规划自身的路径。

2、rgbd相机是2010年左右新兴的一种相机，它最大的特点是可以通过红外结构光或time-of-flight原理，直接测出图像中各像素离相机的距离。因此，它比传统相机能获取更丰富的信息，也不必像单目或双目那样费时费力地计算深度。目前常用的rgbd相机包括kinect/kinect v2/realsense d435等，出于量程的限制，主要用于室内slam。

3、视觉slam可以分为基于特征的方法和直接方法。与基于特征的方法相比，直接方法依赖于光度不变性假设，因此对光照条件敏感，在剧烈运动期间鲁棒性较差。它们在失去跟踪后重新定位方面也面临困难。仅基于点特征的传统方法容易在无纹理场景中发生对齐错误，加入线条和平面特征可以提高跟踪精度。

4、基于特征的方法还可以分为非结构化方法和结构化方法。非结构化方法在没有有效约束的情况下精度有限，并且在位姿估计中容易产生累积误差，从而导致相机轨迹漂移，全局捆绑调整虽然在处理大规模地图方面效果显著，但会产生高计算成本，而环路闭合仅在机器人重新访问特定位置时起作用。

5、轨迹漂移的主要原因是旋转估计不准确。当前的基于曼哈顿世界假设的结构特征方法考虑环境结构线索，依赖于场景包含正交平面表面的假设，提取三个正交平面以估计旋转，这简化了场景布局在相机坐标系统中的表示。这种方法适用于纹理缺失但平面丰富的环境，对平面特征较少的环境存在局限性。

6、此外，当前成熟的视觉 slam方案一般都是在两个潜在的假设条件下完成的，一是机器人的工作环境为静态的，或者环境中绝大多数的物体都是静止不动的，二是机器人的工作环境是纹理充足的，或者绝大多数的相机帧中都是纹理充足的。然而现实生活中的真实环境经常会不满足这种假设，如行走在室内环境中的人、公路上奔驰的车辆，环境单一的场地，空旷的走廊等，如果在这些环境下运行目前的开源方案，系统会因为分不清到底是机器人本身在运动还是环境中的物体在运动，或者因为寻找到的特征过少而最终导致定位信息不准确、环境地图构建发生严重的偏差或者直接定位失败，因此对于复杂环境下如何提升视觉 slam 系统的鲁棒性与可靠性愈加地成为当下的研究重点。

技术实现思路

1、本发明的技术解决问题是：克服现有技术的不足，提出一种基于目标检测和结构化特征的机器人定位方法，用以解决机器人轨迹跟踪问题，实现在包括动态环境和低纹理环境的复杂环境下的定位与地图构建，提高定位的精度和稳定性。

2、本发明的技术解决方案是：

3、一种基于目标检测和结构化特征的机器人定位方法，该方法的步骤包括：

4、步骤 s1：读取视觉传感器数据，视觉传感器数据包括rgb图像和深度图像，对视觉传感器数据进行校准、去噪、畸变矫正，然后对rgb图像进行处理后得到处理后的rgb图像，对rgb图像进行处理的方法为：计算rgb图像的一维信息熵，当一维信息熵时，不进行处理，当一维信息熵时，对rgb图像进行锐化处理；为阈值，取；

5、步骤 s2：基于fast角点和brief描述子提取rgb图像的特征点，基于lsd算法和lbd描述子提取rgb图像的特征线，基于ahc算法提取rgb图像的特征面；

6、步骤 s3：基于卷积神经网络使用yolo对rgb图像进行目标检测，根据目标检测结果去除环境中的高概率动态物体，得到rgb图像去除高概率动态物体时是指去除步骤s2中提取的该高概率动态物体的特征点、特征面和特征线；

7、步骤 s4：对rgb图像进行特征匹配，得到与rgb图像相匹配的帧，采用自适应轨迹跟踪方法根据相匹配的帧对rgb图像进行粗略的相机位姿估计，得到相机位姿粗值，再根据相机位姿粗值使用ba进行精确的相机位姿估计，得到相机位姿精确值；

8、步骤 s5：基于词袋模型进行轨迹的回环检测，回环检测提供了当前数据与历史数据的关联，在跟踪算法丢失之后，利用回环检测进行重定位；

9、步骤 s6：根据估计的轨迹建立与任务要求对应的地图，包括稀疏地图和稠密地图等；

10、步骤 s7：重复步骤s1~s6，使得机器人跟踪轨迹不断趋近于基准轨迹。

11、优选的，所述步骤 s3中的高概率动态物体的检测策略为：基于ms coco预训练模型检测每帧图像中的物体。将物体分为三类：高概率的动态物体、高概率的静态物体和其他物体。其中，与高概率的动态物体相对应的区域边界用表示，与高概率的静态物体相对应的区域边界用表示。变量表示所有特征中的任意特征，其中，集合中的元素分别表示点、线和平面特征。有效特征的集合表示为，集合中的元素满足以下条件：

12、

13、其中表示特征满足边界条件，即位于区域内；否则，表示边界条件不满足，意味着位于区域外。

14、优选的，所述步骤 s4中，对rgb图像进行特征匹配时，当rgb图像为曼哈顿帧时进行结构化特征匹配，当rgb图像不是曼哈顿帧时进行非结构化特征匹配；进行曼哈顿帧的检测方法如下：

15、若在图像中存在三个两两正交的平面，则它们的法向量构成曼哈顿坐标系，图像为曼哈顿帧；

16、若在图像中只存在两个正交平面，它们的法向量构成弱曼哈顿坐标系，构造第三个平面的法向量为：

17、

18、三条法向量构成完整的曼哈顿坐标系，图像为曼哈顿帧；

19、若在当前帧中不存在正交平面，则寻找环境中最长的七条直线，检测它们是否存在正交关系，若存在三条两两正交的直线，则它们的方向向量构成曼哈顿坐标系，图像为曼哈顿帧；

20、若在图像中只存在两条正交直线，它们的方向向量构成弱曼哈顿坐标系，构造第三条直线的方向向量为：

21、

22、三条方向向量构成完整的曼哈顿坐标系，图像为曼哈顿帧；

23、除了以上四种情况，其他情况都为非曼哈顿帧。

24、优选的，所述步骤 s4中的自适应轨迹跟踪方法如下：

25、若，使用点、线和平面特征进行跟踪，若当前帧为曼哈顿帧，则将旋转分量和平移分量进行解耦，基于曼哈顿世界坐标系估计旋转分量，基于levenberg-marquardt算法估计平移分量，若当前帧为非曼哈顿帧，则基于levenberg-marquardt算法同时估计六维位姿变量中的旋转和平移；

26、若，此时环境中存在充足的特征，因平面的提取噪声较大，只使用较为准确的点和线进行跟踪，基于levenberg-marquardt算法同时估计六维位姿变量中的旋转和平移。

27、本发明所提供的自适应视觉slam方法与现有技术相比，显著优点为：

28、（1）针对环境中存在动态物体导致的定位信息不准确或者直接定位失败的问题，使用基于卷积神经网络的yolo算法对图像进行目标检测，去除环境中的高概率动态物体，有效提高了系统在动态环境下定位的精度和稳定性。

29、（2）针对低纹理环境，基于曼哈顿世界假设增加结构特征，不仅能解决特征缺失的问题，还能有效降低旋转估计时产生的累积误差，减少轨迹漂移。

30、（3）本发明涉及一种基于目标检测和结构化特征的机器人定位方法，用以解决机器人轨迹跟踪问题，基于rgb-d深度相机实现了在复杂环境下的定位与地图构建，提高了定位的精度和稳定性。针对环境中存在动态物体导致的定位信息不准确或者直接定位失败的问题，使用基于卷积神经网络的yolo算法对图像进行目标检测，去除环境中的高概率动态物体，针对低纹理环境或旋转估计中产生累积误差进而造成轨迹漂移的问题，基于曼哈顿世界假设增加结构特征。本方法在动态或低纹理的复杂环境下，极大地提高了位姿估计的准确性和鲁棒性，可广泛应用于室内机器人轨迹跟踪等场景。