CN105277206A - 一种实现山地机械等间距直线行驶的方法 - Google Patents

Abstract

本发明公开了一种实现山地机械等间距直线行驶的方法，将山地机械所走的直线分为若干等长的线段，建立了在等分点处的左右偏向检测法和航向角计算法，利用在等分点处测试得到的实时数据计算出航向角的大小；山地机械根据航向角的大小通过PID控制算法推算出前轮转向角的大小自动纠正偏差，实现直线行驶。本发明对山地机械进行分段式纠正航向偏差，简单可靠；能使山地机械在误差允许范围内直线行驶，且波动小、稳定性好，显著提高了山地机械作业精度和效率，有效的降低了驾驶员的劳动强度。

Description

一种实现山地机械等间距直线行驶的方法

技术领域

本发明涉及山地机械行驶技术领域，尤其涉及一种实现山地机械等间距直线行驶的方法。

背景技术

在进行机械化田间作业时，通常需要山地机械按照预先规划好的直线行驶作业，以便合理利用空间，避免浪费。尤其是在大田且实行套种的地区，山地机械播种时，如果没有按照规划好的直线作业，将给后续的农作物收割、套播作业带来很大的困难，所以实现山地机械直线行驶，对提高农业作业效率和质量有至关重要的意义。同时，山地机械在大田作业时，单靠驾驶员的判断很容易出现偏差，并且这种偏差不容易被察觉，所以山地机械直线行驶实现方法能提高作业精度，提高生产效率；使驾驶员从单调重复的劳动中解放出来，降低驾驶员的劳动强度；避免重复作业，减少浪费，有效地降低生产成本。

目前实现山地机械的直线行驶控制系统多数以GPS为主要的技术，采用RTK-DGPS与各种传感器技术相结合的方式；激光三点动态定位方法等等。以GPS为主的导航技术其成本较高，从而影响了基于GPS导航技术在我国农业生产中的推广；激光三点动态定位方法定位实时性好，但固定基站与主站之间要求严格同步，便于基站的激光发射器实时追踪主站的接收器，若激光接收器接收不到激光，容易造成定位精度波动大，从而影响农用机械直线行驶的稳定性。

发明内容

本发明的目的在于克服现有技术中存在的缺点，提供一种分段式、高精度、稳定性好的实现山地机械等间距直线行驶的方法。

实现本发明目的的技术方案是：

一种实现山地机械等间距直线行驶的方法，包括下述步骤：

(1)将山地机械所行驶的基准直线分为等长的x段，设各段长度为L；

(2)在平行于山地机械所行驶的基准直线外的一条直线上设置两个硬件结构完全相同的固定节点，分别固定在高于农作物高度的支架上，在山地机械的正左边靠近山地机械起始点的位置设置第一固定节点，在第一固定节点正前方设置第二固定节点，在第一固定节点上设置第一激光发射器，在第二固定节点上设置第二激光发射器，所述的两个激光发射器，其初始发射的两束激光线在俯视图上的交点刚好落在山地机械的起始点上；

(3)第一激光发射器和第二激光发射器由其各自固定节点上的电机控制转动，所述的电机每次转动的角度与山地机械所行驶直线的等分线段长度L采用类正比控制方法匹配，其角速度的大小与山地机械的行驶速度采用类正比控制方法匹配，控制第一激光发射器和第二激光发射器对行驶的山地机械进行追踪，并始终保持发射的两束激光线在俯视图上的交点刚好落在山地机械的所走的基准直线上的下一个等分点上；

(4)在山地机械上安装一个移动节点，在移动节点上设置惯性传感器、激光测距传感器、第一激光接收器和第二激光接收器，所述的两个激光接收器在竖直方向上共线，安装高度分别与第一激光发射器和第二激光发射器相同；惯性传感器为陀螺仪和加速度计，且陀螺仪和加速度计分别对山地机械的角速度和加速度进行测量，经过计算后得到山地机械的水平航向角(左、右偏转角度正负显示)和垂直方向上的侧倾角测量值，各自精确度小于0.5°，垂直方向上的最大可倾斜的角度设置成固定数值，当侧倾角大于固定值时，则整个系统停止运行，预防出现交通事故，系统一旦停止运行，则需要重新启动方能再次运行；

(5)山地机械行驶到等分点处，根据左右偏向检测法判断是否向左、右偏，再由航向角计算法推算出航向角的大小，航向角用α_n表示，再根据航向角的大小通过PID控制算法推算出前轮转向角的大小自动纠正偏差，实现直线行驶；

(6)山地机械行驶到基准直线末端，由人工操作掉头，并根据激光测距传感器测量山地机械与固定节点的距离大小，快速地把山地机械开到两个激光接收器能同时接收到激光的位置，确定山地机械即将行驶直线的起点，实现山地机械即将行驶直线与刚行驶直线的间距保持一定值。

所述的基准直线，其确定方法：将山地机械开到作业场地起始点，此时陀螺仪的测量值初始化为0，山地机械所在的方向即为基准直线所在的方向。将基准直线分为等长的x段，可以根据实际的情况改变x的取值，x的取值越大，等长线段的数量越多，山地机械的检偏、纠偏的实时性和准确性越高。

所述的两个固定节点包括：中央处理器，激光发射器，电机，无线数据收发模块和电源模块；所述的激光发射器，电机，无线数据收发模块，电源模块分别通过导线连接到中央处理器相应的I/O口。

所述的移动节点包括：中央处理器，第一激光接收器，第二激光接收器，无线数据收发模块，激光测距传感器，电机，惯性传感器，光电测速模块和电源模块；所述的两个激光接收器，无线数据收发模块，激光测距传感器，电路，惯性传感器，光电测速模块，电源模块分别通过导线连接到中央处理器相应的I/O口。

所述固定节点和移动节点的电机为自带驱动电路的步进电机。

所述的左右偏向检测法是在等分点处，当两个激光接收器同时接收到激光，说明山地机械没有出现左、右偏转；当第一激光接收器先接收到第一激光发射器发射的激光，说明山地机械向左偏；当第二激光接收器先接收到第二激光发射器发射的激光说明山地机械向右偏。

所述的固定节点和移动节点之间的通信方式由无线数据收发模块实现。

步骤(3)所述的类正比控制方法是，固定节点上的电机，其每次转动的角度与线段的长度L匹配，其角速度的大小与山地机械的行驶速度匹配，计算两个固定节点上的电机每次转动的角度与角速度的表达式为：

(1)第一固定节点的电机每次转动角度Δθ_n和角速度最小值Δω_n(min)计算表达式：

${\mathrm{\Δ\θ}}_n=arctan\frac{nL}{d+(m-1)\mathrm{\Δ}L}-arctan\frac{(n-1)L}{d+(m-1)\mathrm{\Δ}L}---\left(1\right)$

${\mathrm{\Δ\ω}}_{n\left(\min \right)}=\[arctan\frac{nL}{d+(m-1)\mathrm{\Δ}L}-arctan\frac{(n-1)L}{d+(m-1)\mathrm{\Δ}L}\]/\left(\frac{L}{v}\right)---\left(2\right)$

其中，n表示的是等分点的序号，取值为：1，2，...，x；m的取值为山地机械所走的基准直线的序号；△L表示基准直线之间的间距；d为第一固定节点与山地机械起始点的距离；v为山地机械行驶的速度；

(2)第二固定节点的电机每次转动角度Δθ_n'和角速度最小值Δω_n'_(min)计算表达式：

${{\mathrm{\Δ\θ}}_n}^{\′}=arctan\frac{(x-n+1)L}{d+(m-1)\mathrm{\Δ}L}-arctan\frac{(x-n)L}{d+(m-1)\mathrm{\Δ}L}---\left(3\right)$

${{\mathrm{\Δ\ω}}_n}^{\′}\left(\min \right)=\[arctan\frac{(x-n+1)L}{d+(m-1)\mathrm{\Δ}L}-arctan\frac{(x-n)L}{d+(m-1)\mathrm{\Δ}L}\]/\left(\frac{L}{v}\right)---\left(4\right)$

表达式中各参数的含义与表达式(2)相同，x表示的是等分点的总数。

步骤(5)所述的航向角α_n的计算法，其步骤如下，设山地机械在两个相邻的等分点之间实际行驶的路程为S_n；由陀螺仪数据获得山地机械在两个相邻的等分点之间的航向角测量值为

(1)当第一激光接收器先接收到第一激光发射器发射的激光时，则可判定向左偏离，且α_n的计算表达式：

${\mathrm{\α}}_n=\arcsin \frac{L\sin \[\arctan \frac{d+\left(m-1\right)\mathrm{\Δ}L}{nL}\]}{S_n}-\arctan \frac{d+\left(m-1\right)\mathrm{\Δ}L}{nL}---\left(5\right)$

表达式中各参数的含义与表达式(1)相同。

同理，α_n的计算表达式：

${\mathrm{\α}}_n=\mathrm{\π}-arcsin\frac{Lsin\[arctan\frac{d+(m-1)\mathrm{\Δ}L}{nL}\]}{S_n}-arctan\frac{d+(m-1)\mathrm{\Δ}L}{nL}---\left(6\right)$

表达式中各参数的含义与表达式(1)相同；

(2)当第二激光接收器先接收到第二激光发射器发射的激光时，则可判定向右偏离，且α_n的计算表达式：

${\mathrm{\α}}_n=arcsin\frac{Lsin\[arctan\frac{d+(m-1)\mathrm{\Δ}L}{(x-n)L}\]}{S_n}-arctan\frac{d+(m-1)\mathrm{\Δ}L}{(x-n)L}---\left(7\right)$

表达式中各参数的含义与表达式(1)相同；

同理，α_n的计算表达式：

${\mathrm{\α}}_n=\mathrm{\π}-\arcsin \frac{L\sin \[\arctan \frac{d+\left(m-1\right)\mathrm{\Δ}L}{\left(x-n\right)L}\]}{S_n}-\arctan \frac{d+\left(m-1\right)\mathrm{\Δ}L}{\left(x-n\right)L}---\left(8\right)$

表达式中各参数的含义与表达式(1)相同。

步骤(5)所述的PID控制算法，采用的是增量式PID控制算法，将航向角α_n作为控制输入量输入到PID控制器，然后决策输出前轮转向角的大小，具体如下：

${\mathrm{\δ}}_k={\mathrm{\δ}}_{k-1}+K_p\[{\mathrm{err}}_k-{\mathrm{err}}_{k-1}+\frac{T}{T_i}{\mathrm{err}}_k+\frac{T_d}{T}({\mathrm{err}}_k-2{\mathrm{err}}_{k-1}+{\mathrm{err}}_{k-2})\]---\left(9\right)$

其中，δ_k、δ_k-1为第k、k-1时刻的前轮转向角期望值，err_k、err_k-1、err_k-2为第k、k-1、k-2时刻的前轮转向角期望值与实际前轮转角之差，T为采样周期，K为比例系数，T_d为微分时间常数，T_i为积分时间常数；参数K、T_d、T_i的取值与实际环境有关。

本发明的优点在于：

(1)将山地机械所走的直线分为若干等长的线段，建立了在等分点处的左右偏向检测法和航向角计算法，对山地机械进行分段式纠正航向偏差，简单可靠；

(2)将山地机械所走的直线分为若干等长的线段，可以根据实际的情况改变线段的数量：线段的数量越多，线段的长度越短，山地机械的检偏、纠偏的实时性和准确性越高。

附图说明

图1本发明实现山地机械等间距直线行驶的方法整体框图；

图1中，1.第一固定节点2.第二固定节点3.激光发射器4.固定节点的电机5.固定节点的无线数据收发模块6.挡板7.固定节点的中央处理器8.第一激光接收器9.第二激光接收器10.移动节点的中央处理器11.惯性传感器12.光电测速模块13.移动节点的无线数据收发模块14.激光测距传感器15.移动节点的电机16.移动节点17.基准直线18.与基准直线等间距的直线。

图2本发明实现山地机械等间距直线行驶的方法说明图。

图3本发明固定节点硬件系统框图。

图4本发明移动节点硬件系统框图。

图5本发明实施例山地机械实际行驶路程与航偏角关系仿真图。

具体实施方式

下面结合附图和实施例对本发明内容作进一步的说明，但本发明的实施不限于此。

实施例

参照图1、3、4，在平行于山地机械所行驶的基准直线17外的一条直线上设置两个硬件结构完全相同的固定节点，分别固定在高于农作物高度的支架上，在山地机械的正左边靠近山地机械起始点的位置设定第一固定节点1，在第一固定节1点正前方设定第二固定节点2。

两个固定节点包括：中央处理器7，激光发射器3，电机4，无线数据收发模块5和电源模块；所述的激光发射器3，电机4，无线数据收发模块5，电源模块分别通过导线连接到中央处理器7相应的I/O口。

在山地机械上安装一个移动节点16，移动节点16包括：中央处理器10，第一激光接收器8，第二激光接收器9，无线数据收发模块13，激光测距传感器14，电机15，惯性传感器11，光电测速模块12和电源模块；所述的两个激光接收器8、9，无线数据收发模块13，激光测距传感器14，电路15，惯性传感器11，光电测速模块12，电源模块分别通过导线连接到中央处理器10相应的I/O口。

第一固定节点1、第二固定节2和移动节点16的电机为自带驱动电路的步进电机。

参照图2，图5，将山地机械所行驶的基准直线17分为等长的x段，设各段长度为L；在第一固定节点1上设置第一激光发射器，在第二固定节点2上设置第二激光发射器，所述的两个激光发射器，其初始发射的两束激光线在俯视图上的交点刚好落在山地机械的起始点上。以山地机械到达第一个等分点为例来说明航向角α_n的计算方法，第一激光发射器发射的激光线与基准直线17的夹角β₁，第二激光发射器发射的激光线与基准直线17的夹角β₁'。若第一激光接收器8先检测到第一激光发射器发射的激光，如果陀螺仪的测量值那么路程S₁＝l₁，山地机械实际走的路线与第一激光发射器发射的光线的夹角：γ₁＝χ₁，航向角α₁＝φ₁，具体计算方法采用公式(5)；如果陀螺仪的测量值那么路程S₁＝l₂，山地机械实际走的路线与第一激光发射器发射的光线的夹角：γ₁＝χ₂，航向角α₁＝φ₂，具体计算方法采用公式(6)。若第二激光接收器9先检测到第二激光发射器发射的激光，如果陀螺仪的测量值那么路程S₁＝l₃，山地机械实际走的路线与第二激光发射器发射的光线的夹角：γ₁＝χ₃，航向角α₁＝φ₃，具体计算方法采用公式(7)；如果那么路程S₁＝l₄，山地机械实际走的路线与第二激光发射器发射的光线的夹角：γ₁＝χ₄，航向角α₁＝φ₄，具体计算方法采用公式(8)。

以山地机械走第一条直线为例子(m＝1)，其实现等间距直线行驶的方法，具体步骤如下：

1、将第二固定节点2的激光发射器安装高度比第一固定节点1的激光发射器高，两个固定节点的挡板6安装在同一水平高度。

2、两个固定节点上的激光发射器，由其模块上的步进电机(28BYJ-48-5V)2控制其转动。

3、两个固定节点与移动节点16通过无线数据收发器模块(SI4432无线模块)进行通信。

4、两个激光接收器，其结构完全相同，在竖直方向上共线，安装高度分别与两个固定节点的激光发射器相同。

5、激光测距传感器14安装高度与固定节点的挡板6在同一水平高度。

6、把山地机械在垂直方向上最大可倾斜的角度设为35°，当侧倾角测量值大于设定值35°时，预防出现交通事故，则整个系统停止运行，系统一旦停止运行，则需要重新启动方能再次运行。

7、各个模块都采用以STM32F103C8T6为核心的微控制器。

8、山地机械停在作业大田的起始点，第一固定节点1位于山地机械正左边3米处，第二固定节点2位于第一固定节点1正前方10米处；此时，第一固定节点1的第一激光发射器发射的激光到第一激光接收器8，第二固定节点2的第二激光发射器发射的激光到第二激光接收器9。

9、实现山地机械等间距直线行驶的方法是在等分点处求山地机械与基准直线17的航向角α_n，以走第一条直线为例子，即m＝1，步骤如下：

(1)山地机械开到起点位置A点，且此刻参考方向初始化为0°时，通过激光测距传感器14测得山地机械与挡板6，O之间的距离d为3米。

(2)将参考方向直线m1(总长10米)分为20段(n的取值为：1，2，...，20)，各段长度为L，且L＝AA₁＝A₁A₂＝A₂A₃…A₁₉A₂₀＝0.5米。

(3)直线的间距△L＝0.1米。

(4)山地机械在两个相邻的等分点之间实际行驶的路程分别设为S1、S2、...S20，路程的大小可由光电测速传感器的数据计算获得。

(5)由公式(1)和(2)，求得第一固定节点的步进电机(24BYJ48)控制激光发射器每次转动角度Δθ_n和角速度最小值Δω_n(min)计算表达式：

${\mathrm{\Δ\θ}}_n=arctan\frac{0.5n}{3}-arctan\frac{0.5(n-1)}{3}$

${\mathrm{\Δ\ω}}_{n\left(\min \right)}=\[arctan\frac{0.5n}{3}-arctan\frac{0.5(n-1)}{3}\]/\left(\frac{0.5}{v}\right)$

其中，n的取值为：1，2，……，20；v为山地机械行驶的速度；

同理，由公式(3)和(4)，求得第二固定节点步进电机(24BYJ48)控制激光发射器每次转动角度Δθ_n'和角速度最小值Δω_n'_(min)计算表达式：

${{\mathrm{\Δ\θ}}_n}^{\′}=arctan\frac{0.5(20-n+1)}{3}-arctan\frac{0.5(20-n)}{3}$

${{\mathrm{\Δ\ω}}_n}^{\′}\left(\min \right)=\[\arctan \frac{0.5(20-n+1)}{3}-arctan\frac{0.5(20-n)}{3}\]/\left(\frac{0.5}{v}\right).$

(6)结合公式(5)和(6)，山地机械在等分点处的航偏角α_n的计算方法如下：

1)当第一激光接收器先检测到第一激光发射器发射的激光，则可判定向左偏离，且α_n的计算表达式：

${\mathrm{\α}}_n=arcsin\frac{0.5sin\left(arctan\frac{3}{0.5n}\right)}{S_n}-arctan\frac{3}{0.5n}$

同理，α_n的计算表达式：

${\mathrm{\α}}_n=\mathrm{\π}-arcsin\frac{0.5sin\left(arctan\frac{3}{0.5n}\right)}{S_n}-arctan\frac{3}{0.5n}.$

2)当第二激光接收器先检测到第二激光发射器发射的激光，则可判定向右偏离，且α_n的计算表达式：

${\mathrm{\α}}_n=arcsin\frac{0.5sin\[arctan\frac{3}{0.5(20-n)}\]}{S_n}-arctan\frac{3}{0.5(20-n)}$

同理，α_n的计算表达式：

${\mathrm{\α}}_n=\mathrm{\π}-arcsin\frac{0.5sin\[arctan\frac{3}{0.5(20-n)}\]}{S_n}-arctan\frac{3}{0.5(20-n)}.$

(7)将航向角α_n作为控制输入量输入到PID控制器，然后决策输出前轮转向角的大小，其中参数K、T_d、T_i的值分别为0.6，0.4，0.02。

Claims (7)

1.一种实现山地机械等间距直线行驶的方法，其特征在于包括下述步骤：

(1)将山地机械所行驶的基准直线分为等长的x段，设各段长度为L；

(2)在平行于山地机械所行驶的基准直线外的一条直线上设置两个硬件结构完全相同的固定节点，分别固定在高于农作物高度的支架上，在山地机械的正左边靠近山地机械起始点的位置设置第一固定节点，在第一固定节点正前方设置第二固定节点，在第一固定节点上设置第一激光发射器，在第二固定节点上设置第二激光发射器，所述的两个激光发射器，其初始发射的两束激光线在俯视图上的交点刚好落在山地机械的起始点上；

(3)第一激光发射器和第二激光发射器由其各自固定节点上的电机控制转动，所述的电机每次转动的角度与山地机械所行驶直线的等分线段长度L采用类正比控制方法匹配，其角速度的大小与山地机械的行驶速度采用类正比控制方法匹配，控制第一激光发射器和第二激光发射器对行驶的山地机械进行追踪，并始终保持发射的两束激光线在俯视图上的交点刚好落在山地机械所走的基准直线上的下一个等分点上；

(4)在山地机械上安装一个移动节点，在移动节点上设置惯性传感器、激光测距传感器、第一激光接收器和第二激光接收器，所述的两个激光接收器在竖直方向上共线，安装高度分别与第一激光发射器和第二激光发射器相同；惯性传感器为陀螺仪和加速度计，且陀螺仪和加速度计分别对山地机械的角速度和加速度进行测量，经过计算后得到山地机械的水平航向角和垂直方向上的侧倾角测量值，各自精确度小于0.5°，垂直方向上的最大可倾斜的角度设置成固定数值，当侧倾角大于固定值时，则整个系统停止运行，预防出现交通事故，系统一旦停止运行，则需要重新启动方能再次运行；

(5)山地机械行驶到等分点处，根据左右偏向检测法判断是否向左、右偏，再由航向角计算法推算出航向角的大小，航向角用α_n表示，再根据航向角的大小通过PID控制算法推算出前轮转向角的大小自动纠正偏差，实现直线行驶；

(6)山地机械行驶到基准直线末端，由人工操作掉头，并根据激光测距传感器测量山地机械与固定节点的距离大小，快速地把山地机械开到两个激光接收器能同时接收到激光的位置，确定山地机械即将行驶直线的起点，实现山地机械即将行驶直线与刚行驶直线的间距保持一定值。

2.根据权利要求1所述的实现山地机械等间距直线行驶的方法，其特征在于：所述的两个固定节点包括：中央处理器，激光发射器，电机，无线数据收发模块和电源模块；所述的激光发射器，电机，无线数据收发模块，电源模块分别通过导线连接到中央处理器相应的I/O口。

3.根据权利要求1所述的实现山地机械等间距直线行驶的方法，其特征在于：所述的移动节点包括：中央处理器，第一激光接收器，第二激光接收器，无线数据收发模块，激光测距传感器，电机，惯性传感器，光电测速模块和电源模块；所述的两个激光接收器，无线数据收发模块，激光测距传感器，电路，惯性传感器，光电测速模块，电源模块分别通过导线连接到中央处理器相应的I/O口。

4.根据权利要求1所述的实现山地机械等间距直线行驶的方法，其特征在于：步骤(3)所述的类正比控制方法是，固定节点上的电机，其每次转动的角度与线段的长度L匹配，其角速度的大小与山地机械的行驶速度匹配，计算两个固定节点上的电机每次转动的角度与角速度的表达式为：

(1)第一固定节点的电机每次转动角度Δθ_n和角速度最小值Δω_n(min)计算表达式：

${\mathrm{\Δ\θ}}_n=\arctan \frac{nL}{d+(m-1)\mathrm{\Δ}L}-\arctan \frac{(n-1)L}{d+(m-1)\mathrm{\Δ}L}---\left(1\right)$

${\mathrm{\Δ\ω}}_{n\left(\min \right)}=\[\arctan \frac{nL}{d+(m-1)\mathrm{\Δ}L}-\arctan \frac{(n-1)L}{d+(m-1)\mathrm{\Δ}L}\]/\left(\frac{L}{v}\right)---\left(2\right)$

其中，n表示的是等分点的序号，取值为：1，2，...，x；m的取值为山地机械所走的基准直线的序号；△L表示基准直线之间的间距；d为第一固定节点与山地机械起始点的距离；v为山地机械行驶的速度；

(2)第二固定节点的电机每次转动角度Δθ_n'和角速度最小值Δω_n'_(min)计算表达式：

${{\mathrm{\Δ\θ}}_n}^{\′}=\arctan \frac{\left(x-n+1\right)L}{d+(m-1)\mathrm{\Δ}L}-\arctan \frac{(x-n)L}{d+(m-1)\mathrm{\Δ}L}---\left(3\right)$

${\mathrm{\Δ\ω}}_{n\left(\min \right)}^{\′}=\[\arctan \frac{\left(x-n+1\right)L}{d+(m-1)\mathrm{\Δ}L}-\arctan \frac{(x-n)L}{d+(m-1)\mathrm{\Δ}L}\]/\left(\frac{L}{v}\right)---\left(4\right)$

表达式中各参数的含义与表达式(2)相同，x表示的是等分点的总数。

5.根据权利要求1所述的实现山地机械等间距直线行驶的方法，其特征在于：步骤(5)所述的左右偏向检测法是在等分点处，当两个激光接收器同时接收到激光，说明山地机械没有出现左、右偏转；当第一激光接收器先接收到第一激光发射器发射的激光，说明山地机械向左偏；当第二激光接收器先接收到第二激光发射器发射的激光说明山地机械向右偏。

6.根据权利要求1所述的实现山地机械等间距直线行驶的方法，其特征在于：步骤(5)所述的航向角α_n的计算法，其步骤如下，设山地机械在两个相邻的等分点之间实际行驶的路程为S_n；由陀螺仪数据计算得山地机械在两个相邻的等分点之间的航向角测量值为

(1)当第一激光接收器先接收到第一激光发射器发射的激光时，则可判定向左偏离，且α_n的计算表达式：

${\mathrm{\α}}_n=\arcsin \frac{Lsin\[arctan\frac{d+(m-1)\mathrm{\Δ}L}{nL}\]}{S_n}-arctan\frac{d+(m-1)\mathrm{\Δ}L}{nL}---\left(5\right)$

表达式中各参数的含义与表达式(1)相同。

同理，α_n的计算表达式：

${\mathrm{\α}}_n=\mathrm{\π}-arc\sin \frac{L\sin \[\arctan \frac{d+\left(m-1\right)\mathrm{\Δ}L}{nL}\]}{S_n}-\arctan \frac{d+\left(m-1\right)\mathrm{\Δ}L}{nL}---\left(6\right)$

表达式中各参数的含义与表达式(1)相同；

(2)当第二激光接收器先接收到第二激光发射器发射的激光时，则可判定向右偏离，且α_n的计算表达式：

${\mathrm{\α}}_n=arc\sin \frac{L\sin \[\arctan \frac{d+\left(m-1\right)\mathrm{\Δ}L}{\left(x-n\right)L}\]}{S_n}-\arctan \frac{d+\left(m-1\right)\mathrm{\Δ}L}{\left(x-n\right)L}---\left(7\right)$

表达式中各参数的含义与表达式(1)相同；

同理，α_n的计算表达式：

${\mathrm{\α}}_n=\mathrm{\π}-arc\sin \frac{L\sin \[\arctan \frac{d+\left(m-1\right)\mathrm{\Δ}L}{\left(x-n\right)L}\]}{S_n}-\arctan \frac{d+\left(m-1\right)\mathrm{\Δ}L}{\left(x-n\right)L}---\left(8\right)$

表达式中各参数的含义与表达式(1)相同。

7.根据权利要求1所述的实现山地机械等间距直线行驶的方法，其特征在于：步骤(5)所述的PID控制算法，采用的是增量式PID控制算法，将航向角α_n作为控制输入量输入到PID控制器，然后决策输出前轮转向角的大小，具体如下：

${\mathrm{\δ}}_k={\mathrm{\δ}}_{k-1}+K_p\[{\mathrm{err}}_k-{\mathrm{err}}_{k-1}+\frac{T}{T_i}{\mathrm{err}}_k+\frac{T_d}{T}({\mathrm{err}}_k-2{\mathrm{err}}_{k-1}+{\mathrm{err}}_{k-2})\]---\left(9\right)$

其中，δ_k、δ_k-1为第k、k-1时刻的前轮转向角期望值，err_k、err_k-1、err_k-2为第k、k-1、k-2时刻的前轮转向角期望值与实际前轮转角之差，T为采样周期，K为比例系数，T_d为微分时间常数，T_i为积分时间常数；参数K、T_d、T_i的取值与实际环境有关。