[译]Oculus:"sensor fusion:Keeping It Simple"

Oculars的融合算法

原文地址,翻译如下：
自九月以来，我在Oculars VR团队中一直努力为Rift项目工作着。以尽可能少的延迟和错误为前提，来追踪头部姿态是一个很有挑战的工作。在数学和工程中，这已经是一个老问题了。千年来，人们一直希望追踪海上船只和陆地上车辆的姿态。在过去的一个世纪里，优秀的传感器系统已经应用于跟踪飞机、宇宙飞船、导弹、机器人、虚拟现实设备，以及手机当中。我是从事了多年机器人方面的教授，深入思考关于机器人定位，混合传感器，电机以及计算机方面有趣的问题。我抓住这个机会，利用可用的源代码和开发者社区，为虚拟现实游戏方面做出重要的贡献，我为我在Oculars工作感到十分激动。
这篇文章的剩余部分，我将解释我们的追踪方法是如何工作的，我们面临的挑战是什么，以及我们使用不同的设计策略的原因。在我们的开发过程中，我们始终以方法的简单为目标，使它更容易被人理解，更容易优化其性能，将来更容易被扩展、优化。我们考虑过使用一些成熟的解决方法，如卡尔曼滤波或粒子滤波，但是这些方法需要根据特征建模和假设，来达到理论效果。例如，卡尔曼滤波在线性系统的线性测试和高斯噪声下的最佳估计，但是如果超过这个范围（即不满足线性或正态分布），那么结果就不能保证。此外，这个方法适用于低采样率和高度可预测的系统中，来增强对运动模型的追踪。粒子滤波适用于解决那些状态信息巨大情况，如：周围的障碍物模型（关于机器人识别周围环境的情况）。另外一个提供选择的方法是从经典线性滤波理论中产生的方法，叫做互补滤波，它融合了经过高通滤波的陀螺仪数据和低通滤波的加速度数据。互补滤波和卡尔曼滤波方法的比较，见文末的[3].我们使用的方法是类似于互补滤波算法的，由于目前的计算能力问题，我们运用的可以进行迭代的算法，来解决我们的追踪问题。

积分陀螺仪数据

在Rift中的陀螺仪测量头部姿态的变化，以1000次每秒的频率上报。通过获取“已知”的头部姿态和最新的陀螺仪测量值，软件计算出目前头部的姿态。想象一下，如何通过读取速度计来得出车子走了多远。这个距离更新就像如下公式所示：
Current distance = Previous distance + Time difference * Observed speed.
当”Time difference”趋近于0，并且假定速度计是100%准确的，这个公式能够计算出这段时间内的所走的距离。但是在现实当中，”Time difference”是不为0的，并且速度计数据并非完全正确的，这就引起漂移问题，接下来我们将来讨论它。

现在让我们回到头部追踪的问题上来，它有3个旋转自由度。一个3D刚体的姿态常常用yaw,pitch和roll角来表示。如上图所示，它是直观的，但是它存在万向锁问题，这将导致很多麻烦，所以我们使用四元素来表示姿态。
假设头部只绕Y轴旋转，以每秒w角度的速度进行旋转。假设传感器数据上报的间隔是每秒1000次，旋转角度更新的公式为：
Current orientation = Previous orientation + 0.001 * w
这就是Rift SDK上更新旋转角度的公式，当然在SDK上计算更加复杂一些，他需要同时处理yaw,pitch和roll组合的数据。陀螺仪提供3轴的角速度，生成一个3D矢量：
(Wx,Wy,Wz)
在数学上，我们知道任何一个3D姿态都可以描述为一个物体从最初状态绕轴旋转角度θ后的姿态。旋转可以认为绕不同轴以不同角速度的旋转集合。(Wx,Wy,Wz)表示绕3轴旋转的角速度（为了计算方便，会将它单位化）。此外，矢量的长度就是周旋转的速度。因此，四元素更新的公式如下：
Current quaternion = Previous quaternion * Quat(axis, angle)
“Quat(axis, angle)”表示的是对应轴的旋转角度。使用单位四元素是因为可以很容易的将它们与坐标角度进行相互转化，并且方便后面的旋转矩阵的计算，同时还避免了欧拉角数值奇异性的问题。
这种方法是简单的、准确的。它依赖于硬件上陀螺仪数据的准确性。更多的复杂的数据奇异性的公式可以被尝试（例如上面提到的欧拉奇异性，四阶龙窟塔算法），目前我们没有发现为什么要以1000HZ的频率来获取数据。一些预测过滤将会被使用，我们接下来将介绍这些。

漂移校正

在成千上万次的数据更新之后，真正的姿态与计算得出的姿态存在漂移。因此，需要其他的传感器来矫正这个姿态。在pitch和roll角度的漂移称为tilt error，对应得，我们后续称它为”up”，yaw角度的漂移，我们称为yaw error，对应的我们称为”North”,就好比，用了一段时间后，你发现你头部回到原来的位置上，但是追踪信息已经漂到其他地方了。yaw error将在以后的文章中用地磁的数据来解决；下面的内容，适用于解决tilt error问题。
处理tilt error，让我们想想”up”意味着什么。我们对“Up” 的认识，是完全基于重力的。重力来自于地心，它的方向就像光线一样，穿过你的身体。我们上学时被教导说重力加速度是9.81m/s^2,但是实际上这个值是根据你在地球的位置而有所变化的（我们在赤道上的重力最小—根据万有引力公式，半径越大，重力越小（这一句官方写的是“想象一下你在旋转木马的边缘”，额，有点绕））。
重力用加速度向量来表示，所以它很自然的用加速度计来测量它。当我们站在地面上，就像我们骑上了一个火箭向上加速（重力不是向下么？？？原文说是向上），这就是为什么我们一直被困在地球上。三周加速度计可以测量这个矢量，但是传感器测量值会有一些其他加速度值。放入Rift当中，它除了重力之外还测量头部运动的线性加速度。为了解决这个问题，我们希望重力矢量的测量精度是较高的，且不受其他因素影响。因为漂移增长是非常缓慢的，我们在若干个10毫秒之后，等待2个条件：
1.加速度测量值的长度（长度：x^2+y^2+z^2开根号，我从代码里看到长度是这个意思）接近9.8。
2.陀螺仪上报旋转角度很小（即相对静止的状态）。
另外，所有的加速度值都是经过滤波求平均值的。如果满足这些条件，我们认为加速度计是正确的上报”up”方向的。虽然一个标准的方法[2]，它显然是有缺陷的，因为你可以横向移动刚体，使它的横向加速度达9.8。然而，这个方法是简单的，而且对于我们来说已经足够。
现在假设误差角为φ，满足上面”up”的条件，加速度测量值为矢量ã。传感器融合系统需要运用旋转校正。误差角度为φ，但是旋转轴矢量是多少呢？它一定存在于水平面上，即XZ所在的平面上，并且垂直于矢量ã和Y轴。

将ã映射到水平平面上，表示为(ax,0,az)。保持在水平平面上的垂直矢量为(-az,0,ax),这个就是tilt轴。想象一下抓住tilt轴，然后旋转φ角度，就可以与Y轴对齐了。

一旦检测到tilt error，剩下的问题就是如何去执行校正旋转和旋转多少角度。这实际上是一个正在研究的课题，Rift的开发人员将会带来新的见解。如果使用者感受到tilt校准过程，那么他很可能会产生恶心、眩晕的感觉。另一方面，如果他们快速的旋转头部，所有需要校正的角度都可以校正而不被使用者察觉到。
我们目前采用以下方法。在Rift打开的后的前几秒，如果存在一个巨大的tilt error，我们直接旋转整个φ角度（来做校正）。一个常见的情况是，Rift启动时放在你的腿上或则在一个桌子上。在这种情况下，要做比较大的校正。另外，每次进行传感器数据融合时候都会进行很小的校正。频繁的绕轴旋转，都会进行相应的校正。当系统检测到需要进行tilt校正时，关键的问题就是要在使用者无法察觉到的情况下进行校正。我们尝试了不同的方法，但是每个人所感受到的并不完全一样。
你能想出一个更好的方法处理传感器融合吗？尝试把它创造出来！

引用

[1] Doucet, A., De Freitas, N., and Gordon, N.J., Sequential Monte Carlo Methods in Practice. Springer, 2001.
[2] Favre, J., Jolles, B.M., Siegrist O., and Aminian, K.,
Quaternion-based fusion of gyroscopes and accelerometers to improve 3D angle measurement, Electronics Letters, Volume 32, Issue 11, pp. 612-614, 2006.
[3] Higgins, W. T., A Comparison of Complementary and Kalman Filtering, IEEE Transactions on Aerospace and Electronic Systems, Volume 11, Issue 3, pp. 321-325, 1975.
[4] Stengel, R. F., Optimal Control and Estimation, Dover, 1986.