姿态矫正-Android惯性传感器

惯性传感器包括加速度计和陀螺仪，若要获取三个姿态轴的角度，加速度计+磁力计
可以获取绝对角度[不滤波的情况下噪声较大]，陀螺仪可以获取相对角度（即转过多少度）[存在漂移]，但惯性传感器都存在误差，因此要做姿态矫正。

什么是惯性传感器

惯性传感器是检测和测量加速度、倾斜、冲击、振动、旋转和多自由度(DOF)运动的传感器。惯性传感器，应用惯性原理和测量技术，感受载体运动的加速度、位置和姿态的各种敏感装置。

惯性传感器分类与构成

惯性传感器包括加速度计（或加速度传感计）和角速度传感器（陀螺）以及它们的单、双、三轴组合IMU（惯性测量单元），AHRS（包括磁传感器的姿态参考系统）。

MEMS加速度计是利用传感质量的惯性力测量的传感器，通常由标准质量块（传感元件）和检测电路组成。IMU主要由三个MEMS加速度传感器及三个陀螺和解算电路组成。

航姿参考系统（AHRS）与惯性测量单元IMU的区别在于，航姿参考系统（AHRS）包含了嵌入式的姿态数据解算单元与航向信息，惯性测量单元（IMU）仅仅提供传感器数据，并不具有提供准确可靠的姿态数据。目前常用的航姿参考系统（AHRS）内部采用的多传感器数据融合进行的航姿解算单元为卡尔曼滤波器。

惯性传感器应用

低精度MEMS惯性传感器作为消费电子类产品主要用在手机、GPS导航、游戏机、数码相机、音乐播放器、无线鼠标、PD、硬盘保护器、智能玩具、计步器、防盗系统。由于具有加速度测量、倾斜测量、振动测量甚至转动测量等基本测量功能。

中级MEMS惯性传感器作为工业级及汽车级产品，则主要用于汽车电子稳定系统（ESP或ESC）GPS辅助导航系统，汽车安全气囊、车辆姿态测量、精密农业、工业自动化、大型医疗设备、机器人、仪器仪表、工程机械等。

高精度的MEMS惯性传感器作为军用级和宇航级产品，主要要求高精度、全温区、抗冲击等指数。主要用于通讯卫星无线、导弹导引头、光学瞄准系统等稳定性应用；飞机/导弹飞行控制、姿态控制、偏航阻尼等控制应用、以及中程导弹制导、惯性GP战场机器人等。

手机常用传感器用途

Android惯性传感器

线性加速度计是排除了重力加速度的影响得到的值
重力感应器、方向感应器（电子罗盘）、旋转传感器（姿态传感器）是基于加速度计、陀螺仪、磁传感器（有的还有GPS）的数据获得的派生传感器。重力传感器是通过测量重力加速度方向来判断重力的方向，与旋转传感器区别是，旋转传感器用到了陀螺仪的数据，在动态情况（比如你在边走边看手机）也能很好的判断手机的姿态角。方向传感器则是主要通过磁传感器来测量地磁场来判断方向（类似指南针的作用），有时还需要配合GPS（各地的磁场不一样）。
陀螺仪和加速度计都是通过振动测量加速度的，只不过陀螺仪测量的是科氏加速度，而加速度计是测量的直接加速度。MEMS陀螺仪可以简单理解为一个谐振器+加速度计。因此陀螺仪的结构要比加速度计复杂得多，技术也难得多。

坐标系

飞行器姿态角，欧拉中的俯仰、横滚、偏航角

Android手机的自然坐标系

手机坐标系是跟随设备的自然方向的，即当设备运动或者旋转的时候，这些坐标轴是不会改变的。

pitch是俯仰角，是“点头”，在Android陀螺仪传感器中绕x轴旋转

yaw是偏航角，是“摇头”，在Android陀螺仪传感器中绕y轴旋转

roll是旋转角，是“翻滚”，在Android陀螺仪传感器中绕z轴旋转

用加速度和地磁传感器得到的角度，通过getOrientation())方法，得到的是z，x，y

通过getRotationMatrix())
方法，得到的是世界坐标系

惯性传感器误差

陀螺仪，测量角速度，具有高动态特性，它是一个间接测量角度的器件。它测量的是角度的导数，即角速度，要将角速度对时间积分才能得到角度。

加速度计的低频特性好，可以测量低速的静态加速度。加速度对振动和其他非重力加速度敏感。加速度计仅仅测量的是重力加速度，3轴加速度计输出重力加速度在加速度计所在机体坐标系3个轴上的分量大小。重力加速度的方向和大小是固定的。通过这种关系，可以得到加速度计所在平面与地面的角度关系。加速度计若是绕着重力加速度的轴转动，则测量值不会改变，也就是说加速度计无法感知水平旋转。

因此6轴使用了加速度计和陀螺仪，也只可以用于测得飞机的俯仰和横滚角度。对于偏航角度，由于偏航角和重力方向正交，无法用加速度计测量得到。在这个维度上没有一个可靠的长期值来矫正陀螺仪的值。因此还需要采用其他设备来校准测量偏航角度的陀螺仪的漂移值。校准的设备可以使用磁罗盘计（电子磁罗盘，对磁场变化和惯性力敏感）或者GPS。GPS数据更新较慢（1Hz到10Hz），并且短时间内存在误差。所以用GPS只能长时间的在地面跟踪较为稳定和慢速的飞行器，如果需要矫正航向我们就只有使用电子罗盘了。

1、确定性误差

传感器非正交性（安装误差）：

三轴加速度计和陀螺仪的三个轴不是完全的正交的，例如对于加速度计，理想情况下其中一个轴测量重力，其他两个轴不应该有输出。传感器不正交会出现在安装和封装的时候。生产和标定能够一定程度的解决这个问题，在系统运行的时候持续的估计和矫正也是一种解决方法。

尺度误差（scale）

2、陀螺仪的误差分析

温漂

解决方法：

通过大电阻进行加热，使得传感器的工作温度在一恒定值。如DJI拆机图，IMU模块有个大电阻进行加热。

另外一种就是事先进行温飘标定，即不同温度下，拟合出补偿的数据。不过，这种方法也较为麻烦，比如飞控量产，需要每一个都这么做；其次就是时间长了，标定会有偏差的，所以，能硬件解决的问题，千万不要让算法去做。

零偏

漂移指输出随着时间的变化，零漂是输入为0的时候的输出。

重复性

假设所有的条件一样，对于相同的输入，传感器输出相同的值的能力（对于每次启动都相同）。陀螺仪的零偏不具有重复性。

随机噪声

噪声分析allan variance模型

a、量化噪声Q(t)(Quantization Noise)

AD采样导致的噪声，量化噪声具有很宽的带宽，属于高频噪声，可以用低通滤波器处理或通过导航的积分环节去除，对精度的影响不大；

b、角度随机游走(ARW(t))(Angular Random Walk)

c、零偏不稳定性(F(t))(Bias Instability) ，也叫作闪变噪声（Flicker Noise）

准确的来说，并不是只有零偏导致偏移，零偏是没有运动时平均输出。零偏不稳定性即零偏会随着时间缓慢的变化。这是由于内部电磁的善变噪声和外部影响导致的，而且每次启动之初的零偏是不相同的。

d、速率斜坡(Rate Ramp)

陀螺的角速率输出随着时间缓慢变化，通常由系统误差引起，比如环境温度的缓慢变化，可以通过严格的环境控制或引入补偿来降低此类误差。

e、指数相关噪声(Exponential Correlation Noise)和正弦噪声S(t)（Sinusoidal Noise）

正弦噪声：由于mems单元在特定频率工作，所以一般受到正弦噪声的污染。

从算法上需要解决的：（1）角度随机游走(ARW)（2）零偏不稳定性

惯性传感器姿态校正

1、一种有效的长期漂移消除方法是对陀螺仪实施零角速度更新。只要知道器件处于完全静止状态，便可将相应轴的陀螺仪偏移归零。因具体应用不同，这样的机会有很大差异。但只要系统处于重复出现的安静状态，例如汽车怠速、自主机器人静止或人跨脚步之间的时间，就可以进行归零调整。

2、传感器融合
因为加速度/磁力计具有高频噪声（需要低通滤波），它的瞬时值不够精确，解算出来的姿态会震荡，但长期来看姿态方向是对的。而陀螺仪具有低频噪声（需要高通滤波），即每个时刻的得到的角速度是比较精确的，使用积分就能得到旋转角度（姿态），但是积分会累积误差。可以融合这三种传感器的数据，提高精度和系统的动态特性。

1、FSensor方案

a、Averaging Filters

用来去除随机噪声，但漂移没有去除

Low-Pass Filter 低通过滤器
Mean Filter 均值过滤器
Median Filter 中值过滤器

b、Sensor Fusions

Quaternions Complimentary Filter(四元数互补滤波)

（测试手机魅蓝数据）正常状态绕圈运动角度变化值

到（-180/180）边界值异常变化，所以放弃了该算法

Quaternion Kalman Filter(四元数卡尔曼滤波)

c、Linear Acceleration

Android Linear Acceleration
短时间表现较好
Low-Pass Linear Acceleration
IMU Sensor Fusion Linear Acceleration

2、Oculus融合算法方案

Oculus sensor fusion:Keeping It Simple 传感器融合算法

类似互补融合，它的代码实现在openHMD中ofusion_update接口中
注：openHMD中的oculus融合算法（已更新于2016.12.01），不包含航向偏移校正。

3、MIT互补滤波算法方案

Android Sensor Fusion Tutorial

MIT上发表的互补滤波算法的原理和基于Android平台的算法实现。实验LG有些手机的数据是不准确的（后来发现是磁力计未做矫正导致，需要手动矫正…姿势有点蠢），魅蓝是都准确的。

Angle = (a)(Angle + Gyro dt) + (1-a)*(x_acc)

时间常数t=(a*dt)/(1-a) , 得到a=t/(t+dt)。dt为取样周期，t为时间常数自己设定，一般小于1。比如取样周期为100HZ，也就是0.01s,那么dt=0.01。时间常数设为0.5（对于不同的系统都不一样，自己试验），那么a=0.5/(0.5+0.01)=0.9804。

那么Angle = (0.9804)(Angle + Gyro dt)（高通部分） + (0.0196)*(Acc)（低通部分）

互补滤波不能有效纠正陀螺仪零漂，而卡尔曼可以有效纠正陀螺仪零漂问题。
在温度环境稳定的情况下，例如平衡车一般都是在同一个地域，地面温度环境不会剧变，传感器零漂不大，所以在应用互补滤波和卡尔曼差别不明显。而在飞行器领域，正所谓高处不胜寒，随着高度上升温度也会变化，传感器也会变得不稳定，所以这时候使用卡尔曼滤波才会体现出比互补滤波的优越之处。