欠驅(qū)動自主水下航行器空間曲線路徑跟蹤控制研究

苗建明， 王少萍， 范磊,3， 李元

(1.北京航空航天大學 自動化科學與電氣工程學院, 北京 100083; 2.中國船舶重工集團公司 第710研究所, 湖北 宜昌 443003;3.61267部隊 第41分隊， 北京 101114)

欠驅(qū)動自主水下航行器空間曲線路徑跟蹤控制研究

苗建明1,2， 王少萍1， 范磊1,3， 李元1

(1.北京航空航天大學 自動化科學與電氣工程學院, 北京 100083; 2.中國船舶重工集團公司 第710研究所, 湖北 宜昌 443003;3.61267部隊 第41分隊， 北京 101114)

針對具有模型不確定性和輸入飽和的欠驅(qū)動自主水下航行器(AUV)，提出一種基于改進反步法的簡單實用三維空間曲線路徑跟蹤魯棒控制器。在 Serret-Frenet 坐標系下建立了空間曲線路徑跟蹤誤差模型，結(jié)合視線角制導和虛擬向?qū)ХǎO計了基于李雅普諾夫理論和改進反步法的運動學和動力學控制器。不同于傳統(tǒng)的積分器反步法，該方法在控制器設計中采用跟蹤誤差的積分來增加控制器的魯棒性，不會增加系統(tǒng)的狀態(tài)變量和計算量；針對設計的運動學控制器存在非因果現(xiàn)象的問題，借助動力學模型求解出運動學控制器表達式；針對傳統(tǒng)反步法存在的“微分爆炸”現(xiàn)象及動力學控制器過于復雜的問題，采用非線性跟蹤微分器對控制器進行簡化。仿真結(jié)果表明：采用所設計的基于改進反步法的控制器能夠?qū)崿F(xiàn)欠驅(qū)動 AUV 在模型參數(shù)不確定性和輸入飽和作用下的三維空間曲線路徑跟蹤控制，控制精度和魯棒性明顯優(yōu)于常規(guī)反步法。

控制科學與技術； 欠驅(qū)動AUV； 空間曲線路徑跟蹤； 反步； 李雅普諾夫理論

Abstract: Based on the modified back-stepping technique, a simple and robust spatial curvilinear path following controller for the underactuated autonomous underwater vehicles (AUVs) with model uncertainties and input saturation is presented. A path following error dynamics model is constructed in a moving Serret-Frenet frame, and the kinematic controller and dynamic controller are developed based on line-of-sight (LOS) guidance algorithm and virtual moving target method. Differing from the traditional integrator backstepping technique, the proposed method is to introduce the integral tracking errors into the controller design to improve the robustness against the uncertainties. The dynamic model is used to solve the non-causal form caused by the coupled underactuated degrees. The nonlinear tracking differentiators (NTDs) are employed to construct the numerical solution of differential virtual control commands to tackle the problem of “explosion of terms” in the traditional back-stepping process, and the dynamic controller expressions are simplified. Simulations demonstrate that the designed controller realizes the spatial curvilinear path following control of underactuated AUV with model parameter uncertainties and input saturation, and its accuracy and robustness are more excellent than those of the traditional back-stepping control.

Key words: control science and technology; underactuated AUV; spatial curvilinear path following; back-stepping; Lyapunov’s theory

0 引言

隨著自主式水下航行器(AUV)在海洋研究和開發(fā)領域的應用越來越廣泛，路徑跟蹤控制已經(jīng)成為AUV運動控制的重要技術之一[1-3]。路徑跟蹤是控制AUV跟蹤一條滿足航向要求和性能約束，且與時間無關的期望軌跡[4]。由于受到質(zhì)量、可靠性、復雜性及效率等多方面的因素影響，目前大部分AUV屬于欠驅(qū)動系統(tǒng)，加上其本身具有高度的非線性和耦合性以及加速度不可積的非完整約束等特性[5]，給運動控制帶來了較大挑戰(zhàn)。

為了解決上述問題，國內(nèi)外學者對于欠驅(qū)動AUV路徑跟蹤控制進行了大量研究。文獻[6]采用線性化方法，得到了一個局部穩(wěn)定的控制器。文獻[7]首次在Serret-Frenet坐標系中建立路徑跟蹤誤差方程，基于李雅普諾夫理論和反步法設計了一個水平面內(nèi)的路徑跟蹤控制器，但是此控制器要求AUV的初始位置和期望路徑的誤差小于路徑的最小曲率半徑。文獻[8-9]采用虛擬向?qū)У姆椒ń獬诉@一限制，實現(xiàn)了二維平面內(nèi)的全局路徑跟蹤控制。文獻[10]采用2階自抗擾控制方法來處理典型欠驅(qū)動的路徑跟蹤問題，但是沒有給出控制系統(tǒng)穩(wěn)定性的理論證明。文獻[11]給出了一個基于動態(tài)面技術的控制器，有效地解決了傳統(tǒng)反步法存在的“微分爆炸”現(xiàn)象，并且采用神經(jīng)網(wǎng)絡方法處理水動力參數(shù)的不確定性。為了有效抵消風、浪、流等環(huán)境的干擾，文獻[12]對傳統(tǒng)的視線角制導方法進行改進，提出了一個帶有積分項的視線角制導律，主要用于直線路徑跟蹤。文獻[13]提出了一個自適應積分視線角制導律來估計未知的側(cè)滑角,可以用于直線或者Dubins路徑[14]跟蹤控制。文獻[15]提出了一個改進的自適應積分視線角制導律，采用一個自適應徑向基函數(shù)神經(jīng)網(wǎng)絡和一個輔助系統(tǒng)，結(jié)合反步法實現(xiàn)了無人水面船在存在不確定性和輸入飽和情況下的路徑跟蹤控制問題。

需要指出的是，上述研究成果都局限在二維平面內(nèi)，由于三維空間曲線的路徑跟蹤控制問題更加復雜，欠驅(qū)動的自由度也增加到兩個，因此到目前為止，關于三維空間曲線路徑的研究成果還相對較少。文獻[16-17]將積分視線角方法擴展到有恒定海流影響的三維直線路徑跟蹤控制中。文獻[18]基于李雅普諾夫理論和反步法提出了一個三維空間的非線性自適應控制器。文獻[19]在載體坐標下基于虛擬向?qū)Хń⒘寺窂礁櫿`差模型, 采用濾波反步法實現(xiàn)了欠驅(qū)動AUV的三維路徑跟蹤控制，通過2階濾波過程獲得虛擬控制量的導數(shù), 避免了直接對虛擬控制量解析求導的復雜過程。值得注意的是，文獻[18-19]忽略了側(cè)滑角和沖角的影響，而側(cè)滑角和沖角的存在給路徑跟蹤控制器的設計帶來了復雜性[5]。

基于上述考慮，針對具有模型參數(shù)不確定性的欠驅(qū)動AUV，本文采用基于改進的反步法來進行三維空間曲線路徑跟蹤控制器的設計。不同于傳統(tǒng)的積分器反步法，該方法在控制器設計中采用跟蹤誤差的積分來增加控制器的魯棒性，因此不會增加系統(tǒng)的狀態(tài)變量和計算量。另外，針對傳統(tǒng)反步法存在的“微分爆炸”和控制器過于復雜等問題，采用離散非線性跟蹤微分器[20]來實時計算中間虛擬控制指令導數(shù)的數(shù)值解，避免了常規(guī)反步法直接對虛擬信號解析求導帶來的“微分爆炸”問題。

1 運動控制建模及問題描述

1.1 坐標系及參數(shù)定義

坐標系及參數(shù)定義如圖1所示，其中：{I}為慣性坐標系，坐標原點為海平面任一點，x軸以水下機器人的主航向為正向，z軸指向地心為正，y軸與x軸和z軸構(gòu)成右手坐標系；{B}為載體坐標系，坐標原點和AUV的質(zhì)心位置Q點重合，其xB軸指向AUV艏部方向，yB軸指向右舷，zB軸與xB軸和yB軸構(gòu)成右手坐標系；{SF}為Serret-Frenet曲線坐標系，坐標原點為期望路徑上的任意一點P，xSF軸沿路徑切線方向，ySF軸為法線方向,zSF軸與xSF軸和ySF軸構(gòu)成右手坐標系。

圖1 空間曲線路徑跟蹤控制坐標系及參數(shù)定義Fig.1 Frame definitions of spatial curvilinear path following

1.2 欠驅(qū)動AUV模型

假設所研究的欠驅(qū)動AUV為零浮力，重心位于浮心正下方，忽略高于2階的非線性水動力阻尼項，且不考慮橫搖運動的影響，縱傾角小于90°，欠驅(qū)動AUV的5自由度運動學模型[21-23]和動力學模型[24]分別為

(1)

(2)

考慮到輸入飽和的影響，假設-τimin≤τi≤τimax,i=u,q,r，τu、τq和τr可以用(3)式表示：

(3)

1.3 Serret-Frenet坐標系下的路徑跟蹤誤差模型

參照文獻[8,22]，在Serret-Frenet坐標系下建立欠驅(qū)動AUV的三維空間曲線路徑跟蹤誤差模型，并采用視線角制導結(jié)合虛擬向?qū)У姆椒ǎ员苊鈧鹘y(tǒng)橫向跟蹤控制方法存在的奇異性問題[7]。

如圖1所示，將慣性坐標系{I}下AUV和目標路徑上虛擬向?qū)c的位置誤差轉(zhuǎn)換到曲線坐標系{SF}下，得到誤差方程如下：

(4)

對(4)式進行求導，可得如下誤差動力學方程：

(5)

忽略三維空間非線性引起的誤差， 航跡角誤差和潛浮角誤差動力學模型[21-23]可以表示為

(6)

1.4 問題描述

2 空間曲線路徑跟蹤控制器設計

本文所設計的空間曲線路徑跟蹤控制器由視線角制導率、運動學控制器和動力學控制器3部分組成，然后針對控制器過于復雜的問題對其進行改進，下面給出每個部分的設計。

2.1 視線角制導律設計

文獻[12-13]對視線角制導律進行了介紹，本文設計的制導律為

(7)

2.2 運動學控制器設計

2.2.1 姿態(tài)控制

構(gòu)造李雅普諾夫函數(shù)為

(8)

對(8)式進行時間求導，并代入(6)式，可得

(9)

(10)

式中：k1、k2>0. 將(10)式代入(9)式可得

(11)

(12)

2.2.2 位置控制

構(gòu)造李雅普諾夫函數(shù)為

(13)

對(13)式沿誤差動力學模型(5)式的軌跡對時間求導，可得

(14)

(15)

將(7)式和(15)式代入(14)式得

(16)

2.3 動力學控制器設計

定義虛擬航向角速度和俯仰角速度期望值為

(17)

考慮到輸入飽和的影響，采用如下輔助設計系統(tǒng)[15]：

(18)

構(gòu)造李雅普諾夫函數(shù)為

(19)

式中：k6>0.

當|φr|、|φq|、|φu|≥φμ時，將(19)式對時間求導得

(20)

(21)

將(21)式代入(20)式得

(22)

(23)

3 控制器簡化及穩(wěn)定性證明

3.1 控制器簡化

綜上所述可以看出，上述傳統(tǒng)反步法設計的控制器由于對虛擬控制指令直接解析求導，導致動力學控制器(23)式非常復雜，不適合實際工程應用。動態(tài)面控制方法采用1階濾波器得到虛擬控制的數(shù)值解有效解決了“計算膨脹”的問題[11,21]，本文采用韓京清提出的非線性快速跟蹤微分器[20]來實時求解虛擬航向角速度和俯仰角速度期望值導數(shù)的數(shù)值解，避免了上述非因果現(xiàn)象，同時簡化了動力學控制器表達式。

非線性快速跟蹤微分器的離散型表達式可以表示為

(24)

(25)

簡化后的動力學控制器為

(26)

需要說明的是，采用非線性快速跟蹤微分器對控制器簡化后，必然會引入跟蹤微分器的跟蹤誤差，下面給出其穩(wěn)定性證明。

3.2 穩(wěn)定性證明

考慮(1)式～(6)式描述的欠驅(qū)動AUV三維運動模型，由(7)式計算目標視線角ψlos和θlos，由(12)式計算虛擬航向角速度和俯仰角速度期望值，分別由(24)式和(25)式獲取虛擬控制指令rd和qd的微分信號，采用(15)式獲取虛擬向?qū)У乃俣瓤刂坡?，采?26)式和(3)式計算輸入力和力矩的控制律，假設參數(shù)ki>0,i=1，2，3，4，5，6，7，則有：

2)未直接驅(qū)動的橫向速度v和垂向速度w一致最終有界。

證明：1)首先，重新考察李雅普諾夫函數(shù)V3，當|φr|、|φq|、|φu|≥φμ時，對(19)式沿系統(tǒng)軌跡對時間求導可得

(27)

式中：kv=min (k1k6,k2k6,k5-0.5,k4-0.5,k7,kφr-0.5,kφq-0.5,kφu-0.5).

解方程式(27)式可得

(28)

其次，重新考察李雅普諾夫函數(shù)V2，同理可以證明跟蹤誤差ε2=[xe,ye,ze]一致最終有界。

2)考慮如下李雅普諾夫方程：

(29)

對(27)式沿系統(tǒng)軌跡對時間求導，可得

(30)

由于d22、d33>0，且m11、u、q、r均有界，根據(jù)文獻[26]可知，未直接驅(qū)動的橫向速度v和垂向速度w一致最終有界。

4 仿真分析

為了驗證所設計控制器的有效性和魯棒性，本文對上述控制器進行仿真分析，并與傳統(tǒng)反步法控制器的性能進行對比。

假設AUV跟蹤的期望路徑為：xP(μ)=μ，yP(μ)=20cos (0.05μ)，zP(μ)=20sin (0.05μ).

假設控制輸入力τu的最大幅值為1 000 N，最大輸入力矩τq和τr的最大幅值為500 N·m. 控制器參數(shù)設計為：k1=1，k2=1，k3=0.5，k4=1,k5=10，k6=1，k7=1，kψ=0.1，kθ=0.1，ku=0.1，kq=0.1，kr=0.1，φμ=0.001，h=0.001，kφr=kφq=kφu=10.

假設AUV參數(shù)mii(i=1,2,3,5,6) 在名義值上增加30%，dii(i=1,2,3,5,6) 在名義值上增加50%. 仿真結(jié)果如圖2～圖6所示，圖2為AUV期望路徑和實際路徑，圖3為Serret-Frenet坐標系下AUV和期望路徑上虛擬向?qū)еg的位置誤差，圖4為AUV線速度和角速度，圖5為虛擬控制指令及跟蹤微分器的輸出，圖6給出了在兩種控制器作用下的控制輸入。

圖2 AUV期望路徑和實際路徑Fig.2 The desired path and vehicle actual path

圖3 AUV和期望路徑上虛擬向?qū)еg的位置誤差Fig.3 Relative position errors of AUV and virtual moving target

從圖2和圖3中可以看出，基于改進反步法的控制器在模型參數(shù)不確定和輸入飽和作用下能夠?qū)崿F(xiàn)三維空間曲線的路徑跟蹤控制，而基于傳統(tǒng)反步法的控制器控制精度明顯下降。圖4表明AUV縱向速度在改進控制器作用下快速收斂到設定值2 m/s，而傳統(tǒng)反步法控制器則存在一個恒定的誤差。另外，從圖4還可以看出，未直接驅(qū)動的橫向速度v和垂向速度w均有界。圖5表明所設計的非線性跟蹤微分器在實現(xiàn)對虛擬指令快速跟蹤的同時獲得了其微分信號。圖6表明改進的控制器控制輸入始終在設定值范圍內(nèi)。

圖4 AUV線速度和角速度Fig.4 Linear and angular speeds of AUV

圖5 虛擬控制指令qd、rd和跟蹤微分器輸出的qc、rc、Fig.5 Virtual commands qd,rd, and qc,rc,, produced by nonlinear TDs

圖6 控制輸入力和輸入力矩Fig.6 Control inputs

5 結(jié)論

本文采用一種非常簡單的控制方法實現(xiàn)了欠驅(qū)動AUV在具有參數(shù)不確定性和輸入飽和下的三維空間曲線路徑跟蹤控制問題。不同于傳統(tǒng)的積分器反步法，該方法在增加控制器的魯棒性的同時，不會增加系統(tǒng)的狀態(tài)變量和計算量。另外，本文采用非線性跟蹤微分器對控制器進行了簡化。仿真結(jié)果驗證了基于改進反步法的控制器能夠?qū)崿F(xiàn)欠驅(qū)動AUV在模型參數(shù)不確定性和輸入飽和作用下的三維空間曲線的路徑跟蹤控制，并且控制精度和魯棒性明顯優(yōu)于常規(guī)反步法。

References)

[1] Kohl A M, Pettersen K Y, Kelasidi E, et al. Planar path following of underwater snake robots in the presence of ocean currents[J]. IEEE Robotics and Automation Letters, 2016, 1(1): 383-390.

[2] 王芳, 萬磊, 李曄, 等. 欠驅(qū)動 AUV 的運動控制技術綜述[J]. 中國造船, 2010, 51(2): 227-241. WANG Fang, WAN Lei, LI Ye, et al. A survey on development of motion control for underactuated AUV[J]. Shipbuilding of China, 2010, 51(2): 227-241. (in Chinese)

[3] Shojaei K. Neural network formation control of underactuated autonomous underwater vehicles with saturating actuators[J]. Neurocomputing, 2016, 194(5): 372-384.

[4] Fossen T I. Handbook of marine craft hydrodynamics and motion control[M]. West Sussex: John Wiley & Sons, 2011.

[5] 向先波.二階非完整性水下機器人的路徑跟蹤與協(xié)調(diào)控制研究[D]. 武漢:華中科技大學，2010. XIANG Xian-bo. Research on path following and coordinated control for second-order nonholonomic UUVs [D]. Wuhan：Huazhong University of Science and Technology, 2010. (in Chinese)

[6] Kaminer I, Pascoal A, Hallberg E, et al. Trajectory tracking for autonomous vehicles: an integrated approach to guidance and control[J]. Journal of Guidance, Control, and Dynamics, 1998, 21(1): 29-38.

[7] Encarnacao P, Pascoal A, Arcak M. Path following for autonomous marine craft[C]∥5th IFAC Conference on Manoeuvring and Control of Marine Craft. Aalborg: MCMC, 2000: 117-122.

[8] Lapierre L, Soetanto D. Nonlinear path-following control of an AUV[J]. Ocean engineering, 2007, 34(11): 1734-1744.

[9] Xiang X, Lapierre L, Jouvencel B. Smooth transition of AUV motion control: from fully-actuated to under-actuated configuration[J]. Robotics and Autonomous Systems, 2015, 67: 14-22.

[10] 萬磊, 張英浩, 孫玉山, 等. 欠驅(qū)動智能水下機器人的自抗擾路徑跟蹤控制[J]. 上海交通大學學報, 2014, 48(12): 1727-1731. WAN Lei, ZHANG Ying-hao, SUN Yu-shan, et al. ADRC path following control of underactuated AUVs[J]. Journal of Shanghai Jiao Tong University, 2014, 48(12): 1727-1731.(in Chinese)

[11] Park B S. Neural network-based tracking control of underactuated autonomous underwater vehicles with model uncertainties[J]. Journal of Dynamic Systems, Measurement, and Control, 2015, 137(2): 021004.

[12] Borhaug E, Pavlov A, Pettersen K Y. Integral LOS control for path following of underactuated marine surface vessels in the presence of constant ocean currents[C]∥47th IEEE Conference on Decision and Control. Cancun, Mexico: IEEE, 2008: 4984-4991.

[13] Fossen T I, Pettersen K Y, Galeazzi R. Line-of-sight path following for dubins paths with adaptive sideslip compensation of drift forces[J]. IEEE Transactions on Control Systems Technology, 2015, 23(2): 820-827.

[14] Dubins L E. On curves of minimal length with a constraint on average curvature, and with prescribed initial and terminal positions and tangents[J]. American Journal of Mathematics, 1957, 79(3): 497-516.

[15] Zheng Z, Sun L. Path following control for marine surface vessel with uncertainties and input saturation[J]. Neurocomputing, 2016, 177: 158-167.

[16] Caharija W, Pettersen K Y, Gravdahl J T. Integral LOS guidance for horizontal path following of underactuated autonomous underwater vehicles in the presence of vertical ocean currents[C]∥American Control Conference (ACC). Montreal, Canada: IEEE, 2012: 5427-5434.

[17] Caharija W, Pettersen K Y, Gravdahl J T, et al. Path following of underactuated autonomous underwater vehicles in the presence of ocean currents[C]∥Decision and Control (CDC), IEEE 51st Annual Conference on. NY, US: IEEE, 2012: 528-535.

[18] Do K D, Pan J, Jiang Z P. Robust and adaptive path following for underactuated autonomous underwater vehicles[J]. Ocean Engineering, 2004, 31(16): 1967-1997.

[19] 王宏健, 陳子印, 賈鶴鳴, 等. 基于濾波反步法的欠驅(qū)動 AUV 三維路徑跟蹤控制[J]. 自動化學報, 2015, 41(3): 631-645. WANG Hong-jian, CHEN Zi-yin, JIA He-ming, et al. Three-dimensional path-following control of underactuated autonomous underwater vehicle with command filtered backstepping[J]. Acta Automatica Sinica, 2015, 41(3): 631-645.(in Chinese)

[20] 韓京清. 自抗擾控制技術(估計補償不確定因素的控制技術[M]. 北京:國防工業(yè)出版社, 2009:66-73. HAN Jing-qing.Active disturbance rejection control technique-the technique foe estimating and compensating the uncertainties[M]. Beijing: National Defense Industry Press,2009: 66-73.(in Chinese)

[21] Chwa D. Global tracking control of underactuated ships with input and velocity constraints using dynamic surface control method[J]. IEEE Transactions on Control Systems Technology, 2011, 19(6): 1357-1370.

[22] Qi X. Spatial target path following control based on Nussbaum gain method for underactuated underwater vehicle[J]. Ocean Engineering, 2015, 104: 680-685.

[23] 賈鶴鳴, 張利軍, 程相勤, 等. 基于非線性迭代滑模的欠驅(qū)動 UUV 三維航跡跟蹤控制[J]. 自動化學報, 2012, 38(2): 308-314. JIA He-ming, ZHANG Li-jun, CHENG Xiang-qin, et al. Three-dimensional path following control for an underactuated UUV aased on aonlinear iterative sliding mode[J]. Acta Automatica Sinica, 2012, 38(2): 308-314.(in Chinese)

[24] Pettersen K Y, Egeland O. Time-varying exponential stabilization of the position and attitude of an underactuated autonomous underwater vehicle[J]. IEEE Transactions on Automatic Control, 1999, 44(1): 112-115.

[25] Miao J M, Wang S P, Zhao Z P, et al. Spatial curvilinear path following control of underactuated AUV with multiple uncertainties[J]. ISA Transactions, 2017,67:107-130.

[26] Khalil H K. Nonlinear systems[M]. 3rd ed. NY, US: Pearson Education, 2002.

SpatialCurvilinearPathFollowingControlofUnderactuatedAUV

MIAO Jian-ming1,2, WANG Shao-ping1, FAN Lei1,3, LI Yuan1

(1.School of Automation Science and Electrical Engineering, Beihang University, Beijing 100083, China；2.No. 710 Research Institute, China Shipbuilding Industry Corporation, Yichang 443003, Hubei, China；3. 41th Detachment, Unit 61267 of PLA, Beijing 101114, China)

TP242.3

A

1000-1093(2017)09-1786-11

10.3969/j.issn.1000-1093.2017.09.016

2017-01-06

國家“973”計劃項目(2014CB046402)；國家自然科學基金項目(51575019)

苗建明(1979—), 男，高級工程師, 博士研究生。E-mail: sxmjm@126.com

王少萍(1966—), 女, 教授，博士生導師。E-mail: shaopingwang@vip.sina.com