[發明專利]基于速度與航向雙重制導的無人船精確路徑跟蹤控制方法

在審

• 申請號: 201811117138.4
• 申請日: 2018-09-25
• 公開（公告）號: CN109358499A
• 公開（公告）日: 2019-02-19
• 發明（設計）人: 王寧;孫卓;尹建川;蘇順豐
• 申請（專利權）人: 大連海事大學
• 主分類號: G05B13/04
• 分類號: G05B13/04
• 代理公司: 大連東方專利代理有限責任公司 21212
• 代理人: 李洪福

1.基于速度與航向雙重制導的無人船精確路徑跟蹤控制方法，其特征在于：利用路徑跟蹤控制系統進行跟蹤控制，所述的路徑跟蹤控制系統包括制導子系統和控制子系統；所述的制導子系統是一個決策系統，結合無人船與參考路徑之間的誤差產生速度和航向的參考指令，以此參考指令動作，跟蹤誤差將漸近收斂到零；所述的控制子系統是一個執行系統，使無人船實際姿態和參考指令達到一致，通過設計基于有限時間觀測器的魯棒控制器，實現精確的擾動觀測以及精確跟蹤控制；

所述的路徑跟蹤控制方法，包括以下步驟：

A、建立無人船運動學和動力學模型

基于無人船運動學模型導出路徑跟蹤誤差系統，進而設計制導律穩定制導子系統；基于無人船動力學模型，設計觀測器及控制器穩定控制子系統，進而實現整個路徑跟蹤控制系統的穩定，實現精確的路徑跟蹤控制；

無人船運動學模型如下：

式中，x、y分別是慣性坐標系下無人船的橫坐標、縱坐標，ψ是無人船在慣性坐標系下的航向，u是浮體坐標系下無人船的前向速度，v是浮體坐標系下無人船的橫向速度、r是浮體坐標系下無人船的航向角速度；

無人船動力學模型如下：

式中：d₁₁是無人船在前向速度維度上的阻尼參數，d₂₂是無人船在橫向速度維度上的阻尼參數，d₃₃是無人船在航向角速度維度上的阻尼參數，m₁₁是無人船在前向速度維度上的質量參數，m₂₂是無人船在橫向速度維度上的質量參數，m₃₃是無人船在航向角速度維度上的質量參數，τ_u和τ_r分別為可利用的控制輸入前向推力和轉向力矩，是無人船在前向速度上的外界干擾，是無人船在橫向速度上的外界干擾，是無人船在航向角速度速度上的外界干擾；

B、構造路徑跟蹤誤差動態

在無人船跟蹤參數路徑上選取一個移動虛擬點，以這個移動虛擬點為原點，其在慣性坐標系下的橫縱坐標分別為x_p和y_p，建立一個相對于慣性坐標系旋轉角度φ_p的坐標系，稱之為路徑正切參考坐標系，無人船相對于移動虛擬點在路徑參考坐標系橫坐標方向上的位置誤差為x_e，在縱坐標方向上的位置誤差為y_e，x_e和y_e的計算公式如下：

則路徑跟蹤誤差的動態為：

式中，是無人船在路徑正切參考坐標系下橫坐標方向上的路徑跟蹤誤差的導數，是無人船在路徑正切參考坐標系下縱坐標方向上的路徑跟蹤誤差的導數，是側滑角，u_tar是路徑上移動虛擬點的速度，表達成下列形式：

式中：是路徑參數，是路徑參數的導數，是x_p相對于的偏導數，是y_p相對于的偏導數。

C、設計速度與航向雙重制導律

基于推導出的路徑跟蹤誤差動態(4)，設計制導律如下，使路徑跟蹤誤差漸近穩定到零，制導子系統達到漸近穩定：

其中，u_d是無人船前向速度u的參考值，ψ_d是航向ψ的參考值，β_d是側滑角β的理想信號，k₁＞0是前向速度制導律中的設定常數，Δ>0是前視距離，k₂＞0是航向制導律中的設定常數，是理想航速值。

D、設計有限時間干擾觀測器

為了方便有限時間干擾觀測器的設計，于是將前述無人船的動力學模型進一步整理成矩陣形式：

v＝[u,v,r]^T (7)

τ＝[τ_u 0 τ_r]^T (9)

f(v)＝[f_u,f_v,f_r]^T (10)

式中，v是無人船速度矩陣，是無人船速度導數矩陣，M是無人船質量矩陣，τ是無人船控制輸入矩陣，其中：

有限時間干擾觀測器被設計成如下形式：

是速度導數矩陣的估計值，是速度矩陣的估計值，ζ是在有限時間干擾觀測器設計過程中定義的輔助變量，L是有限時間干擾觀測器增益矩陣，λ₁是有限時間干擾觀測器調節參數，λ₂是有限時間干擾觀測器另一個調節參數，是外界干擾的估計值。

E、基于有限時間觀測器的控制器設計

k_u是速度控制器中的前向速度設定參數，要求是一個正值，k_r是航向控制器中的航向角速度設定參數，要求是一個正值，是前向速度制導律的導數，是前向速度維度上的干擾的估計值，是航向角速度方向上的干擾的估計值，r_d是虛擬控制律：

是虛擬控制律的導數。