简介面向自动驾驶与智能交通领域的Matlab车辆预测跟踪项目提供一套可运行的完整源码适合初学者及有一定经验的算法开发人员快速上手目标检测与轨迹预测。包内共有626个文件以bmp图像帧为主616个便于可视化验证跟踪效果另含4个m主程序与4个asv备份文件配套docx说明文档和db数据库文件整体压缩包仅15.9MB轻量易部署。资源中图像序列覆盖多组道路监控场景可用于测试车辆检测、特征匹配与预测跟踪算法m文件经过校正调通下载后可直接运行。目前已有672人学习下载适合用于课程设计、毕业设计或算法预研。通过分析源码与图像序列读者可掌握基于Matlab的车辆跟踪实现思路并在此基础上扩展多目标跟踪或深度学习检测模块。1. 车辆预测跟踪的工程坐标检测与跟踪之间差了一个状态估计在自动驾驶感知链路上“车辆跟踪”往往被误读成“连续视频帧里给同一辆车画同一个框”。实际做过量产项目的工程师都清楚检测器输出的是一串离散、带噪声、偶尔漏检的矩形框而下游融合、规划模块需要的是每帧平滑、连续、能回答“车在哪儿、往哪儿去”的运动状态。车辆预测跟踪解决的核心问题是用状态估计模型把检测框序列转化为对目标位置和速度的递推预测并在检测丢失时用模型外推撑住若干帧。Matlab 生态里这一套东西既可以用 Sensor Fusion and Tracking Toolbox 的trackerGNN一行接入也可以从filter类手写卡尔曼滤波逐步落地后者更适合理解参数如何影响跟踪质量。本文按“建模—单目标实现—多目标关联—参数调优”的顺序展开所有代码可以直接拷进 Matlab R2021b 及以上版本运行新手能跟完老手可以看到 Q 矩阵和门控阈值这些容易被忽视的坑。2. 车辆运动建模与状态方程预测跟踪的数学地基2.1 先把“预测”翻译成状态转移预测跟踪的第一步不是写滤波代码而是决定状态向量和运动模型。对地面车辆而言最常见的是恒定速度CV模型和恒定转率和速度CTRV模型。CV 模型假设车辆在采样间隔内速度不变状态向量取[px; py; vx; vy]离散化后的状态转移矩阵是F [1 0 dt 0; 0 1 0 dt; 0 0 1 0; 0 0 0 1]其中dt是传感器帧间隔。量测向量一般取[px; py]即检测框中心在自车坐标系下的位置对应观测矩阵H [1 0 0 0; 0 1 0 0]。CV 模型在高速公路直行场景下已经够用但如果目标在弯道或十字路口转弯CV 模型的预测会系统性滞后这时候需要换成 CTRV状态向量为[px; py; v; psi; psi_dot]psi是航向角psi_dot是偏航率。CTRV 的状态转移方程是非线性的因此滤波要用扩展卡尔曼滤波EKF或无迹卡尔曼滤波UKF。2.1.1 为什么状态向量里不加速度项在车辆跟踪中我一般不在初始状态向量里同时放加速度原因有两个。第一加速度在量产传感器噪声量级下很难被准确观测强行估计会引入额外方差第二匀速模型配合适当的过程噪声 Q其实可以吸收小幅加速度带来的模型失配。换句话说把加速度放进“噪声”里比把它放进“状态”里更稳健。只有当目标存在频繁启停如城市场景时才需要切换到恒定加速度CA模型或者用交互多模型IMM做模型自适应。2.2 过程噪声 Q 和量测噪声 R两个决定命运的矩阵卡尔曼滤波的预测误差协方差递推公式是P_pred F * P * F Q这里 Q 表示模型预测和真实运动之间的偏差协方差。Q 的数值大小决定了滤波器对检测新息的信任程度。Q 设得过大估计结果会剧烈跳变跟踪轨迹看起来像是在“追”检测框Q 设得过小滤波器反应迟钝车辆急刹或变道时跟踪框会严重滞后。实用的做法是把 Q 拆成加速度噪声强度sigma_a^2和位置噪声强度sigma_p^2然后按如下方式构造% 构造 CV 模型的过程噪声矩阵 Q sigma_a 2.0; % 加速度噪声标准差单位 m/s^2城市场景常用 1.5~3 sigma_p 0.5; % 位置噪声标准差单位 m用于吸收未建模位置扰动 dt 0.1; % 帧间隔单位 s对应 10Hz 传感器 % 分块矩阵构造方式对位置噪声和速度噪声分别处理 Q11 sigma_p^2 * eye(2); Q12 zeros(2); Q21 zeros(2); Q22 sigma_a^2 * [dt^4/4 0; 0 dt^4/4]; % 合并为 4x4 矩阵 Q [Q11 Q12; Q21 Q22];量测噪声 R 在单目标跟踪时通常直接取自检测器的位置方差。如果检测框中心坐标的抖动标准差是 0.8 米那么R 0.8^2 * eye(2)。这里有一个常见的误用手法某些工程师为了“让跟踪更平滑”把 R 设得比实际噪声大一个数量级结果是在目标转弯时跟踪框迟迟不跟随。量测噪声应当反映传感器真实误差而不是被当作平滑参数使用。2.2.1 从检测框到量测向量框中心的转换代价当检测器输出的是[x, y, w, h]形式的边界框时量测向量取框中心是个自然选择但要注意框宽高不稳定对大目标和小目标的影响不同。一个更好的做法是把量测向量扩展为[cx, cy, s]其中s代表框面积的对数这样滤波器可以同时估计目标尺寸变化减少因车辆远近导致的框尺寸跳变对跟踪稳定性的影响。扩展后观测矩阵 H 变成 3x5如果状态向量包含尺寸项处理预报时效果更稳。3. Matlab 手写卡尔曼滤波实现车辆预测跟踪单目标最小可运行系统3.1 用类封装一个可复用的跟踪滤波器不借助工具箱直接基于面向对象编程实现一个简单的卡尔曼滤波跟踪器有利于看清预测和更新两个步骤的数据流动。下面这段代码定义了一个继承自matlab.System的类也可以直接用普通类配合step方法便于集成进 Simulink 或作为独立脚本调用。类中只实现 CV 模型核心逻辑全部在predict和correct两个方法里。classdef ConstantVelocityTracker handle properties F (4,4) double % 状态转移矩阵 H (2,4) double % 观测矩阵 Q (4,4) double % 过程噪声 R (2,2) double % 量测噪声 xhat (4,1) double % 状态估计 [px; py; vx; vy] P (4,4) double % 估计误差协方差 hasPrior logical false end methods function obj ConstantVelocityTracker(dt, Q, R) obj.F [1 0 dt 0; 0 1 0 dt; 0 0 1 0; 0 0 0 1]; obj.H [1 0 0 0; 0 1 0 0]; obj.Q Q; obj.R R; end function obj init(obj, z) % 用首帧量测初始化位置速度置 0 obj.xhat [z(1); z(2); 0; 0]; obj.P 10 * eye(4); % 初始协方差取较大值 obj.hasPrior true; end function obj predict(obj, dt) % 更新状态转移矩阵支持可变帧间隔 obj.F(1,3) dt; obj.F(2,4) dt; obj.xhat obj.F * obj.xhat; obj.P obj.F * obj.P * obj.F obj.Q; end function [obj, y] correct(obj, z) % 卡尔曼增益修正y 为返回的新息 y z - obj.H * obj.xhat; S obj.H * obj.P * obj.H obj.R; K obj.P * obj.H / S; obj.xhat obj.xhat K * y; obj.P (eye(4) - K * obj.H) * obj.P; end end end使用该滤波器的最小调用流程如下设置Q、R首帧调用init随后每个新帧先predict再correct。预测后的状态向量在检测缺失时可以继续输出最多外推 58 帧具体帧数取决于场景对误报的容忍度。correct方法返回的新息y和协方差S很重要它们正是下一步数据关联和有效性判断的输入。3.1.1 可变帧间隔的处理与时间校准雷达和摄像头通常以不同频率输出数据导致跟踪器每步的dt并不恒定。代码里通过更新obj.F(1,3)和obj.F(2,4)来适配实际帧间隔这一细节在多人协作项目中常被忽略。时序上如果传感器数据带有时间戳而跟踪器采用固定dt递推长时间运行后帧率漂移会累计产生不可忽略的位置偏差。因此量产代码里一律使用时间戳差值作为dt。3.2 完整仿真脚本生成含噪轨迹并观察跟踪收敛下面这段脚本模拟一辆匀速直线运动车辆叠加零均值高斯噪声作为量测用上面的滤波器进行跟踪并绘制真实轨迹、量测和滤波结果的对比图。rng(2024); dt 0.1; T 200; t (0:T-1) * dt; % 真实轨迹vx12m/s, vy0, 起始点 (0,0) truePos zeros(2, T); truePos(1,:) t * 12; % 量测位置加上标准差 1.0m 的高斯白噪声 sigma_z 1.0; z truePos sigma_z * randn(2, T); % 初始化跟踪器Q、R 定义见 2.2 sigma_a 2.0; Q zeros(4,4); Q(3:4,3:4) sigma_a^2 * [dt^4/4 0; 0 dt^4/4]; R sigma_z^2 * eye(2); tracker ConstantVelocityTracker(dt, Q, R); % 递推滤波 estPos zeros(2, T); for k 1:T if ~tracker.hasPrior tracker.init(z(:,k)); estPos(:,k) tracker.xhat(1:2); continue; end tracker.predict(dt); tracker.correct(z(:,k)); estPos(:,k) tracker.xhat(1:2); end % 计算 RMS 误差 rmse_raw sqrt(mean(sum((z - truePos).^2, 1))); rmse_fil sqrt(mean(sum((estPos - truePos).^2, 1))); fprintf(量测 RMSE: %.3f m\n, rmse_raw); fprintf(滤波 RMSE: %.3f m\n, rmse_fil);运行这段脚本会看到滤波后的 RMSE 显著小于原始量测。如果调大sigma_a滤波轨迹会更贴近量测RMSE 反而上升这就是 Q 设置过大的直接表现。反过来把sigma_a降到 0.1滤波轨迹几乎是一条直线在真正转弯场景中会出现系统性滞后。这个仿真本身就可以当作 Q 调整的测试台。3.2.1 从单目标到多目标协方差和门控先于关联算法单目标滤波做通后多目标跟踪的复杂度不在滤波器本身而在量测和轨迹的对应关系判定。多目标场景下每个检测量测都有三种可能关联到已存在轨迹、成为新轨迹、或判定为虚警。Matlab 的assignDetectionsToTracks函数封装了匈牙利算法但前提是你先提供“代价矩阵”而代价矩阵的每个元素就是新息和马氏距离的计算结果。这也是为什么我在上一节的类里保留返回新息和协方差的接口。4. 多目标关联与跟踪生命周期车辆跟踪进入真实场景4.1 马氏距离与门控把关联问题变成带约束的分配问题当画面里出现多辆车时跟踪器不能简单地把最近的检测框当作同一个目标。常见的做法是计算预测状态与每个检测量测之间的马氏距离d(k,j) (z_j - H*xhat_k) * S_k^(-1) * (z_j - H*xhat_k)其中S_k是量测预测的协方差矩阵xhat_k是第 k 条轨迹的预测状态。马氏距离相比欧氏距离的优点是考虑了每个维度的不确定性在不同方向上的差异——例如纵向位置误差通常大于横向位置误差马氏距离可以自动加权。随后用卡方分布表选择阈值量测维度为 2位置 px, py时取 95% 置信度对应的门控值为 5.99取 99% 时为 9.21。一个检测量测如果与所有轨迹的马氏距离都超过门控值就认为它来自新目标或虚警。在 Matlab 中实现门控过滤和分配极为简洁% 预测协方差需要从 P 转换到量测空间 S tracker.P * tracker.H * inv(tracker.H * tracker.P * tracker.H tracker.R); % 实际上这个值在 correct() 内部已经算过正式实现中应当复用 % 马氏距离z 为当前帧所有检测框中心 [2 x N] for i 1:size(z,2) innov z(:,i) - H * tracker.xhat; d(i) innov / S * innov; end % 门控阈值2 自由度下 95% 置信度 gateThreshold chi2inv(0.95, 2); validIdx find(d gateThreshold);chi2inv是 Matlab 内置函数用来直接计算门控阈值避免查表。在实际项目中门控阈值会被做成可调参数因为不同场景对误关联的容忍度不同。高速公路场景车辆轨迹稀疏阈值可以放宽到 95% 甚至 90%城市交叉口目标密集需要收紧到 99%否则相邻车道的车容易被错误关联。4.1.1 全局最近邻GNN与联合概率数据关联JPDA的工程取舍有了门控和代价矩阵之后最简单的关联策略是每次分配让全局总代价最小这就是 GNNMatlab 中直接用assignDetectionsToTracks(costMatrix, costOfNonAssignment)实现。GNN 适合目标数量小于 20 的城市场景计算量和实现复杂度都低但是当两个目标靠近且轨迹交叉时GNN 容易出现“轨迹互换”。更稳的方案是 JPDA它考虑一个量测可能属于多个目标的所有假设并进行概率加权。工程上我一般建议先上 GNN然后用轨迹的“存活期”来抵抗换轨——如果某条轨迹连续若干帧量测都落在门控边缘就降低其置信度而不是立刻删除。JPDA 在 Matlab 中也有官方支持但由于其计算量随目标数量指数增长实际部署时往往是最后的选择。4.2 轨迹生命周期的四个状态初始化、确认、保持、删除多目标跟踪器不能只做“当前帧分配”还必须管理轨迹的生命周期。每一条轨迹有四个阶段初始态Tentative首次检测到目标时创建此时不对外发布轨迹因为单个检测框可能是虚警。确认态Confirmed连续 N 帧通常 3~5 帧都关联到量测后转为确认态只有确认态轨迹才送入下游融合模块。保持态Holding目标被短暂遮挡如被大车挡住时通过预测外推维持轨迹但不再更新协方差。这个阶段应限制最长维持时间通常不超过 1 秒否则轨迹会漂移。删除态持续未关联到量测且超过维持次数后直接删除。以下代码段展示了如何用一段简洁的循环来更新每个轨迹的属性% tracksStatus 保存每条轨迹累计量测命中的次数和连续未命中次数 for i 1:numTracks if ~tracksStatus(i).hitThisFrame tracksStatus(i).missCount tracksStatus(i).missCount 1; else tracksStatus(i).missCount 0; tracksStatus(i).hitCount tracksStatus(i).hitCount 1; end if tracksStatus(i).hitCount 3 tracksStatus(i).isConfirmed true; end if tracksStatus(i).missCount 10 tracks(i).delete(); % 超出维持帧数删除轨迹 end end4.2.1 检测框与轨迹的 IoU 交互另一种被低估的关联信号基于位置的马氏距离只考虑中心坐标完全忽略了框尺寸信息。当两个目标在横向上距离很近但速度方向不同时只用位置距离很难区分。一个实用的补强手段是在代价矩阵里加入一个“尺寸代价”项计算轨迹预测框和当前检测框的交并比当 IoU 过小时即使中心距离很近也给予惩罚。代码上可以用bboxOverlapRatio函数快速计算 IoU乘以一个比例因子加到代价矩阵中。这样做在雨雾天或者检测框噪声大的情况下效果提升很明显。5. 预测跟踪参数校准的进阶技巧让滤波器状态评估可解释当跟踪算法在真实路测中表现异常时第一步不是改参数而是确认异常来自检测器还是滤波器。一个最直接的手段是把“新息序列”拉出来做自相关分析。理想情况下卡尔曼滤波的新息应当是不相关的零均值白噪声如果新息序列出现持续偏置说明模型失配如果新息方差远大于S的理论值说明 Q 设置偏小或量测噪声被低估。在 Matlab 里做这个验证非常方便只需把每帧 correct 步骤中的新息存下来innovations zeros(2, T); % 在滤波循环中收集 [tracker, innovations(:,k)] tracker.correct(z(:,k)); % 检查自相关若 lag0 处相关性显著说明模型没有完全吸收系统偏差 [acf, lags] xcorr(innovations(1,:) - mean(innovations(1,:)), 10, normalized); stem(lags(11:end), acf(11:end));如果自相关图在 lag1 处明显超出 95% 置信带通常不是调 Q 能解决的而是运动模型选型错了——例如目标在持续转弯而你还在用 CV 模型。最后一个容易被忽视的参数是时间基准的同步。如果预测步骤用了上一帧的dt但检测器输出实际来自稍早时刻的数据传感器内部延迟跟踪输出的平滑性会显著下降。在量产架构里我会为每个传感器数据包打上时间戳并让跟踪器运行在统一的周期调度中而不是检测到一帧处理一帧。对于延期到达的数据包用预测步骤把轨迹推进到该时间戳再做一次 correct回到当前时间做下一次预测。这套“时间戳对齐 新息自相关检查 门控与生命周期管理”的组合基本覆盖了预测跟踪落地到路测的 80% 问题。如果你正在用trackerGNN做方案验证我建议依然保留一个手写的单目标滤波器专门用来标定 Q 和 R 数量级——工具箱给出的默认值在真实场景里通常过于乐观。本文还有配套的精品资源点击获取