[发明专利]一种无人作业谷物联合收割机导航方法与导航装置

权利要求书

1.一种无人作业谷物联合收割机导航方法，其特征在于，将北斗RTK定位模块与视觉CCD传感器融合，设计即时路径规划算法，实现导航路径规划；设计基于SUKF的导航跟踪误差的统一估计器，实现收割机的直线行驶和地头转向；将即时路径规划算法、导航跟踪误差的统一估计器应用于联合收割机的导航装置中，并将导航路径规划与统一估计器集成到CPU中，采用CAN总线与执行器和机载传感器通信，实现收割机的行走控制与机具控制。

2.根据权利要求1所述的一种无人作业谷物联合收割机导航方法，其特征在于，所述即时路径规划算法具体为：在联合收割机作业同时利用摄像头采集田块边界图像，并经CPU分析后获得田块边界，利用北斗RTK定位模块输出的位置、航向信息，实时获取矩形田块的四个顶点坐标，并将其转化至高斯平面坐标系，实现导航路径规划，并将规划的路径以二维数组形式存储于导航装置中。

3.如权利要求1所述的一种无人作业谷物联合收割机导航方法，其特征在于，所述导航跟踪状态的统一估计器具体为：设联合收割机待估计的行走状态为x_k＝[x_k y_k θ_k φ_k]^T，包括联合收割机当前二维位置、航向以及双天线测航向的偏置误差，u_k-1＝[u₁ u₂]为左右轮输入速度控制项，其系统模型为：x_k＝Φ_k|k-1x_k-1+G_k-1u_k-1+w_k-1，其中Φ_k|k-1为状态转移矩阵，G_k-1为系统输入控制矩阵，w_k-1为RTK系统噪声，包括位置与航向噪声以及航向测量的偏置误差，θ_k为k时刻车辆与地球真北方向的航向角，W为联合收割机轴距；

以位置和航向为量测输出，系统的量测方程为：z_k＝Hx_k+v_k，其中v_k为量测噪声误差阵，包括RTK系统测量噪声，即位置、航向的量测噪声。

4.根据权利要求1所述的一种无人作业谷物联合收割机导航方法，其特征在于，所述导航跟踪误差的统一估计器的非线性滤波为：

1)设统一估计器的状态维数为n，k-1时刻的状态概率密度分布p_k-1满足其中P_k-1|k-1分别为k-1时刻的均值和协方差，生成2n+1个对称分布的sigma点χ_i和权值ω_i匹配p_k-1，则有：

其中i为sigma点索引，i＝1,…,2n+1，κ为比例系数；

2)SUKF状态预测过程为：

其中为k时刻状态预测均值，P_k|k-1为k时刻预测方差，Q_k-1为k-1时刻系统噪声方差阵；

3)SUKF量测更新过程为：

K_k＝P_k|k-1H^T(HP_k|k-1H^T+R_k)^-1

其中K_k为滤波增益，和P_k|k分别为状态后验估计结果，R_k为量测噪声的方差阵，I为具有对应维数的单位阵。