CN116858187B - 基于单目坡道角度检测方法及坡道角度检测系统 - Google Patents

Abstract

本申请公开一种基于单目坡道角度检测方法，该方法包括：基于运动单目相机获取的包含位于坡道上的待跟踪目标物的多帧图像计算相机运动轨迹；基于相机运动轨迹建立两点直线方程；以及求解两点直线方程与待跟踪目标所在平面的角度。本申请还提供一种用于实现该基于单目坡道角度检测方法的坡道角度检测系统。该基于单目坡道角度检测方法降低使用成本以及避免双目视觉带来的复杂性。

Description

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

技术领域

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

背景技术

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

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

发明内容

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

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

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

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

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

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

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

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

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

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

基于PNP方法获得采集第二帧图像时的单目相机的世界坐标P₁(x₁,y₁,z₁)；

基于PNP方法获得采集第三帧图像时的单目相机的世界坐标P₂(x₂,y₂,z₂)；

其中，x₁，y₁，z₁分别为采集第二帧图像时的单目相机的在世界坐标系中的X轴、Y轴和Z轴坐标，x₂，y₂，z₂分别为采集第三帧图像时的单目相机在世界坐标系的X轴、Y轴和Z轴坐标。

基于第一方面，一种可能的实现方式中，基于相机运动轨迹建立两点直线方程为建立关于P₁P₂直线方程，其中P₁P₂直线方程为：(x’－x₁)/(x₂－x₁)＝(y’－y₁)/(y₂－y₁)＝(z’－z₁)/(z₂－z₁)；

其中，x’为P₁P₂直线方程的自变量，y’和z’为P₁P₂直线方程的因变量。

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

基于第一方面，一种可能的实现方式中，设定第一特征点集合为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方法获得采集第二帧图像时的单目相机的世界坐标P₁(x₁,y₁,z₁)，包括：

以待跟踪目标所在地面为基准平面，调用opencv的solvePnP函数计算第一旋转向量rvec₁和第一平移向量tvec₁：solvePnP(P3D,P2,M,C,rvec₁,tvec₁)；

利用罗德里格斯变换将第一旋转向量rvec₁转换成第一旋转矩阵R₁；

计算世界坐标P₁(x₁,y₁,z₁)：P₁＝－R₁’*tvec₁；

其中，P3D＝{(P1[i].x,P1[i].y,0),i＝1,2…}，i代表集合中的第i个特征点，x代表特征点的横坐标，y代表特征点的纵坐标，M为单目相机的内参矩阵，C为单目相机的畸变参数，R₁’为R₁的转置矩阵。

基于第一方面，一种可能的实现方式中，所述基于PNP方法获得采集第三帧图像时的单目相机的世界坐标P₂(x₂,y₂,z₂)，包括：

以待跟踪目标所在地面为基准平面，调用opencv的solvePnP函数计算第二旋转向量rvec₂和第二平移向量tvec₂：solvePnP(P3D,P3,M,C,rvec₂,tvec₂)；

利用罗德里格斯变换将第二旋转向量rvec₂转换成第二旋转矩阵R₂；

计算世界坐标P₂(x₂,y₂,z₂)：P₂＝－R₂’*tvec₂；

其中，R₂’为R₂的转置矩阵。

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

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

判断该中心点是否位于图像F中心位置；

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

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

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

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

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

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

计算物理距离L₁和物理距离L₂的差值B₁以及物理距离L₂和物理距离L₃的差值B₂；

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

第二方面，本申请提供一种坡道角度检测系统，包括：

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

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

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

附图说明

为了更清楚地说明本申请实施例中的技术方案，下面将对实施例或现有技术描述中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本申请的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动性的前提下，还可以根据这些附图获得其他的附图。

图1为本申请实施例的基于单目坡道角度检测方法的流程图；

图2为本申请实施例的单目相机运动轨迹以及坡面角度示意图；

图3为本申请实施例的基于运动单目相机获取的包含位于坡道上的待跟踪目标物的多帧图像计算相机运动轨迹的步骤流程图；

图4为本申请实施例的判断待跟踪目标是否位于坡道上的方法步骤流程图。

具体实施方式

为了使本申请所要解决的技术问题、技术方案及有益效果更加清楚明白，以下结合附图及实施例，对本申请进行进一步详细说明。应当理解，此处所描述的具体实施例仅仅用以解释本申请，并不用于限定本申请。

需要说明的是，当元件被称为“固定于”或“设置于”另一个元件，它可以直接在另一个元件上或者间接在该另一个元件上。当一个元件被称为是“连接于”另一个元件，它可以是直接连接到另一个元件或间接连接至该另一个元件上。

需要理解的是，术语“长度”、“宽度”、“上”、“下”、“前”、“后”、“左”、“右”、“竖直”、“水平”、“顶”、“底”、“内”、“外”等指示的方位或位置关系为基于附图所示的方位或位置关系，仅是为了便于描述本申请和简化描述，而不是指示或暗示所指的装置或元件必须具有特定的方位、以特定的方位构造和操作，因此不能理解为对本申请的限制。

此外，术语“第一”、“第二”仅用于描述目的，而不能理解为指示或暗示相对重要性或者隐含指明所指示的技术特征的数量。由此，限定有“第一”、“第二”的特征可以明示或者隐含地包括一个或者更多个该特征。在本申请的描述中，“多个”的含义是两个或两个以上，除非另有明确具体的限定。

现结合附图对本申请的基于单目坡道角度检测方法及坡道角度检测系统进行具体说明。

本申请实施例提供的基于单目坡道角度检测方法可应用于清扫机器人中，进行坡道角度检测时采集图像的单目相机设置在清扫机器人上，单目相机随清扫机器人运动，单目相机在清扫机器人在清扫行走过程中采集前方图像。

请参照图1和图2，该基于单目坡道角度检测方法包括如下步骤S100和步骤S300。

步骤S100：基于运动单目相机获取的包含位于坡道上的待跟踪目标物的多帧图像计算相机运动轨迹。

可以理解地，待跟踪目标物为坡道上的选定清扫的垃圾目标。由于清扫机器人的处理对象是垃圾目标，通常清扫机器人可利用单目相机实时采集图像识别检测出垃圾目标，从而控制清扫机器人转向并朝向垃圾目标行走，当该基于单目坡道角度检测方法应用于清扫机器人时，待跟踪目标物直接设定为垃圾目标即可，后续根据清扫机器人清扫行走过程中单目相机采集的垃圾图像进行坡道角度检测。

步骤S200：基于相机运动轨迹建立两点直线方程。

步骤S300：求解两点直线方程与待跟踪目标所在平面的角度。

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

其中，参照图2和图3，对于步骤S100，所述基于运动单目相机获取的包含位于坡道上的待跟踪目标物的多帧图像计算相机运动轨迹包括如下步骤S110和步骤S150：

步骤S110：获取运动单目相机采集的包含位于坡道的目标物的前后三帧图像；其中所述目标物的前后三帧图像是针对同一目标物而言的。

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

步骤S130：获取单目相机的内参矩阵和畸变参数；

步骤S140：基于PNP方法获得采集第二帧图像时的单目相机的世界坐标P₁(x₁,y₁,z₁)；

步骤S150：基于PNP方法获得采集第三帧图像时的单目相机的世界坐标P₂(x₂,y₂,z₂)。

在上述步骤中，x₁，y₁，z₁分别为采集第二帧图像时的单目相机的在世界坐标系中的X轴、Y轴和Z轴坐标，x₂，y₂，z₂分别为采集第三帧图像时的单目相机在世界坐标系的X轴、Y轴和Z轴坐标。

值得说明的是，图2中的P0、P1、P2分别为步骤S110中的前后三帧时单目相机的位姿，其中图中的θ角度为本申请实施例所求的坡道角度，图中P1、P2即分别为上述P₁、上述P₂。在步骤S140中，需利用单目相机的内参矩阵、畸变参数、同一垃圾目标在第一帧图像和第二帧图像中一一对应的多个特征点并基于PNP方法获得世界坐标P₁，同时在步骤S150中，需利用单目相机的内参矩阵、畸变参数、同一垃圾目标在第一帧图像和第三帧图像中一一对应的多个特征点并基于PNP方法获得世界坐标P₂。为了便捷，本申请实施例在第一帧图像中提取多个特征点，后续在第二帧图像和第三帧图像进行多特征点跟踪以获得第二帧图像中的与第一特征点集合对应第二特征点集合以及获得第三帧图像中的与第二特征点集合对应的第三特征点集合，如此不用在不同帧图像重复进行特征点提取、匹配以及同一目标识别。

具体而言，对于步骤S120，进行同一垃圾目标的跟踪依序获得该目标物分别在该前后三帧图像中的第一特征点集合、第二特征点集合和第三特征点集合，以待跟踪目标为一垃圾目标为例，具体为：

步骤S121：提取第一帧图像中的一垃圾目标的多个特征点，获得第一特征点集合P1,P1＝{(P1[i].x,P1[i].y),i＝1,2,…}；

步骤S122：对第一帧图像中的多个特征点进行特征点跟踪，获得该垃圾目标在第二帧图像中多个特征点组成的第二特征点集合P2,P2＝{(P2[i].x,P2[i].y),i＝1,2,…}；

步骤S123：对第二帧图像中的多个特征点进行特征点跟踪，获得该垃圾目标在第三帧图像中多个特征点组成的第三特征点集合P3,P3＝{(P3[i].x,P3[i].y),i＝1,2,…}。

在上述步骤中，i代表集合中的第i个特征点，x代表特征点在图像坐标系中的横坐标，y代表特征点在图像坐标系中的纵坐标，可理解地，P1[i].x代表第一特征点集合P1中的第i个特征点的横坐标，P1[i].y代表第一特征点集合P1中的第i个特征点的纵坐标，依次类推，不再赘述。

在一些实施例中，提取的特征点可为但不限于为ORB角点，特征点跟踪方法采用LK光流法跟踪。

对于步骤S140，所述基于PNP方法获得采集第二帧图像时的单目相机的世界坐标P₁(x₁,y₁,z₁)，包括：

步骤S411：以待跟踪目标所在地面为基准平面，调用opencv的solvePnP函数计算第一旋转向量rvec₁和第一平移向量tvec₁：solvePnP(P3D,P2,M,C,rvec₁,tvec₁)；

其中，P3D＝{(P3D[i].x,P3D[i].y,P3D[i].z),i＝1,2…}，P3D为三维坐标集合，P3D[i].x＝P1[i].x，P3D[i].y＝P1[i].y，由于以待跟踪目标所在平面为基准平面，故而设定在待跟踪目标所在平面处的点的深度信息均为0，故而P3D[i].z＝0，因此P3D＝{(P1[i].x,P1[i].y,0),i＝1,2…}，M为单目相机的内参矩阵，C为单目相机的畸变参数，内参矩阵和畸变参数可通过相机标定获得，相机标定方法可采用现有的标定方法。

步骤S412：利用罗德里格斯变换将第一旋转向量rvec₁转换成第一旋转矩阵R₁；

步骤S413：计算世界坐标P₁(x₁,y₁,z₁)：P₁＝－R₁’*tvec₁，R₁’为R₁的转置矩阵。

对于步骤S150，所述基于PNP方法获得采集第三帧图像时的单目相机的世界坐标P₂(x₂,y₂,z₂)，包括：

步骤S511：以待跟踪目标所在地面为基准平面，调用opencv的solvePnP函数计算第二旋转向量rvec₂和第二平移向量tvec₂：solvePnP(P3D,P3,M,C,rvec₂,tvec₂)；

步骤S512：利用罗德里格斯变换将第二旋转向量rvec₂转换成第二旋转矩阵R₂；

步骤S513：计算世界坐标P₂(x₂,y₂,z₂)：P₂＝－R₂’*tvec₂，R₂’为R₂的转置矩阵。

如图2所示，在步骤S200，基于相机运动轨迹建立两点直线方程为建立关于P₁P₂直线方程，其中P₁P₂直线方程为：(x’－x₁)/(x₂－x₁)＝(y’－y₁)/(y₂－y₁)＝(z’－z₁)/(z₂－z₁)；

其中，x’为P₁P₂直线方程的自变量，y’和z’为P₁P₂直线方程的因变量。

进一步地，对于步骤S300：所述求解两点直线方程与待跟踪目标所在平面的角度为求解P₁P₂直线方程与待跟踪目标所在平面的角度θ：

值得说明的是，基于上述步骤S100至步骤S300可知坡道角度是基于相机位姿直线与待跟踪的垃圾目标所在平面的角度来确定的，而如前步骤S140内容中所述，在求单目相机的世界坐标P1和P2时是以带跟踪目标所在平面为基准平面的，故而可利用上述cosθ公式来求解坡道平面，基准平面的坐标系旋转回地面的坐标系，垃圾坡道斜面的倾斜角不会改变，还是θ，如图2所示。

又值得说明的是，本申请实施例采用的直线与平面的夹角来确定坡道倾斜角的，而采集垃圾目标图像过程中，一般单目相机位于清扫机器人的机体的正前方的，单目相机是跟随清扫机器人运动，通常为了合理规划清扫路径，清扫机器人在清扫行走过程是逐步选定距离单目相机最近的垃圾目标为清扫对象，此时选定最近的垃圾目标即为前述待跟踪目标，在选定目标后，此时驱动单目相机正对清扫的垃圾目标(此时垃圾目标位于图像中心位置)，通常将单目相机设置于机器人前侧的正中间，并以正对垃圾目标行驶并行走至选定的垃圾目标处进行清扫。然而，单目相机在运动过程时持续采集待跟踪目标图像的，并不能保证每个时刻采集图像中待跟踪目标位于图像中心。因此，为了进一步保证坡道角度的更加精确性以及使得坡道角度过程更加符合清洁机器人清扫行走路径规划原理，要尽量保证求解坡道角度的每一帧图像中的待跟踪垃圾目标位于图像的中心，故而在获取每一帧包含位于坡道的待跟踪目标物的图像之前，所述基于单目坡道角度检测方法还包括步骤S400：判断采集图像的运动单目相机是否正对待跟踪目标物，获取的每一帧包含位于坡道的待跟踪目标的图像为相机正对跟踪目标物时采集的。

具体地，上述步骤S400的判断采集图像的运动单目相机是否正对待跟踪目标物，包括如下步骤：

步骤S410：选定一目标为待跟踪目标，求解运动单目相机采集图像F中该待跟踪目标的中心点；

步骤S420：判断该中心点是否位于图像F的中心位置；

若是，执行步骤S430：判定运动单目相机正对待跟踪目标物；

否则，执行步骤S440：判定运动单目相机非正对待跟踪目标物。

可以理解地，相机在不断地采集图片，用于本申请实施例中的前述三帧图像的基准是待跟踪目标的中心位于采集图像的中心。

进一步地，设定采集图像的单目相机运动形式为匀速运动，在获取的所述前后三帧图像之后且在计算相机运动轨迹之前，所述单目的坡道检测方法还包括步骤S500：判断待跟踪目标是否位于坡道上，如图4所示：

步骤S510：计算第一帧图像中待跟踪目标的中心位置距离单目相机的物理距离L₁；

步骤S520：计算第二帧图像中待跟踪目标的中心位置距离单目相机的物理距离L₂；

步骤S530：计算第三帧图像中待跟踪目标的中心位置距离单目相机的物理距离L₃；

步骤S540：计算物理距离L₁和物理距离L₂的差值B₁以及物理距离L₂和物理距离L₃的差值B₂；其中B₁＝L₁－L₂，B₁＝L₂－L₃。

步骤S550：判断差值B₁和差值B₂的差值的绝对值是否小于设定阈值，若是，执行步骤S560：则判定待跟踪目标不位于坡道上，否则，执行步骤S570则判定该跟踪目标位于坡道上。

在一实施例中，设定阈值可为0.5cm，但不限于此，设定阈值可以根据实际需求进行改变。

值得说明的是，步骤S500的步骤中的距离L₁、距离L₂和距离L₃即为清扫机器人清扫过程中垃圾测距，即使不进行步骤S500的步骤，为了完成自动清扫，垃圾测距也是始终要进行的，故而，步骤S500相对于前述步骤S100至步骤S300的坡道角度计算步骤而言，步骤S500计算量是较小的，为了防止时刻进行坡道角度计算带来的复杂度，在清扫机器人清扫过程中，基于单目坡道角度检测方法是需先判断是否需要进行坡度角度计算的必要性，即进行步骤S500的判断待跟踪目标是否位于坡道上，再进行坡道角度的计算，如此可降低计算复杂度，提高清扫控制过程中的整体算法运行时间。当选定的垃圾目标在坡道上时才进行坡道角度的计算，以便后续为清洁机器人提供坡道行驶的合适的驱动力以及调整清扫垃圾的滚刷或者盘刷角度。

可以理解地，在步骤S550中，若是清扫机器人在水平地面行走时，并且机器人行走过程中控制单目相机正对地面待跟踪垃圾目标的行走并采集前述三帧图像，并且在运动单目相机在匀速运动状态下，物理距离L₁和物理距离L₂的差值B₁的绝对值以及物理距离L₂和物理距离L₃的差值B₂的绝对值应当趋于接近的，若是相差较大说明前面需清扫的垃圾位于不水平的坡道上，那么为了后续便于控制清扫机器人顺利在坡道上清扫，此时需要求得坡道角度。

在上述步骤中，图像中的每一像素点q(q_x,q_y)在现实中距离单目相机的物理距离L(Q_x,Q_y)的计算方式为：Q_x＝(B_x－q_x)*Rat_x；Q_y＝(B_y－q_y)*Rat_y+D，即求解上述物理距离L₁、L₂和L₃采用公式Q_x和Q_y公式求得。

值得说明的是，选定一矩形块为在计算物理距离L时设定在地面的矩形的参照物，并且单目相机会设置于矩形块的一个边J的一侧，设定该边J为参照底边。在计算物理距离L时，以单目相机至地面的垂点为原点，以俯视状态下的矩形块的参照底边为X’轴且以与该参照底边垂直的边为Y’轴来建立地面的平面直角坐标系。在上述公式中，Q_x为像素点q在地面的平面直角坐标系中的X’轴上坐标值，Q_y像素点q在地面的平面直角坐标系中的Y’轴上坐标值，D为现实中相机垂直于地面的垂点与参照底边的垂直距离，参照底边的中心点B₀(B_x,B_y)。并且，依据矩形块计算像素坐标和物理坐标的转换比Rat(Rat_x，Rat_y)：计算俯视状态下的矩形块的参照底边的像素长度值PixW和与该矩形块的参照底边垂直的边的像素边长值PixH；Rat_x＝WorldW/PixW且Rat_y＝WorldH/PixH；其中，WorldW为现实中参照底边的实际物理长度值，WorldH为现实中与参照底边垂直的边的实际物理长度值。Rat_x为X’轴上的转换比，Rat_y为Y’轴上的转换比。WorldW和WorldH的单位可以都是cm，而PixW和PixH的单位均是像素pix。

进一步地，前述获取的前后三帧图像为单目相机正对垃圾目标时采集的，即当垃圾图像的中心点在采集图像的中心位置。故而需要在检测坡道角度检测过程中需要控制相机从不正对目标向正对目标转动，从而需要检测相机转动角度。通过采集的一垃圾目标V的图像K求解相机需要转动的角度方式为：利用前述公式L(Q_x,Q_y)求得该垃圾目标的中心位置c₁距离相机的物理距离B₁；利用前述公式L(Q_x,Q_y)求解图像K的中心位置c₂距离相机的物理距离B₂；求解俯视状态下中心位置c₁和中心位置c₂距离的像素距离，再根据该转换比Rat将像素距离转成现实中的物理距离B₃；根据三角形的余弦定理求得物理距离B₁边长和物理距离B₂边长的夹角β，该夹角β就为相机的转动角度，即转动该夹角β使得相机正对选定的垃圾目标，在运动过程在使得相机正对选定的垃圾目标后，再获取前述步骤S100至步骤S500所需的每一帧图像。其中，垃圾目标V可为选定清扫的垃圾对象。

本申请实施例还提供一种坡道角度检测系统，该系统应用于清扫机器人且用于实现上述基于单目坡道角度检测方法。该清扫机器人用于自动清扫地面垃圾。该坡道角度检测系统包括设置于清扫机器人上的单目相机、用于驱动清扫机器人在地面匀速行驶的驱动装置和图像处理器。单目相机用于在清扫机器人运动过程中采集包含坡道上的垃圾目标物的图像，驱动装置包括设置于机器人上的用于地面行走的多个车轮以及用于驱动多个车轮转动并设置于机器人内的电机，车轮带动机器人在地面匀速行驶。图像处理器通信连接单目相机，用于根据单目相机采集的多帧图像来检测清扫机器人行驶过程中的坡道角度，即图像处理器用于执行上述基于单目坡道角度检测方法的步骤。

以上所述仅为本申请的较佳实施例而已，并不用以限制本申请，凡在本申请的精神和原则之内所作的任何修改、等同替换和改进等，均应包含在本申请的保护范围之内。

Claims (6)

1.一种基于单目坡道角度检测方法，其特征在于，包括：

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

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

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

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

基于PNP方法获得采集第二帧图像时的单目相机的世界坐标P₁(x₁,y₁,z₁)；

基于PNP方法获得采集第三帧图像时的单目相机的世界坐标P₂(x₂,y₂,z₂)；

其中，x₁,y₁,z₁分别为采集第二帧图像时的单目相机的在世界坐标系中的X轴、Y轴和Z轴坐标，x₂，y₂，z₂分别为采集第三帧图像时的单目相机在世界坐标系的X轴、Y轴和Z轴坐标；

基于相机运动轨迹建立两点直线方程：建立关于P₁P₂直线方程，其中P₁P₂直线方程为：(x’－x₁)/(x₂－x₁)＝(y’－y₁)/(y₂－y₁)＝(z’－z₁)/(z₂－z₁)，其中，x’为P₁P₂直线方程的自变量，y’和z’为P₁P₂直线方程的因变量；以及

求解两点直线方程与待跟踪目标所在平面的角度：求解P₁P₂直线方程与待跟踪目标所在平面的角度θ：

2.如权利要求1所述的基于单目坡道角度检测方法，其特征在于，设定第一特征点集合为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方法获得采集第二帧图像时的单目相机的世界坐标P₁(x₁,y₁,z₁)，包括：

以待跟踪目标所在地面为基准平面，调用opencv的solvePnP函数计算第一旋转向量rvec₁和第一平移向量tvec₁：solvePnP(P3D,P2,M,C,rvec₁,tvec₁)；

利用罗德里格斯变换将第一旋转向量rvec₁转换成第一旋转矩阵R₁；

计算世界坐标P₁(x₁,y₁,z₁)：P₁＝－R₁’*tvec₁；

其中，P3D为三维坐标集合且P3D＝{(P1[i].x,P1[i].y,0),i＝1,2…}，i代表集合中的第i个特征点，x代表特征点的横坐标，y代表特征点的纵坐标，M为单目相机的内参矩阵，C为单目相机的畸变参数，R₁’为R₁的转置矩阵。

3.如权利要求2所述的基于单目坡道角度检测方法，其特征在于，所述基于PNP方法获得采集第三帧图像时的单目相机的世界坐标P₂(x₂,y₂,z₂)，包括：

以待跟踪目标所在地面为基准平面，调用opencv的solvePnP函数计算第二旋转向量rvec₂和第二平移向量tvec₂：solvePnP(P3D,P3,M,C,rvec₂,tvec₂)；

利用罗德里格斯变换将第二旋转向量rvec₂转换成第二旋转矩阵R₂；

计算世界坐标P₂(x₂,y₂,z₂)：P₂＝－R₂’*tvec₂；

其中，R₂’为R₂的转置矩阵。

4.如权利要求1所述的基于单目坡道角度检测方法，其特征在于，获取的所述前后三帧图像均为单目相机运动过程中正对目标物时采集的图像，在获取每一帧包含位于坡道的待跟踪目标物的图像之前，所述基于单目坡道角度检测方法还包括判断采集图像的运动单目相机是否正对待跟踪目标物，包括：

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

判断该中心点是否位于图像F的中心位置；

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

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

5.如权利要求4所述的基于单目坡道角度检测方法，其特征在于，设定单目相机为匀速运动，在获取的所述前后三帧图像之后且在计算相机运动轨迹之前，所述单目的坡道检测方法还包括判断待跟踪目标是否位于坡道上：

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

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

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

计算物理距离L₁和物理距离L₂的差值B₁以及物理距离L₂和物理距离L₃的差值B₂；

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

6.一种坡道角度检测系统，应用于清扫机器人，其特征在于，包括：

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

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

图像处理器，通信连接单目相机，用于执行如权利要求1至5任一项所述的基于单目坡道角度检测方法的步骤。