[發明專利]一種基于虛擬約束的自動駕駛車輛路徑規劃方法

有效

• 申請號: 201710161509.8
• 申請日: 2017-03-17
• 公開（公告）號: CN107121980B
• 公開（公告）日: 2019-07-09
• 發明（設計）人: 熊光明;郭孔輝;龔建偉;陳慧巖
• 申請（專利權）人: 北京理工大學
• 主分類號: G05D1/02
• 分類號: G05D1/02;G01C21/28;G01C21/34
• 代理公司: 北京天達知識產權代理事務所(普通合伙) 11386
• 代理人: 王濤;龐許倩

1.一種基于虛擬約束的自動駕駛車輛路徑規劃方法，其特征在于，包括以下步驟：

步驟S1：獲取車體坐標系下的車道中心線航向；

步驟S2：利用步驟S1中獲取的車道中心線航向對車輛航向進行修正；

步驟S3：利用步驟S2修正后的車輛航向對原始路徑中的路徑點進行校正；

步驟S4：基于三維激光雷達柵格地圖，獲取自動駕駛車輛與道路的相對橫向位置關系；

步驟S5：根據步驟S4獲取的自動駕駛車輛與道路的相對橫向位置關系生成虛擬約束，所述虛擬約束包括所在車道的邊界約束和鄰車道邊界約束；

步驟S6：根據步驟S5生成的虛擬約束和自動駕駛系統的決策指令，生成期望路徑。

2.根據權利要求1所述的方法，其特征在于，所述步驟S1包括以下步驟：

步驟S101：判斷車道線檢測結果是否有效，若有效，執行步驟S102；若無效，沿用上一周期車道線檢測結果；

步驟S102：獲取車體坐標系下的車道中心線航向。

3.根據權利要求2所述的方法，所述步驟S102包括以下步驟：

步驟S1021：計算車道中心線的位置

其中，車道線檢測結果(x₁,y₁)、(x₂,y₂)、(x₃,y₃)與(x₄,y₄)為左右車道線在車體坐標系下的坐標點，(x₁,y₁)為車輛左前方近車點，(x₂,y₂)為左前方遠車點，(x₃,y₃)為右前方近車點，(x₄,y₄)為右前方遠車點；

(x_c1,y_c1)與(x_c2,y_c2)分別代表車道中心線的近車端與遠車端；

步驟S1022：計算車道中心線的航向：

其中θ_Lane代表車道中心線在車體坐標系下的航向。

4.根據權利要求2或3所述的方法，其特征在于，步驟S2采用卡爾曼濾波器對車輛航向進行修正，包括以下步驟：

步驟S201：初始化卡爾曼濾波器；

步驟S202：根據車輛當前的轉彎半徑和速度預測本周期內的大地坐標系下車輛航向預測值：

其中，θ_veh-pred為大地坐標系下當前時刻車輛航向的預測值，θ_{veh-correct-old}為大地坐標系下上一周期車輛航向的修正值，v為車輛當前的速度，R為車輛當前的轉彎半徑，T為規劃周期；

步驟S203：計算車體坐標系下原始路徑航向與車道中心線航向的差值：

θ_error＝θ_road-θ_Lane

其中，θ_error為車體坐標系下原始路徑航向與車道中心線航向的差值，θ_road為車體坐標系下原始路徑的航向，θ_Lane為車道中心線航向；

步驟S204：計算大地坐標系下車輛的航向測量值：

θ_veh-measure＝θ_veh-gps+θ_error，

其中θ_veh-measure為大地坐標系下車輛的航向測量值，θ_veh-gps為大地坐標系下車輛的GPS航向值；

步驟S205：將步驟S202中θ_veh-pred與步驟S204中θ_veh-measure作為卡爾曼濾波器的輸入得到車輛在大地坐標系下的航向修正值θ_veh-correct；

步驟S206：將θ_veh-correct賦值給θ_{veh-correct-old}并進入到下一周期。