一、研究背景

??無(wú)人機(jī)在在眾多的應(yīng)用領(lǐng)域展示了其良好的能力，如基礎(chǔ)設(shè)施檢查、地下礦井導(dǎo)航、搜索和救援任務(wù)、在事故發(fā)生時(shí)提供急救或除顫器等。然而，在進(jìn)行這些任務(wù)時(shí)需要考慮到環(huán)境的因素如狹窄的通道、有限的入口面積和一般狹窄的受限空間，以及限制可用開(kāi)放空間的多種障礙物。有時(shí)還應(yīng)該考慮移動(dòng)障礙物的情況下。更具體地說(shuō)，要在復(fù)雜的任務(wù)中部署無(wú)人機(jī)，如城市環(huán)境、非結(jié)構(gòu)化和未知或不斷變化的應(yīng)用場(chǎng)景，例如存在多個(gè)移動(dòng)障礙物和其他機(jī)器人代理，必須保證安全操作和與居民的零事故。因此，應(yīng)用一種新穎的非線性模型預(yù)測(cè)控制（NMPC），以建立一個(gè)在受限環(huán)境中的完全自主導(dǎo)航框架非常有意義。

??非線性模型的應(yīng)用在無(wú)人機(jī)路徑規(guī)劃中具有廣泛的應(yīng)用前景，因?yàn)樗梢愿玫啬M無(wú)人機(jī)飛行的復(fù)雜動(dòng)態(tài)特性，以及考慮更多的控制因素。在非線性模型中，可以考慮無(wú)人機(jī)的飛行速度、空氣動(dòng)力學(xué)力學(xué)特性、環(huán)境參數(shù)、傳感器測(cè)量誤差等因素，從而提高無(wú)人機(jī)路徑規(guī)劃的精度和魯棒性。而且預(yù)測(cè)控制是一種常用的非線性控制方法，它可以通過(guò)對(duì)未來(lái)狀態(tài)的預(yù)測(cè)來(lái)調(diào)整當(dāng)前的控制策略，從而提高系統(tǒng)的控制性能。基于預(yù)測(cè)控制的無(wú)人機(jī)路徑規(guī)劃方法可以通過(guò)對(duì)未來(lái)的無(wú)人機(jī)運(yùn)動(dòng)進(jìn)行預(yù)測(cè)，來(lái)優(yōu)化無(wú)人機(jī)的軌跡規(guī)劃，使其更加符合實(shí)際應(yīng)用場(chǎng)景。此外，基于預(yù)測(cè)控制的無(wú)人機(jī)路徑規(guī)劃方法還具有適應(yīng)性強(qiáng)、魯棒性好等優(yōu)點(diǎn)。

二、針對(duì)動(dòng)態(tài)障礙物

??大多數(shù)研究是針對(duì)動(dòng)態(tài)或移動(dòng)障礙物的情況進(jìn)行的運(yùn)動(dòng)規(guī)劃，主要是在自動(dòng)駕駛汽車的背景下，因?yàn)樗枰_保城市環(huán)境中的無(wú)碰撞路徑。如果解決了計(jì)算時(shí)間的問(wèn)題，NMPC的運(yùn)動(dòng)規(guī)劃方法也開(kāi)始在無(wú)人機(jī)領(lǐng)域獲得更多的應(yīng)用。與其他避障方案相比，NMPC方案的優(yōu)勢(shì)在于能夠純粹基于無(wú)人機(jī)的非線性運(yùn)動(dòng)學(xué)產(chǎn)生無(wú)碰撞軌跡，其中軌跡中的每一點(diǎn)都由一系列控制輸入描述。其主要問(wèn)題是求解時(shí)間。參考相應(yīng)文獻(xiàn)，對(duì)于動(dòng)態(tài)避障的做法是通過(guò)對(duì)障礙物軌跡進(jìn)行預(yù)測(cè)，輸入到系統(tǒng)作為控制器的參考，然后MPC控制器可以根據(jù)預(yù)測(cè)模型和當(dāng)前狀態(tài)，計(jì)算出最優(yōu)的控制輸出，以達(dá)到運(yùn)動(dòng)規(guī)劃和避障的目的。優(yōu)化是通過(guò)非線性、非凸的求解器Proximal Averaged Newton for Optimal Control（PANOC）及其相關(guān)軟件OpEn（優(yōu)化引擎），其中應(yīng)用了一種懲罰方法來(lái)適當(dāng)考慮導(dǎo)航過(guò)程中的障礙和其他約束。具體方法可以分為以下5個(gè)步驟。

（1）無(wú)人機(jī)運(yùn)動(dòng)學(xué)：根據(jù)機(jī)體坐標(biāo)系和全球坐標(biāo)系，將無(wú)人機(jī)的6個(gè)自由度由狀態(tài)方程組定義：

其中p為xyz位置，v為線速度，φ和θ∈[ - π , π]分別為沿xW和yW軸的橫滾角和俯仰角，R ( φ ( t )，θ ( t ) )∈SO ( 3 )是描述姿態(tài)的歐拉形式的旋轉(zhuǎn)矩陣。φref∈R，θref∈R和T≥0分別是橫滾、俯仰和總推力的參考輸入。線性阻尼項(xiàng)Ax，Ay，Az∈R和重力加速度g。將姿態(tài)項(xiàng)建模為姿態(tài)(橫滾/俯仰)與參考φ ref∈R，θref∈R之間的一階系統(tǒng)，其增益為K φ，Kθ∈R，時(shí)間常數(shù)為τ φ，τ θ∈R。

（2）定義成本函數(shù)

??用x =[p,v,φ,θ]的轉(zhuǎn)置表示狀態(tài)向量，用u =[T,φref,θref]的轉(zhuǎn)置表示相應(yīng)的控制動(dòng)作。對(duì)采樣時(shí)間為δt∈Z +的每個(gè)時(shí)刻（k + j | k）采用向前歐拉法離散化式。其中（k + j | k）表示第k + j個(gè)時(shí)間步的預(yù)測(cè)，產(chǎn)生于第k個(gè)時(shí)間步。這個(gè)離散模型被用作NMPC的預(yù)測(cè)模型。用xk和uk表示完整的預(yù)測(cè)狀態(tài)和相應(yīng)的控制輸入，沿著預(yù)測(cè)水平線相應(yīng)地表示，控制器的目標(biāo)是使?fàn)顟B(tài)達(dá)到規(guī)定的設(shè)定點(diǎn)并提供平滑的控制輸入。所以，成本函數(shù)表述為：

其中Qx，Qu，QΔu分別是狀態(tài)、輸入、和輸入變化率的正定權(quán)重矩陣，第一項(xiàng)表示狀態(tài)成本，它懲罰偏離某個(gè)狀態(tài)參考xref的行為。第二項(xiàng)表示輸入成本，懲罰偏離穩(wěn)態(tài)輸入uref=[g, 0, 0]的行為。第三個(gè)項(xiàng)，對(duì)連續(xù)輸入的變化進(jìn)行懲罰，來(lái)保證執(zhí)行平滑的控制動(dòng)作。

（3）定義障礙和約束

使用函數(shù)[h]+ =max{0,h}。這使我們可以將約束條件表述為等價(jià)表達(dá)式，如[h]+=0意味著約束條件得到滿足。通過(guò)選擇h作為一個(gè)在約束區(qū)域內(nèi)為正值、在約束區(qū)域外為負(fù)值的表達(dá)式，[h]+將在約束區(qū)域內(nèi)為正值，在約束區(qū)域外為零。障礙物的約束被定義為：

??其中ξobs =[robs,rs,pobs x ,pobs y ,pobs z]，robs是障礙物的半徑，rs是額外的安全半徑。這意味著，只要無(wú)人機(jī)的位置p位于球體之外，上式就為零，可以說(shuō)是一個(gè)平等的約束條件。將這擴(kuò)展到動(dòng)態(tài)障礙物上，為每個(gè)預(yù)測(cè)的時(shí)間步驟形成一個(gè)單獨(dú)的約束。讓pobs x , pobs y , pobs z表示描述障礙物軌跡的大小為N的向量，rs表示沿N線性增加的安全半徑，pk表示在時(shí)間步驟k的全部預(yù)測(cè)的無(wú)人機(jī)位置，那么障礙物約束向量可以表述為：

如果無(wú)人機(jī)沒(méi)有進(jìn)入障礙物軌跡所描述的球形障礙物，則滿足約束條件。此外，為了避免快速移出障礙物的軌跡會(huì)導(dǎo)致控制輸入的激進(jìn)或振蕩行為，因此為控制輸入φ ref和θ ref的變化幅度設(shè)定一個(gè)上界和下界：

最后，還應(yīng)該對(duì)控制輸入施加約束，定義輸入邊界：

（4）進(jìn)行優(yōu)化

綜合考慮上述的成本函數(shù)和相應(yīng)的約束，可以將有N個(gè)球形障礙物的參數(shù)化軌跡的NMPC問(wèn)題表述為：

采用PANOC這種優(yōu)化器和相關(guān)的Open開(kāi)源軟件進(jìn)行優(yōu)化，找到一條移動(dòng)成本最小值，而且不違反約束條件，或者說(shuō)直到滿足一個(gè)特定的容忍度的軌跡。

（5）軌跡的分類

???在進(jìn)行障礙物移動(dòng)軌跡預(yù)測(cè)時(shí)，將其軌跡分為三類：直線運(yùn)動(dòng)、彈丸運(yùn)動(dòng)或靜態(tài)障礙物。因此，在可以計(jì)算障礙物軌跡類別的情況下，可以使用直接預(yù)測(cè)模型來(lái)預(yù)測(cè)障礙物的未來(lái)位置。直線運(yùn)動(dòng)描述為：

其中vobs是障礙物的速度，沒(méi)有力作用在障礙物上。彈丸運(yùn)動(dòng)軌跡定義為：

其中B是線性空氣動(dòng)力阻尼項(xiàng)，通過(guò)前向歐拉離散化，使用與控制器和預(yù)測(cè)模型相同的采樣時(shí)間Ts。對(duì)于障礙物狀態(tài)xobs = [pobs,vobs]，離散預(yù)測(cè)的形式：

其中αt表示軌跡的離散預(yù)測(cè)模型，可以無(wú)限期地迭代。同時(shí)與先前測(cè)量的障礙物位置和速度的矢量做比較測(cè)量誤差為：

對(duì)3類不同的軌跡進(jìn)行評(píng)價(jià)，選擇誤差最小的一類進(jìn)行軌跡預(yù)測(cè)。部分仿真結(jié)果如下圖：

三、針對(duì)靜態(tài)障礙物（狹縫、窄口等）

???無(wú)人機(jī)在狹窄的通道、多障礙物填充的空間和狹小的開(kāi)口等約束環(huán)境中的路徑規(guī)劃同樣采用NMPC的方法，通過(guò)增加懲罰約束，并采用PANOC的優(yōu)化方法最終計(jì)算一條不進(jìn)入障礙物的路徑。這種方法的研究步驟與移動(dòng)障礙物的相似，其中無(wú)人機(jī)運(yùn)動(dòng)學(xué)采用上述第二節(jié)描述的一樣，成本函數(shù)也與上述第二節(jié)描述的一樣，因?yàn)槟繕?biāo)一樣獲得一條平滑且避開(kāi)障礙物的一條軌跡。

（1）障礙物定義與約束

??函數(shù)的定義與上述相同，將周圍環(huán)境中可能會(huì)遇到多種不同類型的障礙物分為三種類型：圓柱體形狀、多角形表面和受約束的入口或通道。應(yīng)該強(qiáng)調(diào)的是，所有的基本約束都是在控制范圍N上加起來(lái)的，以考慮到所有預(yù)測(cè)的未來(lái)時(shí)間步驟的障礙物約束。①圓柱體形狀障礙：該約束條件可用于任何受阻區(qū)域或一般障礙物，其中圓柱體的半徑籠罩著不希望出現(xiàn)的區(qū)域。因此，其約束定義如下：

其中xobs、yobs、zobs和robs分別為障礙物的x、y、z坐標(biāo)和半徑。將這兩個(gè)方程相乘得到約束方程:

障礙物可視化如下圖所示

②表面型障礙物：表面障礙物用六條線或平面方程表示一個(gè)有限定寬度、長(zhǎng)度和高度的墻狀障礙物。為了便于表述，一維線或平面情況可以用沿每個(gè)軸的最小、最大范圍表示為：

其中p表示無(wú)人機(jī)的位置坐標(biāo)，pmin，pmax表示約束區(qū)域的范圍。設(shè)x，y，z的每一個(gè)h項(xiàng)分別描述一個(gè)線/面方程，則全曲面為：

在不失一般性的情況下，該框架可以直接擴(kuò)展為全平面方程來(lái)描述障礙物表面，其可視化如下圖所示：

③限制性出入口：首先可以由y-z平面上的圓柱體形狀來(lái)描述，那么其圓柱形狀的約束表達(dá)式為：

然后為了限制通道面積的大小，定義了x方向上的通道長(zhǎng)度的約束條件：

其中xobs、l1、l2分別是通道的中心和x方向上的通道范圍。與以前的情況一樣，約束在預(yù)測(cè)時(shí)域上加總：

可視化如下圖所示：

（2）控制輸入約束：為了確保即使在非?？量痰谋苷蠄?chǎng)景下，控制輸入φ ref和θ ref的非激進(jìn)行為，可以表示為：

這些約束將控制輸入從一個(gè)時(shí)間步到下一個(gè)時(shí)間步的變化限制在一個(gè)最大值，這里分別記為Δ φ max和Δ θ max。除速率約束外，令控制輸入的硬約束記為：

（3）優(yōu)化：優(yōu)化引擎可以使用二次懲罰方法求解參數(shù)優(yōu)化問(wèn)題，其核心是非線性非凸求解器PANOC：

其中f是一個(gè)Lipschitz微分函數(shù)，F(xiàn)是一個(gè)矢量值映射，c是一個(gè)正的懲罰參數(shù)。將上述描述的成本函數(shù)、約束條件通過(guò)單射來(lái)適應(yīng)這個(gè)框架，并設(shè)置Z為(11)式的約束。此外讓F映射出障礙物和輸入率的平等約束。參數(shù)p包括狀態(tài)向量X的初始條件，以及參考和障礙物數(shù)據(jù)。圓柱形、多邊平面、限制性出入口障礙物仿真結(jié)果如下圖所示：

?四、分析與思考

①對(duì)于移動(dòng)障礙物軌跡將其分為三類，未考慮其他類型的軌跡運(yùn)動(dòng)，比較局限。因此，可以通過(guò)對(duì)事物或人或其他一些移動(dòng)障礙物的軌跡可以進(jìn)行一個(gè)深度學(xué)習(xí)，然后發(fā)現(xiàn)其運(yùn)動(dòng)的規(guī)律，更好的預(yù)測(cè)其行為軌跡，達(dá)到更廣的適應(yīng)性而且也可以更好避障。

②上述提到避開(kāi)移動(dòng)障礙物的方法未考慮到障礙物移動(dòng)速度的因素，如果障礙物以一個(gè)較快的速度飛向無(wú)人機(jī)，無(wú)人機(jī)通過(guò)上述方法可能無(wú)法閃避。因?yàn)槠溆?jì)算的時(shí)間以及機(jī)體整個(gè)按規(guī)劃的路徑移動(dòng)需要時(shí)間。因此，如果對(duì)機(jī)體姿態(tài)的調(diào)整，通過(guò)細(xì)化機(jī)體本身大小與移動(dòng)障礙物大小，增加距離約束和可避開(kāi)半徑約束的，調(diào)整控制橫滾和偏航角的大小來(lái)避開(kāi)移動(dòng)障礙物，姿態(tài)調(diào)整一方面減少所需時(shí)間，另外一方面對(duì)與路徑規(guī)劃其偏離參考軌跡范圍小，保證路徑漸進(jìn)最優(yōu)。

③通過(guò)NMPC對(duì)靜態(tài)障礙物的避障，對(duì)障礙物形狀描述還是比較局限，而且通過(guò)仿真發(fā)現(xiàn)其規(guī)劃的路徑并不是最優(yōu)。與路徑規(guī)劃算法（RRT*等）和軌跡優(yōu)化算法比較起來(lái)，這種方法優(yōu)點(diǎn)：計(jì)算量小，效率高因?yàn)槠洳恍枨岸撕秃蠖藘蓚€(gè)規(guī)劃器。但是得到軌跡平滑性相對(duì)差一點(diǎn)，且路徑長(zhǎng)度相對(duì)與其他算法較長(zhǎng)。

④在使用滾動(dòng)優(yōu)化時(shí)，對(duì)于局部目標(biāo)的設(shè)定非常重要，設(shè)置合理，對(duì)于無(wú)人機(jī)飛行時(shí)軌跡遵循較好，不會(huì)過(guò)激的通過(guò)目標(biāo)位置。

五、參考文獻(xiàn)

[1].Lindqvist B, Mansouri S S, Agha-mohammadi A, et al. Nonlinear MPC for collision?avoidance and control of UAVs with dynamic obstacles[J]. IEEE robotics and automation letters, 2020, 5(4): 6001-6008.

[2].Lindqvist B, Mansouri S S, Nikolakopoulos G. Non-linear mpc based navigation for micro aerial vehicles in constrained environments[C]//2020 European Control Conference (ECC). IEEE, 2020: 837-842.