NAO机器人的快速迷宫拐点测距及航向矫正

朱小波，车 进 ，郝志洋 ，刘 毅 ，杭 立

1.宁夏大学 物理与电子电气工程学院，银川 750021

2.宁夏沙漠信息智能感知重点实验室（宁夏大学），银川 750021

1 引言

随着计算机视觉技术的不断发展，其现代智能化越来越高，它广泛应用于各行各业。利用机器视觉走迷宫的问题也是当今研究热点之一[1-2]。文献[3]以NAO机器人为实验平台，利用AdaBoost机器学习和HOG特征检测来获得识别区域的导航线，但该算法对室内光线变化的鲁棒性以及算法实时性不好。文献[4]提出了利用移动机器单目视觉测距的方法，此方法主要使用Hough变换进行目标检测，由于距离目标远近不同时，图像尺度也不同，因此该方法不适应于复杂环境。文献[5]提出了利用MoZAK算法来统计图像边缘复杂度，进行单目测距。物体在不同的距离时，其边缘复杂度是不同的。但当图像背景较复杂时不能较好地得到边缘信息。文献[6]利用NAO Marks作为机器人通过视觉识别的一种专门标志，来解决NAO机器人识别拐点问题，但是该方法速度较慢，也没有解决航偏矫正的问题。文献[7]提出了航向矫正和机体侧移相结合的方法来解决机器人航向偏移问题，该算法在实现时比较耗时。此外国内外测距的主要方式还有毫米波雷达测距、超声波测距、激光测距仪等，这些方法在室内也容易受到较大的干扰。

目前基于图像测距的方法主要是无人驾驶领域，而在室内这种复杂的环境下研究得较少，所涉及到航向矫正的一些算法也比较容易受到干扰，造成功能不稳定，而在室内图像测距的方法通常运算量较大，比较复杂，不够快速。针对以上问题，提出一种快速测距以及航偏矫正算法。将所提出的方法与当前已有的算法进行了深入的对比与分析，并探讨了本文方法的优势以及不足的地方。

2 拐点检测

由于迷宫在室内搭建，因此环境干扰因素比较大，尤其是光线干扰。阴影、反光、颜色相近的物体多等等构成了图像处理程序不稳定的因素。因此程序的编写选择在最坏的情况下，以此来增加算法的鲁棒性和健壮性[8]。

在拐点检测时需要对获取的图像进行预处理，通过NAO摄像头获取迷宫中导航线的RGB图片，对其平滑滤波，分离各分量，通过颜色空间转换获得HSI各分量[9-10]。其中HSI颜色空间可以解决光照强度带来的影响。然后分别获得HSI各分量的边缘强度再进行合成，此时获取初步处理后的导航线。下一步通过非极大值抑制以及阈值选取获得二值化图像。最后通过直线检测算法检测直线并求图片内各直线的交点，所求交点便是目前机器人遇到的拐点。实现过程如图1所示。

图1 拐点检测流程图

3 测距几何模型推导

3.1 成像几何模型构建

NAO拍摄到的迷宫路径图片是三维空间向二维空间的转换，因此按照摄像机成像原理构建理想的成像几何模型[11-14]。如图2所示为构建的简化几何模型。图中平面IBDG代表实际的路面，梯形平面JBDF为摄像机拍摄到的路面区域。A点为摄像机的光心，AM为光轴，M点为光轴与路面交点，同时也是梯形JBDF的对角线交点，T为迷宫中的拐点。AN为NAO机器人光心到路面的高度。平面dfjb为摄像机投影平面，以像平面的中心m点为坐标原点建立如图2所示的像平面坐标系xmy。以M点为坐标原点建立路面坐标系XMY。

图2 摄像机投影几何模型

由于上面的立体几何模型推导比较复杂，因此需要对上述的几何模型继续建模简化[11-14]，将其放在二维平面推导。在纵轴方向的模型如图3所示；由于本文的测距推导方法为直接测得图2中NTY的距离，以及测量NT的距离（NAO距离拐点的距离），因此只需要建立纵向模型。为了更加利于推导还需继续简化模型[11-14]。如图3（b）所示。

图3 纵轴方向建模

3.2 测距几何推导

距离推导分为两种情况：一种是迷宫拐点在光轴的前方；另一种是拐点在光轴的后方。下面分别进行推导，推导的距离均为图2中NTY的距离。

（1）迷宫拐点在光轴前方时，如图4所示，任何一拐点，都可以过拐点TY作与光轴垂直的直线。A点为摄像机光心，h为光心距地面高度（NAO摄像机光心到NAO脚底平面的高度），TY为拐点T在路平面Y轴上的投影。 fe为投影平面，fe垂直于AM交AM于r点，t′为拐点在成像平面上的投影。2α为摄像机垂直方向视角。θ为NAO头部的俯仰角。NAO拍摄到的图片大小为M×N，即 fe=M。由以上条件可得：

其中 yt′为拐点在像平面的 y轴坐标，由式（1）和式（2）可得：

又可得：

NTY距离为：

图4 拐点在光轴前方

（2）迷宫拐点在光轴后方时，如图5所示。

图5 拐点在光轴后方

图5中的几何关系和条件与（1）中的相同，可得：

由式（6）和（7）可得：

NTY距离为：

4 测距与矫正相结合模型

NAO在迷宫中巡线行走时，由于自身等因素会导致NAO行走发生偏差，需要对其行走方向进行矫正。以下测距与矫正相结合的办法解决了NAO行走缓慢及航偏问题，同时避免了先求特定点坐标再求距离一类算法的复杂性。模型如图6所示。AB、CD代表迷宫墙壁，平面HEFG为拍摄的画面，JQ为机器人正对方向，即图像的垂直平分线。J、Q均为中点。KM、PN为机器人在迷宫中拍摄到的拐点及导航线在图像中的两种情况，M、N为两情况下的拐点。PN交HG为P，KM交JQ为K。NK、ML垂直于JQ。I点为NAO机器人脚底位置。

图6 测距及矫正模型

由于这两种情况的推导方法相同，所以这里选择拐点N来推导。N、P、J坐标可以很方便地求出来。分别为 (xN,yN)、(xP,yP)、(xJ,yJ)。可得：

又由以上条件可得ΔIJP与ΔIKN相似，所以可得：

其中 IK=IJ+JK 。由式（10）和（11）可得：

式（10）到式（12）中使用的长度均为像素长度。θ0即为需要矫正的偏转角。下面将推导IN对应的实际距离。从上面距离的推导已经知道IK对应的实际距离已经知道。所以可得NAO距离拐点的实际距离即为IN。以下的线段长度均为世界坐标系下的实际长度。其长度为：

将式（12）代入式（13）即可。所以只要控制NAO向拐点方向转动θ0角度即可以实现航向矫正。矫正完后只需要沿着旋转后的方向向前沿直线走IN的距离即可到达拐点，然后按照需要进行拐弯。而这里只需要给出NAO头部的俯仰角θ即可以测距和矫正，而θ可以通过内部函数获得。这就完全实现了快速自主测距及航向矫正。

5 实验结果与分析

5.1 成像几何模型构建

为了验证所提算法的有效性，以NAO机器人和室内搭建的迷宫作为实验对象，采用python作为编程语言，NAO在迷宫中行走时通过摄像头获取拐点图像，并通过本文所用的拐点检测算法对图像处理，快速获得拐点坐标，如图7所示。

图7 获取拐点

在图7中（a）为获取的原图，可以看出受光线影响较大，因此采用上文提到的算法将其二值化，如图（b）所示。从图中可以很好地看出算法处理二值图像较好。图（c）为对图（b）优化后进行拐点检测。图中红色即为检测到的拐点。

5.2 测距及矫正结果分析

实验中迷宫大小为360 cm×360 cm，拍摄高度为0.447 m，俯仰角为 45°，垂直视角为 2α=34.80°，NAO拍摄的图片大小为640×480。本实验使NAO在不同的拐点处进行测试，除此之外还和别的算法进行了比较。T1代表使用参考论文[5]中算法走完整个迷宫所用的时间，T2代表用本文的算法所使用的时间，两种方法均进行了5次测试。如表1所示。

表1 测试结果

实验结果表明，所提出的算法能按照要求顺利快速地走完迷宫。经过计算，由几何推导得到的角度、距离理论数据和实际测得的数据相比较，在航偏矫正的错误率低于0.4%，而行走速率提升了约34.8%。和文献[5]所提出的算法相比较，本文所提出的算法更快。因此本文所提出的算法是可行的，节约了时间成本，具有较好的应用价值。

6 结束语

本文提出的基于单目视觉测距改进算法，主要分为三部分。第一部分在复杂的环境中快速地获取迷宫中的拐点坐标；第二部分利用图像几何关系测得机器人当前位置距离拐点的位置；第三部分利用与到航线间的夹角进行航偏矫正。由实验数据可以看出该方法明显提升了NAO机器人在迷宫中的行走速度。同时也避免了求特定点坐标算法的复杂性。因此所提方法是可行与准确的。

：

[1]梁付新，刘洪彬，张福雷，等.基于NAO机器人的目标识别方法[J].计算机工程与设计，2017，38（8）：35-39.

[2]王丹丹，宋怀波，何东健.苹果采摘机器人视觉系统研究进展[J].农业工程学报，2017，33（10）：59-69.

[3]孙翔侃，白宝兴.基于机器学习的NAO机器人检测跟踪[J].长春理工大学学报，2016，39（2）：116-119.

[4]Wahab M N A，Sivadev N，Sundaraj K.Development of monocular vision system for depth estimation in robotsoccer[C]//Sustainable Utilization and Development in Engineering and Technolgy，2011：36-41.

[5]Haris S M，Zakaria M K，Nuawi M Z.Depth estimation from monocular vision using image edge complexity[C]//Advanced Intelligent Mechatronics，2011：868-873.

[6]张婷.基于NAO的迷宫解决算法研究[J].国外电子测量技术，2013，32（5）：68-71.

[7]白雪冰，车进，赵婧.基于NAO的迷宫航偏校正方法[J].计算机工程与设计，2017，38（6）：57-61.

[8]苗姣姣，徐望明.HSV空间和形态学处理相结合的车牌定位方法[J].电视技术，2015，39（5）：146-149.

[9]郭佳，刘晓玉，吴冰，等.一种光照不均匀图像的二值化方法[J].计算机应用与软件，2014，31（3）：183-186.

[10]金浩阳.基于彩色深度图像的室内人体跌倒检测[D].广东深圳：深圳大学，2017.

[11]Li Deren，Liu Yong，Yuan XiuXiao.Image-based selfposition and orientation method for moving platform[J].Science China Information Sciences，2013，56：1-14.

[12]Jurek C，Evangelos K，Dominik P，et al.Patrolling by robots equipped with visibility[C]//21stInternational Colloquium，2014：224-234.

[13]王文波.基于单目视觉的实时测距方法研究[D].辽宁大连：大连理工大学，2014.

[14]胡荣春，杨涛，马娟.视觉巡线机器人中的摄像头图像透视校正方法[J].计算机与数字工程，2012，40（6）：104-108.