一种自动导航定位装置及方法

1.一种自动导航定位装置，其特征是包括安装于作业机械（3）上的全方位视觉传感器（8）、分别设置于作业区（2）四个角落，且呈矩形分布的四个有色标识、与全方位视觉传感器（8）的信号输出端连接的计算机（7）及用于给计算机供电的电源（6）；所述全方位视觉传感器包括上盖（19）、下盖（17）、透明外罩（18）、中心针（12）、曲面镜面（11）、USB摄像机（13）和用于与计算机通讯的USB线（14），其中上盖（19）、下盖（17）分别安装于透明外罩（18）两端；上盖（19）底部安装有中心针（12），该中心针（12）穿过并紧贴曲面镜面中心位置；透明外罩（18）支撑粘合在上盖（19）上的曲面镜面（11）；通过调焦螺纹（15）连接于USB摄像机（13）上的USB摄像机镜头（16）与下盖（17）连接；所述下盖（17）的中间位置为空心结构，且设有用于连接USB摄像机镜头（16）的4mm螺口。
2.一种自动导航定位方法，包括标识设置、图像数据输入、图像处理及计算和结果输出，其特征是，
所述标识设置包括：在作业区的最大外围角落上设置有呈矩形放置的4个有色标识，测量出该矩形的长度和宽度；
所述图像数据输入包括：通过全方位视觉传感器实时拍摄包括有色标识在内的作业区图像并输入计算机；
所述图像处理及计算包括：首先，针对有色标识提取颜色量的阀值；逐行对上述采集到的图像进行扫描，抽取颜色特征象素点，形成颜色点区域，计算该颜色点区域所有特征象素点的重心作为一个有色标识在该图像的位置；依次完成整幅图像的扫描，搜索有色标识在图像中的位置：
A）如果有色标识特征点提取只有一个或者没有，则返回上一步操作，进行下个位置特征点图像的分析；
B）如果提取有色标识特征点为2个，则根据提取到的2个有色标识在图像中的位置，结合全方位视觉传感器在图像中的固定位置，计算出两个有色标识分别在图像中离全方位视觉传感器的图像距离，从而根据成像参数公式变换得出两个有色标识与全方位视觉传感器间的空间距离；再根据已知的两个有色标识的空间位置，求出全方位视觉传感器的空间位置；如果找到3个或者4个标识点，则通过图像处理得出每相邻两个有色标识和全方位视觉传感器形成的方位角度，再结合相邻有色标识的坐标和所述矩形的长宽，得出全方位视觉传感器的位置坐标，即实现了对安装了该全方位视觉传感器的作业机械在标识作业区的几何定位；根据作业机械自身坐标和空间坐标的关系，计算出导向角和导向距离；
结果输出，依据上述步骤得到的导向角和导向距离控制作业机械（3）的行走路线。
3.根据权利要求2所述自动导航定位方法，其特征是，由提取到的3个或者4个有色标识特征点得出全方位视觉传感器坐标的具体实现步骤如下所述：通过每相邻两个有色标识和全方位视觉传感器之间形成的方位角度和相邻有色标识坐标形成1条弧线，共得出3条或4条弧线；每2条弧线相交得到1个交点，共得到3个或4个交点；通过对交点进行几何分析和基于有色标识形成的矩形的长和宽计算出3个或4个交点相对矩形的坐标，所述3个或4个交点的重心即为全方位视觉传感器的坐标点：
设有色标识形成的矩形长为a、宽为b；如果找到4个标识点，则通过图像处理得出每两相邻有色标识与全方位视觉传感器形成的方位角度θ1～θ4，由两个有色标识点和上述对角θ1形成弧S1，依次弧S1～S4相交，得出四个交点，交点通过几何分析，可得出式（3）： 进一步推导出式（4）：
其中
依次可以求出其他三个交点的坐标，最后根据四个交点坐标得出全方位视觉传感器的坐标，如式（5）：
4.根据权利要求2或3所述自动导航定位方法，其特征是，所述有色标识为红色或蓝色标识。
5.根据权利要求2或3所述自动导航定位方法，其特征是，所述有色标识采用透明材料制成，且其内部安装有照明设备。

一种自动导航定位装置及方法\n技术领域\n[0001] 本发明涉及计算机视觉图象处理应用领域，尤其涉及一种机器人及车辆等作业机械的自动导航定位装置及方法。\n背景技术\n[0002] 目前，人们对机械的自动化程度要求越来越高；机械自动化作业及其机器人的发展是未来机械化发展方向；定位对自动作业车辆、机器人来说，非常重要。车辆、机器人在自动作业时，必须知道自己的位置，才可以决定机器人能否按要求完成下一步的执行任务。图像处理技术定位从图像中抽出特征从控制的范围分为局部定位和全球定位；局部定位运用于一定范围内的作业，计算机图象处理一般抽取有特色的特征物作为定位的基准，特征物分为自然物体和人工设置；全球定位，比如现在流行使用的GPS技术。从定位的性质分为几何定位和拓扑定位。几何定位能知道机器人的具体位置，拓扑定位是确定机器人在一定的可允许的范围路径内，并不知道详细的几何位置；对于各种机器人的定位研究不少，现在用于室外的机器人及车辆的定位基本上运用GPS技术，该定位技术误差比较大；在树林、房屋遮掩区域，因微波信号不能传输而不能使用；另外近年来一种在GPS技术上发展的RTK-GPS技术，精度较高，但是价格昂贵。普通的车辆和作业机械尤其是农业机械及农业运输车，作业环境场所面积比较恶劣，尤其温室作业，GPS技术根本不可能实现自动导航定位。\n[0003] 全方位视觉传感器是一种新型的视觉传感器，它能提供360度范围的丰富的信息，图像不会随传感器的旋转改变图像，有利于减少车轮滑移和震动的部分负面影响，而且价格便宜，被广泛应用在机器人领域；在机器人定位方向也得到了应用，但其使用原理和方法一般是在作业环境中人工设置特征物体或利用自然的特征物体，通过图像处理技术抽取特征值，然后通过图像匹配找到特征点或特征点区域来分析拓扑定位；该定位一般计算过程复杂、运行时间长，实用性不太好。\n发明内容\n[0004] 本发明的目的是针对作业的车辆、机器人，尤其是作业环境比较固定，田间、温室、厂房等，以及GPS技术不能使用的区域，利用全方位视觉传感器和计算机视觉图像处理技术开发的一种自动导航定位装置及方法，该发明可代替GPS在一些特殊场合进行定位，运行速度较快、使用范围广、实用性强、价格便宜。\n[0005] 为达到上述发明目的，本发明采取的技术方案是，一种自动导航定位装置，包括安装于作业机械上的全方位视觉传感器、分别设置于作业区四个角落，且呈矩形分布的四个有色标识、与全方位视觉传感器的信号输出端连接的计算机及用于给计算机供电的电源。\n[0006] 所述全方位视觉传感器包括上盖、下盖、透明外罩、中心针、曲面镜面、USB摄像机和用于与计算机通讯的USB线，其中上盖、下盖分别安装于透明外罩两端；上盖底部安装有中心针，该中心针穿过并紧贴曲面镜面中心位置；透明外罩支撑粘合在上盖上的曲面镜面；\n通过调焦螺纹连接于USB摄像机上的USB摄像机镜头与下盖连接。\n[0007] 所述下盖的中间位置为空心结构，且设有4mm螺口用以连接USB摄像机镜头。\n[0008] 本发明还提供了一种与上述自动导航定位装置相应的自动导航定位方法，包括标识设置、图像数据输入、图像处理及计算和结果输出，其特征是\n[0009] 所述标识设置包括：在作业区域的最大外围角落上设置有呈矩形放置的4个有色标识，测量出该矩形的长度和宽度；\n[0010] 所述图像数据输入包括：通过全方位视觉传感器实时拍摄包括有色标识在内的作业区图像并输入计算机；\n[0011] 所述图像处理及计算包括：首先，针对有色标识提取颜色量的阀值；逐行对上述采集到的图像进行扫描，抽取颜色特征象素点，形成颜色点区域，计算该颜色点区域所有特征象素点的重心作为一个有色标识在该图像的位置；依次完成整幅图像的扫描，搜索有色标识在图像中的位置：\n[0012] A)如果有色标识特征点提取只有一个或者没有，则返回上一步操作，进行下个位置特征点图像的分析；\n[0013] B)如果提取了有色标识特征点为2个，则根据提取到的2个有色标识在图像中的位置，结合全方位视觉传感器在图像中的固定位置，计算出两个有色标识分别在图像中离全方位视觉传感器的图像距离，从而根据成像参数公式变换得出两个有色标识与全方位视觉传感器间的空间距离；再根据已知的两个有色标识的空间位置，求出全方位视觉传感器的空间位置；如果找到3个或者4个有色标识特征点，则通过图像处理得出每相邻两个有色标识和全方位视觉传感器形成的方位角度，再结合相邻有色标识的坐标和所述矩形的长宽，得出全方位视觉传感器的位置坐标，即实现了对安装了该全方位视觉传感器的作业机械在标识作业区的几何定位；根据作业机械自身坐标和空间坐标的关系，计算出导向角和导向距离；\n[0014] 结果输出，依据上述步骤得到的导向角和导向距离控制作业机械的行走路线。\n[0015] 本发明的工作原理是：所述自动导航定位装置包括通过三脚架安装在作业机械上的全方位视觉传感器、由人工设置在作业区四个角落，且呈矩形分布的四个有色标识、计算机及与计算机连接的电源；所述全方位视觉传感器包括上盖，下盖、曲面镜面、中心针、透明外罩、USB摄像机和用于与计算机通讯的USB线组成；所述曲面镜面与所述上盖粘合在一起，所述上盖螺纹式旋紧所述透明外罩上；所述下盖是中间空心并伸出约4mm螺口连接所述USB摄像机镜头；所述曲面镜面包括球面、抛物线曲面和双曲线曲面，其特点是单一视点，采集大范围的数据；所述透明外罩套住在曲面镜面的外部，起支撑曲面镜面和防止灰尘的作用；所述中心针螺在上盖上，穿过并紧密粘合曲球面中心，能有效的防止镜面反射；通过螺纹连接在透明外罩上的USB摄像机镜头连接有调焦螺纹，该USB摄像机的信号输出端通过USB线连接至计算机。该计算机作为整个装置的控制中心包括硬件和软件，软件包括实现图像采集、处理、计算和分析功能软件及向作业机械发布导向实施参数控制程序。该计算机借助作业机械发电不停供应电源。\n[0016] 相应的，该自动导航定位装置采取的自动导航定位方法包括标识设置、图像数据输入、图像处理及计算和结果输出四个步骤：所述标识设置，针对作业区域，在其最大外围上角落上，放置4个有色标识组成矩形，并测量出矩形的长度和宽度；所述图像数据输入，通过全方位视觉传感器实时拍摄图像，将图像通过实时接口输入计算机；所述全方位视觉传感器提供的图像是360度信息，信息量大，图形成圆形。离全方位视觉传感器中心越远，图像的分辨率变低；并且在长度上发生变形，因此，图像分析比较复杂，但该全方位视觉传感器的一大优点为其方位角不发生变化。所述图像处理及计算，是本发明的创新部分之一；\n首先，在写好的程序中，设定颜色标识提取颜色量的阀值；逐行对图像进行扫描，抽取颜色特征象素点，形成一个小的颜色点区域，计算该颜色点区域所有特征象素点的重心作为一个有色标识在该图像的位置；依次完成整幅图像的扫描，搜索标识在图像中的位置；如果能成功提取颜色标识特征点2个以上，程序将继续进行下面的工作，得出计算结果；如果特征点提取只有一个或者没有，则返回计算机上一步操作，进行下个位置特征点图像的分析。\n提取2个标识在图像的位置后，因为全方位视觉传感器在图像的位置是固定的，形成圆形的360度信息，根据有色标识的已知位置，算出两个有色标识分别在图像中离全方位视觉传感器的图像距离，根据成像参数公式变换，得出两个有色标识与全方位视觉传感器间的实际空间距离；两个有色标识在空间的位置点是已知的，根据距离公式参数和全方位视觉传感器的x方向和y方向的两个未知数代入距离公式，求出全方位视觉传感器在空间的坐标位置。如果找到3个或者4个标识点，则通过图像处理可以得出每两个相邻标识和全方位视觉传感器之间的方位角度，由两个标识坐标点和所述方位角度，形成一条弧，每2条弧线相交得到1个交点，即4条或3条弧线相交的4或3个交点通过几何分析，结合有色标识形成的矩形的长度和宽度计算出相对矩形的坐标，交点的重心即为全方位视觉传感器的位置坐标；由于全方位视觉传感器安装在车辆或机器人等作业机械上，因此得到了全方位视觉传感器的定位，即完成了车辆、机器人等作业机械在该标识作业区的几何定位；根据机器人自身坐标和空间坐标的关系，计算出导向角和导向距离。最后为结果输出，即将得到的导向角和导向距离输送到操舵控制器，控制车辆、机器人等作业机械执行行走路线。\n[0017] 当有色标识的内部安装上照明设备，采用玻璃、透明塑料等材料，采用本发明在夜间也可以操作。\n[0018] 综上所述，本发明所述自动导航定位装置及其方法只要提供一幅全方位视觉图像，利用图像处理算出图像距离或者借助全方位视觉传感器成像方位角不改变的原理，得出方位角算出全方位视觉传感器在标识空间的绝对坐标；根据车辆、机器人自身坐标和空间坐标的关系，算出导向角和导向距离。该方法相比其它定位方法简单，运行速度快，实用性强，所述自动导航定位装置结构简单，成本不高，便于推广运用。\n附图说明\n[0019] 图1是本发明所述自动导航定位装置的结构示意图；\n[0020] 图2是本发明所述全方位视觉传感器结构示意图；\n[0021] 图3是本发明所述自动导航定位方法的实施步骤图；\n[0022] 图4是本发明实例计算原理图。\n[0023] 在上述图中，\n[0024] 1-标识L1 2-作业区 3-作业机械 4-标识L2 5-三脚架 6-电源 7-计算机8-全方位视觉传感器 9-标识L3 10-标识L4 11-曲型镜面 12-中心针13-USB摄像机 14-USB线 15-调焦螺纹 16-镜头 17-下盖 18-透明外罩19-上盖 20-标识设置 21-图像数据输入 22-图像处理及计算 23-结果输出\n具体实施方式\n[0025] 以下结合实施例及附图，对本发明作进一步说明。\n[0026] 如图1，本实施例包括四个红色标识，如图中标识(L1)1、标识(L2)4、标识(L3)9和标识(L4)10，其分别设置在作业区2的四个角落上，作业区可以是不同形状；全方位视觉传感器8通过三脚架5连接在作业机械3上，所述作业机械3可为自动、半自动机械或机器人；\n作业机械3要不断地供给计算机7电源6以实施应用软件对图像的处理。\n[0027] 如图2，所述全方位视觉传感器8包括曲面镜面11、中心针12、USB摄像机13、USB线14、调焦螺纹15、镜头16、下盖17、透明外罩18和上盖19；曲面镜面11与所述上盖19粘合在一起；所述上盖19螺纹式旋紧所述透明外罩18上；所述下盖17是中间空心并伸出\n4mm螺口连接所述USB摄像机镜头13；所述透明外罩18套接于曲面镜面11外部，以支撑曲面镜面11，并起防止灰尘的作用；所述中心针12通过螺纹连接在上盖19上，穿过并紧密粘合曲面镜面11中心，能有效防止镜面反射；USB摄像机镜头16螺纹连接在下盖17上，调焦螺纹15安装于USB摄像机镜头16上，所述USB摄像机镜头16的信号输出端通过USB线14与计算机7连接。\n[0028] 如图3，本实施例所述自动导航定位方法，包括实施步骤标识设置20、图像数据输入21、图像处理及计算22和结果输出23。具体描述如下：\n[0029] 标识设置20：作业机械3作业前，在作业区域2的最大外围角落上，放置4个红色标识(L1)1、标识(L2)4、标识(L3)9和标识(L4)10组成矩形，并测量出该矩形的长度和宽度。\n[0030] 图像数据输入21：通过全方位视觉传感器8实时拍摄360度图像，一个地点仅一幅图像，并通过实时接口输入计算机7内的储存器中。\n[0031] 图像处理及计算22：设定红色标识，并提取颜色量的阀值；将输入的图像从储存器调入程序中运行；逐行对图像进行扫描，利用公式(1)抽取红色特征象素点，抽取颜色特征象素点，形成一个小的颜色点区域。\n[0032] r＝R-(B+G)/2-|B-G| (1)\n[0033] R、G、B：红色、绿色和蓝色的亮度；r：提取红色像素亮度。\n[0034] 利用公式(2)计算该颜色点区域所有特征象素点的重心作为一个有色标识在该图像的位置；依次完成整幅图像的扫描，搜索有色标识在图像中的位置；\n[0035] \n[0036] 式中：\n[0037] Xr、Yr：图像处理计算的有色标识图像坐标；Rix、Riy：提取第i个有色标识特征像素的的图像坐标。\n[0038] 如果能成功提取有色标识特征点2个以上，程序将继续进行下面的工作，得出计算结果；如果有色标识特征点提取只有一个或者没有，则返回计算机上一步操作，进行下个位置特征点图像的分析。提取2个有色标识在图像的位置后，由于全方位视觉传感器在图像的位置是固定的，根据有色标识的位置，计算得出两个有色标识分别在图像中离全方位视觉传感器的图像距离，根据成像参数公式变换得出两个有色标识与全方位视觉传感器间的实际空间距离；两个有色标识在空间的位置点是已知的，根据距离公式，由上述距离参数和全方位视觉传感器的x方向和y方向的两个未知数，可求出全方位视觉传感器在空间的位置。\n[0039] 如图4，假设有色标识形成的矩形长为a、宽为b；如果找到4个标识点，则通过图像处理可以得出每两相邻有色标识与全方位视觉传感器形成的方位角度θ1～θ4，由两个有色标识点和上述对角θ1形成一条弧S1，形成弧S1～S4，每2条相交得到1个交点，依次得到交点I1～I4，交点通过几何分析，可得出式(3)：\n[0040] \n[0041] 进一步推导出式(4)：\n[0042] \n[0043] 其中\n[0044] 同理求出其他三个交点的坐标，因此可以得出交点重心坐标P(x1，y1)，如式(5)：\n[0045] \n[0046] 由有色标识形成的矩形的长和宽计算出交点重心相对矩形的坐标，交点的重心即全方位视觉传感器的位置点，全方位视觉传感器安装在作业机械3上，即实现了对作业机械3在该作业区2的几何定位；根据作业机械3的坐标系和作业区2的空间坐标系转换关系，计算出导向角和导向距离。\n[0047] 结果输出23，导向角和导向距离输送到作为操舵控制器的计算机，执行作业机械\n3的行走路线。\n[0048] 同理分析可以得到如果找到3个标识点的定位计算。