龍離軍,胡騰飛

(東北大學(xué)信息科學(xué)與工程學(xué)院,遼寧沈陽(yáng) 110819)

1 引言

隨著移動(dòng)機(jī)器人的應(yīng)用越來(lái)越廣泛,許多新的或具有挑戰(zhàn)性的課題被提出,例如,如何實(shí)現(xiàn)機(jī)器人的安全攸關(guān)控制,從而避免其與人或物相碰撞[1-4].因此,為了確保機(jī)器人能正常運(yùn)行,制定實(shí)時(shí)避障策略來(lái)保證動(dòng)態(tài)系統(tǒng)的安全性是必要的.目前,已有許多算法專注于機(jī)器人的避障問(wèn)題,如:全局路徑規(guī)劃算法[1-2]提供了從起點(diǎn)到終點(diǎn)的一條最優(yōu)路徑,然而這種算法只能應(yīng)用于全局信息已知的情形.基于人工勢(shì)場(chǎng)(artificial potential field,APF)和柵格法的虛擬力場(chǎng)法(virtual force field,VFF)[5]可以實(shí)現(xiàn)機(jī)器人實(shí)時(shí)避障,但這種方法信息存儲(chǔ)量大且獲得的控制器不是最優(yōu)的.Hamilton Jacobi可達(dá)性分析法(Hamilton Jacobi reachability analysis,HJ-RA)[6]是一種驗(yàn)證動(dòng)態(tài)系統(tǒng)安全性的形式化方法,該方法在避免不安全集受有界不確定性影響的前提下,將動(dòng)力系統(tǒng)驅(qū)動(dòng)到目標(biāo)集,然而,可達(dá)性分析隨系統(tǒng)維數(shù)的增加(呈指數(shù)增長(zhǎng))而變得難以計(jì)算.

近年來(lái),一種基于集合前向不變性以實(shí)現(xiàn)系統(tǒng)安全攸關(guān)控制的控制障礙函數(shù)(control barrier function,CBF)[7]被提出,且APF為其特例[8].由于CBF具有可擴(kuò)展性、強(qiáng)實(shí)時(shí)性和魯棒性[9]等優(yōu)點(diǎn),其常與PID,控制Lyapunov 函 數(shù)(control Lyapunov function,CLF)以及反步法等算法相結(jié)合并利用二次規(guī)劃(quadratic programming,QP)實(shí)現(xiàn)系統(tǒng)的最優(yōu)安全軌跡跟蹤控制[10-12].該算法的有效性和優(yōu)越性在許多領(lǐng)域得到驗(yàn)證并廣泛應(yīng)用,例如:雙足機(jī)器人[13]、自動(dòng)駕駛汽車[14]、四旋翼無(wú)人機(jī)[15]、多智能體系統(tǒng)[16]等.此后,CBF得到諸多學(xué)者研究和擴(kuò)展,例如:文獻(xiàn)[17]和[18]分別提出指數(shù)和高階CBF用于處理高相對(duì)階系統(tǒng)的安全控制問(wèn)題,文獻(xiàn)[19]根據(jù)系統(tǒng)狀態(tài)和輸入定義的積分CBF可應(yīng)用于非仿射控制系統(tǒng),文獻(xiàn)[20]和[21]分別提出魯棒自適應(yīng)和自適應(yīng)滑模CBF用于處理具有未知參數(shù)不確定系統(tǒng)的安全控制問(wèn)題.此外,CBF從連續(xù)時(shí)間控制系統(tǒng)推廣到了離散時(shí)間控制系統(tǒng)中[22-24].最近,CBF被應(yīng)用于基于機(jī)器學(xué)習(xí)數(shù)據(jù)驅(qū)動(dòng)作用下的安全控制[25-26].文獻(xiàn)[27]對(duì)CBF的發(fā)展進(jìn)行了綜述.

然而,在實(shí)際工程應(yīng)用中,由于物理限制,智能體系統(tǒng)通常需要考慮容許控制輸入等附加約束,但該約束可能與CLF和CBF約束發(fā)生沖突,從而導(dǎo)致QP變得求解不可行.另外,值得注意的是,當(dāng)需要考慮系統(tǒng)自身體積或運(yùn)行環(huán)境中存在快速移動(dòng)障礙物等問(wèn)題時(shí),需時(shí)刻保證系統(tǒng)與障礙物之間有一定安全距離以降低發(fā)生碰撞的風(fēng)險(xiǎn).在前人的一些研究工作中[28-30],使用CLF-CBF-QP實(shí)現(xiàn)安全控制時(shí)并沒(méi)有考慮控制輸入約束,這意味著可能由于物理限制而導(dǎo)致系統(tǒng)無(wú)法正常執(zhí)行操作.雖然文獻(xiàn)[31-32]在QP中顯式考慮了控制輸入約束,但仍沒(méi)有處理輸入約束與CLF和CBF約束之間存在的潛在沖突.文獻(xiàn)[33]雖然提出一種CBF的最優(yōu)衰減形式來(lái)提高優(yōu)化問(wèn)題的可行性,且CBF的衰減率在時(shí)間上是逐點(diǎn)優(yōu)化的,卻沒(méi)有考慮系統(tǒng)自身體積和環(huán)境中存在動(dòng)態(tài)障礙物等問(wèn)題,當(dāng)該方法被應(yīng)用于上述情況時(shí),系統(tǒng)的安全性難以保證.

因此,針對(duì)上述關(guān)于QP求解可行性和動(dòng)態(tài)系統(tǒng)的安全性問(wèn)題,本文的主要工作和創(chuàng)新點(diǎn)如下: 1) 分別從控制輸入空間和狀態(tài)空間的角度分析CLF和CBF中參數(shù)γ和λ對(duì)QP求解可行性和系統(tǒng)性能的影響;2) 提出CLF-CBF-QP新形式,以此提高優(yōu)化問(wèn)題的可解性;3) 設(shè)計(jì)CBF新形式,以此提高動(dòng)態(tài)系統(tǒng)在考慮自身體積或在具有動(dòng)態(tài)障礙物環(huán)境中運(yùn)行時(shí)的安全性;4) 通過(guò)線性平面四旋翼安全軌跡跟蹤驗(yàn)證所提出方法的可行性和有效性.

2 基礎(chǔ)知識(shí)

2.1 符號(hào)定義

x:=(p,,q)∈Rn表示動(dòng)態(tài)系統(tǒng)的狀態(tài),其中p∈Rk為系統(tǒng)在k維空間中的位置狀態(tài),q∈Rn-2k為除了位置狀態(tài)p和速度狀態(tài)的其他狀態(tài).0n×m表示n×m維零矩陣,In表示n維單位矩陣.?S表示集合S的邊界.對(duì)于a,b ＞0,如果連續(xù)函數(shù)α:(-b,a)→(-∞,+∞)嚴(yán)格單調(diào)遞增且α(0)=0,那么稱α為擴(kuò)展的K類函數(shù).如無(wú)特殊說(shuō)明,QP指CLF-CBF-QP.

2.2 控制Lyapunov函數(shù)

考慮如下仿射非線性系統(tǒng):

其中:x ∈D ?Rn,u ∈Uadm?Rm分別是系統(tǒng)狀態(tài)和控制輸入,Uadm是容許控制輸入集,f(x)和g(x)是局部Lipschitz函數(shù).

定義1[11]對(duì)于仿射非線性系統(tǒng)(1),如果存在正常數(shù)c1,c2和γ,使得對(duì)于所有x ∈D,連續(xù)可微函數(shù)V:Rn →R滿足

定理1[27]對(duì)于仿射非線性系統(tǒng)(1),如果存在滿足式(2)的CLFV,那么任意Lipschitz連續(xù)反饋控制器u(x)∈UCLF使系統(tǒng)漸近穩(wěn)定到平衡點(diǎn)x*=0.

由于約束(2)關(guān)于u是仿射的,因此,利用QP可獲得最優(yōu)鎮(zhèn)定控制器,其形式為

其中:H為正定矩陣,ud為期望輸入,p ＞0為懲罰因子,δ為松弛變量.當(dāng)CLF和控制輸入約束沖突時(shí),通過(guò)δ放寬系統(tǒng)穩(wěn)定性而提高QP求解可行性.

2.3 控制障礙函數(shù)

定義2[11]給定一個(gè)連續(xù)可微函數(shù)h:D →R,定義零超水平集

其中:H為正定矩陣,ud為期望輸入.

2.4 利用QP結(jié)合CLF和CBF

Ames[7]等人首次利用QP將CLF和CBF約束相結(jié)合構(gòu)造最優(yōu)安全鎮(zhèn)定控制器以實(shí)現(xiàn)自適應(yīng)巡航控制,其具體形式為

其中:H為正定矩陣,常數(shù)γ,λ≥0,p ＞0為懲罰因子,δ為松弛變量.當(dāng)CLF,CBF和控制輸入約束沖突時(shí),通過(guò)δ放寬系統(tǒng)穩(wěn)定性而提高QP可行性.該形式的QP被廣泛地應(yīng)用于各種安全攸關(guān)控制中[31-33].

2.5 平面四旋翼線性模型

假設(shè)1四旋翼布局對(duì)稱且質(zhì)量分布均勻.

考慮如圖1所示平面四旋翼控制系統(tǒng).圖1的動(dòng)力學(xué)模型為

圖1 平面四旋翼模型Fig.1 Model of planar quadrotor

為獲得平面四旋翼線性模型,考慮在平衡點(diǎn)(懸停狀態(tài))xe=06×1,ug=[mg 0]T處利用泰勒展開(kāi)進(jìn)行線性化處理

2.6 不規(guī)則障礙物圓形近似化

在實(shí)際環(huán)境中存在的障礙物多為不規(guī)則形狀,但CBF的設(shè)計(jì)涉及系統(tǒng)與障礙物距離的計(jì)算,為方便理論計(jì)算與仿真驗(yàn)證,對(duì)不規(guī)則靜或動(dòng)態(tài)障礙物通過(guò)以下兩種方式近似化處理[35],其如圖2所示

圖2 不規(guī)則障礙物近似化處理Fig.2 Approximation processing of irregular obstacles

·不規(guī)則球形狀障礙物近似為單個(gè)半徑為r的圓形或球形.

·不規(guī)則長(zhǎng)條狀障礙物近似為多個(gè)半徑為r的圓形或球形組合而成.

2.7 控制問(wèn)題描述

定義4標(biāo)量函數(shù)d(p,o)被定義為

其中:p ∈Rk為動(dòng)態(tài)系統(tǒng)的位置狀態(tài),o ∈Rk,r分別表示在k維空間中障礙物的中心位置和半徑.

于是,系統(tǒng)相對(duì)于第i ∈{1,2···,n}個(gè)障礙物的幾何安全區(qū)域被定義為Si={p ∈Rk|d(p,oi)≥0}.因此,系統(tǒng)的整體安全區(qū)域?yàn)镾=

假設(shè)2為保證系統(tǒng)正常運(yùn)行,對(duì)于?t ＞0,安全區(qū)域S非空.

3 主要結(jié)論

動(dòng)態(tài)系統(tǒng)能否正常運(yùn)行與QP求解可行性息息相關(guān).當(dāng)QP選擇為式(9)時(shí),由于考慮控制輸入約束u∈Uadm,可能導(dǎo)致QP求解不可行.另外,CBF的設(shè)計(jì)需要考慮這一問(wèn)題:當(dāng)系統(tǒng)體積不能被忽略或環(huán)境中存在快速移動(dòng)的障礙物時(shí),動(dòng)態(tài)系統(tǒng)極有可能與障礙物相碰.因此,為了降低系統(tǒng)碰撞障礙物的風(fēng)險(xiǎn),本文設(shè)計(jì)一種CBF新形式以提高系統(tǒng)的安全性.

3.1 輸入空間角度分析QP求解可行性

基于假設(shè)3,為了直觀起見(jiàn),本文從幾何的角度進(jìn)行QP可行性分析.圖3中凸多邊形區(qū)域表示允許控制輸入集,綠色區(qū)域表示Ufea(xt),箭頭方向表示集合的半空間方向.

1) 如圖3(a)所示情況,此時(shí)Ufea(xt)=Uadm,QP求解可行.CLF和CBF約束在優(yōu)化問(wèn)題中不起作用,且最優(yōu)值在?Uadm處獲得.

2) 如圖3(b)所示情況,此時(shí)Ufea(xt)/=?,QP求解可行.當(dāng)CLF約束起作用時(shí),QP最優(yōu)值在?UCLF(xt)處取得,當(dāng)CBF約束起作用時(shí),QP最優(yōu)值在?Uabf(xt)處取得,若在Uadm的頂點(diǎn)處取得最優(yōu)值時(shí),那么,CLF和CBF約束將都不起作用.

3) 如圖3(c)所示情況,此時(shí)QP求解可行,且CLF和CBF約束都起作用,QP的最優(yōu)值在?UCLF(xt)和?UCBF(xt)處取得.

4) 如圖3(d)所示情況,此時(shí)Ufea(xt)=?,QP求解不可行.

圖3 QP求解可行性分析Fig.3 Feasibility analysis of QP

根據(jù)上述分析可知,QP求解可行性與CLF和CBF約束相關(guān),即與正常數(shù)γ和λ有關(guān).因此,接下來(lái)分析γ和λ的數(shù)值大小對(duì)QP求解可行性和系統(tǒng)性能的影響.受文獻(xiàn)[33]的研究思路啟發(fā),本文將從狀態(tài)空間的角度分析γ和λ對(duì)QP求解可行性的影響.

3.2 γ和λ對(duì)系統(tǒng)性能和QP求解可行性的影響

3.2.1 γ對(duì)系統(tǒng)收斂速度的影響

根據(jù)式(2)和式(16),利用比較引理[34]易知

由式(17)知,‖x(t)‖以不小于的速率收斂到0.因此,增大γ將加快系統(tǒng)收斂速度,但會(huì)降低QP求解的可行性,這將在下一節(jié)中進(jìn)行討論.

3.2.2 γ對(duì)QP求解可行性的影響

定義6狀態(tài)集合R=R(xt,Uadm,Δt)表示在滿足系統(tǒng)(1)和控制輸入約束Uadm時(shí),系統(tǒng)在狀態(tài)xt處經(jīng)過(guò)極小時(shí)間Δt后所能達(dá)到的狀態(tài)集.

3.2.3 λ對(duì)系統(tǒng)安全性能的影響

根據(jù)CBF約束式(6),利用比較引理可得

為了確保h(x)有下界,λ≥0.

根據(jù)式(22)可知,當(dāng)t確定時(shí),隨著λ的逐漸減小,h(x0)e-λt逐漸增大,即系統(tǒng)距離?C越遠(yuǎn).考慮λ=0時(shí)(x)≥0這一極端情況,系統(tǒng)將隨時(shí)間t逐漸遠(yuǎn)離?C.所以,減小λ將使系統(tǒng)的安全性更容易得到保證.然而,由式(6)可知,減小λ將導(dǎo)致CBF約束的加強(qiáng),從而,QP求解可行性也隨之降低,在下一節(jié)中將通過(guò)幾何關(guān)系進(jìn)行更直觀的分析.

圖4 系統(tǒng)穩(wěn)定性和QP可行性的分析Fig.4 Analysis of system stability and QP feasibility

3.2.4 λ對(duì)QP求解可行性的影響

在時(shí)間區(qū)間[t,t+Δt]內(nèi),根據(jù)式(6),可得

因此,在狀態(tài)空間中,定義在x(t+Δt)時(shí)滿足CBF約束的超水平集為

注2SCBF(xt,0)?SCBF(xt,Δt)總是成立的.

因此,若R(xt,Uadm,Δt)∩SCBF(xt,Δt)=?,則表明QP在狀態(tài)xt處求解不可行.接下來(lái),本文將通過(guò)繪制幾何關(guān)系來(lái)直觀的描述λ對(duì)QP求解可行性的影響.

如圖5所示,定義障礙物外部區(qū)域?yàn)榘踩臻g,紅色區(qū)域表示不安全區(qū)域(障礙物),藍(lán)色區(qū)域表示R(xt,Uadm,Δt),黃色區(qū)域表示SCBF(xt,0),黃色實(shí)線表示?SCBF(xt,0),綠色區(qū)域表示SCBF(xt,Δt),綠色實(shí)線表示?SCBF(xt,Δt).根據(jù)不同情況對(duì)QP求解可行性進(jìn)行分析.

1) 如圖5(a)所示情況,R∩SCBF(xt,Δt)/?且R∩SCBF(xt,0)=R,所以系統(tǒng)將朝遠(yuǎn)離障礙物的方向移動(dòng),系統(tǒng)的安全性可以被保證.又由于R?SCBF(xt,Δt),所以此時(shí)CBF 約束在優(yōu)化問(wèn)題中不起作用,QP求解可行性與CLF約束相關(guān).

2) 如圖5(b)所示情況,R∩SCBF(xt,Δt)/?且R∩SCBF(xt,0)?R,所以系統(tǒng)可能朝靠近或遠(yuǎn)離障礙物的方向移動(dòng),但系統(tǒng)的安全性仍可以被保證.此時(shí)CBF約束在優(yōu)化問(wèn)題中不起作用,QP求解可行性與CLF約束相關(guān).

3) 如圖5(c)所示情況,R∩SCBF(xt,Δt)?且R∩SCBF(xt,0)=?,所以系統(tǒng)將朝靠近障礙物的方向移動(dòng),但系統(tǒng)的安全性仍可以被保證.此時(shí)CBF約束是否起作用取決于λ的取值,且QP求解可行性與CLF約束相關(guān).

4) 如圖5(c)-(d)所示情況,當(dāng)Δt一定時(shí),隨著λ數(shù)值的逐漸減小,根據(jù)式(24)可知,SCBF(xt,Δt)的區(qū)域也將逐漸縮小,進(jìn)而導(dǎo)致R ∩SCLF(xt,Δt)=?,此時(shí)QP求解不可行,系統(tǒng)的安全性無(wú)法被保證.

圖5 系統(tǒng)安全性和QP可行性的分析Fig.5 Analysis of system safety and QP feasibility

根據(jù)上述分析,γ和λ對(duì)系統(tǒng)性能和QP求解可行性影響的結(jié)論如表1所示.

表1 γ和λ對(duì)性能的影響Table 1 Performance effects of γ and λ

當(dāng)同時(shí)考慮CLF,CBF和控制輸入約束時(shí),若R(xt,Uadm,Δt)∩SCLF(xt,Δt)∩SCBF(xt,Δt)/?,則QP求解可行,系統(tǒng)可實(shí)現(xiàn)最優(yōu)安全鎮(zhèn)定控制.

3.3 設(shè)計(jì)QP新形式

其中:H為正定矩陣,常數(shù)q,p ＞0為懲罰因子,正常數(shù)ud,γd,λd為期望值,γ和λ為決策變量.

根據(jù)式(26),λ非負(fù),因此,需要始終確保系統(tǒng)的安全性.當(dāng)系統(tǒng)靠近障礙物時(shí),決策變量γ減小,λ增大,在確保系統(tǒng)安全的前提下放寬穩(wěn)定性約束以此提高QP求解可行性.當(dāng)系統(tǒng)遠(yuǎn)離障礙物時(shí),由于成本函數(shù)中的懲罰項(xiàng),使得γ最終會(huì)達(dá)到期望值γd,因此系統(tǒng)的穩(wěn)定性得到保證.

注3雖然提出的QP(26)新形式可以提高優(yōu)化問(wèn)題求解可行性,但當(dāng)輸入約束較苛刻時(shí),QP仍變得求解不可行.

3.4 設(shè)計(jì)CBF新形式

本文基于文獻(xiàn)[10]中CBF的形式定義標(biāo)量函數(shù)

其中:μ,c1,c2,c3＞0為常數(shù),α為擴(kuò)展的K類函數(shù),標(biāo)量函數(shù)d(p,o)為式(13).

假設(shè)4對(duì)于仿射非線性系統(tǒng)(1),系統(tǒng)速度狀態(tài)∈Rk的微分為

定理3對(duì)于滿足假設(shè)4的仿射非線性系統(tǒng)(1),式(27)為全局有效CBF,且保證系統(tǒng)與障礙物間始終保持安全距離.

根據(jù)式(13),可得

注4通過(guò)調(diào)節(jié)正常數(shù)c1,c2,c3可以改變系統(tǒng)與障礙物間的最小距離,但c1的選取不能太小.

4 仿真驗(yàn)證

在假設(shè)1-4滿足的前提下,本文使用線性平面四旋翼(12)在具有障礙物的環(huán)境中進(jìn)行軌跡跟蹤控制1仿真案例視頻鏈接: https://b23.tv/vZL2yN2..同時(shí),基于MATLAB(R2019b)仿真平臺(tái),“ode45”微分方程求解器,YALMIP最優(yōu)化工具箱和IPOPT“內(nèi)點(diǎn)法”求解器來(lái)驗(yàn)證所提出算法的有效性和優(yōu)越性.仿真中所用參數(shù)如表2所示.

表2 仿真中使用的參數(shù)值Table 2 Parameter values used in simulation

注5由于平面四旋翼動(dòng)力學(xué)模型在線性化過(guò)程中進(jìn)行變量替換=u-ue,ue=[mg 0]T.因此,設(shè)置Fmin=-9.81 N.另外,對(duì)于平面四旋翼線性模型(12),僅當(dāng)四旋翼與障礙物中心位置在同一水平線上時(shí),(p-o)T(x)/01×m,此時(shí),假設(shè)4不成立,本文中該情況被忽略.最后,為了提高系統(tǒng)的收斂速度和安全性,γd的選取應(yīng)偏大,λd的選取應(yīng)偏小.

為了確保系統(tǒng)的穩(wěn)定性,本文根據(jù)文獻(xiàn)[24],選擇CLF為

其中:ex=x-xd,x為系統(tǒng)狀態(tài),xd為期望狀態(tài),P為正定矩陣,仿真中通過(guò)MATLAB內(nèi)置函數(shù)lqr獲得P.

4.1 動(dòng)態(tài)障礙物環(huán)境中避障失敗

給定參考軌跡為

半徑為1的動(dòng)態(tài)障礙物1的移動(dòng)軌跡為

根據(jù)文獻(xiàn)[10],選取CBF的形式為

仿真結(jié)果如圖6-7所示.根據(jù)圖7可知,當(dāng)t=15 s時(shí),d(p,o1)＜0表示四旋翼與障礙物相碰;當(dāng)t=35 s時(shí),d(p,o2)=0表示四旋翼緊靠障礙物邊緣移動(dòng),因此當(dāng)系統(tǒng)的體積不能被忽略時(shí),四旋翼將與障礙物發(fā)生碰撞.由此可知,文獻(xiàn)[10]中所設(shè)計(jì)的CBF不能處理考慮動(dòng)態(tài)系統(tǒng)體積或環(huán)境中存在快速移動(dòng)障礙物時(shí)的情況.接下來(lái),將驗(yàn)證本文所給方法的有效性.

圖6 平面四旋翼運(yùn)行軌跡Fig.6 The trajectory of planar quadrotor

圖7 函數(shù)d(p,o1)和d(p,o2)Fig.7 Functions d(p,o1)and d(p,o2)

4.2 靜態(tài)障礙物環(huán)境下軌跡跟蹤控制

給定系統(tǒng)的參考軌跡為式(34),靜態(tài)障礙物1,2分別位于o1=[0 3.5]和o2=[0-3.5],其半徑為r=1.系統(tǒng)初始狀態(tài)為x0=[-4.5-1 0 0 0 0]T,期望狀態(tài)為xd(t)=[yr(t)zr(t) 0(t)(t) 0]T.選擇CBF為式(27).

仿真結(jié)果如圖8-11所示.根據(jù)圖9可知,d(p,o1)＞0,d(p,o2)＞0,四旋翼始終與靜態(tài)障礙物保持一定距離,因此系統(tǒng)觸碰障礙物的風(fēng)險(xiǎn)被大大降低.另外,從圖10可以看出,當(dāng)四旋翼靠近障礙物時(shí),γ將減小,λ將增大,從而提高QP可行性使系統(tǒng)能正常運(yùn)行.根據(jù)圖11可知,控制輸入約束始終得到滿足.因此,相比于文獻(xiàn)[10],本文所給CBF形式使系統(tǒng)的安全性更高.

圖8 平面四旋翼運(yùn)行軌跡Fig.8 The trajectory of planar quadrotor

圖9 函數(shù)d(p,o1)和d(p,o2)Fig.9 Functions d(p,o1)and d(p,o2)

圖10 決策變量γ和λFig.10 Decision variables γ and λ

圖11 控制輸入F和MFig.11 Control inputs F and M

4.3 動(dòng)態(tài)障礙物環(huán)境下軌跡跟蹤控制

給定系統(tǒng)的參考軌跡為式(34),動(dòng)態(tài)障礙物移動(dòng)軌跡為式(35).系統(tǒng)初始狀態(tài)為x0=[-4.5-1 0 0 0 0]T,期望狀態(tài)為xd(t)=[yr(t)zr(t)0(t)(t)0]T.選擇CBF為(27).

仿真結(jié)果如圖12-15所示.從圖13可知,d(p,o)＞0,四旋翼始終與動(dòng)態(tài)障礙物保持一定距離,使得四旋翼能及時(shí)避開(kāi)障礙物,系統(tǒng)的安全性得到提高.從圖14中綠框區(qū)域可以看出,當(dāng)四旋翼靠近障礙物時(shí),γ將減小,λ將增大,從而提高QP求解可行性使四旋翼能正常運(yùn)行,從紅框區(qū)域可以看出,在確保QP求解可行的前提下,λ將減小使得四旋翼能更早避開(kāi)動(dòng)態(tài)障礙物.另外,根據(jù)圖15可知,控制輸入約束始終得到滿足.

圖12 平面四旋翼運(yùn)行軌跡Fig.12 The trajectory of planar quadrotor

圖13 函數(shù)d(p,o)Fig.13 Function d(p,o)

圖14 決策變量γ和λFig.14 Decision variables γ and λ

圖15 控制輸入F和MFig.15 Control inputs F and M

若使用QP(9)并選取常數(shù)γ=γd=10,λ=λd=2,當(dāng)四旋翼靠近動(dòng)態(tài)障礙物時(shí),QP變得求解不可行,其在MATLAB中顯示信息為

因此,本文所給QP新形式的可求解性更高.

5 結(jié)論

本文從控制輸入和狀態(tài)空間的角度分別分析了QP求解可行性以及γ和λ對(duì)QP求解可行性和系統(tǒng)性能的影響,從而提出了一種CLF-CBF-QP新形式,以此來(lái)提高優(yōu)化問(wèn)題的可行性.另外,本文設(shè)計(jì)一種CBF新形式,使得當(dāng)考慮動(dòng)態(tài)系統(tǒng)本身的體積且環(huán)境中存在動(dòng)態(tài)障礙物時(shí),仍能保證系統(tǒng)的安全性.最后,通過(guò)線性平面四旋翼在具有障礙物的環(huán)境中進(jìn)行軌跡跟蹤驗(yàn)證所提出方法的有效性和優(yōu)越性.值得注意的是,當(dāng)控制輸入約束較為苛刻時(shí),QP仍會(huì)變得不可行.如何進(jìn)一步提高優(yōu)化問(wèn)題的可行性,這將是筆者未來(lái)研究的重點(diǎn).