基于三维模型的定位算法研究

摘 要： 为使清洁机器人在清扫的房间中能快速确定自己的位置，根据广角摄像头采集到的墙角画面图像中墙角的三条棱线对应的角度关系，来确定清洁机器人离墙面的距离，进而确定在房间中的位置。首先对广角摄像头采集到的畸变图像进行校正，再识别提取出墙角棱线，最后依据测量出来的房间的高度和位置信息建立三维模型并利用神经网络模拟数学关系实现定位。仿真与实际数据对比表明，此算法稳定，能够满足清洁机器人的定位需求。

关键词： 清洁机器人； 三维建模； 墙角棱线夹角； 定位

中图分类号：TP391 文献标志码：A 文章编号：1006-8228（2013）05-21-04

Research of location algorithm based on three-dimensional models

Liu Mei， Liu Renping， Chen Saiqing

（College of information engineering， North China University of Technology， Beijing 100144， China）

Abstract： In order to make the clean robot quickly position itself in the room， the distance of robot to the wall is determined based on angles of three lines from a corner in the picture collected by wide-angle camera on the robot and then its own position in the room can be calculated. The first step is to correct the distorted graph collected by camera. The second step is to identify the lines from corner. Finally the positioning is realized by 3-dim mathematical model constructed based on the height of the room and coordinate. Simulation results show that this algorithm is stable and can meet the needs of positioning.

Key words： cleaning robot； three-dimensional models； angle between corner lines； positioning

0 引言

随着人们生活水平的提高和智能家居技术的成熟，清洁机器人已经成为较为普遍使用的日常家电产品。在清洁机器人路径规划过程中，清洁机器人的导航定位是路径规划算法中的一个难点。清洁机器人目前有多种导航方式，根据环境信息的完整程度，导航指示信号类型，导航地域等因素的不同，可以分为地图导航，陆标导航，视觉导航，感知器导航等[1]。

本文基于视觉导航进行研究。视觉导航的优点在于探测范围广，能最大采集周围环境信息，缺点是处理图像所需计算量大。我们采用广角摄像头采集到的墙角图像信息及超声波检测到的房间高度信息来建立一个三维模型，从而确定清洁机器人在房间中所处的位置，为之后寻找效率最快的全覆盖路径规划作好准备。

1 广角镜头图像畸变校正

1.1 畸变校正原理

广角镜头光学系统由于摄像机本身的构造，以及制造、安装、工艺等因素导致的各种非线性畸变，如径向畸变，离心畸变和薄棱镜畸变等，其空间点与其图像对应点之间是一种复杂的非线性关系，一般分为径向畸变与切向畸变[2]，Tsai已经证明，对于大多数计算机视觉应用来说，切向畸变可以忽略[3]。因此本文只考虑径向畸变。

本文采用平面网络模板法对摄像机图像进行非线性畸变较正。平面模板法的原理是利用一个特定模板，如平面模板或同心圆模板，描述理想图和畸变图之间的地址映射，以实现对图像的非线性校正。

1.2 平面网格模板法

平面网格模板法一般有三个步聚：制定模板，特征点提取和镜头畸变系数求解。

网格标定模板可以通过程序自动生成点阵图案来制作，如图1所示，图1中各网格线选用不同的灰度值。

图1 网格标定板

在摄像头显示器窗口，增加一个可视十字架。移动标定模板使模板上的十字架和显示器上观察窗口的十字架重合，此时捕获一副图像。其对应的畸变图如图2所示。

图2 畸变图像

特征点提取步骤如下。

理想网格点（x，y）计算：图像中心附近小范围内畸变很小，可以忽略，以中心四邻域的四个网格交点作为无畸变的理想点，由于网格是等间距的，可以推算出其他网格交点的理想坐标。

实际网格点（，）提取：由于网格线平行交叉，以网格交点为顶点形成一个解析曲面。远离标定模板中心的网格交点，在成像后，由于镜头的畸变，曲面顶点将发生偏移。假设解析曲面用二元二次多项式拟合，如公式：

（1.1）

其中，p表示曲面上的灰度值，在像素交点及周围附近寻找6个点（x，y，p），即可以求出多项式的六个系数。

对此曲面进行求导，可得极值点坐标如下：

（1.2）

（1.3）

根据以上两个公式，可以求得K对图像理想点与畸变点的映射表。根据求得的K对映射关系，下一步就可以求解镜头畸变系数。

镜头畸变系数求解：假设点（，）是校正前图像的一点（畸变图像点），那么采用多项式变换，此点（，）与校正后图像的对应点（x，y）（图像理想点）的关系为：

（1.4）

（1.5）

其中n为多项式的次数。一般来讲，n取到3次，畸变校正效果就很好了。

使用最小二乘法，使拟合误差平方和ε为最小，也就是使：

（1.6）

最小，则需要求：

（1.7）

由此得到：

（1.8）

同理可得出：

（1.9）

其中，K为控制点对的个数，s=0，1，2，…，n；t=0，1，…，n-s；s+t≤n。分别求解公式（1.8）（1.9），即可求出aij，bij，再将其代入（1.4）（1.5）就可实现两个坐标系之间的变换。

2 墙角检测及识别

清洁机器人的工作环境多为室内，采集到的墙角信息多为直线信息，可采用Hough变换从图像中提取墙角等直线信息，然后对计算出的直线信息进行模式匹配，找出房间的墙角，进而利用三维模型计算清洁机器人离该墙角两边墙面的距离来确定自己所在的位置。

目前直线提取最成熟、应用最广泛的算法仍然是Hough变换。Hough变换有非常多的优点，比如提取稳定，精度高。但其缺点仍然非常显著，就是实时性太差。Hough变换检测直线的原理是首先得到边缘点，对每一边缘点得到过边缘点的所有直线，直线参数为极坐标下的（γ，δ），而每一个（γ，δ）都对应极坐标中一个点，对每一条直线对应的点记为投票一次，最终记录投票数，投票数大于某一域值的即可看作一条直线。本文采用文献[4]提出的一种子像素级图像测量直线提取算法，采用改进的Hough变换对线状特征进行提取，获得具有确定长度和端点的直线段，以此直线段为初始值自适应生成直线模板，利用最小二乘方法进行模板与图像特征匹配，实现图像线特征的子像素提取。

2.1 边缘检测

边缘是图像最基本的特征，为人们识别目标提供了重要的参数信息[4]。Robert算子提取边缘容易丢失灰度值变化缓慢的局部边缘，平滑噪声作用较小，对噪声较为敏感。Sobel算子对噪声有一定的平滑作用，产生较好的边缘效果，但是对于幅值较小的边缘点，容易丢失其边缘，降低了检测定位精度。LOG算子的抗干扰能力强，且能提取出对比度较弱的边缘。Canny算子具有信噪比高，检测精度高和单边缘响应的优点。

2.2 Hough变换

Hough变换自从1962年由Hough提出以后，已经成为直线检测的重要工具，也是其他参数化形状检测的重要工具，其本质是从图像空间到参数空间的映射。Hough变换是对图像进行某种形式的坐标变换，它将原始图像中给定形伏的直线或曲线变换成变换空间的一个点，即原始图像中给定形状的直线或曲线上的所有点都集中到变换空间的某个点上形成峰值。这样，就把原始图像中给定形状的直线或曲线的检测问题变成了寻找变换空间中的峰点问题，也即把检测整体特性（给定直线的点集）变成检测局部特性的问题。

图像空间中的任意解析曲线可以表述为：

f（（γ1γ2…γn），（x，y））=0 （2.1）

其中，γ1γ2…γn为该曲线的特征参数。如果将上式中的特征参数与变量的角色调换，那么，上式等价于：

g（（x，y），（γ1γ2…γn））=0 （2.2）

也就是说，图像空间中属于同一解析曲线的点经过式（1.2）变换后，都映射到参数空间中，且相交于由参数确定的点。从而说明，只要图像空间中属于同一解析曲线的数据点足够多，就可以通过判断参数空间中各参数点的积累值来确定此解析曲线的描述。

2.3 二维模式匹配

二维模式匹配，就是在一个n×m二维矩阵T中寻找一个p×q二维矩阵P（称为模板）。假定文本T的大小为n×n，模板P的大小为m×m，m

2.4 角度测度

在墙角二值图像中要测量出墙角三条棱线的夹角大小作为后面三维模型的已知参数。最小二乘法就是用误差的平方和作为测量总误差的标准，求所有误差平方和最小的解进而求出夹角大小的方法。

空间直线可由空间两相交平面表示，平面方程的一般形式为：

ax+by+cz+d=0 （2.3）

其中，（x，y，z）为所表示平面上的任一点坐标，用齐次坐标形式表示为（x'，y'，z'，t）（t≠0），则（2.3）式可写为

ax'+by'+cz'+dt=0 （2.4）

其中，下面用（x'，y'，z'，t）表示直线或点的齐次坐标。

设相机内部参数矩阵K，旋转矩阵R和位移向量T分别为

（2.5）

方向为（vx，vy，vz）的直线族，其无穷远点的齐次坐标为（vx，vy，vz，0）。

由相机成像方程可知，齐次坐标所示的无穷远点与其在相机成像平面上的图像坐票（u，v）有以下关系：

（2.6）

由（vx，vy，vz）为单位向量可推出：

（2.7）

将相机内部参数矩阵K代入（2.7）式，可计算出|ρ1|和|ρ2|。则两直线夹角θ的余弦

（2.8）

利用（2.8）式可求出墙角三条棱线夹角θ1和θ2。

3 三维环境模型的建立

在一个矩形房间中清洁机器所处的位置不同，所采集到的墙角信息即两夹角是不同的；将摄像头放置在不同的高度，所采集到的墙角信息，也会不同，由此猜测清洁机器人在某个地方采集到的墙角信息，与房间的高度和离墙面的距离有关。

分离出墙角的三条主要棱，再计算出三条棱之间的角度。检测出的三条棱中，有一条始终是垂直于地面的，将该棱两边的夹角记为θ1和θ2，利用超声波采集到房间的高度为h，清洁机器人距离该墙角相邻墙面的距离为x，y。

本文采用BP神经网络[6]拟合方法来找出墙角棱线夹角与距离的映射关系。BP网络模型处理信息的基本原理是：输入信号Xi通过中间节点（隐层点）作用于输出节点，经过非线形变换，产生输出信号Yk，网络训练的每个样本包括输入向量X和期望输出量t，网络输出值Y与期望输出值t之间的偏差，通过调整输入节点与隐层节点的联接强度取值Wij和隐层节点与输出节点之间的联接强度Tjk以及阈值，使误差沿梯度方向下降，经过反复学习训练，确定与最小误差相对应的网络参数（权值和阈值），训练即告停止。此时经过训练的神经网络即能对类似样本的输入信息，自行处理输出误差最小的经过非线形转换的信息。

隐节点输出模型：

Oj=f（∑Wij×Xi-q j） （3.1）

输出节点输出模型：

Yk=f（∑Tjk×Oj-qk） （3.2）

f-非线形作用函数；q -神经单元阈值。输入信号Xi包括棱两边夹角θ1和θ2及房间的高度h。

作用函数是反映下层输入对上层节点刺激脉冲强度的函数又称刺激函数，一般取为（0，1）内连续取值Sigmoid函数：

f（x）=1/（1+e-x） （3.3）

误差计算模型是反映神经网络期望输出与计算输出之间误差大小的函数：

Ep=1/2×∑（tpi-Opi）2 （3.4）

tpi-i节点的期望输出值；Opi-i节点计算输出值。

神经网络的学习过程，即连接下层节点和上层节点之间的权重拒阵Wij的设定和误差修正过程。BP网络有师学习方式-需要设定期望值和无师学习方式-只需输入模式之分。自学习模型为

ΔWij（n+1）= h ×Фi×Oj+a×ΔWij（n） （3.5）

h -学习因子；Фi-输出节点i的计算误差；Oj-输出节点j的计算输出；a-动量因子。

在matlab中实验，由BP神经网络算出的离墙面的距离与实际距离对比情况如图3所示。

图3 测验数据与实际数据对比图

4 结束语

本文提出了基于墙角棱线信息的提取来定位的问题，建立了房间三维模型，进而确定位置的方法。该算法适用于天花板较简单无复杂线条且形状为矩形的房间。仿真实验结果表明，该方法是有效且稳定的。因为清洁机器人本身大小在30-40cm之间，所以误差控制在该数据范围内都是可行的。利用三维建模来定位的方法为后续清洁机器人在房间中实时确定其位置提供了可靠依据。但该算法受环境影响较大，主要影响因素是房间亮度、是否有较大遮挡物等。

参考文献：

[1] 陆新华，张桂林.自主移动面器人智能导航研究进展[J].机器人

Robot，2003.1.

[2] Lin J M，Xing M L，Sha D G，et al. Distortion measurement of CCD

imaging system with short focal length and large-field objective[J]. Optics and Lasers in Engineering，2005.43：1137-1144

[3] Tsai R Y.A versatile camera calibration technique for high-accuracy

3D machine vision metrology using off-the-shelf TV cameras and lenses[J].IEEE Journal of Robotics and Automation，1987.3（4）：323-344

[4] 张春森，姜楠.一种子像素图像测量直线提取算法[J].中国矿业大学

学报，2009.10.

[5] 陈海峰.数字图像中基本几何形状检测算法的研究与应用[D].浙江大

学，2007.

[6] 唐万梅.BP神经网络网络结构优化问题的研究[J].系统工程理论与

实践，2005.10.