1.引言

四軸飛行器是一種利用四個(gè)旋翼作為飛行引擎來(lái)進(jìn)行空中飛行的飛行器。進(jìn)入20世紀(jì)以來(lái)，電子技術(shù)飛速發(fā)展四軸飛行器開始走向小型化，并融入了人工智能，使其發(fā)展趨于無(wú)人機(jī)，智能機(jī)器人。

四軸飛行器不但實(shí)現(xiàn)了直升機(jī)的垂直升降的飛行性能，同時(shí)也在一定程度上降低了飛行器機(jī)械結(jié)構(gòu)的設(shè)計(jì)難度。四軸飛行器的平衡控制系統(tǒng)由各類慣性傳感器組成。在制作過(guò)程中，對(duì)整體機(jī)身的中心、對(duì)稱性以及電機(jī)性能要求較低，這也正是制作四軸飛行器的優(yōu)勢(shì)所在，而且相較于固定翼飛機(jī)，四軸也有著可垂直起降，機(jī)動(dòng)性好，易維護(hù)等優(yōu)點(diǎn)。

在實(shí)際應(yīng)用方面，四軸飛行器可以在復(fù)雜、危險(xiǎn)的環(huán)境下可以完成特定的飛行任務(wù)，也可以用于監(jiān)控交通，環(huán)境等。比如，在四軸飛行器上安裝甲烷等有害氣體的檢測(cè)裝置，則可以在高空定點(diǎn)地檢測(cè)有害氣體；進(jìn)入輻射區(qū)檢查核設(shè)施；做軍事偵察；甚至搬運(yùn)材料，搭建房屋等等。本設(shè)計(jì)利用四軸搭載云臺(tái)實(shí)現(xiàn)航拍任務(wù)，當(dāng)然經(jīng)過(guò)改裝也可以執(zhí)行其他任務(wù)。

本設(shè)計(jì)主要研究了四軸飛行器的姿態(tài)結(jié)算和飛行控制，并設(shè)計(jì)制作了一架四軸飛行器，對(duì)關(guān)鍵傳感器做了標(biāo)定，并利用用matlab分析數(shù)據(jù)，設(shè)計(jì)算法，還進(jìn)行了單通道平衡試驗(yàn)調(diào)試，進(jìn)行試飛實(shí)驗(yàn)取得了一定的效果。

2.系統(tǒng)方案

本設(shè)計(jì)采用STM32F4作為核心處理器，該處理器內(nèi)核架構(gòu) ARM Cortex-M4，具有高性能、低成本、低功耗等特點(diǎn)。

主控板包括傳感器MPU6050電路模塊、無(wú)線藍(lán)牙模塊、電機(jī)啟動(dòng)模塊，電源管理模塊等；遙控使用商品遙控及接收機(jī)?？刂菩酒东@接收機(jī)的PPM命令信號(hào)，傳感器與控制芯片之間采用IIC總線連接，MCU與電調(diào)之間用PWM傳遞控制信號(hào)。

軟件算法才用基于四元數(shù)的互補(bǔ)濾波解算姿態(tài)叫，控制算法才用經(jīng)典PID控制器控制云臺(tái)舵機(jī)和四軸電機(jī)。如圖2-1為本設(shè)計(jì)總體框圖。

3.系統(tǒng)硬件設(shè)計(jì)

針對(duì)前面提出的整體設(shè)計(jì)方案，本設(shè)計(jì)采取模塊化策略,將各個(gè)功能部分開來(lái)設(shè)計(jì)，最后組合起來(lái)。

3.1 電源管理模塊

四軸飛行器要求整體設(shè)計(jì)質(zhì)量較輕，體積較小，因此在電池的選取方面，采用體積小、質(zhì)量輕、容量大的鋰電池供電最合適。系統(tǒng)的核心芯片為STM32F103，常用工作電壓為3.3V，同時(shí)慣性測(cè)量傳感器，藍(lán)牙通信模塊的常規(guī)供電電壓也為3.3V，鋰電池的電壓為11.4V，要使系統(tǒng)正常工作，需要將11.4V的鋰電池電壓穩(wěn)壓到3.3V。常用的78系列穩(wěn)壓芯片已不再適用，必須選擇性能更好的穩(wěn)壓芯片。

經(jīng)綜合考慮，本電路采用LM1117-3.3和LM2940-5電源部分的核心芯片。電池電源經(jīng)過(guò)LM2940-5降到5V后在輸入LM1117-3.3穩(wěn)壓為3.3V。由于電機(jī)部分電流較大，故在飛控電路部分加入了過(guò)流保護(hù)，使用500mA的保險(xiǎn)絲。電路圖如下。

表3-1 四軸飛行器硬件清單

4.系統(tǒng)軟件設(shè)計(jì)

軟件設(shè)計(jì)上由控制核心STM32F4讀取傳感器信息，解算姿態(tài)角，以姿態(tài)角為被控制量融合遙控信息后，輸出到四個(gè)電機(jī)及兩個(gè)舵機(jī)以完成四軸飛行控制和云臺(tái)的穩(wěn)定補(bǔ)償。下圖是軟件流程：

4.1.四元素計(jì)算姿態(tài)角的實(shí)現(xiàn)

根據(jù)前面給出的姿態(tài)解算方程與四元數(shù)，即可得到姿態(tài)計(jì)算系統(tǒng)的計(jì)算原理如下圖（4-1）

本設(shè)計(jì)基于互補(bǔ)濾波的思想上完成的四元素算法，其核心思路為利用加速度測(cè)得的重力向量與估計(jì)姿態(tài)得到重力向量的誤差來(lái)矯正陀螺儀積分誤差，然后利用矯正后的陀螺儀積分得到姿態(tài)角。

首先不妨設(shè)處理后的加速度數(shù)據(jù)為：ax,ay,az，單位m/s^2。加速度計(jì)的向量為 （ax,ay,az）陀螺儀數(shù)據(jù)為：gx,gy,gz，單位rad/s。陀螺儀向量 （gx,gy,gz），由式（4-5）可得由載體到導(dǎo)航坐標(biāo)系的四元數(shù)形式轉(zhuǎn)換矩陣為：

根據(jù)余弦矩陣和歐拉角的定義，地理坐標(biāo)系的重力向量，轉(zhuǎn)到機(jī)體坐標(biāo)系，是 中的第三列的三個(gè)元素，即。所以加速的向量與估計(jì)重力向量叉積：

然后利用向量的叉積， 可視為誤差向量，這個(gè)叉積向量仍舊是位于機(jī)體坐標(biāo)系上的，而陀螺積分誤差也是在機(jī)體坐標(biāo)系，而且叉積的大小與陀螺積分誤差成正比，正好拿來(lái)糾正陀螺。由于陀螺是對(duì)機(jī)體直接積分，所以對(duì)陀螺的糾正量會(huì)直接體現(xiàn)在對(duì)機(jī)體坐標(biāo)系的糾正。用上面得到的結(jié)果校正陀螺儀：

此處k為一個(gè)常量系數(shù)。

再利用二階畢卡法解四元數(shù)微分方程（4-6），更新四元數(shù)為下一次計(jì)算做準(zhǔn)備。畢卡二階算法為：

最后將四元數(shù)轉(zhuǎn)變?yōu)闅W拉角：

數(shù)據(jù)流程圖如下：

通過(guò)以上算法我成功得出了飛行器的姿態(tài)角，在開啟電機(jī)的情況下，角度誤差在+/-2°以內(nèi)，滿足了控制要求。姿態(tài)計(jì)算效果如下圖所示，其中紅色和藍(lán)色是直接由加速度計(jì)算出的俯仰角和橫滾角，青色和黃色為姿態(tài)結(jié)算后的俯仰角和橫滾角。由圖中數(shù)據(jù)可看出，解算的姿態(tài)角不僅能即時(shí)的反應(yīng)角度變化切曲線平滑，說(shuō)明姿態(tài)解算算法有效。

4.2.控制設(shè)計(jì)

由于四軸飛行器獨(dú)特的機(jī)械結(jié)構(gòu)，即結(jié)構(gòu)上的對(duì)稱設(shè)計(jì)，使得四軸在俯仰角的控制欲橫滾角的控制上有這近乎相同的控制特性，且兩者相對(duì)獨(dú)立。四軸飛行器的俯仰，橫滾，偏航，升降可以通過(guò)四個(gè)輸入量來(lái)控制。通過(guò)設(shè)定一個(gè)期望角度，調(diào)整電機(jī)轉(zhuǎn)速，使得測(cè)得的姿態(tài)角穩(wěn)定在期望角?？刂坡傻脑O(shè)計(jì)主要采用是閉環(huán)控制。以姿態(tài)角做為被控制量，采用經(jīng)典的PID控制算法。

四軸飛行器系統(tǒng)是一個(gè)時(shí)變且非線性的系統(tǒng)，采用傳統(tǒng)PID算法的單一的反饋控制會(huì)使系統(tǒng)存在不同程度的超調(diào)和振蕩現(xiàn)象，無(wú)法得到理想的控制效果。本文將前饋控制引入到了四軸飛行器系統(tǒng)的控制中，有效地改善了系統(tǒng)的實(shí)時(shí)性，提高了系統(tǒng)的反應(yīng)速度；并且根據(jù)四軸飛行器系統(tǒng)的特點(diǎn)，對(duì)數(shù)字PID算法進(jìn)行了改進(jìn)，引入了微分先行環(huán)節(jié)，改善了系統(tǒng)的動(dòng)態(tài)特性；使得控制器能夠更好地適應(yīng)四軸飛行器系統(tǒng)的實(shí)際情況。

在姿態(tài)角的控制中，本設(shè)計(jì)將控制器捕獲到的遙控器信號(hào)轉(zhuǎn)換為一個(gè)角度，作為期望角，與解算出來(lái)的測(cè)量角作差，得到偏差error。將error乘以一個(gè)比例系數(shù)kp。在只有kp作用下，系統(tǒng)會(huì)有靜差所以考慮利用積分ki控制消除穩(wěn)態(tài)誤差。但積分控制會(huì)降低系統(tǒng)的動(dòng)態(tài)性能，甚至造成閉環(huán)系統(tǒng)不穩(wěn)定，因此要對(duì)積分進(jìn)行限幅，防止積分過(guò)大。

對(duì)于微分，如果采用傳統(tǒng)的D方法，在人為操縱四軸時(shí)會(huì)產(chǎn)生輸入的設(shè)定值變化頻繁且幅度較大，從而造成系統(tǒng)的振蕩。對(duì)人為控制十分不利，為了解決設(shè)定值的頻繁變化給系統(tǒng)帶來(lái)的不良影響，本文在姿態(tài)角控制上引入了微分先行PID算法，其特點(diǎn)是只對(duì)輸出量進(jìn)行微分，即只對(duì)陀螺儀角速度測(cè)量值進(jìn)行微分，而不對(duì)姿態(tài)角的設(shè)定值進(jìn)行微分。這樣，在設(shè)定值發(fā)生變化時(shí)，輸出量并不會(huì)改變，而被控量的變化相對(duì)是比較緩和的，這就很好地避免了設(shè)定值的頻繁變化給系統(tǒng)造成的振蕩，明顯地改善了系統(tǒng)的動(dòng)態(tài)性能?？刂浦芷诙?ms，姿態(tài)控制系統(tǒng)示意圖如下：

圖5-1姿態(tài)控制系統(tǒng)示意圖

通過(guò)前面一章的介紹我們已經(jīng)的達(dá)到了俯仰，橫滾，航向三個(gè)控制量，然后將它們分別輸入三個(gè)獨(dú)立的如上圖所示的PID控制器，我們可以得到三個(gè)PID輸出：pid_roll，pid_pitch，pid_yaw將這三個(gè)輸出量做簡(jiǎn)單的線性運(yùn)算輸出給電機(jī)。部分代碼如下：

#define PIDMIX(X,Y,Z) Motor_Thr + pid_pitch* Y+ pid_roll*X + pid_yaw*Z

MOTOR1=MOTORLimit(PIDMIX(+1,+1,-1)); //REAR_R 后右電機(jī)

MOTOR2=MOTORLimit(PIDMIX(-1,-1,-1)); //FRONT_R 前右電機(jī)

MOTOR3=MOTORLimit(PIDMIX(-1,+1,+1)); //REAR_L 后左電機(jī)

MOTOR4=MOTORLimit(PIDMIX(+1,-1,+1)); //FRONT_L 前左電機(jī)

4.3. PID參數(shù)調(diào)節(jié)

PID參數(shù)的整定是PID控制的關(guān)鍵環(huán)節(jié)，直接影響到控制的效果。故一個(gè)PID設(shè)計(jì)的好不好往往要看其參數(shù)能否調(diào)節(jié)好，本設(shè)計(jì)的PID參數(shù)調(diào)節(jié)采用湊試法。湊試法是通過(guò)實(shí)際的閉環(huán)系統(tǒng)，通過(guò)觀察系統(tǒng)的響應(yīng)曲線，在本設(shè)計(jì)中通過(guò)觀察被調(diào)量，PID輸出，設(shè)定值三條曲線，判斷出kp,ki,kd對(duì)系統(tǒng)響應(yīng)的影響，反復(fù)嘗試，最終達(dá)到滿意響應(yīng)，從而達(dá)到確定控制參數(shù)的kp,ki,kd的目的。在參數(shù)調(diào)節(jié)過(guò)程總遵循以下原則[17]:

（1）在輸出不振蕩時(shí)，增大比例增益P。

（2）在輸出不振蕩時(shí)，減小積分時(shí)間常數(shù)Ti。

（3）在輸出不振蕩時(shí)，增大微分時(shí)間常數(shù)Td。

（4）一般步驟：

a.確定比例增益P

確定比例增益P時(shí)，首先去掉PID的積分項(xiàng)和微分項(xiàng)，一般是令Ti=0、Td=0（具體見PID的參數(shù)設(shè)定說(shuō)明），使PID為純比例調(diào)節(jié)。輸入設(shè)定為系統(tǒng)允許的最大值的60%~70%，由0逐漸加大比例增益P，直至系統(tǒng)出現(xiàn)振蕩；再反過(guò)來(lái)，從此時(shí)的比例增益P逐漸減小，直至系統(tǒng)振蕩消失，記錄此時(shí)的比例增益P，設(shè)定PID的比例增益P為當(dāng)前值的60%~70%。比例增益P調(diào)試完成。

b.確定積分時(shí)間常數(shù)Ti

比例增益P確定后，設(shè)定一個(gè)較大的積分時(shí)間常數(shù)Ti的初值，然后逐漸減小Ti，直至系統(tǒng)出現(xiàn)振蕩，之后在反過(guò)來(lái)，逐漸加大Ti，直至系統(tǒng)振蕩消失。記錄此時(shí)的 Ti，設(shè)定PID的積分時(shí)間常數(shù)Ti為當(dāng)前值的150%~180%。積分時(shí)間常數(shù)Ti調(diào)試完成。

c.確定積分時(shí)間常數(shù)Td

積分時(shí)間常數(shù)Td一般不用設(shè)定，為0即可。若要設(shè)定，與確定 P和Ti的方法相同，取不振蕩時(shí)的30%。

d.系統(tǒng)空載、帶載聯(lián)調(diào)，再對(duì)PID參數(shù)進(jìn)行微調(diào)，直至滿足要求。

（5）最終調(diào)試好的標(biāo)準(zhǔn)應(yīng)該是，PID輸出曲線在有一個(gè)階躍響應(yīng)來(lái)是，響應(yīng)一大一小兩個(gè)波，小波是大波的四分之一。

四軸飛行器的PID整定，我們首先四軸固定在單軸平衡平臺(tái)上，讓飛行器完成單軸平衡，主要觀察姿態(tài)角的（1）穩(wěn)定性，能否平衡在期望角度；（2）響應(yīng)性，當(dāng)操縱命令改變時(shí)，四軸能否即時(shí)的響應(yīng)期望的變化；（3）操縱性，由操縱員感受四軸的姿態(tài)是否已與操縱，會(huì)不會(huì)產(chǎn)生響應(yīng)過(guò)沖。

在參數(shù)調(diào)整時(shí)，先調(diào)P,將I,D給0，先給一個(gè)小值P1，如果飛行器不能穩(wěn)定在一個(gè)角度，則P1給小了，下一次給一個(gè)較大值P2，如果飛行器產(chǎn)生震蕩則證明P2給大了，那么合適的P在P1-P2之間，反復(fù)試驗(yàn)幾次可找到P震蕩的臨界點(diǎn)P0,然后保持P0不變按照調(diào)P的方法來(lái)調(diào)D,D是用來(lái)消除誤差的，當(dāng)抖動(dòng)差不多被消除時(shí)，此時(shí)我們有較合適的P0,D0，在這兩個(gè)值附近再試幾組參數(shù)，觀察效果得到最優(yōu)參數(shù)。調(diào)好P,D后此時(shí)四軸的穩(wěn)定狀態(tài)與期望狀態(tài)間也學(xué)會(huì)有靜差，接下來(lái)加入I，參數(shù)有小到大，當(dāng)靜差差不多被消除時(shí)，我們?cè)賹?duì)P,I,D三組參數(shù)在小范圍內(nèi)聯(lián)調(diào)。最后確定恰當(dāng)?shù)姆e分限幅值，完成整個(gè)PID參數(shù)的整定。

5.系統(tǒng)創(chuàng)新

• 采用STM32F407這樣一款高性能芯片作為控制核心，計(jì)算快速，擴(kuò)展空間大。
• 云臺(tái)飛控一體化設(shè)計(jì)，既能完成飛行任務(wù)也能實(shí)現(xiàn)云臺(tái)穩(wěn)定。
• 姿態(tài)算法采用基于四元數(shù)的互補(bǔ)濾波，姿態(tài)角無(wú)奇點(diǎn)，比起卡爾曼李春波等高端算法有著計(jì)算量小的特點(diǎn)且能投入使用，大大節(jié)約了cpu計(jì)算時(shí)間，也降低了對(duì)cpu的性能要求。
• 利用四軸作為云臺(tái)載體有著靈活機(jī)動(dòng)，可讓攝像頭獲得比較好的視野，且云臺(tái)能消除四軸機(jī)體抖動(dòng)。
• 方便改裝用于執(zhí)行其他任務(wù)。

6.評(píng)測(cè)與結(jié)論

6.1.四軸飛行器的調(diào)試

6.1.1電源調(diào)試

將電路板焊接完成后，為電路板供電，用萬(wàn)用表測(cè)得個(gè)供電芯片電壓如下

表6-1各電源芯片電壓值

由表中數(shù)據(jù)可知電源管理部分可滿足系統(tǒng)供電要求。

6.1.2 姿態(tài)角調(diào)試

在完成控制器底層的硬件驅(qū)動(dòng)后，開始姿態(tài)角算法的調(diào)試。利用藍(lán)牙模塊將解算出的姿態(tài)角數(shù)據(jù)發(fā)回上位機(jī)，搖動(dòng)四軸機(jī)體，觀察上位機(jī)數(shù)據(jù)曲線與姿態(tài)演示立方塊。

圖6-1 姿態(tài)角數(shù)據(jù)對(duì)比曲線

圖6-1中黃色和藍(lán)色為直接由加速度計(jì)算出的俯仰與橫滾角，紅色和青色為姿態(tài)解算后的姿態(tài)角，可明顯看出直接由加速度計(jì)算出的姿態(tài)角噪聲大，不平滑，不能真實(shí)反映姿態(tài)角的變化。而由四元數(shù)算法解算后的姿態(tài)角反應(yīng)快，噪聲小，足以滿足控制要求。光觀察曲線并不直觀，還可以觀察上位機(jī)中的立方塊，當(dāng)小立方塊的姿態(tài)與四軸機(jī)體能夠保持一致變化時(shí)說(shuō)明姿態(tài)解算良好。通過(guò)以上手段可知姿態(tài)角解算滿足需求。

6.1.3 控制電機(jī)調(diào)試

在第五章，已經(jīng)針對(duì)四軸飛行器進(jìn)行了PID的控制設(shè)計(jì),但為了得到實(shí)際使用的PID控制參數(shù)我們還需要通過(guò)反復(fù)試驗(yàn)來(lái)整定出最優(yōu)參數(shù)。為了方便參數(shù)的調(diào)試，在本設(shè)計(jì)過(guò)程中才用了先調(diào)平衡一個(gè)方向，再調(diào)另一個(gè)方向，最后聯(lián)達(dá)到平衡的效果。為此設(shè)計(jì)了如下圖所示試試驗(yàn)平臺(tái)：

圖6-3 四軸單軸平衡試驗(yàn)

在調(diào)試過(guò)程中，為了避免頻繁的下載程序帶來(lái)的不變，采用串口藍(lán)牙發(fā)送參數(shù)的做法，大大提高了調(diào)試效率。當(dāng)俯仰與橫滾兩個(gè)軸都能夠獨(dú)立的平衡時(shí)，可以脫離單軸平臺(tái)開始在空中進(jìn)行航向角的穩(wěn)定性調(diào)試。

6.2.四軸飛行器的試飛

當(dāng)前面的各項(xiàng)調(diào)試都完成了之后，就準(zhǔn)備要進(jìn)行試飛實(shí)驗(yàn)。飛行試驗(yàn)是對(duì)控制系統(tǒng)的功能和技術(shù)指標(biāo)進(jìn)行驗(yàn)證的最終手段，也是衡量四軸設(shè)計(jì)是否成功的重要標(biāo)志。

試飛前要確保系統(tǒng)各部分工作正常，穩(wěn)定。檢查各個(gè)接口連接是否正確，各部件安裝是否牢固，電池電量是否充足。打開電源前檢查遙控器油門是否在最低位置，起飛前先輕推油門確保電機(jī)工作正常。一切準(zhǔn)備就緒，即可進(jìn)行試飛。將四種飛行器放在水平地面上，開始啟動(dòng)姿態(tài)初始化程序，聽到電調(diào)提示音后，緩慢增加油門，螺旋槳轉(zhuǎn)速上升，將飛機(jī)拉離地面。由實(shí)際情況可看出當(dāng)姿態(tài)發(fā)生傾斜時(shí)，姿態(tài)解算及PID控制能夠及時(shí)調(diào)整電機(jī)轉(zhuǎn)速，穩(wěn)定飛行姿態(tài)。飛行器還能根據(jù)遙控指令的變化完成相應(yīng)的動(dòng)作。本此設(shè)計(jì)的四軸飛行器可實(shí)現(xiàn)垂直升降的要求，能保持姿態(tài)的穩(wěn)定，機(jī)體晃動(dòng)小，在微風(fēng)的干擾下能夠自動(dòng)調(diào)整姿態(tài)，確保平穩(wěn)飛行，且系統(tǒng)響應(yīng)快，續(xù)航時(shí)間大概在8分鐘。因此本次的設(shè)計(jì)是有效的。

附錄

部分代碼

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

{

static float q0 = 1.0f, q1 = 0.0f, q2 = 0.0f, q3 = 0.0f;

float delta_2=0;

const static float FACTOR = 0.002;

float norm=0.0f;

float vx, vy, vz;

float ex, ey, ez;

float q0q0 = q0*q0;

float q0q1 = q0*q1;

float q0q2 = q0*q2;

float q1q1 = q1*q1;

float q1q3 = q1*q3;

float q2q2 = q2*q2;

float q2q3 = q2*q3;

float q3q3 = q3*q3;

norm = sqrt(ax*ax + ay*ay +az*az); // 測(cè)量正?；?把加速度計(jì)的三維向量轉(zhuǎn)成單位向量。

ax = ax /norm;

ay = ay / norm;

az = az / norm;

vx = 2*(q1q3 - q0q2); // 估計(jì)方向的重力

vy = 2*(q0q1 + q2q3);

vz = q0q0 - q1q1 - q2q2 + q3q3;

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

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

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

halfT=0.002;

gx = gx + ex*FACTOR/halfT; //校正陀螺儀測(cè)量值 用叉積誤差來(lái)做PI修正陀螺零偏

gy = gy + ey*FACTOR/halfT;

gz = gz + ez*FACTOR/halfT;

delta_2=(2*halfT*gx)*(2*halfT*gx)+(2*halfT*gy)*(2*halfT*gy)+(2*halfT*gz)*(2*halfT*gz);

q0 = (1-delta_2/8)*q0 + (-q1*gx - q2*gy - q3*gz)*halfT; // 整合四元數(shù)率 四元數(shù)微分方程 四元數(shù)更新算法，二階畢卡法

q1 = (1-delta_2/8)*q1 + (q0*gx + q2*gz - q3*gy)*halfT;

q2 = (1-delta_2/8)*q2 + (q0*gy - q1*gz + q3*gx)*halfT;

q3 = (1-delta_2/8)*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;

//轉(zhuǎn)換為歐拉角

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

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

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

}