简介：这是一份基于模糊控制的自动泊车仿真源码，面向自动驾驶、智能控制方向的开发者与研究者，旨在通过Matlab环境演示模糊逻辑在复杂泊车场景中的应用。压缩包为rar格式，共含2个文件：一个fis文件用于定义模糊推理系统，含输入输出隶属度函数与模糊规则库；一个m脚本文件负责执行仿真主流程，整体仅2KB，轻量且便于直接运行与修改。已有248人学习该资源。代码虽精简，但覆盖了从传感器输入到控制输出的完整链路：通过设置距离、角度等输入变量，经模糊化、模糊推理与去模糊化生成转向控制指令，并结合车辆运动学模型模拟泊车轨迹。读者可借此学习模糊控制器的设计思路，掌握fis文件与m脚本的联合使用方法，并在此基础上扩展更复杂的泊车场景，如障碍物避让、车位识别等，具有较高的启发性与学习价值。

1. 基于模糊控制的自动泊车仿真：为什么值得上手

自动泊车是控制仿真里少有的“模型简单、反馈直观、做完有成就感”的综合练习。而基于模糊控制的方案，恰好卡在几何路径规划与模型预测控制之间：它不依赖精确车位模型，也不要求你把动力学方程推到底，规则表写的就是驾驶员“方向盘该往哪打”的直觉。这次我带你从运动学模型写到模糊控制器，再跑到闭环仿真和参数调试，全程用可复现的 Python 代码。适合三类人：想学模糊控制但看腻了水箱液位例子的，需要给毕设或项目做泊车 Demo 的，以及手里有一份泊车源码但看不懂规则表为什么这么写的。读完你会清楚这套代码怎么跑、参数按什么顺序调、翻车时先去查哪里。

2. 泊车仿真骨架：运动学模型与控制路线怎么选

2.1 三条控制路线摆在桌面上：几何、模型预测和模糊

自动泊车的控制方案大致分成三类。几何方法（Reeds-Shepp 曲线、纯跟踪）先把路径算出来再跟踪，优点是计算量小、轨迹光滑，缺点是一旦初始位姿偏差较大，路径重规划频繁，而且对车位边界的感知误差特别敏感。模型预测控制（MPC）精度最高，能同时处理约束和预测时域，但要写车辆模型、配求解器、调权重矩阵，新手往往被淹没在参数海洋里，一个权重改错就要重新调半天。

模糊控制的定位比较特殊。它不追求精确模型，而是把“误差大小”和“方向盘角度”都用语言等级描述，规则表可以直接对着驾驶经验写。泊车正好是低速、低自由度、执行机构响应远快于被控对象的场景，模糊推理的运算量在这里完全不是瓶颈。它最大的价值是过程可读：你能看到某一步是哪条规则在起作用，而不是面对一个黑匣子。下表是三条路线的直观对比：

我一般会把模糊控制当成“第一个能动的方案”：先让它跑起来，确认控制方向和误差定义没问题，再决定要不要升级到 MPC。这篇文章就按这个思路展开。

2.2 先让车辆动起来：自行车运动学模型与最小仿真场景

泊车仿真里车辆模型不需要太复杂，自行车模型（Bicycle Model）是主流选择：把前后轴各看成一个轮子，用后轴中心位置描述车辆，状态量是位置 (x, y) 和航向角 θ，控制量是前轮转角 δ。模型递推式很简单：

x' = x + v·cos(θ)·dt y' = y + v·sin(θ)·dt θ' = θ + (v / L)·tan(δ)·dt

注意这里 v 带符号，正为前进、负为倒车。倒车时 v 为负，同样的前轮转角会让航向角向相反方向变化，这也是后面规则表容易踩坑的地方。代码实现如下：

import numpy as np L = 2.5 # 轴距，单位 m dt = 0.1 # 仿真步长，单位 s v = -1.5 # 倒车速度，单位 m/s，负号表示倒车 def vehicle_step(state, delta, v=v, L=L, dt=dt): """自行车模型单步递推。 state: [x, y, theta]，后轴中心位置和航向角 delta: 前轮转角，正值表示左打 """ x, y, theta = state x = x + v * np.cos(theta) * dt y = y + v * np.sin(theta) * dt theta = theta + (v / L) * np.tan(delta) * dt return np.array([x, y, theta])

轴距 L 按实车取 2.5m 左右，低速泊车场景下这样简化足够。dt 取 0.1 秒，对应 10Hz 的控制频率，先跑通逻辑，后面如果发现轨迹毛刺再加密到 0.05。场景我定义为：车位中心参考点 (10.0, 0.0)，车辆初始位置 (7.0, 3.0)，初始航向 0，也就是车头朝右、车位在右后方。

target = np.array([10.0, 0.0]) # 车位内目标参考点 init_state = np.array([7.0, 3.0, 0.0]) # 车辆初始位姿 # 验证模型：固定方向盘左打 0.3 rad，观察倒车轨迹方向 state = init_state.copy() for _ in range(20): state = vehicle_step(state, 0.3) print("固定左打倒车 20 步后状态:", state)

这段验证非常重要：先固定一个转角，看车尾往哪个方向偏。在我的坐标系定义里，左打（δ 为正）倒车会让 θ 减小，车辆绕弧线运动。你不需要背结论，只需要跑一次，记住你坐标系里“左打对应车尾往哪边偏”，后面填规则表就靠这个符号关系。很多人的源码跑不出来，问题就出在这里，我放到第 5 章专门讲。

3. 模糊控制器实现：误差定义、隶属度函数与规则表

3.1 两个输入量的定义：横向误差和航向误差的坐标系约定

模糊泊车控制器最常用的输入是两个：横向误差和航向误差。横向误差描述“目标点在车身左侧还是右侧”，航向误差描述“车辆当前朝向与目标朝向差多少”。只用距离一个输入是不够的，因为倒车时目标在车前方和后方，控制行为完全相反，单输入没法区分这种歧义。

横向误差我取目标点在车身坐标系下的横向分量。设 dx = gx - x，dy = gy - y，则车身坐标系下的横向误差 e_lat 为：

e_lat = -dx·sin(θ) + dy·cos(θ)

这个式子的物理含义是：把目标点旋转到车身坐标系，正数表示目标在车身左侧，负数表示在右侧。航向误差 e_th 就简单了，目标航向减去当前航向，再归一化到 [-π, π]：

e_th = normalize(target_theta - θ)

def compute_errors(state, target, target_theta=0.0): """计算车身坐标系下的横向误差和航向误差""" x, y, theta = state dx = target[0] - x dy = target[1] - y # 车身坐标系横向分量，正 = 目标在左侧 e_lat = -dx * np.sin(theta) + dy * np.cos(theta) # 航向误差，归一化到 [-pi, pi] e_th = target_theta - theta e_th = np.arctan2(np.sin(e_th), np.cos(e_th)) return e_lat, e_th

论域选择上，横向误差取 ±3m，航向误差取 ±0.6 rad（约 ±34°）。这两个数值不是拍脑袋：泊车场景里车辆离车位中心最远一般不超过 3m，航向偏差超过 0.6 rad 时基本已经可以判定泊车失败，继续细分没有意义。论域越小，量化分辨率越高，但覆盖不住极端位姿，这个平衡在第 4 章细讲。

3.2 5×5规则表与零阶TS推理：核心代码

模糊控制器我推荐用零阶 TS（Takagi-Sugeno）模型：规则前件是“横向误差和航向误差的模糊集合”，规则后件是确定的输出值（方向盘转角），去模糊用加权平均。相比 Mamdani 先生成输出模糊集再做面积重心，零阶 TS 计算量小、代码短，泊车这种执行机构响应远快于被控对象的场景完全够用。

每个输入变量划分 5 个模糊集合：NB（负大）、NS（负小）、Z（零）、PS（正小）、PB（正大）。隶属度函数用三角形，首尾用梯形饱和。先写隶属度函数和模糊化逻辑：

def trimf(x, a, b, c): """三角形隶属度函数，b 点峰值 1，a 和 c 点隶属度为 0""" if x <= a or x >= c: return 0.0 if x < b: return (x - a) / (b - a) return (c - x) / (c - b) E_POINTS = [-3.0, -1.5, 0.0, 1.5, 3.0] # 横向误差论域 TH_POINTS = [-0.6, -0.3, 0.0, 0.3, 0.6] # 航向误差论域 D_POINTS = [-0.4, -0.2, 0.0, 0.2, 0.4] # 输出转角论域，单位 rad def fuzzify(x, points): """计算输入 x 在 5 个标签上的隶属度，返回长度为 5 的数组""" mem = [] for i in range(5): if i == 0: # 左边界梯形：x 小于等于第一个顶点时隶属度为 1 mem.append(1.0 if x <= points[0] else trimf(x, points[0], points[0], points[1])) elif i == 4: # 右边界梯形：x 大于等于最后一个顶点时隶属度为 1 mem.append(1.0 if x >= points[4] else trimf(x, points[3], points[4], points[4])) else: mem.append(trimf(x, points[i-1], points[i], points[i+1])) return np.array(mem)

规则表是整个控制器的核心。我用的 5×5 规则表如下，行是横向误差 E，列是航向误差 TH，数值是输出转角，正值表示左打：

RULE_D = np.array([ # TH: NB NS Z PS PB [ -0.2, -0.2, -0.4, -0.4, -0.4], # E: NB 目标在右侧远处 [ 0.0, -0.2, -0.2, -0.4, -0.4], # E: NS 目标在右侧近处 [ 0.4, 0.2, 0.0, -0.2, -0.4], # E: Z 目标在正后方 [ 0.4, 0.4, 0.2, 0.0, -0.2], # E: PS 目标在左侧近处 [ 0.4, 0.4, 0.4, 0.2, 0.0], # E: PB 目标在左侧远处 ])

规则表为什么要这样填？看左上角：目标在右侧远处（E=NB）且航向偏左需要回正（TH=NB），这时优先向右侧转向靠近目标，所以输出 -0.2 小角度右打。看中间行：目标在正后方（E=Z），航向偏左（TH=NS）时输出 +0.2 左打回正。规则的总体趋势是：横向误差主导方向选择，航向误差做修正。需要强调，这套符号体系只适用于“倒车时左打会让车尾向左偏”的坐标系设定，如果你的代码里符号相反，把整个 RULE_D 取反即可。

推理部分用加权平均去模糊。对每条规则，激活强度取两个前提隶属度的最小值，输出量用激活强度加权：

def defuzzify(mem_e, mem_th): """零阶TS推理：加权平均去模糊""" num = 0.0 den = 0.0 for i in range(5): for j in range(5): w = min(mem_e[i], mem_th[j]) # 前提激活强度 num += w * RULE_D[i][j] # 后件加权 den += w return num / den if den > 1e-8 else 0.0

这套推理只有 25 条规则，每步计算量微乎其微。你没看错，整个模糊控制器的“智能”就藏在这张表里，后期调参主要也是调这张表和输入论域。

3.3 第一次闭环运行：把模糊控制器接进运动学模型

控制器写好后，把它接进运动学模型跑闭环。主循环每步：计算误差 → 量化到论域 → 模糊化 → 规则推理 → 限幅 → 执行一步运动学递推。停止条件我设为位置误差小于 0.1m 且航向误差小于 0.05 rad：

E_MAX = 3.0 TH_MAX = 0.6 DELTA_LIMIT = 0.4 state = init_state.copy() traj = [state.copy()] for step in range(2000): x, y, theta = state e_lat, e_th = compute_errors(state, target) # 量化到模糊论域，超过论域就截断 e_norm = np.clip(e_lat / E_MAX, -1, 1) * 3.0 th_norm = np.clip(e_th / TH_MAX, -1, 1) * 0.6 mem_e = fuzzify(e_norm, E_POINTS) mem_th = fuzzify(th_norm, TH_POINTS) delta = defuzzify(mem_e, mem_th) delta = np.clip(delta, -DELTA_LIMIT, DELTA_LIMIT) # 前轮转角限幅 state = vehicle_step(state, delta) traj.append(state.copy()) if np.hypot(target[0]-x, target[1]-y) < 0.1 and abs(e_th) < 0.05: print(f"收敛于第 {step} 步") break traj = np.array(traj) print("最终位姿:", traj[-1])

把这段代码和前两节的函数拼在一起，第一次闭环就能跑通。你大概率会看到轨迹是一条先向右后方绕行、再回正进入目标点的弧线，这就是模糊规则在起作用。如果轨迹没有收敛，不要急着怀疑规则表，先去查符号体系，也就是第 5 章的第一条。

4. 闭环参数调节：量化因子、输出限幅和收敛性

4.1 量化因子决定分辨率和手感

闭环跑通只是起点，真正决定仿真好不好用的是三个参数：E_MAX、TH_MAX 和输出限幅。它们的作用是把物理量映射到模糊论域，本质是量化因子。E_MAX 太大，横向误差在 ±3m 内变化时隶属度变化平缓，控制器对侧向偏差不敏感，车辆会“钝”，停不准；E_MAX 太小，误差稍微大一点就饱和到 NB/PB，规则表只剩两端在起作用，输出变成开关式控制，车辆会震荡。TH_MAX 同理，它影响航向回正的力度。

我给出一张调试起点和调整方向的速查表，实际调参时按这个顺序来：

我的建议是先固定 E_MAX 和 TH_MAX，只把 DELTA_LIMIT 从 0.4 往 0.3 方向调，观察轨迹从“绕大圈”变“贴线走”的过程。这一步能帮你建立参数和轨迹形态之间的直觉，比对着公式算增益有用得多。

4.2 输出限幅与速度匹配：转弯半径的物理边界

前轮转角限幅不是随便设的，它对应车辆的最小转弯半径。由运动学模型可以推出 R_min = L / tan(delta_max)。当 L=2.5m、delta_max=0.4 rad 时，最小转弯半径约 6.4m；如果把 delta_max 压到 0.3 rad，最小转弯半径变成 8.6m。这个数字要和场景空间匹配：如果车位区域宽度只有 4m，而最小转弯半径 8.6m，车辆根本转不进车位。

另一方面，倒车速度 v 和输出限幅也要匹配。v 的绝对值越大，每步航向变化越快，同样的规则表输出会让轨迹更“冲”。我习惯把 v 控制在 1.0~1.5 m/s，速度再大就需要把 RULE_D 里的数值整体调小，否则容易出现第 5 章说的极限环震荡。低速下模糊控制的鲁棒性会明显好于高速，这也是泊车仿真里速度通常设得比较保守的原因。

4.3 规则表微调：从对角线开始，一次只改一个值

规则表不是一次写对的，微调是常态。我的经验是：先从 RULE_D 的对角线（E=Z、TH=Z 附近）开始调，因为对角线决定车辆“小误差时”的行为，那里是最容易先出问题的区域。车辆在目标附近来回摆，多数是对角线附近输出过强或过弱。把中心四个值（E 为 NS/Z、TH 为 NS/Z）先调好，再逐步往外围扩展。

外围值影响的是大误差工况。比如 E=NB、TH=PB 对应“目标在右侧且车头偏右很大”，这种状态出现在车辆绕过头的时候，如果轨迹显示车辆从右侧冲过头，就把这个角的值从 -0.4 往 -0.2 方向减。调规则表有一条铁律：一次只改一个值，改完跑一遍仿真看轨迹变化。同时改三个值出问题你根本不知道是哪个改坏了。另外，所有规则表里的符号必须与你的坐标系统一，改表前先确认“左打对应车尾往哪偏”。

5. 高频翻车点排查：5个让泊车仿真失败的坑

5.1 车辆在目标点附近反复画圈

现象：轨迹是一条越来越大的弧线，车辆围着目标点转圈，永远停不下来。原因基本是符号体系反了：规则表的“左打”方向和你代码里运动学模型的实际方向相反。解决方法是做一次单步实验：给一个固定正转角跑 20 步，打印 y 坐标变化，确认左打时车尾往哪边偏，然后把 RULE_D 整体取反。这个坑最常见，而且越早排查越省时间，我每次新建工程第一件事就是跑这个符号验证。

state = np.array([0.0, 0.0, 0.0]) for _ in range(20): state = vehicle_step(state, 0.3) # 固定左打 print("左打 20 步后状态:", state) # 看 y 是增还是减

5.2 换一个初始位置就撞边界

现象：仿真在初始位姿 A 下能顺利停进车位，换成位姿 B 后车辆跑到车位边界外面去。原因通常是论域不够宽，误差超过 E_MAX 后直接截断成最大值，规则表的分辨率在极端工况下失效。解决方法是统计整个泊车过程中横向误差出现的最大值，把 E_MAX 和 TH_MAX 调大到能覆盖这个范围，同时延长仿真步数。注意论域调大后量化分辨率会下降，需要同步缩小规则表输出值来补偿。

5.3 车能停进去但车头回不正

现象：最终位置在目标点附近，但航向角停在 0.1~0.2 rad 的偏差上，怎么看都不对。原因有两个：一是 TH_MAX 太大，航向误差在 ±0.3 rad 以内都落在 Z 集合里，控制器觉得“差不多归零了”就不再修正；二是停止条件里角度容差设得太宽松。解决方法是把 TH_MAX 从 0.6 缩到 0.4，让航向误差在较小范围时仍有规则能激活；同时把停止条件里的角度容差从 0.05 rad 收紧到 0.03 rad。

5.4 方向盘转角输出抖动

现象：打印 delta 曲线，发现每几步之间转角跳变明显，轨迹线上也出现毛刺。原因是相邻隶属度函数重叠太少，误差在集合边界小范围波动时，激活的规则在两套之间来回跳，导致输出不连续。解决方法：把 E_POINTS 和 TH_POINTS 相邻峰值间距略微缩小，让重叠率保持在 50% 左右；同时把 dt 从 0.1 改成 0.05，让误差变化在每个周期内更平缓。工程上还可以给输出加一阶惯性滤波，但那是治标，先治规则重叠的根因。

5.5 别人的源码能停，你自己重写后就不能停

现象：拿到了别人能收敛的规则表，抄进自己的代码却完全不收敛。原因九成在坐标系的定义差异：别人的横向误差可能正方向朝右，你的朝左；别人的正值可能代表右打，你的代表左打。规则表只是纯符号，离开坐标系定义没有意义。解决方法是先梳理自己的四件事：目标点坐标、车身坐标系定义、右打方向定义、停止条件，把这四条写在一张纸上，再对照规则表逐项检查符号。这一步看起来笨，但能避免在错误方向上消耗大量时间。

6. 进阶验证习惯：批量位姿扫描与鲁棒性评估

6.1 批量扫描：把运气变成成功率

单条轨迹收敛并不代表控制器没问题，很可能只是初始位姿恰好落在规则表最舒服的区域。我现在的习惯是每次调完规则表，立刻在初始位姿网格上批量跑一遍，统计成功率。网格覆盖横向位置、纵向位置和航向角三个维度：

init_poses = [] for x0 in [6.0, 7.0, 8.0]: for y0 in [2.0, 3.0, 4.0]: for th0 in [-0.3, 0.0, 0.3]: init_poses.append([x0, y0, th0]) def run_parking(init_state, max_steps=2000): """跑一次泊车仿真，返回是否收敛""" state = np.array(init_state, dtype=float) for _ in range(max_steps): e_lat, e_th = compute_errors(state, target) e_norm = np.clip(e_lat / E_MAX, -1, 1) * 3.0 th_norm = np.clip(e_th / TH_MAX, -1, 1) * 0.6 delta = np.clip(defuzzify(fuzzify(e_norm, E_POINTS), fuzzify(th_norm, TH_POINTS)), -DELTA_LIMIT, DELTA_LIMIT) state = vehicle_step(state, delta) if (np.hypot(target[0]-state[0], target[1]-state[1]) < 0.2 and abs(target[0]-state[0]) < 0.2 and abs(target[1]-state[1]) < 0.2 and abs(state[2]) < 0.06): return True return False success = sum(run_parking(p) for p in init_poses) print(f"批量成功率: {success}/{len(init_poses)}")

成功率低于 80% 时不要急着加规则，先回到第 4 章的参数检查顺序，把 E_MAX 和 TH_MAX 覆盖范围做好。批量扫描的价值在于把“偶尔能停”和“稳定能停”区分开，避免被单条漂亮轨迹误导。

6.2 值得投入的判断标准：看得见决策过程才有学习价值

这套模糊泊车仿真跑通之后，你手里就有了一个可以观察控制决策过程的实验台：打印每一步激活了哪条规则、输出怎么变化、轨迹如何响应。这比直接调一个黑盒 PID 更有学习价值。你可以尝试改掉规则表里某个角落的值，观察它对轨迹的影响，用一整个晚上把“规则表参数”和“轨迹形态”之间的关系摸清楚。某次我把 E_MAX 从 3.0 改成 2.0，轨迹立刻从稳定入库变成绕圈，那次踩坑让我彻底理解了量化因子在模糊控制里的分量。从那以后，我每次改动都坚持跑批量扫描，不让单条轨迹的好运气骗自己。希望帮到你，这套代码值得你花一个下午完整复现一遍。

本文还有配套的精品资源，点击获取