[發明專利]一種非完整約束條件下的連續曲率路徑優化方法

在審

• 申請號: 202010797068.2
• 申請日: 2020-08-10
• 公開（公告）號: CN112269965A
• 公開（公告）日: 2021-01-26
• 發明（設計）人: 汪洋;李勝飛;程文;趙熙俊;崔星;高建鋒
• 申請（專利權）人: 中國北方車輛研究所
• 主分類號: G06F17/11
• 分類號: G06F17/11;G06F30/20;G06F111/04
• 代理公司: 北京理工大學專利中心 11120
• 代理人: 許姣

本發明提供一種非完整約束條件下的連續曲率路徑優化方法，該方法構造出來的平滑曲線滿足非完整約束條件、曲率連續、存在解析解便于實時計算，有助于提高路徑跟蹤控制的精度。具體步驟包括：依據車輛轉向能力約束確定可行駛的最大曲率；從原始路徑上起始點開始按序選取三個路徑點；構造兩段參數化的三階Bezier曲線，確定參數值以滿足曲率連續和最大值受限雙重約束條件；依據參數化方程計算平滑曲線上各點的曲率；對平滑曲線進行非均勻離散化，獲取平滑后的目標路徑點；重復上述步驟完成對全局路徑點的平滑處理；檢查平滑曲線與原路徑點的擬合匹配情況，并與障礙地圖進行碰撞檢測。由此，最終得到的優化路徑兼顧了無人車輛行駛控制的可通過性和平順性。

技術領域

本發明屬于自動駕駛或無人駕駛車輛路徑規劃與自動控制技術領域，具體涉及一種非完整約束條件下的連續曲率路徑優化計算方法。

背景技術

對于無人駕駛車輛而言，目標路徑如果按直線連接會給路徑跟蹤控制帶來兩個問題，其一，路徑點構成的線段存在曲率跳變增大了控制量以及控制量的變化程度，系統能耗和沖擊都會增大；其二，車輛行駛不是任意的路線都可以通過的，需要滿足非完整約束條件，即最大曲率或最小轉向半徑受限，如果不滿足該條件，會加大系統的跟蹤誤差，甚至出現車輛失穩，影響行駛安全。

針對目標路徑點的平滑與優化處理方法，目前已有研究提出采用回旋曲線Clothoids或B-Spline樣條曲線來構造，然而回旋曲線方程擬合缺乏閉環解析解，B樣條曲線參數化函數過于復雜，曲率連續和曲率最大值約束難以同時滿足。一些研究構造出來的平滑曲線也往往忽略或簡化了車輛的行駛通過能力，缺乏后續的可行解檢查和碰撞檢測，無法確保車輛行駛的安全和平順性。

發明內容

有鑒于此，本發明提出一種非完整約束條件下的連續曲率路徑優化方法，該方法構造出來的平滑曲線滿足非完整約束條件、曲率連續、存在解析解便于實時計算、能夠實現可行解檢查以及碰撞檢測，采用該方法對無人駕駛車輛進行路徑優化，提高了車輛行駛的安全性、平順性，也有助于提高路徑跟蹤控制的精度，降低極限彎道下出現的車輛失穩的可能。

所述的非完整約束條件下的連續曲率路徑優化方法，具體步驟包括：

步驟一：依據車輛行駛能力約束計算車輛可行駛路徑的最大曲率，記為κ_max；

步驟二：令給定的期望路徑包括n個離散路徑點，依次為P₁,P₂…P_n，n≥3；在給定的期望路徑上，從起始點開始選取三個路徑點；

步驟三：在選取的路徑點的連線上選取控制點位置以構造兩條幾何對稱的Bezier曲線段；并計算兩條幾何對稱的Bezier曲線段的特征長度，確定滿足兩條Bezier曲線連接點處曲率連續以及非完整約束條件時的曲線參數化方程；所述非完整約束條件指最大曲率值為κ_max；

步驟四：對步驟三構造的Bezier曲線段進行采樣，得到一系列離散點作為目標路徑點；

步驟五：在給定的期望路徑上，按序依次選取三個路徑點，對每次選取的三個路徑點重復上述步驟三至步驟四，直到給定的期望路徑中的所有離散路徑點計算結束，得到平滑曲線；每次選取時去掉當前三個路徑點中的第一個路徑點，然后增加下一個路徑點；

步驟六：檢查步驟五得到的平滑曲線與期望路徑中離散路徑點的擬合匹配情況，當平滑曲線中出現往回折返的錯誤結果時，調整給定的期望路徑中的路徑點，然后返回至步驟一重新進行路徑優化。

作為本發明的一種優選方式，還包括：

步驟七；對平滑曲線與已知的障礙地圖進行障礙物的碰撞檢測，若出現平滑曲線與障礙物碰撞的情況，則調整給定的期望路徑中的路徑點，然后返回至步驟一重新進行路徑優化。

作為本發明的一種優選方式，所述步驟一中：