基于狀態(tài)空間MPC的無人機無人車聯(lián)合運動控制*

周銳森 馮友兵

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

1 引言

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

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

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

2 四旋翼無人機與無人車的非線性動力學模型轉化

2.1 四旋翼無人機非線性動力學模型線性化變換

如圖1所示，慣性坐標系為OE—XEYEZE，OB—XBYBZB為機體標系。建立準確的無人機模型是設計控制器關鍵，為便于求解，對模型做出下列假設：

圖1 四旋翼結構示意圖

1）四旋翼是剛體；

2）四旋翼質量轉動慣量不變；

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

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

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

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

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

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

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

將gtanθ和-gtanФsecθ作為新的輸入UXUY。

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

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

2.2 無人車運動模型

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

不考慮電機驅動動態(tài)性能對無人車影響，不考慮車內部摩擦、阻尼對系統(tǒng)影響。運動學方程為

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

3 控制器設計

3.1 控制策略

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

圖2 聯(lián)合運動策略示意圖

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

領航者的狀態(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)合運動隊形要求，根據(jù)虛擬領航者提供的狀態(tài)信息生成參考軌跡用于模型預測控制中的滾動優(yōu)化求解。將優(yōu)化求解的控制量作用于控制對象以及對應模型。

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

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

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

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

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

3.3 四旋翼無人機位置運動狀態(tài)空間模型

對式（5）離散化，假設采樣時間為T得

式中：

定義無人機聯(lián)合運動增廣狀態(tài)矩陣為

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

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

其中：

3.4 無人車位置運動狀態(tài)空間模型

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

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

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

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

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

其中：

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

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

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

式中：

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

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

其中無人機受實際飛控姿態(tài)角角度約束，而無人車受驅動電機性能約束，可以有：

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

其中：

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

4 仿真實驗與分析

參考文獻［15］無人機的姿態(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)控制器預測時域長度NPA為8，控制時域長度NCA為2，加權矩陣QA為diag［1 1 0.1 0.1］，加權矩陣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］。無人車預測時域長度NP為20，控制時域長度NC為4，加權矩陣QG為diag［1 1 0.1 0.1］，加權矩陣RG為diag［0.1 0.1］。輸入約束為［-4-π］≤v≤［4 π］，［-2.4-π/5］≤Δv≤［2.4 π/5］。

4.1 單無人機單無人車聯(lián)合運動的仿真驗證

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

隊形要求無人車緊跟虛擬領航者，無人機保持在無人車X軸后方1m的距離。仿真時間100s，仿真步長0.05s。設虛擬領航者的初始位置為（0m，0m）運動軌跡為

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

圖4 單無人車單無人機聯(lián)合運動控制二維軌跡圖

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

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

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

圖8 無人機XY軸方向上速度誤差

圖9 無人機俯仰角橫滾角的實時輸出

4.2 多無人機多無人車的仿真驗證

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

圖10 多無人機無人車聯(lián)合運動二維軌跡圖

圖11 多無人機無人車聯(lián)合運動三維圖

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

5 結語

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