一次被尊重的面试之后：重新梳理我的 AGV 系统

在正文开始之前，我想先感谢一位面试官。

那次面试里，我很紧张，表达也有不够顺畅的地方。但他始终尊重我，在面试的最后，还给了我一些建议，比如继续更新博客、更新 GitHub，把自己的学习过程和项目工作整理出来。

这些建议对我很有帮助。做过的东西需要认真复盘：当时为什么这样设计，模块之间怎样配合，遇到了哪些问题，还有哪些地方需要改进。把这些内容写清楚，也能让我在下一次介绍项目时表达得更有条理。

所以，我想从这篇文章开始，把 AGV 项目重新梳理一遍。后面的内容围绕 Linux 感知与上层控制、STM32 底盘实时执行，以及两者之间的数据与任务协作展开。我的个人工作主要在 Linux 端，底盘和云端部分会说明各自的分工。

这里记录的是一份系统改进方案。文中的线程优先级、周期、队列容量和超时等数值属于方案配置，性能表现需要结合实际测试记录来看。后续更新博客和 GitHub 时，我也会持续补充实现过程、遇到的问题与验证结果。

感谢这位面试官的尊重和建议。这次更新，就是我对这些建议的一次回应。

AGV 嵌入式建图与云端策略执行系统

整理日期：2026 年 10 月 14 日。

1 项目概述与个人分工

本项目面向实验室物流取送场景，完成了Linux车端感知、温湿度补偿、传感器融合、栅格建图、云端任务与策略对接、局部闭环执行、界面展示和事件记录。系统以i.MX6ULL Linux开发板作为感知与上层控制平台，以STM32F407和FreeRTOS作为底盘实时控制平台，两块板通过500kbps CAN通信。

四路SDAM单点激光通过I²C接入Linux，分别观测前、左、后、右四个方向；板载ICM20608通过SPI提供加速度和角速度；SHTC3通过I²C提供温湿度。STM32采集HC-SR04超声原始回波时间和编码器测速/计数数据，每次获得新结果后通过CAN实时上传。Linux根据温湿度计算补偿后的超声距离，再与同方向激光数据融合，输出四方向障碍距离、质量和有效状态。车辆的位姿、速度、移动距离和累计行驶里程由IMU、编码器及有效测距约束共同计算。

系统启动后先准备栅格地图。现场建图模式下，车端执行低速采样与栅格建图，完成地图审核、冻结和云端确认后进入任务阶段；预设地图模式下，系统加载云端地图，按地图摆放障碍物并确认布局和初始位姿后进入任务阶段。云端只向地图已就绪的车辆发布取送任务与对应策略。

2 总体架构与主要数据流

已有云端服务
    地图保存 / 预设地图下发 / map_ready确认
    地图就绪后：取货点、送货点与策略发布
                         ↕ MQTT
i.MX6ULL Linux
    四路SDAM ─ I²C ──────→ 激光方向观测 ───────────────┐
    SHTC3 ─── I²C ──────→ 温湿度 → 湿空气声速 ──┐     │
    HC-SR04原始回波时间 ← CAN ──────────────────→ 补偿距离
                                                   ↓
                                      四方向距离融合与质量
    板载ICM20608 ─ SPI → 角速度/加速度 ──┐           ↓
    编码器计数/速度 ← CAN ────────────────┴→ 位姿/速度/里程
                                                   ↓
                                      栅格建图与地图就绪
                                                   ↓
                                      云端策略执行/局部跟踪
                                                   ↓
                                      command运动请求仲裁
    LVGL显示 / SQLite记录                           ↓
                         ↕ CAN 500kbps
    下行：速度、运行许可、心跳、停止/复位请求
    上行：超声Echo原始测时、编码器计数、底盘状态与故障
                         ↕
STM32F407 / FreeRTOS
    编码器 → 定时采样 → 本地电机闭环 → PWM/方向/使能
    四路HC-SR04 → 顺序触发 → 定时器捕获Echo → 原始测时上报
    通信/故障检查 + 原始回波门限 → 最终运行许可

激光数据在Linux本地采集和处理。超声原始测时和编码器数据通过CAN上传，Linux统一完成补偿、距离融合和位姿估计。上报新测量值、融合状态更新、地图准备和任务执行构成连续的数据处理链。

STM32保留电机闭环与通信超时处理；原始回波近障门限作为底盘附加保护。正式的四方向距离结果由Linux融合模块发布。

3 硬件连接与运行参数

3.1 硬件接口与地址

四路SDAM已分别通过串口提前配置为0x71～0x74，断电后地址仍保持，以上地址保持行为由本人实测确认。正常运行时，Linux直接使用改后的I²C地址访问四个模组。

SDAM手册标注测量范围为0.2～20m。四路采用稳定3.3V供电，典型工作电流合计约0.4A，供电留有启动和波动余量。HC-SR04使用与模组要求匹配的供电，Echo进入STM32前完成电平匹配。SDAM手册、HC-SR04资料

SHTC3安装在通风且远离处理器、稳压器与电机驱动热源的位置。传感器位置、朝向和车体外伸部分一并记录为外参。

3.2 几何与周期配置

HC-SR04参考资料建议测量周期大于60ms，本方案采用四路顺序、65ms全局触发间隔，保守减少跨方向余波。四路扫描约260ms，CAN即时上传表示测完即发，不把它陈述为四路同步高频更新。HC-SR04时序

车体旋转外接半径约0.146m，连续空间基础膨胀半径取0.22m；栅格使用保守邻域膨胀，局部运动另检查真实足迹、支架、位姿质量和传感器盲区。30cm为地图表示分辨率。

4 Linux 系统与设备接入

4.1 启动链路与运行环境

Linux 端完成 U-Boot、内核、设备树与 BusyBox rootfs 的移植和构建，支持 NFS 调试及 eMMC 独立启动。设备树配置SPI、I²C、显示与CAN等外设，应用使用 C、pthread、SocketCAN、Paho C、SQLite3 和 LVGL。

系统启动后先加载配置、打开设备并建立初始状态。底盘通信、传感器、位姿初始化和运动许可检查通过后，应用进入待机；进程启动本身不产生非零速度。关键线程创建、调度属性设置或设备初始化失败时，系统保持待机并上报原因。

4.2 四路激光I²C直连

四路SDAM通过同一I²C总线按独立地址接入，SHTC3使用0x70，SDAM使用0x71、0x72、0x73、0x74。方向与地址的对应关系写入配置，驱动按地址创建四个设备实例。

Linux I²C控制器
    ├── 0x70  SHTC3
    ├── 0x71  前SDAM
    ├── 0x72  左SDAM
    ├── 0x73  后SDAM
    └── 0x74  右SDAM

模组在装配阶段逐个通过UART完成地址设置和读回，断电重启后再次检查地址保持；该保持行为已由本人实测确认。上电按模组接口选择要求进入I²C模式，应用直接访问持久化地址。Linux设备地址使用7位表示，串口配置命令按厂家格式编码。SDAM地址配置说明

I²C以100kHz运行，各设备分时进行有界事务。激光驱动输出原始距离、质量、有效性与时间信息；UART协议的包头和帧长不套用到I²C读取。单路读失败不刷新旧样本时间，连续故障触发该设备恢复和方向降级。

4.3 IMU 与温湿度采集

ICM20608 通过板载 SPI 接入，驱动负责识别、量程、采样配置与原始数据读取。应用完成静止零偏估计、坐标轴转换和安装标定，将 IMU 坐标统一到车体坐标。ICM20608 手册

SHTC3 通过独立设备实例完成唤醒、测量、数据读取及CRC检查。测量使用非时钟拉伸流程：发送命令后退出本次处理，在后续调度中读取结果，不让温湿度转换等待阻塞整个采集循环。测量窗口协调激光 I²C 访问；SPI IMU 的采样不依赖该转换窗口。SHTC3 手册

激光、IMU、温湿度数据都包含来源、数值、有效标志、采样/接收时刻和配置版本。驱动错误、格式错误、物理范围异常与数据过期分别记录，避免用一个“读取失败”掩盖不同故障。

5 Linux温湿度补偿与四方向距离融合

5.1 原始测时到补偿距离

STM32通过定时器捕获每路HC-SR04的Echo脉宽，并上传direction、echo_time_us、validity、sample_age_ms、sequence。Linux接收后恢复测量时间，选取时间对应的SHTC3环境快照，计算湿空气声速和该次测量的补偿距离。

STM32：Trig → Echo上升/下降沿捕获 → 原始脉宽 → CAN
Linux：原始脉宽 + 温度 + 相对湿度 + 配置气压
     → 湿空气声速 → 超声距离 → 时间/外参对齐 → 激光与超声融合

模块名称为本人确认的HC-SR04。超声脉宽测量值与编码器测速值分别命名，避免把车辆速度和超声回波时间混为一个字段。

5.2 同时使用温度和湿度的补偿方法

补偿采用室内常温条件下的湿空气理想混合近似。温度T单位℃、相对湿度RH单位%，气压p_ref单位Pa。当前未增加气压传感器，p_ref是环境配置值，缺省101325Pa，并在日志中标记为参考值。

Tk = T + 273.15
p_sat = 611.2 × exp(17.62 × T / (243.12 + T))     # Pa
p_v = (RH / 100) × p_sat
x_v = p_v / p_ref                               # 水蒸气摩尔分数

M_mix  = (1-x_v) × 0.02896546 + x_v × 0.01801528  # kg/mol
Cp_mix = (1-x_v) × 29.1 + x_v × 33.6             # J/(mol·K)，常温近似
gamma  = Cp_mix / (Cp_mix - 8.314462618)
c      = sqrt(gamma × 8.314462618 × Tk / M_mix)   # m/s

r_U = c × echo_time_us × 1e-6 / 2                # m

饱和蒸汽压采用常温水面近似，比热采用常温常数；因此这是一种明确包含温湿度输入的工程补偿模型，适用温区和模型误差纳入配置。它没有把温度线性公式冒充湿度补偿，也不将参考气压当成实测气压。声速的理想气体关系见NASA声速推导，水汽与饱和压关系见Vaisala湿度计算资料。

温度补偿的简化对照式为c≈331.3+0.606T，用于诊断与模型比较。正式温湿度融合路径采用上面的湿空气模型。温湿度过期、输入越界、脉宽无效或模型结果异常时，对应超声距离标记无效；激光有效方向继续按降级规则处理，停车许可取决于当前运动方向的有效覆盖。

5.3 时间与安装位置对齐

每条观测先换算到统一车体坐标，再比较同方向距离。设传感器安装位置为t_i、测量方向为u_i，量程为r_i，原始观测端点为q_i=t_i+r_i u_i。前、左、后、右分别对应车体方向，扣除对应车体边界和安装突出量后得到外部净空。

激光窄束和超声锥形视场分别保留。只有时间接近、观测区域重合、质量有效且有一致目标证据时，两个量程才进行数值融合。旧超声样本依据位姿历史对齐并增加运动不确定性；无法确认同一目标的观测分别保留。

5.4 融合结果与计算公式

每方向输出DirectionRange：distance_est、distance_guard、variance、source_mask、sample_time、age、status。distance_est用于方向距离展示与可关联的几何处理，distance_guard用于近障和运动门控；两者共同构成该方向的融合结果。

对已经确认同一目标、误差近似独立的激光距离r_L和补偿超声距离r_U：

w_L = 1 / sigma_L²
w_U = 1 / sigma_U²
r_fused = (w_L × r_L + w_U × r_U) / (w_L + w_U)
variance_fused = 1 / (w_L + w_U)

方差按量程、质量、时间误差、入射角及环境补偿误差动态调整。厂家最大误差界限不直接等同高斯标准差，也不使用无依据的固定“激光占80%”权重。

保护距离采用可信观测的保守下界，并扣除车体偏置、运动期间可能接近的距离与误差余量：d_guard=max(0, min(d_i-k×sigma_i)-v_close×age-margin)，其中k和margin为配置参数，v_close取朝障碍物方向的接近速度非负上界。各来源分别计算自身年龄补偿后再取保守结果。它不是额外独立传感器，后续模块不能将原始观测与这个派生融合量当成相互独立的信息重复融合。

栅格模块消费方向融合结果及其来源明细：激光窄束仍使用激光射线，超声仍使用锥形区域；疑似透明目标后方保留未知，不把超声距离变成一条精确激光射线。

6 STM32 实时任务与中断划分

6.1 FreeRTOS 任务优先级

底盘采用抢占式调度，configMAX_PRIORITIES=8，任务优先级范围0～7，数值越大优先级越高。优先级0留给 Idle，业务任务在等待事件或周期时主动阻塞。FreeRTOS 任务优先级

安全检查和电机控制分开：safety_task决定当前是否允许运动，motor_task只在允许范围内执行闭环。CAN 收到合法速度并不直接写 PWM，必须经过统一运行许可。

电机任务采用绝对周期推进，例如 vTaskDelayUntil()，避免每轮“计算时间+相对延时”造成周期漂移。共享互斥量只保护短快照，不在持锁期间输出串口、等待CAN或延时。任务栈分别分配并统计高水位，不用同一个过小栈覆盖所有任务。

6.2 HC-SR04测时与实时上报

四路HC-SR04顺序触发，任意相邻触发间隔65ms，一轮约260ms。Trig高电平按模块要求至少10µs，定时器以1µs计数单位捕获Echo上升沿与下降沿，记录原始脉宽；等待回波超时设为40ms，覆盖无回波等异常状态。

Echo捕获完成或超时后，ultrasonic_task生成原始测时快照并立即通知can_tx_task，不等到四路全部完成才打包。每条数据携带方向、采样年龄、序号和有效标志；超时条目也上报，以区分设备正常更新但无回波与链路完全无更新。

正式距离补偿与四方向融合在Linux端完成。底盘保留以原始Echo脉宽为输入的保守近障门限和通信超时处理：原始门限按允许温区内声速下界配置，作为附加停车条件；无回波、卡高和过期分别处理。该门限不冒充Linux的温湿度补偿距离，也不替代激光/超声融合结果。

ISR只读取捕获值、处理计数回绕并投递有界记录，不执行温湿度模型、几何融合、打印或完整CAN解析。HC-SR04参考时序

6.3 NVIC 中断优先级

STM32F407 的 NVIC 数值越小，中断紧迫性越高，与 FreeRTOS 任务优先级方向相反。系统使用统一的抢占优先级配置，以下为未移位的逻辑优先级：

与此对应，configLIBRARY_MAX_SYSCALL_INTERRUPT_PRIORITY使用5，移位形式按芯片实际优先级位数生成。调用 FromISR 的中断位于允许区间，不能把高于该阈值的中断当成普通RTOS回调使用。外部急停硬件链路本身不依赖 ISR 是否及时运行。FreeRTOS Cortex-M 中断规则

7 Linux 多线程与优先级划分

7.1 调度原则

Linux 按“运动许可与停车发送、通信接收、感知融合、建图执行、显示与持久化”划分工作。实时线程只做短、可阻塞等待的关键操作，地图、JSON、文件和界面不进入高优先级控制链。

Linux 的 SCHED_FIFO 使用1～99的实时优先级，数值越大越高；SCHED_OTHER 的 sched_priority为0，使用nice调整普通调度权重，nice越小权重越高。两类数值不直接比较。SCHED_FIFO没有普通时间片轮转，因此关键线程必须有工作预算并及时阻塞。Linux 调度说明

7.2 线程配置表

hmi独立于核心控制进程运行，通过版本化快照和有界IPC交换数据；核心控制进程内不从其他线程直接调用LVGL。第三方网络库自行创建的辅助线程不按业务线程表重复计数，也不擅自提升为实时线程。

7.3 这样划分的原因

can_tx优先级最高，用于尽快送出停止和当前有效请求；command紧随其后，负责生成唯一合法运动请求。can_rx快速接收底盘数据，但不在接收线程内完成融合或写盘。monitor能够在普通计算任务繁忙时设置停止门控。

sensor_io和state_fusion使用较高的普通调度权重，保证感知更新受到重视，同时隔离 I²C/SPI 驱动等待。navigation维持固定工作预算；网络、显示和存储允许短时延后。i.MX6ULL为单核平台，多线程用于职责隔离与异步等待，不把线程数量当成多核并行算力。

7.4 实时线程的约束

• can_rx单次最多处理32帧，达到预算后通过定时等待短暂阻塞；持续报文洪泛触发诊断和降级，避免接收线程一直占用CPU。
• can_tx采用非阻塞发送及有界重试。Socket发送成功只表示交给内核，底盘执行状态仍通过上行反馈确认；内核发送队列长度受限，不提前堆入大量旧速度。
• 周期线程使用CLOCK_MONOTONIC与绝对到期时间；错过周期时记录超期并推进到未来时刻，不补跑一串过时控制周期。clock_nanosleep说明
• 跨实时与普通线程的短互斥量使用优先级继承，初始化与设置结果均检查。优先级继承用于限制优先级反转，不能让长临界区变得合理。pthread互斥协议
• 网络、磁盘和图形操作不进入实时临界区。关键快照与队列启动时预分配，控制周期内不反复进行动态内存分配。
• 线程调度权限通过服务配置提供，启动后回读实际策略。关键策略配置失败时保持待机，不把静默回退当作配置成功。

8 线程邮箱、共享数据与通信拓扑

8.1 邮箱实现与数据所有权

线程间采用有界队列、最新值邮箱和故障锁存三种通信形式。队列传递必须逐条处理的事件；最新值邮箱传递位姿、方向距离和速度请求；故障锁存传递必须立即生效的停车条件。

队列使用互斥锁与条件变量，唤醒按while检查条件并设超时；实时与普通线程之间的短锁启用优先级继承。最新值采用双缓冲/短锁复制，消息携带版本与采样时刻。多个消费者读取发布快照时分别维护已读版本，不能让一个消费者取走队列消息后造成其他消费者漏收。

8.2 邮箱清单

mqtt线程消费mb_store_policy的完成状态用于应用确认；storage接收一个文件块只表示入队，完整性校验完成后才向mb_policy_ready发布。hmi使用独立进程，其“邮箱”由有界IPC适配，不把普通进程内指针传到另一进程。

8.3 Linux主处理与控制邮箱拓扑

flowchart LR
  S["sensor_io"] --> MS["mb_sensor<br/>激光 / IMU / 温湿度"]
  R["can_rx"] --> MR["mb_chassis_raw<br/>Echo原始测时 / 编码器"]
  MS --> F["state_fusion<br/>补偿 / 距离融合 / 位姿 / 里程"]
  MR --> F
  F --> PR["pub_range<br/>四方向距离 / 保护距离 / 质量"]
  F --> PP["pub_pose<br/>位姿 / 速度 / 角速度 / 里程"]
  PR --> N["navigation<br/>地图准备 / 建图 / 策略执行"]
  PP --> N
  PR --> C["command<br/>仲裁 / 许可 / 限速"]
  PP --> C
  R --> PC["pub_chassis<br/>底盘状态"]
  PC --> C
  M["mqtt"] --> MT["mb_task_nav"] --> N
  M --> MC["mb_remote_ctrl"] --> C
  H["hmi进程"] --> MO["mb_operator - IPC"] --> C
  C --> MN["mb_mode_nav"] --> N
  N --> MM["mb_motion<br/>有限期运动请求"] --> C
  C --> CT["mb_control<br/>最新合法请求"] --> T["can_tx"]
  T --> B["STM32底盘"]
  B --> R

8.4 Linux服务、日志与监控邮箱拓扑

flowchart LR
  S["sensor_io"] --> LOG["mb_log<br/>遥测队列 / 关键事件队列"]
  R["can_rx"] --> LOG
  F["state_fusion"] --> LOG
  N["navigation"] --> LOG
  C["command"] --> LOG
  T["can_tx"] --> LOG
  W["monitor"] --> LOG
  LOG --> D["storage<br/>SQLite唯一写者"]
  M["mqtt"] --> BLOCK["mb_store_policy"] --> D
  D --> READY["mb_policy_ready"] --> N
  D --> ACK["文件接收 / 校验状态"] --> M
  N --> OUT["mb_mqtt_out<br/>地图 / 就绪 / 状态 / 结果"]
  W --> OUT
  C --> OUT
  OUT --> M
  F --> UI["pub_ui - IPC<br/>分来源版本快照"]
  N --> UI
  W --> UI
  UI --> H["hmi进程"]
  ALL["各工作线程的完成进展"] --> BEAT["heartbeat_slots"] --> W
  W --> FAULT["fault_latch<br/>故障位图 + 唤醒"]
  FAULT --> C
  FAULT --> T

heartbeat_slots覆盖sensor_io、can_rx、can_tx、command、state_fusion、navigation、mqtt和storage；HMI通过IPC进展状态单独监测。心跳标记关键工作完成，不能只在空转循环中更新。关键模块发现故障也可直接置fault_latch，然后交monitor汇总。

8.5 STM32任务与邮箱拓扑

flowchart LR
  EI["Echo捕获ISR"] --> EQ["echo_capture_queue"] --> U["ultrasonic_task"]
  U --> UQ["raw_echo_mailbox<br/>原始测时 / 方向 / 状态"] --> TX["can_tx_task"]
  U --> UG["echo_guard_snapshot"] --> SF["safety_task"]
  CI["CAN接收ISR"] --> CQ["can_rx_queue"] --> RX["can_rx_task"]
  RX --> SP["speed_request_mailbox"] --> MOT["motor_task"]
  RX --> CS["command_health_snapshot"] --> SF
  SF --> PER["run_permit / fault_latch"] --> MOT
  MOT --> EN["encoder_snapshot<br/>统一采样组"] --> TX
  SF --> FQ["fault_tx_mailbox"] --> TX
  TX --> CAN["CAN上行至Linux"]
  TASKS["各任务进展槽"] --> HE["health_task"]
  HE --> WD["喂狗许可 / 健康状态"]
  HE --> SF
  TASKS --> DG["diag_queue"] --> DI["diag_task"]

捕获队列和CAN接收队列使用允许的FromISR接口；原始测时完成即通知发送任务，编码器使用统一采样组最新快照。停止/故障锁存独立于普通发送队列。所有队列固定容量、定义溢出状态，ISR和控制任务不等待慢速诊断输出。

8.6 时间对齐

Linux使用64位单调时钟计算有效年龄，UTC用于日志与云端期限。STM32上报Echo脉宽、从Trig采样起点到发送的年龄，以及编码器采样组号/时刻；Linux记录接收时刻并计入CAN延迟不确定性。

激光记录读取时间与模组测量周期的不确定性，四方向分别对齐；温湿度从环境历史中匹配。原始测时、补偿距离、方向融合距离和位姿均保留各自版本及采样时间。相同样本被多个线程读取不会产生新的采样时刻。

9 CAN原始测量与运动控制协议

9.1 报文布局

板间采用500kbps经典CAN，单帧8字节。沿用Profile 5保护配置，前3字节为CRC16与8位Counter，余下5字节承载应用字段，DataID按配置参与CRC。应用数值统一小端：

Echo字段使用原始微秒值，状态区分有效、无回波、捕获超时、卡高与设备故障；0xFFFF保留为无效，40ms超时以内的有效脉宽可用16位表示。温湿度、声速、补偿距离与融合距离在Linux内部计算，CAN主链上传感器报文保留原始测量。

各方向与轮索引分配固定CAN ID/DataID及独立接收状态。编码器在同一MCU采样点读取各轮计数，该组帧使用一致组号并通过EncoderStamp匹配；缺帧组不假装完整同步采样。Counter、组号和采样时间各自承担不同职责。

停止与关键故障报文使用较高仲裁优先级。E2E接收状态机检查DLC、CRC、DataID、序列、跳号和重复，接收异常不刷新数据期限；原始测时重传也保留原采样年龄。E2E与模组接口校验分层处理，不构成身份认证或安全认证。

9.2 超时与运动门控

速度邮箱携带生成时刻与截止时间，can_tx发前复查，禁止将旧速度重新打包刷新寿命。STM32接收端与电机输出端再检查本地运行许可。原始Echo近障门限与通信超时维持底盘附加保护，Linux的融合结果控制正常导航限速和停车。

10 物理量、传感器优先级与融合计算

10.1 输出物理量与所需输入

10.2 传感器采用优先级

这里的优先级表示不同物理量的可信信息采用顺序，与线程调度优先级分开。所有来源先经过有效性、时间、量程与残差检查，过期数据不会因“高优先级”继续使用。

10.3 编码器换算

对第j个轮子，C_j是轮子转一圈对应的有效计数，包含减速比与计数倍率；r_j为标定轮半径：

delta_N_j = unwrap(N_j[k] - N_j[k-1])
delta_s_j = 2 × pi × r_j × delta_N_j / C_j
v_j = delta_s_j / delta_t
delta_s_L = 左侧有效轮位移的稳健均值
delta_s_R = 右侧有效轮位移的稳健均值
v_enc = (delta_s_R + delta_s_L) / (2 × delta_t)
omega_enc = (delta_s_R - delta_s_L) / (B_eff × delta_t)

B_eff为滑移转向底盘的有效轮距，包含转向滑移影响。轮间异常、计数回绕、方向符号和MCU重启分别处理。编码器派生的位移、速度、航向存在相关性，主滤波按独立观测选择输入，不把同一组计数反复当作多份独立信息。融合输入去重原则

10.4 IMU处理与状态预测

原始加速度和角速度经过量程换算、零偏与安装轴向校正，转到车体坐标。系统在平坦场地运行，静止/低动态时用重力方向约束roll/pitch；前向加速度先去除重力投影，动态振动期间降低其权重。

主滤波采用平面EKF，状态定义为X=[x,y,yaw,v,b_g,b_a]，后两项为陀螺和前向加速度偏置。预测采用：

omega = gyro_z - b_g
a = a_forward_without_gravity - b_a
delta_s_pred = v × dt + 0.5 × a × dt²
theta_mid = yaw + 0.5 × omega × dt
x_pred = x + delta_s_pred × cos(theta_mid)
y_pred = y + delta_s_pred × sin(theta_mid)
yaw_pred = wrap(yaw + omega × dt)
v_pred = v + a × dt
P_pred = F × P × F^T + Q

随后以编码器速度和可用测距观测更新。低滑移且轮间一致时，gyro_z-omega_enc作为陀螺偏置的辅助校准信息；检测到滑移或转向模型不可靠时暂停该校准。相同编码器计数派生的航向与角速度不再作为独立信息重复计权。

IMU加速度用于短时间预测，长期位移由编码器与可靠几何观测共同约束；ICM20608为六轴IMU，其自身不提供无漂移绝对航向或绝对位置。偏置在静止和具备可观测约束时更新，不能把缺少约束的偏置估计写成已经准确收敛。

10.5 测距如何参与位置与航向修正

当激光对应已知墙面、边界或稳定结构时，使用量程观测模型。设平面直线为n^T q=b，传感器世界位置为p_s=p+R(yaw)t_i，世界测量方向为u_w=R(yaw)u_i：

h_i(X) = (b - n^T p_s) / (n^T u_w)
innovation = r_measured - h_i(X_pred)
S = H × P_pred × H^T + R_measurement
K = P_pred × H^T × inverse(S)
X_new = X_pred + K × innovation
P_new = (I-K×H) × P_pred × (I-K×H)^T + K × R_measurement × K^T

H为该观测对状态的雅可比；协方差采用保持对称与数值稳定的更新形式。仅接收正向、视场有效、目标关联稳定且创新门限通过的观测。射线接近平行、目标身份变化、疑似玻璃或相关方向不可观测时，关闭对应修正或增大观测噪声。

直行且同一静态目标持续可见时，距离差可以辅助约束移动量；发生转弯、障碍变化或目标切换时，不能直接用“上一帧距离减本帧距离”当车辆位移。超声只有在其锥形视场确实对应同一已知结构时才以更大方差参与几何修正，其余情况用于方向近障与占据证据。

建图阶段对未知目标先形成地图证据，不把自己刚生成的地图当无误差真值反复纠正自己。已确认边界、预设地图和独立复核的稳定结构才提供明确参照；平行墙、开阔区等退化几何只约束可观测分量，其他分量维持里程计预测并增加不确定性。

10.6 移动距离、累计里程与净位移

delta_s = 0.5 × (v_fused[k-1] + v_fused[k]) × dt
signed_distance += delta_s
travel_distance += abs(delta_s)                  # 仅在有效运动状态累计
net_displacement = sqrt((x-x_start)² + (y-y_start)²)

融合速度已经包含编码器、IMU预测与测距更新的约束，因此里程体现多源融合。累计路程不把位置滤波修正造成的坐标跳变算成真实运动；轨迹位置用于地图/目标跟踪，累计路程用于运动统计，两者各有语义。

静止由编码器低速、陀螺低角速度和加速度稳定等条件共同确认，静止时执行零速更新并抑制噪声里程。原地转向主要改变航向，不凭轮子滚动量给车体中心累计大量平移。打滑由编码器差速与陀螺残差、惯性响应和可信测距残差联合判断，触发增大编码器噪声、降速或停车。

10.7 质量、权重与避免重复融合

过程噪声和观测噪声由配置与标定管理，随速度、振动、量程、质量、时间误差和打滑状态变化。单维创新采用如abs(innovation)≤3×sqrt(S)的门限进行筛选，持续拒绝后降低该来源可信度。

方向融合结果由原始激光和超声派生，不能再与同一批原始量程作为三份独立观测进入EKF。主滤波选择可关联的原始几何观测，方向保护结果用于门控；或者使用保留完整协方差的融合观测。每次状态更新输出来源、质量与当前可观测性，质量不足时停止任务运动。

11 占据建图与局部执行

11.1 启动地图准备与两种模式

启动自检后进入MAP_PREPARE，先选择现场建图或云端预设地图，两种模式统一经过地图一致性与初始位姿确认，生成map_ready后进入物流任务阶段。

现场建图任务由车端建图状态机执行，采样使用人工遥控或车端预设路线。建图期间允许云端接收地图与状态，取送任务入口保持关闭。地图完成指允许运行区域的地图已审核，可达走廊与目标区域具备有效覆盖；未观测区域保持UNKNOWN并禁止任务穿越。

预设地图包含坐标原点、轴向、分辨率、障碍轮廓、允许区及版本。障碍摆放由人工按格线、尺寸和坐标核对；四方向传感器的抽样检查用于验证可见结构，不能代替对全场布置的人工确认。地图装载完成与物理布局确认是两个独立状态。

11.2 地图表示

navigation独占地图写入权，每格保存log-odds、UNKNOWN/FREE/OCCUPIED状态、观测时间及来源标志。基础更新参数采用占据证据+0.85、自由证据-0.4、截断范围[-4,+4]，多次观测形成稳定分类。

方向距离融合结果与来源明细按采样时刻位姿和外参投影；激光射线更新已观测自由证据与可信端点，超声按锥形区域补充近障占据证据。无效回波、未观测空间和疑似透明障碍后方不因“没有返回”而变成自由区。代价层结合车体足迹、边界、障碍膨胀和动态近障生成。

场地坐标原点位于左下角，x/y沿场地边界。内部缓冲原点为(-0.6m,-0.6m)：

internal_col = floor((x + 0.6) / 0.3)
internal_row = floor((y + 0.6) / 0.3)
场内索引为2～21，输出云端地图时映射为0～19。

四方向窄束之间存在未观测区域。建图采用低速采样、受控转向补扫和多角度累积；只有完整转向足迹处于已确认净空范围内才执行旋转。启动区域通过人工确认，未知空间不靠盲转来证明安全。建图完成由可达区域覆盖与人工审核决定，不把未到达区域强行填成FREE。

11.3 地图就绪后的策略适配与目标跟踪

云端在地图就绪后发布任务与策略，适配层将其转换为任务号、阶段、地图版本、目标、约束与截止时间等统一内部结构。内部结构是车端统一接口，不强行把某个未提供的PPO格式、评分表或模型文件定义成现有云端协议。

车端执行顺序为：

策略接收 → 长度/哈希/格式检查 → 地图/坐标/任务绑定检查
         → 局部目标生成 → 足迹与走廊检查
         → 航向对齐 → 连续位姿闭环跟踪 → 到达确认
         → 更新阶段 → 上传执行结果

局部跟踪输出线速度和角速度请求，统一交command仲裁，再由can_tx下发。到达依据连续位置、航向及稳定低速状态，不以“车进入目标栅格”替代精确到点。新障碍、位姿质量不足、目标无进展或策略过期时停车，等待云端更新或人工处理，车端不自行承担另一套全局路线规划。

12 MQTT地图就绪门控与任务状态机

12.1 地图完成与任务发布顺序

云端和车端都使用地图就绪门控。车端上传或加载地图后，完成布局审核与初始位姿确认，发布map_ready；云端确认当前会话和地图绑定后才发布取送任务。map_ready至少绑定robot_id、boot_session、map_id、map_version、map_hash、frame_id、layout_id、calibration_version。

地图未就绪时收到任务，返回map_not_ready，不暂存一个可能在以后自动执行的旧任务。地图或物理布局改变后清除就绪状态，暂停任务并重新确认；旧会话的就绪消息不能使本次上电自动运行。

12.2 网络、分块与应用确认

MQTT回调只做有界检查和入队，地图和策略处理由对应业务线程负责。连接采用TLS、自身机器人命名空间权限和有界重连；命令和策略不以retained消息自动启动运动。

策略分块4KiB，应用消息上限8KiB，单策略缓存上限64KiB。mqtt投递文件块给storage，完整性校验后发布mb_policy_ready，navigation再次检查地图/任务/阶段后激活。超限拒绝、缺块重传、重复幂等，PUBACK与文件完整、策略就绪、动作完成分别确认。

12.3 完整状态机

启动：
BOOT → SELF_CHECK → MAP_PREPARE → SELECT_MAP_MODE

现场建图：
SELECT_MAP_MODE → INITIAL_POSE → MAPPING → COVERAGE_REVIEW
                → FREEZE_MAP → MAP_UPLOAD → MAP_ACK → MAP_READY

云端预设地图：
SELECT_MAP_MODE → DOWNLOAD_MAP → ARRANGE_OBSTACLES
                → CONFIRM_LAYOUT → CHECK_VISIBLE_GEOMETRY
                → INITIAL_POSE → MAP_READY

就绪与任务：
MAP_READY → PUBLISH_READY → CLOUD_READY_ACK → READY
READY → RECEIVE_TASK → VALIDATE_BINDING → GO_PICKUP
      → WAIT_LOAD_CONFIRM → GO_DROPOFF → WAIT_UNLOAD_CONFIRM → COMPLETE

异常：
RUNNING → STOPPED → REPORT → WAIT_UPDATED_STATE → 显式继续
地图/布局改变 → 清就绪 → MAP_PREPARE
关键故障 → FAULT_LATCHED → 排障/人工复位 → SELF_CHECK

现场建图与取送执行是不同任务阶段。command在建图模式只接收当前建图/遥控会话允许的请求，在物流模式只接收已绑定地图的任务请求。人工装卸确认绑定任务号、阶段和序号，上电、急停释放、网络重连及地图加载均不自动恢复旧非零速度。

13 界面、记录与资源管理

13.1 LVGL界面

界面显示地图、轨迹、当前位置、当前目标、取送阶段、四方向激光/补偿超声/融合距离、位姿与里程、温湿度、补偿和地图就绪状态、CAN/云端连接及故障原因。启动、暂停、装卸确认和恢复操作通过IPC提交给核心进程，不由界面直接发送电机指令。

地图显示5Hz，状态显示按需要刷新；界面主循环只消费快照。HMI卡顿或退出不阻塞采集与底盘控制，核心进程能够按当前任务状态保持运行或有序停车。

13.2 SQLite与策略记录

storage作为SQLite唯一写者，采用批量事务记录echo_raw_log、range_fusion_log、imu_log、encoder_log、pose_log、env_log、map_event、task_event、policy_event、can_diag、fault_event。记录包含采样/接收时间、有效性、任务与地图版本、温湿度补偿模型、来源权重/残差、布局确认、模型版本和错误原因。

普通遥测按100ms批次或累计128条提交；任务阶段和故障事件进入独立关键队列。文件配额、日志归档和WAL大小受控，慢盘、满盘或数据库错误不阻塞command与can_tx。关键阶段无法可靠记录时明确上报，不能在重启后假装拥有完整恢复状态。

13.3 内存与负载

512MB内存条件下，资源管理关注实际MemAvailable、RSS/PSS、队列高水位、周期超期和写入延迟，不用数组大小代替整进程内存。普通业务繁忙时优先降低界面和遥测频率；感知质量或控制进展不满足期限时停车，不能通过提升所有线程优先级掩盖过载。

14 异常停车与恢复

停车距离按D=v×T_total+v²/(2a_min)+margin管理。T_total包含当前参与保护的最慢有效来源采样等待、接收/融合、指令和制动延迟；HC-SR04一轮约260ms时，不能沿用“全链路固定200ms”的旧预算。激光与超声分别计算来源年龄和运动余量，存在盲区或慢测距降级时降低速度并增加停车净空。

底盘原始回波门限只作为独立附加条件，Linux正式的方向保护距离来自传感器融合。物理急停、运行许可、软件停止和故障复位分别处理，不把普通传感器、CRC或RTOS本身写成人员保护或SIL认证。

15 工程组织、调试与记录

工程按职责组织，驱动协议与业务处理分离：

agv_linux/
  drivers/          # SDAM I²C、ICM20608 SPI、SHTC3 I²C
  sensor_io/        # I²C直连激光、SPI IMU、温湿度与原始快照
  canbus/           # 接收、唯一发送、E2E与会话
  state_fusion/     # 湿空气补偿、方向距离融合、EKF、速度与里程
  navigation/       # 启动建图/预设地图、就绪门控、策略适配与局部跟踪
  command/          # 运行模式、请求仲裁、期限与限速
  mqtt/             # 地图、任务/策略、确认、重连
  storage/          # SQLite、策略文件与配额
  monitor/          # 线程进展、数据年龄与故障门控
  hmi/              # 独立LVGL进程
  mailbox/          # 有界队列、发布槽、故障锁存与IPC适配
  configs/          # 0x71～0x74地址、外参、融合噪声、调度、地图与资源
  tools/            # 回放、标定、通信与运行统计

调试使用GDB/gdbserver、can-utils、tcpdump/Wireshark、逻辑分析仪和示波器。I²C检查地址、ACK、时序与总线恢复，SPI检查配置与连续采样，超声检查捕获和串扰，CAN检查错误帧与请求期限。

运行记录包含软件版本、设备/配置版本、布局与任务号、地图/策略哈希、补偿模型、异常与恢复原因。调度统计记录每个周期线程的唤醒延迟、执行耗时、超期次数和队列峰值；优先级配置与运行结果分开保存，不用“设置了高优先级”代替延迟证据。

量化记录覆盖定位/地图误差、测距补偿前后差异、超声串扰、停车距离、CAN/MQTT延迟、任务成功与失败原因、资源曲线和长时间运行情况。本文不补造成功率、定位精度、p99延迟、稳定运行时长或认证结果。

16 项目介绍与追问口径

16.1 项目介绍

我做的是Linux和STM32分工的AGV车端平台。四路SDAM通过I²C接Linux，板载ICM20608走SPI，SHTC3读取温湿度；STM32负责电机、编码器和四路HC-SR04原始回波测时，通过CAN实时上传。Linux统一计算温湿度补偿距离，融合激光与超声得到四方向障碍距离，再融合IMU、编码器和可信测距约束计算位姿、速度和行驶里程。车端先完成栅格建图，或者加载云端地图并按图布场，地图就绪后云端才发布取送任务和策略。我主要负责Linux端系统、驱动、融合、多线程邮箱、通信、界面、数据库和整车联调。

16.2 为什么超声原始测时上传Linux

STM32利用定时器稳定捕获Echo脉宽并立即通过CAN上传；Linux同时持有SHTC3温湿度和激光数据，因此在同一模块完成补偿、时间/外参对齐与方向距离融合，模型、质量和原始数据也能统一记录。底盘保留原始回波近障门限、运行许可和通信超时处理。

16.3 车辆移动距离如何得到

编码器给出轮子位移和车体速度，IMU提供转向及短时运动预测，已知边界或稳定结构的可信测距用来纠正位置和航向漂移。滤波后速度积分得到运动增量和累计里程，融合位置用于地图与目标跟踪。累计里程与起终点净位移分别计算，位置修正的跳变不会被当作真实行驶距离。

16.4 传感器谁优先

优先级随物理量和有效性变化。常规几何距离主要用有效激光；近距离或疑似透明目标场景，可信超声更近时优先保留近障证据。角速度主要用陀螺仪，前向速度主要用编码器，IMU作短时预测，稳定几何测距负责纠偏。数据过期、打滑或残差异常时动态降权，不给所有场景套固定比例。

16.5 线程如何配合

sensor_io采激光、IMU与温湿度，can_rx接收原始Echo和编码器，两边通过邮箱送到state_fusion，输出方向距离、位姿和里程。navigation用这些结果建图或执行任务，经command仲裁后由can_tx统一下发。MQTT、HMI和SQLite各自独立，通过有界邮箱与快照通信，故障锁存可以直接唤醒控制链。

16.6 激光I²C地址如何配置

四个SDAM在装配阶段分别通过串口设为0x71、0x72、0x73、0x74，我已经实测确认断电保持。运行时直接使用这四个I²C地址通信，SHTC3使用0x70，每个地址与固定方向绑定。

16.7 建图和云端任务的先后顺序

现场建图模式先执行低速采样、栅格更新、审核和冻结，上报map_ready并得到云端确认后再接任务。预设地图模式先下载云端地图，按图摆放障碍并确认布局与初始位姿，再进入就绪状态。车端和云端都检查就绪及地图绑定，防止地图还没准备好就启动取送任务。

17 技术依据与已确认硬件事实

• 本人实测确认：四路SDAM通过UART提前改为I²C 7位地址0x71～0x74，断电后保持，运行时直接按这些地址通信；该掉电保持结论来自本人实测。
• 本人确认超声型号为HC-SR04，采用Trig/Echo测时接口；CAN同时上传原始超声测时和编码器数据。
• SDAM手册：0.2～20m测量范围、I²C/UART、电源与地址配置命令。
• HC-SR04参考资料：Trig/Echo、回波时间与测量间隔。文中65ms触发间隔为本方案时序配置。
• ICM20608手册与SHTC3手册：SPI惯性数据及I²C温湿度接口。
• NASA声速关系、Vaisala湿度计算：理想气体声速与水汽计算关系；本文给出常温工程近似并标注参考气压。
• robot_localization融合配置说明：传感器坐标、平面状态和避免重复融合。本文使用原生C融合模块，不据此增加ROS运行依赖。
• Linux调度、互斥协议、周期等待：线程策略与同步机制。
• FreeRTOS任务优先级与Cortex-M说明：任务和中断优先级区分。

接下来，持续把工作记录下来

我会继续整理 GitHub 上的项目说明，把模块分工、构建步骤、接口约定和测试记录补充完整，也会在博客里记录具体的调试过程和改进原因。每次完成一个可以复现的小步骤，就把对应的代码和说明整理好。

这份方案还有需要通过实际联调验证的地方。之后的更新会围绕这些问题逐步展开，让每一次修改都有理由，也有记录。