基于双目视觉的电力机器人三维定位方法

李聪利，徐善军，任书楠*，李 崧，刘胜军

（1.国网天津市电力公司，天津 300010；2.北京国电富通科技发展有限责任公司，北京 100070；3.国网电力科学研究院有限公司，南京 211106）

0 引言

电力机器人技术对于提升电力系统自动化运维水平具有重要意义，目前已在输电、变电、配电、用电等领域取得积极进展。为实现电力机器人自主化作业，其对作业目标位姿的精准定位则显得尤为重要。考虑到电力作业场景对感知定位系统要求的相似性，即具有全天候、室外工作、精准感知定位能力的特点，本文以用电环节中的自动充电机器人为例开展论述，相关结论亦可推广至其他领域。自动充电是电动汽车未来大面积推广应用的关键技术。为适应的电动汽车的自动化、智能化、无人化的发展趋势，电动汽车配套相关的基础设施的智能化也受到电动汽车研究机构广泛关注，而作为电动汽车的能源补给的充电装置，发挥着至关重要的作用。为了能够更加快速、便捷的给电动汽车充电，智能化的全自主充电技术是大势所趋，全自主充电要求充电机器人能够跟车辆配合，在不需要人工干预下，自主完成充电作业，其中对充电口所在位置和空间姿态的检测则是整个自动充电技术中的核心步骤。

当前普遍研究的电动汽车充电口的定位技术，大都是以充电口的结构特点为基础，通过直流充电口或者交流充电口进行椭圆检测，来实现充电口的精准定位[1～14]。但由于椭圆检测方法语义模糊的特点，在针对具有同心圆特征的充电孔检测时，容易出现内外圆混淆问题。同时，基于单目摄像机的充电口检测方法，往往由于尺度问题，当检出椭圆存在误差时，往往无法确定检出该椭圆是内圆还是外圆，导致在实际应用中，算法的鲁棒性较差。部分检测算法不对充电孔进行细粒度区分，忽略了充电孔之间的形状约束，造成精度降级。

本文将双目相机引入到充电口检测应用中，提出基于双目视觉的电动汽车充电口三维定位方法，使用双目视觉的对极几何约束，提取双目图像中语义唯一的充电孔中心点作为特征，对充电孔之间的三维形状先验进行精确建模，显著的提高了算法的定位精度和鲁棒性。

本文的创新贡献包括：1）提出了一种基于双目视觉的电动汽车充电口三维定位方法，通过在双目图像中充电口特征点的检测，实现对充电口三维位置的感知；2）利用直流快充型充电口三维结构先验进行建模，并利用ICP算法，将感知到的充电口关键点与先验结构关键点进行配准，实现对充电口三维姿态的精确估计；3）通过采集双目充电口数据进行检测精度实验，验证本文方法的精确性和鲁棒性。

1 相关研究

1.1 充电口检测方法

充电口检测是一种典型的视觉目标检测问题，当前有两类较为典型的求解方法，分别是基于图像灰度值的方法、基于特征点的方法等。基于图像灰度值的检测方法[1]，以滑窗在待检图像上与模板图像进行匹配，进而输出检测结果，该类方法对光照很敏感，且在处理目标部分遮挡问题上存在不足。而基于特征点的充电口检测方法[2]，首先根据目标的表观特点提取图像特征，包括利用SIFT特征[5]，以及利用SURF特征[6]等，并用算法迭代对特征点进行匹配，进而优化求解这些匹配特征点之间的最优匹配关系，这类方法对光照和部分遮挡不敏感，但在充电口实际检测处理过程中，特征点的定义与描述依据人工经验，往往存在主观感受的偏差，因而检测失败率依然较高，且特征点计算量过大，影响算法实时性。基于深度学习的检测方法[3,4]，鲁棒性较好，但是需要采集大量的实物图片并进行标注，前期准备工作量大，同时对于没有针对性设计的检测网络，算法稳定性不好保证。

1.2 基于椭圆检测的充电口定位方法

基于椭圆检测的充电口定位方法，是属于基于人工设计特征点的视觉检测方法，该类方法依据设计人员对充电口的感受，将椭圆作为检测的表观特征，进而利用算法对充电口进行自动检测。文献[7]使用Halcon中的模板匹配方法直接匹配图像中的充电孔椭圆形位置，当充电口姿态存在较大偏移时，会因为各种图像形变导致检测困难，进而造成匹配误差较大。霍夫变换是一种形状检测的常用方法，文献[8～10]中，基于霍夫变换的椭圆检测利用霍夫变换将图像空间向参数空间映射完成椭圆检测，但是这种方法参数高达5维，计算量大的原因导致不适合实时在线的充电口检测。文献[11]提出一种定位精度达到亚像素级的改进方法，降低了随机霍夫变换的无效采样。文献[12]对霍夫变换参数空间的计算进行了优化，但是这一类算法无法避免计算量大，耗时长的先天缺陷。进一步，研究者们将用点来拟合椭圆，提升为从曲线弧的角度拟合椭圆。文献[13]先检测目标轮廓，再利用轮廓寻找连续区域的方法去检测椭圆弧，文献[14]提出基于不变特征数的椭圆弧段检测，进而选择合适椭圆弧段来拟合椭圆。文献[15]更加详细地提出了一种基于分割曲线弧段的方法，通过判断边缘是否相邻，连接相邻边缘组成有效弧段，进而产生椭圆检测结果。文献[16]提出结合差分函数与因子矩阵的椭圆检测方法，该方法加强边缘的信息检测精度，但会拟合出大量的假椭圆，造成性能降级。文献[17]提出基于快速聚类有效弧段的椭圆检测方法，通过有效弧段参数拟合获得椭圆，但该类方法在低光照或不理想位置条件下，检测性能较差。

1.3 基于关键点的检测方法

本文提出了基于2D-3D关键点的充电口检测方法。关键点检测是计算机视觉中的经典问题，已经在人脸识别系统中被广泛应用。当前关键点检测方法大致分为两类，分别是基于形状、外观模型的传统方法[18～20]和基于深度学习的方法[21～23]。ASM(Active Shape Model)[18]是由Cootes于1995年提出的经典的人脸关键点检测算法，主动形状模型即通过形状模型对目标物体进行抽象，ASM是一种基于点分布模型（PDM）的算法。在PDM中，外形相似的物体，例如人脸、人手、心脏、肺部等的几何形状可以通过若干关键点的坐标依次串联形成一个形状向量来表示。ASM算法需要通过人工标定的方法先标定训练集，经过训练获得形状模型，再通过关键点的匹配实现特定物体的匹配。AAM[19,20]在ASM进行改进，不仅采用形状约束，而且又加入整个脸部区域的纹理特征。Zhang等人提出一种多任务级联卷积神经网络[21]用以同时处理人脸检测和人脸关键点定位问题，但是此类基于深度学习的方法[22,23]不仅计算复杂，而且需要标注大量的训练数据。

2 本文方法

针对当前充电口检测与定位方法的局限性，本文提出了基于双目视觉的电动汽车充电口三维定位方法。方法的整体流程如下：

第一步，对输入的双目图像进行预处理和双目校正，使得校正后双目图像垂直方向视差为零，水平方向视差完全由目标的深度决定。

第二步，对输入双目图像分别提取充电口在图像坐标系下的2D位置信息，提取方法为，先提取图像中的边缘轮廓，在物体边缘轮廓图上识别充电口的初始位置；而后提取边缘梯度特征，利用该特征进行充电口模板匹配，并将充电口图像块裁剪出来；最后基于充电口图像块进行定位，定位算法初始化各充电孔中心点位置，然后利用充电孔中心约束求解2D中心点精确位置。

第三步，根据立体匹配原理计算出双目图像中对应点的3D空间位置，同时，利用ICP算法将其与充电口3D先验模型进行匹配，获得高精度的充电口位姿。整体流程图如图1所示。

图1 充电口检测与定位流程

2.1 双目图像预处理

本文提出使用双目图像检测充电口的精确位姿，因而应首先对双目图像依据标定参数进行矫正、去畸变和降噪等预处理操作。通过上述操作，得到标准的双目视觉图像。本文标准双目图像中，垂直方向视差为零，水平方向视差完全由目标的深度决定，可根据水平方向视差，精确计算得到目标像素的精确三维位置。

2.1.1 双目图像矫正

通过双目相机标定的程序，可以获取了双目相机系统中，两个摄像机各自的内参矩阵和畸变系数，以及两个相机旋转到平行时各自的旋转矩阵。实际操作中，Opencv和MATLAB都提供了双目标定工具箱，利用标定数据，可进行去畸变或者双目平行校正。本文采用的去畸变的程序和双目平行校正方法如下：

去畸变的过程就是针对单目相机进行的变换，主要用来去除相机因光学参数不精确造成图像存在的径向和切向畸变，而平行校正就是针对双目相机系统的操作，使得校正后图像只存在水平方向视差。单目相机内参标定可参阅相关文献。在双目标定中，需要构建一个两个相机共同的理想内参矩阵。

双目摄像机通过各自的旋转投影矩阵，将其二者产生的图像变换到理想内参矩阵建模的图像中，即得到水平校正后的双目图像，校正方法为：1）分别将两个图像的像素坐标系通过共同的内参矩阵转换到相机坐标系；2）分别对两个相机坐标系进行旋转得到新的相机坐标系，通过左乘旋转矩阵R1和R2，其中R1，R2为双目摄像机各自的旋转投影矩阵；3）针对新的相机坐标分别进行左、右相机的去畸变操作；4）去畸变操作结束后，分别用左、右相机的内参矩阵将左、右两个相机坐标系重新转换到左、右图像像素坐标系；5）并分别用左、右源图像的像素值对新左、右图像的像素点进行插值。

2.1.2 图像预处理与边缘提取

实际环境中，由于光照变换、背景复杂，拍摄的充电口双目图像有大量的噪声，显著降低检测算法性能。因而需要对输入图像进行去噪处理，并提取图像封闭轮廓，本节主要包括图像滤波和Sobel边缘提取。

滤波操作去除图像中的噪声，减少环境噪声对后续图像数据处理的影响，由于灰度图像有利于提取图像梯度，本文首先将输入图像灰度化，再使用高斯滤波来去除图像噪声。具体操作是：用一个高斯卷积核与图像中的每一个像素做卷积，以卷积核覆盖的邻域内像素的加权平均灰度值取代中心像素点的值，过滤后的图像为I(x，y)。

为提取出图像中包含充电口区域的轮廓，首先对图像进行边缘提取。图像边缘指图像像素值突变的地方，通常是通过该像素邻域的梯度信息进行判断，其目标是能够完整地提取图像中的所有目标轮廓的边缘位置。由于获取的边缘在图像可作为显著性特征，因而边缘提取后，图像背景将变得单一，有利于后续目标检测。本文使用Sobel算子提取双目图像的一阶梯度信息，进而对双目图像分别分割出图像边缘。

Sobel边缘提取算法通过2个卷积核与输入图像不断进行卷积运算，获得像素i在x方向和y方向上的梯度值和，从而计算出像素梯度值Gi及梯度方向ai，计算方法见式(1)～式(4)。

2.2 基于形状结构特征的充电口检测方法

2.2.1 梯度一致性的充电口视觉检测方法

在获取目标物体的边缘轮廓双目图像后，本文提出基于梯度一致性的充电口视觉检测方法，该方法使用对物体边缘的梯度及梯度方向进行特征匹配，构建基于圆弧特征的目标检测器。检测器的构建过程分为训练和验证两个阶段。

训练阶段：输入模板图像如图2所示，为适应不同距离和角度情况下对充电口的检测要求，本文对模板图像进行多尺度旋转和缩放，从而建立以图像旋转角度、缩放尺度和模板特图像梯度信息为变量的充电口模板特征库。

图2 充电口模板

验证阶段：输入为经过边缘轮廓提取的充电口双目图像。验证过程中，通过检测器分别提取双目图像的全局梯度信息，然后使用特征库中的模板特征在输入图像上进行滑窗搜索，计算模板特征与对应位置的梯度特征之间的余弦相似度矩阵，得到相似度矩阵中最大值对应的模板信息，并输出充电口位置。

2.2.2 基于椭圆中心点的二维充电孔中心定位方法

当前基于椭圆检测的充电口定位方法，因考虑透视成像问题，将充电孔建模成椭圆形状，利用检测椭圆技术确定椭圆的边缘，然后利用椭圆边缘的2D精确坐标和各种3D-2D投影约束，来解算充电孔的三维位置。该类方法有两个问题，首先，充电孔的物理结构导致其有两个同心圆结构，当检测算法不能鲁棒检测出两个圆时（或多或少），就无法判断检测出的是内圆或者外圆，导致利用椭圆边缘的算法不稳定；同时，这些方法只利用了充电孔自身的几何特性，并没有对充电孔位之间的几何关系进行显示建模。

本文提出基于椭圆中心点及其几何关联的充电孔定位方法，直接预测每个充电孔椭圆的圆心，虽然椭圆圆心本质上并不存在，没有物理实体，但因为其是多个同心圆共同的圆心，当检测算法检出多个圆形时，可以很方便的进行融合，提升系统的鲁棒性。同时，本文使用预建充电口模型描述充电孔各特征之间的几何约束，确保了检测算法的鲁棒性。本文方法主要包括2个步骤：1）构建基于椭圆圆心的充电口形状模型，然后对充电口形状模型中每个充电孔中心邻域位置上进行搜索匹配，得到概率响应图；2）以概率响应图中极值点作为候选点，并根据充电孔几何关联，计算各中心点的位置。

2.3 基于形状先验的充电口三维位姿估计

2.3.1 三维充电口形状模型构建

本文提出向量化的三维充电口几何表征，将充电口关键特征点的坐标依次串联形成一个形状向量来表示。向量化的三维几何表征能够适应于高效矩阵运算，方便后续充电口的检测和配准操作。具体表征如下：

图3 直流快充充电口特征点分布

本文采集了M个三维直流快充充电口图像，充电口包含9个特征孔，如图3所示，将9个充电孔同心圆的圆心作为特征点，每个特征点的坐标为，(xk,yk,zk)，k∈[1,9]因此充电口三维形状特征向量表示为：

为去除M个三维直流快充口的个体偏差，本文将M个充电口求平均得到充电口的标准三维形状，计算方法为：

其中，表示第i个充电口对应的充电口形状向量。

考虑图像中充电口形状受到旋转、缩放和平移的影响，本文提出充电口形状模型变形函数，如式(7)所示。

θ={s,R,t,q}为表示缩放、旋转、平移和主成分权重的参数向量；

x-表示充电口形状的平均特征向量，由式(6)在少量标注的数据集中统计获得；

φ表示形变主成分矩阵，计算方法为：对于数据集中每个充电口形状特征向量减去平均特征向量得到形状变换矩阵X，均值为0。

对形状变换矩阵X 进行主成分分析（Principal Component Analysis，PCA），即特征向量P和对应的特征值λ，选择前K个特征值的特征向量，按列排布组成形变主成分矩阵φ，本文中K选取为动态阈值，选取为特征值从大到小排列后，总能量超过90%的特征值对应的特征向量。

2.3.2 基于概率图模型的充电孔三维中心点定位方法

基于双目图像的视差信息，可以得到充电孔中心点的三维位置，方法如下，设左目图像的充电孔中心为，右目图像的充电孔中心，根据双目立体几何原理，可计算出中心点i的三维坐标：

其中，fx,fy,cx,cy为左目相机的内参，di为中心点i的视差。

接下来，本文提出基于概率图模型的充电孔中心点三维位置回归方法，该方法分为训练和验证两个阶段：

训练阶段，求解所有中心点都匹配最优时模型参数的最大似然估计问题。首先，考虑到充电孔中心点空间位置概率分布是相互独立的，列出似然函数如式(11)所示：

式(11)中，左边为已知匹配点和充电孔三维位置描述时，模型参数的似然函数，右边是对该似然函数进行条件独立分解。其中θ为需要求解的参数，li∈{1,-1}表示特征点i是否为有效匹配点。通过求解上述似然函数的最大似然估计，从而得到参数θ，即得到充电孔的最优位置估计。

测试阶段，以充电口形状模型为基准建立每个充电孔中心点的初始特征，通过式(12)对特征点三维邻域进行评分，得到三维评分响应，响应分数最大值所对应的位置即为充电孔中心空间位置。

2.3.3 基于双目相机三维信息的充电口位姿估计

本文使用基于随机采样一致方法（RANSAC）改进ICP算法，将上式求出的充电孔三维坐标与充电口3D模型进行对应点匹配，计算单应性矩阵，得到充电口在相机坐标系下的位姿。具体如下：

首先，通过反复选择检测到的充电口三维位置中的一组随机子集来与充电口三维先验进行ICP匹配。被选取的子集被假设为局内点，并用下述方法进行验证：

1）有一个子集适应于预设的充电口三维先验，即所有的未知位姿参数都能从该子集计算得出。

2）用1）中得到的位姿模型去测试所有的其它检测到充电口位置的子集，如果某个点适用于估计的位姿模型，认为它也是局内点。

3）如果有足够多的点被归类为假设的局内点，那么估计的模型就足够合理。

4）然后，用所有假设的局内点去重新估计模型，因为它仅仅被初始的假设局内点估计过。

5）最后，通过估计局内点与位姿模型的错误率来评估模型。

上述过程被重复执行多次，本文为50次，根据错误率最小值来得到最终的充电口三维位姿。

3 实验结果与分析

3.1 充电口检测实验结果与分析

为测试基于双目视觉的电动汽车充电口三维定位方法的准确性和鲁棒性，本文收集了300张包含充电口不同位姿不同光照背景的双目图像作为测试集，并对比了基于单目检测方法与本文提出的基于双目方法在验证集上的准确率，准确率定义为检测出的充电口位置包含完整的充电口图像块。实验结果如表1所示。

表1 充电口检测准确率、误检率对比

由表1可知，本文提出的方法检测准确率在验证集上，以较大差距超过基于单目的充电口检测方法，在误检率指标上也显示出较好的性能。

3.2 充电口定位实验结果与分析

3.2.1 数据集构建

为了训练充电口形状模型及中心点回归模型，本文收集并标注了600张包含充电口不同位姿、不同光照背景的图片作为实验数据集，其中训练集300张，测试集300张。数据集标签中涵盖充电孔编号和中心点位置，中心点的位置采用标注圆的圆心位置，减小标注误差。标注样例如图4所示。

图3 数据集标注样例

3.2.2 评估方法

为了评估提出方法的有效性，本文提出平均绝对距离误差（MADE）指标对方法的性能进行评估。MADE的计算方法如下：

其中x的量纲为毫米。

3.2.3 实验结果与分析

本文测试了提出方法在测试集上的平均绝对距离误差，本文首先对所提方法在预测每个中心点位置的误差水平进行验证，测试结果如表2所示。

表2 提出方法对每个充电孔中心的预测误差

由表2可知，本文提出的方法在预测目标位置时位置误差MADE均小于1个像素，根据像素与实际距离的对应关系计算，可以满足充电口位姿定位精度要求。进一步地，相较于基于椭圆边缘检测的方法，本文所提方法性能也有较大提升，对比结果如表3所示。

表3 与基于椭圆检测的方法对比

由表3可以看出，本文所提方法在定位中心点位置时的误差显著低于基于椭圆边缘检测的方法，也低于本文方法的单目视觉检测版本，说明了本文方法的二维定位算法精度更高，同时双目结构的冗余性带来进一步的精度提升。

4 结语

充电口检测定位是电动汽车自动充电中的关键性使能技术，充电口检测定位需要确定充电口高精度的三维位置，来指导充电机器人进行作业。本文提出了基于双目视觉的电动汽车充电口三维定位方法，通过使用充电口三维模型建立训练数据并初始化充电孔中心位置，然后通过双目视觉对极几何约束得到充电孔中心点的三维位置坐标，并利用ICP方法对中心点位置进一步优化，获得精确的充电口三维位姿信息。实验结果表明，本文提出的方法提高了充电口定位的准确率和鲁棒性，且可应用于电力领域其他作业目标的检测定位，具有较强的通用性。