CN102183250A - 一种农业机械的田间道路自动导航定位装置及方法 - Google Patents

Abstract

本发明公开了一种农业机械的田间道路自动导航定位装置及方法，在作业车所行使的道路的同一侧设定2个有色标识，在待定位的作业车上设有用于拍摄全方位图像的全方位视觉传感器，在作业车上还设有微处理器，微处理器中包含有1)用于从全方位图像中获取投影中心到2个有色标识的距离r1和r2、以及根据全方位图像获得2个有色标识与全方位视觉传感器形成的方位角度θ₁的图像处理单元和2)用于根据农业机械的田间道路自动导航定位方法最终求出定位坐标的定位坐标计算单元。该农业机械的田间道路自动导航定位装置及方法可代替或补偿GPS进行定位，运行速度较快、使用范围广、实用性强、价格便宜。

Description

一种农业机械的田间道路自动导航定位装置及方法

技术领域

本发明属于计算机视觉图象处理应用领域，涉及一种农业机械的田间道路自动导航定位装置及方法。

背景技术

目前，人们对机械的自动化程度要求越来越高；机械自动化作业及其机器人的发展是未来机械化发展方向；定位对自动作业车辆、机器人来说，非常重要。车辆、机器人在自动作业时，必须知道自己的位置，才可以决定机器人能否按要求完成下一步的执行任务。图像处理技术定位从图像中抽出特征从控制的范围分为局部定位和全球定位；局部定位运用于一定范围内的作业，计算机图象处理一般抽取有特色的特征物作为定位的基准，特征物分为自然物体和人工设置；全球定位，比如现在流行使用的GPS技术。从定位的性质分为几何定位和拓扑定位。几何定位能知道机器人的具体位置，拓扑定位是确定机器人在一定的可允许的范围路径内，并不知道详细的几何位置；对于各种机器人的定位研究不少，现在用于室外的机器人及车辆的定位基本上运用GPS技术，该定位技术误差比较大；在树林、房屋遮掩区域，因微波信号不能传输而不能使用；另外近年来一种在GPS技术上发展的RTK-GPS技术，精度较高，但是价格昂贵。普通的车辆和作业机械尤其是农业机械及农业运输车，作业环境场所面积比较恶劣，尤其温室作业，GPS技术根本不可能实现自动导航定位。

全方位视觉传感器是一种新型的视觉传感器，它能提供360度范围的丰富的信息，图像不会随传感器的旋转改变图像，有利于减少车轮滑移和震动的部分负面影响，而且价格便宜，被广泛应用在机器人领域；在机器人定位方向也得到了应用，但其使用原理和方法一般是在作业环境中人工设置特征物体或利用自然的特征物体，通过图像处理技术抽取特征值，然后通过图像匹配找到特征点或特征点区域来分析拓扑定位；该定位一般计算过程复杂、运行时间长，实用性不太好。发明专利：一种自动导航定位装置及方法(申请号：200910044412.4)，针对作业的车辆、机器人，尤其是作业环境比较固定，田间、温室、厂房等，以及GPS技术不能使用的区域，利用全方位视觉传感器和计算机视觉图像处理技术开发的一种自动导航定位装置及方法，该发明可代替GPS在一些特殊场合进行定位，运行速度较快、使用范围广、 实用性强、价格便宜。该发明使用了4个标识配合全方位视觉传感器来完成定位；由于田间道路较窄，布置4个标识费时，妨碍农业机械的行驶、GPS价格贵且存在使用限制等缺点。

发明内容

本发明所要解决的技术问题是提出一种农业机械的田间道路自动导航定位装置及方法，该农业机械的田间道路自动导航定位装置及方法可代替GPS进行定位，运行速度较快、使用范围广、实用性强、价格便宜。

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

一种农业机械的田间道路自动导航定位方法，包括以下步骤：

步骤1：准备工作：在道路的同一侧设定2个有色标识L₁和L₂，在待定位的作业车上设有全方位视觉传感器，全方位视觉传感器拍摄当前的包含有2个有色标识成像的全方位图像；

步骤2：测量在全方位图像中的投影中心分别到2个有色标识成像的成像实际距离，进而根据以下公式算出现实环境中作业车分别到2个有色标识的距离r1和r2；并根据全方位图像获得2个有色标识与全方位视觉传感器形成的方位角度θ₁；

由成像实际距离计算空间距离的公式如下：

$\frac{{(\frac{-2{\mathrm{cx}}_i}{x_{0}f+x_{i}H}+c)}^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;$

其中a，b，c为全方位视觉传感器的双曲面表面的结构参数，f为焦距；H为当前的全方位视觉系统的高度；a，b，c，f和H均为定值；

x_i为成像实际距离，即某一有色标识所在的空间点P在全方位图像中的成像点到全方位视觉传感器投影中心的距离；

x₀为空间距离，即全方位视觉传感器到该有色标识的空间距离；

步骤3：求出3个候选位置的坐标：(x_I1，y_I1)、(x_I2，y_I2)和(x_I3，y_I3)；

步骤4：根据3个候选位置的坐标求出定位坐标，定位完成：根据下式求出定位坐标(x_I，y_I)，即完成作业机械在田间道路的几何定位，

$\left\{\begin{array}{c}{x}_I=(x_{I1}+x_{I2}+x_{I3})/3\\ y_I=(y_{I1}+y_{I2}+y_{I3})/3\end{array}\right..$

步骤3中，3个候选位置的坐标的求取方法如下：

首先，以某一个有色标识为原点，以两个标识的连线作为平面坐标的一个坐标轴，建立实际环境的平面坐标系；(x_a，y_a)为标识L₁在所述平面坐标系中的位置坐标，(x_b，y_b)为标识L₂在所述平面坐标系中的位置坐标；

I)第一候选位置I1的坐标的求法：

根据公式 

得到两个位置坐标的解，再根据道路幅宽限制这一约束条件和有色标识设置在道路的同一侧原则，确定其中的一个解为第一候选位置I1的坐标(x_I1，y_I1)；r1和r2也表示作业车分别到2个有色标识的连线；

II)第二个候选位置I2的坐标(x_I2，y_I2)为：

$\left\{\begin{array}{c}{x}_{I2}=L_{12}-r_1\cos \mathrm{\θ}\\ y_{I2}=r_1\sin \mathrm{\θ}\end{array}\right.,$ 其中，L₁₂为有色标识L₁和L₂的几何直线距离，同时L₁₂也表示两个有色标识的连线，θ为r₁与L₁₂形成的夹角；

$\sin \mathrm{\θ}=\sqrt{1-{\cos }^2\mathrm{\θ}},$ cosθ＝)(r₁ ²+L₁₂ ²-X²)/2L₁₂r₁， $X=r_1\cos {\mathrm{\θ}}_1+\sqrt{{\left(r_1\cos {\mathrm{\θ}}_1\right)}^2-{r_1}^2+{L_{12}}^2},$

其中X为三角形中θ角的对边；

III)第三个候选位置I3的坐标(x_I3，y_I3)为：

$\left\{\begin{array}{c}{x}_{I3}=L_{12}-r_2\cos \mathrm{\α}\\ y_{I3}=r_2\sin \mathrm{\α}\end{array}\right.;$ 其中， $\sin \mathrm{\α}=\sqrt{1-{\cos }^2\mathrm{\α}},$

cosα＝(r₂ ²+L₁₂ ²-X′²)2L₁₂r₂； 

式中，α为r₂与L₁₂形成的中间夹角；X′为三角形中α角的对边。

道路幅宽限制指主要是指程序设计中，比如路宽2m，那么求出的值必须小于2m，而且必须为正数。

说明，求X利用的条件是方位角度θ₁和距离r1，X并非r₂。

成像实际距离的求取方法为：

首先计算有色标识成像到全方位视觉传感器投影中心的成像像素距离，再将该成像像素距离乘以标定像素距离计算成像实际距离。

标定像素距离为0.01389inch/pixel。

一种农业机械的田间道路自动导航定位装置，在在作业车所行使的道路的同一侧设定2个有色标识，在待定位的作业车上设有用于拍摄全方位图像的全方位视觉传感器，在作业车上还设有微处理器，微处理器中包含有1)用于从全方位图像中获取投影中心到2个有色标识的距离r1和r2、以及根据全方位图像获得2个有色标识与全方位视觉传感器形成的方位角度θ₁的图像处理单元和2)用于采用农业机械的田间道路自动导航定位方法最终求出定位坐标的定位坐标计算单元。

所述的全方位视觉传感器采用双曲面透镜。

本发明的工作原理是：所述田间道路自动导航定位装置包括通过三脚架安装在作业机械上的全方位视觉传感器、分别设置于道路一边的2个有色标识、计算机及与计算机连接的电源；所述全方位视觉传感器包括上盖，下盖、曲面镜面、中心针、透明外罩、USB摄像机和用于与计算机通讯的USB线组成；所述曲面镜面与所述上盖粘合在一起，所述上盖螺纹式旋紧所述透明外罩上；所述下盖是中间空心并伸出约4mm螺口连接所述USB摄像机镜头；所述曲面镜面包括球面、抛物线曲面和双曲线曲面，其特点是单一视点，采集大范围的数据；所述透明外罩套住在曲面镜面的外部，起支撑曲面镜面和防止灰尘的作用；所述中心针螺在上盖上，穿过并紧密粘合曲球面中心，能有效的防止镜面反射；通过螺纹连接在透明外罩上的USB摄像机镜头连接有调焦螺纹，该USB摄像机的信号输出端通过USB线连接至计算机。所述标识，设计外形圆形或圆锥形的人工有色标识；所述有色，由蓝色和红色组成；并在标识内安装灯光来增加颜色亮度，并且采用遮光板分为二等分，避免2个标识以上的混淆。该计算机作为整个装置的控制中心包括硬件和软件，软件包括实现图像采集、处理、计算和分析功能软件及向作业机械发布导向实施参数控制程序。该计算机借助作业机械发电不停供应电源。

相应的，该田间道路自动导航定位装置采取的田间道路自动导航定位方法包括标识设置、图像数据输入、图像处理及计算和结果输出四个步骤：所述标识设置包括：在田间道路的一边设置2个有色标识，每两个标识间设置距离为20m；所述图像数据输入，通过全方位视觉传感器实时拍摄图像，将图像通过实时接口输入计算机；所述全方位视觉传感器提供的图像是360度信息，信息量大，图形成圆形。离全方位视觉传感器中心越远，图像的分辨率变低；并且在长度上发生变形，因此，图像分析比较复杂，但该全方位视觉传感器的一大优点为其方位角不发生变化。所述图像处理及计算，是本发明的创新部分之一；首先，针对输入计算机的全方位图像计算图像中有色标识颜色的强度；根据实验设定的颜色阈值；逐行对上述采集到的图像进行扫描，抽取高于阈值的颜色特征像素；根据欧几里得距离判断分成特 征像素点区域；计算红色和蓝色像素点区域的像素点的重心；然后根据红色像素点重心和蓝色像素点重心的距离判断是否为标识。最后根据所有红色和蓝色像素特征点的重心作为一个有色标识在该图像的位置；依次完成整幅图像的扫描，搜索出2个有色标识在图像中的位置；如果有色标识特征点提取只有一个或者没有，则返回上一步操作，进行下个位置特征点图像的分析；如果提取了有色标识特征点为2个，则根据提取到的2个有色标识在图像中的位置：一则通过图像处理得出每相邻两个有色标识和全方位视觉传感器形成的方位角度；其二，根据全方位视觉传感器在图像中的位置，计算出两个有色标识分别在图像中离全方位视觉传感器的图像距离，从而根据全方位视觉传感器成像参数公式变换得出两个有色标识与全方位视觉传感器间的空间距离；再根据已知的两个有色标识的空间位置，利用圆周定理求出全方位视觉传感器的2个空间候选位置，根据道路幅宽的限制，选取其中可行的一个候选位置；其三，结合标识和全方位视觉传感器形成的方位角度和根据成像参数公式变换得出两个有色标识与全方位视觉传感器间的空间距离的三角关系，得出全方位视觉传感器的另外2个空间候选位置；最后，根据其二和其三求出的三个候选位置坐标的平均值即为全方位视觉传感器在空间上的坐标，即是作业机械在田间道路的几何定位；根据作业机械自身坐标和空间坐标的关系，计算出导向角和导向距离；结果输出，依据上述步骤得到的导向角和导向距离控制作业机械的行走路线。

当有色标识的内部安装上照明设备，采用玻璃、透明塑料等材料，采用本发明在光线弱或夜间也可以操作。

有益效果：

本发明的农业机械的田间道路自动导航定位装置及方法，根据全方位视觉传感器在线实时不断提供全方位视觉图像，一个位置一副图像，利用图像处理提取标识在图像中的位置，计算图像距离和空间距离；结合借助全方位视觉传感器成像方位角不改变的原理，得出方位角，共同算出全方位视觉传感器在田间道路相对于标识的空间绝对坐标；更进一步，根据车辆、机器人自身坐标和空间坐标的关系，算出导向角和导向距离，为作业车的导航提供必要的定位信息。该方法相比其它定位方法简单，运行速度快，实用性强，所述自动导航定位装置结构简单，成本不高，便于推广运用。

根据全方位视觉传感器的成像原理，通过图像处理，相比于通过图像处理得到的两点的距离，得出的方位角度θ₁是非常准确的，因此为了减少误差，提高定位精度，本发明利用方位角度θ₁，最后求出3个候选点的平均值作为定位坐标，这是本发明的最关键的技术创新点。

附图说明

图1是本发明所述田间道路自动导航定位装置的结构示意图；

图2是本发明所述全方位视觉传感器结构示意图；

图3是本发明所述田间道路自动导航定位方法的实施步骤图；

图4是本发明实例标识方向角图；

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

图6是本发明实例装置系统高度和全方位视觉传感器与标识空间距离、成像距离的关系式推导示意图；

图7是本发明实例定位计算候选点I1计算原理图；

图8是本发明实例定位计算候选点I2计算原理图；

图9是本发明实例定位计算候选点I3计算原理图；

图10是本发明实例定位计算原理图。

在上述图中，

1-标识L1  2-田间道路  3-作业机械  4-标识L2  5-三脚架  6-电源  7-计算机  8-全方位视觉传感器  9-曲型镜面  10-中心针s  11-USB摄像机  12-USB线  13-调焦螺纹  14-镜头  15-下盖  16-透明外罩  17-上盖  18-标识设置  19-图像数据输入  20-图像处理及计算  21-结果输出  30-成像平面，40-双曲面。

具体实施方式

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

一种田间道路自动导航定位装置，包括安装于作业机械上的全方位视觉传感器、分别设置于道路一边的2个有色标识、与全方位视觉传感器的信号输出端连接的计算机及用于给计算机供电的电源。

所述全方位视觉传感器包括上盖、下盖、透明外罩、中心针、曲面镜面、USB摄像机和用于与计算机通讯的USB线，其中上盖、下盖分别安装于透明外罩两端；上盖底部安装有中心针，该中心针穿过并紧贴曲面镜面中心位置；透明外罩支撑粘合在上盖上的曲面镜面；通过调焦螺纹连接于USB摄像机上的USB摄像机镜头与下盖连接。

所述标识，设计外形圆形或圆锥形的人工有色标识，所述有色，包括蓝色和红色；并在标识内安装灯光来增加颜色亮度，并且采用遮光板分为二等分，避免2个标识以上的混淆。

本发明还提供了一种与上述自动导航定位装置相应的自动导航定位方法，包括标识设置、图像数据输入、图像处理及计算和结果输出，所述标识设置包括：在田间道路的一边设置2个有色标识，一般根据全方位视觉传感器成像原理，每两个标识间距离至少可设置为20m；

所述图像数据输入包括：通过全方位视觉传感器实时拍摄包括有色标识在内的图像并输入计算机；

所述图像处理及计算包括：首先，针对输入计算机的全方位图像计算图像中有色标识颜色的强度；根据计算设定的颜色阈值；逐行对上述采集到的图像进行扫描，抽取高于阈值的颜色特征像素；根据欧几里得距离判断分成特征像素点区域；计算红色和蓝色像素点区域的像素点的重心；然后根据红色像素点重心和蓝色像素点重心的距离判断是否为标识。最后根据所有红色和蓝色像素特征点的重心作为一个有色标识在该图像的位置；依次完成整幅图像的扫描，搜索出2个有色标识在图像中的位置：

A)如果有色标识特征点提取只有一个或者没有，则返回上一步操作，进行下个位置特征点图像的分析；

B)如果提取了有色标识特征点为2个，则根据提取到的2个有色标识在图像中的位置：

1)通过图像处理得出两个有色标识和全方位视觉传感器形成的方位角度；

2)根据全方位视觉传感器在图像中的位置，计算出两个有色标识分别在图像中离全方位视觉传感器的图像距离，从而根据成像参数公式变换得出两个有色标识与全方位视觉传感器间的空间距离；再根据已知的两个有色标识的空间位置，利用圆周定理求出全方位视觉传感器的2个空间候选位置，根据道路幅宽的限制，选取其中可行的一个候选位置；

3)结合标识和全方位视觉传感器形成的方位角度和根据成像参数公式变换得出两个有色标识与全方位视觉传感器间的空间距离的三角关系，得出全方位视觉传感器的另外2个空间候选位置；

4)2)和3)求出的三个候选位置坐标的平均值即为全方位视觉传感器在空间上的坐标，即是作业机械在田间道路的几何定位；根据作业机械自身坐标和空间坐标的关系，计算出导向角和导向距离；

结果输出，依据上述步骤得到的导向角和导向距离控制作业机械的行走路线。

实施例1：

如图1，本实施例包括2个红色标识，如图中标识L₁、标识L₂，其分别设置在田间道路 同一侧的路边上，标识L₁和标识L₂间距离设置为20m；全方位视觉传感器通过三脚架连接在作业机械上，所述作业机械可为自动、半自动机械或机器人；作业机械要不断地供给计算机电源以实施应用软件对图像的处理。

如图2，所述全方位视觉传感器包括曲面镜面、中心针、USB摄像机、USB线、调焦螺纹、镜头、下盖、透明外罩和上盖；曲面镜面与所述上盖粘合在一起；所述上盖螺纹式旋紧所述透明外罩上；所述下盖是中间空心并伸出4mm螺口连接所述USB摄像机镜头；所述透明外罩套接于曲面镜面外部，以支撑曲面镜面，并起防止灰尘的作用；所述中心针通过螺纹连接在上盖上，穿过并紧密粘合曲面镜面中心，能有效防止镜面反射；USB摄像机镜头螺纹连接在下盖上，调焦螺纹安装于USB摄像机镜头上，所述USB摄像机镜头的信号输出端通过USB线与计算机连接。

如图3，本实施例所述田间道路自动导航定位方法，包括实施步骤标识设置、图像数据输入、图像处理及计算和结果输出。具体描述如下：

标识设置：作业机械作业前，在田间道路同一侧的路边上，放置2个有色标识L₁和标识L₂，并设置距离为20m。

图像数据输入：通过全方位视觉传感器实时拍摄360度图像，一个地点仅一幅图像，并通过实时接口输入计算机内的储存器中。

图像处理及计算：将输入的图像从储存器调入程序中运行；逐行对图像进行扫描，利用公式(1)分别计算图像中红色和蓝色像素强度(标识采用红色或蓝色或红色/蓝色组合色标识)，根据实验值设定红色和蓝色组合标识提取颜色量的阈值，当计算图像中的红色像素点强度大于设定红色量阈值时，即为红色特征像素点；当计算图像中的蓝色像素点强度大于设定蓝色量阈值时，即为蓝色特征像素点；抽取出红色和蓝色特征像素点。根据任意标识在图像中最大红色像素距离设置红色像素距离最大值，计算抽取出红色像素间欧几里得距离，当红色像素间欧几里得距离小于红色距离最大值时判断为一个红色特征像素点区域；根据任意标识在图像中最大蓝色像素距离设置蓝色像素距离最大值，计算抽取出蓝色像素间欧几里得距离，当蓝色像素间欧几里得距离小于红色距离最大值时判断为一个蓝色特征像素点区域；依次完成整个图像的特征像素点区域分区。

[re，be]＝[R-(B+G)/2-|B-G|，B-(R+G)/2-|R-G|](1)

R、G、B：红色、绿色和蓝色的亮度；re：提取红色像素亮度；be：提取蓝色像素亮度。

在图像上以左上角为原点，定义直角坐标系u，v，每个像素的坐标(u，v)分别以该像素为单位；利用公式(2)计算红色和蓝色特征像素点区域的像素点的重心；然后计算红色特征 像素点区域像素点重心和蓝色特征像素点区域像素点重心的距离；根据红色特征像素点区域像素点重心和蓝色特征像素点区域像素点重心的距离约束值(距离约束值借助经验值通过实验验证)判断是否为标识，当计算红色特征像素点区域像素点重心和蓝色特征像素点区域像素点重心的距离在距离约束值范围之内，即判断为一个标识；否则判断不是标识。

$[\mathrm{Xre},\mathrm{Yre}]=[\frac{\underset{i=1}{\overset{n}{\mathrm{\Σ}}}\mathrm{Riu}}{n_1},\frac{\underset{i=1}{\overset{n}{\mathrm{\Σ}}}\mathrm{Riv}}{n_1}]---\left(2\right)$

$[\mathrm{Xbe},\mathrm{Ybe}]=[\frac{\underset{i=1}{\overset{n}{\mathrm{\Σ}}}\mathrm{Biu}}{n_2},\frac{\underset{i=1}{\overset{n}{\mathrm{\Σ}}}\mathrm{Biv}}{n_2}]$

式中：

Xre、Yre：图像处理计算的红色标识图像坐标；Riu、Riv：提取第i个红色标识特征像素的图像坐标；n₁：：红色特征像素点区域的像素总量。Xbe、Ybe：图像处理计算的蓝色标识图像坐标；Biu、Biv：提取第i个蓝色标识特征像素的的图像坐标；n₂：：蓝色特征像素点区域的像素总量。

如果能成功提取有色标识特征点2个，程序将继续进行下面的工作，得出计算结果；如果有色标识特征点提取只有一个或者没有，则返回计算机上一步操作，进行下个位置特征点图像的分析。提取2个有色标识在图像的位置后，如图4，通过图像处理得出两个有色标识和全方位视觉传感器形成的方位角度θ₁，求出的角度误差小，是本发明主要应用之一。

如图5，全方位视觉传感器成像时，空间点P(X，Y，Z)与形成的图像点p(x，y)在任何方向同一直线上，相对于全方位视觉传感器的投影中心的方位角θ是相同的。基于成像原理，空间点和成像点在空间上处在同一平面上。

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

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

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

a，b，c为双曲面表面的结构参数，一般厂家提供标准值；H为全方位视觉传感器系 统离地面的高度；从全方位视觉传感器成像原理可以知道，空间点和成像点横截面都可以用如图6所示的坐标系表示。定义全方位视觉传感器到P(x₀，z₀)间的距离为全方位视觉传感器与标识空间距离x₀；定义成像点p(x_i，z_i)到全方位视觉传感器投影中心的距离为成像距离x_i。

点O_M(0，H)，M(x_m，z_m)和p(x_o，z_o)在同一条直线上，可以写成(4)：

$z_m=H-\frac{H}{x_0}{x}_m---\left(4\right)$

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

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

根据(4)和(5)求出(6)：

$x_m=\frac{2c{x}_0{x}_i}{x_{0}f+x_{i}H}---\left(6\right)$

将(6)式代入(5)中，可以求出Zm：

$z_m=(1-\frac{2c{x}_i}{x_{0}f+x_{i}H})H---\left(7\right)$

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

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

因此，根据(8)式，a，b，c，f，和H为定值，假如得到成像点p(x_i，z_i)到全方位视觉传感器投影中心的距离为成像距离x_i，则能推导出全方位视觉传感器到P(x₀，z₀)间的距离为全方位视觉传感器与标识空间距离x₀。

根据全方位视觉传感器在图像中的位置，计算出两个有色标识分别在图像中离全方位视觉传感器的图像距离x_i，

从而根据全方位视觉传感器成像参数公式变换得出两个有色标识与全方位视觉传感器间的空间距离x₀；通过像素距离标定计算出标识到全方位视觉传感器实际距离。如图7，定义 坐标系xy，再根据已知的两个有色标识的空间位置，利用圆周定理方程(9)求出全方位视觉传感器的2个空间候选位置I1和I-I1，根据道路幅宽限制和标识设置路边的原则，选取其中可行的一个候选位置I1；

$\left\{\begin{array}{c}{(x_{I1}-x_a)}^2+{(y_{I1}-y_a)}^2={r_1}^2\\ {(x_{I1}-x_b)}^2+{(y_{I1}-y_b)}^2={r_2}^2\end{array}\right.---\left(9\right)$

其中，(x_a，y_a)：标识L₁的空间位置；(x_b，y_b)：标识L₂的空间位置；r₁：标识L₁与全方位视觉传感器实际距离；r₁：标识L₂与全方位视觉传感器实际距离。

如图8，结合标识和全方位视觉传感器形成的方位角度θ₁和有色标识L₁与全方位视觉传感器间的空间距离r₁的三角关系可以求出下列关系式(10)～(12)：

$X=r_1\cos {\mathrm{\θ}}_1+\sqrt{{\left(r_1\cos {\mathrm{\θ}}_1\right)}^2-{r_1}^2+{L_{12}}^2}---\left(10\right)$

cosθ＝(r₁ ²+L₁₂ ²-X²)/2L₁₂r₁    (11)

$\sin \mathrm{\θ}=\sqrt{1-{\cos }^2\mathrm{\θ}}---\left(12\right)$

其中，L₁₂：有色标识L₁和L₂的几何直线距离；θ：r₁与L₁₂形成的中间夹角；X：三角形θ角对边。

式(13)得出全方位视觉传感器的另外1个空间候选位置(I2)的坐标值：

$\left\{\begin{array}{c}{x}_{I2}=L_{12}-r_1\cos \mathrm{\θ}\\ y_{I2}=r_1\sin \mathrm{\θ}\end{array}\right.---\left(13\right)$

如图9，跟求(I2)位置同理，根据式(14)～(17)求出全方位视觉传感器空间候选位置(I3)的坐标值；

$X^{\′}=r_2\cos {\mathrm{\θ}}_1+\sqrt{{\left(r_2\cos {\mathrm{\θ}}_1\right)}^2-{r_2}^2+{L_{12}}^2}---\left(14\right)$

cosα＝(r₂ ²+L₁₂ ²-X′²)/2L₁₂r²        (15)

$\sin \mathrm{\α}=\sqrt{1-{\cos }^2\mathrm{\α}}---\left(16\right)$

其中，L₁₂：有色标识L₁和L₂的几何直线距离；α：r₂与L₁₂形成的中间夹角；X′：三角形α角对边。

式(17)得出全方位视觉传感器的另外1个空间候选位置(I3)的坐标值：

$\left\{\begin{array}{c}{x}_{I3}=L_{12}-r_2\cos \mathrm{\α}\\ y_{I3}=r_2\sin \mathrm{\α}\end{array}\right.---\left(17\right)$

如图10，为提高定位精度，运用不同计算方法求出的三个候选位置(I1～I3)坐标并根据它们的平均坐标值来减少候选点可能的定位误差，根据(18)式求出重心，即是作业机械在田间道路的几何定位。

$\left\{\begin{array}{c}{x}_I=(x_{I1}+x_{I2}+x_{I3})/3\\ y_I=(y_{I1}+y_{I2}+y_{I3})/3\end{array}\right.---\left(18\right)$

全方位视觉传感器安装在作业机械3上，即实现了对作业机械3在该田间道路的几何定位；根据作业机械3的坐标系和道路2的空间坐标系转换关系，计算出导向角和导向距离。

结果输出，导向角和导向距离输送到作为操舵控制器的计算机，执行作业机械的行走路线。

Claims (6)

1.一种农业机械的田间道路自动导航定位方法，其特征在于，包括以下步骤：

步骤1：准备工作：在道路的同一侧设定2个有色标识L₁和L₂，在待定位的作业车上设有全方位视觉传感器，全方位视觉传感器拍摄当前的包含有2个有色标识成像的全方位图像；

步骤2：测量在全方位图像中的投影中心分别到2个有色标识成像的成像实际距离，进而根据以下公式算出现实环境中作业车分别到2个有色标识的距离r1和r2；并根据全方位图像获得2个有色标识与全方位视觉传感器形成的方位角度θ₁；

由成像实际距离计算空间距离的公式如下：

$\frac{{(\frac{-2{\mathrm{cx}}_i}{x_{0}f+x_{i}H}+c)}^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;$

其中a，b，c为全方位视觉传感器的双曲面表面的结构参数，f为焦距；H为当前的全方位视觉系统的高度；a，b，c，f和H均为定值；

x_i为成像实际距离，即某一有色标识所在的空间点P在全方位图像中的成像点到全方位视觉传感器投影中心的距离；

x₀为空间距离，即全方位视觉传感器到该有色标识的空间距离；

步骤3：求出3个候选位置的坐标：(x_I1，y_I1)、(x_I2，y_I2)和(x_I3，y_I3)；

步骤4：根据3个候选位置的坐标求出定位坐标，定位完成：根据下式求出定位坐标(x_I，y_I)，即完成作业机械在田间道路的几何定位，

$\left\{\begin{array}{c}{x}_I=(x_{I1}+x_{I2}+x_{I3})/3\\ y_I=(y_{I1}+y_{I2}+y_{I3})/3\end{array}\right..$

2.根据权利要求1所述的农业机械的田间道路自动导航定位方法，其特征在于，步骤3中，3个候选位置的坐标的求取方法如下：

首先，以某一个有色标识为原点，以两个标识的连线作为平面坐标的一个坐标轴，建立实际环境的平面坐标系；(x_a，y_a)为标识L₁在所述平面坐标系中的位置坐标，(x_b，y_b)为标识L₂在所述平面坐标系中的位置坐标；

I)第一候选位置I1的坐标的求法：

根据公式

得到两个位置坐标的解，再根据道路幅宽限制这一约束条件和有色标识设置在道路的同一侧原则，确定其中的一个解为第一候选位置I1的坐标(x_I1，y_I1)；r1和r2也表示作业车分别到2个有色标识的连线；

II)第二个候选位置I2的坐标(x_I2，y_I2)为：

$\left\{\begin{array}{c}{x}_{I2}=L_{12}-r_1\cos \mathrm{\θ}\\ y_{I2}=r_1\sin \mathrm{\θ}\end{array}\right.,$ 其中，L₁₂为有色标识L₁和L₂的几何直线距离，同时L₁₂也表示两个有色标识的连线，θ为r₁与L₁₂形成的夹角；

$\sin \mathrm{\θ}=\sqrt{1-{\cos }^2\mathrm{\θ}},$ cosθ＝(r₁ ²+L₁₂ ²-X²)/2L₁₂r₁， $X=r_1\cos {\mathrm{\θ}}_1+\sqrt{{\left(r_1\cos {\mathrm{\θ}}_1\right)}^2-{r_1}^2+{L_{12}}^2},$

其中X为三角形中θ角的对边；

III)第三个候选位置I3的坐标(x_I3，y_I3)为：

$\left\{\begin{array}{c}{x}_{I3}=L_{12}-r_2\cos \mathrm{\α}\\ y_{I3}=r_2\sin \mathrm{\α}\end{array}\right.;$ 其中， $\sin \mathrm{\α}=\sqrt{1-{\cos }^2\mathrm{\α}},$

cosα＝(r₂ ²+L₁₂ ²-X′²)/2L₁₂r₂；

式中，α为r₂与L₁₂形成的中间夹角；X′为三角形中α角的对边。

3.根据权利要求1或2所述的农业机械的田间道路自动导航定位方法，其特征在于，成像实际距离的求取方法为：

首先计算有色标识成像到全方位视觉传感器投影中心的成像像素距离，再将该成像像素距离乘以标定像素距离计算成像实际距离。

4.根据权利要求3所述的农业机械的田间道路自动导航定位方法，其特征在于，标定像素距离为0.01389inch/pixel。

5.一种农业机械的田间道路自动导航定位装置，其特征在于，在在作业车所行使的道路的同一侧设定2个有色标识，在待定位的作业车上设有用于拍摄全方位图像的全方位视觉传感器，在作业车上还设有微处理器，微处理器中包含有1)用于从全方位图像中获取投影中心到2个有色标识的距离r1和r2、以及根据全方位图像获得2个有色标识与全方位视觉传感器形成的方位角度θ1的图像处理单元和2)用于根据权利要求2的农业机械的田间道路自动导航定位方法最终求出定位坐标的定位坐标计算单元。

6.根据权利要求5所述的农业机械的田间道路自动导航定位装置，其特征在于，所述的全方位视觉传感器采用双曲面透镜。

Images