搞寻北的兄弟应该都有过这样一段被磨得没脾气的时光陀螺零偏明明标定过结果换个方向、换次通电方位角又偏了。先别急着怀疑算法因为我们真正要测量的是一个量级极其微弱的物理量——地球自转的水平分量。赤道上的地球自转角速度是15°/h到了中纬度地区水平分量也就11°/h上下而这个分量对陀螺敏感轴的投影还要再乘上一个cosA目标方位在东西两侧时这个投影能跌到0.5°/h以下随便一点零偏漂移都会把信号整个吞掉。这篇内容把双位置单陀螺和双陀螺这两种经典寻北结构完整过一遍从原理推导、水平调平、零偏估计、航向角解算到可复现的代码实现和工程上真正会踩的坑。适合正在做寻北算法、自研陀螺罗盘或者想把手头陀螺变成定向仪表的工程师参考。1. 为什么寻北要看地球自转物理链路与量级认知1.1 地球自转信号的分解与量级判断地球每24小时自转一圈对应的角速度是常数Ω_e 15.041067°/h 7.292115×10⁻⁵ rad/s这个数值在任何地方都一样但陀螺能感受到的是它在地理坐标系下的投影。把地球自转轴方向分解到当地水平坐标系可以得到一个垂直分量和一个水平分量垂直分量Ω_v Ω_e·sin(L)指向天向水平分量Ω_h Ω_e·cos(L)指向北向其中L是当地纬度。这里的关键认知是陀螺仪在水平面上能测到的有用信号只有Ω_h它随纬度升高而变小。北纬40°时Ω_h约11.5°/h北纬60°时只剩7.5°/h。如果你做的是高纬度寻北设备信号弱了几乎一半难度翻倍。为什么说这是微弱信号因为陀螺输出的一角秒级别变化对应的角速度极小。1角秒/秒大约是0.001°/h要分辨这个量级的信号对陀螺噪声和采样时间都提出了苛刻要求。寻北系统本质上做的是“小信号检测”不是简单的转位加平均。1.2 陀螺敏感轴方向与方位角的几何关系定义方位角A为陀螺敏感轴在水平面上的投影方向与地理北向的夹角顺时针为正范围0°~360°。当敏感轴完全水平时它测得的地球自转投影为ω Ω_h·cos(A) Ω_e·cos(L)·cos(A)这就是寻北的核心测量方程。陀螺敏感轴朝北时输出最大等于Ω_h朝东或朝西时输出为0朝南时输出最大负值。双陀螺方案则是把第二个陀螺的敏感轴安装在第一个陀螺的正交方向也就是方位角A90°的位置。它的输出为ω_y Ω_h·sin(A)有了cos(A)和sin(A)就能用atan2直接得到完整方位角不需要像单陀螺那样为了判象限绞尽脑汁。这个差别在后面会反复出现。1.3 为什么这是一个“从噪声里捞信号”的问题很多第一次接触寻北的人会问陀螺输出不是直接就是角速度吗测一下不就知道方向了问题在于量级。假设陀螺的短期噪声等效到0.1°/h零偏稳定性0.5°/h而我们要测的投影信号在最差情况下只有0.2°/h。信噪比连1都不到直接读输出毫无意义。更麻烦的是零偏陀螺在静止状态下的输出并不是零而是某个随温度和时间缓慢漂移的偏置。这个东西出厂标定过一次不代表下次通电还是这个值这叫turn-on bias重复性是寻北方案设计里绕不开的坎。所以寻北算法的全部工作可以浓缩成一句话想办法把陀螺零偏和噪声干掉把地球自转水平投影这个微小信号捞出来。双位置法就是干这件事的经典手段。2. 双位置方法与零偏抵消的数学本质2.1 单陀螺双位置的测量模型与差分推导单陀螺双位置的思路很朴素既然零偏是同一个陀螺、同一个通电状态下的常量那么在两个不同的位置上测两次做差就能把它消掉。先建立静态测量模型。陀螺在某个固定位置的输出近似为ω_meas ω_true B n其中ω_true是真实角速度在敏感轴上的投影B是当前通电状态下的零偏n是随机噪声。位置1时敏感轴对准目标方位A测量值为ω₁ Ω_h·cos(A) B n₁然后把整个回转机构转180°让敏感轴方向变为A180°。此时投影反向测量值为ω₂ -Ω_h·cos(A) B n₂将两式相减再除以2(ω₁ - ω₂)/2 Ω_h·cos(A) (n₁ - n₂)/2零偏B被完全抵消只剩下cos(A)的估计。同样把两式相加再除以2(ω₁ ω₂)/2 B (n₁ n₂)/2得到的是零偏的估计值B̂。虽然这只是个副产品但在工程上很有价值可以用来监控陀螺状态也能在后续补偿中做参考。2.2 双陀螺正交布局与零偏补偿方式如果手里有两个陀螺且把它们的敏感轴严格正交安装在一个基座上位置1不动时就能同时获得两个正交方向的投影ω_x Ω_h·cos(A) B_x n_xω_y Ω_h·sin(A) B_y n_y这里有两个分叉选择。第一种方式双陀螺单位置不做转位。前提是B_x和B_y已经被精确标定那么方位角直接解算A atan2(ω_y - B_y, ω_x - B_x)一次测量就出结果没有转位时间响应快很适合对实时性有要求的场景。代价是它把宝押在了零偏偏离标定值不大这个假设上。如果开机后零偏变了0.2°/h方位角就会产生明显误差。第二种方式双陀螺双位置。把整个基座同样转180°两个轴同时获得反向的投影做差分cos(A) (ω_x1 - ω_x2) / (2Ω_h)sin(A) (ω_y1 - ω_y2) / (2Ω_h)两个轴的零偏都被差分消掉然后用atan2解算既保留了差分消零偏的优势又避免了单陀螺的象限歧义。这是静态高精度寻北里最稳的结构。2.3 三种方案横向对比与选型建议方案传感器数量是否需要转位零偏处理方式方位解算盲区情况典型场景单陀螺双位置1个陀螺需要180°差分相减cos(A)象限需外部辅助判断东西方向误差放大低成本、静态定向双陀螺单位置2个正交陀螺不需要依赖标定零偏atan2无盲区实时性要求高、快速响应双陀螺双位置2个正交陀螺需要180°双轴差分atan2无盲区高精度、静态或慢动态测量实际选型时我的建议是预算紧张、对精度要求中等、允许几分钟测量时间的项目选单陀螺双位置需要快速出方向甚至支持微弱动态的项目选双陀螺单位置精度优先、不赶时间的项目直接上双陀螺双位置。后者也是很多成熟产品内部采用的组合逻辑。3. 水平调平把敏感轴压到水平面的关键工艺3.1 倾斜对测量的破坏力垂直分量的耦合前面所有公式都建立在一个前提上陀螺敏感轴完全水平。但现实中没有任何手动机构能做到绝对水平总会残留一个倾角θ。一旦倾斜敏感轴就不再只看到水平分量地球自转的垂直分量也会耦合进来ω Ω_h·cos(A) Ω_v·sin(θ) BΩ_v比Ω_h大不少。北纬40°时Ω_v约为9.66°/h而Ω_h约11.5°/h。假如残余倾角是1角分约0.000291 rad耦合进输出的垂直分量是9.66 × 0.000291 ≈ 0.0028°/h看起来很小别忘了你的目标是测0.2°/h甚至0.05°/h的信号。0.0028°/h的耦合误差在解算后可能产生远超允许值的方位误差尤其是在信号投影本来就小的东西方向附近。夹角越小破坏力越明显。这就是为什么寻北系统的机械结构必须有一个可靠的调平机构而且调平要先于数据采集完成。3.2 从气泡调平到加速度计数字闭环调平手段通常分三个层次。第一层是气泡水平仪粗调。这是最传统的方式把双轴气泡水平仪贴在基座基准面上手工调整脚螺旋让气泡居中。气泡调平的极限精度大概在角分量级也就是1到2角分的水平。对于精度要求大于1°的MEMS寻北设备这通常够用了。第二层是加速度计辅助精调。在基座上安装两轴加速度计加速度计感受重力倾角通过公式实时解算横滚角和俯仰角roll atan2(a_y, a_z)pitch atan2(-a_x, sqrt(a_y² a_z²))这里的加速度计要用±1g量程的零偏和安装角需要先标定掉。把解算出的角度显示在界面上操作人员手动微调直到两个角都小于设定阈值。这套方式可以把调平精度推高到20角秒以内也能用来做二次校验。第三层是全自动闭环调平。基座安装在两轴云台上加速度计实时输出姿态角伺服回路用PI控制器把角度收敛到零。这种方式适合批量生产或无人值守场景精度取决于加速度计噪声和机械传动回程间隙。3.3 残余倾角的软件补偿与调平判据如果机械调平已经做到极限还是有残余倾角可以在软件层面做一部分补救。原理是把敏感轴的方向看作一个三维矢量通过姿态矩阵把垂直分量对输出的影响扣除ω_corrected ω_meas - Ω_v·sin(θ_x)·cos(γ) - Ω_v·sin(θ_y)·sin(γ)其中γ是敏感轴在水平面内的方向参数θ_x和θ_y是两轴倾角。这个补偿的精度完全取决于加速度计测倾角的精度它能把调平要求从“机械绝对水平”放宽到“机械接近水平软件修正”。但要注意软件补偿只能修正已知的倾斜项没办法修正机构在测量过程中因为受力变形产生的微小倾角变化。我给的工程判据是精度目标1°以上残余倾角控制在5角分内即可精度目标0.1°到0.3°残余倾角最好压到30到60角秒精度目标0.05°以下残余倾角必须优于10角秒并且要做好测量全过程的倾角监测而不是只调完就不管了。4. 零偏估计与数据预处理一切精度的地基4.1 零偏的类型与双位置为什么能消掉它陀螺零偏不是一个单一概念至少可以拆成三部分看一是turn-on bias即每次开机后形成的一个相对固定的常值偏置二是慢变漂移它随温度和时间缓慢变化三是零偏不稳定性表现为一定时间尺度内随机徘徊。双位置差分能消掉的是第一类也基本能消掉在两次测量期间变化不大的第二类。因为整个双位置测量通常只需要几分钟慢变漂移在这段时间内可以近似看成常量差分后残差很小。但如果测量时间被拉长到几十分钟零偏漂移的线性变化部分就会留下残差。这也是为什么后面要讨论多周期换向平均。4.2 数据预处理流水线野值剔除、滤波与均值陀螺原始输出的噪声通常包含白噪声成分偶尔还有振动等引起的野值。我的处理流水线固定四步第一步静止检测。用滑动窗口计算最近1秒数据的方差方差超过阈值的区段直接标记为无效。转位过程、电机抖动、外部碰撞都会让方差瞬间变大这些数据不能进解算。第二步野值剔除。对有效段落里的每个样本与当前段落均值比较偏离超过3倍标准差的点剔除掉。这一条对保护均值不被个别异常点拉偏非常有效。第三步平滑滤波。先做一次滑动平均或低通滤波把高频噪声压下去。第四步求段落均值。所有样本做算术平均后噪声标准差会按1/√N的比例下降。N是参与平均的样本数。比如原始噪声标准差0.3°/h采样率50Hz采20秒共1000个样本平均后的标准差就能降到0.3/√1000约0.0095°/h。4.3 多周期换向平均与采样时间预算单次双位置测量的结果受零偏慢变漂移影响要想再提高精度工程上常用的是多周期换向平均。做法是位置1测一段转180°到位置2测一段再转回来到位置1测一段再转到位置2测一段如此反复。每一圈的测量都得到一组(ω₁ - ω₂)对多组差分值取平均。这相当于把零偏漂移在不同时间段内的变化也平均掉了最终等价于用更长的有效测量压噪声。采样时间怎么定假设要求方位角标准差小于0.05°A60°时sin(A)0.866那么cos(A)的标准差需要控制在0.05°弧度换算后乘sin(A)约7.6×10⁻⁴。再代入公式σ_cosA σ_m / (√2·Ω_h)反推需要的σ_m再结合1/√N关系就能算出最少采样秒数。实际项目里通常不会精确卡这个数而是按“静态测10分钟全流程”来设计留足余量。4.4 温度、预热对零偏漂移的影响这节是纯经验。陀螺零偏对温度相当敏感尤其是MEMS陀螺。开关机几十秒内零偏往往还在往回爬如果这时候急着测得到的结果很差。我在实际项目里的标准动作是上电后至少预热10分钟再开始寻北流程。温度条件极端的环境预热时间还要加倍。有条件的话把陀螺的零点输出随温度变化的曲线提前标定出来用温度补偿表把零偏的确定性部分先扣除剩下交给差分和平均处理。还有一个容易忽略的细节双陀螺设备里两个陀螺要保证工作温度一致。如果两个陀螺一冷一热零偏变化不同步差分后残留会比预想大得多。5. 航向角解算与代码实现从公式到可运行程序5.1 单陀螺双位置的完整解算流程把前面的公式整理成可执行的步骤纬度L由定位或人工输入计算Ω_h 15.041067 × cos(L)。确认调平完成后锁定基座位置1静止采样T秒剔除野值后求均值ω₁。回转机构转180°等待机构稳定若干秒位置2静止采样T秒求均值ω₂。计算cosA (ω₁ - ω₂) / (2Ω_h)并对结果限幅到[-1, 1]。判象限。单陀螺双位置只有cos(A)无法区分东北和西北方向。工程上可以用磁罗盘粗定向、外部粗北向信息或者在位置2的基础上再转90°补测一次来定象限。得到最终方位角。注意acos函数在cosA接近±1时对噪声极其敏感因此不要在东西方向附近指望单陀螺双位置能给出好结果。5.2 双陀螺方案与象限处理双陀螺双位置差分后直接得到cosA和sinA用atan2就结束了A atan2(sinA, cosA)atan2天然返回(-π, π]范围内的角度要转成0°到360°就加2π再取模。这里没有象限判断的烦恼这是双陀螺最让人省心的地方。5.3 Python仿真验证码下面给一套完整的Python仿真把零偏、噪声、双位置测量、双陀螺双位置差分都跑一遍。单位统一用deg/h和度。import numpy as np import math OMEGA_E 15.041067 # 地球自转角速度deg/h LAT 39.9 # 北纬39.9度 LAT_RAD math.radians(LAT) OMEGA_H OMEGA_E * math.cos(LAT_RAD) A_TRUE_DEG 73.0 A_TRUE math.radians(A_TRUE_DEG) BIAS 0.02 # 单陀螺零偏deg/h SIGMA 0.3 # 单样本噪声标准差deg/h FS 50.0 # 采样率 Hz T_MEAS 20.0 # 每位置采样时长 s N int(FS * T_MEAS) np.random.seed(42) def measure(signal): noise np.random.normal(0.0, SIGMA, N) return np.mean(signal BIAS noise) # 单陀螺双位置 m1 measure(OMEGA_H * math.cos(A_TRUE)) m2 measure(-OMEGA_H * math.cos(A_TRUE)) cosA_est (m1 - m2) / (2.0 * OMEGA_H) cosA_est np.clip(cosA_est, -1.0, 1.0) # 单陀螺只有cos存在象限歧义这里用真实符号演示判象限 alpha math.acos(cosA_est) A_est alpha if math.sin(A_TRUE) 0 else 2 * math.pi - alpha # 双陀螺双位置差分 def measure_dual(signal_x, signal_y): nx np.random.normal(0.0, SIGMA, N) ny np.random.normal(0.0, SIGMA, N) return np.mean(signal_x BIAS nx), np.mean(signal_y 0.015 ny) mx1, my1 measure_dual(OMEGA_H * math.cos(A_TRUE), OMEGA_H * math.sin(A_TRUE)) mx2, my2 measure_dual(-OMEGA_H * math.cos(A_TRUE), -OMEGA_H * math.sin(A_TRUE)) cosA_d (mx1 - mx2) / (2.0 * OMEGA_H) sinA_d (my1 - my2) / (2.0 * OMEGA_H) A_est_d math.atan2(sinA_d, cosA_d) if A_est_d 0: A_est_d 2 * math.pi def wrap_deg(deg): return deg - 360.0 * np.round(deg / 360.0) print(真值方位角: {:.3f} 度.format(A_TRUE_DEG)) print(单陀螺双位置估计: {:.3f} 度, 误差 {:.4f} 度.format( math.degrees(A_est), wrap_deg(math.degrees(A_est) - A_TRUE_DEG))) print(双陀螺双位置估计: {:.3f} 度, 误差 {:.4f} 度.format( math.degrees(A_est_d), wrap_deg(math.degrees(A_est_d) - A_TRUE_DEG)))跑完你会看到两种方案的误差都在百分之几度以内且双陀螺无象限问题。把噪声调大一个量级或缩短采样时间就能直观感受到信号被淹没时解算结果是如何变差的。5.4 C语言嵌入式核心实现真正上设备的时候C代码只需要实现几个核心函数。下面的示例足够改造成自己的模块#include math.h #include stdint.h #define EARTH_RATE_H 15.041067f #define TWO_PI 6.28318530718f #define DEG2RAD 0.01745329252f typedef struct { float m1; /* 位置1采样均值 */ float m2; /* 位置2采样均值 */ } double_pos_t; float sample_mean(const float *buf, uint16_t n) { float sum 0.0f; for (uint16_t i 0; i n; i) { sum buf[i]; } return sum / (float)n; } float estimate_bias(double_pos_t *dp) { /* 双位置相加平均得到零偏估计 */ return (dp-m1 dp-m2) * 0.5f; } float estimate_cosA(double_pos_t *dp, float lat_rad) { float omega_h EARTH_RATE_H * cosf(lat_rad); float cosA (dp-m1 - dp-m2) / (2.0f * omega_h); if (cosA 1.0f) cosA 1.0f; if (cosA -1.0f) cosA -1.0f; return cosA; } /* 双陀螺双位置差分x轴指向目标方位y轴正交 */ typedef struct { float x1, y1; /* 位置1两轴均值 */ float x2, y2; /* 位置2两轴均值 */ } dual_gyro_pos_t; float azimuth_dual_gyro(dual_gyro_pos_t *d, float lat_rad) { float omega_h EARTH_RATE_H * cosf(lat_rad); float cosA (d-x1 - d-x2) / (2.0f * omega_h); float sinA (d-y1 - d-y2) / (2.0f * omega_h); float a atan2f(sinA, cosA); if (a 0.0f) { a TWO_PI; } return a; /* 弧度调用方自行转角度 */ }代码里有一个容易被忽略的细节两个陀螺的采样必须对齐时间。如果x轴和y轴是两片独立的ADC芯片最好用同一个触发信号同步采集否则引入的时间差会造成两轴之间的等效相位错位解算出的方位角会偏。5.5 误差传播公式与盲区分析从公式cosA (ω₁ - ω₂)/(2Ω_h)出发假设两个位置的均值标准差都是σ_m那么cosA估计的标准差为σ_cosA σ_m / (√2·Ω_h)而方位角A的误差与cosA误差的关系是σ_A ≈ σ_cosA / |sin(A)|注意这里的|sin(A)|。当A接近0°或180°时sin(A)接近0σ_A被放大很多当A接近90°或270°时sin(A)接近1误差最小。这正好和直觉相反——单陀螺在东西方向定位最准在南北方向定位反而最差。所以单陀螺双位置不是万能的。双陀螺双位置没有这个问题因为它同时使用cos和sinatan2的误差不会在某个方位角上被系统性放大。这就是它结构复杂也值得采用的根本原因。6. 工程落地最容易踩的坑转位、振动、温度与时间同步6.1 转位角度不准带来的误差及应对双位置法的灵魂是那个“180°”。如果转位机构的实际转角不是180°而是180°δ位置2的投影就不是严格相反方向。把δ代入差分公式cosA的估计会变成cosA_est ≈ cosA - (δ/2)·sinA对应方位角误差约为(δ/2)·tanA。举个例子A45°δ1角分方位误差约0.5角分约0.008度。这在很多场合可以接受但如果目标精度到0.01度这个误差就不能忽略。应对方法有三板斧一是转位机构用步进电机加编码器闭环定位把重复定位精度做到角秒级二是转位到位后不要立即采数留出机械稳定时间三是如果转位误差是系统性固定的可以在标定时测出来解算时代入实际转角修正。6.2 振动、温度与时间同步的干扰地面寻北系统最常见的振动源反而很平凡人走动、空调风、机器运转。低频振动和缓慢倾斜会直接叠加进陀螺输出而且很难靠滤波完全分离。我在现场调试时的经验是基座下垫橡胶减振垫测量期间人员离场全程用远程监控界面操作。温度方面除了预热还要注意测量过程不要有剧烈温度变化。太阳直射、空调出风口对着设备都会让零偏短时间内漂移。用双陀螺时更要注意两个陀螺必须处在相同温度环境下否则两轴零偏变化不同步差分后会留下很大的方向性残差。时间同步问题前面提过双陀螺数据要统一时间基准。具体到代码上每次采样中断里同时锁存两个通道的数据避免一个通道采到的是t时刻另一个通道采到的是tΔ时刻。6.3 验收标定流程与故障排查表设备做完了怎么验收我的标准流程是安装在稳定的基座上高精度调平预热充足然后选一条已知方位角的基准线把陀螺敏感轴对准基准线方向进行至少5次完整的寻北测量统计均值和重复性。再用外部参考方位与寻北结果对比得到系统偏差。如果测量结果不好先对照故障排查表定位问题现象可能原因排查手段结果整体偏移固定角度纬度输入错误、调平残差、安装角标定错了核对纬度复测调平复核安装角重复性差每次结果乱跳采样时间不足、振动干扰、零偏漂移过大加长采样、多周期换向、做好隔离减振解算出现NaN或明显越界cosA越界、野值未剔除、采样混入转动数据限幅、加强野值剔除、转位完成后再延时采数双陀螺结果差但单陀螺正常两轴时间不同步、第二轴安装角不是90°检查同步触发实测轴间角并修正有规律性偏差转位角度不准、温度梯度造成零偏差编码器闭环转位改善两陀螺热均衡最后说一个我个人的习惯每次上电后先不做正式测量用双位置预测量把B̂算出来盯着它观察几分钟看零偏是平稳的还是持续在爬。如果零偏曲线明显单调上升说明设备还没热透这时候跑正式流程是浪费数据的。等B̂曲线进入缓慢波动状态再开始解算出来的结果基本就稳了。这个动作虽然只花几分钟却能让整套系统的成功率上一个台阶。 
长河君 · 2026/10/18 5:48:57 · 阅读 1.2k 陀螺仪寻北算法详解：双位置与双陀螺结构如何消除零偏误差
搞寻北的兄弟应该都有过这样一段被磨得没脾气的时光陀螺零偏明明标定过结果换个方向、换次通电方位角又偏了。先别急着怀疑算法因为我们真正要测量的是一个量级极其微弱的物理量——地球自转的水平分量。赤道上的地球自转角速度是15°/h到了中纬度地区水平分量也就11°/h上下而这个分量对陀螺敏感轴的投影还要再乘上一个cosA目标方位在东西两侧时这个投影能跌到0.5°/h以下随便一点零偏漂移都会把信号整个吞掉。这篇内容把双位置单陀螺和双陀螺这两种经典寻北结构完整过一遍从原理推导、水平调平、零偏估计、航向角解算到可复现的代码实现和工程上真正会踩的坑。适合正在做寻北算法、自研陀螺罗盘或者想把手头陀螺变成定向仪表的工程师参考。1. 为什么寻北要看地球自转物理链路与量级认知1.1 地球自转信号的分解与量级判断地球每24小时自转一圈对应的角速度是常数Ω_e 15.041067°/h 7.292115×10⁻⁵ rad/s这个数值在任何地方都一样但陀螺能感受到的是它在地理坐标系下的投影。把地球自转轴方向分解到当地水平坐标系可以得到一个垂直分量和一个水平分量垂直分量Ω_v Ω_e·sin(L)指向天向水平分量Ω_h Ω_e·cos(L)指向北向其中L是当地纬度。这里的关键认知是陀螺仪在水平面上能测到的有用信号只有Ω_h它随纬度升高而变小。北纬40°时Ω_h约11.5°/h北纬60°时只剩7.5°/h。如果你做的是高纬度寻北设备信号弱了几乎一半难度翻倍。为什么说这是微弱信号因为陀螺输出的一角秒级别变化对应的角速度极小。1角秒/秒大约是0.001°/h要分辨这个量级的信号对陀螺噪声和采样时间都提出了苛刻要求。寻北系统本质上做的是“小信号检测”不是简单的转位加平均。1.2 陀螺敏感轴方向与方位角的几何关系定义方位角A为陀螺敏感轴在水平面上的投影方向与地理北向的夹角顺时针为正范围0°~360°。当敏感轴完全水平时它测得的地球自转投影为ω Ω_h·cos(A) Ω_e·cos(L)·cos(A)这就是寻北的核心测量方程。陀螺敏感轴朝北时输出最大等于Ω_h朝东或朝西时输出为0朝南时输出最大负值。双陀螺方案则是把第二个陀螺的敏感轴安装在第一个陀螺的正交方向也就是方位角A90°的位置。它的输出为ω_y Ω_h·sin(A)有了cos(A)和sin(A)就能用atan2直接得到完整方位角不需要像单陀螺那样为了判象限绞尽脑汁。这个差别在后面会反复出现。1.3 为什么这是一个“从噪声里捞信号”的问题很多第一次接触寻北的人会问陀螺输出不是直接就是角速度吗测一下不就知道方向了问题在于量级。假设陀螺的短期噪声等效到0.1°/h零偏稳定性0.5°/h而我们要测的投影信号在最差情况下只有0.2°/h。信噪比连1都不到直接读输出毫无意义。更麻烦的是零偏陀螺在静止状态下的输出并不是零而是某个随温度和时间缓慢漂移的偏置。这个东西出厂标定过一次不代表下次通电还是这个值这叫turn-on bias重复性是寻北方案设计里绕不开的坎。所以寻北算法的全部工作可以浓缩成一句话想办法把陀螺零偏和噪声干掉把地球自转水平投影这个微小信号捞出来。双位置法就是干这件事的经典手段。2. 双位置方法与零偏抵消的数学本质2.1 单陀螺双位置的测量模型与差分推导单陀螺双位置的思路很朴素既然零偏是同一个陀螺、同一个通电状态下的常量那么在两个不同的位置上测两次做差就能把它消掉。先建立静态测量模型。陀螺在某个固定位置的输出近似为ω_meas ω_true B n其中ω_true是真实角速度在敏感轴上的投影B是当前通电状态下的零偏n是随机噪声。位置1时敏感轴对准目标方位A测量值为ω₁ Ω_h·cos(A) B n₁然后把整个回转机构转180°让敏感轴方向变为A180°。此时投影反向测量值为ω₂ -Ω_h·cos(A) B n₂将两式相减再除以2(ω₁ - ω₂)/2 Ω_h·cos(A) (n₁ - n₂)/2零偏B被完全抵消只剩下cos(A)的估计。同样把两式相加再除以2(ω₁ ω₂)/2 B (n₁ n₂)/2得到的是零偏的估计值B̂。虽然这只是个副产品但在工程上很有价值可以用来监控陀螺状态也能在后续补偿中做参考。2.2 双陀螺正交布局与零偏补偿方式如果手里有两个陀螺且把它们的敏感轴严格正交安装在一个基座上位置1不动时就能同时获得两个正交方向的投影ω_x Ω_h·cos(A) B_x n_xω_y Ω_h·sin(A) B_y n_y这里有两个分叉选择。第一种方式双陀螺单位置不做转位。前提是B_x和B_y已经被精确标定那么方位角直接解算A atan2(ω_y - B_y, ω_x - B_x)一次测量就出结果没有转位时间响应快很适合对实时性有要求的场景。代价是它把宝押在了零偏偏离标定值不大这个假设上。如果开机后零偏变了0.2°/h方位角就会产生明显误差。第二种方式双陀螺双位置。把整个基座同样转180°两个轴同时获得反向的投影做差分cos(A) (ω_x1 - ω_x2) / (2Ω_h)sin(A) (ω_y1 - ω_y2) / (2Ω_h)两个轴的零偏都被差分消掉然后用atan2解算既保留了差分消零偏的优势又避免了单陀螺的象限歧义。这是静态高精度寻北里最稳的结构。2.3 三种方案横向对比与选型建议方案传感器数量是否需要转位零偏处理方式方位解算盲区情况典型场景单陀螺双位置1个陀螺需要180°差分相减cos(A)象限需外部辅助判断东西方向误差放大低成本、静态定向双陀螺单位置2个正交陀螺不需要依赖标定零偏atan2无盲区实时性要求高、快速响应双陀螺双位置2个正交陀螺需要180°双轴差分atan2无盲区高精度、静态或慢动态测量实际选型时我的建议是预算紧张、对精度要求中等、允许几分钟测量时间的项目选单陀螺双位置需要快速出方向甚至支持微弱动态的项目选双陀螺单位置精度优先、不赶时间的项目直接上双陀螺双位置。后者也是很多成熟产品内部采用的组合逻辑。3. 水平调平把敏感轴压到水平面的关键工艺3.1 倾斜对测量的破坏力垂直分量的耦合前面所有公式都建立在一个前提上陀螺敏感轴完全水平。但现实中没有任何手动机构能做到绝对水平总会残留一个倾角θ。一旦倾斜敏感轴就不再只看到水平分量地球自转的垂直分量也会耦合进来ω Ω_h·cos(A) Ω_v·sin(θ) BΩ_v比Ω_h大不少。北纬40°时Ω_v约为9.66°/h而Ω_h约11.5°/h。假如残余倾角是1角分约0.000291 rad耦合进输出的垂直分量是9.66 × 0.000291 ≈ 0.0028°/h看起来很小别忘了你的目标是测0.2°/h甚至0.05°/h的信号。0.0028°/h的耦合误差在解算后可能产生远超允许值的方位误差尤其是在信号投影本来就小的东西方向附近。夹角越小破坏力越明显。这就是为什么寻北系统的机械结构必须有一个可靠的调平机构而且调平要先于数据采集完成。3.2 从气泡调平到加速度计数字闭环调平手段通常分三个层次。第一层是气泡水平仪粗调。这是最传统的方式把双轴气泡水平仪贴在基座基准面上手工调整脚螺旋让气泡居中。气泡调平的极限精度大概在角分量级也就是1到2角分的水平。对于精度要求大于1°的MEMS寻北设备这通常够用了。第二层是加速度计辅助精调。在基座上安装两轴加速度计加速度计感受重力倾角通过公式实时解算横滚角和俯仰角roll atan2(a_y, a_z)pitch atan2(-a_x, sqrt(a_y² a_z²))这里的加速度计要用±1g量程的零偏和安装角需要先标定掉。把解算出的角度显示在界面上操作人员手动微调直到两个角都小于设定阈值。这套方式可以把调平精度推高到20角秒以内也能用来做二次校验。第三层是全自动闭环调平。基座安装在两轴云台上加速度计实时输出姿态角伺服回路用PI控制器把角度收敛到零。这种方式适合批量生产或无人值守场景精度取决于加速度计噪声和机械传动回程间隙。3.3 残余倾角的软件补偿与调平判据如果机械调平已经做到极限还是有残余倾角可以在软件层面做一部分补救。原理是把敏感轴的方向看作一个三维矢量通过姿态矩阵把垂直分量对输出的影响扣除ω_corrected ω_meas - Ω_v·sin(θ_x)·cos(γ) - Ω_v·sin(θ_y)·sin(γ)其中γ是敏感轴在水平面内的方向参数θ_x和θ_y是两轴倾角。这个补偿的精度完全取决于加速度计测倾角的精度它能把调平要求从“机械绝对水平”放宽到“机械接近水平软件修正”。但要注意软件补偿只能修正已知的倾斜项没办法修正机构在测量过程中因为受力变形产生的微小倾角变化。我给的工程判据是精度目标1°以上残余倾角控制在5角分内即可精度目标0.1°到0.3°残余倾角最好压到30到60角秒精度目标0.05°以下残余倾角必须优于10角秒并且要做好测量全过程的倾角监测而不是只调完就不管了。4. 零偏估计与数据预处理一切精度的地基4.1 零偏的类型与双位置为什么能消掉它陀螺零偏不是一个单一概念至少可以拆成三部分看一是turn-on bias即每次开机后形成的一个相对固定的常值偏置二是慢变漂移它随温度和时间缓慢变化三是零偏不稳定性表现为一定时间尺度内随机徘徊。双位置差分能消掉的是第一类也基本能消掉在两次测量期间变化不大的第二类。因为整个双位置测量通常只需要几分钟慢变漂移在这段时间内可以近似看成常量差分后残差很小。但如果测量时间被拉长到几十分钟零偏漂移的线性变化部分就会留下残差。这也是为什么后面要讨论多周期换向平均。4.2 数据预处理流水线野值剔除、滤波与均值陀螺原始输出的噪声通常包含白噪声成分偶尔还有振动等引起的野值。我的处理流水线固定四步第一步静止检测。用滑动窗口计算最近1秒数据的方差方差超过阈值的区段直接标记为无效。转位过程、电机抖动、外部碰撞都会让方差瞬间变大这些数据不能进解算。第二步野值剔除。对有效段落里的每个样本与当前段落均值比较偏离超过3倍标准差的点剔除掉。这一条对保护均值不被个别异常点拉偏非常有效。第三步平滑滤波。先做一次滑动平均或低通滤波把高频噪声压下去。第四步求段落均值。所有样本做算术平均后噪声标准差会按1/√N的比例下降。N是参与平均的样本数。比如原始噪声标准差0.3°/h采样率50Hz采20秒共1000个样本平均后的标准差就能降到0.3/√1000约0.0095°/h。4.3 多周期换向平均与采样时间预算单次双位置测量的结果受零偏慢变漂移影响要想再提高精度工程上常用的是多周期换向平均。做法是位置1测一段转180°到位置2测一段再转回来到位置1测一段再转到位置2测一段如此反复。每一圈的测量都得到一组(ω₁ - ω₂)对多组差分值取平均。这相当于把零偏漂移在不同时间段内的变化也平均掉了最终等价于用更长的有效测量压噪声。采样时间怎么定假设要求方位角标准差小于0.05°A60°时sin(A)0.866那么cos(A)的标准差需要控制在0.05°弧度换算后乘sin(A)约7.6×10⁻⁴。再代入公式σ_cosA σ_m / (√2·Ω_h)反推需要的σ_m再结合1/√N关系就能算出最少采样秒数。实际项目里通常不会精确卡这个数而是按“静态测10分钟全流程”来设计留足余量。4.4 温度、预热对零偏漂移的影响这节是纯经验。陀螺零偏对温度相当敏感尤其是MEMS陀螺。开关机几十秒内零偏往往还在往回爬如果这时候急着测得到的结果很差。我在实际项目里的标准动作是上电后至少预热10分钟再开始寻北流程。温度条件极端的环境预热时间还要加倍。有条件的话把陀螺的零点输出随温度变化的曲线提前标定出来用温度补偿表把零偏的确定性部分先扣除剩下交给差分和平均处理。还有一个容易忽略的细节双陀螺设备里两个陀螺要保证工作温度一致。如果两个陀螺一冷一热零偏变化不同步差分后残留会比预想大得多。5. 航向角解算与代码实现从公式到可运行程序5.1 单陀螺双位置的完整解算流程把前面的公式整理成可执行的步骤纬度L由定位或人工输入计算Ω_h 15.041067 × cos(L)。确认调平完成后锁定基座位置1静止采样T秒剔除野值后求均值ω₁。回转机构转180°等待机构稳定若干秒位置2静止采样T秒求均值ω₂。计算cosA (ω₁ - ω₂) / (2Ω_h)并对结果限幅到[-1, 1]。判象限。单陀螺双位置只有cos(A)无法区分东北和西北方向。工程上可以用磁罗盘粗定向、外部粗北向信息或者在位置2的基础上再转90°补测一次来定象限。得到最终方位角。注意acos函数在cosA接近±1时对噪声极其敏感因此不要在东西方向附近指望单陀螺双位置能给出好结果。5.2 双陀螺方案与象限处理双陀螺双位置差分后直接得到cosA和sinA用atan2就结束了A atan2(sinA, cosA)atan2天然返回(-π, π]范围内的角度要转成0°到360°就加2π再取模。这里没有象限判断的烦恼这是双陀螺最让人省心的地方。5.3 Python仿真验证码下面给一套完整的Python仿真把零偏、噪声、双位置测量、双陀螺双位置差分都跑一遍。单位统一用deg/h和度。import numpy as np import math OMEGA_E 15.041067 # 地球自转角速度deg/h LAT 39.9 # 北纬39.9度 LAT_RAD math.radians(LAT) OMEGA_H OMEGA_E * math.cos(LAT_RAD) A_TRUE_DEG 73.0 A_TRUE math.radians(A_TRUE_DEG) BIAS 0.02 # 单陀螺零偏deg/h SIGMA 0.3 # 单样本噪声标准差deg/h FS 50.0 # 采样率 Hz T_MEAS 20.0 # 每位置采样时长 s N int(FS * T_MEAS) np.random.seed(42) def measure(signal): noise np.random.normal(0.0, SIGMA, N) return np.mean(signal BIAS noise) # 单陀螺双位置 m1 measure(OMEGA_H * math.cos(A_TRUE)) m2 measure(-OMEGA_H * math.cos(A_TRUE)) cosA_est (m1 - m2) / (2.0 * OMEGA_H) cosA_est np.clip(cosA_est, -1.0, 1.0) # 单陀螺只有cos存在象限歧义这里用真实符号演示判象限 alpha math.acos(cosA_est) A_est alpha if math.sin(A_TRUE) 0 else 2 * math.pi - alpha # 双陀螺双位置差分 def measure_dual(signal_x, signal_y): nx np.random.normal(0.0, SIGMA, N) ny np.random.normal(0.0, SIGMA, N) return np.mean(signal_x BIAS nx), np.mean(signal_y 0.015 ny) mx1, my1 measure_dual(OMEGA_H * math.cos(A_TRUE), OMEGA_H * math.sin(A_TRUE)) mx2, my2 measure_dual(-OMEGA_H * math.cos(A_TRUE), -OMEGA_H * math.sin(A_TRUE)) cosA_d (mx1 - mx2) / (2.0 * OMEGA_H) sinA_d (my1 - my2) / (2.0 * OMEGA_H) A_est_d math.atan2(sinA_d, cosA_d) if A_est_d 0: A_est_d 2 * math.pi def wrap_deg(deg): return deg - 360.0 * np.round(deg / 360.0) print(真值方位角: {:.3f} 度.format(A_TRUE_DEG)) print(单陀螺双位置估计: {:.3f} 度, 误差 {:.4f} 度.format( math.degrees(A_est), wrap_deg(math.degrees(A_est) - A_TRUE_DEG))) print(双陀螺双位置估计: {:.3f} 度, 误差 {:.4f} 度.format( math.degrees(A_est_d), wrap_deg(math.degrees(A_est_d) - A_TRUE_DEG)))跑完你会看到两种方案的误差都在百分之几度以内且双陀螺无象限问题。把噪声调大一个量级或缩短采样时间就能直观感受到信号被淹没时解算结果是如何变差的。5.4 C语言嵌入式核心实现真正上设备的时候C代码只需要实现几个核心函数。下面的示例足够改造成自己的模块#include math.h #include stdint.h #define EARTH_RATE_H 15.041067f #define TWO_PI 6.28318530718f #define DEG2RAD 0.01745329252f typedef struct { float m1; /* 位置1采样均值 */ float m2; /* 位置2采样均值 */ } double_pos_t; float sample_mean(const float *buf, uint16_t n) { float sum 0.0f; for (uint16_t i 0; i n; i) { sum buf[i]; } return sum / (float)n; } float estimate_bias(double_pos_t *dp) { /* 双位置相加平均得到零偏估计 */ return (dp-m1 dp-m2) * 0.5f; } float estimate_cosA(double_pos_t *dp, float lat_rad) { float omega_h EARTH_RATE_H * cosf(lat_rad); float cosA (dp-m1 - dp-m2) / (2.0f * omega_h); if (cosA 1.0f) cosA 1.0f; if (cosA -1.0f) cosA -1.0f; return cosA; } /* 双陀螺双位置差分x轴指向目标方位y轴正交 */ typedef struct { float x1, y1; /* 位置1两轴均值 */ float x2, y2; /* 位置2两轴均值 */ } dual_gyro_pos_t; float azimuth_dual_gyro(dual_gyro_pos_t *d, float lat_rad) { float omega_h EARTH_RATE_H * cosf(lat_rad); float cosA (d-x1 - d-x2) / (2.0f * omega_h); float sinA (d-y1 - d-y2) / (2.0f * omega_h); float a atan2f(sinA, cosA); if (a 0.0f) { a TWO_PI; } return a; /* 弧度调用方自行转角度 */ }代码里有一个容易被忽略的细节两个陀螺的采样必须对齐时间。如果x轴和y轴是两片独立的ADC芯片最好用同一个触发信号同步采集否则引入的时间差会造成两轴之间的等效相位错位解算出的方位角会偏。5.5 误差传播公式与盲区分析从公式cosA (ω₁ - ω₂)/(2Ω_h)出发假设两个位置的均值标准差都是σ_m那么cosA估计的标准差为σ_cosA σ_m / (√2·Ω_h)而方位角A的误差与cosA误差的关系是σ_A ≈ σ_cosA / |sin(A)|注意这里的|sin(A)|。当A接近0°或180°时sin(A)接近0σ_A被放大很多当A接近90°或270°时sin(A)接近1误差最小。这正好和直觉相反——单陀螺在东西方向定位最准在南北方向定位反而最差。所以单陀螺双位置不是万能的。双陀螺双位置没有这个问题因为它同时使用cos和sinatan2的误差不会在某个方位角上被系统性放大。这就是它结构复杂也值得采用的根本原因。6. 工程落地最容易踩的坑转位、振动、温度与时间同步6.1 转位角度不准带来的误差及应对双位置法的灵魂是那个“180°”。如果转位机构的实际转角不是180°而是180°δ位置2的投影就不是严格相反方向。把δ代入差分公式cosA的估计会变成cosA_est ≈ cosA - (δ/2)·sinA对应方位角误差约为(δ/2)·tanA。举个例子A45°δ1角分方位误差约0.5角分约0.008度。这在很多场合可以接受但如果目标精度到0.01度这个误差就不能忽略。应对方法有三板斧一是转位机构用步进电机加编码器闭环定位把重复定位精度做到角秒级二是转位到位后不要立即采数留出机械稳定时间三是如果转位误差是系统性固定的可以在标定时测出来解算时代入实际转角修正。6.2 振动、温度与时间同步的干扰地面寻北系统最常见的振动源反而很平凡人走动、空调风、机器运转。低频振动和缓慢倾斜会直接叠加进陀螺输出而且很难靠滤波完全分离。我在现场调试时的经验是基座下垫橡胶减振垫测量期间人员离场全程用远程监控界面操作。温度方面除了预热还要注意测量过程不要有剧烈温度变化。太阳直射、空调出风口对着设备都会让零偏短时间内漂移。用双陀螺时更要注意两个陀螺必须处在相同温度环境下否则两轴零偏变化不同步差分后会留下很大的方向性残差。时间同步问题前面提过双陀螺数据要统一时间基准。具体到代码上每次采样中断里同时锁存两个通道的数据避免一个通道采到的是t时刻另一个通道采到的是tΔ时刻。6.3 验收标定流程与故障排查表设备做完了怎么验收我的标准流程是安装在稳定的基座上高精度调平预热充足然后选一条已知方位角的基准线把陀螺敏感轴对准基准线方向进行至少5次完整的寻北测量统计均值和重复性。再用外部参考方位与寻北结果对比得到系统偏差。如果测量结果不好先对照故障排查表定位问题现象可能原因排查手段结果整体偏移固定角度纬度输入错误、调平残差、安装角标定错了核对纬度复测调平复核安装角重复性差每次结果乱跳采样时间不足、振动干扰、零偏漂移过大加长采样、多周期换向、做好隔离减振解算出现NaN或明显越界cosA越界、野值未剔除、采样混入转动数据限幅、加强野值剔除、转位完成后再延时采数双陀螺结果差但单陀螺正常两轴时间不同步、第二轴安装角不是90°检查同步触发实测轴间角并修正有规律性偏差转位角度不准、温度梯度造成零偏差编码器闭环转位改善两陀螺热均衡最后说一个我个人的习惯每次上电后先不做正式测量用双位置预测量把B̂算出来盯着它观察几分钟看零偏是平稳的还是持续在爬。如果零偏曲线明显单调上升说明设备还没热透这时候跑正式流程是浪费数据的。等B̂曲线进入缓慢波动状态再开始解算出来的结果基本就稳了。这个动作虽然只花几分钟却能让整套系统的成功率上一个台阶。