開源一個自己做的小四旋翼工程，會有相關講解

1.

float q0q0 = q0*q0;

float q0q1 = q0*q1;

float q0q2 = q0*q2;

// float q0q3 = q0*q3;

float q1q1 = q1*q1;

// float q1q2 = q1*q2;

float q1q3 = q1*q3;

float q2q2 = q2*q2;

float q2q3 = q2*q3;

float q3q3 = q3*q3;

這段程序就是為了把需要用到的姿態(tài)矩陣的元素求出來給出的。

2.

vx = 2*(q1q3 - q0q2); /

vy = 2*(q0q1 + q2q3);

vz = q0q0 - q1q1 - q2q2 + q3q3 ;

可以看到vx，vy，vz為CRb的最后一列的三項，四元數(shù)矩陣帶入（1）式得vx，vy，vz分別是axB，ayB，azB每一項g前的系數(shù)。且靜止情況下vx，vy，vz組成向量模長基本可以認為為1.

3.

norm = sqrt(ax*ax + ay*ay + az*az); //acc數(shù)據(jù)歸一化

ax = ax /norm;

ay = ay / norm;

az = az / norm;

以上已說，由四元數(shù)倒推回去的加速度，向量模長為1，為了比較誤差進行歸1化，算的由加計得出的向量。

4.

ex = (ay*vz - az*vy) ;

ey = (az*vx - ax*vz) ;

ez = (ax*vy - ay*vx) ;

接著可以通過叉乘（向量外積）計算誤差

5.

exInt = exInt + ex * Ki;

eyInt = eyInt + ey * Ki;

ezInt = ezInt + ez * Ki;

對誤差進行積分

6.

gx = gx + Kp*ex + exInt;

gy = gy + Kp*ey + eyInt;

gz = gz + Kp*ez + ezInt;

進行pi濾波，其實就是互補濾波

7．

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;

龍格庫塔法。。。就是方程的數(shù)值解法。。近似解。。一階解法

0736fac7228d4220f912874ee8cee5e5_21.png (0 Bytes, 下載次數(shù): 0)

下載附件

2010-12-14 22:54 上傳

這個跟四元數(shù)的微分方程對應有興趣的看看書。。。。

8．

norm = sqrt(q0*q0 + q1*q1 + q2*q2 + q3*q3);

q0 = q0 / norm;

q1 = q1 / norm;

q2 = q2 / norm;

q3 = q3 / norm;

對四元數(shù)進行規(guī)范化，即化為模長為1 ，因為只有規(guī)范化的四元數(shù)才能表示剛體旋轉。

9．

Q_ANGLE.Y = asin(-2 * q1 * q3 + 2 * q0* q2)* 57.3; // pitch

Q_ANGLE.X = atan2(2 * q2 * q3 + 2 * q0 * q1, -2 * q1 * q1 - 2 * q2* q2 + 1)* 57.3; // roll

仍舊一一對應關系發(fā)現(xiàn)2（q1q3 -q0q2）剛好跟歐拉角對應，由此利用自帶庫函數(shù)即可求得俯仰角，橫滾角類似，偏航角由于沒有羅盤進行校正求沒有意義，控制中采用采用PD控制。

補充，由于陀螺儀會有零點漂移開始一定要進行補償。這段是在mpu6050.c中程序，對直流偏執(zhí)進行了補償。

MPU6050_ACC_LAST.X=((((int16_t)mpu6050_buffer[0]) << 8) | mpu6050_buffer[1]) - ACC_OFFSET.X;

MPU6050_ACC_LAST.Y=((((int16_t)mpu6050_buffer[2]) << 8) | mpu6050_buffer[3]) - ACC_OFFSET.Y;

MPU6050_ACC_LAST.Z=((((int16_t)mpu6050_buffer[4]) << 8) | mpu6050_buffer[5]) - ACC_OFFSET.Z;

MPU6050_GYRO_LAST.X=((((int16_t)mpu6050_buffer[8]) << 8) | mpu6050_buffer[9]) - GYRO_OFFSET.X;

MPU6050_GYRO_LAST.Y=((((int16_t)mpu6050_buffer[10]) << 8) | mpu6050_buffer[11]) - GYRO_OFFSET.Y;

MPU6050_GYRO_LAST.Z=((((int16_t)mpu6050_buffer[12]) << 8) | mpu6050_buffer[13]) - GYRO_OFFSET.Z;

這里還要說一點，這里加速計的數(shù)據(jù)用的是滑動平均值濾波法，一定要有這個。。不然由于機械振動造成的影響非常大。。。

void repare_Data(void)

{

static uint8_t filter_cnt=0;

int32_t temp1=0,temp2=0,temp3=0;

uint8_t i;

MPU6050_Read();

MPU6050_Dataanl();

ACC_X_BUF[filter_cnt] = MPU6050_ACC_LAST.X;

ACC_Y_BUF[filter_cnt] = MPU6050_ACC_LAST.Y;

ACC_Z_BUF[filter_cnt] = MPU6050_ACC_LAST.Z;

for(i=0;i<FILTER_NUM;i++)

{

temp1 += ACC_X_BUF;

temp2 += ACC_Y_BUF;

temp3 += ACC_Z_BUF;

}

選擇716電機，720的轉速跟716的差不多，注意電機孔直徑一定要大一點。。不然塞不進去。。然后補充一下MPU6050的擺放位置沒有關系。。同一坐標系下測的的加速度角速度都是沒有關系的。。。

關于電源還有QFN的芯片。。。注意這個其實很好焊，用烙鐵沾點錫加點焊錫膏在對準的引腳上拖焊就行。?？梢阅脗€燈照著看反光比較容易對準。

關于軟件最關建的說說。。只有姿態(tài)解算部分。。PID部分我的算法還得改。。。。這個網上有開源的就是串機PID。。。額。。本人菜鳥。。還沒看懂。。大二還沒學自控。?；厝吹摹?。。注意這里MPU最好用硬件IIc，因為小四軸的姿態(tài)更新頻率是1000HZ比較快，這里的IIC只是一個器件。。目前還沒出什么問題。
這里有一部分是我之前寫的總結

（1）歐拉角法靜止狀態(tài)，或者總加速度只是稍微大于g時，由加計算出的值比較準確。

使用歐拉角表示姿態(tài)，令Φ,θ和Φ代表ZYX 歐拉角，分別稱為偏航角、俯仰角和橫滾角 。 載體坐標系下的 加 速 度(axB,ayB,azB)和參考坐標系下的加速度(axN, ayN, azN)之間的關系可表示為(1)。其中 c 和 s 分別代表 cos 和 sin。axB,ayB,azB就是mpu讀出來的三個值。

這個矩陣就是三個旋轉矩陣相乘得到的，因為矩陣的乘法可以表示旋轉。

這是程序

void IMUupdate(float gx, float gy, float gz, float ax, float ay, float az)

{

float norm;

// float hx, hy, hz, bx, bz;

float vx, vy, vz;// wx, wy, wz;

float ex, ey, ez;

// 先把這些用得到的值算好

float q0q0 = q0*q0;

float q0q1 = q0*q1;

float q0q2 = q0*q2;

// float q0q3 = q0*q3;

float q1q1 = q1*q1;

// float q1q2 = q1*q2;

float q1q3 = q1*q3;

float q2q2 = q2*q2;

float q2q3 = q2*q3;

float q3q3 = q3*q3;

if(ax*ay*az==0)

return;

norm = sqrt(ax*ax + ay*ay + az*az); //acc數(shù)據(jù)歸一化

ax = ax /norm;

ay = ay / norm;

az = az / norm;

// estimated direction of gravity and flux (v and w)

vx = 2*(q1q3 - q0q2); //四元素中xyz的 vy = 2*(q0q1 + q2q3);

vz = q0q0 - q1q1 - q2q2 + q3q3 ;

// error is sum of cross product between reference direction of fields and direction measured by sensors

ex = (ay*vz - az*vy) ; //向量外積在相減得到差分就是誤差

ey = (az*vx - ax*vz) ;

ez = (ax*vy - ay*vx) ;

exInt = exInt + ex * Ki; //對誤差進行積分

eyInt = eyInt + ey * Ki;

ezInt = ezInt + ez * Ki;

// adjusted gyroscope measurements

gx = gx + Kp*ex + exInt; //將誤差PI后補償?shù)酵勇輧x，即補償零點漂移

gy = gy + Kp*ey + eyInt;

gz = gz + Kp*ez + ezInt; //這里的gz由于沒有觀測者進行矯正會產生漂移，表現(xiàn)出來的就是積分自增或自減

// 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;

//Q_ANGLE.Yaw = atan2(2 * q1 * q2 + 2 * q0 * q3, -2 * q2*q2 - 2 * q3* q3 + 1)* 57.3; // yaw

Q_ANGLE.Y = asin(-2 * q1 * q3 + 2 * q0* q2)* 57.3; // pitch

Q_ANGLE.X = atan2(2 * q2 * q3 + 2 * q0 * q1, -2 * q1 * q1 - 2 * q2* q2 + 1)* 57.3; // roll

}

1.

float q0q0 = q0*q0;

float q0q1 = q0*q1;

float q0q2 = q0*q2;

// float q0q3 = q0*q3;

float q1q1 = q1*q1;

// float q1q2 = q1*q2;

float q1q3 = q1*q3;

float q2q2 = q2*q2;

float q2q3 = q2*q3;

float q3q3 = q3*q3;

這段程序就是為了把需要用到的姿態(tài)矩陣的元素求出來給出的。

2.

vx = 2*(q1q3 - q0q2); /

vy = 2*(q0q1 + q2q3);

vz = q0q0 - q1q1 - q2q2 + q3q3 ;

可以看到vx，vy，vz為CRb的最后一列的三項，四元數(shù)矩陣帶入（1）式得vx，vy，vz分別是axB，ayB，azB每一項g前的系數(shù)。且靜止情況下vx，vy，vz組成向量模長基本可以認為為1.

3.

norm = sqrt(ax*ax + ay*ay + az*az); //acc數(shù)據(jù)歸一化

ax = ax /norm;

ay = ay / norm;

az = az / norm;

以上已說，由四元數(shù)倒推回去的加速度，向量模長為1，為了比較誤差進行歸1化，算的由加計得出的向量。

4.

ex = (ay*vz - az*vy) ;

ey = (az*vx - ax*vz) ;

ez = (ax*vy - ay*vx) ;

接著可以通過叉乘（向量外積）計算誤差

5.

exInt = exInt + ex * Ki;

eyInt = eyInt + ey * Ki;

ezInt = ezInt + ez * Ki;

對誤差進行積分

6.

gx = gx + Kp*ex + exInt;

gy = gy + Kp*ey + eyInt;

gz = gz + Kp*ez + ezInt;

進行pi濾波，其實就是互補濾波

7．

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;

龍格庫塔法。。。就是方程的數(shù)值解法。。近似解。。一階解法

這個跟四元數(shù)的微分方程對應有興趣的看看書。。。。

8．

norm = sqrt(q0*q0 + q1*q1 + q2*q2 + q3*q3);

q0 = q0 / norm;

q1 = q1 / norm;

q2 = q2 / norm;

q3 = q3 / norm;

對四元數(shù)進行規(guī)范化，即化為模長為1 ，因為只有規(guī)范化的四元數(shù)才能表示剛體旋轉。

9．

Q_ANGLE.Y = asin(-2 * q1 * q3 + 2 * q0* q2)* 57.3; // pitch

Q_ANGLE.X = atan2(2 * q2 * q3 + 2 * q0 * q1, -2 * q1 * q1 - 2 * q2* q2 + 1)* 57.3; // roll

仍舊一一對應關系發(fā)現(xiàn)2（q1q3 -q0q2）剛好跟歐拉角對應，由此利用自帶庫函數(shù)即可求得俯仰角，橫滾角類似，偏航角由于沒有羅盤進行校正求沒有意義，控制中采用采用PD控制。

補充，由于陀螺儀會有零點漂移開始一定要進行補償。這段是在mpu6050.c中程序，對直流偏執(zhí)進行了補償。

MPU6050_ACC_LAST.X=((((int16_t)mpu6050_buffer[0]) << 8) | mpu6050_buffer[1]) - ACC_OFFSET.X;

MPU6050_ACC_LAST.Y=((((int16_t)mpu6050_buffer[2]) << 8) | mpu6050_buffer[3]) - ACC_OFFSET.Y;

MPU6050_ACC_LAST.Z=((((int16_t)mpu6050_buffer[4]) << 8) | mpu6050_buffer[5]) - ACC_OFFSET.Z;

MPU6050_GYRO_LAST.X=((((int16_t)mpu6050_buffer[8]) << 8) | mpu6050_buffer[9]) - GYRO_OFFSET.X;

MPU6050_GYRO_LAST.Y=((((int16_t)mpu6050_buffer[10]) << 8) | mpu6050_buffer[11]) - GYRO_OFFSET.Y;

MPU6050_GYRO_LAST.Z=((((int16_t)mpu6050_buffer[12]) << 8) | mpu6050_buffer[13]) - GYRO_OFFSET.Z;

這里還要說一點，這里加速計的數(shù)據(jù)用的是滑動平均值濾波法，一定要有這個。。不然由于機械振動造成的影響非常大。。。

void repare_Data(void)

{

static uint8_t filter_cnt=0;

int32_t temp1=0,temp2=0,temp3=0;

uint8_t i;

MPU6050_Read();

MPU6050_Dataanl();

ACC_X_BUF[filter_cnt] = MPU6050_ACC_LAST.X;

ACC_Y_BUF[filter_cnt] = MPU6050_ACC_LAST.Y;

ACC_Z_BUF[filter_cnt] = MPU6050_ACC_LAST.Z;

for(i=0;i<FILTER_NUM;i++)

{

temp1 += ACC_X_BUF;

temp2 += ACC_Y_BUF;

temp3 += ACC_Z_BUF;

}