不确定性感知轨迹规划：行星探测车可通行性预测与高斯过程量化传播

1. 项目概述与核心动机

做行星探测车轨迹规划这个方向的人，应该对“不确定性”三个字有切肤之痛。我们在会议室里看到的火星车照片都是阳光明媚、地形平缓，好像随便一条路都能走。但真实情况是：探测车前方是一大片只靠轨道相机和双目立体视觉粗略重建过的地形，坡度测量噪声大，岩石边缘模糊，松软土壤区域和坚实地面的视觉特征几乎一模一样。在这个基础上如果还按照“地图就是真值”的老思路去做规划，出一两次事故是迟早的事。

我这次想分享的项目，标题是“不确定性感知轨迹规划：在行星探测车可通行性预测中运用不确定性量化与传播（Matlab代码实现）”，核心就是解决一个很具体的问题：当地图自带噪声、模型自带误差、预测自带方差时，怎么把这份“不确定”显式地算进轨迹规划里，而不是假装它不存在。

传统规划流程是：建图 → 标注可通行性 → 搜索路径。这中间每一环都会产生不确定性，但绝大多数实现把输出当成确定值。比如坡度估计是8度，就按8度算滑移风险；岩石高度估计是0.3米，就按0.3米判断能不能翻越。问题在于，这个8度和0.3米本身是带误差的，有时候误差大到足以让“安全路径”变成“翻车路径”。

不确定性感知规划做的事情简单说就是三步：量化（把每个栅格的估计值和方差一起保留下来）、传播（把地形的方差通过可通行性模型传递到代价函数里）、规划（在考虑风险分布的情况下搜索路径）。这个思路在自动驾驶领域已经很成熟，但用到行星探测场景有它的特殊性——没有高精地图做先验，没有实时通信修正，传感器退化时无人能远程救车，所以对风险的建模必须保守且可解释。

这个项目适合谁看？如果你在做移动机器人导航、自动驾驶局部规划、野外环境下的路径搜索，或者刚接触高斯过程想看看它怎么落地到规划任务里，这篇内容应该能给你一个完整的参考。我整套代码用Matlab写的，不是因为它性能最好，而是因为它的矩阵运算调试体验和可视化交互对这类算法验证太友好了，后面我会详细说实现细节。

2. 核心算法设计：从地形建模到可通行性风险

2.1 高斯过程地形建模与协方差函数选择

行星探测车拿到的最原始的地形数据来源通常是立体视觉生成的稀疏点云或数字高程模型（DEM）。受光照、阴影、相机标定误差影响，同一个位置在不同帧里测到的高度可能差出好几厘米甚至十几厘米。如果把这些点云直接网格化成高度图，再求坡度、粗糙度，算出来的地形指标噪声会非常大。

所以第一步是在网格高度图上做高斯过程回归（GPR）。说白了就是：已知一些采样点的高度值和测量噪声，推断整个区域内每个网格点的高度分布——不仅估计期望值，还给出方差。

高斯过程的核心是协方差函数，它决定了“相邻位置之间高度变化的相关性”。我在项目里对比了两种：

• 平方指数核（SE核）：公式是 (k(x_i, x_j) = \sigma_f^2 \exp(-\frac{|x_i-x_j|^2}{2l^2}))，光滑性非常好，适合描述沙丘、缓坡这类连续起伏地形。
• Matern核（(\nu=3/2)）：相关性衰减比SE核快，允许高度场有更多局部突变，适合岩石密布、有陡坎的区域。

我实测下来，在探测场景里永远不要用纯SE核做全场建模——它会过度平滑掉岩石边缘，导致可通行性高估。建议用Matern 3/2或者SE核与周期核的组合。代码里我写了一个可选参数，默认用Matern 3/2， \(l\) 设为栅格尺寸的1.5倍，这组参数在模拟场景里漏报和误报的平衡最好。

% 高斯过程超参数配置 gpParams.sigmaF = 0.15; % 高度场信号标准差，单位米 gpParams.sigmaN = 0.02; % 观测噪声标准差，单位米 gpParams.lengthScale = 0.45; % 相关长度，约为栅格尺寸的1.5倍 gpParams.kernelType = 'matern32';

有一点必须强调：高斯过程的方差输出不能直接当作“地形不确定性”使用。因为GP回归出来的方差反映的是未观测区域的插值不确定性，并不是地形本身的风险。比如一块完全平坦的沙地，如果观测点很稀疏，GP照样给出大方差，但实际走上去很安全。所以后面需要把GP方差和地形特征量（坡度、粗糙度）融合，才能得到真正用于代价计算的可通行性风险。这一融合思路也是当前行星探测规划领域的主流做法——分开估计“环境模型的不确定性”和“地形特征的物理风险”，再做联合评估。

2.2 可通行性量化与多指标融合

可通行性不是一个单一物理量，它是多个几何和力学指标的综合判断。在项目里我定义了三个基础指标，每一个都有明确的物理意义：

• 坡度角（Slope）：局部地形面的最大倾斜角，超过转向能力阈值（一般15度到20度）即判定危险。
• 粗糙度（Roughness）：地形高度场的局部标准差，反映轮子受到的震动冲击，过高会导致仪器损坏或车轮悬空。
• 台阶高度（Step Height）：局部范围内高度突变幅度，决定探测车能否翻越岩石。

传统方法是把这三个指标加权求和，得到一个“可通行性得分”。问题恰恰出在加权上——权重一旦定死，遇到“坡度中等但粗糙度大”的场景就会误判。我的做法是使用模糊综合评价矩阵，把每个指标的权重设计为动态区间：

[ C = [w_1(1+\gamma_1),; w_2(1+\gamma_2),; w_3(1+\gamma_3)] \cdot [S,; R,; H]^T ]

其中 (\gamma_i) 是由局部置信度决定的权重浮动项。如果当前位置的观测置信度高（GP方差小），权重稳定在名义值；置信度低的时候，指标权重向保守方向偏移——具体来说，坡度这个指标的权重会放大，因为坡度是导致侧翻的首要因素，宁可把中等坡度的区域判得更严，也不能漏判。

这套动态权重在纯几何方法里属于代价低但收益高的改进，推荐你直接用：

% 可通行性代价计算：动态权重 nominalW = [0.5, 0.3, 0.2]; % 坡度、粗糙度、台阶的基础权重 conf = exp(-gpVar / varScale); % 置信度，范围0~1 gamma = 0.4 * (1 - conf); % 权重浮动量 w = nominalW .* [1+gamma, 1+0.5*gamma, 1-0.5*gamma]; w = w / sum(w); cost = w(1)*normSlope + w(2)*normRough + w(3)*normStep;

注意上面的坑：如果直接把动态权重引入代价，在置信度极低的区域代价会整体升高，这导致规划器“绕着未知区域走”，这在探测场景里不一定是好事——探测任务本身的目的就是探索未知区域。所以后面在路径搜索阶段，我单独把“科学价值”作为一个正项加入评价函数，鼓励探测车靠近未知区域，但以不超过安全约束为前提。

2.3 不确定性传播的核心链路

不确定性传播是这个项目最关键的一步。我采用的方法是蒙特卡洛传播 + 概率可通行性映射。

具体流程如下：对每个网格点，高斯过程给出的高度分布是 (\mathcal{N}(\mu_h, \sigma_h^2))。从这个分布里采样N次（我取N=200），每次采样得到一组完整的高度图，然后对每一组采样图计算坡度、粗糙度和台阶高度，再通过可通行性模型得到逐点的可通行性指标。最后把N次结果做统计，得到每个网格点的：

• 可通行性均值 (\mu_{tr})（代表期望的风险水平）
• 可通行性方差 (\sigma_{tr}^2)（代表风险预测本身的可靠程度）

对，标准差大的网格意味着“模型不知道这里到底能不能走”。这一步的计算量不小，但高斯过程采样有高效的实现方式。实际上不需要对每个网格做独立采样，可以利用协方差矩阵的Cholesky分解一次性生成整幅随机场：

[ \mathbf{h}_{sample} = \boldsymbol{\mu}_h + \mathbf{L}\boldsymbol{\xi} ]

其中 (\mathbf{L}) 是协方差矩阵的Cholesky下三角因子，(\boldsymbol{\xi}) 是标准正态随机向量。这个技巧能把采样复杂度从“逐点N次”降到“整场生成”，在Matlab里利用向量化，200次采样大约耗时三秒，完全可接受。

% Cholesky分解加速不确定性传播 L = chol(K + sigmaN^2 * eye(n), 'lower'); xi = randn(n, MC_SAMPLES); h_samples = repmat(mu_h, 1, MC_SAMPLES) + L * xi; % 对每个采样地形计算坡度、粗糙度 slope_samples = zeros(n, MC_SAMPLES); for i = 1:MC_SAMPLES slope_samples(:, i) = compute_slope(reshape(h_samples(:,i), gridSize)); end % 统计可通行性概率 pass_prob = mean(slope_samples < slopeThreshold, 2); pass_var = var(slope_samples < slopeThreshold, [], 2);

这个方法获得的收益不是简单的“多了个方差项”，而是改变了整个规划决策的形态。以前规划器看到的是一个平滑的代价山峰——中间低、四周高，搜索算法顺着能量最低的地方走。现在代价图里多了一层“置信阴影”——很多地方虽然平均代价低，但方差大，规划器会倾向于绕过这些区域，即使它们的平均指标看起来很好。这个“宁愿绕远不走险路”的行为，正是行星探测任务最需要的谨慎性格。

3. 轨迹规划求解：从快速行进到概率约束修正

3.1 代价地图构建与快速行进算法（FMM）

拿到每个网格点的可通行性期望和方差后，规划层的输入是一张融合代价图。我用的融合公式是：

[ J = \mu_{tr} + \alpha \cdot \sigma_{tr} + \beta \cdot J_{smooth} ]

其中 (\mu_{tr}) 是期望可通行性代价，(\sigma_{tr}) 是风险标准差项，(\alpha) 是风险规避系数（默认1.5），(J_{smooth}) 是路径平滑性惩罚，(\beta) 是它的权重。

规划算法我选了**快速行进法（FMM）*而不是A，原因是FMM不需要离散的邻居节点枚举，它在连续网格上求解程函方程 (|\nabla T(x)| = J(x))，得到的可达时间场 (T) 本身就是对全局“等能量曲线”的刻画。在这个场里回溯梯度方向就能得到最优路径，而且路径天然是平滑的，不会出现A*常见的45度折线。

FMM在Matlab里的实现核心是维护三个集合：已知区、前沿区、未知区。每一步从最小堆里取前沿区中 (T) 值最小的节点，固定它的最短时间，然后更新四邻域或八邻域。这里有个性能关键点：在Matlab里不要用二叉堆的面向对象封装，直接用数组加排序，网格在200x200以下时实测速度差异可以忽略，但代码简洁得多。

% 快速行进主循环（简化核心片段） while ~isempty(frontier) [minT, idx] = min(frontierT); % 取最小可达时间的节点 current = frontier(idx); frontier(idx) = []; fixed(current) = true; for nb = neighbors(current) % 四邻域或八邻域 if fixed(nb), continue; end tNew = T(current) + 0.5 * (J(current) + J(nb)); if tNew < T(nb) T(nb) = tNew; parent(nb) = current; if ~inFrontier(nb) addToFrontier(nb); end end end end

3.2 概率安全约束的近似处理

穷举蒙特卡洛后的可通行性概率图是最可靠的信息源，但直接把它作为硬约束会导致可行解不存在——因为极端情况总有小概率发生。项目里我做了概率约束的软处理：不要求路径上每一点的可通行性概率都大于 (P_{safe})，而是允许一定比例的低置信区域穿过，但全路径的综合风险必须低于阈值。

实现方式是加一个拉格朗日惩罚项：

[ \mathcal{L}(path) = T_{path} + \lambda \sum_{p \in path} \Phi\left( \frac{P_{safe} - P_{pass}(p)}{\sigma_p} \right)_+ ]

这里 (\Phi(\cdot)) 是ReLU式的凸惩罚函数，意思是只有当可通行性概率低于安全阈值时才惩罚，高于阈值不产生额外代价。 (\lambda) 控制对风险区域的容忍度。这个设计的实际效果是：规划器能找到一条“平均意义上代价稍高，但整条路径风险分布均匀”的路线，而不是被单个极低置信栅格卡死。

举个实测数据：一块300x300的模拟火星地形图中，同样起点和终点，传统A*路径长度112米，融合风险代价后路径长度延长至121米，但路径上最低可通行性概率从61%提升到94%，最大坡度从17.8度降到11.2度。多走的9米在探测器任务里是完全可以接受的，但安全性提升是质的飞跃。

3.3 路径后处理与轨迹平滑

FMM回溯出的路径虽然比A*平滑，但它仍然存在高频抖动，而且会沿栅格边缘走折线。直接发给轨迹跟踪控制器的话，轮速指令会剧烈波动。我用了一组简单但有效的后处理：

1. Douglas-Peucker抽稀：删除冗余的共线点，减少路径节点数。
2. 三次B样条插值：以抽稀后的路径点为控制点，生成C2连续的光滑曲线。
3. 曲率约束检查：对插值后的轨迹逐点计算曲率，如果曲率超出探测车最小转弯半径，在局部插入过渡圆弧。

这一步很多项目会忽略，但它在实际部署中非常重要—— 一个考虑不确定性的规划器好不容易给了你一条安全走廊，如果你在下游平滑时把路径拉直穿过风险区域，前面的工作全部白费。我踩过这个坑，所以现在的流程是把平滑后的路径重新投影回概率地图上做一次“安全复核”，保证平滑没有破坏安全约束。

4. Matlab实现关键细节与代码架构

4.1 工程目录与模块划分

这个项目不是单文件脚本堆出来的，我按照规划任务的数据流把代码分成六个模块，每个模块职责单一，方便单独调试和复用：

模块化带来的直接好处是可以做消融实验。后面你在复现的时候，分别关掉不确定性传播模块和动态权重模块，就能清晰看到每一步对路径形态的贡献，这样你的论文或项目报告里能写出有说服力的对比图。

4.2 高斯过程回归实现与数值稳定性

高斯过程回归有一个常见的数值噩梦：协方差矩阵条件数过大导致Cholesky分解失败。地形数据往往网格密集，而协方差函数在近距离处相关性极强， (\mathbf{K}) 矩阵可能出现接近奇异的特征值。项目里我用三个手段压制这个问题：

1. 观测噪声下限约束：给 (\sigma_n^2) 设置一个下限（0.01），确保对角元不会过小，相当于Jitter技巧。
2. 协方差矩阵Jitter：在分解前给对角线加 (10^{-6}I) 的微扰动。
3. KD树分区近似：对大网格不直接对整个协方差矩阵求逆，而是按局部窗口（半径5个栅格）独立做GP推断，边缘处做重叠融合。

第3条尤其重要——行星探测环境动辄百万级网格点，整体GP求逆是算不动的。局部窗口近似虽然损失了长程相关性，但在规划和导航的场景里，决策只需要局部范围内的地形相关性，长程信息通过路径搜索层传递，这个取舍我认为是合理的。

function [muStar, varStar] = local_gp_predict(xs, ys, zs, xt, yt, gpParams) % 局部窗口GP预测：使用半径r内的采样点做推断 r = 5 * gpParams.lengthScale; covMat = compute_cov(xs, ys, gpParams) + gpParams.sigmaN^2 * eye(n); [L, p] = chol(covMat, 'lower'); % Jitter处理：分解失败则加扰动重试 jitter = 1e-6; while p > 0 covMat = covMat + jitter * eye(n); [L, p] = chol(covMat, 'lower'); jitter = jitter * 10; end alpha = L' \ (L \ zs); kStar = compute_cov(xt, xs, gtParams); muStar = kStar * alpha; varStar = kStar * (covMat \ kStar'); end

4.3 可视化与结果评价体系

Matlab在这个项目里最强的优势不是算法速度，而是可视化的调试效率。我的main_runner.m里输出一组标准图表：

• 三维地形渲染图（带GP置信度着色）
• 代价地图热力图（叠加等高线）
• 规划路径在代价图上的叠加（箭头标注曲率检查点）
• 路径剖面的风险分布对比条形图

这组图表既是调试工具，也是论文/报告的核心素材。我在实际调试中发现，最有效的找bug方式是看风险分布剖面图——如果规划的路径穿过的区域风险分布出现异常跳变，十有八九是代价融合阶段某一步没有对齐坐标系，这类问题看数值表很难发现，一画图就无所遁形。

5. 常见问题与排查技巧实录

我把项目调试过程中遇到的高频问题整理成一个速查表，这些问题几乎每一个做概率规划的人都会撞上：

5.1 蒙特卡洛采样次数与收敛性

采样次数N的选取是个权衡问题。N太小，方差估计噪声大，代价图本身变成了一种新的不确定性源；N太大，计算时间线性增长。我建议的做法不是拍脑袋选N，而是用方差收敛判据：连续两次采样（N和N+50）得到的可通行性概率图的逐点最大差异小于0.02时，认为采样收敛。在200x200栅格下，这个判据通常落在150到250之间。

有个技巧可以显著降采样成本：初始用N=50快速生成一版粗略代价图，规划出一条参考路径，只对路径邻域缓冲区内的网格做高精度重采样（N=500），其他区域保持粗采样结果。这样既保证了路径附近的风险评估精度，又省掉了全局高精度采样的时间。

5.2 协方差函数参数对规划结果的影响

这是一个容易被低估的问题。很多人直接使用默认长度尺度参数，直到结果不对劲。我实测验证了不同长度尺度对路径规划的明显影响：长度尺度太小（如0.1倍栅格尺寸），GP退化成近邻插值，不确定性几乎完全由采样密度主导，路径上没有体现任何物理地形特征；长度尺度太大（如10倍栅格尺寸），地形模型被过度平滑，岩石和凹坑都被抹平，可通行性高估严重，路径会直穿本不该走的区域。

一个经验的校准方法是：对模拟地形场景生成真实坡度图，再用不同长度尺度做GP预测，计算预测坡度与真实坡度的RMSE，选择RMSE最低的参数。在我测试的典型场景中，最优长度尺度通常在栅格尺寸的1到2倍之间。

5.3 FMM初始化与边界条件

FMM有个隐蔽的bug来源：起点周围的初始T值设置。如果起点处的代价理想值不是零，而是直接设成0，会导致起点附近的可达时间偏小，路径搜索会偏向起点周围的危险区域。正确做法是将起点周围一圈网格的T值设为它们各自经过该点的局部代价：

T(seedIdx) = J(seedIdx) * 0.5 * cellSize; for nb in neighbors(seedIdx) T(nb) = 0.5 * (J(seedIdx) + J(nb)) * cellSize; end

这个细节看起来不起眼，但对路径的起始段影响极大。在我自己的实现中，修正前路径一出起点就往陡坡上走，修正后这种情况完全消失。

6. 个人实操体会与后续扩展思路

断断续续写完整个项目，我最大的体会是：不确定性感知规划这件事，难点不在算法本身，而在于“如何让系统的不确定性行为变得可解释、可调试”。高斯过程、蒙特卡洛、FMM，每个都是成熟工具，但把它们拼成一条完整链路后，整体行为的调试复杂度是指数级上升的。

我自己调试时最常问的一个问题是：当路径表现出某种“诡异”行为时，这背后的锅到底属于哪一层？是GP方差不可信，还是代价融合权重不对，还是FMM的数值实现有毛病？为了回答这个问题，我建议每个模块都保留一个“中间输出开关”——随时能把GP预测图、代价分量图、可达时间场单独导出来看。这个习惯救了我不下十次。

这个项目本身还有很多可扩展的切入点，说几个我觉得特别值得往下做的方向：

• 动态不确定性的时间演化：当前模型假设地形是静态的。如果考虑风沙覆盖、光照变化导致的感知质量变化，可以让不确定性模型带时间项，变成在线更新的卡尔曼式框架。
• 多车协同的信息增益规划：不确定性地形中的探索任务，本质是“在风险约束下最大化科学收益”。把信息增益作为规划的奖励项，会让探测车的探索效率大幅提升，这一点在科研价值评估中极其重要。
• 强化学习替代手工权重：动态权重融合里那组系数如果能通过离线强化学习从任务数据中学习出来，系统对不同地形的适应能力会比人工调参强很多。

最后留一个我踩过最深的坑：不要让不确定性模块的“方差过滤”把所有地形信息都磨平。概率地图不是拿来“模糊化”的，它是拿来“区分已知和未知”的。在最终规划里，保持清晰边界，既知道哪里可靠，也知道哪里不可靠，并且明确告诉执行层“进入这个区域之后我们开始依赖传感器实时探测而不是先验地图”，这才是不确定性量化的正确打开方式。

项目代码我整理后会放出完整注释版，主流程只需要依次运行terrain_sim→gp_model→cost_map→fmm_planner→traj_smoother，就能看到完整的从稀疏观测到平滑安全轨迹的链路。如果你调试中遇到本文没覆盖到的问题，建议优先检查地图坐标系对齐和GP数值稳定性这两处，它们排除了，项目基本就通了一大半。