本發(fā)明屬于水下無人航行器(unmannedunderwatervehicle，uuv)自主控制領(lǐng)域，尤其涉及針對(duì)復(fù)雜作業(yè)環(huán)境uuv進(jìn)行自主航行的，一種uuv抵近海底作業(yè)過程中航路生成方法。

背景技術(shù)：

在水下無人航行器抵近海底作業(yè)過程中，由于距離遠(yuǎn)、環(huán)境復(fù)雜，一般要求uuv具有高續(xù)航能力與自主能力，當(dāng)uuv執(zhí)行抵近海底作業(yè)任務(wù)時(shí)，uuv難以隨時(shí)返回水面或者母船附近進(jìn)行任務(wù)下載和信息反饋，一般需要uuv在完成一系列任務(wù)后再返回。uuv需要先航行到作業(yè)區(qū)，根據(jù)接收到的任務(wù)目標(biāo)點(diǎn)進(jìn)行航路生成，uuv沿著生成的航路避開障礙物到達(dá)目標(biāo)點(diǎn)。因此對(duì)uuv抵近海底作業(yè)的安全性要求非常高。

由于傳感器本身就存在測(cè)量誤差，導(dǎo)致已知障礙物的位置其實(shí)就存在原始誤差；當(dāng)uuv進(jìn)行抵近海底作業(yè)時(shí)，如果在生成uuv航路過程中對(duì)障礙物的原始位置誤差不予考慮，那么實(shí)際障礙物會(huì)嚴(yán)重影響uuv的航行安全。同樣由于uuv深海作業(yè)的特殊性，uuv不能及時(shí)進(jìn)行導(dǎo)航校準(zhǔn)，因此uuv的導(dǎo)航誤差也會(huì)影響uuv的航行。

現(xiàn)有技術(shù)中，申請(qǐng)?zhí)?01210102688.5的專利《一種水下航行器的避障方法》和申請(qǐng)?zhí)?01110086941.8的專利《基于迷你聲吶的水下目標(biāo)探測(cè)與auv自動(dòng)避碰方法及系統(tǒng)》。雖然兩個(gè)專利都涉及uuv對(duì)障礙物的避碰處理，但并沒有涉及傳感器測(cè)量障礙物導(dǎo)致障礙物位置誤差對(duì)uuv航行安全的影響。以及申請(qǐng)?zhí)?01410264172.x的專利《基于多項(xiàng)式的uuv空間軌跡規(guī)劃方法》也沒有涉及障礙物位置誤差及uuv導(dǎo)航誤差對(duì)規(guī)劃軌跡的影響。因此在實(shí)際障礙物的不確定性以及uuv導(dǎo)航誤差影響下，如何生成高精度的航路，提高uuv深海作業(yè)安全性與魯棒性，成為我們亟待解決的問題。

技術(shù)實(shí)現(xiàn)要素：

本發(fā)明的目的是提供一種能夠生成最優(yōu)航路，進(jìn)一步提高uuv遠(yuǎn)距離作業(yè)安全性與可靠性的，uuv抵近海底作業(yè)過程中航路生成方法。

一種uuv抵近海底作業(yè)過程中航路生成方法，包括以下幾個(gè)步驟，

步驟一：uuv利用傳感器采集當(dāng)前自身位姿信息，包括：uuv的初始位置(xs,ys)，航行速度v0，航行時(shí)間t0；uuv接收任務(wù)目標(biāo)點(diǎn)(xg,yg)，uuv接收障礙物信息，其中uuv接收到的第k個(gè)障礙物ok為以cok為圓心，以rok為半徑的圓，障礙物圓心坐標(biāo)為0≤k≤m；m為障礙物數(shù)量；

步驟二：構(gòu)建柵格地圖，柵格iij的坐標(biāo)為(xi,yj)；根據(jù)uuv的幾何約束對(duì)障礙物進(jìn)行第一次膨脹處理，膨脹寬度d1＝l，l為uuv體長(zhǎng)，第一次膨脹后障礙物o′k的圓心不變，半徑為r′ok＝rok+d1，將第一次膨脹后的障礙物表示在相應(yīng)的柵格地圖中，有障礙物的柵格障礙屬性記為1，沒有障礙物的柵格障礙屬性記為0；

步驟三：根據(jù)柵格障礙屬性將柵格地圖分為可行區(qū)和不可行區(qū)，如果柵格障礙屬性為0，該柵格屬于可行區(qū)s1，否則該柵格屬于不可行區(qū)s2；

步驟四：根據(jù)障礙物原始位置誤差和uuv導(dǎo)航誤差計(jì)算可行區(qū)s1中柵格的潛在危險(xiǎn)性

(1)對(duì)障礙物進(jìn)行第二次膨脹處理，在第一次膨脹后的障礙物基礎(chǔ)上，將障礙物的半徑向外擴(kuò)展寬度d2，第二次膨脹后障礙物o″k的半徑r″ok＝r′ok+d2，圓心不變，第二次膨脹區(qū)域?yàn)檎系K物的潛在危險(xiǎn)區(qū)，潛在危險(xiǎn)區(qū)屬于柵格地圖中的可行區(qū)，

潛在危險(xiǎn)區(qū)的寬度d2＝αdobs+βdnav，其中dobs為由障礙物位置誤差引起危險(xiǎn)寬度，dnav為由uuv導(dǎo)航誤差引起的危險(xiǎn)寬度，α和β為系數(shù)，α+β＝1，0≤α≤1，0≤β≤1；

步驟五：根據(jù)柵格的潛在危險(xiǎn)性利用逆向a^*算法生成航路。

本發(fā)明一種uuv深海海底作業(yè)過程中航路生成方法，還可以包括：

1、由障礙物位置誤差引起危險(xiǎn)寬度dobs取值為2～20米。

2、由uuv導(dǎo)航誤差引起的危險(xiǎn)寬度dnav：

其中，b為系數(shù)。

3、所述的利用逆向a^*算法生成航路的過程中：

以u(píng)uv的初始位置(xs,ys)作為航路起始點(diǎn)s，以u(píng)uv接收的任務(wù)目標(biāo)(xg,yg)點(diǎn)作為航路目標(biāo)點(diǎn)g；

將一個(gè)柵格對(duì)應(yīng)一個(gè)節(jié)點(diǎn)，節(jié)點(diǎn)之間的邊表示柵格的潛在危險(xiǎn)性，用節(jié)點(diǎn)n表示柵格iij，節(jié)點(diǎn)n的潛在危險(xiǎn)性

待擴(kuò)展節(jié)點(diǎn)n的最佳性評(píng)價(jià)函數(shù)f(n)為：

f(n)＝g(n)+h(n)

式中，f(n)表示從節(jié)點(diǎn)g開始約束通過節(jié)點(diǎn)n的一條最優(yōu)路徑的代價(jià)；g(n)表示從節(jié)點(diǎn)g到節(jié)點(diǎn)n的最優(yōu)路徑代價(jià)，這個(gè)代價(jià)等于到目前為止已經(jīng)產(chǎn)生的最優(yōu)路徑的代價(jià)g(p)加上從當(dāng)前節(jié)點(diǎn)p到節(jié)點(diǎn)n的邊的代價(jià)g′(p,n)，具體為：g(n)＝g(p)+g′(p,n)；h(n)表示從節(jié)點(diǎn)n到節(jié)點(diǎn)s的最小估計(jì)代價(jià)，從節(jié)點(diǎn)n到節(jié)點(diǎn)s的直線歐氏距離作為h(n)的最小估計(jì)代價(jià)；

柵格地圖具有8個(gè)方向的連通性，從節(jié)點(diǎn)p到節(jié)點(diǎn)n的邊的代價(jià)函數(shù)g′(p,n)為：

本發(fā)明具有如下有益效果：

1.本發(fā)明在uuv航路生成過程中考慮到障礙物原始位置誤差以及uuv導(dǎo)航誤差對(duì)uuv安全性的影響，將障礙物原始位置誤差視為障礙物自身不確定性，將uuv導(dǎo)航誤差轉(zhuǎn)化為障礙物相對(duì)位置誤差，因此根據(jù)障礙物原始位置誤差和uuv導(dǎo)航誤差求取障礙物的潛在危險(xiǎn)區(qū)，并求取潛在危險(xiǎn)區(qū)對(duì)uuv威脅的概率。因此本發(fā)明減小了由于傳感器器件自身測(cè)量誤差導(dǎo)致的障礙物原始位置誤差，并且補(bǔ)償了uuv導(dǎo)航誤差對(duì)uuv航路生成的影響，進(jìn)一步得到最優(yōu)航路，提高uuv海底作業(yè)安全性與可靠性。

2.本發(fā)明將障礙物原始位置誤差和uuv導(dǎo)航誤差均轉(zhuǎn)化為障礙物的潛在危險(xiǎn)區(qū)進(jìn)行處理，減少了復(fù)雜度，簡(jiǎn)單方便，效率高。

附圖說明

圖1本發(fā)明航路生成流程圖。

圖2圓形障礙物的模型圖。

圖3對(duì)障礙物進(jìn)行第一次膨脹處理示意圖。

圖4柵格地圖示意圖。

圖5對(duì)障礙物進(jìn)行第二次膨脹處理示意圖。

具體實(shí)施方式

下面將結(jié)合附圖對(duì)本發(fā)明做進(jìn)一步詳細(xì)說明。

如圖1所示，一種uuv抵近海底作業(yè)過程中航路生成方法，包括以下幾個(gè)步驟：

步驟一：uuv利用傳感器采集當(dāng)前自身信息，包括：uuv的初始位置(xs,ys)；uuv接收任務(wù)目標(biāo)點(diǎn)(xg,yg)，uuv接收障礙物信息；m為障礙物數(shù)量；

步驟二：構(gòu)建柵格地圖，首先對(duì)uuv作業(yè)區(qū)域的二維環(huán)境空間進(jìn)行柵格離散化，采用直角坐標(biāo)標(biāo)識(shí)柵格，以柵格陣左下角為坐標(biāo)原點(diǎn)。任一柵格iij均可用唯一的直角坐標(biāo)標(biāo)識(shí)(xi,yj)其位置，0≤i≤n1，0≤j≤n2，n1為柵格陣在x軸方向的柵格個(gè)數(shù)，n2為柵格陣在y軸方向柵格個(gè)數(shù)。柵格間距δl一般為10l～20l之間，其中l(wèi)為uuv的體長(zhǎng)，一般可考慮uuv的作業(yè)區(qū)域范圍為100km×100km。

任一個(gè)柵格iij通過一個(gè)四元組表示(xi,yj)為柵格的位置坐標(biāo)，為柵格的障礙屬性，為柵格的潛在危險(xiǎn)性。

根據(jù)uuv的幾何約束對(duì)障礙物進(jìn)行第一次膨脹處理。如圖2所示，一般障礙物為不規(guī)則形狀，以一個(gè)半徑為rok的外切圓去包絡(luò)該障礙物，從而將不規(guī)則障礙物擴(kuò)充為規(guī)則的凸障礙。因此，uuv接收到的第k個(gè)障礙物ok為以cok為圓心，以rok為半徑的圓，障礙物圓心坐標(biāo)為0≤k≤m?？紤]到uuv的幾何約束，對(duì)障礙物進(jìn)行第一次膨脹處理，膨脹寬度d1＝l，即將包絡(luò)障礙物的圓的半徑延長(zhǎng)d1，第一次膨脹后障礙物o′k的半徑r′ok＝rok+d1，圓心不變，如圖3所示。

將第一次膨脹后的障礙物表示在相應(yīng)的柵格地圖中，有障礙物的柵格障礙屬性記為1，沒有障礙物的柵格障礙屬性記為0。

步驟三：根據(jù)每個(gè)柵格障礙屬性將柵格地圖分為可行區(qū)和不可行區(qū)，如果柵格障礙屬性為0，該柵格屬于可行區(qū)s1，否則該柵格屬于不可行區(qū)s2，如圖4所示。

下面僅對(duì)可行區(qū)進(jìn)行分析處理，大大減少了工作量。

步驟四：根據(jù)障礙物原始位置誤差和uuv導(dǎo)航誤差計(jì)算可行區(qū)s1中每一柵格iij的潛在危險(xiǎn)性具體過程為：

(1)對(duì)障礙物進(jìn)行第二次膨脹處理，將第一次膨脹后的障礙物模型向外擴(kuò)展寬度d2，得到障礙物的潛在危險(xiǎn)區(qū)，潛在危險(xiǎn)區(qū)屬于柵格地圖中的可行區(qū)；

由于傳感器器件自身存在測(cè)量誤差，導(dǎo)致傳送給uuv的已知障礙物本身存在位置誤差，此時(shí)為障礙物自身的不確定性。也就是說uuv獲得的障礙物位置信息和實(shí)際障礙物位置之間存在位置誤差，即障礙物的原始位置誤差。

由于在航行時(shí)，uuv不能及時(shí)進(jìn)行導(dǎo)航校準(zhǔn)，因此uuv的導(dǎo)航誤差也會(huì)影響uuv的航行，為了提高可靠性，將uuv的導(dǎo)航誤差轉(zhuǎn)化為障礙物的相對(duì)誤差進(jìn)行處理，減少導(dǎo)航誤差對(duì)uuv航行的影響。此處為由uuv導(dǎo)航誤差轉(zhuǎn)化為障礙物不確定性。

障礙物不確定性，也就是障礙物中心位置的不確定性，表現(xiàn)為uuv在航行過程中可能與障礙物發(fā)生碰撞，對(duì)障礙物進(jìn)行第二次膨脹處理，得到障礙物的潛在危險(xiǎn)區(qū)，潛在危險(xiǎn)區(qū)屬于柵格地圖中的可行區(qū)。潛在危險(xiǎn)區(qū)的引入是為了表示由于障礙物不確定性對(duì)航路生成的影響。

因此潛在危險(xiǎn)區(qū)的寬度由兩部分因素決定，一個(gè)是障礙物原始位置誤差，另一個(gè)是uuv的導(dǎo)航誤差。

潛在危險(xiǎn)區(qū)的寬度d2＝αdobs+βdnav，其中dobs為由障礙物原始位置誤差產(chǎn)生危險(xiǎn)寬度，dnav為由uuv導(dǎo)航誤差轉(zhuǎn)化為障礙物相對(duì)位置誤差產(chǎn)生的危險(xiǎn)寬度，α和β為系數(shù)，α+β＝1，0≤α≤1，0≤β≤1；第二次膨脹后障礙物o″k的半徑r″ok＝r′ok+d2，圓心不變，如圖5所示，其中α＝0.37，β＝0.63。

障礙物原始位置誤差產(chǎn)生的危險(xiǎn)寬度dobs的取值一般為2～20米。

將uuv的導(dǎo)航誤差轉(zhuǎn)化為障礙物相對(duì)誤差進(jìn)行處理，uuv的導(dǎo)航誤差轉(zhuǎn)化為障礙物相對(duì)誤差的危險(xiǎn)寬度dnav:

其中，b為系數(shù)，一般取值為0.4％～0.6％，最優(yōu)為0.53％。

(2)計(jì)算可行區(qū)s1中每一柵格(xi,yj)的潛在危險(xiǎn)性

障礙物不確定性，表現(xiàn)為障礙物ok的圓心將不再固定在一個(gè)點(diǎn)上，而是以一定的概率密度分布在以η＝e(cωok)為圓心，以rω為半徑的一個(gè)圓域ω內(nèi)，用集合表示為：

其中(xη,yη)為圓域ω圓心坐標(biāo)。

定義p(r)為cωok分布在以η＝e(cωok)為圓心，以r為半徑的圓dr內(nèi)的概率，則有：

其中，ρ(x,y)為cωok分布在圓域ω內(nèi)點(diǎn)(x,y)處的概率密度，ρ(x,y)≥0。障礙物中心cωok在圓域ω內(nèi)服從均勻分布時(shí)，ρ(x,y)為：

柵格地圖不可行區(qū)s2包括m個(gè)獨(dú)立的禁入?yún)^(qū)fbdk，0≤k≤m。一個(gè)禁入?yún)^(qū)對(duì)應(yīng)一個(gè)第一次膨脹后的障礙物o′k，每個(gè)禁入?yún)^(qū)邊界柵格集合為可行區(qū)s1中任一柵格的潛在危險(xiǎn)性為：

其中，(xe，ye)為任一邊界柵格的坐標(biāo)，為可行區(qū)中一個(gè)柵格，其坐標(biāo)為(xi，yj)。

因此，障礙物的潛在危險(xiǎn)區(qū)可以看作是第二次膨脹后障礙物o″k的圓心cok以一定的概率密度分布在以cok為圓心，以d2為半徑的一個(gè)圓域ω內(nèi)，進(jìn)而產(chǎn)生的障礙物的潛在危險(xiǎn)區(qū)。為柵格受禁入?yún)^(qū)fbdk威脅的概率。進(jìn)一步得到，可行區(qū)s1中任一柵格的潛在危險(xiǎn)性為：

步驟五：將每一個(gè)柵格對(duì)應(yīng)一個(gè)節(jié)點(diǎn)，節(jié)點(diǎn)之間的邊表示柵格的潛在危險(xiǎn)性，如果用節(jié)點(diǎn)n表示柵格iij，節(jié)點(diǎn)n的潛在危險(xiǎn)性

利用逆向a^*算法生成航路。

令uuv的初始位置(xs,ys)作為柵格地圖中的起始節(jié)點(diǎn)s，令uuv接收的任務(wù)目標(biāo)(xg,yg)點(diǎn)作為柵格地圖中的目標(biāo)節(jié)點(diǎn)g。

逆向a^*算法是最佳優(yōu)先的啟發(fā)式搜索方法，它從目標(biāo)節(jié)點(diǎn)g出發(fā)，通過每次向路徑中加入“最佳節(jié)點(diǎn)”來逐步擴(kuò)展路徑，最終產(chǎn)生一條從目標(biāo)節(jié)點(diǎn)g到起始節(jié)點(diǎn)s的最優(yōu)路徑。逆向a^*算法的核心就是設(shè)計(jì)一個(gè)能表示待擴(kuò)展節(jié)點(diǎn)n是否最佳的評(píng)價(jià)函數(shù)f(n)：

f(n)＝g(n)+h(n)

f(n)：表示從節(jié)點(diǎn)g開始約束通過節(jié)點(diǎn)n的一條最優(yōu)路徑的代價(jià)。

g(n)：表示從節(jié)點(diǎn)g到節(jié)點(diǎn)n的最優(yōu)路徑代價(jià)，由于逆向a^*算法是從目標(biāo)節(jié)點(diǎn)g開始向外擴(kuò)展的，所以這個(gè)代價(jià)就等于到目前為止已經(jīng)產(chǎn)生的最優(yōu)路徑的代價(jià)g(p)加上從當(dāng)前節(jié)點(diǎn)p到節(jié)點(diǎn)n的邊的代價(jià)g′(p,n)，具體為：

g(n)＝g(p)+g′(p,n)。

h(n)：表示從節(jié)點(diǎn)n到節(jié)點(diǎn)s的最小估計(jì)代價(jià)。由于h(n)是一個(gè)估計(jì)值，所以h(n)被稱為啟發(fā)函數(shù)。h(n)被設(shè)計(jì)為從節(jié)點(diǎn)n到節(jié)點(diǎn)s的直線歐氏距離作為h(n)的最小估計(jì)代價(jià)，這個(gè)直線距離小于實(shí)際要經(jīng)過的路徑長(zhǎng)度。

柵格地圖具有8個(gè)方向的連通性，定義從節(jié)點(diǎn)p到節(jié)點(diǎn)n的邊的代價(jià)函數(shù)g′(p,n)為：

基于逆向a^*算法的路徑搜索算法為：

步驟1.將目標(biāo)節(jié)點(diǎn)g放入未擴(kuò)展節(jié)點(diǎn)表open中，記f(n)＝h(n)，令擴(kuò)展節(jié)點(diǎn)表closed為空。

步驟2.若open表為空，則失敗退出，無解。

步驟3.從open表中選取具有最小f(n)值的節(jié)點(diǎn)n進(jìn)行擴(kuò)展。

步驟4.將節(jié)點(diǎn)n從open表中移出，并把它放入closed表中。

步驟5.若節(jié)點(diǎn)n為起始節(jié)點(diǎn)s，則成功退出，并給出s到g的路徑。

步驟6.若節(jié)點(diǎn)n不是起始節(jié)點(diǎn)s，則擴(kuò)展，并生成其后繼節(jié)點(diǎn)集msn＝{mi}。

步驟7.對(duì)每個(gè)后繼結(jié)點(diǎn)mi進(jìn)行如下操作：

(1)計(jì)算g(mi)＝g(n)+g′(n,mi)。

(2)若mi在open表中和closed表中出現(xiàn)過，則把它放入open表中，并添加到節(jié)點(diǎn)n的后裔表中。

(3)若mi在open表中或closed表中有重復(fù)節(jié)點(diǎn)k，并且g(mi)＜g(k)，則

a、令g(k)＝g(mi)，并修改節(jié)點(diǎn)k的父輩節(jié)點(diǎn)指針，使其指向n；

b、如果節(jié)點(diǎn)mi是在closed表中，還要根據(jù)后裔表修改k的所有在open表和closed表中的后節(jié)點(diǎn)g值和f值。

c、將k添加到節(jié)點(diǎn)n后裔表中。

步驟8.根據(jù)更行的f(n)值，重排open表的次序。

步驟9.轉(zhuǎn)至步驟2。