龍離軍,胡騰飛

(東北大學信息科學與工程學院,遼寧沈陽 110819)

1 引言

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

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

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

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

2 基礎(chǔ)知識

2.1 符號定義

x:=(p,,q)∈Rn表示動態(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的邊界.對于a,b ＞0,如果連續(xù)函數(shù)α:(-b,a)→(-∞,+∞)嚴格單調(diào)遞增且α(0)=0,那么稱α為擴展的K類函數(shù).如無特殊說明,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]對于仿射非線性系統(tǒng)(1),如果存在正常數(shù)c1,c2和γ,使得對于所有x ∈D,連續(xù)可微函數(shù)V:Rn →R滿足

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

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

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

2.3 控制障礙函數(shù)

定義2[11]給定一個連續(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)定控制器以實現(xiàn)自適應(yīng)巡航控制,其具體形式為

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

2.5 平面四旋翼線性模型

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

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

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

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

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

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

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

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

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

2.7 控制問題描述

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

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

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

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

3 主要結(jié)論

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

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

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

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

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

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

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

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

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

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

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

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

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

3.2.2 γ對QP求解可行性的影響

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

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

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

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

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

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

3.2.4 λ對QP求解可行性的影響

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

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

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

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

如圖5所示,定義障礙物外部區(qū)域為安全空間,紅色區(qū)域表示不安全區(qū)域(障礙物),藍色區(qū)域表示R(xt,Uadm,Δt),黃色區(qū)域表示SCBF(xt,0),黃色實線表示?SCBF(xt,0),綠色區(qū)域表示SCBF(xt,Δt),綠色實線表示?SCBF(xt,Δt).根據(jù)不同情況對QP求解可行性進行分析.

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

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

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

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

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

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

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

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

3.3 設(shè)計QP新形式

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

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

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

3.4 設(shè)計CBF新形式

本文基于文獻[10]中CBF的形式定義標量函數(shù)

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

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

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

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

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

4 仿真驗證

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

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

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

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

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

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

給定參考軌跡為

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

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

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

圖6 平面四旋翼運行軌跡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)觸碰障礙物的風險被大大降低.另外,從圖10可以看出,當四旋翼靠近障礙物時,γ將減小,λ將增大,從而提高QP可行性使系統(tǒng)能正常運行.根據(jù)圖11可知,控制輸入約束始終得到滿足.因此,相比于文獻[10],本文所給CBF形式使系統(tǒng)的安全性更高.

圖8 平面四旋翼運行軌跡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 動態(tài)障礙物環(huán)境下軌跡跟蹤控制

給定系統(tǒng)的參考軌跡為式(34),動態(tài)障礙物移動軌跡為式(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,四旋翼始終與動態(tài)障礙物保持一定距離,使得四旋翼能及時避開障礙物,系統(tǒng)的安全性得到提高.從圖14中綠框區(qū)域可以看出,當四旋翼靠近障礙物時,γ將減小,λ將增大,從而提高QP求解可行性使四旋翼能正常運行,從紅框區(qū)域可以看出,在確保QP求解可行的前提下,λ將減小使得四旋翼能更早避開動態(tài)障礙物.另外,根據(jù)圖15可知,控制輸入約束始終得到滿足.

圖12 平面四旋翼運行軌跡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,當四旋翼靠近動態(tài)障礙物時,QP變得求解不可行,其在MATLAB中顯示信息為

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

5 結(jié)論

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