[發(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)

本發(fā)明公開(kāi)的一種基于FLOYD和Astar的混合路徑規(guī)劃方法，首先建立車輛數(shù)學(xué)模型和構(gòu)建高精度地圖，在此條件下，將Floyd算法和A*算法相結(jié)合，使用混合型算法進(jìn)行自動(dòng)駕駛車輛的路徑規(guī)劃。常見(jiàn)的路徑規(guī)劃算法中，F(xiàn)loyd算法時(shí)間復(fù)雜高，其計(jì)算得到的距離矩陣和路徑矩陣無(wú)法滿足自動(dòng)駕駛車輛動(dòng)態(tài)行駛時(shí)的高實(shí)時(shí)性要求；A*算法由于存在邏輯設(shè)計(jì)問(wèn)題，其規(guī)劃結(jié)果不一定是全局最優(yōu)的。本發(fā)明提出的方法針對(duì)傳統(tǒng)路徑規(guī)劃算法中Floyd算法與A*算法的問(wèn)題，提出了一種結(jié)合兩種方法特點(diǎn)的混合型路徑規(guī)劃算法，同時(shí)兼顧了全局最優(yōu)性和實(shí)時(shí)性，增強(qiáng)了算法對(duì)環(huán)境的適應(yīng)性，以此實(shí)現(xiàn)自動(dòng)駕駛參考路徑的在線規(guī)劃和動(dòng)態(tài)全局避障。

技術(shù)領(lǐng)域

本發(fā)明涉及自動(dòng)駕駛技術(shù)領(lǐng)域，特別涉及一種基于FLOYD和Astar的混合路徑規(guī)劃方法。

背景技術(shù)

路徑規(guī)劃是自動(dòng)駕駛的關(guān)鍵技術(shù)之一，是重點(diǎn)和難點(diǎn)。自動(dòng)駕駛車輛路徑規(guī)劃是指基于一定的環(huán)境模型和行車規(guī)則策略，給定行駛起始點(diǎn)與目標(biāo)點(diǎn)后，按照特定性能指標(biāo)，在保證行車安全和效率最大的基礎(chǔ)上，規(guī)劃出一條無(wú)碰撞、能安全到達(dá)目標(biāo)點(diǎn)的可行駛路徑。路徑規(guī)劃主要包含兩個(gè)步驟，一是建立包含障礙物區(qū)域與自由區(qū)域的環(huán)境地圖，二是結(jié)合環(huán)境地圖選擇合適的路徑搜索算法，快速實(shí)時(shí)地搜索可行駛路徑。自動(dòng)駕駛車輛路徑規(guī)劃包括全局路徑規(guī)劃和局部路徑規(guī)劃。全局路徑規(guī)劃是基于環(huán)境信息(如地圖、障礙物、道路邊界)規(guī)劃出滿足車輛行駛約束的最優(yōu)可行駛路徑，具有導(dǎo)航能力。局部路徑規(guī)劃是指在全局路徑的基礎(chǔ)上，車輛為躲避障礙物而快速規(guī)劃出的安全可行駛路徑。參考機(jī)器人移動(dòng)技術(shù)和機(jī)器學(xué)習(xí)技術(shù)，局部路徑規(guī)劃主要有人工勢(shì)場(chǎng)算法、蟻群算法和神經(jīng)網(wǎng)絡(luò)算法等。

在傳統(tǒng)算法中，F(xiàn)loyd路徑規(guī)劃算法時(shí)間復(fù)雜度高，道路節(jié)點(diǎn)分布均勻，距離矩陣和路徑矩陣的迭代過(guò)程無(wú)法滿足自動(dòng)駕駛車輛動(dòng)態(tài)行駛規(guī)劃的高實(shí)時(shí)性要求，不能及時(shí)適應(yīng)環(huán)境模型的改變；A*(A-Star)算法結(jié)合啟發(fā)搜索思想和Dijkstra算法，是典型的啟發(fā)式搜索算法，通過(guò)構(gòu)建代價(jià)函數(shù)F搜索每一步代價(jià)最小的節(jié)點(diǎn)從而找到最短路徑，但其規(guī)劃結(jié)果可能非全局最優(yōu)，存在因算法邏輯設(shè)計(jì)而出現(xiàn)的方向性或結(jié)構(gòu)性結(jié)果傾向問(wèn)題，這對(duì)于自動(dòng)駕駛路徑規(guī)劃而言存在不可跟蹤性路徑；快速探索隨機(jī)樹(shù)(RRT)是基于隨機(jī)采樣理論的一種路徑規(guī)劃算法，它存在采樣節(jié)點(diǎn)密集，路徑曲折復(fù)雜等問(wèn)題。此外，在傳統(tǒng)路徑規(guī)劃算法中，通常將環(huán)境內(nèi)的物體進(jìn)行“輪廓膨脹處理”，將物體占用位置面積按一定比例增大，以此進(jìn)行參考路徑行駛時(shí)的動(dòng)態(tài)規(guī)劃，但會(huì)使得環(huán)境模型有一定的失真，狹窄路段的通過(guò)性降低。

針對(duì)單個(gè)算法存在缺陷，江鵬程等在專利申請(qǐng)“無(wú)人車混合路徑規(guī)劃算法(CN110609557A)”中，提出了結(jié)合蟻群算法、A*算法和VFH算法的無(wú)人車混合路徑規(guī)劃算法，首先利用周圍環(huán)境信息構(gòu)建柵格地圖，然后用改進(jìn)蟻群算法進(jìn)行兩次全局路徑規(guī)劃，若兩次得到的結(jié)果不完全重合，則判定為陷入局部最優(yōu)，此時(shí)引入A*算法進(jìn)行對(duì)比和優(yōu)化，得到最終最優(yōu)路徑。對(duì)于行駛過(guò)程中的未知障礙物，采用VFH+算法進(jìn)行局部路徑規(guī)劃。與單個(gè)算法相比，該方法減少路徑搜索時(shí)長(zhǎng)、增強(qiáng)了自動(dòng)駕駛車輛實(shí)時(shí)性、避免陷入局部最優(yōu)解，但仍存在以下問(wèn)題：將周圍環(huán)境信息構(gòu)建為柵格地圖，無(wú)法保證路徑的可行駛性；使用改進(jìn)蟻群算法進(jìn)行了兩次全局路徑規(guī)劃，且在陷入局部最優(yōu)后再進(jìn)行A*算法的重新優(yōu)化，耗時(shí)仍較長(zhǎng)；改進(jìn)蟻群算法在進(jìn)行節(jié)點(diǎn)選擇時(shí)，仍包含有隨機(jī)數(shù)選擇的形式，是一種盲目性的選擇，缺乏啟發(fā)信息。

發(fā)明內(nèi)容

本發(fā)明提供一種基于FLOYD和Astar的混合路徑規(guī)劃方法，通過(guò)將傳統(tǒng)的Floyd算法和A*算法相結(jié)合，解決當(dāng)前傳統(tǒng)算法中存在的實(shí)時(shí)性與全局最優(yōu)性問(wèn)題。

針對(duì)上述描述的路徑規(guī)劃算法的實(shí)時(shí)性和全局最優(yōu)性問(wèn)題，本發(fā)明提供的一種基于FLOYD和Astar的混合路徑規(guī)劃方法，其特征在于，包括以下步驟：

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

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