專利名稱：一種水下自主式航行器的局部路徑規(guī)劃方法
技術(shù)領(lǐng)域：
本發(fā)明涉及水下自主式航行器領(lǐng)域，特別涉及一種水下自主式航行器的局部路徑規(guī)劃方法。
背景技術(shù)：
水下自主式航行器在對指定的未知海域進(jìn)行海底勘查與測量時需要在水平投影內(nèi)沿指定目標(biāo)航跡行駛。如圖I所示，水下自主式航行器一般采取在水平投影上沿“弓”字形航跡來回掃描的方式進(jìn)行海底勘查與測量。該航跡可被分解為一系列有向直線段。水下自主式航行器在沿目標(biāo)航跡行駛的過程中，由于受到洋流的干擾、傳感器測量誤差與自身的控制誤差等，會偏離目標(biāo)航跡行駛，導(dǎo)致水下自主式航行器不能按照目標(biāo)航跡進(jìn)行海底掃描測量，從而影響測量效果。水下自主式航行器必須能夠減小和消除這種航跡跟蹤偏差， 要有自主進(jìn)行局部路徑規(guī)劃的能力，實現(xiàn)在一定精度范圍內(nèi)的航跡跟蹤。這是水下自主式航行器完成水下作業(yè)和使命的重要前提。目前水下自主式航行器常用的航跡跟蹤方法有神經(jīng)網(wǎng)絡(luò)法(參考文獻(xiàn)湯莉，AUV神經(jīng)網(wǎng)絡(luò)水平面航跡跟蹤控制研究，哈爾濱工程大學(xué)碩士學(xué)位論文，2009)等。但該方法抗瞬時擾動能力和抗恒定洋流干擾能力不是十分理想調(diào)節(jié)距離長，跟蹤里程長，計算復(fù)雜、算法的時間和空間開銷大，難以滿足水下自主式航行器進(jìn)行海底勘查與測量的需求。

發(fā)明內(nèi)容
本發(fā)明提供了一種水下自主式航行器的局部路徑規(guī)劃方法，本方法實現(xiàn)了調(diào)節(jié)距離短，跟蹤里程短，計算簡單、時間和空間開銷小，滿足了水下自主式航行器進(jìn)行海底勘查與測量的需求，詳見下文描述一種水下自主式航行器的局部路徑規(guī)劃方法，所述方法包括以下步驟(I)將全局目標(biāo)航跡分解為一系列有向直線段PiPwI i = 1，2，3，...，N，并依次將 N個預(yù)期航點存儲在水下自主式航行器控制系統(tǒng)任務(wù)文件里；(2)將Pi和Pi+1定義為一段航跡的起始點和結(jié)束點，P為水下自主式航行器當(dāng)前位置，P到直線PiPw的垂線段的長度d = |ph|定義為當(dāng)前航跡跟蹤偏差，H為垂足，
為目標(biāo)航跡的期望航向角,％ 為未修正的期望航向角，￥ph為與期望航向角％&垂直的航向，vE為修正以后的期望航向角；(3)從所述水下自主式航行器控制系統(tǒng)任務(wù)文件里讀取第一個目標(biāo)航點Pi, i = I ;(4)若i > N，則執(zhí)行步驟(12)，否則，從所述水下自主式航行器控制系統(tǒng)任務(wù)文件里讀取下一目標(biāo)航點Pi+1，確定當(dāng)前局部目標(biāo)航跡有向線段PiPw ;(5)從慣性導(dǎo)航系統(tǒng)獲得水下自主式航行器當(dāng)前位置P和當(dāng)前航向，判斷是否已經(jīng)到達(dá)Pi+1，如果是，i = i+1，執(zhí)行步驟⑷；如果否，執(zhí)行步驟(6)；(6)計算水下自主式航行器當(dāng)前航跡跟蹤偏差d，判斷所述當(dāng)前航跡跟蹤偏差d是否小于等于第一閾值dmin，如果是，執(zhí)行步驟(7);如果否，執(zhí)行步驟(8)；(7)修正以后的期望航向角Ve等于，期望航向不做調(diào)整，執(zhí)行步驟(12)；(8)判斷所述當(dāng)前航跡跟蹤偏差d是否小于等于第二閾值d_，且大于第一閾值 dmin，如果是，執(zhí)彳了步驟(9);如果否，執(zhí)彳了步驟(10);(9)根據(jù)第一公式計算輔助動態(tài)圓半徑r，以所述水下自主式航行器當(dāng)前位置P為圓心、以！■為半徑建立動態(tài)輔助圓，根據(jù)所述動態(tài)輔助圓和所述當(dāng)前局部目標(biāo)航跡有向線段PiPw的位置關(guān)系選出臨時目標(biāo)航點，計算出修正以后的期望航向￥e;(10)所述當(dāng)前航跡跟蹤偏差d大于所述第二閾值d_，使期望航向Ve= ￥ph;(11)控制所述水下自主式航行器沿Ve方向行駛，重新執(zhí)行步驟(5)；(12)流程結(jié)束。所述第一公式具體為r = f (d), f (d)彡 0，f (dmin) = rmax, f (dmax) = rmin, dmin < dmax其中，r.，rmax分別表示動態(tài)圓半徑r的最大值和最小值。所述根據(jù)所述動態(tài)輔助圓和所述當(dāng)前局部目標(biāo)航跡有向線段PiPw的位置關(guān)系選出臨時目標(biāo)航點，計算出修正以后的期望航向Ve具體為I)當(dāng)動態(tài)輔助圓與直線PiPw相離且P在PiPw上的投影H在PwPi的延長線上時，選擇Pi作為臨時目標(biāo)航點，修改期望航向使;2)當(dāng)動態(tài)輔助圓與直線PiPp1相離且P在PiPp1上的投影H在PiPp1上時，選擇H 作為臨時目標(biāo)航點，修改期望航向使Ve= Vph;3)當(dāng)動態(tài)輔助圓與直線PiPw相離且P在PiPw上的投影H在PiPw的延長線上時，選擇Pi+1作為臨時目標(biāo)航點，修改期望航向使仏=% ;4)當(dāng)動態(tài)輔助圓與直線PiPw相交且P在PiPw上的投影H在PwPi的延長線上時，選擇Pi為臨時目標(biāo)航點，修改期望航向使;5)當(dāng)動態(tài)輔助圓和直線PiPw相交于點E、F且P在PiPw上的投影在PiPw上，且 Pi在動態(tài)輔助圓內(nèi)Pi+1在動態(tài)輔助圓外時，選擇靠近Pi+1的E點為臨時目標(biāo)航點，修改期望航向使Ve = Vpe ;6)當(dāng)動態(tài)輔助圓和直線PiPw相交于點E、F且P在PiPw上的投影在PiPw上，且 1和？1+1在動態(tài)輔助圓外時，選擇E點為臨時目標(biāo)航點，修改期望航向使Ve= Vpe;7)當(dāng)P在PiPw上的投影在PiPw上且Pi和Pi+1在動態(tài)輔助圓內(nèi)時，選擇Pi+1點為臨時目標(biāo)航點，修改期望航向使仏=;8)當(dāng)動態(tài)輔助圓和直線PiPp1相交，且P在PiPw上的投影在PiPw上，且Pi+1在動態(tài)輔助圓內(nèi)Pi在動態(tài)輔助圓外時，選擇Pi+1點為臨時目標(biāo)航點，修改期望航向使；9)當(dāng)動態(tài)輔助圓和直線PiPw相交且P在PiPw上的投影在PiPw的延長線上，選擇Pi+1點為臨時目標(biāo)航點，修改期望航向使仏=% 。本發(fā)明提供的技術(shù)方案的有益效果是本發(fā)明提供了一種水下自主式航行器的局部路徑規(guī)劃方法，當(dāng)航跡跟蹤偏差d小于等于第二閾值d_，且大于第一閾值dmin時，通過動態(tài)輔助圓和當(dāng)前局部目標(biāo)航跡有向線段PiPw的位置關(guān)系選出臨時目標(biāo)航點，計算出修正以后的期望航向VE，控制水下自主式航行器沿Ve方向行駛，本方法能夠有效實現(xiàn)水下自主式航行器的局部路徑規(guī)劃，其抵抗瞬間干擾的能力和抵抗恒定洋流干擾的能力都強(qiáng)于傳統(tǒng)的神經(jīng)網(wǎng)絡(luò)法，并且具有時空開銷小、計算簡便的優(yōu)點，滿足了水下自主式航行器進(jìn)行海底勘查與測量的需求。


圖I為現(xiàn)有技術(shù)提供的水下自主式航行器欲采取的全局目標(biāo)航跡的分解與表達(dá)示意圖；圖2為本發(fā)明提供的圖示說明及相關(guān)定義；圖3-1與3-2為本發(fā)明提供的檢驗本方法的抗瞬時擾動能力的實驗效果圖；圖4為本發(fā)明提供的檢驗本方法的抗恒定洋流擾動能力的實驗效果圖；圖5-1至5-9為本發(fā)明提供的本方法的動態(tài)輔助圓與臨時局部期望航跡的位置關(guān)系及相應(yīng)的新的期望航點選取的示意圖；圖6為本發(fā)明提供的一種水下自主式航行器的局部路徑規(guī)劃方法的流程圖。
具體實施例方式為使本發(fā)明的目的、技術(shù)方案和優(yōu)點更加清楚，下面將結(jié)合附圖對本發(fā)明實施方式作進(jìn)一步地詳細(xì)描述。為了實現(xiàn)調(diào)節(jié)距離短，跟蹤里程短，計算簡單、時間和空間開銷小，滿足水下自主式航行器進(jìn)行海底勘查與測量的需求，參見圖6，本發(fā)明實施例提供了一種水下自主式航行器的局部路徑規(guī)劃方法，詳見下文描述101 :將全局目標(biāo)航跡分解為一系列有向直線段PiPwI i = 1，2，3，...，N，并依次將N個預(yù)期航點存儲在水下自主式航行器控制系統(tǒng)任務(wù)文件里；其中，參見圖1，該步驟具體為將水下自主式航行器進(jìn)行海底測量欲采取的全局目標(biāo)航跡的各個頂點PiIi = 1，2，3，...，N，按照先后順序存儲于水下自主式航行器控制系統(tǒng)任務(wù)文件中，共N個預(yù)期航點，N為正整數(shù)，每個航點包括預(yù)期經(jīng)度、預(yù)期緯度和預(yù)期速度
等信息。其中，參見圖2，設(shè)水下自主式航行器目前行駛在局部目標(biāo)航跡PiPw附近。102 :將Pi和Pi+1定義為一段航跡的起始點和結(jié)束點，P為水下自主式航行器當(dāng)前位置，P到直線PiPw的垂線段的長度d= |ph|定義為航跡跟蹤偏差，h為垂足，為目標(biāo)航跡的期望航向角，為未修正的期望航向角，VPhS與期望航向角垂直的航向，￥e 為修正以后的期望航向角；103 :從水下自主式航行器控制系統(tǒng)任務(wù)文件里讀取第一個目標(biāo)航點Pi, i = I ;104 :若i SN，則執(zhí)行步驟112，否則，從水下自主式航行器控制系統(tǒng)任務(wù)文件里讀取下一目標(biāo)航點Pi+1，確定當(dāng)前局部目標(biāo)航跡有向線段PiPw ;其中，通過第一個目標(biāo)航點Pi和下一目標(biāo)航點Pi+1從而確定當(dāng)前局部目標(biāo)航跡有向線段PiPw。105 :從慣性導(dǎo)航系統(tǒng)獲得水下自主式航行器當(dāng)前位置P和當(dāng)前航向，判斷是否已經(jīng)到達(dá)Pi+1，如果是，i = i+1，執(zhí)行步驟104 ;如果否，執(zhí)行步驟106 ；其中，當(dāng)前位置P通常包括經(jīng)度和緯度信息，例如經(jīng)度為117.00000000°，緯度為 39. 00000000。、當(dāng)前航向為 100. 000°。
106 :計算水下自主式航行器當(dāng)前航跡跟蹤偏差d，判斷當(dāng)前航跡跟蹤偏差d是否小于等于第一閾值dmin，如果是，執(zhí)行步驟107 ;如果否，執(zhí)行步驟108 ；107 -yE = WPPi+i，期望航向不做調(diào)整，執(zhí)行步驟112 ；108 :判斷當(dāng)前航跡跟蹤偏差d是否小于等于第二閾值Clmax,且大于第一閾值dmin， 如果是，執(zhí)行步驟109 ;如果否，執(zhí)行步驟110 ；109:根據(jù)第一公式計算輔助動態(tài)圓半徑r，以水下自主式航行器當(dāng)前位置P為圓心、以r為半徑建立動態(tài)輔助圓，根據(jù)動態(tài)輔助圓和當(dāng)前局部目標(biāo)航跡有向線段PiPw的位置關(guān)系選出臨時目標(biāo)航點，計算出修正以后的期望航向Ve,執(zhí)行步驟111 ;其中，第一公式具體為r = f (d), f (d) ^ 0, f (dmin) = rmax, f (dmax) = rmin, dmin < dmax (I)其中，rmin, rmax分別表示動態(tài)圓半徑r的最大值和最小值，dmin, dmax, rmin和rmax的選取根據(jù)水下自主式航行器的尺寸、水動力特性、航跡跟蹤精度需求確定，具體實現(xiàn)時，本發(fā)明實施例對此不做限制。滿足(I)式的函數(shù)表達(dá)式有多種方式，本發(fā)明實施例以滿足其中的一種形式(2)式為例進(jìn)行說明。
權(quán)利要求
1.一種水下自主式航行器的局部路徑規(guī)劃方法，其特征在于，所述方法包括以下步驟(1)將全局目標(biāo)航跡分解為一系列有向直線段PiPwIi = 1,2,3,...，N，并依次將N個預(yù)期航點存儲在水下自主式航行器控制系統(tǒng)任務(wù)文件里；(2)將Pi和Pi+1定義為一段航跡的起始點和結(jié)束點，P為水下自主式航行器當(dāng)前位置， P到直線PiPw的垂線段的長度d = PH定義為當(dāng)前航跡跟蹤偏差，H為垂足，％^+1為目標(biāo)航跡的期望航向角，為未修正的期望航向角，VphS與期望航向角Kfw垂直的航向，Ve為修正以后的期望航向角；(3)從所述水下自主式航行器控制系統(tǒng)任務(wù)文件里讀取第一個目標(biāo)航點Pi,i = I ;(4)若i> N，則執(zhí)行步驟(12)，否則，從所述水下自主式航行器控制系統(tǒng)任務(wù)文件里讀取下一目標(biāo)航點Pi+1，確定當(dāng)前局部目標(biāo)航跡有向線段PiPw ;(5)從慣性導(dǎo)航系統(tǒng)獲得水下自主式航行器當(dāng)前位置P和當(dāng)前航向，判斷是否已經(jīng)到達(dá)Pi+1，如果是，i = i+1，執(zhí)行步驟⑷；如果否，執(zhí)行步驟(6)；(6)計算水下自主式航行器當(dāng)前航跡跟蹤偏差d，判斷所述當(dāng)前航跡跟蹤偏差d是否小于等于第一閾值dmin，如果是，執(zhí)行步驟(7);如果否，執(zhí)行步驟(8)；(7)修正以后的期望航向角￥E等于，期望航向不做調(diào)整，執(zhí)行步驟(12)；(8)判斷所述當(dāng)前航跡跟蹤偏差d是否小于等于第二閾值d_，且大于第一閾值dmin，如果是，執(zhí)行步驟(9);如果否，執(zhí)行步驟(10)；(9)根據(jù)第一公式計算輔助動態(tài)圓半徑r，以所述水下自主式航行器當(dāng)前位置P為圓心、以r為半徑建立動態(tài)輔助圓，根據(jù)所述動態(tài)輔助圓和所述當(dāng)前局部目標(biāo)航跡有向線段 PiPw的位置關(guān)系選出臨時目標(biāo)航點，計算出修正以后的期望航向VE，執(zhí)行步驟(11)；(10)所述當(dāng)前航跡跟蹤偏差d大于所述第二閾值d_，使期望航向UTe=Vph，執(zhí)行步驟(11);(11)控制所述水下自主式航行器沿Ve方向行駛，重新執(zhí)行步驟(5)；(12)流程結(jié)束。
2.根據(jù)權(quán)利要求I所述的一種水下自主式航行器的局部路徑規(guī)劃方法，其特征在于， 所述第一公式具體為r = f (d), f (d) ≤0, f (dmin) = rmax, f (dmax) = Ymin, dmin < dmax其中，^in, rmax分別表示動態(tài)圓半徑r的最大值和最小值。
3.根據(jù)權(quán)利要求I所述的一種水下自主式航行器的局部路徑規(guī)劃方法，其特征在于， 所述根據(jù)所述動態(tài)輔助圓和所述當(dāng)前局部目標(biāo)航跡有向線段PiPw的位置關(guān)系選出臨時目標(biāo)航點，計算出修正以后的期望航向Ve具體為1)當(dāng)動態(tài)輔助圓與直線PiPw相離且P在PiPw上的投影H在PwPi的延長線上時，選擇？1作為臨時目標(biāo)航點，修改期望航向使;2)當(dāng)動態(tài)輔助圓與直線PiPw相離且P在PiPw上的投影H在PiPw上時，選擇H作為臨時目標(biāo)航點，修改期望航向使Ve= ￥ph ;3)當(dāng)動態(tài)輔助圓與直線PiPw相離且P在PiPw上的投影H在PiPw的延長線上時，選擇Pi+1作為臨時目標(biāo)航點，修改期望航向使仏=￥PPm ；4)當(dāng)動態(tài)輔助圓與直線PiPw相交且P在PiPw上的投影H在PwPi的延長線上時，選擇？1為臨時目標(biāo)航點，修改期望航向使;5)當(dāng)動態(tài)輔助圓和直線PiPw相交于點E、F且P在PiPw上的投影在PiPw上，且Pi在動態(tài)輔助圓內(nèi)Pi+1在動態(tài)輔助圓外時，選擇靠近Pi+1的E點為臨時目標(biāo)航點，修改期望航向使 Ve = Vpe ；6)當(dāng)動態(tài)輔助圓和直線PiPw相交于點E、F且P在PiPw上的投影在PiPw上，且Pi和 Pi+1在動態(tài)輔助圓外時，選擇E點為臨時目標(biāo)航點，修改期望航向使Ve= Vpe;7)當(dāng)P在PiPw上的投影在PiPw上且Pi和Pi+1在動態(tài)輔助圓內(nèi)時，選擇Pi+1點為臨時目標(biāo)航點，修改期望航向使仏=;8)當(dāng)動態(tài)輔助圓和直線PiPw相交，且P在PiPw上的投影在PiPw上，且Pi+1在動態(tài)輔助圓內(nèi)Pi在動態(tài)輔助圓外時，選擇Pi+1點為臨時目標(biāo)航點，修改期望航向使仏=% ;9)當(dāng)動態(tài)輔助圓和直線PiPp1相交且P在PiPp1上的投影在PiPp1的延長線上，選擇Pi+1 點為臨時目標(biāo)航點，修改期望航向使仏=^+1。
全文摘要
本發(fā)明公開了一種水下自主式航行器的局部路徑規(guī)劃方法，涉及水下自主式航行器領(lǐng)域，當(dāng)航跡跟蹤偏差d小于等于第二閾值dmax，且大于第一閾值dmin時，通過動態(tài)輔助圓和當(dāng)前局部目標(biāo)航跡有向線段PiPi+1的位置關(guān)系選出臨時目標(biāo)航點，計算出修正以后的期望航向ψE，控制水下自主式航行器沿ψE方向行駛，本方法能夠有效實現(xiàn)水下自主式航行器的局部路徑規(guī)劃，其抵抗瞬間干擾的能力和抵抗恒定洋流干擾的能力都強(qiáng)于傳統(tǒng)的神經(jīng)網(wǎng)絡(luò)法，并且具有時空開銷小、計算簡便的優(yōu)點，滿足了水下自主式航行器進(jìn)行海底勘查與測量的需求。
文檔編號G05D1/02GK102541062SQ20121004518
公開日2012年7月4日 申請日期2012年2月24日 優(yōu)先權(quán)日2012年2月24日
發(fā)明者曾周末, 李一博, 陳世利, 靳世久, 黃新敬 申請人:天津大學(xué)