四旋翼無人機路徑跟蹤控制系統軟硬件設計與實驗研究

闞亞雄

（鎮江高等?？茖W?，F代裝備制造學院，江蘇鎮江 212028）

0 引言

四旋翼無人機在近年不斷成為人們日常生活中的關注熱點[1]。目前國內相關四旋翼無人機的研究已初步展開，潘春榮[2]研究了無人機姿態測量系統的設計，以ARM微處理器作為處理單元，采用互補濾波進行姿態解算，基本滿足了無人機姿態測量的要求。劉輝邦等[3]和WUY等[4]的四軸無人機系統采用STM32單片機為主控制器單元，接受系統多傳感器數據，并計算飛行器航姿。文中的飛行控制系統通過濾波算法提高了傳感器數據的精確性，但對飛行器的軌跡跟蹤等實驗較少涉及。張新英等[5]和SCHIRRER A[6]討論的四旋翼無人機自主循跡系統設計，提出了四旋翼利用機載攝像頭進行自主循跡的方案，通過單目或雙目相機視覺測距系統上經過圖像采集、雙目矯正、雙目立體匹配等步驟得到視覺結果，實現了四旋翼飛行器的自主飛行功能。以上的研究為本文的四旋翼無人機軌跡研究提供了有益的參考，但是文獻中自主軌跡循跡系統的智能度不高，依賴實驗場地中的黑色標線才可以完成穩定流暢的循跡任務。同時，以上關于上位機PC軟件界面尤其是四旋翼無人機的實時狀態直觀顯示的設計較少，缺少進行姿態控制、路徑跟蹤的仿真實驗。因此，一種新型的四旋翼無人機路徑跟蹤系統軟硬件研究顯得尤為必要。

本文以某四旋翼無人機實物為研究對象，提出一種基于STM32微處理器的四旋翼無人機路徑跟蹤控制系統的設計方案，闡述了控制系統的總體結構、硬件電路的設計、軟件算法的設計與實現，在軟件仿真和路上實驗中實現四旋翼無人機的基本位姿控制、路徑跟蹤、自主避障、機上各傳感器的數據采集與通信并能實現上位控制系統對無人機直觀的實時狀態顯示功能。

1 控制系統總體框架

1.1 某小型四旋翼無人機控制系統結構

四旋翼無人機控制系統主要由上位機地面控制系統和下位機機載控制系統組成。其中下位機機載控制系統以STM32F103RCT6嵌入式芯片為系統的核心芯片，主要負責與上位機控制芯片的無線通信、傳感器數據讀取、數據集成、控制算法擬合、電機控制和電池管理。機載傳感器包括采用MPU9250微慣導運動處理組件、BMP280高度氣壓傳感器、無線通信模塊和12 V鋰電池，本系統的總體結構如圖1所示。四旋翼無人機控制系統主控芯片與超聲波傳感器和紅外傳感器之間采用USART串口通訊，主控芯片與微慣導運動組件和高度氣壓計之間采用I2C總線通訊協議實現姿態角和速度數據；上位機地面控制系統與下位機機載控制系統間通過無線通訊模塊實現數據指令的互傳。

圖1 四旋翼無人機控制系統硬件分布

1.2 四旋翼主控芯片最小系統設計

如圖2所示，最小系統電路圖給出了控制電路的其他引腳定義，如PA0、PA1、PA2、PA3是四旋翼無人機4路電機的PWM控制信號輸出引腳，IMU_SDA:PB6、IMU_SDL:PB7為MPU9250的I2C數據接口。系統通過NRF_RX:PA4、NRF_TX:PA5和NRF_FLOW:PA6引腳與無線通訊模塊連接等。

圖2 四旋翼主控芯片STM32F103RCT6最小系統

1.3 四旋翼無人機動力學建模及控制算法流程設計

根據相關文獻資料[6]，四旋翼無人機4個對稱安裝的旋翼正反轉后產生的升力、下降力和不同旋翼之間的速度差可以實現四旋無人機的在三維空間里的前進、后退、上升、下降、縱搖和轉向6個自由度運動。根據拉格朗日方程[7]，四旋翼無人機的六自由度動力學模型可表示為：

式中：x、y、z分別為四旋翼無人機在慣性坐標系中X、Y、Z方向上的位置坐標；、φ、θ、ψ分別為四旋翼無人機的滾轉角、俯仰角和偏航角；l為四旋翼無人機旋翼末端到質心的直線距離；m為四旋翼無人機的負載總重量；Ii為轉動慣量[6]；U1、U2、U3和U4為計算得到的中間控制輸入，其定義為：

式中：b為升力系數；Ωi為第i個旋翼的轉速；d為反扭矩系數。

本文提出了一種將滑模法（SMC），反步法（backstepping）和自適應率控制（Adaptive Control）相結合的自適應反步滑模算法（ABSMC），其特點在于具有較好的魯棒性、較好的跟蹤精度和較強的抗干擾容錯能力。根據ABSMC算法演算出四旋翼無人機滾轉角自適應容錯控制率U2為：

同理偏航角控制率U4為：

2 四旋翼控制系統軟硬件設計與實現

2.1 機載傳感器電路設計

如圖3所示，姿態傳感器采用MPU9250微慣導9軸運動處理組件。主要功能是感知無人機靜態和動態過程中實時的姿態角變化（俯仰角、橫滾角、航向角），靜態角度誤差為0.1°，動態角度誤差為1°，基本滿足要求。氣壓傳感器BMP280通過實時監測無人機所處環境氣壓值間接得到飛行高度數據。MPU9250微慣導9軸運動處理組件和BMP280氣壓傳感器固定安裝在四旋翼無人機中心位置，均采用I2C通信方式與主控制芯片實現數據交換。

圖3 四旋翼無人機控制系統位姿傳感器和氣壓傳感器電路設計

2.2 直流電機及舵機驅動電路設計

四旋翼無人機系統采用直流電機驅動，無人機依靠4個空心杯直流電機不同速度的正反轉控制實現期望的飛行動作、避障動作和軌跡跟蹤動作，因此驅動電路的設計是四旋翼的運動性能指標的關鍵。本系統采用4個微型716專用無刷直流電機，響應速度快、調速范圍大及可靠性高。轉速最快可達45 000 r/min。如圖4所示，每個MOS管SI2302控制1個電機，實際使用中引腳與主控制器STM32的PWM輸出信號端口PA0、PA1、PA2、PA3相連，當信號端口收到高電平狀態時，其控制的直流電機處于運行狀態，電機正傳、反轉、剎車等，反之處于停止狀態。

圖4 四旋翼無人機控制系統驅動電路

2.3 四旋翼無人機控制系統軟件程序設計

圖5 所示為四旋翼無人機控制系統軟件流程。首先，四旋翼未工作時系統處于待機模式，STM32屬于斷電狀態。當按鍵按下后機載STM32系統進行初始化，同時地面控制站進行界面初始化。然后，上位機地面控制站將啟動RadiolinkTask進程等待機載STM32主控芯片的連接申請。連接確認后將獲取下位機機載傳感器中的姿態角、四旋翼無人機高度和氣壓等的實時數據，簡單校對后使用Sendto（）函數轉送地面控制站，同時使用rev（）函數將地面控制站發送來的控制命令以串口的形式發送給機載STM32系統，完成地面控制命令的傳輸。機載STM32控制器運行AtkpRxAnlTask進程接受處理地面控制站下發的數據，解算出目標位置坐標，再下發到StabilizerTask控制進程中去。StabilizerTask進程包含姿態解算互補濾波算法ImuUpdate（），更新周期為100Hz。STM32芯片讀取MPU9250傳感器和UM220傳感器采集到的加速度計、微慣導陀螺儀的數據和GPS/北斗位置定位信息后利用ImuUpdate（）函數對數據進行低通濾波和偏置調整，數據融合后輸出四旋翼實際的姿態數據和位置坐標信息。還包含自適應姿態控制函數AttitudeControl（）和自適應位置控制函數PositionControl（），其中自適應姿態控制函數，更新周期為300 Hz，期望姿態角度和目標位置信息由地面控制站下達后，與ImuUpdate（）函數輸出的四旋翼實際角度和實際位置值做差值比較，利用偏差值作為姿態控制器和位置控制器的輸入值，經過自適應容錯算法的計算后，輸出控制目標姿態和飛行到目標位置值所需要的電機油門參數，最后發送給運動控制函數MotorInit（），從而通過PWM控制電機使四旋翼到達目標位置并獲得目標姿態角。控制軟件程序集成燒錄在STM32微處理器中，開發工具為Keil uVision5，開發時采用C語言編制。

圖5 四旋翼無人機控制系統軟件流程

2.4 上位機地面控制站界面設計及實現

圖6 所示為本四旋翼無人機控制系統的地面控制站人機交互界面。地面控制站軟件整體分為機載傳感器數據采集區、數據顯示區、手動控制區和自主路徑跟蹤區。當地面控制接收到數據后，通過無線通信模塊實現機載傳感器的數據接收，將數據上傳到機載傳感器采集區，Labview不斷通過VISA配置數據、讀取數據并將讀取后的數據存儲到地面控制站芯片的內存中，再使用儀表盤或數字形式進行實現姿態角、航速、高度和路徑跟蹤的圖像顯示。人機交互界面仍設計有遙控和自動飛行的切換按鈕。當按鈕按下時，系統切換到地面控制人員的遙控飛行模式，通過人機交互界面上的飛行控制界面可以任意實時改變飛行器的姿態角、加減速度和急停返航動作。地面控制站的人機交互界面軟件可以幫助地面控制人員更好、更直觀地觀察顯示的參數并實時給四旋翼系統發送控制指令。

圖6 四旋翼無人機地面控制站人機交互界面

3 仿真實驗結果與分析

3.1 運動控制仿真實驗

本文研究的對象是某型四旋翼無人機，重量1.5 kg，Ix＝Iy=1.745×10-2kg·m2，Iz=3.175×10-2kg·m2，l＝0.225 m，b＝4 N·m，d＝0.223 N/(m/s)2，動力方式為4電機推動，輸出功率10 W，最大航速8 m/s。仿真實驗平臺為Matlab2019b，在四旋翼無人機模型的基礎上進行仿真研究。假設四旋翼的初始姿態角均為0，初始位置為原點，即（x＝0，y＝0，z＝0），其所需控制的期望位置擬合為三維軌跡為水平面內和垂直面內的三角函數曲線，仿真系統分別向四旋翼無人機下達控制指令：

位置軌跡跟蹤結果如圖7所示。由圖可以看出基于自適應容錯算法的四旋翼位置控制器對三維軌跡有著良好的控制效果。跟蹤過程中，距離誤差不超過±0.7 m，x方向的速度誤差不超過1.8 m/s。在0時刻范圍內存在較大偏差是由于期望的位置軌跡所需的速度方向與實際速度方向偏差較大所導致的。仿真實驗中控制系統在6 s時同時受到X、Y、Z軸的外力干擾，X、Y、Z軸方向各受到3 N的干擾力。觀察受干擾后的跟蹤曲線，在受到外界干擾后能夠快速恢復原控制目標的要求。基于自適應反步滑模算法的路徑跟蹤系統可以實現四旋翼無人機的三維路徑跟蹤，具有一定的抗干擾能力，實現了設計目標。

圖7 四旋翼無人機路徑跟蹤仿真曲線

3.2 實物實驗

四旋翼無人機控制系統在人工遙控和自主路徑跟蹤模式下分別進行了實驗，首先讓四旋翼無人機進行一段路徑軌跡跟蹤飛行，設定在飛行過程中，無人機的飛行高度z＝0.7 m，無人機的偏航角始終為0，無人機跟蹤一段雙扭線軌跡。四旋翼無人機實物實驗結果如圖8所示。

如圖9所示，四旋翼無人機在跟蹤路徑軌跡路線時，跟蹤快速性較好，第5 s時即進行期望路徑，跟蹤時軌跡誤差在z方向較小，穩定在0.7 m。而在x、y方向上存在著一定的誤差，誤差范圍在0.1 m以內，同時姿態角誤差小于±3°具有良好的位置跟蹤控制效果?；緦崿F了系統的運動控制、路徑跟蹤和機上多傳感器的數據采集收發與集成處理顯示。測試結果顯示系統軟硬件基本滿足設計要求。

圖8 四旋翼無人機實物實驗

圖9 四旋翼無人機路徑跟蹤實驗曲線

4 結束語

本文針對四旋翼無人機樣機開發了一套具備三維路徑跟蹤功能的控制系統，研究并介紹了該系統的軟硬件分布，主控芯片、MPU9250微慣導傳感器、BMP280氣壓傳感器和716直流電機的軟硬件設計。該系統具備了包括四旋翼無人機的直線運動、調整姿態角、三維路徑跟蹤在內的飛行路徑跟蹤能力，同時可以實現機載傳感器數據的實時采集和系統融合，使得地面控制站操作人員可以遠程調整四旋翼無人機的飛行狀態，觀察機上數據。在今后的研究中將此基礎上重點研究四旋翼無人機的導航規劃功能，提高該系統的實用性和穩定性。