周銳森 馮友兵

（江蘇科技大學(xué)電子信息學(xué)院 鎮(zhèn)江 212003）

1 引言

無人機(jī)無人車聯(lián)合運(yùn)動(dòng)作為異構(gòu)智能體協(xié)同運(yùn)動(dòng)控制重要組成部分［1］，受到國內(nèi)外普遍關(guān)注。無人車在危險(xiǎn)、骯臟環(huán)境下的救援運(yùn)輸能力與無人機(jī)在災(zāi)情探測、搜救、檢查的能力具有很強(qiáng)的互補(bǔ)性，二者協(xié)同可以有效拓寬UAV以及UGV的應(yīng)用范圍，在軍事和民用活動(dòng)中都具有很大的實(shí)用價(jià)值［2］。

當(dāng)前無人器聯(lián)合運(yùn)動(dòng)方法主要有基于行為法、虛擬結(jié)構(gòu)法、人工勢場法、領(lǐng)導(dǎo)-跟隨法等。文獻(xiàn)［4］基于反步法運(yùn)用行為法生成虛擬機(jī)器人實(shí)現(xiàn)無人車的聯(lián)合運(yùn)動(dòng)控制；文獻(xiàn)［5］根據(jù)領(lǐng)導(dǎo)-跟隨法基于Lyapunov方法設(shè)計(jì)狀態(tài)觀測器實(shí)現(xiàn)多無人機(jī)的聯(lián)合運(yùn)動(dòng)跟蹤控制。其中一致性算法已經(jīng)有大量研究［6～7］，比如文獻(xiàn)［3］基于虛擬結(jié)構(gòu)法將自適應(yīng)拓?fù)渫ㄐ排c一致性算法相結(jié)合實(shí)現(xiàn)多無人機(jī)的聯(lián)合運(yùn)動(dòng)控制，可是一般都只是將聯(lián)合運(yùn)動(dòng)的成員看作質(zhì)點(diǎn)忽略其模型運(yùn)動(dòng)特征，在建立了無人器非線性模型后根據(jù)位置速度誤差得到控制率。卻忽略了模型本身運(yùn)動(dòng)中姿態(tài)控制的約束問題，雖然有學(xué)者提出基于hp自適應(yīng)偽譜法的控制［8］，但約束只考慮到轉(zhuǎn)換矩陣中歐拉角奇異值問題而忽略實(shí)際工程中的物理意義。在實(shí)際工程中，不論是四旋翼無人機(jī)還是無人車都會(huì)因?yàn)榭刂戚斎肓窟^大或者對(duì)其變化率過大，造成無人器過分響應(yīng)導(dǎo)致飛機(jī)翻機(jī)墜毀、無人車失聯(lián)等問題。

模型預(yù)測控制可以將運(yùn)動(dòng)系統(tǒng)實(shí)際的約束條件加入預(yù)測運(yùn)動(dòng)中，實(shí)現(xiàn)最優(yōu)控制。Ahmed Hafez等［9～10］提出一種基于模型預(yù)測控制的無人機(jī)無人車聯(lián)合運(yùn)動(dòng)方案，相比其他方法能夠更好的控制對(duì)智能體運(yùn)動(dòng)的約束。本文參考相關(guān)文獻(xiàn)并結(jié)合實(shí)際工程的經(jīng)驗(yàn)，在滿足無人機(jī)無人車聯(lián)合運(yùn)動(dòng)的要求下，對(duì)無人機(jī)無人車模型分別進(jìn)行各自要求解耦變化得到符合實(shí)際無人機(jī)無人車聯(lián)合運(yùn)動(dòng)的四旋翼無人機(jī)狀態(tài)空間模型以及無人車模型。并引入虛擬領(lǐng)航者作為領(lǐng)導(dǎo)者，各個(gè)智能體在聯(lián)合運(yùn)動(dòng)位置要求下將領(lǐng)導(dǎo)者軌跡幾何計(jì)算得到各自參考軌跡，完成聯(lián)合運(yùn)動(dòng)。

2 四旋翼無人機(jī)與無人車的非線性動(dòng)力學(xué)模型轉(zhuǎn)化

2.1 四旋翼無人機(jī)非線性動(dòng)力學(xué)模型線性化變換

如圖1所示，慣性坐標(biāo)系為OE—XEYEZE，OB—XBYBZB為機(jī)體標(biāo)系。建立準(zhǔn)確的無人機(jī)模型是設(shè)計(jì)控制器關(guān)鍵，為便于求解，對(duì)模型做出下列假設(shè)：

圖1 四旋翼結(jié)構(gòu)示意圖

1）四旋翼是剛體；

2）四旋翼質(zhì)量轉(zhuǎn)動(dòng)慣量不變；

3）四旋翼重心和幾何重心一致。

其中四旋翼無人機(jī)質(zhì)量為m；重力加速度為g；設(shè)［Vx Vy Vz］T為慣性坐標(biāo)系下的速度矢量；［x y z］T為無人機(jī)慣性坐標(biāo)系下的質(zhì)心位置；［Фθ Ψ］T為無人機(jī)機(jī)體坐標(biāo)系下姿態(tài)角分別為滾轉(zhuǎn)角、俯仰角、偏航角。

利用Newton-Euler法［13］忽略空氣阻力可將動(dòng)力模型描述為

式中U1為四旋翼無人機(jī)升力輸入；U2、U3、U4為姿態(tài)控制輸入量，由下級(jí)PID姿態(tài)控制飛控控制；l為每個(gè)旋翼末端到飛行器重心距離；Ii為每個(gè)軸轉(zhuǎn)動(dòng)慣量；Ki為阻力系數(shù)。

一 般 將U1（cosФcosΨsinθ+sinФsinΨ）、U1（cosФsinθsinΨ）以及U1（cosФcosθ）當(dāng)做XYZ軸的輸入U(xiǎn)XUYUZ獨(dú)立設(shè)計(jì)其控制率［14］，并把UXUYUZ作為下級(jí)姿態(tài)控制量解算歐拉角。然而這樣在實(shí)際工程中很難得到XYZ軸位移與各個(gè)歐拉角的直接關(guān)系；同時(shí)由于耦合的關(guān)系，它們相互影響也很難使得所謂控制率能夠精確實(shí)現(xiàn)；在實(shí)際工程的模型預(yù)測控制中無法對(duì)應(yīng)找到準(zhǔn)確的輸入約束。但是由于無人機(jī)無人車聯(lián)合運(yùn)動(dòng)的特殊性，可進(jìn)一步變形：

1）設(shè)置無人機(jī)在固定高度，所以不需要Z軸變化，Z軸的合力為0，速度以及加速度都為0：

2）在很多參考文獻(xiàn)，如文獻(xiàn)［4］～［5］都有預(yù)先限定偏航角完成四旋翼無人機(jī)運(yùn)動(dòng)控制，所以可以限定偏航角為0并帶入原先模型，于是可得：

將gtanθ和-gtanФsecθ作為新的輸入U(xiǎn)XUY。

綜合式（1）～（3）可以得到聯(lián)合運(yùn)動(dòng)控制中無人機(jī)狀態(tài)方程：

其中PA=［xA yA vXA vYA］T是無人機(jī)位置速度矢量，UA=［UXA UYA］T為無人機(jī)輸入控制量，其對(duì)應(yīng)的姿態(tài)控制量為滾轉(zhuǎn)角φ，俯仰角θ。

2.2 無人車運(yùn)動(dòng)模型

由于無人車為一階運(yùn)動(dòng)系統(tǒng)較為簡單，本文無人車采用兩輪差速運(yùn)動(dòng)，假設(shè)單位時(shí)間內(nèi)左右輪位移分別為Δl和Δr，兩輪之間距離為d，無人車的角度變化為ω，線速度變化為V。則有：

不考慮電機(jī)驅(qū)動(dòng)動(dòng)態(tài)性能對(duì)無人車影響，不考慮車內(nèi)部摩擦、阻尼對(duì)系統(tǒng)影響。運(yùn)動(dòng)學(xué)方程為

其中PG=［xGyGθG］T是無人車質(zhì)心M在慣性坐標(biāo)系下的位置矢量包括坐標(biāo)以及航向角，輸入為線速度v、角速度ω。

3 控制器設(shè)計(jì)

3.1 控制策略

以領(lǐng)導(dǎo)者-跟隨者為控制策略，如圖3通過引入虛擬領(lǐng)航者，假設(shè)一個(gè)虛擬點(diǎn)為虛擬領(lǐng)航者提供領(lǐng)航者的運(yùn)動(dòng)軌跡和實(shí)時(shí)速度，跟隨者無人機(jī)與無人車通過接收到領(lǐng)航者位置后幾何解算得到期望軌跡，并作為跟隨者的參考軌跡輸入無人車無人機(jī)的MPC聯(lián)合運(yùn)動(dòng)控制器。

圖2 聯(lián)合運(yùn)動(dòng)策略示意圖

虛擬領(lǐng)航者策略一方面能夠解決由于領(lǐng)航者故障導(dǎo)致整體聯(lián)合運(yùn)動(dòng)失控問題，另一方面可以設(shè)計(jì)準(zhǔn)確的控制率得到每時(shí)每刻的準(zhǔn)確軌跡與速度，為模型預(yù)測中求解優(yōu)化提供準(zhǔn)確的參考輸入，設(shè)虛擬領(lǐng)航者狀態(tài)空間方程為

領(lǐng)航者的狀態(tài)量為XL（K）=［xL（K），L（K）］T，輸入量為UL（K）=［VXL（K），VYL（K）］T，輸出為YL（K）=［xL（K），yL（K），VXL（K），VYL（K）］T。無人器通過聯(lián)合運(yùn)動(dòng)隊(duì)形要求，根據(jù)虛擬領(lǐng)航者提供的狀態(tài)信息生成參考軌跡用于模型預(yù)測控制中的滾動(dòng)優(yōu)化求解。將優(yōu)化求解的控制量作用于控制對(duì)象以及對(duì)應(yīng)模型。

設(shè)YRA（K）=［xRA（K），yRA（K），VRAX（K），VRAY（K）］T為無人機(jī)接收虛擬領(lǐng)航者信息后為滿足隊(duì)形需求保持距離幾何計(jì)算得到的參考軌跡輸入，YRG（K）=［xRG（K），yRG（K），VRGX（K），VRGY（K）］T為無人車接收虛擬領(lǐng)航者信息后幾何計(jì)算的參考軌跡輸入，與傳統(tǒng)軌跡跟蹤不同，為了滿足聯(lián)合運(yùn)動(dòng)控制效果，提取速度信息為參考軌跡元素，可以提高聯(lián)合運(yùn)動(dòng)的動(dòng)態(tài)效果。控制器通過滾優(yōu)化得到輸入控制率完成無人機(jī)無人車聯(lián)合運(yùn)動(dòng)控制。

由于無人車為一階系統(tǒng)，速度本為輸入量。為了提高運(yùn)動(dòng)中實(shí)時(shí)跟隨效果，對(duì)傳統(tǒng)無人車運(yùn)動(dòng)狀態(tài)空間模型變化得到增廣模型設(shè)計(jì)對(duì)應(yīng)MPC控制器。而無人機(jī)雖然為二階運(yùn)動(dòng)模型，但是又分為外環(huán)軌跡運(yùn)動(dòng)控制與內(nèi)環(huán)姿態(tài)控制，本文內(nèi)環(huán)姿態(tài)控制使用傳統(tǒng)PID控制器，并按照工程實(shí)際要求對(duì)其設(shè)置姿態(tài)角輸出限幅，控制器結(jié)構(gòu)示意圖如圖3。

3.2 四旋翼無人機(jī)姿態(tài)控制

姿態(tài)控制模型可以簡化為

［Фθ Ψ］T為機(jī)體坐標(biāo)系下姿態(tài)角分別為滾轉(zhuǎn)角、俯仰角、偏航角；u2、u3、u4為姿態(tài)控制輸入量，由PID姿態(tài)控制飛控控制；l為每個(gè)旋翼末端到飛行器重心距離；Ii為每個(gè)軸轉(zhuǎn)動(dòng)慣量；Ki為阻力系數(shù)。其PID較為簡單也并非本文主要研究內(nèi)容，在此不再贅述。

3.3 四旋翼無人機(jī)位置運(yùn)動(dòng)狀態(tài)空間模型

對(duì)式（5）離散化，假設(shè)采樣時(shí)間為T得

式中：

定義無人機(jī)聯(lián)合運(yùn)動(dòng)增廣狀態(tài)矩陣為

可得增廣狀態(tài)空間模型為

設(shè)NPA為預(yù)測步長，NCA為控制步長，在任意采樣時(shí)刻K，離散狀態(tài)輸出可表示為

其中：

3.4 無人車位置運(yùn)動(dòng)狀態(tài)空間模型

首先假定一個(gè)針對(duì)無人車模型的參考軌跡微分動(dòng)態(tài)為

其中Pr=［xryrθr］T為假定參考軌跡。

利用泰勒公式局部線性化，把方程在參考軌跡點(diǎn)（pr，vr）處展開得到

其中ε（p-pr，v-vr）為高階余項(xiàng)可以忽略不計(jì)，而和分別為ζ（p，v）相對(duì)于P和v的雅可比矩陣。將方程（14）與方程（15）相減可得線性狀態(tài)誤差方程為

但是實(shí)際中無人車的參考信息是離散形式Y(jié)RG（K）=［xRG（K），yRG（K），VRGX（K），VRGY（K）］?？梢杂闷浞聪虮硎咎幩枰募俣I(lǐng)航者信息，對(duì)式（16）離散化，假設(shè)采樣時(shí)間為T得：

其中：

定義聯(lián)合運(yùn)動(dòng)增廣狀態(tài)矩陣為

可得增廣狀態(tài)空間模型為

設(shè)NPG為預(yù)測步長，NCG為控制步長，在任意采樣時(shí)刻K，離散狀態(tài)輸出可表示為

式中：

3.5 滾動(dòng)優(yōu)化控制輸入量

MPC控制器的輸出是由最小化目標(biāo)函數(shù)得到的，由于在設(shè)計(jì)運(yùn)動(dòng)預(yù)測模型時(shí)，已將速度作為狀態(tài)量并且將控制輸入的偏差考慮到輸入中，而輸出偏差即為加速度。對(duì)無人機(jī)運(yùn)動(dòng)模型而言加速度就是外環(huán)輸入，對(duì)無人車而言超出控制范圍，所以不必再把輸出偏差作為控制器的最優(yōu)函數(shù)參考量，因此采用經(jīng)典目標(biāo)函數(shù)，設(shè)計(jì)無人車無人機(jī)的目標(biāo)函數(shù)：

其中無人機(jī)受實(shí)際飛控姿態(tài)角角度約束，而無人車受驅(qū)動(dòng)電機(jī)性能約束，可以有：

式（18）～（19）轉(zhuǎn)化為二次規(guī)劃問題可得：

其中：

YA（K），YG（K）為無人機(jī)無人車預(yù)測輸出，YRA（K）YRG（K）為無人機(jī)無人車的參考軌跡，QAQG為各自系統(tǒng)誤差權(quán)重矩陣，RARG為各自輸入權(quán)重矩陣。通過求minJA（X（K），ΔU），minJG（X（K），ΔV）的二次優(yōu)化問題得到控制時(shí)域控制輸入增量序列，將其中第一個(gè)控制增量作為聯(lián)合運(yùn)動(dòng)運(yùn)動(dòng)中無人機(jī)無人車各自的輸入。

4 仿真實(shí)驗(yàn)與分析

參考文獻(xiàn)［15］無人機(jī)的姿態(tài)系統(tǒng)參數(shù)為m=0.5kg，l=0.3m，g=9.8m/s2，K1=0.01，K2=0.01，K3=0.01。IX=0.006kg·m2，IY=0.006kg·m2，IZ=0.012kg·m2。初始姿態(tài)角Ф=0rad，θ=0rad，Ψ=0rad。

外環(huán)控制器預(yù)測時(shí)域長度NPA為8，控制時(shí)域長度NCA為2，加權(quán)矩陣QA為diag［1 1 0.1 0.1］，加權(quán)矩陣RA為diag［0.1 0.1］無狀態(tài)約束，參考工程中飛控代碼，將角度取0.6rad為俯仰角，橫滾角的上下限，輸入約束為［-6.1-8.3］≤u≤［6.1 8.3］，［1.5 1.5］≤Δu≤［1.5 1.5］。無人車預(yù)測時(shí)域長度NP為20，控制時(shí)域長度NC為4，加權(quán)矩陣QG為diag［1 1 0.1 0.1］，加權(quán)矩陣RG為diag［0.1 0.1］。輸入約束為［-4-π］≤v≤［4 π］，［-2.4-π/5］≤Δv≤［2.4 π/5］。

4.1 單無人機(jī)單無人車聯(lián)合運(yùn)動(dòng)的仿真驗(yàn)證

四旋翼無人機(jī)1號(hào)初始位置為（1m，2m，4m）初始速度為（0m/s，0m/s，0m/s）。兩輪差速無人車1號(hào)初始位置為（0m，0m）初始速度為（0m/s，0m/s）。

隊(duì)形要求無人車緊跟虛擬領(lǐng)航者，無人機(jī)保持在無人車X軸后方1m的距離。仿真時(shí)間100s，仿真步長0.05s。設(shè)虛擬領(lǐng)航者的初始位置為（0m，0m）運(yùn)動(dòng)軌跡為

圖4 為單無人車無人機(jī)的二維軌跡圖，圖5～圖8為無人機(jī)無人車和參考軌跡之間的位置誤差以及速度誤差的關(guān)系圖，由圖可知位置誤差保持在1m之內(nèi)，雖然X方向參考軌跡是平滑的，但是無人車由于自身運(yùn)動(dòng)限制，不如無人機(jī)靈活，會(huì)產(chǎn)生波動(dòng)，尤其在拐彎處波動(dòng)更加明顯?？傮w追蹤精度較好。而由圖9可知無人機(jī)的俯仰角θ橫滾角Ф變化在0.5rad之內(nèi)，滿足飛控穩(wěn)定性。

圖4 單無人車單無人機(jī)聯(lián)合運(yùn)動(dòng)控制二維軌跡圖

圖5 無人車XY軸方向上與參考軌跡的軌跡誤差

圖6 無人機(jī)XY軸方向上與參考軌跡的軌跡誤差

圖7 無人車XY軸方向上速度誤差

圖8 無人機(jī)XY軸方向上速度誤差

圖9 無人機(jī)俯仰角橫滾角的實(shí)時(shí)輸出

4.2 多無人機(jī)多無人車的仿真驗(yàn)證

在前述實(shí)驗(yàn)的基礎(chǔ)上增加無人機(jī)2號(hào)，無人車2號(hào)；之前配置保持不變。并設(shè)之前為無人機(jī)1號(hào)，無人車1號(hào)，設(shè)無人機(jī)2號(hào)初始位置為（1m，3m，4m）初始速度為（0m/s，0m/s，0m/s）設(shè)其與1號(hào)無人車保持在Y軸上方1m，同樣2號(hào)無人車在1號(hào)無人車Y軸上方1m?？傻脽o人器聯(lián)合運(yùn)動(dòng)的二維以及三維軌跡如下。

圖10 多無人機(jī)無人車聯(lián)合運(yùn)動(dòng)二維軌跡圖

圖11 多無人機(jī)無人車聯(lián)合運(yùn)動(dòng)三維圖

根據(jù)運(yùn)動(dòng)軌跡可知，即便在曲線運(yùn)動(dòng)中，在位置誤差允許范圍內(nèi)，無人機(jī)無人車完成了較好的聯(lián)合運(yùn)動(dòng)。

5 結(jié)語

本文將傳統(tǒng)四旋翼無人機(jī)運(yùn)動(dòng)模型以及兩輪差速無人車運(yùn)動(dòng)模型進(jìn)行約束變化得到對(duì)應(yīng)的MPC運(yùn)動(dòng)控制模型，并且將實(shí)際工程中角度約束帶入預(yù)測運(yùn)算，將速度約束加入軌跡約束條件提高聯(lián)合編隊(duì)運(yùn)動(dòng)性能，對(duì)無人機(jī)無人車聯(lián)合運(yùn)動(dòng)控制實(shí)現(xiàn)和運(yùn)行有一定工程意義，但是其中還未考慮各種阻力約束，在以后研究中將考慮加入干擾觀測器提高其運(yùn)動(dòng)控制效果。