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 坐标系章节)。
〇、先说结论(防止迷失)¶
- 本工程虽然是 MEMS 板,但
CSINS::Update走的是完整(高精度)分支——因为全工程没有任何地方调用SetMemsgrade(1),isMemsgrade恒为0(CSINS::Init L3243)。严老师宁可多算,也不为了 MEMS 省这几笔运算。 - 机械编排 = 两个阶段:
- ①
imu.Update:把子样角增量/速度增量合成phim(角增量+圆锥)与dvbm(比力积分+划桨)——见 11_算法附录; - ② 三步递推:姿态 ⊗ 旋转矢量 → 速度加比力投影+gcc → 位置梯形积分。
- 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 篇)¶
2.2 传感器标定(本工程为单位阵,未做)¶
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),完整分支)¶
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 姿态更新(考虑导航系旋转)¶
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¶
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 篇)¶
- 想省算力:对 MEMS 可放心调用
SetMemsgrade(1),省下wnie/wnin/高阶重力后误差仍在 MEMS 噪声框内——这是本工程代码"演示"过的取舍。 - 要高机动(2.5g 转弯):至少把
nSamples提到 2-3 + 开onePlusPre之外的多子样系数,否则圆锥/划桨精度不够(见 11 篇)。 - 要标定:
Kg/Ka、gSens*、Ka2均需转台标定数据,动态任务建议至少做温补零偏 + 尺度+十字轴标定,把结果填回SetgSens/SetKa2。 - 坐标注意:本工程 ENU;你项目 H743 用 NED——速度
vU↔−vD、qnb定义、gcc符号都要翻,见 14 篇 和项目ins_eskf_15d.h。
参考资料¶
- 严恭敏, 严热宝. 捷联惯导算法与组合导航原理. 西北工业大学出版社 —— 机械编排、地球模型、圆锥/划桨详情
- PSINS 官网——
CSINS/CEarth原始实现 - 本 wiki:./11_算法附录_旋转矢量与机械编排.md(rv2q/圆锥/划桨底层)、./08_SINS_GNSS静态组合.md(
TDUpdate里调用sins.Update的组合上下文)、./14_H743移植指南.md
导航¶
- 上一篇:./09_SINS_GNSS动态组合.md
- 下一篇:./11_算法附录_旋转矢量与机械编排.md
- 回到总览:./00_总览与架构.md