移動機(jī)器人最小化snap與A*的聯(lián)合軌跡優(yōu)化

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

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

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

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

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

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

1 A*算法

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

對真實(shí)環(huán)境完成柵格地圖構(gòu)建之后,起點(diǎn)、終點(diǎn)、障礙物區(qū)域以及可行區(qū)域均隸屬于同一八連通結(jié)構(gòu)樹,其簡述路網(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)表示,此時地圖中所有節(jié)點(diǎn)均連接在同一結(jié)構(gòu)圖中,同時對起點(diǎn)、終點(diǎn)、障礙物及可行區(qū)域進(jìn)行標(biāo)注,則路徑搜索問題轉(zhuǎn)化成在圖中找到一支滿足代價最小要求的結(jié)構(gòu)樹,且從終點(diǎn)子節(jié)點(diǎn)向前回溯父節(jié)點(diǎn)直至起點(diǎn)節(jié)點(diǎn)即可找到所需靜態(tài)最短路徑.構(gòu)造代價函數(shù)建立搜索準(zhǔn)則,其函數(shù)一般形式如式(1)所示.

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

(1)

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

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

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

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

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

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

2.1 算法框架

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

(a)算法框架

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

2.2 時間均勻化處理

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

(2)

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

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

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

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

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

(3)

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

(4)

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

(5)

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

(6)

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

(7)

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

J(T)=pTQp.

(8)

2.4 約束條件建立

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

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

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

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

(9)

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

(10)

簡化模型為式(11),

Ajpj=dj.

(11)

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

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

(12)

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

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

(13)

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

(14)

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

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

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

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

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

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

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

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

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

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

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

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

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

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

(17)

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

表1 算法性能對比Table 1 Comparison of algorithm performances

4 結(jié) 論

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