信號時序邏輯約束下基于終點回溯的高效規(guī)劃

田戴熒，方 浩，楊慶凱

（北京理工大學(xué)自動化學(xué)院，北京 100081）

1 引 言

近年來，時序邏輯理論作為計算機領(lǐng)域中用來進行正則化檢驗的一套方法[1]，被應(yīng)用于機器人規(guī)劃與控制中，取得了極大的應(yīng)用成果。時序邏輯理論的主要思想是將時序邏輯任務(wù)約束轉(zhuǎn)換為可以處理的約束形式，從而在規(guī)劃時考慮這些約束，生成滿足任務(wù)的路徑。其中，線性時序邏輯（Linear Temporal Logic, LTL）將時序邏輯約束轉(zhuǎn)換為Buchi˙˙ 自動機[2]，而對于信號時序邏輯（Signal Temporal Logic, STL），由于其處理的主要是連續(xù)系統(tǒng)，不能找到一個合適的圖來作為其約束集。

針對連續(xù)系統(tǒng)中信號時序邏輯的控制方法，近年來有越來越多的學(xué)者進行了研究。伯克利大學(xué)的Raman 等人[3]將信號時序邏輯編碼為離散值的組合作為整數(shù)約束，之后結(jié)合模型預(yù)測控制，將信號時序邏輯約束下的規(guī)劃問題建模為混合整數(shù)線性規(guī)劃問題。Bapinar 等人[4]將混合整數(shù)線性規(guī)劃方法用到了多無人機系統(tǒng)的規(guī)劃與控制中。Farahani 等人[5]在混合整數(shù)線性規(guī)劃方法的基礎(chǔ)上，針對自動系統(tǒng)所處的不確定性環(huán)境研究了控制的魯棒性問題，對最壞情況下的模型預(yù)測控制問題進行了求解，從而得到了一個反應(yīng)式的控制器。舒新峰等人[6]提出了一種命題投影時序邏輯的分布式模型檢測方法，緩解模型檢測的狀態(tài)空間爆炸問題。此外，為了給STL 約束構(gòu)建一個光滑的約束函數(shù)，Haghighi 等人[7]提出了一種新的光滑可微STL 量化語義來累積魯棒性，并通過策略上升方法求解一系列光滑優(yōu)化問題有效地計算控制策略。Lars 等人對此提供了一種新的視角，他們首先定義了控制屏障函數(shù)與信號時序邏輯進行結(jié)合，同時，考慮時變控制屏障函數(shù)，其中時間特性用于滿足信號時序邏輯任務(wù)。該控制器由計算效率高的凸二次規(guī)劃和局部反饋控制律給出[8]。同時，基于三維時空的規(guī)劃方法[9]也有了較多研究。這些方法可以與傳統(tǒng)控制方法有很好的結(jié)合，比如將混合規(guī)劃得到的結(jié)果輸入到具有非線性前饋補償與閉環(huán)反饋控制系統(tǒng)[10]中，或者基于航路點分段的預(yù)測校正方法[11]中，作為額外知識指導(dǎo)校正。這些方法可作為無人機的避障航路規(guī)劃算法[12]，引導(dǎo)無人機生成滿足任務(wù)約束的路徑。

除控制視角之外，有大量學(xué)者嘗試從學(xué)習(xí)的角度出發(fā)，對信號時序邏輯約束的規(guī)劃問題進行有效求解。其中Aksaray 等人[13]針對STL 的健壯性不是Q-learning 標準回報形式這一問題，對STL健壯性指標用標準回報形式的函數(shù)進行了近似，從而能夠利用標準強化學(xué)習(xí)生成滿足STL 約束的路徑。Balakrishnan 等人[14]進一步設(shè)計了每一個局部狀態(tài)局部任務(wù)的回報獎勵，并且將局部狀態(tài)組合后能夠滿足整體STL 任務(wù)的約束。除了Q-learning 之外，考慮到STL 的正確性是以與子公式數(shù)量和時間約束數(shù)量呈指數(shù)的計算復(fù)雜量來保證的，Kurtz 等人[15]提出了一種替代的求解方法，這種新的求解方法依賴于貝葉斯優(yōu)化而不是混合整數(shù)線性規(guī)劃。貝葉斯優(yōu)化放寬了滿足STL任務(wù)的概率完備性，即路徑在一定概率范圍內(nèi)可能違反任務(wù)，但是求解效率大幅提高。此外，考慮利用STL 任務(wù)具有特定模式，其中的一部分規(guī)劃結(jié)果可以作為知識指導(dǎo)其他任務(wù)的路徑生成，Li 等人[16]提出了一種新的基于圖的時空邏輯，通過將Hilbert 公理化應(yīng)用于基于圖的時空邏輯，采用Modus ponens 和IRR 作為推理規(guī)則最終實現(xiàn)了一套完備的邏輯推理框架，可以用于知識表示和自動推理。從STL 自身具有時間約束的特性出發(fā)，Varnai 等人[17]針對具有部分已知動力學(xué)的系統(tǒng)，提出了一種基于抽樣的學(xué)習(xí)方法來解決包含任務(wù)滿足約束的最優(yōu)控制問題。用STL 構(gòu)建動態(tài)輔助任務(wù)，引入了一個控制器衍生框架，從而能夠?qū)W(xué)習(xí)進行高效率的指導(dǎo)。有了高效率的控制器，STL 語言完全可以應(yīng)用于復(fù)雜系統(tǒng)，比如多機器人的規(guī)劃問題[18-20]中。

以上提及的方法中，基于控制的方法在線規(guī)劃時具有極高的計算復(fù)雜度，基于學(xué)習(xí)的方法收斂時間長，且得到的路徑重用性差，當起點或者目標點發(fā)生改變時，需要重新進行學(xué)習(xí)以生成最優(yōu)路徑。針對這些問題，本文提出一種基于終點回溯的高效規(guī)劃方法。注意到未來操作符以及過去操作符的對應(yīng)順序，將所有未來（過去）操作符轉(zhuǎn)換為對應(yīng)的過去（未來）操作符，對所有轉(zhuǎn)換后的操作符構(gòu)建對應(yīng)的Transducer[21]。在離線構(gòu)建階段，在機器人的工作環(huán)境中加入時間維信息，構(gòu)建時空聯(lián)合空間，在該聯(lián)合空間中，提前指定滿足任務(wù)的合理終點，在Transducer 的指導(dǎo)下，以終點作為快速隨機搜索樹的起點[22]，回溯至初始狀態(tài)平面。當采樣點足夠多時，每一個初始狀態(tài)都能夠在快速隨機搜索樹中尋找到一條時間反序序列滿足任務(wù)約束。在線規(guī)劃階段，每一個狀態(tài)根據(jù)快速隨機搜索樹生成完成任務(wù)代價最小的時間反序序列，之后利用模型預(yù)測控制進行跟蹤。

2 信號時序邏輯指導(dǎo)的離線構(gòu)建

信號時序邏輯是一類形式化語言，能夠表述時間與邏輯上的約束關(guān)系。其在機器人規(guī)劃領(lǐng)域的應(yīng)用價值在于其能夠提供高階任務(wù)的表述方法，來控制機器人完成一系列具有先后順序關(guān)系的動作，比如，先打開門才能進入房間，以及機器人每30 分鐘要前去充電。STL 的具體語法定義為

其中，I=[a,b]為一時間區(qū)域；p為原子命題，通常為環(huán)境的標簽或者機器人的某一個動作；?φ是對φ取反；為φ1或是未來形式，指在φ1為假之后的I時刻內(nèi)，φ2需要為真；φ1SIφ2是上式的過去形式，指在φ2成真之后的I時刻內(nèi)φ1需要為真。各公式的形式化定義如下，其中w為一帶有時間的序列。

此外，F(xiàn)（Eventually）、G（Always）以及它們對應(yīng)的過去形式可以定義如式(3)。

Thomas 等人[21]已經(jīng)說明，所有STL 公式均可以通過F、F、UI以及SI構(gòu)造得到，因此之后以這幾個公式為例子。

定義h為判斷每一個公式是否為真的最小時間間隔，其定義如式(4)：

對于STL 公式，存在度量一條路徑w對于公式的滿足程度的公制單位，稱為魯棒程度，r：w×φ→?。r采用迭代形式進行計算，如式(5)所示。

每一個 STL 的時序邏輯符都有一個Transducer 與之對應(yīng)，如圖1 所示。對于任意一個任務(wù)，可以將其所有的時序邏輯符的Transducer 按照公式樹的順序組合起來，便能夠形成整個公式的Transducer。

定義 1.對于任意一個帶有時間的序列，其時間反序序列定義為

定理1.對于任何一個任務(wù)，若存在一個序列滿足未來（過去）形式任務(wù)，則此序列的時間反序序列滿足該任務(wù)對應(yīng)的過去（未來）形式。

圖1 (a)為 F(0,b)對應(yīng)的Transducer，(b)為 U(0,∞) 對應(yīng)的Transducer，(c)為φ：=G(0,∞) F[0,5)?p1 (p2U[0,10] p3G(0,15) p4)對應(yīng)的語法樹Fig.1 (a), (b) are the transducer of F(0,b)and U(0,∞),respectively.(c) is the parse tree of φ：=G(0,∞) F[0,5)?p1 (p2U[0,10] p3G(0,15) p4)

證.證明僅以 UI以及 SI為例，其他對應(yīng)公式證明方法類似。假設(shè)w =( s0, t0)( s1, t1)...( sn, tn∣)= a UIb，則根據(jù)公式（2），有

則在構(gòu)建時間反序序列時，有

這恰好符合a SIb的定義，因此如果w∣= aUIb，則有 w∣'= a SIb。

在機器人工作的( x,y)空間中，加入時間維信息，構(gòu)建(x , y,t ) 三維空間，并根據(jù)機器人被指定的任務(wù)預(yù)估其終點，則在t h= 的平面上提前放置終點，表示為sd=( xd, yd, h)。將該點作為根節(jié)點。同時，將給定任務(wù)的所有未來（過去）操作符轉(zhuǎn)換為過去（未來）操作符，并構(gòu)建轉(zhuǎn)換后任務(wù) φn的Transducer，表示為TSn。在TSn的指導(dǎo)下，自sd向t= 0的平面生長快速隨機搜索樹。根據(jù)定理1，快速隨機搜索樹生長的所有滿足轉(zhuǎn)換后任務(wù)φn的時間反序序列，在正向執(zhí)行時一定滿足原任務(wù)要求。

將所有采樣點的集合表示為S，所有完成任務(wù)的采樣點的集合表示為 S ',對于每一個采樣點sa ∈ S，根據(jù)其與終點距離給定一個代價值，同時，將對STL 任務(wù)的符合程度乘以偏好因子加入到代價值中，定義如式(6)。

在構(gòu)建完搜索樹之后，樹上的每一個節(jié)點都有對應(yīng)的一個代價值，表征著從這個節(jié)點開始到達終點的路徑的整體代價。

3 在線規(guī)劃

當定理1 采樣到足夠多的數(shù)據(jù)點之后，離線構(gòu)建搜索樹結(jié)束，此時所有滿足任務(wù) φn的葉子節(jié)點前往根節(jié)點的路徑在正向執(zhí)行時一定滿足原任務(wù)。在這些葉子節(jié)點足夠多時，初始狀態(tài)平面的每個有可能完成任務(wù)的狀態(tài)點就將被覆蓋到?？紤]一個以完成任務(wù)的葉子節(jié)點為頂點，最大速度作為斜面斜率絕對值的圓錐（如圖2 所示），則圓錐中所有狀態(tài)點都可以通過導(dǎo)航至圓錐頂點，并由頂點到隨機搜索樹根節(jié)點連接一條后續(xù)路徑來生成滿足任務(wù)約束的路徑。

在任意給定一個初始狀態(tài)之后，機器人將在所有的滿足任務(wù)葉子節(jié)點L 中選擇代價值 Jsa最小的那個葉子節(jié)點，從葉子節(jié)點到根節(jié)點依次連接，能夠生成一條滿足任務(wù)的時間序列w。在w中以一定時間分辨率進行點的采樣，便可以構(gòu)成一條由帶有時間的路徑點構(gòu)成的路徑pa。本文中采用模型預(yù)測控制對pa 進行跟蹤，具體問題建模如式(7)。

該式可以根據(jù)不同的系統(tǒng)進行不同的定義，并可以額外加入其他的約束條件，比如加入控制屏障函數(shù)的約束以保證任意分辨率內(nèi)路徑都會滿足任務(wù)約束。在本文中，由模型預(yù)測控制生成的最優(yōu)控制量可以儲存至對應(yīng)的節(jié)點中，當又一次規(guī)劃至該節(jié)點，且控制對象的狀態(tài)與該點預(yù)置狀態(tài)相近時，可以直接應(yīng)用之前預(yù)置好的控制量進行控制，進一步減少在線運算時間。至此，離線構(gòu)建以及在線規(guī)劃階段的算法都已完成，本文算法整體框架如圖3 所示。

圖2 節(jié)點的覆蓋范圍圓錐Fig.2 Cone representing cover region of leaf nodes

圖3 算法整體框架Fig.3 Framework of algorithm

4 仿真結(jié)果及分析

對于算法的結(jié)果，在內(nèi)核為i5-7300HQ 的計算機上基于Python3 構(gòu)建了仿真程序，進行了仿真實驗。

實驗中指定給機器人的任務(wù)表述為，在實驗開始后的0s 到10s 內(nèi)，到達a區(qū)域，在到達a區(qū)域的10s 到18s 內(nèi)，到達b區(qū)域。STL 公式為實驗的具體環(huán)境如圖 4所示。其中，標為“obs”的藍色圓圈代表障礙物，a區(qū)域和b區(qū)域均為黃色圓圈。

離線構(gòu)建階段生長出的隨機搜索樹如圖5 所示。在構(gòu)建好隨機搜索樹之后，在線規(guī)劃階段任意給定初始狀態(tài)點，可以立即生成離散路徑，三維離散路徑如圖6 所示。對該路徑每隔一定時間間隔進行采樣，便可以生成一系列路徑點。

本文考慮兩輪差速小車模型。其模型描述如式(8)。

圖4 仿真環(huán)境Fig.4 Simulation environment

圖5 快速隨機搜索樹Fig.5 Rapidly-exploring randomized tree

針對該動力學(xué)模型，采用模型預(yù)測控制解決式(7)問題，所得出結(jié)果如圖7 所示。圖7 中藍色三角形為本次規(guī)劃各個離散化時間點處車輛的位置，紅色三角形為上一次規(guī)劃時預(yù)置的控制量。當藍色三角形與紅色三角形基本重疊時，直接采用預(yù)置控制量進行控制，不再進行模型預(yù)測控制。

圖6 三維規(guī)劃路徑Fig.6 Planning path in 3-D view

圖7 最終控制效果 Fig.7 Final control effect

為了進行對比，實現(xiàn)了混合整數(shù)線性規(guī)劃以及貝葉斯優(yōu)化方法?；旌险麛?shù)線性規(guī)劃生成最終路徑耗時108.6 s，貝葉斯優(yōu)化方法使用了17.9 s 在線規(guī)劃生成了在一定概率下滿足任務(wù)的路徑。本文提出的方法在離線為所有初始狀態(tài)點構(gòu)建路徑時，采樣了5000 個點，耗時68.3 s，在線規(guī)劃尋找相關(guān)路徑耗時0.2 s，生成控制量耗時2.3 s，且在成功匹配到預(yù)置狀態(tài)之后，不再需要在線計算控制量。

5 結(jié) 論

本文針對連續(xù)空間內(nèi)在STL 約束下進行路徑規(guī)劃時間復(fù)雜度高，且規(guī)劃的結(jié)果不易重用的問題，將問題放到帶有時間信息的三維空間中進行求解，根據(jù)STL 未來時序操作符號與過去時序操作符的對應(yīng)關(guān)系，利用由終點反溯的隨機搜索樹覆蓋初始狀態(tài)平面，之后在在線規(guī)劃階段尋找代價最小的點，采用模型預(yù)測控制跟蹤該代價最小路徑。通過對這一過程進行仿真，證實了本方法的可行性，且通過與混合整數(shù)線性規(guī)劃以及貝葉斯優(yōu)化方法進行對比，本文所提出方法能夠在較短時間內(nèi)有效求得所有初始狀態(tài)符合任務(wù)約束的可行解。