共軸雙旋翼無人機設計與實現

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

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

關鍵詞：FreeRTOS實時操作系統;共軸雙旋翼;Mahoney互補濾波;串級PID

中圖分類號：TP311 ? ?文獻標識碼：A

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

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

1飛行器結構與控制原理

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

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

1.1飛行控制系統總體框圖

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

2系統硬件設計

2.1飛控系統硬件設計

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

2.2數據傳輸

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

2.3控制信號輸出

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

3系統軟件設計

3.1 FreeRTOS任務調度

主要任務之間的調度如圖7所示。

UsbRxTask任務負責將上位機通過串口發送過來，存在UsbData隊列的數據，打包成ATKP格式數據包，再存入RxQueue隊列。

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

StabTask任務負責將傳感器數據融合，得出姿態，再根據獲取的控制數據，進行PID控制和動力分配，控制電機和舵機使機身保持平穩。

TxTask任務負責將StabTask任務的姿態數據、電機和舵機PWM輸出數據等打包存入TxQueue_R和TxQueue_U隊列，再分別由RadioTask發送給遙控器，UsbTxTask發送給上位機。

3.2 Mahoney互補濾波數據融合算法

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

3.2.1 旋轉矩陣

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

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

由地理坐標系[E]至最終的機體坐標系[B]的姿態變換矩陣如下：

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

3.2.2 四元數與姿態變換矩陣

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

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

[CEB]與[E]系至[B]系的姿態變換矩陣[CBE]互為轉置的關系：

[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]

得到四元數轉歐拉角公式：

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

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

根據上一采樣周期得到的姿態矩陣[CBE]左乘[Ge]，可以得到理論上在機體坐標系下表示的重力加速度[Gb]，根據矩陣乘法，[Gb]也就是地理至機體坐標系姿態變換矩陣[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θ]

當[θ]很小時，[sinθ≈θ]，即[ρ≈θ=error]。

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

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

[error_GYRO=Kp?error+Ki?error]

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

[Value_GYRO=Value_GYRO+error_GYRO]

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

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

再將更新后的四元數代入四元數轉歐拉角公式，得出最新姿態角，再利用串級PID控制姿態。

3.3 PID控制算法

對模擬PID離散化后的PID算法數學表達式如下：

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

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

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

4 實驗與結論

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

參考文獻：

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

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

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

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

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

[6] 梅春波.捷聯慣性導航系統慣性系非線性初始對準技術研究[D].西安：西北工業大學，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] 黃權，李立欣，高昂，等.基于Runge-Kutta法的小型無人機姿態角算法研究[J].電子設計工程，2017，25（11）：133-136.

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

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

【通聯編輯：梁書】