企业官网建设流程全解析

在动态环境下做移动机器人轨迹规划，预测模块要不要加、加多复杂，一直是评估规划器性能绕不开的问题。我最近把一个对比实验的示例工程整理完了，把无预测、恒速模型、恒加速模型和概率预测模型四种预测量都接到同一套轨迹规划框架里，用Matlab做了蒙特卡洛对比，量化了它们对规划成功率、跟踪误差和计算耗时的影响。这篇从预测建模、规划器耦合方式、评价指标设计和实测结果几个维度展开，适合正在做避障规划、智能车决策或机器人导航的开发者参考。

先说结论：预测模块不是越复杂越好，恒速模型在大多数场景下性价比最高，概率预测在目标运动不确定性大时能明显提升安全余量，而无预测模型只适合低速、近场和障碍物近似静止的场景。下面的内容不打算从理论公式推到天上去，重点讲清楚每一层怎么建模、怎么实现、怎么和规划器接起来，以及我在实测中踩过的坑。

1. 为什么轨迹规划要单独引入预测模块

1.1 规划器看到的只是“现在”这一帧

很多刚接触轨迹规划的同学会有一个直觉：每次控制周期重新规划一次，把障碍物当前位置塞进去算一条轨迹不就行了吗？这个思路处理静止障碍物没问题，但一旦障碍物在动，问题就来了。

规划器在当前时刻找出来的无碰撞轨迹，是“基于当前障碍物位置”的。可机器人从起点沿轨迹走到第3秒的那个位置时，障碍物可能也走到了同一个位置。这就是动态环境里常见的“规划滞后”问题：规划器用的是一帧快照，世界却一直在变化。

预测模块的作用，就是把这帧快照往前外推，给规划器提供“未来某个时刻障碍物大概在哪”的信息。轨迹规划从“静态避障”变成“动态避障”，核心差异就在这个时间维度上。没有这一维度，规划器只能靠高频重规划来兜底，一旦障碍物速度变快或者转身，就很容易撞上。

1.2 四种预测模型的定位差异

我在这套对比工程里选的四种模型，恰好覆盖了从“完全不用预测”到“显式表达不确定性”的完整谱系：

选这四种不是为了凑数。无预测是基线，用来对照“预测到底带来多少收益”；恒速和恒加速分别是一阶和二阶运动学假设，代表工程里最常见的外推方式；概率预测把观测噪声和目标机动不确定性显式建模，代表更高阶的做法。四者放在一起，才能看出什么时候加复杂度是值得的。

2. 四种预测模型的建模原理与Matlab实现

先统一状态向量表示。假设目标的状态为z = [x; y; vx; vy; ax; ay]，有些模型用不到加速度项，缺失位置补0即可。预测输出统一为N个未来时刻的位置序列，其中N = round(horizon / dt)。

2.1 无预测模型：静态世界下的基线

无预测模型的实现最简单：不管预测时域多长，所有未来时刻都返回障碍物当前这个点。

function pred = predictNone(z0, N) % 无预测：所有时刻都返回当前位置 pred = repmat(z0(1:2), 1, N); end

这个模型的定位是基线。在障碍物低速、规划周期短、机器人与障碍物距离远的场景里，它其实够用。因为每次重规划都能拿到新的障碍物位置，相当于用高频更新来弥补预测缺失。但它的风险也最明显：预测时域越长，静态假设和真实运动之间的偏差越大。在高速会车场景下，轨迹后半段基本是“盲区”。

2.2 恒速模型：一阶保持

恒速模型是工程里最常用的外推方式，也叫CV模型。它假设目标在未来一段时间内保持当前速度不变，位置随时间线性增长：

function pred = predictCV(z0, dt, horizon) % 恒速模型：p(t) = p0 + v0 * t N = round(horizon / dt); tau = (1:N) * dt; pred = [z0(1) + z0(3) * tau; z0(2) + z0(4) * tau]; end

很多情况下，恒速模型的表现已经足够好。一个关键细节是：这里的v0必须是经过滤波或估计后的速度，不能直接用传感器瞬间测量值。雷达或视觉输出的瞬时速度常有抖动，直接外推会把噪声放大成位置偏差，后面的规划器反而会选出莫名其妙的绕行轨迹。

2.3 恒加速模型：二阶保持

恒加速模型在恒速基础上增加加速度项：

function pred = predictCA(z0, dt, horizon) % 恒加速模型：p(t) = p0 + v0*t + 0.5*a0*t^2 N = round(horizon / dt); tau = (1:N) * dt; pred = [z0(1) + z0(3)*tau + 0.5*z0(5)*tau.^2; z0(2) + z0(4)*tau + 0.5*z0(6)*tau.^2]; end

理论上，当目标确实在匀加速运动时，这个模型会比恒速模型更接近真实轨迹。但实操中加速度估计是个麻烦事，它是速度的差分，对噪声极其敏感。目标在匀速段时，加速度估计值往往在0附近抖动，把这些噪声带入二次项，预测轨迹会出现弯曲不定的“假加速”。这正是恒加速模型在实际对比中不一定优于恒速模型的一个重要原因。

2.4 概率预测模型：给不确定性画范围

概率预测模型在这套工程里用了一个相对轻量的实现：假设目标速度估计存在噪声，用协方差随时间增长的高斯分布描述未来位置的不确定性，而不是只给一条质点轨迹。

function pred = predictProb(z0, dt, horizon) % 概率预测：均值按恒速传播，协方差随时间扩张 N = round(horizon / dt); tau = (1:N) * dt; mu = [z0(1) + z0(3) * tau; z0(2) + z0(4) * tau]; % 初始位置不确定度 Sigma0 = diag([0.10, 0.10]); % 过程噪声：近似为随机游走加速度驱动 Q = diag([0.25, 0.25]); sigma = zeros(2, 2, N); for k = 1:N sigma(:,:,k) = Sigma0 + Q * tau(k)^3 / 3.0; end pred = struct('mu', mu, 'sigma', sigma); end

位置方差随t^3增长这个结论，很多资料里直接给结果，没说来源。其实它是从“加速度为随机游走白噪声”这个假设推出来的：加速度积分一次得到速度方差q*t，再积分一次得到位置方差q*t^3/3。所以预测时域越长，这个分布扩散得越快，体现的正是“越往后越不确定”的直觉。

这里必须提醒一点：概率预测模型如果输出时只取均值mu去走和恒速一样的逻辑，那它和恒速模型没有本质区别，价值就丢了。概率模型的价值在于下游规划器把sigma也消费掉，用碰撞概率而不是单纯的距离来判断风险。

3. 轨迹规划器如何消费预测结果

3.1 用“候选轨迹生成+评价”框架做控制变量

预测模块的外推结果要送到规划器里才有意义。为了对比公平，我在这套示例工程里没有用特别复杂的最优控制算法，而是选择“候选轨迹生成+代价评价+选择”的规划框架：每次控制周期生成一组满足机器人运动学约束的备选曲线，逐条让评价函数打分，选得分最高的执行。

这个框架的好处是：预测模块变成了一个可插拔接口，四种模型用完全相同的规划逻辑和评价参数，所有性能差异都归因于预测模块本身。如果用MPC，会发现预测模型同时影响了优化问题的动态约束，变量就不好分离了。

候选轨迹一般可以用圆弧加直线、五次多项式或样条曲线生成。我工程里用的是在曲率空间中均匀采样曲率，然后积分出轨迹，每条轨迹按固定时间间隔输出采样点，方便和预测轨迹对齐。

3.2 碰撞检测与代价函数设计

规划器消费预测结果的方式，直接决定了预测模型的价值能否发挥。无预测、恒速、恒加速输出的是一条确定性位置序列，可以直接做最近距离检测；概率预测输出的是均值和置信椭圆，必须用概率距离来检测。

核心评价函数可以这样实现：

function cost = evaluateCandidate(P, pred, cfg) % P: 候选轨迹上按时间对齐的采样点，维度 2xN % pred: 预测模块输出，type 为 'point' 或 'gauss' R = cfg.robotRadius + cfg.obsRadius; cost = 0.0; for k = 1:size(P, 2) switch pred.type case 'point' d = norm(P(:,k) - pred.point(:,k)); if d < R cost = cost + cfg.penCollision * (R - d)^2; else cost = cost + cfg.lambda_dist * exp(-(d - R) / cfg.sigma_d); end case 'gauss' mu_k = pred.mu(:,k); Sigma_k = pred.sigma(:,:,k); md2 = (P(:,k) - mu_k)' / Sigma_k * (P(:,k) - mu_k); pColl = chi2cdf(md2, 2, 'upper'); cost = cost + cfg.lambda_prob * pColl; end end end

对于确定性的三种预测，我采用软约束惩罚：距离小于安全半径时给高额代价，距离大于安全半径时给予指数衰减的奖励，鼓励轨迹远离障碍物。对于概率预测，用马氏距离转化为碰撞概率，碰撞概率超过一定阈值（比如5%）的轨迹直接淘汰，在阈值之内则把概率加权进代价。

一个需要注意的细节是，在概率预测模型下，如果只计算到均值点的欧氏距离，就完全浪费了协方差信息；必须算马氏距离，让分布在长轴方向上的不确定性充分反映到代价里。这也是很多概率预测落不了地的原因——上游模型辛辛苦苦输出了分布，下游规划器却还是按点去用。

3.3 定量评价指标这样设计

对比不同预测模型的性能，不能只看某一个指标，否则很容易得出片面结论。我在工程里统计了四个维度：

• 成功率：机器人从起点成功到达终点、全程未碰撞的次数比例。
• 平均最小距离：整个运行过程中，机器人实际位置与障碍物真实位置之间的最小距离，反映安全余量。
• 单步规划耗时：一次控制周期内完成候选轨迹生成、碰撞检查、选择的平均时间。
• 终端跟踪误差：机器人到达终点时的实际位置与目标点之间的偏差。

重点说一下“平均最小距离”的计算：我用的“真实位置”指的是仿真里障碍物实际在的位置，不是预测模块输出的位置。只有这样才能公平比较不同预测模型对真实安全性的贡献，而不是比谁的预测输出更贴近障碍物自身，毕竟规划器最终关心的永远是实际物理世界的安全。

4. 仿真对比实验与结果分析

4.1 实验场景与参数设定

为了模拟真实环境里最常见的非平稳目标，我搭了一个20m × 12m的矩形场景，机器人需要从左侧起点运动到右侧终点。障碍物从场景中部进入，运动规律分为三段：先以2m/s匀速直线运动，再以0.8m/s²加速度匀加速2秒，最后折线转向。整个过程包含匀速、加速和转弯，能同时考验四种预测模型对低速段、机动段和方向突变段的外推能力。

仿真参数如下：

• 机器人最大线速度：1.5m/s
• 规划周期：0.2s
• 预测时域：4s
• 安全距离：0.5m
• 观测噪声：位置加高斯噪声，标准差0.05m；速度加高斯噪声，标准差0.15m/s
• 每种预测模型跑30次蒙特卡洛，每次使用相同随机种子保证噪声序列一致，严格做到控制变量

4.2 蒙特卡洛对比数据

在我这套示例工程的默认参数下，30次蒙特卡洛平均结果如下：

需要说明，具体数值会随场景尺寸、障碍物运动特性和噪声水平变化，但相对排序在多数动态场景下是稳定的。这里面最值得琢磨的不是概率预测拿了最好成绩，而是恒加速模型竟然输给了恒速模型。

4.3 现象解读：为什么恒速常常比恒加速稳

恒加速模型理论复杂度更高，但实验成绩反而不如恒速，原因在于加速度项的噪声放大效应。障碍物进入匀速段后，真实加速度为0，但从带噪声的速度差分出的加速度估计值在零附近来回跳动。这些跳动经过二次积分之后变成明显的轨迹弯曲，让规划器误以为目标在转弯，于是选出过度保守甚至来回摇摆的轨迹。

恒速模型吃亏的地方只有目标发生真实加速的那一小段时间，但它在这段时间之外保持了稳定、可解释的外推结果。工程上很多系统在用“恒速+过程噪声”的组合，本质上就是对这种权衡的妥协：用不确定性包容加速度变化，而不是冒险去估计一个噪声极大的二阶量。

概率预测模型取得最高成功率和最大安全余量，代价是耗时几乎翻倍。这个结果说明概率预测带来真实收益的前提是规划器在每一帧都做了足够多次的分布查询，如果目标真的很多、候选轨迹候选集很大，“概率查询”这个环节就会成为性能瓶颈。下一节专门聊怎么处理这些坑。

5. 常见问题与避坑指南

5.1 预测时域怎么取

预测时域是第一个容易踩坑的参数。设短了，预测模块形同虚设，规划器又回到高频重规划兜底的老路上；设长了，恒速恒加速模型的外推误差快速放大，概率模型的置信椭圆也会变得非常大，导致规划器频繁绕路。

我的经验公式是：预测时域至少要覆盖机器人从当前速度到完全刹停所需的距离，再加上一个反应时间余量。以机器人最大速度1.5m/s、最大减速度0.8m/s²为例，理论制动距离约1.4m，按运动时间算大约1.9s，再留0.5s反应时间，预测时域取2.5s到3s比较合理。如果场景里目标机动性强，比如有人突然转向，这个时域还要适当加长。

5.2 加速度估计必须滤波

用恒加速模型时，最忌讳直接对速度做差分拿加速度。一帧差分出来的加速度信噪比极低，预测出来的轨迹大概率是抖动的。我实际试过几种方案，比较好用的是对速度序列做二阶低通滤波之后再差分，或者直接用一个线性卡尔曼滤波器同时估计位置、速度和加速度。恒加速模型只有在加速度估计稳定时才有意义，否则它就是不如恒速模型。

5.3 概率模型的协方差标定

概率预测模型里协方差矩阵初值Sigma0和过程噪声Q的设定，不是拍脑袋定的。设太小，置信椭圆包不住真实目标位置，规划器会过度自信；设太大，椭圆扩散太快，规划器会把很多可行轨迹都判成高碰撞概率，绕路绕得厉害。

比较靠谱的标定方式是先录一段障碍物的真实运动轨迹，用恒速模型外推一段时域，统计外推位置与实际位置之间的残差，再根据残差协方差来定Sigma0和Q。简单说，就是用数据反推模型误差，而不是靠调参碰运气。

5.4 对比实验的控制变量细节

做横向对比时有一个特别容易忽略的细节：噪声序列必须一致。如果每次跑都重新生成噪声，指标差异可能来自随机性而不是预测模型。我在工程里给所有仿真固定rng(seed)，并且同一个seed下跑完一组四种模型再换下一个seed，这样统计结果才能落到模型差异上。

另外一个容易踩的坑是，评估预测模块时不要拿规划器自己输出的轨迹去和预测轨迹比，然后得出结论说哪个预测准。规划器结构的补偿作用会影响结果，必须用预测输入以外的真实世界观测来评价。

6. 实测后的选型建议

跑完这一组对比，我对预测模块的实际价值有了更明确的判断。低速、规则、传感器噪声小的环境，无预测模型完全够用，省下的计算量可以用来提高规划频率；一般室内移动平台和仓储机器人建议从恒速模型起步，它简单、稳定、可解释；要处理人车交叉、突然转向这类非平稳目标，概率预测带来的安全余量是值得的。

如果计算资源紧张，还有一个折中方案值得试试：平时用恒速模型做预筛选，只有检测到目标进入高风险距离范围时，才把这个目标切到概率预测模型来评估。我在示例工程里顺手接上了这个双模式逻辑，实际跑下来，单步规划耗时比纯概率模型下降约40%，同时成功率没有明显下降。这个方案我觉得是实践里性价比最高的选择。

最后分享一个小技巧：给预测模块单独做一层接口抽象，输出统一成确定性点序列或高斯分布结构体。这样以后想换更复杂的预测算法，比如交互多模型或者学习式预测，只需要改一个函数，下游规划器一行不用动。对比实验做完之后，这套工程还可以继续复用，我后来加的几组场景测试都直接受益于这个接口设计。