[發明專利]一種無人駕駛車輛路徑規劃與軌跡跟蹤方法

有效

• 申請號: 202110733880.3
• 申請日: 2021-06-30
• 公開（公告）號: CN113359757B
• 公開（公告）日: 2022-07-01
• 發明（設計）人: 楊正才;楊俊;周奎;蔡林;姚勝華;張友賓;呂科;尹長城;馮櫻
• 申請（專利權）人: 湖北汽車工業學院
• 主分類號: G05D1/02
• 分類號: G05D1/02
• 代理公司: 濟南泉城專利商標事務所 37218
• 代理人: 張貴賓

1.一種無人駕駛車輛路徑規劃與軌跡跟蹤方法，其特征在于：包括以下步驟：

步驟1：設置車輛的初始點、目標點位置、姿態，并將起點信息數據進行預處理，然后放入Open表中；

步驟2：以車載激光雷達檢測的數據為基礎，將一定范圍的車輛周圍環境柵格化，另外根據車速高低及檢測到的障礙物遠近分布情況，建立不同邊長的柵格地圖，為實時規劃減少計算量；

步驟3：設計混合A*算法進行路徑規劃，通過對啟發函數增加轉向約束，保證規劃的路徑充分平滑；

步驟4：路徑搜索：在相同時間內，給車輛固定速度、不同的轉向進行前向模擬路徑搜索，此過程中，將車輛運動學模型線性化，求解搜索末端的車輛狀態；

步驟5：計算不同搜索末端的車輛狀態代價，將代價最小的車輛狀態放入Close表中，同時作為下次規劃的起始節點；

步驟6：判斷搜索車輛狀態是否滿足條件，如果不滿足，持續上述路徑搜索并計算代價函數，直至滿足為止；

步驟7：Close表中的節點，即為最終軌跡的關鍵節點，將其作為期望軌跡，采用自適應模型預測算法進行軌跡跟蹤；

步驟8：設置一段時間內累計的軌跡誤差闕值，若軌跡誤差大于闕值，用軌跡曲率計算的縱向速度更新預測模型的輸出量，直到保持在闕值范圍內，以增強車輛行駛的安全性;

具體的詳細步驟如下所示：

步驟1：預設數據處理，具體包括：車輛起始點的位置和姿態及目標點的位置和姿態，及車輛當前的車輪轉角和車速，其中X、Y表示車輛在大地坐標系下的縱、橫坐標，表示車輛的橫擺角；

步驟2：車載激光雷達檢測周圍障礙物信息，并繪制柵格地圖；

步驟3：激光雷達根據檢測到的障礙物距離、分布情況及當前車速，以車輛后軸中心為原點，在步驟2的基礎上建立不同邊長的單位柵格，分為S級、M級和L級，將每一個柵格標記為自由區域和障礙區域；

步驟4：根據車輛軸距和車速因素，對障礙區域進行膨脹處理；膨脹后的單位柵格邊長為：

T為采樣時間，為權重系數，l為車輛軸距，L為障礙區域所在單位柵格邊長；

步驟5：設計混合狀態A*的代價函數，包括：已行駛路徑的距離代價和啟發代價；啟發代價又包括當前位置、姿態到目標位置姿態的估計代價、下一次的轉向切換代價及累計的轉向代價，特別是對于反向路徑規劃的轉向代價，施加乘法懲罰；其中，采用歐式距離計算距離代價；

步驟6：讀取起始點和目標點信息，創建Open表和Close表；把起點的位置和姿態信息放入Open中，同時將Close置空，判斷Open表是否為空表，若為空表則規劃失敗，如Open表非空，選擇Open表中代價最小的節點作為當前節點，并將其放入Close表中；

步驟7：以當前節點作為初始節點，在時間內，以固定的前輪轉向角、縱向速度前向模擬一段始于，止于的軌跡，計算不同的代價，將代價最小的放入closed-list中，同時將代價最小的其視為下一次規劃的；

步驟8：以車輛運動學模型表示車輛模型，車輛的位置、姿態與前輪轉向角存在如下關系：

，式中為車輛軸長；

將上述車輛運動學模型線性化，線性化后的車輛模型以狀態空間表示：

其中：

在時刻，計算節點的狀態；

步驟9：拓展節點的過程中，沒有考慮車輛周圍的障礙物，拓展的模擬路徑難以保證車輛避障，這里采用RS曲線間接解決車輛避障問題，RS曲線雖然不能避障，但突出優點是計算速度快，RS曲線由不同的圓弧和直線段組成，總是可以在平面內連接任意位置的位姿，設置迭代次數，直到連接起點和終點的RS曲線能夠避開障礙物為止，退出RS模式，為進一步提高車輛的實時性，沒有必要在每一次拓展節點的過程中都使用RS曲線連接和，僅僅在障礙物分布密集的場景及前向模擬的路徑不能避障使下用；

步驟10：判斷，如果是，則判定規劃成功，輸出最終軌跡，最終軌跡由closed表中的節點組成，節點之間是模擬軌或RS曲線連接，如果否，則返回步驟7，8，直到滿足條件為止；

步驟11：將最終軌跡作為軌跡跟蹤的參考軌跡，設計軌跡跟蹤自適應模型預測控制器，具體包括：車輛模型線性化、離散化，組合狀態量與控制量的狀態空間模型、計算預測模型的輸出量、狀態量與控制量及其增量的約束條件設計、權重矩陣Q、R調整、目標函數的滾動優化求解、控制增量的反饋校正、由曲率計算的縱向速度更新預測模型的輸出量；

在軌跡跟蹤控制器的設計中，以車輛運動學模型表示車輛，以步驟10中的輸出軌跡作為期望軌跡，進行線性化、進行離散化，如下

其中，T采樣時間，為車輛軸長；

步驟12：計算參考軌跡不同點的曲率，根據曲率設計車輛在各處位置的縱向速度，

其中，曲率，縱向速度，a，b，c可根據軌跡形狀調整；

步驟13：計算一段時間內車輛實際軌跡與期望軌跡的累計偏差，若偏差大于闕值，則從下一時刻開始，用縱向速度代替更新系統的預測輸出量；若偏差在規定闕值內，則下一時刻的速度與上一時刻保持相同。