构建虚拟陀螺仪(Michael E Stanley)

      在《表示定位》第1部分和第2部分，我们探讨了一些表示对象方位的一些数学方法。现在我们即将应用这些知识，利用3轴加速度传感器和3轴磁力计来构建一个虚拟陀螺仪。完成这个任务的理由你可能会想到：“成本”，还是“成本”。第1个成本是指财务方面。陀螺仪往往比其他两种传感器的成本更加昂贵。因此从物料清单(BOM)中剔除这项成本十分诱人。第2个便是指功耗。典型加速度传感器/磁力计对消耗的功率大大低于微电子机械系统(MEMS)陀螺仪消耗的功率。虚拟陀螺仪的不利之处在于它对线性加速度和未修正的磁场干扰十分敏感。如果这两种现象存在其一，你可能还是需要一个实体的陀螺仪。

那么我们如何计算方位和角速率呢？如果回顾一下它的概念十分简单，从高处考虑一下问题。角速率可以定义为每单位时间方位的变化量。我们已经了解许多方位建模的方法。弄清如何得出方位的导数，我们便可以开始了！

在我们之前的博客文章中，我们已经探讨了许多表示方位的方法。在这次讨论中，我们将会使用基本旋转矩阵。Jack B. Kuipers在他的《四元法和旋转顺序》一文中曾经得出了一个出色的方向余弦矩阵导数推导方法——他的这本书成了我使用最为频繁的教材之一。这是一个有利的起点。将他的公式改写如下：

令：

1.v_f = 固定参考坐标系中的某个矢量 v 测量值
2.v_b = 在运动坐标系中测量的相同矢量
3.RM_t = 将vf转化为的vb旋转矩阵
4.ω = 通过旋转得出的角速率

然后在任意时间下 t:
5.v_b= RM_t v_f

求两侧的微分 (利用RHS链规则)：
6.dv_b/d_t = (dRM_t/dt) v_f + RM_t(dv_f /dt)

我们没有线性加速度或磁场干扰的限制意味着：
7.dv_f/dt = 0

则：
8.dv_b/dt = (dRM_t/dt) v_f

我们已知：
9.v_f = RM_t^-1 v_b

将其插入 (8) 得到：
10.dv_b/dt = (dRM_t/dt) RM_t^-1 v_b

在之前的博客文章中（加速度传感器布置 – 位置和原因）中，我们了解到传输定理，即描述运动框架的矢量变化速度：
11.dv_f/dt = dv_b/dt - ω X v_b

在这些需要耗时检查的公式中，可以注意到我们已经插入了之前博客文章所示等式11中的ω极性。在这种情况下，ω是固定参考坐标系的角速度。在此处，我们希望得到逆向结果（与陀螺仪的输出进行匹配）。

于是再次，
12.dv_f/dt = 0

因此
13.dv_b/dt = ω X v_b

将等式10和等式13相等化：
14.ω X v_b = (dRM_t/dt) RMt_t^-1v_b
15.ω X = (dRM_t/dt) RM_t^-1

其中：
16.

返回我们第一个微积分过程的基本要素，利用导数的一侧近似值：
17.dRM_t/dt = (1/Δt)(RM_t+1 – RM_t)

其中 Δt = 方位采样之间的时间
18.ω_b X = (1/Δt)(RM_t+1 – RM_t) RM_t^-1

想一想旋转矩阵，移项与倒数相同：
19.RM_t^T = RM_t^-1
20.ω_b X = (1/Δt)(RM_t+1 - RM_t) RM_t^T

等式15是一个真正理想的等式。它表示你可以根据最后两个方位的变量便可以计算出角速率。这变得十分直观，很惭愧的是我在第一次看到它研究了很久时间才明白。

一个等式变形甚至可以变得更加理想，根据RHS进行乘法运算：
21.ω_b X = (1/Δt)(RM_t+1 RM_t^T - RM_t RM_t^T)
22.ω_b X = (1/Δt)(RM_t+1 RM_t^T - I_3x3)

为了清晰明显考虑，让我们来扩展一下这些等式。一个旋转矩阵外形尺寸为3x3。因此等式22的左侧和右侧外形尺寸为3x3。

23.(1/Δt)(RM_t+1 RM_t^T – I_3x3) = (1/Δt)W

24.

W的零值对角元素由小角度近似值产生，因为RMt+1 RMtT的对角线项接近于1，它会被恒等矩阵的减法运算所抵消。则：
25.

且我们会得出：
26.ω_x= (1/2Δt) (W_3,2 - W_2,3)
27.ω_y= (1/2Δt) (W_1,3 - W_3,1)
28.ω_z= (1/2Δt) (W_2,1 - W_1,2)

一旦我们得出方位，我们可以计算出以下相应的角速率：

• 一个 3x3 矩阵乘法运算
• 3 个标量减法
• 3 个标量乘法

每一个点。非常清晰！

不久之前，我运行Matlab软件仿真，根据加速度传感器/磁力计读数来观察陀螺仪的输出与“虚拟陀螺仪”输出的对比。在调整了陀螺仪偏差和比例因子之后，我获得了非常不错的相互关系，在下图中我们可以看到。

你会注意到我们从假设开始，现在已经了解如何根据指定的加速度传感器/磁力计读数计算方位。还有许多方法可以达到这个目的。我现在可以想到三种方法：

• 计算《飞思卡尔AN4248》中描述的滚动、倾斜和偏移。 利用这些数值计算《方位表示：第1部分》中描述的旋转矩阵。这种方法使用欧拉角，我个人建议不用这种方法，但各位不妨一试。
• 利用Android获得旋转矩阵[4]直接计算旋转矩阵。这种方法利用一系列关联产品计算出当前方位。
• 利用Wahba问题解决方案计算每一时间点的最优旋转。这一项属于我的个人爱好，但我想自己会在日后的博客文章中详细阐释这个问题。

无论你采用哪种方法计算方位，你都需要留意以下这些细节：

• 牢记非零线性加速度和/或未修正磁场干扰会违反理论物理假设。
• 以上所示的表达式通常依赖于小角度假设。即，一次一步到下一步的方位变化相对很小。你可以利用极短的采样间隔来达到这个目的。你会很快看到我的同事Mark Pedley的应用笔记，他力图抛弃这些假设，直接处理大角度假设。我更倾向于此处我展示的方法，因为它看上去更加直观。
• 加速度传感器和磁力计输出的噪声会在虚拟陀螺仪输出中产生极为明显的噪声。你需要在使用它们之前，对输出使用低通滤波器。Mark会在他的应用笔记中提供相关的实施案例。

这是我十分热衷的问题之一。其美妙之处在于，大自然会提供不同角度的角运动。我希望各位也欣赏这一点。

参考文献

1. 飞思卡尔应用笔记编号AN4248：利用加速度传感器和磁力传感器实施倾斜补偿的电子罗盘
2. 方位表示：第1部分，嵌入式当道的博客文章
3. 方位表示：第2部分，嵌入式当道的博客文章
4. 获得旋转矩阵()功能，定义网址：http://developer.android.com/reference/android/hardware/SensorManager.htmlWikipedia entry for "Wahba’s problem"
5. 美国专利申请13/748381，陀螺仪校准的系统和方法，Michael Stanley，飞思卡尔半导体

本文由Michael E Stanley撰写，发布于 2013 年 03 月 12 日。

mark:http://blogs.freescale.com.cn/iframe/Michael-E-Stanley-March0312.shtml