本發(fā)明屬于水下機(jī)器人路徑規(guī)劃領(lǐng)域,具體涉及一種基于聲吶成像的兩棲無(wú)人平臺(tái)路徑規(guī)劃方法。
背景技術(shù):
1、兩棲無(wú)人平臺(tái)是一種能在水下環(huán)境完成特定任務(wù)的機(jī)器人,在漁業(yè)、水下勘探、軍工方面都有很高應(yīng)用價(jià)值,但是受限于對(duì)先驗(yàn)高精水下地圖的需求和隨下環(huán)境復(fù)雜多變的特點(diǎn),自主導(dǎo)航和自主避障一直以來(lái)都是兩棲平臺(tái)在應(yīng)用過程中的重大挑戰(zhàn),通過聲吶的高低頻段切換來(lái)實(shí)時(shí)規(guī)劃路徑對(duì)于解決對(duì)全局地圖的高度依賴和提高兩棲平臺(tái)的運(yùn)動(dòng)能力、避障能力,保障兩棲平臺(tái)水下作業(yè)時(shí)的安全性具有十分重要的作用。
2、傳統(tǒng)的水下機(jī)器人大都基于視覺、聲吶等傳感器進(jìn)行障礙物探測(cè),當(dāng)前機(jī)器人的水下自主導(dǎo)航算法有動(dòng)態(tài)窗口法、人工勢(shì)場(chǎng)法等,這些方法雖說可以滿足基本的避障,但是仍存在許多問題,尤其是高速行進(jìn)時(shí)在沒有先驗(yàn)地圖的情況下往往僅依靠局部路徑規(guī)劃進(jìn)行避障,由于沒有預(yù)先的路徑參考,在面對(duì)大型障礙物時(shí)往往無(wú)法及時(shí)有效地規(guī)避障礙物,同時(shí)由于沒有全局地圖,上述算法僅能針對(duì)小范圍區(qū)域進(jìn)行避障,導(dǎo)致很有可能出現(xiàn)急減速的情況,并不能很好的考慮到機(jī)器人的動(dòng)力學(xué)或是運(yùn)動(dòng)學(xué)特性,所規(guī)劃出的路徑不一定能讓機(jī)器人有效的跟蹤。
技術(shù)實(shí)現(xiàn)思路
1、本發(fā)明的目的在于提供一種基于聲吶成像的兩棲無(wú)人平臺(tái)路徑規(guī)劃方法。
2、實(shí)現(xiàn)本發(fā)明目的的技術(shù)解決方案為:基于聲吶成像的兩棲無(wú)人平臺(tái)路徑規(guī)劃方法,包括如下步驟:
3、步驟(1):分別根據(jù)聲吶低頻段和高頻段兩種工作模式,對(duì)環(huán)境進(jìn)行聲吶成像,高亮區(qū)域認(rèn)定為障礙物,并根據(jù)聲吶成像獲取兩棲無(wú)人平臺(tái)所處環(huán)境的原始全局地圖和原始局部地圖;
4、步驟(2):基于步驟(1)的原始全局地圖和原始局部地圖,采用形態(tài)學(xué)算法進(jìn)行濾波,然后采用外接圓法對(duì)障礙物進(jìn)行模擬以獲取濾波后的全局地圖和濾波后的局部地圖;
5、步驟(3):基于步驟(2)獲取的濾波后的全局地圖,采用rrt算法進(jìn)行全局路徑搜素,得到全局路徑的粗解;
6、步驟(4):采用二維三次樣條插值的方法對(duì)rrt算法搜索出來(lái)的全局路徑的粗解進(jìn)行優(yōu)化,獲得優(yōu)化后的全局路徑;
7、步驟(5):基于步驟(4)獲得的優(yōu)化后的全局路徑,根據(jù)步驟(2)濾波后的局部地圖,對(duì)時(shí)間進(jìn)行m次采樣,對(duì)橫向位移l進(jìn)行n次采樣,得到m*n條s-t-l的縱向-時(shí)間-橫向三維路徑,然后以時(shí)間代價(jià)、橫向偏移代價(jià)、碰撞代價(jià)、加速度急變代價(jià)分別取對(duì)應(yīng)的權(quán)重后進(jìn)行加權(quán),根據(jù)加權(quán)后的代價(jià)函數(shù)選取最優(yōu)路徑。
8、進(jìn)一步的,步驟(2)具體包括如下步驟:
9、步驟(21):將聲吶成像轉(zhuǎn)換為hsv色彩空間圖像;
10、步驟(22):根據(jù)步驟(21)得到的hsv色彩空間圖像,設(shè)置上下閾值,采用閾值法對(duì)亮度區(qū)域進(jìn)行過濾,得到地圖粗解;
11、步驟(23):對(duì)地圖粗解采用形態(tài)學(xué)開運(yùn)算的方法消除由于硬件缺陷導(dǎo)致閾值法無(wú)法完全消除所殘存下來(lái)的噪點(diǎn),運(yùn)算公式為:
12、
13、f為輸入圖像,b為結(jié)構(gòu)元素,為腐蝕操作,為膨脹操作;
14、步驟(24):通過追蹤像素邊界尋找出障礙物;
15、步驟(25):在搜索完所有邊界后,調(diào)用opencv的minenclosingcircle函數(shù)計(jì)算并返回障礙物幾何最小外接圓的圓心和半徑,對(duì)于返回的外接圓半徑乘以系數(shù)k,最終返回所有獲取到的外接圓的圓心坐標(biāo)及半徑,至此獲得濾波后的全局地圖。
16、進(jìn)一步的,步驟(21)具體為:
17、1.v=max(r,g,b)
18、2.cmin=min(r,g,b)
19、
20、4.c=v-cmin
21、
22、其中:h∈[0,360],s∈[0,1],v∈[0,1],r、g、b分別為紅綠藍(lán)三原色的值,范圍在[0-1]內(nèi),v為明度,s為飽和度,c為色彩分量的差值,h為色相。
23、進(jìn)一步的,步驟(24)具體為:
24、初始化當(dāng)前節(jié)點(diǎn)為起始節(jié)點(diǎn),當(dāng)前朝向?yàn)?:current_point=start_point,direction=0,然后對(duì)于每個(gè)邊界點(diǎn)current_point和方向direction,計(jì)算下一個(gè)候選點(diǎn)new_point:
25、new_point=(x+dxi,y+dyi)where(dxi,dyi)∈directions
26、若當(dāng)前節(jié)點(diǎn)不是起點(diǎn),則對(duì)當(dāng)前的節(jié)點(diǎn)和方向進(jìn)行更新:
27、current_point=new_point
28、direction=(new_direction+5)%8
29、直到回到起點(diǎn)或沒有找到下一個(gè)邊界點(diǎn)時(shí)停止;得到障礙物幾何。
30、進(jìn)一步的,步驟(3)具體為:
31、步驟(31):根據(jù)兩棲平臺(tái)慣導(dǎo)及多普勒計(jì)程儀dvl融合后的定位信息獲取當(dāng)前坐標(biāo)位置,并且設(shè)置終點(diǎn)位置;
32、步驟(32):由步驟(22)獲取的濾波后的全局地圖,首先從起點(diǎn)出發(fā),記為父節(jié)點(diǎn),取終點(diǎn)采樣率f后在地圖空間中隨機(jī)撒點(diǎn),記隨機(jī)點(diǎn)與其父節(jié)點(diǎn)夾角為θ,以父節(jié)點(diǎn)為基準(zhǔn)節(jié)點(diǎn),取步長(zhǎng)step,并以θ為生長(zhǎng)方向開始擴(kuò)散子節(jié)點(diǎn),遍歷計(jì)算障礙物的坐標(biāo)中心點(diǎn)和子節(jié)點(diǎn)距離l,若l>r,則對(duì)生成的子節(jié)點(diǎn)連接進(jìn)行碰撞判斷,計(jì)算中心點(diǎn)到父子節(jié)點(diǎn)所在直線的距離l1,若l1>r則生成子節(jié)點(diǎn),并更新子節(jié)點(diǎn)為父節(jié)點(diǎn),直到搜索到終點(diǎn)位置為止,最后從終點(diǎn)向父節(jié)點(diǎn)追溯,找到搜索出來(lái)的路徑點(diǎn),即全局坐標(biāo)點(diǎn)。
33、進(jìn)一步的,步驟(4)具體為:
34、步驟(41):對(duì)步驟(3)搜索出的坐標(biāo)點(diǎn)、假設(shè)為7(n+1)個(gè),每隔七個(gè)坐標(biāo)點(diǎn)進(jìn)行一次采樣,若不滿七個(gè)則按一次采樣來(lái)計(jì)算,最終得到n+1個(gè)坐標(biāo)點(diǎn),每?jī)蓚€(gè)坐標(biāo)點(diǎn)視為一個(gè)區(qū)間則共有n個(gè)區(qū)間,每個(gè)區(qū)間[xi,xi+1]都是由三次多項(xiàng)式y(tǒng)=ai+bix+cix2+dix3構(gòu)成,且此多項(xiàng)式滿足在端點(diǎn)處連續(xù)且一階、二階導(dǎo)皆連續(xù),共有n個(gè)區(qū)間,需求解4n個(gè)未知數(shù),除了兩個(gè)端點(diǎn),所有n-1個(gè)內(nèi)部點(diǎn)的每個(gè)點(diǎn)都滿足前后兩個(gè)分段三次方程,則有2(n-1)個(gè)邊界條件,再加上兩個(gè)端點(diǎn)分別滿足第一個(gè)和最后一個(gè)三次方程,則總共有2n個(gè)邊界條件;其次s′i(xi+1)=s′i+1(xi+1)共n-1個(gè)邊界條件s″i(xi+1)=s″i+1(xi+1)共n-1個(gè)邊界條件,加上自然邊界s″(x0)=0=s″(xn)總共4n個(gè)邊界條件,最終得三對(duì)角矩陣:
35、
36、步驟(42):求解三對(duì)角矩陣獲得一維三次樣條曲線。
37、進(jìn)一步的,步驟(5)具體為:
38、步驟(51):縱向采樣:定義t為采樣時(shí)間,沿s方向的初始位置xs,s方向初始速度vxs,s方向初始加速度axs,在時(shí)間t時(shí)沿s軸期望的終點(diǎn)速度vxe、終點(diǎn)加速度axe,由此得到四次多項(xiàng)式f(t)=a0+a1*t+a2*t2+a3*t3+a4*t4,其中f′(0)為a0等于xs,f″(0)為a1等于vxs,f′(t)為t時(shí)刻的期望速度vxe,f″(t)為t時(shí)刻的期望加速度axe,最終求解出四次多項(xiàng)式,多項(xiàng)式滿足規(guī)劃出的縱向速度,加速度,jerk連續(xù);
39、步驟(52):橫向采樣:根據(jù)已知s方向的采樣點(diǎn),每個(gè)采樣點(diǎn)取不同的縱向采樣步長(zhǎng),定義t為采樣時(shí)間,沿l方向的初始位置xl,l方向初始速度vxl,l方向初始加速度axl,在時(shí)間t時(shí)沿l軸期望的終點(diǎn)速度vxe、終點(diǎn)加速度axe,由此得到五次多項(xiàng)式f(t)=a0+a1*t+a2*t2+a3*t3+a4*t4+a5*t5,其中f(0)為起始的l軸坐標(biāo),f′(0)為l1方向的a0,等于xs,f″(0)為a1等于vxs,f(t)為時(shí)間為t時(shí)的l軸坐標(biāo),f′(t)為t時(shí)刻的期望速度vxe,f″(t)為t時(shí)刻的期望加速度axe,最終求解出五次多項(xiàng)式,此多項(xiàng)式滿足機(jī)器人運(yùn)動(dòng)時(shí)橫縱向的速度、加速度、jerk連續(xù);
40、步驟(53):設(shè)計(jì)代價(jià)函數(shù)對(duì)步驟(51)和步驟(52)采樣得到的路徑進(jìn)行評(píng)價(jià),選取最優(yōu)解即為最優(yōu)路徑。
41、進(jìn)一步的,步驟(53)具體為:
42、步驟(531):計(jì)算時(shí)間代價(jià):time_cost=k_time*t1;
43、步驟(532):計(jì)算碰撞代價(jià):collision_cost=k_collision*is_collision,其中is_collision為bool值0或1,具體解釋為若發(fā)生碰撞則該路徑代價(jià)極大,不去選擇;
44、步驟(533):計(jì)算橫向偏移代價(jià):
45、offset_cost=k_offset*abs(path_pre.l)/2;
46、步驟(534):計(jì)算加速度急變代價(jià):
47、jerk_cos?t=kjerk*(l_jerk_sum+s_jerk_sum)
48、其中:
49、
50、步驟(535):計(jì)算當(dāng)前路徑總代價(jià):
51、path_pre.cost=k_collision*is_collision+k_jerk*(l_jerk_sum+s_jerk_sum)+k_time*t1*k_offset*abs(path_pre.l)/2
52、步驟(536):選取所有采樣點(diǎn)中代價(jià)最小的路徑,即為最優(yōu)路徑。
53、本發(fā)明與現(xiàn)有技術(shù)相比,其顯著優(yōu)點(diǎn)在于:
54、(1)本發(fā)明設(shè)計(jì)了一種基于前置多波束聲吶成像并進(jìn)行離散空間采樣的兩棲無(wú)人平臺(tái)路徑規(guī)劃方法,該方法針對(duì)在沒有先驗(yàn)地圖信息的情況,借助聲吶的低頻段來(lái)獲取全局地圖信息,初步求解出采樣空間,再基于該采樣空間進(jìn)行有效的自主導(dǎo)航和避障,擺脫了水下自主機(jī)器人全局規(guī)劃對(duì)先驗(yàn)地圖的高度依賴。
55、(2)本發(fā)明在對(duì)rrt算法搜索出的全局路徑進(jìn)行優(yōu)化時(shí),針對(duì)x和y的三次樣條插值方法只有在x不斷增加的情況下才有效,倘若在搜索路徑的過程中出現(xiàn)x突然減小的情況則需要進(jìn)行重規(guī)劃,然后進(jìn)行拼接,此類方法不僅算力消耗較大,而且會(huì)出現(xiàn)在拼接處的路徑不符合機(jī)器人的運(yùn)動(dòng)學(xué)約束的問題,所以本方法提出了一種二維三次樣條插值方法,首先求出兩坐標(biāo)點(diǎn)間的距離u,然后分別對(duì)u-x、u-y進(jìn)行插值,由于u不可能為負(fù)數(shù),所以在進(jìn)行曲線優(yōu)化的時(shí)候u不會(huì)出現(xiàn)減少的情況,很好的避免了傳統(tǒng)三次樣條插值的局限性。
56、(3)相較于經(jīng)典的人工勢(shì)場(chǎng)法,dwa算法,本發(fā)明的算法彌補(bǔ)了無(wú)法很好的滿足機(jī)器人的運(yùn)動(dòng)學(xué)約束的缺陷,針對(duì)機(jī)器人在水下高速運(yùn)動(dòng)并進(jìn)行避障的過程中速度變化不平滑的情況,該算法增加了加速度規(guī)劃,并且采用五次多項(xiàng)式進(jìn)行擬合從而使路徑更加平滑,更適用于高速運(yùn)動(dòng)過程中的水下自主機(jī)器人路徑規(guī)劃,可以作為一種兩棲平臺(tái)的路徑規(guī)劃方法。