CN110275529B - 基于中位角自适应调节的收获机智能辅助驾驶方法 - Google Patents

Abstract

本发明公开了一种基于中位角自适应调节的收获机智能辅助驾驶方法，包括如下步骤：1、在当前k时刻，根据前k_max个时刻收获机的位置计算收获机与期望路径的距离偏差的平均值

2、判断中位角θ是否有偏差，如果

保持中位角不变；如果

计算中位角θ的修正值θ^correct，并对中位角进行修正；3、计算当前k时刻收获机的期望转角δ(k)、车轮目标转角α(k)＝θ+δ(k)；利用角度跟踪控制算法，调整车辆转向轮，跟踪车轮目标转角α(k)；4、循环执行步骤1‑3，在每一时刻判断收获机中位角是否有偏差，计算车轮目标转角，并调整转向轮跟踪车轮目标转角。该方法能够提高提高中位角标定精度、补偿航向传感器安装误差，从而提高了收获机的操作便捷性。

Description

基于中位角自适应调节的收获机智能辅助驾驶方法

技术领域

本发明涉及收获机路径跟踪领域，具体是基于中位角自适应调节的收获机智能辅助驾驶方法。

背景技术

收获机的辅助驾驶技术作为农业自动驾驶技术的重要组成部分，有助于推进农业机械化、智能化，对于农业生产精细化、规模化、智能化研究有着重要意义。高精度的收获机辅助驾驶系统是开展精准农业实践的前提与技术保障，具有如下意义：提高收获机作业效率，使收获过程不受昼夜及能见度低的恶劣天气的影响；降低驾驶员劳动强度及驾驶技术要求，可使驾驶员集中精力调节割台、拨禾轮、割刀等其余设备的协调运转；有效改善重复作业和遗漏作业的问题。

中位角是指收获机沿直线行驶时的车轮实际转角，该角度需与收获机的航向采集系统相匹配，否则将造成恒偏误差，使收获机的实际跟踪路线始终位于目标路径的一侧，造成漏割或重割情况，降低作业效率。目前该角度多为人工设定，通过驾驶员人为驾驶收获机以直线方向行驶，将该时间段内的实际转角的平均值作为中位角的实际值。随着系统的使用，由于车辆机械结构的磨损及地面不平整度的影响，需对中位角再次进行人工标定，极大的限制了系统的智能性和便捷性。为提高中位角的标定精度，需延长行驶距离，但受限于农田不平整的地形和弯曲的行驶道路，降低标定精度，影响辅助驾驶的跟踪性能。此方法无法标定航向传感器的安装误差，影响系统性能。

开展对中位角标定自适应算法的研究，对智能车辅助驾驶技术的发展，实现智能农业、精准农业的战略目标具有重要意义。

发明内容

发明目的：本发明旨在提供一种自适应调整中位角的收获机辅助驾驶方法，该方法能够提高提高中位角标定精度、补偿航向传感器安装误差，从而提高了收获机的操作便捷性。

技术方案：本发明采用如下技术方案：

基于中位角自适应调节的收获机智能辅助驾驶方法，包括如下步骤：

(1)在当前k时刻，根据k-k_max,k-k_max+1,…,k-1时刻收获机的位置和航向角，计算收获机与期望路径的距离偏差d_e(k-i)的平均值

mean(·)为求均值运算；i＝1,2,…,k_max；

(2)判断中位角θ是否有偏差：如果

中位角无偏差，保持中位角不变；如果

计算中位角θ的修正值θ^correct，并对中位角进行修正：θ＝θ^old+θ^correct；其中

为距离偏差阈值，θ^old为修正前的中位角；

(3)计算当前k时刻收获机的期望转角δ(k)、车轮目标转角α(k)＝θ+δ(k)；

利用角度跟踪控制算法，调整车辆转向轮，跟踪车轮目标转角α(k)；

(4)循环执行步骤1-3，在每一时刻判断收获机中位角是否有偏差，计算车轮目标转角，并调整转向轮跟踪车轮目标转角。

k-i时刻收获机与期望路径的距离偏差d_e(k-i)的计算为：

k-i时刻利用卫星导航系统获取收获机的位置(x_h(k-i),y_h(k-i))；

计算d_e(k-i)：

Ax+By+C＝0为期望路径的数学模型。

所述步骤(2)中，计算中位角的修正值包括如下步骤：

(2.1)令当前平均误差

如果k＝0，d_last＝d_next＝0；

(2.2)计算中位角增量θ^correct_inc：

θ^correct_inc＝k_p×(d_error-d_next)+k_i×d_error+k_d×(d_error-2×d_next+d_last)

其中k_p为比例系数，k_i为积分系数，k_d为微分系数；

(2.3)更新d_last和d_next：d_last＝d_next，d_next＝d_error；

(2.4)中位角的修正值θ^correct为：θ^correct＝θ^correct′+θ^correct_inc，其中θ^correct′为上一次对中位角进行修正时计算的中位角修正值。

步骤(2.2)中系数k_p、k_i、k_d的确定包括如下步骤：

(2.2.1)定义m个粒子组成粒子集X＝{x_j|j＝1..m}，随机初始化每个粒子的位置和速度，将待确定的系数k_p、k_i、k_d作为粒子的位置，记第j个粒子的位置为pos_j＝(k_pj,k_ij,k_dj)，速度为V_j＝(V_pj,V_ij,V_dj)；

定义位置在(k_p′,k_i′,k_d′)的粒子的适应度为：

其中d_e(z|(k_p′,k_i′,k_d′))为比例系数、积分系数和微分系数分别为k_p′,k_i′,k_d′的条件下，在z时刻收获机与期望路径的距离偏差，z_max为适应度统计时宽；

初始化迭代次数l＝0，初始化每个粒子的最佳位置为所述粒子的初始位置，第j个粒子的最佳位置pos_j,best为

(2.2.2)将m个粒子的初始位置

分别作为参数计算适应度，选择其中适应度值最小的粒子位置作为初始的粒子集最佳位置gbest；

(2.2.3)按如下更新方程更新粒子集X中粒子x_j的位置和速度：

其中

和

分别为第l次迭代时第j个粒子的速度和位置；c₁和c₂为学习因子，rand₁()和rand₂()为取(0,1)之间的随机数；

更新完成后重新计算每个粒子的适应度；如果

则将pos_j,best的值更新为

如果

则将gbest更新为

迭代次数l加一；

(2.2.4)重复步骤(2.2.3)，直到迭代次数l达到预设的阈值L_th，或gbest的适应度小于预设的阈值F_th，结束迭代；此时gbest的值(k_pbest,k_ibest,k_dbest)为步骤2.2中比例系数k_p、积分系数k_i、微分系数k_d的最优值。

步骤(3)中计算k时刻收获机的期望转角δ(k)的计算式为：

其中L为收获机前轮轴心和后轮轴心的距离，L_d为前视距离参数；

为k-1时刻导航系统获取的收获机的航向角

与期望路径的航向

之差，

步骤(3)中采用PID角度跟踪控制算法，通过实时调整收获机转向比例阀的输入信号，实现控制收获机转向轮跟踪车轮目标转角α(k)。

有益效果：与现有技术相比，本发明公开的收获机智能辅助驾驶方法具有以下优点：1、根据收获机与期望路径的距离偏差自适应地对中位角进行标定，无需人工操作，不受田间地形和机械磨损的影响，提高了中位角标定的便捷性和标定精度；2、利用粒子群算法对中位角增量计算式中的参数进行优化，消除了人为干预，通过设计合理的适应度函数，可快速准确的确定比例系数、积分系数和微分系数，提高中位角标定的快速性、准确性；

附图说明

图1为本发明公开的收获机智能辅助驾驶方法的流程图；

图2为实施例中本发明公开的方法与其他调节方法的效果对比图。

具体实施方式

为使本发明的目的、技术方案和优点更加清楚，下面结合附图对本发明的具体实施案例做说明。

如图1所示，本发明公开了一种基于中位角自适应调节的收获机智能辅助驾驶方法，包括如下步骤：

步骤1、在当前k时刻，根据k-k_max,k-k_max+1,…,k-1时刻收获机的位置和航向角，计算收获机与期望路径的距离偏差d_e(k-i)的平均值

mean(i)为求均值运算；i＝1,2,…,k_max；

本实施例中，利用卫星导航系统GNSS获取收获机的位置和航向角信息，k-i时刻收获机与期望路径的距离偏差d_e(k-i)的计算为：

k-i时刻利用卫星导航系统获取收获机的位置(x_h(k-i),y_h(k-i))和航向角；

d_e(k-i)为k-i时刻收获机位置到期望路径的距离，一般情况下，收获机的期望路径均为直线，对其建立数学模型，期望路径以直线方程表示为：Ax+By+C＝0，由此，

步骤2、判断中位角θ是否有偏差：如果

中位角无偏差，保持中位角不变；如果

计算中位角θ的修正值θ^correct，并对中位角进行修正：θ＝θ^old+θ^correct；其中

为距离偏差阈值，θ^old为修正前的中位角；

本发明采用增量式PID控制算法来计算中位角的修正值θ^correct，包括如下步骤：

(2.1)令当前平均误差

如果k＝0，d_last＝d_next＝0；

(2.2)计算中位角增量θ^correct_inc：

θ^correct_inc＝k_p×(d_error-d_next)+k_i×d_error+k_d×(d_error-2×d_next+d_last)

其中k_p为比例系数，k_i为积分系数，k_d为微分系数；

(2.3)更新d_last和d_next：d_last＝d_next，d_next＝d_error；

(2.4)中位角的修正值θ^correct为：θ^correct＝θ^correct′+θ^correct_inc，其中θ^correct′为上一次对中位角进行修正时计算的中位角修正值。

步骤3、计算当前k时刻收获机的期望转角δ(k)、车轮目标转角α(k)＝θ+δ(k)；

计算k时刻收获机的期望转角δ(k)的计算式为：

其中L为收获机前轮轴心和后轮轴心的距离，L_d为前视距离参数；

为k-1时刻导航系统获取的收获机的航向角

与期望路径的航向

之差，即

利用角度跟踪控制算法，通过实时调整收获机转向比例阀的输入信号，调整车辆转向轮，跟踪车轮目标转角α(k)；

步骤4、循环执行步骤1-3，在每一时刻判断收获机中位角是否有偏差，计算车轮目标转角，并调整转向轮跟踪车轮目标转角。

本发明中，步骤(2.2)中的系数k_p、k_i、k_d采用离线确定，包括如下步骤：

(2.2.1)定义m个粒子组成粒子集X＝{x_j|j＝1..m}，随机初始化每个粒子的位置和速度，将待确定的系数k_p、k_i、k_d作为粒子的位置，记第j个粒子的位置为pos_j＝(k_pj,k_ij,k_dj)，速度为V_j＝(V_pj,V_ij,V_dj)；

定义位置在(k_p′,k_i′,k_d′)的粒子的适应度为：

其中d_e(z|(k_p′,k_i′,k_d′))为比例系数、积分系数和微分系数分别为k_p′,k_i′,k_d′的条件下，在z时刻收获机与期望路径的距离偏差，z_max为适应度统计时宽。即固定比例系数、积分系数和微分系数不变，循环执行步骤1-3，获取z_max个时刻的收获机与期望路径的距离偏差，求和得到适应度。

初始化迭代次数l＝0，初始化每个粒子的最佳位置为所述粒子的初始位置，第j个粒子的最佳位置pos_j,best为

(2.2.2)将m个粒子的初始位置

分别作为参数计算适应度，选择其中适应度值最小的粒子位置作为初始的粒子集最佳位置gbest；

(2.2.3)按如下更新方程更新粒子集X中粒子x_j的位置和速度：

其中

和

分别为第l次迭代时第j个粒子的速度和位置；c₁和c₂为学习因子，rand₁()和rand₂()为取(0,1)之间的随机数；

更新完成后重新计算每个粒子的适应度；如果

则将pos_j,best的值更新为

如果

则将gbest更新为

迭代次数l加一；

(2.2.4)重复步骤(2.2.3)，直到迭代次数l达到预设的阈值L_th，或gbest的适应度小于预设的阈值F_th，结束迭代；此时gbest的值(k_pbest,k_ibest,k_dbest)为步骤2.2中比例系数k_p、积分系数k_i、微分系数k_d的最优值。

为验证本发明公开的收获机辅助驾驶方法的效果，在中位角转向偏差为1度，航向偏差为0.1度的情况下，与未进行中位角标定的辅助驾驶系统、人工消除中位角误差的辅助驾驶系统、人工设定PID参数的辅助驾驶系统进行对比，误差如图2所示，图中“+”为未进行中位角标定的辅助驾驶系统的误差曲线，“□”为人工消除中位角误差的辅助驾驶系统的误差曲线，“—”为采用本发明公开的收获机辅助驾驶方法的误差曲线，具体的对比效果如下：

未进行中位角标定的辅助驾驶系统的最大误差为5.62cm，收敛后恒偏误差为-4.71cm；人工消除中位角误差的辅助驾驶系统的最大误差为1.79cm，收敛后恒偏误差为-0.7726cm；本发明提出的方法最大误差为5.67cm，收敛后恒偏误差为3.3×10^-3cm。

未进行中位角标定且包含航向偏差的辅助驾驶系统具有恒偏误差，严重影响收获机作业精度和作业效率。

仅对中位角标定的辅助驾驶系统由于航向偏差的存在，仍存在较小的恒偏误差，影响作业效率，但人工标定航向偏差需高精度仪器设备对整车进行标定，浪费人力物力。

本发明提出的基于中位角自适应调节的收获机辅助驾驶方法通过粒子群算法对PID参数进行自整定，并实时调节中位角参数，收敛速度最快且对横向偏差抑制效果最好。

Claims (5)

1.基于中位角自适应调节的收获机智能辅助驾驶方法，其特征在于，包括如下步骤：

(1)在当前k时刻，根据k-k_max,k-k_max+1,…,k-1时刻收获机的位置和航向角，计算收获机与期望路径的距离偏差d_e(k-i)的平均值

mean(·)为求均值运算；i＝1,2,…,k_max；

(2)判断中位角θ是否有偏差：如果

中位角无偏差，保持中位角不变；如果

计算中位角θ的修正值θ^correct，并对中位角进行修正：θ＝θ^old+θ^correct；其中

为距离偏差阈值，θ^old为修正前的中位角；

(3)计算当前k时刻收获机的期望转角δ(k)、车轮目标转角α(k)＝θ+δ(k)；

利用角度跟踪控制算法，调整车辆转向轮，跟踪车轮目标转角α(k)；

(4)循环执行步骤1-3，在每一时刻判断收获机中位角是否有偏差，计算车轮目标转角，并调整转向轮跟踪车轮目标转角；

所述步骤(2)中，计算中位角的修正值包括如下步骤：

(2.1)令当前平均误差

如果k＝0，d_last＝d_next＝0；

(2.2)计算中位角增量θ^correct_inc：

θ^correct_inc＝k_p×(d_error-d_next)+k_i×d_error+k_d×(d_error-2×d_next+d_last)

其中k_p为比例系数，k_i为积分系数，k_d为微分系数；

(2.3)更新d_last和d_next：d_last＝d_next，d_next＝d_error；

(2.4)中位角的修正值θ^correct为：θ^correct＝θ^correct′+θ^correct_inc，其中θ^correct′为上一次对中位角进行修正时计算的中位角修正值。

2.根据权利要求1所述的基于中位角自适应调节的收获机智能辅助驾驶方法，其特征在于，k-i时刻收获机与期望路径的距离偏差d_e(k-i)的计算为：

k-i时刻利用卫星导航系统获取收获机的位置(x_h(k-i),y_h(k-i))；

计算d_e(k-i)：

Ax+By+C＝0为期望路径的数学模型。

3.根据权利要求1所述的基于中位角自适应调节的收获机智能辅助驾驶方法，其特征在于，步骤(2.2)中系数k_p、k_i、k_d的确定包括如下步骤：

(2.2.1)定义m个粒子组成粒子集X＝{x_j|j＝1..m}，随机初始化每个粒子的位置和速度，将待确定的系数k_p、k_i、k_d作为粒子的位置，记第j个粒子的位置为pos_j＝(k_pj,k_ij,k_dj)，速度为V_j＝(V_pj,V_ij,V_dj)；

定义位置在(k_p′,k_i′,k_d′)的粒子的适应度为：

其中d_e(z|(k_p′,k_i′,k_d′))为比例系数、积分系数和微分系数分别为k_p′,k_i′,k_d′的条件下，在z时刻收获机与期望路径的距离偏差，z_max为适应度统计时宽；

初始化迭代次数l＝0，初始化每个粒子的最佳位置为所述粒子的初始位置，第j个粒子的最佳位置pos_j,best为

(2.2.2)将m个粒子的初始位置

分别作为参数计算适应度，选择其中适应度值最小的粒子位置作为初始的粒子集最佳位置gbest；

(2.2.3)按如下更新方程更新粒子集X中粒子x_j的位置和速度：

其中

和

分别为第l次迭代时第j个粒子的速度和位置；c₁和c₂为学习因子，rand₁()和rand₂()为取(0,1)之间的随机数；

更新完成后重新计算每个粒子的适应度；如果

则将pos_j,best的值更新为

如果

则将gbest更新为

迭代次数l加一；

(2.2.4)重复步骤(2.2.3)，直到迭代次数l达到预设的阈值L_th，或gbest的适应度小于预设的阈值F_th，结束迭代；此时gbest的值(k_pbest,k_ibest,k_dbest)为步骤2.2中比例系数k_p、积分系数k_i、微分系数k_d的最优值。

4.根据权利要求1所述的基于中位角自适应调节的收获机智能辅助驾驶方法，其特征在于，步骤(3)中计算k时刻收获机的期望转角δ(k)的计算式为：

其中L为收获机前轮轴心和后轮轴心的距离，L_d为前视距离参数；

为k-1时刻导航系统获取的收获机的航向角

与期望路径的航向

之差，

5.根据权利要求1所述的基于中位角自适应调节的收获机智能辅助驾驶方法，其特征在于，步骤(3)中采用PID角度跟踪控制算法，通过实时调整收获机转向比例阀的输入信号，实现控制收获机转向轮跟踪车轮目标转角α(k)。

Images