楊兵　王昆　馮揚(yáng)帆　陳福

摘 要：為了研究微型多旋翼的定點(diǎn)控制，設(shè)計(jì)并實(shí)現(xiàn)了一種基于RT-Thread的微型多旋翼定點(diǎn)控制系統(tǒng)。采用Digital Motion Processing庫(kù)和雙閉環(huán)PID進(jìn)行姿態(tài)控制，使用磁力計(jì)與激光測(cè)距儀進(jìn)行航向與高度的鎖定；融合光流傳感器、姿態(tài)、高度數(shù)據(jù)感知飛行器的水平速度、位置；針對(duì)水平位置，提出了基于滑?？刂破鞯奈⑿投嘈矶c(diǎn)控制方法。最終，通過(guò)實(shí)驗(yàn)驗(yàn)證了該系統(tǒng)的有效性。

關(guān)鍵詞：RT-Thread；微型多旋翼；定點(diǎn)控制；數(shù)據(jù)融合

中圖分類號(hào)：TP29? 文獻(xiàn)標(biāo)志碼：A? 文章編號(hào)：1671-0797（2023）09-0012-03

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

0? ? 引言

微型飛行器在情報(bào)偵察[1]、遙感測(cè)繪[2]等領(lǐng)域有著極高的應(yīng)用價(jià)值，而定點(diǎn)懸停是微型飛行器各項(xiàng)動(dòng)作的基礎(chǔ)，研究微型飛行器的定點(diǎn)懸停有著不可小覷的實(shí)用價(jià)值。

目前，主流的多旋翼定點(diǎn)方法有基于GPS的定點(diǎn)方法、基于視覺(jué)系統(tǒng)的定點(diǎn)方法[3]、基于光流的定點(diǎn)方法[4]以及基于UWB的定點(diǎn)方法[5]。本文以微型無(wú)人機(jī)光流定點(diǎn)控制為研究對(duì)象，完成了微型飛行器定點(diǎn)控制系統(tǒng)的設(shè)計(jì)與實(shí)現(xiàn)，提出了基于滑?？刂频亩c(diǎn)控制方法，并通過(guò)實(shí)驗(yàn)進(jìn)行了驗(yàn)證。

1? ? 總體方案與硬件設(shè)計(jì)

微控制器采用基于Coretex-M3內(nèi)核的STM32F103C8，內(nèi)置浮點(diǎn)運(yùn)算單元等，主頻最高72 MHz，非常適合微型飛控開(kāi)發(fā)。無(wú)線通信模塊為nRF24L01，支持實(shí)時(shí)雙向通信，用于控制指令的接收與控制器參數(shù)無(wú)線調(diào)試。姿態(tài)傳感器為MPU6050，內(nèi)置數(shù)字運(yùn)動(dòng)處理引擎。羅盤為HMC5883L，最大輸出頻率可達(dá)160 Hz。光流模塊為PMW3901MB，支持80 mm至無(wú)限遠(yuǎn)的工作距離。ToF測(cè)距傳感器為VL53L0X，測(cè)量頻率可達(dá)50 Hz?？傮w方案設(shè)計(jì)如圖1所示。

硬件設(shè)計(jì)如圖2所示，為盡可能減小PCB面積，便于在微型無(wú)人機(jī)上安裝，元器件采用雙面布局，完成后PCB為邊長(zhǎng)不超過(guò)4.5 cm的正方形。

2? ? 軟件設(shè)計(jì)

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

多線程調(diào)度方式、線程優(yōu)先級(jí)、線程棧大小、中斷優(yōu)先級(jí)等的設(shè)置是本設(shè)計(jì)中稍難的點(diǎn)，如果安排不合理，則會(huì)導(dǎo)致硬實(shí)時(shí)控制無(wú)法實(shí)現(xiàn)、線程死鎖、硬錯(cuò)誤等問(wèn)題。一般而言，在單核多任務(wù)系統(tǒng)中，并發(fā)運(yùn)行的主線程及其他各線程可視為后臺(tái)，而支持操作系統(tǒng)運(yùn)行的SysTick、PendSV中斷及其他中斷則可視為前臺(tái)。

除操作系統(tǒng)自帶的tShell和tidle線程外，其余線程負(fù)責(zé)完成遠(yuǎn)程控制指令接收，姿態(tài)、高度、光流信息的獲取等，具體如表1所示。

設(shè)置NVIC分組為兩位搶占優(yōu)先級(jí)、兩位響應(yīng)優(yōu)先級(jí)。SysTick中斷負(fù)責(zé)實(shí)現(xiàn)RTOS的調(diào)度，PendSV中斷負(fù)責(zé)上下文切換，定時(shí)器TIM4負(fù)責(zé)脈沖寬度調(diào)制波的發(fā)生，定時(shí)器TIM1工作于周期中斷模式，負(fù)責(zé)進(jìn)行4路脈沖寬度調(diào)制，完成電機(jī)轉(zhuǎn)速控制與姿態(tài)控制，USART1實(shí)現(xiàn)字符緩沖接收以支持FinSH，具體如表2所示。

3? ? 控制方法

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

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

整體控制設(shè)計(jì)如圖4所示，高度和航向控制較為簡(jiǎn)單，圖中不進(jìn)行說(shuō)明。姿態(tài)控制上選擇串級(jí)PID，內(nèi)環(huán)控制角速度，以100 Hz運(yùn)行，外環(huán)控制角度，以25 Hz運(yùn)行，同時(shí)將外環(huán)角度環(huán)輸出的期望角速度等份遞增地輸出至內(nèi)環(huán)角速度控制器以獲得平滑的控制效果。

定點(diǎn)過(guò)程中往往需要定航向和定高，由羅盤和測(cè)距儀可得出航向與高度，經(jīng)過(guò)簡(jiǎn)單的反饋控制即可實(shí)現(xiàn)航向和高度的鎖定。對(duì)于航向的鎖定采用簡(jiǎn)單的比例反饋控制即可實(shí)現(xiàn)良好的效果，對(duì)于高度采用比例積分微分控制可得到良好的效果，值得一提的是，經(jīng)過(guò)激光測(cè)距儀測(cè)得的距離需引入橫滾角和俯仰角校正[7]后才能得到高度。

采用式（1）計(jì)算所得偏航角存在漂移現(xiàn)象，因此根據(jù)磁力計(jì)在其x、y方向測(cè)得的磁感應(yīng)強(qiáng)度分量可計(jì)算得到偏航角，在計(jì)算前需要對(duì)磁力計(jì)進(jìn)行校正。

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

式中：Sx、Sy為根據(jù)光流傳感器得到的x、y方向的位移，初值均為0；L為測(cè)距傳感器測(cè)量結(jié)果；R為飛行器在距地面1 m時(shí)產(chǎn)生1個(gè)像素變化量對(duì)應(yīng)的位移；Kx為針對(duì)飛行器橫滾的校正系數(shù)；Ky為針對(duì)飛行器俯仰的校正系數(shù)；Kx、Ky和R均由實(shí)驗(yàn)測(cè)得。

光流速度由光流位移差分并經(jīng)一階低通濾波器得到，差分間隔不宜過(guò)短，否則計(jì)算所得光流速度會(huì)有很多毛刺，無(wú)法反映飛行器實(shí)際運(yùn)動(dòng)。

以橫滾通道為例分析定點(diǎn)控制過(guò)程，假定微型多旋翼定高后橫滾角γ保持在某非水平值γd＞0，由運(yùn)動(dòng)學(xué)分析可知多旋翼將向右做加速運(yùn)動(dòng)。因定高定點(diǎn)過(guò)程中運(yùn)動(dòng)速度低，故可忽略風(fēng)阻?？煞治龅贸觯?/p>

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

盡管采用速度—位置雙閉環(huán)控制可將無(wú)人機(jī)水平位移控制在零附近，實(shí)現(xiàn)定點(diǎn)控制，但該方法存在參數(shù)多、調(diào)參復(fù)雜的問(wèn)題，因此提出了基于滑模控制的多旋翼定點(diǎn)控制方法。

式中：pxd為期望的無(wú)人機(jī)位置，一般取0；s、z1、z2為滑?？刂破髦虚g變量；其余的c、k、η為控制器參數(shù)。

相比于速度—位置雙閉環(huán)PID控制，調(diào)試參數(shù)由6個(gè)下降至3個(gè)。

4? ? 飛行實(shí)驗(yàn)

對(duì)所設(shè)計(jì)的多旋翼定點(diǎn)控制系統(tǒng)進(jìn)行實(shí)驗(yàn)驗(yàn)證。依次調(diào)節(jié)角速度環(huán)PID、角度環(huán)PID參數(shù)，保證角度控制快速無(wú)靜差。校正系數(shù)Kx和Ky均取480.0像素，R取0.213 cm/（m·像素），滑?？刂浦衏=0.017 2，k=0.017 2，η=5×10-5。開(kāi)始光流定點(diǎn)后，微型飛行器在其x方向和y方向的位移曲線及位置如圖5所示。

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

5? ? 結(jié)語(yǔ)

本文采用RT-Thread設(shè)計(jì)并實(shí)現(xiàn)了一種微型多旋翼定點(diǎn)控制系統(tǒng)，其軟件中多個(gè)中斷/異常嵌套運(yùn)行、多個(gè)線程并發(fā)運(yùn)行，并采用信號(hào)量保護(hù)了臨界資源；控制上采用DMP和角速度—角度雙閉環(huán)PID進(jìn)行姿態(tài)控制，應(yīng)用羅盤和激光測(cè)距儀實(shí)現(xiàn)偏航角與高度的閉環(huán)控制，融合光流傳感器、測(cè)距儀、姿態(tài)傳感器數(shù)據(jù)得到水平速度、位移，并對(duì)水平位移采用滑模控制器進(jìn)行控制，解決了速度—位置雙閉環(huán)定點(diǎn)方式控制參數(shù)多、調(diào)參難的問(wèn)題。最終通過(guò)飛行實(shí)驗(yàn)驗(yàn)證了本系統(tǒng)的可行性。

[參考文獻(xiàn)]

[1] 趙曉宏.國(guó)外軍用小型無(wú)人機(jī)發(fā)展現(xiàn)狀及趨勢(shì)[J].輕兵器，2017（11）：23-27.

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

[3] 陳至坤，張博，程朋飛.無(wú)人機(jī)定點(diǎn)降落中識(shí)別定點(diǎn)方法的研究[J].電光與控制，2021，28（8）：88-91.

[4] 於小杰，賀勇，劉盛華.基于金字塔光流法的無(wú)人機(jī)室內(nèi)定點(diǎn)懸停設(shè)計(jì)[J].自動(dòng)化技術(shù)與應(yīng)用，2021，40（2）：16-18.

[5] 姚春鵬，許謹(jǐn)，楊榮德.基于UWB的無(wú)人機(jī)室內(nèi)定位設(shè)計(jì)[J].中國(guó)科技信息，2020（16）：32-34.

[6] 丁一明，李文魁，張煦光.AUV運(yùn)動(dòng)控制器CAN通信模塊在RT-Thread上的設(shè)計(jì)與實(shí)現(xiàn)[J].自動(dòng)化儀表，2021，42（9）：10-13.

[7] 湛柏明，馮浩文，黃海波，等.小型室內(nèi)四旋翼飛行器懸停及避障系統(tǒng)設(shè)計(jì)[J].自動(dòng)化儀表，2023，44（1）：42-48.

收稿日期：2023-02-13

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