技术领域
[0001] 本
发明涉及
机器人领域，尤其涉及
移动机器人导航与
定位技术， 具体为一种基于单目视觉灭点检测的ARV姿态测量方法。
背景技术
[0002] 随着工业自动化程度的提高，各种先进技术和设备的应用愈发普 及，越来越多的工厂和仓储车间由劳动密集型转向技术密集型，实现 了智能化生产和运输，由传统模式向智能化、现代化升级，使得复合 机器人(ARV)在自动化生产和智能物流中的使用呈现爆发式增长， 越来越多的应用于
汽车制造、电
力、
烟草、医疗等行业。
[0003] 导航定位技术是ARV系统中最为核心的技术，目的在于确定ARV 当前的
位置和姿态，将其提供于调度和路径规划系统。目前可用的方 法主要有：通过
激光雷达扫描获得ARV周围环境的点
云数据，通过点 云与已知环境地图配准从而实现ARV的定位，其
精度高，实时性强， 但成本较高；利用
里程计、
加速度计和
陀螺仪，通过航迹推算出ARV 的位置和姿态，数据更新
频率高，短期精度和
稳定性较好，但定位误 差会随时间积累；基于视觉的方法同样受到误差累积的影响，通常采 用闭环检测来减少累计误差，会产生较大的数据冗余。
发明内容
[0004] 本发明针对ARV的姿态估计问题，提出了一种基于单目视觉灭点 检测的姿态测量方法。
[0005] 在透视投影中，灭点是空间中平行直线在无穷远点处的交点，其 对应着影像中的一组直线。而直线特征是影像的一种重要特征，普遍 存在于ARV工作环境中，同时具有很高的稳定性，因此在
位姿估计、
三维重建、目标识别中广泛使用。通常情况下，在ARV工作场景中 存在着三个互相垂直的方向XYZ，基于对单目视觉影像的直线特征 提取计算出的一组三个灭点分别与之对应，通过三个灭点所在的方向 向量的对比关系，能够反映拍摄影像的相机的姿态，也即ARV的姿 态。
[0006] 根据以上原理，本发明提供的基于单目视觉灭点检测的ARV姿态 测量方法，包括以下步骤：
[0007] (1.1)对ARV获取的影像进行直线特征提取；
[0008] (1.2)对提取得到的直线进行灭点检测；
[0009] (1.3)计算旋转矩阵；
[0010] 根据旋转矩阵与姿态
角的变换关系，即可得到ARV姿态角。
[0011] 进一步地，对所述影像进行直线特征提取的方法包括：
[0012] 对影像进行直线特征的初步提取；
[0013] 对初步提取到的直线段中的断裂、连续短直线进行合并；
[0014] 删除短直线段。
[0015] 进一步地，对初步提取到的直线段，按照如下准则判断是否需要 进行合并：
[0016] a.角度：两直线段的方向夹角θ是否小于角度
阈值θT；
[0017] b.垂距：两直线段的中点分别与另一条直线的垂距D1，D2是否小于 垂距阈值DT；
[0018] 当两条直线段同时满足上述两个准则时，则对其进行合并。
[0019] 进一步地，对直线段进行合并的方法包括：
[0020] a.确定合并后直线段上一点P：假设合并前两直线段为别为AB， CD，A、B、C、D分别为其端点，其长度分别为lAB，lCD，则P点的坐 标为
[0021]
[0022]
[0023] b.确定合并后直线段的角度：直线段AB，CD的角度分别为 θAB，θCD，合并后直线段的角度为
[0024]
[0025] c.确定合并直线段的端点：将原直线段AB、CD分别投影到合并 后的新直线上，取其包含的最长直线段，则得到合并后直线段的端点。
[0026] 进一步地，对提取得到的直线进行灭点检测的方法包括以下步骤：
[0027] (2.1)生成灭点假设集合：假设提取出的直线段有N条，则从 中随机选取M组直线对(两条直线互不相同)，求出每组直线对的交 点，即为灭点假设集合，灭点假设集合的大小为M；
[0028] (2.2)直线段与假设灭点的一致性度量：对于灭点VP与直线L， 假设 为直线L的中心点，a，b为直线段的两个端点，则灭点VP与 的连线为 其一致性度量值为直线段端点a或b到I的垂 距：
[0029]
[0030] (2.3)构建倾向矩阵W，其大小为N×M，其中矩阵每一行表 示提取到的一条直线段，每一列表示(2.1)中生成的假设灭点，如 果直线Li与假设灭点VPj的一致性度量值D(Li，VPj)小于阈值，则Wij＝ 1；反之，则Wij＝0；
[0031] (2.4)倾向矩阵优化：遍历倾向矩阵，计算直线类之间的Jaccard 距离，将Jaccard距离小于1的两个直线类合并，直到所有直线类之 间的Jaccard距离都为1，从而完成直线段的分类；
[0032] (2.5)计算灭点：对于每一类直线，其均对应一个灭点，假设 某一类直线有s条直线段，其直线方程为 Akx+Bky+ck＝0，k＝1，2，...，s
[0033] 则采用最小二乘法可求出该s条直线对应的灭点：
[0034]
[0035] 进一步地，旋转矩阵的计算方法包括如下步骤：
[0036] (3.1)灭点单位化，仅取其方向向量：
[0037]
[0038] 其中，j＝1，2，3，VP1，VP2，VP3为所述步骤(1.2)中计算出的灭 点，对应场景中三个互相垂直的方向XYZ；
[0039] 将方向向量用齐次坐标表示如下：
[0040]
[0041] (3.2)在XYZ三个方向上的无穷远点的齐次坐标分别为：
[0042]
[0043] 由相机成像模型则有：
[0044]
[0045] 其中K为相机的内参数矩阵，由厂商给出或标定得到；由此可 得到旋转矩阵R。
[0046] 本发明通过提取单张影像上的灭点作为全局观测量，对ARV的姿 态进行修正。与
现有技术相比，这种方法的优势在于：无需在物理环 境中布设精密的标记点，而充分利用物理环境中的垂直结构，仅需利 用ARV自身搭载的单目相机，即可获得ARV相对于物理空间
坐标系的 姿态。
[0047] 应了解的是，上述一般描述及以下具体实施方式仅为示例性及阐 释性的，其并不能限制本发明所欲主张的范围。
附图说明
[0048] 下面的附图是本发明的
说明书的一部分，其绘示了本发明的示例
实施例，所附附图与说明书的描述一起用来说明本发明的原理。
[0049] 图1为根据示例性实施例的基于单目视觉灭点检测的ARV姿态 测量方法
流程图。
具体实施方式
[0050] 现详细说明本发明的多种示例性实施方式，该详细说明不应认为 是对本发明的限制，而应理解为是对本发明的某些方面、特性和实施 方案的更详细的描述。
[0051] 在不背离本发明的范围或精神的情况下，可对本发明说明书的具 体实施方式做多种改进和变化，这对本领域技术人员而言是显而易见 的。由本发明的说明书得到的其他实施方式对技术人员而言是显而易 见得的。本
申请说明书和实施例仅是示例性的。
[0052] 如附图1所示，根据示例性实施例的基于单目视觉灭点检测的姿 态测量方法，主要包括以下步骤：
[0053] 步骤1：对ARV所获影像的直线特征提取。
[0054] 现有的直线特征提取方法较多；在示例性实施例中，为了获得稳 定的直线特征，采用以下方法完成直线特征提取：
[0055] (1.1)进行直线特征的初步提取。
[0056] 优选采用LSD
算法进行直线特征的初步提取。该算法具有很高 的实时性、稳定性和准确性，计算效率高，无需设置过多参数。该算 法包括梯度幅值和梯度方向估计、提取直线
支撑区域、矩形逼近支撑 区域和直线检测四个步骤。
[0057] 当然，本领域技术人员也可采用其他现有算法完成直线特征的初 步提取。
[0058] (1.2)直线特征精化。由于影像噪声、遮挡和提取算法误差的 影响，现有的各种算法提取的直线特征会出现断裂、连续短直线等， 因此对在方向、垂距上相近的直线进行合并操作。
[0059] 优选按照如下准则判断是否进行合并：
[0060] a.角度：两直线段的方向夹角θ是否小于角度阈值θT；
[0061] b.垂距：两直线段的中点分别与另一条直线的垂距D1，D2是否小于 垂距阈值DT；
[0062] 当两条直线段同时满足上述两个准则时，则对其进行合并。
[0063] 优选按照如下方法确定合并后的直线段：
[0064] a.确定合并后直线段上一点P。假设合并前两直线段为别为AB， CD，A、B、C、D分别为其端点，其长度分别为lAB，lCD，则P点的坐 标为：
[0065]
[0066]
[0067] b.确定合并后直线段的角度。直线段AB，CD的角度分别为 θAB，θCD，合并后直线段的角度为：
[0068]
[0069] c.确定合并直线段的端点。将原直线段AB,CD分别投影到合并 后的新直线上，取其包含的最长直线段，则得到合并后直线段的端点。
[0070] (1.3)如果合并后的直线段的长度大于长度阈值lT，则将其保留； 否则将其删除。
[0071] 完成后，即可得到稳定的直线段特征。
[0072] 步骤2：对提取到的直线段进行灭点检测。实际上，灭点的检测 对应着影像上直线的分类。对于每一类直线，其均对应一个灭点。
[0073] 同样，现有可用的直线分类方法较多；在示例性实施例中，采用 多模型分类方法J-Linkage算法对提取到的直线段进行分类，然后再 进行灭点的计算。具体包括以下步骤：
[0074] (2.1)生成灭点假设集合。假设在步骤1中提取出N条直线段， 则从中随机选取M组直线对(两条直线互不相同)。求出每组直线对 的交点，即为灭点假设集合，灭点假设集合的大小为M。
[0075] (2.2)直线段与假设灭点的一致性度量。对于灭点VP与直线L， 假设 为直线L的中心点，a，b为直线段的两个端点，则灭点VP与 的连线为 其一致性度量为直线段端点a或b到I的垂距：
[0076]
[0077] (2.3)构建倾向矩阵W，其大小为N×M。其中矩阵每一行表 示步骤1中提取的一条直线段，每一列表示步骤(2.1)中生成的假 设灭点。如果直线Li与假设灭点VPj的一致性度量D(Li，VPj)小于阈值， 则Wij＝1；反之，则Wij＝0。
[0078] (2.4)倾向矩阵优化。遍历倾向矩阵，计算直线类之间的Jaccard 距离，将Jaccard距离小于1的两个直线类合并，直到所有直线类之 间的Jaccard距离都为1，从而完成直线段的分类。
[0079] (2.5)计算灭点。假设某一类直线有s条直线段，其直线方程为 Akx+Bky+ck＝0，k＝1，2，...，s
[0080] 则采用最小二乘求出该s条直线求出对应的灭点：
[0081]
[0082] 步骤3：计算旋转矩阵，从而得到相机姿态，即ARV的姿态。 通常情况下，在场景中存在着三个互相垂直的方向XYZ，在步骤2 中计算出的灭点VP1，VP2，VP3与之对应。
[0083] (3.1)灭点单位化，仅取其方向向量：
[0084]
[0085] 之后将方向向量用齐次坐标表示如下：
[0086]
[0087] (3.2)在XYZ三个方向上的无穷远点的齐次坐标分别为：
[0088]
[0089] 由相机成像模型则有：
[0090]
[0091] 其中K为相机的内参数矩阵，由厂商给出或标定得到。旋转矩 阵与平移向量t组成的矩阵就表示了物理空间坐标系与相机坐标系之 间的对应关系。
[0092] 由此可得到旋转矩阵R。
[0093] 根据现有技术中旋转矩阵与姿态角的变换关系，即可得到最终的 姿态角。
[0094] 以上所述仅为本发明示意性的具体实施方式，在不脱离本发明的 构思和原则的前提下，任何本领域的技术人员所做出的等同变化与修 改，均应属于本发明保护的范围。
