[發(fā)明專利]一種基于FLOYD和Astar的混合路徑規(guī)劃方法

在審

• 申請(qǐng)?zhí)枺?/td>: 202110049821.4
• 申請(qǐng)日: 2021-01-14
• 公開(kāi)（公告）號(hào): CN112947406A
• 公開(kāi)（公告）日: 2021-06-11
• 發(fā)明（設(shè)計(jì)）人: 胡習(xí)之;周健威
• 申請(qǐng)（專利權(quán)）人: 華南理工大學(xué)
• 主分類號(hào): G05D1/02
• 分類號(hào): G05D1/02
• 代理公司: 廣州粵高專利商標(biāo)代理有限公司 44102
• 代理人: 何淑珍;江裕強(qiáng)

1.一種基于FLOYD和Astar的混合路徑規(guī)劃方法，其特征在于，包括以下步驟：

根據(jù)車輛的幾何特征與車身角點(diǎn)約束，建立車輛數(shù)學(xué)幾何模型，用以分析車輛的運(yùn)動(dòng)覆蓋空間；

建立包含有道路層語(yǔ)義信息的電子地圖，并在電子地圖的道路上設(shè)定行駛航點(diǎn)；

使用Floyd算法獲得全局距離矩陣以及最優(yōu)路徑矩陣，然后通過(guò)A*算法遞歸求取最優(yōu)全局參考路徑，完成全局路徑規(guī)劃；

導(dǎo)入實(shí)時(shí)環(huán)境信息和車輛行駛狀態(tài)，使用碰撞判斷規(guī)則和通行判斷規(guī)則，對(duì)全局路徑規(guī)劃基于避障目的進(jìn)行決策與重規(guī)劃，實(shí)現(xiàn)可行駛參考路徑的在線動(dòng)態(tài)規(guī)劃；

對(duì)最優(yōu)全局參考路徑進(jìn)行數(shù)學(xué)描述，其中針對(duì)直線車道和非直車道采用分段函數(shù)描述參考路徑：直線車道采用固定某一軸數(shù)值的直線函數(shù)，非直車道則采用均勻細(xì)分樣條曲線函數(shù)。

2.根據(jù)權(quán)利要求1所述的一種基于FLOYD和Astar的混合路徑規(guī)劃方法，其特征在于，所述車輛數(shù)學(xué)幾何模型為車輛四個(gè)角點(diǎn)的位置表達(dá)式，定義車輛的四個(gè)角點(diǎn)分別為車輛左前角點(diǎn)A、車輛右前角點(diǎn)B、車輛右后角點(diǎn)C和車輛左后角點(diǎn)D，則各角點(diǎn)的位置表達(dá)式如下：

車輛左前角點(diǎn)A點(diǎn)(X_A，Y_A)為：

車輛右前角點(diǎn)B點(diǎn)(X_B，Y_B)為：

車輛右后角點(diǎn)C點(diǎn)(X_C，Y_C)為：

車輛左后角點(diǎn)D點(diǎn)(X_D，Y_D)為：

3.根據(jù)權(quán)利要求1所述的一種基于FLOYD和Astar的混合路徑規(guī)劃方法，其特征在于，所述建立包含有道路層語(yǔ)義信息的電子地圖，具體包括：

測(cè)量并繪制所要規(guī)劃區(qū)域的平面地圖；

基于OpenDRIVE技術(shù)補(bǔ)充道路層語(yǔ)義信息；

定義坐標(biāo)原點(diǎn)，繪制所要規(guī)劃區(qū)域的高精度電子地圖。

4.根據(jù)權(quán)利要求1所述的一種基于FLOYD和Astar的混合路徑規(guī)劃方法，其特征在于，所述在電子地圖的道路上設(shè)定行駛航點(diǎn)中，交叉路口道路的航點(diǎn)分布密集，長(zhǎng)直道上的航點(diǎn)分布較交叉路口道路的稀疏。

5.根據(jù)權(quán)利要求1所述的一種基于FLOYD和Astar的混合路徑規(guī)劃方法，其特征在于，所述使用Floyd算法計(jì)算全局距離矩陣以及最優(yōu)路徑矩陣，然后通過(guò)A*算法遞歸求取最優(yōu)路徑，完成全局路徑規(guī)劃，具體包括：

通過(guò)Floyd算法計(jì)算獲得全局距離矩陣以及最優(yōu)路徑矩陣；

將全局距離矩陣引入到A*算法的估計(jì)代價(jià)函數(shù)中，啟發(fā)式地搜索給定起止點(diǎn)的最優(yōu)路徑，得到路徑點(diǎn)集Path；

對(duì)路徑點(diǎn)集Path進(jìn)行遞歸和判斷，將路徑點(diǎn)集Path中的任意兩點(diǎn)與通過(guò)Floyd算法得到的最優(yōu)路徑矩陣進(jìn)行遞歸比較，若最優(yōu)路徑中的該兩點(diǎn)的距離短于通過(guò)Floyd算法得到的最優(yōu)路徑矩陣中該兩點(diǎn)的距離，則將Floyd算法得到的最優(yōu)路徑片段引入替換通過(guò)A*算法得到的最優(yōu)全局參考路徑中相應(yīng)的路徑片段，得到最終的最優(yōu)全局參考路徑。

6.根據(jù)權(quán)利要求1所述的一種基于FLOYD和Astar的混合路徑規(guī)劃方法，其特征在于，所述通過(guò)A*算法遞歸求取最優(yōu)路徑中，A*算法采用的啟發(fā)函數(shù)為歐幾里德距離，A*算法的估計(jì)代價(jià)函數(shù)用于表示從x到y(tǒng)經(jīng)過(guò)點(diǎn)k的估計(jì)代價(jià)，具體為：

F_xy(k)＝G(x,k)+αH(k,y)

其中，G(x，k)為Floyd算法中距離矩陣從x到k的距離權(quán)重，α為偏重調(diào)節(jié)因子，H(k，y)為從k到y(tǒng)的歐幾里德距離。

7.根據(jù)權(quán)利要求6所述的一種基于FLOYD和Astar的混合路徑規(guī)劃方法，其特征在于，所述偏重調(diào)節(jié)因子α為85/cm。