[發明專利]一種穩定平臺實時軌跡規劃方法

在審

• 申請號: 202111462097.4
• 申請日: 2021-12-02
• 公開（公告）號: CN114265400A
• 公開（公告）日: 2022-04-01
• 發明（設計）人: 王力航;史乃煜;范麗榮
• 申請（專利權）人: 常州工學院
• 主分類號: G05D1/02
• 分類號: G05D1/02
• 代理公司: 暫無信息
• 代理人: 暫無信息

1.一種穩定平臺實時軌跡規劃方法，其特征在于：包括如下步驟：

步驟一、建立穩定補償控制系統，由上位機和下位機構成，上位機用于監督和產生一系列指令信號，下位機實時地執行控制算法；控制過程以循環的方式進行工作，并且上位機與下位機使用不同的工作周期，受姿態傳感器性能的限制，上位機采用周期T，下位機使用Ts作為執行周期；實際工作中需要實時地通過軌跡規劃得到每個Ts執行周期的位置與速度以供控制器參考，進而形成跟蹤指令；

在穩定平臺系統實時運動規劃方案整體結構中，上位機按照周期T進行采集船體姿態信息，驅動系統以周期Ts進行閉環控制，為協調控制過程中兩者周期差異，采用基于貝塞爾曲線的插值算法針對姿態信息節點進行插補；然后，將插補后的姿態信息經由運動學反解生成三缸實時軌跡；

步驟二、基于貝塞爾曲線的路徑規劃

m階貝塞爾曲線定義為控制點與伯恩斯坦多項式乘積形式

式中，系數p_j為控制點，基函數為m階伯恩斯坦多項式，定義為

式中，二項式系數定義為

選擇三階貝塞爾曲線進行插值，當m＝3時，貝塞爾曲線可以表示成

對參數u求導，可由端點處一階導數表達

b⁽¹⁾(0)＝3(p₁-p₀),b⁽¹⁾(1)＝3(p₃-p₂)

p₀,p₃為待插補的兩個端點，為了完成插補算法需要確定中間控制點坐標；

其中間控制點的確定方法，令貝塞爾曲線在起始點和終點導數具有相同的幅值

|b⁽¹⁾(0)|＝|b⁽¹⁾(1)|＝|b⁽¹⁾(0.5)|＝α (5)

即滿足

式中，α是以下方程的正數解

aα²+bα+c＝0 (7)

式中，

a＝16-|t₀+t₃|²,b＝12(p₃-p₀)^T(t₀+t₃),c＝-36|p₃-p₀|²

根據上述中間控制點確定過程，需要對待插補節點斜率進行估計，一種常用的斜率估計方法可以表示為

t_k＝(1-α_k)δ_k+α_kδ_k+1,k＝1,...,n-1 (8)

式中，

Δq_k＝q_k-q_k-1,

表征節點對應時間屬性，用于計算軌跡瞬時速度，假設軌跡以單位速度勻速生成并以節點直線距離替代曲線距離，則有

以上過程僅可以對中間節點斜率進行估計，對端點采用如下方式進行估計

t₀＝2δ₁-t₁,t_n＝2δ_n-t_n-1 (10)

步驟三、基于變結構狀態濾波器的軌跡規劃

采用基于變機構非線性濾波的方法進行實時軌跡規劃，對其速度加速度以及加加速度進行限制，建立離散系統，給定任意標準參考信號r(t)，都可以得到滿足如下約束條件的光滑信號q(t):

軌跡優化方法依賴于軌跡誤差，定義歸一化標準誤差變量

式中，U為有效控制量的最大值；

驅動在速度及加速度上的約束在轉化為標準約束范圍

控制器C3的具體表達形式為

式中，s_δ＝sign(δ)

符號函數sign(·)定義為

函數u_v(·)定義為