[發明專利]一體式平行泊車軌跡規劃和跟蹤控制方法及系統

在審

• 申請號: 202110882890.3
• 申請日: 2021-08-02
• 公開（公告）號: CN113635891A
• 公開（公告）日: 2021-11-12
• 發明（設計）人: 劉立;董國新;王國棟;孟宇;白國星
• 申請（專利權）人: 北京科技大學
• 主分類號: B60W30/06
• 分類號: B60W30/06;B60W40/00;B60W60/00
• 代理公司: 北京市廣友專利事務所有限責任公司 11237
• 代理人: 張仲波;付忠林

1.一種一體式平行泊車軌跡規劃和跟蹤控制方法，其特征在于，包括：

搭建車輛運動學模型；

基于所述車輛運動學模型，設計預測模型和泊車系統的約束；

設計代價函數，將車輛的速度規劃、路徑規劃和跟蹤控制整合為一個優化問題，得到服從于所述預測模型和所述約束的控制問題描述；

采用內點法求解所述控制問題，得到最優開環控制序列，選取所述最優開環控制序列中的第一組元素應用于待控制的車輛，實現自動平行泊車。

2.如權利要求1所述的一體式平行泊車軌跡規劃和跟蹤控制方法，其特征在于，所述車輛運動學模型的表達式為：

其中，v為車輛的運動速度，和分別為車輛在X和Y方向上的速度分量，θ為車輛的航向角，δ為車輛的轉向輪等效轉角，l為車輛的軸距。

3.如權利要求2所述的一體式平行泊車軌跡規劃和跟蹤控制方法，其特征在于，基于所述車輛運動學模型，設計預測模型和泊車系統的約束，包括：

將式(1)向量化，得到時域預測模型：

其中，ξ＝[x y θ]^T為狀態變量，u＝[v δ]^T為控制輸入，(t)表示時域；

采用式(4)，將所述時域預測模型轉化空間域預測模型：

其中，(s)表示空間域，s和分別為車輛行駛過的路程及其變化率；

得到空間域預測模型如下：

將所述空間域預測模型離散化，得到離散化的預測模型：

其中，(k_s+1)表示相對當前采樣點位置(k_s)下一個步長對應的位置；

將離散化的預測模型轉化為增量預測模型：

其中，Δu(k_s)＝u(k_s)-u(k_s-1)，(k_s)表示當前采樣位置，(k_s-1)表示前一采樣點的位置；

假設預測時域為N_p，根據模型預測控制理論，將式(7)所示的增量預測模型在預測時域N_p內的預測方程表示為：

設計泊車系統的約束，以對終止時刻車輛的速度和車身姿態、泊車過程中車輛的橫向位置和縱向位置，以及執行器進行約束。

4.如權利要求3所述的一體式平行泊車軌跡規劃和跟蹤控制方法，其特征在于，將所述空間域預測模型離散化，包括：

采用四階龍格庫塔方法，以離散步長h＝0.1，對式(5)進行離散化。

5.如權利要求3或4所述的一體式平行泊車軌跡規劃和跟蹤控制方法，其特征在于，設計泊車系統的約束，以對終止時刻車輛的速度和車身姿態、泊車過程中車輛的橫向位置和縱向位置，以及執行器進行約束，包括：

設計泊車目標位置和姿態約束，對終止時刻車輛速度和車身姿態進行約束：

其中，(x_ref,y_ref)為泊車目標點的位置；(x_r,y_r)為車輛后軸的中點；

設計安全約束，對泊車過程中車輛的橫向位置和縱向位置進行約束：

其中，X_left和X_right分別為庫位的左右邊界位置，a為前懸長度與軸距以及車頭安全裕度之和，b為后懸長度與車尾安全裕度之和，Y_bound為車庫的側邊界，w為車輛的寬度；

設計執行器約束，對執行器進行約束：

其中，Δv_min和Δv_max是設定的速度增量輸入的上、下限，Δδ_min和Δδ_max是設定的前輪轉向角變化量輸入的上、下限，δ_min和δ_max是前輪轉角的上、下限。