韓曉微,劉宏宇,石澤亮,劉 曉

(沈陽(yáng)大學(xué) a.科技創(chuàng)新研究院,b.信息工程學(xué)院,遼寧 沈陽(yáng) 110041)

路徑規(guī)劃是移動(dòng)機(jī)器人技術(shù)研究中的重要組成部分[1],其目的是為移動(dòng)機(jī)器人尋找一條從起點(diǎn)到終點(diǎn)的安全(無(wú)碰撞)、高效(最短距離或最短時(shí)間)的最優(yōu)或接近最優(yōu)的路徑[2].

常用的路徑規(guī)劃算法有蟻群算法、粒子群算法、模擬退火算法和神經(jīng)網(wǎng)絡(luò)等[2].目前對(duì)路徑規(guī)劃算法的研究主要基于真實(shí)環(huán)境的柵格分解描述,把機(jī)器人的運(yùn)動(dòng)環(huán)境分解成許多簡(jiǎn)單的柵格,根據(jù)是否被障礙物占據(jù)來(lái)進(jìn)行狀態(tài)描述[3].Dijkstra算法解決的是有向圖中從一個(gè)節(jié)點(diǎn)到其他節(jié)點(diǎn)的最短路徑問(wèn)題.以起點(diǎn)為中心,逐步向周?chē)噜徆?jié)點(diǎn)拓展,被拓展的點(diǎn)都是到該點(diǎn)的最短路徑,直到達(dá)到目標(biāo)點(diǎn)[4].該方法在地圖較小時(shí)可以使用,隨著地圖的增大計(jì)算量也會(huì)急劇增長(zhǎng)[5].針對(duì)此問(wèn)題提出的A*算法融合了Dijkstra算法及貪心算法思想,通過(guò)計(jì)算啟發(fā)函數(shù)來(lái)評(píng)估當(dāng)前節(jié)點(diǎn)到下一節(jié)點(diǎn)的代價(jià)大小來(lái)尋找一條最優(yōu)路徑.此算法應(yīng)用較為靈活,但當(dāng)起點(diǎn)與終點(diǎn)之間的距離較長(zhǎng)時(shí),A*算法評(píng)估代價(jià)時(shí)會(huì)計(jì)算大量重復(fù)數(shù)據(jù),降低搜索效率[6].

2017年Charri等設(shè)計(jì)了松弛A*算法,其執(zhí)行時(shí)間大大減少,增大了約束性復(fù)雜度,松弛A*能成為最佳路徑規(guī)劃器是因?yàn)槠湓诙攘恐g提供了良好的權(quán)衡[7].石征錦等[8]在啟發(fā)函數(shù)中加入余弦相似性和方向信息,再做歸一化處理,改進(jìn)A*算法.辛煜等[9]成功擴(kuò)大搜索鄰域,重新定義中心節(jié)點(diǎn)位置,在每個(gè)節(jié)點(diǎn)周?chē)鷶U(kuò)大無(wú)限的可搜索鄰域,快速實(shí)現(xiàn)無(wú)碰撞的路徑規(guī)劃.王殿君[10]針對(duì)路徑規(guī)劃數(shù)據(jù)包含規(guī)劃點(diǎn)的冗余坐標(biāo)和移動(dòng)機(jī)器人無(wú)法在拐點(diǎn)處調(diào)整姿態(tài)的問(wèn)題,提出能夠計(jì)算拐點(diǎn)、發(fā)現(xiàn)最優(yōu)旋轉(zhuǎn)角度的A*路徑規(guī)劃改進(jìn)算法.王洪斌等[11]通過(guò)引入二次A*算法減少路徑軌跡冗余點(diǎn),縮短長(zhǎng)度并采用自適應(yīng)圓弧與加權(quán)障礙步長(zhǎng)調(diào)節(jié)優(yōu)化算法,利用預(yù)瞄偏差角度追蹤移動(dòng)目標(biāo)點(diǎn),有效提升路徑規(guī)劃效率.

本文充分分析了目前已有的路徑規(guī)劃算法,針對(duì)移動(dòng)機(jī)器人路徑規(guī)劃中使用A*算法時(shí)存在的折線(xiàn)多、轉(zhuǎn)折角度大等無(wú)法實(shí)際應(yīng)用的問(wèn)題,提出一種采用MS優(yōu)化A*算法的路徑規(guī)劃思路,將折線(xiàn)路徑變?yōu)槠交€(xiàn)路徑,解決了該算法規(guī)劃的路徑不滿(mǎn)足移動(dòng)機(jī)器人物理特性而無(wú)法較好實(shí)際應(yīng)用的問(wèn)題,保證路徑規(guī)劃算法的優(yōu)越性和可行性.

1 A*算法

A*算法是由斯坦福大學(xué)教授Nils Nilsson于1968年提出的一種基于圖的啟發(fā)式搜索算法.該算法旨在保留Dijkstra算法路徑搜索全局最優(yōu)性的同時(shí)引入代價(jià)函數(shù),用于節(jié)點(diǎn)動(dòng)態(tài)篩選的遍歷定量描述[12-13].由于其具有絕對(duì)路徑搜索可靠性且大幅提升算法實(shí)時(shí)性的優(yōu)勢(shì),滿(mǎn)足實(shí)際工程對(duì)自主移動(dòng)機(jī)器人的考量,因此被作為基礎(chǔ)搜索算法得到了廣泛的學(xué)習(xí)改進(jìn)與實(shí)踐應(yīng)用.

對(duì)真實(shí)環(huán)境完成柵格地圖構(gòu)建之后,起點(diǎn)、終點(diǎn)、障礙物區(qū)域以及可行區(qū)域均隸屬于同一八連通結(jié)構(gòu)樹(shù),其簡(jiǎn)述路網(wǎng)節(jié)點(diǎn)拓?fù)淠Ｐ腿鐖D1所示.

圖1 A*算法節(jié)點(diǎn)Fig.1 Nodes for A* algorithm

將搜索區(qū)域內(nèi)所有節(jié)點(diǎn)以全局坐標(biāo)系下二維坐標(biāo)表示,此時(shí)地圖中所有節(jié)點(diǎn)均連接在同一結(jié)構(gòu)圖中,同時(shí)對(duì)起點(diǎn)、終點(diǎn)、障礙物及可行區(qū)域進(jìn)行標(biāo)注,則路徑搜索問(wèn)題轉(zhuǎn)化成在圖中找到一支滿(mǎn)足代價(jià)最小要求的結(jié)構(gòu)樹(shù),且從終點(diǎn)子節(jié)點(diǎn)向前回溯父節(jié)點(diǎn)直至起點(diǎn)節(jié)點(diǎn)即可找到所需靜態(tài)最短路徑.構(gòu)造代價(jià)函數(shù)建立搜索準(zhǔn)則,其函數(shù)一般形式如式(1)所示.

f(n)=g(n)+h(n).

(1)

式中f(n)是從自定義起點(diǎn)到自定義終點(diǎn)的代價(jià)函數(shù),g(n)是從起點(diǎn)到中間節(jié)點(diǎn)n已產(chǎn)生的累計(jì)代價(jià)函數(shù),h(n)表示從中間節(jié)點(diǎn)n到終點(diǎn)還需產(chǎn)生的累計(jì)估計(jì)代價(jià).

算法執(zhí)行流程是:首先使用一個(gè)優(yōu)先級(jí)隊(duì)列對(duì)搜索樹(shù)里的所有節(jié)點(diǎn)進(jìn)行排序.開(kāi)始時(shí),每個(gè)節(jié)點(diǎn)的值都是未知的.當(dāng)擴(kuò)展到該節(jié)點(diǎn)的時(shí)候再根據(jù)事先規(guī)定的歐式距離或者馬氏距離計(jì)算得到,同樣優(yōu)先級(jí)隊(duì)列一開(kāi)始時(shí)是空的,初始化時(shí)只放入起始節(jié)點(diǎn),并且把累計(jì)代價(jià)設(shè)為零,其他節(jié)點(diǎn)的累計(jì)代價(jià)設(shè)為無(wú)窮大,進(jìn)行迭代.首先從搜索樹(shù)里彈出最小的節(jié)點(diǎn),將該節(jié)點(diǎn)標(biāo)記為已經(jīng)擴(kuò)展的點(diǎn)以后不再訪問(wèn),同時(shí)判斷是不是終點(diǎn)以及循環(huán)是否結(jié)束.接著擴(kuò)展當(dāng)前節(jié)點(diǎn)的鄰節(jié)點(diǎn),對(duì)于未被擴(kuò)展過(guò)的節(jié)點(diǎn),首先判斷是否為新發(fā)現(xiàn)的點(diǎn),如果是計(jì)算它的值,將其放入優(yōu)先級(jí)隊(duì)列;如果該節(jié)點(diǎn)之前就被別的點(diǎn)當(dāng)作鄰節(jié)點(diǎn)發(fā)現(xiàn)過(guò),即現(xiàn)在已經(jīng)存在于優(yōu)先級(jí)隊(duì)列中,則看g(n)的值能否通過(guò)當(dāng)前節(jié)點(diǎn)獲得一個(gè)更小的值,如果能獲得則對(duì)其值進(jìn)行更新.對(duì)于A*而言,更新g(n)值的同時(shí)還需要將f(n)的值重新計(jì)算一遍.此時(shí)優(yōu)先級(jí)隊(duì)列會(huì)形成新的排序.Dijkstra算法與A*算法搜索結(jié)果如圖2所示(見(jiàn)封3).

圖2中綠色點(diǎn)為起點(diǎn)、黃色點(diǎn)為終點(diǎn)、黑色點(diǎn)為障礙物區(qū)域、紅色格子表示算法搜索過(guò)程中需要拓展的柵格、深藍(lán)色為下一步待拓展位置、青色為算法最終搜索出的全局最優(yōu)路徑.

從理論上分析,A*算法在節(jié)省計(jì)算時(shí)間方面是最優(yōu)的,但隨著搜索空間的擴(kuò)大,節(jié)點(diǎn)數(shù)目的增加,搜索時(shí)間也會(huì)增加,且其應(yīng)用受到室內(nèi)環(huán)境圖大小的限制,也存在客觀因素的不合理性,例如沒(méi)有考慮移動(dòng)小車(chē)自身體積大小及物理學(xué)狀態(tài)、機(jī)器人步長(zhǎng)、障礙物形態(tài)等多約束條件.

2 基于最小化snap的軌跡優(yōu)化算法

移動(dòng)機(jī)器人運(yùn)動(dòng)規(guī)劃問(wèn)題分為前端路徑規(guī)劃和后端軌跡優(yōu)化,由于路徑搜索通常不考慮機(jī)器人動(dòng)力學(xué)模型,機(jī)器人難以執(zhí)行,因此需要引入軌跡優(yōu)化.根據(jù)微分平坦理論可以將高維的狀態(tài)空間轉(zhuǎn)化為低維的平坦的輸出空間表示[14].snap是加速度的二階導(dǎo)數(shù),將其作為被控量處理相當(dāng)于最小化角加速度即動(dòng)力的微分,使得動(dòng)力變化盡可能的平緩,達(dá)到節(jié)省能量的效果.

2.1 算法框架

分析A*算法得到路徑點(diǎn)步驟并針對(duì)其難以應(yīng)用于移動(dòng)機(jī)器人上的不足設(shè)計(jì)了本文算法,圖3是本文算法框架及流程圖.輸入軌跡的起點(diǎn)和終點(diǎn),經(jīng)A*算法搜索得到中間路徑點(diǎn),通過(guò)本文算法的軌跡優(yōu)化處理后得到一條滿(mǎn)足機(jī)器人動(dòng)力學(xué)約束的最優(yōu)軌跡.

(a)算法框架

從圖中可以看出,在A*算法得到離散路徑點(diǎn)后,算法在每?jī)牲c(diǎn)間做時(shí)間均勻化處理,在對(duì)每小段路徑分別優(yōu)化之后構(gòu)造多段多項(xiàng)式函數(shù)用于描述整條軌跡.對(duì)每小段軌跡建立最小化snap目標(biāo)函數(shù)并根據(jù)機(jī)器人起點(diǎn)、終點(diǎn)狀態(tài)空間參數(shù)建立導(dǎo)數(shù)約束;根據(jù)機(jī)器人到達(dá)兩段軌跡連接節(jié)點(diǎn)時(shí)狀態(tài)空間值保持不變建立連續(xù)性約束.最后將問(wèn)題轉(zhuǎn)化為二次規(guī)劃問(wèn)題,由規(guī)劃器進(jìn)行數(shù)值求解,得到軌跡參數(shù)向量即得到最優(yōu)軌跡.

2.2 時(shí)間均勻化處理

由于A*搜索得到的路徑點(diǎn)較稀疏,容易造成軌跡不平滑,為了更好地控制機(jī)器人運(yùn)動(dòng),需要將稀疏的路徑點(diǎn)變成平滑的曲線(xiàn)或者稠密的軌跡.在兩點(diǎn)間做時(shí)間均勻化處理,用多段多項(xiàng)式表示軌跡.選用多項(xiàng)式的原因在于其計(jì)算簡(jiǎn)便且可以快速得到軌跡中點(diǎn)的狀態(tài)空間信息,控制器易于對(duì)機(jī)器人控制,具有良好的實(shí)時(shí)性,其對(duì)應(yīng)軌跡表達(dá)式如式(2)所示.

(2)

式中,整個(gè)軌跡f(t)由N段軌跡f1(t),f2(t),…,fN(t)拼接組成;每段軌跡由同階的n階多項(xiàng)式表示;每段軌跡的時(shí)間間隔相同且將整個(gè)軌跡時(shí)間均分為N份[15].

圖4(a)呈現(xiàn)了3組A*搜索結(jié)果,圖4(b)為相應(yīng)處理細(xì)節(jié)顯示表達(dá)(見(jiàn)封3).紅色◆表示機(jī)器人經(jīng)過(guò)的路徑點(diǎn),連線(xiàn)表示A*生成的全局最優(yōu)路徑,綠色矩形框表示按照時(shí)間間隔對(duì)其進(jìn)行均勻化分割處理.

2.3 最小化snap目標(biāo)函數(shù)

為了滿(mǎn)足機(jī)器人的理運(yùn)行特性,使其更加符合運(yùn)動(dòng)模型,本文對(duì)A*規(guī)劃結(jié)果進(jìn)行相應(yīng)處理,分別對(duì)每一段軌跡做最小化snap的最優(yōu)化處理,式(3)是軌跡經(jīng)最小化snap處理后的代數(shù)形式表達(dá)式.

f(4)(t)=∑i(i-1)(i-2)(i-3)ti-4pi.

(3)

為防止正負(fù)號(hào)相消的情況,選擇取函數(shù)二范數(shù)做最小化優(yōu)化,式(4)是二范數(shù)表達(dá)式.

(4)

式(5)是目標(biāo)函數(shù)表達(dá)式,

(5)

將式(5)以矩陣形式展開(kāi)可得式(6),

(6)

將式(6)簡(jiǎn)化為二次型形式表示如式(7),

(7)

每段軌跡疊加可得最終整個(gè)優(yōu)化軌跡,如式(8),

J(T)=pTQp.

(8)

2.4 約束條件建立

為了使機(jī)器人能夠很好地執(zhí)行優(yōu)化算法,本文針對(duì)實(shí)際問(wèn)題選用導(dǎo)數(shù)約束和連續(xù)性約束2類(lèi)模型.

導(dǎo)數(shù)約束包括起點(diǎn)和終點(diǎn)的位置、速度、加速度的邊界條件以及中間點(diǎn)的位置坐標(biāo)[16].連續(xù)性約束為保證每段軌跡一定經(jīng)過(guò)相應(yīng)的連接點(diǎn),雖然中間段軌跡不限制到達(dá)中間點(diǎn)時(shí)的速度或加速度,但是需要前后相接的2段到達(dá)同一點(diǎn)的速度和加速度值相同[17].圖5為約束條件建立示意圖.

圖5 約束條件建立示意圖Fig.5 Schematic diagram of constraint establishment

圖5中P0,P1,…,PN點(diǎn)為機(jī)器人將要經(jīng)過(guò)的點(diǎn)序列,每2點(diǎn)間經(jīng)時(shí)間均勻化分別形成軌跡,式(9)是導(dǎo)數(shù)約束模型.

(9)

第j條軌跡在第Tj時(shí)刻的k階導(dǎo)數(shù)需要滿(mǎn)足起點(diǎn)終點(diǎn)邊界條件以及中間點(diǎn)位置約束.這里k取1、2、3、4,表示軌跡中最后一個(gè)位姿點(diǎn)與下一段軌跡中第1個(gè)位姿點(diǎn)的位置、速度、加速度及加速度的變化率分別對(duì)應(yīng)相等.將式(9)按矩陣展開(kāi),得到式(10),

(10)

簡(jiǎn)化模型為式(11),

Ajpj=dj.

(11)

式中Aj是軌跡在此時(shí)刻的已知量,pj為軌跡系數(shù)向量,dj為此時(shí)刻機(jī)器人的邊界條件或者中間點(diǎn)位置值.

為保證機(jī)器人運(yùn)動(dòng)過(guò)程柔順,建立連續(xù)性約束,見(jiàn)式(12).

(12)

式(12)表示j軌跡在Tj時(shí)刻的k階導(dǎo)數(shù)與其相連接的下一段軌跡在Tj時(shí)刻的k階導(dǎo)數(shù)相等.這里k取1、2、3、4,表示軌跡中最后一個(gè)位姿點(diǎn)與下一段軌跡中第1個(gè)位姿點(diǎn)的位置、速度、加速度及加速度的變化率分別對(duì)應(yīng)相等

將式(12)以矩陣形式展開(kāi)得式(13):

(13)

將軌跡系數(shù)整理成列向量,如式(14):

(14)

結(jié)合式(11)和式(14)整理成等式約束,將優(yōu)化模型轉(zhuǎn)化為二次規(guī)劃問(wèn)題,目標(biāo)函數(shù)為式(15),約束條件為式(16).

在MATLAB中有求解標(biāo)準(zhǔn)二次規(guī)劃問(wèn)題的工具箱,使用QP求解器并調(diào)用quadprog函數(shù)求得相應(yīng)數(shù)值解即可.

3 實(shí)驗(yàn)與分析

3.1 實(shí)驗(yàn)設(shè)計(jì)

本文設(shè)置了2組測(cè)試實(shí)驗(yàn).第1組為仿真實(shí)驗(yàn),主要用于說(shuō)明算法的優(yōu)化效果,算法測(cè)試軟件平臺(tái)為MATLAB 2016b.第2組實(shí)驗(yàn)是真機(jī)測(cè)試實(shí)驗(yàn),用于驗(yàn)證算法的可行性,實(shí)驗(yàn)平臺(tái)為自主設(shè)計(jì)實(shí)現(xiàn)的基于ROS kinetic的移動(dòng)機(jī)器人,圖6是移動(dòng)機(jī)器人實(shí)物圖.

圖6 移動(dòng)機(jī)器人實(shí)物Fig.6 Robot used in the experiment

3.2 仿真結(jié)果與分析

對(duì)時(shí)間均勻化處理后的A*路徑結(jié)果做最小化snap優(yōu)化,同時(shí)針對(duì)起點(diǎn)、終點(diǎn)的空間狀態(tài)量以及相鄰段間位置、速度、加速度連續(xù)建立等式約束,實(shí)驗(yàn)結(jié)果如圖7所示(無(wú)量綱)(見(jiàn)封3).

圖中紅色◆為A*搜索得到機(jī)器人路徑代價(jià)最優(yōu)情況下所需經(jīng)過(guò)的節(jié)點(diǎn),黑色折線(xiàn)表示A*算法給出的機(jī)器人最優(yōu)運(yùn)行路徑,紅色平滑曲線(xiàn)為本文算法優(yōu)化軌跡.

經(jīng)過(guò)仿真實(shí)驗(yàn)可以看出,軌跡在平滑度上有了明顯改善.相鄰中間路徑點(diǎn)實(shí)現(xiàn)了滿(mǎn)足真實(shí)機(jī)器人速度、加速度連續(xù)條件的平滑軟連接.

3.3 真機(jī)測(cè)試實(shí)驗(yàn)

在真實(shí)環(huán)境中完成2D柵格地圖建立,將本算法部署在機(jī)器人上進(jìn)行測(cè)試,圖8(a)是算法在室內(nèi)環(huán)境中生成的3組軌跡效果圖,圖8(b)是機(jī)器人在室外環(huán)境中巡檢測(cè)試效果.驗(yàn)證了機(jī)器人具有安全運(yùn)行的能力,提升了軌跡的平滑性和魯棒性.選用軌跡長(zhǎng)度、軌跡生成時(shí)間作為評(píng)價(jià)標(biāo)準(zhǔn),同時(shí)也提出機(jī)器人轉(zhuǎn)彎效率評(píng)價(jià)標(biāo)準(zhǔn).

圖8 真機(jī)測(cè)試實(shí)驗(yàn)效果Fig.8 Effect diagram of real machine test experiment

轉(zhuǎn)彎效率(η)以真實(shí)機(jī)器人通過(guò)所有彎道的時(shí)間間隔的期望作為評(píng)價(jià)標(biāo)準(zhǔn).

(17)

式中:T1,T2,Tn表示機(jī)器人通過(guò)彎道的時(shí)間間隔;n表示轉(zhuǎn)彎的個(gè)數(shù).任意定義4個(gè)巡邏點(diǎn)開(kāi)展真機(jī)測(cè)試實(shí)驗(yàn).性能對(duì)比如表1所示.與傳統(tǒng)A*算法相比,本文算法縮減了7.24%的軌跡長(zhǎng)度,機(jī)器人轉(zhuǎn)彎效率提升了26.88%,證明本文算法在實(shí)際應(yīng)用中具有良好的可行性.

表1 算法性能對(duì)比Table 1 Comparison of algorithm performances

4 結(jié) 論

針對(duì)A*算法存在轉(zhuǎn)角大、折線(xiàn)多且應(yīng)用于移動(dòng)機(jī)器人時(shí)軌跡平滑性差的問(wèn)題,提出一種基于最小化snap改進(jìn)A*的軌跡優(yōu)化算法.將離散路徑點(diǎn)用分段多項(xiàng)式曲線(xiàn)化描述,且將機(jī)器人的各項(xiàng)動(dòng)力學(xué)性能指標(biāo)引入軌跡生成約束,使所得路徑相對(duì)較短,更適合真實(shí)機(jī)器人的順滑控制與執(zhí)行.通過(guò)仿真和真機(jī)測(cè)試,算法在軌跡長(zhǎng)度方面縮減了7.24%,機(jī)器人轉(zhuǎn)彎效率提升了26.88%.