一、先把架构画出来：四层各放什么

架构的意义在于【谁坏了会怎样】：感知层坏 → 决策拿不到输入 → 应该停；决策层坏 → 执行收到非法值 → 应该夹住；执行层坏 → 靠物理断电兜底；供电层坏 → 前面三层全部失效，所以它是唯一必须一次做对的一层。

二、决策层：一个四状态的小状态机

不用写得很复杂，四个状态就够把本章所有能力串起来：

两个设计要点：

1. AVOID 必须写在最前面（16-2 已立的规矩）——安全逻辑一票否决，触发后本轮直接跳过其它判断。
2. STOP 才是默认状态，不是 LANE——看不见就不动。反过来说，如果默认状态是"继续按上次的动作走"，那么摄像头一被遮住，车就会以最后的速度一直冲出去。这个设计选择就是"玩具"和"机器"的分界。

三、性能预算：30fps 意味着每帧只有 33 毫秒

先把账算清楚，才知道哪里该省：

• 摄像头 30fps → 每帧 33ms（1000 ÷ 30 ≈ 33.3）
• 车道检测全流程在 Pi 5 上（320×240）约 10ms 以内
• 剩下的 20 多毫秒要留给：读超声波、算电机、以及系统调度本身的抖动

三招省时间（按性价比排序）：

① 降分辨率——最划算的一招

像素少 75%，处理时间大致按同比例下降。而车道线检测本来就不需要高分辨率——它看的是边缘梯度，不是纹理细节。320×240 是自驾车这个任务的甜点分辨率。

② 跳帧——控制环不需要 30Hz

车身的机械惯性决定了每秒 10~15 次转向更新就够了。所以可以隔一帧处理一次：

frame_count += 1
if frame_count % 2 != 0:      # 奇数帧只取不算
    continue

CPU 占用直接砍半，而开车的手感几乎没差别。这正是 11-5 / 12-5 那条【推理不必每帧都做】的老规矩。

③ 多线程——把【取帧】和【算帧】分开

单线程里，cap.read() 是阻塞的：它必须等摄像头把下一帧准备好。于是主循环的时间被"等摄像头"和"算图"两件事串起来叠加，帧率上不去。

正解是一个专线程只管取帧，主线程只管算，而且取帧线程只保留最新一帧（旧帧直接扔掉）：

class Camera:
    def __init__(self, src=0, w=320, h=240):
        self.cap = cv2.VideoCapture(src)
        self.cap.set(cv2.CAP_PROP_FRAME_WIDTH, w)
        self.cap.set(cv2.CAP_PROP_FRAME_HEIGHT, h)
        self.frame = None
        self.lock = Lock()
        self.running = True
        self.t = Thread(target=self._loop, daemon=True)
        self.t.start()

    def _loop(self):
        while self.running:
            ok, f = self.cap.read()
            if ok:
                with self.lock:
                    self.frame = f        # 只存最新一帧，旧的自动被覆盖

    def read(self):
        with self.lock:
            return None if self.frame is None else self.frame.copy()

    def close(self):
        self.running = False
        self.t.join(timeout=1)
        self.cap.release()

四、动手 Demo：完整可跑的骨架

先在场地上贴两条平行胶带当车道线（黑胶带贴在浅色地面上效果最好），把摄像头朝下前方、能看见前方 1 米左右的路面。第一版务必把 BASE 调小。

import time
from time import sleep
from threading import Thread, Lock
import cv2
import numpy as np
from gpiozero import Motor, DistanceSensor

# ================= 配置 =================
W, H = 320, 240
BASE = 0.45            # 巡航基速：第一版一定要慢（0.4 ~ 0.5）
KP = 0.6               # 比例系数
DEAD_ZONE = 18         # 死区：偏差小于 18 像素就不修（防抖）
OBSTACLE_CM = 25       # 避障阈值
STALE_SEC = 1.0        # 看门狗：超过 1 秒没成功处理就停车

# ================= 执行端（15-2） =================
left = Motor(forward=17, backward=18, enable=12, pwm=True)
right = Motor(forward=22, backward=23, enable=13, pwm=True)
sonar = DistanceSensor(echo=24, trigger=25, max_distance=4.0, queue_len=5)


def clamp(v):
    return max(-1.0, min(1.0, v))


def set_wheels(lv, rv):
    left.value = clamp(lv)
    right.value = clamp(rv)


def stop():
    left.stop()
    right.stop()


# ================= 感知端：摄像头线程 =================
class Camera:
    def __init__(self, src=0, w=W, h=H):
        self.cap = cv2.VideoCapture(src)
        self.cap.set(cv2.CAP_PROP_FRAME_WIDTH, w)
        self.cap.set(cv2.CAP_PROP_FRAME_HEIGHT, h)
        self.frame = None
        self.lock = Lock()
        self.running = True
        self.t = Thread(target=self._loop, daemon=True)
        self.t.start()

    def _loop(self):
        while self.running:
            ok, f = self.cap.read()
            if ok:
                with self.lock:
                    self.frame = f

    def read(self):
        with self.lock:
            return None if self.frame is None else self.frame.copy()

    def close(self):
        self.running = False
        self.t.join(timeout=1)
        self.cap.release()


# ================= 感知端：两个视觉函数 =================
KERNEL = np.ones((5, 5), np.uint8)
LOWER = np.array([40, 70, 70])          # 绿色（16-1）
UPPER = np.array([80, 255, 255])


def find_target_cx(frame):
    """找绿色色块，返回中心横坐标；找不到返回 None（16-1）"""
    hsv = cv2.cvtColor(frame, cv2.COLOR_BGR2HSV)
    mask = cv2.inRange(hsv, LOWER, UPPER)
    mask = cv2.erode(mask, KERNEL, iterations=1)
    mask = cv2.dilate(mask, KERNEL, iterations=2)
    contours, _ = cv2.findContours(mask, cv2.RETR_EXTERNAL,
                                   cv2.CHAIN_APPROX_SIMPLE)
    if not contours:
        return None
    biggest = max(contours, key=cv2.contourArea)
    if cv2.contourArea(biggest) <= 500:
        return None
    x, y, w, h = cv2.boundingRect(biggest)
    return x + w // 2


def detect_lane_center(frame):
    """返回车道中心横坐标；看不到车道线返回 None（16-3）"""
    gray = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY)
    blur = cv2.GaussianBlur(gray, (5, 5), 0)
    edges = cv2.Canny(blur, 50, 150)

    pts = [(0, H), (W, H), (W // 2, int(H * 0.4))]
    polygon = np.array([pts], np.int32)
    roi_mask = np.zeros_like(edges)
    cv2.fillPoly(roi_mask, polygon, 255)
    edges = cv2.bitwise_and(edges, roi_mask)

    lines = cv2.HoughLinesP(edges, 1, np.pi / 180,
                            threshold=50, minLineLength=40, maxLineGap=20)
    if lines is None:
        return None

    left_fit, right_fit = [], []
    for ln in lines:
        x1, y1, x2, y2 = ln[0]
        if x2 == x1:
            continue
        slope = (y2 - y1) / (x2 - x1)
        if abs(slope) < 0.5:
            continue
        intercept = y1 - slope * x1
        (left_fit if slope < 0 else right_fit).append((slope, intercept))

    if not left_fit and not right_fit:
        return None

    def bottom_x(fit):
        slope, intercept = fit
        return int((H - intercept) / slope)

    if left_fit and right_fit:
        lx = bottom_x(np.average(left_fit, axis=0))
        rx = bottom_x(np.average(right_fit, axis=0))
        return (lx + rx) // 2
    if left_fit:
        return bottom_x(np.average(left_fit, axis=0)) + W // 2
    return bottom_x(np.average(right_fit, axis=0)) - W // 2


# ================= 决策 + 主循环 =================
def proportional(base, center, ref):
    """偏差点 center（目标/车道中心）相对 ref（画面中心）算出左右轮速度"""
    if abs(center - ref) < DEAD_ZONE:
        turn = 0.0
    else:
        turn = KP * (center - ref) / (W / 2)      # 归一化后乘 KP
    return base + turn, base - turn                # 左快 = 右转（16-2）


cam = Camera()
mode = "STOP"
last_ok = time.time()
frame_count = 0

try:
    while True:
        # ---- ① 看门狗：处理不动了就停 ----
        if time.time() - last_ok > STALE_SEC:
            stop()
            mode = "STOP"

        # ---- ② 避障：最高优先级，一票否决 ----
        if sonar.distance < OBSTACLE_CM / 100.0:
            mode = "AVOID"
            stop()
            sleep(0.15)
            set_wheels(-0.35, -0.35)      # 后退
            sleep(0.30)
            set_wheels(0.40, -0.40)       # 原地右转（左正右负 = 右转）
            sleep(0.35)
            stop()
            last_ok = time.time()
            continue

        # ---- ③ 取帧（跳帧：奇数帧不算） ----
        frame = cam.read()
        frame_count += 1
        if frame is None or frame_count % 2 != 0:
            sleep(0.01)
            continue

        # ---- ④ 先找色块，再找车道 ----
        cx = find_target_cx(frame)
        if cx is not None:
            mode = "TRACK"
            lv, rv = proportional(BASE, cx, W // 2)
            set_wheels(lv, rv)
        else:
            lane = detect_lane_center(frame)
            if lane is None:
                mode = "STOP"             # 安全默认：看不见就不动
                stop()
            else:
                mode = "LANE"
                lv, rv = proportional(BASE, lane, W // 2)
                set_wheels(lv, rv)

        last_ok = time.time()
        sleep(0.02)

except KeyboardInterrupt:
    pass
finally:
    stop()                    # ★ 15-4 的规矩，绝不能省
    sonar.close()
    cam.close()
    cv2.destroyAllWindows()

看到什么算成功：

场地：贴两条平行胶带的浅色地面，车放在车道中央
期望：车沿车道走一段不压线；手掌挡在车前 25cm 内 ->
      停车、后退、转开，然后继续找车道
地面无胶带/摄像头被遮 -> 车停下（进入 STOP），而不是乱冲

读结果与首调顺序（按这个顺序，别跳步）：

1. 车完全不动 → 先查 sonar.pin_factory 不是 MockFactory、left.value/right.value 有没有越界、BASE = 0.45 能不能推动车（15-2 的静摩擦）。
2. 车在车道里画龙（左右猛摆） → 降 KP（0.6 → 0.35），或加大死区（18 → 28）。先动这两个，别去改视觉参数。
3. 车总是往一边偏 → 摄像头装歪了或镜头畸变。用一个常数补偿（在 proportional 里给 turn 加个固定偏移），比反复调视觉参数有效。
4. 明明有车道线却进入 STOP → 打开 16-3 的可视化窗口看 roi_edges，先确认 Canny 和 ROI 正常，再怀疑车道线本身（对比度太低？胶带颜色太接近地面？）。
5. 跑一段后突然失控 → 大概率是电源（15-4 的老账：电机一转树莓派就重启）或过热降频。先量电压，再看代码。

五、30 秒实验：速度与容错，只能选一个

把 BASE = 0.45 改成 BASE = 0.8，其余一切不动，在场地上重跑。

预期：车明显更容易冲出车道、画龙幅度更大、避障后回到车道的时间变长——参数一个没改，只是快了。

为什么：从"看见偏差"到"轮子真正改变车速"之间存在总延迟（取帧 + 计算 + 串口/GPIO + 电机响应），基速越快，这段延迟内车已经跑出去的距离越长，而比例控制修正的是"当前看到的误差"，等它生效时车早就偏更多了——偏差被放大，于是振荡。

结论一句话：自驾车的第一版必须慢。 想跑快，要先把延迟压下来（降分辨率、跳帧、提高 KP 的稳定裕度），或者上更高级的控制（PID 里的 D 项就是专门用来对付这种滞后振荡的）。

六、安全兜底：三层，一层都不能少

做完任何一次上车实验，都要亲手测一遍这三层：按 Ctrl+C 看它停不停、用手遮住摄像头看它停不停、拍一下电池开关看它断电。测试停车比测试开车重要。

七、常见误区

1. 【把三个感知全塞进一个大循环里一起算】——CPU 只有那么多，三个全开的结果是帧率掉到个位数，车反应迟钝。按场景选一个主感知（场上跑赛道就用车道检测），其余当兜底。
2. 【功能越"聪明"越好】——本章的每一种感知都有失效场景（16-3 明确列过）。可靠性来自"失效时知道要停"，不来自"功能清单很长"。
3. 【摄像头掉了没关系，继续按上次的动作走】——这是最危险的写法。默认状态必须是 STOP，任何"拿不到输入"的分支都要通向停车。
4. 【BASE 拉满才叫性能好】——基速越快，同样的延迟带来的偏差越大，越容易振荡失控（第五节实验亲测）。第一版一律慢速。
5. 【能跑起来就算成功】——要验证的是【沿车道跑完一段不压线、遇障能停】，而不是【轮子会转】。验收标准要写清楚，否则你会以为"能动"就是"做好了"。
6. 【调参全凭感觉乱改】——顺序很重要：先修机械与装配（摄像头角度、电源），再调 KP 与死区，最后才碰视觉参数（Canny／Hough）。反着来的话，你会在视觉参数上白耗一整天。
7. 【状态机是多余的心智负担】——恰恰相反。没有状态机，四件事会互相打架（避障时还在追色块、看不见路还在按上次速度跑）。状态机就是把"此刻该听谁的"写成代码，它是最便宜的安全装置。

这本教材到此，全部完结。 🎉

十六章节链路（回头看一眼自己走过的路）：

三个【带得走的】东西（比任何一个库都值钱）：

1. 先断电的习惯——接线之前先断电，是所有硬件实验的第零步；
2. 先核实再照抄的习惯——教材永远比软件老，老教程的每一条命令都先问一句【现在还这样吗】；
3. 默认安全的习惯——任何会动的东西（车、电机、灯）都要有它自己的【停下来】，拿不到可靠输入就不动。

• ☑ 第十六章 完结 —— 教材这一章已读完四节：用 HSV 掩膜让程序认得出彩色目标并算出它的中心；给车装上超声波与比例控制，让它既能避开障碍、又能追着目标走；再用 Canny 与霍夫把一堆车道边缘变成方向盘上的一个数；最后以状态机、看门狗与差速执行为骨，拼成一台看不见路就会停下的自走车
• ☑ 全书完结 —— 《树莓派超应用圣经》16 章已全部读完 🎉

相关笔记

• OpenCV 色彩检测与追踪 —— 本节 find_target_cx() 的出处
• 打造自动避障和物体追踪车 —— 超声波、比例控制与差速转向的出处
• 车道自动检测系统 —— 本节 detect_lane_center() 的出处
• 认识树莓派智能车 —— 感知／决策／执行／供电四层架构的出处
• 打造树莓派 WiFi 遥控视频车 —— finally: stop() 与电源铁律的出处
• AI 实验范例：TensorFlow Lite+Webcam 实时物体识别 —— 【推理不必每帧都做】的第一生存法则
• 🍓 树莓派 · 索引 —— 全课路线图