最近有时间也玩一下姿态解算。楼主看下我的理解正确么
Quaternion nowQ; //获得原始姿态的四元数 [a]
void loop() { //进入循环
int intx, inty,intz;
float pitch,roll,yaw;
gyro.ReadGyroOutCalibrated_Radian(&pitch, &roll, &yaw); //读取陀螺输出。并且校准（弧度）
EulerAngle dt; //获得dt。就是采样刷新time
dt.fRoll=roll; //计算roll的变化量 （gyrox*dt）
dt.fPitch=pitch; //计算pitch的变化量（gyroy*dt）
dt.fYaw=-yaw; //计算yaw的变化量（有反向，和安装方向有关）（gyroz*dt）
Quaternion dQ; //计算当前姿态的四元数
dQ.FromEulerAngle(dt); //把dt的欧拉转换为四元数
nowQ=nowQ.Multiply(dQ); //做四元数的乘法运算，得到新的四元数（nowQ=nowQ*dQ(是dt转换得到)）[a=a*b]
count++; //计数器++
if (count>1000){ //如果count大于1000则将nowQ四元转换为欧拉角
EulerAngle nowG=nowQ.ToEulerAngle();
Serial.print(nowG.fRoll/3.1415926535*180,11);//横滚
Serial.print(","); //将弧度转换为度，串口输出
Serial.print(nowG.fPitch/3.1415926535*180,11);//俯仰
Serial.print(",");
Serial.print(nowG.fYaw/3.1415926535*180,11);//偏航
Serial.print(",");
Serial.print(nowQ.getS(),11);//偏航
Serial.println();
count=0;
}
}
有点迷惑的是这里：
Quaternion dQ; //计算当前姿态的四元数
dQ.FromEulerAngle(dt); //把dt的欧拉转换为四元数
既然后面做四元乘法用的是a=a*b。那Quaternion dQ; //计算当前姿态的四元数
有什么用呢。
后面的计算就是将上一次的nowq作为原始姿态和更新的dt做乘法。这样就用三轴陀螺计算出姿态，如果要做acc的融合的话就另说了。
找到了一个算法：
//=====================================================================================================
// IMU.c
// S.O.H. Madgwick
// 25th September 2010
//=====================================================================================================
// Description:
//
// Quaternion implementation of the 'DCM filter' [Mayhony et al].
//
// User must define 'halfT' as the (sample period / 2), and the filter gains 'Kp' and 'Ki'.
//
// Global variables 'q0', 'q1', 'q2', 'q3' are the quaternion elements representing the estimated
// orientation. See my report for an overview of the use of quaternions in this application.
//
// User must call 'IMUupdate()' every sample period and parse calibrated gyroscope ('gx', 'gy', 'gz')
// and accelerometer ('ax', 'ay', 'ay') data. Gyroscope units are radians/second, accelerometer
// units are irrelevant as the vector is normalised.
//
//=====================================================================================================
//----------------------------------------------------------------------------------------------------
// Header files
#include "IMU.h"
#include <math.h>
//----------------------------------------------------------------------------------------------------
// Definitions
#define Kp 2.0f // proportional gain governs rate of convergence to accelerometer/magnetometer
#define Ki 0.005f // integral gain governs rate of convergence of gyroscope biases
#define halfT 0.5f // half the sample period
//---------------------------------------------------------------------------------------------------
// Variable definitions
float q0 = 1, q1 = 0, q2 = 0, q3 = 0; // quaternion elements representing the estimated orientation
float exInt = 0, eyInt = 0, ezInt = 0; // scaled integral error
//====================================================================================================
// Function
//====================================================================================================
void IMUupdate(float gx, float gy, float gz, float ax, float ay, float az) {
float norm;
float vx, vy, vz;
float ex, ey, ez;
// normalise the measurements
norm = sqrt(ax*ax + ay*ay + az*az);
ax = ax / norm;
ay = ay / norm;
az = az / norm;
// estimated direction of gravity
vx = 2*(q1*q3 - q0*q2);
vy = 2*(q0*q1 + q2*q3);
vz = q0*q0 - q1*q1 - q2*q2 + q3*q3;
// error is sum of cross product between reference direction of field and direction measured by sensor
ex = (ay*vz - az*vy);
ey = (az*vx - ax*vz);
ez = (ax*vy - ay*vx);
// integral error scaled integral gain
exInt = exInt + ex*Ki;
eyInt = eyInt + ey*Ki;
ezInt = ezInt + ez*Ki;
// adjusted gyroscope measurements
gx = gx + Kp*ex + exInt;
gy = gy + Kp*ey + eyInt;
gz = gz + Kp*ez + ezInt;
// integrate quaternion rate and normalise
q0 = q0 + (-q1*gx - q2*gy - q3*gz)*halfT;
q1 = q1 + (q0*gx + q2*gz - q3*gy)*halfT;
q2 = q2 + (q0*gy - q1*gz + q3*gx)*halfT;
q3 = q3 + (q0*gz + q1*gy - q2*gx)*halfT;
// normalise quaternion
norm = sqrt(q0*q0 + q1*q1 + q2*q2 + q3*q3);
q0 = q0 / norm;
q1 = q1 / norm;
q2 = q2 / norm;
q3 = q3 / norm;
}
//====================================================================================================
// END OF CODE
//====================================================================================================
这个四元貌似有acc的融合功能？？？请指教，谢谢
[ 本帖最后由 峰回路转 于 2012-1-28 02:38 编辑 ] |
/1 
楼主
发表于 2011-10-16 23:58
