1. 问题引入
之前聊过基于车辆运动学模型的PID/PP路径跟踪和MPC路径跟踪，本次聊聊如何设计模糊控制器实现车辆路径跟踪。我们直接带入一个阿克曼模型的车辆控制问题（问题来源：某课程课后作业），如图所示。1.目标是设计一个控制器，在理想的车辆运动学数学模型的情况下，控制车辆沿着x轴方向前进，这便是一个典型的简化方向的路径跟踪问题。
假设小车可以以v=0.5m/s的恒定速度向前移动。并且假设小车配备有陀螺仪等传感器，该传感器可以随时测量小车当前的位置（x，y）和方向（角度）。那么本跟踪问题的解决就是需要设计一个跟踪控制器，根据小车的实际位置，调整小车的方向盘，从而实现小车从当前位置靠近参考路径（y= θ = 0）并稳定的沿路径前进。这个跟踪控制器就可以是模糊控制器。
2. 模糊控制器原理
模糊控制是以模糊集合理论、模糊语言及模糊逻辑为基础的控制，它是模糊数学在控制系统中的应用，是一种非线性智能控制。模糊控制是利用人的知识对控制对象进行控制的一种方法，通常用"if条件，then结果"的形式来表现，所以又通俗地称为语言控制。一般用于无法以严密的数学表示的控制对象模型，即可利用人（熟练专家）的经验和知识来很好地控制。因此利用人的智力模糊地进行系统控制的方法就是模糊控制^[1]。
模糊控制器设计一般可以概括为四个主要部分：
- 模糊化。主要作用是选定模糊控制器的输入量与输出量，并将其转换为系统可识别的模糊量，具体包含以下三步： 1.确定输入/出量并对输入/出量进行满足模糊控制需求的处理; 2.对输入/出量进行规则化处理; 3.确定各输入/出量的模糊语言取值和相应的隶属度函数。
- 规则库。根据人类专家的经验建立模糊规则库。模糊规则库包含众多控制规则，是从实际控制经验过渡到模糊控制器的关键步骤。
- 模糊推理。主要实现基于知识的推理决策，定义输入量与输出量控制逻辑表。
- 解模糊。主要作用是将推理得到的控制量转化为控制量输出。
3．模糊控制实现路径跟踪
3.1 小车运动学建模
假设顺时针旋转为正方向，逆时针旋转为负方向。根据小车跟踪示意图小车的模型与方位信息，可以获得小车位置（x，y）、方向θ（角度）与小车前轮转角u的简化离散模型，写成状态方程的形式如下所示
<math xmlns="http://www.w3.org/1998/Math/MathML" display="block"> { θ ( k + 1 ) = θ ( k ) + v T t a n ( u ( k ) ) / L x ( k + 1 ) = x ( k ) + v T c o s ( θ ( k ) ) y ( k + 1 ) = y ( k ) + s i n ( θ ( k ) ) \left\{ \begin{aligned} θ(k + 1) & = θ(k) + vTtan(u(k))/L \\ x(k + 1) & = x(k) + vTcos(θ(k)) \\ y(k + 1) & = y(k) + sin(θ(k)) \end{aligned} \right. </math>⎩ ⎨ ⎧θ(k+1)x(k+1)y(k+1)=θ(k)+vTtan(u(k))/L=x(k)+vTcos(θ(k))=y(k)+sin(θ(k))
其中：
- L ---小车的前后轮距（这里给定L = 2.5m）;
- T ---离散模型的采样时间（这里给定T = 0.1s） ;
- v ---小车的前进速度（这里给定v = 0.5m/s）。
3.2 模糊控制器设计
针对本文提出的跟踪问题和小车模型，设计一个模糊控制器，其输入变量为小车当前位置与参考路径（y=θ=0）之间的纵向偏差y（参考路径的y=0，因此纵坐标y即为纵向偏差），方向偏差θ（参考路径的θ=0，因此纵坐标y即为方向偏差差）；输出量为u，代表小车前轮（方向盘）的旋转角度，从而操纵小车从任何给定的初始条件到达参考路径并沿路径前进。在模拟中。变量范围结合实际情况限定为：
<math xmlns="http://www.w3.org/1998/Math/MathML" display="block"> − 100 ≤ y ≤ 100 , − 180 ° ≤ θ ≤ 180 ° , − 30 ° ≤ u ≤ 30 ° -100 ≤y≤100, -180°≤θ≤180°, -30°≤u≤30° </math>−100≤y≤100,−180°≤θ≤180°,−30°≤u≤30°
3.2.1 模糊化
对两个输入量和一个输出量的模糊语言定义如下表所示：
| 纵向偏差y | 方向偏差θ | 前轮转角u |
|---|---|---|
| AB(正大) | AO(正大) | NB(负大) |
| AC(正小) | AR(正中) | NM(负中) |
| CE(中间) | AH(正小) | NS(负小) |
| BC(负小) | HZ(中间) | ZE(中间) |
| BE(负大) | BH(负小) | PS(正小) |
| - | BR(负中) | PM(正中) |
| - | BO(负大) | PB正大) |
输入变量【纵向偏差y】的率属函数选择梯形隶属函数（trapmf）、三角形隶属函数（trimf）都可。
输入变量【方向偏差θ】的率属函数同样选择梯形隶属函数（trapmf）、三角形隶属函数（trimf）都可。
输出变量【前轮旋转角度u】的率属函数都选择三角形隶属函数（trimf），定义如下
3.2.2 模糊规则定义及去模糊函数选择
从开车的经验可得出基本的控制规则：
- 当司机发现车在路左侧时，则需要将方向盘往右打以回到路径;
- 当司机发现车在路右i侧时，则需要将方向盘往左打以回到路径；
- 当司机发现车头向左偏离路径时，需要将方向盘往右打以回到路径;
- 当司机发现车头向又偏离路径时，则需要将方向盘往左打以回到路径；
基于这个基本规则，定义模糊规则如下表，每个规则都有这样的形式，if y is Y and θ is φ，then u is U。使用matlab的fuzzy工具箱定义如下。
最终生成模糊控制器如下。 
其中，去模糊函数选择centroid，为面积重心法。
3.3 matlab仿真搭建及结果
3.3.1 simulink仿真
使用simulink搭建出本文的控制问题仿真如下
其中
- 【exp_single】是路径生成模块，生成参考路径y= θ = 0，s函数核心代码如下。
bash
function sys=mdlOutputs(t,x,u)
% 期望位姿
sys(1) = t;% 期望x，无用
sys(2) = 0;% 期望y
sys(3) = 0;% 期望xita- 【robot_auxctrl】是误差计算模块，生出本周期小车当前位置与参考路径（y=θ=0）之间的纵向偏差y，方向偏差θ两个变量，即为模糊控制器的输入。s函数核心代码如下。
bash
function sys=mdlOutputs(t,x,u)
% 纵向距离
sys(1) = u(2) - u(5);
% 角度
sys(2) = u(3) - u(6)*180/pi;-
【Fuzzy Logic Controller】即为本文所述的模糊控制器。
-
【robot_keniticmodel】则为小车的运动学模型，根据输入的前轮转向角，更新当前周期的小车位置。s函数核心代码如下。
bash
function [sys,x0,str,ts]=mdlInitializeSizes
sizes = simsizes;
sizes.NumContStates = 3;
sizes.NumDiscStates = 0;
sizes.NumOutputs = 3;
sizes.NumInputs = 1;
sizes.DirFeedthrough = 0;
sizes.NumSampleTimes = 1;
sys = simsizes(sizes);
%% 设置小车初始位置.依次是[x，y，xita]，注意要将角度转成弧度
% x0 = [20;40;0*pi/180.0];
x0 = [10;-30;90*pi/180.0];
% x0 = [40;30;(220-360)*pi/180.0];% 注意换算成-180至180的范围内
% x0 = [50;10;-10*pi/180.0];
str = [];
ts = [0,0];
function sys=mdlDerivatives(t,x,u)
%% 变量定义
v=0.5;%线速度
L = 2.5;%轴距
w=u*pi/180;%转角
theta = x(3);%当前角度
%% 运动学模型状态方程
sys(1) = v*cos(theta);
sys(2) = v*sin(theta);
sys(3) = v*tan(w)/L;
function sys=mdlOutputs(t,x,u)
sys(1) = x(1);
sys(2) = x(2);
sys(3) = x(3);3.3.2 仿真结果
- 纵向偏差y随时间变化仿真结果
- 方向偏差θ随时间变化仿真结果
- 前轮转角u随时间变化仿真结果
可见，随着时间推移，纵向偏差y，方向偏差θ全部归零，这就说明，小车最终从初始位置实现了对参考路径的跟踪运行。
参考