[發(fā)明專利]一種基于動(dòng)態(tài)窗口和冗余節(jié)點(diǎn)過(guò)濾的路徑優(yōu)化方法

有效

• 申請(qǐng)?zhí)枺?/td>: 202110678202.1
• 申請(qǐng)日: 2021-06-18
• 公開（公告）號(hào): CN113341991B
• 公開（公告）日: 2022-08-09
• 發(fā)明（設(shè)計(jì)）人: 宋永端;張聰毅;譚力琿;賴俊峰;王賽宇;張艷鍇
• 申請(qǐng)（專利權(quán)）人: 重慶大學(xué);星際（重慶）智能裝備技術(shù)研究院有限公司;迪比（重慶）智能科技研究院有限公司
• 主分類號(hào): G05D1/02
• 分類號(hào): G05D1/02
• 代理公司: 重慶晟軒知識(shí)產(chǎn)權(quán)代理事務(wù)所(普通合伙) 50238
• 代理人: 王海鳳

本發(fā)明公開了一種基于動(dòng)態(tài)窗口和冗余節(jié)點(diǎn)過(guò)濾的路徑優(yōu)化方法，其包括利用已有的柵格地圖數(shù)據(jù)集，確定運(yùn)動(dòng)的起始位置和目標(biāo)位置的坐標(biāo)信息，在柵格地圖中標(biāo)注目標(biāo)節(jié)點(diǎn)和障礙物節(jié)點(diǎn)；利用A^*算法規(guī)劃出一條全局路徑；利用A^*算法規(guī)劃處的全局路徑進(jìn)行全局優(yōu)化，過(guò)濾掉冗余節(jié)點(diǎn)；結(jié)合動(dòng)態(tài)窗口法對(duì)優(yōu)化后的全局路徑進(jìn)行分段局部?jī)?yōu)化，得到最終的全局路徑。本發(fā)明使用組合算法減少了移動(dòng)機(jī)器人的單次運(yùn)動(dòng)時(shí)長(zhǎng)，提高了運(yùn)動(dòng)路徑曲線的平滑性；同時(shí)改善了機(jī)器人在靜態(tài)行駛路線過(guò)程中的問(wèn)題，在規(guī)劃路徑段內(nèi)可以實(shí)現(xiàn)靈活的避障功能。

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

本發(fā)明涉及移動(dòng)機(jī)器人技術(shù)中的運(yùn)動(dòng)規(guī)劃領(lǐng)域，更具體地涉及一種可用于移動(dòng)機(jī)器人的路徑規(guī)劃及優(yōu)化方法。

背景技術(shù)

機(jī)器人技術(shù)作為一個(gè)綜合了多個(gè)學(xué)科的技術(shù)，是多年研究的熱門。其中，路徑規(guī)劃作為移動(dòng)機(jī)器人技術(shù)研發(fā)的關(guān)鍵技術(shù)，其目的在于參照某一個(gè)參數(shù)的指標(biāo)(如代價(jià)最低、選擇路徑最短、運(yùn)算時(shí)間最短等)，在任務(wù)區(qū)域選擇出一條可以從起點(diǎn)運(yùn)動(dòng)到終點(diǎn)的最優(yōu)或次優(yōu)的避障路徑。路徑規(guī)劃按照機(jī)器人對(duì)工作區(qū)域信息的理解層次，可以分為給予完整區(qū)域信息理解的全局路徑規(guī)劃和基于部分區(qū)域信息理解的局部路徑規(guī)劃，兩者各有優(yōu)缺。現(xiàn)在實(shí)際工作中大多情況下都需要融合兼用全局和局部路徑規(guī)劃，前者旨在尋找全局優(yōu)化路徑，后者旨在實(shí)時(shí)避障。

A^*算法作為最流行的全局路徑查找選擇方式之一，具有高度靈活性，且可以廣泛應(yīng)用于不同的環(huán)境之中。A^*算法的成功秘訣在于它結(jié)合了Dijkstra算法使用的信息(對(duì)接近起點(diǎn)的位置有利)和貪婪最佳優(yōu)先搜索使用的信息(對(duì)接近目的地的位置有利)。其通過(guò)計(jì)算路由節(jié)點(diǎn)成本，選擇成本最低的節(jié)點(diǎn)，通過(guò)依次擴(kuò)展具有最小啟發(fā)函數(shù)的節(jié)點(diǎn)來(lái)尋找最優(yōu)路徑。雖然傳統(tǒng)的A*算法能得到一條長(zhǎng)度相對(duì)比較優(yōu)秀的路徑，但是該路徑存在的問(wèn)題也很明顯：有冗余節(jié)點(diǎn)、轉(zhuǎn)折點(diǎn)較多，這些對(duì)機(jī)器人實(shí)際的運(yùn)動(dòng)都是不利的存在。

動(dòng)態(tài)窗口法因?yàn)槠浞弦苿?dòng)機(jī)器人的運(yùn)動(dòng)特征，具有靈活性強(qiáng)等優(yōu)點(diǎn)，成為了動(dòng)態(tài)環(huán)境下局部避障的主要算法。動(dòng)態(tài)窗口法的基本思想是結(jié)合移動(dòng)機(jī)器人的運(yùn)動(dòng)特性，在路徑規(guī)劃過(guò)程中，實(shí)時(shí)預(yù)測(cè)時(shí)間t內(nèi)移動(dòng)機(jī)器人的速度空間(v_t,w_t)和狀態(tài)空間(x_t,y_t,yaw_t,v_t,w_t)，模擬移動(dòng)機(jī)器人在預(yù)測(cè)時(shí)間內(nèi)的運(yùn)動(dòng)軌跡，再根據(jù)評(píng)價(jià)函數(shù)確定最優(yōu)的運(yùn)動(dòng)軌跡，從而到達(dá)目標(biāo)位置完成路徑規(guī)劃。該方法屬于一個(gè)局部的路徑規(guī)劃，機(jī)器人如果只在這個(gè)算法下運(yùn)動(dòng)，雖然可以保證一個(gè)良好的運(yùn)動(dòng)姿態(tài)，但是要達(dá)到目標(biāo)點(diǎn)需要遍歷太多節(jié)點(diǎn)，效率很低。

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

針對(duì)現(xiàn)有技術(shù)存在的上述問(wèn)題，本發(fā)明的要解決的技術(shù)問(wèn)題是：如何盡可能的優(yōu)化路線，使機(jī)器人的路線更平滑和可靠。

為解決上述第一個(gè)技術(shù)問(wèn)題，本發(fā)明采用如下技術(shù)方案：一種基于動(dòng)態(tài)窗口和冗余節(jié)點(diǎn)過(guò)濾的路徑優(yōu)化方法，包括以下步驟：

S100：利用已有的柵格地圖數(shù)據(jù)集，確定運(yùn)動(dòng)的起始位置和目標(biāo)位置的坐標(biāo)信息，在柵格地圖中標(biāo)注目標(biāo)節(jié)點(diǎn)和障礙物節(jié)點(diǎn)；

S200：利用A^*算法規(guī)劃出一條全局路徑；

S300：利用A^*算法規(guī)劃出的全局路徑進(jìn)行全局優(yōu)化，過(guò)濾掉冗余節(jié)點(diǎn)；

S400：結(jié)合動(dòng)態(tài)窗口法對(duì)S300優(yōu)化后的全局路徑進(jìn)行分段局部?jī)?yōu)化，得到最終的全局路徑。

作為改進(jìn)，所述S200中利用A^*算法規(guī)劃出一條全局路徑，其包括以下步驟：

S210：構(gòu)建并且初始化兩個(gè)空表OpenList和CloseList，將起始位置設(shè)置為當(dāng)前節(jié)點(diǎn)并存入OpenList列表；如果當(dāng)前節(jié)點(diǎn)不是目標(biāo)點(diǎn)，那么調(diào)用擴(kuò)充節(jié)點(diǎn)函數(shù)選出與當(dāng)前節(jié)點(diǎn)相鄰且可擴(kuò)展的所有節(jié)點(diǎn)，并且將擴(kuò)展的所有節(jié)點(diǎn)信息都存入OpenList列表；