[發明專利]一種非結構化道路全局路徑規劃方法與系統

有效

• 申請號: 202011406640.4
• 申請日: 2020-12-04
• 公開（公告）號: CN112612266B
• 公開（公告）日: 2022-04-01
• 發明（設計）人: 徐彪;陳鑫鵬;秦兆博;王曉偉;邊有鋼;秦洪懋;秦曉輝;謝國濤;胡滿江;丁榮軍
• 申請（專利權）人: 湖南大學
• 主分類號: G05D1/02
• 分類號: G05D1/02
• 代理公司: 廣州容大知識產權代理事務所(普通合伙) 44326
• 代理人: 劉新年

1.一種非結構化道路全局路徑規劃方法，其特征在于：包括如下步驟：

步驟S1，輸入車輛起始位姿、目標位姿和全局地圖信息，設置搜索空間的柵格位置分辨率ξ和角度分辨率γ，并根據柵格位置分辨率ξ對地圖進行柵格化處理，其中，起始位姿信息和目標位姿信息包括車輛的后軸中心坐標信息(x，y)和車輛橫擺角信息

步驟S2，設置評估函數F＝G+H，并通過預計算得到H值查詢表，預計算的方式為選取廣度優先算法、Dijkstra算法或動態規劃算法的其中一個，在二維柵格地圖中計算得到從每個離散柵格位置到達目標柵格位置的最近距離作為啟發值H；

步驟S3，初始化open集與close集，將起始節點放入open集中，該起始節點包含車輛位姿信息車輛擋位信息d、所在位置柵格信息所在角度柵格信息G值、F值和父節點信息，其中的車輛檔位信息d包括車輛的前進、倒退信息；所述的位置柵格信息和角度柵格信息通過以下各式計算得到：

其中，o為選取的坐標原點；

步驟S4，從open集中選出F值最小的節點作為當前節點，并將該節點從open集中移除，加入到close集中，若open集為空，則規劃失敗，即無法找到從起始位姿到達目標位姿的可行路徑；

步驟S5，判斷當前點是否到達終點附近，若是，則采用曲線擬合方法精確擬合得到從當前位姿到目標位姿的路徑；若否，則執行步驟S7；

步驟S6，對曲線擬合得到的路徑中的各個離散路徑點進行碰撞檢測，若所有的離散路徑點位姿都通過碰撞檢測，則搜索結束，執行步驟S12，否則執行步驟S7：

步驟S7，以當前節點作為父節點，其位姿信息與車輛擋位信息可表述為根據車輛運動學模型分別向前和向后進行節點拓展，得到若干個待處理子節點；

步驟S8，選擇一個子節點，判斷車輛在該子節點處是否與障礙物發生碰撞，若無碰撞則計算其所在位置柵格信息與所在角度柵格信息，并計算從起始節點到該子節點的累計代價G_i，然后根據該子節點所在位置柵格信息在H值查詢表中查得該子節點的啟發值H_i，繼而計算出F_i；

步驟S9，判斷選擇的子節點是否已經存在于close集當中，若是，則放棄該節點，返回步驟S8，其中判斷方法為：計算得到當前節點的所在位置柵格信息、角度柵格信息和車輛擋位信息，遍歷close集判斷其中是否存在具有同樣上述信息的節點；

步驟S10，判斷選擇的子節點是否已經存在于open集當中，若是，則進行累計代價比較與節點更新處理；若否，則將該節點加入到open集當中；

步驟S11，以步驟S4所述的當前節點位姿作為起點，根據每個節點所存儲的父節點信息進行路徑回溯，得到搜索出的初始路徑；

步驟S12，對搜索出的初始路徑進行圓弧插值處理，得到一條路點間隔均勻的路徑，并結合曲線擬合路徑，最終得到從起始位姿到達目標位姿的全局路徑；所述步驟S10中的累計代價G_i比較與節點更新處理的具體方法為：判斷存在于open集中的節點N_o的累計代價值G_o與當前節點N_i的累計代價值G_i的大小，若G_o＞G_i，則將open集中存放的節點N_o的位姿信息替換為節點N_i的位姿信息，車輛擋位信息d_o替換為d_i，然后將累計代價值G_o替換為G_i，最后將父節點信息替換為節點N_i的父節點信息；若G_o＜G_i，則不進行任何處理，繼續執行步驟S8，直到所有子節點處理完畢后返回步驟S4；所述步驟S7根據車輛運動學模型分別向前和向后進行節點拓展具體包括以下步驟：

S7-1，設置車輛的軸距W_b、最大前輪轉向角θ_max；

S7-2，確定前輪轉角離散數量N，將前輪轉角范圍[-θ_max，θ_max]進行均勻離散，得到每個離散位置的前輪轉向角計算公式為：

θ_i＝-θ_max+i·Δθ

其中，i＝0，1，...，N-1；Δθ為相鄰轉角差，計算公式為：

S7-3，確定從每個離散角度處向前或向后拓展的子節點數量M； 當向后拓展時，在該離散角度處的拓展子節點數量M固定為1；當向前拓展時，在該離散角度處的拓展子節點數量M由下式計算得到：

其中，i與步驟S7-2中式(1)中的i一致；N為前輪轉角離散數量，其必須為一個奇數，可根據實際需要和角度離散分辨率γ合理選擇；

S7-4，從父節點開始，在每個離散前輪轉角處根據車輛運動學模型生成M個子節點

其中，k＝1，2，...，M，即當k取不同數值時，在同一個離散前輪轉角處可拓展得到多個子節點；L為拓展步長；W_b為車輛軸距；R為車輛的轉彎半徑；f為正負號標志，向前拓展時f取1，向后拓展時f取-1；

S7-5，將所有拓展得到的子節點的父節點都設為同一點，即步驟S7-4中的N_p。