基于RT-Thread與滑模控制的微型多旋翼定點控制系統研究

楊兵　王昆　馮揚帆　陳福

摘 要：為了研究微型多旋翼的定點控制，設計并實現了一種基于RT-Thread的微型多旋翼定點控制系統。采用Digital Motion Processing庫和雙閉環PID進行姿態控制，使用磁力計與激光測距儀進行航向與高度的鎖定；融合光流傳感器、姿態、高度數據感知飛行器的水平速度、位置；針對水平位置，提出了基于滑模控制器的微型多旋翼定點控制方法。最終，通過實驗驗證了該系統的有效性。

關鍵詞：RT-Thread；微型多旋翼；定點控制；數據融合

中圖分類號：TP29? 文獻標志碼：A? 文章編號：1671-0797（2023）09-0012-03

DOI：10.10.19514/j.cnki.cn32-1628/tm.2023.09.004

0? ? 引言

微型飛行器在情報偵察[1]、遙感測繪[2]等領域有著極高的應用價值，而定點懸停是微型飛行器各項動作的基礎，研究微型飛行器的定點懸停有著不可小覷的實用價值。

目前，主流的多旋翼定點方法有基于GPS的定點方法、基于視覺系統的定點方法[3]、基于光流的定點方法[4]以及基于UWB的定點方法[5]。本文以微型無人機光流定點控制為研究對象，完成了微型飛行器定點控制系統的設計與實現，提出了基于滑模控制的定點控制方法，并通過實驗進行了驗證。

1? ? 總體方案與硬件設計

微控制器采用基于Coretex-M3內核的STM32F103C8，內置浮點運算單元等，主頻最高72 MHz，非常適合微型飛控開發。無線通信模塊為nRF24L01，支持實時雙向通信，用于控制指令的接收與控制器參數無線調試。姿態傳感器為MPU6050，內置數字運動處理引擎。羅盤為HMC5883L，最大輸出頻率可達160 Hz。光流模塊為PMW3901MB，支持80 mm至無限遠的工作距離。ToF測距傳感器為VL53L0X，測量頻率可達50 Hz。總體方案設計如圖1所示。

硬件設計如圖2所示，為盡可能減小PCB面積，便于在微型無人機上安裝，元器件采用雙面布局，完成后PCB為邊長不超過4.5 cm的正方形。

2? ? 軟件設計

雖然僅依靠前臺系統即可實現飛行控制，但隨著飛控中傳感器種類的擴增以及飛行任務需求的日益增多，前后臺系統已無法勝任日趨復雜的應用場景。引入RTOS后，將有效解決該問題。本設計中，操作系統采用RT-Thread NanoV3.1.5，小巧精悍，擁有完整內核，支持搶占式調度。此外還添加了FinSH[6]組件，用以查看各線程棧的最大使用量并合理分配各線程棧大小，避免線程棧溢出導致硬錯誤。姿態傳感器和羅盤基于同一路I2C與微控制器通信，因此信號量互斥地使用I2C總線。

多線程調度方式、線程優先級、線程棧大小、中斷優先級等的設置是本設計中稍難的點，如果安排不合理，則會導致硬實時控制無法實現、線程死鎖、硬錯誤等問題。一般而言，在單核多任務系統中，并發運行的主線程及其他各線程可視為后臺，而支持操作系統運行的SysTick、PendSV中斷及其他中斷則可視為前臺。

除操作系統自帶的tShell和tidle線程外，其余線程負責完成遠程控制指令接收，姿態、高度、光流信息的獲取等，具體如表1所示。

設置NVIC分組為兩位搶占優先級、兩位響應優先級。SysTick中斷負責實現RTOS的調度，PendSV中斷負責上下文切換，定時器TIM4負責脈沖寬度調制波的發生，定時器TIM1工作于周期中斷模式，負責進行4路脈沖寬度調制，完成電機轉速控制與姿態控制，USART1實現字符緩沖接收以支持FinSH，具體如表2所示。

3? ? 控制方法

微型飛行器中各傳感器方向的定義、坐標系定義如圖3所示。

姿態獲取不作為本文重點，采用Digital Motion Processing庫獲取四元數（q0，q1，q2，q3）及三軸角速度（ωx，ωy，ωz），按式（1）可由四元數轉換得歐拉角（φ，γ，θ），配合三軸角速度作為姿態控制的輸入。

整體控制設計如圖4所示，高度和航向控制較為簡單，圖中不進行說明。姿態控制上選擇串級PID，內環控制角速度，以100 Hz運行，外環控制角度，以25 Hz運行，同時將外環角度環輸出的期望角速度等份遞增地輸出至內環角速度控制器以獲得平滑的控制效果。

定點過程中往往需要定航向和定高，由羅盤和測距儀可得出航向與高度，經過簡單的反饋控制即可實現航向和高度的鎖定。對于航向的鎖定采用簡單的比例反饋控制即可實現良好的效果，對于高度采用比例積分微分控制可得到良好的效果，值得一提的是，經過激光測距儀測得的距離需引入橫滾角和俯仰角校正[7]后才能得到高度。

采用式（1）計算所得偏航角存在漂移現象，因此根據磁力計在其x、y方向測得的磁感應強度分量可計算得到偏航角，在計算前需要對磁力計進行校正。

定點控制需要光流傳感器的輔助，光流傳感器輸出連續兩幀之間像素變化量Δpx和Δpy。由于光流傳感器的水平位移與像素變化量、光流傳感器距離地面高度、姿態角存在聯系，因此根據光流傳感器距離地面高度、像素變化量、姿態角可計算出光流傳感器即飛行器的水平位移，計算方法如下：

式中：Sx、Sy為根據光流傳感器得到的x、y方向的位移，初值均為0；L為測距傳感器測量結果；R為飛行器在距地面1 m時產生1個像素變化量對應的位移；Kx為針對飛行器橫滾的校正系數；Ky為針對飛行器俯仰的校正系數；Kx、Ky和R均由實驗測得。

光流速度由光流位移差分并經一階低通濾波器得到，差分間隔不宜過短，否則計算所得光流速度會有很多毛刺，無法反映飛行器實際運動。

以橫滾通道為例分析定點控制過程，假定微型多旋翼定高后橫滾角γ保持在某非水平值γd＞0，由運動學分析可知多旋翼將向右做加速運動。因定高定點過程中運動速度低，故可忽略風阻。可分析得出：

中：px為水平位置；g為重力加速度。

盡管采用速度—位置雙閉環控制可將無人機水平位移控制在零附近，實現定點控制，但該方法存在參數多、調參復雜的問題，因此提出了基于滑模控制的多旋翼定點控制方法。

式中：pxd為期望的無人機位置，一般取0；s、z1、z2為滑模控制器中間變量；其余的c、k、η為控制器參數。

相比于速度—位置雙閉環PID控制，調試參數由6個下降至3個。

4? ? 飛行實驗

對所設計的多旋翼定點控制系統進行實驗驗證。依次調節角速度環PID、角度環PID參數，保證角度控制快速無靜差。校正系數Kx和Ky均取480.0像素，R取0.213 cm/（m·像素），滑模控制中c=0.017 2，k=0.017 2，η=5×10-5。開始光流定點后，微型飛行器在其x方向和y方向的位移曲線及位置如圖5所示。

可以看到微型飛行器位置保持在（0，0）附近，x、y方向位置均值分別為1.12 cm、-0.86 cm，方差分別為16.39 cm2、32.69 cm2，成功實現了定點控制。

5? ? 結語

本文采用RT-Thread設計并實現了一種微型多旋翼定點控制系統，其軟件中多個中斷/異常嵌套運行、多個線程并發運行，并采用信號量保護了臨界資源；控制上采用DMP和角速度—角度雙閉環PID進行姿態控制，應用羅盤和激光測距儀實現偏航角與高度的閉環控制，融合光流傳感器、測距儀、姿態傳感器數據得到水平速度、位移，并對水平位移采用滑模控制器進行控制，解決了速度—位置雙閉環定點方式控制參數多、調參難的問題。最終通過飛行實驗驗證了本系統的可行性。

[參考文獻]

[1] 趙曉宏.國外軍用小型無人機發展現狀及趨勢[J].輕兵器，2017（11）：23-27.

[2] 路璐.基于微型無人機航測大比例尺地形圖的測繪[J].黑龍江科學，2022，13（8）：130-131.

[3] 陳至坤，張博，程朋飛.無人機定點降落中識別定點方法的研究[J].電光與控制，2021，28（8）：88-91.

[4] 於小杰，賀勇，劉盛華.基于金字塔光流法的無人機室內定點懸停設計[J].自動化技術與應用，2021，40（2）：16-18.

[5] 姚春鵬，許謹，楊榮德.基于UWB的無人機室內定位設計[J].中國科技信息，2020（16）：32-34.

[6] 丁一明，李文魁，張煦光.AUV運動控制器CAN通信模塊在RT-Thread上的設計與實現[J].自動化儀表，2021，42（9）：10-13.

[7] 湛柏明，馮浩文，黃海波，等.小型室內四旋翼飛行器懸停及避障系統設計[J].自動化儀表，2023，44（1）：42-48.

收稿日期：2023-02-13

作者簡介：楊兵（1996—），男，陜西人，助理工程師，研究方向：機載嵌入式軟件。