本發(fā)明屬于無(wú)人船導(dǎo)航及自動(dòng)駕駛技術(shù)技術(shù)領(lǐng)域，特別涉及一種無(wú)人船導(dǎo)航和自動(dòng)駕駛設(shè)備及其方法。

背景技術(shù)：

現(xiàn)在市場(chǎng)上用于測(cè)繪、水文探測(cè)、水質(zhì)監(jiān)測(cè)、水環(huán)境監(jiān)測(cè)、海事搜救、安防、抗洪搶險(xiǎn)、水面交通疏導(dǎo)、近遠(yuǎn)程目標(biāo)識(shí)別與跟蹤等領(lǐng)域的無(wú)人船層出不窮，但是這一類(lèi)應(yīng)用中對(duì)船只航行速度、定位精度、航跡與預(yù)設(shè)軌跡偏差和船只航行姿態(tài)要求相對(duì)較低，特別是在水面環(huán)境相對(duì)復(fù)雜的情況下，對(duì)無(wú)人船的靈活性和機(jī)動(dòng)性要求比較低。當(dāng)無(wú)人船面對(duì)多艘交通船時(shí)，考慮到交通船人為操作的不確定性，避免無(wú)人船與交通船發(fā)生碰撞事故，以及通過(guò)分析不同運(yùn)動(dòng)狀態(tài)的交通船從而準(zhǔn)確靈活地做出不同的避碰行為規(guī)劃有待進(jìn)一步去探索和研究。無(wú)人船技術(shù)應(yīng)用領(lǐng)域比較廣泛，但是不管其具體應(yīng)用于哪一個(gè)領(lǐng)域，都要求無(wú)人船在面對(duì)復(fù)雜的水面環(huán)境時(shí)必須做出及時(shí)有效地行為選擇和航行規(guī)劃，并根據(jù)規(guī)劃信息控制無(wú)人船避開(kāi)固定的和移動(dòng)的障礙物，最后到達(dá)目的地完成各自任務(wù)。

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

為了解決現(xiàn)有技術(shù)中的問(wèn)題，本發(fā)明提供一種無(wú)人船導(dǎo)航和自動(dòng)駕駛設(shè)備及其方法，能夠?qū)o(wú)人船進(jìn)行更加精確定位的同時(shí)，實(shí)施有效地針對(duì)移動(dòng)障礙物的避碰規(guī)則以及面對(duì)不同狀態(tài)的移動(dòng)障礙物做出最佳的行為規(guī)劃。在水面環(huán)境復(fù)雜的情況下，滿(mǎn)足高精度的定位與導(dǎo)航，更精確的、靈活的船體控制和復(fù)雜情況下的運(yùn)動(dòng)規(guī)劃，以及更準(zhǔn)確的姿態(tài)測(cè)量。

為實(shí)現(xiàn)上述目的，本發(fā)明采用的技術(shù)方案為：

一種無(wú)人船導(dǎo)航和自動(dòng)駕駛設(shè)備，包括：安裝于無(wú)人船頂部的雷達(dá)、慣性導(dǎo)航系統(tǒng)、無(wú)線通訊系統(tǒng)和存儲(chǔ)設(shè)備，安裝于無(wú)人船頂部前端的攝像機(jī)，安裝于無(wú)人船駕駛艙內(nèi)的決策系統(tǒng)，決策系統(tǒng)包括規(guī)則選擇器、航行規(guī)劃器和無(wú)人船控制器；無(wú)人船通過(guò)攝像機(jī)采集周邊環(huán)境信息，將信息發(fā)送至存儲(chǔ)設(shè)備，無(wú)線通訊系統(tǒng)將信息實(shí)時(shí)發(fā)送給無(wú)人船監(jiān)管中心；在信息采集過(guò)程中，若攝像機(jī)觀測(cè)到其視野范圍內(nèi)有其他交通船，結(jié)合雷達(dá)對(duì)交通船進(jìn)行跟蹤定位和運(yùn)動(dòng)狀態(tài)評(píng)估，將獲取的信息發(fā)送至存儲(chǔ)設(shè)備并傳送給規(guī)則選擇器，規(guī)則選擇器結(jié)合無(wú)人船、交通船的定位和運(yùn)動(dòng)狀態(tài)信息，識(shí)別特定的水上航行規(guī)則，并將信息傳遞給航行規(guī)劃器，航行規(guī)劃器結(jié)合速度障礙法做出最佳航行規(guī)劃，然后將最佳航行規(guī)劃信息傳遞給無(wú)人船控制器，無(wú)人船控制器控制無(wú)人船實(shí)施有效的避讓行動(dòng)，避免無(wú)人船與交通船發(fā)生碰撞，并安全到達(dá)目的地。

一種無(wú)人船導(dǎo)航和自動(dòng)駕駛方法，包括以下步驟：

(1)、無(wú)人船通過(guò)攝像機(jī)采集周邊環(huán)境信息，將信息傳送至存儲(chǔ)設(shè)備，在信息采集過(guò)程中，利用攝像機(jī)和慣性導(dǎo)航系統(tǒng)對(duì)無(wú)人船進(jìn)行實(shí)時(shí)定位和運(yùn)動(dòng)狀態(tài)評(píng)估，將獲取的信息傳送至存儲(chǔ)設(shè)備，并通過(guò)無(wú)線通信系統(tǒng)將信息實(shí)時(shí)發(fā)送給無(wú)人船監(jiān)管中心；

(2)、在無(wú)人船視野范圍內(nèi)出現(xiàn)其他交通船時(shí)，利用攝像機(jī)和雷達(dá)對(duì)交通船進(jìn)行跟蹤定位和運(yùn)動(dòng)狀態(tài)評(píng)估，將獲取的信息傳送至存儲(chǔ)設(shè)備；

(3)、規(guī)則選擇器根據(jù)步驟(1)和(2)獲得的無(wú)人船和交通船的位置和速度信息進(jìn)行預(yù)碰撞檢查，判斷無(wú)人船和交通船在維持當(dāng)前航向和航速的情況下，短時(shí)間內(nèi)是否會(huì)發(fā)生碰撞或接近碰撞；

(4)、若規(guī)則選擇器判斷到在短時(shí)間內(nèi)無(wú)人船將會(huì)與交通船發(fā)生碰撞或接近碰撞時(shí)，規(guī)則選擇器根據(jù)無(wú)人船和交通船的幾何位置關(guān)系識(shí)別特定的水上航行規(guī)則，將識(shí)別的特定水上航行規(guī)則信息傳送至航行規(guī)劃器；否則無(wú)人船維持當(dāng)前航線繼續(xù)航行；

(5)、航行規(guī)劃器接收到規(guī)則選擇器的信息后，利用速度障礙法計(jì)算無(wú)人船和交通船的碰撞時(shí)間r；并根據(jù)步驟(3)、(4)所得到的信息，利用速度障礙法做出最佳航行規(guī)劃，并將最佳航行規(guī)劃信息傳送至無(wú)人船控制器；

(6)、無(wú)人船控制器根據(jù)獲取的最佳航行規(guī)劃信息控制無(wú)人船實(shí)施有效的避讓行動(dòng)，避免與交通船發(fā)生碰撞，從而安全到達(dá)目的地。

進(jìn)一步的，上述步驟(1)中，利用攝像機(jī)和慣性導(dǎo)航系統(tǒng)對(duì)無(wú)人船進(jìn)行實(shí)時(shí)定位和運(yùn)動(dòng)狀態(tài)評(píng)估，該定位是指無(wú)人船相對(duì)于其始發(fā)位置的位置矢量該運(yùn)動(dòng)狀態(tài)是指無(wú)人船的航向和航速信息，即速度矢量

進(jìn)一步的，上述步驟(2)中，利用攝像機(jī)和雷達(dá)對(duì)交通船進(jìn)行跟蹤定位和運(yùn)動(dòng)狀態(tài)評(píng)估，該跟蹤定位是指交通船相對(duì)于無(wú)人船起始位置的位置矢量該運(yùn)動(dòng)狀態(tài)評(píng)估是指交通船的航向及航速信息，即速度矢量

進(jìn)一步的，上述步驟(3)中，規(guī)則選擇器根據(jù)上述步驟(1)和(2)獲得的無(wú)人船和交通船的位置和速度信息進(jìn)行預(yù)碰撞檢查，判斷無(wú)人船和交通船在維持當(dāng)前航向和航速的情況下，短時(shí)間內(nèi)是否會(huì)發(fā)生碰撞或接近碰撞，其具體方法包括以下步驟：

(3a)、計(jì)算無(wú)人船與交通船的到達(dá)最靠近接近點(diǎn)的時(shí)間：

其中，t表示向量或矩陣的轉(zhuǎn)置，|·|表示絕對(duì)值，||·||表示向量或矩陣的2-范數(shù)，ε表示足夠小的正數(shù)；

(3b)、計(jì)算最靠近接近點(diǎn)時(shí)無(wú)人船與交通船的距離：

其中，||·||表示向量或矩陣的2-范數(shù)；

(3c)、針對(duì)無(wú)人船視野范圍內(nèi)的每艘交通船，判斷無(wú)人船與交通船是否會(huì)發(fā)生碰撞或接近碰撞，具體判斷規(guī)則為：

0≤tcpa≤tmax,dcpa≤dmin(3)

其中：dmin表示無(wú)人船與交通船的最小安全距離，tmax表示無(wú)人船和交通船到達(dá)最小安全距離點(diǎn)時(shí)所花費(fèi)的時(shí)間，即當(dāng)無(wú)人船與交通船到達(dá)最靠近接近點(diǎn)的距離小于或等于最小安全距離以及到達(dá)最靠近節(jié)點(diǎn)的時(shí)間大于到達(dá)安全距離的時(shí)間時(shí)，則說(shuō)明無(wú)人船會(huì)與交通船會(huì)發(fā)生碰撞或接近碰撞；否則，認(rèn)為無(wú)人船不會(huì)與交通船發(fā)生碰撞或接近碰撞。

進(jìn)一步的，上述步驟(4)中，若規(guī)則選擇器判斷到在短時(shí)間內(nèi)無(wú)人船將會(huì)與交通船發(fā)生碰撞或接近碰撞時(shí)，規(guī)則選擇器根據(jù)無(wú)人船和交通船的幾何位置關(guān)系識(shí)別特定的水上航行規(guī)則，水上航行規(guī)則具體包括：

(4a)、當(dāng)交通船從無(wú)人船右前方橫穿時(shí)，規(guī)定無(wú)人船有義務(wù)主動(dòng)避讓橫穿的交通船，即無(wú)人船從交通船的左后方繞行，避開(kāi)橫穿的交通船，交通船無(wú)需改變其航行軌跡；

(4b)、當(dāng)交通船從無(wú)人船左前方橫穿時(shí)，規(guī)定交通船有義務(wù)采取避讓措施給無(wú)人船讓行，無(wú)人船必須保持當(dāng)前航行軌跡；

(4c)、當(dāng)交通船在無(wú)人船的正前方并與無(wú)人船同向而行時(shí)，無(wú)人船從交通船的右后側(cè)進(jìn)行超車(chē)動(dòng)作，超車(chē)時(shí)無(wú)人船必須保持相應(yīng)的橫向安全距離，以確保其航行軌跡遠(yuǎn)離交通船的航行軌跡；

(4d)、當(dāng)交通船在無(wú)人船正前方并與無(wú)人船相對(duì)而行時(shí)，無(wú)人船和交通船都必須向右轉(zhuǎn)舵，無(wú)人船從交通船的左側(cè)無(wú)害通過(guò)，交通船從無(wú)人船的左側(cè)無(wú)害通過(guò)。

進(jìn)一步的，若無(wú)人船同時(shí)滿(mǎn)足多條上述水上航行規(guī)則，即出現(xiàn)交通船從無(wú)人船的左前方或右前方不斷靠近無(wú)人船的情況，簡(jiǎn)單疊加所滿(mǎn)足的每條水上航行規(guī)則形成多規(guī)則約束條件；水上航行規(guī)則具體識(shí)別依據(jù)如下：

其中：下標(biāo)a和b分別表示無(wú)人船和交通船的變量，θb-θa表示交通船航向與無(wú)人船航向的夾角，bb表示交通船相對(duì)于無(wú)人船的方位，yb表示交通船相對(duì)于無(wú)人船的縱向距離，xb表示交通船相對(duì)于無(wú)人船的橫向距離，不同的門(mén)限值(·)min和(·)max被用于識(shí)別每條水上航行規(guī)則。

進(jìn)一步的，上述步驟(5)中航行規(guī)劃器利用速度障礙法計(jì)算無(wú)人船和交通船的碰撞時(shí)間r，速度障礙法具體如下：

位置矢量為的無(wú)人船以速度矢量航行一定時(shí)間t后的位置矢量可以表示為：

其中：和分別表示二維空間中無(wú)人船的位置矢量和速度矢量，t表示航行時(shí)間；

設(shè)定無(wú)人船和交通船的模型分別為矩形a和b，當(dāng)交通船位于無(wú)人船的左前方并不斷靠近無(wú)人船時(shí)，根據(jù)公式(5)在無(wú)人船的速度空間中定義速度障礙為：

其中，符號(hào)表示閔可夫斯基求和：

考慮到不同傳感器的評(píng)估誤差以及交通船本身的不精確運(yùn)動(dòng)，交通船的速度矢量被定義為：

其中：表示無(wú)人船的船載傳感器所評(píng)估的交通船期望速度矢量，表示交通船速度矢量的不確定度，且其中wb是的閾值且為常量，則具有最壞情況不確定度的速度障礙為：

其中：表示無(wú)人船的船載傳感器所評(píng)估的交通船期望速度矢量，表示無(wú)人船的期望速度障礙，wb表示交通船的不確定度所形成的速度障礙；

將交通船b占據(jù)的障礙區(qū)域用無(wú)人船a所占區(qū)域擴(kuò)展為只要無(wú)人船的速度矢量不在速度障礙區(qū)域內(nèi)，就能夠避免無(wú)人船與交通船發(fā)生碰撞；當(dāng)無(wú)人船左前方或右前方出現(xiàn)多艘交通船時(shí)，計(jì)算疊加的速度障礙，保證無(wú)人船的速度矢量不在疊加的速度障礙區(qū)域內(nèi)，即可避免無(wú)人船與任意交通船發(fā)生碰撞。

進(jìn)一步的，上述步驟(5)中；航行規(guī)劃器利用速度障礙法計(jì)算無(wú)人船和交通船的碰撞時(shí)間r，其具體計(jì)算方法如下：

當(dāng)無(wú)人船的速度矢量在速度障礙區(qū)域時(shí)，無(wú)人船與交通船的碰撞時(shí)間可通過(guò)計(jì)算無(wú)人船的相對(duì)速度與的邊界相交所花費(fèi)的時(shí)間來(lái)獲取，計(jì)算公式為：

其中，表示區(qū)域邊界，如果計(jì)算的碰撞時(shí)間包含多個(gè)r，選擇其中的最小值。

進(jìn)一步的，步驟(5)中，航行規(guī)劃器根據(jù)步驟(3)、(4)所得到的信息，利用速度障礙法做出航行規(guī)劃，航行規(guī)劃的具體方法如下：

在無(wú)人船航行區(qū)域構(gòu)建速度空間網(wǎng)格，即在v-θ空間中形成一個(gè)規(guī)則的離散網(wǎng)格并將其作為決策空間，其中v表示無(wú)人船的速度，θ表示無(wú)人船航向；一旦生成速度障礙和水上航行規(guī)則的約束集，對(duì)于每個(gè)vi和θj在短時(shí)間內(nèi)仍維持當(dāng)前值，此時(shí)航行規(guī)劃器通過(guò)公式(11)計(jì)算成本，當(dāng)計(jì)算完所有成本，選擇最小成本下的(vi,θj)對(duì)，即可得到無(wú)人船避讓交通船的最佳速度矢量，從而做出最佳航行規(guī)劃；

其中，wr表示碰撞時(shí)間權(quán)重，rij表示碰撞時(shí)間，表示到達(dá)下一個(gè)近期目標(biāo)的期望速度，wv表示期望速度偏差權(quán)重，||·||q表示加權(quán)的向量2-范數(shù)。

與現(xiàn)有技術(shù)相比，本發(fā)明具有以下有益效果：

(1)、本發(fā)明將立體攝像機(jī)和雷達(dá)探測(cè)到的交通船位置、速度和航向信息以及慣性導(dǎo)航設(shè)備獲取的無(wú)人船本身位置和速度信息用矢量來(lái)表示，這樣更有利于分析和評(píng)估每艘交通船以及無(wú)人船本身的運(yùn)動(dòng)狀態(tài)。

(2)、本發(fā)明提出利用水上航行規(guī)則和速度障礙法來(lái)實(shí)現(xiàn)無(wú)人船的自主導(dǎo)航和駕駛，不僅能夠避開(kāi)固定障礙物，還能有效地避開(kāi)移動(dòng)障礙物，可以充分應(yīng)對(duì)復(fù)雜的水面環(huán)境，自主規(guī)劃避讓交通船的路徑，并做出最優(yōu)速度和航向選擇，大大提高了避讓交通船的效率。

(3)、本發(fā)明充分考慮到了交通船人為操作的不確定性，在交通船不遵守水上航行規(guī)則的條件下，速度障礙的存在仍能夠有效地避免無(wú)人船與交通船發(fā)生碰撞。

(4)、本發(fā)明充分考慮到在單位時(shí)間步長(zhǎng)內(nèi)決策系統(tǒng)可能做出較頻繁的航行規(guī)劃，引入遲滯機(jī)制，使得無(wú)人船在做出決策后在短時(shí)間內(nèi)仍然維持當(dāng)前航速和航向，能夠有效避免無(wú)人船發(fā)生“顫振”行為。

附圖說(shuō)明

圖1為本發(fā)明的設(shè)備組成模塊框圖；

圖2為本發(fā)明中導(dǎo)航和自動(dòng)駕駛方法的流程圖；

圖3為本發(fā)明中水上航行的規(guī)則示意圖；

圖4為本發(fā)明中速度障礙的示意圖；

圖5為本發(fā)明中由水上航行規(guī)則施加在速度空間中的約束集示意圖；

圖6為本發(fā)明中無(wú)人船與交通船最靠近的接近點(diǎn)的示意圖。

具體實(shí)施方式

下面結(jié)合實(shí)施例對(duì)本發(fā)明作更進(jìn)一步的說(shuō)明。

如圖1所示，一種無(wú)人船導(dǎo)航和自動(dòng)駕駛設(shè)備，包括：安裝于無(wú)人船頂部的雷達(dá)、慣性導(dǎo)航系統(tǒng)、無(wú)線通訊系統(tǒng)和存儲(chǔ)設(shè)備，安裝于無(wú)人船頂部前端的攝像機(jī)，安裝于無(wú)人船駕駛艙內(nèi)的決策系統(tǒng)，所述決策系統(tǒng)包括規(guī)則選擇器、航行規(guī)劃器和無(wú)人船控制器；所述無(wú)人船通過(guò)攝像機(jī)采集周邊環(huán)境信息，將信息發(fā)送至存儲(chǔ)設(shè)備，所述無(wú)線通訊系統(tǒng)將信息實(shí)時(shí)發(fā)送給無(wú)人船監(jiān)管中心；在信息采集過(guò)程中，若攝像機(jī)觀測(cè)到其視野范圍內(nèi)有其他交通船，結(jié)合雷達(dá)對(duì)交通船進(jìn)行跟蹤定位和運(yùn)動(dòng)狀態(tài)評(píng)估，將獲取的信息發(fā)送至存儲(chǔ)設(shè)備并傳送給規(guī)則選擇器，規(guī)則選擇器結(jié)合無(wú)人船、交通船的定位和運(yùn)動(dòng)狀態(tài)信息，識(shí)別特定的水上航行規(guī)則，并將信息傳遞給航行規(guī)劃器，航行規(guī)劃器結(jié)合速度障礙法做出最佳航行規(guī)劃，然后將最佳航行規(guī)劃信息傳遞給無(wú)人船控制器，無(wú)人船控制器控制無(wú)人船實(shí)施有效的避讓行動(dòng)，避免無(wú)人船與交通船發(fā)生碰撞，并安全到達(dá)目的地。

如圖2所示，一種無(wú)人船自主導(dǎo)航和駕駛方法，包括以下步驟：

(1)、無(wú)人船通過(guò)立體攝像機(jī)采集周邊環(huán)境信息，將信息傳送至存儲(chǔ)設(shè)備。在信息采集過(guò)程中，利用立體攝像機(jī)和慣性導(dǎo)航系統(tǒng)對(duì)無(wú)人船進(jìn)行實(shí)時(shí)定位和運(yùn)動(dòng)狀態(tài)評(píng)估，將獲取的信息傳送至存儲(chǔ)設(shè)備并通過(guò)無(wú)線通信系統(tǒng)將信息實(shí)時(shí)發(fā)送給無(wú)人船監(jiān)管中心；

(2)、在無(wú)人船視野范圍內(nèi)出現(xiàn)其他交通船時(shí)，利用立體攝像機(jī)和雷達(dá)對(duì)交通船進(jìn)行跟蹤定位和運(yùn)動(dòng)狀態(tài)評(píng)估，將獲取的信息傳送至存儲(chǔ)設(shè)備；

(3)、規(guī)則選擇器根據(jù)步驟(1)和(2)獲得的無(wú)人船和交通船的位置和速度信息進(jìn)行預(yù)碰撞檢查，判斷無(wú)人船和交通船在維持當(dāng)前航向和航速的情況下，短時(shí)間內(nèi)是否會(huì)發(fā)生碰撞或接近碰撞；

(4)、若規(guī)則選擇器判斷到在短時(shí)間內(nèi)無(wú)人船將會(huì)與交通船發(fā)生碰撞或接近碰撞時(shí)，規(guī)則選擇器根據(jù)無(wú)人船和交通船的幾何位置關(guān)系識(shí)別特定的水上航行規(guī)則，將識(shí)別的特定水上航行規(guī)則信息傳送至航行規(guī)劃器；否則無(wú)人船維持當(dāng)前航線繼續(xù)航行；

(5)、航行規(guī)劃器接收到規(guī)則選擇器的信息后，利用速度障礙法計(jì)算無(wú)人船和交通船的碰撞時(shí)間r；并根據(jù)步驟(3)、(4)所得到的信息，利用速度障礙法做出最佳的航行規(guī)劃，并將最佳的航行規(guī)劃信息傳送至無(wú)人船控制器；

(6)、無(wú)人船控制器根據(jù)獲取的最佳航行規(guī)劃信息控制無(wú)人船實(shí)施有效的避讓行動(dòng)，避免與交通船發(fā)生碰撞，從而安全到達(dá)目的地。

步驟(1)中，利用立體攝像機(jī)和慣性導(dǎo)航系統(tǒng)對(duì)無(wú)人船進(jìn)行實(shí)時(shí)定位和運(yùn)動(dòng)狀態(tài)評(píng)估，定位是指無(wú)人船相對(duì)于其始發(fā)位置的位置矢量運(yùn)動(dòng)狀態(tài)評(píng)估是指無(wú)人船的航向和航速信息，即速度矢量

步驟(2)中，利用立體攝像機(jī)和雷達(dá)對(duì)交通船進(jìn)行跟蹤定位和運(yùn)動(dòng)狀態(tài)評(píng)估，跟蹤定位是指交通船相對(duì)于無(wú)人船起始位置的位置矢量運(yùn)動(dòng)狀態(tài)評(píng)估是指交通船的航向及航速信息，即速度矢量

步驟(3)中，規(guī)則選擇器根據(jù)步驟(1)和(2)獲得的無(wú)人船和交通船的位置和速度信息進(jìn)行預(yù)碰撞檢查，判斷無(wú)人船和交通船在維持當(dāng)前航向和航速的情況下，短時(shí)間內(nèi)是否會(huì)發(fā)生碰撞或接近碰撞，其具體策略如下：

(3a)、計(jì)算無(wú)人船與交通船的到達(dá)最靠近接近點(diǎn)的時(shí)間，最靠近接近點(diǎn)示意圖如圖3所示：

其中，t表示向量或矩陣的轉(zhuǎn)置，|·|表示絕對(duì)值，||·||表示向量或矩陣的2-范數(shù)，ε表示足夠小的正數(shù)；

(3b)、計(jì)算最靠近接近點(diǎn)時(shí)無(wú)人船與交通船的距離：

其中，||·||表示向量或矩陣的2-范數(shù)；

(3c)、針對(duì)無(wú)人船視野范圍內(nèi)的每艘交通船，判斷無(wú)人船與交通船是否會(huì)發(fā)生碰撞或接近碰撞，具體判斷規(guī)則為：

0≤tcpa≤tmax,dcpa≤dmin(3)

其中：dmin表示無(wú)人船與交通船的最小安全距離，tmax表示無(wú)人船和交通船到達(dá)最小安全距離點(diǎn)時(shí)所花費(fèi)的時(shí)間。

即當(dāng)無(wú)人船與交通船到達(dá)最靠近接近點(diǎn)的距離小于或等于最小安全距離以及到達(dá)最靠近節(jié)點(diǎn)的時(shí)間大于到達(dá)安全距離的時(shí)間時(shí)，則說(shuō)明無(wú)人船會(huì)與交通船會(huì)發(fā)生碰撞或接近碰撞；否則，認(rèn)為所述無(wú)人船不會(huì)與所述交通船發(fā)生碰撞或接近碰撞。

步驟(4)中，若規(guī)則選擇器判斷到在短時(shí)間內(nèi)無(wú)人船將會(huì)與交通船發(fā)生碰撞或接近碰撞時(shí)，規(guī)則選擇器根據(jù)無(wú)人船和交通船的幾何位置關(guān)系識(shí)別特定的水上航行規(guī)則，水上航行規(guī)則示意圖如圖4所示，具體包括如下幾項(xiàng)：

(4a)、當(dāng)交通船從無(wú)人船右前方橫穿時(shí)，規(guī)定無(wú)人船有義務(wù)主動(dòng)避讓橫穿的交通船，即無(wú)人船從交通船的左后方繞行，避開(kāi)橫穿的交通船，交通船無(wú)需改變其航行軌跡；

(4b)、當(dāng)交通船從無(wú)人船左前方橫穿時(shí)，規(guī)定交通船有義務(wù)采取避讓措施給無(wú)人船讓行，無(wú)人船必須保持當(dāng)前航行軌跡；

(4c)、當(dāng)交通船在無(wú)人船的正前方并與無(wú)人船同向而行時(shí)，無(wú)人船從交通船的右后側(cè)進(jìn)行超車(chē)動(dòng)作，超車(chē)時(shí)無(wú)人船必須保持與交通船的橫向安全距離，以確保其航行軌跡遠(yuǎn)離交通船的航行軌跡；

(4d)、當(dāng)交通船在無(wú)人船正前方并與無(wú)人船相對(duì)而行時(shí)，無(wú)人船和交通船都必須向右轉(zhuǎn)舵，無(wú)人船從交通船的左側(cè)無(wú)害通過(guò)，交通船從無(wú)人船的左側(cè)無(wú)害通過(guò)；

若無(wú)人船同時(shí)滿(mǎn)足多條上述水上航行規(guī)則，即出現(xiàn)交通船從無(wú)人船的左前方或右前方不斷靠近無(wú)人船的情況，可以簡(jiǎn)單疊加所滿(mǎn)足的每條水上航行規(guī)則形成多規(guī)則約束條件。

上述水上航行規(guī)則具體識(shí)別依據(jù)如圖5所示，具體包括如下：

其中：下標(biāo)a和b分別表示無(wú)人船和交通船的變量，θb-θa表示交通船航向與無(wú)人船航向的夾角，bb表示交通船相對(duì)于無(wú)人船的方位，yb表示交通船相對(duì)于無(wú)人船的縱向距離，xb表示交通船相對(duì)于無(wú)人船的橫向距離，不同的門(mén)限值(·)min和(·)max被用于識(shí)別每條水上航行規(guī)則。

步驟(5)中航行規(guī)劃器利用速度障礙法計(jì)算無(wú)人船和交通船的碰撞時(shí)間r，速度障礙的示意圖如圖6所示，速度障礙法具體如下：

位置矢量為的無(wú)人船以速度矢量為航行時(shí)間t后的位置矢量可以表示為：

其中：和分別表示二維空間中無(wú)人船的位置矢量和速度矢量，t表示航行時(shí)間。

設(shè)定無(wú)人船和交通船的模型分別為矩形a和b，當(dāng)交通船位于無(wú)人船的左前方并不斷靠近無(wú)人船時(shí)，根據(jù)公式(5)在無(wú)人船的速度空間中定義速度障礙為：

其中：符號(hào)表示閔可夫斯基求和：

考慮到不同傳感器的評(píng)估誤差以及交通船本身的不精確運(yùn)動(dòng)，交通船的速度矢量被定義為：

其中：表示無(wú)人船的船載傳感器所評(píng)估的交通船期望速度矢量，表示交通船速度矢量的不確定度，且其中wb是的閾值且為常量。

則具有最壞情況不確定度的速度障礙為：

其中：表示無(wú)人船的船載傳感器所評(píng)估的交通船期望速度矢量，表示無(wú)人船的期望速度障礙，wb表示交通船的不確定度所形成的速度障礙。

將所述交通船b占據(jù)的障礙區(qū)域用無(wú)人船a所占區(qū)域擴(kuò)展為只要所述無(wú)人船的速度矢量不在速度障礙區(qū)域內(nèi)，就能夠避免無(wú)人船與交通船發(fā)生碰撞。當(dāng)所述無(wú)人船左前方或右前方出現(xiàn)多艘交通船時(shí)，計(jì)算疊加的速度障礙，保證無(wú)人船的速度矢量不在疊加的速度障礙區(qū)域內(nèi)，即可避免無(wú)人船與任意交通船發(fā)生碰撞。

步驟(5)中，航行規(guī)劃器利用速度障礙法計(jì)算無(wú)人船和交通船的碰撞時(shí)間r，其具體計(jì)算方法如下：

當(dāng)無(wú)人船的速度矢量在速度障礙區(qū)域時(shí)，無(wú)人船與交通船的碰撞時(shí)間可通過(guò)計(jì)算無(wú)人船的相對(duì)速度與的邊界相交所花費(fèi)的時(shí)間來(lái)獲取，計(jì)算公式為：

其中：表示區(qū)域邊界，如果計(jì)算的碰撞時(shí)間包含多個(gè)r，選擇其中的最小值。

步驟(5)中，航行規(guī)劃器根據(jù)步驟(3)、(4)、所得到的信息，利用速度障礙法做出最佳的航行規(guī)劃，最佳航行規(guī)劃的具體方法如下：

在無(wú)人船航行區(qū)域構(gòu)建速度空間網(wǎng)格，即在v-θ空間中形成一個(gè)規(guī)則的離散網(wǎng)格并將其作為決策空間(其中v表示無(wú)人船的速度，θ表示無(wú)人船航向)。一旦生成速度障礙和水上航行規(guī)則的約束集，對(duì)于每個(gè)vi和θj在短時(shí)間內(nèi)仍維持當(dāng)前值，此時(shí)航行規(guī)劃器通過(guò)公式(11)計(jì)算成本，當(dāng)計(jì)算完所有成本，選擇最小成本下的(vi,θj)對(duì)，即可得到無(wú)人船避讓交通船的最佳速度矢量，從而做出最佳航行規(guī)劃。

其中：wr表示碰撞時(shí)間權(quán)重，rij表示碰撞時(shí)間，表示到達(dá)下一個(gè)近期目標(biāo)的期望速度，wv表示期望速度偏差權(quán)重，||·||q表示加權(quán)的向量2-范數(shù)。

以上所述僅是本發(fā)明的優(yōu)選實(shí)施方式，應(yīng)當(dāng)指出：對(duì)于本技術(shù)領(lǐng)域的普通技術(shù)人員來(lái)說(shuō)，在不脫離本發(fā)明原理的前提下，還可以做出若干改進(jìn)和潤(rùn)飾，這些改進(jìn)和潤(rùn)飾也應(yīng)視為本發(fā)明的保護(hù)范圍。