CN103076014A - 一种作业机械自动导航三标识定位装置及方法 - Google Patents

Abstract

本发明公开了一种作业机械自动导航三标识定位装置及方法，所述的装置包括作业机械和3个有色标识L₁、L₂和L₃；有色标识L₁和有色标识L₂均设置在作业区间同一侧的边界处；有色标识L₃设置在作业区的另一边界处(如田埂上)，且有色标识L₃和有色标识L₂的连线L₂L₃与有色标识L₁和有色标识L₂连线L₁L₂垂直；作业机械上通过三脚架安装有全方位视觉传感器；作业机械上还设有用于执行定位数据计算功能的计算机；全方位视觉传感器与计算机连接。本发明能简化标识空间布置和主标识判别，减少图像处理过程，提高精度，可代替GPS进行定位，运行速度较快、使用范围广、实用性强、成本低。

Description

一种作业机械自动导航三标识定位装置及方法

技术领域

本发明设有机械领域，涉及一种作业机械自动导航三标识定位装置及方法。

背景技术

实现田间精确定位是农业机械实施精准农业作业的第一步；农业机械智能化和机器人化是目前及将来农业机械的发展方向。自动导航是实现农业机械智能化和机器人化的核心问题之一，而定位是实现自动导航的基础性难题，也是完成执行任务的前提。目前，GPS与普通机器视觉是农业机械自动导航定位研究的两大热门。但是，GPS存在障碍物阻碍微波传送时精度明显下降和普通视觉定位主要用于作物、田埂等直线导航，不能普遍使用的不足。

全方位视觉传感器是一种新型的视觉传感器，它能提供360度范围的丰富的信息，图像不会随传感器的旋转改变图像，有利于减少车轮滑移和震动的部分负面影响，而且价格便宜，被广泛应用在机器人领域；在机器人定位方向也得到了应用，但其使用原理和方法一般是在作业环境中人工设置特征物体或利用自然的特征物体，通过图像处理技术抽取特征值，然后通过图像匹配找到特征点或特征点区域来分析拓扑定位；该定位一般计算过程复杂、运行时间长，实用性不太好。发明专利：一种自动导航定位装置及方法(ZL200910044412.4)，针对作业的车辆、机器人，尤其是作业环境比较固定，田间、温室、厂房等，以及GPS技术不能使用的区域，利用全方位视觉传感器和计算机视觉图像处理技术开发的一种自动导航定位装置及方法，该发明可代替GPS在一些特殊场合进行定位，运行速度较快、使用范围广、实用性强、价格便宜。该发明使用了4个标识配合全方位视觉传感器来完成定位。发明专利：一种农业机械的田间道路自动导航定位装置及方法(ZL201110007784.7)，针对田间道路较窄，布置4个标识费时，妨碍农业机械的行驶、使用GPS价格贵且存在使用限制等缺点，利用全方位视觉传感器和2个标识发明的一种道路定位方法。发明专利：一种自动导航定位装置及方法(ZL200910044412.4)要求四个标识并且矩形布置，一方面考虑到田间布置标识的困难，另一方面，矩形的精度也难以保证；田埂可能也存在一侧不方便布置标识的情况，造成4个标识无法全部布置。发明专利：一种农业机械的田间道路自动导航定位装置及方法(ZL201110007784.7)比较适合短距离道路使用，在较大面积中使用的精度降低。东京大学博士论文文献“Study on Localization System for Agricultural Vehicle Navigation UsingOmnidirectional Vision”中，研究农业机械的全方位视觉定位系统，提到了4个标识图像特征识别过程中可能由于环境噪音等原因出现图像特征提取失败，造成其中1个标识信息丢失，出现3个标识的情况，但不是作为一个定位系统，且如何实现定位的方法没有做出具体的研究和分析。

因此，有必要设计一种作业机械自动导航定位装置及方法。

发明内容

本发明所要解决的技术问题是提供一种作业机械自动导航三标识定位装置及方法，该作业机械自动导航三标识定位装置及方法能简化标识空间布置和主标识判别，减少图像处理过程，提高精度，可代替GPS进行定位，运行速度较快、使用范围广、实用性强、成本低。

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

一种作业机械自动导航三标识定位装置，包括作业机械和3个有色标识L₁、L₂和L₃；有色标识L₁和有色标识L₂均设置在作业区同一侧的边界处(如作业田间同一侧的田埂上)；有色标识L₃设置在作业区另一边界处(如另一条田埂处)，且有色标识L₃和有色标识L₂的连线L₂L₃与有色标识L₁和有色标识L₂连线L₁L₂垂直；

作业机械上通过三脚架安装有全方位视觉传感器；作业机械上还设有用于执行定位数据计

算功能的计算机；全方位视觉传感器与计算机连接。

有色L₁和有色标识L₂之间的距离小于或等于150m。

所述全方位视觉传感器包括上盖、下盖、曲面镜面、中心针、透明外罩、USB摄像机和USB线；所述曲面镜面与所述上盖粘合在一起，所述上盖通过螺纹旋装在所述透明外罩上；所述下盖中间空心，并伸出有螺口连接所述USB摄像机的镜头；所述曲面镜面为球面、抛物线曲面或双曲线曲面【这里3个曲面都可以实现，本发明使用双曲面镜面】，具有单一视点；所述透明外罩套在曲面镜面的外部；所述中心针固定在上盖上，穿过并粘合在曲面镜面的镜面中心；所述有色标识为圆形或圆锥形的人工有色标识，有色标识内设有增强颜色效果的光源。

一种基于所述的作业机械自动导航三标识定位装置的作业机械自动导航三标识定位方法，包括以下步骤：

步骤1：通过全方位视觉传感器采集向前位置的全方位图像；

步骤2：在所述的全方位图像中识别出各有色标志，并获取各有色标志相对于全方位视觉传感器的投影中心的方位角；

步骤3：计算出作业机械的绝对位置坐标；完成作业机械定位。

步骤2中识别出各有色标志的方法为：

对全方位图像进行逐行扫描，利用公式[re，be]＝[R-(B+G)/2-|B-G|，B-(R+G)/2-|R-G|]计算图像中每一个像素点红色像素强度re和蓝色像素强度be，标识采用红色/蓝色组合色标识，其中R、G、B分别为当前像素点的红、绿和蓝色强度分量；

当当前像素点中的红色像素点强度大于预设红色量阈值时，则判定当前像素点为红色特征像素点；

当当前像素点中的蓝色像素点强度大于预设蓝色量阈值时，则判定当前像素点为蓝色特征像素点；

从而抽取出红色和蓝色特征像素点；

根据任意一个标识在图像中最大红色像素距离设置红色像素距离最大值，计算抽取出的任意两个红色像素间的欧几里得距离，当红色像素间欧几里得距离小于红色距离最大值时判断为一个红色特征像素点区域；根据任意一个标识在图像中最大蓝色像素距离设置蓝色像素距离最大值，计算抽取出的任意两个蓝色像素间的欧几里得距离，当蓝色像素间欧几里得距离小于蓝色距离最大值时判断为一个蓝色特征像素点区域；依次完成整个图像的特征像素点区域分区；【这一部分的判断原理说明：比如一个有色标志的整个图像中的最大尺寸为50个像素，且由于任意两个有色标识的距离远远大于50个像素，那么如果两个红色像素的距离小于50，可以认为该2个像素属于同一个标识，否则属于不同的标识。】

在图像上以左上角为原点，定义直角坐标系u，v，每个像素的坐标为(u，v)分别以像素为单位；利用下式计算红色或蓝色特征像素点区域的像素点的重心；然后计算红色特征像素点区域像素点重心和蓝色特征像素点区域像素点重心的距离；根据红色特征像素点区域像素点重心和蓝色特征像素点区域像素点重心的距离约束值判断是否为标识，当计算红色特征像素点区域像素点重心和蓝色特征像素点区域像素点重心的距离在距离约束值范围之内，即判断为一个标识；否则判断不是标识。【这里的是组合标识是指一个标识中既有蓝色标识，又有红色标识】

$[\mathrm{Xre},\mathrm{Yre}]=[\underset{i=1}{\overset{n}{\mathrm{\Σ}}}\mathrm{Riu}/n_1,\underset{i=1}{\overset{n}{\mathrm{\Σ}}}\mathrm{Riv}/n_1]$

$[\mathrm{Xbe},\mathrm{Ybe}]=[\underset{i=1}{\overset{n}{\mathrm{\Σ}}}\mathrm{Riu}/n_2,\underset{i=1}{\overset{n}{\mathrm{\Σ}}}\mathrm{Riv}/n_2]$

式中：

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

如果能成功识别3个标识，则完成标识识别；

完成标识识别后，对每一个有色标识的所有特征像素(包括蓝色特征像素和红色特征像素)求重心作为该有色标识的坐标。

步骤2中获取各有色标志相对于全方位视觉传感器的投影中心的方位角方法为：在全方位图像中，投影中心O和3个有色标识的坐标确定后，直接求得方位角θ₁、θ₁和θ₃；其中θ₁、θ₁和θ₃分别为OL₁与OL₂的夹角、OL₂与OL₃的夹角以及OL₁与OL₃的夹角。【参见图8】

步骤3中计算出作业机械的绝对位置坐标的步骤为：

采用三种方法计算3个候选位置点I₁～I₃的位置坐标，计算3个候选位置点I₁～I₃的重心，作为作业机械在工作区域的最终定位坐标(x₁，y₁)；

$\left\{\begin{array}{c}{x}_1=(x_{I1}+x_{I2}+x_{I3})/3\\ y_1=(y_{I1}+y_{I2}+y_{I3})/3\end{array}\right.;$     其中x_I1，x_I2，x_I3分别为3个候选位置点I₁～I₃的横坐标；y_I1，y_I2，y_I3分别为3个候选位置点I₁～I₃的横坐标；

候选位置点I₁的坐标为：

$\left\{\begin{array}{c}{x}_{I1}=w(c_1+1/\tan {\mathrm{\θ}}_1)/(1+{c_1}^2)\\ y_{I1}=c_1{x}_{I1}=w{c}_1(c_1+1/\tan {\mathrm{\θ}}_1)/(1+{c_1}^2)\end{array}\right.,$

其中，c₁＝l/[tanθ₂(w/tanθ₁+w/tanθ₂-l)]，

候选位置点I₂的坐标为：

$\left\{\begin{array}{c}{x}_{I2}=l(c_2+1/\tan {\mathrm{\θ}}_3)/(1+{c_2}^2)\\ y_{I2}=c_2{x}_{I2}=l{c}_2(c_2+1/\tan {\mathrm{\θ}}_3)/(1+{c_2}^2)\end{array}\right.$

其中，c₂＝w/[tanθ₂(l/tanθ₃+l/tanθ₂-w)]

候选位置点I₃的坐标为：

$\left\{\begin{array}{c}{x}_{I3}=l(c_3+1/\tan {\mathrm{\θ}}_3)/(1+{c_3}^2)\\ y_{I3}=c_3{x}_{I1}=l{c}_3(c_3+1/\tan {\mathrm{\θ}}_3)/(1+{c_3}^2)\end{array}\right.$

其中，c₃＝(l/tanθ₃-w)/(w/tanθ₁-l)；式中，w为标识L₁到标识L₂的距离，l为标识L₁到标识L₃的距离。

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

相应的，该农业机械自动导航三标识定位装置采取的自动导航三标识定位方法包括标识设置、成像系统校正、图像数据输入、图像处理及计算和结果输出四个步骤：所述标识设置包括：分别设置2个有色标识于作业区域一侧的田埂上，再设置一个与上面2个标识连线垂直的第3个有色标识于田埂上，3个标识形成的三角形区域一侧为执行自动导航定位的作业区；所述成像系统校正，采用Matlab工具箱对成像系统进行校正，得出相机投影中心、焦距、镜面参数、失真参数和歪曲参数，对全方位视觉图像进行校正；所述图像数据输入，通过全方位视觉传感器实时拍摄图像，将图像通过实时接口输入计算机；所述全方位视觉传感器提供的图像是360度信息，信息量大，图形成圆形。离全方位视觉传感器中心越远，图像的分辨率变低；并且在长度上发生变形，因此，图像分析比较复杂，但该全方位视觉传感器的一大优点为其方位角不发生变化。所述图像处理及计算，是本发明的创新部分之一；首先，针对输入计算机的全方位图像计算图像中有色标识颜色的强度；根据实验设定的颜色阈值；逐行对上述采集到的图像进行扫描，抽取高于阈值的颜色特征像素；根据欧几里得距离判断分成特征像素点区域；计算红色和蓝色像素点区域的像素点的重心；然后根据红色像素点重心和蓝色像素点重心的距离判断是否为标识。最后根据所有红色和蓝色像素特征点的重心作为一个有色标识在该图像的位置；依次完成整幅图像的扫描，搜索出3个有色标识在图像中的位置；如果有色标识特征点提取2个以下或者4个以上，则返回上一步操作，进行下个位置特征点图像的分析；如果提取了有色标识特征点为3个，则根据提取到的3个有色标识在图像中的位置：一则通过图像处理得出每相邻两个有色标识和全方位视觉传感器投影中心形成的方位角度；其二，根据3个标识和投影中心在图像中的位置，根据成像点和空间点关系式计算标识在空间的位置点，根据距离公式求出投影中心与三标识三边的空间长度；借助余弦定理利用两边和方位角度来计算三角形的另一边，根据三角形斜边定理斜边大于其它两边的关系来判断出斜边、斜边所对的角度和构成斜边的2标识点外的第3点为主标识点，作为图像坐标系的方位角度估算的起始点，同时作为空间坐标系中的固定坐标点；其三，结合标识和全方位视觉传感器形成的方位角度计算全方位视觉传感器的3个空间候选位置；最后，求出的3个候选位置坐标的平均值即为全方位视觉传感器在空间上的坐标，即是作业机械在田间道路的几何定位；根据作业机械自身坐标和空间坐标的关系，计算出导向角和导向距离；结果输出，依据上述步骤得到的导向角和导向距离控制作业机械的行走路线。

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

有益效果：

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

附图说明

图1是本发明所述农业机械自动导航三标识定位装置的结构示意图；

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

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

图4是本发明所述农业机械自动导航三标识定位方法的实施步骤图；

图5是本发明所述农业机械自动导航三标识定位方法方位角度估算的实施步骤图；

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

图7是本发明实例装置高度示意图；

图8是本发明实例方位角度示意图；

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

图10是本发明实例定位计算候选点I₂计算原理图；

图11是本发明实例定位计算候选点I₃计算原理图；

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

标号说明：1-有色标识L₁，2-田间道路，3-作业机械，4-有色标识L₂，5-三脚架，6-电源，7-计算机，8-全方位视觉传感器，9-有色标识L₃，10-曲型镜面，11-中心针，12-USB摄像机，13-USB线，14-调焦螺纹，15-镜头，16-下盖，17-透明外罩，18-上盖，19-成像平面，20-双曲面。

具体实施方式

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

实施例1：

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

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

所述标识，设计外形圆形或圆锥形的人工有色标识，所述有色，包括蓝色和红色；并在标识内安装灯光来增加颜色亮度。

本发明还提供了一种与上述自动导航定位装置相应的自动导航定位方法，包括标识设置、成像系统校正、图像数据输入、图像处理及计算和结果输出，所述标识设置包括：分别设置2个有色标识于作业区域一侧的田埂上，再设置一个与上面2个标识连线垂直的第3个有色标识于田埂上，3个标识形成的三角形区域一侧为执行自动导航定位的作业区；

所述成像系统校正，采用Matlab工具箱对成像系统进行校正，得出相机投影中心、焦距、镜面参数、失真参数和歪曲参数，对全方位视觉图像进行校正；

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

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

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

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

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

2)根据3个标识在图像中的位置，根据成像点和空间点关系式计算标识在空间的位置点，根据距离公式求出投影中心与三标识三边的空间长度；借助余弦定理利用两边和方位角度来计算三角形的另一边，根据三角形斜边定理斜边大于其它两边的关系来判断出斜边、斜边所对的角度和构成斜边的2标识点外的第3点为主标识点，作为图像坐标系的方位角度估算的起始点，同时作为空间坐标系中的固定坐标点；

3)结合标识和全方位视觉传感器形成的方位角度计算全方位视觉传感器的3个空间候选位置；

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

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

实施例1：

如图1，本实施例包括3个红色/蓝色组合标识【即标识L₁、标识L₂和标识L₃】，标识L₁、标识L₂分别设置在作业区间同一侧的边界处，标识L₁和标识L₂间距离设置根据作业区长度设置(实践中，根据目前的实验结果，≤150m)；全方位视觉传感器通过三脚架连接在作业机械上，所述作业机械可为自动、半自动机械或机器人；作业机械要不断地供给计算机电源以实施应用软件对图像的处理。

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

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

如图4和5，本实施例所述农业机械自动导航三标识定位方法，包括实施步骤标识设置、图像数据输入、图像处理及计算和结果输出。具体描述如下：

标识设置：作业机械作业前，分别设置2个有色标识于作业区域一侧的田埂上，再设置一个与上面2个标识连线垂直的第3个有色标识于田埂上，3个标识形成的三角形区域一侧为执行自动导航定位的作业区。

成像系统校正，采用Matlab工具箱对成像系统进行校正，得出相机投影中心、焦距、镜面参数、失真参数和歪曲参数，对全方位视觉图像进行校正；

图像数据输入：通过全方位视觉传感器实时拍摄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}]=[\underset{i=1}{\overset{n}{\mathrm{\Σ}}}\mathrm{Riu}/n_1,\underset{i=1}{\overset{n}{\mathrm{\Σ}}}\mathrm{Riv}/n_1]---\left(2\right)$

$[\mathrm{Xbe},\mathrm{Ybe}]=[\underset{i=1}{\overset{n}{\mathrm{\Σ}}}\mathrm{Riu}/n_2,\underset{i=1}{\overset{n}{\mathrm{\Σ}}}\mathrm{Riv}/n_2]$

式中：

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

如果能成功提取有色标识特征点3个，程序将继续进行下面的工作，得出计算结果；如果有色标识特征点提取不是3个，则返回计算机上一步操作，进行下个位置特征点图像的分析。

提取3个有色标识在图像的位置后，通过图像处理得出3个有色标识和全方位视觉传感器形成的方位角度θ₁、θ₂、θ₃，如图3，求出的角度误差小，是本发明主要应用之一。

如图6，空间坐标系XYZ和成像平面xy，a，b、c(其中

)为双曲面的结构参数，，一般厂家提供标准值；根据双曲线方程可以得出双曲面表达式：

$\frac{X^2+Y^2}{a^2}-\frac{Z^2}{b^2}=-1(Z>0)---\left(3\right)$

式中，a为双曲面半虚轴，m；b为双曲面半实轴，m；X，Y，Z分别为双曲面上点的三维坐标值，m。

空间点P(X，Y，Z)入射线聚焦焦点O_M(0，0，+c)点，通过双曲面反射后，聚焦于投影中心O_C(0，0，-c)点，在成像面上形成与空间点P相对应的像点p(x，y)；通过几何关系得出

$Z=\sqrt{X^2+Y^2}\tan \mathrm{\α}+c---\left(4\right)$

$\mathrm{\γ}={\tan }^{-1}\frac{f}{\sqrt{x^2+y^2}}---\left(5\right)$

根据(4)和(5)式求出

$\mathrm{\α}={\tan }^{-1}\frac{(b^2+c^2)\sin \mathrm{\γ}-2\mathrm{bc}}{(b^2-c^2)\cos \mathrm{\γ}}---\left(6\right)$

式(4)～(6)中，f为相机焦距，mm；α为俯视角，(°)；γ为仰视角，(°)。

由图3，

tanθ＝Y/X＝y/x         (7)

通过几何数学变形，即得出像点p坐标x，y与空间点P坐标X，Y的关系式(8)

$\left[\begin{array}{c}x\\ y\end{array}\right]=\frac{f(b^2-c^2)}{(b^2+c^2)Z-2\mathrm{bc}\sqrt{X^2+Y^2+Z^2}}\left[\begin{array}{c}X\\ Y\end{array}\right]---\left(8\right)$

如图7，考虑到定位系统中传感器一般通过三脚架安装在拖拉机上，以地面为XY平面，定义Z轴上从水平地面到双曲镜面焦点的高度为定位系统高度H(m)，则像点p坐标x，y与空间点P坐标X，Y的关系式为

$\left[\begin{array}{c}x\\ y\end{array}\right]=\frac{f(b^2-c^2)}{(b^2+c^2)(Z+c-H)-2\mathrm{bc}\sqrt{X^2+Y^2+{(Z+c-H)}^2}}\left[\begin{array}{c}X\\ Y\end{array}\right]---\left(9\right)$

式中，H为系统高度，m；f为相机焦距，mm。

传感器安装固定后，系统的高度可视为固定恒值H，由于

为常数；因此，当空间点Z值能确定时，经过数学公式变换，得出空间点P坐标(X，Y)与像点p坐标(x，y)间的关系式(10)

$\left[\begin{array}{c}X\\ Y\end{array}\right]=\frac{\{f(b^4-c^4)(Z+c-H)+\sqrt{4b^2{c}^2{(b^2-c^2)}^2{(Z+c-H)}^2(f^2+x^2+y^2)}\}}{f^2{(b^2-c^2)}^2-4b^2{c}^2(x^2+y^2)}\left[\begin{array}{c}x\\ y\end{array}\right]---\left(10\right)$

因此，根据(10)式，a，b，c，f，和H为定值，假如提取标识特征得出标识在图像中的位置p(x，y)，根据成像点和空间点关系式(10)计算标识在空间的位置点P(X，Y，Z)，其中Z按标识高度的1/2来设定；

根据距离公式求出投影中心与3个标识3边的空间长度；借助余弦定理利用两边和方位角(θ₁、θ₂、θ₃)来计算三角形的另一边；

根据斜边大于其它两边的关系来判断出ΔL₁L₂L₃斜边和斜边所对的角度θ₂，同时确定构成斜边的2标识点L₂、L₃外的第3点L₁为主标识点，作为图像坐标系的方位角确定的起始点，以便确定θ₁、θ₂、θ₃的顺序；同时，作为空间坐标系中的固定坐标点(如原点、x轴或y轴上固定点)；

如图9-12，标识空间坐标系xy中，根据圆周定理，由L₁L₂直线和方向角θ₁形成圆弧S₁，同理，由方向角θ₁～θ₃形成相应的圆弧S₁～S₃；在无误差的情况下，3条圆弧相交于同一点；但大多数情况下，由于标识识辨、测量等误差的存在，可能产生3个交点I₁～I₃，其中I₁是S₁与S₂的交点；I₂是S₂与S₃的交点；I₃是S₁与S₃的交点，3个交点的重心作为传感器的位置。

如图9，根据几何关系得出：

θ₁＝α₁+β₁                           (11)

θ₂＝α₂+β₂+π/2                        (12)

$\tan {\mathrm{\α}}_1=\frac{w-y_{I1}}{x_{I1}}---\left(13\right)$

$\tan {\mathrm{\β}}_1=\frac{y_{I1}}{x_{I1}}---\left(14\right)$

$\tan {\mathrm{\α}}_2=\frac{x_{I1}}{y_{I1}}---\left(15\right)$

$\tan {\mathrm{\β}}_2=\frac{w-y_{I1}}{{l-x}_{I1}}---\left(16\right)$

由式(11～16)得出：

$\tan {\mathrm{\θ}}_1=\tan ({\mathrm{\α}}_1+{\mathrm{\β}}_1)=\frac{\frac{w}{x_{I1}}}{1-\frac{w{y}_{I1}-y_{I1}^2}{x_{I1}^2}}=\frac{w{x}_{I1}}{x_{I1}^2+y_{I1}^2-w{y}_{I1}}---\left(17\right)$

同理：

$\tan {\mathrm{\θ}}_2=\tan ({\mathrm{\α}}_2+{\mathrm{\β}}_2+\mathrm{\π}/2)=\frac{l{y}_{I1}-w{x}_{I1}}{x_{I1}^2+y_{I1}^2-l{x}_{I1}-w{y}_{I1}}---\left(18\right)$

变成方程组：

$\left\{\begin{array}{c}{x}_{I1}^2+y_{I1}^2-w{y}_{I1}-\frac{w{x}_{I1}}{\tan {\mathrm{\θ}}_1}=0\\ x_{I1}^2+y_{I1}^2-l{x}_{I1}-w{y}_{I1}-\frac{l{y}_{I1}-w{x}_{I1}}{\tan {\mathrm{\θ}}_2}=0\end{array}\right.---\left(20\right)$

方程组(20)上式减去下式得出：

$\frac{l}{\tan {\mathrm{\θ}}_2}{y}_{I1}+(l-\frac{w}{\tan {\mathrm{\θ}}_1}-\frac{w}{\tan {\mathrm{\θ}}_2})x_{I1}=0---\left(21\right)$

当 $l-\frac{w}{\tan {\mathrm{\θ}}_1}-\frac{w}{\tan {\mathrm{\θ}}_2}\≠0$ 时，

$y_{I1}=\frac{\frac{l}{\tan {\mathrm{\θ}}_2}}{(\frac{w}{\tan {\mathrm{\θ}}_1}+\frac{w}{\tan {\mathrm{\θ}}_2}-l)}{x}_{I1}---\left(22\right)$

定义：

c₁＝l/[tanθ₂(w/tanθ₁+w/tanθ₂-l)]   (23)

可以求出交点I₁的坐标值：

$\left\{\begin{array}{c}{x}_{I1}=w(c_1+1/\tan {\mathrm{\θ}}_1)/(1+{c_1}^2)\\ y_{I1}=c_1{x}_{I1}={\mathrm{wc}}_1(c_1+1/\tan {\mathrm{\θ}}_1)/(1+{c_1}^2)\end{array}\right.---\left(24\right)$

如图10，同理求出I₂的坐标值。

候选位置点I₂的坐标为：

$\left\{\begin{array}{c}{x}_{I2}=l(c_2+1/\tan {\mathrm{\θ}}_3)/(1+{c_2}^2)\\ y_{I2}=c_2{x}_{I2}={\mathrm{lc}}_2(c_2+1/\tan {\mathrm{\θ}}_3)/(1+{c_2}^2)\end{array}\right.$

其中，c₂＝w/[tanθ₂(l/tanθ₃+l/tanθ₂-w)]

如图11，根据几何关系，得出：

θ₁＝α₁+β₁                         (25)

θ₃＝α₃+β₃                         (26)

$\tan {\mathrm{\α}}_1=\frac{w-y_{I3}}{x_{I3}}---\left(27\right)$

$\tan {\mathrm{\β}}_1=\frac{y_{I3}}{x_{I3}}---\left(28\right)$

$\tan {\mathrm{\α}}_3=\frac{l-x_{I3}}{w-y_{I3}}---\left(29\right)$

$\tan {\mathrm{\β}}_3=\frac{x_{I3}}{w-y_{I3}}---\left(30\right)$

根据交点I₁数学、几何计算方法，可以得出

$\left\{\begin{array}{c}{x}_{I3}=l(c_3+1/\tan {\mathrm{\θ}}_3)/(1+{c_3}^2)\\ y_{I3}=c_3{x}_{I1}=l{c}_3(c_3+1/\tan {\mathrm{\θ}}_3)/(1+{c_3}^2)\end{array}\right.---\left(31\right)$

其中，c₃＝(l/tanθ₃-w)/(w/tanθ₁-l)。

如图12，为提高定位精度，运用不同计算方法求出的3个交点(传感器定位的候选位置)(I₁～I₃)坐标并根据它们的平均坐标值来减少候选点可能的定位误差，根据(32)式求出3个交点的重心，即是作业机械在田间的几何定位。

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

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

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

Claims (7)

1.一种作业机械自动导航三标识定位装置，其特征在于，包括作业机械和3个有色标识L₁、L₂和L₃；有色标识L₁和有色标识L₂均设置在作业区同一侧的边界处；有色标识L₃设置在作业区另一边界处，且有色标识L₃和有色标识L₂的连线L₂L₃与有色标识L₁和有色标识L₂连线L₁L₂垂直；

作业机械上通过三脚架安装有全方位视觉传感器；作业机械上还设有用于执行定位数据计算功能的计算机；全方位视觉传感器与计算机连接。

2.根据权利要求1所述的作业机械自动导航三标识定位装置，其特征在于，有色L₁和有色标识L₂之间的距离小于或等于150m。

3.根据权利要求2所述的作业机械自动导航三标识定位装置，其特征在于，所述全方位视觉传感器包括上盖、下盖、曲面镜面、中心针、透明外罩、USB摄像机和USB线；

所述曲面镜面与所述上盖粘合在一起，所述上盖通过螺纹旋装在所述透明外罩上；所述下盖中间空心，并伸出有螺口连接所述USB摄像机的镜头；所述曲面镜面为球面、抛物线曲面或双曲线曲面，具有单一视点；所述透明外罩套在曲面镜面的外部；所述中心针固定在上盖上，穿过并粘合在曲面镜面的镜面中心；所述有色标识为圆形或圆锥形的人工有色标识，有色标识内设有增强颜色效果的光源。

4.一种基于权利要求1-3任一项所述的作业机械自动导航三标识定位装置的作业机械自动导航三标识定位方法，其特征在于，包括以下步骤：

步骤1：通过全方位视觉传感器采集向前位置的全方位图像；

步骤2：在所述的全方位图像中识别出各有色标志，并获取各有色标志相对于全方位视觉传感器的投影中心的方位角；

步骤3：计算出作业机械的绝对位置坐标；完成作业机械定位。

5.根据权利要求4所述的作业机械自动导航三标识定位方法，其特征在于，步骤2中识别出各有色标志的方法为：

对全方位图像进行逐行扫描，利用公式[re，be]＝[R-(B+G)/2-|B-G|，B-(R+G)/2-|R-G|]计算图像中每一个像素点红色像素强度re和蓝色像素强度be，标识采用红色/蓝色组合色标识，其中R、G、B分别为当前像素点的红、绿和蓝色强度分量；

当当前像素点中的红色像素点强度大于预设红色量阈值时，则判定当前像素点为红色特征像素点；

当当前像素点中的蓝色像素点强度大于预设蓝色量阈值时，则判定当前像素点为蓝色特征像素点；

从而抽取出红色和蓝色特征像素点；

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

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

$[\mathrm{Xre},\mathrm{Yre}]=[\underset{i=1}{\overset{n}{\mathrm{\Σ}}}\mathrm{Riu}/n_1,\underset{i=1}{\overset{n}{\mathrm{\Σ}}}\mathrm{Riv}/n_1]$

$[\mathrm{Xbe},\mathrm{Ybe}]=[\underset{i=1}{\overset{n}{\mathrm{\Σ}}}\mathrm{Riu}/n_2,\underset{i=1}{\overset{n}{\mathrm{\Σ}}}\mathrm{Riv}/n_2]$

式中：

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

如果能成功识别3个标识，则完成标识识别；

完成标识识别后，对每一个有色标识的所有特征像素(包括蓝色特征像素和红色特征像素)求重心作为该有色标识的坐标。

6.根据权利要求4所述的作业机械自动导航三标识定位方法，其特征在于，步骤2中获取各有色标志相对于全方位视觉传感器的投影中心的方位角方法为：在全方位图像中，投影中心O和3个有色标识的坐标确定后，直接求得方位角θ₁、θ₂和θ₃；其中θ₁、θ₂和θ₃分别为OL₁与OL₂的夹角、OL₂与OL₃的夹角以及OL₁与OL₃的夹角。

7.根据权利要求5或6所述的作业机械自动导航三标识定位方法，其特征在于，步骤3中计算出作业机械的绝对位置坐标的步骤为：

采用三种方法计算3个候选位置点I₁～I₃的位置坐标，计算3个候选位置点I₁～I₃的重心，作为作业机械在工作区域的最终定位坐标(x₁，y₁)；

$\left\{\begin{array}{c}{x}_1=(x_{I1}+x_{I2}+x_{I3})/3\\ y_1=(y_{I1}+y_{I2}+y_{I3})/3\end{array}\right.;$ 其中x_I1，x_I2，x_I3分别为3个候选位置点I₁～I₃的横坐标；

y_I1，y_I2，y_I3分别为3个候选位置点I₁～I₃的横坐标；

候选位置点I₁的坐标为：

$\left\{\begin{array}{c}{x}_{I1}=w(c_1+1/\tan {\mathrm{\θ}}_1)/(1+{c_1}^2)\\ y_{I1}=c_1{x}_{I1}=w{c}_1(c_1+1/\tan {\mathrm{\θ}}_1)/(1+{c_1}^2)\end{array}\right.,$

其中，c₁＝l/[tanθ₂(w/tanθ₁+w/tanθ₂-1)]，

候选位置点I₂的坐标为：

$\left\{\begin{array}{c}{x}_{I2}=l(c_2+1/\tan {\mathrm{\θ}}_3)/(1+{c_2}^2)\\ y_{I2}=c_2{x}_{I2}=l{c}_2(c_2+1/\tan {\mathrm{\θ}}_3)/(1+{c_2}^2)\end{array}\right.$

其中，c₂＝w/[tanθ₂(l/tanθ₃+l/tanθ₂-w)]

候选位置点I₃的坐标为：

$\left\{\begin{array}{c}{x}_{I3}=l(c_3+1/\tan {\mathrm{\θ}}_3)/(1+{c_3}^2)\\ y_{I3}=c_3{x}_{I1}=l{c}_3(c_3+1/\tan {\mathrm{\θ}}_3)/(1+{c_3}^2)\end{array}\right.$

其中，c₃＝(l/tanθ₃-w)/(w/tanθ₁-l)；式中，w为标识L₁到标识L₂的距离，l为标识L₁到标识L₃的距离。

Images