本發(fā)明涉及一種高精度軌跡跟蹤的固定翼無(wú)人機(jī)控制系統(tǒng)及橫側(cè)向飛行軌跡控制方法，屬于導(dǎo)航定位與控制領(lǐng)域。

背景技術(shù)：

固定翼無(wú)人機(jī)因其飛行速度快，續(xù)航時(shí)間長(zhǎng)，可掛載荷大等優(yōu)點(diǎn)被廣泛應(yīng)用在敵情偵察，精確打擊，森林防火，勘測(cè)測(cè)繪等領(lǐng)域。但固定翼無(wú)人機(jī)的飛行范圍受到操作人員的視覺(jué)距離和遙控器無(wú)線電傳輸范圍限制，因此有必要設(shè)計(jì)一套具備高精度軌跡跟蹤的固定翼無(wú)人機(jī)自動(dòng)控制系統(tǒng)。

目前固定翼無(wú)人機(jī)采用單點(diǎn)GPS來(lái)提供位置信息，通過(guò)傳統(tǒng)的PID控制來(lái)實(shí)現(xiàn)軌跡跟蹤，跟蹤精度低，快速性差，抗風(fēng)性能差。無(wú)人機(jī)自動(dòng)飛行的核心問(wèn)題就是解決軌跡跟蹤問(wèn)題，傳統(tǒng)的控制方法以及傳感器的選型，無(wú)法滿足這一要求。

技術(shù)實(shí)現(xiàn)要素：

針對(duì)上述固定翼無(wú)人機(jī)軌跡跟蹤精度低快速性差等缺陷，本發(fā)明所要解決的技術(shù)問(wèn)題是提供一種高精度軌跡跟蹤的的固定翼無(wú)人機(jī)控制系統(tǒng)及橫側(cè)向飛行軌跡控制方法，提高軌跡跟蹤的快速性和跟蹤的精度，實(shí)現(xiàn)固定翼無(wú)人機(jī)的高質(zhì)量自動(dòng)飛行。

本發(fā)明為解決上述技術(shù)問(wèn)題采用以下技術(shù)方案：

一方面，本發(fā)明提供一種基于偏航距誤差修正的橫側(cè)向飛行軌跡控制方法，將偏航距誤差作為控制輸入經(jīng)過(guò)PI控制器后輸出，與L1算法給出的滾轉(zhuǎn)角疊加得到期望滾轉(zhuǎn)角，將期望滾轉(zhuǎn)角輸出給舵機(jī)，實(shí)現(xiàn)橫側(cè)向飛行軌跡的控制；

其中，期望滾轉(zhuǎn)角式中，為L(zhǎng)1算法給出的滾轉(zhuǎn)角，為偏航距誤差的放大系數(shù)，e_c為偏航距誤差，為偏航距誤差積分項(xiàng)系數(shù)，為側(cè)向速度的負(fù)反饋系數(shù)，為側(cè)向速度誤差。

另一方面，本發(fā)明提供一種固定翼無(wú)人機(jī)控制系統(tǒng)，包括飛行器平臺(tái)、飛行控制器和地面站部分，其中：

飛行器平臺(tái)包括固定翼機(jī)體、電子調(diào)速器、電機(jī)、槳葉、舵機(jī)；電機(jī)位于固定翼機(jī)體腰部，用來(lái)控制飛機(jī)的速度，通過(guò)PWM輸出驅(qū)動(dòng)電路與飛控計(jì)算機(jī)中輸出接口連接；舵機(jī)用來(lái)控制升降舵、副翼、方向舵，通過(guò)PWM輸出驅(qū)動(dòng)電路與飛控計(jì)算機(jī)中輸出接口連接；

飛行控制器包括飛控計(jì)算機(jī)、遙控器接收機(jī)、傳感器模塊、無(wú)線數(shù)傳模塊，傳感器模塊包括航姿傳感器、高度傳感器、空速計(jì)和位置傳感器，其中，飛控計(jì)算機(jī)根據(jù)如上所述的基于偏航距誤差修正的橫側(cè)向飛行軌跡控制方法對(duì)固定翼無(wú)人機(jī)的橫側(cè)向飛行軌跡進(jìn)行控制；

地面站部分包括地面站、地面無(wú)線數(shù)傳模塊，地面站通過(guò)地面無(wú)線數(shù)傳模塊與機(jī)載的無(wú)線數(shù)傳模塊進(jìn)行數(shù)據(jù)通訊。

作為本發(fā)明的進(jìn)一步優(yōu)化方案，位置傳感器采用精度為分米級(jí)的差分GPS。

作為本發(fā)明的進(jìn)一步優(yōu)化方案，無(wú)線數(shù)傳模塊通過(guò)串口與飛控計(jì)算機(jī)連接。

作為本發(fā)明的進(jìn)一步優(yōu)化方案，航姿傳感器采用掛載的方式，通過(guò)串口與飛控計(jì)算機(jī)連接。

作為本發(fā)明的進(jìn)一步優(yōu)化方案，高度傳感器采用氣壓計(jì)，采用板載的方式，通過(guò)I2C總線與飛控計(jì)算機(jī)連接。

作為本發(fā)明的進(jìn)一步優(yōu)化方案，空速計(jì)通過(guò)I2C總線與飛控計(jì)算機(jī)連接。

本發(fā)明采用以上技術(shù)方案與現(xiàn)有技術(shù)相比，具有以下技術(shù)效果：

(1)在同等GPS精度條件下，本發(fā)明具有更好的軌跡跟蹤效果，跟蹤精度高，響應(yīng)速度快；

(2)運(yùn)用總能量控制，在同等電量下顯著提高飛機(jī)的巡航時(shí)間；

(3)用新型的基于消除偏航距誤差的L1算法，提高了飛機(jī)的抗風(fēng)性能，使飛機(jī)在存在陣風(fēng)的情況下可以有較好的軌跡跟蹤效果。

附圖說(shuō)明

圖1是飛機(jī)能量變化關(guān)系圖；

圖2是L1算法原理圖；

圖3是L1算法的軌跡跟蹤示意圖；

圖4是飛機(jī)在側(cè)風(fēng)影響下的受力示意圖

圖5是固定翼無(wú)人機(jī)飛行控制系統(tǒng)的硬件實(shí)現(xiàn)與原理框圖

圖6是軌跡控制回路的結(jié)構(gòu)圖

圖7是側(cè)風(fēng)風(fēng)速6m/s時(shí)飛機(jī)的飛行軌跡圖

圖8是直線段飛行的偏航距誤差。

具體實(shí)施方式

下面結(jié)合附圖對(duì)本發(fā)明的技術(shù)方案做進(jìn)一步的詳細(xì)說(shuō)明：

本發(fā)明設(shè)計(jì)一種高精度軌跡跟蹤的固定翼無(wú)人機(jī)自動(dòng)控制系統(tǒng)，包括飛行器平臺(tái)，飛行控制器和地面站部分。其中飛行器平臺(tái)包括常規(guī)布局的固定翼機(jī)體、電子調(diào)速器、電機(jī)、槳葉、舵機(jī)、電池等部件；飛行控制器包括飛控計(jì)算機(jī)、遙控器接收機(jī)、傳感器模塊、無(wú)線數(shù)傳模塊等。傳感器模塊包括航姿傳感器、高度傳感器(氣壓計(jì))、空速計(jì)和位置傳感器。本發(fā)明為解決高精度軌跡跟蹤問(wèn)題，選用精度為分米級(jí)的差分GPS作為位置傳感器，控制方法中采用一種新型的基于偏航距誤差消除的L1算法。

電機(jī)位于機(jī)體腰部用來(lái)控制飛機(jī)的速度，通過(guò)PWM輸出驅(qū)動(dòng)電路與飛控計(jì)算機(jī)中輸出接口連接，舵機(jī)用來(lái)控制升降舵、副翼、方向舵，通過(guò)PWM輸出驅(qū)動(dòng)電路與飛控計(jì)算機(jī)中輸出接口連接，航姿傳感器SBG采用掛載的方式，通過(guò)串口與飛控計(jì)算機(jī)相連接，氣壓計(jì)采用板載的方式，通過(guò)I2C總線與飛控計(jì)算機(jī)連接；無(wú)線數(shù)傳模塊通過(guò)串口與飛控計(jì)算機(jī)連接；地面站部分的地面無(wú)線數(shù)傳模塊與機(jī)載部分的無(wú)線數(shù)傳模塊進(jìn)行數(shù)據(jù)通訊；空速計(jì)模塊通過(guò)I2C總線與飛控計(jì)算機(jī)連接；EEPROM模塊通過(guò)I2C總線與飛控計(jì)算機(jī)連接。

本發(fā)明采用100HZ的控制頻率，完成對(duì)傳感器的讀取與處理，得到期望的舵面角度通過(guò)PWM輸出給舵機(jī)，從而實(shí)現(xiàn)飛機(jī)姿態(tài)的變化進(jìn)而改變飛機(jī)的飛行軌跡。

航線飛行過(guò)程中需要控制飛機(jī)的飛行高度、飛行速度以及飛機(jī)的橫側(cè)向飛行軌跡。本發(fā)明中，高度與速度控制采用總能量控制的方法實(shí)現(xiàn)飛行速度與高度變化的平緩過(guò)度，飛機(jī)的橫側(cè)向飛行軌跡控制采用新型的基于偏航距誤差消除的L1算法來(lái)實(shí)現(xiàn)高精度的軌跡控制。

總能量控制利用了物理中的能量守恒原理，由飛機(jī)腰部的電機(jī)帶動(dòng)螺旋槳產(chǎn)生的能量分配到重力勢(shì)能和動(dòng)能，其中，E_T是電機(jī)帶動(dòng)螺旋槳產(chǎn)生的能量，G是飛機(jī)的重力，H是飛機(jī)飛行的高度，V是飛機(jī)的飛行速度。如圖1所示，當(dāng)電機(jī)能量發(fā)生變化時(shí)飛機(jī)的飛行速度和飛行高度也發(fā)生變化時(shí)，滿足其中，ΔE_T是能量的變化，V₀是T₀時(shí)刻的飛行速度，V₁是T₁時(shí)刻的飛行速度，ΔH是高度的變化量。

油門(mén)通道控制律為δ_T＝T_vcr+k_PEe_E，其中，e_E＝V_cr²-V_cu²+k_eHe_H為能量誤差，V_cr為巡航速度，V_cu為飛機(jī)當(dāng)前的飛行速度，e_H是高度誤差，k_eH為重力勢(shì)能的轉(zhuǎn)化系數(shù)，T_vcr是巡航油門(mén)，δ_T是期望的油門(mén)值。

高度控制律為其中，k_pVH是高度變化速度誤差放大系數(shù)，e_H＝H_target-H_current為高度誤差，k_pH為高度誤差放大系數(shù)，k_iVH為高度變化速度誤差積分項(xiàng)系數(shù)，k_rtp滾轉(zhuǎn)與俯仰的耦合系數(shù)。

本發(fā)明中，飛機(jī)的橫側(cè)向飛行軌跡控制采用新型的基于偏航距誤差修正的L1算法。L1算法是一種理想條件下的軌跡控制算法，如圖2所示，這種方法是在期望軌跡上選取參考點(diǎn)，利用參考點(diǎn)和飛機(jī)當(dāng)前點(diǎn)計(jì)算飛機(jī)的側(cè)向加速度其中，a是側(cè)向加速度，L₁＝2Rsinη是飛機(jī)當(dāng)前位置與參考點(diǎn)之間的距離，R是飛機(jī)做圓周運(yùn)動(dòng)的半徑，η是飛機(jī)飛行速度與L₁直線的夾角。根據(jù)得到的側(cè)向加速度，計(jì)算期望的滾轉(zhuǎn)角其中，θ是俯仰角，從而實(shí)現(xiàn)飛機(jī)橫側(cè)向的軌跡跟蹤，如圖3所示。

但是，當(dāng)飛機(jī)受大風(fēng)干擾時(shí)會(huì)存在偏航距的穩(wěn)態(tài)誤差，如圖4所示，當(dāng)存在側(cè)風(fēng)時(shí)飛機(jī)需要一定的滾轉(zhuǎn)角度提供向心力以抵消側(cè)風(fēng)的影響，因此L1算法給出的期望滾轉(zhuǎn)角提供的向心力不足以使飛機(jī)修正已存在的偏航距。為了提高軌跡跟蹤的抗風(fēng)性，本發(fā)明提出了一種基于偏航距誤差修正的L1算法，將偏航距誤差作為控制輸入經(jīng)PI控制器輸出期望滾轉(zhuǎn)角，與L1算法給出的期望滾轉(zhuǎn)角疊加得到最終的控制量輸出給舵機(jī)，橫側(cè)向的軌跡控制律為其中，δ_φ為期望滾轉(zhuǎn)角，為L(zhǎng)1算法給出的期望滾轉(zhuǎn)角，為偏航距誤差的放大系數(shù)，e_c為偏航距誤差，為偏航距誤差積分項(xiàng)系數(shù)，是側(cè)向速度的負(fù)反饋系數(shù)，側(cè)向速度誤差。

本發(fā)明還提供一種高精度軌跡跟蹤的固定翼無(wú)人機(jī)自動(dòng)控制系統(tǒng)，采用常規(guī)布局的固定翼無(wú)人機(jī)機(jī)體結(jié)構(gòu)，飛機(jī)的飛行姿態(tài)通過(guò)舵機(jī)帶動(dòng)副翼、升降舵和方向舵來(lái)控制。飛機(jī)的飛行速度由電機(jī)轉(zhuǎn)速控制。如圖5所示的該固定翼無(wú)人機(jī)飛行控制系統(tǒng)的硬件實(shí)現(xiàn)與原理框圖，包括機(jī)載部分和地面控制部分。機(jī)載部分包括所有機(jī)載傳感器，飛控計(jì)算機(jī)，電源模塊，無(wú)線傳輸模塊、遙控器接收機(jī)，電機(jī)及舵機(jī)。地面部分包括地面站，遙控器，GPS基站和無(wú)線傳輸模塊。飛控計(jì)算機(jī)采用STM32F407作為控制器，有豐富的硬件接口資源。電機(jī)與舵機(jī)通過(guò)PWM輸出驅(qū)動(dòng)電路與控制器相連接；氣壓計(jì)、EEPROM、空速計(jì)通過(guò)IIC總線與控制器連接；無(wú)線數(shù)傳模塊通過(guò)UART串口與控制器連接；航姿傳感器SBG通過(guò)UART串口與控制器連接；差分GPS通過(guò)USART串口與控制器連接；接收機(jī)通過(guò)UART串口與控制器連接；差分GPS基站和移動(dòng)站通過(guò)無(wú)線電數(shù)傳連接；電源模塊通過(guò)AD接口與控制器連接。

其中，主要的傳感器模塊有：

姿態(tài)傳感器模塊采用的是法國(guó)SBG公司的IG-500N，包含方位測(cè)量解決方案、動(dòng)態(tài)傳感器、姿態(tài)航向參考系統(tǒng)(A.H.R.S)、全球定位系統(tǒng)(GPS)精度增強(qiáng)的姿態(tài)航向參考系統(tǒng)、靜態(tài)傳感器、兩軸傾角儀、三維羅盤(pán)等。可以用在無(wú)人機(jī)、車(chē)輛運(yùn)動(dòng)分析、航天、機(jī)器人、航海等領(lǐng)域。具有抗機(jī)械抖動(dòng)和撞擊的優(yōu)異性能，能直接輸出高精度的3自由度角度、3自由度角速率和3自由度加速度等數(shù)據(jù)。靜態(tài)條件下，滾轉(zhuǎn)角與俯仰角測(cè)量偏差在-0.5°～+0.5°之間，偏航角測(cè)量偏差在-1.0°～+1。0°；動(dòng)態(tài)情況下，其測(cè)量偏差分別為-1.0°-1.0°之間。姿態(tài)和速度的輸出頻率在0～500Hz可調(diào)，可用串口與主控制器連接。工作電壓在3.3V到30V，功耗550mW。

無(wú)線數(shù)傳模塊，用于實(shí)現(xiàn)飛機(jī)和地面站之間的通信功能，將機(jī)載參數(shù)下載到地面站或?qū)⒌孛嬲镜臄?shù)據(jù)上傳到飛控中。該設(shè)計(jì)選用X-Tend是MAXStream公司推出的一款超遠(yuǎn)距離無(wú)線數(shù)傳電臺(tái)。在最高傳輸速率115kbps下，其室內(nèi)距離可達(dá)900米，室外可視傳輸距離最大達(dá)64公里。X-Tend內(nèi)置了256位的AES加密算法，可以完全保證用戶的數(shù)據(jù)安全。通過(guò)串口進(jìn)行數(shù)據(jù)傳輸。

差分GPS提供飛機(jī)當(dāng)前位置的定位，以及飛機(jī)的速度信息和高度信息。使用的是NovAtel推出的OEM617D，OEM617D是NovAtel基于OEM6全新硬件平臺(tái)研制開(kāi)發(fā)的兼容BDS多頻測(cè)向板卡。該板卡支持雙天線輸入，能夠同時(shí)使用GPS、GLONASS、BDS的雙頻信號(hào)進(jìn)行測(cè)向、RTK解算。同時(shí)，小尺寸、超低功耗、重量輕、接口靈活等特點(diǎn)尤其適合于各領(lǐng)域的集成應(yīng)用。

圖6是軌跡控制回路的結(jié)構(gòu)圖，根據(jù)地面站輸入的軌跡指令，解算出飛機(jī)的目標(biāo)位置，將目標(biāo)位置與差分GPS返回的飛機(jī)當(dāng)前位置輸入給L1軌跡控制器得到L1算法的期望的滾轉(zhuǎn)角再利用目標(biāo)位置與差分GPS返回的飛機(jī)當(dāng)前位置計(jì)算偏航距誤差e_c，將計(jì)算得到的偏航距誤差輸入到PI控制器中得到其對(duì)應(yīng)的期望的滾轉(zhuǎn)角其中

將L1軌跡控制器和基于偏航距誤差消除的PI控制器得到滾轉(zhuǎn)角相疊加，得到最終的期望滾轉(zhuǎn)角δ_φ輸入到飛機(jī)的姿態(tài)控制回路，最后通過(guò)舵機(jī)輸出使飛機(jī)滾轉(zhuǎn)，從而實(shí)現(xiàn)軌跡的改變。

如圖7所示，當(dāng)存在較大正側(cè)風(fēng)時(shí)飛機(jī)仍能保持高精度的軌跡飛行，其中，黑色是飛機(jī)實(shí)際的飛行軌跡，白色是在地面站寫(xiě)入的期望的飛行軌跡；如圖8所示為飛機(jī)在圖7中航點(diǎn)4和航點(diǎn)5之間直線段的飛行，偏航距誤差在-0.5m～+0.5m范圍內(nèi)，是高精度的軌跡跟蹤。

以上所述，僅為本發(fā)明中的具體實(shí)施方式，但本發(fā)明的保護(hù)范圍并不局限于此，任何熟悉該技術(shù)的人在本發(fā)明所揭露的技術(shù)范圍內(nèi)，可理解想到的變換或替換，都應(yīng)涵蓋在本發(fā)明的包含范圍之內(nèi)，因此，本發(fā)明的保護(hù)范圍應(yīng)該以權(quán)利要求書(shū)的保護(hù)范圍為準(zhǔn)。