基于航向約束的無人艇位姿保持制導策略

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

(1.中國科學院沈陽自動化研究所,沈陽 110016;2.廣州中國科學院沈陽自動化研究所分所, 廣州 511458;3.中國科學院大學,北京 100049)

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

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

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

1 問題描述

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

圖1 無人艇運動模型：船體固定坐標系ObXbYb和慣性坐標系OEXEYEFig.1 Motion model of USV:body-fixed coordinate system ObXbYb and inertial coordinate system OEXEYE

無人艇具有6個自由度的復雜運動，往往難以建立精確的6自由度模型.為簡化研究，本文不考慮無人艇的縱搖、橫搖和升沉運動，只建立無人艇的縱蕩、橫蕩和首搖3自由度耦合線性模型[9].建立的3自由度簡化動力學模型為

(1)

系統(tǒng)在平衡點附近運動時，可看作線性模型，經(jīng)簡化得到考慮側(cè)滑運動的一階線性野本模型[1]，推導過程參考文獻[10]：

(2)

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

2 基于航向約束的制導策略

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

2.1 前提

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

2.2 位姿保持制導策略

(3)

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

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

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

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

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

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

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

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

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

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

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

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

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

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

在區(qū)域A、B和C分別設置了不同的制導策略:當無人艇在位于不同的區(qū)域時，回到期望點的策略是不同的，本文將以無人艇位于第一象限為例進行說明.

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

圖4 制導策略示意圖——情形1Fig.4 Guidance strategy:situation 1

圖5 制導策略示意圖——情形2Fig.5 Guidance strategy:situation 2

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

(4)

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

(5)

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

在制導方式上，采用視線法制導策略:

(6)

式中：xLos和yLos為期望點坐標.

3 仿真試驗

3.1 仿真結構及相關參數(shù)

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

圖6 位姿保持控制結構Fig.6 Control structure of station keeping

一階線性速度動力學模型如下:

(7)

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

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

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

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

3.2 仿真實驗

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

圖7 當Re=0.1 m和de=0.3 m時的位置誤差和首向角誤差Fig.7 Position error and heading error at Re=0.1 m and de=0.3 m

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

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

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

圖8 PD控制器和本文制導策略的位置誤差和首向角誤差對比(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)

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

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

4 結語

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

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