AN-1157 应用笔记 One Technology Way • P.O. Box 9106 • Norwood, MA 02062-9106, U.S.A. • Tel: 781.329.4700 • Fax: 781.461.3113 • www.analog.com 在ADIS16480中调谐扩展卡尔曼滤波器 作者：Mark Looney和Glenn Romano 简介 ADIS16480是一款MEMS惯性测量单元(IMU)，内置一个三 ADIS16480在用户可访问的寄存器中为陀螺仪偏置、陀螺 轴加速度计、一个三轴陀螺仪、一个三轴磁力计和一个气 仪速率随机游动(RRW)以及加速度计和磁力计的协方差值 压计。除了提供完全校准、帧同步的惯性MEMS传感器， 提供了用户写入控制(见表1)。在ADIS16480中使用自适应 ADIS16480还集成了一个扩展卡尔曼滤波器(EKF)，可计算 的EKF选项时，这些寄存器可根据传感器的测量条件实时 动态方位角。 更新。 EKF使用陀螺仪、加速度计和磁力计所测数据估算方位 表1. ADIS16480协方差项寄存器 角。加速度计根据地球重力测量方位角。磁力计根据地球 寄存器 QCVR_NOIS_UPR QCVR_NOIS_LWR QVCR_RRW_UPR QVCR_RRW_LWR RCVR_ACC_UPR RCVR_ACC_LWR RCVR_MAG_UPR RCVR_MAG_LWR 磁场测量方位角。图1表示使用两种独立数据输入的EKF： 集成陀螺仪与当前方位，由加速度计和磁力计的几何测量 结合而得。卡尔曼增益级包含加权因子，在最终角度估算 中决定每一因素的贡献程度。ADIS16480根据分配给每个 传感器的不确定程度的协方差项实时导出加权因子。 在给定的应用环境下，性能的优化来自为传感器选择合适 地址1 0x62 0x60 0x66 0x64 0x6E 0x6C 0x72 0x70 用途 陀螺仪噪声，高位字 陀螺仪噪声，低位字 陀螺仪RRW，高位字 陀螺仪RRW，低位字 加速度计，高位字 加速度计，低位字 磁力计，高位字 磁力计，低位字 的协方差。选择优化的协方差值时，可能需经过反复测 试、观察、分析、调整以及再测试的过程。虽然本应用笔 记提供简化此过程的分析工具，但针对不同应用的观察和 1 这些寄存器均可在第3页的ADIS16480用户寄存器存储器映射中找到。访 问这些地址前，先设置PAGE_ID = 0x0003 (DIN = 0x8003)。 调整也是必须的。 ∫ ωdt GEOMETRIC COMBINER ACCELEROMETERS MAGNETOMETERS ORIENTATION K–1 KALMAN GAIN K COVARIANCES 图1. ADIS16480扩展卡尔曼滤波器的信号通道 Rev. B | Page 1 of 8 10825-001 GYROSCOPES AN-1157 目录 简介...................................................................................................... 1 调谐指南 ............................................................................................. 4 修订历史 ............................................................................................. 2 针对动态性能调谐 ...................................................................... 4 理论协方差值 .................................................................................... 3 针对静态精度调谐 ...................................................................... 7 自适应特性......................................................................................... 3 结论 ................................................................................................ 8 修订历史 2013年1月—修订版A至修订版B 更改表2和表3 .................................................................................... 3 2012年12月—修订版0至修订版A 更改“调谐指南” ................................................................................ 4 2012年7月—修订版0：初始版 Rev. B | Page 2 of 8 AN-1157 理论协方差值 ADIS16480采用的默认协方差值如表3所列。注意，由于纳 调谐EKF通常是个迭代过程，因为某些误差源难以建立模 入了典型、未建模误差源，表中数值大于理论值。 型。开始该过程的方法之一是假设这些误差源不存在，并 表3. 出厂默认协方差项 设置协方差值为理论噪声值和稳定值。陀螺仪噪声的协方 陀螺仪噪声 1 × 10-5 (rad/sec)2 差初始值来源于该陀螺仪的总噪声规格。将该数值的单位 陀螺仪漂移 5 × 10-11 (rad/sec)2 转换为弧度/每秒(即rad/sec)并求它的平方。 加速度计 4 × 10-9 LSB2 磁力计 3 × 10-8 LSB2 π σ noise = 0.16 × 180 2 2 = 7.8 × 10− 6 (rad/sec)2 自适应特性 陀螺仪的RRW协方差初始值可取自运行时的偏置稳定度 (每小时6.25°)。 (RCVR_ACC_xxx)和磁力计(RCVR_MAG_xxx)的协方差 2 6.25 π = × = 3.7 × 10 −13 (rad/sec)2 3600 180 σ RRW 2 ADIS16480集成了创新残差算法，可自适应调整加速度计 项。该算法实时调整RCVR_ACC和RCVR_MAG寄存器， 可让用户观察环境条件变化的影响。该特性在调谐EKF的 实际计算中，影响长期漂移的其他因素可能亦须纳入考量 过程中极为有用。 范围以优化该参数，如温度漂移、振动和其他线性加速。 此算法生成的协方差值通常比计算得到的理论值大。差别 加速度计和磁力计的协方差项取自它们的噪声规格，并采 在于一些未建模的误差源，如传感器的失调和比例因子、 2 对齐误差导致的交叉耦合、以及硬铁/软铁磁性失真。 用LSB 作为单位。 σ 2 Accel 1 0.0015 = × 1.221× 10 −8 231 σ Accel = 3.39 × 10 2 −9 σ 2 Mag 1 0.00045 × 0.1× 10 − 4 215 = 1.98 × 10 −8 导致协方差值较大的另一因素。 LSB2 σ Mag = 2 片上温度补偿能尽可能降低与温度有关的误差，但残差是 2 LSB 陀螺仪的比例因子误差未建模，对协方差项会产生噪声影 响。ADIS16480的EKF_CNFG[9]具有存储器淡出功能，以 2 提供一定的补偿。设置EKF_CNFG[9] = 1 (DIN = 0x8003， 0xC102)以使能此项功能；它可向方差中添加陀螺仪速率 2 的百分比，以便计算比例因子误差。当传感器稳定时，对 表2. 理论协方差值 陀螺仪协方差(Q)参数不作修正，但增加旋转速率会使协 陀螺仪噪声 7.8 × 10-6 陀螺仪漂移 3.7 × 10-13 加速度计 3.39 × 10 磁力计 1.98 × 10-8 方差增加，从而增大潜在误差。Q值的调整对器件而言产 生于内部，并且无法通过读取QCVR_NOIS寄存器而观察到。 -9 Rev. B | Page 3 of 8 AN-1157 调谐指南 如前所述，EKF算法通过对陀螺仪、加速度计和磁力计测 磁力计的软铁/硬铁影响 量值的优化组合估算出ADIS16480的方位。每个传感器的 硬铁和软铁影响是常见的磁力计测量值的误差源，并且似 协方差项决定了它们在角度估算中的影响(权重)。较小的 乎与特定应用有关。在尝试调谐EKF前，将ADIS16480绕 协方差值表示该传感器噪声对其他传感器的噪声影响较 三轴中的两轴缓慢旋转360°并测量所有三个磁力计以便表 小，将会提供稳定的测量值。因此，这些传感器的测量值 征磁力计的响应。使用这些测量值获得硬铁和软铁校正系 在最终的估算结果中所占的权重就大。 数，并将这些数值写入SOFT_IRON_Sxx和HARD_IRON_x 协方差项使特定应用中抑制噪声影响变得可行。振动是导 寄存器。 致出厂默认协方差项产生变化的因素之一。本例中，与 折衷空间 RCVR_ACCL_xxx寄存器有关的数值可能会变大，降低加 调谐包括在动态和静态性能之间做出折衷。针对跟踪高速 速度计所占权重，因为加速度计的测量值将会有与振动相 移动来调谐滤波器可能会牺牲稳态精度。同样，针对稳态 关的误差。 精度的调谐将以带宽和跟踪快速移动的能力为代价。这两 磁场干扰和其他误差源的存在提供了协方差项的更新可改 者的相对重要性由应用决定。 进角度精度的另一例证。本例中，增加RCVR_MAG_xxx寄 针对动态性能调谐 存器中的数值可降低磁场误差的影响。降低加速度计和磁 力计的权重所带来的折衷之一是管理陀螺仪饱和度的能 力。较小的加速度计或磁力计的影响力可能导致角度跟踪 的丢失以及从饱和度事件恢复缓慢。另一折衷是设备表现 针对动态条件调谐EKF的第一步是制定协方差值的原因/效 果分析测试方法。该测试方法包括4个基本部分：惯性运 动配置、可观察性能指标、分析技术和数据采集。下述实 例展示了这些对所有应用都有用的步骤。 出了较高的稳态误差。另一方面，降低加速度计和磁力计 惯性运动配置 供了一个控制位，可屏蔽磁力计对角度测量的影响。设置 EKF_CFG[1] = 1 (DIN = 0x8003，0x5001)以屏蔽磁力计对角 惯性运动配置必须包含能够反映实际应用条件的惯性条 件。图2表示本例中涉及的惯性条件——利用1 Hz正弦曲线， 在0至30度之间围绕z轴旋转。 度估算造成的影响。屏蔽磁力计的影响将迫使偏航角度仅 依赖陀螺仪，造成的结果是角度漂移速率与陀螺仪的总偏 置误差成正比。性能的优化包括在互相矛盾的系统需求之 间取得正确的平衡，这通常很困难。 协方差调整对于持续环境噪声的补偿很有用，而对于管理巨 大、偶发性的干扰，ACC_DIST_THR和MAG_DISTB_THR 30 15 0 0 1 2 3 10825-003 若磁场误差对于角度精度目标来说过大，则EKF_CFG[1]提 ROTATION ANGLE (Degree) 的权重似乎可带来较少的噪声输出。 寄存器提供了用户可编程的“干扰水平”。ADIS16480持续 图2. 动态测试激励 将三个维度与反映它们在最后EKF初始化过程中状态的存 相对于水平方向，该配置还使用了两个不同的方向作为旋 储 值 相 比 较 。 当 幅 度 大 于 或 等 于 ACC_DIST_THR或 转轴：水平(图3)和垂直(图4)。当旋转轴位于垂直方向上， MAG_DISTB_THR寄存器指定阈值的某一百分比时，便在 加速度计的输出基本未被采用，因为输出恒定。将旋转轴 干扰持续期间暂时禁用受影响的测量值。 设置在水平方向上，则旋转平面与磁北一致，确保陀螺 仪、加速度计和磁力计均受到激励。 Rev. B | Page 4 of 8 AN-1157 可观察性能指标 偏航角度输出寄存器YAW_C32_OUT为观察提供了最佳的 输出变量，假定此时惯性运动配置为z轴。 分析技术 为测量信噪比(SNR)而采用的FFT分析技术可为代表惯性的 ADIS16480 偏航输出寄存器提供简单、精确的质量指标。 数据采集 ADIS16480输出数据数率为2460 (DEC_RATE = 0x0000)，且 10825-008 数据记录长度为35200个样本，足以代表惯性运动。 图3. 水平旋转轴 下列3D柱状图表示了一系列协方差设置下的信噪比。所有 实例中，陀螺仪漂移协方差QCVR_RRW均设为10-12。至 于其余三个协方差项，一个值固定在一特定图形上(在图形 标题中给出)；柱高表示SNR的观察值，该值与几个独立的 设置相关(在10-14至10-4之间)。它证明两个不同的旋转方 IS1 648 0 向导致不同的结果。当旋转轴位于水平方向时，ADIS16480 的协方差项范围较大。 10825-004 AD 图4. 垂直旋转轴 Rev. B | Page 5 of 8 AN-1157 垂直方向 水平方向 下图中的信噪比(SNR)测于旋转轴为垂直方向时。 下图中的SNR测于旋转轴为水平方向时。 40000 40000 35000 35000 30000 30000 25000 20000 15000 15000 10000 10000 5000 5000 10–6 AG 10–14 10–8 10–10 –12 10 RCV 10–14 R_M AG 图8. 动态性能，水平轴，RCVR_ACC = 1 × 10 -8 图5. 动态性能，垂直轴，RCVR_ACC= 1 × 10 -8 40000 40000 35000 35000 30000 25000 25000 20000 15000 SNR 30000 20000 15000 10000 10000 5000 5000 0 10–4 10–6 10–8 10–10 RCV R_A CC 10–12 10–14 10–8 E IS 10–10 NO 10–12 _ R 10–14 CV Q 10–4 10825-006 10–4 10–6 图6. 动态性能，垂直轴，RCVR_MAG = 1 × 10 -8 10–6 10–8 10–10 –12 10 RCV 10–14 R_A CC 0 10–4 10–6 10–8 E 10–10 IS O 10–12 _N R 10–14 CV Q 图9. 动态性能，水平轴，RCVR_MAG = 1 × 10 -8 40000 40000 35000 35000 30000 25000 SNR 30000 25000 20000 20000 15000 15000 10000 10000 5000 5000 0 10–4 10–6 10–8 RCV R_M 10–10 AG 10–12 10–14 RC 0 10 –4 10 –6 10 –4 10825-007 10–4 10–6 10–8 10–10 CC 10–12 _A 10–14 VR SNR 10–12 10825-010 RCV R_M 10–4 10–6 SNR 10–10 10 –6 10 –8 10 –8 10 –10 10 –10 RCV R_M AG 10 –12 10 –12 10 –14 10 –14 C AC R_ V RC 图10. 动态性能，水平轴，QCVR_NOISE= 1 × 10 -6 图7. 动态性能，垂直轴，QCVR_NOISE= 1 × 10 -6 Rev. B | Page 6 of 8 10825-011 10–8 10–8 E IS 10–10 O _N R CV Q 10–12 10–14 10825-005 10–4 10–6 0 10–4 10–6 E 10–8 IS O 10–10 _N 10–12 R 10–14 CV Q 10825-009 0 10–4 SNR 25000 SNR 20000 AN-1157 针对静态精度调谐 水平方向 在检查静态精度的测试中，记录下初始方向，将传感器旋 图15至图17数据为旋转轴处于水平方向时获得。 90 1.0 0.6 RCVR_ ACC 1.0 0.9 0.8 0.7 0.6 0.5 0.4 0.3 0.2 0.1 0 1.0 10 –4 10 –6 10 – 10 8 –1 0 10 – 12 10 – 14 图12. 静态性能，垂直轴，RCVR_ACC = 1 × 10 -8 RCVR_ MAG 10–4 10–6 10–8 10–10 10–12 10–14 0.8 0.6 0.4 10825-014 Q 12 14 10 – 10 – 10 10 –8 10 – CV R_ NO 4 IS E 10–10 10–4 6 10 – 10 – RCV R_A CC 图13. 静态性能，垂直轴，RCVR_MAG = 1 × 10 -8 1 0.8 0.6 0.4 10–4 10–6 10–8 10–10 10–12 10–14 0.2 10825-015 10 10 –8 10 –6 10 AG _M VR RC 10 – 12 10 – 14 10 – 12 10 – 10 4 10 –8 – –4 0 RC VR _A CC SE 10–10 10–4 图17. 静态性能，水平轴，QCVR_NOISE= 1 × 10 -6 0.2 0 10–4 10–6 10–8 10–10 10–12 10–14 10825-018 10825-013 10 –14 10 –10 10 –12 10 –8 AG R_M RCV 14 图16. 静态性能，水平轴，RCVR_MAG = 1 × 10 -8 10–4 10–6 10–8 10–10 10–12 10–14 10 –4 10 –6 1.0 0.8 0.6 0.4 0.2 0 Q CV 10 –12 R_ 10 –8 NO 10 –4 IS E 10–10 10–4 10 – 封顶为1°，以避免图中到处都是更小的误差。 10 10 – 4 方向。测量方向与理想的90°方向之间的差异已标明。误差 12 图12至图14提供了不同协方差项的响应值，旋转轴为垂直 10–4 10–6 10–8 10–10 10–12 10–14 10 – 垂直方向 1.0 0.9 0.8 0.7 0.6 0.5 0.4 0.3 0.2 0.1 0 10 –8 10 – 的范围内变动。 6 个协方差固定在标称值，剩下的其余两个则在1e-14至1e-4 图15. 静态性能，水平轴，RCVR_ACC = 1 × 10 -8 10 – 和动态测试一样，陀螺仪漂移的协方差固定在1e-12，另一 10825-017 60 QC VR _N OI 40 图11. 静态测试激励 RC VR _A CC 20 0.2 0 10–4 E 10–8 IS 10–12 _NO VR QC AG 0 10825-012 0 0.4 10–14 10–12 10–10 10–8 10–6 10–4 10825-016 10–4 10–6 10–8 10–10 10–12 10–14 0.8 _M VR RC ROTATION ANGLE (Degrees) 转90°，然后用初始值减去最终方向值。 图14. 静态性能，垂直轴，QCVR_NOISE = 1 x 10 -6 Rev. B | Page 7 of 8 AN-1157 结论 调谐卡尔曼滤波器以获得优化性能是一个多数情况下需要 虽然本文中的结论并不适用于所有应用，但可作为选择初 反复试错的过程。本应用笔记提供了一种有助于找到合适 始协方差值以供测试的起点。根据所需规格不同，应谨慎 协方差值组合的方法。由于每个SNR读数均采用较大的步 检查静态以及动态性能，然后选择优化组合。 长，良好的调谐对于最终应用的改进极为关键。 ©2012–2013 Analog Devices, Inc. All rights reserved. Trademarks and registered trademarks are the property of their respective owners. AN10825sc-0-1/13(B) Rev. B | Page 8 of 8