2.5 导航路径跟踪控制算法

常见的导航路径跟踪控制算法有线性模型、PID控制、最优控制、模糊控制、神经网络及纯追踪模型等。线性模型具有算法简单、控制参数易于优化的特点，但是它无法取消稳态误差，稳健性差；PID控制作为常见控制方法，具有算法简单的特点，它稳健性较强，可以消除稳态误差，但是控制参数优化比较困难；最优控制方法可直接解算获得较优的控制参数，但是需要精确的运动学和动力学模型参数，对曲线跟踪的适应性差；模糊控制无须被控对象的模型参数，稳健性较强，但是其跟踪误差一般比较大，难以快速修正；神经网络对非线性运动特性具有较好的适应性，但是它需要获得大量高质量的训练和验证样本对，泛化能力较弱；纯追踪模型可以从几何的角度直观理解和推导，仅需调节前视距离参数，同时前视距离的选择也直接影响控制精度。农机作业环境不确定因素的存在，以及农机本身的复杂性，对农机导航控制都是不小的挑战，具有较大的影响，单一模式的导航控制器很难达到理想的控制效果。

2.5.1 模糊控制导航算法

美国教授查德（L.A.Zandeh）于1965年首先提出了模糊集合的概念，由此开创了模糊数学及其应用的新纪元。模糊控制是模糊集合理论应用的一个重要方面。模糊控制在一定程度上模仿了人的控制，它不需要有准确的控制对象模型，是一种智能控制方法。近年来，模糊控制和遗传算法、神经网络控制及混沌理论等学科相融合显示出了巨大的应用潜力。

模糊控制器的基本结构如图2-28所示，主要具备以下功能。

（1）模糊化。这部分的作用是将输入的精确量转换成模糊化量。其中输入量包括外界的参考输入、系统的输出或状态等。模糊化的具体过程如下：首先，对输入量进行处理，以变换成模糊控制器要求的输入量；其次，将处理过的输入量进行尺度变换，使其变换到各自的论域范围；最后，将已经变换到论域范围的输入量进行模糊处理，使原先精确的输入量变换成模糊量，并用相应的模糊集合来表示。

（2）知识库。知识库包含具体应用领域的知识和要求的控制目标。它通常由数据库和模糊控制规则库两部分组成。数据库主要包括各语言变量的隶属度函数、尺度变换因子及模糊空间的分级数等。模糊控制规则库包括用模糊语言变量表示的一系列控制规则，它们反映了控制专家的经验和知识。

（3）模糊推理。模糊推理是模糊控制器的核心，具有模拟人的基于模糊概念的推理能力。该推理过程是基于模糊逻辑中的蕴含关系及推理规则来进行的。

（4）清晰化。清晰化的作用是将模糊推理得到的控制量（模糊量）变换成实际用于控制的清晰量。它包含两部分内容，一是将模糊的控制量经清晰化变换成在论域范围的清晰量，二是将在论域范围内的清晰量经尺度变换变成实际的控制量。

模糊控制器的设计流程如下。

1.确定模糊控制器的输入变量和输出变量（即控制量）

通常，将模糊控制器输入变量的个数称为模糊控制器的维数。从理论上讲，模糊控制器的维数越高，控制越精细。但如果维数过高，模糊控制规则就变得过于复杂，控制算法实现相对困难。本书选用广泛流行的二维模糊控制器，取横向偏差 P _e 和航向偏差 Ψ _e 作为输入量，输出为期望前轮转角 δ _d 。根据先验知识，将 P _e 的实际论域（m）设置为[-0.6，0.6]； Ψ _e 的实际论域（度）取[-90，90]； δ _d 的实际论域（度）取[-30，30]。

2.设计模糊控制器的控制规则

控制规则的设计是设计模糊控制器的关键，一般包括三个部分的设计内容。

1）选择输入输出变量的词集

一般选用“大、中、小”3个词汇来描述模糊控制器的输入输出变量的状态。由于人的行为在正、负两个方向的判断基本上是对称的，所以，将大、中、小加上正、负两个方向并考虑变量的零状态，共有7个词汇，即{负大，负中，负小，零，正小，正中，正大}，用英文表示为{NB，NM，NS，O，PS，PM，PB}。为方便分析，做如下规定：当拖拉机定位点位于期望路径左侧时，横向偏差为负，反之为正；当拖拉机航向在期望路径的右侧时，航向角为负，反之为正。

2）定义各模糊变量的模糊子集

定义一个模糊子集，实际上就是要确定模糊子集隶属函数曲线的形状。将确定的隶属函数曲线离散化，即可得到有限个点上的隶属度，从而构成一个相应的模糊变量的子集。本书选用三角形隶属度函数。

3）建立模糊控制器的控制规则

模糊控制器的控制规则是基于手动控制策略，利用模糊集合理论和语言变量的概念，可以把利用语言归纳的手动控制策略上升为数值运算，于是可以利用计算机代替人的手动控制，实现模糊自动控制。利用语言归纳手动控制策略的过程，实际上就是建立模糊控制器的控制规则的过程。手动控制策略一般都可以用条件语句加以描述。建立模糊控制规则表的基本原则如下：当误差大或较大时，选择控制量以尽快消除误差为主；当误差较小时，选择控制量要注意防止超调，以系统的稳定性为主要出发点。综合考虑以上因素，以及实际驾驶经验设计模糊控制规则，应用MATLAB实现的模糊控制曲面如图2-29所示。

3.确定模糊化和清晰化的方法

将精确量转换为模糊量的过程称为模糊化，模糊化一般采用以下两种方法：

（1）将精确量离散化，把在某个区间范围内变化的连续量分成若干个档次，每个档次对应一个模糊集，这样处理使模糊过程简单。

（2）将在某区间的精确量模糊化转换成这样的一个模糊子集，在该精确量处隶属度为1，除该点外，其余各点的隶属度均取0。

对建立的模糊规则要经过模糊推理才能决策出控制变量的一个模糊子集，还需要采取合理的方法将模糊量转换为精确量，以便最好地发挥出模糊推理结果的决策效果。该模糊量转换为精确量的过程称为清晰化。清晰化的方法主要有最大隶属度法、中位数法及加权平均法等。本书选用最大隶属度法。

4.编制模糊控制算法的应用程序

考虑到导航控制实时性的要求，首先需要对连续的论域进行离散化，也称为量化。经过量化的输入量个数是有限的，因此，可以针对输入情况的不同组合离线计算出相应的控制量，从而组成一张控制表，实际控制时只需直接查这张控制表即可，论域为离散时的模糊控制系统结构如图2-30所示。这样就减少了在线的计算量，从而满足实时控制的要求。按照均匀量化的方法，将各实际论域离散化为{-6，-5，-4，-3，-2，-1，0，1，2，3，4，5，6}。图2-30中量化的功能是将比例变换后的连续值经四舍五入变换为整数量。 K ₁ 、 K ₂ 和 K ₃ 是尺度变换的比例因子，各参数的值如下：

模糊控制器的控制算法是由计算机的程序实现的。这个程序一般包括两部分：一是计算机离线计算查询表的程序，属于模糊矩阵运算；二是计算机在模糊控制过程中在线计算输入变量，并将它们模糊化处理，查表后再做输出处理的程序。

2.5.2 自适应模糊控制导航算法

上一节基于模糊逻辑设计了导航控制系统，模糊推理系统的设计不依靠对象的模型，比较适合表达那些模糊或定性的知识，其推理方式类似于人的思维模式。但是它缺乏自学习和自适应能力，依靠专家或操作人员的经验和知识，如果缺乏这样的经验，则很难获得满意的控制效果。另外，模糊控制在零位附近跟踪误差一般比较大，难以快速修正。因此，有必要研究自适应模糊控制方法，在控制过程中的不同阶段，对模糊控制器进行自动调整。

本节拟以模糊控制算法为基础，采用神经网络对模糊控制的规则修正因子及输出比例因子进行在线整定，以达到自适应控制的目的。自适应模糊导航控制原理结构图如图2-31所示，采用拖拉机的横向偏差 P _e 和航向偏差 ψ _e 作为模糊控制器的输入，输出为期望前轮转角，在线整定的参数 ω _ij 、 c _ij 、 σ _ij 分别指模糊神经网络最后一层的连接权，以及第二层的隶属度函数的中心值和宽度。

神经网络系统具有并行计算、分布式信息存储、容错能力强、具备自适应学习功能等一系列的优点，将模糊逻辑和神经网络适当结合起来，整合两者优势，可以组成比单独模糊逻辑控制系统性能更加优良的自适应模糊神经导航控制系统。

1.模糊系统的标准模型

图2-32所示为基于标准模型的MISO模糊系统的原理结构图，其中， x ∈ R ⁿ ， y ∈ R 。如果该模糊系统的输出作用于一个控制对象，那么它的作用便是一个模糊控制器；否则，它可用于模糊逻辑决策系统、模糊逻辑诊断系统等方面。

设输入量 x =[ x ₁ ， x ₂ ，…， x _n ] ^T 时，每个分量 x _i 均为模糊语言变量，并设

式中， 是 x _i 的第 j 个语言变量值，它是定义在论域 U _i 上的一个模糊集合，相应的隶属度函数为 。

输出量 y 也为模糊语言变量，且 ，其中 B ^j （ j =1，2，…， m _i ）是 y 的第 j 个语言变量值，它是定义在论域 U _y 上的模糊集合，相应的隶属度函数为 。

设描述输入输出量关系的模糊规则为

R _i ：如果 x ₁ 是 ， x ₂ 是 ，…， x _n 是 ，则 y 是 B ⁱ ，其中 i =1，2，…， m ， m 表示规则总数。

若输入量采用单点模糊集合的模糊化方法，则对于给定的输入 x ，可以求得对于每条规则的适用度为

通过模糊推理可得，对于每条模糊规则的输出量模糊集合 B _i 的隶属度函数为

从而输出量总的模糊集合为

若采用加权平均的清晰化方法，则可求得输出的清晰化量为

由于计算式（2-139）的积分很麻烦，所以，实际计算时通常用下面的近似公式：

其中， 是指 取最大值的点，它一般就是隶属度函数的中心点。显然

从而输出量的表达式可变为

其中

2.模糊神经网络的结构

根据上面给出的模糊系统的模糊模型，结合拖拉机自动导航控制的实际情况，可以设计出图2-33所示的基于标准模型的模糊神经网络结构。

图2-33中第一层为输入层。该层的各个节点直接与输入向量的各分量 x _i 连接，它起着将输入值 x =[ x ₁ x ₂ … x _n ] ^T 传送到下一层的作用。选用航向偏差和横向偏差作为控制器输入，该层节点数 N ₁ =2。

第二层的每个节点代表一个语言变量值，如NB、PS等。它的作用是计算各输入分量属于各语言变量值模糊集合的隶属度函数 ，其中

i =1，2，…， n ， j =1，2，…， m _i 。 n 是输入量的维数， m _i 是 x _i 的模糊分割数。此处隶属度函数采用钟形函数，则

式中， c _ij 和 σ _ij 分别表示隶属度函数的中心和宽度。该层的节点总数 。

第三层的每个节点代表一条模糊规则，其作用是用来匹配模糊规则的前件，计算出每条规则的适应度，即

其中

该层的总数 N ₃ = m =49。对于给定输入，只有在输入点附近的那些语言变量值才有较大的隶属度，远离输入点的语言变量值的隶属度或者很小（高斯隶属度函数）或者为0（三角形隶属度函数）。当隶属度函数很小（如小于0.05）时，近似取为0。因此，在 α _j 中只有少量节点输出非0，而多数节点的输出为0。

第四层的节点数与第三层相同，即 N ₄ = N ₃ = m =49，它实现的是归一化计算，即

第五层是输出层，它实现的是清晰化计算，输出期望转角，即

与前面所给出的标准模糊模型的清晰化计算相比较，这里的 ω _ij 相当于 y _i 的第 j 个语言值隶属度函数的中心值，式（2-148）写成向量形式则为

其中

3.学习算法

假设各输入分量的模糊分割数是预先确定的，那么需要学习的参数主要是最后一层的连接权 ω _ij =（ i =1，2，…， r ； j =1，2，…， m ），以及第二层的隶属度函数的中心值 c _ij 和隶属度函数的宽度 σ _ij （ i =1，2，…， n ； j =1，2，…， m ）。

上面给出的模糊神经网络本质上也是一种多层前馈网络，所以，可以仿照BP网络用误差反传的方法来设计调整参数的学习算法。为了导出误差反传的迭代算法，需要对每个神经元的输入输出关系加以形式化的描述。

设图2-34表示模糊神经网络中第 q 层第 j 个节点，其中，节点的纯输入= ，节点的输出= ，对于一般的神经元节点，通常有

对于图2-34所示的模糊神经网络，其神经元节点的输入输出函数具有较为特殊的形式。下面具体给出它的每一层的节点函数。

第一层：

第二层：

第三层：

第四层：

第五层：

取误差代价函数为

式中， y _di 和 y _i 分别表示期望输出和实际输出。下面给出误差反向传播算法来计算 ，然后利用一阶梯度寻优算法来调节 ω _ij 、 c _ij 和 σ _ij 。

首先计算

其次求得

再次计算

如果 f ^（3） 采用最小值运算，则当 是第 k 个规则节点输入的最小值时，有

否则

如果 f ^{（ 3 ）} 采用相乘运算，则当 是第 k 个规则节点的一个输入时，有

否则

从而可得所求一阶梯度为

最后在求得所需的一阶梯度后，可给出参数调整的学习算法为

式中， β 为学习率。

2.5.3 基于纯追踪模型的路径跟踪控制算法

模糊神经网络消除了单一模糊控制过于依靠专家或操作人员的经验和知识，缺乏自学习和自适应能力的缺点，而且它需要大量的样本，泛化能力较差，算法相对复杂，所以，编程实现困难。基于此，本节对纯追踪算法展开研究。

1.拖拉机运动学模型

拖拉机模型是仿真分析控制方法的基础，为验证控制算法的正确性，建立拖拉机的运动模型是很有必要的。本书的研究对象是福田M1004拖拉机，前轮负责转向。利用Ellis提出的二轮车简化模型建立基于后轴中心的拖拉机运动学模型。假设地面平坦、前进速度不变、忽略车辆离心力和侧滑，将转向机构看作一阶惯性环节，以预定义路径作为横轴 X 建立跟踪坐标系，前进方向为横轴 X 的正方向，跟踪起点的横坐标为零，则可以得到图2-35所示的拖拉机运动学模型。

由图2-35可知

式中， P _e ——横向偏差，m。

ψ _e ——航向偏差，rad。

δ ——前轮转角，rad。

L ——轴距，m。

V ——速度，m/s。

τ ——惯性时间常数，s。

δ _d ——期望前轮转角，rad。

在 δ 和 ψ _e 都比较小的情况下，用一阶泰勒展开式进行线性化，可近似得到拖拉机的运动学模型为

该模型中，轴距 L 为2.2m，速度 V 可以测得，而参数τ是未知的。在导航系统中，负责拖拉机转向操作的系统可以视作一阶惯性环节，参数τ是惯性时间常数，要获得此参数大小，需要对操纵转向系统的特性进行分析。很明显有以下传递函数：

式中， Y （ s ）为转向机构输入的拉氏变换，即期望前轮偏角的拉氏变换； F （ s ）为转向机构输出的拉氏变换，即实际前轮偏角的拉氏变换。通过试验方式记录期望前轮转角和实际转角，利用MATLAB的系统辨识工具箱解算，求得 τ 为0.2。

2.大地坐标系与车体坐标系转化

由于纯追踪模型的参数是基于车体坐标系下定义的，而农业机械的实时位置是基于大地坐标系下定义的，因此，有必要建立两个坐标系下的转换公式。

在应用纯追踪模型前，将农业机械的目标点位置转换成车体坐标系下的目标点坐标。设 x _d 、 y _d 、 θ 分别为农业机械在大地坐标系下的横轴坐标、纵轴坐标和当前航向角。用 x _goal 和 y _goal 表示纯追踪算法的目标点在大地坐标系下的横轴坐标和纵轴坐标。通过简单的数学运算可得如下转换公式：

（ x _change ， y _change ）为转换后机体坐标系下的目标点坐标。

3.纯追踪跟踪模型

纯追踪（Pure Pursuit）算法是一种几何计算方法，该方法具有简单、直观和容易实现的特点，其核心是确定一个合适的前视距离。该算法模拟车内驾驶员的视觉，具有仿生学的特点。

图2-36所示为拖拉机的车体坐标系 O'x'y' ，其中点 P （ x' ， y' ）为路径上的目标点， E 为连接车体坐标系原点和点 P 的弧段弦长，即前视距离， R 是该弧段的半径。按照几何关系，有以下关系式：

由式（2-165）可得

式（2-166）中， x' 为跟踪路径上目标点在车体坐标系下的横坐标。 x' 的计算公式为

式中， P _e ——车体质心相对于跟踪路径的横向误差，在车体前进方向偏右为正，偏左为负。

ψ _e ——车体当前航向角度与跟踪直线目标航向角度之差。

依据简化的二轮车模型，可知车体转向轮偏角和转弯半径之间的关系为

式中， α ——转向轮偏角。

L ——车体轴距。

由以上各式可求得在直线跟踪条件下，纯追踪模型计算的转向轮偏角控制量为

式中， L 为已知， p _e 和 ψ _e 可以解算获得，只有前视距离 E 有待确定。

在农业车辆的自动导航中，横向跟踪偏差是衡量控制效果的首要指标，故选用横向跟踪偏差的ITAE作为优化指标。ITAE是指时间乘绝对值误差的积分，根据ITAE指标设计的系统超调量小，阻尼适中，具有良好的动态特性，故常用于自动控制系统的优化设计。其中

考虑到仿真过程中，仿真时间有限、仿真步长具有可变性，对式（2-170）修正如下：

利用该式即可对不同前视距离条件下得到的横向跟踪误差数据进行对比分析，使该值减至最小的前视距离即为最优值。

2.5.4 多模变结构智能控制算法

模型参考自适应控制是将模型控制算法与自适应控制算法相结合，对解决系统自适应控制问题具有较好的适用性。在农机导航控制过程中，模型参考自适应控制既可发挥模型控制算法在导航控制的独特优势，又可降低导航过程中不确定干扰的不良影响，从而提高导航控制精度。自适应导航控制系统原理图如图2-37所示，其主要由参考模型、模糊自适应调整单元、模型控制规则等组成。在模型参考自适应控制器中引入多模变结构控制思想，采用模糊控制算法自适应在线调节模型控制规则参数。

参考模型为

模型控制规则为

当设定路径为直线导航时，模型控制规则简化为

模型控制规则基于当前的位姿信息计算控制量转向轮期望转角 δ ，并将它输出给转向执行机构，控制农机按照预定轨迹行走。根据检测出的扰动误差，模糊自适应调节机构采用模糊控制方法，基于驾驶员的专业知识设计模糊控制规则，通过不断修正模型控制规则中的参数 K _d 或 K _p 的值，从而调节控制输出量 δ ，增强控制系统跟踪的稳健性。

1.确定输入输出量

模糊自适应调节器有两个输入变量：实际位置相对理论位置的横向偏差变量 E 、偏差变化量EC。输出变量为模型控制规则中参数 K _p 的调整量Δ K _p 。

定义 E 的模糊集为{NB，NM，NS，NO，PO，PS，PM，PB}；论域为{-6，-5，-4，-3，-2，-1，0，1，2，3，4，5，6}；基本论域为[-60，60]；量化因子 K _e 为0.1。

定义 E _C 的模糊集为{NB，NM，NS，O，PS，PM，PB}；论域为{-6，-5，-4，-3，-2，-1，0，1，2，3，4，5，6}；基本论域为[-6，6]；量化因子 K _ec 为1。

定义Δ K _p 的模糊集为{NB，NM，NS，O，PS，PM，PB}；论域为{-7，-6，-5，-4，-3，-2，-1，0，1，2，3，4，5，6，7}；基本论域为[-0.07，0.07]；量化因子为100。

3个隶属度函数均采用高斯函数，横向偏差变化量 E _C 的隶属度如图2-38所示。

2.控制规则设计

根据驾驶员实际驾驶的专家知识，得到控制规则设计原则如下：当偏差绝对值较大时，控制量将尽量减小偏差；当偏差绝对值较小时，控制量除了要考虑减小偏差，还要考虑系统稳定性问题。如果偏差绝对值的变化趋势增大，则修正模型控制器参数，从而输出期望转角来校正转向轮，使得农机向偏差减小的方向运动；如果偏差绝对值的变化趋势减小，则表明此刻的控制效果与期望模型控制效果基本吻合，模型控制器参数修正量应该减小或者不进行参数修正。

由于被控对象参数变化或外界干扰等情况，使得参考模型输出结果 y _m 和对象瞬时响应 y 之间的关系会出现图2-39所示的9种情况。由此可获得模糊自适应控制规则描述如表2-7所示。

2.5.5 基于SOA理论的改进纯追踪算法

1.纯追踪算法

基于纯追踪模型的路径跟踪控制方法是一种几何方法，具有模拟人类驾驶的特征，可以依据目标点距离和当前跟踪误差动态调整期望前轮转角，实现路径跟踪。图2-40所示为拖拉机自动导航系统的纯追踪模型，其中，导航坐标系选用高斯坐标系 X ^g -O ^g -Y ^g ，拖拉机车体坐标系为 X ^b -O ^b -Y ^b 。

在图2-40中， G （ x ， y ）为预设路径上的跟踪目标点，其中 x 为目标点 G 在车体坐标系中的横坐标， y 为目标点 G 在车体坐标系中的纵坐标； θ _e 为拖拉机当前航向和目标点航向的偏差； ρ 为拖拉机的即时转弯曲率，这里规定拖拉机逆时针转动时曲率为正（ ρ ＞0），顺时针转动时的曲率为负（ ρ ＜0）； L _d 为前视距离； ϕ 为拖拉机沿着预期弧线到达目标点所转过的航向角度； d 为拖拉机相对于预期目标点的横向偏差，以拖拉机前进方向为基准，本书规定农机在预设路径左侧时 d ＜0，在预设路径右侧时 d ＞0； l 为拖拉机轴距。

根据图2-40中的几何关系，可以求得目标点 G 在车体坐标系 X ^b -O ^b -Y ^b 中的横、纵坐标表达式为

在直角三角形 O ^b RG 中，依据勾股定理可知

由式（2-175）和式（2-176）可得

转弯半径和拖拉机的即时转弯曲率 ρ 的关系为

根据图2-40中的几何关系可以推导出

结合式（2-177）、式（2-178）和式（2-179）可得纯追踪模型表达式为

结合轮式拖拉机运动学模型式（2-175）、式（2-177）和式（2-179）可得基于纯追踪模型的跟踪控制律为

拖拉机的横向偏差 d 和航向偏差 θ _e 可以通过传感器的测量值和预设路径计算得到，由式（2-181）可知，控制器仅包含前视距离 L _d 一个可调参数。 L _d 的数值将直接影响拖拉机自动导航系统的路径跟踪效果。从纯追踪模型的表达式可以看出，当前视距离较大时，拖拉机会沿着较小的曲率驶向期望路径，有助于抑制振荡现象的产生，但是控制系统的响应速度较慢；反之，当前视距离较小时，拖拉机会沿着较大曲率驶向期望路径，控制响应速度快，但是会产生较大的振荡。因此，采用较大或者较小的前视距离都不能得到满意的跟踪效果。

在前期研究中，通过仿真或者试验手段设定合适的前视距离，但是在实际应用过程中，固定前视距离的方法在车速动态变化的情况下无法取得理想的控制效果。因此，如何在线自适应调节前视距离，是纯追踪算法需要解决的一个关键问题。

2.改进纯追踪算法

人群搜索算法（Seeker Optimization Algorithm，SOA）直接模拟人类智能搜索行为。与常用的全局优化算法相比，如遗传算法、粒子群算法等，具有收敛速度快、收敛精度高、稳健性好的特点，适合应用于在线自适应寻优场合。其参数寻优过程如下：当搜索者位置较好时，应搜索较小的邻域；当搜索者位置不佳时，应搜索较大的邻域。这种寻优机制和纯追踪算法前视距离的调优机理是相互吻合的，因此，采用SOA理论改进传统纯追踪算法。

基于SOA理论的改进纯追踪算法基本原理如图2-41所示。改进算法在原有纯追踪算法的基础上增加了SOA优化环节，可以依据评价结果在线动态调整前视距离，以改善控制品质。

1）参数编码

考虑到纯追踪算法只需要调整前视距离参数，因此，令种群中个体的位置矢量的维数为1，该种群 P 可以用一个 S ×1的矩阵表示：

式中， （ k =1，2，…， S ）为前视距离； S 为种群数。前视距离的取值范围可以预先通过仿真或试验方法测定，初始种群可以在允许的取值范围内随机产生。

2）适应度函数选取

SOA算法使用适应度值评估搜索进化过程中每个个体或前视距离的优劣，并作为后续搜索各个位置更新的依据，最终使初始设定的前视距离逐渐进化到最优解。为了获得满意的控制效果，采用前文提出的路径跟踪控制性能指标式（2-112）作为评价参数。由于式（2-112）反映的是全局作业性能，不适合用于在线动态评价，所以将其修改为

式（2-183）反映了当前时刻 k 及其前 N 个时刻内的拖拉机自动导航系统路径跟踪作业性能。其中， E _v 为路径跟踪性能评价参数， d （ m ）表示 m 时刻农机的横向偏差， m = k ， k -1，…， k-N 。在评价参数式（2-183）中加入控制信号的平方项 i ² （ m ），以防止控制能量过大，于是确定适应度函数为

式中， λ ₁ 和 λ ₂ 为权值。

3）算法流程

参数的编码和适应度函数确定后，用于在线调节纯追踪算法的基于SOA算法的前视距离优化流程如表2-8所示。

2.5.6 实时动态寻优轨迹规划算法

现场试验发现，当拖拉机距离期望路径较远时，改进纯追踪算法上线距离较长，而且在上线点处跟踪误差较大。拖拉机驾驶员为了提高上线精度，在上线过程中只能依靠手动驾驶，自动化程度较低。为解决该问题，在改进纯追踪算法基础上，研究实时动态寻优轨迹规划算法。该算法需要综合考虑最大曲率约束、起止点航向约束和最大转向角约束，把最优轨迹规划问题转化为B样条控制点参数优化问题，采用量子遗传算法对目标函数进行寻优，从而求得距离最短的可行驶上线轨迹。

1.上线轨迹描述

拖拉机上线轨迹示意图如图2-42所示， S _goal 是上线轨迹的目标点，本书将拖拉机从其初始位置出发行驶至目标点的过程定义为上线过程。上线过程行驶过的距离称为上线距离；将上线过程中拖拉机经过的一系列离散位置点序列 S _path =（ S _start ， S ₁ ， S ₂ ，…， S _k ， S _goal ）定义为上线轨迹，其中 S _k =（ x _k ， y _k ， θ _k ），由定义在高斯平面坐标系下的位置（ x _k ， y _k ）和航向 θ _k 组成。

将上线轨迹规划问题定义如下：在满足多种非线性约束条件的轨迹簇 S _i ={ S _path1 ， S _path2 ，…， S _{path n} }中寻找一条距离最短的上线轨迹作为最优目标路径。函数描述为

式中， S _optimal ——最优目标路径。

Δ _d ——轨迹中路径点间距。

——轨迹长度。

令 x = f _x （ u ）， y = f _y （ u ），其中 u 为B样条曲线的节点（李军等，2012），则 x 、 y 对 u 的1、2阶导数为 ，路径 s 各点处的航向角 θ 、曲率 ρ 和等效前轮转角 δ 的计算公式为

2.轨迹约束

为保证上线轨迹具有可跟踪性，需要对规划的轨迹曲率、起止点航向角及等效前轮转角范围进行限制。

由定义可知，当车辆逆时针转动时，曲率与拖拉机转弯半径的关系公式为

图2-43所示为简化二轮车模型（缪存孝等，2017），拖拉机最小转弯半径主要受轴距和转向前轮的最大转向角限制，其数值是固定的，计算式为

式中， R _min ——最小转弯半径。

δ _max ——前轮最大转向角。

结合式（2-187）、式（2-189）和式（2-190）可知，上线轨迹各点处的曲率均需满足约束

为避免拖拉机在上线轨迹的起始时刻出现原地转向或振荡现象，需要对上线路径的初始点航向进行约束。因此，当路径点 S _i = S _start 时，建立约束条件为

式中， θ _s ——路径航向。

θ _v ——车身航向。

ε ——航向阈值（取较小的正数）。

为了保证拖拉机上线轨迹与全局路径结合处的平滑度，提高上线点 S _goal 处的路径跟踪精度，当路径点 S _i = S _goal 时，需满足约束

式中， θ _g ——目标点航向。

拖拉机前轮转向范围决定了其路径跟踪的灵活性，如果上线轨迹某点处等效前轮转角过大，该点处的路径将不具备可跟踪性。因此，需要对等效前轮转角进行约束。联立式（2-187）和式（2-188）可以得到等效前轮转角，建立约束条件为

农机在实际作业过程中的行驶速度一般控制在10km/h以内。经测试，在该速度条件下只要规划的上线轨迹满足上述约束条件，拖拉机转向系统均可有效跟踪期望转角信号。因此，本书上线轨迹规划算法未考虑转向角速度及角加速度约束。

3.实时动态寻优轨迹规划器

轨迹的表示方法有很多种，GALLINA探讨研究了7种曲线表示方法（胡书鹏等，2017），并对它们的优缺点进行了对比分析。曲线最终形状方便控制是B样条的优点之一，为了改变B样条曲线的形状，可以修改一个或多个控制参数，如控制点的位置、节点位置或曲线的次数。另外，B样条曲线分段组成，修改某一控制点只引起与该控制点相邻的曲线形状发生变化，远处的曲线形状不受影响。

B样条函数本身的特性（Dai C等，2006；李红等，2016；SNIDER J M等，2009）使其非常适合应用于路径规划场合，因此，本书选用B样条曲线进行上线轨迹规划。B样条曲线表达式为

式中， n ——B样条曲线阶次。

S _{i ， n} （ u ）——对应于节点 u 的第 i 段曲线上的点。

F _{k ， n} （ u ） ——n 次B样条基函数。

P _i+k ——第 i + k 个控制点。

其中，节点 u ∈[0，1]；设控制点个数为 m ，则 i =1，2，…， m-n ，表示整条曲线由 m-n 段B样条曲线平滑连接而成，每段曲线由 n +1个控制点生成。

由式（2-194）可知，上线轨迹至少要保证2阶可导，为满足约束条件且降低计算量，选用3次B样条曲线，即 n =3。此时，B样条基函数为

式中， u ¹ 、 u ² 、 u ³ 分别为 u 的1～3次幂。

将式（2-197）代入式（2-195），可得对应节点 u 的第 i 段3阶B样条曲线上的点坐标为

式（2-198）的1阶、2阶导数分别为

其中

P =[ P _i P _{i+ 1} P _{i+ 2} P _{i+ 3} ] ^T , i =1,2,…, m -3

式中， P _i ～ P _{i +3} 是用于计算第 i 段B样条方程的4个控制点。

当样条曲线的控制点较少时，不易求得满足约束的期望路径；控制点过多则会造成方程求解困难，增加运算时间，影响轨迹规划的实时性。根据经验采用6个控制点 P ₀ ～ P ₅ 生成满足约束条件的3阶B样条上线轨迹。

现有文献大多采用优化算法直接求取 N 个控制点（李军等，2012；GALLINA P等，2000），对于6个控制点 P （ x ， y ）需要优化的未知参数为12个，当使用迭代方法求解时，运算量非常大。为了提高轨迹规划速度，有必要研究曲线约束与控制点间的关系以简化计算。

对于3阶B样条曲线，当3个相邻的控制点在同一条直线上且间距相等时，生成的B样条曲线与该直线相切于中间的控制点，如图2-44所示。

依据上线路径初始点的航向角约束，选择控制点 P ₀ 、 P ₁ 、 P ₂ 。当3个控制点满足条件：选定拖拉机初始位置为 P ₁ ； P ₀ 、 P ₁ 、 P ₂ 三点共线；矢量 P ₀ P ₂ 的方向角与拖拉机初始航向一致； P ₀ 和 P ₂ 关于 P ₁ 对称，即可满足拖拉机初始状态约束。

同理，依据目标点航向约束，确定控制点 P ₃ 、 P ₄ 、 P ₅ ，并选定 P ₄ 为目标点位置坐标。

设线段 和 的长度为 l ₁ ，线段 和 的长度为 l ₂ 。依据起止点航向约束选定控制点 P ₁ 和 P ₄ 后，只要确定 l ₁ 和 l ₂ 的数值即可求得其他控制点并生成上线轨迹，这样便把最优轨迹规划问题转变为求 l ₁ 和 l ₂ 的最优解问题，相比求解12个未知参数，该方法只需要优化两个参数，有效地减少了算法的运算量。

本书采用量子遗传算法求解 l ₁ 和 l ₂ 。量子遗传算法（Quantum Ggenetic Algorithm，QGA）是遗传算法和量子计算结合产生的一种新的概率进化算法，比传统遗传算法具有更快的收敛速度和并行处理能力，适合应用于在线优化场合。

将 l ₁ 和 l ₂ 作为个体进行寻优，把上线轨迹的距离取负值作为适应度函数（轨迹越短，适应度值越大），按照如表2-9所示的量子遗传算法优化过程循环执行，直至结束条件。

当通过优化算法找到6个控制点后，代入B样条曲线的表达式即可求出由3段B样条曲线组成的最优上线轨迹。

综上所述，基于实时动态寻优规划器的改进纯追踪算法执行流程如图2-45所示。自动导航系统实时采集拖拉机当前的位姿信息，首先判断当前位姿信息与期望位姿的偏差，如果偏差在阈值允许范围内，则运行SOA改进纯追踪算法；如果位姿偏差大于阈值，则运行实时动态寻优规划器，然后运行SOA改进纯追踪算法跟踪最优上线轨迹和全局路径。导航控制器响应SOA改进纯追踪算法输出的控制指令，动态调整拖拉机的前轮角度，使位姿偏差逐渐收敛至期望精度。