(轉)一種常見的四軸飛行器姿態解算方法分析


全國各地已經陸續開放低空管制,北京也將在2015年全面開放低空領域,這對低空飛行器將是一個十分重大的好消息!低空飛行器也將迎來一個新的發展春天。實際上,近年四軸飛行器發展相當迅速,國內的航拍水平越來越高,順豐及亞馬遜已在嘗試將無人機用於快遞行業。越來越多的人開始關注並研究四軸飛行器。

本文將分析一種常見的四軸飛行器姿態解算方法,Mahony的互補濾波法。此法簡單有效,希望能給學習四軸飛行器的朋友們帶來幫助。關於姿態解算和濾波的理論知識,推薦秦永元的兩本書,一是《慣性導航》,目前已出到第二版了;二是《卡爾曼濾波與組合導航原理》。程序中的理論基礎,可在書中尋找。

下面開始進入正題:

先定義Kp,Ki,以及halfT 。

Kp,Ki,控制加速度計修正陀螺儀積分姿態的速度

halfT ,姿態解算時間的一半。此處解算姿態速度為500HZ,因此halfT 為0.001

#define Kp 2.0f

#define Ki 0.002f

#define halfT 0.001f

初始化四元數

float q0 = 1, q1 = 0, q2 = 0, q3 = 0;

定義姿態解算誤差的積分

float exInt = 0, eyInt = 0, ezInt = 0;

以下為姿態解算函數。

參數gx,gy,gz分別對應三個軸的角速度,單位是弧度/秒;

參數ax,ay,az分別對應三個軸的加速度原始數據

由於加速度的噪聲較大,此處應采用濾波后的數據

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

{

float norm;

float vx, vy, vz;

float ex, ey, ez;

將加速度的原始數據,歸一化,得到單位加速度

norm = sqrt(ax*ax + ay*ay + az*az);

ax = ax / norm;

ay = ay / norm;

az = az / norm;

把四元數換算成“方向余弦矩陣”中的第三列的三個元素。根據余弦矩陣和歐拉角的定義,地理坐標系的重力向量,轉到機體坐標系,正好是這三個元素。所以這里的vx、vy、vz,其實就是當前的機體坐標參照系上,換算出來的重力單位向量。(用表示機體姿態的四元數進行換算)

vx = 2*(q1*q3 - q0*q2);

vy = 2*(q0*q1 + q2*q3);

vz = q0*q0 - q1*q1 - q2*q2 + q3*q3;

這里說明一點,加速度計由於噪聲比較大,而且在飛行過程中,受機體振動影響比陀螺儀明顯,短時間內的可靠性不高。陀螺儀噪聲小,但是由於積分是離散的,長時間的積分會出現漂移的情況,因此需要將用加速度計求得的姿態來矯正陀螺儀積分姿態的漂移。

在機體坐標參照系上,加速度計測出來的重力向量是ax、ay、az;陀螺積分后的姿態來推算出的重力向量是vx、vy、vz;它們之間的誤差向量,就是陀螺積分后的姿態和加速度計測出來的姿態之間的誤差。

向量間的誤差,可以用向量積(也叫外積、叉乘)來表示,ex、ey、ez就是兩個重力向量的叉積。這個叉積向量仍舊是位於機體坐標系上的,而陀螺積分誤差也是在機體坐標系,而且叉積的大小與陀螺積分誤差成正比,正好拿來糾正陀螺。由於陀螺是對機體直接積分,所以對陀螺的糾正量會直接體現在對機體坐標系的糾正。

叉乘是數學基礎,百度百科里有詳細解釋。

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;

用叉乘誤差來做PI修正陀螺零偏,通過調節Kp,Ki兩個參數,可以控制加速度計修正陀螺儀積分姿態的速度

gx = gx + Kp*ex + exInt;

gy = gy + Kp*ey + eyInt;

gz = gz + Kp*ez + ezInt;

四元數微分方程,沒啥好說的了,看上面推薦的書吧,都是理論的東西,自個琢磨琢磨

實在琢磨不明白,那就把指定的參數傳進這個函數,再得到相應的四元數,最后轉化成歐拉角即可了。不過建議還是把理論弄清楚一點。

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;

四元數單位化

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

q0 = q0 / norm;

q1 = q1 / norm;

q2 = q2 / norm;

q3 = q3 / norm;

}

姿態解算后,就得到了表示姿態的四元數。但四元數不夠直觀,一般將其轉化為歐拉角。轉化時根據旋轉軸的次序不同,公式也不同。以下給出其中一種公式:

1.png

讀完程序,深刻的意識到了理論基礎的重要性。Mahony的互補濾波函數,確實很巧妙,利用叉乘誤差來修正四軸的姿態,姿態解算速度越快,則解算的精度越高。在許多國內開源程序中,也是用到了這種方法。在解四元數微分方程時,該程序用到了一階畢卡解法。同樣可用於解四元數微分方程的還有龍格-庫塔法,由於篇幅有限,此處就不介紹龍格-庫塔法了,有興趣的網友請自行查閱相關資料。


免責聲明!

本站轉載的文章為個人學習借鑒使用,本站對版權不負任何法律責任。如果侵犯了您的隱私權益,請聯系本站郵箱yoyou2525@163.com刪除。



 
粵ICP備18138465號   © 2018-2025 CODEPRJ.COM