專利名稱：浮游式水下機器人運動的s面控制方法
技術(shù)領(lǐng)域：
本發(fā)明涉及浮游式水下機器人運動的S面控制方法。
背景技術(shù)：
浮游式水下機器人的動力學(xué)系統(tǒng)是非線性的、時變的。各個自由度的運動相互耦合，海流等環(huán)境干擾力的影響較大?？傊?，水下機器人是一個強非線性系統(tǒng)。而且考慮到運動的時變性、環(huán)境的復(fù)雜性和不確定性，建立精確的水下機器人運動模型是不現(xiàn)實的。所以，水下機器人控制系統(tǒng)的設(shè)計必須考慮非線性問題和運動模型的復(fù)雜性。同時，從加強機器人的自主性與適應(yīng)性，改善機器人工作性能的觀點來看，控制系統(tǒng)還應(yīng)該具備良好的自適應(yīng)與自學(xué)能力。水下機器人的運動控制器通常有PID控制器，神經(jīng)網(wǎng)絡(luò)控制器和模糊邏輯控制器三種，由于水下機器人運動模型的復(fù)雜性，因此更多的研究人員把注意力和精力放在現(xiàn)代控制論的模糊邏輯和神經(jīng)網(wǎng)絡(luò)控制技術(shù)上。
美國的佛羅里達(dá)大西洋大學(xué)成功地將模糊邏輯控制應(yīng)用于智能水下機器人”O(jiān)cean Explor”上，并據(jù)此完成了一系列海上試驗。模糊控制器的設(shè)計比較簡單實用。然而，模糊控制屬于主觀性很強的規(guī)則控制。對于水下機器人，人們不太可能得到較好的操縱經(jīng)驗，因此其隸屬度函數(shù)的設(shè)定、推理方法和反模糊化方法以及模糊規(guī)則的獲取存在相當(dāng)?shù)碾y度。而這些參數(shù)設(shè)計的好與壞，又將直接影響到控制的效果。所以，參數(shù)調(diào)整的復(fù)雜度制約了模糊控制技術(shù)在水下機器人運動控制中的廣泛應(yīng)用。
神經(jīng)網(wǎng)絡(luò)控制的優(yōu)點是充分考慮到了水下機器人的強非線性和各個自由度之間的耦合，同時其學(xué)習(xí)機制能夠跟蹤系統(tǒng)自身或外圍環(huán)境的緩慢變化。1996年，哈爾濱工程大學(xué)海洋綜合技術(shù)研究中心采用神經(jīng)網(wǎng)絡(luò)控制，ZS-II型水下機器人分別在水池和湖中進(jìn)行了試驗。但其缺點是當(dāng)環(huán)境變化比較劇烈，如在波浪中或是漲潮退潮時的港灣中(存在較大的環(huán)流)，外界干擾的幅度和周期與水下機器人自身的運動幅度和周期相近時，神經(jīng)網(wǎng)絡(luò)的學(xué)習(xí)就出現(xiàn)明顯的滯后現(xiàn)象，控制容易發(fā)生振蕩。
因此，研制出一種能有效適用于水下機器人復(fù)雜的強非線性系統(tǒng)，參數(shù)數(shù)目少，結(jié)構(gòu)簡單，便于調(diào)節(jié)的控制方法，是水下機器人運動控制的關(guān)鍵性技術(shù)問題。而本申請所述的S面控制方法體現(xiàn)了上述優(yōu)點，適用于水下機器人的運動控制。
目前已經(jīng)受專利保護(hù)或已提出申請的水下機器人控制方法只涉及到柔性機器魚的控制。但這些運動控制方法都是針對機器魚柔性尾鰭的控制。與本申請所述的剛體浮游式水下機器人的運動控制方法有極大不同。如中國專利局在2004年3月24日已公開的發(fā)明專利(申請?zhí)?2142836.0)“仿生機器魚游動方向的模糊控制方法”。

發(fā)明內(nèi)容
本發(fā)明的目的是提供一種適合于浮游式水下機器人運動的S面控制方法?？梢杂行У剡m應(yīng)水下機器人強非線性動力學(xué)特性和復(fù)雜的運動關(guān)系，實現(xiàn)精確的運動控制。
為實現(xiàn)上述目的，浮游式水下機器人運動的S面控制方法步驟為基于短基線和深度計獲取位置信息，基于多普勒速度計獲取速度信息，基于羅經(jīng)獲取姿態(tài)角信息，通過控制器解算，由螺旋槳和舵、翼執(zhí)行運動。
小范圍定位時，通過六自由度位置偏差和偏差變化率(速度)作為輸入的位置控制及精確動力定位。
大范圍航渡時，通過六自由度速度偏差和偏差變化率(加速度)作為輸入的速度控制。
通過PD控制的指數(shù)化實現(xiàn)對水下機器人的非線性控制。
本發(fā)明可有效提高水下機器人運動控制的精度，實際應(yīng)用于水下機器人控制系統(tǒng)設(shè)計，進(jìn)行水下探測，考古，水下救撈，水下設(shè)施的維護(hù)和海洋領(lǐng)土的防御等。


圖1浮游式水下機器人運動的S面控制方法輸入輸出關(guān)系曲面2浮游式水下機器人運動的S面控制方法解算流程3WXX-I型水下機器人S面控制器精確動力定位響應(yīng)曲線圖具體實施方式
浮游式水下機器人運動控制工作包括當(dāng)前位置姿態(tài)的獲取，控制器的解算，執(zhí)行機構(gòu)的力學(xué)輸出。在本發(fā)明中，我們基于短基線和深度計獲取位置信息，基于多普勒速度計獲取速度信息，基于羅經(jīng)獲取姿態(tài)角信息，通過控制器解算，由螺旋槳和舵、翼執(zhí)行運動。
短基線能提供水下平面坐標(biāo)系中相對于基陣的坐標(biāo)x，y；深度計提供深度信息，即坐標(biāo)z；羅經(jīng)提供航態(tài)角(艏向角，縱傾角和橫搖角)。多普勒速度計提供速度信息，根據(jù)目標(biāo)與當(dāng)前的偏差和偏差變化律(若為位置控制，偏差為位置差，偏差變化律為速度；若為速度控制，偏差為速度差，偏差變化律為加速度)決定控制器的輸入。通過運動控制器解算，得到系統(tǒng)輸出各推力器的應(yīng)輸出推力。進(jìn)而決定施力裝置的電壓輸出和脈沖輸出。
S面控制方法是控制器的解算方法。輸入為偏差和偏差變化律，輸出為各施力裝置的推力。
小范圍定位時，通過六自由度位置偏差和偏差變化率(速度)作為輸入的位置控制及精確動力定位。
大范圍航渡時，通過六自由度速度偏差和偏差變化率(加速度)作為輸入的速度控制。
從常規(guī)模糊控制器的控制規(guī)則(表1)可以看出，控制輸出的變化是有規(guī)律可循的。取主對角線數(shù)值連成折線，可以用一條光滑曲線(是一個Sigmoid函數(shù))來擬合，事實上，光滑曲線可以看作無數(shù)條長度趨向于零的折線相連。我們在設(shè)計模糊控制器的時候，通常采用的是兩邊疏松，中間密的形式(即偏差大時采用控制比較粗，而偏差小時控制比較細(xì))，這一點與Sigmoid函數(shù)的變化形式是一致的，因此，Sigmoid函數(shù)在一定程度上體現(xiàn)了模糊控制的思想。同理，整個模糊控制規(guī)則庫對應(yīng)的折線面，也就可以用光滑曲線的曲線面代替。
表1常規(guī)模糊控制器的控制規(guī)則

一般地，Sigmoid曲線函數(shù)為y＝2.0/(1.0+e-kx)-1.0 (1)那么，Sigmoid曲面函數(shù)為z=2.0/(1.0+e(-k1x-k2y))-1.0---(2)]]>因此，我們設(shè)計的S面控制器的控制模型為
其中，e和&為控制的輸入信息(偏差和偏差變化率，通過歸一化處理)，u為控制輸出，k1和k2分別為對應(yīng)偏差和偏差變化率的控制參數(shù)，可以改變其對應(yīng)自由度的變化速度。如附圖1(浮游式水下機器人運動的S面控制方法輸入輸出關(guān)系曲面圖)所示。在這里控制器需要調(diào)整的參數(shù)只有k1和k2。
人工調(diào)整控制器參數(shù)k1和k2，使得在一般情況下水下機器人的運動控制滿足要求。k1和k2取得越大，響應(yīng)對小偏差的敏感性越高，但取得太大容易引起振蕩。如果超調(diào)大了，可以適當(dāng)減小k1而增加k2，反之，如果收斂速度慢了，則可以適當(dāng)增加k1而減小k2。通過調(diào)整k1和k2，調(diào)整偏差和偏差變化率在控制輸入中所占的比例，最終達(dá)到一個近最優(yōu)的結(jié)果。
對于海流、未知擾動等因素，可以考慮成一段時間內(nèi)的固定干擾力，也就是說在目標(biāo)點附近，可能會存在一個固定偏差，我們可以通過增加積分項，調(diào)整S面的偏移，來達(dá)到消除固定偏差的目的。為此，我們對S面方法進(jìn)行改進(jìn)，增加積分項。采用如附圖2控制模型。
控制器輸出為u(t)=Σi=13ki(t)xi(t)---(5)]]>u0(t)=f(u)=1-e-u(t)1+e-u(t)---(6)]]>ug(t)＝Umaxu0(t) (7)式中
ki——控制器參數(shù)Umax——最大控制量。
其中積分項x3的計算我們采用自適應(yīng)的方式，自適應(yīng)方式如下①判斷機器人的運動速度是否小于一個設(shè)定的閾值，若是，轉(zhuǎn)②，若不是，轉(zhuǎn)③；②將該自由度的偏差值賦予一個設(shè)定的數(shù)組，同時將設(shè)定的計數(shù)值加1，當(dāng)該計數(shù)值達(dá)到一個預(yù)定的定值時，轉(zhuǎn)④；③將數(shù)組的值往前竄一位，同時計數(shù)值減1，轉(zhuǎn)①；④對這一數(shù)組的數(shù)值加權(quán)平均，得到的平均偏差值用于計算控制輸出的偏移量，自適應(yīng)調(diào)整控制器的輸出，以消除固定偏差，同時將計數(shù)值和數(shù)組歸零，進(jìn)行下一個循環(huán)。
如此就構(gòu)造了一個簡單、實用的水下機器人控制器，在復(fù)雜的海洋環(huán)境下該控制器控制效果可以滿足作業(yè)要求。但此時S面控制器的參數(shù)調(diào)整完全是手工的，我們可以尋找一種自學(xué)習(xí)的方法，讓控制器通過自學(xué)習(xí)在線的調(diào)整參數(shù)，使控制器有更高的精度，達(dá)到更好的控制效果。
為了給S面控制增加自學(xué)習(xí)的功能，我們可以借鑒神經(jīng)網(wǎng)絡(luò)的誤差反傳學(xué)習(xí)算法，定義一個合適的輸出誤差函數(shù)，通過學(xué)習(xí)算法修改控制器參數(shù)，使控制器的輸出誤差最小。因為水下機器人的運動具有自己的運動意識或則說它能反映人的意識，所以水下機器人有一個期望的運動狀態(tài)，對控制而言，也就有一個期望的輸出。因此我們首先定義輸出誤差函數(shù)為Ep=12(ud-u)2---(8)]]>式中，ud是由輸出規(guī)劃器得到的期望輸出，u是前一時刻輸出。
通過梯度下降法最優(yōu)化方法，可用Ep的梯度來修正系數(shù)k1和k2。
Δki=-η∂Ep∂ki---(9)]]>式中η是學(xué)習(xí)率(0＜η＜1.0)∂Ep∂ki=-(ud-u)∂u∂ki]]> 所以系數(shù)k1和k2按以下公式來修正ki(t+1)＝ki(t)+Δki 這樣就使得S面控制方法具有了自學(xué)習(xí)的能力，可以通過自學(xué)習(xí)自動調(diào)整參數(shù)。
具體實施例方式
本發(fā)明提供的浮游式水下機器人運動的S面控制方法應(yīng)用于哈爾濱工程大學(xué)海洋綜合技術(shù)研究中心研制的WXX-I型水下機器人。該機器人是為探索智能水下機器人小型化和集群化而研制的。其B/L大，回轉(zhuǎn)性好，而航行穩(wěn)定性差，精確運動控制難度較大。
控制流的過程為短基線系統(tǒng)提供大地坐標(biāo)系中當(dāng)前的x，y值，深度計提供z值，磁羅經(jīng)提供當(dāng)前姿態(tài)角度(艏向角，縱傾角，橫搖角)信息。規(guī)劃系統(tǒng)(軟件的一部分)決定目標(biāo)位姿的x，y，z，艏向角，縱傾角，橫搖角。從而得到六自由度控制偏差，差分得偏差變化律(速度)。運用S面控制方法解算六自由度輸出力大小，進(jìn)行推力分配得到施力裝置應(yīng)施加的力。通過光纖傳至水下控制機。通過相應(yīng)電路接口將力數(shù)據(jù)傳至螺旋槳和翼的控制電機，產(chǎn)生控制力，驅(qū)動WXX-I完成精確運動。
上述方案作為實施例1，其配比如下


表4自學(xué)習(xí)后的ke，kv值以位置控制為例，S面控制方法經(jīng)過參數(shù)整定后，試驗得到如附圖3的響應(yīng)曲線?？梢钥闯鲰憫?yīng)較快，且超調(diào)很小。能滿足工程實際的需要。
本發(fā)明的優(yōu)點是適用于浮游式水下機器人復(fù)雜的強非線性系統(tǒng)，參數(shù)數(shù)目少，結(jié)構(gòu)簡單，便于調(diào)節(jié)，控制精度高?？梢詰?yīng)用于通用的浮游式水下機器人控制系統(tǒng)設(shè)計，進(jìn)行水下探測，考古，水下救撈，水下設(shè)施的維護(hù)和海洋領(lǐng)土的防御等。
權(quán)利要求
1.一種浮游式水下機器人運動的S面控制方法，其特征在于它包括步驟基于短基線和深度計獲取位置信息，基于羅經(jīng)獲取姿態(tài)角信息，基于多普勒速度計獲取速度信息，通過控制器解算，由螺旋槳和舵、翼執(zhí)行運動；小范圍定位時，通過六自由度位置偏差和偏差變化率作為輸入的位置控制及精確動力定位；大范圍航渡時，通過六自由度速度偏差和偏差變化率作為輸入的速度控制。
2.如權(quán)利要求1所述的浮游式水下機器人運動的S面控制方法，其特征在于其控制器通過PD控制的指數(shù)化實現(xiàn)對水下機器人的非線性控制。
全文摘要
本發(fā)明浮游式水下機器人運動的S面控制方法，包括步驟基于短基線和深度計獲取位置信息，基于羅經(jīng)獲取姿態(tài)角信息，基于多普勒速度計獲取速度信息，通過控制器解算，通過螺旋槳和舵、翼執(zhí)行運動。小范圍定位時，通過六自由度位置偏差和偏差變化率作為輸入的位置控制及精確動力定位。大范圍航渡時，通過六自由度速度偏差和偏差變化率作為輸入的速度控制。通過PD控制的指數(shù)化實現(xiàn)對水下機器人的非線性控制。本發(fā)明適用于浮游式水下機器人復(fù)雜的強非線性系統(tǒng)，參數(shù)數(shù)目少，便于調(diào)節(jié)，控制精度高?？蓪嶋H應(yīng)用于浮游式水下機器人控制系統(tǒng)設(shè)計，進(jìn)行水下探測，考古，水下救撈，水下設(shè)施的維護(hù)和海洋領(lǐng)土的防御等。
文檔編號B25J13/00GK1718378SQ20051001011
公開日2006年1月11日 申請日期2005年6月24日 優(yōu)先權(quán)日2005年6月24日
發(fā)明者李曄, 萬磊, 蘇玉民, 龐永杰, 劉學(xué)敏 申請人:哈爾濱工程大學(xué)