全方位视觉传感器自动导航Z轴定位方法及其定位系统

摘要

本发明公开了一种全方位视觉传感器自动导航Z轴定位方法及其定位系统，围绕作业区域的外围角落上放置4个组成矩形的有色标识，全方位视觉传感器设置在作业车辆上；全方位视觉传感器获取现场的包含有4个有色标识成像的全方位图像；根据公式

说明书

技术领域

本发明涉及一种全方位视觉传感器自动导航Z轴定位方法及其定位系统。

背景技术

目前，人们对机械的自动化程度要求越来越高；机械自动化作业及其机器人的发展是未来机械化发展方向；定位对自动作业车辆、机器人来说，非常重要。车辆、机器人在自动作业时，必须知道自己的位置，才可以决定机器人能否按要求完成下一步的执行任务。GPS是目前唯一商业化并使用最多的定位系统；但GPS的精度取决于作业位置，在城市高楼街道或山区，GPS由于房屋、树林、山脉等障碍物阻碍微波传送，精度明显下降；尤其GPS不能在室内使用。类似于哺乳动物，通过视觉来实现定位是现在机器人定位的研究热点之一，视觉传感器定位方法主要分为几何定位和拓扑定位。典型的几何定位是在2D平面中通过图像处理识别标识位置算出或者图像匹配找出机器人的绝对位置。拓扑定位是在建立拓扑地图的基础，找到机器人的抽象位置；拓扑定位应用范围广，不需要人工设置标识，但一般建立拓扑地图复杂，图像信息计算量大，运行速度较慢。通过设置人工标识来实现几何定位方法简单，在机器人定位中得到广泛的应用。而通过普通视觉传感器实施几何绝对定位，一般得出平面二维定位结果；GPS可求出机器人空间三维的定位结果。在实际凹凸不平的作业环境中，空间3D定位是有必要的，可以有效判断机器人的地面环境；另外近年来一种在GPS技术上发展的RTK-GPS技术，精度较高，但是价格昂贵。普通的车辆和作业机械尤其是农业机械及农业运输车，作业环境场所面积比较恶劣，尤其温室作业，GPS技术根本不可能实现自动导航定位。

全方位视觉传感器是一种新型的视觉传感器，它能提供360度范围的丰富的信息，图像不会随传感器的旋转而改变，有利于减少车轮滑移和震动的部分负面影响，而且价格便宜，被广泛应用在机器人领域；发明专利“一种自动导航装置及方法”(申请号：200910044412.4)公开了平面空间二维的计算方法，而要实现三位定位，还必须完成Z轴的定位。

发明内容

本发明所要解决的技术问题是提出一种全方位视觉传感器自动导航Z轴定位方法及其定位系统，该全方位视觉传感器自动导航Z轴定位方法及其定位系统能实现Z轴的定位，从而在现有二维定位的基础上完成空间三维定位，该方法和装置定位速度快、使用范围广、实用性强、具有价格优势。

本发明的技术解决方案如下：

一种全方位视觉传感器自动导航Z轴定位方法，

在作业区域的外围角落上放置4个组成矩形的有色标识：L₁、L₂、L₃、L₄，全方位视觉传感器设置在作业车辆上；全方位视觉传感器获取现场的包含有4个有色标识成像的全方位图像；

全方位视觉传感器系统高度和全方位视觉传感器与标识距离、成像距离的关系式为：

$\frac{{(\frac{-2c{x}_i}{X_{0}f+x_{i}H}+c)}^2}{{b_1}^2}-\frac{{\left(\frac{{2\mathrm{cX}}_0{x}_i}{X_{0}f+x_{i}H}\right)}^2}{{a_1}^2}=1,$X_o，x_i＞0；

其中a₁，b₁，c为双曲面表面的结构参数；f为焦距；H为当前的全方位视觉系统的计算高度；

X₀为在XOY平面上全方位视觉传感器到空间点P(X₀，Z₀)间的距离，X₀为空间点P(X₀，Z₀)在XOY平面上的投影点与全方位视觉传感器在XOY平面上的投影点之间的距离；

x_i为成像距离，即4个有色标识到全方位视觉传感器投影中心的成像实际距离平均值；x_i由分别计算标识到全方位视觉传感器投影中心的成像像素距离，再通过标定像素距离计算成像实际距离；

计算当前的全方位视觉系统的计算高度H；最后计算高度H与标准高度H_s的差值，即Z轴方向的定位坐标值，从而完成作业机械的Z轴定位。

四个有色标识所在的位置是作业区域的外接矩形的角点位置。

全方位视觉传感器投影中心在全方位图像中的位置是固定的。

x_i的求法如下：

标定像素距离为0.01389inch/pixel，先计算出4个标识分别到全方位视觉传感器投影中心的成像实际距离，再求成像实际距离的平均值得到x_i；

X₀的求法如下：

根据4个有色标识在图像中的4个位置点和全方位视觉传感器投影中心在图像中的位置点，计算出四个有色标识中的每相邻两个有色标识与全方位视觉传感器投影中心在全方位图像中的方位角θ₁～θ₄；

4个有色标识L1～L4在作业区周围形成的矩形长为a、宽为b；根据几何学圆弧形成原理，由L1和L2有色标识点和上述方位角θ₁即圆心角形成弧S₁，同理，L2和L3、θ₂形成弧S₂，L3和L4、θ₃形成弧S₃，L3和L4，θ₄形成弧S₄，弧S₁～S₄分别两条相交，得出四个交点，4个交点坐标为：

$\left(\begin{array}{c}{\tan \theta }_1={\mathrm{bX}}_{I1}/({X_{I1}}^2+{Y_{I1}}^2-{\mathrm{bY}}_{I1})\\ {\tan \theta }_2={\mathrm{aY}}_{I1}/({X_{I1}}^2+{Y_{I1}}^2-{\mathrm{aX}}_{I1})\end{array}\right);$其中$c_1=\frac{b/{{\tan \theta }_1}^{-a}}{a/{{\tan \theta }_2}^{-b}};$

$\left(\begin{array}{c}{X}_{I2}=\frac{b(c_2+1/{\tan \theta }_2)}{1+{c_2}^2}\\ Y_{I2}=\frac{{\mathrm{bc}}_2(c_2+1/{\tan \theta }_2)}{1+{c_2}^2}\end{array}\right),$其中$c_2=\frac{b/{{\tan \theta }_2}^{-a}}{a/{{\tan \theta }_3}^{-b}};$

$\left(\begin{array}{c}{X}_{I3}=\frac{b(c_3+1/{\tan \theta }_3)}{1+{c_3}^2}\\ Y_{I3}=\frac{{\mathrm{bc}}_3(c_3+1/{\tan \theta }_3)}{1+{c_3}^2}\end{array}\right),$其中$c_3=\frac{b/{{\tan \theta }_3}^{-a}}{a/{{\tan \theta }_4}^{-b}};$

$\left(\begin{array}{c}{X}_{I4}=\frac{b(c_4+1/{\tan \theta }_4)}{1+{c_4}^2}\\ Y_{I4}=\frac{{\mathrm{bc}}_4(c_4+1/{\tan \theta }_4)}{1+{c_4}^2}\end{array}\right),$其中$c_4=\frac{b/{{\tan \theta }_4}^{-a}}{a/{{\tan \theta }_1}^{-b}};$

得出交点重心坐标P(X₁，Y₁)，如下：

$\left(\begin{array}{c}{X}_1=(X_{I1}+X_{I2}+X_{I3}+X_{I4})/4\\ Y_1=(Y_{I1}+Y_{I2}+Y_{I3}+Y_{I4})/4\end{array}\right);$

重心坐标P(X₁，Y₁)即全方位视觉传感器的平面二维定位；

在XOY平面根据普通的两点距离公式计算P(X₁，Y₁)点到4个标识L1、L2、L3和L4的距离l1、l2、l3和l4；最后得到

一种全方位视觉传感器自动导航Z轴定位系统，在作业区域的外围角落上放置4个组成矩形的有色标识：L₁、L₂、L₃、L₄，用于获取现场的包含有4个有色标识成像的全方位图像的全方位视觉传感器设置在作业车辆上；在作业车辆上设有微处理器，微处理器中具有用于根据全方位视觉传感器获取的图像进行数据处理获取有色标识位置坐标的图像处理单元，微处理器中还具有用于根据前述全方位视觉传感器自动导航Z轴定位方法并最终获得Z轴方向的定位坐标值的计算单元。

全方位视觉传感器具有双曲面镜。

有益效果：

本发明的一种全方位视觉传感器自动导航Z轴定位方法及其定位系统，作业区用4个标识形成矩形状，测量出矩形的长度和宽度，通过图像处理提取出标识在图像中位置，进一步求出在图像中，各标识与传感器投影中心形成的4个方位角度，通过几何算法求出作业机械相对4个标识形成的空间平面XY的二维定位绝对坐标值(X，Y)，采用笛卡尔坐标系，以4个标识中的一个作为原点，在实际中，以图像中左下角的那个标识为原点。

本发明提供了一种与发明专利(申请号：200910044412.4)“一种自动导航定位装置和方法”相符的Z轴定位计算方法，包括推导装置系统高度和全方位视觉传感器与标识空间距离、成像距离的关系式；计算标识到全方位视觉传感器投影中心的成像距离；计算全方位视觉传感器与标识的空间距离；计算Z轴定位值，其特征是：

所述推导装置系统高度和全方位视觉传感器与标识空间距离、成像距离的关系式包括：全方位视觉传感器双曲面镜双曲线方程建立、全方位视觉传感器成像原理运用和数学几何学运用；所述全方位视觉传感器双曲面镜双曲线方程建立包括由平面双曲线方程变换为装置系统高度相关的双曲线方程；所述全方位视觉传感器成像原理运用包括空间点入射线通过双曲面折射在成像面成像原理，其特点是入射线通过双曲面折射后相交于投影中心于一点；空间直线在成像面上的成像为圆弧；所述数学几何学运用包括空间点全方位视觉传感器成像原理几何图像表示中三角形的几何数学关系运用推导出装置系统高度和传感器与标识距离、成像距离的关系式；

所述计算标识到全方位视觉传感器投影中心的成像距离包括根据发明专利(申请号：200910044412.4)一种自动导航定位装置和方法的标识提取，通过图像处理提取标识特征并求出标识特征像素的重心代表标识在图像中的位置，计算标识到全方位视觉传感器投影中心的成像像素距离，通过像素距离标定计算出标识到全方位视觉传感器投影中心的成像实际距离；所述像素距离标定通过实验结果标定实际距离和像素数的关系，得出单个像素标定的实际距离，像素距离乘以单像素标定距离即实际距离。

所述计算全方位视觉传感器与标识的空间距离包括根据发明专利(申请号：200910044412.4)一种自动导航定位装置和方法求出XY平面上(X，Y)坐标值；所求的传感器平面位置运用直线距离计算方程计算全方位视觉传感器与标识的空间距离；

所述计算Z轴定位值包括运用求出的装置系统高度和全方位视觉传感器与标识空间距离、成像距离的关系式和标识到全方位视觉传感器投影中心的成像距离、全方位视觉传感器与标识的空间距离三者，计算Z轴定位值。

综上所述，本发明所述一种全方位视觉传感器自动导航Z轴定位计算方法提供了一种与发明专利(申请号：200910044412.4)一种自动导航定位装置和方法相符的Z轴定位计算方法；只要提供一副包括标识的实时图像，通过图像处理提取标识特征和借助发明专利(申请号：200910044412.4)一种自动导航定位装置和方法求出的平面二维定位值，完成空间三维定位，达到和GPS同样的定位功能，真正实现自动化机械或机器人自动导航定位。该方法简单，运行速度快，实用性强；自动导航定位装置结构简单，成本不高，便于推广运用。

附图说明

图1本发明全方位视觉传感器点成像原理；

图2是本发明的系统高度和全方位视觉传感器与标识空间距离、成像距离的相互关系原理示意图；

图3本发明实例全方位视觉传感器标准高度H_s示意图；

图4本发明方法的流程图；

图5本发明实例全方位视觉传感器与标识的空间距离计算原理示意图。

标号说明：1-全方位视觉传感器，2-作业机械，3-成像平面，4-双曲面。

具体实施方式

以下将结合附图和具体实施例对本发明做进一步详细说明：

实施例1：

如图1，全方位视觉传感器成像时，空间点P(X，Y，Z)、形成的图像点p(x，y)和投影中心形成的平面与水平面(XOY)垂直，空间点P(X，Y，Z)到坐标原点连成的直线在XOY平面的投影直线相对于X轴的方位角为α1，图像点p(x，y)和成像平面原点的连线相对于成像平面x轴的方位角为α2，α1与α2相同，同为图1中的θ。

如图2，定义坐标系XZ；根据全方位视觉传感器成像原理，空间地面点P(X₀，Z₀)，入射线i通过焦点O_M(0，H)经双曲面上点M(x_m，z_m)反射后汇聚于O_c，在成像面上形成像点p(x_i，z_i)；O_c为投影中心，f为焦距；根据一般双曲线方程：

推导符合本发明的方程(1)：

$\frac{{(z_m-H+c)}^2}{b^2}-\frac{{x_m}^2}{a^2}=1;$$c=\sqrt{a^2+b^2}---\left(1\right)$

a，b，c为双曲面表面的结构参数，一般厂家提供标准值；H为全方位视觉传感器系统离地面的高度；从全方位视觉传感器成像原理可以知道，空间点和成像点横截面都可以用如图2所示的坐标系表示。定义全方位视觉传感器到P(X₀，Z₀)间的距离为全方位视觉传感器与标识分别(说明：图2是公式推导示意图，只有一个标识；后面如果有四个标识，则求出与四个标识距离的平均值。---4个标志距离的平均值，指4个标志到全方位视觉传感器的距离值的平均值)在XOY平面上投影的距离X₀；定义成像点p(x_i，z_i)到全方位视觉传感器成像面中心的xoy平面距离为成像距离x_i，如图2所示。

点O_M(0，H)，M(x_m，z_m)和P(X_o，Z_o)在同一条直线上，可以写成(2)：

$z_m=H-\frac{H}{X_0}{x}_m---\left(2\right)$

根据几何数学三角形的相似性，得出(3)：

$\frac{x_i}{x_m}=\frac{f}{z_m-H+2c}---\left(3\right)$

根据(2)和(3)求出(4)：

$x_m=\frac{{2\mathrm{cX}}_0{x}_i}{X_{0}f+x_{i}H}---\left(4\right)$

将(4)式代入(3)中，可以求出Zm：

$z_m=(1-\frac{{2\mathrm{cx}}_i}{X_{0}f+x_{i}H})H---\left(5\right)$

将(4)式和(5)代入(1)中，求出下面全方位视觉传感器系统高度和全方位视觉传感器与标识距离、成像距离的关系式：

$\frac{{\left(\frac{{-2\mathrm{cx}}_i}{X_{0}f+x_{i}H}\right)}^2}{b^2}-\frac{{\left(\frac{{2\mathrm{cX}}_0{x}_i}{X_{0}f+x_{i}H}\right)}^2}{a^2}=1,(X_o,x_i>0)---\left(6\right)$

因此，根据(6)式，a，b，c和f为定值，假如能得出x_i和X₀的值代入(6)式，即可反求得到此时的全方位视觉系统的计算高度H。全方位视觉传感器安装时，离地高度是固定的常数，如图3坐标系Z轴方向上，定义为标准高度H_s。求出计算高度与标准高度的差值，即Z轴方向的定位坐标值，完成作业机械的Z轴定位。

定义大写字母如XYZ表示空间坐标量，小写字母如xyz表示与成像面相关的量。

下一步说明x_i和X₀的值计算过程：

如图4，本实施例所述标识在图像中的位置计算方法，包括设置人工标识，打开标识电源、图像数据输入、图像处理计算、结果寄存。具体描述如下：

设置标识：采用染红色玻璃透，明材制作内部安装有电源照明设备的人工标识，围绕作业区域的最大外围角落上放置的4个红色标识：标识L₁、标识L₂、标识L₃、标识L₄并组成矩形，并打开电源开关，增强颜色亮度。

图像数据输入：通过全方位视觉传感器摄像装置实时拍摄标识L₁、标识L₂、标识L₃、标识L₄的全方位360度图像，并通过实时USB接口输入到计算机内的储存器中。

图像处理及计算：调取计算机中储存的全方位图像，定义图像坐标，图像左上角为原点(0，0)，单位为像素；图像处理中，能根据标识颜色对红色标识进行图像处理。

利用式7对图像像素的红色亮度进行计算，并程序循环计算出图像像素的红色强度中最大值为r_max，储存在内存中；

根据式8，在计算机中，设定提取红色标识特征颜色量的阈值t₁；逐行对图像进行扫描，当利用式7计算图像中红色像素的亮度大于t₁时，从图像提取出符合以上条件所有的红色特征像素；

r＝R-(B+G)/2-|B-G|     ----7

t₁＝[r_max-N₁]    ----8

R、G、B：红色、绿色和蓝色的亮度；r：提取红色像素亮度；r_max：图像中像素的红色强度最大值；t₁：提取红色标识特征颜色量的阈值；N₁为测试常数，10＜N₁＜50；

根据任意标识在图像中最大红色像素距离设置红色像素距离最大值，计算抽取出红色像素间欧几里得距离，当红色像素间欧几里得距离小于设定红色距离最大值时的所有像素判断为一个红色特征像素点区域，分成4个特征像素点区域；

单个特征区像素的计算；提取特征区像素点的横坐标和纵坐标，第i个像素的图像坐标定义为：Rix和Riy；并统计出该特征像素点区域的像素总量为n₁；

利用式9计算该特征像素点区域所有像素的坐标平均值，即求出特征像素点区域的重心，重心的横坐标和纵坐标分别定义为Xr和Yr；坐标(Xr，Yr)对应红色标识在图像中代表的位置点；

$[\mathrm{Xr},\mathrm{Yr}]=[\underset{i=1}{\overset{n}{\Sigma }}\mathrm{Rix}/n_1,\underset{i=1}{\overset{n}{\Sigma }}\mathrm{Riy}/n_1]---9$

式中：Rix、Riy：提取的第i个红色标识特征像素的图像坐标；

当图像识别标识不等于4个，则返回图像输入；通过图像处理得出每相邻两个图像标识点和全方位视觉传感器投影中心在全方位图像中的方位角度θ₁～θ₄。(在实际中，全方位视觉传感器的投影中心在传感器参数不变的情况下，在图像中的位置是唯一固定的，在程序中人工输入数据)

根据得出标识与传感器的方位角度θ₁～θ₄后，如图5，根据发明专利(申请号：200910044412.4-公开号为101660912。)一种自动导航定位装置和方法，标识(L1～L4)形成的矩形长为a、宽为b；由L1和L2有色标识点和上述对角θ₁形成弧S₁，依次L2和L3，θ₂形成弧S₂，L3和L4，θ₃形成弧S₃，L3和L4，θ₄形成弧S₄，弧S₁～S₄分别两条相交，得出四个交点，交点通过几何分析，可得出式(10)：

$\left(\begin{array}{c}{\tan \theta }_1={\mathrm{bX}}_{I1}/({X_{I1}}^2+{Y_{I1}}^2-{\mathrm{bY}}_{I1})\\ {\tan \theta }_2={\mathrm{aY}}_{I1}/({X_{I1}}^2+{Y_{I1}}^2-{\mathrm{aX}}_{I1})\end{array}\right)---10$

对公式1进行变换，得到：

$\left(\begin{array}{c}{X}_{I1}=\frac{b(c+1/{\tan \theta }_1)}{1+c^2}\\ Y_{I1}=\frac{\mathrm{bc}(c+1/{\tan \theta }_1)}{1+c^2}\end{array}\right)---11$

其中$c=\frac{b/\tan {{\theta }_1}^{-a}}{a/{{\tan \theta }_2}^{-b}}.$

同理求出其他三个交点的坐标(X_I2，Y_I2)，(X_I3，Y_I3)，(X_I4，Y_I4)，如下：

$\left(\begin{array}{c}{X}_{I2}=\frac{b(c+1/{\tan \theta }_2)}{1+c^2}\\ Y_{I2}=\frac{\mathrm{bc}(c+1/{\tan \theta }_2)}{1+c^2}\end{array}\right),$$c=\frac{b/\tan {{\theta }_2}^{-a}}{a/{{\tan \theta }_3}^{-b}}$

$\left(\begin{array}{c}{X}_{I3}=\frac{b(c+1/{\tan \theta }_3)}{1+c^2}\\ Y_{I3}=\frac{\mathrm{bc}(c+1/{\tan \theta }_3)}{1+c^2}\end{array}\right),$$c=\frac{b/\tan {{\theta }_3}^{-a}}{a/{{\tan \theta }_4}^{-b}}$

$\left(\begin{array}{c}{X}_{I4}=\frac{b(c+1/{\tan \theta }_4)}{1+c^2}\\ Y_{I4}=\frac{\mathrm{bc}(c+1/{\tan \theta }_4)}{1+c^2}\end{array}\right),$$c=\frac{b/\tan {{\theta }_4}^{-a}}{a/{{\tan \theta }_1}^{-b}}$

得出交点重心坐标P(X₁，Y₁)，如下：

$\left(\begin{array}{c}{X}_1=(X_{I1}+X_{I2}+X_{I3}+X_{I4})/4\\ Y_1=(Y_{I1}+Y_{I2}+Y_{I3}+Y_{I4})/4\end{array}\right)---12$

重心坐标P(X₁，Y₁)即全方位视觉传感器的平面二维定位。

算出相机的相对于标识的平面二维定位P(X₁，Y₁)后，在XOY平面根据普通的两点距离公式计算P(X₁，Y₁)点到4个标识L1、L2、L3和L4的距离l1、l2、l3和l4。标识的坐标根据设置标识时确定。

$X_0=\frac{11+12+13+14}{4}.$

然后，根据9式求出标识特征像素的重心代表标识在图像中的位置，计算标识到全方位视觉传感器投影中心的成像像素距离(在实际中，传感器投影中心是恒定值，人工输入)。本发明实例中标定像素距离为0.01389inch/pixel计算出标识到全方位视觉传感器投影中心的成像实际距离平均值x_i。

得出xi和X0，运用6式计算出机器人及车辆等作业机械的全方位视觉传感器的实际计算值，求出计算高度与标准高度的差值，即Z轴方向的定位坐标值，完成作业机械的Z轴定位。结合发明专利(申请号：200910044412.4)一种自动导航定位装置和方法，既可以完成机器人及车辆等作业机械的全方位视觉传感器自动导航的三维(X，Y，Z)定位。