从方向余弦阵到舒勒回路：惯性导航误差如何被“调”住

简介这份东南大学惯性导航期末复习资料面向备考该课程的本科生与需要梳理知识框架的读者围绕课堂重点整理成便于查阅的问答式复习文档。内容覆盖坐标变换矩阵、地球模型、惯性器件与陀螺仪、舒勒摆与舒勒回路、哥氏定理、比力方程、惯导基本方程、方向余弦阵以及指北方位与游移方位惯导、四元数、捷联惯导和初始对准等专题并配有25个典型问题帮助定位考点与查漏补缺。压缩包内共1个docx文件约18.25MB适合打印或按专题标注复习。当前已有955人学习可用于期末冲刺时快速回顾概念、公式推导思路与系统分类。1. 惯性导航里最反直觉的一件事精度不是“测”出来的是“调”出来的第一次翻惯性导航的复习资料多数人会把注意力全押在陀螺和加速度计上——器件越准导航越准天经地义。可把坐标变换矩阵、比力方程、舒勒回路这几块拼起来之后结论恰好相反一台惯导能连续跑几小时而不发散靠的不是器件有多神而是回路被调到了一个特定频率上。器件决定误差的底噪回路决定误差长不长个儿。这也是那几个推导——方向余弦阵微分方程、比力方程、舒勒调谐条件——反复出现的原因它们不是数学练习是整套系统的设计契约。某高校那份复习资料把内容拆成 25 个问题从坐标变换一直问到捷联精对准看着零散主线其实就这一条误差是怎么被设计出来的又是怎么被摁住的。2. 从方向余弦阵到比力方程惯导方程的骨架是怎么搭起来的惯导解算说到底就回答三件事我在哪、我朝哪、我怎么动。位置、姿态、速度三者由一组微分方程耦合在一起而所有方程的地基是坐标变换。先把“两根坐标系怎么对齐”讲清楚后面比力方程、惯导基本方程才有落脚点。2.1 方向余弦阵姿态为什么非得用 3×3 正交阵两个三维坐标系之间的关系写成矩阵就是 9 个元素每个元素是两个坐标轴夹角的余弦所以叫方向余弦阵。但刚体姿态只有 3 个自由度9 个元素里必然有 6 个约束表现到矩阵上就是正交性和行列式为 1$C^T C I$$\det C 1$。这两个性质不是数学装饰是后面所有反解、转置代替求逆的前提。求取方法在复习资料里通常列三类。第一类是欧拉角连乘把姿态拆成三次基本旋转按 Z-Y-X 顺序依次右乘第二类是四元数用四个参数描述绕某根轴转某个角度第三类是等效旋转矢量直接把一段有限转动压成一个矢量再做指数映射。三类方法解决的是同一个问题差别在奇异性和计算量。import numpy as np def dcm_from_euler(yaw, pitch, roll): 按 Z-Y-X 顺序合成方向余弦阵先偏航再俯仰最后滚转 cy, sy np.cos(yaw), np.sin(yaw) cp, sp np.cos(pitch), np.sin(pitch) cr, sr np.cos(roll), np.sin(roll) # 三次基本旋转矩阵注意右乘表示每次旋转都相对“当前”坐标系 Cz np.array([[cy, -sy, 0], [sy, cy, 0], [0, 0, 1]]) Cy np.array([[ cp, 0, sp], [ 0, 1, 0], [-sp, 0, cp]]) Cx np.array([[1, 0, 0], [0, cr, -sr], [0, sr, cr]]) return Cz Cy Cx C dcm_from_euler(np.deg2rad(30), np.deg2rad(5), np.deg2rad(-10)) print(np.allclose(C C.T, np.eye(3))) # True正交性成立 print(round(np.linalg.det(C), 6)) # 1.0仍是右手系这段代码的重点不在算出一个矩阵而在两个检查一是转置乘自身是否回到单位阵二是行列式是否保持为 1。参数顺序一旦写反矩阵照样正交、行列式照样为 1但含义完全错了——俯仰和滚转互换姿态解算会稳定地给出错误答案这种 bug 最难查。工程上更在意的是微分方程设 $b$ 系相对 $n$ 系的转动角速度为 $\omega_{nb}^b$则$$\dot{C}b^n C_b^n,[\omega{nb}^b\times]$$其中 $[\cdot\times]$ 是把矢量写成反对称阵。这个方程的物理含义很直白姿态矩阵的变化率等于“当前姿态”乘上“当前角速度造成的瞬时旋转”。对矩阵每一列也就是 $b$ 系基矢量在 $n$ 系中的投影求导再用一次哥氏定理就能得到它。注意用欧拉角递推姿态俯仰角接近 ±90° 时会出现万向锁两个轴重合、丢掉一个自由度。四元数之所以成为捷联惯导的默认选择就是为了绕开这个奇异点。2.2 哥氏定理把绝对导数拆成“相对 牵连”哥氏定理也译作科里奥利定理解决的是一个纯运动学问题同一个矢量在静止系里看的变化率和在旋转系里看的变化率差在哪里。结论是$$\left.\frac{d\mathbf{r}}{dt}\right|_i \left.\frac{d\mathbf{r}}{dt}\right|e \omega{ie}\times\mathbf{r}$$左边是惯性系里的绝对导数右边第一项是地球系里看到的相对导数第二项是坐标系旋转带来的牵连项。这一项不是修正是必须付的“过路费”只要你在旋转坐标系里做微积分就得补上它。把位置换成速度再求一次导就会出现两项新东西$2\omega_{ie}\times\mathbf{v}e$ 和 $\omega{ie}\times(\omega_{ie}\times\mathbf{r})$。前者是哥氏加速度系数 2 来自“矢量在转、参考系也在转”的双重效应后者是向心加速度方向指向自转轴。复习资料里问“结果中各项含义”考的正是这三个角色能不能对上号相对加速度、哥氏加速度、向心加速度。推导思路上有个容易卡住的地方哥氏定理对“任意矢量”都成立位置矢量成立速度矢量也成立但两次使用时参考系不能中途换。先在地球系里写位置关系求导得速度关系再对这个速度矢量用一次定理才能得到加速度关系。中间换系系数就会错这是手推时最常见的翻车点。2.3 比力方程与惯导基本方程加速度计读到的到底是什么加速度计测的从来不是“加速度”而是比力也就是非引力合力除以质量。用 $\mathbf{f}$ 表示比力则 $\mathbf{f} \mathbf{a} - \mathbf{g}$。静止放在桌面上运动加速度为零加速度计输出的是 $-\mathbf{g}$ 方向的分量这是很多人第一次做静态标定时困惑的根源仪表没坏是你把定义记混了。比力方程就是把牛顿第二定律搬到导航系里$$\dot{\mathbf{V}}^n \mathbf{f}^n - (2\omega_{ie}^n \omega_{en}^n)\times\mathbf{V}^n \mathbf{g}^n$$各项含义逐一对上$\mathbf{f}^n$ 是加速度计输出在导航系的投影代表载体感受到的非引力加速度$2\omega_{ie}^n\times\mathbf{V}^n$ 是哥氏加速度因地球自转与载体相对地球运动共同产生$\omega_{en}^n\times\mathbf{V}^n$ 是向心加速度因载体沿地球曲面运动、导航系本身跟着旋转产生$\mathbf{g}^n$ 是重力加速度已经在里面扣掉了离心效应。证明思路只有一条主线在惯性系里写 $\mathbf{a}_i \mathbf{f} \mathbf{G}$用哥氏定理把 $\mathbf{a}_i$ 换成导航系里的量再把引力 $\mathbf{G}$ 与离心加速度合并成重力 $\mathbf{g}$整理即得。看起来绕但每一步都对应一个物理量不是纯代数变形。惯导基本方程是比力方程再加上姿态方程和位置方程构成闭合方程组方程表达式作用姿态方程$\dot{C}b^n C_b^n[\omega{nb}^b\times]$$\omega_{nb}^b \omega_{ib}^b - C_n^b(\omega_{ie}^n\omega_{en}^n)$由陀螺输出更新姿态速度方程$\dot{\mathbf{V}}^n C_b^n\mathbf{f}^b - (2\omega_{ie}^n\omega_{en}^n)\times\mathbf{V}^n\mathbf{g}^n$由比力积分得速度位置方程$\dot{\varphi}V_N/(R_Mh)$$\dot{\lambda}V_E/[(R_Nh)\cos\varphi]$$\dot{h}V_U$由速度积分得位置所谓“闭合”是说陀螺给角速度、加速度计给比力代入这三组方程就能一路积分出姿态、速度、位置全程不需要外部信息。这也是惯导最硬核的地方它是自主的不依赖任何外部信号。代价同样明显——所有误差都靠自己扛没有任何外部参考来拉一把误差只能靠回路设计压住。3. 地球模型与惯性器件惯导为什么甘愿用一套“近似”的地球导航解算要在地球上做就必须先回答“地球是什么形状”。这个问题听着像地理课实际决定了整套方程的写法曲率半径怎么取、纬度怎么定义、重力怎么算全都挂在这个模型上。3.1 三种垂线、三种纬度惯导为什么最终选大地纬度常见的描述模型有两类。球形模型把地球当成半径约 6371 km 的正球公式最简短时间、低精度场合够用旋转椭球模型用长半轴 $a$、短半轴 $b$ 和扁率 $f(a-b)/a$ 描述能反映赤道鼓起、两极压扁的事实。垂线和纬度是成对出现的。地心垂线是地心到载体的连线对应地心纬度椭球法线垂直于椭球面对应大地纬度重力垂线沿重力方向对应天文纬度。三者并不重合差值虽然只有角分量级但对惯导来说已经不能忽略。惯导最终选的是“椭球模型 大地纬度 重力垂线”这一套。原因很实在平台的“水平”是由重力定义的加速度计测的是比力在垂线方向的分量物理上能对齐的只有重力垂线不是几何上的法线。选大地纬度是因为椭球法线与重力垂线在高精度下足够接近同时曲率半径有解析式可用子午圈半径 $R_M$ 和卯酉圈半径 $R_N$ 都随纬度变化位置方程里那两个分母就是它们。这也解释了一个常被问到的细节为什么位置方程里经度变化率要除以 $\cos\varphi$。经线在高纬度收拢同样的东向速度对应的经度变化更快$\cos\varphi$ 就是收拢系数。它同时埋了个雷——$\varphi$ 接近 90° 时分母趋零指北方位系统在极区没法用这个雷在后面第 4 章还会再炸一次。3.2 重力不是引力离心力那一项躲不掉重力加速度和引力加速度的关系只有一句$$\mathbf{g} \mathbf{G} - \omega_{ie}\times(\omega_{ie}\times\mathbf{r})$$引力是万有引力指向地心离心加速度由地球自转产生方向背离自转轴。两者矢量相加才是重力。赤道处离心加速度最大重力被削得最多两极处离心加速度为零重力最大。这就是重力随纬度变化的来源也是正常重力公式里那个 $\sin^2\varphi$ 项的由来。对惯导而言这个区别不是学术细节。加速度计静止时输出的是比力等于 $-\mathbf{g}$其中已经包含了离心效应如果在解算时把 $\mathbf{g}$ 当成纯引力处理等于凭空多算了一项离心加速度位置会缓慢漂移。这类错误在静态测试里看不出来跑动态轨迹才暴露属于典型的“血泪经验”型 bug。提示在比力方程里$\mathbf{g}^n$ 一律按重力理解不要拆成引力加离心再分别处理。拆开写容易在符号上打结尤其是在推导误差方程的时候。3.3 惯性级器件的门槛一条二阶方程撑起两种传感器加速度计的核心是一个摆组件。绕输出轴的转动用二阶方程描述$$J\ddot{\theta} D\dot{\theta} K\theta M_c M_d$$$J$ 是摆组件转动惯量$D$ 是阻尼系数$K$ 是弹性刚度$M_c$ 是再平衡力矩由力矩器产生$M_d$ 是干扰力矩。取零初始条件做拉普拉斯变换利用 $\mathcal{L}[\ddot{\theta}] s^2\Theta(s)$、$\mathcal{L}[\dot{\theta}] s\Theta(s)$得到$$\Theta(s) \frac{M_c(s)M_d(s)}{Js^2DsK}$$这就是一个标准二阶环节。引入再平衡力矩之后闭环把摆维持在零位附近稳态时 $\theta\approx 0$力矩器电流与输入加速度成正比——所以惯性级加速度计输出的是电流不是角度。这一步很关键它把“测角度”变成了“测电流”量程和线性度都由力矩器决定。陀螺这边的逻辑类似只是对象换成角速度。动力调谐式挠性陀螺靠挠性接头和平衡环在特定转速下调谐使弹性恢复力矩与惯性力矩相互抵消等效刚度趋近于零转子自转轴就能敏感极其微小的角速度输入。简化后的运动方程同样可以做拉普拉斯变换求传递函数考试爱考这一步是因为它把“机械结构”翻译成了“控制环节”。惯性级器件的门槛本质上就是这条二阶方程的系数要够稳指标加速度计关注点陀螺关注点零位稳定性零偏要能压到 $10^{-5}g$ 量级零偏稳定性要到 $0.01^\circ/h$ 量级标度因数线性度好全量程内一致标度因数误差小大角速度下不翘量程覆盖载体机动过载捷联场合要覆盖数百 $^\circ/s$带宽与动态能跟上载体振动与冲击能跟上快速机动不产生相位滞后环境适应性温度、振动下系数不漂同理且要抗冲击“惯性级”三个字不是精度高那么简单它要求器件在长时间、宽温区、强振动下二阶方程的三个系数都别乱动。系数一动传递函数就变后面所有积分结果都会跟着偏。4. 舒勒回路把振荡周期调到 84.4 分钟意味着什么前面所有方程都是“开环”的给什么积分什么误差也一起积分。真正让惯导能连续工作的是修正回路。而修正回路里最核心的一条设计约束就是舒勒调谐。4.1 舒勒摆与舒勒回路一个造不出来的摆一个造得出来的回路舒勒摆是一个思想实验如果单摆的摆长等于地球半径周期就是 $T 2\pi\sqrt{R/g}\approx 84.4$ 分钟。这个摆有个奇特性质——当载体沿地球表面运动时摆的指示方向始终与当地垂线重合载体的加速度不会让它偏转。原因是载体前进造成的垂线变化恰好被摆自身因加速度产生的偏转抵消掉了。物理上造不出来。摆长 6371 km、还要无阻尼无摩擦工程上不可能。但它的数学性质可以被“仿造”出来用加速度计测比力、积分得速度、再乘 $g/R$ 反馈给陀螺力矩器让平台跟着当地水平面转就构成了一个等效的舒勒回路。推导只要两行。设平台绕水平轴有倾角 $\alpha$加速度计测到的比力里包含 $g\alpha$ 这一项倾斜导致重力在敏感轴上的投影修正回路让平台以正比于该信号的角速度转动系数取 $1/R$。把平台转动与地球曲率变化合起来写得到$$\ddot{\alpha} \frac{g}{R}\alpha 0$$这是一个无阻尼二阶振荡角频率 $\omega_s\sqrt{g/R}\approx 1.24\times10^{-3}$ rad/s周期正是 84.4 分钟。只要回路增益严格等于 $g/R$载体加速度就进不到误差方程里。两者的相同点都满足调谐条件都有 84.4 分钟周期都对载体加速度不敏感都是无阻尼等幅振荡。不同点舒勒摆是单自由度的机械装置靠重力恢复力矩工作物理上不可实现舒勒回路是双通道的控制回路靠修正力矩工作误差角是“算出来再补回去”的。一个是被动物理一个是主动控制——这是复习资料里最爱考的那道对比题。import math R 6378137.0 # 地球长半轴单位米 g 9.80665 # 平均重力加速度单位 m/s^2 Omega 7.292115e-5 # 地球自转角速度单位 rad/s phi math.radians(30) # 当地纬度 omega_s math.sqrt(g / R) # 舒勒角频率 T_schuler 2 * math.pi / omega_s / 60 # 舒勒周期换算成分钟 T_foucault 2 * math.pi / (Omega * math.sin(phi)) / 3600 # 傅科周期小时 T_earth 2 * math.pi / Omega / 3600 # 地球自转周期小时 print(f舒勒角频率 {omega_s:.3e} rad/s) print(f舒勒周期 {T_schuler:.1f} min) print(f傅科周期 {T_foucault:.1f} h) print(f地球自转周期 {T_earth:.1f} h)算出来的三个周期对应误差传播里的三种振荡舒勒振荡 84.4 分钟、傅科振荡随纬度变化赤道处趋于无穷两极处等于 24 小时、地球自转周期振荡 24 小时。参数里最敏感的是 $g/R$它的精度直接决定调谐是否准确实际系统里 $g$ 用当地重力公式算$R$ 用当地曲率半径不能用平均值糊弄否则高纬度会积累明显偏差。4.2 平台的三种工作状态与三条基本回路惯导平台按用途有三种工作状态。几何稳定状态是把平台三轴稳定在惯性空间陀螺不进动平台相当于一个惯性基准多用于需要空间稳定参考的场合。空间积分状态是让方位轴按给定角速度绕天向轴转动比如跟踪地球自转分量使平台相对地球保持某种固定关系。锁定状态也叫方位跟踪是外框跟随载体方位转动、内框保持水平便于直接输出航向。三种状态对应三组基本回路。稳定回路是内环由陀螺敏感平台偏角、信号放大后驱动力矩马达把平台拉回原位带宽最高决定平台的抗干扰能力。修正回路是外环把导航计算机算出的水平修正量和方位修正量加到陀螺力矩器上让平台跟踪导航坐标系舒勒回路就活在这一层。锁定回路负责框架角的跟随与传递保证内外框不打架。三条回路的关系是嵌套的稳定回路必须比修正回路快一个量级否则修正信号会被内环吃掉平台跟不上指令修正回路的增益又必须严格满足 $g/R$否则舒勒调谐被破坏。这两个约束一快一准是平台设计的核心矛盾。4.3 指北与游移方位全球导航的两种取舍指北方位系统让平台始终跟踪当地地理系东-北-天三轴固定。优点是航向直接可读误差方程清晰位置解算简单。缺点是方位轴要持续以 $\omega_{ie}\sin\varphi$ 的角速度跟踪高纬度时 $\cos\varphi$ 趋零经度解算的 $1/\cos\varphi$ 项发散全球导航做不到。游移方位系统换了个思路方位轴不跟踪地理北而是相对惯性空间保持某个姿态或者按固定规律游移方位角本身作为状态量解算出来。这样就没有 $1/\cos\varphi$ 的奇点全球都能跑。代价是多了一个待估的方位角误差耦合更复杂航向不能直接读得换算。误差传播这块指北系统的误差分三类。振荡性误差包括舒勒振荡、傅科振荡和地球自转周期振荡它们不随时间增长只是来回晃。积累性误差由陀螺常值漂移引起尤其是东向陀螺漂移会让经度误差随时间线性增长。随机误差由器件噪声积分而来走的是随机游走的路子。三类里对精度影响最大的是积累性误差——振荡还能靠滤波压一压线性增长没救只能靠器件本身和外部校正。5. 避坑与排查学惯导最容易翻车的五个地方概念看着都懂一动笔就错是这门课的常态。下面五条是从推导和调试里反复踩出来的每条都按“现象—原因—解决”写对上号了能省不少时间。5.1 器件与方程层面的三个坑坑一把比力当成运动加速度。现象是静态标定时加速度计输出不为零怀疑零偏超标动态跑轨迹时速度解算总差一个常值。原因是比力的定义是 $\mathbf{f}\mathbf{a}-\mathbf{g}$静止时 $\mathbf{a}0$输出就是 $-\mathbf{g}$本来就该非零。解决办法是先明确符号约定静态校零时把当地重力分量算出来扣掉再看残差同时检查比力方程里 $\mathbf{g}^n$ 的符号有没有和器件输出对上这一项写反位置会一路往一个方向跑。坑二以为方向余弦阵的转置永远等于逆。现象是数值积分一段时间后 $CC^T$ 明显偏离单位阵姿态角开始漂。原因是理论上正交离散积分下不正交——每一步的截断误差都在破坏约束跑几千步就积累出来了。解决办法是定期做正交化归一化或者干脆换四元数每步归一化四个参数比校正九个元素便宜得多。捷联系统里这一步是标配不是可选优化。坑三小角度近似到处用。现象是小机动时一切正常大角度转弯后姿态误差突然变大甚至发散。原因是 $\sin\theta\approx\theta$、$\cos\theta\approx1$ 只在几度范围内成立误差方程里用它做线性化没问题姿态更新里用就是另一回事。解决办法是分清场合推导误差方程、做卡尔曼滤波时用小角度线性化姿态解算必须用完整旋转四元数或等效旋转矢量都行别图省事。5.2 回路与对准层面的两个坑坑四对准结束后不切内反馈回路。现象是初始对准时收敛得挺快一进入导航状态就出现周期不对的振荡或者缓慢发散。原因是对准期间为了加快收敛人为加了内反馈把舒勒回路的无阻尼等幅振荡改成了提高频率的减幅振荡这个附加反馈破坏了 $g/R$ 的调谐条件进导航必须切掉。解决办法是在对准结束、转入导航之前明确切除内反馈并确认回路增益回到舒勒条件。复习资料里问“对准结束后应怎样处理这些内反馈回路、为什么”问的就是这一刀。坑五高纬度还在用指北方位。现象是纬度超过一定值后经度解算跳变误差迅速放大。原因是位置方程里 $1/\cos\varphi$ 在极点附近奇异分母趋零任何小的速度误差都会被放大成巨大的经度误差。解决办法是换游移方位方案把方位角作为状态量而不是跟踪量从根上避开奇点。这个坑在纸面上不明显一跑高纬度轨迹立刻现形。再补一条不算坑但常被误解的舒勒振荡消不掉。有人以为调参数能把 84.4 分钟的振荡压下去实际上它是无阻尼系统的固有特性只能不激励不能消除。初始条件对齐、避免突变加速度才是正路。6. 从对准到捷联物理平台与数学平台差在哪6.1 对准的本质是把等幅振荡改成衰减振荡初始对准用的两个自然参考量一个是重力矢量一个是地球自转角速度。重力定水平自转角速度定方位前者靠加速度计后者靠陀螺。粗对准直接用这两个矢量做双矢量定姿解析算出初始姿态阵精对准在此基础上估计失准角把残差压下去。对准过程里绕不开罗经项。地球自转的水平分量会让方位误差与北向误差耦合形成绕天向轴的地球周期振荡这就是罗盘效应。从罗经项里提取方位误差有两条路一条是观测罗经振荡的相位和幅值另一条是转动方位双位置法取两次测量求差。方位对准精度最终受东向陀螺漂移限制因为漂移在罗经项里和方位误差同阶分不开。关键结论在这里平台式和捷联式的对准本质完全一样都是把舒勒等幅振荡回路改成提高振荡频率的减幅振荡回路。差别只在实现——平台式有实际的对准回路修正信息和其他误差项一起加到陀螺力矩器上捷联式没有物理回路只有对准计算程序全靠导航计算机跑。6.2 平台本文还有配套的精品资源点击获取