如何从旋转矩阵计算偏航,俯仰滚动?

时间:2012-10-17 12:06:57

标签: orientation gyroscope pitch rotational-matrices

我必须使用陀螺仪的智能手机输出计算Android偏航,滚动和俯仰,我写了这段代码:

如果(event.sensor.getType()== Sensor.TYPE_GYROSCOPE){

        float xgyr=event.values[0];                //rotation around x-axis  [rad/sec]
        float ygyr=event.values[1];                // rotation around y-axis
        float zgyr=event.values[2];               // rotation around z-axis


        // This timestep's delta rotation to be multiplied by the current rotation
         // after computing it from the gyro sample data.

        double EPSILON = 0.0;           //EPSILON value to be defined
         if (timestamp != 0) {
             final float dT = (event.timestamp - timestamp) * NS2S;
             // Axis of the rotation sample, not normalized yet.
             float axisX = event.values[0];
             float axisY = event.values[1];
             float axisZ = event.values[2];

             // Calculate the angular speed of the sample, teta is the vector length 
             float omegaMagnitude = (float) Math.sqrt(axisX*axisX + axisY*axisY + axisZ*axisZ);

             // Normalize the rotation vector if it's big enough to get the axis
             if (omegaMagnitude > EPSILON) {                  //EPSILON TO BE DEFINED
                 axisX /= omegaMagnitude;
                 axisY /= omegaMagnitude;
                 axisZ /= omegaMagnitude;
             }

您好,我必须使用陀螺仪的输出来计算偏航滚动和俯仰,我写了这段代码:

             float thetaOverTwo = omegaMagnitude * dT / 2.0f;      //Insert initial value for orientation omegaMagnitude
             float sinThetaOverTwo = (float) Math.sin(thetaOverTwo);
             float cosThetaOverTwo = (float) Math.cos(thetaOverTwo);

             /*rotation vector, a non-normalized three-dimensional vector the direction of which specifies the rotation axis,
             and the length of which is teta,    Combining two consecutive quaternion rotations is therefore just as simple as using the rotation matrix. 
             Remember that two successive rotation matrices, A1 , A2 are combined A3 = A2*A1*/ 

             //Quaternions
             deltaRotationVector[0] = sinThetaOverTwo * axisX;
             deltaRotationVector[1] = sinThetaOverTwo * axisY;
             deltaRotationVector[2] = sinThetaOverTwo * axisZ;
             deltaRotationVector[3] = cosThetaOverTwo;
         }
         timestamp = event.timestamp;
         float[] deltaRotationMatrix = new float[9];
         SensorManager.getRotationMatrixFromVector(deltaRotationMatrix, deltaRotationVector);
         // User code should concatenate the delta rotation we computed with the current rotation
         // in order to get the updated rotation.
         // rotationCurrent = rotationCurrent * deltaRotationMatrix; The initial rotation has to be computedand then at each step updated
         /*Rotation current is a vector with rotation on pitch, roll, yaw (3x1) multiplied by 3x3 rotation matrix i have the new orientation*/

         /*In the "xyz (pitch-roll-yaw) convention," theta is pitch, psi is roll, and phi is yaw. */




         double pitch = Math.atan2(deltaRotationMatrix[6], deltaRotationMatrix[7]);
         double roll = Math.acos(deltaRotationMatrix[8]);
         double yaw = - Math.atan2(deltaRotationMatrix[2], deltaRotationMatrix[5]);

我不知道问题出在哪里,但是我使用此代码获得的偏航,滚动,俯仰值是错误的,因为即使手机处于相同的方向,我也获得了不同的值。我从最后三行代码中的旋转矩阵计算它们。并且在公式中写的偏航,俯仰滚动的值是弧度?

1 个答案:

答案 0 :(得分:2)

我在Android上的经验有限,但根据参考手册,您可以从SensorManager.getOrientation (...)获取这些值而无需任何计算。如果这是正确的,我建议使用它而不是任何自行计算,因为由于传感器融合算法在引擎盖下工作,因此值应该更精确。