李朋博,苑明哲,肖金超,熊俊峰

(1.中國(guó)科學(xué)院沈陽(yáng)自動(dòng)化研究所,沈陽(yáng) 110016;2.廣州中國(guó)科學(xué)院沈陽(yáng)自動(dòng)化研究所分所, 廣州 511458;3.中國(guó)科學(xué)院大學(xué),北京 100049)

隨著現(xiàn)代控制理論和智能控制技術(shù)的不斷發(fā)展，無(wú)人艇智能水平得到了極大提高，推動(dòng)了無(wú)人艇在多領(lǐng)域的廣泛應(yīng)用.目前在無(wú)人艇-無(wú)人機(jī)和無(wú)人艇-水下機(jī)器人協(xié)作系統(tǒng)中，當(dāng)無(wú)人機(jī)或水下機(jī)器人等子系統(tǒng)進(jìn)行回收時(shí)，作為母體的無(wú)人艇應(yīng)保持固定的位姿為子系統(tǒng)提供穩(wěn)定的回收平臺(tái).位姿保持是指系統(tǒng)的位置和姿態(tài)持續(xù)穩(wěn)定或可快速回到穩(wěn)定狀態(tài).然而無(wú)人艇作為典型的欠驅(qū)動(dòng)系統(tǒng)，無(wú)法依靠自身推力系統(tǒng)同時(shí)實(shí)現(xiàn)自身的前向、側(cè)向和轉(zhuǎn)首運(yùn)動(dòng)[1].當(dāng)無(wú)人艇受到風(fēng)浪流等擾動(dòng)產(chǎn)生位姿變化時(shí)，尤其是受到擾動(dòng)發(fā)生較大橫蕩時(shí)，欠驅(qū)動(dòng)無(wú)人艇無(wú)法像全驅(qū)動(dòng)船舶依靠動(dòng)力分配策略在在側(cè)向施加動(dòng)力，也無(wú)法依靠傳統(tǒng)控制律兼顧其欠驅(qū)動(dòng)特性，只能依靠前向和轉(zhuǎn)首運(yùn)動(dòng)改變無(wú)人艇位姿，難以兼顧首向角保持或位置保持效果.因此，如何兼顧欠驅(qū)動(dòng)特性已成為無(wú)人艇位姿保持研究的關(guān)鍵難題.

當(dāng)前，學(xué)者解決無(wú)人艇位姿保持問(wèn)題多采用傳統(tǒng)控制理論和深度學(xué)習(xí)等智能方法.采用傳統(tǒng)控制理論方法，主要基于非線性控制律.文獻(xiàn)[2-3]針對(duì)風(fēng)擾動(dòng)問(wèn)題，設(shè)計(jì)了自適應(yīng)前饋控制器，并將滑模控制器應(yīng)用于無(wú)人艇，降低了風(fēng)擾動(dòng)對(duì)位姿保持的影響，位姿保持效果較好.文獻(xiàn)[4]設(shè)計(jì)了基于干擾觀測(cè)器的魯棒位姿控制系統(tǒng)，由干擾觀測(cè)器對(duì)干擾建模誤差給出補(bǔ)償，有效減小了干擾對(duì)水下機(jī)器人位姿保持的影響.目前也有基于深度學(xué)習(xí)和視覺(jué)等智能方法的無(wú)人艇位姿保持控制方法研究，文獻(xiàn)[5]提出了無(wú)模型的深度強(qiáng)化學(xué)習(xí)算法在水下航行器的6個(gè)自由度保持中的應(yīng)用，平衡了計(jì)算的易處理性和性能，使系統(tǒng)可以迅速響應(yīng)，但仍舊無(wú)法保持航向角的穩(wěn)定.文獻(xiàn)[6]創(chuàng)新地提出了一種視覺(jué)導(dǎo)航方法，通過(guò)CIALAB和k均值方法進(jìn)行波峰檢測(cè)并制定導(dǎo)航策略，從而實(shí)現(xiàn)系統(tǒng)的位姿保持.文獻(xiàn)[7-8]針對(duì)無(wú)人艇不同行為設(shè)計(jì)了切換控制器，基于性能的監(jiān)督切換控制系統(tǒng)指示控制器之間的切換以提高閉環(huán)性能，減小了在位姿保持時(shí)控制器切換帶來(lái)的位姿誤差.

目前，基于非線性控制器的位姿保持無(wú)法有效兼顧無(wú)人艇的欠驅(qū)動(dòng)性，且控制器設(shè)計(jì)相對(duì)復(fù)雜.深度學(xué)習(xí)訓(xùn)練數(shù)據(jù)集大，訓(xùn)練集外的場(chǎng)景控制效果差;視覺(jué)等其他方法的響應(yīng)速度慢，實(shí)時(shí)性有待提高.針對(duì)以上問(wèn)題，本文提出了一種基于航向約束的位姿保持制導(dǎo)策略，通過(guò)將航向約束納入外環(huán)制導(dǎo)，無(wú)人艇在不同區(qū)域執(zhí)行不同的行為策略，生成的位姿期望與內(nèi)環(huán)控制相結(jié)合，實(shí)現(xiàn)無(wú)人艇的位姿保持.相比于常規(guī)控制器，制導(dǎo)策略在執(zhí)行過(guò)程中嚴(yán)格約束了航向角，降低了控制器設(shè)計(jì)的難度，同時(shí)兼顧了無(wú)人艇的欠驅(qū)動(dòng)特性，簡(jiǎn)化了控制器設(shè)計(jì).

1 問(wèn)題描述

無(wú)人艇作為典型的欠驅(qū)動(dòng)系統(tǒng)，其系統(tǒng)輸入維數(shù)少于控制輸出維數(shù)，即只依賴(lài)前向動(dòng)力和偏航力矩控制無(wú)人艇的運(yùn)動(dòng)狀態(tài)，因而無(wú)法同時(shí)控制無(wú)人艇的前向、側(cè)向和轉(zhuǎn)首運(yùn)動(dòng).圖1所示為無(wú)人艇的3自由度平面運(yùn)動(dòng)模型.

圖1 無(wú)人艇運(yùn)動(dòng)模型：船體固定坐標(biāo)系ObXbYb和慣性坐標(biāo)系OEXEYEFig.1 Motion model of USV:body-fixed coordinate system ObXbYb and inertial coordinate system OEXEYE

無(wú)人艇具有6個(gè)自由度的復(fù)雜運(yùn)動(dòng)，往往難以建立精確的6自由度模型.為簡(jiǎn)化研究，本文不考慮無(wú)人艇的縱搖、橫搖和升沉運(yùn)動(dòng)，只建立無(wú)人艇的縱蕩、橫蕩和首搖3自由度耦合線性模型[9].建立的3自由度簡(jiǎn)化動(dòng)力學(xué)模型為

(1)

系統(tǒng)在平衡點(diǎn)附近運(yùn)動(dòng)時(shí)，可看作線性模型，經(jīng)簡(jiǎn)化得到考慮側(cè)滑運(yùn)動(dòng)的一階線性野本模型[1]，推導(dǎo)過(guò)程參考文獻(xiàn)[10]：

(2)

式中：Tr和Kr為船舶操縱性指數(shù)；δ為輸入舵角；β為側(cè)滑角；Tβ、Kβ為常數(shù).欠驅(qū)動(dòng)無(wú)人艇通常不配備側(cè)向推進(jìn)器，無(wú)法在Yb方向直接施加推力改變Yb軸方向位姿，而是通過(guò)前向運(yùn)動(dòng)和轉(zhuǎn)首運(yùn)動(dòng)改變無(wú)人艇Yb方向位姿，因此無(wú)人艇無(wú)法以原路徑返回期望位姿點(diǎn).

2 基于航向約束的制導(dǎo)策略

基于航向約束的制導(dǎo)策略是指依據(jù)制導(dǎo)策略，對(duì)無(wú)人艇航向角施加約束，在整個(gè)過(guò)程中將首向角控制在偏差范圍內(nèi)，以此將航向約束由內(nèi)環(huán)控制轉(zhuǎn)為外環(huán)制導(dǎo).當(dāng)無(wú)人艇偏離期望位姿后，由外環(huán)制導(dǎo)根據(jù)當(dāng)前位姿狀態(tài)給出期望速度和期望航向角.下文將對(duì)制導(dǎo)策略作簡(jiǎn)要說(shuō)明.

2.1 前提

本文只考慮在擾動(dòng)場(chǎng)景下無(wú)人艇可控的情況.設(shè)定本文的航向角偏差范圍為[-ψe，ψe]，ψe為允許的誤差角，期望位姿為(xd，yd，ψd)，其中：xd、yd和ψd分別為期望縱向位移、橫向位移和首向角；將任意時(shí)刻的位姿記為(x，y，ψ)，x、y和ψ分別為任意時(shí)刻的縱向位移、橫向位移和首向角.

2.2 位姿保持制導(dǎo)策略

(3)

式中：(xE，yE，ψE)為無(wú)人艇在慣性坐標(biāo)系下的位姿.

將固定坐標(biāo)系劃分為3個(gè)區(qū)域:可到達(dá)區(qū)域A、B和不可到達(dá)區(qū)域C.在可到達(dá)區(qū)域內(nèi)，無(wú)人艇可依靠前向運(yùn)動(dòng)和轉(zhuǎn)首運(yùn)動(dòng)回到期望位姿，避免由于無(wú)法側(cè)滑導(dǎo)致首向角誤差超出期望范圍.同時(shí)設(shè)置容差半徑Re，表示當(dāng)無(wú)人艇在這個(gè)區(qū)域內(nèi)時(shí)即視為其位置達(dá)到期望，容差帶de是為了防止由于擾動(dòng)影響在可到達(dá)區(qū)域A和不可到達(dá)區(qū)域C之間發(fā)生頻繁的區(qū)域跳變，將容差帶寬設(shè)置為可到達(dá)區(qū)域B.區(qū)域劃分如圖2所示.

圖2 固定坐標(biāo)系劃分示意圖Fig.2 Region-dependent segmentation of fixed coordinate system

可到達(dá)區(qū)域A定義為

A={(x，y)||x|≥|ytan(90°-ψe)|+de}

可到達(dá)區(qū)域B定義為

B={(x，y)||ytan(90°-ψe)|<

|x|<|ytan(90°-ψe)|+de}

不可到達(dá)區(qū)域C定義為

C={(x，y)||x|≤|ytan(90°-ψe)|}

令m為區(qū)域切換標(biāo)記，初始化為0，定義為

C→B:m=1
B→A:m=0

2.2.2制導(dǎo)策略 如圖3所示，無(wú)人艇受到外界擾動(dòng)并偏離容差半徑區(qū)域時(shí)，執(zhí)行本文制導(dǎo)策略.

圖3 策略流程圖Fig.3 Flowchart of strategy

在區(qū)域A、B和C分別設(shè)置了不同的制導(dǎo)策略:當(dāng)無(wú)人艇在位于不同的區(qū)域時(shí)，回到期望點(diǎn)的策略是不同的，本文將以無(wú)人艇位于第一象限為例進(jìn)行說(shuō)明.

策略1如圖4所示，當(dāng)m=0且位于區(qū)域A、B時(shí)，此時(shí)無(wú)人艇將直接以最終期望進(jìn)行制導(dǎo)，整個(gè)過(guò)程中首向角均在期望誤差范圍內(nèi)，因此直接制導(dǎo)可以返回期望.

圖4 制導(dǎo)策略示意圖——情形1Fig.4 Guidance strategy:situation 1

圖5 制導(dǎo)策略示意圖——情形2Fig.5 Guidance strategy:situation 2

將當(dāng)前位姿設(shè)為(x，y，ψ)，最終期望位姿(xd，yd，ψd)，無(wú)人艇受到擾動(dòng)偏離平衡后，位于不可到達(dá)區(qū)域C內(nèi)時(shí)，需要執(zhí)行策略2計(jì)算到達(dá)區(qū)域A的最短距離和期望位姿，計(jì)算當(dāng)前至可到達(dá)區(qū)域A的最短距離:

(4)

式中：de為容差帶高度，其作用為防止無(wú)人艇受限于自身回轉(zhuǎn)半徑或者受到環(huán)境擾動(dòng)時(shí)，在區(qū)域切換過(guò)程中可能會(huì)出現(xiàn)頻繁的區(qū)域跳變.

(5)

式中：θ=2ψ-Δ，Δ為容差角，由控制首向角控制的誤差決定.

在制導(dǎo)方式上，采用視線法制導(dǎo)策略:

(6)

式中：xLos和yLos為期望點(diǎn)坐標(biāo).

3 仿真試驗(yàn)

3.1 仿真結(jié)構(gòu)及相關(guān)參數(shù)

比例-積分-微分(PID)控制器是迄今最為常用的反饋控制形式，具有簡(jiǎn)單易用、魯棒性強(qiáng)和適應(yīng)性強(qiáng)等優(yōu)點(diǎn)，因此本文采用PID控制器作為內(nèi)環(huán)控制器，其控制結(jié)構(gòu)如圖6所示.

圖6 位姿保持控制結(jié)構(gòu)Fig.6 Control structure of station keeping

一階線性速度動(dòng)力學(xué)模型如下:

(7)

式中：ε為輸入油門(mén)大??；a和b為速度方程參數(shù).模型參數(shù)見(jiàn)表1[10].

表1 帶側(cè)滑的模型參數(shù)Tab.1 Parameters of model with sideslip

在MATLAB 2018a環(huán)境下進(jìn)行仿真，在仿真實(shí)驗(yàn)中，加入擾動(dòng)的解耦分量，各項(xiàng)速度上加入周期性擾動(dòng)，同時(shí)加入利用MATLAB內(nèi)置函數(shù)randn生成的隨機(jī)數(shù)作為隨機(jī)擾動(dòng).具體實(shí)驗(yàn)參數(shù)設(shè)置如表2.

表2 仿真實(shí)驗(yàn)的參數(shù)Tab.2 Parameters of simulation experiment

3.2 仿真實(shí)驗(yàn)

3.2.1仿真實(shí)驗(yàn)一 本文提出的基于航向約束的制導(dǎo)策略中，為了防止無(wú)人艇在制導(dǎo)過(guò)程中發(fā)生頻繁的區(qū)域跳變，定義了容差半徑Re和容差帶高度de兩個(gè)調(diào)整參數(shù).在仿真實(shí)驗(yàn)中選取了不同的Re和de，圖7所示為當(dāng)Re=0.1 m，de=0.3 m時(shí)的仿真結(jié)果.從仿真結(jié)果可得，位置誤差保持在一個(gè)較小的范圍內(nèi).當(dāng)由不可到達(dá)區(qū)域C穿過(guò)區(qū)域B到達(dá)區(qū)域A時(shí)，此時(shí)因?yàn)闊o(wú)人艇執(zhí)行制導(dǎo)策略，導(dǎo)致部分仿真時(shí)刻出現(xiàn)XE向位移增大，致使整體位置誤差變大，并且在整個(gè)制導(dǎo)過(guò)程中均會(huì)出現(xiàn)這種位置誤差變化.仿真過(guò)程中，首向角誤差長(zhǎng)時(shí)間保持在期望誤差ψ∈[-20°，20°]波動(dòng).

圖7 當(dāng)Re=0.1 m和de=0.3 m時(shí)的位置誤差和首向角誤差Fig.7 Position error and heading error at Re=0.1 m and de=0.3 m

受周期性擾動(dòng)n1(t)、n2(t)和隨機(jī)擾動(dòng)的影響，在轉(zhuǎn)向過(guò)程中由于轉(zhuǎn)向能力不足致部分時(shí)刻的首向角越過(guò)期望誤差角范圍.表3所示為當(dāng)Re和de取不同值時(shí)的位置誤差和首向角誤差.容差半徑Re為0，即無(wú)人艇在期望位置附近，需要進(jìn)行頻繁制導(dǎo)，不斷地改變自身位姿，導(dǎo)致位置和首向角不斷變化.容差帶高度de為0，會(huì)導(dǎo)致無(wú)人艇在區(qū)域切換時(shí)，發(fā)生頻繁跳變.由于航向角變化過(guò)快，無(wú)法跟蹤期望航向角跳變，前進(jìn)后退控制困難.在低速下受環(huán)境干擾更大，會(huì)出現(xiàn)更大的位置偏差.當(dāng)容差半徑過(guò)大時(shí)，會(huì)由于容差帶區(qū)域過(guò)大形成新的制導(dǎo)盲區(qū)，位置誤差和首向角誤差均有不同幅度的增大.逐漸增大容差帶高度，首向角誤差和位置誤差無(wú)明顯變化，是因?yàn)楫?dāng)容差帶半徑過(guò)大時(shí)，無(wú)人艇在容差帶區(qū)域可以有足夠的時(shí)間進(jìn)行轉(zhuǎn)向調(diào)整.容差半徑和容差帶高度的設(shè)置降低了在期望位置附近制導(dǎo)的頻次以及執(zhí)行機(jī)構(gòu)的執(zhí)行頻次，有利于無(wú)人艇在期望位置附近保持狀態(tài)穩(wěn)定.由于無(wú)人艇進(jìn)行位姿保持時(shí)是在低速狀態(tài)下運(yùn)行，推進(jìn)器頻繁的正反轉(zhuǎn)切換會(huì)引起執(zhí)行器過(guò)飽和，極易損壞執(zhí)行機(jī)構(gòu).因此依據(jù)當(dāng)前的制導(dǎo)策略，在實(shí)際進(jìn)行容差帶高度和容差半徑設(shè)定時(shí)，容差半徑的設(shè)定應(yīng)當(dāng)防止過(guò)大，容差帶的設(shè)定應(yīng)當(dāng)防止過(guò)小，否則無(wú)人艇在環(huán)境干擾下，容易引起區(qū)域切換頻繁導(dǎo)致的區(qū)域跳變，使得無(wú)人艇處于區(qū)域邊界.

表3 不同Re和de下的位置誤差和首向角誤差Tab.3 Position error and heading error at different Re and de values

3.2.2仿真實(shí)驗(yàn)二 仿真實(shí)驗(yàn)二是本文提出的基于航向約束的制導(dǎo)策略與PD控制器對(duì)比仿真實(shí)驗(yàn).在仿真實(shí)驗(yàn)中，設(shè)置了相同PD參數(shù)和相同的擾動(dòng).圖8所示為當(dāng)Re=0.1 m，de=0.3 m時(shí)，本文的方法和PD控制器通過(guò)仿真實(shí)驗(yàn)得到的一組對(duì)比結(jié)果.從圖8(b)可以看出，本文方法的位置誤差略大于PD控制器是由于執(zhí)行制導(dǎo)策略時(shí)，無(wú)人艇向期望點(diǎn)M運(yùn)動(dòng)時(shí)導(dǎo)致位置誤差偏大.從圖8(b)可以看出，本文方法的首向角誤差明顯優(yōu)于PD控制器是由于在制導(dǎo)策略中加入了首向角約束，致使首向角偏差明顯減小.

圖8 PD控制器和本文制導(dǎo)策略的位置誤差和首向角誤差對(duì)比(Re=0.1 m，de=0.3 m)Fig.8 Error for position and heading of PD controller and the guidance strategy proposed in this paper (Re=0.1 m，de=0.3 m)

進(jìn)行了多次對(duì)比仿真實(shí)驗(yàn)，隨機(jī)選擇三組實(shí)驗(yàn)數(shù)據(jù)，結(jié)果如表4所示.從多次仿真結(jié)果可得，本文提出的基于航向約束的制導(dǎo)策略由于對(duì)實(shí)時(shí)航向施加約束，無(wú)人艇瞬時(shí)航向保持在期望范圍內(nèi)，無(wú)人艇首向角得到約束，因此首向保持效果更好.本文方法中的位置偏差相比PD控制器，會(huì)出現(xiàn)局部增大，這是由于本文制導(dǎo)策略下由區(qū)域C到穿過(guò)區(qū)域B最終到達(dá)區(qū)域A時(shí)引起的正常位置誤差變化.考慮整體無(wú)人艇的首向和位置保持效果，認(rèn)為增大的位置誤差是可以接受的.因此，由仿真實(shí)驗(yàn)結(jié)果可以得出，本文提出的基于航向約束的制導(dǎo)策略相對(duì)傳統(tǒng)的PD控制器具有更好的位姿保持能力.

表4 本文制導(dǎo)策略與PD控制器仿真對(duì)比Tab.4 Simulation comparison of PD controller and the guidance strategy proposed in this paper

4 結(jié)語(yǔ)

本文針對(duì)欠驅(qū)動(dòng)無(wú)人艇位姿保持問(wèn)題，提出了基于航向約束的無(wú)人艇位姿保持制導(dǎo)策略.將航向約束納入外環(huán)制導(dǎo)，依據(jù)無(wú)人艇當(dāng)前位姿執(zhí)行相應(yīng)的制導(dǎo)策略，給出符合期望的航向.通過(guò)內(nèi)環(huán)控制器生成執(zhí)行量，最終可以使無(wú)人艇返回期望位姿.相較于常規(guī)控制律，將首向角控制放入制導(dǎo)環(huán)，外環(huán)制導(dǎo)約束與內(nèi)環(huán)控制約束相結(jié)合以兼顧欠驅(qū)動(dòng)特性.實(shí)驗(yàn)一中仿真不同制導(dǎo)參數(shù)下的位姿保持實(shí)驗(yàn)，驗(yàn)證了本文方法的可行性.實(shí)驗(yàn)二仿真本文方法和常規(guī)PD控制器的對(duì)比實(shí)驗(yàn)，本文方法在航向約束的位姿保持過(guò)程中，無(wú)人艇具有更好的首向保持效果，驗(yàn)證了本文方法的有效性.

在仿真過(guò)程中雖引入了周期性擾動(dòng)以及隨機(jī)擾動(dòng)，但在實(shí)際作業(yè)環(huán)境下，一方面需要對(duì)風(fēng)浪流進(jìn)行建模;另一方面，由于系統(tǒng)的位姿保持是在低速運(yùn)動(dòng)下進(jìn)行的，需要提升低速運(yùn)動(dòng)狀態(tài)下的無(wú)人艇控制精度以及無(wú)人艇進(jìn)行前進(jìn)和倒退的控制優(yōu)化.