基于单目坡道角度检测方法及坡道角度检测系统与流程

本技术涉及图像处理领域，更具体地说，是基于单目坡道角度检测方法及坡道角度检测系统。

背景技术：

1、为降低人工作业劳动强度，利用清扫机器人进行地面垃圾清扫越受青睐。清扫机器人在进行自动清扫中需基于相机采集的图像实现垃圾测距，以便进行清扫路径规划。但是需清扫垃圾的地面并非一直水平的，非水平的地面对清扫机器人的工作过程势必产生一定的影响。例如，当在非水平的具有坡度的地面上行走时，清扫机器人若还保持着在水平地面行走所需的驱动力会使得清扫机器人无法顺利地越障爬坡去清扫坡道上的垃圾，同时，当清扫器人在不同坡度的地面进行行走清扫时，其滚刷或者盘刷也要根据坡度进行角度调整，才能保证滚刷和盘刷合适的清扫力度。

2、在现有一些机器人的行驶过程中的坡道检测方式中，除了安装垃圾测距所用相机之外，还在机器人的壳体上加装多个雷达，从而利用雷达检测出路面的坡度大小，然而雷达成本较高，且雷达测量坡度过程较为复杂，从而增加了清扫机器人的设备成本。另一些坡道检测方式采用双目视觉方式进行坡道检测，然而双目视觉方式的配置复杂度高且计算量一般都比较大。

技术实现思路

1、本技术旨在提供一种基于单目视觉实现坡角检测的基于单目坡道角度检测方法及坡道角度检测系统，以解决现有技术中的因另外安装多个雷达而导致的成本高的问题以及双目视觉带来的复杂度的问题。

2、第一方面，本技术提供一种基于单目坡道角度检测方法，该方法包括：

3、基于运动单目相机获取的包含位于坡道上的待跟踪目标物的多帧图像计算相机运动轨迹；

4、基于相机运动轨迹建立两点直线方程；

5、求解两点直线方程与待跟踪目标所在平面的角度。

6、在所述基于单目坡道角度检测方法中，通过运动的单目相机获取不同时刻的关于坡道上的待跟踪目标的图像，并通过前述多帧图像计算单目相机运动轨迹，基于相机运动轨迹获取相机的两点直线方程，以两点直线方程与待跟踪目标所在平面的所成角度作为坡道角度。该坡道角度检测方法基于单目视觉避免了双目视觉的较高的复杂度的问题，并且避免了因除测距相机之外加装雷达而导致的提高设备成本的问题。

7、基于第一方面，一种可能的实现方式中，基于运动单目相机获取的包含位于坡道上的待跟踪目标物的多帧图像计算相机运动轨迹，包括：

8、获取运动单目相机采集的包含位于坡道的目标物的前后三帧图像；

9、进行同一目标物的跟踪匹配依序获得该待跟踪目标物分别在该前后三帧图像中的第一特征点集合、第二特征点集合和第三特征点集合；

10、获取单目相机的内参矩阵和畸变参数；

11、基于pnp方法获得采集第二帧图像时的单目相机的世界坐标p1(x1,y1,z1)；

12、基于pnp方法获得采集第三帧图像时的单目相机的世界坐标p2(x2,y2,z2)；

13、其中，x1，y1，z1分别为采集第二帧图像时的单目相机的在世界坐标系中的x轴、y轴和z轴坐标，x2，y2，z2分别为采集第三帧图像时的单目相机在世界坐标系的x轴、y轴和z轴坐标。

14、基于第一方面，一种可能的实现方式中，基于相机运动轨迹建立两点直线方程为建立关于p1p2直线方程，其中p1p2直线方程为：(x’－x1)/(x2－x1)＝(y’－y1)/(y2－y1)＝(z’－z1)/(z2－z1)；

15、其中，x’为p1p2直线方程的自变量，y’和z’为p1p2直线方程的因变量。

16、基于第一方面，一种可能的实现方式中，所述求解两点直线方程与待跟踪目标所在平面的角度为求解p1p2直线方程与待跟踪目标所在平面的角度θ：。

17、基于第一方面，一种可能的实现方式中，设定第一特征点集合为p1,p1={(p1[i].x,p1[i].y),i=1,2,…}、第二特征集合为p2,p2={(p2[i].x,p2[i].y),i=1,2,…}和第三特征点集合为p3,p3={(p3[i].x,p3[i].y),i=1,2,… }，所述基于pnp方法获得采集第二帧图像时的单目相机的世界坐标p1(x1,y1,z1)，包括：

18、以待跟踪目标所在地面为基准平面，调用opencv的solvepnp函数计算第一旋转向量rvec1和第一平移向量tvec1：solvepnp(p3d,p2,m,c,rvec1,tvec1)；

19、利用罗德里格斯变换将第一旋转向量rvec1转换成第一旋转矩阵r1；

20、计算世界坐标p1(x1,y1,z1)：p1=－r1’*tvec1；

21、其中，p3d={(p1[i].x,p1[i].y,0),i=1,2…}，i代表集合中的第i个特征点，x代表特征点的横坐标，y代表特征点的纵坐标，m为单目相机的内参矩阵，c为单目相机的畸变参数，r1’为r1的转置矩阵。

22、基于第一方面，一种可能的实现方式中，所述基于pnp方法获得采集第三帧图像时的单目相机的世界坐标p2(x2,y2,z2)，包括：

23、以待跟踪目标所在地面为基准平面，调用opencv的solvepnp函数计算第二旋转向量rvec2和第二平移向量tvec2：solvepnp(p3d,p3,m,c,rvec2,tvec2)；

24、利用罗德里格斯变换将第二旋转向量rvec2转换成第二旋转矩阵r2；

25、计算世界坐标p2(x2,y2,z2)：p2=－r2’*tvec2；

26、其中，r2’为r2的转置矩阵。

27、基于第一方面，一种可能的实现方式中，获取的所述前后三帧图像均为单目相机运动过程中正对目标物时采集的图像，在获取每一帧包含位于坡道的待跟踪目标物的图像之前，所述基于单目坡道角度检测方法还包括判断采集图像的运动单目相机是否正对待跟踪目标物，包括：

28、选定一目标为待跟踪目标，求解运动单目相机采集图像f中该待跟踪目标的中心点；

29、判断该中心点是否位于图像f中心位置；

30、若是，判定运动单目相机正对待跟踪目标物；

31、否则，判定运动单目相机非正对待跟踪目标物。

32、基于第一方面，一种可能的实现方式中，设定单目相机为匀速运动，在获取的所述前后三帧图像之后且在计算相机运动轨迹之前，所述单目的坡道检测方法还包括判断待跟踪目标是否位于坡道上：

33、计算第一帧图像中待跟踪目标的中心位置距离单目相机的物理距离l1；

34、计算第二帧图像中待跟踪目标的中心位置距离单目相机的物理距离l2；

35、计算第三帧图像中待跟踪目标的中心位置距离单目相机的物理距离l3；

36、计算物理距离l1和物理距离l2的差值b1以及物理距离l2和物理距离l3的差值b2；

37、判断差值b1和差值b2的差值的绝对值是否小于设定阈值，若是，则判定待跟踪目标不位于坡道上，否则，则判定该跟踪目标位于坡道上。

38、第二方面，本技术提供一种坡道角度检测系统，包括：

39、设置于所述清扫机器人上的单目相机，用于在清扫机器人运动过程中采集包含坡道上的垃圾目标物的图像；

40、驱动装置，用于驱动清扫机器人在地面匀速行驶；

41、图像处理器，通信连接单目相机，用于根据单目相机采集的多帧图像来检测清扫机器人行驶过程中的坡道角度。