|
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 上傳
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 { 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部分我的算法還得改。。。。這個網(wǎng)上有開源的就是串機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由于沒有觀測者進行矯正會產(chǎn)生漂移，表現(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ù)值解法。。近似解。。一階解法
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; } |
|
|
repare_Data(void) 
