[發明專利]基于閉環控制的路徑規劃方法及裝置

有效

• 申請號: 201910201621.9
• 申請日: 2019-03-18
• 公開（公告）號: CN109900289B
• 公開（公告）日: 2023-02-10
• 發明（設計）人: 左思翔;徐成;張放;李曉飛;張德兆;王肖;霍舒豪
• 申請（專利權）人: 北京智行者科技股份有限公司
• 主分類號: G01C21/34
• 分類號: G01C21/34;G05D1/02
• 代理公司: 北京慧誠智道知識產權代理事務所(特殊普通合伙) 11539
• 代理人: 李楠

1.一種基于閉環控制的路徑規劃方法，其特征在于，所述方法包括：

獲取車輛的參考路徑，所述參考路徑包括起點和終點；

在所述參考路徑上取第一終點；所述第一終點位于所述起點和終點之間；

判斷以所述起點和所述第一終點間的距離為直徑的圓內是否存在障礙物；

當存在第一障礙物時，確定所述起點與所述第一障礙物的第一距離；所述第一障礙物為障礙物中距離起點最近的障礙物；所述第一距離為所述起點距離所述第一障礙物的距離減去自車的安全距離之差；

以所述起點為圓心，以所述第一距離為半徑，設置第一父圓；

以所述第一終點為圓心，以所述第一距離為半徑，設置第一終點圓；

以所述第一父圓的圓周上的點為圓心，確定該圓周上的多個圓心中的每個與距離其最近的障礙物的距離，生成子距離集；

計算該圓周上的多個圓心中的每個與所述第一終點的幾何距離，生成幾何距離集；

根據所述子距離集和所述幾何距離集，計算多個圓心對應的子圓的啟發值集；所述子圓的半徑為圓心與距離其最近的障礙物的距離減去自車的安全距離之差；

確定所述啟發值集中最小的啟發值對應的子圓為第二父圓；

當第m父圓與所述第一終點圓的重合度大于預設第一閾值時，生成第一列表和第二列表；所述第一列表包括所述第一父圓至第m父圓的圓心位置和半徑；所述第二列表包括以所述第一父圓的圓周為圓心的子圓至以所述第m父圓的圓周為圓心的子圓中圓心的位置和半徑；m為大于2的整數；

在當前時刻，獲取所述車輛的第一參數；

根據所述當前時刻的第一參數和車輛的動力學模型，計算車輛在下一時刻的第二參數；

根據所述第一參數和所述第二參數，計算所述車輛從當前時刻至下一時刻的第一軌跡集；

根據所述第一列表和所述第二列表，對所述第一軌跡集進行處理，生成處理后的第一軌跡集；

通過啟發值函數，對處理后的所述第一軌跡集中的軌跡進行評價；

根據評價結果，從處理后的所述第一軌跡集中確定第一目標軌跡；

在所述第一目標軌跡上確定預瞄點的位置；

根據車輛的當前速度和所述預瞄點的位置，計算預瞄距離；

根據所述預瞄距離和車輛的動力學模型，計算預瞄點的轉向角度；

根據所述預瞄點的期望速度和車輛的當前速度，計算車輛在預瞄點的加速度；

將所述第一目標軌跡的預瞄點的轉向角度和加速度發送給底層控制器，以使所述底層控制器根據所述預瞄點的轉向角度和加速度，在所述預瞄點對車輛進行控制；

當第n目標軌跡的終點與所述終點的距離之差小于預設第二閾值時，將所述第一目標軌跡至第n目標軌跡進行處理，生成目標路徑；n為大于2的整數。