[發明專利]一種履帶拖拉機及其路徑跟蹤控制方法和系統

有效

• 申請號: 201810008519.2
• 申請日: 2018-01-04
• 公開（公告）號: CN110007667B
• 公開（公告）日: 2021-06-11
• 發明（設計）人: 賈全;苑嚴偉;偉利國;毛文華;趙博;付拓;張愛國
• 申請（專利權）人: 中國農業機械化科學研究院
• 主分類號: G05D1/02
• 分類號: G05D1/02
• 代理公司: 北京律誠同業知識產權代理有限公司 11006
• 代理人: 梁揮;祁建國

1.一種路徑跟蹤控制方法，其特征在于，包括：利用拖拉機主驅動輪半徑及其轉速計算兩側履帶的理論速度，結合履帶拖拉機滑移特性建立滑移率與該理論速度，建立航向預估模型；根據采樣周期和該航向預估模型計算得到航向變化預估量；把拖拉機當前航向和該航向變化預估量之和作為航向反饋量，底層控制器根據期望航向與該航向反饋量，計算得到控制輸出，控制輸出控制電控裝置動作，實現履帶拖拉機轉向以跟蹤期望路徑；

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

步驟1，通過定位系統對拖拉機進行測速，得到該拖拉機的移動速度；

步驟2，根據該拖拉機履帶與地面的接觸特性、該移動速度以及該拖拉機兩側履帶的理論速度，得到該拖拉機兩側履帶的滑移率；

步驟3，根據該滑移率、該拖拉機兩側履帶的理論速度和該拖拉機的履帶間距，獲得該拖拉機的航向變化率表達式作為該航向預估模型；

步驟4，根據該航向預估模型和采樣周期，計算該拖拉機的航向變化預估量；

步驟5，將該航向變化預估量和該拖拉機的當前航向之和作為航向反饋量，根據該航向反饋量和期望航向，控制該拖拉機的舵機角度，以使該拖拉機跟蹤期望路徑；

該步驟2還包括：根據該拖拉機兩側驅動輪的轉速和尺寸，得到該履帶拖拉機兩側履帶的該理論速度：

v_r＝w_r·r

v_l＝w_l·r

式中，v_l和v_r分別為左右兩側履帶的理論速度，w_r為右側驅動輪轉速，w_l為左側驅動輪轉速，r為驅動輪半徑；

該步驟2還包括，通過聯立以下公式確定該滑移率：

式中n為可調參數，v為該移動速度，v_l和v_r分別為左右兩側履帶的理論速度，a_l和a_r分別為左右兩側履帶的滑移率；

該步驟5還包括：將該航向反饋量和期望航向輸入至底層控制器，該底層控制器根據該該航向反饋量和期望航向，生成調整控制量，以控制該拖拉機的舵機角度；

其中該底層控制器采用增量式PID控制算法，通過以下公式生成該調整控制量：

Δμ＝μ(k)-μ(k-1)＝K_p[e(k)-e(k-1)]+K_ie(k)+K_d[e(k)-2e(k-1)+e(k-2)]

式中K_p，K_i和K_d分別為比例、積分和微分系數，μ(k)表示第k個采樣時刻的控制量，e(k)表示第k個采樣時刻的航向輸入偏差，k為自然數，Δμ為該調整控制量；

該步驟3還包括，通過以下公式確定該航向變化率表達式：

為航向變化率，a_l和a_r分別為左右兩側履帶的滑移率，d為履帶間距；

步驟4中的該航向變化預估量通過以下公式確定：

式中v_l和v_r分別為左右兩側履帶的理論速度，a_l和a_r分別為左右兩側履帶的滑移率，d為履帶間距，T為采樣周期，Δθ為該航向變化預估量。