CN108490943A - 一种农业机械自适应曲线自动驾驶控制方法 - Google Patents

Abstract

本发明提供了一种农业机械自适应曲线自动驾驶控制方法，包括以下步骤：步骤(1)：建立车辆运动学模型；步骤(2)：获取实时数据，并计算得到状态变量、状态转移矩阵、控制量系数、观测矩阵、半正定对称加权阵、正半定对称时变加权阵和正定对称时变加权阵；步骤(3)：基于步骤(2)计算控制量；步骤(4)：通过PWM技术控制电磁阀或者电机驱动前轮转向，从而实现圆曲线路径跟踪自动控制，本发明针对实际应用中对自适应曲线标轨迹跟踪需求，设计了一种自适应曲线自动控制方法，进而填补这一技术空白，为后期自动避障、自动调头和无人驾驶的应用提供相关研究和实现基础。

Description

一种农业机械自适应曲线自动驾驶控制方法

技术领域

本发明涉及自动驾驶控制算法领域，具体涉及到一种农业机械自适应曲线自动驾驶控制方法。

背景技术

随着MEMS(Micro-Electro-Mechanical-System)传感器、导航和控制技术的发展，精准农业正在快速变成一种趋势。目前农业机械自动驾驶大部分是以AB直线的行驶进行作业，然而在实际使用过程中，对于一些不规则地块、避障或者特殊应用的场合，需要使用自适应曲线进行自动驾驶。

发明内容

为了解决上述的缺陷，本发明提供了一种农业机械自适应曲线自动驾驶控制方法，本发明针对实际应用中对自适应曲线标轨迹跟踪需求，设计了一种自适应曲线自动控制方法，进而填补这一技术空白，为后期自动避障、自动调头和无人驾驶的应用提供相关研究和实现基础。

本发明提供了一种农业机械自适应曲线自动驾驶控制方法，包括以下步骤：

步骤(1)：建立车辆运动学模型；

步骤(2)：获取实时数据，并计算得到状态变量、状态转移矩阵、控制量系数、观测矩阵、半正定对称加权阵、正半定对称时变加权阵和正定对称时变加权阵；

步骤(3)：基于步骤(2)计算控制量；

步骤(4)：通过PWM技术控制电磁阀或者电机驱动前轮转向，从而实现圆曲线路径跟踪自动控制。

上述的自动驾驶控制方法，其中，所述步骤(1)具体包括：

步骤(1.1)：基于自适应曲线运动模型，得到

其中R₀为圆半径，α₀为圆轨迹起始角度，α为车辆转过的角度，l₂为质心到控制点的距离，如果控制点和参考点重合，l₂＝0，ξ为航向修正角，R(s)为极坐标下曲率半径，以及定义ξ顺时针为正，车辆控制点位于参考点的后面，所以l₂<0；

步骤(1.2)：在惯性坐标系下，车辆的航向角计算公式为：

其中ψ_e、ψ'_s、l₁、l₂、V_x、R'、R、r'_s、δ_e、δ_∞、e'_s、e”_s、n'_s、n”_s分别为航向偏差、航向角在极坐标系下一阶导数、前后轮距离、质心到控制点的距离、车速、曲率半径中间变量、曲率半径、极坐标系下横向偏差一阶导数、前轮角度偏差、稳态前轮角度、极坐标下东坐标一阶导数、极坐标下东坐标二阶导数、极坐标下北坐标一阶导数、极坐标下北坐标二阶导数；其中：

步骤(1.3)：车辆稳态前轮角度计算方式为：

步骤(1.4)：车辆前轮角度偏差动态模型为：

步骤(1.5)：基于公式4，如果曲率半径恒定，则公式4变化为公式5；

其中u为控制量。

上述的自动驾驶控制方法，其中，所述步骤(1)还包括：

步骤(1.6)：基于公式5得到车辆跟踪误差模型为：

其中，

基于公式2、4、6得到运动学模型为：

x＝[ψ_e δ_e r_e]^T

y(t)＝C(t)x(t)

e(t)＝y_r(t)-y(t) (公式7)

其中，x为状态变量，A为状态转移矩阵，B为控制量系数，C为观测矩阵，y_r为目标值向量。

上述的自动驾驶控制方法，其中，所述步骤(2)还包括：

基于步骤(1)中的公式7，选择以下性能指标函数：

其中F为半正定对称加权阵，Q(t)为正半定对称时变加权阵，R(t)为正定对称时变加权阵，t₀、t_f分别为起始和结束时间。

上述的自动驾驶控制方法，其中，还包括自适应曲线路径生成，具体为：

假设对采样点坐标为(x_k，y_k)，k为采样点索引，k＞＝3，基于3阶样条曲线拟合通用表示方法为：

y_k(x)＝A_k+B_k(x-x_k)+C_k(x-x_k)²+D_k(x-x_k)³ (公式9)

基于农机采样点曲线极坐标表示方法为：

s₁＝0

n(s)＝A_n，k+B_n，k(s-s_k)+C_n，k(s-s_k)²+D_n，k(s-s_k)³

e(s)＝A_e，k+B_e，k(s-s_k)+C_e，k(s-s_k)²+D_e，k(s-s_k)³ (公式10)。

上述的自动驾驶控制方法，其中，所述步骤(3)包括：根据计算黎卡提方程：

来计算控制量u；

u(t)＝-R^-1B^TP(t)x(t) (公式12)。

本发明具有以下有益效果：

(1)本发明实现的自适应曲线自动控制方法简单可行，计算量小对硬件要求低。

(2)本发明的自适应曲线自动控制方法控制目标跟踪精度在±5cm，满足实际用户需求。

附图说明

通过阅读参照以下附图对非限制性实施例所作的详细描述，本发明及其特征、外形和优点将会变得更明显。在全部附图中相同的标记指示相同的部分。并未刻意按照比例绘制附图，重点在于示出本发明的主旨。

图1为本发明提供的自适应曲线运动模型示意图。

具体实施方式

在下文的描述中，给出了大量具体的细节以便提供对本发明更为彻底的理解。然而，对于本领域技术人员而言显而易见的是，本发明可以无需一个或多个这些细节而得以实施。在其他的例子中，为了避免与本发明发生混淆，对于本领域公知的一些技术特征未进行描述。

为了彻底理解本发明，将在下列的描述中提出详细的步骤以及详细的结构，以便阐释本发明的技术方案。本发明的较佳实施例详细描述如下，然而除了这些详细描述外，本发明还可以具有其他实施方式。

如图1所示，本发明提供的自适应曲线运动模型示意图，图中R₀为圆半径，α₀为圆轨迹起始角度，α为车辆转过的角度，l₂为质心到控制点的距离，如果控制点和参考点重合，l₂＝0，ξ为航向修正角，R(s)为极坐标下曲率半径。

在公式(1)中，定义ξ顺时针为正，在图1中，车辆控制点位于参考点的后面，所以l₂<0。

模型建立所用到的变量定义：ψ_e、ψ'_s、l₁、l₂、V_x、R'、R、r'_s、δ_e、δ_∞、e'_s、e”_s、n'_s、n”_s分别为航向偏差、航向角在极坐标系下一阶导数、前后轮距离、质心到控制点的距离、车速、曲率半径中间变量、曲率半径、极坐标系下横向偏差一阶导数、前轮角度偏差、稳态前轮角度、极坐标下东坐标一阶导数、极坐标下东坐标二阶导数、极坐标下北坐标一阶导数、极坐标下北坐标二阶导数。

在惯性坐标系下，车辆的航向角计算公式如公式(2)所示：

其中：

车辆稳态前轮角度计算通过公式3计算：

车辆前轮角度偏差动态模型：

在公式4中，如果曲率半径恒定，则公式4变化为公式5：其中u为控制量

车辆跟踪误差模型：

其中，

由以上模型分析，建立车辆运动学模型，模型建立和控制量计算：

x＝[ψ_e δ_e r_e]^T

y(t)＝C(t)x(t)

e(t)＝y_r(t)-y(t) (公式7)

其中，x为状态变量，A为状态转移矩阵，B为控制量系数，C为观测矩阵，y_r为目标值向量。

选择以下性能指标函数：

其中F为半正定对称加权阵，Q(t)为正半定对称时变加权阵，R(t)为正定对称时变加权阵，t₀、t_f分别为起始和结束时间。

以下提供一具体的实施例

实施例1

一种农业机械自适应曲线自动驾驶控制方法，包括步骤(1)：建立车辆运动学模型，具体为通过上述公式2、4、6建立运动学模型，如公式7所示，其中还包括自适应曲线路径生成

假设对采样点坐标为(x_k，y_k)，k为采样点索引，k＞＝3，基于3阶样条曲线拟合通用表示方法为：

y_k(x)＝A_k+B_k(x-x_k)+C_k(x-x_k)²+D_k(x-x_k)³ (公式9)

基于农机采样点曲线极坐标表示方法为：

s₁＝0

n(s)＝A_n，k+B_n，k(s-s_k)+C_n，k(s-s_k)²+D_n，k(s-s_k)³

e(s)＝A_e，k+B_e，k(s-s_k)+C_e，k(s-s_k)²+D_e，k(s-s_k)³ (公式10)。

步骤(2)：获取实时数据，计算公式7中的x，A，B，C和e，计算公式8中的F、Q、R、t₀、t_f；

步骤(3)：根据计算黎卡提方程：

计算控制量u：

u(t)＝-R^-1B^TP(t)x(t) (公式12)。

步骤(4)：通过PWM技术控制电磁阀或者电机驱动前轮转向，从而实现自适应曲线路径跟踪自动控制。

本发明的方法，针对实际应用中对自适应曲线标轨迹跟踪需求，设计了一种自适应曲线自动控制方法，进而填补这一技术空白，为后期自动避障、自动调头和无人驾驶的应用提供相关研究和实现基础；以及自适应曲线自动控制方法控制目标跟踪精度在±5cm，满足实际用户需求。

以上对本发明的较佳实施例进行了描述。需要理解的是，本发明并不局限于上述特定实施方式，其中未尽详细描述的设备和结构应该理解为用本领域中的普通方式予以实施；任何熟悉本领域的技术人员，在不脱离本发明技术方案范围情况下，都可利用上述揭示的方法和技术内容对本发明技术方案做出许多可能的变动和修饰，或修改为等同变化的等效实施例，这并不影响本发明的实质内容。因此，凡是未脱离本发明技术方案的内容，依据本发明的技术实质对以上实施例所做的任何简单修改、等同变化及修饰，均仍属于本发明技术方案保护的范围内。

Claims (6)

1.一种农业机械自适应曲线自动驾驶控制方法，其特征在于，包括以下步骤：

步骤(1)：建立车辆运动学模型；

步骤(2)：获取实时数据，并计算得到状态变量、状态转移矩阵、控制量系数、观测矩阵、半正定对称加权阵、正半定对称时变加权阵和正定对称时变加权阵；

步骤(3)：基于步骤(2)计算控制量；

步骤(4)：通过PWM技术控制电磁阀或者电机驱动前轮转向，从而实现圆曲线路径跟踪自动控制。

2.如权利要求1所述的一种农业机械自适应曲线自动驾驶控制方法，其特征在于，所述步骤(1)具体包括：

步骤(1.1)：基于自适应曲线运动模型，得到

其中R₀为圆半径，α₀为圆轨迹起始角度，α为车辆转过的角度，l₂为质心到控制点的距离，如果控制点和参考点重合，l₂＝0，ξ为航向修正角，R(s)为极坐标下曲率半径，以及定义ξ顺时针为正，车辆控制点位于参考点的后面，所以l₂<0；

步骤(1.2)：在惯性坐标系下，车辆的航向角计算公式为：

其中ψ_e、ψ′_s、l₁、l₂、V_x、R'、R、r′_s、δ_e、δ_∞、e'_s、e″_s、n'_s、n″_s分别为航向偏差、航向角在极坐标系下一阶导数、前后轮距离、质心到控制点的距离、车速、曲率半径中间变量、曲率半径、极坐标系下横向偏差一阶导数、前轮角度偏差、稳态前轮角度、极坐标下东坐标一阶导数、极坐标下东坐标二阶导数、极坐标下北坐标一阶导数、极坐标下北坐标二阶导数；其中：

步骤(1.3)：车辆稳态前轮角度计算方式为：

步骤(1.4)：车辆前轮角度偏差动态模型为：

步骤(1.5)：基于公式4，如果曲率半径恒定，则公式4变化为公式5；

其中u为控制量。

3.如权利要求2所述的一种农业机械自适应曲线自动驾驶控制方法，其特征在于，所述步骤(1)还包括：

步骤(1.6)：基于公式5得到车辆跟踪误差模型为：

其中，

基于公式2、4、6得到运动学模型为：

x＝[ψ_e δ_e r_e]^T

y(t)＝C(t)x(t)

e(t)＝y_r(t)-y(t) (公式7)

其中，x为状态变量，A为状态转移矩阵，B为控制量系数，C为观测矩阵，yr为目标值向量。

4.如权利要求3所述的一种农业机械自适应曲线自动驾驶控制方法，其特征在于，所述步骤(2)还包括：

基于步骤(1)中的公式7，选择以下性能指标函数：

其中F为半正定对称加权阵，Q(t)为正半定对称时变加权阵，R(t)为正定对称时变加权阵，t₀、t_f分别为起始和结束时间。

5.如权利要求4所述的一种农业机械自适应曲线自动驾驶控制方法，其特征在于，还包括自适应曲线路径生成，具体为：

假设对采样点坐标为(x_k，y_k)，k为采样点索引，k＞＝3，基于3阶样条曲线拟合通用表示方法为：

y_k(x)＝A_k+B_k(x-x_k)+C_k(x-x_k)²+D_k(x-x_k)³ (公式9)

基于农机采样点曲线极坐标表示方法为：

s₁＝0

n(s)＝A_n，k+B_n，k(s-s_k)+C_n，k(s-s_k)²+D_n，k(s-s_k)³

e(s)＝A_e，k+B_e，k(s-s_k)+C_e，k(s-s_k)²+D_e，k(s-s_k)³ (公式10)。

6.如权利要求5所述的一种农业机械自适应曲线自动驾驶控制方法，其特征在于，所述步骤(3)包括：根据计算黎卡提方程：

来计算控制量u；

u(t)＝-R^-1B^TP(t)x(t) (公式12)。