[發明專利]一種無人駕駛車輛路徑規劃與控制系統及方法

有效

• 申請號: 201910236824.1
• 申請日: 2019-03-27
• 公開（公告）號: CN109945882B
• 公開（公告）日: 2021-11-02
• 發明（設計）人: 楊明;郝繼偉;袁偉;王春香;王冰
• 申請（專利權）人: 上海交通大學
• 主分類號: G01C21/34
• 分類號: G01C21/34;G01C21/32;G05D1/02
• 代理公司: 上海恒慧知識產權代理事務所(特殊普通合伙) 31317
• 代理人: 徐紅銀;劉翠

1.一種無人駕駛車輛路徑規劃與控制方法，其特征在于，采用無人駕駛車輛路徑規劃與控制系統，該系統包括：行程信息獲取模塊、總控模塊、路徑規劃模塊、通信模塊以及定位控制模塊；其中：

所述行程信息獲取模塊，獲取行程的起點及終點信息作為系統輸入信息發送至總控模塊；

所述總控模塊，接收行程的起點及終點信息后，通過路徑規劃模塊生成無人駕駛車輛的路徑，并通過通信模塊將路徑輸出至定位控制模塊；

所述定位控制模塊，根據接收到的路徑，控制無人駕駛車輛行駛，并對運行中的無人駕駛車輛進行定位，將定位結果及車輛行駛狀態通過通信模塊反饋至總控模塊，實現無人駕駛車輛按照路徑規劃模塊生成的路徑進行自動駕駛。

所述無人駕駛車輛路徑規劃與控制方法，包括：

獲取行程的起點及終點信息；

根據行程的起點及終點信息，生成從起點到終點的全局路徑；

使用跟蹤算法，控制無人駕駛車輛沿全局路徑行駛，并對運行中的無人駕駛車輛進行定位，將定位結果及車輛行駛狀態進行實時反饋；

根據定位結果及車輛行駛狀態的反饋結果，確定全局路徑的不可用情況，并生成局部路徑對全局路徑進行修正，并以修正后的全局路徑控制無人駕駛車輛行駛；

所述全局路徑通過以下方法得到：

S1，建立無人駕駛車輛運行區域的全局地圖，在能夠通行的道路上每隔m米點劃定一個GPS坐標點，稱作節點，將節點連成線，稱作路徑，將沿道路前進方向稱作路徑的方向；

S2，建立一個開集合queue和一個閉集合CLOSE；啟發函數f(n)為路徑從起點出發經由節點n通向終點的距離長度，其值為由起點通向節點n的實際距離g(n)與由節點n到終點的最佳路徑估計距離h(n)之和；若由節點x有一條路徑通向節點y，稱節點y為節點x的子節點；

S3，找到起點的全部子節點i，將其全部放入開集合queue，計算所有子節點的啟發函數值f(i)，并將開集合queue中的節點按照f(i)的大小由小到大排序；

S4，從開集合queue中取出第一個節點X，并把節點X從開集合queue中刪去，如果節點X是終點，則從節點X回溯到起點生成路徑path，返回path列表，如果節點X不是終點，則取出節點X的每個子節點Y，如果子節點Y不在開集合queue也不在閉集合CLOSE中，則將子節點Y加入開集合queue，如果子節點Y已經在開集合queue中，則計算啟發函數f(Y)的值，若此時f(Y)的值小于其在開集合queue中的值，則更新集合queue中Y的啟發函數值，如果子節點Y在閉集合CLOSE中，則計算啟發函數f(Y)的值，若此時f(Y)的值小于其在閉集合CLOSE中的值，則更新閉集合CLOSE中Y的啟發函數值，同時把子節點Y從閉集合CLOSE中刪去加入到開集合queue中；

S5，把節點X放入閉集合CLOSE中；

S6，將開集合queue中的節點按照啟發函數值f(i)由小到大排序；

S7，重復S4、S5和S6，直到生成一條全局路徑；

所述局部路徑采用以下方法得到：

s 1，將起點坐標初始化為根節點I；

s2，使用采樣函數，從前方遮擋的激光雷達柵格圖中隨機選取一個采樣點S；

s3，依照最近原則，從隨機樹中選擇一個距離采樣節點S最近的節點N；

s4，采用隨機生長原則，從采樣節點S向節點N擴展一段距離，得到一個新的節點Q；

s5，如果節點Q與障礙物發生碰撞，則返回為空，放棄此次生長，否則將節點Q放入到隨機樹之中；

s6，重復S2、S3、S4及S5，直到找到的節點Q小于設定的距離閾值，則搜索成功，返回一條局部路徑，如果在設定搜索次數內或搜索時間內沒有找到目標的，則視為搜索失敗；

所述總控模塊包括：數據管理單元和任務分派單元；其中：

所述數據管理單元存儲無人駕駛車輛的運行狀況和行駛狀態；其中，所述行駛狀態包括無人駕駛車輛當前行駛的位置、速度以及方向；

所述任務分派單元對無人駕駛車輛進行選擇與任務分派，對于每一組行程的起點及終點信息，選擇與起點信息的地理位置最近的無人駕駛車輛分派任務；

所述路徑規劃模塊包括全局路徑規劃單元以及局部路徑規劃單元：其中：

所述全局路徑規劃單元用于生成從起點到終點的全局路徑；

所述局部路徑規劃單元用于在全局路徑不可用時生成繞過不可用區域的局部路徑；

所述全局路徑和局部路徑分別為經度及緯度坐標序列；

所述定位控制模塊連接于無人駕駛車輛上，包括定位傳感器以及車輛控制單元；其中：

所述定位傳感器包括GPS傳感器、激光雷達傳感器和視覺傳感器，所述GPS傳感器通過接收GPS信號對無人駕駛車輛行駛的地理位置進行定位，得到路徑GPS信息和定位GPS信息；所述激光雷達傳感器和視覺傳感器通過實時檢測道路標線以及路沿信息，對無人駕駛車輛行駛的道路位置進行定位；

所述車輛控制單元對無人駕駛車輛的速度及方向進行控制；

所述定位控制模塊采用跟蹤算法對無人駕駛車輛進行控制，包括：

根據激光雷達傳感器和視覺傳感器實時檢測的道路位置信息，控制無人駕駛車輛沿當前道路軌跡形狀行駛；

根據接收到的路徑GPS信息與定位GPS信息，跟蹤路徑規劃模塊生成的路徑坐標序列。