共軸雙旋翼無人機設(shè)計與實現(xiàn)

王棒 蘇成悅 徐勝 陳元電 施振華

摘要：文章首先通過SolidWorks設(shè)計一種矢量共軸雙旋翼無人機，其主要由共軸式電機和兩個舵機組合而成，共軸式電機提供升力、利用上下旋翼的差速產(chǎn)生反扭矩控制偏航，舵機控制升力方向?qū)崿F(xiàn)翻滾和俯仰。為研究如何控制其飛行，文章接著設(shè)計一款專用的飛行器控制系統(tǒng)。硬件方面，主控采用STM32F4，姿態(tài)傳感器采用mpu9250九軸傳感器，氣壓傳感器采用BMP280。軟件方面，實時操作系統(tǒng)采用FreeRTOS嵌入式，姿態(tài)解算采用Mahoney互補濾波算法，姿態(tài)控制算法采用串級PID。姿態(tài)控制量和油門值融合得到總控制量，總控制量轉(zhuǎn)化為4路PWM信號分別控制兩個電機和兩個舵機。

關(guān)鍵詞：FreeRTOS實時操作系統(tǒng);共軸雙旋翼;Mahoney互補濾波;串級PID

中圖分類號：TP311 ? ?文獻(xiàn)標(biāo)識碼：A

文章編號：1009-3044（2022）13-0001-04

隨著科技的發(fā)展，無人機逐漸進(jìn)入人們的視野，各種氣動布局的無人機層出不窮。共軸雙旋翼無人機具有除和常規(guī)旋翼無人機垂直起降的功能外，其還具有結(jié)構(gòu)緊湊、重量小、便攜性高、效率高、續(xù)航久等功能特點[1]。因此，常規(guī)多旋翼能勝任的航拍、測繪、救援等任務(wù)，共軸雙旋翼無人機也可以勝任。但由于其本身的結(jié)構(gòu)和氣動布局的特殊性，飛行時易受到外界因素的干擾。所以控制其穩(wěn)定飛行、提高響應(yīng)速度也成為必須，共軸雙旋翼無人機整體結(jié)構(gòu)如圖1所示。

1飛行器結(jié)構(gòu)與控制原理

如圖2所示，共軸雙旋翼飛行器動力與方向控制部分包含一對8040槳葉、一個1400KV 2204共軸電機、兩個20A電調(diào)、兩個金屬數(shù)字舵機。①②③④分別為翻滾舵機、俯仰舵機、順時針電機、逆時針電機。

共軸雙旋翼在空中懸停后，可進(jìn)行八種運動。如垂直升降運動時，順逆電機同時加速或減速，俯仰和翻滾舵機保持水平;俯仰和翻滾運動時，保持機身穩(wěn)定不掉高情況下，舵機發(fā)生對應(yīng)的傾轉(zhuǎn)，使升力與重力的合力方向為俯仰或翻滾方向;偏航運動，利用上下電機的轉(zhuǎn)速差產(chǎn)生的反扭矩實現(xiàn)。如順時針偏航時，逆時針電機加速（其產(chǎn)生順時針的扭矩增大），順時針電機減速（其產(chǎn)生逆時針的扭矩減?。?，產(chǎn)生使機身順時針旋轉(zhuǎn)的總扭矩。各種姿態(tài)運動時，順逆電機和俯仰翻滾舵機的狀態(tài)如表1所示。

1.1飛行控制系統(tǒng)總體框圖

整個飛行控制系統(tǒng)主要包括三個部分[2]：上位機系統(tǒng)、遙控系統(tǒng)、飛行控制器。飛行控制器通過USB數(shù)據(jù)線連接上位機系統(tǒng)，在上位機界面調(diào)整飛行控制器的PID參數(shù)、校準(zhǔn)遙控器各通道量程、校準(zhǔn)加速度計陀螺儀等。遙控系統(tǒng)采用支持SBUS協(xié)議且工作頻段為2.4GHZ的航模遙控器和接收機。飛行控制器依據(jù)遙控器傳入的數(shù)據(jù)，控制共軸雙旋翼無人機的姿態(tài)，同時將電量等信息回傳給遙控器，具體過程如圖3所示。

2系統(tǒng)硬件設(shè)計

2.1飛控系統(tǒng)硬件設(shè)計

文章選擇了STM32F405RGT6作為飛行控制器的主控，上電自動復(fù)位，啟動模式可通過BOOT0引腳的按鍵選擇。采用MPU9250檢測姿態(tài)[3]，BMP280氣壓計測量高度。支持SBUS和PPM兩種無線通信協(xié)議，分別采用兩路12位ADC檢測電源的電壓和電流，當(dāng)電量不足時觸發(fā)蜂鳴器報警。通過三色指示燈3528RGB指示飛行控制器的狀態(tài)，采用TF卡記錄飛行數(shù)據(jù)。采用SWD協(xié)議下載和調(diào)試代碼，采用3個定時器產(chǎn)生6路不同頻率的PWM作為控制信號，如圖4所示。

2.2數(shù)據(jù)傳輸

接收機與飛行控制器可采用SBUS或者PPM通信，可以通過焊盤的焊接選擇。SBUS總線是一根串行總線，通過這根線即可獲取遙控器各通道的數(shù)據(jù)，SBUS總線協(xié)議使用的是TTL電平的反向電平，可利用三極管的開關(guān)特性實現(xiàn)。接收機的輸出端連接SBUS_IN，其為高電平時，NPN三極管導(dǎo)通，此時輸入端SBUS電平為低電平信號。反之若SBUS_IN為低電平時，三極管不導(dǎo)通，SBUS為高電平。SBUS引腳連接的是主控芯片的串口RX引腳，利用外設(shè)串口6實現(xiàn)數(shù)據(jù)接收，如圖5所示。

2.3控制信號輸出

輸入給無刷電調(diào)的PWM頻率，不會影響電機的轉(zhuǎn)速，但會影響控制頻率和精度，飛控輸出給電調(diào)的PWM傳遞的是1ms～2ms脈寬的信號，脈寬越寬，轉(zhuǎn)速越快。電調(diào)會根據(jù)這個脈寬來驅(qū)動電機。為了滿足對共軸電機控制頻率和精度的需求，使用定時器3的兩個通道產(chǎn)生頻率為400Hz的PWM信號分別輸入給兩電調(diào);因同一個定時器各通道的PWM頻率一樣，故舵機采用定時器4的兩個通道產(chǎn)生脈寬為0.5ms～2.5ms、周期為20ms的PWM信號分別輸入給舵機，對應(yīng)舵機轉(zhuǎn)動角度為0°～180°[4]。另外，利用定時器5設(shè)計了兩路輔助PWM用于驅(qū)動掛載設(shè)備，如圖6所示。

3系統(tǒng)軟件設(shè)計

3.1 FreeRTOS任務(wù)調(diào)度

主要任務(wù)之間的調(diào)度如圖7所示。

UsbRxTask任務(wù)負(fù)責(zé)將上位機通過串口發(fā)送過來，存在UsbData隊列的數(shù)據(jù)，打包成ATKP格式數(shù)據(jù)包，再存入RxQueue隊列。

RxAnlTask任務(wù)負(fù)責(zé)將RxQueue隊列的ATKP格式包解析，將上位機或者遙控器發(fā)來的控制指令發(fā)送給StabTask任務(wù)。

StabTask任務(wù)負(fù)責(zé)將傳感器數(shù)據(jù)融合，得出姿態(tài)，再根據(jù)獲取的控制數(shù)據(jù)，進(jìn)行PID控制和動力分配，控制電機和舵機使機身保持平穩(wěn)。

TxTask任務(wù)負(fù)責(zé)將StabTask任務(wù)的姿態(tài)數(shù)據(jù)、電機和舵機PWM輸出數(shù)據(jù)等打包存入TxQueue_R和TxQueue_U隊列，再分別由RadioTask發(fā)送給遙控器，UsbTxTask發(fā)送給上位機。

3.2 Mahoney互補濾波數(shù)據(jù)融合算法

MPU9250自帶的數(shù)字運動處理器DMP不需要主控參與運算還能直接輸出四元數(shù)，但由于DMP最短采集數(shù)據(jù)周期為5ms，故采用直接讀取數(shù)據(jù)的方法。在數(shù)據(jù)融合前，先對加速度計數(shù)據(jù)IIR濾波，對陀螺儀數(shù)據(jù)加偏置調(diào)整，再通過Mahoney互補濾波融合數(shù)據(jù)[5]。

3.2.1 旋轉(zhuǎn)矩陣

設(shè)有兩個坐標(biāo)系分別為地理坐標(biāo)系[E]和機體坐標(biāo)系[B0]，起初機體坐標(biāo)系[B0]和地理坐標(biāo)系[E]重合。令機頭方向為X軸的正方向，將X軸稱為翻滾軸，Y軸稱為俯仰軸，Z軸稱為偏航軸。當(dāng)機體坐標(biāo)系繞著Z軸旋轉(zhuǎn)一定角度y，得到新的機體坐標(biāo)系[B1]，此時機[B1]已經(jīng)不和地理坐標(biāo)系重合。再繞[B1]坐標(biāo)系中的Y軸旋轉(zhuǎn)一定角度得到新的坐標(biāo)系[B2]。最后繞[B2]坐標(biāo)系的X軸旋轉(zhuǎn)一定角度r，得到最后的機體坐標(biāo)[B]。即旋轉(zhuǎn)順序為Z—>X—>Y。繞各軸旋轉(zhuǎn)時，旋轉(zhuǎn)方向為右手方向，每次轉(zhuǎn)動對應(yīng)一個基本旋轉(zhuǎn)矩陣如下：

[CB1E=cosysiny0-sinycosy0001（繞Z軸）CB2B1=cosp0-sinp010sinp0cosp（繞Y軸） CBB2=1000cosrsinr0-sinrcosr（繞X軸）]

由地理坐標(biāo)系[E]至最終的機體坐標(biāo)系[B]的姿態(tài)變換矩陣如下：

[CBE=CBB2CB2B1CB1E=cospcosycospsiny-sinpcosysinpsinr-cosrsinycosrcosy+sinpsinrsinycospsinrsinrsiny+cosrcosysinpcosrsinpsiny-cosysinrcospcosr]

3.2.2 四元數(shù)與姿態(tài)變換矩陣

當(dāng)剛體定點轉(zhuǎn)動，只考慮[E]系相對[B]系的角位置時，可當(dāng)作[B]系是由[E]系沒有經(jīng)過中間過程，而是一次性等效旋轉(zhuǎn)形成的[6]，得到[B]系至[E]系的姿態(tài)變換矩陣：

[CEB=q02+q12-q22-q322q1q2-q0q32q1q3+q0q22q1q2+q0q3q02-q12+q22-q322q2q3-q0q12q1q3-q0q22q2q3+q0q1q02-q12-q22+q32]

[CEB]與[E]系至[B]系的姿態(tài)變換矩陣[CBE]互為轉(zhuǎn)置的關(guān)系：

[CBE=CEBT=q02+q12-q22-q322q1q2+q0q32q1q3-q0q22q1q2-q0q3q02-q12+q22-q322q2q3+q0q12q1q3+q0q22q2q3-q0q1q02-q12-q22+q32]

對比兩式：

[-sinp=2（q1q3-q0q2）cospsinycospcosy=tany=2（q1q2+q0q3）q02+q12-q22-q32cospsinrcospcosr=tanr=2（q2q3+q0q1）q02-q12-q22+q32]

得到四元數(shù)轉(zhuǎn)歐拉角公式：

[p=arcsin2（q1q3-q0q2）y=arctan2（q1q2+q0q3）q02+q12-q22-q32r=arctan2（q2q3+q0q1）q02-q12-q22+q32]

加速度計測量到的加速度數(shù)據(jù)是基于機體坐標(biāo)系的，而重力加速度向量[Ge]=[00gT]是地理坐標(biāo)系的，兩者要在同一個坐標(biāo)系下做叉積運算。如圖8所示，把[Ge]轉(zhuǎn)到機體坐標(biāo)系，得到其在機體坐標(biāo)系表示的向量[Gb]。

根據(jù)上一采樣周期得到的姿態(tài)矩陣[CBE]左乘[Ge]，可以得到理論上在機體坐標(biāo)系下表示的重力加速度[Gb]，根據(jù)矩陣乘法，[Gb]也就是地理至機體坐標(biāo)系姿態(tài)變換矩陣[CBE]的第三列：

[Gb=CBEGe=2q1q3-q0q22q2q3+q0q1q02-q12-q22+q32=g1g2g3]

則有：

[ρ=Ab×Gb=ijka1a2a3g1g2g3=0-a3a2a30-a1-a2a10g1g2g3ijk]

其中，[Gb]為單位理論加速，[Ab]為傳感器測量的單位加速，所以[Gb=1]和[Ab=1]，推出：

[ρ=AbGbsinθ=sinθ]

當(dāng)[θ]很小時，[sinθ≈θ]，即[ρ≈θ=error]。

[error=exeyez=0-a3a2a30-a1-a2a10g1g2g3=a2g3-a3g2a3g1-a1g3a1g2-a2g1]

[ρ]向量大小可以看作是[Ab]與[Gb]兩向量的夾角。夾角大，說明理論上的加速度和測量得到的加速度偏差大。這是由于陀螺儀的積分誤差使旋轉(zhuǎn)矩陣不準(zhǔn)確，造成理論加速度不夠真實。陀螺儀數(shù)據(jù)雖不完全準(zhǔn)確，但是也不能因此完全信任加速度測得的數(shù)據(jù)，為了減小這種偏差，使兩向量盡可能重合，需二者數(shù)據(jù)合理采納。因此采用比例積分PI器控制補償誤差的大小與精度，得到補償量[error_GYRO]：

[error_GYRO=Kp?error+Ki?error]

其中，比例項用來控制傳感器的“可信度”，[Kp]大則偏向加速度計，小則偏向陀螺儀，積分項用來消除靜態(tài)誤差，把補償量加到角速度上，得到可信度較高的陀螺儀數(shù)據(jù)。

[Value_GYRO=Value_GYRO+error_GYRO]

最后，代入一階龍格庫塔（Runge—Kutta）算法[7-8]，更新得到能代表當(dāng)前姿態(tài)的四元數(shù)，其中[wxwywz]就是修正后的角速度，[t+Δt]表示下一時刻。

[q0q1q2q3t+Δt=q0q1q2q3t+12?Δt-wxq1-wyq2-wzq3wxq0-wyq3+wzq2wxq3+wyq0-wzq1-wxq2+wyq1+wzq0]

再將更新后的四元數(shù)代入四元數(shù)轉(zhuǎn)歐拉角公式，得出最新姿態(tài)角，再利用串級PID控制姿態(tài)。

3.3 PID控制算法

對模擬PID離散化后的PID算法數(shù)學(xué)表達(dá)式如下：

[U（n）=Kp×e（n）+Ki×i=0ne（i）+Kd×（e（n）-e（n-1））]

式中：[Kp]為比例系數(shù)，[Ki]為積分系數(shù)，[Kd]為微分系數(shù)，[e（n）]=期望角度-實際角度

算法流程圖如圖9所示，定高模式采用的是單級PID，遙控器油門值轉(zhuǎn)換為Z軸速度值，高度估計值來自Z軸加速度值和氣壓計BMP280測量的高度值的數(shù)據(jù)融合，高度期望值為Z軸速度值的積分，偏差值為高度期望值與高度估計值之差，經(jīng)過單級PID作用后輸出油門控制量。手動模式下遙控器油門值為油門控制量。姿態(tài)控制采用串級PID，角速度環(huán)作為內(nèi)環(huán)，角度環(huán)作為外環(huán)，即P-PID[9-10]。外環(huán)的輸入為期望角度與測量角度之差，輸出作為期望角速度，其與測量的角速度之差作為內(nèi)環(huán)輸入，內(nèi)環(huán)的輸出即為姿態(tài)控制量。

4 實驗與結(jié)論

文章通過STM32單片機和MPU9250姿態(tài)傳感器芯片設(shè)計了一款共軸雙旋翼飛控。通過Mahoney互補濾波算法，將陀螺儀和加速度計的數(shù)據(jù)融合，得到姿態(tài)角。再采用串級PID（角度環(huán)+角速度環(huán)）得到姿態(tài)控制量，將姿態(tài)控制量和油門控制量整合后，產(chǎn)生對應(yīng)的PWM信號控制電機和舵機。實物飛行如圖10所示，因未融合磁力計數(shù)據(jù)，飛行一段時間后會偏航，還略有掉高現(xiàn)象，但總體飛行效果達(dá)到預(yù)期。后續(xù)可以采用串級PID（位置環(huán)+速度環(huán)）作為定高方式。

參考文獻(xiàn)：

[1] 陳銘.共軸雙旋翼直升機的技術(shù)特點及發(fā)展[J].航空制造技術(shù)，2009，52（17）：26-31.

[2] 陳旺才，陳虢，徐君，等.基于STM32的四旋翼飛行器姿態(tài)控制器設(shè)計與實現(xiàn)[J].電子測量技術(shù)，2020，43（19）：165-168.

[3] 趙嵌嵌.一種基于MPU9250的四旋翼姿態(tài)解算改進(jìn)算法[J].機械工程師，2019（5）：36-39.

[4] 單樹清.基于STM32的無刷直流電機控制系統(tǒng)研究[D].蘭州：蘭州交通大學(xué)，2021.

[5] 徐恩松，陸文華，劉云飛，等.基于Mahony濾波算法的姿態(tài)解算與應(yīng)用研究[J].智能計算機與應(yīng)用，2019，9（5）：80-83.

[6] 梅春波.捷聯(lián)慣性導(dǎo)航系統(tǒng)慣性系非線性初始對準(zhǔn)技術(shù)研究[D].西安：西北工業(yè)大學(xué)，2016.

[7] Shi T N，Lu N，Zhang Q，et al.Brushless DC motor sliding mode control with Kalman Filter[C]//2008 IEEE International Conference on Industrial Technology.April 21-24，2008，Chengdu.IEEE，2008：1-6.

[8] 黃權(quán)，李立欣，高昂，等.基于Runge-Kutta法的小型無人機姿態(tài)角算法研究[J].電子設(shè)計工程，2017，25（11）：133-136.

[9] 吳東旭，丁茹，張梓嵩，等.基于P-PID控制器對共軸雙旋翼飛行器的姿態(tài)控制[J].沈陽理工大學(xué)學(xué)報，2019，38（4）：58-61.

[10] 馮慶端，裴海龍.串級PID控制在無人機姿態(tài)控制的應(yīng)用[J].微計算機信息，2009，25（22）：9-10，45.

【通聯(lián)編輯：梁書】