田戴熒，方 浩，楊慶凱

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

1 引 言

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

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

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

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

2 信號(hào)時(shí)序邏輯指導(dǎo)的離線(xiàn)構(gòu)建

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

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

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

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

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

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

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

定義 1.對(duì)于任意一個(gè)帶有時(shí)間的序列，其時(shí)間反序序列定義為

定理1.對(duì)于任何一個(gè)任務(wù)，若存在一個(gè)序列滿(mǎn)足未來(lái)（過(guò)去）形式任務(wù)，則此序列的時(shí)間反序序列滿(mǎn)足該任務(wù)對(duì)應(yīng)的過(guò)去（未來(lái)）形式。

圖1 (a)為 F(0,b)對(duì)應(yīng)的Transducer，(b)為 U(0,∞) 對(duì)應(yīng)的Transducer，(c)為φ：=G(0,∞) F[0,5)?p1 (p2U[0,10] p3G(0,15) p4)對(duì)應(yīng)的語(yǔ)法樹(shù)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為例，其他對(duì)應(yīng)公式證明方法類(lèi)似。假設(shè)w =( s0, t0)( s1, t1)...( sn, tn∣)= a UIb，則根據(jù)公式（2），有

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

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

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

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

在構(gòu)建完搜索樹(shù)之后，樹(shù)上的每一個(gè)節(jié)點(diǎn)都有對(duì)應(yīng)的一個(gè)代價(jià)值，表征著從這個(gè)節(jié)點(diǎn)開(kāi)始到達(dá)終點(diǎn)的路徑的整體代價(jià)。

3 在線(xiàn)規(guī)劃

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

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

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

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

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

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

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

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

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

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

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

圖5 快速隨機(jī)搜索樹(shù)Fig.5 Rapidly-exploring randomized tree

針對(duì)該動(dòng)力學(xué)模型，采用模型預(yù)測(cè)控制解決式(7)問(wèn)題，所得出結(jié)果如圖7 所示。圖7 中藍(lán)色三角形為本次規(guī)劃各個(gè)離散化時(shí)間點(diǎn)處車(chē)輛的位置，紅色三角形為上一次規(guī)劃時(shí)預(yù)置的控制量。當(dāng)藍(lán)色三角形與紅色三角形基本重疊時(shí)，直接采用預(yù)置控制量進(jìn)行控制，不再進(jìn)行模型預(yù)測(cè)控制。

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

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

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

5 結(jié) 論

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