CN102368158A - 一种果园机械导航定位方法 - Google Patents

Abstract

本发明公开了一种果园机械导航定位方法。该方法将果园机械在果园中的导航定位分为三部分：当果园机械在果树行间行驶时，通过融合激光扫描信息和视觉传感器检测得到的信息，得到果园机械前方三维环境，提取有效的导航信息；当果园机械即将行驶到果树行尽头时，通过卡尔曼滤波器融合果园机械各内部传感器测量得到的信息，获得用于果园机械导航定位的信息；当果园机械行驶到果园地头时，利用最优控制规划转弯路径，采用GPS传感器测量得到果园机械位置信息。本发明可为果园机械导航控制提供精确的横向偏差和航向偏差，并通过多传感器的组合定位，实现果园机械在整个果园环境下的自动行走。

Description

一种果园机械导航定位方法

技术领域

本发明属于导航技术领域，尤其涉及一种果园机械的导航定位方法。

背景技术

精确导航是实现智能化果园机械装备的重要支持技术之一。导航定位方法的研究作为果园机械导航的基础工作，其性能直接影响果园机械导航控制精度，因此，提高果园机械导航定位的精度，是提高果园机械自动行走质量的首要问题。

目前，果园机械导航定位方法常见的是利用视觉传感器或者激光扫描仪获取导航信息，视觉图像具有收集的信息丰富、目标信息完整的优点，但由于果园环境中复杂，存在大量的噪声信号，会造成图像信息丢失，导致存在实时性的问题；激光导航具有探测距离远，受光照等外界环境因素影响小等优点，在果园环境下，果树恰好可以看作障碍物，可以利用激光扫描的果树位置信息作为导航信息，但激光扫描也存在获取障碍物信息不完整，在障碍物缺失时无法获取导航信息的缺点。

鉴于上述利用单一传感器的导航方法中出现的问题，现有技术中也提出了利用组合定位的导航方法，主要利用视觉传感器和激光的组合，利用视觉传感器获取用于果园机械导航的基本导航参数，利用激光检测障碍物，通过卡尔曼滤波等方法实现两种信息的融合，获得用于果园机械的导航信息，但由于视觉传感器受环境影响大，果园机械并不能稳定的提取导航路径，同时在果园地头，由于果树的缺失，视觉传感器和激光都无法为果园机械的转弯提供有效导航参数。

发明内容

针对上述现有技术中存在的问题与不足，本发明的目的在于提供一种高精度的果园机械导航定位方法，该方法通过将多种传感器组合使用，实现在整个果园环境下的导航定位。

本发明提供一种果园机械导航定位方法，包括：

（1）当果园机械在果树行间行驶时，通过融合激光扫描信息和视觉传感器检测得到的信息构建果园机械前方三维环境，提取有效导航定位信息。

（2）当果园机械即将行驶到果树行尽头时，利用卡尔曼滤波器融合果园机械内部传感器测量得到的信息，获得果园机械有效导航定位信息。

（3）当果园机械行驶到果园地头时，首先采用最优控制规划果园机械地头转弯路径，利用GPS传感器测量得到其位置信息，通过比较理论路径坐标与实际位置坐标，得到果园机械有效导航定位信息。

步骤1中，以激光扫描得到的果园机械两侧的果树位置信息为主要导航信息，获取用于果园机械导航的初始航向偏差和横向偏差，以视觉传感器测量得到的果树行间信息为辅助导航信息，通过两者信息的融合，构建果园机械前方三维环境，预测果园机械在果树行间的通过性。

步骤1中，所述激光扫描信息和视觉传感器信息的融合是：利用激光扫描仪对图像实时标定，将激光扫描仪与CCD摄像机安装在同一竖直位置，分别建立激光扫描直角坐标系和视觉坐标系，用激光扫描所拟合出的路径与y′轴的截距已知，视觉传感器拟合的路径与y轴的截距与其相等，因此能够建立图像中像素点数量与实际环境中距离长度的对应关系。在图像中建立两个在实际环境中都垂直于水平面的平面，并在实际环境中对这两个面进行等面积划分，在图像栅格中反映出实际环境中相对应空间位置的障碍物信息。通过判断二值化图像中障碍物信息量，预测机器人在果树行间的通过性。

步骤2中，利用航向角传感器测量果园机械的航向角，利用角度传感器测量果园机械的前轮转角，利用速度传感器测量果园机械的速度，然后构建卡尔曼滤波器，对三者信息进行融合，获取用于果园机械导航的航向偏差和前轮转角。

步骤3中，首先利用最优控制规划转弯路径，然后利用GPS传感器测量得到果园机械的位置信息，比较理论路径坐标与（实际位置）坐标，获得用于果园机械导航的横向偏差和航向偏差。

本发明与现有技术相比具有如下优点和效果：

（1）本发明提出了果园机械行间行走的多源信息融合方法，利用激光扫描信息实时标定视觉检测得到的信息，可以构建果园机械前方的三维环境，预测果园机械在果树行间的通过性，并为行间导航控制提供更精确的横向偏差和航向偏差。

（2）本发明组合了多种传感器用于果园机械的导航定位。将果园机械的导航定位分为三部分，充分考虑果园环境的特点以及各种传感器的优缺点，每段利用不同传感器获取果园机械的导航信息，提高了果园机械的导航精度和适应性。

附图说明

图1为果园机械导航定位系统结构框图。

图2为果园机械在整个果园环境中的运动示意图。

图3为果园机械在果树行间信息流向图。

图4为果园机械在即将行驶到果树行尽头信息流向图。

图5为果园机械在果园地头信息流向图。

图6为视觉坐标系。

图7为行间有效导航参数提取流程图。

图8为果园机械在果园环境中导航定位技术流程图。

具体实施方式

为使本发明的目的、技术方案和优点更加清楚，下面结合实施例对本发明作进一步详细的描述，但本发明的实施方式不限于此。

本实施采用果园机械为本体，其导航系统主要包括：导航传感器组合、PC机、RS232串口、A/D转换卡和图像信息采集卡。导航传感器主要由激光扫描仪、视觉传感器、GPS传感器和内部传感器组（航向角传感器、角度传感器及速度传感器）组成。激光扫描仪采集果园机械前方两侧果树的位置信息；视觉传感器检测果园机械前方障碍物信息；GPS在地头转弯时采集果园机械的位置信息；速度传感器测量果园机械速度信息，角度传感器测量果园机械前轮转角；航向角传感器测量果园机械航向角。PC机进行数据采集与处理、多源信息融合、路径规划及导航控制等任务。激光扫描仪、GPS传感器与航向角传感器采集到的数据通过RS232串口传送给PC机，角度传感器通过A/D转换卡将采集到的角度模拟信号传送给PC机，速度传感器通过A/D转换卡将采集到的速度信号传送给PC机，视觉传感器将采集到的信息通过图像采集卡传送到PC机上。所有软件程序均采用Visual C++编程工具在Windows XP操作系统下开发。果园机械导航定位系统框图如图1所示。

为实现果园机械在整个果园环境中的自动行走，本发明将导航定位分为三部分，如图2所示：

1、当果园机械在果树行间行驶时，即图2中的AB段，通过融合激光扫描信息和视觉传感器检测得到的信息构建果园机械前方三维环境，提取有效导航定位信息，预测果园机械在果树行间的通过性。此部分信息的流向如图3所示。

2、 当果园机械即将行驶到果树行尽头时，即图2中的BC段，此时果树的数量不能满足激光扫描获取有效导航信息，GPS传感器受树冠的遮挡不能稳定接收卫星信号，利用卡尔曼滤波器融合果园机械内部传感器测量得到的速度、角度及航向角信息，获得果园机械有效导航定位信息。此部分信息的流向如图4所示。

3、当果园机械行驶到果园地头时，GPS传感器能够稳定接收卫星信号，首先采用最优控制规划果园机械地头转弯路径，利用GPS传感器测量得到其位置信息，通过比较理论路径坐标与实际位置坐标，得到果园机械有效导航定位信息。此部分信息的流向如图5所示。

具体步骤包括：

1． 果树行间的导航定位：

（1）通过激光扫描获取果树位置信息；以激光扫描仪的位置为原点，果园机械本体前进方向为y′轴，建立直角坐标系，将激光扫描得到的果树极坐标位置点转换到直角坐标系，完成坐标系内所有果树树干特征参数的提取，获得主要导航路径信息，并得到导航路径与y′轴的截距。

（2）通过视觉传感器获取果树行间信息，首先对获取的图像进行预处理；然后采用图像分割的方法定位树干与地面的交点，通过这些交点进行路径的拟合，作为辅助导航信息。

（3）利用激光扫描仪对图像实时标定，实现激光信息和视觉信息的融合。建立视觉坐标系，如图6所示，以摄像机为中心，照片的底边为x轴，其垂直方向为y轴，建立直角坐标系xoy，L为通过图像拟合出的路径、w为机械本体宽度的一半，H为机械本体的高度，l为路径L与y轴的截距。将激光扫描仪与CCD摄像机安装在同一竖直位置，两个坐标系中的截距相等，建立图像中像素点数量与实际环境中距离长度的对应关系。

（4）以图像中路径为中心，建立距路径L距离为w、高度为H的两个面，两个面在实际环境中都垂直于水平面。在实际环境中对这两个面进行等面积划分，然后利用激光扫描信息对图像信息进行标定，在图像栅格中反映出实际环境中相对应空间位置的障碍物信息，从而可以构建果园机械前方的三维环境，预测果园机械在果树行间的通过性。

（5）激光扫描信息通过RS232串口1传送给计算机，图像信息通过图像采集卡传送给计算机。

行间有效导航参数提取技术流程如图7所示。首先利用激光扫描仪获取果园机械前方位置信息，然后对激光信息进行分析处理，完成果树树干位置信息的提取，获取果树行中线。同时，利用视觉传感器采集果园机械前方环境图像，并对图像进行处理，识别出果树行接地点，获取果树行中线。将处理后的激光扫描信息和视觉传感器采集的信息进行融合，构建果园机械前方三维环境，完成对前方障碍物的检测，提取有效的导航定位信息。

2． 即将行驶到果树行尽头时的导航定位

当果园机械即将行驶到果树行尽头时，激光能扫描到的果树数量不能提供准确的导航信息，由于树冠遮挡，GPS也不能稳定接收卫星信号，此时融合果园机械内部传感器信息，为果园机械提供有效导航信息。利用航向角传感器检测得到果园机械航向角信息，利用角度传感器检测得到果园机械前轮转角信息，利用速度传感器检测得到果园机械速度信息。速度信息和前轮转角信息通过A/D转换卡传送给计算机，航向角信息通过RS232串口2传送给计算机。构建卡尔曼滤波器融合三者信息，得到用于果园机械导航的航向偏差和前轮转角。

本发明采用如下公式：

在不考虑果园机械重力和侧滑时，根据果园机械的运动学关系，果园机械的运动学方程式为：

x、y为果园机械后轮轴中心点的坐标，m； θ为果园机械的航向角，rad；α为果园机械的前轮转角，rad；V为果园机械的速度，m/s；L为果园机械前后轮的轴距，m；u为转向角速度，rad/s。

系统的非线性状态方程为：

系统差分方程为：

系统观测方程为：

为系统离散观测序列， 

为k时刻的测量噪声， 为k-1时刻的测量噪声。

3．果园地头的定位导航：

在果园地头，激光能扫描到的果树数量不能提供准确的导航信息，同时因为不受树冠的遮挡，GPS可以稳定的接收卫星信号。因此，在地头转弯处，利用GPS传感器获取果园机械有效导航信息。首先基于最优控制理论规划果园机械转弯的理论路径，利用GPS传感器获取果园机械的实际位置信息，将理论路径坐标与实际位置坐标相比较，获取有效的导航信息，即果园机械的横向偏差和航向偏差。

    果园机械在整个果园环境中技术流程如图8所示。首先判断激光扫描到的果树数量能否满足精确的路径规划要求。如果能够满足，说明在果树行间，分别利用激光扫描仪和视觉传感器获取果园机械前方两侧果树位置信息和前方环境信息，并对信息分析与处理，完成行间主要导航信息和行间辅助导航信息的提取，并融合两者的信息，构建果园机械前方三维环境，完成前方环境中障碍物的检测，提取用于果园机械导航的有效导航信息，进行路径规划处理，获得果园机械的横向偏差和航向偏差。

    当扫描到的果树数量不能够满足要求时，判断果园机械是否已到地头，如果没到地头，利用航向角传感器检测果园机械航向角信息，利用角度传感器检测得到果园机械前轮转角信息，利用速度传感器检测得到果园机械速度信息。并构建卡尔曼滤波器融合三者的信息，求解果园机械的航向偏差和前轮转角。当果园机械已经行驶到地头时，首先基于最优控制理论规划果园机械转弯的理论路径，利用GPS传感器获取果园机械的实际位置信息，将理论路径坐标与实际位置坐标相比较，得到果园机械的横向偏差和航向偏差。

Claims (5)

1.一种果园机械导航定位方法，其特征在于，包括以下步骤：

1）当果园机械在果树行间行驶时，通过融合激光扫描信息和视觉传感器检测得到的信息，构建果园机械前方三维环境，提取有效导航信息；

2）当果园机械即将行驶到果树行尽头时，利用卡尔曼滤波器融合果园机械内部传感器测量得到的信息，获得用于果园机械导航定位的信息；

3）当果园机械行驶到果园地头时，首先采用最优控制规划果园机械地头转弯路径，利用GPS传感器测量得到其位置信息，通过比较理论路径坐标与实际位置坐标，获得用于果园机械导航定位的信息。

2.根据权利要求1所述的果园机械导航定位方法，其特征在于：果园机械在果树行间导航定位是：以激光扫描得到的果园机械两侧果树位置信息为主要导航信息，获取用于果园机械导航的初始航向偏差和横向偏差，以视觉传感器测量得到的果树行间信息为辅助导航信息，通过两者信息的融合，构建果园机械前方三维环境，预测果园机械在果树行间的通过性。

3.根据权利要求2所述的果园机械导航定位方法，其特征在于：利用激光扫描信息实时标定视觉传感器检测得到的信息，将激光扫描仪和视觉传感器建立直角坐标系，建立图像中像素点数量与实际环境中距离长度的对应关系；然后以图像中路径为中心，在图像上建立两个在实际环境中垂直于水平面的面，在图像栅格中反映实际环境对应空间的障碍物信息。

4.根据权利要求1所述的果园机械导航定位方法，其特征在于：果园机械在即将行驶到果树行尽头时的导航定位是：利用航向角传感器测量果园机械的航向角，利用角度传感器测量果园机械的前轮转角，利用速度传感器测量果园机械的速度，然后构建卡尔曼滤波器，对三者信息进行融合，获取用于果园机械导航的横向偏差和前轮转角。

5.根据权利要求1所述的果园机械导航定位方法，其特征在于：果园机械行驶到果园地头的导航定位是：首先利用最优控制规划转弯路径，然后利用GPS传感器得到果园机械的定位坐标，比较理论路径坐标与实际位置坐标，获得用于果园机械导航的横向偏差和航向偏差。

Images