一種基于不確定性預(yù)測的航跡規(guī)劃方法
【技術(shù)領(lǐng)域】
[0001] 本發(fā)明屬于航跡規(guī)劃領(lǐng)域，涉及復(fù)雜環(huán)境下運(yùn)動(dòng)體受運(yùn)動(dòng)不確定性影響較大時(shí)， 通過預(yù)測航跡不確定性來規(guī)劃航跡的方法。
【背景技術(shù)】
[0002] 航跡規(guī)劃的目的是找到空間中從給定起點(diǎn)出發(fā)到目標(biāo)區(qū)域的可行的航跡?，F(xiàn)有的 航跡規(guī)劃方法主要有概略圖法、單元分解法、人工勢場法和基于搜索的航跡規(guī)劃方法，這些 方法的主要缺陷是不予考慮運(yùn)動(dòng)體的運(yùn)動(dòng)學(xué)約束，也不考慮運(yùn)動(dòng)體在實(shí)際運(yùn)動(dòng)過程中受到 的過程噪聲和觀測噪聲的影響，因而無法預(yù)測運(yùn)動(dòng)體在沿所規(guī)劃航跡快速運(yùn)動(dòng)時(shí)的偏差， 不能保證運(yùn)動(dòng)體不與障礙物發(fā)生碰撞。

【發(fā)明內(nèi)容】

[0003] 本發(fā)明目的在于：針對(duì)上述存在的問題，提供一種基于不確定性預(yù)測的航跡規(guī)劃 方法，從而可以在運(yùn)動(dòng)體高速運(yùn)動(dòng)狀態(tài)下，能夠準(zhǔn)確預(yù)測運(yùn)動(dòng)體偏離所規(guī)劃航跡的偏差，保 證航跡的可行性。
[0004] 本發(fā)明的技術(shù)方案如下：一種基于不確定性預(yù)測的航跡規(guī)劃方法，其利用運(yùn)動(dòng)體 的動(dòng)力學(xué)模型、過程噪聲模型和觀測噪聲模型，通過將真實(shí)航跡的偏差劃分為真實(shí)航跡偏 離估計(jì)航跡的偏差和估計(jì)航跡偏離所規(guī)劃航跡的偏差之和，以概率形式預(yù)測真實(shí)航跡的不 確定性，在此基礎(chǔ)上結(jié)合快速擴(kuò)展隨機(jī)樹方法，規(guī)劃出可行的航跡。
[0005] 所述的基于不確定性預(yù)測的航跡規(guī)劃方法，其包括如下具體步驟：
[0006] 步驟1 :確定運(yùn)動(dòng)體的動(dòng)力學(xué)約束和噪聲模型以及其反饋控制器
[0007] 將運(yùn)動(dòng)體的動(dòng)力學(xué)約束表示成如下的線性隨機(jī)離散系統(tǒng)形式
[0008] Xt=AtXt^1+B tUt^1+wt (1)
[0009] zt=Ctxt+vt
[0010] 其中X為狀態(tài)向量，U為控制輸入，過程噪聲Wt~N(0, Qt)服從正態(tài)分布，Z為測 量向量，觀測噪聲Vt~N(0, Rt)服從正態(tài)分布，At為系統(tǒng)矩陣，Bt為控制矩陣，Ct為控制矩 陣，
[0011] 真實(shí)航跡Xt由其估計(jì)值毛用穩(wěn)定控制器K反饋跟蹤某一所規(guī)劃航跡<生成，滿足
[0012]
【主權(quán)項(xiàng)】
1. 一種基于不確定性預(yù)測的航跡規(guī)劃方法，其特征在于，利用運(yùn)動(dòng)體的動(dòng)力學(xué)模型、過 程噪聲模型和觀測噪聲模型，通過將真實(shí)航跡的偏差劃分為真實(shí)航跡偏離估計(jì)航跡的偏差 和估計(jì)航跡偏離所規(guī)劃航跡的偏差之和，以概率形式預(yù)測真實(shí)航跡的不確定性，在此基礎(chǔ) 上結(jié)合快速擴(kuò)展隨機(jī)樹方法，規(guī)劃出可行的航跡。
2. 根據(jù)權(quán)利要求1所述的基于不確定性預(yù)測的航跡規(guī)劃方法，其特征在于，包括如下 具體步驟： 步驟1 :確定運(yùn)動(dòng)體的動(dòng)力學(xué)約束和噪聲模型以及其反饋控制器 將運(yùn)動(dòng)體的動(dòng)力學(xué)約束表示成如下的線性隨機(jī)離散系統(tǒng)形式 XfAtXH+BtUH+Wt (1) zt=Ctxt+vt 其中X為狀態(tài)向量，u為控制輸入，過程噪聲wt~N(0，Qt)服從正態(tài)分布，Z為測量向 量，觀測噪聲vt~N(0,Rt)服從正態(tài)分布，At為系統(tǒng)矩陣，Bt為控制矩陣，Ct為控制矩陣， 真實(shí)航跡xt由其估計(jì)值為用穩(wěn)定控制器K反饋跟蹤某一所規(guī)劃航跡<生成，滿足
步驟2 :初始化頂點(diǎn)集合與邊集合 頂點(diǎn)集合V只包含起點(diǎn)，邊集合E為空集，即 V={xinitial},E={} 其中，V為頂點(diǎn)集合，xinitial為起點(diǎn)，E為邊集合，以用于保存搜索結(jié)果； 步驟3 :設(shè)置循環(huán)變量i的取值范圍 步驟4至步驟10對(duì)于i=l，2, 3, 時(shí)執(zhí)行，這里i為循環(huán)變量，M為最大的循環(huán)次數(shù)； 步驟4 :從無障礙的空間中任取一點(diǎn)，這里無障礙空間是指運(yùn)動(dòng)體不會(huì)與環(huán)境中的 障礙物發(fā)生碰撞的空間的集合； 步驟5 :從V中找到距離最近的點(diǎn)，記作4??.，； 步驟6 :根據(jù)r;aw/、和運(yùn)動(dòng)體動(dòng)力學(xué)方程（1)，在給定時(shí)間步長條件下找到距離Acw.e彳盡可遺近的點(diǎn)知停5以及與?間的航跡； 步驟7 :用下述公式預(yù)測航跡的不確定性協(xié)方差矩陣 t L I I
其中，W是與Qt同維度的協(xié)方差矩陣，用來描述真實(shí)航跡圍繞航跡乂^時(shí)的不確定 性，2t用來描述真實(shí)航跡與估計(jì)航跡間的偏差的不確定性，At用來描述估計(jì)航跡與航跡 的偏差的不確定性，艮為中間變量； 步驟8:在協(xié)方差矩陣< 的條件下，若滿足P(xteX>Si表明航跡可行，繼續(xù) 下一步，否則，返回步驟2 ; 步驟9 :將X。添加到V中，將〇"添加到E中； 步驟10 :判斷循環(huán)是否終止。如果胃eJ-) >戌，貝U循環(huán)終止，終點(diǎn)xU記為x6nd， 繼續(xù)下一步，這里xg°al為目標(biāo)區(qū)域，否則，返回步驟2 ; 步驟11 :回溯找出所規(guī)劃航跡 在V中從xmd開始依次回溯尋找上一個(gè)節(jié)點(diǎn)至起點(diǎn)xinitial,這條從xmd出發(fā)經(jīng)過各個(gè)節(jié) 點(diǎn)到達(dá)xinitial的航跡即為所規(guī)劃的可行航跡。
【專利摘要】本發(fā)明屬于航跡規(guī)劃領(lǐng)域，涉及復(fù)雜環(huán)境下運(yùn)動(dòng)體受運(yùn)動(dòng)不確定性影響較大時(shí)，通過預(yù)測航跡不確定性來規(guī)劃航跡的方法。本發(fā)明利用運(yùn)動(dòng)體的動(dòng)力學(xué)模型、過程噪聲模型和觀測噪聲模型，通過將真實(shí)航跡的偏差劃分為真實(shí)航跡偏離估計(jì)航跡的偏差和估計(jì)航跡偏離所規(guī)劃航跡的偏差之和，以概率形式預(yù)測真實(shí)航跡的不確定性，在此基礎(chǔ)上結(jié)合快速擴(kuò)展隨機(jī)樹方法，規(guī)劃出可行的航跡。本發(fā)明同時(shí)考慮高速運(yùn)動(dòng)體的動(dòng)力學(xué)約束和所受過程噪聲和觀測噪聲的影響，通過基于運(yùn)動(dòng)模型和誤差模型的預(yù)測，事先預(yù)測所規(guī)劃航跡的不確定性，從而保證在任意時(shí)刻運(yùn)動(dòng)體與障礙物發(fā)生碰撞的概率都小于(1-δ1)，且能夠到達(dá)目標(biāo)區(qū)域的概率大于δ2，因此在概率意義上保證所規(guī)劃的路線是可行的。
【IPC分類】G06F19-00
【公開號(hào)】CN104699935
【申請(qǐng)?zhí)枴緾N201310674923
【發(fā)明人】顧天元, 張寧, 陳小龍
【申請(qǐng)人】中國航空工業(yè)第六一八研究所
【公開日】2015年6月10日
【申請(qǐng)日】2013年12月10日