路径跟踪算法之模糊控制器跟踪

1. 问题引入

之前聊过基于车辆运动学模型的PID/PP路径跟踪和MPC路径跟踪，本次聊聊如何设计模糊控制器实现车辆路径跟踪。我们直接带入一个阿克曼模型的车辆控制问题（问题来源：某课程课后作业），如图所示。1.目标是设计一个控制器，在理想的车辆运动学数学模型的情况下，控制车辆沿着x轴方向前进，这便是一个典型的简化方向的路径跟踪问题。

假设小车可以以v=0.5m/s的恒定速度向前移动。并且假设小车配备有陀螺仪等传感器，该传感器可以随时测量小车当前的位置（x，y）和方向（角度）。那么本跟踪问题的解决就是需要设计一个跟踪控制器，根据小车的实际位置，调整小车的方向盘，从而实现小车从当前位置靠近参考路径（y= θ = 0）并稳定的沿路径前进。这个跟踪控制器就可以是模糊控制器。

2. 模糊控制器原理

模糊控制是以模糊集合理论、模糊语言及模糊逻辑为基础的控制，它是模糊数学在控制系统中的应用，是一种非线性智能控制。模糊控制是利用人的知识对控制对象进行控制的一种方法，通常用“if条件，then结果”的形式来表现，所以又通俗地称为语言控制。一般用于无法以严密的数学表示的控制对象模型，即可利用人（熟练专家）的经验和知识来很好地控制。因此利用人的智力模糊地进行系统控制的方法就是模糊控制¹。

模糊控制器设计一般可以概括为四个主要部分：

• 模糊化。主要作用是选定模糊控制器的输入量与输出量，并将其转换为系统可识别的模糊量，具体包含以下三步：
  1.确定输入/出量并对输入/出量进行满足模糊控制需求的处理;
  2.对输入/出量进行规则化处理;
  3.确定各输入/出量的模糊语言取值和相应的隶属度函数。
• 规则库。根据人类专家的经验建立模糊规则库。模糊规则库包含众多控制规则，是从实际控制经验过渡到模糊控制器的关键步骤。
• 模糊推理。主要实现基于知识的推理决策，定义输入量与输出量控制逻辑表。
• 解模糊。主要作用是将推理得到的控制量转化为控制量输出。
  

3．模糊控制实现路径跟踪

3.1 小车运动学建模

假设顺时针旋转为正方向，逆时针旋转为负方向。根据小车跟踪示意图小车的模型与方位信息，可以获得小车位置（x，y）、方向θ（角度）与小车前轮转角u的简化离散模型，写成状态方程的形式如下所示
$$\{\begin{array}{rl}\theta (k+1)& =\theta (k)+vTtan(u(k))/L\\ x(k+1)& =x(k)+vTcos(\theta (k))\\ y(k+1)& =y(k)+sin(\theta (k))\end{array}$$
其中：

• L —小车的前后轮距（这里给定L = 2.5m）;
• T —离散模型的采样时间（这里给定T = 0.1s） ;
• v —小车的前进速度（这里给定v = 0.5m/s）。

3.2 模糊控制器设计

针对本文提出的跟踪问题和小车模型，设计一个模糊控制器，其输入变量为小车当前位置与参考路径（y=θ=0）之间的纵向偏差y（参考路径的y=0，因此纵坐标y即为纵向偏差），方向偏差θ（参考路径的θ=0，因此纵坐标y即为方向偏差差）；输出量为u，代表小车前轮（方向盘）的旋转角度，从而操纵小车从任何给定的初始条件到达参考路径并沿路径前进。在模拟中。变量范围结合实际情况限定为：
$$-100\le y\le 100,-180°\le \theta \le 180°,-30°\le u\le 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函数核心代码如下。

function sys=mdlOutputs(t,x,u)
% 期望位姿
sys(1) = t;% 期望x，无用
sys(2) = 0;% 期望y
sys(3) = 0;% 期望xita

• 【robot_auxctrl】是误差计算模块，生出本周期小车当前位置与参考路径（y=θ=0）之间的纵向偏差y，方向偏差θ两个变量，即为模糊控制器的输入。s函数核心代码如下。

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函数核心代码如下。

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，方向偏差θ全部归零，这就说明，小车最终从初始位置实现了对参考路径的跟踪运行。

参考

1. 百度百科：关键词模糊控制理论
2. 模糊控制路径跟踪仿真代码链接

---

1. 1 ↩︎