基于動(dòng)態(tài)威脅狀態(tài)估計(jì)和威脅概率評(píng)估的auv在線航路規(guī)劃方法
【技術(shù)領(lǐng)域】
[0001] 本發(fā)明涉及的是一種在線航路規(guī)劃方法，特別是一種基于狀態(tài)估計(jì)和威脅概率評(píng) 估的AUV在線航路規(guī)劃方法。
【背景技術(shù)】
[0002] 根據(jù)對(duì)AUV航行空間中環(huán)境信息的已知程度，可將航路規(guī)劃分為兩類(lèi)：一種是全 局航路規(guī)劃，此時(shí)整個(gè)規(guī)劃空間中的環(huán)境信息均為已知；另一種是局部航路規(guī)劃，此時(shí)航行 器周?chē)沫h(huán)境情況為未知或僅能獲得部分信息。在線航路規(guī)劃是一種局部規(guī)劃方法，通過(guò) 將傳感器探測(cè)到的實(shí)時(shí)環(huán)境信息和先驗(yàn)知識(shí)相結(jié)合，實(shí)時(shí)地優(yōu)化修正航路。與全局航路規(guī) 劃方法相比，在線航路規(guī)劃方法更適合于存在動(dòng)態(tài)威脅或者環(huán)境信息部分或者全部未知的 應(yīng)用場(chǎng)景。常用的在線航路規(guī)劃方法有混合整型線性規(guī)劃方法、人工勢(shì)場(chǎng)法、啟發(fā)式算法 等。如在公開(kāi)號(hào)為CN103077 425A的專(zhuān)利文件中，提出了一種基于免疫遺傳算法的AUV實(shí) 時(shí)路徑規(guī)劃方法，該專(zhuān)利利用優(yōu)化算法來(lái)求解實(shí)時(shí)航路規(guī)劃問(wèn)題，可以獲得有效的AUV全 局路徑。但是，上述方法并不適合存在動(dòng)態(tài)威脅的情況。在存在動(dòng)態(tài)威脅的航行空間中，為 了及時(shí)規(guī)避動(dòng)態(tài)威脅以保證航行安全，有必要對(duì)動(dòng)態(tài)威脅的運(yùn)動(dòng)狀態(tài)和威脅程序進(jìn)行實(shí)時(shí) 估計(jì)，并在此基礎(chǔ)上進(jìn)行在線航路規(guī)劃。

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

[0003] 本發(fā)明的目的在于提供一種能夠保證AUV有效規(guī)避動(dòng)態(tài)威脅，且具有較高的實(shí)時(shí) 性的基于動(dòng)態(tài)威脅狀態(tài)估計(jì)和威脅概率評(píng)估的AUV在線航路規(guī)劃方法。
[0004] 本發(fā)明的目的是這樣實(shí)現(xiàn)的：
[0005] 步驟一：通過(guò)狀態(tài)估計(jì)方法對(duì)動(dòng)態(tài)威脅的運(yùn)動(dòng)狀態(tài)進(jìn)行估計(jì)；
[0006] 步驟二：遍歷未來(lái)N個(gè)時(shí)刻AUV所能采取的所有控制量，通過(guò)控制方程得到其可能 位置；
[0007] 步驟三：結(jié)合動(dòng)態(tài)威脅的運(yùn)動(dòng)狀態(tài)，利用貝葉斯網(wǎng)絡(luò)對(duì)未來(lái)可能位置的威脅概率 進(jìn)tx評(píng)估；
[0008] 步驟四：綜合考慮AUV未來(lái)位置的威脅概率和與目標(biāo)點(diǎn)的距離，選擇最優(yōu)的控制 量并實(shí)施，在下一個(gè)時(shí)刻點(diǎn)重復(fù)以上步驟，直至到達(dá)目標(biāo)點(diǎn)。
[0009] 本發(fā)明針對(duì)動(dòng)態(tài)威脅機(jī)動(dòng)性強(qiáng)的特點(diǎn)，將狀態(tài)估計(jì)和威脅概率評(píng)估引入在線航路 規(guī)劃方法，提出一種基于狀態(tài)估計(jì)和威脅概率評(píng)估的AUV在線航路規(guī)劃方法。
[0010] 本發(fā)明是為了解決動(dòng)態(tài)威脅存在情況下的AUV在線航路規(guī)劃的問(wèn)題，首先對(duì)動(dòng)態(tài) 威脅的運(yùn)動(dòng)狀態(tài)進(jìn)行估計(jì)，并對(duì)其威脅進(jìn)行分析評(píng)估，在此基礎(chǔ)上進(jìn)行AUV航路規(guī)劃。本發(fā) 明優(yōu)點(diǎn)在于：
[0011] 針對(duì)水下環(huán)境中動(dòng)態(tài)威脅速度較快，機(jī)動(dòng)性較強(qiáng)的特點(diǎn)，利用狀態(tài)估計(jì)方法估計(jì) 動(dòng)態(tài)威脅的運(yùn)動(dòng)狀態(tài)，利用貝葉斯網(wǎng)絡(luò)評(píng)估航路威脅程度，并采用滾動(dòng)優(yōu)化的方法求解出 航路。該方法能夠保證AUV有效規(guī)避動(dòng)態(tài)威脅，且具有較高的實(shí)時(shí)性。
【附圖說(shuō)明】
[0012] 圖1 :在線航路規(guī)劃方法流程圖。
[0013] 圖2 :模糊離散動(dòng)態(tài)貝葉斯推理網(wǎng)絡(luò)結(jié)構(gòu)圖。
【具體實(shí)施方式】
[0014] 下面舉例對(duì)本發(fā)明做更詳細(xì)的描述。
[0015] 結(jié)合圖1，對(duì)具體的步驟說(shuō)明如下：
[0016] 步驟一：通過(guò)狀態(tài)估計(jì)方法對(duì)動(dòng)態(tài)威脅的運(yùn)動(dòng)狀態(tài)進(jìn)行估計(jì)。
[0017] 步驟1. 1利用AUV對(duì)動(dòng)態(tài)威脅的觀測(cè)量得到動(dòng)態(tài)威脅位置坐標(biāo)。
[0018] 利用AUV自身的傳感器獲得動(dòng)態(tài)威脅的運(yùn)動(dòng)狀況，具體包括動(dòng)態(tài)威脅相對(duì)于AUV 的距離R，俯仰角A和偏向角E。
[0019] 利用觀測(cè)量得到動(dòng)態(tài)威脅的絕對(duì)坐標(biāo)，結(jié)合AUV自身位置（X，y，z)，建立動(dòng)態(tài)威脅 的世界坐標(biāo)系坐標(biāo)
[0020]
(1)
[0021] 式中，R是動(dòng)態(tài)威脅相對(duì)于AUV的距離，A是動(dòng)態(tài)威脅相對(duì)于AUV的平面偏向角，E 是動(dòng)態(tài)威脅相對(duì)于AUV的垂直偏向角，U，n，O為動(dòng)態(tài)威脅的世界坐標(biāo)系坐標(biāo)。
[0022] 步驟1. 2通過(guò)無(wú)跡卡爾曼濾波方法對(duì)當(dāng)前時(shí)刻至N個(gè)采樣時(shí)間內(nèi)動(dòng)態(tài)威脅的位置 fg息進(jìn)行估計(jì)。
[0023] 使用無(wú)跡卡爾曼濾波方法，對(duì)未來(lái)時(shí)刻的動(dòng)態(tài)威脅位置進(jìn)行估計(jì)。
[0024] 無(wú)跡卡爾曼濾波的狀態(tài)方程為：
[0025]
[0026] 觀測(cè)方程
[0027]yk=HXk+Vk (3)
[0028] 更新無(wú)跡卡爾曼濾波器的參數(shù)。
[0029] 計(jì)算Sigma點(diǎn)對(duì)應(yīng)的觀測(cè)值：
[0030] _
[0031] 加權(quán)計(jì)算觀測(cè)值的估計(jì)值：
[0032]
(，）
[0033] 計(jì)算觀測(cè)估計(jì)值的協(xié)方差：
[0034]
[0035] 計(jì)算觀測(cè)估計(jì)值與預(yù)測(cè)估計(jì)值的聯(lián)合分布的協(xié)方差：
[0036]
[0037] 計(jì)算卡爾曼增益：
[0038]
(8)
[0039] 計(jì)算基于實(shí)際觀測(cè)值的協(xié)方差：
[0040]
(9)
[0041] 獲得基于實(shí)際觀測(cè)值的狀態(tài)值：
[0042]
(10)
[0043] 最后，預(yù)測(cè)未來(lái)時(shí)刻動(dòng)態(tài)威脅的位置：
[0044]計(jì)算Sigma點(diǎn)：
[0045]
[0046] 預(yù)測(cè)：首先計(jì)算每個(gè)Sigma點(diǎn)的函數(shù)值，然后利用Sigma點(diǎn)的函數(shù)值進(jìn)行加權(quán)，得 到函數(shù)預(yù)測(cè)值，最后計(jì)算估計(jì)協(xié)方差。
[0047]
[0048]
[0049]
[0050] 在未獲取觀測(cè)信息時(shí)，可以將估計(jì)值作為輸入，通過(guò)以上方程，得到k+n時(shí)刻的動(dòng) 態(tài)威脅位置。
[0051] 步驟二：遍歷未來(lái)N個(gè)時(shí)刻AUV所能采取的所有控制量，通過(guò)控制方程得到AUV的 所有可能位置。
[0052] 視在單個(gè)時(shí)間片內(nèi)的AUV的運(yùn)動(dòng)為勻速直線運(yùn)動(dòng)，建立AUV的運(yùn)動(dòng)模型如下：
[0053]
[0054] 當(dāng)存在海流時(shí)，其運(yùn)動(dòng)模型如下：
[0055]
[0056] 遍歷未來(lái)N個(gè)時(shí)刻AUV所能采取的控制量ue、\和^的所有值，通過(guò)控制方程確 定其位置。
[0057] 由于AUV機(jī)械性能的約束，其控制量約束如下：
[0058]
[0059] 步驟三：結(jié)合動(dòng)態(tài)威脅的運(yùn)動(dòng)狀態(tài)，利用貝葉斯網(wǎng)絡(luò)對(duì)未來(lái)可能位置的威脅概率 進(jìn)行評(píng)估。
[0060] 步驟3. 1構(gòu)建用于威脅概率評(píng)估的模糊離散動(dòng)態(tài)貝葉斯推理網(wǎng)絡(luò)。
[0061] 建立模糊離散動(dòng)態(tài)貝葉斯推理網(wǎng)絡(luò)如附圖2所示。當(dāng)前時(shí)刻的威脅概率由前一時(shí) 刻的威脅概率、動(dòng)態(tài)威脅與AUV的相對(duì)距離、動(dòng)態(tài)威脅與AUV的相對(duì)距離變化均有關(guān)。
[0062] 步驟3. 2選擇用于對(duì)觀測(cè)量進(jìn)行模糊化的模糊函數(shù)以及用于威脅概率評(píng)估的模 糊離散動(dòng)態(tài)貝葉斯推理網(wǎng)絡(luò)的參數(shù)。
[0063] (1)相對(duì)距離采取采用三角型隸屬函數(shù)，其計(jì)算方法如下： U
1UU
[0065] (2)距離變化采取高斯型隸屬函數(shù)，其計(jì)算方法如下：
[0066]
[0067] (3)選擇貝葉斯網(wǎng)絡(luò)推理參數(shù)，即條件概率表和時(shí)間片間轉(zhuǎn)移概率表如下：
[0068] 表1威脅距離與威脅等級(jí)的條件概率表
[0069]
[0070] 表2運(yùn)動(dòng)趨勢(shì)與威脅等級(jí)的條件概率表 ' '
[0071]
[0072] 表3時(shí)間片間轉(zhuǎn)^概率表~'
' ' '
[0073]
[0074] 步驟3. 3將動(dòng)態(tài)威脅的觀測(cè)數(shù)據(jù)、估計(jì)數(shù)據(jù)和AUV的未來(lái)可能位置模糊化，帶入模 糊離散動(dòng)態(tài)貝葉斯網(wǎng)絡(luò)，獲得該位置的威脅概率。
[0075] 利用動(dòng)態(tài)威脅的觀測(cè)數(shù)據(jù)和估計(jì)數(shù)據(jù)，以及AUV的未來(lái)可能位置，計(jì)算AUV與動(dòng)態(tài) 威脅的相對(duì)距離及其相鄰時(shí)刻間的距離變化。
[0076] 利用式（18)和（19)，計(jì)算AUV與動(dòng)態(tài)威脅的相對(duì)距離和其變化量在各個(gè)模糊集合 中的隸屬度，將其帶入模糊離散動(dòng)態(tài)貝葉斯網(wǎng)絡(luò)進(jìn)行概率推理，推理方程如下：
[0077]
[0078] 這樣，即可得到對(duì)于該位置的威脅概率。
[0079] 步驟四：利用AUV未來(lái)位置的威脅概率和與目標(biāo)點(diǎn)的距離，選擇最優(yōu)的控制量并 實(shí)施，在下一個(gè)時(shí)刻點(diǎn)重復(fù)以上步驟，直至到達(dá)目標(biāo)點(diǎn)。
[0080] 步驟4. 1利用威脅概率與AUV和目標(biāo)點(diǎn)的距離，構(gòu)建代價(jià)函數(shù)。
[0081] 某一未來(lái)航路點(diǎn)的代價(jià)函數(shù)即為，如果AUV下一刻運(yùn)動(dòng)到該點(diǎn)，那么下一時(shí)刻動(dòng) 態(tài)威脅對(duì)于AUV的威脅程度和該點(diǎn)與目標(biāo)點(diǎn)的距離的加權(quán)和?？梢杂霉奖硎緸椋?br>[0082] J=CpP(威脅=高)+Des (5-3)
[0083] 式中，為若AUV下一刻運(yùn)動(dòng)到該點(diǎn)，動(dòng)態(tài)威脅對(duì)其威脅代價(jià)為高的概率， 即步驟四得到的結(jié)果。Cp為其加權(quán)系數(shù)。Des為該點(diǎn)到目標(biāo)點(diǎn)的距離。
[0084] 其中，Cp可以選取Des的倍數(shù)，即Cp=cXDes，c一般取0.5~1.5之間，其值越 大，AUV對(duì)威脅躲避的趨勢(shì)越強(qiáng)。
[0085] 步驟4. 2將所有AUV的未來(lái)可能位置和威脅概率帶入目標(biāo)函數(shù)，求取其代價(jià)值。
[0086] 步驟4. 3選取代價(jià)值最小的可能位置對(duì)應(yīng)的控制量并執(zhí)行，并在下一個(gè)時(shí)刻點(diǎn)重 復(fù)以上步驟，直至到達(dá)目標(biāo)點(diǎn)。
[0087] 通過(guò)以上步驟，即可在線規(guī)劃處AUV的安全航路。
【主權(quán)項(xiàng)】
1. 一種基于動(dòng)態(tài)威脅狀態(tài)估計(jì)和威脅概率評(píng)估的AUV在線航路規(guī)劃方法，其特征是： 步驟一：通過(guò)狀態(tài)估計(jì)方法對(duì)動(dòng)態(tài)威脅的運(yùn)動(dòng)狀態(tài)進(jìn)行估計(jì)； 步驟二：遍歷未來(lái)N個(gè)時(shí)刻AUV所能采取的所有控制量，通過(guò)控制方程得到其可能位 置； 步驟=:結(jié)合動(dòng)態(tài)威脅的運(yùn)動(dòng)狀態(tài)，利用貝葉斯網(wǎng)絡(luò)對(duì)未來(lái)可能位置的威脅概率進(jìn)行 評(píng)估； 步驟四：綜合考慮AUV未來(lái)位置的威脅概率和與目標(biāo)點(diǎn)的距離，選擇最優(yōu)的控制量并 實(shí)施，在下一個(gè)時(shí)刻點(diǎn)重復(fù)W上步驟，直至到達(dá)目標(biāo)點(diǎn)。2. 根據(jù)權(quán)利要求1所述的基于動(dòng)態(tài)威脅狀態(tài)估計(jì)和威脅概率評(píng)估的AUV在線航路規(guī)劃 方法，其特征是所述的通過(guò)狀態(tài)估計(jì)方法對(duì)動(dòng)態(tài)威脅的運(yùn)動(dòng)狀態(tài)進(jìn)行估計(jì)具體包括： 步驟1. 1利用AUV對(duì)動(dòng)態(tài)威脅的觀測(cè)量得到動(dòng)態(tài)威脅的位置坐標(biāo)； 步驟1. 2通過(guò)無(wú)跡卡爾曼濾波方法對(duì)之后一到N個(gè)采樣時(shí)間的動(dòng)態(tài)威脅位置進(jìn)行估 計(jì)。3. 根據(jù)權(quán)利要求2所述的基于動(dòng)態(tài)威脅狀態(tài)估計(jì)和威脅概率評(píng)估的AUV在線航路規(guī)劃 方法，其特征是步驟1. 1具體包括： 利用AUV自身的傳感器獲得動(dòng)態(tài)威脅的運(yùn)動(dòng)狀況，包括動(dòng)態(tài)威脅相對(duì)于AUV的距離R， 俯仰角A和偏向角E， 利用觀測(cè)量得到動(dòng)態(tài)威脅的絕對(duì)坐標(biāo)，結(jié)合AUV自身位置（x，y，z)，建立動(dòng)態(tài)威脅的世 界坐標(biāo)系坐標(biāo) :當(dāng).=巧COS￡'COS/! +.-、' < 巧=歷sin￡' + 'v' (!) :( = /?cos￡'sin/l+z 式中，R是動(dòng)態(tài)威脅相對(duì)于AUV的距離，A是動(dòng)態(tài)威脅相對(duì)于AUV的平面偏向角，E是動(dòng) 態(tài)威脅相對(duì)于AUV的垂直偏向角，（I，n，C)為動(dòng)態(tài)威脅的世界坐標(biāo)系坐標(biāo)。4. 根據(jù)權(quán)利要求1、2或3所述的基于動(dòng)態(tài)威脅狀態(tài)估計(jì)和威脅概率評(píng)估的AUV在線 航路規(guī)劃方法，其特征是所述利用貝葉斯網(wǎng)絡(luò)對(duì)未來(lái)可能位置的威脅概率進(jìn)行評(píng)估具體包 括： 步驟3. 1構(gòu)建用于威脅概率評(píng)估的模糊離散動(dòng)態(tài)貝葉斯推理網(wǎng)絡(luò)； 步驟3. 2選擇用于威脅概率評(píng)估的模糊離散動(dòng)態(tài)貝葉斯推理網(wǎng)絡(luò)的參數(shù)； 步驟3. 3將動(dòng)態(tài)威脅的觀測(cè)數(shù)據(jù)、估計(jì)數(shù)據(jù)和AUV的未來(lái)可能位置帶入模糊離散動(dòng)態(tài) 貝葉斯網(wǎng)絡(luò)，評(píng)估該位置的威脅概率。
【專(zhuān)利摘要】本發(fā)明提供的是一種基于動(dòng)態(tài)威脅狀態(tài)估計(jì)和威脅概率評(píng)估的AUV在線航路規(guī)劃方法。步驟一：通過(guò)狀態(tài)估計(jì)方法對(duì)動(dòng)態(tài)威脅的運(yùn)動(dòng)狀態(tài)進(jìn)行估計(jì)；步驟二：遍歷未來(lái)N個(gè)時(shí)刻AUV所能采取的所有控制量，通過(guò)控制方程得到其可能位置；步驟三：結(jié)合動(dòng)態(tài)威脅的運(yùn)動(dòng)狀態(tài)，利用貝葉斯網(wǎng)絡(luò)對(duì)未來(lái)可能位置的威脅概率進(jìn)行評(píng)估；步驟四：綜合考慮AUV未來(lái)位置的威脅概率和與目標(biāo)點(diǎn)的距離，選擇最優(yōu)的控制量并實(shí)施，在下一個(gè)時(shí)刻點(diǎn)重復(fù)以上步驟，直至到達(dá)目標(biāo)點(diǎn)。本發(fā)明能夠保證AUV有效規(guī)避動(dòng)態(tài)威脅，且具有較高的實(shí)時(shí)性。
【IPC分類(lèi)】G01C21/20
【公開(kāi)號(hào)】CN105203108
【申請(qǐng)?zhí)枴緾N201510676842
【發(fā)明人】劉廠, 趙俊翔, 趙玉新, 高峰, 劉利強(qiáng)
【申請(qǐng)人】哈爾濱工程大學(xué)
【公開(kāi)日】2015年12月30日
【申請(qǐng)日】2015年10月15日