专利名称：农用机械的导航方法、导航装置和农用机械的制作方法
技术领域：
本发明涉及机械导航技术领域，特别涉及一种农用机械的导航方法、导航装置和 农用机械。
背景技术：
农用机械的导航可定义为根据农用机械的实际位置与 期望路径的误差确定车辆 转向角的大小，使其按照预定的路径行使。农用机械的导航系统可以包括定位、路径规划、 导航控制、机械模型和通信等模块。其中的定位主要是根据相应的传感器检测农用机械的位置、姿态、速度、加速度等 信息，具体可以使用机械视觉、激光定位、全球定位系统(GlobalPositioning System,以下 简称GPS)、惯性导航定位或电子罗盘定位的方法。同时也可以采用多种定位方法的融合， 充分利用多个传感器资源实现检测信息的合理支配和应用。路径规划是指确定农用机械的 路径信息，包括路线的弯曲半径、长度等几何信息以及农用机械的航向角、前轮转向角、地 面的倾斜度等信息。导航控制模块用于控制农用机械按照规划的路线行驶。机械模型是为 研究对象建立的数学模型，能够进行系统仿真。通信模块用于完成上述各个模块之间的数 据交换。发明人在实现本发明的过程中发现，现有的农用机械对导航系统存在跟踪精度低 的缺点，容易引起较大的跟踪误差，无法实现精确导航。
发明内容
本发明的目的是提供一种农用机械的导航方法、导航装置和农用机械，以实现对 农用机械的精确导航。为实现上述目的，本发明提供了一种农用机械的导航方法，包括通过一个以上的传感器获取包括农用机械的位置坐标、速度和航向信息的定位信 息；根据农用机械的两轮运动学模型以及最优控制理论中的最小时间问题进行计算 以获取农用机械的预期路径信息；根据农用机械的两轮运动学模型以及跟踪问题终端时间无限调节器设计准则生 成路径跟踪最优控制器；根据所述农用机械的定位信息和所述预期路径信息获取路径误差；根据所述路径误差信息及路径跟踪最优控制器获取路径调整信息并发送给自动 转向系统。本发明还提供了一种农用机械的导航装置，包括定位信息获取模块，用于通过一个以上的传感器获取包括农用机械的位置坐标、 速度和航向信息的定位信息；预期路径计算模块，用于根据农用机械的两轮运动学模型以及最优控制理论中的最小时间问题进行计算以获取农用机械的预期路径信息；最优控制器生成模块，用于根据农用机械的两轮运动学模型以及跟踪问题终端时 间无限调节器设计准则生成路径跟踪最优控制器；路径误差获取模块，用于根据所述农用机械的定位信息和所述预期路径信息获取 路径误差；发送模块，用于根据所述路径误差信息及路径跟踪最优控制器获取调整信息并发 送给自动转向系统。本发明还提供了一种农用机械，包括农用机械本体和人工转向系统，还包括导航装置，用于通过一个以上的传感器获取包括农用机械的位置坐标、速度和航 向信 息的定位信息；根据农用机械的两轮运动学模型以及最优控制理论中的最小时间问题 进行计算以获取农用机械的预期路径信息；根据农用机械的两轮运动学模型以及跟踪问题 终端时间无限调节器设计准则生成路径跟踪最优控制器；根据所述农用机械的定位信息和 所述预期路径信息获取路径误差；根据所述路径误差信息及路径跟踪最优控制器获取路径 调整信息并发送给自动转向系统；自动转向系统，与导航装置连接，用于根据导航装置的调整信息对农用机械进行 控制。本发明上述实施例提供的农用机械的导航方法、导航装置和农用机械，通过一个 以上的传感器获取农用机械的定位信息，并根据农用机械的两轮运动学模型以及最优控制 理论中的最小时间问题计算获取了农用机械的预期路径信息；根据上述信息可获取农用 机械的路径误差，最后根据上述的路径误差通过路径跟踪最优控制器计算获取路径调整信 息，并将其发送给农用机械上设置的自动转向系统，可实现对农用机械的精确导航。
图1为本发明农用机械的导航方法实施例的流程示意图；图2为本发明农用机械的运动示意图；图3为本发明农用机械的导航装置实施例的结构示意图；图4为本发明农用机械的导航装置具体实施例的结构示意图；图5为本发明农用机械实施例的结构示意图；图6为本发明实施例中转向系统具体实施例示意图。
具体实施例方式下面通过附图和实施例，对本发明的技术方案做进一步的详细描述。本发明实施例提供了一种农用机械的导航方法，图1为本发明农用机械的导航方 法实施例的流程示意图，如图1所示，包括如下步骤步骤101、通过一个以上的传感器获取包括农用机械的位置坐标、速度和航向信 息的定位信息；本步骤中一个或多个传感器获取农用机械的位姿信息，包括位置、航向、 前轮转向角及速度等数据，上述的传感器可以包括全球定位系统(Global Positioning System,以下简称GPS)、惯性测量单元(Inertia Measurement Unit,以下简称IMU)以及 电子罗盘中的一个或多个，上述传感器通过标准串口与工控机连接，并将获取的数据存储到工控机中，然后进行信号转换和处理即可得到农用机械的位姿数据，例如包括数据由经 纬度向大地坐标的转换，位姿数据的坐标统一化等处理；上述的位姿数据是相对独立的，需 要进行数据融合以获得最终的定位信息，本步骤中可以根据基于双模糊控制器的自适应卡 尔曼滤波算法对上述获取的位姿数据进行数据融合，并获取农用机械的位置坐标、速度和 航向信息的定位信息。例如可以取系统的状态向量为X = [X，y，θ，α，ν]τ，其中X，y为 融合后的位置坐标，θ为融合后的航向角，α为农用机械的前轮转向角，ν为农用机械的行 驶速度；观测向量为=Z = [xDE yDE xGPS yGPS ΘΒΕ θ c α ]τ，其中xDK，yDK和θ ^分别为由IMU 获取的信息经航位推算后得到的位置坐标和航向角，Xgps, Ygps为GPS获取的位置信息经坐标 转换后的位置坐标，θ。为电子罗盘获取的航向角；
步骤102、根据农用机械的两轮运动学模型以及最优控制理论中的最小时间问题 进行计算以获取农用机械的预期路径信息；本步骤是根据两轮运动学模型建立农用机械的 状态方程，并根据最优控制器中的最小时间问题获取农用机械的预期路径信息；步骤103、根据农用机械的两轮运动学模型以及跟踪问题终端时间无限调节器设 计准则进行路径跟踪最优控制器设计；步骤104、根据所述农用机械的定位信息和所述预期路径信息获取路径误差；本 步骤是在上述步骤101和步骤102的基础上，通过计算获取路径误差；步骤105、根据所述路径误差信息以及步骤103设计的路径跟踪最优控制器获取 调整信息并发送给自动转向系统，本步骤是在获取到路径误差之后，即可计算得出相应路 径调整信息并发送给自动转向系统，由自动转向系统控制农用机械进行路径调整。本步骤 中的自动转向系统可以是在原人工转向系统的基础上并列设置，并且可以选择使用其中的 自动转向系统或者是人工转向系统。本发明上述实施例提供的农用机械的导航方法，能够根据获取的农用机械的定位 信息、预期路径信息获取路径误差，且上述的预期路径信息为根据农用机械的两轮运动学 模型以及最优控制理论的最小时间问题计算方法获取；并根据该路径误差及路径跟踪最优 控制器获得路径调整信息，进而将其发送给自动转向系统实现对农用机械的调整。以下是本发明一个具体实施例，在GPS、IMU和电子罗盘获取到位置、航向角等数 据信息后，利用自适应卡尔曼滤波器对上述的数据信息进行融合，首先建立状态方程模型， 此时假设农用机械在平面上行驶并且不考虑车轮与地面的作用，认为左右部分对称无倾 斜、俯仰、侧滑等运动，则农用机械的运动可以看作是一种平面的刚体的平动与旋转运动的 二轮模型，如图2所述示，其中θ为航向角，α为前轮转向角，1为轴距，ν为车速，沿x、y 轴可以分解为Vx和vy，vf为前轮速度。则其运动学模型为
V
^ = — tan a I= v cos a ；> =ν sin a并取系统的状态变量为X = [x,y, θ , α，ν]τ，其中x，y为融合后的位置坐标，θ 为融合后的航向角，α为农用机械的前轮转角的角度，ν为农用机械的行驶速度。因此可得 系统的状态方程为 其中，τ α和τ ν为时间常数，Wa和Wv分别为农用机械前轮转角和速度的噪声。将 状态方程离散化，得到系统离散的状态方程为Xk = f[Xk_i; k-U+ff^上式中，k为离散时各向量的索引数，Wlri = W，0，0，wa，wv]T。根据卡尔曼(Kalman)滤波原理可以得到 _4] Xk=上式中 其中，T为滤波器的递推时间间隔。在上述步骤基础上，可以建立量测方程，取量测向量为 可建立量测方程Zk = HXk+Vk上式中H为观测矩阵，可表示为 其中，ν为传感器的观测噪声，近似为(0，σ 2)的高斯白噪声。再次利用Kalman滤波算法进行处理，该算法可包括如下的流程状态一步预测方程Xkk_,
一步预测误差方差阵 滤波增益矩阵 状态估计方程xk =xk^+kkzk 估计误差方差阵 在上述对数据信息进行融合后，即可采用模糊逻辑控制器对卡尔曼滤波器的增益 矩阵K、测量误差协方差R以及观测误差协方差Q进行实时修正，将卡尔曼滤波其调整到最 优状态。通过两个模糊控制器分别确定滤波增益、滤波噪声的调整系统μ ^和系统噪声的 调整系数μ ,，具体包括如下的步骤首先，建立滤波器增益Kk和测量噪声R(t)系数模糊控制器；滤波器增益Kk和测量噪声R(t)都受到传感器信号质量的影响，它们同GPS、航位 推算两种定位方式获得的位置差值Ay和GPS的位置精度因子间存在一定的模糊关系。该 模糊控制器的输入为GPS、航位推算两种定位方式获得的位置差Ay和GPS的位置精度因 子，输出为滤波增益、测量噪声的调整系数μ — 其次，建立基于新息向量Zk变化的Q(t)系数模糊控制器；Q(t)是系统的过程噪声的方程强度阵，受到系统过程噪声的影响，该值不易直接 测得，常通过对各预测信号的系统噪声估计得到。然而这种估计是建立在新息是零均值白 噪声的基础上的。由于新息是观测值与预测值的差，故当滤波器处于最优状态时，新息的值 是一组零均值的白噪声。而在田间作业过程中，由于受到各种干扰的影响，新息的值会夹杂 不同程度的有色噪声，新息的值越大，说明此时系统的观测值和预测值差异越大，即系统的 过程噪声越大，导致Q(t)的值也越大，因此Q(t)的值与新息向量Zk的大小存在一定的模 糊关系。建立基于新息变化的模糊控制器，对Q(t)的值进行实时在线调整，另卡尔曼滤波 器始终处于最优状态。该模糊控制器的输入为横向偏差与航向角的新息值Z(X)，Z(y)和 2(9)，输出为000，0(7)和Q(9)。则过程噪声Q(t)的调整系数矩阵为Pq为
上述实施例中采用具有双模糊控制器的卡尔曼滤波算法对个传感器获取的数据 进行了融合，获取了包括农用机械的位置坐标、速度和航向信息的定位信息。对于农用机械的导航系统来说，除了要获取上述的农用机械的位置信息，还需要 获取农用机械理想的路径，即预期路径信息，本实施例中首先由农用机械的两轮运动学模 型示意图获取农用机械的状态方程，即 其中状态量为X = [x，y，θ , a]T;u为要求解的控制量。在本实施例中采用最优控制理论中的最小时间问题进行预期路径规划，其性能指 标如下 上式中XQ= [x0,y0, θ。，α Jt为农用机械要到达的目标点；α _为转向系统的最 大转向角；r为惩罚因子；tf为终端时间。根据目标点信息，解上述最小时间问题即可获取农用机械的预期路径信息，包括 位置信息，航向信息和转向信息。将拖拉机当前点Q的状态量和目标点P的状态量的差表示为 对上述状态方程进行变分得到如下的状态方程 状态量为δχ=[δχ，δγ，δθ，δα]τ， Su = Sd ,其中 上式中的氏是规划的路径中包含的信息，因此最优控制器的参数随路径上的参 考点变化而变化。在本实施例中采用最优控制理论中的终端无限时间调节器设计准则设计导航跟踪最优控制器，上述设计准则如下 其中，矩阵Q为对角阵，R为常数，反映前轮转向角和拖拉机速度的权重。可得出Riccati代数方程为PA+ATP-PBR_1BTP+Q = O解上述矩阵方程即可求得路径跟踪最优控制器 图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是转向系统的动力源，为转向控制系统供给具有压力的液压油，油泵的技术参 数可以设置为额定压力lOMpa，排量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. O 5. ON · m,流量7. 51/min,排量IOOml/ r，额定工作压力16Mpa，最大瞬时背压6. 3Mpa，最大连续背压2. 5Mpa，系统工作温度-20 80 "C。全液压转向器地转向扭矩的大小直接决定了所需步进电机地功率，如果液压转向 器的扭矩过大，受车载电源电压的限制能够满足功率要求的步进电机很难选择，全液压转
向器的额定转速η可以如下的公式计算获得"=|·，其中q为液压转向器的流量，V为液压 转向器的排量，根据上述参数的液压转向器，则液压转向器的额定转速《 = 2 = g = 75rpm。在
ν 0.1
液压转向器以额定转速转动时步进电机的功率ρ由如下公式计算P =Χ 150% = 2^rx75x5 X150% = 98.125W L 」 6077s"m 60x0.75x0.8其中液压转向器的额定转速η为75rpm，转动扭矩M为5Ν·πι，步进电机的效率ns为75%，机械传动效率1^为80%，因步进电机过载能力差，选择功率余量为50%。步进电 机的工作电压选择12伏的直流电压，功率为99W时，步进电机的工作电流I为 P 99 在上述实施例中，步进电机的电流需要达到8. 25A。本发明实施例中还可以进一步 减少转向器中复位弹簧片的数量，将其复位弹簧片减至两片，将液压转向器的转动扭矩降 到0. 8N · m以下，能够降低了转向所需要的扭矩。改装后步进电机的功率为 计算可得其电流I为1.4A，从而能够更好的选择合适的步进电机，具体的可选 择具有如下技术参数的步进电机步距角1. 8°，相数4，电压4. 4V，电流2A，最大静转矩 2. IN · m，转动惯量 2Kg · cm2，阻抗 2. 2士 lOohm，重量 2. 2Kg。角位移传感器能够将机械装置的角度变化为电压信号，在本系统中利用角位移转 向器测量转向轮的转向角度，并且角位置传感器的精度直接决定着控制系统的转向精度， 是农用机械自动驾驶控制系统控制精度的基础，需要选择具有较高精度的角位移传感器， 具体的可以参考选择具有如下技术参数的角位移传感器电压12VDC，机械转角360连续可 转，线性量程0 120，输出信号0 5V，分辨率为连续，精度0. 1，相对湿度95士3% RH，使 用温度-30 80°C，抗电等级500V，防护等级IP64。本发明上述实施例提供的农用机械的导航方法、导航装置和农业机械，能够通过 获取农用机械的定位信息和预期路径，并根据上述定位信息和预期路径信息计算得出调整 信息，对农用机械的运行进行导航，并能够提高导航的精确度。最后应说明的是以上实施例仅用以说明本发明的技术方案而非对其进行限制， 尽管参照较佳实施例对本发明进行了详细的说明，本领域的普通技术人员应当理解其依 然可以对本发明的技术方案进行修改或者等同替换，而这些修改或者等同替换亦不能使修 改后的技术方案脱离本发明技术方案的精神和范围。
权利要求
一种农用机械的导航方法，其特征在于，包括通过一个以上的传感器获取包括农用机械的位置坐标、速度和航向信息的定位信息；根据农用机械的两轮运动学模型以及最优控制理论中的最小时间问题进行计算以获取农用机械的预期路径信息；根据农用机械的两轮运动学模型以及跟踪问题终端时间无限调节器设计准则生成路径跟踪最优控制器；根据所述农用机械的定位信息和所述预期路径信息获取路径误差；根据所述路径误差信息及路径跟踪最优控制器获取路径调整信息并发送给自动转向系统。
2.根据权利要求1所述的农用机械的导航方法，其特征在于，所述通过一个以上的传 感器获取包括农用机械的位置坐标、速度和航向信息的定位信息包括从一个以上的传感器中获取农用机械的位姿数据；根据基于双模糊控制器的自适应卡尔曼滤波算法对上述获取的位姿数据进行数据融 合以获取包括农用机械的位置坐标、速度和航向信息的定位信息。
3.根据权利要求1所述的农用机械的导航方法，其特征在于，所述最优控制器随机械 在路径中位置的变化而变化。
4.根据权利要求1所述的农用机械的导航方法，其特征在于，还包括自动转向系统的驱动模块根据调整信息生成调整信号，并将所述调整信号发送给液压 转向器，由所述液压转向器驱动转向轮进行转向。
5.一种农用机械的导航装置，其特征在于，包括定位信息获取模块，用于通过一个以上的传感器获取包括农用机械的位置坐标、速度 和航向信息的定位信息；预期路径计算模块，用于根据农用机械的两轮运动学模型以及最优控制理论中的最小 时间问题进行计算以获取农用机械的预期路径信息；最优控制器生成模块，用于根据农用机械的两轮运动学模型以及跟踪问题终端时间无 限调节器设计准则生成路径跟踪最优控制器；路径误差获取模块，用于根据所述农用机械的定位信息和所述预期路径信息获取路径 误差；发送模块，用于根据所述路径误差信息及所述路径跟踪最优控制器获取调整信息并发 送给自动转向系统。
6.根据权利要求5所述的农用机械的导航装置，其特征在于，所述定位信息获取模块 包括一个以上的传感器及计算单元，所述传感器用于获取农用机械的位姿数据；所述计算 单元用于根据基于双模糊控制器的自适应卡尔曼滤波算法对上述获取的位姿数据进行数 据融合以获取包括农用机械的位置坐标、速度和航向信息的定位信息。
7.一种农用机械，包括农用机械本体和人工转向系统，其特征在于，还包括导航装置，用于通过一个以上的传感器获取包括农用机械的位置坐标、速度和航向信 息的定位信息；根据农用机械的两轮运动学模型以及最优控制理论中的最小时间问题进行 计算以获取农用机械的预期路径信息；根据农用机械的两轮运动学模型以及跟踪问题终端 时间无限调节器设计准则生成路径跟踪最优控制器；根据所述农用机械的定位信息和所述预期路径信息获取路径误差；根据所述路径误差信息及路径跟踪最优控制器获取路径调整 信息并发送给自动转向系统；自动转向系统，与导航装置连接，用于根据导航装置的调整信息对农用机械进行控制。
8.根据权利要求7所述的农用机械，其特征在于，所述自动转向系统包括 油泵，用于为自动转向系统提供转向动力；电磁换向阀，用于进行人工转向系统和自动转向系统的切换，选择自动控制系统作为 农用机械的控制系统；驱动模块，用于根据接收到的导航装置发送的调整信息生成控制信号； 步进电机，与所述驱动模块连接，用于根据控制信号调整液压转向器转向和转向速度， 控制转向轮进行转向。
9.根据权利要求8所述的农用机械，其特征在于，还包括角位移传感器，用于测量转向轮的转向角度，并将测量结果发送给导航装置。
全文摘要
本发明提供了一种农用机械的导航方法、导航装置和农用机械，其中农用机械的导航方法包括通过一个以上的传感器获取包括农用机械的位置坐标、速度和航向信息的定位信息；根据农用机械的两轮运动学模型以及最优控制理论中的最小时间问题进行计算以获取农用机械的预期路径信息；根据农用机械的两轮运动学模型以及跟踪问题终端时间无限调节器设计准则设计了路径跟踪最优控制器；根据所述农用机械的定位信息和所述预期路径信息获取路径误差；根据所述路径误差信息以及路径跟踪最优控制器获取路径调整信息并发送给自动转向系统。本发明实施例还提供了农用机械的导航装置以及具有上述导航装置的农用机械。上述方法、装置和设备能够提高导航的精确度。
文档编号G05D1/00GK101866181SQ20091008246
公开日2010年10月20日 申请日期2009年4月16日 优先权日2009年4月16日
发明者刘刚, 宋晓波, 宋正河, 张漫, 朱忠祥, 毛恩荣, 王尚俊, 谢斌 申请人:中国农业大学