本發(fā)明涉及機(jī)器人導(dǎo)航和混成系統(tǒng)形式化驗(yàn)證組合領(lǐng)域，尤其涉及一種基于形式化描述的機(jī)器人自主安全導(dǎo)航方法。
背景技術(shù)：
機(jī)器人安全導(dǎo)航(Robot Safety Navigation)技術(shù)用于檢測動態(tài)的非結(jié)構(gòu)化環(huán)境下機(jī)器人系統(tǒng)存在的碰撞危險，解決機(jī)器人在自主移動中遇到的路徑規(guī)劃問題，實(shí)現(xiàn)機(jī)器人在復(fù)雜場景下的無碰撞導(dǎo)航。機(jī)器人安全導(dǎo)航技術(shù)作為自主移動機(jī)器人的關(guān)鍵技術(shù)，是基于移動機(jī)器人的控制體系結(jié)構(gòu)的。根據(jù)系統(tǒng)中可控制的變量數(shù)目是否小于其姿態(tài)空間的維數(shù)分為完整系統(tǒng)運(yùn)動規(guī)劃和非完整系統(tǒng)運(yùn)動規(guī)劃，進(jìn)一步考慮基于運(yùn)動學(xué)約束的系統(tǒng)路徑規(guī)劃。在過去的研究中，研究人員已經(jīng)提出了很多導(dǎo)航方法，如人工試場法，隨機(jī)路徑規(guī)劃法，柵格法，A*/D*法，但是這些方法都沒有從混成系統(tǒng)的角度出發(fā)，對導(dǎo)航系統(tǒng)的安全性質(zhì)進(jìn)行完備驗(yàn)證。機(jī)器人導(dǎo)航系統(tǒng)是一個典型的混成系統(tǒng)，包括離散的控制指令跳轉(zhuǎn)和連續(xù)的運(yùn)動行為交織。近年來，在混成系統(tǒng)形式化建模與驗(yàn)證領(lǐng)域的研究越來越多，采用模型檢驗(yàn)和定理驗(yàn)證的方法對復(fù)雜的軟硬件系統(tǒng)進(jìn)行安全驗(yàn)證取得了很好的效果。
技術(shù)實(shí)現(xiàn)要素：
為了解決移動機(jī)器人自主安全導(dǎo)航問題，本發(fā)明提供了一種實(shí)時的，滿足形式化規(guī)約的安全導(dǎo)航算法。由于人力無法窮盡地遍歷導(dǎo)航系統(tǒng)所有可能的運(yùn)行輸入與場景，采用該算法保障實(shí)時控制系統(tǒng)的安全性。
為了達(dá)到上述技術(shù)效果，本發(fā)明技術(shù)方案如下：
一種基于形式化描述的機(jī)器人自主安全導(dǎo)航方法，包含以下步驟：
步驟1：對機(jī)器人導(dǎo)航系統(tǒng)進(jìn)行抽象建模，并且對其需要滿足的安全性質(zhì)進(jìn)行形式化規(guī)約；
步驟2：采用形式化語言描述機(jī)器人安全導(dǎo)航算法；
步驟3：建立機(jī)器人導(dǎo)航系統(tǒng)的狀態(tài)遷移模型，通過形式化解算判定任意時刻滿足安全性質(zhì)的控制輸入，并對機(jī)器人運(yùn)動狀態(tài)進(jìn)行預(yù)測和更新。
進(jìn)一步地，所述步驟1的過程如下：
1)抽象機(jī)器人的運(yùn)動軌跡為分段直線，機(jī)器人的運(yùn)動狀態(tài)模型為：
式中表示機(jī)器人在二維平面中的位置，和分別表示在x，y方向上的分量；
式中表示機(jī)器人在二維平面中的速度，vr表示速度的模，和分別表示速度方向向量在x，y方向上的分量；
式中ar表示機(jī)器人在二維平面中的加速度，加速度與速度具有相同的方向；
t表示運(yùn)動時間。
2)采用建模語言對系統(tǒng)中物體的離散控制行為和連續(xù)運(yùn)動狀態(tài)進(jìn)行形式化描述：包括機(jī)器人的離散控制行為ctrlo和環(huán)境中障礙物的離散控制行為ctrlr，以及系統(tǒng)中所有運(yùn)動物體的連續(xù)運(yùn)動演化dyn。
3)首先根據(jù)全向移動機(jī)器人的運(yùn)動學(xué)約束，對模型中的離散控制輸入?yún)?shù)進(jìn)行限定：ar∈[-B,A],vr∈[0,V]，根據(jù)移動平臺的最大制動能力定義加速度下限B；根據(jù)最大加速能力定義加速度上限A；根據(jù)最大速度限制定義速度上限V。
再根據(jù)被動友好(Passive Friendly Safety)安全性質(zhì)定義導(dǎo)航系統(tǒng)的安全限制條件，避免機(jī)器人和障礙物發(fā)生碰撞：
其中φ為需要滿足的安全條件；為障礙物的當(dāng)前位置；為障礙物到機(jī)器人的距離；為機(jī)器人從當(dāng)前狀態(tài)到下一次控制單元發(fā)送制動指令前的運(yùn)行距離，其中ε為從機(jī)器人控制單元發(fā)送指令到運(yùn)動機(jī)構(gòu)開始執(zhí)行的間隔時間；為控制單元發(fā)送制動指令到機(jī)器人到達(dá)靜止?fàn)顟B(tài)，機(jī)器人運(yùn)行的距離；為障礙物從原速運(yùn)動到發(fā)現(xiàn)機(jī)器人后減速至靜止期間運(yùn)行的距離，其中vo為工作環(huán)境中障礙物的最大運(yùn)動速度，Bo為障礙物的制動能力，τ為障礙物的最長反應(yīng)時間；分別為機(jī)器人和障礙物的運(yùn)動位移在兩者位移差向量上的投影。
進(jìn)一步地，所述步驟2的過程如下：
采用一種基于動態(tài)窗口的安全導(dǎo)航算法。根據(jù)運(yùn)動學(xué)約束和安全限制條件過濾掉不符合要求的控制輸入，建立當(dāng)前時刻的窗口Wt，從Wt按需選取控制輸入?yún)?shù)實(shí)現(xiàn)自主導(dǎo)航。運(yùn)行至下一時刻，迭代更新動態(tài)窗口Wt+1，重復(fù)之前的算法。
進(jìn)一步地，所述步驟3的過程如下：
建立機(jī)器人導(dǎo)航系統(tǒng)的狀態(tài)機(jī)遷移圖，狀態(tài)機(jī)遷移圖中的狀態(tài)節(jié)點(diǎn)對應(yīng)機(jī)器人系統(tǒng)的不同運(yùn)動模式，并為每個節(jié)點(diǎn)添加相應(yīng)的連續(xù)運(yùn)動變化規(guī)則。定義狀態(tài)機(jī)模型為多元組H＝(X,∑,S,E,S0,α,β,γ)。
X是導(dǎo)航系統(tǒng)實(shí)數(shù)變量的有限集合，X中變量的個數(shù)為狀態(tài)機(jī)模型的維數(shù)；
∑是控制命令的有限集合，S是運(yùn)動狀態(tài)節(jié)點(diǎn)的有限集合；
E是轉(zhuǎn)化關(guān)系的集合，E中的元素e具有形式其中，s，s'是S中的元素；σ∈∑是轉(zhuǎn)換上的事件名；是一個將E中的轉(zhuǎn)換e標(biāo)注為一組約束的標(biāo)注函數(shù)，表示當(dāng)系統(tǒng)行為觸發(fā)轉(zhuǎn)換e時，相應(yīng)變量的取值滿足此約束；ψ是形為ar:＝A的重置動作集合，表示當(dāng)系統(tǒng)行為觸發(fā)此轉(zhuǎn)換后，相應(yīng)變量ar的取值會被重置為A。
是系統(tǒng)初始狀態(tài)的集合。
α是一個標(biāo)注函數(shù)它將每個位置映射到一個節(jié)點(diǎn)不變式，表示系統(tǒng)行為停留在相關(guān)節(jié)點(diǎn)時，相應(yīng)變量取值滿足此約束。
β是一個為S中每個位置節(jié)點(diǎn)添加流條件(微分方程)的標(biāo)注函數(shù)，表示當(dāng)系統(tǒng)行為停留在相關(guān)節(jié)點(diǎn)時，相應(yīng)變量取值變化隨著時間增長滿足此條件。
γ是一個標(biāo)注函數(shù)，它將初始位置So中每個狀態(tài)映射到一組初始條件，初始條件具有形式對任意狀態(tài)s∈So，有且僅有一組滿足條件。
根據(jù)狀態(tài)機(jī)模型，利用差分動態(tài)邏輯dL(Differential Dynamic Logic)對系統(tǒng)控制輸入進(jìn)行推演，驗(yàn)證從初始狀態(tài)出發(fā)經(jīng)過導(dǎo)航算法計算出的所有變遷序列，始終滿足被動友好安全性質(zhì)：
與現(xiàn)有技術(shù)相比，本發(fā)明技術(shù)方案的有益效果是：
本發(fā)明通過建立導(dǎo)航系統(tǒng)的抽象模型，控制機(jī)器人從安全初始狀態(tài)出發(fā)，經(jīng)過基于動態(tài)窗口的導(dǎo)航算法的迭代計算，保障機(jī)器人系統(tǒng)始終處于安全邊界內(nèi)。相比以往的導(dǎo)航算法，具有更完備的安全性質(zhì)。
附圖說明
圖1為本發(fā)明算法的流程圖；
具體實(shí)施方式
附圖僅用于示例性說明，不能理解為對本專利的限制；
于本領(lǐng)域技術(shù)人員來說，附圖中某些公知結(jié)構(gòu)及其說明可能省略是可以理解的。
下面結(jié)合附圖和實(shí)施例對本發(fā)明做進(jìn)一步的說明。
實(shí)施例1
一種基于形式化描述的機(jī)器人自主安全導(dǎo)航方法，包含以下步驟：
步驟1：對機(jī)器人導(dǎo)航系統(tǒng)進(jìn)行抽象建模，并且對其需要滿足的安全性質(zhì)進(jìn)行形式化規(guī)約；步驟2：采用形式化語言描述機(jī)器人安全導(dǎo)航算法；
步驟3：建立機(jī)器人導(dǎo)航系統(tǒng)的狀態(tài)遷移模型，通過形式化解算判定任意時刻滿足安全性質(zhì)
的控制輸入，并對機(jī)器人運(yùn)動狀態(tài)進(jìn)行預(yù)測和更新。
進(jìn)一步地，所述步驟1的過程如下：
1)抽象機(jī)器人的運(yùn)動軌跡為分段直線，機(jī)器人的運(yùn)動狀態(tài)模型為：
式中表示機(jī)器人在二維平面中的位置，和分別表示在x，y方向上的分量；
式中表示機(jī)器人在二維平面中的速度，vr表示速度的模，和分別表示速度方向向量在x，y方向上的分量；
式中ar表示機(jī)器人在二維平面中的加速度，加速度與速度具有相同的方向；
t表示運(yùn)動時間。
2)采用建模語言對系統(tǒng)中物體的離散控制行為和連續(xù)運(yùn)動狀態(tài)進(jìn)行形式化描述：包括機(jī)器人的離散控制行為ctrlr和環(huán)境中障礙物的離散控制行為ctrlo，以及系統(tǒng)中所有運(yùn)動物體的連續(xù)運(yùn)動演化dyn。
機(jī)器人的離散行為可描述為：
障礙物的離散行為可描述為：
機(jī)器人的連續(xù)運(yùn)動演化可描述為：
3)首先根據(jù)全向移動機(jī)器人的運(yùn)動學(xué)約束，對模型中的離散控制輸入?yún)?shù)進(jìn)行限定：ar∈[-B,A],vr∈[0,V]，根據(jù)移動平臺的最大制動能力定義加速度下限B；根據(jù)最大加速能力定義加速度上限A；根據(jù)最大速度限制定義速度上限V。
再根據(jù)被動友好(Passive Friendly Safety)安全性質(zhì)定義導(dǎo)航系統(tǒng)的安全限制條件，避免機(jī)器人和障礙物發(fā)生碰撞：
其中為需要滿足的安全條件；為障礙物的當(dāng)前位置；為障礙物到機(jī)器人的距離；為機(jī)器人從當(dāng)前時刻開始，到下一時刻控制單元發(fā)送制動指令，至減速為零期間運(yùn)行的距離，其中ε為從機(jī)器人控制單元發(fā)送指令到運(yùn)動機(jī)構(gòu)開始執(zhí)行的間隔時間；為障礙物從當(dāng)前時刻到機(jī)器人停止運(yùn)動期間運(yùn)行的距離；為障礙物從原速運(yùn)動到發(fā)現(xiàn)機(jī)器人后減速至靜止期間運(yùn)行的距離，其中vo為工作環(huán)境中障礙物的最大運(yùn)動速度，Bo為障礙物的制動能力，τ為障礙物的最長反應(yīng)時間；分別為機(jī)器人和障礙物的運(yùn)動位移在兩者位移差向量上的投影。
所述步驟2的過程如下：
采用一種基于動態(tài)窗口的安全導(dǎo)航算法。根據(jù)運(yùn)動學(xué)約束和安全限制條件過濾掉不符合要求的控制輸入，建立當(dāng)前時刻的窗口Wt，從Wt按需選取控制輸入?yún)?shù)實(shí)現(xiàn)自主導(dǎo)航。運(yùn)行至下一時刻，迭代更新動態(tài)窗口Wt+1，重復(fù)之前的算法。
所述步驟3的過程如下：
建立機(jī)器人導(dǎo)航系統(tǒng)的狀態(tài)機(jī)遷移圖，狀態(tài)機(jī)遷移圖中的狀態(tài)節(jié)點(diǎn)對應(yīng)機(jī)器人系統(tǒng)的不同運(yùn)動模式，并為每個節(jié)點(diǎn)添加相應(yīng)的連續(xù)運(yùn)動變化規(guī)則。定義狀態(tài)機(jī)模型為多元組H＝(X,∑,S,E,S0,α,β,γ)。
X是導(dǎo)航系統(tǒng)實(shí)數(shù)變量的有限集合，X中變量的個數(shù)為狀態(tài)機(jī)模型的維數(shù)；
Σ是控制命令的有限集合，S是運(yùn)動狀態(tài)節(jié)點(diǎn)的有限集合；
E是轉(zhuǎn)化關(guān)系的集合，E中的元素e具有形式其中，s，s'是S中的元素；σ∈Σ是轉(zhuǎn)換上的事件名；是一個將E中的轉(zhuǎn)換e標(biāo)注為一組約束的標(biāo)注函數(shù)，表示當(dāng)系統(tǒng)行為觸發(fā)轉(zhuǎn)換e時，相應(yīng)變量的取值滿足此約束；ψ是形為ar:＝A的重置動作集合，表示當(dāng)系統(tǒng)行為觸發(fā)此轉(zhuǎn)換后，相應(yīng)變量ar的取值會被重置為A。
是系統(tǒng)初始狀態(tài)的集合。
α是一個標(biāo)注函數(shù)它將每個位置映射到一個節(jié)點(diǎn)不變式，表示系統(tǒng)行為停留在相關(guān)節(jié)點(diǎn)時，相應(yīng)變量取值滿足此約束。
β是一個為S中每個位置節(jié)點(diǎn)添加流條件(微分方程)的標(biāo)注函數(shù)，表示當(dāng)系統(tǒng)行為停留在相關(guān)節(jié)點(diǎn)時，相應(yīng)變量取值變化隨著時間增長滿足此條件。
γ是一個標(biāo)注函數(shù)，將初始位置So中每個狀態(tài)映射到一組初始條件，初始條件具有形式對任意狀態(tài)s∈So，有且僅有一組滿足條件。
根據(jù)狀態(tài)機(jī)模型，利用差分動態(tài)邏輯dL(Differential Dynamic Logic)對系統(tǒng)控制輸入進(jìn)行推演，驗(yàn)證從初始狀態(tài)出發(fā)經(jīng)過導(dǎo)航算法計算出的所有變遷序列，始終滿足被動友好安全性質(zhì)：
以上對本發(fā)明的具體實(shí)施例進(jìn)行了詳細(xì)描述，但其只是作為范例，本發(fā)明并不限制于以上描述的具體實(shí)施例。對于本領(lǐng)域技術(shù)人員而言，任何對本發(fā)明進(jìn)行的等同修改和替代也都在本發(fā)明的范疇之中。因此，在不脫離本發(fā)明的精神和范圍下所作的均等變換和修改，都應(yīng)涵蓋在本發(fā)明的范圍內(nèi)。