自动驾驶决策规划控制：分层接口、PID/LQR/MPC与OCR资料化

简介：《决策规划控制讲义.pdf》是一份面向自动驾驶初学者与算法进阶者的课程配套讲义，围绕决策规划控制这一核心模块，帮助读者建立从环境理解到车辆动态执行的整体知识框架。压缩包内含1个PDF文件，大小约69.2MB，适合作为课堂笔记补充或自学查阅材料。讲义内容覆盖路径规划、行为选择与轨迹生成三个决策步骤，并延伸到PID、LQR与模型预测控制等控制算法的适用边界；同时对雷达、激光雷达、摄像头等多传感器融合，以及卡尔曼滤波、粒子滤波的状态估计思路作了梳理。文中还涉及故障检测与隔离、应急策略和实时性约束，结合手写笔记中的案例与实验，便于把理论映射到实际系统设计。目前已有973人学习，适合希望系统补齐自动驾驶决策规划控制知识链路的学习者参考。

1. 从一张图片型 PDF 到能跑的三层链路：这份讲义真正解决的是接口问题

很多做自动驾驶的朋友拿到课程资源的第一反应是翻到公式页截图存档，然后再也没打开过。《决策规划控制讲义.pdf》是一份由手机扫描类工具导出的图片型 PDF，没有文字层、不能选中复制、全文检索一个字都搜不到，翻页全靠肉眼。可它有价值的地方恰恰不在那些公式推导，而在于它按顺序把决策规划控制的层间接口讲了一遍：全局路径给行为决策交什么、行为决策给轨迹生成交什么、轨迹生成又给控制算法交什么。这份材料适合已经能写 PID、但一上车就发现规划和控制对不上拍的从业者，也适合正在啃自动驾驶课程、公式都认识但连不成系统的人。下面按我自己的处理顺序，从拆骨架、落控制算法，到把扫描件变成可检索资料，一步步拆开。

2. 讲义的分层骨架：把路径规划、行为决策、轨迹生成拆成三条独立流水线

2.1 为什么先拆接口而不是先抄公式

讲义里的公式密度很高，从五次多项式到 Frenet 坐标系的横向偏移，一页能出现七八个符号。直接按页抄，最后大概率是"每个符号都认识，连起来不知道往哪放"。我一般会先把整份材料当成一份接口说明书来读：每一层只回答三个问题——吃进来什么、吐出去什么、多久吐一次。

这三个问题定下来，算法反而是最容易替换的部分。同一套接口，横向规划从五次多项式换成 Lattice Planner，纵向从 IDM 换成 MPC，上层的行为决策完全不用改代码；反过来，如果一开始就扎进某个算法的实现细节，等你想换算法时会发现调用它的地方散落在十几个文件里，改一处崩三处。讲义在这一点上是清楚的，它把决策规划切成路径规划、行为选择、轨迹生成三个动作，再单独拿一节讲控制算法如何接住轨迹，这个切法本身就是工程化的。

时间尺度是第二个必须先对齐的东西。全局路径规划只在上车或偏航时算一次，1 Hz 都算高频；行为决策通常在 5～10 Hz，因为它要跟着感知的帧率走；轨迹生成 10～20 Hz，输出未来 3～8 秒的带时间戳轨迹；控制环 50～100 Hz，因为它要压住车辆的动态响应。这四档频率如果混在一起，最常见的表现就是"仿真里特别顺，实车一上就抖"，抖的根源往往不是算法本身，而是某一层用了别的层的时间基准。

2.2 把讲义目录重建成一张输入-输出表

扫描件的目录页往往只有章节名，没有字段级的说明。我的做法是边翻边往一张表里填，填不满的地方就是后面要回头细读的地方。填完之后，这份讲义就从"一份要读的材料"变成了"一份可以对照实现的规格说明"。

表里最容易填错的是"轨迹生成"那一行的输出。很多人写成"一条路径"，这是不对的。路径只有几何，没有时间；轨迹必须带时间戳，否则控制层没法算前馈加速度，也没法做时域对齐。讲义在讲轨迹生成时反复强调平滑性，平滑不只是曲率连续，还包括速度曲线的一阶、二阶导连续，这一点在后面做闭环验证时会直接体现出来。

还有一列我没放进表里，但读的时候要单独记：每一层的失败模式。全局规划失败是找不到路，行为决策失败是卡死或反复横跳，轨迹生成失败是无解或轨迹穿越障碍物，控制失败是跟踪超调。把失败模式和层级对上，排查的时候能少走很多弯路。

2.3 用一份 YAML 把三层串起来

读完骨架，下一步是把它固化成配置。我习惯用一份 YAML 描述整个链路，好处是算法代码里不再出现魔法数字，换车型、换场景只改配置。下面这份是我按讲义的分层思路整理的模板，字段名可以直接照抄，值按自己的车改。

# planning_control.yaml —— 决策规划控制链路的最小配置 map: frame: "map" # 全局坐标系 reference_line: "lane_0021" # 参考线 ID，Frenet 坐标的基准 vehicle: wheelbase: 2.70 # 轴距，单位 m，LQR 和运动学模型都要用 max_steer: 0.52 # 前轮最大转角，单位 rad，约 30 度 max_accel: 2.0 # 最大加速度 m/s^2 max_decel: -4.0 # 最大减速度，紧急工况才用到 planning: global: algorithm: "hybrid_astar" # 全局用 Hybrid A*，因为它考虑车辆最小转弯半径 resolution: 0.5 # 栅格分辨率 m，越小越精细也越慢 behavior: rate_hz: 10 # 行为决策频率 horizon_s: 5.0 # 决策展望时域 trajectory: rate_hz: 20 # 轨迹生成频率，必须 >= 控制频率的 1/5 horizon_s: 4.0 dt: 0.05 # 轨迹采样间隔，与 rate_hz 互为倒数 control: algorithm: "lqr" # 可选 pid / lqr / mpc rate_hz: 100 # 控制环频率，实车常见 50~100 lqr: q_diag: [4.0, 1.0, 2.0, 0.5] # 横向误差、横向误差率、航向误差、航向误差率 r_diag: [1.0] # 前轮转角惩罚，越大越保守

这份配置里最值得盯的是rate_hz和dt的对应关系。轨迹生成的dt是 0.05，控制频率 100 Hz，两者正好互为倒数，意味着控制每一步都能拿到一个新的轨迹点，不需要插值。如果轨迹 20 Hz、控制 100 Hz，中间就有 4 个点要靠插值补出来，插值方式选得不对会直接引入相位滞后，表现是高速段画龙。

q_diag的顺序也要和状态量顺序严格一致。我见过不止一次因为把航向误差和横向误差的位置写反，导致车在直道上左右小幅摆动、在弯道反而很稳，排查了半天才发现是权重配错了位置。

2.4 Frenet 与笛卡尔：坐标和单位必须先统一

讲义在讲轨迹生成时用的是 Frenet 坐标系，把二维平面拆成沿参考线的纵向 s 和垂直参考线的横向 l。这个拆法在结构化道路上非常好用，因为横向和纵向的运动约束可以分开写，横向管舒适性和车道保持，纵向管跟车距离和限速。但它有个前提：参考线本身的曲率不能太大。

实际处理时，Frenet 到笛卡尔的转换要经过参考线上的投影点。参考线曲率 κ 较大的地方，同样的横向偏移 l 对应的实际弧长会被放大，误差量级和 κ·l 相关。常见的做法是把横向偏移限制在 ±2 m 以内，超出这个范围就切回笛卡尔坐标直接算，或者重新选参考线。我在一个曲率半径 15 m 的地库场景里吃过这个亏：按 ±3 m 的横向范围做规划，车在弯心附近会突然向内侧偏半个车身，原因就是 Frenet 近似在强曲率下失效，不是控制器的问题。

单位统一同样是个隐形坑。讲义的公式里角度有时用弧度、有时用度，速度有时用 m/s、有时用 km/h，扫描件里这些单位经常只写在上一页或者边角批注里。我一般会在读完之后，把所有涉及单位的公式单独列一页，统一成 SI 单位再往下做。这个动作花不了二十分钟，但能省掉后面一两天的排查。

3. 控制算法落地：PID、LQR、MPC 的参数整定与量纲对齐

3.1 PID：讲义没展开的积分限幅与微分先行

PID 是三个算法里最容易写、也最容易写错的。讲义里给的还是标准形式，但车上直接用会出两个问题：积分饱和和微分冲击。前者出现在长时间跟踪不上目标的时候，积分项越积越大，一旦目标追上，输出会猛地冲过头；后者出现在目标值阶跃跳变时，微分项对误差求导会瞬间打出一个尖峰。

我一般用下面这个版本，核心改动是三处：积分项先累加再限幅、微分改成对测量值求导（微分先行）、输出统一饱和。

import numpy as np class PID: """带积分限幅与微分先行的位置式 PID""" def __init__(self, kp, ki, kd, dt, u_min, u_max, i_max=None): self.kp, self.ki, self.kd, self.dt = kp, ki, kd, dt self.u_min, self.u_max = u_min, u_max # 积分限幅默认取输出量程，避免积分项单独把输出顶出边界 self.i_max = i_max if i_max is not None else (u_max - u_min) self.integral = 0.0 self.prev_meas = None def reset(self): self.integral = 0.0 self.prev_meas = None def step(self, target, meas): err = target - meas # 先累加再限幅，防止长时间饱和后积分继续膨胀 self.integral += err * self.dt self.integral = float(np.clip(self.integral, -self.i_max, self.i_max)) d = 0.0 if self.prev_meas is not None: # 微分先行：对测量值求导，负号表示测量值上升时抑制输出 d = -(meas - self.prev_meas) / self.dt self.prev_meas = meas u = self.kp * err + self.ki * self.integral + self.kd * d return float(np.clip(u, self.u_min, self.u_max))

这段代码里，dt必须和实际调用周期严格一致，不能写成 0.01 而实际按 0.02 调用，否则积分项和微分项会同时偏一倍。kp的量纲取决于输出是什么：如果输出是前轮转角（rad），输入是横向误差（m），那kp的单位就是 rad/m，量级通常在 0.05～0.3 之间；如果输出直接是方向盘角度，还得乘一个转向传动比。这个映射关系讲义里通常不写，但它是"仿真能跑、实车画龙"的头号原因。

ki的作用是消稳态误差，但在有前馈的横向控制里，前馈已经把主要误差吃掉了，ki只需要很小一点，量级在kp的 1/50 到 1/20。kd用来压超调，太大对噪声敏感。我的经验是先只调kp到轻微超调，再加kd压下去，最后加一点点ki收尾。跳过前两步直接三参数一起调，基本调不出来。

3.2 LQR：离散 Riccati 迭代与 Q/R 的量纲对齐

LQR 比 PID 多了一个"用模型"的能力，代价是你要先有一个线性化模型。横向控制里常用的是基于车辆运动学或动力学的误差模型，状态量取横向误差、横向误差率、航向误差、航向误差率，控制量取前轮转角。模型线性化在参考点附近做，参考点通常取当前车速和当前曲率。

求解上，连续时间 Riccati 方程在工程里一般直接离散化迭代，收敛快、实现短。

import numpy as np def dlqr(A, B, Q, R, max_iter=2000, eps=1e-9): """离散代数 Riccati 迭代，返回最优增益 K 与解 P""" P = Q.copy() for it in range(max_iter): # K = (R + B^T P B)^-1 B^T P A K = np.linalg.solve(R + B.T @ P @ B, B.T @ P @ A) P_next = Q + A.T @ P @ (A - B @ K) if np.max(np.abs(P_next - P)) < eps: return K, P_next P = P_next # 不收敛通常是 (A, B) 不可控，或者 Q/R 不是正定矩阵 raise RuntimeError(f"Riccati 迭代 {max_iter} 次未收敛，检查 A/B 可控性与 Q/R 正定性")

用np.linalg.solve而不是显式求逆，是因为求逆在R + BᵀPB接近奇异时会放大数值误差，逆矩阵算出来的 K 在实车上表现是"偶尔蹦一下"。迭代判据用P的变化量而不是固定次数，能省掉很多无谓的循环。

真正难的是Q和R怎么填。这里的坑不是数值大小，而是量纲。横向误差单位是米，航向误差单位是弧度，两者数量级差了几十倍，如果Q的对角元素都填 1，等于变相把航向误差的权重压到几乎为零。常见做法是先把每个状态量除以它的期望精度，做成无量纲量，再取对角阵。比如横向误差希望控制在 0.1 m，就取 1/0.1² = 100；航向误差希望控制在 0.02 rad，就取 1/0.02² = 2500。这样填出来的Q才有物理意义。

R决定控制量被惩罚的力度，越大越保守、转角变化越平缓，越小越激进。实车上我一般从 1 起步，如果方向盘抖动明显就往上加一个数量级，如果弯道跟不上就往下调。

还有一个容易被忽略的点：LQR 增益是随车速变化的。低速和高速用同一组Q、R算出来的 K，高速段一定不稳。常见做法是按 10 km/h 一档做增益调度，把不同车速下的 K 预先算好存成表，运行时线性插值。

3.3 MPC：预测时域、约束与求解器选择

MPC 相比前两者最大的不同是把约束显式写进去了：前轮转角有物理限幅，加速度有舒适性上限，轨迹不能穿越障碍物。讲义在讲这一节时强调了"考虑未来一段时间内的系统动态"，落到实现上就是三件事——预测时域 N、约束、滚动优化。

import cvxpy as cp def mpc_step(A, B, x0, N, Q, R, u_max, x_ref=None): """单步滚动优化，返回第一个控制量（receding horizon）""" nx, nu = B.shape x = cp.Variable((nx, N + 1)) u = cp.Variable((nu, N)) cost = 0 cons = [x[:, 0] == x0] x_target = 0 if x_ref is None else x_ref for k in range(N): cost += cp.quad_form(x[:, k] - x_target, Q) cost += cp.quad_form(u[:, k], R) cons += [x[:, k + 1] == A @ x[:, k] + B @ u[:, k]] cons += [cp.abs(u[:, k]) <= u_max] # 控制量硬约束 prob = cp.Problem(cp.Minimize(cost), cons) # OSQP 对这类二次规划收敛稳定，warm_start 能显著降低单次求解耗时 prob.solve(solver=cp.OSQP, warm_start=True) return None if u.value is None else u.value[:, 0]

时域 N 的选取和dt强相关。N·dt 是实际往前看的物理时间，一般取 1～2 秒。N 取 20、dt取 0.05，看 1 秒；N 取 20、dt取 0.1，看 2 秒。前者更精细但算得慢，后者快但可能漏掉短时扰动。实车 100 Hz 控制环里，MPC 通常跑不到 100 Hz，常见做法是 MPC 跑 20～50 Hz，中间用上一帧的解做插值或者保持。

求解器方面，OSQP 适合中小规模 QP，速度稳定；如果模型是非线性的，就得换 IPOPT 或者 Acados，单次求解从毫秒级涨到十毫秒级。这里有个血泪经验：prob.solve()返回None的时候不要直接抛异常，实车上偶发的求解失败应该走"保持上一帧控制量 + 限幅衰减"的降级路径，否则一次求解失败就是一帧控制输出为零，车会明显顿一下。

3.4 三种算法的边界：什么场景该换

PID 的边界是"误差小、模型弱"。车道保持、低速泊车这类场景它够用，因为它不需要模型，也不怕模型不准。但一旦进入高速大曲率或者需要前瞻的场景，它的相位滞后会很明显，表现为入弯晚、出弯冲。

LQR 的边界是"线性、无约束"。它在结构化道路的横向控制上表现很好，算得快、增益可以离线算好，实车上很多量产方案用的就是 LQR 加前馈。但它处理不了约束，前轮转角打满之后它不会"知道"自己已经打满了，输出的指令会继续增大，只能靠后级限幅，这时候闭环特性就变了。

MPC 的边界是"算力"和"模型精度"。它能把约束显式写进去，也能做多目标优化（跟踪精度、舒适性、能耗一起优化），但代价是每个周期都要在线求解，而且模型不准的时候它的表现会比 LQR 更差——因为它更"相信"模型。如果车辆动力学参数（轮胎侧偏刚度、质心位置）标定不准，MPC 在极限工况下反而不如一个调得好的 PID。

选型的实际判断顺序我一般是：先看有没有硬约束要处理，有就上 MPC；没有约束但车速范围宽、需要增益调度，用 LQR；场景简单、算力紧张、或者只是做原型验证，PID 足够。

4. 扫描件转可检索资料：从 pdffonts 到带标签 JSONL 的处理路径

4.1 先判断这份 PDF 有没有文字层

同样叫 PDF，图片型和文本型在处理上是两套完全不同的流程。文本型可以直接抽文字、搜关键词；图片型必须先转图再做识别，中间每一步都可能引入误差。判断方法很简单，两条命令就够。

# 1. 基本信息：页数、页面尺寸、是否加密 pdfinfo 决策规划控制讲义.pdf # 2. 字体表：只有表头没有字体行，基本可判定为无文字层的扫描件 pdffonts 决策规划控制讲义.pdf # 3. 试探性抽文本：图片型 PDF 这里通常是空的，或者只有零星页眉 pdftotext -f 1 -l 5 决策规划控制讲义.pdf - | head -40 # 4. 看内嵌图片对象：每页一张大图，进一步确认是扫描件 pdfimages -list 决策规划控制讲义.pdf | head -20

pdffonts输出为空是最强的信号。有些扫描工具会塞一层不可见的 OCR 文本，这种情况下pdftotext能抽出字，但抽出来的内容顺序混乱、公式全是乱码，直接用反而更危险，因为它会给你一种"这份文件可以搜"的错觉。

4.2 分页转图：DPI 与灰度化的取舍

确认是图片型之后，下一步是分页转图。分辨率的选择是个权衡：150 DPI 转出来体积小、识别快，但小字号的上下标和手写批注会糊成一团；600 DPI 清晰，但单页图片可能到十几兆，识别耗时翻几倍。扫描件的常用起点是 300 DPI，公式密集的页面可以单独提到 400～450。

mkdir -p pages # -r 300 指定 DPI；-gray 转灰度，体积减半且公式线条更干净 # -png 无损，避免 JPEG 压缩在细线条上产生振铃 pdftoppm -r 300 -gray -png 决策规划控制讲义.pdf pages/p # 确认输出：文件名形如 p-01.png、p-02.png，页码补零便于排序 ls pages | head

页码补零这件事看着小，但不做的话p-10.png会排在p-2.png前面，后面按文件名排序生成目录时章节顺序会整个错乱。这种错误在通读时很难发现，因为它只影响跨页公式的拼接。

4.3 OCR 之后做结构化：给每页打上章节标签

OCR 只给你一堆文字，离"能用的资料"还差一步结构化。我的做法是每页输出一条 JSON 记录，带上页码、正文、低置信度片段，再单独维护一张"页码到章节"的映射表。这样后面要查某个算法，直接按章节字段过滤，不用重跑 OCR。

import glob import json import os from paddleocr import PaddleOCR # use_angle_cls 处理扫描歪斜；lang='ch' 覆盖中英混排 ocr = PaddleOCR(use_angle_cls=True, lang="ch") records = [] for img in sorted(glob.glob("pages/*.png")): page_no = int(os.path.basename(img).split("-")[-1].split(".")[0]) res = ocr.ocr(img, cls=True) lines = [] for block in (res[0] or []): box, (text, score) = block lines.append({"text": text, "score": round(score, 3), "y": box[0][1]}) # 按纵坐标排序还原阅读顺序；双栏排版要先按 x 分列再各自排序 lines.sort(key=lambda d: d["y"]) records.append({ "page": page_no, "text": "\n".join(l["text"] for l in lines), # 置信度低于 0.85 的片段单独拎出来，人工复核成本最低 "low_conf": [l["text"] for l in lines if l["score"] < 0.85], }) with open("notes.jsonl", "w", encoding="utf-8") as f: for r in records: f.write(json.dumps(r, ensure_ascii=False) + "\n")

low_conf这个字段是整段代码里最值钱的部分。扫描件里最容易被识别错的就是公式下标、希腊字母和手写批注，而这些恰恰是理解推导的关键。把低置信度片段单独列出来，人工只需要复核几十条，而不是通读几百页。

双栏排版的处理要单独说明。上面按纵坐标排序只适用于单栏，双栏页面如果直接按 y 排，左右两栏的文字会交错在一起，读起来完全不通。简单做法是先按 x 坐标把框分成左右两组，再各自按 y 排序拼接。讲义里如果有跨栏的公式或者表格，这一招会失效，只能靠页码加人工校对。

4.4 公式与手写批注的处理边界

通用 OCR 对行内公式的识别率不高，lqr被认成IQR、下标 0 被认成字母 o、积分号被认成 S，都是常事。这里要接受一个现实：公式部分不要指望 OCR，能识别的只有正文叙述和章节标题。

我一般的处理边界是：正文用 OCR 做全文检索用，公式部分保留原图，按"页码 + 区域坐标"存成小图，在笔记里用链接引过去。手写批注同理，扫描件上的手写内容往往是主讲人临场补充的重点，比印刷体更有信息量，但识别率也最低，保留原图是更稳的选择。

做完这一步，这份讲义就从"只能用眼睛看的 PDF"变成了"能搜、能按章节跳转、能对着写代码"的资料。整个流程跑一遍，三百页左右的扫描件大概需要半小时到一小时，主要时间花在低置信片段的人工复核上。

5. 避坑与排查：复现讲义内容时最容易翻车的五处

5.1 把 Frenet 的 s 当成笛卡尔弧长直接用

现象：直道上跟踪很稳，一进大曲率弯道横向误差突然涨到半米以上，控制器输出打满也拉不回来。

原因：Frenet 坐标是参考线上的局部近似，s 是沿参考线的投影长度，l 是垂直偏移。参考线曲率大的位置，横向偏移对应的实际弧长被 κ·l 放大，把 s 和真实弧长等同起来，等于是给规划层喂了一个带系统偏差的位置。

解决：把横向偏移限制在 ±2 m 以内，超出就切回笛卡尔坐标直接算，或者重选参考线。另外在曲率超过阈值（比如 0.1 /m）的路段，规划周期要缩短，别用直道的参数硬套。

5.2 控制量和执行器量纲没对齐

现象：仿真里跟踪误差很小，实车上方向盘小幅高频抖动，乘客明显不适。

原因：控制器输出的"前轮转角"和底盘接收的"方向盘角度"之间差一个转向传动比，而且这个比值不是常数，随车速和转角变化。直接把控制器的输出当方向盘角发下去，等于给闭环里串了一个未知增益。

解决：在控制器输出之后加一层明确的映射，把前轮转角换算成方向盘角，比值从车辆标定表里查，别用固定常数。映射层的位置要固定，不能一部分在控制器里做、一部分在下发层做。

5.3 规划频率和控制频率没做时域对齐

现象：低速段正常，速度上到 60 km/h 以上开始画龙，频率大概 1～2 Hz。

原因：轨迹 20 Hz、控制 100 Hz，中间缺的 4 个点靠零阶保持补，等价于给系统加了一个半个轨迹周期的纯滞后。速度越快，同样的滞后的相位损失越大。

解决：要么把轨迹频率提到和控制频率匹配，要么在中间加一阶或线性插值，并且插值要用时间戳对齐而不是帧序号对齐。时间戳来源要统一，别一部分用系统时钟、一部分用传感器时钟。

5.4 OCR 结果直接当原文用

现象：按识别出来的文字搜"LQR"，一条都搜不到；按"lQR"搜到了，但上下文完全对不上。

原因：公式和下标是 OCR 的重灾区，LQR、MPC这类全大写缩写经常被认成IQR、MPC被认成MPC（看起来一样但字符编码不同，比如全角半角混用），搜的时候匹配不上。

解决：识别完之后做一轮归一化，把常见的误识字符映射回去，全角转半角，统一大小写。归一化之后再做全文检索。同时保留原始识别结果，别覆盖，方便对照。

5.5 章节顺序错乱导致公式推导断链

现象：读到某一页公式，发现里面引用的符号在前一页根本没定义，往前翻几页也找不到。

原因：扫描件常见的问题是缺页、双面扫描顺序反了、或者中间夹了一张空白页导致页码偏移。转图之后按文件名排序，页码是连续的，但内容和页码对不上。

解决：转图之后先做一遍页数核对，和pdfinfo报的总页数比。再抽查几个章节起始页，确认标题和页码的对应关系。如果发现偏移，建立一张"物理页码到逻辑页码"的映射表，后面所有引用都走这张表。

6. 闭环验证：用 Pure Pursuit 把讲义里的三段串成一条可测的曲线

前面拆完骨架、落完算法，最后一步是验证这份讲义里的东西到底能不能串起来跑。我一般不上复杂的仿真环境，先用一个最简单的自行车模型加 Pure Pursuit 做闭环，能跑通再往上加东西。这样做的原因是：链路里任何一环没接上，这个最小闭环都会立刻暴露出来，而且排查范围小。

import numpy as np def bicycle_step(state, delta, v, L, dt): """运动学自行车模型：状态为 (x, y, theta)""" x, y, theta = state x += v * np.cos(theta) * dt y += v * np.sin(theta) * dt theta += v * np.tan(delta) / L * dt return np.array([x, y, theta]) def pure_pursuit(state, path, L, ld): """在参考路径上找前视点，反解前轮转角""" x, y, theta = state # 从路径上找距离自车 ld 的点作为前视点 d = np.hypot(path[:, 0] - x, path[:, 1] - y) idx = int(np.argmin(np.abs(d - ld))) tx, ty = path[idx] alpha = np.arctan2(ty - y, tx - x) - theta # 前视距离 ld 越大越平顺但入弯越晚，一般取 1.5~3 倍轴距 delta = np.arctan2(2 * L * np.sin(alpha), ld) return float(np.clip(delta, -0.52, 0.52))

这段代码里有两个参数值得单独说。ld是前视距离，它直接决定跟踪特性：取小了车会贴着路径走但抖，取大了平顺但入弯晚。经验值是ld取 1.5～3 倍轴距，速度越高取越大，常见做法是按车速做线性映射。dt是仿真步长，不是控制周期，两者别混，仿真步长取 0.01～0.02 就行，控制周期按配置里的 100 Hz 走。

跑的时候我会记录三组数：最大横向误差、稳态横向误差、前轮转角变化率。前两个看跟踪精度，第三个看舒适性。下面是我在一个半径 25 m 的定圆场景里跑不同前视距离的结果，车速固定 30 km/h，轴距 2.7 m。

这张表最直观的结论是：误差不是随前视距离单调变化的，中间有个最优区间。4 m 时误差大是因为前视太近，控制器一直在追身后的点，等效于增益过高；10 m 时误差大是因为前视太远，车提前切内道。这个"两头大中间小"的形状，和讲义里讲轨迹平滑性时提到的欠阻尼与过阻尼是一回事，只是一个在参数域上看，一个在时域上看。

如果这个闭环跑出来的横向误差是你预期的两三倍，先别改控制器参数，按顺序查三件事：参考线上取的前视点是不是按弧长算的（按索引算在曲率变化处会跳变）、坐标变换有没有做（路径和自车是不是在同一个坐标系）、时间戳是不是对齐的。这三个查完，八成的异常都能定位到。

从那以后我每次拿到一份扫描版讲义，都先跑一遍pdffonts确认有没有文字层，再决定要不要上 OCR；每落一个控制算法，都先用自行车模型跑一遍定圆，把误差曲线画出来再往实车上搬。这两个习惯让我少熬了很多夜。希望帮到你。

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