CN110926405A - 一种基于单目视觉灭点检测的arv姿态测量方法 - Google Patents

Abstract

本发明提供一种基于单目视觉灭点检测的ARV姿态测量方法，该包括：对ARV获取的影像进行直线特征提取；对提取得到的直线进行灭点检测；计算旋转矩阵；根据旋转矩阵与姿态角的变换关系，即可得到ARV姿态角。该方法无需在物理环境中布设精密的标记点，而充分利用物理环境中的垂直结构，仅需利用ARV自身搭载的单目相机，即可获得ARV相对于物理空间坐标系的姿态。

Description

一种基于单目视觉灭点检测的ARV姿态测量方法

技术领域

本发明涉及机器人领域，尤其涉及移动机器人导航与定位技术， 具体为一种基于单目视觉灭点检测的ARV姿态测量方法。

背景技术

随着工业自动化程度的提高，各种先进技术和设备的应用愈发普 及，越来越多的工厂和仓储车间由劳动密集型转向技术密集型，实现 了智能化生产和运输，由传统模式向智能化、现代化升级，使得复合 机器人(ARV)在自动化生产和智能物流中的使用呈现爆发式增长， 越来越多的应用于汽车制造、电力、烟草、医疗等行业。

导航定位技术是ARV系统中最为核心的技术，目的在于确定ARV 当前的位置和姿态，将其提供于调度和路径规划系统。目前可用的方 法主要有：通过激光雷达扫描获得ARV周围环境的点云数据，通过点 云与已知环境地图配准从而实现ARV的定位，其精度高，实时性强， 但成本较高；利用里程计、加速度计和陀螺仪，通过航迹推算出ARV 的位置和姿态，数据更新频率高，短期精度和稳定性较好，但定位误 差会随时间积累；基于视觉的方法同样受到误差累积的影响，通常采 用闭环检测来减少累计误差，会产生较大的数据冗余。

发明内容

本发明针对ARV的姿态估计问题，提出了一种基于单目视觉灭点 检测的姿态测量方法。

在透视投影中，灭点是空间中平行直线在无穷远点处的交点，其 对应着影像中的一组直线。而直线特征是影像的一种重要特征，普遍 存在于ARV工作环境中，同时具有很高的稳定性，因此在位姿估计、 三维重建、目标识别中广泛使用。通常情况下，在ARV工作场景中 存在着三个互相垂直的方向XYZ，基于对单目视觉影像的直线特征 提取计算出的一组三个灭点分别与之对应，通过三个灭点所在的方向 向量的对比关系，能够反映拍摄影像的相机的姿态，也即ARV的姿 态。

根据以上原理，本发明提供的基于单目视觉灭点检测的ARV姿态 测量方法，包括以下步骤：

(1.1)对ARV获取的影像进行直线特征提取；

(1.2)对提取得到的直线进行灭点检测；

(1.3)计算旋转矩阵；

根据旋转矩阵与姿态角的变换关系，即可得到ARV姿态角。

进一步地，对所述影像进行直线特征提取的方法包括：

对影像进行直线特征的初步提取；

对初步提取到的直线段中的断裂、连续短直线进行合并；

删除短直线段。

进一步地，对初步提取到的直线段，按照如下准则判断是否需要 进行合并：

a.角度：两直线段的方向夹角θ是否小于角度阈值θ_T；

b.垂距：两直线段的中点分别与另一条直线的垂距D₁，D₂是否小于 垂距阈值D_T；

当两条直线段同时满足上述两个准则时，则对其进行合并。

进一步地，对直线段进行合并的方法包括：

a.确定合并后直线段上一点P：假设合并前两直线段为别为AB， CD，A、B、C、D分别为其端点，其长度分别为l_AB，l_CD，则P点的坐 标为

b.确定合并后直线段的角度：直线段AB，CD的角度分别为 θ_AB，θ_CD，合并后直线段的角度为

c.确定合并直线段的端点：将原直线段AB、CD分别投影到合并 后的新直线上，取其包含的最长直线段，则得到合并后直线段的端点。

进一步地，对提取得到的直线进行灭点检测的方法包括以下步骤：

(2.1)生成灭点假设集合：假设提取出的直线段有N条，则从 中随机选取M组直线对(两条直线互不相同)，求出每组直线对的交 点，即为灭点假设集合，灭点假设集合的大小为M；

(2.2)直线段与假设灭点的一致性度量：对于灭点VP与直线L， 假设

为直线L的中心点，a，b为直线段的两个端点，则灭点VP与

的连线为

其一致性度量值为直线段端点a或b到I的垂 距：

(2.3)构建倾向矩阵W，其大小为N×M，其中矩阵每一行表 示提取到的一条直线段，每一列表示(2.1)中生成的假设灭点，如 果直线L_i与假设灭点VP_j的一致性度量值D(L_i，VP_j)小于阈值，则W_ij＝ 1；反之，则W_ij＝0；

(2.4)倾向矩阵优化：遍历倾向矩阵，计算直线类之间的Jaccard 距离，将Jaccard距离小于1的两个直线类合并，直到所有直线类之 间的Jaccard距离都为1，从而完成直线段的分类；

(2.5)计算灭点：对于每一类直线，其均对应一个灭点，假设 某一类直线有s条直线段，其直线方程为 A_kx+B_ky+c_k＝0，k＝1，2，...，s

则采用最小二乘法可求出该s条直线对应的灭点：

进一步地，旋转矩阵的计算方法包括如下步骤：

(3.1)灭点单位化，仅取其方向向量：

其中，j＝1，2，3，VP₁，VP₂，VP₃为所述步骤(1.2)中计算出的灭 点，对应场景中三个互相垂直的方向XYZ；

将方向向量用齐次坐标表示如下：

(3.2)在XYZ三个方向上的无穷远点的齐次坐标分别为：

由相机成像模型则有：

其中K为相机的内参数矩阵，由厂商给出或标定得到；由此可 得到旋转矩阵R。

本发明通过提取单张影像上的灭点作为全局观测量，对ARV的姿 态进行修正。与现有技术相比，这种方法的优势在于：无需在物理环 境中布设精密的标记点，而充分利用物理环境中的垂直结构，仅需利 用ARV自身搭载的单目相机，即可获得ARV相对于物理空间坐标系的 姿态。

应了解的是，上述一般描述及以下具体实施方式仅为示例性及阐 释性的，其并不能限制本发明所欲主张的范围。

附图说明

下面的附图是本发明的说明书的一部分，其绘示了本发明的示例 实施例，所附附图与说明书的描述一起用来说明本发明的原理。

图1为根据示例性实施例的基于单目视觉灭点检测的ARV姿态 测量方法流程图。

具体实施方式

现详细说明本发明的多种示例性实施方式，该详细说明不应认为 是对本发明的限制，而应理解为是对本发明的某些方面、特性和实施 方案的更详细的描述。

在不背离本发明的范围或精神的情况下，可对本发明说明书的具 体实施方式做多种改进和变化，这对本领域技术人员而言是显而易见 的。由本发明的说明书得到的其他实施方式对技术人员而言是显而易 见得的。本申请说明书和实施例仅是示例性的。

如附图1所示，根据示例性实施例的基于单目视觉灭点检测的姿 态测量方法，主要包括以下步骤：

步骤1：对ARV所获影像的直线特征提取。

现有的直线特征提取方法较多；在示例性实施例中，为了获得稳 定的直线特征，采用以下方法完成直线特征提取：

(1.1)进行直线特征的初步提取。

优选采用LSD算法进行直线特征的初步提取。该算法具有很高 的实时性、稳定性和准确性，计算效率高，无需设置过多参数。该算 法包括梯度幅值和梯度方向估计、提取直线支撑区域、矩形逼近支撑 区域和直线检测四个步骤。

当然，本领域技术人员也可采用其他现有算法完成直线特征的初 步提取。

(1.2)直线特征精化。由于影像噪声、遮挡和提取算法误差的 影响，现有的各种算法提取的直线特征会出现断裂、连续短直线等， 因此对在方向、垂距上相近的直线进行合并操作。

优选按照如下准则判断是否进行合并：

a.角度：两直线段的方向夹角θ是否小于角度阈值θ_T；

b.垂距：两直线段的中点分别与另一条直线的垂距D₁，D₂是否小于 垂距阈值D_T；

当两条直线段同时满足上述两个准则时，则对其进行合并。

优选按照如下方法确定合并后的直线段：

a.确定合并后直线段上一点P。假设合并前两直线段为别为AB， CD，A、B、C、D分别为其端点，其长度分别为l_AB，l_CD，则P点的坐 标为：

b.确定合并后直线段的角度。直线段AB，CD的角度分别为 θ_AB，θ_CD，合并后直线段的角度为：

c.确定合并直线段的端点。将原直线段AB,CD分别投影到合并 后的新直线上，取其包含的最长直线段，则得到合并后直线段的端点。

(1.3)如果合并后的直线段的长度大于长度阈值l_T，则将其保留； 否则将其删除。

完成后，即可得到稳定的直线段特征。

步骤2：对提取到的直线段进行灭点检测。实际上，灭点的检测 对应着影像上直线的分类。对于每一类直线，其均对应一个灭点。

同样，现有可用的直线分类方法较多；在示例性实施例中，采用 多模型分类方法J-Linkage算法对提取到的直线段进行分类，然后再 进行灭点的计算。具体包括以下步骤：

(2.1)生成灭点假设集合。假设在步骤1中提取出N条直线段， 则从中随机选取M组直线对(两条直线互不相同)。求出每组直线对 的交点，即为灭点假设集合，灭点假设集合的大小为M。

(2.2)直线段与假设灭点的一致性度量。对于灭点VP与直线L， 假设

为直线L的中心点，a，b为直线段的两个端点，则灭点VP与

的连线为

其一致性度量为直线段端点a或b到I的垂距：

(2.3)构建倾向矩阵W，其大小为N×M。其中矩阵每一行表 示步骤1中提取的一条直线段，每一列表示步骤(2.1)中生成的假 设灭点。如果直线L_i与假设灭点VP_j的一致性度量D(L_i，VP_j)小于阈值， 则W_ij＝1；反之，则W_ij＝0。

(2.4)倾向矩阵优化。遍历倾向矩阵，计算直线类之间的Jaccard 距离，将Jaccard距离小于1的两个直线类合并，直到所有直线类之 间的Jaccard距离都为1，从而完成直线段的分类。

(2.5)计算灭点。假设某一类直线有s条直线段，其直线方程为 A_kx+B_ky+c_k＝0，k＝1，2，...，s

则采用最小二乘求出该s条直线求出对应的灭点：

步骤3：计算旋转矩阵，从而得到相机姿态，即ARV的姿态。 通常情况下，在场景中存在着三个互相垂直的方向XYZ，在步骤2 中计算出的灭点VP₁，VP₂，VP₃与之对应。

(3.1)灭点单位化，仅取其方向向量：

之后将方向向量用齐次坐标表示如下：

(3.2)在XYZ三个方向上的无穷远点的齐次坐标分别为：

由相机成像模型则有：

其中K为相机的内参数矩阵，由厂商给出或标定得到。旋转矩 阵与平移向量t组成的矩阵就表示了物理空间坐标系与相机坐标系之 间的对应关系。

由此可得到旋转矩阵R。

根据现有技术中旋转矩阵与姿态角的变换关系，即可得到最终的 姿态角。

以上所述仅为本发明示意性的具体实施方式，在不脱离本发明的 构思和原则的前提下，任何本领域的技术人员所做出的等同变化与修 改，均应属于本发明保护的范围。

Claims (6)

1.一种基于单目视觉灭点检测的ARV姿态测量方法，包括以下步骤：

(1.1)对ARV获取的影像进行直线特征提取；

(1.2)对提取得到的直线进行灭点检测；

(1.3)计算旋转矩阵；

根据旋转矩阵与姿态角的变换关系，即可得到ARV姿态角。

2.根据权利要求1所述的ARV姿态测量方法，其特征在于，对所述影像进行直线特征提取的方法包括：

对影像进行直线特征的初步提取；

对初步提取到的直线段中的断裂、连续短直线进行合并；

删除短直线段。

3.根据权利要求2所述的ARV姿态测量方法，其特征在于，对所述初步提取到的直线段，按照如下准则判断是否需要进行合并：

a.角度：两直线段的方向夹角θ是否小于角度阈值θ_T；

b.垂距：两直线段的中点分别与另一条直线的垂距D₁，D₂是否小于垂距阈值D_T；

当两条直线段同时满足上述两个准则时，则对其进行合并。

4.根据权利要求2所述的ARV姿态测量方法，其特征在于，所述对直线段进行合并的方法包括：

a.确定合并后直线段上一点P：假设合并前两直线段为别为AB，CD，A、B、C、D分别为其端点，其长度分别为l_AB，l_CD，则P点的坐标为

b.确定合并后直线段的角度：直线段AB，CD的角度分别为θ_AB，θ_CD，合并后直线段的角度为

c.确定合并直线段的端点：将原直线段AB、CD分别投影到合并后的新直线上，取其包含的最长直线段，则得到合并后直线段的端点。

5.根据权利要求1所述的ARV姿态测量方法，其特征在于，所述对提取得到的直线进行灭点检测的方法包括以下步骤：

(2.1)生成灭点假设集合：假设提取出的直线段有N条，则从中随机选取M组直线对(两条直线互不相同)，求出每组直线对的交点，即为灭点假设集合，灭点假设集合的大小为M；

(2.2)直线段与假设灭点的一致性度量：对于灭点VP与直线L，假设

为直线L的中心点，a，b为直线段的两个端点，则灭点VP与

的连线为

其一致性度量值为直线段端点a或b到I的垂距：

(2.3)构建倾向矩阵W，其大小为N×M，其中矩阵每一行表示提取到的一条直线段，每一列表示(2.1)中生成的假设灭点，如果直线L_i与假设灭点VP_j的一致性度量值D(L_i，VP_j)小于阈值，则W_ij＝1；反之，则W_ij＝0；

(2.4)倾向矩阵优化：遍历倾向矩阵，计算直线类之间的Jaccard距离，将Jaccard距离小于1的两个直线类合并，直到所有直线类之间的Jaccard距离都为1，从而完成直线段的分类；

(2.5)计算灭点：对于每一类直线，其均对应一个灭点，假设某一类直线有s条直线段，其直线方程为

A_kx+B_ky+c_k＝0，k＝1，2，...，s

则采用最小二乘法可求出该s条直线对应的灭点：

6.根据权利要求1所述的ARV姿态测量方法，其特征在于，所述旋转矩阵的计算方法包括如下步骤：

(3.1)灭点单位化，仅取其方向向量：

其中，j＝1，2，3，VP₁，VP₂，VP₃为所述步骤(1.2)中计算出的灭点，对应场景中三个互相垂直的方向XYZ；

将方向向量用齐次坐标表示如下：

(3.2)在XYZ三个方向上的无穷远点的齐次坐标分别为：

由相机成像模型则有：

其中K为相机的内参数矩阵，由厂商给出或标定得到；由此可得到旋转矩阵R。

Images