本發(fā)明涉及水下機(jī)器人技術(shù)領(lǐng)域，尤其涉及一種可旋轉(zhuǎn)舵推進(jìn)器的水下機(jī)器人(簡稱AUV)控制方法，實(shí)現(xiàn)對AUV的五自由度(不包含橫滾控制)運(yùn)動(dòng)控制。
背景技術(shù)：
在海洋應(yīng)用中，水下機(jī)器人發(fā)揮越來越重要的作用。水下機(jī)器人分成兩類：一種是遙控式有纜水下機(jī)器人(簡稱ROV)，一種是無人自治水下機(jī)器人(簡稱AUV)。ROV需要水面母船支持，同時(shí)受到電纜長度的限制，其作業(yè)距離有限，一般只有幾百米；而AUV自身攜帶能源，可以遠(yuǎn)離母船，活動(dòng)距離達(dá)到幾十公里甚至上百公里。所以AUV的研究越來越受到各國的重視，AUV的發(fā)展代表了未來水下機(jī)器人的發(fā)展方向。但是AUV的控制方法比ROV的控制方法復(fù)雜，只有設(shè)計(jì)出好的控制方法才能發(fā)揮出AUV強(qiáng)大的作業(yè)能力。傳統(tǒng)AUV控制方法主要采用經(jīng)典線性控制理論進(jìn)行控制和對執(zhí)行結(jié)構(gòu)進(jìn)行控制力分配，這種方法最大的好處是算法簡單，但是AUV的水動(dòng)力性能是非線性和時(shí)變的，水動(dòng)力試驗(yàn)也很難獲得精確的水動(dòng)力系數(shù)，再加上AUV負(fù)載變化引起重心和浮心的改變，導(dǎo)致當(dāng)AUV的外界條件發(fā)生改變時(shí)，經(jīng)典線性控制理論的控制方法控制性能急劇下降。本發(fā)明采用經(jīng)典控制理論與變結(jié)構(gòu)控制相結(jié)合的方法，對于水動(dòng)力參數(shù)變化的不敏感性和控制抗干擾性優(yōu)于傳統(tǒng)的控制方法，更加適合AUV這種非線性時(shí)變模型系統(tǒng)。
技術(shù)實(shí)現(xiàn)要素：
為了克服傳統(tǒng)線性控制方法在對于水動(dòng)力系數(shù)敏感性較強(qiáng)和抗干擾性較弱的問題，本發(fā)明要解決的技術(shù)問題提供一種可旋轉(zhuǎn)舵推進(jìn)器的水下機(jī)器人控制方法，降低控制系統(tǒng)對水動(dòng)力參數(shù)敏感性和提高系統(tǒng)的控制抗干擾性。
本發(fā)明為實(shí)現(xiàn)上述目的所采用的技術(shù)方案是：一種可旋轉(zhuǎn)舵推進(jìn)器的水下機(jī)器人控制方法，包括速度控制、航向控制和垂直面控制；
所述速度控制包括速度開環(huán)控制和速度閉環(huán)控制，所述速度開環(huán)控制不考慮速度反饋直接輸出控制推進(jìn)器軸向推力；所述速度閉環(huán)控制將速度反饋引入到航速控制器中；
所述航向控制為變結(jié)構(gòu)航向控制；
所述垂直面控制分為強(qiáng)機(jī)動(dòng)控制和弱機(jī)動(dòng)控制，在水下機(jī)器人與目標(biāo)位置的距離大于設(shè)定值時(shí)，采用強(qiáng)機(jī)動(dòng)控制；否則采用弱機(jī)動(dòng)控制。
所述速度開環(huán)控制的控制方法如下：
TX＝-X|u|u|ud|ud
其中，Tx是推進(jìn)器的前向推力，是控制輸出量；ud是AUV的目標(biāo)速度，是控制輸入量；X|u|u是AUV與前向速度相關(guān)的水動(dòng)力系數(shù)，是已知參數(shù)。
所述速度閉環(huán)控制的控制方法如下：
其中，Tx是推進(jìn)器的前向推力，是控制輸出量；ud是AUV的目標(biāo)速度，u是AUV的反饋速度，ud和u都是控制輸入量；X|u|u是AUV與前向速度相關(guān)的水動(dòng)力系數(shù)，是AUV的慣性質(zhì)量，X|u|u和都是已知參數(shù)，Kp和Ki是需要整定的控制參數(shù)，通過線性控制理論的參數(shù)整定方法來計(jì)算。
所述變結(jié)構(gòu)航向控制的控制方法如下：
τN＝Kp2(ψ-ψd)+Kd2(r-rd)+ηtanh(σ/0.2)
其中，τN是AUV的水平面轉(zhuǎn)艏力矩，是航向控制的控制輸出變量；σ是滑膜面，是航向控制的中間變量；(ψ-ψd)是當(dāng)前航向角誤差，(r-rd)是當(dāng)前航向角誤差的微分，它們是控制輸入量；Kp2是航向角誤差系數(shù)，Kd1是航向角誤差微分系數(shù)，η是滑膜系數(shù)，它們可以通過試驗(yàn)測定，是已知參數(shù)。
所述弱機(jī)動(dòng)控制的控制方法如下：
τM＝Kdp(d-dd)+Kpθ(θ-θd)+Kdθq+BGzWsinθ
其中，τM是垂直面的縱傾力矩，是控制輸出量；d是AUV的當(dāng)前深度值， dd是AUV的當(dāng)前目標(biāo)深度值，θ是AUV的縱傾角，θd是AUV當(dāng)前的目標(biāo)縱傾角，它們是AUV的狀態(tài)量，通過AUV的深度傳感器和姿態(tài)傳感器可以測量和計(jì)算；(d-dd)是AUV的深度誤差，(θ-θd)是AUV的縱傾角誤差，q是AUV的垂直角速率，它們是控制輸入量；Kdp是AUV的深度誤差系數(shù)，Kpθ是AUV的縱傾角誤差系數(shù)，Kdθ是AUV的垂直角速率系數(shù)，BGzWsinθ是AUV的重力/浮力垂直靜力矩，它們可以通過試驗(yàn)測定，是已知參數(shù)。
所述強(qiáng)機(jī)動(dòng)控制的控制方法如下：
τM＝Kpθ2(θ-θd)+Kdθ2q+BGzWsinθ
其中，τM是垂直面的縱傾力矩，是控制輸出量；θ是AUV的當(dāng)前縱傾角，θd是AUV當(dāng)前的目標(biāo)縱傾角，它們是AUV的狀態(tài)量，通過AUV的姿態(tài)傳感器可以測量和計(jì)算；(θ-θd)是AUV的縱傾角誤差，q是AUV的垂直角速率，它們是控制輸入量；Kpθ2是AUV的縱傾角誤差系數(shù)，Kdθ2是AUV的垂直角速率系數(shù)，BGzWsinθ是AUV的重力/浮力垂直靜力矩，它們可以通過試驗(yàn)測定，是已知參數(shù)。
本發(fā)明具有以下有益效果及優(yōu)點(diǎn)
1.對于水動(dòng)力系數(shù)不敏感。相比傳統(tǒng)的控制方法，本方法對于水動(dòng)力系數(shù)不敏感，能夠在水動(dòng)力模型不準(zhǔn)確的情況下使用。
2.能夠更好地適應(yīng)外界環(huán)境的改變。當(dāng)外界環(huán)境發(fā)生改變時(shí)，例如AUV負(fù)載的變化和外界海流的變化，傳統(tǒng)的控制方法會偏離最佳工作狀態(tài)，降低控制系統(tǒng)性能；本方法具有更好的魯棒性，更好地適應(yīng)外界環(huán)境的改變。
3.應(yīng)用范圍廣。本發(fā)明不但可以應(yīng)用于AUV，還可以用于各種機(jī)器人的運(yùn)動(dòng)控制。
附圖說明
圖1是本發(fā)明的組成示意圖。
具體實(shí)施方式
下面結(jié)合附圖及實(shí)施例對本發(fā)明做進(jìn)一步的詳細(xì)說明。
本發(fā)明的硬件要求是一臺AUV，AUV的艏部配置2個(gè)水平舵，在艏部水平舵上裝有2個(gè)推進(jìn)電機(jī)；艉部配置2個(gè)水平舵，在艉部水平舵上裝有2個(gè)推進(jìn)電機(jī)；在艉部配置一個(gè)垂直舵，如圖1所示。
本發(fā)明的AUV運(yùn)動(dòng)控制方法包括以下幾個(gè)內(nèi)容：
速度控制的作用是保持AUV以指定的前向速度航行。速度控制包括兩種，一種是速度開環(huán)控制，是不考慮速度反饋直接輸出控制推進(jìn)器軸向推力的控制；另一種是速度閉環(huán)控制，是將速度反饋引入到航速控制器設(shè)計(jì)中。速度控制線性性比較高，所以采用經(jīng)典線性控制方法。
速度開環(huán)控制的控制方法如下：
TX＝-X|u|u|ud|ud
其中，Tx是推進(jìn)器的前向推力，是控制輸出量；ud是AUV的目標(biāo)速度，是控制輸入量；X|u|u是AUV與前向速度相關(guān)的水動(dòng)力系數(shù)，是已知參數(shù)。
速度閉環(huán)控制的控制方法如下：
其中，Tx是推進(jìn)器的前向推力，是控制輸出量；ud是AUV的目標(biāo)速度，u是AUV的反饋速度，ud和u都是控制輸入量；X|u|u是AUV與前向速度相關(guān)的水動(dòng)力系數(shù)，是AUV的慣性質(zhì)量，X|u|u和都是已知參數(shù)。Kp和Ki是需要整定的控制參數(shù)，可以通過線性控制理論的參數(shù)整定方法來計(jì)算。
航向控制的作用是保持AUV在指定的航向上沿直線穩(wěn)定航行。在要求AUV進(jìn)行水平面機(jī)動(dòng)時(shí)，航向控制能夠及時(shí)改變AUV的航向，轉(zhuǎn)到指定的目標(biāo)航向航行。傳統(tǒng)航向控制的線性控制方法對于水動(dòng)力參數(shù)敏感，當(dāng)外界環(huán)境發(fā)生改變時(shí)，線性控制方法的航向控制就偏離最佳工作狀態(tài)，增加操舵次數(shù)和推進(jìn)器轉(zhuǎn)速變更次數(shù)，造成舵機(jī)和推進(jìn)器磨損，降低執(zhí)行機(jī)構(gòu)的使用壽命。當(dāng)線性控制方法的航向控制偏離最佳工作狀態(tài)時(shí)，還會造成周期性的航向震蕩，降低AUV 的航行品質(zhì)。所以本發(fā)明設(shè)計(jì)了變結(jié)構(gòu)航向控制。變結(jié)構(gòu)航向控制設(shè)計(jì)如下：
首先，設(shè)計(jì)航向控制的滑膜面，它的作用抑制外界改變對航向控制器的影響，提高航向控制器的魯棒性：
σ＝Ki1(ψi-ψid)+Kp1(ψ-ψd)+Kd1(r-rd)
其中σ是滑膜面，是航向控制的中間變量；ψi是當(dāng)前航向角誤差的積分，ψid是當(dāng)前目標(biāo)航向角的積分，ψ是當(dāng)前航向角，ψd是當(dāng)前目標(biāo)航向角，r是當(dāng)前航向角的微分，rd是目標(biāo)航向角微分，它們是AUV的狀態(tài)量，通過AUV的姿態(tài)傳感器可以測量和計(jì)算；(ψi-ψid)是航向角誤差的積分，(ψ-ψd)是當(dāng)前航向角誤差，(r-rd)是當(dāng)前航向角誤差的微分，它們是控制輸入量；Ki1是航向角誤差積分系數(shù)，Kp1是航向角誤差系數(shù)，Kd1是航向角誤差微分系數(shù)，它們可以通過水池試驗(yàn)測定，是已知參數(shù)。
下一步，根據(jù)滑膜面設(shè)計(jì)的變結(jié)構(gòu)航向控制：
τN＝Kp2(ψ-ψd)+Kd2(r-rd)+ηtanh(σ/0.2)
其中τN是AUV的水平面轉(zhuǎn)艏力矩，是航向控制的控制輸出變量；σ是滑膜面，是航向控制的中間變量；(ψ-ψd)是當(dāng)前航向角誤差，(r-rd)是當(dāng)前航向角誤差的微分，它們是控制輸入量；Kp2是航向角誤差系數(shù)，Kd1是航向角誤差微分系數(shù)，η是滑膜系數(shù)，它們可以通過試驗(yàn)測定，是已知參數(shù)。
垂直面控制的作用是保持AUV在指定的深度下穩(wěn)定航行和在垂直面內(nèi)進(jìn)行深度機(jī)動(dòng)操縱。表面上看，垂直面控制和水平面控制相同，實(shí)際上垂直面控制復(fù)雜度高于水平面控制，AUV在垂直面運(yùn)動(dòng)，不但像水平面運(yùn)動(dòng)受到流體動(dòng)力的作用外，還受到靜力和靜力矩的作用。垂直面控制分成強(qiáng)機(jī)動(dòng)控制和弱機(jī)動(dòng)控制，其中強(qiáng)機(jī)動(dòng)控制是指大范圍的垂直面機(jī)動(dòng)，弱機(jī)動(dòng)控制的垂直面機(jī)動(dòng)范圍比較小。
弱機(jī)動(dòng)控制設(shè)計(jì)如下：
τM＝Kdp(d-dd)+Kpθ(θ-θd)+Kdθq+BGzWsinθ
其中，τM是垂直面的縱傾力矩，是控制輸出量；d是AUV的當(dāng)前深度值，dd是AUV的當(dāng)前目標(biāo)深度值，θ是AUV的縱傾角，θd是AUV當(dāng)前的目標(biāo)縱傾角，它們是AUV的狀態(tài)量，通過AUV的深度傳感器和姿態(tài)傳感器可以測量和計(jì)算；(d-dd)是AUV的深度誤差，(θ-θd)是AUV的縱傾角誤差，q是AUV的垂直角速率，它們是控制輸入量；Kdp是AUV的深度誤差系數(shù)，Kpθ是AUV的縱傾角誤差系數(shù)，Kdθ是AUV的垂直角速率系數(shù)，BGzWsinθ是AUV的重力/浮力垂直靜力矩，它們可以通過試驗(yàn)測定，是已知參數(shù)。
強(qiáng)機(jī)動(dòng)控制設(shè)計(jì)如下：
τM＝Kpθ2(θ-θd)+Kdθ2q+BGzWsinθ
其中，τM是垂直面的縱傾力矩，是控制輸出量；θ是AUV的當(dāng)前縱傾角，θd是AUV當(dāng)前的目標(biāo)縱傾角，它們是AUV的狀態(tài)量，通過AUV的姿態(tài)傳感器可以測量和計(jì)算；(θ-θd)是AUV的縱傾角誤差，q是AUV的垂直角速率，它們是控制輸入量；Kpθ2是AUV的縱傾角誤差系數(shù)，Kdθ2是AUV的垂直角速率系數(shù)，BGzWsinθ是AUV的重力/浮力垂直靜力矩，它們可以通過試驗(yàn)測定，是已知參數(shù)。
在AUV設(shè)計(jì)完成后，首先進(jìn)行水池水動(dòng)力試驗(yàn)，獲得AUV的水動(dòng)力系數(shù)。然后按照本發(fā)明方法設(shè)計(jì)AUV的速度、水平面和垂直面控制器。最后，將使用本發(fā)明方法的AUV進(jìn)行航行試驗(yàn)，驗(yàn)證控制方法的正確性。