多體位變換椅自主定位導(dǎo)航A*算法改進(jìn)研究

高興華,王從永,崔曉越

(北華大學(xué) 機(jī)械工程學(xué)院,吉林 吉林 1320012)

對(duì)老人或病人、失能及半失能人的生活照顧是社會(huì)主要問題之一[1]。輔助智能裝置在一定程度上賦予老人或病人生活自理能力,對(duì)于減輕社會(huì)和家庭的養(yǎng)老負(fù)擔(dān)有重要幫助。因此,研制能夠協(xié)助老年人完成坐、躺、立、蹲、行體位變化、室內(nèi)外自主導(dǎo)航與定位和多模態(tài)輸入的多體位自主導(dǎo)航智能輪椅,對(duì)提高患者的康復(fù)質(zhì)量、生活質(zhì)量、減輕社會(huì)負(fù)擔(dān)具有重要的實(shí)際意義。路徑規(guī)劃是多體位輪椅自主定位導(dǎo)航的重要環(huán)節(jié)之一。2007年,中北大學(xué)學(xué)者利用雙向Dijkstra算法,結(jié)合路徑規(guī)劃算法在車輛定位導(dǎo)航系統(tǒng)中的實(shí)際應(yīng)用,給出該算法的實(shí)際應(yīng)用并進(jìn)行分析。2015年西北工業(yè)大學(xué)學(xué)者應(yīng)用A*算法進(jìn)行離線規(guī)劃生成預(yù)規(guī)劃航路,預(yù)規(guī)劃航路作為參考選擇局部目標(biāo)點(diǎn)進(jìn)行在線規(guī)劃;2021年,海南大學(xué)學(xué)者對(duì)A*算法的子節(jié)點(diǎn)擴(kuò)展優(yōu)化與oyd算法優(yōu)化路徑,解決轉(zhuǎn)角和轉(zhuǎn)折節(jié)點(diǎn)過多的問題,使規(guī)劃路徑更加平穩(wěn)及合理。常見的幾種路徑搜索算法中A*算法是當(dāng)前主流的一種路徑搜索算法,它是基于評(píng)估函數(shù)的啟發(fā)式路徑搜索算法,相比Dijkstra算法A*算法規(guī)劃的路徑更快、更短,但往往會(huì)出現(xiàn)搜索空間變大、路徑冗余及轉(zhuǎn)彎半徑大等問題,因此需對(duì)A*算法改進(jìn)以減小路徑規(guī)劃的范圍和轉(zhuǎn)彎半徑,從而提高路徑規(guī)劃的效率。

1 多體位變換椅結(jié)構(gòu)與控制系統(tǒng)

1.1 多體位變換椅結(jié)構(gòu)

多體位變換椅機(jī)械結(jié)構(gòu)如圖1所示。主要結(jié)構(gòu)包括:底盤機(jī)構(gòu)、驅(qū)動(dòng)機(jī)構(gòu)、雷達(dá)掃描機(jī)構(gòu)、升降機(jī)構(gòu)、座椅翻轉(zhuǎn)機(jī)構(gòu)、小腿板翻轉(zhuǎn)機(jī)構(gòu)和坐板翻轉(zhuǎn)機(jī)構(gòu)等。

圖1 體位自主導(dǎo)航智能輪椅機(jī)械結(jié)構(gòu)方案示意

本輪椅使用絲杠傳動(dòng)機(jī)構(gòu)及其他輔助機(jī)構(gòu)配合完成高坐姿、坐姿和低坐姿的位置變換;采用背板翻轉(zhuǎn)機(jī)構(gòu)及其他機(jī)構(gòu)輔助解決座椅與床之間的姿態(tài)轉(zhuǎn)換;采用絲杠傳動(dòng)機(jī)構(gòu)、背板翻轉(zhuǎn)機(jī)構(gòu)和小腿翻轉(zhuǎn)機(jī)構(gòu)完成坐姿與立姿的轉(zhuǎn)換;采用后驅(qū)差速機(jī)構(gòu)進(jìn)行移動(dòng)以確保在姿態(tài)變換時(shí),重心偏移不會(huì)導(dǎo)致智能輪椅發(fā)生偏移。可實(shí)現(xiàn)坐、行、蹲、立和躺五種姿態(tài),如圖2所示。

圖2 多體位自主導(dǎo)航智能輪椅總體位姿變換

1.2 控制系統(tǒng)

為實(shí)現(xiàn)室內(nèi)外可靈活移動(dòng),該智能輪椅設(shè)置差速行走機(jī)構(gòu),動(dòng)力輪的編碼器實(shí)時(shí)傳輸速度等信息。由于使用群體的特殊性,為方便老年人操作,多體位輪椅設(shè)有操縱桿控制、語音交互、無線遙控以及人機(jī)交互界面,最大限度地為使用對(duì)象降低難度,做到對(duì)老年人與殘疾人極度人性化。為了實(shí)現(xiàn)無人駕駛,智能輪椅基于ROS平臺(tái)建立智能輪椅的運(yùn)動(dòng)模型,運(yùn)用Move_base框架對(duì)高精度二維激光雷達(dá)與MPU-6050六軸姿態(tài)傳感器的數(shù)據(jù)進(jìn)行處理,從而實(shí)現(xiàn)SLAM定位和建圖[2-3]。

控制主體由上位機(jī)系統(tǒng)與下位機(jī)系統(tǒng)組成[4-6]。上位機(jī)系統(tǒng)負(fù)責(zé)處理系統(tǒng)綜合數(shù)據(jù)并發(fā)布指令,下位機(jī)系統(tǒng)主要完成底層驅(qū)動(dòng);上位機(jī)系統(tǒng)與下位機(jī)系統(tǒng)通過串口通信,下位機(jī)與遙控采用無線傳輸進(jìn)行通信。其中,上位機(jī)為Mini工控機(jī),主要負(fù)責(zé)將控制指令信息發(fā)送至下位機(jī),進(jìn)而驅(qū)動(dòng)控制系統(tǒng),并實(shí)時(shí)顯示智能輪椅的狀態(tài),同時(shí),接受來自驅(qū)動(dòng)控制系統(tǒng)的狀態(tài)信息;上位機(jī)通過串口接收雷達(dá)的信息,實(shí)現(xiàn)建圖、導(dǎo)航及避障信息的處理。下位機(jī)的處理器使用STM32芯片,主要負(fù)責(zé)接收和發(fā)送硬件驅(qū)動(dòng)的信息,為驅(qū)動(dòng)控制系統(tǒng)提供控制信號(hào),實(shí)時(shí)監(jiān)測(cè)并且記錄系統(tǒng)狀態(tài)。操作遙控的處理器使用STM32芯片,主要是給下位機(jī)發(fā)送控制指令,控制原理如圖3所示。

圖3 控制系統(tǒng)原理結(jié)構(gòu)圖

2 自主導(dǎo)航算法及改進(jìn)

2.1 自主導(dǎo)航總體框架

多體位變換輪椅導(dǎo)航框架的核心是感知層、規(guī)劃層和控制層,信息由上層向下傳遞,共同實(shí)現(xiàn)多體位輪椅的導(dǎo)航。導(dǎo)航框架如圖4所示。

圖4 導(dǎo)航框架

上位機(jī)系統(tǒng)接收雷達(dá)掃描的數(shù)據(jù)和姿態(tài)傳感器的數(shù)據(jù)輸入到感知層,感知層提取外部環(huán)境信息,高精度定位并做出任務(wù)規(guī)劃,Move_base對(duì)感知層傳來的數(shù)據(jù)進(jìn)行規(guī)劃處理后發(fā)送給規(guī)劃層,規(guī)劃層對(duì)行為、運(yùn)動(dòng)進(jìn)行規(guī)劃,采用改進(jìn)A*算法并融合正則表達(dá)進(jìn)行限制性路徑規(guī)劃將規(guī)劃的路徑航跡點(diǎn)等運(yùn)動(dòng)信息送至控制層,進(jìn)而對(duì)底盤運(yùn)動(dòng)進(jìn)行控制。導(dǎo)航控制中融合全局路徑規(guī)劃技術(shù)、局部路徑規(guī)劃技術(shù)、自動(dòng)避障技術(shù)等重要技術(shù),其導(dǎo)航控制能力將直接影響智能輪椅的安全性和有效性。

2.2 基于DWA算法的局部路徑規(guī)劃

動(dòng)態(tài)窗口(Dynamic Window Approach,DWA)[7-9]算法是一種局部路徑規(guī)劃算法,它考慮到加速度的影響,開創(chuàng)一個(gè)速度矢量空間(υ,ω),其中線速度υ和角速度ω是在設(shè)定T時(shí)間內(nèi)的可能值,從該空間選取多組速度用于模擬輪椅在接下來的時(shí)間內(nèi)的運(yùn)動(dòng)軌跡,然后對(duì)每一個(gè)軌跡進(jìn)行評(píng)價(jià),選取最優(yōu)的軌跡對(duì)應(yīng)的速度對(duì)輪椅進(jìn)行驅(qū)動(dòng)[10-12]。DWA算法主要包括兩個(gè)過程:建立速度矢量空間和對(duì)運(yùn)動(dòng)軌跡進(jìn)行評(píng)價(jià)。

假設(shè)智能輪椅存在速度矢量空間,則Vm的取值為

Vm={v∈[vmin,vmax],ω∈[ωmin,ωmax] ,

(1)

其中vmin和vmax分別為預(yù)先設(shè)置的輪椅的最小線速度和最大線速度;ωmin和ωmax分別為輪椅的最小角速度和最大角速度。設(shè)輪椅的最大線加速度為vmax,最大線減速度為vmin,最大角加速度為ωmax,最大角減速度為ωmin,設(shè)模擬周期T內(nèi),輪椅可能達(dá)到的速度矢量空間Vn為

Vn={(v,ω)·v∈[vo-vminT,vo+voT]∧ω∈[ωo-ωminT,ωo+ωoT]} ,

(2)

其中,vo和ωo分別表示當(dāng)前的線速度和角速度。Vn為一個(gè)以速度對(duì)(vo,ωo)為中心的矩形窗口,窗口中的速度矢量表示當(dāng)前模擬周期內(nèi)可達(dá)的速度矢量,窗口外的速度矢量均為不可達(dá)。為了避免和障礙物發(fā)生碰撞,在最大減速能力下,有如下速度矢量空間:

(3)

其中distant(v,ω)表示智能輪椅與速度對(duì)(v,ω)前向模擬的運(yùn)動(dòng)軌跡上最近的障礙物的距離。綜上所述,可以得到最終需要搜索的速度矢量空間:

Vr=Vm∩Vn∩Vmax,

(4)

現(xiàn)已得到速度矢量空間,下一步是計(jì)算速度矢量空間中各個(gè)速度矢量的評(píng)價(jià)值,DWA算法評(píng)價(jià)函數(shù)表達(dá)式為

Assess(v,ω)=σ[α·heading(v,ω)+β·distant(v,ω)+γ·velocity(v,ω)] ,

(5)

其中,α、β、γ均為權(quán)重系數(shù),可根據(jù)實(shí)際需要來進(jìn)行設(shè)定。heading(v,ω)用來評(píng)價(jià)在當(dāng)前速度矢量下,θ表示智能輪椅到達(dá)模擬軌跡終點(diǎn)時(shí)的姿態(tài)角和目標(biāo)姿態(tài)角之間的差值。heading(v,ω)的表達(dá)式為

(6)

可以看出,θ值越小則heading(v,ω)函數(shù)輸出值越大。distant(v,ω)的含義與上文相同,其表達(dá)式為

(7)

其中,l為多體位輪椅與模擬軌跡上最近的障礙物之間的距離;L為障礙物判定的閾值,即距離大于L時(shí),可將此障礙物忽略。velocity(v,ω)用來評(píng)價(jià)下一時(shí)刻智能輪椅的速度大小,表達(dá)式為

(8)

可以看出,速度v的值越大velocity(v,ω)函數(shù)值越大。從評(píng)價(jià)函數(shù)的各個(gè)表達(dá)式可以看出,DWA算法是一種以目標(biāo)點(diǎn)為導(dǎo)向的局部路徑規(guī)劃算法,力求盡可能快速地移動(dòng)到目標(biāo)點(diǎn),在實(shí)際應(yīng)用中,可以結(jié)合實(shí)際需求來設(shè)置權(quán)重系數(shù),以達(dá)到最優(yōu)的實(shí)時(shí)避障效果。

2.3 柵格構(gòu)建地圖

本設(shè)計(jì)選擇地圖構(gòu)建領(lǐng)域應(yīng)用最廣泛的柵格地圖法,來實(shí)現(xiàn)全局路徑規(guī)劃算法路徑搜索,柵格地圖路徑規(guī)劃如圖5所示。根據(jù)智能輪椅的實(shí)際工作環(huán)境,將地圖分割成大小一致的二維網(wǎng)格圖,“0”代表自由柵格,表示當(dāng)前網(wǎng)格沒有被障礙物占領(lǐng),在網(wǎng)格圖中用白色填充;而“1”代表占用柵格,表示當(dāng)前網(wǎng)格被障礙物占領(lǐng),在網(wǎng)格圖中用黑色填充。

2.4 傳統(tǒng)A*算法

Dijkstra算法在二維平面上從初始節(jié)點(diǎn)向四周蔓延,直到蔓延到目標(biāo)節(jié)點(diǎn)。在向四周蔓延時(shí)不會(huì)因?yàn)槟繕?biāo)節(jié)點(diǎn)而改變蔓延方向和范圍,對(duì)四周的行為是一致的,沒有盲區(qū)地進(jìn)行遍歷,如圖6所示。

圖6 Dijkstra算法示意圖

A*算法基于評(píng)估函數(shù)的啟發(fā)式路徑搜索算法,從當(dāng)前坐標(biāo)開始,采用向下搜索的方式搜索最小代價(jià)點(diǎn),并把搜索到的最小代價(jià)點(diǎn)更新為當(dāng)前位置坐標(biāo)繼續(xù)搜索,直到最小代價(jià)點(diǎn)達(dá)到目的坐標(biāo)點(diǎn)[13-14]。搜索公式可顯示為

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

(9)

其中,f(n)代表代價(jià)評(píng)估函數(shù),它的含義是評(píng)估坐標(biāo)起點(diǎn)到目的坐標(biāo)的距離評(píng)估;g(n)表示的是坐標(biāo)起點(diǎn)與當(dāng)前節(jié)點(diǎn)之間的實(shí)際距離;h(n)為啟發(fā)函數(shù),代表的是當(dāng)前任意節(jié)點(diǎn)到目的坐標(biāo)點(diǎn)的最優(yōu)距離估計(jì)。

g(n)可以通過以下公式計(jì)算得出:

(10)

h(n)是算法搜索過程中的距離評(píng)估,其約束條件為

h(n)≤cos*(n,ngoal)

(11)

其中cos*(n,ngoal)是柵格節(jié)點(diǎn)n到目標(biāo)節(jié)點(diǎn)的最優(yōu)距離。

以d(n)表示狀態(tài)n到目標(biāo)狀態(tài)的距離,那么h(n)的選取大致有如下三種情況:

第一,如果h(n)

第二,如果h(n)=d(n),即距離估計(jì)h(n)等于最短距離,那么搜索將嚴(yán)格沿著最短路徑進(jìn)行,此時(shí)的搜索效率是最高的;

第三,如果h(n)>d(n),搜索的點(diǎn)數(shù)少,搜索范圍小,效率高,但不能保證得到最優(yōu)解。

傳統(tǒng)A*算法的不足點(diǎn):

1.重復(fù)尋找節(jié)點(diǎn),使尋路的效率下降;

2.啟發(fā)函數(shù)h(n)代價(jià)值小于實(shí)際值,搜索范圍變大、路徑重復(fù);

3.輪椅視為質(zhì)點(diǎn)遇見障礙物時(shí)精度不夠甚至穿模。

2.5 A*算法改進(jìn)理論

在尋找最優(yōu)路徑時(shí),要對(duì)f(n)或h(n)的取值充分考慮,h(n)作為啟發(fā)函數(shù),作用是估計(jì)代價(jià),通過把估計(jì)出來的距離與實(shí)際值相比較,可以得出,當(dāng)估計(jì)距離和真實(shí)距離差越小,則路徑搜索所達(dá)到的效果就越好。

假設(shè)在柵格地圖中當(dāng)前的節(jié)點(diǎn)為n(xo,yo),目標(biāo)節(jié)點(diǎn)為goal(xgoal,ygoal),則兩節(jié)點(diǎn)間的歐幾里得距離為Do、曼哈頓距離Dm和對(duì)角線距離Dd公式如下所示:

(12)

Dm=|xo-xgoal|+|yo-ygoal| ,

(13)

Dd=max(|xo-xgoal|,|yo-ygoal|) 。

(14)

柵格地圖中,搜索路徑的節(jié)點(diǎn)連接方式包括任意點(diǎn)連接、八節(jié)點(diǎn)連接和四節(jié)點(diǎn)連接,如圖7所示。

圖7 柵格節(jié)點(diǎn)連接關(guān)系示意圖

依據(jù)歐幾里得函數(shù)在任意節(jié)點(diǎn)連接方式中有很大的作用,用它作為啟發(fā)函數(shù),為了克服求解過程中計(jì)算量較大的問題,選用歐幾里得公式,如式(15)所示;此外,根據(jù)節(jié)點(diǎn)連接個(gè)數(shù)的不同選擇不同的評(píng)估函數(shù),如針對(duì)8節(jié)點(diǎn)連接選用切比雪夫距離,4節(jié)點(diǎn)的連接選用曼哈頓距離。

(15)

在以上的基礎(chǔ)上通過創(chuàng)建表OPEN記錄已被考察的節(jié)點(diǎn),創(chuàng)建CLOSED表記錄已經(jīng)被檢測(cè)過的節(jié)點(diǎn),通過表P記錄當(dāng)前節(jié)點(diǎn)的父節(jié)點(diǎn),同時(shí),A*算法在執(zhí)行過程中還需要對(duì)上述表格進(jìn)行維護(hù)。

基于改進(jìn)的A*算法多體位輪椅路徑規(guī)劃流程如圖8所示。

圖8 A*算法改進(jìn)流程圖

多體位變換輪椅基于改進(jìn)后A*算法的路徑規(guī)劃過程為

1.建立數(shù)據(jù)表OPEN表與CLOSE表,將起點(diǎn)S寫入OPEN表中,將障礙點(diǎn)寫入CLOSE表中。

2.計(jì)算OPEN表中最小節(jié)點(diǎn)值f并將其納入CLOSE表中,若OPEN表為空則直接結(jié)束。

3.判斷當(dāng)前節(jié)點(diǎn)m是否為目標(biāo)節(jié)點(diǎn),若是目標(biāo)節(jié)點(diǎn)則根據(jù)m的前向路徑生成最優(yōu)路徑;若不是則擴(kuò)展目標(biāo)節(jié)點(diǎn)m,生成后繼節(jié)點(diǎn)n。

4.在OPEN表中建立從后繼節(jié)點(diǎn)n返回到m的指針,計(jì)算f(n)=g(n)+h(n)。

5.對(duì)于生成的后繼節(jié)點(diǎn)判斷表OPEN中是否已含有節(jié)點(diǎn)n,若無節(jié)點(diǎn)n則將m放入表OPEN中;若有節(jié)點(diǎn)n則比較不同的前向指針f(n)值大小,保留較小的f(n)值。

6.更新g(n),f(n)以及后繼節(jié)點(diǎn)n的前向指針。

7.將表OPEN中f(n)的數(shù)值由大到小排列,并返回第2步。

3 A*算法改進(jìn)MATLAB仿真及驗(yàn)證

3.1.1 傳統(tǒng)A*算法路徑規(guī)劃

采用柵格法構(gòu)建地圖,利用傳統(tǒng)A*算法規(guī)劃路徑。在傳統(tǒng)A*算法中啟發(fā)函數(shù)h(n)一般使用曼哈頓距離定義,則啟發(fā)函數(shù):

Dm=|xo-xgoal|+|yo-ygoal| ,

(16)

那么多體位變換椅行駛到n點(diǎn)的代價(jià)函數(shù)為

F(n)=g(n)+Dm=|x0-xgoal|+|yo-ygoal| 。

(17)

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

基于MATLAB進(jìn)行仿真模擬,仿真環(huán)境為30 m*30 m,采用曼哈頓距離作為啟發(fā)函數(shù)對(duì)算法進(jìn)行仿真,結(jié)果如圖9所示。

(a)

從仿真結(jié)果上看傳統(tǒng)A*算法可以規(guī)劃出一條從起點(diǎn)到終點(diǎn)的多折線路徑。在路徑規(guī)劃時(shí),拐點(diǎn)與拐點(diǎn)優(yōu)先以對(duì)角線規(guī)劃,直到對(duì)角線碰到障礙物,之后緊貼障礙物以直線規(guī)劃。此算法存在搜索空間變大、緊貼障礙物,路徑冗余及轉(zhuǎn)彎半徑大的問題。針對(duì)上述傳統(tǒng)A*算法的不足,通過對(duì)啟發(fā)函數(shù)的優(yōu)化可提升路徑規(guī)劃性能。

3.2 改進(jìn)A*算法仿真與分析

為驗(yàn)證A*算法的可行性,選用MATLAB進(jìn)行仿真模擬,仿真環(huán)境為30 m*30 m,任意節(jié)點(diǎn)采用歐式距離函數(shù)作為啟發(fā)函數(shù),8節(jié)點(diǎn)連接選用切比雪夫距離,4節(jié)點(diǎn)的連接選用曼哈頓距離作為啟發(fā)函數(shù)對(duì)算法進(jìn)行仿真驗(yàn)證[15]。在構(gòu)建地圖中小車從起始點(diǎn)出發(fā),到達(dá)目的地后由改進(jìn)A*算法規(guī)劃的路徑如圖10所示,而實(shí)際運(yùn)行軌跡如圖11所示。

圖10 改進(jìn)A*算法模擬運(yùn)動(dòng)軌跡

圖11 實(shí)際運(yùn)行軌跡

從仿真結(jié)果可以看出A*算法模擬軌跡較短,經(jīng)擬合平滑處理后在拐點(diǎn)處轉(zhuǎn)彎半徑小,折線段優(yōu)化為弧線段,較符合實(shí)際情況,由此可見對(duì)改進(jìn)后的A*算法是可行的。

4 結(jié) 論

本文通過對(duì)多體位姿態(tài)變換智能輪椅的自主導(dǎo)航算法研究,在ROS操作系統(tǒng)下由高精度激光雷達(dá)與姿態(tài)傳感器,在Move base導(dǎo)航框架下構(gòu)建地圖并實(shí)現(xiàn)定位,針對(duì)A*算法中啟發(fā)函數(shù)h(n)代價(jià)值小于實(shí)際值,導(dǎo)致搜索空間變大、路徑冗余的問題,采用不同節(jié)點(diǎn)代價(jià)函數(shù)選取不同公式對(duì)A*算法進(jìn)行改進(jìn)。針對(duì)重復(fù)搜索節(jié)點(diǎn),使尋路效率下降問題,通過建立數(shù)據(jù)表中的OPEN表與CLOSE表記錄被考察過的節(jié)點(diǎn),改進(jìn)傳統(tǒng)A*算法中重復(fù)搜索節(jié)點(diǎn),使尋路效率下降問題;通過建立數(shù)據(jù)表OPEN表與CLOSE表記錄被考察過的節(jié)點(diǎn),增強(qiáng)方向性,提高了算法的效率。采用MATLAB進(jìn)行仿真驗(yàn)證,對(duì)小車規(guī)劃路徑進(jìn)行擬合處理,使得優(yōu)化之后的路徑更加平滑,小車最終實(shí)現(xiàn)從起點(diǎn)至終點(diǎn)的路徑規(guī)劃,實(shí)現(xiàn)機(jī)器人SLAM功能。在改進(jìn)A*算法下對(duì)全局路徑進(jìn)行規(guī)劃,利用DWA算法對(duì)局部路徑進(jìn)行規(guī)劃以達(dá)到最優(yōu)的實(shí)時(shí)避障效果。