CN104571109A - 一种基于扰动观测器的农用车辆自主导航转向控制方法 - Google Patents

Abstract

本发明公开了一种基于扰动观测器的农用车辆自主导航转向控制方法，属于农用车辆自主导航控制的高精度跟踪控制领域。该方法采用闭环控制方式，开环粗略计算车辆实际的横向偏差，闭环校正，消除累积误差，直到精准跟踪，实现一个完整的控制周期。鉴于农用车辆运动模型的多样化，采用的控制手段也灵活多变：采用极点配置的方式，实现三阶线性和非线性模型的精确控制；本发明农用车辆运动控制方法在对应控制手段整定下的控制效果均可达到：超调量小于5％，调整时间小于3s，最终实现无静差跟踪。

Description

一种基于扰动观测器的农用车辆自主导航转向控制方法

技术领域

本发明属于农用车辆智能路径导航与高精度控制的研究领域，具体涉及到农用车辆智能导航的控制器设计与主动抗干扰的扰动观测器的设计。

背景技术

农用车辆智能导航控制器的设计可以考虑***多变量、非线性、时变性的特征，利用各种智能控制方案：包括模糊算法，整数阶分数阶PID算法，对***进一步设计。具体可以根据***的特点提出优化的控制方案；在农用车辆自主导航的过程中，土壤的结构与软硬程度、随机噪声的不确定性、路面的凹凸不平、执行机构负载转矩的变化和摩擦，车辆的运行受外界环境影响及模型本身的颤动，都会对***的性能造成不可预估的影响。因此需要主动抗干扰技术的扰动观测器根据具体的控制对象进行滤波器的设计，保证***本身具有较好的鲁棒性能。

农用车辆自主导航技术主要包括：导航传感器、路径规划、车辆模型的建立、跟踪控制器的设计以及扰动观测器的设计。农业车辆田间作业环境对***的结构和性能要求很高，因此跟踪控制器与扰动观测器的设计正吸引越来越多学者的研究兴趣，并为此做了大量的工作。

国内外研究表明，农用车辆工作环境极为复杂，其运动受自然环境、土壤条件、负荷变化等许多非线性因素的影响，这就要求设计的控制***不仅要满足跟踪路径稳、准、快，还要考虑其他影响农机导航精度的不确定因素。例如：***模型参数的摄动，低频的外部干扰和高频的测量噪声，控制器的不确定饱和都是需要研究的问题。在传统的控制策略中，PID是农用车辆自主导航控制***中最常用的控制手段。其控制效果相对稳定，技术成熟。但其在时变的控制***和非线性***中存在一定的局限性。即该控制策略只关注***跟踪精度的PID控制器设计，而忽略或者单纯通过反馈控制消除***本身存在的模型参数的非线性摄动、低频的外部干扰、高频的测量噪声、控制器的饱和等一系列不确定因素。这种控制策略对于农用车辆自主导航控制***日益要求的高精度，干扰抑制能力，鲁棒稳定性是明显不足的。正是基于这个背景，寻找新的控制方法和优化方案是必要而迫切的。

发明内容

本发明提供了基于扰动观测器的农用车辆自主导航模型的设计方法。同时针对车辆非线性运动模型结构采用不同的控制策略，用于高精度跟踪给定的路径，实现农用车辆的智能精确导航。设计的控制***都可以实现无静差跟踪，调整时间均在3s范围内，超调量均在5％以内，加入扰动后的***仍能保持原来的动态品质和稳态性能。

为了实现上述的发明目的，本发明的目的之一在于，提供一种稳定且实用的农用车辆自主导航的路径跟踪控制方法。具体为：一种基于扰动观测器的农用车辆自主导航转向控制方法，其特征在于，包括如下几个步骤：

1)首先根据农用车辆实际导航需求的情况，拟定农用车辆路径跟踪控制***的控制流程设计图；

2)组建农用车辆路径跟踪控制***，由转向控制器G_c(s)，扰动观测器Q(s)和车辆运动学模型G(s)这几个部分构成；

3)结合实际的控制需求，设定农用车辆路径跟踪控制***的输入为控制变量u，输出为横向偏差y，此时设定期望输出量y₁＝u＝1；

4)确定农用车辆路径跟踪控制***的研究对象：农用车辆的运动学模型；此时农用车辆的运动学模型取O’Connor提出的三阶线性模型和三阶非线性模型，其中对于三阶线性的模型处理是为了便于对三阶非线性模型的研究；

5)基于三阶线性模型，利用极点配置设计***的控制器G_c(s)，并将此控器用于三阶非线性模型的控制与整定；

6)在车辆三阶非线性模型基础上，引入***本质存在的三类扰动：模型误差Δ(s)，外界干扰d，测量噪声ζ，同时对这三种扰动进行不同组合的模拟，分析它们对农用车辆路径跟踪控制***的影响；

7)在步骤5设计的控制器G_c(s)基础上，加入扰动观测器Q(s)的设计，此时设计两种扰动观测器，分别为Q_B(s)和Q_b(s)，在这两种控制手段的作用下，讨论***对外界干扰的主动抑制能力，并且对比分析两种扰动观测器的控制效果；

8)利用DSP与现有的农用车辆平台，构建一种农用车辆路径跟踪控制装置：包括主控制器和路径跟踪闭环传感器以及农用车辆硬件平台。

进一步，所述步骤1)中，农用车辆路径跟踪控制***的控制流程设计图具体为：首先设定车辆跟踪目标y₁和确定控制输入u，其次选取控制决策设计控制器G_c(s)和扰动观测器Q(s)；然后农用车辆开始实际运动，确定实际的跟踪效果y，同时将车辆的实际运动效果与期望目标作比较：u-y；此时***开始性能的综合分析：分别计算调整时间t_s和超调量σ％，若满足***要求，则设计工作结束；若不满足***要求，则继续调整控制决策部分，直至满足***要求为止。

进一步，所述步骤2)中，农用车辆路径跟踪控制***由三部分组成：控制器G_c(s)、扰动观测器Q(s)、农用车辆运动学模型G(s)；根据公知技术，农用车辆运动学模型由执行机构与被控对象两部分组成，此时输出横向偏差y和输入u的推导关系如下：

$y=\frac{G_c\left(s\right)G\left(s\right)}{1+G_c\left(s\right)G\left(s\right)}u---\left(1\right)$

即控制输入u和输出横向偏差y的关系是基于信号传递函数的数学关系式，控制器G_c(s)的决定了输入和输出两者的具体关系。

进一步，所述步骤3)中，***的控制输入为u，***的输出为横向偏差y，期望输出y₁＝u＝1；***的评价指标为：调整时间t_s和超调量σ％；规定：

t_s＜3s,σ％＜5％其中，规定误差带为±0.05   (2)

为满足***要求的性能指标，若满足则结束设计，若不满足则继续调整控制决策部分。

进一步，，所述步骤4)中，农用车辆运动学模型采用三阶线性模型和三阶非线性模型，其中线性模型是为了便于分析非线性模型提出的；根据公知技术，三阶线性模型可以用状态方程描述，如下：

$\left[\begin{array}{c}\stackrel{\·}{y}\\ \stackrel{\·}{\mathrm{\Ψ}}\\ \stackrel{\·}{\mathrm{\δ}}\end{array}\right]=\left[\begin{array}{ccc}0& V_x& 0\\ 0& 0& \frac{V_x}{L}\\ 0& 0& 0\end{array}\right]\left[\begin{array}{c}y\\ \mathrm{\Ψ}\\ \mathrm{\δ}\end{array}\right]+\left[\begin{array}{c}0\\ 0\\ 1\end{array}\right]u---\left(3\right)$

$y=\left[\begin{array}{ccc}1& 0& 0\end{array}\right]\left[\begin{array}{c}y\\ \mathrm{\Ψ}\\ \mathrm{\δ}\end{array}\right]---\left(4\right)$

其中车辆速度V_x取1m/s，轴距L取2.314m；此时根据公知技术，三阶线性模型还可以用传递函数G(s)来描述：

$G\left(s\right)=\frac{Y\left(s\right)}{d\left(s\right)}=\frac{0.432}{s^3}---\left(5\right)$

利用获得的三阶线性模型进行控制器的设计，并将设计的控制器用于农用车辆三阶非线性原模型整定中去。

进一步，，所述步骤5)中，基于三阶线性模型，利用极点配置求出状态反馈矩阵，从而获得控制器G_c(s)。此时需判断三阶线性模型的能控性，能控性判断矩阵为：

$\mathrm{rank}\left[\begin{array}{cccc}B& \mathrm{AB}& A^2& B\end{array}\right]=\mathrm{rank}\left[\begin{array}{ccc}0& 0& \frac{{V_x}^2}{L}\\ 0& \frac{V_x}{L}& 0\\ 1& 0& 0\end{array}\right]=3---\left(6\right)$

根据公知技术，此时***是完全能控的，可以通过构造状态反馈矩阵K，实现***极点的任意配置；此时规定***期望的闭环极点为：

λ₁ ^*＝-5，λ^* _2,3＝-1±j    (7)

求得的状态反馈矩阵K为：

K＝[23.148 27.778 7]    (8)

极点配置后，***的控制器G_c(s)为：

$G_c\left(s\right)=\frac{{10s}^2}{0.432s^2+3.024s+5.184}---\left(9\right)$

此时三阶***模型在控制器G_c(s)整定下的性能指标为：

t_s＝2.3s,σ％＝4.2％    。(10)

进一步，，所述步骤6)中，农用车辆自主导航转向控制中不可避免的存在：模型误差Δ(s)，外界干扰d，测量噪声ζ；根据公知技术，此时需重点讨论外界干扰d和测量噪声ζ对输出y的影响，

$y_1=G_{\mathrm{uy}}\left(s\right)u=\frac{G_P\left(s\right)G_n\left(s\right)}{G_n\left(s\right)+[G_P\left(s\right)-G_n\left(s\right)]Q\left(s\right)}u---\left(11\right)$

$y_2=G_{\mathrm{dy}}\left(s\right)d=\frac{G_P\left(s\right)G_n\left(s\right)[1-Q\left(s\right)]}{G_n\left(s\right)+[G_P\left(s\right)-G_n\left(s\right)]Q\left(s\right)}d---\left(12\right)$

$y_3=G_{\mathrm{zy}}\left(s\right)z=\frac{G_P\left(s\right)Q\left(s\right)}{G_n\left(s\right)+[G_P\left(s\right)-G_n\left(s\right)]Q\left(s\right)}z---\left(13\right)$

y＝y₁+y₂+y₃＝G_uy(s)u+G_dy(s)d+G_zy(s)z   (14)

由公知技术可知：G_p(s)为***实际模型，G_n(s)为***的标称模型；式(11)表示***期望输入对***输出的影响，式(12)表示外部干扰对***输出的影响，式(13)表示测量噪声对***输出的影响；此时需：y₂,y₃均为0，才能保证y＝y₁，这样便可以保证***不受外部扰动和测量扰动的影响；采用扰动观测器Q(s)的技术手段来实现这个目的，并且Q(s)可以保证***的性能和鲁棒稳定性。

进一步，所述步骤7)中，农用车辆三阶非线性原始模型，由于本身存在的非线性，导致了此时***本身存在模型误差Δ(s)，首先解决模型误差给***带来的影响；由权利要求6可知单纯利用控制器G_c(s)，无法实现***的镇定；此时需要采用扰动观测器Q(s)这个技术手段来克服***模型误差Δ(s)对***造成的影响；根据公知技术，扰动观测器Q(s)一般采用巴特沃思型Q_B(s)和二项系数型Q_b(s)：

$Q_B\left(s\right)=\frac{P_{n-k}\left(s\right)}{P_n\left(s\right)}---\left(15\right)$

此时取截止频率：10Hz，50Hz，150Hz；根据公知技术，结合鲁棒稳定性和干扰抑制及设计成本的考虑，取截止频率为50Hz，时间常数为0.003s；此时Q_B(s)和Q_b(s)分别为：

$Q_B\left(s\right)=\frac{1}{{0.003}^3{s}^3+2*{0.003}^2{s}^2+2*0.003s+1}---\left(17\right)$

$Q_b\left(s\right)=\frac{1}{{0.003}^3{s}^3+3*{0.003}^2{s}^2+3*0.003s+1}---\left(18\right)$

在设计后的扰动观测器Q(s)和控制器G_c(s)共同作用下，农用车辆三阶原始模型的模型误差Δ(s)得到了有效地克服；两种扰动观测器下，***的性能指标分别为：

t_s＝2.32s,σ％＝2.1％(Q(s)＝Q_B(s))  (19)

t_s＝2.32s,σ％＝4.2％(Q(s)＝Q_b(s))  (20)

下面就要在扰动观测器Q(s)和控制器G_c(s)共同作用的前提下，讨论模型误差Δ(s)、外部干扰d、测量噪声ζ的不同形式以及不同组合对***可能造成的影响。由三阶非线性模型可知，模型误差Δ(s)是本质存在的；外部干扰d典型的存在方式有三种：

低频正弦外部干扰：

单位脉冲外部干扰：d₂＝δ(t)      (22)

随机外部干扰：d₃，mean＝0,variance＝0.5     (23)

测量噪声ζ典型的存在方式为高频测量噪声：

此时在巴特沃思型扰动观测器Q_B(s)和控制器G_c(s)共同作用下，***的性能均达不到：t_s＜3s,σ％＜5％的要求，而在二项系数型扰动观测器Q_b(s)和控制器G_c(s)共同作用下，***的性能均为：

t_s＝2.34s,σ％＝4.3％       (25)

此种技术手段，可以保证y＝u＝1；加入扰动观测器Q(s)的设计，不但可以克服***本身的模型误差，还可以使***对于外界干扰和测量噪声具有主动补偿的能力。同时值得指出的是，它有助于改善***的性能指标与鲁棒特性，因此基于农用车辆自主导航的转向控制方法技术可行，并且符合车辆导航的实际情况。

所述的方法包括：农用车辆路径跟踪闭环控制***的搭建，通过误差的不断修正，闭环校正，消除累积误差，直到精准跟踪，实现无静差跟踪的目的。采用的车辆模型搭建方法包括：由三阶的非线性车辆运动模型、三阶的线性车辆运动模型，这两种模型是基于O’Connor等人的研究确定的，经过大量的试验数据和仿真说明了该***模型的有效性。因此两种模型均稳定可靠，适合作为***分析的依据。针对三阶线性***，采用状态反馈的方法，通过极点配置的方式，实现了***等效控制器的设计；利用等效控制器和扰动观测器，实现了三阶非线性***控制效果的镇定。同时针对非线性原始模型设计抗干扰技术：包括闭环***本身抑制干扰以及基于主动抗干扰技术设计的扰动观测器。综合分析设计***的控制器和扰动观测器是本发明的关键所在。

本发明的另一目的在于，提供一种农用车辆路径跟踪控制装置，其由主控制器和闭环跟踪传感器两部分组成。主控制器包括：DSP控制电路、RTC实时时钟、数字信号输入输出单元、模拟信号输入单元、信号放大电路、外部存储器、人机交互装置和显示装置、看门狗电路、异步电机、一路串口通信接口与监控计算机通信，一路串口通信接口与转向轮偏角编码器通信。闭环跟踪传感器将实际的横向偏差信号转换成电信号，同给定的转向轮偏角信号作差，并通过信号放大电路放大电信号，经过模拟信号输入单元，由DSP控制电路采集。

与现有设计相比，本发明有如下优点：(1)结合了开、闭环控制的优点，即不会出现累积误差，也不会因车辆运行实际的干扰导致跟踪***的失效。(2)多模型化设计，针对不同的控制对象和模型，采取对应的控制策略，确保了***的动态品质和稳态性能，跟踪控制算法可以保证***调整时间均小于3s，超调量不超过5％,最后实现无静差跟踪。(3)加入多种干扰对***进行设计，提出的扰动观测器设计，可以有效抑制这些集总干扰，确保***的性能不受扰动的影响。(4)采用DSP作为处理器，利用其为复杂的控制算法和强大的运算速度提供技术支撑，提高了农用车辆路径跟踪的精度和速度。(5)多模型多功能多模块化处理，***便于集成控制，可以实现进一步的商业化，有效转变为生产力。(6)采用主动补偿干扰的控制思想，拓宽了控制***的设计维度，增加了***的主观能动性。

附图说明

图1为本发明控制原理设计流程图。

图2为农用车辆路径跟踪闭环控制***的设计图。

图3为三阶模型的物理原型图。

图4为农用车辆三阶运动的状态方程。

图5为极点配置后闭环***的内部结构原理图。

图6为三阶线性***在极点配置下的***仿真图。

图7为三阶非线性***在极点配置控制器作用下的***仿真图。

图8和图9为扰动观测器的基本原理图。

图10和图11为两种滤波器在三种截至频率下的频率曲线图。

图12为三阶非线性***在两种滤波器结构下的仿真对比图。

图13和图14及图15引入三种外部干扰的三阶非线性***在两种扰动观测器下的仿真效果对比图。

图16、图17、图18、图19为引入外部干扰和测量噪声的三阶非线性***在两种扰动观测器作用下的仿真效果对比图。

图20为农用车辆自主导航路径跟踪装置的设计图。

具体实施方式

下面结合附图说明本发明的具体实施方式。

鉴于研究的农用车辆自主导航控制***模型本质上是非线性的，所以首先讨论三阶线性***的控制方案。通过对三阶线性模型的分析和设计来对非线性原始模型进行优化研究。三阶***分为非线性模型和线性模型，设计的思想在于线性模型下设计的控制器和观测器同样可以用于非线性原始模型。

三阶线性***模型如图3和图4所示，根据状态空间表达，利用状态反馈进行极点配置：拖拉机的运动方程如下

$\left[\begin{array}{c}\stackrel{\·}{y}\\ \stackrel{\·}{\mathrm{\Ψ}}\\ \stackrel{\·}{\mathrm{\δ}}\end{array}\right]=\left[\begin{array}{ccc}0& V_x& 0\\ 0& 0& \frac{V_x}{L}\\ 0& 0& 0\end{array}\right]\left[\begin{array}{c}y\\ \mathrm{\Ψ}\\ \mathrm{\δ}\end{array}\right]+\left[\begin{array}{c}0\\ 0\\ 1\end{array}\right]u$

$y=\left[\begin{array}{ccc}1& 0& 0\end{array}\right]\left[\begin{array}{c}y\\ \mathrm{\Ψ}\\ \mathrm{\δ}\end{array}\right]$

***矩阵 $A=\left[\begin{array}{ccc}0& V_x& 0\\ 0& 0& \frac{V_x}{L}\\ 0& 0& 0\end{array}\right],$ 输入矩阵 $B=\left[\begin{array}{c}0\\ 0\\ 1\end{array}\right].$ 判断该***的能控性。求状态反馈增益矩阵K，使反馈闭环特征值为l₁ ^*＝-5，l^* _2,3＝-1±j，选取的主导极点是标准二阶***下性能最优的。

解:(1) $\mathrm{rank}\left[\begin{array}{cccc}B& \mathrm{AB}& A^2& B\end{array}\right]=\mathrm{rank}\left[\begin{array}{ccc}0& 0& \frac{{V_x}^2}{L}\\ 0& \frac{V_x}{L}& 0\\ 1& 0& 0\end{array}\right]=3,$ 可见给定的***是完全能控的，通过状态反馈控制律u＝v-Kx能任意配置闭环特征值。下面求状态反馈增益矩阵K。

(2)确定闭环***的希望特征多项式

闭环***的希望极点是λ₁ ^*＝-5，λ^* _2,3＝-1±j，由此可以确定闭环***的希望特征多项式：

f^*(s)＝(s-l₁ ^*)(s-l₂ ^*)(s-l₃ ^*)＝(s+5)(s+1+j)(s+1-j)＝s³+7s²+12s+10

(3)确定完成极点配置任务的状态反馈矩阵

设状态反馈矩阵为

K＝[k₁ k₂ k₃]

得闭环特征多项式

f(s)＝det(sI-A+BK)

$f\left(s\right)=\frac{{\mathrm{Ls}}^3+k_3*{\mathrm{Ls}}^2+k_2{V}_{x}s+k_1{V_x}^2}{L}$

这里采用东方红-X804拖拉机的参数，即轴距取L＝2.314m，速度取

V_x＝1m/s。同时令f(s)＝f^*(s)，由s的同次幂系数相等得:

k₁＝23.148，k₂＝27.778，k₃＝7

(4)K＝[23.148 27.778 7]

于是状态反馈增益矩阵K可求出。此时闭环***的状态空间表达式为：

$\begin{array}{c}\stackrel{\·}{x}=(A-\mathrm{BK})x+\mathrm{Bv}\\ =\{\left[\begin{array}{ccc}0& 1& 0\\ 0& 0& 0.432\\ 0& 0& 0\end{array}\right]-\left[\begin{array}{c}0\\ 0\\ 1\end{array}\right]\left[\begin{array}{ccc}23.148& 27.778& 7\end{array}\right]\}x+\left[\begin{array}{c}0\\ 0\\ 1\end{array}\right]v\\ =\left[\begin{array}{ccc}0& 1& 0\\ 0& 0& 0.432\\ -23.148& -27.778& -7\end{array}\right]x+\left[\begin{array}{c}0\\ 0\\ 1\end{array}\right]v\end{array}$

y＝[1 0 0]x

可以验证闭环***的特征值的确为λ₁ ^*＝-5，λ^* _2,3＝-1±j。而闭环传递函数矩阵为：G(s)＝C[sI-A+BK]^-1B

$\frac{0.432}{s^3+{7s}^2+12s+10}$

此时***内部结构原理图如图5所示。

注：由***的内部结构原理图可知，***经历了二阶超前校正的过程，幅值和能量衰减为0.0432，即此时***的稳态值为0.0432，为了仿真分析的方便，在整个闭环***前串入一个功率放大环节，由计算可知放大环节系数为23.148，确保***的稳态值为1。即此时的单位闭环***传递函数为：

$\frac{10}{s^3+{7s}^2+12s+10}$

闭环三阶线性***状态反馈镇定后的时域效果通过仿真可知：***调整调整时间为2.3s，超调量为4.2％。三阶线性闭环***的单位阶跃响应如图6所示。

根据极点配置的闭环***以及开环传递函数，可以求出控制器的具体结构和参数。

令***的控制器为G_c(s)，开环传递函数为C，闭环传递函数为X。则有

$\frac{{\mathrm{CG}}_c\left(s\right)}{1+{\mathrm{CG}}_c\left(s\right)}=X$

则控制器的结果为：

$G_c\left(s\right)=\frac{X}{C-\mathrm{CX}}$

已知X为:

$\frac{10}{s^3+{7s}^2+12s+10}$

C为:

$G\left(s\right)=\frac{Y\left(s\right)}{d\left(s\right)}=\frac{0.432}{s^3}$

则控制器G_c(s)的结构形式为：

$\frac{10}{{0.432s}^2+3.024s+5.184}$

利用对三阶线性***极点配置的等效控制器，对三阶非线性***进行控制效果的镇定，由于非线性***受初值条件的影响，控制器对非线性***的控制效果存在不稳定性。等效控制器对三阶非线性***的时域仿真效果图如图7所示。

比较分析：三阶线性***下设计的等效控制器对于三阶非线性***不具有同样优越的控制效果，仅靠等效的控制器不足以保证非线性***模型的动态品质和稳态性能。因此有必要讨论新的技术手段来实现非线性***的进一步整定。下文引入扰动观测器来克服非线性模型误差给***带来的影响。

原因分析：三阶非线性模型本质上是存在***误差，而这种误差导致了***控制品质的不确定及不稳定性。这种模型误差要求引入下文的技术手段来克服。

干扰主动控制根据对干扰的测量值或者估计值进行针对性的前馈补偿设计。其特点在于能够对未建模动态，模型参数摄动，外部干扰，测量噪声这些集总干扰全部考虑在内进行估计并最终实现补偿设计。并且其引入前馈，相对于反馈，其可以更快的抑制干扰，所以干扰主动控制引起了许多研究者极大的研究兴趣。其研究的难点便在于本身的测量和估计。

扰动观测器是一种有效的干扰估计测量方法，它的出现极大地拓宽了前馈控制—干扰主动控制的应用范围，其使得不可测、不易测的扰动抑制成为可能。对提高***的抗扰性能具有极大的裨益。

扰动观测器的基本结构和原理如图8和图9所示，其核心在于实际模型，名义模型，滤波器的选取和设计，针对上述的三阶线性***：

实际模型为： $G\left(s\right)=\frac{Y\left(s\right)}{d\left(s\right)}=\frac{0.432}{s^3},$ 名义模型： $G\left(s\right)=\frac{Y\left(s\right)}{d\left(s\right)}=\frac{0.432}{{1.003s}^3}$

滤波器的鲁棒稳定性条件为：||Δ(s)Q(s)||_∞≤1，滤波器常采用巴特沃思型和二项系数型，具体结构如下：

$Q_B\left(s\right)=\frac{P_{n-k}\left(s\right)}{P_n\left(s\right)}$

根据名义模型可知两种滤波器的结构为：

$Q_B\left(s\right)=\frac{1}{t^3{s}^3+2t^2{s}^2+2\mathrm{ts}+1}$

$Q_b\left(s\right)=\frac{1}{t^3{s}^3+3t^2{s}^2+3\mathrm{ts}+1}$

稳定性条件||Δ(s)Q(s)||_∞≤1，取截止频率10Hz、50Hz、150Hz来判定两种滤波器的稳定性条件，通过图10和图11可见在三种截止频率下均满足稳定性条件。出于干扰抑制与设计成本的考虑，取截止频率为50Hz。此时两种滤波器分别为：

$Q_B\left(s\right)=\frac{1}{{0.003}^3{s}^3+2*{0.003}^2{s}^2+2*0.003s+1}$

$Q_b\left(s\right)=\frac{1}{{0.003}^3{s}^3+3*{0.003}^2{s}^2+3*0.003s+1}$

上文中利用线性模型下设计的控制器不能够实现非线性模型的有效镇定，所以根据求得的两种滤波器对三阶非线性***进行仿真，两种滤波器结构下的***仿真效果对比如图12所示。在巴特沃思型滤波器作用下，非线性***模型的时域指标为：调整时间为2.32s，超调量为2.1％，稳态值为0.98；在二项系数型滤波器作用下，非线性***模型的时域指标为：调整时间为2.32s，超调量为4.2％，稳态值为1。可见非线性***模型在两种扰动观测器作用下，非线性***本身存在的模型误差得到了有效地克服，非线性***的动态品质和稳态性能得到了充分的保证。

实际的农用车辆自主导航控制***不可避免地受到外部干扰和传感器测量噪声及车辆本身颤动的影响。所以有必要把这些干扰引入到研究的车辆模型当中去，通过对这些干扰进行不同的组合，来研究两种扰动观测器对这些扰动的抑制效果，得出一些具体的结论。

这些扰动大致由三部分构成：车辆的模型误差，如车辆的抖动与车辆模型的建立带来的误差；农业作业环境的外部干扰：土壤的结构和软硬程度、锯齿形和近似为正弦形的路况、随机信号、地面积水和低洼、地形的坡度、车辆和障碍物的瞬间碰撞、车辆的翻滚和俯仰、外部环境的变化(风吹、日晒、雨淋)；传感器的测量噪声：电磁干扰、传感器融合和测量带来的噪声。已有的控制理论与技术手段都是通过保守且高成本的方式用能量压制或抵消这些扰动对***的影响。本发明旨在通过主动干扰补偿的思想，通过扰动观测器的设计去主动补偿这些扰动。

农用车辆的工作环境极为复杂，一般我们用低频正弦信号、随机信号、脉冲信号去模拟外部干扰对***造成的影响。并且在两种滤波器下，模拟外部扰动对***的影响，***仿真效果对比如图13，图14，图15所示。

实际的物理环境中，除了上述的外部干扰，在传感器测量与融合的过程中存在一系列噪声，通常用高频正弦测量噪声、电磁干扰、高斯测量噪声来模拟测量噪声对***造成的影响。农用车辆实际工作时，模型误差、农业环境外部干扰、传感器测量噪声是同时作用的。因此有必要将这些模拟信号进行有机组合，并且把他们加入到影响***的对应区域，在两种扰动观测器作用下，研究***具体的性能。

几种组合分别为：外部低频正弦干扰+模型误差+高频正弦测量噪声；外部单位脉冲信号+模型误差+高频正弦测量噪声；外部随机信号+模型误差+高频正弦测量噪声；模型误差+高频正弦测量噪声。在这些扰动的有机组合下，非线性***的时域仿真效果图如图16，图17，图18，图19所示。

比较分析：通过上文的仿真效果对比图可以看出，在应对三阶非线性***的模型误差时，巴特沃思型滤波器和二项系数型滤波器表现出了优越的控制效果。性能指标：调整时间均为2.32s；超调量分别为2.1％，4.2％。在应对模型误差和农业环境外部干扰共同作用时，两种扰动观测器的表现呈现出了差异性。巴特沃思型滤波器下的***响应曲线无法达到期望的稳态值，不具有实际的价值，而二项系数型滤波器下的***响应曲线达到预期的控制效果。在应对模型误差和农业环境外部干扰及传感器测量噪声的有机组合时，二项系数型滤波器的表现仍能够主动补偿这三类扰动给***带来的影响。巴特沃思型滤波器作用下的***响应曲线仍无法达到期望的稳态值。

综合比较这两类扰动观测器，对于农用车辆自主导航控制***而言，选择二项系数型的扰动观测器更符合农业作业环境的实际。其对非线性原始模型的控制与设计也更具一般性与稳定性。

本发明的另一目的在于，提供一种农用车辆路径跟踪控制装置，其由主控制器和闭环跟踪传感器两部分组成。主控制器包括：DSP控制电路、RTC实时时钟、数字信号输入输出单元、模拟信号输入单元、信号放大电路、外部存储器、人机交互装置和显示装置、看门狗电路、异步电机、一路串口通信接口与监控计算机通信，一路串口通信接口与转向轮偏角编码器通信。具体的模型结构如图20所示。达到硬件设计和软件控制的双重效果。采用RS485接口为串口通信接口，键盘为人机界面装置，液晶(LCD)为显示装置。闭环跟踪传感器其电路连接方式是：其采集农用车辆路径跟踪时的横向偏差，与给定的转向轮偏角信号作差，并通过信号放大电路将电信号放大，DSP控制电路检测电信号，通过发明中提供的控制算法，驱动电机转动。监控计算机和转向轮偏角编码器通过光电隔离RS485接口与控制器通信。外部存储器通过DSP总线与DSP控制电路相连。键盘包括数字输入、小数点输入、上下翻页、确认、取消等功能。LCD显示液晶菜单，连接DSP控制电路的输出引脚。

以上具体实施方式仅用于说明本发明，而非用于限定本发明。

Claims (8)

1.一种基于扰动观测器的农用车辆自主导航转向控制方法，其特征在于，包括如下几个步骤：

1)首先根据农用车辆实际导航需求的情况，拟定农用车辆路径跟踪控制***的控制流程设计图；

2)组建农用车辆路径跟踪控制***，由转向控制器G_c(s)，扰动观测器Q(s)和车辆运动学模型G(s)这几个部分构成；

3)结合实际的控制需求，设定农用车辆路径跟踪控制***的输入为控制变量u，输出为横向偏差y，此时设定期望输出量y₁＝u＝1；

4)确定农用车辆路径跟踪控制***的研究对象：农用车辆的运动学模型；此时农用车辆的运动学模型取O’Connor提出的三阶线性模型和三阶非线性模型，其中对于三阶线性的模型处理是为了便于对三阶非线性模型的研究；

5)基于三阶线性模型，利用极点配置设计***的控制器G_c(s)，并将此控器用于三阶非线性模型的控制与整定；

6)在车辆三阶非线性模型基础上，引入***本质存在的三类扰动：模型误差Δ(s)，外界干扰d，测量噪声ζ，同时对这三种扰动进行不同组合的模拟，分析它们对农用车辆路径跟踪控制***的影响；

7)在步骤5设计的控制器G_c(s)基础上，加入扰动观测器Q(s)的设计，此时设计两种扰动观测器，分别为Q_B(s)和Q_b(s)，在这两种控制手段的作用下，讨论***对外界干扰的主动抑制能力，并且对比分析两种扰动观测器的控制效果；

8)利用DSP与现有的农用车辆平台，构建一种农用车辆路径跟踪控制装置：包括主控制器和路径跟踪闭环传感器以及农用车辆硬件平台。

2.根据权利要求1所述的一种基于扰动观测器的农用车辆自主导航转向控制方法，其特征在于，所述步骤1)中，农用车辆路径跟踪控制***的控制流程设计图具体为：首先设定车辆跟踪目标y₁和确定控制输入u，其次选取控制决策设计控制器G_c(s)和扰动观测器Q(s)；然后农用车辆开始实际运动，确定实际的跟踪效果y，同时将车辆的实际运动效果与期望目标作比较：u-y；此时***开始性能的综合分析：分别计算调整时间t_s和超调量σ％，若满足***要求，则设计工作结束；若不满足***要求，则继续调整控制决策部分，直至满足***要求为止。

3.根据权利要求1所述的一种基于扰动观测器的农用车辆自主导航转向控制方法，其特征在于，所述步骤2)中，农用车辆路径跟踪控制***由三部分组成：控制器G_c(s)、扰动观测器Q(s)、农用车辆运动学模型G(s)；根据公知技术，农用车辆运动学模型由执行机构与被控对象两部分组成，此时输出横向偏差y和输入u的推导关系如下：

$y=\frac{G_c\left(s\right)G\left(s\right)}{1+G_c\left(s\right)G\left(s\right)}u---\left(1\right)$

即控制输入u和输出横向偏差y的关系是基于信号传递函数的数学关系式，控制器G_c(s)的决定了输入和输出两者的具体关系。

4.根据权利要求1所述的一种基于扰动观测器的农用车辆自主导航转向控制方法，其特征在于，所述步骤3)中，***的控制输入为u，***的输出为横向偏差y，期望输出y₁＝u＝1；***的评价指标为：调整时间t_s和超调量σ％；规定：

t_s＜3s,σ％＜5％其中，规定误差带为±0.05   (2)

为满足***要求的性能指标，若满足则结束设计，若不满足则继续调整控制决策部分。

5.根据权利要求1所述的一种基于扰动观测器的农用车辆自主导航转向控制方法，其特征在于，所述步骤4)中，农用车辆运动学模型采用三阶线性模型和三阶非线性模型，其中线性模型是为了便于分析非线性模型提出的；根据公知技术，三阶线性模型可以用状态方程描述，如下：

$\left[\begin{array}{c}\stackrel{\·}{y}\\ \stackrel{\·}{\mathrm{\Ψ}}\\ \stackrel{\·}{\mathrm{\δ}}\end{array}\right]=\left[\begin{array}{ccc}0& V_x& 0\\ 0& 0& \frac{V_x}{L}\\ 0& 0& 0\end{array}\right]\left[\begin{array}{c}y\\ \mathrm{\Ψ}\\ \mathrm{\δ}\end{array}\right]+\left[\begin{array}{c}0\\ 0\\ 1\end{array}\right]u---\left(3\right)$

$y=\left[\begin{array}{ccc}1& 0& 0\end{array}\right]\left[\begin{array}{c}y\\ \mathrm{\Ψ}\\ \mathrm{\δ}\end{array}\right]---\left(4\right)$

其中车辆速度V_x取1m/s，轴距L取2.314m；此时根据公知技术，三阶线性模型还可以用传递函数G(s)来描述：

$G\left(s\right)=\frac{Y\left(s\right)}{d\left(s\right)}=\frac{0.432}{s^3}---\left(5\right)$

利用获得的三阶线性模型进行控制器的设计，并将设计的控制器用于农用车辆三阶非线性原模型整定中去。

6.根据权利要求1所述的一种基于扰动观测器的农用车辆自主导航转向控制方法，其特征在于，所述步骤5)中，基于三阶线性模型，利用极点配置求出状态反馈矩阵，从而获得控制器G_c(s)。此时需判断三阶线性模型的能控性，能控性判断矩阵为：

$\mathrm{rank}\left[\begin{array}{cccc}B& \mathrm{AB}& A^2& B\end{array}\right]=\mathrm{rank}\left[\begin{array}{ccc}0& 0& \frac{{V_x}^2}{L}\\ 0& \frac{V_x}{L}& 0\\ 1& 0& 0\end{array}\right]=3---\left(6\right)$

根据公知技术，此时***是完全能控的，可以通过构造状态反馈矩阵K，实现***极点的任意配置；此时规定***期望的闭环极点为：

λ₁ ^*＝-5，λ^* _2,3＝-1±j         (7)

求得的状态反馈矩阵K为：

K＝[23.148 27.778 7]       (8)

极点配置后，***的控制器G_c(s)为：

$G_c\left(s\right)=\frac{10s^2}{0.432s^2+3.024s+5.184}---\left(9\right)$

此时三阶***模型在控制器G_c(s)整定下的性能指标为：

t_s＝2.3s,σ％＝4.2％。    (10)

7.根据权利要求1所述的一种基于扰动观测器的农用车辆自主导航转向控制方法，其特征在于，所述步骤6)中，农用车辆自主导航转向控制中不可避免的存在：模型误差Δ(s)，外界干扰d，测量噪声ζ；根据公知技术，此时需重点讨论外界干扰d和测量噪声ζ对输出y的影响，

$y_1=G_{\mathrm{uy}}\left(s\right)u=\frac{G_P\left(s\right)G_n\left(s\right)}{G_n\left(s\right)+[G_P\left(s\right)-G_n\left(s\right)]Q\left(s\right)}u---\left(11\right)$

$y_2=G_{\mathrm{dy}}\left(s\right)d=\frac{G_P\left(s\right)G_n\left(s\right)[1-Q\left(s\right)]}{G_n\left(s\right)+[G_P\left(s\right)-G_n\left(s\right)]Q\left(s\right)}d---\left(12\right)$

$y_3=G_{\mathrm{zy}}\left(s\right)z=\frac{G_P\left(s\right)Q\left(s\right)}{G_n\left(s\right)+[G_P\left(s\right)-G_n\left(s\right)]Q\left(s\right)}z---\left(13\right)$

y＝y₁+y₂+y₃＝G_uy(s)u+G_dy(s)d+G_Zy(s)Z       (14)

由公知技术可知：G_p(s)为***实际模型，G_n(s)为***的标称模型；式(11)表示***期望输入对***输出的影响，式(12)表示外部干扰对***输出的影响，式(13)表示测量噪声对***输出的影响；此时需：y₂,y₃均为0，才能使y＝y₁，这样便可以保证***不受外部扰动和测量扰动的影响；采用扰动观测器Q(s)的技术手段来实现这个目的，并且Q(s)可以保证***的性能和鲁棒稳定性。

8.根据权利要求1所述的一种基于扰动观测器的农用车辆自主导航转向控制方法，其特征在于，所述步骤7)中，农用车辆三阶非线性原始模型，由于本身存在的非线性，导致了此时***本身存在模型误差Δ(s)，首先解决模型误差给***带来的影响；由权利要求6可知单纯利用控制器G_c(s)，无法实现***的镇定；此时需要采用扰动观测器Q(s)这个技术手段来克服***模型误差Δ(s)对***造成的影响；根据公知技术，扰动观测器Q(s)一般采用巴特沃思型Q_B(s)和二项系数型Q_b(s)：

$Q_B\left(s\right)=\frac{P_{n-k}\left(s\right)}{P_n\left(s\right)}---\left(15\right)$

此时取截止频率：10Hz，50Hz，150Hz；根据公知技术，结合鲁棒稳定性和干扰抑制及设计成本的考虑，取截止频率为50Hz，时间常数为0.003s；此时Q_B(s)和Q_b(s)分别为：

$Q_B\left(s\right)=\frac{1}{{0.003}^3{s}^3+2*{0.003}^2{s}^2+2*0.003s+1}---\left(17\right)$

$Q_b\left(s\right)=\frac{1}{{0.003}^3{s}^3+3*{0.003}^2{s}^2+3*0.003s+1}---\left(18\right)$

在设计后的扰动观测器Q(s)和控制器G_c(s)共同作用下，农用车辆三阶原始模型的模型误差Δ(s)得到了有效地克服；两种扰动观测器下，***的性能指标分别为：

t_s＝2.32s,σ％＝2.1％(Q(s)＝Q_B(s))  (19)

t_s＝2.32s,σ％＝4.2％(Q(s)＝Q_b(s))  (20)

下面就要在扰动观测器Q(s)和控制器G_c(s)共同作用的前提下，讨论模型误差Δ(s)、外部干扰d、测量噪声ζ的不同形式以及不同组合对***可能造成的影响。由三阶非线性模型可知，模型误差Δ(s)是本质存在的；外部干扰d典型的存在方式有三种：

低频正弦外部干扰：

单位脉冲外部干扰：d₂＝δ(t)                  (22)

随机外部干扰：d₃，mean＝0,variance＝0.5           (23)

测量噪声ζ典型的存在方式为高频测量噪声：

此时在巴特沃思型扰动观测器Q_B(s)和控制器G_c(s)共同作用下，***的性能均达不到：t_s＜3s,σ％＜5％的要求，而在二项系数型扰动观测器Q_b(s)和控制器G_c(s)共同作用下，***的性能均为：

t_s＝2.34s,σ％＝4.3％              (25)

此种技术手段，可以保证y＝u＝1；加入扰动观测器Q(s)的设计，不但可以克服***本身的模型误差，还可以使***对于外界干扰和测量噪声具有主动补偿的能力。同时值得指出的是，它有助于改善***的性能指标与鲁棒特性，因此基于农用车辆自主导航的转向控制方法技术可行，并且符合车辆导航的实际情况。