CN115701524B - 空间结构框架的构建方法及使用其的导航方法和系统 - Google Patents

Abstract

本发明公开了一种用于果园垄内车辆定位的空间结构框架的构建方法以及采用该空间结构框架进行果园垄内车辆导航的方法和系统。所述空间结构框架的构建方法包括：利用车载的传感器沿着车辆所在果园垄的中心线在车辆行驶预定距离内连续采集车辆前方该果园垄的预定范围内的点云图像帧；通过ICP方法使得后续每帧点云图像帧与初始点云图像帧对齐，获取在所述预定范围内的果园垄内环境点云信息；通过对所述预定范围的空间体素化(Voxelization)处理以获得所述预定范围对应的空间结构框架；以及统计空间结构框架的每个体素单元内果园垄内环境点云频次，以获得每个体素单元存在具体目标对象的概率。

Description

空间结构框架的构建方法及使用其的导航方法和系统

技术领域

本公开涉及一种车辆导航方法和系统。更具体地说，本公开涉及一种构建用于果园垄内车辆定位的空间结构框架的方法以及采用该空间结构框架进行果园垄内车辆导航的方法和系统。

背景技术

随着农业种植的大规模化，人们对农业机械化以及农业的智能化的需求越来越高。因此人们需要在大规模的农业种植中实现农业机械的智能化，这就使得在大规模的果园中能够实现车辆的自动驾驶称为现实的需求。

采用卫星定位(GNSS)，例如北斗导航和GPS导航，进行果园的车辆自动驾驶定位和导航是目前普遍采用的定位和导航措施。但是，在果园中，尤其是高大果树的果园中，由于高密度的树叶和枝条的存在而会被遮挡或者发生多路径折射效应的果园垄内环境中，卫星定位和导航导致定位信号接收不到或较弱，经常会出现定位中断的情况，这会导致基于GNSS信号的定位和导航方式在果园内难以稳定实现自动驾驶。

在常用的定位导航技术中，目前人们通常通过识别图像中特定的特征来进行定位，例如在图像中识别道路边界线。由于公路道路的结构化特点以及相对清晰稳定的道路标识的存在，这种特征识别的方式通常被应用在乘用车自动驾驶的定位中。因此，有人也试图通过这种特征识别的方式来消除高大果园中由于采用GNSS信号进行定位和导航的不稳定性问题。但是农业果园是一个半结构化的工作环境，图像特征会随着光线变化，季节变化，而且也会随着植物生长和种类变化等改变，因此，依赖图像特征的方式进行环境识别来进行定位的方式，往往不能够跨时间跨空间的稳定工作。一旦光线变化、季节变化以及职务生长或果树种类的变化，就会形成新环境，因此新环境就需要大量人工标注和重新训练算法来保障定位的准确性。

为此，人们也采用一次性对整个果园生成导航地图，并基于生成的导航地图进行定位导航，类似于扫地机器人，一次性形成整个被打扫空间的地图。但是，由于果园巨大，果树随着季节的生长变化会形成不同的物理空间，这也会导致用户需要不断更新地图，这一方面导致车辆需要准备巨大的地图存储空间以及经常性的地图更新，另一方面，这种频繁的地图更新过程给用户造成了极大的操作负担。

因此，对于大规模高大植株种植园的所有者而言，急需一种简单易行，成本低廉的车辆在种植园内自动驾驶时的定位和导航方法和系统。

发明内容

为此，为解决上述技术问题，本公开的发明人注意到了种植园或果园通常的重复性特点，为此本公开提出了一种简单易行的果园或高大植株种植园的垄内定位导航方法和系统。本公开提出了一种构建用于果园垄内车辆定位的空间结构框架的方法，包括：利用车载的传感器沿着车辆所在果园垄的中心线在车辆行驶预定距离内连续采集车辆前方该果园垄的预定范围内的点云图像帧；通过ICP方法使得后续每帧点云图像帧与初始点云图像帧对齐，获取在所述预定范围内的果园垄内环境点云信息；通过对所述预定范围的空间体素化(Voxelization)处理以获得所述预定范围对应的空间结构框架；以及统计空间结构框架的每个体素单元内果园垄内环境点云频次，以获得每个体素单元存在具体目标对象的概率。

根据本公开的构建用于果园垄内车辆定位的空间结构框架的方法，还包括：对点云图像帧执行直通滤波处理，以消除超出所述预定范围之外的点云以及所述预定范围内的天空噪点点云以及地面噪点点云。

根据本公开的构建用于果园垄内车辆定位的空间结构框架的方法，还包括：在车载卫星定位组件检测到所述车载传感器连续两次进入同一果园的时间间隔相差预定时间间隔时，重新启动所述空间结构框架的构建。

根据本公开的构建用于果园垄内车辆定位的空间结构框架的方法，其中所述预定范围为传感器前方5-30米的范围。

根据本公开的构建用于果园垄内车辆定位的空间结构框架的方法，其中所述体素单元对应的空间为边长为0.1-0.3米的立方体空间。

根据本公开的另一个方面，提供了一种基于空间结构框架进行果园垄内车辆导航的方法，包括：采用如前所述的方法构建用于果园垄内车辆定位的空间结构框架；利用车载的传感器实时获取车辆所在果园垄的前方的预定范围内的实时点云图像帧；通过ICP方法使得实时点云图像帧与初始点云图像帧对齐；通过蒙特卡洛方式模拟实时点云图像帧沿着垂直于果园垄的延伸方向的水平方向的偏移量以及围绕果园垄的延伸方向的偏航角组合获得多个对应的模拟校正实时点云图像帧；选择所述多个对应的模拟校正实时点云图像帧中与所述空间结构框架的果园垄中心最接近的模拟校正实时点云图像帧；以及利用最接近的模拟校正实时点云图像帧进行模拟时所对应的水平方向的偏移量以及偏航角对所述车辆进行反向定位纠偏，从而使得车辆返回到所述果园垄的中心位置。

根据本公开的基于空间结构框架进行果园垄内车辆导航的方法，还包括：通过闭环算法持续定位车辆，调整车辆的速度和方向，使得车辆被导航趋向果园垄的中心位置。

根据本公开的基于空间结构框架进行果园垄内车辆导航的方法，其中选择最接近的模拟校正实时点云图像帧包括：针对每个模拟校正实时点云图像帧，按照所述空间结构框架获取每个体素单元中的点云概率；计算所有非零的点云概率的乘积，获得与每个模拟校正实时点云图像帧的总体点云概率；以及选择总体点云概率最高的模拟校正实时点云图像帧作为最接近的模拟校正实时点云图像帧。

根据本公开的基于空间结构框架进行果园垄内车辆导航的方法，还包括：测量车辆从进入果园垄后行走距离，并在行走距离大于果园垄长时，将车辆转向导航到相邻的果园垄。

根据本公开的另一个方面，提供了一种的基于空间结构框架进行果园垄内车辆导航的系统，包括：车载传感器，用于沿着车辆所在果园垄的中心线在车辆行驶预定距离内连续采集车辆前方该果园垄的预定范围内的点云图像帧；空间结构框架构建组件，包括图像帧对齐组件，通过ICP方法使得后续每帧点云图像帧与初始点云图像帧对齐，获取在所述预定范围内的果园垄内环境点云信息；体素化处理组件，通过对所述预定范围的空间体素化(Voxelization)处理以获得所述预定范围对应的空间结构框架；以及点云频次统计组件，统计空间结构框架的每个体素单元内果园垄内环境点云频次，以获得每个体素单元存在具体目标对象的概率；模拟校正组件，通过蒙特卡洛方式模拟车载传感器实时获取的实时点云图像帧沿着垂直于果园垄的延伸方向的水平方向的偏移量以及围绕果园垄的延伸方向的偏航角组合，获得多个对应的模拟校正实时点云图像帧；选择组件，选择所述多个对应的模拟校正实时点云图像帧中与所述空间结构框架的果园垄中心最接近的模拟校正实时点云图像帧；以及导航纠偏指令组件，利用最接近的模拟校正实时点云图像帧进行模拟时所对应的水平方向的偏移量以及偏航角对所述车辆进行反向定位纠偏，从而使得车辆返回到所述果园垄的中心位置。

根据本公开的基于空间结构框架进行果园垄内车辆导航的系统，还包括：滤波组件，对点云图像帧执行直通滤波处理，以消除超出所述预定范围之外的点云以及所述预定范围内的天空噪点点云以及地面噪点点云。

根据本公开的基于空间结构框架进行果园垄内车辆导航的系统，还包括：更新指令组件，在车载卫星定位组件检测到所述车载传感器连续两次进入同一果园的时间间隔相差预定时间间隔时，指令重新启动所述空间结构框架的构建。

通过根据本公开的基于空间结构框架进行果园垄内车辆导航的系统极及其方法，利用激光雷达产生的点云信号，通过建立果园或高大植株的种植园内空间结构框架进行车辆相对于果园垄中心线的定位，该技术可以让车辆在没有GNSS信号或是GNSS信号不佳的环境下进行车辆的自动导航。

本发明的其它优点、目标和特征将部分通过下面的说明体现，部分还将通过对本发明的研究和实践而为本领域的技术人员所理解。

附图说明

图1所示的是根据本公开应用场景示意图。

图2所示的是根据本公开的构建用于果园垄内车辆定位的空间结构框架的方法的流程示意图。

图3所示的是采用车载的传感器采集的一定范围内的含有点云信号的点云图像帧的示意图。

图4所示的是根据本公开的方法所形成的空间结构框架的立体实例的示意图。

图5所示的根据本公开的方法所形成的空间结构框架的平面投影实例的示意图。

图6所示的是根据本公开的基于空间结构框架进行果园垄内车辆导航的方法的流程示意图。

图7所示的是根据公开的基于空间结构框架进行果园垄内车辆导航的系统的原理示意图。

具体实施方式

下面结合实施例和附图对本发明做进一步的详细说明，以令本领域技术人员参照说明书文字能够据以实施。

这里将详细地对示例性实施例进行说明，其示例表示在附图中。下面的描述涉及附图时，除非另有表示，不同附图中的相同数字表示相同或相似的要素。以下示例性实施例中所描述的实施方式并不代表与本公开相一致的所有实施方式。相反，它们仅是与如所附权利要求书中所详述的、本公开的一些方面相一致的装置和方法的例子。

在本公开使用的术语是仅仅出于描述特定实施例的目的，而非旨在限制本开。在本公开和所附权利要求书中所使用的单数形式的“一种”、“所述”和“该”也旨在包括多数形式，除非上下文清楚地表示其他含义。还应当理解，本文中使用的术语“和/或”是指并包含一个或多个相关联的列出项目的任何或所有可能组合。

应当理解，尽管在本公开可能采用术语第一、第二、第三等来描述各种信息，但这些信息不应限于这些术语。这些术语仅用来将同一类型的信息彼此区分开。例如，在不脱离本公开范围的情况下，在下文中，两个可能设备之一可以被称为第一图像帧也可以被称为第二图像帧。取决于语境，如在此所使用的词语“如果”可以被解释成为“在……时”或“当……时”或“响应于确定”。

为了使本领域技术人员更好地理解本公开，下面结合附图和具体实施方式对本公开作进一步详细说明。

图1所示的是根据本公开应用场景示意图。如图1所示，农用车辆在分垄的高大植株种植园的垄内行走。高大植株种植园内通常种植有高于农用车辆的果树或其他经济作物。由于树冠的自然生长经常或彼此交盖，因此会使得车辆接收到的GNNS信号不稳定，导致车辆在这种种植园中难以实现稳定的垄内定位和导航，因此基于GNNS信号的自动驾驶也变得难以实现。同样，由于果树或其他高大植物一年四季存在不同的特征和形态，导致用户会为此投入大量的精力和费用进行重复构建果园地图，因此，采用视觉特征进行定位导航也变得不经济和现实。

为了解决果园中的自动驾驶和定位导航问题，本发明申请人认识到果园中果园垄的重复性特征以及为了简化定位导航的操作性，本公开首先提供了一种操作简单并且用户友好的空间结构框架构建方法。图2所示的是根据本公开的构建用于果园垄内车辆定位的空间结构框架的方法的流程示意图。如图2所示，首先，在步骤S100处，随着初始布置在任意果园垄中心的农用车辆沿着果园垄中心线行走一段预定距离，车载的传感器310(例如车载激光雷达)(如后面详述)会连续采集车辆前方该果园垄的预定范围内的点云图像帧。如何采集点云图像帧属于本领域公知的技术手段，因此不在此赘述。后面的图3所示的是采用车载的传感器310所采集的一定范围内的含有点云信号的点云图像帧。点云图像帧的采集时间间隔可以根据实际需要由用户设定，可以在0.01秒到3秒之间，例如0.1秒、0.2秒、0.5秒、0.8秒、1秒等。该采集间隔时间按既可以更短，也可以更长。图3显示的点云图像帧中的每个图像云点代表了在该图像帧中的该位置处存在该图像帧所对应物理空间中存在具体的物理对对象。举例而言，该点云可能是果树的叶子、果子、树干等等。

由于在车辆行驶过程中连续获取的点云图像帧彼此之间存在沿着果园垄延伸方向的坐标位置差，在步骤S110处，通过后面描述的空间结构框架构建组件320对在该预定距离内连续采集的点云图像帧进行坐标对齐。坐标对齐方式常规的坐标对齐方式，在此不进行赘述。尽管图3中显示的点云图像帧只有6幅，但是根据需要，在预定的行驶距离内，所连续采集的点云图像帧的数量可以更多，也可以只有一幅。通过后续每次点云图像帧与初始点云图像帧的坐标对齐，从而可以对所有连续采集的点云图像帧在垂直方向叠加，从而形成形成所述传感器前方在预定行驶距离内的立体点云空间结构，该空间结构中的每个点云代表了在该行驶距离内的视野内的所有物理对象的反射信号，代表了该空间对应的位置存在物理对象，从而获取在所述预订范围内的果园垄内的点云信息。

接着在步骤S120处，空间结构框架构建组件320通过对所述预定范围的空间体素化(Voxelization)处理以获得所述预定范围对应的空间结构框架。通过体素化处理，使得预定范围的空间被格点化。体素化过程本身属于空间格点化的一种常用技术手段，因此不在此赘述。通过体素化后的预定范围的空间间由一些体素单元构成。每个体素单元对应的空间为边长为0.1-0.3米的立方体空间。如果需要精细一些，则边长可以更小，例如0.05米。通常边长为0.1米。尽管体素化步骤S120在步骤S100和步骤S110中进行描述，但是并不意味着该步骤的顺序一定如此安排。根据本公开的构建方法，可以在构建的初始阶段直接规定预定范围的体素化过程，随后进行步骤S100和S110。因此针对图2的描述和显示并不是对两者顺序关系的限定。可选择地，可以在执行步骤S100和步骤S110的同时进行体素化步骤S120。

接着，在步骤S130处，空间结构框架构建组件320统计空间结构框架的每个体素单元内果园垄内环境点云频次，以获得每个体素单元存在具体目标对象的概率。这样一个体素单元内点云的频次，代表了在该体素单元中存在对应物理对象的概率。例如如果连续10幅点云图像帧通过对齐之后，在一个体素单元内存在8个点云，则意味着该体素单元对应的物理空间内存在物理对象的概率为0.8。由此形成一种含有点云概率点云信息的空间结构框架。图4所示的是根据本公开的方法所形成的空间结构框架的立体实例的示意图。而图5所示的根据本公开的方法所形成的空间结构框架的平面投影实例的示意图。如图4所示，可以通过立体空间结构框架中的不同色彩或的浓淡来代表每个体素单元中存在物理对象的概率高低。为了满足专利申请要求，本申请采用不同深度的灰度来表达不同的概率。如图5所示，在平面空间结构框架示意图中，可以采用每个方格的灰度的深浅来代表体素单元中存在物理对象的高低，也可以采用不同的纹路来代表。

可选择地，需要指出的是，在点云图像帧采集过程中，必然会存在一些不符合要求的噪声点。为了减少后续处理过程中的噪声的干扰，根据本公开的构建用于果园垄内车辆定位的空间结构框架的方法可以在获取原始点云图像帧后，可以对点云图像帧执行直通滤波处理，以消除超出所述预定范围之外的点云以及所述预定范围内的天空噪点点云以及地面噪点点云。所述预定范围为传感器前方5-30米的范围。

进一步，可选择地，根据本公开的构建用于果园垄内车辆定位的空间结构框架的方法还可以在车载卫星定位组件(未示出)检测到所述车载传感器连续两次进入同一果园的时间间隔相差预定时间间隔时，重新启动所述空间结构框架的构建。也可以在车载卫星定位组件(未示出)检测到所述车载传感器进入不同果园时，重新启动所述空间结构框架的构建。

由于果园垄环境的重复性，因此，通过短距离采集形成的空间结构框架可以被用于整个果园的自动驾驶定位导航而无需建立整个果园的导航地图，从而极大地减少了地图导航所需的存储空间，更重要的是，可以根据季节的变换来实时建立更适合果园当前生长状态的空间结构框架，减少了对大面积果园每次在不同季节重复建立果园地图的成本，降低了用户的可操作性。

根据本公开的另一个实施例，提供了一种如图6所示的基于空间结构框架进行果园垄内车辆导航的方法。图6所示的是根据本公开的基于空间结构框架进行果园垄内车辆导航的方法的流程示意图。如图6所示，首先，在步骤S10处，采用如前针对图2所描述的方法构建用于果园垄内车辆定位的空间结构框架。随后，在步骤S20处，利用车载的传感器310实时获取车辆所在果园垄的前方的预定范围内的实时点云图像帧。接着，在步骤S30处，通过ICP方法使得实时点云图像帧与初始点云图像帧对齐。该对齐过程可直接采用空间结构框架构建组件320中的对齐单元来执行。可选择地，也可以通过直通滤波方式对实时点云图像帧进行滤波处理。

接着，在步骤S30处，模拟校正组件330通过蒙特卡洛方式模拟实时点云图像帧沿着垂直于果园垄的延伸方向的水平方向的偏移量以及围绕果园垄的延伸方向的偏航角组合获得多个对应的模拟校正实时点云图像帧。蒙特卡洛模拟是一种常用的模拟方式，因此不在此赘述。具体而言，针对每个实时点云图像帧，假设其实在车辆水平方向偏移一定的距离以及围绕果园垄的中心线偏转一定的角度的情况下获得的，因此通过蒙特卡洛模拟方式，使得实时点云图像帧进行偏移变换和旋转变换，形成模拟校正实时点云图像帧。由于模拟的偏移距离或偏转角度的幅度和组合不同，因此针对一幅实时点云图像帧，会形成多个模拟校正实时点云图像帧。其目的是为了试图获得一种校正后的图像与正确行驶方向获得点云图像帧相近或相同。这就为后续的车辆定位和导航进行纠偏提供了依据。因此，随后，在步骤S40处，车辆导航系统中选择组件340选择所述多个对应的模拟校正实时点云图像帧中与所述空间结构框架的果园垄中心最接近的模拟校正实时点云图像帧。举例来说，比对的具体过程为通过蒙特卡洛模拟来对可能的y方向偏移和yaw方向偏移取样得到N个模拟取样姿态，通过假设被模拟取样姿态为真实姿态来将当前获得的点云转换到空间框架结构坐标系下(y＝0,yaw＝0，如取样姿态是真实的姿态转换后的点云应该正对垄中心)。通过对比，获取模拟校正实时点云图像帧中存在点云图像的体素单元，并找到空间框架结构中的对应的体素单元以及该对应的体素单元的点云频次或归一化的频率。随后对所有找到的对应体素单元的点云频次或归一化的频率。因此，车辆前进方向视图的网格化空间中各个小格子的点云概率分布。其中每一个格子的深浅代表了概率的大小(这个格子所代表的空间有物体存在的概率0～1)。如图4所示，所有的点云体素单元组合在一起实际上是一个三维框架，如图4所示，其中不同的部分的体素单元的颜色深度代表不同的点云概率。由于三维的网格空间无法直观可视化，为此，采用图5所示的二维视图进行代表。如图4所示的三维空间结构，空间都被点占满，只是概率不同而已，几乎所有点的概率都大于零的。为了可视化，图4中没有显示小于一定概率的体素单元。具体到实际物体来说，这些体素单元的概率对应所有存在实际空间的东西的概率(比如树干，树枝条，树叶，和传感器的噪声点的分布)。在空间框架结构中取出转换后的点云的每一个点的概率，所有点概率相乘可以得到一个总体的概率。所有取样姿态中，转换后的点云总体概率最高的姿态即为最佳估计姿态。可选择地，为了消除噪声影响。对于实际没有点点云的体素单元，则赋予其一个固定的概率值，例如0.1。

最后，在步骤S50处导航纠偏指令组件350根据所选择最接近的模拟校正实时点云图像帧进行模拟时所对应的水平方向的偏移量以及偏航角对所述车辆进行反向定位纠偏，从而使得车辆返回到所述果园垄的中心位置。举例而言，如果最接近的模拟校正实时点云图像帧在被模拟是采用的左向偏移距离为0.5米、逆时针偏转角度为5度，则在进行定位纠偏时，则指令车辆顺时针转向5度并向右行走偏移0.5米，从而使得车辆重新回到果园垄的中心，实现重新准确定位和正确导航。

可选择地，根据本公开的基于空间结构框架进行果园垄内车辆导航的方法还可以通过闭环算法持续定位车辆，调整车辆的速度和方向，使得车辆被导航趋向果园垄的中心位置。

可选择地，需要指出的是，选择组件340选择最接近的模拟校正实时点云图像帧是，首先针对每个模拟校正实时点云图像帧，按照所述空间结构框架获取每个体素单元中的点云概率；接着计算所有非零的点云概率的乘积，获得与每个模拟校正实时点云图像帧的总体点云概率；最后选择总体点云概率最高的模拟校正实时点云图像帧作为最接近的模拟校正实时点云图像帧。

进一步，根据本公开的基于空间结构框架进行果园垄内车辆导航的方法还可以测量车辆从进入果园垄后行走距离，并在行走距离大于果园垄长时，将车辆转向导航到相邻的果园垄。

根据本公开的另一实施例，提供了一种的基于空间结构框架进行果园垄内车辆导航的系统。图7所示的是根据公开的基于空间结构框架进行果园垄内车辆导航的系统的原理示意图。如图7所示，基于空间结构框架进行果园垄内车辆导航的系统包括：车载传感器310、空间结构框架构建组件320、模拟校正组件330、选择组件340以及导航纠偏指令组件350。

车载传感器310首先沿着车辆所在果园垄的中心线在车辆行驶预定距离内连续采集车辆前方该果园垄的预定范围内的点云图像帧。

空间结构框架构建组件320包括：图像帧对齐组件321、体素化处理组件322以及点云频次统计组件323。图像帧对齐组件321通过ICP方法使得后续每帧点云图像帧与初始点云图像帧对齐，获取在所述预定范围内的果园垄内环境点云信息。体素化处理组件322通过对所述预定范围的空间体素化(Voxelization)处理以获得所述预定范围对应的空间结构框架。点云频次统计组件323统计空间结构框架的每个体素单元内果园垄内环境点云频次，以获得每个体素单元存在具体目标对象的概率

模拟校正组件330通过蒙特卡洛方式模拟车载传感器实时获取的实时点云图像帧沿着垂直于果园垄的延伸方向的水平方向的偏移量以及围绕果园垄的延伸方向的偏航角组合，获得多个对应的模拟校正实时点云图像帧。选择组件340选择所述多个对应的模拟校正实时点云图像帧中与所述空间结构框架的果园垄中心最接近的模拟校正实时点云图像帧。

导航纠偏指令组件340，利用最接近的模拟校正实时点云图像帧进行模拟时所对应的水平方向的偏移量以及偏航角对所述车辆进行反向定位纠偏，从而使得车辆返回到所述果园垄的中心位置。

可选择地，基于空间结构框架进行果园垄内车辆导航的系统，还包括滤波组件(未示出)。滤波组件对点云图像帧执行直通滤波处理，以消除超出所述预定范围之外的点云以及所述预定范围内的天空噪点点云以及地面噪点点云。根据本公开的基于空间结构框架进行果园垄内车辆导航的系统还包括更新指令组件300。更新指令组件300在车载卫星定位组件检测到所述车载传感器连续两次进入同一果园的时间间隔相差预定时间间隔时，指令重新启动所述空间结构框架的构建。所述预订的时间间隔，例如为1周、两周、一个月。

以上结合具体实施例描述了本公开的基本原理，但是，需要指出的是，对本领域的普通技术人员而言，能够理解本公开的方法和装置的全部或者任何步骤或者部件，可以在任何计算装置(包括处理器、存储介质等)或者计算装置的网络中，以硬件、固件、软件或者它们的组合加以实现，这是本领域普通技术人员在阅读了本公开的说明的情况下运用他们的基本编程技能就能实现的。

因此，本公开的目的还可以通过在任何计算装置上运行一个程序或者一组程序来实现。所述计算装置可以是公知的通用装置。因此，本公开的目的也可以仅仅通过提供包含实现所述方法或者装置的程序代码的程序产品来实现。也就是说，这样的程序产品也构成本公开，并且存储有这样的程序产品的存储介质也构成本公开。显然，所述存储介质可以是任何公知的存储介质或者将来所开发出来的任何存储介质。

还需要指出的是，在本公开的装置和方法中，显然，各部件或各步骤是可以分解和/或重新组合的。这些分解和/或重新组合应视为本公开的等效方案。并且，执行上述系列处理的步骤可以自然地按照说明的顺序按时间顺序执行，但是并不需要一定按照时间顺序执行。某些步骤可以并行或彼此独立地执行。

上述具体实施方式，并不构成对本公开保护范围的限制。本领域技术人员应该明白的是，取决于设计要求和其他因素，可以发生各种各样的修改、组合、子组合和替代。任何在本公开的精神和原则之内所作的修改、等同替换和改进等，均应包含在本公开保护范围之内。

Claims (10)

1.一种基于空间结构框架进行果园垄内车辆导航的方法，包括：

在车载卫星定位组件检测到车载传感器连续两次进入同一果园的时间间隔相差预定时间间隔时，重新启动所述空间结构框架的构建;

利用车载的传感器实时获取车辆所在果园垄的前方的预定范围内的实时点云图像帧；

通过ICP方法使得实时点云图像帧与初始点云图像帧对齐；

获取在所述预定范围内的果园垄内环境点云信息；

通过对所述预定范围的空间体素化处理以获得所述预定范围对应的空间结构框架；

统计空间结构框架的每个体素单元内果园垄内环境点云频次，以获得每个体素单元存在具体目标对象的概率；

通过蒙特卡洛方式模拟实时点云图像帧沿着垂直于果园垄的延伸方向的水平方向的偏移量以及围绕果园垄的延伸方向的偏航角组合获得多个对应的模拟校正实时点云图像帧；以及

选择所述多个对应的模拟校正实时点云图像帧中与所述空间结构框架的果园垄中心最接近的模拟校正实时点云图像帧，包括针对每个模拟校正实时点云图像帧，按照所述空间结构框架获取每个体素单元中的点云概率，计算所有非零的点云概率的乘积，获得与每个模拟校正实时点云图像帧的总体点云概率，以及选择总体点云概率最高的模拟校正实时点云图像帧作为最接近的模拟校正实时点云图像帧；以及

利用最接近的模拟校正实时点云图像帧进行模拟时所对应的水平方向的偏移量以及偏航角对所述车辆进行反向定位纠偏，从而使得车辆返回到所述果园垄的中心位置。

2.根据权利要求1所述的基于空间结构框架进行果园垄内车辆导航的方法，还包括：

通过闭环算法持续定位车辆，调整车辆的速度和方向，使得车辆被导航趋向果园垄的中心位置。

3.根据权利要求1所述的基于空间结构框架进行果园垄内车辆导航的方法，还包括：

测量车辆从进入果园垄后行走距离，并在行走距离大于果园垄长时，将车辆转向导航到相邻的果园垄。

4.根据权利要求1所述的基于空间结构框架进行果园垄内车辆导航的方法，还包括：

对点云图像帧执行直通滤波处理，以消除超出所述预定范围之外的点云以及所述预定范围内的天空噪点点云以及地面噪点点云。

5.根据权利要求1-4之一所述的基于空间结构框架进行果园垄内车辆导航的方法，其中所述预定范围为传感器前方5米-30米的范围。

6.根据权利要求1-4之一所述的基于空间结构框架进行果园垄内车辆导航的方法，其中所述体素单元对应的空间为边长为0.1米-0.3米的立方体空间。

7.一种基于空间结构框架进行果园垄内车辆导航的系统，包括:

更新指令组件，在车载卫星定位组件检测到车载传感器连续两次进入同一果园的时间间隔相差预定时间间隔时，指令重新启动空间结构框架的构建；

车载传感器，用于沿着车辆所在果园垄的中心线在车辆行驶预定距离内连续采集车辆前方该果园垄的预定范围内的点云图像帧；

空间结构框架构建组件，包括：图像帧对齐组件，通过ICP方法使得后续每帧点云图像帧与初始点云图像帧对齐，获取在所述预定范围内的果园垄内环境点云信息；体素化处理组件，通过对所述预定范围的空间体素化处理以获得所述预定范围对应的空间结构框架；以及点云频次统计组件，统计空间结构框架的每个体素单元内果园垄内环境点云频次，以获得每个体素单元存在具体目标对象的概率；

模拟校正组件，通过蒙特卡洛方式模拟车载传感器实时获取的实时点云图像帧沿着垂直于果园垄的延伸方向的水平方向的偏移量以及围绕果园垄的延伸方向的偏航角组合，获得多个对应的模拟校正实时点云图像帧；

选择组件，选择所述多个对应的模拟校正实时点云图像帧中与所述空间结构框架的果园垄中心最接近的模拟校正实时点云图像帧，包括针对每个模拟校正实时点云图像帧，按照所述空间结构框架获取每个体素单元中的点云概率，计算所有非零的点云概率的乘积，获得与每个模拟校正实时点云图像帧的总体点云概率，以及选择总体点云概率最高的模拟校正实时点云图像帧作为最接近的模拟校正实时点云图像帧；以及

导航纠偏指令组件，利用最接近的模拟校正实时点云图像帧进行模拟时所对应的水平方向的偏移量以及偏航角对所述车辆进行反向定位纠偏，从而使得车辆返回到所述果园垄的中心位置。

8.根据权利要求7所述的基于空间结构框架进行果园垄内车辆导航的系统，还包括：

滤波组件，对点云图像帧执行直通滤波处理，以消除超出所述预定范围之外的点云以及所述预定范围内的天空噪点点云以及地面噪点点云。

9.根据权利要求7-8之一所述的基于空间结构框架进行果园垄内车辆导航的系统，其中所述预定范围为传感器前方5米-30米的范围。

10.根据权利要求7-8之一所述的基于空间结构框架进行果园垄内车辆导航的系统，其中所述体素单元对应的空间为边长为0.1米-0.3米的立方体空间。