自主避障
与路径规划

为什么"边走边重算"很难

预设航线在真实现场会失效：GPS 在室内/峡谷/电磁干扰下不可靠，环境是先验未知的，障碍还会动态出现。 于是设备必须同时回答两个互相纠缠的问题——"我在哪、周围长什么样"（定位与建图），和"下一步往哪走最安全、最省"（规划与控制）。 两者都要在机载算力、毫秒级延迟内闭环完成。

我们站在哪些工作之上

SLAM = 因子图上的最大后验估计

设待估状态为所有时刻的位姿与地图路标 $X=\{x_{0:T},\,\ell_{1:M}\}$，观测集合为 $Z=\{z_k\}$（视觉重投影、IMU 预积分等）。SLAM 的本质是求最大后验（MAP）：

假设每个观测服从高斯噪声 $z_k=h_k(X)+\eta_k,\ \eta_k\sim\mathcal N(0,\Sigma_k)$，对后验取负对数，MAP 就化成一个非线性加权最小二乘问题：

从 MAP 到可实时求解的正规方程

在动态地图上算路：RRT* + MPC

有了实时地图，先用采样式规划求一条全局参考路径。RRT* 在自由空间 $\mathcal X_{\text{free}}$ 里随机扩展并不断「重连」，使代价 $c(\cdot)$ 随采样数趋于最优：

再把参考路径交给模型预测控制（MPC）：在每个控制周期，于有限时域 $N$ 内滚动求解一个带动力学与避障约束的最优控制问题，只执行第一步，下一周期用最新状态重解——这正是"环境变了就当场绕开"的数学来源。

把这套理论压进机载、压进可靠性

阿尔戈的导航栈把上面这套跑在设备自己身上：紧耦合 VIO + 因子图增量优化做实时定位建图，RRT* 出全局参考、MPC 做局部滚动避障。 和通用方案不同，我们按「可靠性优先」取舍——给避障约束留足安全裕度 $d_{\text{safe}}$、对位姿估计的协方差做在线监控、定位置信度跌破阈值就触发降级（减速悬停 / 安全返航），而不是赌它一定对。