跳转至

10 机械编排与惯导解算(CSINS::Update 精读)

本文拆解 SINS 的心脏:从 IMU 原始增量到 att/vn/pos 的三步递推,逐行对照 CSINS::Update、CEarth::Update、CIMU::Update。 坐标系:ENU(东-北-天),vn=[vE,vN,vU],pos=[lat,lon,h](弧度),qnb=body→ENU(见 ./00_总览与架构.md 坐标系章节)。


〇、先说结论(防止迷失)

  1. 本工程虽然是 MEMS 板,但 CSINS::Update 走的是完整(高精度)分支——因为全工程没有任何地方调用 SetMemsgrade(1),isMemsgrade 恒为 0(CSINS::Init L3243)。严老师宁可多算,也不为了 MEMS 省这几笔运算。
  2. 机械编排 = 两个阶段:
  3. ① imu.Update:把子样角增量/速度增量合成 phim(角增量+圆锥)与 dvbm(比力积分+划桨)——见 11_算法附录;
  4. ② 三步递推:姿态 ⊗ 旋转矢量 → 速度加比力投影+gcc → 位置梯形积分。
  5. MEMS 真正"不值得补偿"的项(地球自转、科氏、高阶重力、多子样、g 灵敏度等),与高精度导航必须补的项,差异全在量级对比——见第四节总表,这是一眼看出差距的地方。

一、调用链与全景

main:  kf.Update(&wm, &vm, 1, TS, 5)          // nSamples=1
          └─ CSINSTDKF::TDUpdate
               └─ sins.Update(pwm,pvm,1,ts)   ← 机械编排(本文主体)
                    ├─ imu.Update(...)        // ① 圆锥/划桨 → phim, dvbm
                    ├─ 标定:Kg·phim−eb·ts, Ka·dvbm−db·ts(本工程 Kg=Ka=I31)
                    ├─ eth.Update(pos01,vn01) // ② 地球参数(RMh/RNh/wnie/wnin/gcc)
                    ├─ fn = qnb·fb           // 比力投影到导航系
                    ├─ 姿态:qnb ⊗ rv2q(phim)(完整版再补 −wnin·nts 旋转)
                    ├─ 速度:vn += (an)·ts
                    └─ 位置:pos += vn2dpos(vn, ts)

二、完整分支逐行拆解(本工程实际路径)

2.1 IMU 子样合成(imu.Update,已详述见 11 篇)

imu.Update(pwm, pvm, nSamples);                     // → phim(角增量), dvbm(比力积分)

2.2 传感器标定(本工程为单位阵,未做)

CSINS::Update L3273-L3276:

imu.phim = Kg*imu.phim - eb*nts;                    // 尺度 Kg + 零偏 eb
imu.dvbm = Ka*imu.dvbm - db*nts;                    // 尺度 Ka + 零偏 db
imu.phim = Kg*imu.phim - eb*nts;  imu.dvbm = Ka*imu.dvbm - db*nts;
  • Kg=Ka=I31, eb=db=O31(Init L3238)。
  • 本工程把零偏扣减放在 main 里(wm=(Gyro*DPS-eb)*TS),CSINS 内部不再重复扣——所以此处的 eb 保持 0,标定链路是"预留"的。

2.3 地球模型(CEarth::Update(pos01, vn01),完整分支)

CEarth::Update L3086-L3094:

wnie.i = 0.0,     wnie.j = wie*cl,     wnie.k = wie*sl;    // 地球自转(ENU):只有北/天分量
wnen.i = -vn.j*f_RMh,  wnen.j = vn.i*f_RNh,  wnen.k = wnen.j*tl;  // 载体相对地球运动造成的系旋转
wnin  = wnie + wnen;                        // 导航系相对惯性系旋转 = 自转 + 载体运动
sl2=sl*sl, sl4=sl2*sl2;
gn.k = -( glv.g0*(1+5.27094e-3*sl2+2.32718e-5*sl4) - 3.086e-6*pos.k ); // 正常重力(纬度高阶+自由空气)
gcc  = gn;
gcc -= (wnie+wnin)*vn;                      // 重力&科氏合并:向心/科氏 → gcc = gn − (ωin×vn)

物理含义: - wnie:地球自转角速度(7.29e-5 rad/s ≈ 15.04°/h)在导航系的投影,只贡献北向与天向(东向=0)。(ENU 分量 [0,ωcosL,ωsinL]、[−VN/RM, +VE/RN, +VE·tanL/RN];NED 对照与坐标系转换见 02 解算篇 · 06 机械编排方程 · 符号分量一览。) - wnen:载体在东/北面移动时,当地水平坐标系随地形旋转的角速度。 - wnin = wnie + wnen:姿态方程里"导航系自身也在转",四元数更新必须先把它去掉(见 2.4)。 - gcc:重力与科氏的组合,直接进速度方程。g0 的 5.27e-3·sin²L 项是椭球扁率引起的重力随纬度变化,2.33e-5·sin⁴L 是高阶;-3.086e-6·h 是自由空气高度修正。

2.4 姿态更新(考虑导航系旋转)

CSINS::Update L3296-L3305:

wib = imu.phim/nts;   fb = imu.dvbm/nts;
web = wib - Cbn*eth.wnie;      // 相对地球的角速度(供杠杆臂等用)
wnb = wib - Cbn*eth.wnin;      // 相对导航系角速度:扣掉导航系自身旋转
fn  = qnb*fb;                  // 比力投影到导航系
an  = rv2q(-eth.wnin*nts2)*fn + eth.gcc;   // 比力方向修正到中间时刻 + 重力&科氏
vn1 = vn + an*nts;                           // 梯形法外推 vn1(供位置用)
pos = pos + eth.vn2dpos(vn+vn1, nts2);     vn = vn1;
qnb = rv2q(-eth.wnin*nts)*qnb*rv2q(imu.phim);  // ⊗ 姿态更新:先卸掉导航系旋转,再右乘体轴转动
Cnb = q2mat(qnb);  att = m2att(Cnb);  Cbn = ~Cnb;  vb = Cbn*vn;

逐条物理:

表达式 作用 MEMS 分支(若被启用)
web = wib − Cbn·wnie 地球自转扣除(进杠杆臂/量测模型) 直接 web=wib
wnb = wib − Cbn·wnin 导航系旋转扣除(姿态率) 直接 wnb=wib
an = rv2q(−wnin·ts/2)·fn + gcc ① 比力方向转回半个周期前的中间时刻(减少旋转导致的速度误差);② 加重力+科氏 an = fn + gcc(省掉方向修正旋转)
qnb=rv2q(−wnin·ts)·qnb·rv2q(phim) 左乘卸"导航系跟着地球转",右乘加"体轴转动" qnb = qnb·rv2q(phim)

💡 为什么这里其实现的是"梯形积分":vn1 = vn + an*ts 用当前时刻的 an,位置更新再取 (vn+vn1)/2——相当于速度做了一次梯形外推,位置精度高一阶。这是完整版的做法;MEMS 分支用 pos(而非 pos01)的 eth.Update 属于简化。

2.5 位置更新 vn2dpos

CEarth::vn2dpos L3097-L3100:

CVect3 CEarth::vn2dpos(const CVect3 &vn, double ts) const
{
    return CVect3( vn.j*f_RMh,         // 纬度率 = 北速 / 子午圈曲率半径
                   vn.i*f_clRNh,       // 经度率 = 东速 / (卯酉圈曲率半径·cosL)
                   vn.k )*ts;          // 高度率 = 天速
}
  • f_RMh、f_RNh 由 CEarth::Update L3076-L3077 每一拍根据当前 pos 重算,是非线性(曲率半径随纬度/高度变化);f_clRNh 额外带了 1/cos L(纬度越高 1° 经度越短)。
  • 这正是"惯导积分到 85°N 会出现纬度高阶发散警告"的原因(psinsassert(pos.i<85°…))。

三、MEMS 分支长什么样(SetMemsgrade(1) 会走到)

代码里其实已经写好了 MEMS 简化版,只是本工程没开。对照(完整版 vs MEMS):

环节 完整(本工程实际走) MEMS(isMemsgrade=1) 被砍掉的量 为什么 MEMS 可以省
地球自转 wnie 北/天分量满算 置 0(CEarth L3081) 15.04°/h MEMS 陀螺零偏(初始可达 °/s 级,稳定后约 5~50°/h)远大于此,扣了也看不见
wnen(载体运动系旋转) 满算 置 0 速度×曲率/~10⁻⁴°/s 量级 量级 < 零偏噪声,扣了是噪声
wnin(导航系旋转) 姿态更新左乘 rv2q(−wnin·ts) 不绕(CSINS L3304 vs L3286) ~15°/h 同上
科氏力 (wnie+wnin)×vn 并入 gcc 不并入(CEarth L3084 vs L3093) 100m/s 时 ~10⁻³ g 远低于 MEMS 加计噪声 mVg
正常重力高阶项 2.33e-5·sl4 有 不要(CEarth L3091 vs L3083) < 0.02% g MEMS 加计分辨率达不到
位置更新的用中点 pos01/vn01 是 直接 pos(L3294/L3278) 高阶小量 省一步 eth.Update 求逆
姿态比力方向修正 rv2q(−wnin·ts/2) 有 无(L3301 vs L3283) 旋转耦合速度项 被速度噪声覆盖
IMU 安装轴重定义 IMURFU 未调用(prfu=NULL) 同左(与器件等级无关,可有 SetRFU) 装反即三轴全错(90° 级) 装配不可控时必须设置 R/L/F/B/U/D

结论:MEMS 分支砍掉的全部是"与自转/科氏/高阶重力相关的项"——它们的贡献都小于 MEMS 传感器的自身噪声。保留的 gcc 只含正常重力 gn,这是加计估算姿态必须的。


四、对比总表:MEMS 无所谓的补偿 vs 高精度必须的补偿 ⭐

按"量级 vs 噪声"一句话判据:补偿项的量级若小于传感器噪声,补了也是噪声;反之必须补。 下表按量级从小到大排:

补偿项 代表量级 MEMS(MPU9250 级) 战术级(1°/h) 惯导级(0.001°/h)
圆锥补偿(多子样) 高频振动下不可交换误差 单子样+1/12 近似(本工程) 多子样 2-5 多子样+纯软补偿
划桨补偿 角×线振动耦合速度 one-plus-previous 低阶 多子样 多子样+剥离误差
导航系旋转 wnin ~15°/h 可省 必须 必须
地球自转 wnie 15.04°/h 可省 必须 必须
科氏 (ω_in)×vn 100m/s ~1e-3 g 可省 必须 必须
正常重力 gn(g0 + 纬度项) ~g 主导 必须(保姿态基准) 必须 必须
高阶重力 sl4 项 <0.02% g 可省 要 必须
自由空气 -3.086e-6·h 1km ~0.3 mg 可省 要 必须
g 灵敏度 gSens/gSens2/gSensX 大 g 下 ±10⁻³ rad 姿态漂 本工程未启用 标定后要 必要(高过载平台)
加计二阶非线性 Ka2 满量程 0.1-0.5% 本工程未启用(代码有) 标定后补 必补
尺寸效应 / 杠杆臂 安装偏心~cm 本工程 lvGNSS 当状态估 必须 必须(亚 mm 级)
零偏一阶 Markov tauG/tauA 慢漂 本工程 β=0(白噪声) 常开 必开
交叉轴标定(Kg/Ka 非对角) 十字轴 ~10⁻² rad 未标定 I31 标定后填(需求高) 必填
IMU 轴反向重定义 SetRFU 90° 级(装反即失效) 未设置 必须 必须

(量级为典型参考,具体以器件手册为准;"必须/可省"指对该级器件在典型任务里的工程判断。)

读法: - 老手一眼看 wnie/wnin/科氏/高阶重力 四行——可省→必须→必须 的跃迁就是器件等级的分水岭; - 新手最快的对比点是 MEMS 分支 CEarth::Update 里那三行被注释掉的 wnie/wnen 代码(L3079-L3080)——被注释 = 这级器件"省掉的尊严"。

4.1 MEMS 器件噪声量级参考(MPU9250 级)

  • 陀螺:初始零偏可达 ±0.5~3°/s(≈1800~10800°/h),偏置稳定性约 5~50°/h;角度随机游走 0.1~1°/√h。
  • 加计:零偏约 10~100 mg 级,噪声密度 0.1~1 mg/√Hz 级。
  • 判据对表:地球自转 15.04°/h、科氏 100m/s 时 ~10⁻³ g、高阶重力 <0.02% g——全部落在上述陀螺零偏/偏置稳定与加计噪声(积分后)的内部。这就是 MEMS 分支将 wnie/wnen/科氏/高阶重力 注释掉的数值依据:补了也是被噪声淹没。
  • 结论:对 MEMS,"噪声框"盖住了一切低于 ~10⁻²°/s 或 ~10⁻³ g 的物理效应;只有正常重力 gn(~g)大到必须保留。

五、本工程真实配置快照

项 实际值 影响
isMemsgrade 0(未开 MEMS 分支) 编排按完整版算(预估多耗算力,静态可忽略)
Kg/Ka I31(未标定) 尺度误差直接进导航解
eb/db main 里固定常数扣减 残余零偏由 EKF 状态 9:14 估
gSens/gSens2/gSensX NULL(未启用) 高 g 下无 g 灵敏度补偿
Ka2 未调用 SetKa2 无二阶非线性补偿
nSamples 1 + onePlusPre 只走 1/12 低阶圆锥/划桨
tauG/tauA INF(β=0) 零偏建模为白噪声(08 篇 8.4 的坑)
杠杆臂 lvGNSS 进 EKF 状态 15:17 估 静态不敏感,动态需要
IMURFU(轴重定义) 未调用 SetRFU 若实际安装轴与标称不符需设 R/L/F/B/U/D
地球椭球 CEarth(a0,f0) 默认 WGS84(a≈6378137 m) 中低纬度低动态可忽略;高纬/长航必须

六、给 H743 移植的提示(衔接 14 篇)

  1. 想省算力:对 MEMS 可放心调用 SetMemsgrade(1),省下 wnie/wnin/高阶重力 后误差仍在 MEMS 噪声框内——这是本工程代码"演示"过的取舍。
  2. 要高机动(2.5g 转弯):至少把 nSamples 提到 2-3 + 开 onePlusPre 之外的多子样系数,否则圆锥/划桨精度不够(见 11 篇)。
  3. 要标定:Kg/Ka、gSens*、Ka2 均需转台标定数据,动态任务建议至少做温补零偏 + 尺度+十字轴标定,把结果填回 SetgSens/SetKa2。
  4. 坐标注意:本工程 ENU;你项目 H743 用 NED——速度 vU↔−vD、qnb 定义、gcc 符号都要翻,见 14 篇 和项目 ins_eskf_15d.h。

参考资料


导航