GNSS信号丢失怎么办？用PSINS工具箱模拟极端场景下的纯惯导救急方案

GNSS信号丢失的应急方案：用PSINS工具箱模拟与优化纯惯导性能

你正在调试一台自动驾驶车辆的导航系统，车辆即将进入一条长达三公里的穿山隧道。隧道入口的警示牌一闪而过，紧接着，车载显示屏上那个代表GNSS信号强度的绿色图标瞬间变红，并弹出“卫星信号丢失”的警告。此刻，车辆完全依赖惯性导航系统（INS）继续前行。你能信任它吗？三分钟后驶出隧道时，车辆的定位会偏离真实位置多远？是五米、五十米，还是已经“穿墙”到了另一条车道？这种在隧道、城市峡谷或地下停车场中GNSS信号被拒止的场景，正是组合导航系统设计中必须攻克的“阿克琉斯之踵”。单纯的理论分析在此刻显得苍白，我们需要一个能在电脑上复现极端场景、量化评估风险、并找到优化路径的实战工具。PSINS（Precise Strapdown Inertial Navigation System）工具箱，正是这样一位沉默而强大的“数字风洞工程师”。

本文将带你深入PSINS工具箱，超越基础的仿真演示，聚焦于如何主动构造GNSS拒止环境，系统性地对比纯惯导（SINS）与SINS/GNSS组合导航在信号中断期间的性能差异，并深入探讨卡尔曼滤波器（KF）的关键参数调优策略。我们的目标不是重复工具箱的说明书，而是分享一套从场景建模、仿真对比到参数整定的工程化方法论，帮助你在下一个项目里，面对信号丢失的突发状况时，能做到心中有“数”，手中有“策”。

1. 构建极端测试场景：超越标准轨迹

在开始任何导航算法测试之前，一份能反映真实挑战的轨迹数据是基石。PSINS自带的test_trj.m示例提供了一个很好的起点，但它是一个“温和”的全景展示。对于GNSS拒止环境研究，我们需要更有针对性的“压力测试”。

1.1 定制化轨迹分段：模拟城市复杂机动

trjsegment函数是PSINS中构建复杂轨迹的神器。其核心思想是将整个运动过程分解为多个“段”（segment），每段定义一种标准机动模式。为了模拟城市中可能遇到GNSS信号断续的场景，我们可以设计一段包含频繁启停、转弯，并特意插入长时间GNSS信号盲区的轨迹。

% 示例：设计包含GNSS中断测试段的复杂轨迹
ts = 0.1; % 采样间隔0.1秒
avp0 = [[0;0;0]; [0;0;0]; glv.pos0]; % 初始姿态、速度、位置

seg = trjsegment([], 'init', 0);
% 阶段1：正常行驶，GNSS信号良好
seg = trjsegment(seg, 'uniform', 50); % 匀速50秒
seg = trjsegment(seg, 'turnleft', 90, 5); % 5秒内左转90度（模拟路口转弯）

% 阶段2：进入隧道，GNSS信号完全丢失 - 这是我们的核心测试段
seg = trjsegment(seg, 'uniform', 30); % 进入隧道，匀速30秒（模拟隧道内直线行驶）
seg = trjsegment(seg, 'turnright', 45, 3); % 隧道内转弯
seg = trjsegment(seg, 'accelerate', 10, [], 2); % 隧道内加速
seg = trjsegment(seg, 'uniform', 40); % 继续匀速
seg = trjsegment(seg, 'climb', 5, 1, [], 30); % 隧道内上坡
seg = trjsegment(seg, 'deaccelerate', 8, [], 1.5); % 减速准备出隧道
% 阶段2总时长约 30+3+10+40+5+8 = 96秒，模拟一个中长隧道

% 阶段3：驶出隧道，GNSS信号恢复
seg = trjsegment(seg, 'uniform', 50); % 出隧道后匀速行驶
seg = trjsegment(seg, 'coturnleft', 180, 10, [], 6); % 一个缓和的掉头
seg = trjsegment(seg, 'uniform', 30); % 最终段

% 生成并保存轨迹
trj = trjsimu(avp0, seg.wat, ts, 1); % 只生成1次
trjfile('tunnel_scenario.mat', trj);

提示：trjsegment中'uniform'参数后的数字代表该段运动的持续时间（秒）。精心设计各段时长，特别是信号丢失阶段的时长和机动复杂性，直接决定了后续纯惯导误差累积的严重程度。一个常见的做法是准备多个不同盲区时长（如30秒、60秒、120秒）的轨迹文件，用于评估系统在不同压力下的表现。

1.2 注入更贴近现实的IMU误差

PSINS中的imuerrset函数用于设置IMU误差参数。示例中给出的误差参数（如陀螺零偏0.01°/h）可能对应中高精度光纤陀螺。为了测试算法的鲁棒性，或者模拟消费级MEMS IMU在自动驾驶、无人机上的应用，我们需要使用误差更大的参数。

% 对比不同等级IMU的误差设置
% 高精度（战术级）IMU误差参数
imuerr_high = imuerrset(0.01, 100, 0.001, 10); % 陀螺零偏(°/h)，陀螺随机游走(°/√h)，加计零偏(μg)，加计随机游走(μg/√Hz)

% 中低精度（消费级MEMS）IMU误差参数 - 更常见的场景
imuerr_low = imuerrset(10.0, 1000, 5.0, 500); % 陀螺零偏增大1000倍，加计零偏增大5000倍

% 将误差添加到“理想”的轨迹IMU数据中
imu_ideal = trj.imu;
imu_with_err_high = imuadderr(imu_ideal, imuerr_high);
imu_with_err_low = imuadderr(imu_ideal, imuerr_low);

通过使用imu_with_err_low进行后续仿真，我们可以直观地看到，在同样的GNSS盲区内，低精度IMU会导致怎样灾难性的定位漂移。这迫使我们在滤波器设计时，必须考虑更宽的噪声协方差和更强的鲁棒性。

2. 纯惯导（SINS）仿真：量化误差的累积过程

运行test_SINS.m进行纯惯导解算，是我们建立性能基准的关键一步。但仅仅看最终的误差曲线图是不够的，我们需要深入解读误差增长的动力学。

2.1 理解误差传播的关键因素

纯惯导的定位误差主要来源于两个方面：初始对准误差和惯性传感器误差。PSINS通过avperrset和imuerrset来模拟这两者。

• 初始对准误差：包括失准角（姿态误差）、速度误差和位置误差。即使在静态环境下，初始的微小姿态误差也会在积分过程中，因地球自转和比力测量偏差而被不断放大。
• 惯性传感器误差：包括常值零偏、比例因子误差、随机游走等。其中，陀螺的常值零偏是导致位置误差随时间三次方增长的主要元凶。这是因为角速度误差积分一次得到姿态误差，姿态误差导致比力在导航坐标系下投影错误，错误比力积分一次得到速度误差，速度误差再积分一次才得到位置误差。

为了直观展示不同误差源的影响，我们可以设计一个对比实验：

注意：上表中的“预计位置误差量级”是一个定性描述，旨在强调陀螺零偏对长时纯惯导性能的决定性影响。实际误差需要通过仿真具体计算。

2.2 从仿真结果中提取关键指标

运行test_SINS.m后，除了观察图形，我们更应关注程序输出的数据。avperr变量包含了位置、速度、姿态误差随时间变化的数据。我们可以编写简单的脚本来计算在GNSS盲区时段（例如第50秒到第146秒）内的误差统计特征。

% 假设avperr是一个Nx10的矩阵，第10列是时间，第7-9列是东、北、天方向位置误差（米）
% 找出GNSS盲区时间段内的数据索引
blind_start_idx = find(avperr(:,10) >= 50, 1);
blind_end_idx = find(avperr(:,10) <= 146, 1, 'last');

% 提取盲区内的位置误差
pos_err_blind = avperr(blind_start_idx:blind_end_idx, 7:9);

% 计算关键统计指标
max_pos_err = max(abs(pos_err_blind), [], 1); % 三个方向的最大绝对误差
mean_pos_err = mean(pos_err_blind, 1); % 平均误差
std_pos_err = std(pos_err_blind, 0, 1); % 误差标准差
drift_rate = (pos_err_blind(end, :) - pos_err_blind(1, :)) / (avperr(blind_end_idx,10) - avperr(blind_start_idx,10)); % 误差漂移率 (米/秒)

fprintf('GNSS盲区期间（%.1f - %.1f 秒）位置误差分析：\n', avperr(blind_start_idx,10), avperr(blind_end_idx,10));
fprintf('  东向：最大误差=%.2fm，均值=%.2fm，漂移率=%.4f m/s\n', max_pos_err(1), mean_pos_err(1), drift_rate(1));
fprintf('  北向：最大误差=%.2fm，均值=%.2fm，漂移率=%.4f m/s\n', max_pos_err(2), mean_pos_err(2), drift_rate(2));
fprintf('  天向：最大误差=%.2fm，均值=%.2fm，漂移率=%.4f m/s\n', max_pos_err(3), mean_pos_err(3), drift_rate(3));

这些量化的指标（特别是漂移率）比图形更能清晰地告诉我们，在当前IMU等级和初始条件下，系统在无GNSS辅助时，定位精度衰减的速度有多快。这是评估是否需要引入其他传感器（如轮速计、视觉里程计）进行辅助的重要依据。

3. SINS/GNSS组合导航仿真：滤波器如何“力挽狂澜”

现在，我们打开test_SINS_GPS_153.m（或更高维度的193等），看看当GNSS信号恢复后，卡尔曼滤波器如何修正已经“放飞自我”的惯导系统，以及它在信号中断期间如何维持状态估计。

3.1 深入解析15状态卡尔曼滤波器模型

psinstypedef(153)定义了一个15维的状态向量，这是SINS/GNSS松组合中最经典的模型。理解这15个状态的具体含义，是进行参数调优的基础。

状态向量 X 通常包括：

1. 失准角误差 (3维)：数学平台系相对于导航系的姿态误差角（东、北、天方向）。
2. 速度误差 (3维)：东、北、天方向的速度误差。
3. 位置误差 (3维)：纬度、经度、高度误差。
4. 陀螺零偏 (3维)：三个轴向陀螺的常值零偏误差。
5. 加速度计零偏 (3维)：三个轴向加速度计的常值零偏误差。

滤波器的工作就是利用GNSS测量的位置（有时还有速度）与SINS解算的位置/速度之间的差值（即量测新息），来持续估计并反馈校正这15个状态量。

3.2 核心调优参数：过程噪声Q与量测噪声R

在PSINS的kfinit函数初始化中，有两个矩阵对滤波器性能至关重要：过程噪声协方差矩阵 Q 和 量测噪声协方差矩阵 R。它们本质上是告诉滤波器，你对系统模型和传感器测量有多信任。

• 过程噪声 Q：代表了系统状态（即上面那15个误差）随时间变化的不确定性。它主要由IMU噪声特性决定。在PSINS中，Q矩阵通常通过imuerrset函数生成的imuerr参数间接构造。
• 量测噪声 R：代表了GNSS接收机测量值的不确定性。在示例代码中，rk = poserrset([1;1;3])这一行就是在设置R矩阵，它表示东、北、天三个方向的位置测量噪声标准差分别为1米、1米和3米。

调优实战：如何设置R矩阵？ poserrset的参数设置需要结合你使用的GNSS接收机的实际性能。例如：

• 单点定位的消费级GPS模块：噪声可能达到[3; 3; 5]米。
• 使用RTK（实时动态差分）技术：噪声可以小到[0.01; 0.01; 0.03]米。
• 信号遮挡严重的城市峡谷：即使使用高端接收机，水平噪声也可能增大到[2; 2; 4]米。

% 不同场景下的R矩阵设置示例
% 场景1：开阔天空，双频GNSS接收机
rk_good = poserrset([0.5; 0.5; 1.0]); % 水平0.5米，高程1.0米

% 场景2：城市环境，多径效应显著
rk_urban = poserrset([2.0; 2.0; 4.0]);

% 场景3：模拟GNSS信号完全丢失（在仿真中动态切换）
% 这需要在kfupdate的量测更新环节进行条件判断
if gnss_available == false
    % 一种方法是设置一个巨大的R值，等效于不使用该量测
    % 另一种更优雅的方式是直接跳过kfupdate的量测更新步骤
end

一个关键技巧：动态调整R。 在真实系统中，GNSS的定位精度并非恒定。我们可以根据接收机输出的位置精度因子（PDOP）、卫星数或载噪比（C/N0） 来动态调整R矩阵的值。在仿真中，我们可以模拟这一过程：当车辆进入隧道时，逐渐增大R矩阵中的值，模拟GNSS精度下降；当完全无信号时，暂停量测更新。这比简单地让GNSS数据“消失”更能反映滤波器的鲁棒性。

4. 滤波器高级调优与工程实践

掌握了基础模型和参数后，我们还需要关注一些工程实现上的细节，这些细节往往决定了算法在实际产品中的成败。

4.1 应对GNSS信号中断与跳变

GNSS信号并非总是优雅地消失和出现。更常见的是断续续、时好时坏，甚至出现跳变（当接收机在遮挡后重新捕获卫星时，可能会产生一个位置跳变）。滤波器必须能妥善处理这些情况。

• 量测新息检测：在调用kfupdate进行量测更新前，计算新息（INS位置 - GNSS位置）的范数。如果新息突然远大于根据滤波器协方差预测的范围（例如，超过5 * sqrt(diag(kf.Pxk))），则很可能遇到了GNSS跳变或故障。此时，可以采取的策略包括：
  1. 丢弃该次量测：不进行本次更新。
  2. 渐消记忆滤波：临时增大过程噪声Q，让滤波器更快地“忘记”旧信息，适应新的量测。
  3. 重置部分状态：如果确信GNSS跳变是接收机问题而非INS问题，可以只重置位置和速度状态，保持姿态和传感器零偏的估计。

% 简化的新息检测逻辑（在量测更新循环内）
if mod(t,1)==0 && gnss_available
    posGPS = trj.avp(k1,7:9)' + gnss_noise.*randn(3,1);
    z = ins.pos - posGPS; % 量测新息

    % 计算新息的归一化平方（Mahalanobis距离）
    S = kf.Hk * kf.Pxk * kf.Hk' + kf.Rk;
    rho = z' / S * z;

    if rho < chi2inv(0.95, 3) % 95%置信度的卡方检验，3为量测维数
        % 新息正常，执行标准更新
        kf = kfupdate(kf, z, 'M');
        [kf, ins] = kffeedback(kf, ins, 1, 'avp');
    else
        fprintf('时间 %.1f秒: 检测到GNSS量测跳变，新息过大(%.2f)，本次更新被抑制。\n', t, rho);
        % 可选：增大过程噪声Q，让滤波器在接下来几秒内更信任预测
        % kf.Qk = kf.Qk * 10;
    end
end

4.2 状态可观测性与反馈控制

PSINS示例中使用的kffeedback是反馈校正，即用估计出的误差状态去校正INS的导航解算结果。这是最稳定、最常用的方法。kffeedback的第三个参数是反馈系数，通常设为1（全反馈）。但在某些情况下，例如当某个状态的可观测性很差、估计值波动很大时，对其进行全反馈可能会引入噪声。这时，可以考虑对不同的状态设置不同的反馈系数，或者只反馈那些收敛性好、置信度高的状态。

如何判断状态的可观测性？一个直观的方法是观察kfplot输出的状态估计均方差（P阵对角线元素的平方根）曲线。收敛到较低水平并保持稳定的状态，其估计值就较为可靠。例如，在静止或匀速直线运动阶段，天向陀螺零偏和加速度计零偏的可观测性就很差，它们的估计误差可能不会收敛。

4.3 融合其他传感器：从153状态到更多状态

15状态模型是基础。在实际工程中，为了获得更好的性能，尤其是应对长时GNSS拒止，我们常常需要扩展状态向量，融合其他传感器信息。PSINS工具箱也提供了相应的接口（如test_SINS_GPS_186、test_SINS_GPS_193）。

• 186状态模型：在153基础上增加了杆臂误差（3维）。杆臂是GNSS天线相位中心相对于IMU测量中心的位移。如果这个物理量测量不准或车辆结构形变，就会引入一个系统性误差。在组合导航中估计杆臂误差，可以提高GNSS量测与INS预测之间的一致性。
• 193状态模型：在186基础上增加了时间同步误差（1维）。GNSS数据与IMU数据的时间戳如果不同步（即使只有几十毫秒），在高速运动下也会导致显著的融合误差。193状态模型将这个时间差作为一个状态进行估计和补偿。

选择哪种模型，取决于你的系统配置和性能需求。一个简单的原则是：先使用简单模型（153）进行调试和验证，当发现残余误差有规律且无法通过调参消除时，再考虑引入更复杂的模型（186/193）来解释这些误差。

在完成了上述所有仿真、分析和调优之后，你得到的不仅仅是一组更优的滤波器参数，更是一套对自家导航系统在极端环境下性能边界的深刻认知。下次当产品经理问“我们的车在最长隧道里能坚持多久不跑偏？”时，你可以调出相应的仿真误差曲线图，指着上面明确标注的“60秒盲区，水平定位误差95%概率小于15米”的数据点，给出一个基于数据的、令人信服的答案。这种从“大概可能”到“定量确定”的能力跃迁，正是工程仿真的核心价值所在。