開(kāi)源一個(gè)自己做的小四旋翼工程，會(huì)有相關(guān)講解

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)矩陣的元素求出來(lái)給出的。

2.

vx = 2*(q1q3 - q0q2); /

vy = 2*(q0q1 + q2q3);

vz = q0q0 - q1q1 - q2q2 + q3q3 ;

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

3.

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

ax = ax /norm;

ay = ay / norm;

az = az / norm;

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

4.

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

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

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

接著可以通過(guò)叉乘（向量外積）計(jì)算誤差

5.

exInt = exInt + ex * Ki;

eyInt = eyInt + ey * Ki;

ezInt = ezInt + ez * Ki;

對(duì)誤差進(jìn)行積分

6.

gx = gx + Kp*ex + exInt;

gy = gy + Kp*ey + eyInt;

gz = gz + Kp*ez + ezInt;

進(jìn)行pi濾波，其實(shí)就是互補(bǔ)濾波

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;

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

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

下載附件

2010-12-14 22:54 上傳

這個(gè)跟四元數(shù)的微分方程對(duì)應(yīng)有興趣的看看書(shū)。。。。

8．

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

q0 = q0 / norm;

q1 = q1 / norm;

q2 = q2 / norm;

q3 = q3 / norm;

對(duì)四元數(shù)進(jìn)行規(guī)范化，即化為模長(zhǎng)為1 ，因?yàn)橹挥幸?guī)范化的四元數(shù)才能表示剛體旋轉(zhuǎn)。

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

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

補(bǔ)充，由于陀螺儀會(huì)有零點(diǎn)漂移開(kāi)始一定要進(jìn)行補(bǔ)償。這段是在mpu6050.c中程序，對(duì)直流偏執(zhí)進(jìn)行了補(bǔ)償。

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;

這里還要說(shuō)一點(diǎn)，這里加速計(jì)的數(shù)據(jù)用的是滑動(dòng)平均值濾波法，一定要有這個(gè)。。不然由于機(jī)械振動(dòng)造成的影響非常大。。。

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電機(jī)，720的轉(zhuǎn)速跟716的差不多，注意電機(jī)孔直徑一定要大一點(diǎn)。。不然塞不進(jìn)去。。然后補(bǔ)充一下MPU6050的擺放位置沒(méi)有關(guān)系。。同一坐標(biāo)系下測(cè)的的加速度角速度都是沒(méi)有關(guān)系的。。。

關(guān)于電源還有QFN的芯片。。。注意這個(gè)其實(shí)很好焊，用烙鐵沾點(diǎn)錫加點(diǎn)焊錫膏在對(duì)準(zhǔn)的引腳上拖焊就行。?？梢阅脗€(gè)燈照著看反光比較容易對(duì)準(zhǔn)。

關(guān)于軟件最關(guān)建的說(shuō)說(shuō)。。只有姿態(tài)解算部分。。PID部分我的算法還得改。。。。這個(gè)網(wǎng)上有開(kāi)源的就是串機(jī)PID。。。額。。本人菜鳥(niǎo)。。還沒(méi)看懂。。大二還沒(méi)學(xué)自控。?；厝?huì)看的。。。注意這里MPU最好用硬件IIc，因?yàn)樾∷妮S的姿態(tài)更新頻率是1000HZ比較快，這里的IIC只是一個(gè)器件。。目前還沒(méi)出什么問(wèn)題。
這里有一部分是我之前寫(xiě)的總結(jié)

（1）歐拉角法靜止?fàn)顟B(tài)，或者總加速度只是稍微大于g時(shí)，由加計(jì)算出的值比較準(zhǔn)確。

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

這個(gè)矩陣就是三個(gè)旋轉(zhuǎn)矩陣相乘得到的，因?yàn)榫仃嚨某朔梢员硎拘D(zhuǎn)。

這是程序

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; //對(duì)誤差進(jìn)行積分

eyInt = eyInt + ey * Ki;

ezInt = ezInt + ez * Ki;

// adjusted gyroscope measurements

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

gy = gy + Kp*ey + eyInt;

gz = gz + Kp*ez + ezInt; //這里的gz由于沒(méi)有觀測(cè)者進(jìn)行矯正會(huì)產(chǎn)生漂移，表現(xiàn)出來(lái)的就是積分自增或自減

// 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)矩陣的元素求出來(lái)給出的。

2.

vx = 2*(q1q3 - q0q2); /

vy = 2*(q0q1 + q2q3);

vz = q0q0 - q1q1 - q2q2 + q3q3 ;

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

3.

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

ax = ax /norm;

ay = ay / norm;

az = az / norm;

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

4.

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

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

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

接著可以通過(guò)叉乘（向量外積）計(jì)算誤差

5.

exInt = exInt + ex * Ki;

eyInt = eyInt + ey * Ki;

ezInt = ezInt + ez * Ki;

對(duì)誤差進(jìn)行積分

6.

gx = gx + Kp*ex + exInt;

gy = gy + Kp*ey + eyInt;

gz = gz + Kp*ez + ezInt;

進(jìn)行pi濾波，其實(shí)就是互補(bǔ)濾波

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;

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

這個(gè)跟四元數(shù)的微分方程對(duì)應(yīng)有興趣的看看書(shū)。。。。

8．

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

q0 = q0 / norm;

q1 = q1 / norm;

q2 = q2 / norm;

q3 = q3 / norm;

對(duì)四元數(shù)進(jìn)行規(guī)范化，即化為模長(zhǎng)為1 ，因?yàn)橹挥幸?guī)范化的四元數(shù)才能表示剛體旋轉(zhuǎn)。

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

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

補(bǔ)充，由于陀螺儀會(huì)有零點(diǎn)漂移開(kāi)始一定要進(jìn)行補(bǔ)償。這段是在mpu6050.c中程序，對(duì)直流偏執(zhí)進(jìn)行了補(bǔ)償。

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;

這里還要說(shuō)一點(diǎn)，這里加速計(jì)的數(shù)據(jù)用的是滑動(dòng)平均值濾波法，一定要有這個(gè)。。不然由于機(jī)械振動(dòng)造成的影響非常大。。。

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;

}