解决校准四元数

我正在编写一个需要旋转向量的Android应用程序。 我想使用TYPE_ROTATION_VECTOR但是在我的一些测试设备中，磁强计的表现并不好。 相反， TYPE_GAME_ROTATION_VECTOR提供了更平滑的数据（但我不能获得相对于地球的方向）。 我最终做的是当我的数据加载时，我运行了两个虚拟传感器。 我现在有一个平均四元数，都称为R （ TYPE_ROTATION_VECTOR ）和TYPE_GAME_ROTATION_VECTOR （ TYPE_GAME_ROTATION_VECTOR ）。

一旦校准完成，我只运行TYPE_GAME_ROTATION_VECTOR ，但想要更正为北。 我想我可以做的是这样的： R = R g * C其中C是我的校准， R g是低通滤波器后新的TYPE_GAME_ROTATION_VECTOR数据。 我试过的： 

 1. R = Rg * C 2. R * R' = Rg * C * R' 3. U = Rg * C * R' // Here U is the unit quaternion 4. C * R' = Rg' // This is because quaternion multiplication is associative // Rg * (C * R') = U from line 3 therefore (C * R') must be // equal to the conjugate of Rg 5. C = Rg' * R'' // I found this online somewhere (I hope this is right) 6. C = Rg' * R // R'' is just R

现在我已经有了C ，我可以在TYPE_GAME_ROTATION_VECTOR上乘以新的值（在低通滤波器之后）乘以C并得到实际的旋转四元数R ，该旋转四元数应该类似于TYPE_ROTATION_VECTOR提供的稳定的北。

这让我非常接近，但它不工作。 我正在测试使用一个非常简单的AR类似的应用程序，显示一个项目（谁的位置是由设备的方向确定）漂浮在屏幕上。 如果我忽略了校准，角色就会出现并完美地跟踪，但是它不会显示在我的北面（现在我已经固定在（0,1,0））。 如果我把旋转矢量，得到四元数，乘以校准常数，跟踪被抛弃：

  1. 围绕Y轴旋转设备会正确地将项目水平移动，但是也会增加一个垂直分量，在正向旋转（使用右手规则）将项目向上移动（屏幕上的负Y）。
  2. 围绕X轴旋转设备可以垂直移动项目，但是它也会增加一个水平分量，正向旋转（使用右手规则）将我的项目右移（屏幕上的正X）。
  3. 围绕Z轴旋转设备。

对不起，很长的描述，我只是想确保所有的细节都在那里。 问题总结：我想能够得到大致北方的旋转矩阵，并避免使用磁力计。 我试图通过获取TYPE_ROTATION_VECTOR和TYPE_GAME_ROTATION_VECTOR之间的平均差异，并使用它来“校准”来自TYPE_GAME_ROTATION_VECTOR未来值，但它不起作用。 有谁知道这个问题可能与我如何计算校准（或其他任何部分）有关？

一些额外的信息： 

 private float[] values = null public void onSensorChanged(SensorEvent event) { values = lowPass(event.values.clone(), values); Quaternion rawQuaternion = Quaternion.fromRotationVector(values); Quaternion calibratedQuaternion = rawQuaternion.mult(calibration); float[] rotationMatrix = calibratedQuaternion.getRotationMatrix(); float[] pos = new float[] { 0f, 1f, 0f, 1f }; Matrix.multiplyMV(pos, 0, rotationMatrix, 0, pos, 0); Matrix.multiplyMV(pos, 0, matrixMVP, 0, pos, 0); // Screen position should be found at pos[0], -pos[1] on a [-1,1] scale } Quaternion fromRotationVector(float[] r) { float[] Q = new float[4]; SensorManager.getQuaternionFromVector(Q, r); return new Quaternion(Q); } Quaternion mult(Quaternion q) { Quaternion qu = new Quaternion(); qu.w = w*qw - x*qx - y*qy - z*qz; qu.x = w*qx + x*qw + y*qz - z*qy; qu.y = w*qy + y*qw + z*qx - x*qz; qu.z = w*qz + z*qw + x*qy - y*qx; return qu; } float[] getRotationMatrix() { float[] M = new float[16]; float[] V = new float[] { x, y, z, w }; SensorManager.getRotationMatrixFromVector(M, V); return M; }

我有同样的问题，并做了一些研究，并认识到问题所在。 所以基本上只需要看IMU的一个固定的方向，你只需要在重力方向上对齐垂直轴坐标系的一个轴。 这就是为什么你绕Z轴旋转的原因。

要完成静态校准，必须包含一个平面运动，并找出运动的主要向量，例如X轴。 Y轴遵循右边的规则。

只需在全局X轴上旋转IMU，然后查看IMU的陀螺仪输出即可。 陀螺仪的主要部件应该朝向X轴。 在第一步找到Z轴，第二步找到X轴后，可以通过两者的叉积找到Y轴。 使用这些坐标轴，为翻译创建旋转矩阵或四元数。

这就是我最终做的事情（有一些变化即将到来，一旦完成，我将作为一个库发布在jcenter上）。 这个试图解决的问题是能够运行游戏旋转矢量传感器（它比漂移矢量传感器漂移少得多），同时仍然指向大致北方。 答案在Kotlin： 

 class RotationMatrixLiveData(context Context): LiveData<FloatArray>(), SensorEventListener { private val sensorManager = context.getSystemService(Context.SENSOR_SERVICE) as SensorManager private val rotationSensor = sensorManager.getDefaultSensor(Sensor.TYPE_ROTATION_VECTOR) private val gameRotationSensor = if (Build.VERSION.SDK_INT >= Build.VERSION_CODES.JELLY_BEAN_MR2) sensorManager.getDefaultSensor(Sensor.TYPE_GAME_ROTATION_VECTOR) else null private var isActive = false private var isCalibrating = false private var rotationValues: FloatArray? = null var calibrationCount = 0 var calibrationQuaternion: FloatArray? = null var calibrationGameCount = 0 var calibrationGameQuat: FloatArray? = null var calibration: Quaternion? = null var rotationQuaternionValues = FloatArray(4) var gameQuaternionValues = FloatArray(4) private val rotationVectorQuaternion = Quaternion() init { value = floatArrayOf( 1f, 0f, 0f, 0f, 0f, 1f, 0f, 0f, 0f, 0f, 1f, 0f, 0f, 0f, 0f, 1f) } /** * Starts calibrating the rotation matrix (if the game rotation vector sensor * is available. */ fun beginCalibration() { gameRotationSensor?.let { isCalibrating = true calibration = null calibrationQuaternion = null calibrationCount = 0 calibrationGameQuat = null calibrationGameCount = 0 sensorManager.registerListener(this, rotationSensor, SensorManager.SENSOR_DELAY_FASTEST) sensorManager.registerListener(this, it, SensorManager.SENSOR_DELAY_FASTEST) } } /** * Stop calibrating the rotation matrix. */ fun stopCalibration() { isCalibrating = false if (!isActive) { // Not active, just turn off everthing sensorManager.unregisterListener(this) } else if (gameRotationSensor != null) { // Active and has both sensors, turn off rotation and leave the game rotation running sensorManager.unregisterListener(this, rotationSensor) } } override fun onActive() { super.onActive() isActive = true val sensor = gameRotationSensor ?: rotationSensor sensorManager.registerListener(this, sensor, SensorManager.SENSOR_DELAY_FASTEST) } override fun onInactive() { super.onInactive() isActive = false if (!isCalibrating) { sensorManager.unregisterListener(this) } } // // SensorEventListener // override fun onAccuracyChanged(sensor: Sensor, accuracy: Int) {} override fun onSensorChanged(event: SensorEvent) { if (isCalibrating) { if (event.sensor.type == Sensor.TYPE_ROTATION_VECTOR) { SensorManager.getQuaternionFromVector(rotationQuaternionValues, event.values) calibrationQuaternion?.let { quat -> for (i in 0..3) { rotationQuaternionValues[i] += quat[i] } } calibrationQuaternion = rotationQuaternionValues calibrationCount++ } else if (event.sensor.type == Sensor.TYPE_GAME_ROTATION_VECTOR) { SensorManager.getQuaternionFromVector(gameQuaternionValues, event.values) calibrationGameQuat?.let {quat -> for (i in 0..3) { gameQuaternionValues[i] += quat[i] } } calibrationGameQuat = gameQuaternionValues calibrationGameCount++ } } else if (gameRotationSensor == null || event.sensor.type != Sensor.TYPE_ROTATION_VECTOR) { // Only calculate rotation if there is no game rotation sensor or if the event is a game // rotation val calibrationQ = calibrationQuaternion val calibrationQg = calibrationGameQuat if (calibrationQ != null && calibrationQg != null) { for (i in 0..3) { calibrationQ[i] /= calibrationCount.toFloat() calibrationQg[i] /= calibrationGameCount.toFloat() } calibration = (Quaternion(calibrationQg).apply { conjugate() } * Quaternion(calibrationQ)).apply { x = 0f y = 0f normalize() } } calibrationQuaternion = null calibrationGameQuat = null // Run values through low-pass filter val values = lowPass(event.values, rotationValues) rotationValues = values rotationVectorQuaternion.setFromRotationVector(values) // Calibrate if available calibration?.let { rotationVectorQuaternion.preMult(it) } // Generate rotation matrix value = rotationVectorQuaternion.getRotationMatrix(value) } } }

对于我正在使用的四元数类： 

 class Quaternion(val values: FloatArray = floatArrayOf(1f, 0f, 0f, 0f)) { companion object { fun fromRotationVector(rv: FloatArray): Quaternion { val Q = FloatArray(4) SensorManager.getQuaternionFromVector(Q, rv) return Quaternion(Q) } } private val buffer = FloatArray(4) var w: Float get() = values[0] set(value) { values[0] = value } var x: Float get() = values[1] set(value) { values[1] = value } var y: Float get() = values[2] set(value) { values[2] = value } var z: Float get() = values[3] set(value) { values[3] = value } fun setFromRotationVector(rv: FloatArray) { SensorManager.getQuaternionFromVector(values, rv) } fun conjugate() { x = -x y = -y z = -z } fun getRotationMatrix(R: FloatArray? = null): FloatArray { val matrix = R ?: FloatArray(16) for (i in 0..3) { buffer[i] = values[(i+1)%4] } SensorManager.getRotationMatrixFromVector(matrix, buffer) return matrix } fun magnitude(): Float { var mag = 0f for (i in 0..3) { mag += values[i]*values[i] } return Math.sqrt(mag.toDouble()).toFloat() } fun normalize() { val mag = magnitude() x /= mag y /= mag z /= mag w /= mag } fun preMult(left: Quaternion) { buffer[0] = left.w*this.w - left.x*this.x - left.y*this.y - left.z*this.z buffer[1] = left.w*this.x + left.x*this.w + left.y*this.z - left.z*this.y buffer[2] = left.w*this.y + left.y*this.w + left.z*this.x - left.x*this.z buffer[3] = left.w*this.z + left.z*this.w + left.x*this.y - left.y*this.x for (i in 0..3) { values[i] = buffer[i] } } operator fun times(q: Quaternion): Quaternion { val qu = Quaternion() qu.w = w*qw - x*qx - y*qy - z*qz qu.x = w*qx + x*qw + y*qz - z*qy qu.y = w*qy + y*qw + z*qx - x*qz qu.z = w*qz + z*qw + x*qy - y*qx return qu } operator fun times(v: FloatArray): FloatArray { val conj = Quaternion(values.clone()).apply { conjugate() } return multiplyQV(multiplyQV(values, v), conj.values) } override fun toString(): String { return "(${w.toString(5)}(w), ${x.toString(5)}, ${y.toString(5)}, ${z.toString(5)}) |${magnitude().toString(5)}|" } private fun multiplyQV(q: FloatArray, r: FloatArray): FloatArray { val result = FloatArray(4) result[0] = r[0]*q[0]-r[1]*q[1]-r[2]*q[2]-r[3]*q[3] result[1] = r[0]*q[1]+r[1]*q[0]-r[2]*q[3]+r[3]*q[2] result[2] = r[0]*q[2]+r[1]*q[3]+r[2]*q[0]-r[3]*q[1] result[3] = r[0]*q[3]-r[1]*q[2]+r[2]*q[1]+r[3]*q[0] return result } }
Interesting Posts