农用机械的导航方法、导航装置和农用机械.pdf

具体实施方式

下面通过附图和实施例，对本发明的技术方案做进一步的详细描述。

本发明实施例提供了一种农用机械的导航方法，图1为本发明农用机械的导航方法实施例的流程示意图，如图1所示，包括如下步骤：

步骤101、通过一个以上的传感器获取包括农用机械的位置坐标、速度和航向信息的定位信息；本步骤中一个或多个传感器获取农用机械的位姿信息，包括位置、航向、前轮转向角及速度等数据，上述的传感器可以包括全球定位系统(Global Positioning System，以下简称：GPS)、惯性测量单元(Inertia Measurement Unit，以下简称：IMU)以及电子罗盘中的一个或多个，上述传感器通过标准串口与工控机连接，并将获取的数据存储到工控机中，然后进行信号转换和处理即可得到农用机械的位姿数据，例如包括数据由经纬度向大地坐标的转换，位姿数据的坐标统一化等处理；上述的位姿数据是相对独立的，需要进行数据融合以获得最终的定位信息，本步骤中可以根据基于双模糊控制器的自适应卡尔曼滤波算法对上述获取的位姿数据进行数据融合，并获取农用机械的位置坐标、速度和航向信息的定位信息。例如可以取系统的状态向量为：X＝[x，y，θ，α，v]^T，其中x，y为融合后的位置坐标，θ为融合后的航向角，α为农用机械的前轮转向角，v为农用机械的行驶速度；观测向量为：Z＝[x_DR y_DR x_GPS y_GPS θ_DR θ_c α]^T，其中x_DR，y_DR和θ_DR分别为由IMU获取的信息经航位推算后得到的位置坐标和航向角，x_GPS，y_GPS为GPS获取的位置信息经坐标转换后的位置坐标，θ_c为电子罗盘获取的航向角；

步骤102、根据农用机械的两轮运动学模型以及最优控制理论中的最小时间问题进行计算以获取农用机械的预期路径信息；本步骤是根据两轮运动学模型建立农用机械的状态方程，并根据最优控制器中的最小时间问题获取农用机械的预期路径信息；

步骤103、根据农用机械的两轮运动学模型以及跟踪问题终端时间无限调节器设计准则进行路径跟踪最优控制器设计；

步骤104、根据所述农用机械的定位信息和所述预期路径信息获取路径误差；本步骤是在上述步骤101和步骤102的基础上，通过计算获取路径误差；

步骤105、根据所述路径误差信息以及步骤103设计的路径跟踪最优控制器获取调整信息并发送给自动转向系统，本步骤是在获取到路径误差之后，即可计算得出相应路径调整信息并发送给自动转向系统，由自动转向系统控制农用机械进行路径调整。本步骤中的自动转向系统可以是在原人工转向系统的基础上并列设置，并且可以选择使用其中的自动转向系统或者是人工转向系统。

本发明上述实施例提供的农用机械的导航方法，能够根据获取的农用机械的定位信息、预期路径信息获取路径误差，且上述的预期路径信息为根据农用机械的两轮运动学模型以及最优控制理论的最小时间问题计算方法获取；并根据该路径误差及路径跟踪最优控制器获得路径调整信息，进而将其发送给自动转向系统实现对农用机械的调整。

以下是本发明一个具体实施例，在GPS、IMU和电子罗盘获取到位置、航向角等数据信息后，利用自适应卡尔曼滤波器对上述的数据信息进行融合，首先建立状态方程模型，此时假设农用机械在平面上行驶并且不考虑车轮与地面的作用，认为左右部分对称无倾斜、俯仰、侧滑等运动，则农用机械的运动可以看作是一种平面的刚体的平动与旋转运动的二轮模型，如图2所述示，其中θ为航向角，α为前轮转向角，l为轴距，v为车速，沿x、y轴可以分解为v_x和v_y，v_f为前轮速度。则其运动学模型为：

θ.=vltanαx.=vcosαy.=vsinα]]>

并取系统的状态变量为：X＝[x，y，θ，α，v]^T，其中x，y为融合后的位置坐标，θ为融合后的航向角，α为农用机械的前轮转角的角度，v为农用机械的行驶速度。因此可得系统的状态方程为：

x.y.θ.α.v.=v·cosθv·sinθvl·tanθ-1τα·α-1τv·v+000wαwv]]>

其中，τ_α和τ_v为时间常数，w_α和w_v分别为农用机械前轮转角和速度的噪声。将状态方程离散化，得到系统离散的状态方程为：

X_k＝f[X_k-1，k-1]+W_k-1

上式中，k为离散时各向量的索引数，W_k-1＝[0，0，0，w_α，w_v]^T。

根据卡尔曼(Kalman)滤波原理可以得到：

X_k＝Φ_k，k-1X_k-1+W_k-1

上式中：

Φk,k-1=10-T·vk·sinθk0T·cosθk01T·vk·cosθk0T·sinθk001T·vkl·sec2αkTl·tanθk0001-Tτα000001-Tτv]]>

其中，T为滤波器的递推时间间隔。

在上述步骤基础上，可以建立量测方程，取量测向量为：

Z＝[x_DR，y_DR，x_GPS，y_GPS，θ_DR，θ_c，α]^T

可建立量测方程：

Z_k＝HX_k+V_k

上式中H为观测矩阵，可表示为：

H=10000010001000001000001000010000010,]]>

其中，v为传感器的观测噪声，近似为(0，σ²)的高斯白噪声。

再次利用Kalman滤波算法进行处理，该算法可包括如下的流程：

状态一步预测方程：

X^k,k-1=Φk,k-1X^k-1]]>

一步预测误差方差阵：

Pk/k-1=Φk,k-1Pk-1Φk,k-1T+Qk-1]]>

滤波增益矩阵：

Kk=Pk,k-1HkT[HkPk,k-1HkT+Rk]-1]]>

状态估计方程：

X^k=X^k,k-1+KkZ^k]]>

估计误差方差阵：

P_k＝[I-K_kH_k]P_k，k-1

在上述对数据信息进行融合后，即可采用模糊逻辑控制器对卡尔曼滤波器的增益矩阵K、测量误差协方差R以及观测误差协方差Q进行实时修正，将卡尔曼滤波其调整到最优状态。通过两个模糊控制器分别确定滤波增益、滤波噪声的调整系统μ_r和系统噪声的调整系数μ_q，具体包括如下的步骤：

首先，建立滤波器增益K_k和测量噪声R(t)系数模糊控制器；

滤波器增益K_k和测量噪声R(t)都受到传感器信号质量的影响，它们同GPS、航位推算两种定位方式获得的位置差值Δy和GPS的位置精度因子间存在一定的模糊关系。该模糊控制器的输入为GPS、航位推算两种定位方式获得的位置差Δy和GPS的位置精度因子，输出为滤波增益、测量噪声的调整系数μ_r。

μr=μDRμDRμGPSμGPSμDR11]]>

其次，建立基于新息向量Z_k变化的Q(t)系数模糊控制器；

Q(t)是系统的过程噪声的方程强度阵，受到系统过程噪声的影响，该值不易直接测得，常通过对各预测信号的系统噪声估计得到。然而这种估计是建立在新息是零均值白噪声的基础上的。由于新息是观测值与预测值的差，故当滤波器处于最优状态时，新息的值是一组零均值的白噪声。而在田间作业过程中，由于受到各种干扰的影响，新息的值会夹杂不同程度的有色噪声，新息的值越大，说明此时系统的观测值和预测值差异越大，即系统的过程噪声越大，导致Q(t)的值也越大，因此Q(t)的值与新息向量Z_k的大小存在一定的模糊关系。建立基于新息变化的模糊控制器，对Q(t)的值进行实时在线调整，另卡尔曼滤波器始终处于最优状态。该模糊控制器的输入为横向偏差与航向角的新息值Z(x)，Z(y)和Z(θ)，输出为Q(x)，Q(y)和Q(θ)。则过程噪声Q(t)的调整系数矩阵为μ_q为：

μq=Q(x)Q(y)Q(θ)11]]>

上述实施例中采用具有双模糊控制器的卡尔曼滤波算法对个传感器获取的数据进行了融合，获取了包括农用机械的位置坐标、速度和航向信息的定位信息。

对于农用机械的导航系统来说，除了要获取上述的农用机械的位置信息，还需要获取农用机械理想的路径，即预期路径信息，本实施例中首先由农用机械的两轮运动学模型示意图获取农用机械的状态方程，即：

x.=vcosθy.=vsinθθ.=vtanαlα.=u]]>

其中状态量为：X＝[x，y，θ，α]^T；u为要求解的控制量。

在本实施例中采用最优控制理论中的最小时间问题进行预期路径规划，其性能指标如下：

J=[x(tf)-x0]2+[y(tf)-y0]2+[θ(tf)-θ0]2+[α(tf)-α0]2+∫0tfrα2max-α2]]>

上式中：X₀＝[x₀，y₀，θ₀，α₀]^T为农用机械要到达的目标点；α_max为转向系统的最大转向角；r为惩罚因子；t_f为终端时间。

根据目标点信息，解上述最小时间问题即可获取农用机械的预期路径信息，包括位置信息，航向信息和转向信息。

将拖拉机当前点Q的状态量和目标点P的状态量的差表示为：

δx=xQ-x‾P,δy=yQ-y‾P,δθ=θQ-θ‾P,δα=αQ-α‾P]]>

对上述状态方程进行变分得到如下的状态方程：

δx.=A(t)δx+Bδu]]>

状态量为δx＝[δx，δy，δθ，δα]^T，其中B＝[0，0，0，1]^T

上式中的是规划的路径中包含的信息，因此最优控制器的参数随路径上的参考点变化而变化。

在本实施例中采用最优控制理论中的终端无限时间调节器设计准则设计导航跟踪最优控制器，上述设计准则如下：

J(δu)=∫0∞(δxTQδx+δuTRδu)dt]]>

其中，矩阵Q为对角阵，R为常数，反映前轮转向角和拖拉机速度的权重。

可得出Riccati代数方程为：

PA+A^TP-PBR^-1B^TP+Q＝0

解上述矩阵方程即可求得路径跟踪最优控制器：

δu＝K_uδx，      K_u＝-R^-1B^TP

图3为本发明农用机械的导航装置实施例的结构示意图，如图3所示，农用机械的导航装置包括：定位信息获取模块11、预期路径计算模块12、最优控制器生成模块13、路径误差获取模块14和发送模块15，其中定位信息获取模块11用于通过一个以上的传感器获取包括农用机械的位置坐标、速度和航向信息的定位信息；预期路径计算模块12用于根据农用机械的两轮运动学模型以及最优控制理论中的最小时间问题进行计算以获取农用机械的预期路径信息；最优控制器生成模块13用于根据农用机械的两轮运动学模型以及跟踪问题终端时间无限调节器设计准则生成路径跟踪最优控制器；路径误差获取模块14用于根据所述农用机械的定位信息和所述预期路径信息获取路径误差；发送模块15用于根据所述路径误差信息及路径跟踪最优控制器获取调整信息并发送给自动转向系统。

本实施例提供的农用机械的导航装置，通过农用机械的定位信息和预期路径信息获取路径误差，并根据路径误差信息及路径跟踪最优控制器获取路径调整信息并发送给自动转向系统。其中预期路径信息为预期路径计算模块根据农用机械的两轮运动学模型以及最优控制理论中的最小时间问题进行计算获得；路径跟踪最优控制器根据最优控制理论中的终端无限时间调节器设计准则进行设计获得。

上述实施例中的定位信息获取模块包括一个以上的传感器及计算单元，所述传感器用于获取农用机械的位姿数据；所述计算单元用于根据基于双模糊控制器的自适应卡尔曼滤波算法对上述获取的位姿数据进行数据融合以获取包括农用机械的位置坐标、速度和航向信息的定位信息。

图4为本发明农用机械的导航装置具体实施例的结构示意图，如图4所示，其中GPS、IMU以及电子罗盘获取农用机械的位姿数据，经过计算单元进行上述数据信息的融合处理获取农用机械的定位信息，并且预期路径获取模块进行计算获取预期的路径信息，通过对上述的信息进行分析，经过路径跟块进行计算获取预期的路径信息，通过对上述的信息进行分析，经过路径跟踪最优控制器可以获取对农用机械的调整信息，可将该调整信息发送给自动转向系统实现调整，角位移传感器能够将机械装置的角度变化为电压信号。

图5为本发明农用机械实施例的结构示意图，如图5所示，包括农用机械本体21、人工转向系统22、导航装置23和自动转向系统24，其中导航装置23用于通过一个以上的传感器获取包括农用机械的位置坐标、速度和航向信息的定位信息；根据农用机械的两轮运动学模型以及最优控制理论中的最小时间问题进行计算以获取农用机械的预期路径信息；根据农用机械的两轮运动学模型以及跟踪问题终端时间无限调节器设计准则生成路径跟踪最优控制器；根据所述农用机械的定位信息和所述预期路径信息获取路径误差；根据所述路径误差信息获取调整信息并发送给自动转向系统；自动转向系统24与导航装置23连接，用于根据导航装置的调整信息对农用机械进行控制。

本实施例提供的农用机械，其中的导航装置根据获取的农用机械的定位信息以及计算获取的农用机械的预期路径信息获取路径误差，并根据路径误差获取调整信息，由自动转向系统对农用机械进行控制，能够实现对农用机械的精确导航。

上述的自动转向系统包括油泵、电磁换向阀、驱动模块和步进电机，其中用于为自动转向系统提供转向动力；电磁换向阀用于进行人工转向系统和自动转向系统的切换，选择自动控制系统作为农用机械的控制系统；驱动模块用于根据接收到的调整信息生成控制信号；步进电机与所述驱动模块连接，用于根据控制信号调整液压转向器转向和转向速度。

图6为本发明实施例中转向系统具体实施例示意图，其中包括并列设置的自动转向系统1与人工转向系统2，从而可以选择使用其中的一个转向系统进行转向。具体的在实施过程中，可以由电磁换向阀3根据控制器的指令进行人工装箱模式或自动换向模式的切换，具体的可以是一个二位三通电磁换向阀实现，该二位三通电磁换向阀与油泵4配合使用，油泵4是转向系统的动力源，为转向控制系统供给具有压力的液压油，油泵的技术参数可以设置为：额定压力10Mpa，排量14ml/r，最大工作压力12.5Mpa，安全阀调整范围5～12.5Mpa，额定转速2000r/min，转速范围800～3000r/min，输出流量为12、14或16L/min；油泵的主要技术参数可以设为：工作电压12V，功率30W，最高压力35Mpa，流量50L/min；在具体实施过程中，可以在电磁换向阀断电时，阀芯在弹簧力的作用下处于右位，油泵的液压油流向原人工转向器，此时人工转向系统起作用；当加装的转向器处于中位时，它与油源的连接电磁换向阀断开，与转向油缸的连接被自身特定结构断开，处于隔离状态，不起转向作用。当上述的电磁换向阀通电时，电磁换向阀的阀芯处于左位，液压油流向加装的自动转向系统，此时由步进电机驱动自动转向系统实现自动转向功能，同理，原转向器处于隔离状态，不起转向作用。调整步进电机的转速实现转向轮转向速度控制，控制步进电机的转向实现转向轮的转向控制。

自动转向系统的液压转向器5由步进电机6驱动，根据步进电机的转向、转角大小，定向、定量的将液压油输入转向油缸7左腔或右腔，推动导向轮驶向动力转向，另一腔的油则经过转向器回油箱8。自动转向系统可选用与人工转向系统相同的液压转向器9，以便于系统匹配，即能够满足原液压转向系统压力、流量和温度等性能的要求，例如可选用具有如下技术参数的液压转向器：转动扭矩4.0～5.0N·m，流量7.51/min，排量100ml/r，额定工作压力16Mpa，最大瞬时背压6.3Mpa，最大连续背压2.5Mpa，系统工作温度-20～80℃。

全液压转向器地转向扭矩的大小直接决定了所需步进电机地功率，如果液压转向器的扭矩过大，受车载电源电压的限制能够满足功率要求的步进电机很难选择，全液压转向器的额定转速n可以如下的公式计算获得：其中q为液压转向器的流量，V为液压转向器的排量，根据上述参数的液压转向器，则液压转向器的额定转速在液压转向器以额定转速转动时步进电机的功率P由如下公式计算：

P=2πnM60ηsηm×150%=2π×75×560×0.75×0.8×150%=98.125W]]>

其中液压转向器的额定转速n为75rpm，转动扭矩M为5N·m，步进电机的效率η_s为75％，机械传动效率η_m为80％，因步进电机过载能力差，选择功率余量为50％。步进电机的工作电压选择12伏的直流电压，功率为99W时，步进电机的工作电流I为：

I=PU=9912≈8.25A]]>

在上述实施例中，步进电机的电流需要达到8.25A。本发明实施例中还可以进一步减少转向器中复位弹簧片的数量，将其复位弹簧片减至两片，将液压转向器的转动扭矩降到0.8N·m以下，能够降低了转向所需要的扭矩。改装后步进电机的功率为：

P=2πnM60ηsηm×150%=2π×75×0.860×0.75×0.8×150%=98.125W]]>

计算可得其电流I为1.4A，从而能够更好的选择合适的步进电机，具体的可选择具有如下技术参数的步进电机：步距角1.8°，相数4，电压4.4V，电流2A，最大静转矩2.1N·m，转动惯量2Kg·cm²，阻抗2.2±10ohm，重量2.2Kg。

角位移传感器能够将机械装置的角度变化为电压信号，在本系统中利用角位移转向器测量转向轮的转向角度，并且角位置传感器的精度直接决定着控制系统的转向精度，是农用机械自动驾驶控制系统控制精度的基础，需要选择具有较高精度的角位移传感器，具体的可以参考选择具有如下技术参数的角位移传感器：电压12VDC，机械转角360连续可转，线性量程0～120，输出信号0～5V，分辨率为连续，精度0.1，相对湿度95±3％RH，使用温度-30～80℃，抗电等级500V，防护等级IP64。

本发明上述实施例提供的农用机械的导航方法、导航装置和农业机械，能够通过获取农用机械的定位信息和预期路径，并根据上述定位信息和预期路径信息计算得出调整信息，对农用机械的运行进行导航，并能够提高导航的精确度。

最后应说明的是：以上实施例仅用以说明本发明的技术方案而非对其进行限制，尽管参照较佳实施例对本发明进行了详细的说明，本领域的普通技术人员应当理解：其依然可以对本发明的技术方案进行修改或者等同替换，而这些修改或者等同替换亦不能使修改后的技术方案脱离本发明技术方案的精神和范围。