[發明專利]一種高抗擾自適應路徑跟隨方法及系統

有效

• 申請號: 201810106099.1
• 申請日: 2018-02-02
• 公開（公告）號: CN108415423B
• 公開（公告）日: 2020-12-04
• 發明（設計）人: 姜權權;李曄;廖煜雷;苗玉剛;潘愷文;張偉;范佳佳
• 申請（專利權）人: 哈爾濱工程大學
• 主分類號: G05D1/02
• 分類號: G05D1/02
• 代理公司: 暫無信息
• 代理人: 暫無信息

1.一種高抗擾自適應路徑跟隨方法，其特征在于：包括以下步驟：

步驟一：輸入期望路徑命令，期望路徑由N個期望路徑點組成，期望路徑點P＝(P₁,P₂,P₃…P_n)N≥2，其中第n個期望路徑點P_n＝(x_n,y_n)，1≤n＜N，初始化令n＝1，初始化安全距離的閾值a，a為大于0的常數；

步驟二：得到期望路徑點P_n＝(x_n,y_n)和P_n+1＝(x_n+1,y_n+1)兩點連線直線路徑并得出直線路徑方向角ψ_pn，ψ_pn表示直線路徑與X軸正方向的夾角，滿足：

ψ_pn＝atan2(y_n+1-y_n,x_n+1-x_n),ψ_pn∈[-π,π]

步驟三：根據傳感器實時測得的水中航行裝備的位置(x_t,y_t)、期望路徑點P_n坐標(x_n,y_n)以及直線路徑方向角ψ_pn獲得跟蹤誤差Z_e，Z_e滿足：

Z_e＝-(x_t-x_n)sin(ψ_pn)+(y_t-y_n)cos(ψ_pn)

步驟四：設計收斂圓半徑R_n，R_n滿足：

其中，β為正常數，用于調節跟對過程的動態行為，且β＞1；

步驟五：由Z_e和R_n求出超前距離Δ，Δ滿足：

步驟六：計算視線角ψ^*_los，視線角ψ^*_los滿足：

ψ^*_los＝arctan(-K_P*Ze_i-K_i*ξ(Ze_i)),i＝1、2、...

其中，Ze_i為系統運行第i次的跟蹤誤差，K_i為積分項的可調參數，ξ(Ze_i)滿足：

其中，Δt表示系統運行的時間步長，Ze_max是和水中航行裝備的長度有關的參數；

步驟七：獲得期望航向ψ_d，ψ_d滿足：

ψ_d＝ψ^*_los+ψ_pn

步驟八：獲得水中航行裝備的實際航向狀態ψ和實時位置，ψ包括艦船的航向和角速度r的信息，令其中k₁為與艦船動力學特性有關的參數，取適當的k₁值，計算ψ_d與ψ之差得e(k)，k表示系統運行第k次；

步驟九：將e(k)作為CFDL-MFAC(重定義緊格式動態線性化無模型自適應控制器)航向控制器的輸入，解算期望指令u(k)，舵機或速度差動機構執行期望指令u(k)，利用CFDL-MFAC(重定義緊格式動態線性化無模型自適應控制算法)算法解算過程滿足：

當|Δu(k-1)|≤ε或或

其中，ρ∈(0,1]為步長因子，η∈(0,1]為步長因子，μ＞0為權重系數，λ＞0為常變量，φ(k)為偽偏導數，為方法運行k次偽偏導數估計值，為方法運行k-1時偽偏導數估計值，Δy(k)為方法運行k次時的航向系統輸出量與方法運行k-1次時的航向系統輸出量的差值，u(k)為方法運行k次時航向系統期望輸入，u(k-1)為方法運行k-1次時航向系統期望輸入，Δu(k-1)為方法運行k-1次時航向系統期望輸入與方法運行k-2次時航向系統期望輸入之差，ε為一個充分小的正常，ε∈(0,0.001]；

步驟十：根據水中航行裝備的實時位置，計算水中航行裝備距離期望路徑點P_n+1的距離PL,當PL＜a時，執行步驟十一；當PL≥a，執行步驟三；

步驟十一：當n+1＝N時，結束；當n+1＜N，令n＝n+1，執行步驟二。

2.一種基于權利要求1的高抗擾自適應路徑跟隨方法的路徑跟隨系統，其特征在于：輸入期望路徑點，生成期望路徑；將期望路徑點信息和通過位置傳感器模塊獲得的水中航行裝備時時位置信息輸入至制導模塊，通過所述的高抗擾自適應路徑跟隨方法解算出期望航向狀態ψ_d；將期望航向狀態ψ_d和通過航向傳感器模塊獲得的并經濾波器模塊濾波的水中航行裝備實際航向狀態ψ做差，獲得航向狀態偏差e(k)并輸入至CFDL-MFAC控制器，輸出期望指令u(k)至操縱機構模塊；操縱機構模塊收到并執行期望指令u(k)，將執行結果輸入至水中航行裝備模塊，令水中航行裝備不斷趨近期望航向ψ_d。