專利名稱：一種無纜自治水下航行體的航跡控制方法
技術(shù)領(lǐng)域：
本發(fā)明涉及一種無纜自治水下航行體(AUV, Autonomous Underwater Vehicle)的航行控制技術(shù)，具體地說是無纜自治水下航行體的航跡控制方法，主控計(jì)算機(jī)采用適當(dāng)?shù)目刂品椒?，使無纜自治水下航行體可以準(zhǔn)確地沿著規(guī)劃的路徑航行。
背景技術(shù)：
以往的無纜自治水下航行體通常采用目標(biāo)點(diǎn)閉環(huán)的方式來按預(yù)定航路航行，即目標(biāo)航向角始終指向航行目標(biāo)點(diǎn)，如圖1所示，圖中AB表示規(guī)劃的路徑,B為航行路徑目標(biāo)點(diǎn)， L表示AUV在靜水水域航行時(shí)的航跡，LI表示在有海流情況下，以往的目標(biāo)點(diǎn)閉環(huán)控制方法所實(shí)現(xiàn)的航跡；如果按照該航跡航行，水下航行體就會(huì)在海流的作用下偏離規(guī)劃路徑，無法完成航跡精確跟蹤任務(wù)，例如，在狹窄水域航行或海底地形地貌的全覆蓋探測(cè)時(shí)，就要求無纜自治水下航行體嚴(yán)格按照規(guī)劃的航路航行。發(fā)明內(nèi)容
為了克服目前無纜自治水下航行體航跡精確跟蹤控制方面的不足，本發(fā)明提供了一種無纜自治水下航行體的航跡精確跟蹤控制方法。
本發(fā)明采用的技術(shù)方案是一種無纜自治水下航行體的航跡控制方法，包括以下步驟
計(jì)算水下航行體的航行路徑偏移量Λ S ;將Λ S帶入PID算法得到航行路徑偏移量的控制量ASefa ;計(jì)算水下航行體到達(dá)航行路徑目標(biāo)點(diǎn)的航向控制量;將控制量 Λ Sea與航向控制量AHea的和作為航向角的總控制量，根據(jù)水下航行體的推進(jìn)器布置情況將控制量分配至各螺旋槳電機(jī)，實(shí)現(xiàn)水下航行體的航跡控制。
所述計(jì)算水下航行體的航行路徑偏移量△ S為水下航行體的當(dāng)前位置到規(guī)劃路徑的距離。
所述計(jì)算水下航行體到達(dá)目標(biāo)點(diǎn)的航向控制量AHea采用PID控制算法，目標(biāo)值為到達(dá)規(guī)劃路徑目標(biāo)點(diǎn)的航向角，反饋值為實(shí)時(shí)測(cè)量的航向角，將二者的差帶入到PID控制算法得到控制量AHeat5
本發(fā)明具有以下有益效果及優(yōu)點(diǎn)
1.本發(fā)明采用的無纜 自治水下航行體的航跡精確跟蹤控制方法可在有海流的情況下實(shí)現(xiàn)無纜自治水下航行體的精確跟蹤航跡控制，穩(wěn)定航行時(shí)AUV實(shí)際航跡距規(guī)劃航路的距離可控制在2米以內(nèi)，為無纜自治水下航行體完成精確航跡跟蹤任務(wù)提供保障。
2.在有海流的情況下，保證無纜自治水下航行體高精度地按照預(yù)定航路航行。
3.本發(fā)明方法簡(jiǎn)單可行，工作可靠，可有效提高高海流條件下自治水下航行體的航跡跟蹤精度。


圖1是本發(fā)明的水下航行體航行路徑示意圖2是目標(biāo)定向航向角控制方框圖3是精確航路控制方框圖4是精確航路控制程序流程圖。
具體實(shí)施方式
下面結(jié)合附圖及實(shí)施例對(duì)本發(fā)明做進(jìn)一步的詳細(xì)說明。
(I)計(jì)算AUV當(dāng)前位置到規(guī)劃路徑(直線)的垂直距離Λ S，即AUV目前沿規(guī)劃航線航行的偏差Δ S ；
(2)計(jì)算出AUV航行路徑偏移量的控制量Λ Se$(。
采用PID算法計(jì)算出AUV航行路徑偏移量的控制量Λ Seffl標(biāo)準(zhǔn)的PID算法如下
權(quán)利要求
1.一種無纜自治水下航行體的航跡控制方法，其特征在于包括以下步驟 計(jì)算水下航行體的航行路徑偏移量AS;將AS帶入PID算法得到航行路徑偏移量的控制量;計(jì)算水下航行體到達(dá)航行路徑目標(biāo)點(diǎn)的航向控制量;將控制量ASe與航向控制量AHea的和作為航向角的總控制量，根據(jù)水下航行體的推進(jìn)器布置情況將控制量分配至各螺旋槳電機(jī)，實(shí)現(xiàn)水下航行體的航跡控制。
2.根據(jù)權(quán)利要求1所述的一種無纜自治水下航行體的航跡控制方法，其特征在于所述計(jì)算水下航行體的航行路徑偏移量AS為水下航行體的當(dāng)前位置到規(guī)劃路徑的距離。
3.根據(jù)權(quán)利要求1所述的一種無纜自治水下航行體的航跡控制方法，其特征在于所述計(jì)算水下航行體到達(dá)目標(biāo)點(diǎn)的航向控制量AHea采用PID控制算法，目標(biāo)值為到達(dá)規(guī)劃路徑目標(biāo)點(diǎn)的航向角，反饋值為實(shí)時(shí)測(cè)量的航向角，將二者的差帶入到PID控制算法得到控制量Λ H·。
全文摘要
本發(fā)明涉及一種無纜自治水下航行體的航行控制技術(shù)，控制方法包括計(jì)算水下航行體當(dāng)前位置到規(guī)劃路徑的距離ΔS；將距離ΔS帶入PID算法得到航行路徑偏移量的控制量ΔS控制；計(jì)算水下航行體到達(dá)目標(biāo)點(diǎn)的航向控制量ΔH控制；將控制量ΔS控制與航向控制量ΔH控制的和作為總控制量，根據(jù)水下航行體的推進(jìn)器布置情況將控制量分配至各螺旋槳電機(jī)，實(shí)現(xiàn)水下航行體的航跡控制。本發(fā)明可在有海流的情況下實(shí)現(xiàn)無纜自治水下航行體的精確航跡控制，穩(wěn)定航行時(shí)AUV實(shí)際航跡距規(guī)劃航路的距離可控制在2米以內(nèi)，為無纜自治水下航行體精確完成航跡跟蹤任務(wù)提供航行保障。
文檔編號(hào)G05B13/04GK103048997SQ20121048519
公開日2013年4月17日 申請(qǐng)日期2012年11月23日 優(yōu)先權(quán)日2012年11月23日
發(fā)明者劉健, 于闖, 林揚(yáng), 賈松力, 劉愛民, 劉鐵軍 申請(qǐng)人:中國(guó)科學(xué)院沈陽自動(dòng)化研究所