陀螺问题与设备方向

我使用本教程获取设备陀螺仪的俯仰和滚动数据: http : //www.thousand-thoughts.com/2012/03/android-sensor-fusion-tutorial/

所有的读数都非常准确(在教程中有一个滤波器应用于代码,以消除陀螺漂移)。 不幸的是,代码仅在我的设备平放在与地面平行的表面上时才起作用。 我的应用程序工作的最理想的位置是设备的顶部朝上(即,设备垂直于地面,屏幕面向用户)。 每当我把我的装置定位在这个位置时,音调值就会变成+90度(如预期的那样)。 我想要做的就是将这个位置设置为我的设备的0度点(或者初始位置),这样当我的设备朝向竖直(纵向模式),屏幕面向用户时,音高读数为0度。

我问了这个教程的作者在这个问题上的帮助,他回答说:

“如果你想把直立的位置作为最初的位置,你将不得不旋转你的参照系,最简单的方法是将产生的旋转matrix围绕X轴旋转-90度,但是你必须一定要记住旋转不是交换操作,为了更具体的说明,我必须再次查看代码,因为我还没有用过一段时间现在。”

我真的很困惑和难住,如何旋转我的参考框架。 我想底线是,我不知道如何旋转matrix围绕X轴-90度。 如果有人能帮助我这个部分,这将是太棒了。 这里是我的代码,以防有人想要提及它:

public class AttitudeDisplayIndicator extends SherlockActivity implements SensorEventListener { private SensorManager mSensorManager = null; // angular speeds from gyro private float[] gyro = new float[3]; // rotation matrix from gyro data private float[] gyroMatrix = new float[9]; // orientation angles from gyro matrix private float[] gyroOrientation = new float[3]; // magnetic field vector private float[] magnet = new float[3]; // accelerometer vector private float[] accel = new float[3]; // orientation angles from accel and magnet private float[] accMagOrientation = new float[3]; // final orientation angles from sensor fusion private float[] fusedOrientation = new float[3]; // accelerometer and magnetometer based rotation matrix private float[] rotationMatrix = new float[9]; public static final float EPSILON = 0.000000001f; private static final float NS2S = 1.0f / 1000000000.0f; private float timestamp; private boolean initState = true; public static final int TIME_CONSTANT = 30; public static final float FILTER_COEFFICIENT = 0.98f; private Timer fuseTimer = new Timer(); // The following members are only for displaying the sensor output. public Handler mHandler; DecimalFormat d = new DecimalFormat("#.##"); //ADI background image. private ImageView adiBackground; //ADI axes. private ImageView adiAxes; //ADI frame. private ImageView adiFrame; //Layout. private RelativeLayout layout; //Pitch and Roll TextViews. private TextView pitchAngleText; private TextView bankAngleText; //Instantaneous output values from sensors as the device moves. public static double pitch; public static double roll; //Matrix for rotating the ADI (roll). Matrix mMatrix = new Matrix(); @Override public void onCreate(Bundle savedInstanceState) { super.onCreate(savedInstanceState); setContentView(R.layout.activity_attitude_display_indicator); gyroOrientation[0] = 0.0f; gyroOrientation[1] = 0.0f; gyroOrientation[2] = 0.0f; // initialise gyroMatrix with identity matrix gyroMatrix[0] = 1.0f; gyroMatrix[1] = 0.0f; gyroMatrix[2] = 0.0f; gyroMatrix[3] = 0.0f; gyroMatrix[4] = 1.0f; gyroMatrix[5] = 0.0f; gyroMatrix[6] = 0.0f; gyroMatrix[7] = 0.0f; gyroMatrix[8] = 1.0f; // get sensorManager and initialise sensor listeners mSensorManager = (SensorManager) this.getSystemService(SENSOR_SERVICE); initListeners(); // wait for one second until gyroscope and magnetometer/accelerometer // data is initialised then scedule the complementary filter task fuseTimer.scheduleAtFixedRate(new calculateFusedOrientationTask(), 1000, TIME_CONSTANT); mHandler = new Handler(); adiBackground = (ImageView) findViewById(R.id.adi_background); adiFrame = (ImageView) findViewById(R.id.adi_frame); adiAxes = (ImageView) findViewById(R.id.adi_axes); layout = (RelativeLayout) findViewById(R.id.adi_layout); new Color(); layout.setBackgroundColor(Color.rgb(150, 150, 150)); pitchAngleText = (TextView) findViewById(R.id.pitch_angle_text); bankAngleText = (TextView) findViewById(R.id.bank_angle_text); } // This function registers sensor listeners for the accelerometer, magnetometer and gyroscope. public void initListeners(){ mSensorManager.registerListener(this, mSensorManager.getDefaultSensor(Sensor.TYPE_ACCELEROMETER), SensorManager.SENSOR_DELAY_FASTEST); mSensorManager.registerListener(this, mSensorManager.getDefaultSensor(Sensor.TYPE_GYROSCOPE), SensorManager.SENSOR_DELAY_FASTEST); mSensorManager.registerListener(this, mSensorManager.getDefaultSensor(Sensor.TYPE_MAGNETIC_FIELD), SensorManager.SENSOR_DELAY_FASTEST); } //@Override public void onAccuracyChanged(Sensor sensor, int accuracy) { } //@Override public void onSensorChanged(SensorEvent event) { switch(event.sensor.getType()) { case Sensor.TYPE_ACCELEROMETER: // copy new accelerometer data into accel array and calculate orientation System.arraycopy(event.values, 0, accel, 0, 3); calculateAccMagOrientation(); break; case Sensor.TYPE_GYROSCOPE: // process gyro data gyroFunction(event); break; case Sensor.TYPE_MAGNETIC_FIELD: // copy new magnetometer data into magnet array System.arraycopy(event.values, 0, magnet, 0, 3); break; } } // calculates orientation angles from accelerometer and magnetometer output public void calculateAccMagOrientation() { if(SensorManager.getRotationMatrix(rotationMatrix, null, accel, magnet)) { SensorManager.getOrientation(rotationMatrix, accMagOrientation); } } // This function is borrowed from the Android reference // at http://developer.android.com/reference/android/hardware/SensorEvent.html#values // It calculates a rotation vector from the gyroscope angular speed values. private void getRotationVectorFromGyro(float[] gyroValues, float[] deltaRotationVector, float timeFactor) { float[] normValues = new float[3]; // Calculate the angular speed of the sample float omegaMagnitude = (float)Math.sqrt(gyroValues[0] * gyroValues[0] + gyroValues[1] * gyroValues[1] + gyroValues[2] * gyroValues[2]); // Normalize the rotation vector if it's big enough to get the axis if(omegaMagnitude > EPSILON) { normValues[0] = gyroValues[0] / omegaMagnitude; normValues[1] = gyroValues[1] / omegaMagnitude; normValues[2] = gyroValues[2] / omegaMagnitude; } // Integrate around this axis with the angular speed by the timestep // in order to get a delta rotation from this sample over the timestep // We will convert this axis-angle representation of the delta rotation // into a quaternion before turning it into the rotation matrix. float thetaOverTwo = omegaMagnitude * timeFactor; float sinThetaOverTwo = (float)Math.sin(thetaOverTwo); float cosThetaOverTwo = (float)Math.cos(thetaOverTwo); deltaRotationVector[0] = sinThetaOverTwo * normValues[0]; deltaRotationVector[1] = sinThetaOverTwo * normValues[1]; deltaRotationVector[2] = sinThetaOverTwo * normValues[2]; deltaRotationVector[3] = cosThetaOverTwo; } // This function performs the integration of the gyroscope data. // It writes the gyroscope based orientation into gyroOrientation. public void gyroFunction(SensorEvent event) { // don't start until first accelerometer/magnetometer orientation has been acquired if (accMagOrientation == null) return; // initialisation of the gyroscope based rotation matrix if(initState) { float[] initMatrix = new float[9]; initMatrix = getRotationMatrixFromOrientation(accMagOrientation); float[] test = new float[3]; SensorManager.getOrientation(initMatrix, test); gyroMatrix = matrixMultiplication(gyroMatrix, initMatrix); initState = false; } // copy the new gyro values into the gyro array // convert the raw gyro data into a rotation vector float[] deltaVector = new float[4]; if(timestamp != 0) { final float dT = (event.timestamp - timestamp) * NS2S; System.arraycopy(event.values, 0, gyro, 0, 3); getRotationVectorFromGyro(gyro, deltaVector, dT / 2.0f); } // measurement done, save current time for next interval timestamp = event.timestamp; // convert rotation vector into rotation matrix float[] deltaMatrix = new float[9]; SensorManager.getRotationMatrixFromVector(deltaMatrix, deltaVector); // apply the new rotation interval on the gyroscope based rotation matrix gyroMatrix = matrixMultiplication(gyroMatrix, deltaMatrix); // get the gyroscope based orientation from the rotation matrix SensorManager.getOrientation(gyroMatrix, gyroOrientation); } private float[] getRotationMatrixFromOrientation(float[] o) { float[] xM = new float[9]; float[] yM = new float[9]; float[] zM = new float[9]; float sinX = (float)Math.sin(o[1]); float cosX = (float)Math.cos(o[1]); float sinY = (float)Math.sin(o[2]); float cosY = (float)Math.cos(o[2]); float sinZ = (float)Math.sin(o[0]); float cosZ = (float)Math.cos(o[0]); // rotation about x-axis (pitch) xM[0] = 1.0f; xM[1] = 0.0f; xM[2] = 0.0f; xM[3] = 0.0f; xM[4] = cosX; xM[5] = sinX; xM[6] = 0.0f; xM[7] = -sinX; xM[8] = cosX; // rotation about y-axis (roll) yM[0] = cosY; yM[1] = 0.0f; yM[2] = sinY; yM[3] = 0.0f; yM[4] = 1.0f; yM[5] = 0.0f; yM[6] = -sinY; yM[7] = 0.0f; yM[8] = cosY; // rotation about z-axis (azimuth) zM[0] = cosZ; zM[1] = sinZ; zM[2] = 0.0f; zM[3] = -sinZ; zM[4] = cosZ; zM[5] = 0.0f; zM[6] = 0.0f; zM[7] = 0.0f; zM[8] = 1.0f; // rotation order is y, x, z (roll, pitch, azimuth) float[] resultMatrix = matrixMultiplication(xM, yM); resultMatrix = matrixMultiplication(zM, resultMatrix); return resultMatrix; } private float[] matrixMultiplication(float[] A, float[] B) { float[] result = new float[9]; result[0] = A[0] * B[0] + A[1] * B[3] + A[2] * B[6]; result[1] = A[0] * B[1] + A[1] * B[4] + A[2] * B[7]; result[2] = A[0] * B[2] + A[1] * B[5] + A[2] * B[8]; result[3] = A[3] * B[0] + A[4] * B[3] + A[5] * B[6]; result[4] = A[3] * B[1] + A[4] * B[4] + A[5] * B[7]; result[5] = A[3] * B[2] + A[4] * B[5] + A[5] * B[8]; result[6] = A[6] * B[0] + A[7] * B[3] + A[8] * B[6]; result[7] = A[6] * B[1] + A[7] * B[4] + A[8] * B[7]; result[8] = A[6] * B[2] + A[7] * B[5] + A[8] * B[8]; return result; } class calculateFusedOrientationTask extends TimerTask { public void run() { float oneMinusCoeff = 1.0f - FILTER_COEFFICIENT; /* * Fix for 179° <--> -179° transition problem: * Check whether one of the two orientation angles (gyro or accMag) is negative while the other one is positive. * If so, add 360° (2 * math.PI) to the negative value, perform the sensor fusion, and remove the 360° from the result * if it is greater than 180°. This stabilizes the output in positive-to-negative-transition cases. */ // azimuth if (gyroOrientation[0] < -0.5 * Math.PI && accMagOrientation[0] > 0.0) { fusedOrientation[0] = (float) (FILTER_COEFFICIENT * (gyroOrientation[0] + 2.0 * Math.PI) + oneMinusCoeff * accMagOrientation[0]); fusedOrientation[0] -= (fusedOrientation[0] > Math.PI) ? 2.0 * Math.PI : 0; } else if (accMagOrientation[0] < -0.5 * Math.PI && gyroOrientation[0] > 0.0) { fusedOrientation[0] = (float) (FILTER_COEFFICIENT * gyroOrientation[0] + oneMinusCoeff * (accMagOrientation[0] + 2.0 * Math.PI)); fusedOrientation[0] -= (fusedOrientation[0] > Math.PI)? 2.0 * Math.PI : 0; } else { fusedOrientation[0] = FILTER_COEFFICIENT * gyroOrientation[0] + oneMinusCoeff * accMagOrientation[0]; } // pitch if (gyroOrientation[1] < -0.5 * Math.PI && accMagOrientation[1] > 0.0) { fusedOrientation[1] = (float) (FILTER_COEFFICIENT * (gyroOrientation[1] + 2.0 * Math.PI) + oneMinusCoeff * accMagOrientation[1]); fusedOrientation[1] -= (fusedOrientation[1] > Math.PI) ? 2.0 * Math.PI : 0; } else if (accMagOrientation[1] < -0.5 * Math.PI && gyroOrientation[1] > 0.0) { fusedOrientation[1] = (float) (FILTER_COEFFICIENT * gyroOrientation[1] + oneMinusCoeff * (accMagOrientation[1] + 2.0 * Math.PI)); fusedOrientation[1] -= (fusedOrientation[1] > Math.PI)? 2.0 * Math.PI : 0; } else { fusedOrientation[1] = FILTER_COEFFICIENT * gyroOrientation[1] + oneMinusCoeff * accMagOrientation[1]; } // roll if (gyroOrientation[2] < -0.5 * Math.PI && accMagOrientation[2] > 0.0) { fusedOrientation[2] = (float) (FILTER_COEFFICIENT * (gyroOrientation[2] + 2.0 * Math.PI) + oneMinusCoeff * accMagOrientation[2]); fusedOrientation[2] -= (fusedOrientation[2] > Math.PI) ? 2.0 * Math.PI : 0; } else if (accMagOrientation[2] < -0.5 * Math.PI && gyroOrientation[2] > 0.0) { fusedOrientation[2] = (float) (FILTER_COEFFICIENT * gyroOrientation[2] + oneMinusCoeff * (accMagOrientation[2] + 2.0 * Math.PI)); fusedOrientation[2] -= (fusedOrientation[2] > Math.PI)? 2.0 * Math.PI : 0; } else { fusedOrientation[2] = FILTER_COEFFICIENT * gyroOrientation[2] + oneMinusCoeff * accMagOrientation[2]; } // overwrite gyro matrix and orientation with fused orientation // to comensate gyro drift gyroMatrix = getRotationMatrixFromOrientation(fusedOrientation); System.arraycopy(fusedOrientation, 0, gyroOrientation, 0, 3); // update sensor output in GUI mHandler.post(updateOrientationDisplayTask); } } 

在此先感谢您的帮助!

Solutions Collecting From Web of "陀螺问题与设备方向"

理论…

我并不确定表示“参考系”matrix的格式,但通常使用matrix乘法进行旋转。

基本上,你会把你的“参考matrix框架”,并乘以一个90度的旋转matrix。

这种matrix可以在维基百科上find: 三维旋转matrix

由于你的angular度是90度,所以你的正弦和余弦将解决为1或0,你可以直接插入matrix,而不是计算正弦和余弦。 例如,一个围绕x轴逆时针旋转90度的matrix看起来像这样:

 1 0 0 0 0 1 0 -1 0 

另外,请不要这样的matrix对xyz坐标的行向量进行操作。 例如,如果你的空间点在(2,5,7)处,而你想用上面的matrix来旋转它,那么你必须执行下面的操作:

 |2 5 7| |1 0 0| |0 0 1| |0 -1 0| 

这给了[2 -7 5]

…应用于您的代码

我已经快速浏览了你的代码,看起来你需要做的修改涉及calculateAccMagOrientation()的输出,因为它用于初始化设备的方向。

 1: public void calculateAccMagOrientation() { 2: if(SensorManager.getRotationMatrix(rotationMatrix, null, accel, magnet)) { 3: SensorManager.getOrientation(rotationMatrix, accMagOrientation); 4: } 5: } 

在上面的代码片段的第2行是你得到你的初始rotationMatrix的地方。 尝试在第3行调用getOrientation之前将rotationMatrix乘以手工制作的90度旋转matrix。我认为这将有效地重新调整您的参考方向:

 public void calculateAccMagOrientation() { if(SensorManager.getRotationMatrix(rotationMatrix, null, accel, magnet)) { rotationMatrix = matrixMultiplication(rotationMatrix, my90DegRotationMatrix); SensorManager.getOrientation(rotationMatrix, accMagOrientation); } } 

请注意,根据Android中angular度的工作方式,您可能需要使用90度顺时针旋转matrix,而不是逆时针旋转matrix。

替代scheme

刚才我想到,也许你也可以简单地从最后的音调结果中减去90,然后再显示它?