[發明專利]一種履帶式車輛導航軌跡跟蹤控制系統

有效

• 申請號: 201910956979.2
• 申請日: 2019-10-10
• 公開（公告）號: CN110716565B
• 公開（公告）日: 2022-10-28
• 發明（設計）人: 路恩;徐立章;李耀明;唐忠
• 申請（專利權）人: 江蘇大學
• 主分類號: G05D1/02
• 分類號: G05D1/02
• 代理公司: 暫無信息
• 代理人: 暫無信息

1.一種履帶式車輛導航軌跡跟蹤控制系統，其特征在于：包括GNSS導航系統基準站(1)、GNSS導航系統移動站(2)、履帶式車輛(3)和軌跡跟蹤控制器(4)，所述GNSS導航系統基準站(1)將觀測值和測站坐標信息發送給GNSS導航系統移動站(2)，GNSS導航系統移動站(2)接收GNSS導航系統基準站(1)數據的同時采集GNSS觀測數據，據此計算履帶式車輛(3)的實時位置和航向，軌跡跟蹤控制器(4)根據實時位置和航向跟蹤控制履帶式車輛的軌跡；

所述軌跡跟蹤控制器(4)包括信號連接的協調控制層、組織控制層和執行層，具體為：

1)建立軌跡跟蹤誤差模型

設履帶式車輛(3)的質心在全局坐標系XOY中的投影坐標為(x,y)，θ為履帶式車輛(3)的姿態角，根據全局坐標系XOY中的幾何關系建立如下的履帶式車輛(3)運動學方程：

式中，v為履帶式車輛(3)在全局坐標系下的前進速度，ω為履帶式車輛(3)在全局坐標系下的轉向角速度；

定義全局坐標系XOY中履帶式車輛(3)的軌跡跟蹤誤差為[x_r-x,y_r-y,θ_r-θ]^T，則履帶式車輛(3)自身局部坐標系xoy下，履帶式車輛(3)的軌跡跟蹤誤差方程為：

式中，x_r、y_r、θ_r為履帶式車輛(3)在全局坐標系下的參考位姿，x_e、y_e、θ_e為履帶式車輛(3)在局部坐標系下的位姿偏差；

對公式2)進行微分，并結合公式1)的運動學方程可得到履帶式車輛(3)的軌跡跟蹤誤差微分方程為：

式中，v_r為履帶式車輛(3)在全局坐標系下的參考前進速度，ω_r為履帶式車輛(3)的在全局坐標系下的參考轉向角速度；

2)組織層控制器設計

根據履帶式車輛(3)軌跡跟蹤誤差模型設計車輛位姿滑模控制器，依據分段跟隨控制策略將理論規劃路徑分解為履帶式車輛(3)位置和航向的參考路徑，建立車輛位姿滑模控制器的狀態向量為：

對公式4)進行微分，并結合公式3)可以得到：

建立車輛位姿滑模控制器的滑模面s₁、s₂：

式中，參數ξ₁、ξ₂0；

對滑模面s₁、s₂進行求導，并代入公式3)可以得到：

選擇滑模指數趨近律如下：

式中，參數κ₁、κ₂、λ₁、λ₂0，slaw表示滑模趨近律；

最終，結合公式7)和8)得到履帶式車輛(3)的位姿滑模控制律為：

通過如下的狀態估計模型在線辨識轉彎過程中履帶式車輛(3)的速度和角速度：

式中，為n時刻估計的履帶式車輛(3)速度和角速度，r_L、r_R分別為履帶式車輛(3)左右驅動輪的半徑，ω_L(n-1)、ω_R(n-1)分別為n-1時刻履帶式車輛(3)左右驅動輪的角速度，i_L、i_R分別為履帶式車輛(3)左右履帶的滑移系數，D表示履帶式車輛(3)車體的寬度，b表示履帶的寬度；

結合公式1)并通過積分運算估計出轉彎過程中履帶式車輛(3)的位置和航向，利用卡爾曼濾波器對狀態估計器和GNSS導航系統的數據進行融合；

3)協調層控制器設計

根據履帶速度映射關系將履帶式車輛(3)的速度v和角速度ω的控制量轉成左右履帶驅動輪速度v_L、v_R的控制量，映射關系如公式11)所示：

式中：D表示履帶式車輛(3)車體的寬度，b表示履帶的寬度。