引言

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

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

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

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

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

2軟件設(shè)計

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

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

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

設(shè)置NVIC分組為兩位搶占優(yōu)先級、兩位響應(yīng)優(yōu)先級。sysTick中斷負(fù)責(zé)實(shí)現(xiàn)RT0s的調(diào)度,PendsV中斷負(fù)責(zé)上下文切換,定時器TIM4負(fù)責(zé)脈沖寬度調(diào)制波的發(fā)生,定時器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),采用DigitalMotionProcessing庫獲取四元數(shù)(g0,g1,g2,g3)及三軸角速度(ox,oy,o:),按式(1)可由四元數(shù)轉(zhuǎn)換得歐拉角(o,y,9),配合三軸角速度作為姿態(tài)控制的輸入。

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

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

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

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

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

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

以橫滾通道為例分析定點(diǎn)控制過程,假定微型多旋翼定高后橫滾角y保持在某非水平值yd>0,由運(yùn)動學(xué)分析可知多旋翼將向右做加速運(yùn)動。因定高定點(diǎn)過程中運(yùn)動速度低,故可忽略風(fēng)阻。可分析得出:

式中:px為水平位置:g為重力加速度。

盡管采用速度一位置雙閉環(huán)控制可將無人機(jī)水平位移控制在零附近,實(shí)現(xiàn)定點(diǎn)控制,但該方法存在參數(shù)多、調(diào)參復(fù)雜的問題,因此提出了基于滑?？刂频亩嘈矶c(diǎn)控制方法。

式中:pxd為期望的無人機(jī)位置,一般取0:s、:1、:2為滑?？刂破髦虚g變量:其余的c、k、7為控制器參數(shù)。

相比于速度一位置雙閉環(huán)Pp1控制,調(diào)試參數(shù)由D個下降至3個。

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

對所設(shè)計的多旋翼定點(diǎn)控制系統(tǒng)進(jìn)行實(shí)驗(yàn)驗(yàn)證。依次調(diào)節(jié)角速度環(huán)Pp1、角度環(huán)Pp1參數(shù),保證角度控制快速無靜差。校正系數(shù)Kx和Ky均取46080像素,R取08213cm/(m·像素),滑模控制中c=080172,k=080172,7=5×10-5。開始光流定點(diǎn)后,微型飛行器在其x方向和y方向的位移曲線及位置如圖5所示。

可以看到微型飛行器位置保持在(0,0)附近,x、y方向位置均值分別為1812cm、-086Dcm,方差分別為1D839cm2、328D9cm2,成功實(shí)現(xiàn)了定點(diǎn)控制。

5結(jié)語

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