[发明专利]一种农业机械的导航控制方法

权利要求书

1、一种农业机械的导航控制方法，其特征在于包括下述步骤：

(1)通过导航传感器测量得到农业机械当前时刻的位姿和运动状态数据；

(2)采用基于预瞄控制的动态目标点搜索算法，将所述位姿和运动状态数据与预定义路线数据进行对比分析，推算出两个导航状态参数即横向跟踪误差和纵向航向偏差；

(3)再通过Fuzzy-PID控制算法推算出操纵控制量即前轮转向偏角；

(4)最后根据所述操纵控制量对农业机械进行实时控制，实现路径跟踪。

2、根据权利要求1所述的农业机械的导航控制方法，其特征在于：所述农业机械当前时刻的位姿和运动状态数据包括当前定位点P_c、当前航向角度θ和当前速度V。

3、根据权利要求1所述的农业机械的导航控制方法，其特征在于：所述基于预瞄控制的动态目标点搜索算法，是通过动态计算前视距离，确定预定义路线上的预瞄点：

(1)前视距离L_f通过下述公式计算得到：

L_f＝L₀+K_lfvV+K_lfrR_df

其中，L₀为前视距离基值；K_lfv，K_lfr分别为前进速度、前视偏差变化率的比例系数，可在仿真或实验过程中通过整定获得，K_lfv与K_lfr均取正值；R_df为前视偏差变化率，定义为：

R df = | P k + 2 m P ′ k + 2 m → | - | P k P ′ k | → | P k + m P ′ k + m | → - | P k P ′ k → | - - - ( 2 ) ]]>

其中 m = int ( VT s L s ) , ]]>P_k表示预定义路线上的第k坐标点，P′_k映射点为在直线P_kP_k-1上以P_k为垂足引垂线和农业机械当前纵向方向线的相交点，T_s为导航的采样和控制周期，Ls为预定义路线上点与点之间的平均间距；

(2)预瞄点P′_p的确定：在农业机械当前纵向方向上，由当前定位点P_c点起截取前视距离L_f得到P_p点；再由P_p点向预定义路线投影，得到预瞄点P′_p。