基于自抗擾的自主水下航行器地形跟蹤控制

萬磊，張英浩，孫玉山，李岳明

(哈爾濱工程大學(xué) 水下機(jī)器人技術(shù)重點(diǎn)實(shí)驗(yàn)室，黑龍江哈爾濱150001)

0 引言

自主水下航行器(AUV)要完成海底管線跟蹤和海床地質(zhì)調(diào)查等任務(wù)，前提是需要具有精確的地形跟蹤能力。所謂地形跟蹤是指，AUV 以合適的方式下潛到作業(yè)水深之后，與水底保持一定高度，并按照一定的航線航行［1］。隨著AUV 工作任務(wù)的日益復(fù)雜化，人們對(duì)于AUV 在未知環(huán)境中的控制精度，以及抵抗外界干擾的要求也在不斷提高。

當(dāng)前AUV 的地形跟蹤難點(diǎn)在于:1)海底崎嶇不平，地形變化劇烈，導(dǎo)致跟蹤困難;2)水下機(jī)器人垂向運(yùn)動(dòng)的機(jī)動(dòng)能力有限;3)聲學(xué)傳感器對(duì)高度測(cè)量結(jié)果噪聲大［1］。針對(duì)以上難點(diǎn)，國(guó)內(nèi)外的相關(guān)學(xué)者進(jìn)行了大量的研究，并取得了一定的成果［1-6］。Smith 等［2］將模糊控制方法引入運(yùn)動(dòng)控制器的設(shè)計(jì)，從而提高AUV 在跟蹤任務(wù)時(shí)的魯棒性。Jose 等［3］對(duì)MARES AUV 上安裝的傳感器數(shù)據(jù)進(jìn)行適當(dāng)處理，并結(jié)合設(shè)計(jì)的控制策略生成AUV 的跟蹤控制指令。段海慶等［4］則利用神經(jīng)網(wǎng)絡(luò)控制方法，來降低AUV 在跟蹤過程中不確定擾動(dòng)對(duì)精度的影響。李岳明等［1］利用Takagi-Sugeno 推理方法對(duì)高度計(jì)和DVL 兩種傳感器的高度信息進(jìn)行信息融合，從而提高高度信息的感知能力，以保障AUV 可準(zhǔn)確進(jìn)行地形跟蹤。Carlos 等［5］利用水聲設(shè)備測(cè)量地形時(shí)的聲學(xué)特征，來對(duì)AUV 進(jìn)行指令下達(dá)，并進(jìn)行了相關(guān)仿真測(cè)試。

本文將重點(diǎn)放在AUV 的運(yùn)動(dòng)控制上，主要設(shè)計(jì)了具有保護(hù)功能的控制策略以及能夠抵抗環(huán)境干擾的控制器，并與PID 控制方法進(jìn)行對(duì)比實(shí)驗(yàn)，完成地形跟蹤所需的全過程三維運(yùn)動(dòng)仿真。

1 控制對(duì)象及運(yùn)動(dòng)建模

1.1 控制對(duì)象及坐標(biāo)系

本文的控制對(duì)象是由哈爾濱工程大學(xué)水下機(jī)器人技術(shù)重點(diǎn)實(shí)驗(yàn)室設(shè)計(jì)的某型AUV，其主體大致成細(xì)長(zhǎng)回轉(zhuǎn)體，艇體艏部和艉部各裝有垂向推力器一套，艉部裝有舵、翼和主推等各一套。為研究問題和后文敘述方便，分別建立大地坐標(biāo)系Eξηζ 和艇體坐標(biāo)系Oxyz，如圖1所示。Eξηζ 是慣性坐標(biāo)系，而Oxyz 屬于非慣性坐標(biāo)系，并且Oxyz 的原點(diǎn)O 與AUV 的重心G 重合。

圖1 AUV 主體及所建立坐標(biāo)系Fig.1 AUV and coordinates

為清楚表述縱傾角θ，圖1顯示的是坐標(biāo)軸Eη和坐標(biāo)軸Oy 平行的情況，并標(biāo)注出θ 的正方向。

1.2 動(dòng)力學(xué)及運(yùn)動(dòng)學(xué)方程

由文獻(xiàn)［7］可知，當(dāng)AUV 在垂直面內(nèi)運(yùn)動(dòng)時(shí)，其動(dòng)力學(xué)及運(yùn)動(dòng)學(xué)方程可分別表示為(1)式和(2)式。

式中:M 為慣性質(zhì)量矩陣，鑒于所控制AUV 外形在垂向?qū)ΨQ性，因而可表示為

2 垂直面地形跟蹤策略

2.1 “高度－深度”轉(zhuǎn)化

AUV 在進(jìn)行地形跟蹤控制時(shí)，一般在水平面會(huì)沿指定路徑運(yùn)動(dòng)，而在垂直面會(huì)以與水底呈一定高度的方式運(yùn)動(dòng)。其會(huì)通過“高度計(jì)”和“深度計(jì)”等傳感器分別獲得當(dāng)前位置距跟蹤地形的高度信息和當(dāng)前深度信息，以此為地形跟蹤的依據(jù)。

圖2所示為“高度”與“深度”之間的信息關(guān)系。假使AUV 處于位置P，如需對(duì)指定地形進(jìn)行跟蹤，當(dāng)前距跟蹤地形高度為hP，所在位置深度為ζP，若期望跟蹤高度為h0，則“高度-深度”之間的轉(zhuǎn)化關(guān)系，可表述為(3)式。

式中:ζd為期望深度，也就是AUV 的期望控制深度。

圖2 “高度-深度”關(guān)系圖Fig.2 Relation graph of altitude and depth

2.2 “深度－縱傾”轉(zhuǎn)化

本文所研究的AUV 雖然布置有垂向推進(jìn)器，但是當(dāng)AUV 縱向速度或外界流較大時(shí)，垂向推進(jìn)器的效率會(huì)大幅減少。因而，在進(jìn)行AUV 的深度控制時(shí)，需考慮“深度”與“縱傾”之間的轉(zhuǎn)化關(guān)系。

因而，設(shè)計(jì)“深度-縱傾”的轉(zhuǎn)化關(guān)系，可表述為(4)式。

式中:θd為期望縱傾角;ζe為期望深度ζd與當(dāng)前深度ζP之差;A 為可調(diào)參數(shù)，與AUV 的運(yùn)動(dòng)性能呈正相關(guān)，且A ＞0.

這種方式可能會(huì)出現(xiàn)解算出的θd過大的情況，因而，為避免AUV 運(yùn)動(dòng)過程中危險(xiǎn)姿態(tài)的出現(xiàn)，保證其安全，進(jìn)而建立“期望縱傾角保護(hù)”機(jī)制，即為(4)式解算出的期望縱傾角θd設(shè)定“安全閾值”。當(dāng)解算出的θd超過設(shè)定閾值，則按安全閾值進(jìn)行控制輸入。因而，整個(gè)垂直面內(nèi)的地形跟蹤控制策略，可由圖3表示。

圖3 AUV 垂直面地形跟蹤策略Fig.3 AUV bottom-following strategy on vertical plane

3 AUV 跟蹤控制器的建立

3.1 自抗擾控制器

自抗擾控制(ADRC)是由中國(guó)科學(xué)院系統(tǒng)科學(xué)院研究所韓京清研究員提出的一種控制方法［8］，其繼承了傳統(tǒng)PID“基于誤差，消除誤差”的思想。主要由跟蹤微分控制器(TD)、擴(kuò)張狀態(tài)觀測(cè)器(ESO)、誤差的非線性反饋(NLSEF)以及擾動(dòng)估計(jì)補(bǔ)償?shù)炔糠纸M成。若以2 階ADRC 控制器建立AUV 水平舵控制器，則其信息流框架如圖4所示。

圖4 水平舵ADRC 控制器信息流框架Fig.4 Information frame of wing’s controller based on ADRC

圖4中的θd和θ 分別為期望縱傾角和AUV 當(dāng)前縱傾角，其他變量為ADRC 控制器計(jì)算所需的中間變量。本文根據(jù)控制需求，將圖4中各部分算法設(shè)計(jì)如下:

TD:

式中:r 為快速因子;b0為濾波因子。

ESO:

式中:β01、β02、β03為增益系數(shù);δ 為可調(diào)參數(shù)。

NLSEF:

式中:e1為縱傾角偏差;e2為縱傾角偏差變化率。

擾動(dòng)估計(jì)補(bǔ)償:

式中:b 為可調(diào)參數(shù)。

(5)式～(7)式中:h 為積分步長(zhǎng);fhan(·)和fal(·)的定義如文獻(xiàn)［9］中所述。

通過ADRC，可以將縱傾角的控制問題，轉(zhuǎn)化為縱傾角偏差e1和縱傾角偏差變化率e2的鎮(zhèn)定問題。主推ADRC 控制器和垂直舵ADRC 控制器的設(shè)計(jì)與水平舵的類似。

3.2 地形跟蹤控制器

AUV 在進(jìn)行完整的地形跟蹤的任務(wù)時(shí)，一般會(huì)按照“下潛-巡航-跟蹤”的模式進(jìn)行?！跋聺摗钡倪^程又可分為“自主下潛”和“非自主下潛”。所謂“自主下潛”是指AUV 依靠自身的運(yùn)動(dòng)執(zhí)行器完成下潛任務(wù)，而“非自主下潛”是指AUV 依靠“自身重力與自身浮力”之間的差值進(jìn)行下潛，如拋載塊、油囊等［10］。“自主下潛”與“非自主下潛”各有優(yōu)劣，“自主下潛”對(duì)于未知環(huán)境的突然變化具有良好的應(yīng)對(duì)能力，而“非自主下潛”則在能源節(jié)省方面具有明顯優(yōu)勢(shì)。本文主要研究通過“自主下潛”的方式完成“下潛”過程。

對(duì)于淺水情況，AUV 可“直接”潛到指定深度。對(duì)于深水情況，AUV 一般采用“螺旋”下潛的方式。圖5顯示，在無外界干擾情況下，航速4 kn 時(shí)，利用(4)式中的“深度-縱傾”轉(zhuǎn)化關(guān)系以及圖4中的水平舵ADRC 控制器完成的淺水區(qū)多深度定深運(yùn)動(dòng)。在此過程中，分別定深3 m、6 m 和12 m. 可見ADRC的控制參數(shù)一旦選定，就可具有較大的適用范圍，并且控制效果良好。

圖5 定深運(yùn)動(dòng)結(jié)果Fig.5 Results of depth control

在到達(dá)指定深度之后，AUV 會(huì)進(jìn)入“巡航”過程，主要是為了對(duì)周圍環(huán)境進(jìn)行信息采集，并確認(rèn)跟蹤區(qū)域?！把埠健苯Y(jié)束之后，AUV 會(huì)根據(jù)任務(wù)需求，開始按照指定路徑進(jìn)入“跟蹤”過程。AUV 的完整地形跟蹤控制器信息流框架可表示為圖6.

4 仿真實(shí)驗(yàn)與結(jié)果分析

4.1 實(shí)驗(yàn)設(shè)計(jì)

本文嘗試通過進(jìn)行AUV 完整的地形跟蹤三維仿真，來測(cè)試所設(shè)計(jì)地形跟蹤控制器的有效性。為顯示ADRC 的控制效果，將其與在工程上廣泛應(yīng)用的PID 控制器進(jìn)行對(duì)比，相關(guān)設(shè)計(jì)如下。

干擾的設(shè)計(jì)。添加Eξ 和Eζ 方向的隨機(jī)干擾，用于模擬外界環(huán)境干擾以及AUV 自身的信號(hào)傳輸干擾;添加Eη 方向的恒值干擾，用于模擬流的作用效果;添加ζE 方向的瞬時(shí)干擾，用于模擬水底暗涌或噴流所導(dǎo)致的環(huán)境突然變化。

圖6 AUV 地形跟蹤控制器信息流框架Fig.6 Information flow frame of AUV＇s bottom-following control

運(yùn)動(dòng)航跡的設(shè)計(jì)。結(jié)合參考文獻(xiàn)［11 -12］，“下潛”過程，在Eξη 面內(nèi)，設(shè)計(jì)航跡為以(0，0)為圓心，半徑為50 m 的圓;當(dāng)?shù)竭M(jìn)入“巡航”過程之后，繼續(xù)以先前的圓軌跡運(yùn)行一段時(shí)間;當(dāng)?shù)竭M(jìn)入“跟蹤”過程之后，以Eξη 面內(nèi)過(0，0)，艏向角φ=90°的直線進(jìn)行運(yùn)動(dòng)。

AUV 艇長(zhǎng)4.5 m，質(zhì)量700 kg，轉(zhuǎn)動(dòng)慣量Iy=1 430 kg·m2，部分無因次化水動(dòng)力系數(shù)如表1所示。

表1 AUV 部分無因次化水動(dòng)力系數(shù)Tab.1 Part of AUV’s dimensionless coefficients

運(yùn)動(dòng)初始AUV 已通過垂推下潛到ζ=5 m 的位置，目的是為避免環(huán)境表面風(fēng)浪的影響保證舵效，此時(shí)AUV 的艏向角φ=0°，縱傾角θ=0°，懸浮位置坐標(biāo)為(0，0，5 m). 設(shè)計(jì)AUV 以航速2 kn，地形跟蹤期望高度h0=5 m 運(yùn)動(dòng)，“下潛”、“巡航”和“跟蹤”的切換深度位于ζ=100 m 位置。

4.2 實(shí)驗(yàn)分析

調(diào)整好相關(guān)控制器參數(shù)后，假使ADRC 和PID兩種控制器跟蹤的地形相同，則得到如圖7～圖10所示結(jié)果。

圖7中的1、2 和3 過程，分別指“下潛”、“巡航”和“跟蹤”?？芍?，基于ADRC 的地形跟蹤控制器和基于傳統(tǒng)PID 的地形跟蹤控制器均能按指定航跡完成地形跟蹤任務(wù)。其中，“下潛”過程，對(duì)應(yīng)t 為0 ～400 s;“巡航”過程，對(duì)應(yīng)t 為400 ～600 s;“跟蹤”過程，對(duì)應(yīng)t 為600 ～1 000 s;瞬時(shí)干擾，出現(xiàn)于t 為800 s 時(shí)。

圖7 地形跟蹤全過程三維航跡Fig.7 Three-dimensional track of bottom-following control

圖8顯示，當(dāng)分別以ADRC 和PID 控制縱向速度時(shí)，ADRC 控制器相比于PID 控制在進(jìn)入期望值之前不會(huì)出現(xiàn)超調(diào)，并且在遭遇瞬時(shí)干擾后，ADRC控制器的波動(dòng)值要遠(yuǎn)小于PID 控制器的波動(dòng)值。

圖8 地形跟蹤全過程速度變化曲線Fig.8 Curves of velocity in bottom-following control

圖9顯示，當(dāng)AUV 由“巡航”轉(zhuǎn)為“跟蹤”時(shí)，相比PID 控制器，ADRC 可以實(shí)現(xiàn)平滑過渡，幾乎沒有超調(diào)。并且在進(jìn)入“跟蹤”過程后，面對(duì)出現(xiàn)的瞬時(shí)干擾，也有較好地抑制作用，從而保證維持在水底上方5 m 處運(yùn)動(dòng)。

圖9 地形跟蹤全過程深度變化曲線及虛線框放大曲線Fig.9 Curve of depth in bottom-following control and enlarged detail of the dashed frame

圖10 顯示，AUV 的翼角有些段數(shù)據(jù)近似于0，這是由于建模過程中未考慮扶正力矩引起的，但這并不影響對(duì)ADRC 和PID 控制指令輸出的性能對(duì)比。在t 為0 ～100 s 和t 為600 ～900 s 過程中，PID 控制的翼為達(dá)到目標(biāo)狀態(tài)會(huì)不斷修改運(yùn)動(dòng)執(zhí)行器指令，相比而言，ADRC 控制下的翼運(yùn)動(dòng)則穩(wěn)定得多。

圖10 地形跟蹤全過程翼角變化曲線Fig.10 Curves of wing’s degree in bottom-following control

圖11 顯示，AUV 處于“下潛”過程時(shí)，有一段縱傾角維持在-30°的過程，這是由于按照2.2 節(jié)中的地形跟蹤策略，將“安全閾值”設(shè)定為30°的原因。在AUV 進(jìn)入“跟蹤”過程之后，ADRC 控制器的縱傾變化一直處于一種緩變過程，而PID 控制器的縱傾變化則處于一種震顫過程，特別是在“巡航”與“跟蹤”的切換過程，以及受到瞬時(shí)干擾的作用后。PID控制效果的震顫是由于PID 控制器自身性能以及隨機(jī)干擾的作用引起的，由此顯示出ADRC 控制器的優(yōu)勢(shì)。縱傾的緩變，將有利于AUV 上傳感器的工作。

圖11 地形跟蹤全過程縱傾角變化曲線Fig.11 Curves of pitch angle in bottom-following control

5 結(jié)論

基于自抗擾的自主水下航行器地形跟蹤控制方法，結(jié)合了自抗擾控制器的優(yōu)點(diǎn)并且設(shè)有保護(hù)機(jī)制的地形跟蹤策略。與傳統(tǒng)PID 控制器的對(duì)比實(shí)驗(yàn)中，顯示所設(shè)計(jì)地形跟蹤控制方法的有效性，在“下潛-巡航-跟蹤”的整個(gè)地形跟蹤任務(wù)中，能夠?qū)Ω蓴_具有更優(yōu)的抑制作用，進(jìn)而更準(zhǔn)確地完成預(yù)定地形的跟蹤任務(wù)。

References)

［1］李岳明，萬磊，孫玉山，等. 水下機(jī)器人高度信息融合與欠驅(qū)動(dòng)地形跟蹤控制［J］. 控制理論與應(yīng)用，2013，30(1):118 -121.LI Yue-ming，WAN Lei，SUN Yu-shan，et al. Altitude information fusion and bottom-following control for underacutated autonomous underwater vehicle［J］. Control Theory ＆ Appllications，2013，30(1):118 -121. (in Chinese)

［2］Smith S M，White K，Xu M. Fuzzy logic flight and bottom following controllers for the ocean voyager II AUV［C］∥Proceedings of the Joint Conference on Information Sciences. Pinehurst:Duke University/Association for Intelligent Machinery，1994:56 -59.

［3］Jose M，Anibal M. Bottom estimation and following with the MARES AUV［C］∥OCEANS 2012 MTS/IEEE:Harnessing the Power of the Ocean. Virginia:IEEE Computer Society，2012:1 -8.

［4］段海慶，賈鶴鳴，周佳加，等. 基于神經(jīng)網(wǎng)絡(luò)的欠驅(qū)動(dòng)水下機(jī)器人地形跟蹤控制［J］. 東南大學(xué)學(xué)報(bào):自然科學(xué)版，2012，42(1):203 -207.DUAN Hai-qing，JIA He-ming，ZHOU Jia-jia，et al. Bottom following control forUnderactuated AUV based on neural network［J］.Jouranal of Southeast University:Natural Science Edition，2012，42(1):203 -207. (in Chinese)

［5］Carlos S，Rita C，Nuno P，et al. A bottom-following preview controller for autonomous underwater vehicles［J］. IEEE Transactions on Control Systems Technology，2009，17(2):257 -266.

［6］Adhami-Mirhosseini A，Yazdanpanah M J，Aguiar A P. Automatic bottom-following for underwater robotic vehicles［J］. Automatica，2014，50(8):2155 -2162.

［7］施生達(dá).潛艇操縱性［M］. 北京:國(guó)防工業(yè)出版社，1995:149 -159.SHI Sheng-da. Submarine maneuverablity［M］. BeiJing:National Defense Industry Press，1995:149 -159. (in Chinese)

［8］韓京清. 自抗擾控制器及其應(yīng)用［J］. 控制與決策，1998，13(1):19 -23.HAN Jing-qing. Auto-disturbances-rejection controller and its applications［J］. Control and Decision，1998，13(1):19 -23. (in Chinese)

［9］韓京清. 自抗擾控制技術(shù)——估計(jì)補(bǔ)償不確定因素的控制技術(shù)［M］.北京:國(guó)防工業(yè)出版社，2009:243 -261.HAN Jing-qing. Active disturbance rejection control technique—the technique for estimating and compensating the uncertainties［M］.Beijing:National Defense Industry Press，2009:243 - 261. (in Chinese)

［10］朱繼懋. 潛水器設(shè)計(jì)［M］.上海:上海交通大學(xué)出版社，1992:65 -109.ZHU Ji-mao. The design of underwater vehicle［M］. Shanghai:Shanghai Jiao Tong University Press，1992:65 -109. (in Chinese)

［11］Sun Y S，Zhang Y H，Zhang G C，et al. Path tracking control of underactuated AUVs based on ADRC［C］∥Proceeding of 2013 Chinese Intelligent Automation Conference:Intelligent Automation.Yangzhou:Spring Verlag，2013:609 -615.

［12］萬磊，張英浩，孫玉山，等. 欠驅(qū)動(dòng)智能水下機(jī)器人的自抗擾路徑跟蹤控制［J］. 上海交通大學(xué)學(xué)報(bào)，2014，48(12):49 -54.WAN Lei，ZHANG Ying-hao，SUN Yu-shan，et al. ADRC pathfollowing control of underactuated AUVs［J］. Journal of Shanghai Jiao Tong University，2014，48(12):49 -54. (in Chinese)