CN116149189A - 一种改进Pursuit算法的4WID高地隙喷雾机轨迹跟踪控制方法 - Google Patents

Abstract

本发明公开了一种改进Pursuit算法的4WID高地隙喷雾机轨迹跟踪控制方法，属于无人驾驶系统领域。所述控制方法将横向误差和航向误差引入4WID纯跟踪模型，对RTK定位误差进行修正，设计评价函数动态改变前视距离，解决了传统纯跟踪控制器中前视距离不可动态调整而导致跟踪精度低的问题，有效提高了转弯处导航轨迹跟踪精度,可满足水田环境下高精度作业的需求。

Description

一种改进Pursuit算法的4WID高地隙喷雾机轨迹跟踪控制 方法

技术领域

本发明涉及无人驾驶系统技术，具体涉及一种改进Pursuit算法的4WID高地隙喷雾机轨迹跟踪控制方法。

背景技术

在无人驾驶系统领域，农业机器人受到广泛关注。路径跟踪的效果直接决定导航控制的优劣，提高期望路径跟踪任务的性能和精度，对于农业机器人的整体控制非常重要。

纯追踪由于其控制简单而具有很大的优势。然而，在农业应用中，由于环境和地形特征的不确定性，它存在不能及时跟踪，收敛速度慢，跟踪精度低的问题，性能和准确性并不理想。

对此，许多学者对纯跟踪算法进行了改进。研究者基于ITAE优化准则、模糊算法、粒子群优化算法、BP神经网络算法、蚁群优化算法实现动态调节纯追踪模型的前视距离，有效提高了农机跟踪的精度，减少了收敛时间和跟踪误差。

发明内容

本发明提出了一种基于Pursuit的4WID高地隙喷雾机轨迹跟踪控制优化算法。优化的算法将横向误差和航向误差引入4WID纯跟踪模型，设计评价函数动态改变前视距离，并对RTK定位误差进行修正，有效提高了转弯处导航轨迹跟踪精度。

本发明的技术方案是：一种基于改进Pursuit的4WID高地隙喷雾机轨迹跟踪控制方法，包括以下步骤：

步骤1，采用高精度导航定位系统RTK，提供喷雾机实时高精度的位置、速度和姿态导航参数。

步骤2，针对4WID高地隙喷雾机特殊的行走底盘，建立其运动学模型和纯跟踪模型。

步骤3，将横向误差和航向误差引入理想状态下纯跟踪模型得到改进纯跟踪模型。

步骤4，基于改进纯跟踪模型，设计评价函数动态改变前视距离，实现路径跟踪。

进一步，所述步骤1具体包括：

步骤1.1，步骤1所述的喷雾机采用高度集成的GNSS/INS高精度组合导航系统，其配套的GNSS高精度定位定向接收机内置高精度定位定向板卡，能够快速精确的解算出两个天线相对位置信息和两个天线相位中心连线与真北之间的夹角(方位角)。同时通过接受基准站差分数据，可实现实时载波相位差分定位(RTK)，为喷雾机提供厘米级的高精度位置信息。

进一步，所述步骤2具体包括：

步骤2.1，首先建立全局坐标系与车体坐标系，基于几何学原理建立喷雾机运动学模型，为方便设计算法将喷雾机运动学模型简化为：

其中，P＝[x y θ]^T为喷雾机质心在全局坐标系下的位姿，(x,y)为车体质心O点坐标，θ为喷雾机中心线相对于惯性系的航向，L为喷雾机底盘的轴距，δ为前后转向轴的转向角度，v为喷雾机相对于惯性系的速度。

步骤2.2，建立高地隙喷雾机纯跟踪模型。

步骤2.2.1，在导航坐标系中，A表示喷雾机的当前后轮中心位置，C表示参考路径上预览点的喷雾机坐标，R表示转弯半径。设置逆时针移动R>0，顺时针移动R<0，2α表示中心角，L_d表示前视距离，L表示喷雾机底盘的轴距，δ表示喷雾机前后转向轴的转向角度。

步骤2.2.2，在ΔAOC中，根据正弦定理得：

步骤2.2.3，四轮转向模型中转向角存在如下关系：

步骤2.2.4，联立公式(2)与公式(3)得到转向角表达式：

进一步，所述步骤3具体包括：

步骤3.1，(x_i,y_i)是目标点的坐标，(x_o,y_o)是后轮中心的坐标,L_d是待确定的变量，满足以下几何关系：

步骤3.2，L₁为实际横向误差，α为实际航向角，θ_e为航向误差，满足以下几何关系：

步骤3.3，联立公式(5)、公式(6)、公式(7)与公式(4)得到改进纯跟踪模型：

进一步，所述步骤4具体包括：

步骤4.1，由喷雾机当前位置与参考路径之间的位置关系确定前视区域。

步骤4.2，遍历前视区域中路径点，代入改进纯跟踪模型。

步骤4.3，进行喷雾机位置预测，Δt为更新时间间隔，由于农机的位置数据更新很快，因此可以假设速度和车轮旋转角度在Δt内保持不变：

步骤4.4，进行RTK定位坐标修正。

步骤4.4.1，当喷雾机在水田环境行驶时，车身往往会发生倾斜，从而使得输出的RTK定位坐标并非实际的车辆后轴中点的坐标，直接将定位系统输出的坐标值作为车辆位置坐标会存在较大误差。

步骤4.4.2，设定位天线的安装高度为H，横滚角为φ，俯仰角为ψ，车辆航向角为θ，修正后的喷雾机位置坐标为：

步骤4.5，设计评价函数得到最优前视距离。

步骤4.5.1，横向误差表达式为：

步骤4.5.2，航向误差表达式为：

θ_e＝θ-θ_r (12)

步骤4.5.3，评价函数表达式为：

其中，

分别是下一时刻的预测横向误差和预测航向误差。

步骤4.5.4，在前视区域遍历路径点，以获得最大值A_max，A_max对应路径点到喷雾机的距离即为最优前视距离。

本发明的有益效果是：本发明将横向误差和航向误差引入4WID纯跟踪模型，对RTK定位误差进行修正，设计评价函数动态改变前视距离，解决了传统纯跟踪控制器中前视距离不可动态调整而导致跟踪精度低的问题，有效提高了转弯处导航轨迹跟踪精度,可满足水田环境下高精度作业的需求。

附图说明：

图1为喷雾机运动学模型简化图。

图2为喷雾机四轮转向模型。

图3为改进纯跟踪模型。

图4为RTK定位误差示意图。

具体实施方式：

下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，所描述的实例仅仅是本发明的一部分实施例，不是全部的实施例。

具体实施方式的步骤如下：

步骤1：基于4WID高地隙喷雾机，可提供喷雾机实时高精度的位置、速度和姿态导航参数。

步骤1.1：步骤1所述的喷雾机采用高度集成的GNSS/INS高精度组合导航系统，其配套的GNSS高精度定位定向接收机内置高精度定位定向板卡，能够快速精确的解算出两个天线相对位置信息和两个天线相位中心连线与真北之间的夹角(方位角)。同时通过接受基准站差分数据，可实现实时载波相位差分定位(RTK)，为喷雾机提供厘米级的高精度位置信息。

步骤2：针对4WID高地隙喷雾机特殊的行走底盘，建立其运动学模型并简化。包括以下步骤：

步骤2.1：如图1所示，首先建立全局坐标系与车体坐标系，基于几何学原理建立喷雾机运动学模型，为方便设计算法将喷雾机运动学模型简化为：

步骤2.2：如图2所示，建立高地隙喷雾机纯跟踪模型。主要包括以下步骤：

步骤2.2.1：在导航坐标系中，A表示喷雾机的当前后轮中心位置，C表示参考路径上预览点的喷雾机坐标，R表示转弯半径。设置逆时针移动R>0，顺时针移动R<0，2α表示中心角，L_d表示前视距离，L表示喷雾机底盘的轴距，δ表示喷雾机前后转向轴的转向角度。

步骤2.2.2：在ΔAOC中，根据正弦定理得：

步骤2.2.3：四轮转向模型中转向角存在如下关系：

步骤2.2.4：联立公式(2)与公式(3)得到转向角表达式：

步骤3：如图3所示，包括以下步骤：

步骤3.1，(x_i,y_i)是目标点的坐标，(x_o,y_o)是后轮中心的坐标，R是转弯半径,L_d是待确定的变量，满足以下几何关系：

步骤3.2，L₁为实际横向误差，α为实际航向角，θ_e为航向误差，满足以下几何关系：

步骤3.3，联立公式(5)、公式(6)、公式(7)与公式(4)得到改进纯跟踪模型：

步骤4.3，进行喷雾机位置预测，Δt为更新时间间隔，由于农机的位置数据更新很快，因此可以假设速度和车轮旋转角度在Δt内保持不变：

步骤4.4.2，如图4所示，设定位天线的安装高度为H，横滚角为φ，俯仰角为ψ，车辆航向角为θ，修正后的喷雾机位置坐标为：

步骤4.5，设计评价函数得到最优前视距离。

步骤4.5.1，横向误差表达式为：

步骤4.5.2，航向误差表达式为：

θ_e＝θ-θ_r (12)

步骤4.5.3，评价函数表达式为：

其中，

分别是下一时刻的预测横向误差和预测航向误差。

步骤4.5.4，在前视区域遍历路径点，以获得最大值A_max，A_max对应路径点到喷雾机的距离即为最优前视距离。

综上所述，本发明将横向误差和航向误差引入4WID纯跟踪模型，对RTK定位误差进行修正，设计评价函数动态改变前视距离，解决了传统纯跟踪控制器中前视距离不可动态调整而导致跟踪精度低的问题，有效提高了转弯处导航轨迹跟踪精度,可满足水田环境下高精度作业的需求。

尽管已经示出和描述了本发明的实施例，本领域的普通技术人员可以理解：在不脱离本发明的原理和宗旨的情况下可以对这些实施例进行多种变化、修改、替换和变型，本发明的范围由权利要求及其等同物限定。

Claims (4)

1.一种改进Pursuit算法的4WID高地隙喷雾机轨迹跟踪控制方法，其特征在于，包括以下步骤：

步骤1，采用高精度导航定位系统RTK，提供喷雾机实时高精度的位置、速度和姿态导航参数；

步骤2，针对4WID高地隙喷雾机特殊的行走底盘，建立其运动学模型和纯跟踪模型；

步骤3，将横向误差和航向误差引入理想状态下纯跟踪模型得到改进纯跟踪模型；

步骤4，基于改进纯跟踪模型，设计评价函数动态改变前视距离，实现路径跟踪并仿真验证。

2.根据权利要求1所述的方法，其特征在于：所述步骤2具体过程如下：

针对4WID高地隙喷雾机特殊底盘，建立其运动学模型并简化：

其中，P＝[x y θ]^T为喷雾机质心在全局坐标系下的位姿，(x,y)为车体质心O点坐标，θ为喷雾机中心线相对于惯性系的航向，L为喷雾机底盘的轴距，δ为前后转向轴的转向角度，v为喷雾机相对于惯性系的速度；

在高地隙喷雾机模型ΔAOC中，根据正弦定理得：

四轮转向模型中转向角存在如下关系：

联立公式(2)与公式(3)得到转向角表达式：

其中，R表示转弯半径，2α表示中心角，L_d表示前视距离，L表示喷雾机底盘的轴距，δ表示喷雾机前后转向轴的转向角度。

3.根据权利要求2所述的方法，其特征在于：所述的步骤3具体过程如下：

在改进纯跟踪模型中，存在以下几何关系：

联立公式(5)、公式(6)、公式(7)与公式(4)得到：

其中，d为横向误差，(x_i,y_i)是目标点的坐标，(x_o,y_o)是后轮中心的坐标，R是转弯半径,L_d是待确定的变量，L₁为实际横向误差，α为实际航向角，θ_e为航向误差。

4.根据权利要求3所述的方法，其特征在于：所述的步骤4具体过程如下：

步骤4.1，由喷雾机当前位置与参考路径之间的位置关系确定前视区域；

步骤4.2，遍历前视区域中路径点，代入改进纯跟踪模型；

步骤4.3，进行喷雾机位置预测：

其中，(x(t+Δt)，y(t+Δt)，θ(t+Δt))为下一时刻喷雾机质心在全局坐标系下的位姿，(x(t)，y(t)，θ(t))为当前时刻喷雾机质心在全局坐标系下的位姿，(x,y)为车体质心O点坐标，θ为喷雾机中心线相对于惯性系的航向，Δt为更新时间间隔，L为喷雾机底盘的轴距，δ为喷雾机前后转向轴的转向角度,v为喷雾机相对于惯性系的速度；

步骤4.4，进行RTK定位坐标修正：

其中，(x_m,y_m)为订正后的RTK定位坐标，(x′_m,y′_m)为RTK定位坐标，H为定位天线的安装高度，φ为横滚角，ψ为俯仰角，θ为喷雾机中心线相对于惯性系的航向；

步骤4.5，设计评价函数得到最优前视距离；

步骤4.5.1，横向误差表达式为：

步骤4.5.2，航向误差表达式为：

θ_e＝θ-θ_r (12)

其中，d_e为横向误差，θ_e为航向误差，(x,y,θ)当前位置信息，(x_r,y_r,θ_r)为目标点位置信息,(x,y)为车体质心O点坐标，θ为喷雾机中心线相对于惯性系的航向；

步骤4.5.3，评价函数表达式为：

其中，A_i为评价函数，

分别是下一时刻的预测横向误差和预测航向误差；

步骤4.5.4，在前视区域遍历路径点，以获得最大值A_max，A_max对应路径点到喷雾机的距离即为最优前视距离。

Images