基于铲车载荷监测的GNSS平地作业路径动态规划与导航方法

摘要

本发明公开了基于铲车载荷监测的GNSS平地作业路径动态规划与导航方法，该方法包括平地前和平地中；平地前，地形测量，划分栅格，插值处理，计算农田基准高程并设置导航路径更新周期T；开始平地过程，每经过时间T，在铲车所在的栅格位置处读取所在位置三维坐标、当前行驶方向角及铲车载荷量，计算目标栅格并计算拖拉机转向角；铲车行走过程中改变的地形使用新的高程值覆盖原有数据；为避免多次重复行走，对于已经达到基准高程精度要求的栅格进行标记。该方法实现了整体的最优规划以及实时的最优规划；缩短整个土地平整过程拖拉机的行走路程，提高平整作业效率，解决了传统技术中路径规划方式固定、规划决策缺少实时反馈的问题。

说明书

技术领域

本发明涉及土地平整中的路径导航技术领域，更具体涉及基于铲 车载荷监测的GNSS平地作业路径动态规划与导航方法。

背景技术

农田土地平整可改善农田表面的地形状况，提高农田灌溉效率与 灌水均匀度，达到节水增产的效果。土地精细平整对于水资源管理和 农业现代化均具有积极深远的推动作用。GPS控制平地技术作为一种 智能化的土地平整技术在国内外均已有较好较快的发展，在美国已有 商品化产品。国产GNSS控制平地系统选用高精度高集成性的 GPS+BDS双星定位设备代替国外高成本的GPS接收设备，降低了系 统开发成本，提高了产业化水平，同时，提高了定位精度，降低了农 田平整硬件设备的故障率，增强了系统的实用性与集成性。

现有较普遍的GNSS智能控制平地系统主要包括：GNSS接收设 备、工控机、阀控制器、液压系统和平地铲运设备等几部分。基于这 类GNSS控制平地系统中的导航功能的实现是通过对平地前地形测量 得到的高程数据进行分析，根据一定的最优规划原则，在常见的路径 规划方式，例如梭形和环形中选择最优的一种路径。其中，最优规划 原则不一而足，各有侧重，具体有挖填土方量平衡、运距最短、效率 最高等。一般情况下根据实际情况只能保证其中一个或两个原则达到 最优；并且使用这些规划原则实现的导航方式强调整体的规划，并未 将平地过程中地形的实时改变以及操作者对导航命令不服从的因素考 虑在内，无法体现导航的实时性和灵活性。同时，以往的平地过程中， 平地作业者频繁的观察铲车中载土量从而判断拖拉机行走方向的方式 极大的降低了工作效率，同时也给操作者平地作业造成不便。

发明内容

(一)要解决的技术问题

本发明要解决的技术问题是如何根据农田土地的实时状况，进行 导航，提高导航效率和灵活性。

(二)技术方案

为了解决上述技术问题，本发明提供了基于铲车载荷监测的GNSS 平地作业路径动态规划与导航方法，所述方法包括以下步骤：

S1、平地前，

S101、地形数据采集；标定原点三维坐标，进行边界测量和内部 测量，相对所述原点三维坐标，以相对坐标的形式即高程值保存农田 地形数据；

S102、对农田进行栅格划分，对所述高程值进行插值处理、并以 矩阵H形式存储所述高程值；

S103、计算农田基准高程；利用公式(1)进行计算

$H_{\mathrm{ref}}=H_{\mathrm{avr}}-\frac{1+R}{2\mathrm{RN}}(R\times \Sigma (H_{\mathrm{avr}}-H_f)-\Sigma (H_c-H_{\mathrm{avr}}))---\left(1\right)$

其中，H_ref为基准高程；H_avr为所述步骤S102的插值后得到的高 程值的平均值；R为挖填土方比；N为栅格总数；H_f为所述步骤S102 的插值后得到的高程值小于H_avr的高程值、H_c为所述步骤S102的插值 后得到的高程值大于H_avr的高程值；

S104、设定导航更新周期T，利用公式(2)求取；

$T\approx 2r/\bar{v}(T\in N)---\left(2\right)$

其中，为行驶速度；r为栅格边长；

S2、平地中，

S201、周期性读取当前铲车的方向角、三维坐标以及载荷量；

S202、根据所述步骤S201得到的当前铲车的方向角划定目标区域 范围，并确定目标栅格；

S203、根据所述步骤S201得到的当前铲车的三维坐标，确定当前 铲车所在位置的栅格，根据铲车当前所在的位置栅格和所述目标栅格， 并由当前铲车的方向角计算出铲车转向角；

S204、铲车根据步骤S203给出的转向角行走，并对行走过的栅格 的高程值进行实时数据更新；

S205、对高程值改变的栅格的更新后的高程值进行判断；

S206、如果所述步骤S205中更新后的高程值达到所述基准高程的 精度范围，即满足H_ref-2cm≤H_ij≤H_ref+2cm，则将该栅格的标记参数设 定为0，即已达到平整标准，否则为默认值1，避免多次重复行走；

S207、判断是否达到导航更新周期T时间，若从步骤S201开始已 经完成一个周期T时间的循环，则进入新的一轮循环，重复上述步骤 S201～S206，直至平地全过程结束；若没有达到T时间，则铲车继续按 照当次提示的转向角行驶，直到下一轮循环开始。

优选地，所述步骤S202具体包括以下步骤：

(1)根据当前铲车的方向角θ，确定铲车的目标区域，即目标区域 为铲车当前位置为圆心，中心角为90°、半径长度不固定的扇形，并 且所述扇形随着拖拉机的前进同步推进；

(2)目标区域中，设定有50％以上面积处在区域内的栅格即为有 效栅格，然后找到所有有效栅格的高程值H_ij，其中栅格面积为S₀，则 单个栅格上的土方量(H_ij-H_ref)S₀；

(3)假定各个有效栅格为目标栅格，预描出铲车达到目标栅格的 行走路径，并列出行走路径上所经过的所有有效栅格；

(4)分别计算各个预描出的铲车行走路径上的总装卸土方量 ∑(H_ij-H_ref)S₀；

(5)已知铲车满载时土方量体积为V₀，将步骤(4)中的所述总装 卸土方量换算为相对V₀的比值

(6)根据铲车当前载荷量的百分比参数值R_V，计算出铲车达到各 个目标栅格时铲车的预期载荷量百分比P_ij：

$P_{\mathrm{ij}}=\frac{\Sigma (H_{\mathrm{ij}}-H_{\mathrm{ref}})S_0}{V_0}+R_V---\left(3\right)$

其中，P_ij的取值范围为[0,1]，对于P_ij取值大于1的栅格，采取滤除 的处理方法；

(7)设定平地作业全程最优的铲车载荷参数值为k，栅格标记参数 为n_ij，则可得栅格评价参数D_ij：

$D_{\mathrm{ij}}=n_{\mathrm{ij}}·\frac{P_{\mathrm{ij}}}{k}---\left(4\right)$

其中，k的取值范围为0.6～0.8，对应的栅格标记参数n_ij如下表示

计算所述目标区域中所有有效栅格的栅格评价参数；

(8)步骤(7)得到的所有D_ij值中，最接近1的有效栅格即为目标 栅格。

优选地，所述步骤S203具体包括以下步骤：

(1)根据当前铲车的三维坐标，确定当前铲车所在位置的栅格， 记作栅格A，目标栅格记作栅格B；

(2)所述栅格A和所述栅格B的中心点坐标分别为P(x1,y1)、 Q(x2,y2)，铲车当前位置在行走轨迹曲线上的切线记为L，L直线方 程为

L：y＝l·x+c      (6)

其中l为L的斜率，l＝tan(π/2-θ)＝cotθ；c值通过将P点坐标带入 方程求得，θ为铲车当前位置的方向角；

P、Q的连线为铲车的目标航向，设其所在直线为M，斜率为m：

$m=\frac{y_1-y_2}{x_1-x_2}---\left(7\right)$

所述直线L和M的倾斜角分别为α1、α2，有：

$\left(\begin{array}{c}l=\tan {\alpha }_1\\ m=\tan {\alpha }_2\end{array}\right)---\left(8\right)$

则可得：

$\tan \left|\alpha \right|=\left|\tan ({\alpha }_1-{\alpha }_2)\right|=\left|\frac{\tan {\alpha }_1-\tan {\alpha }_2}{1+\tan {\alpha }_1·\tan {\alpha }_2}\right|=\left|\frac{l-m}{1+l·m}\right|---\left(9\right)$

对求得的结果求解反正切就可以得到转向角的绝对值大小；

(3)确定转向角的方向；铲车在原来行驶方向上左转时转向角α符 号为负，右转时转向角α符号为正；将栅格B中心点坐标带入下式进 行判定：

L＝l·x-y+c      (10)

若L>0，则转向角α>0，铲车右转；若L<0，则α<0，铲车左转。

优选地，所述方向角θ的取值范围为[-45°,45°]。

优选地，所述挖填土方比R的取值范围为(1.0，1.2)。

(三)有益效果

本发明提供了基于铲车载荷监测的GNSS平地作业路径动态规划 与导航方法，该方法在平地前首先进行数据采集，平底过程中实时读 取铲车所在的栅格位置的三维坐标、当前行驶方向角及铲车载荷量， 计算得到目标栅格和转向角；并对已经达到基准高程要求的栅格进行 标记。该方法实现了整体的最优规划以及实时的最优规划；缩短整个 土地平整过程拖拉机的行走路程，提高平整作业效率。

附图说明

为了更清楚地说明本发明实施例或现有技术中的技术方案，下面 将对实施例或现有技术描述中所需要使用的附图作简单地介绍，显而 易见地，下面描述中的附图仅仅是本发明的一些实施例，对于本领域 普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些 附图获得其他的附图。

图1为本发明的基于铲车载荷监测的GNSS平地作业路径动态规 划与导航方法的流程图；

图2为实现本发明的基于铲车载荷监测的GNSS平地作业路径动 态规划与导航方法的GNSS智能控制平地系统的结构示意图；

图3为本发明的铲车的目标区域示意图；

图4为本发明的目标栅格搜索示意图；

图5为本发明的铲车转向角示意图。

具体实施方式

下面结合附图和实施例对本发明作进一步详细描述。以下实施例 用于说明本发明，但不能用来限制本发明的范围。

图1为本发明的基于铲车载荷监测的GNSS平地作业路径动态规 划与导航方法的流程图；所述方法包括以下步骤：

S1、平地前，

S101、地形数据采集；装载有如图2所示GNSS智能控制平地系 统的铲车标定原点三维坐标，进行边界测量和内部测量，相对所述原 点三维坐标，以相对坐标的形式即高程值保存农田地形数据；

S102、对农田进行栅格划分，所划分栅格的边长r取值与铲车使 用的平地铲的宽接近即可；使用克里格差值方法对所述高程值进行插 值处理，并以矩阵H的形式保存地块高程值；

S103、计算农田基准高程，以挖填土方量平衡为原则的，利用公 式(1)进行计算

$H_{\mathrm{ref}}=H_{\mathrm{avr}}-\frac{1+R}{2\mathrm{RN}}(R\times \Sigma (H_{\mathrm{avr}}-H_f)-\Sigma (H_c-H_{\mathrm{avr}}))---\left(1\right)$

其中，H_ref为基准高程；H_avr为所述步骤S102的插值后得到的高 程值的平均值；R为挖填土方比，其取值范围为(1.0,1.2)；N为栅格 总数；H_f为所述步骤S102的插值后得到的高程值小于H_avr的高程值、 H_c为所述步骤S102的插值后得到的高程值大于H_avr的高程值；

S104、设定导航更新周期T，利用公式(2)求取；

$T\approx 2r/\bar{v}(T\in N)---\left(2\right)$

其中，为行驶速度；r为栅格边长；T的取值为整数；

S2、平地中，

S201、周期性读取当前铲车的方向角、三维坐标以及载荷量；所 述载荷量用载荷百分比参数R_V表示，R_V为拉力传感器实时返回的，取 值范围为[0,1]，铲车满载时R_V取值为1，对应土方体积为V₀，空载时R_V取值为0；

S202、根据所述步骤S201得到的当前铲车的方向角划定目标区域 范围，并确定目标栅格；

具体为：

(1)根据当前铲车的方向角θ，确定铲车的目标区域，即目标区 域为铲车当前位置为圆心，中心角为90°、半径长度不固定的扇形， 并且所述扇形随着拖拉机的前进同步推进；为了使铲车运行轨迹相对 圆滑，可以控制铲车在行走时的转向角不能过大，因此设定左右转向 角最大均为45°，如图3所示；

(2)目标区域中，设定有50％以上面积处在区域内的栅格即为有 效栅格，然后保存栅格高程值的矩阵数据中找到所有有效栅格的高程 值H_ij，其中栅格面积为S₀，则单个栅格上的土方量(H_ij-H_ref)S₀；

(3)假定各个有效栅格为目标栅格，如图4所示，例如设定有效 栅格B1为目标栅格，预描出铲车达到目标栅格的行走路径，并列出行 走路径上所经过的所有有效栅格；

(4)分别计算各个预描出的铲车行走路径上的总装卸土方量 ∑(H_ij-H_ref)S₀；

(5)已知铲车满载时土方量体积为V₀，将步骤(4)中的所述总 装卸土方量换算为相对V₀的比值

(6)根据铲车当前载荷量的百分比参数值R_V，计算出铲车达到各 个目标栅格时铲车的预期载荷量百分比P_ij：

$P_{\mathrm{ij}}=\frac{\Sigma (H_{\mathrm{ij}}-H_{\mathrm{ref}})S_0}{V_0}+R_V---\left(3\right)$

其中，P_ij的取值范围为[0,1]，对于P_ij取值大于1的栅格，采取滤 除的处理方法；

(7)设定平地作业全程最优的铲车载荷参数值为k，栅格标记参数 为n_ij，则可得栅格评价参数D_ij：

$D_{\mathrm{ij}}=n_{\mathrm{ij}}·\frac{P_{\mathrm{ij}}}{k}---\left(4\right)$

其中，k的取值范围为0.6～0.8，对应的栅格标记参数n_ij如下表示

计算所述目标区域中所有有效栅格的栅格评价参数；

(8)对于目标区域中其他有效栅格，例如B2，如图4，重复上述 步骤(3)～(7)，分别计算出各个有效栅格的评价参数D_ij，其中D_ij值 最接近1的有效栅格B即为目标区域中的目标栅格；

S203、根据所述步骤S201得到的当前铲车的三维坐标，确定当前 铲车所在位置的栅格，根据铲车当前所在的位置栅格和所述目标栅格， 并由当前铲车的方向角计算出铲车转向角；

具体为：

(1)根据当前铲车的三维坐标，确定当前铲车所在位置的栅格， 记作栅格A，目标栅格记作栅格B；

(2)所述栅格A和所述栅格B的中心点坐标分别为P(x1,y1)、 Q(x2,y2)，如图5所示，铲车当前位置在行走轨迹曲线上的切线记为 L，L直线方程为

L：y＝l·x+c      (6)

其中l为L的斜率，l＝tan(π/2-θ)＝cotθ；c值通过将P点坐标带入 方程求得，θ为铲车当前位置的方向角；

P、Q的连线为铲车的目标航向，设其所在直线为M，斜率为m：

$m=\frac{y_1-y_2}{x_1-x_2}---\left(7\right)$

所述直线L和M的倾斜角分别为α1、α2，有：

$\left(\begin{array}{c}l=\tan {\alpha }_1\\ m=\tan {\alpha }_2\end{array}\right)---\left(8\right)$

则可得：

$\tan \left|\alpha \right|=\left|\tan ({\alpha }_1-{\alpha }_2)\right|=\left|\frac{\tan {\alpha }_1-\tan {\alpha }_2}{1+\tan {\alpha }_1·\tan {\alpha }_2}\right|=\left|\frac{l-m}{1+l·m}\right|---\left(9\right)$

对求得的结果求解反正切就可以得到转向角的绝对值大小；

(3)确定转向角的方向；铲车在原来行驶方向上左转时转向角α符 号为负，右转时转向角α符号为正；在坐标系中，若目标点B在直线L 的左半部则为左转，在L右半部则为右转；将栅格B中心点坐标带入 下式进行判定：

L＝l·x-y+c      (10)

若L>0，则转向角α>0，铲车右转；若L<0，则α<0，铲车左转；

S204、铲车根据步骤S203给出的转向角行走，行驶过程中用GNSS 智能控制平地系统接收的新地形数据对保存的栅格高程值的矩阵数据 实时更新；

S205、对高程值改变的栅格的更新后的高程值进行判断；

S206、如果所述步骤S205中更新后的高程值达到所述基准高程的 精度范围，即满足H_ref-2cm≤H_ij≤H_ref+2cm，则将该栅格的标记参数n_ij设定为0，即已达到平整标准，否则为默认值1，避免多次重复行走；

S207、判断是否达到导航更新周期T时间，若从步骤S201开始已 经完成一个周期T时间的循环，则进入新的一轮循环，重复上述步骤 S201～S206，直至平地全过程结束；若没有达到T时间，则铲车继续按 照当次提示的转向角行驶，直到下一轮循环开始。

本发明的方法通过图2所示的GNSS智能控制土地平整系统完成。 系统包括：GNSS基站、集成控制终端、拉力传感器、液压系统、平地 铲。其中智能集成控制终端的内部结构包括：核心处理模块、GNSS 接收模块、拉力采集转换模块、阀控制模块、电台模块。GNSS接收模 块由GNSS OEM板卡、无线电台板卡和串口电路组成，支持GPS和 BDS联合定位，通过RTK定位获得农田地势三维数据；铲车载荷监控 模块由I-U转换电路、电压跟随电路、单片机和串口电路组成，通过 拉力传感器获得铲车载荷从而实现实时监控；阀控制模块由单片机、 驱动电路和串口电路组成，将数字控制命令转换成电压信号从而控制 铲车电磁液压阀实现铲车的自动升降。

与现有技术相比，本发明为GNSS控制平地过程提供了动态的路 径规划和实时的辅助导航。通过拉力传感器反馈铲车的实时载土量， 以减少全程铲车满载和空载时间为原则，通过动态路径规划和导航达 到提高整个平地过程的工作效率的目的。

以上实施方式仅用于说明本发明，而非对本发明的限制。尽管参 照实施例对本发明进行了详细说明，本领域的普通技术人员应当理解， 对本发明的技术方案进行各种组合、修改或者等同替换，都不脱离本 发明技术方案的精神和范围，均应涵盖在本发明的权利要求范围当中。