多體位變換椅自主定位導航A*算法改進研究

高興華,王從永,崔曉越

(北華大學 機械工程學院,吉林 吉林 1320012)

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

1 多體位變換椅結構與控制系統

1.1 多體位變換椅結構

多體位變換椅機械結構如圖1所示。主要結構包括:底盤機構、驅動機構、雷達掃描機構、升降機構、座椅翻轉機構、小腿板翻轉機構和坐板翻轉機構等。

圖1 體位自主導航智能輪椅機械結構方案示意

本輪椅使用絲杠傳動機構及其他輔助機構配合完成高坐姿、坐姿和低坐姿的位置變換;采用背板翻轉機構及其他機構輔助解決座椅與床之間的姿態轉換;采用絲杠傳動機構、背板翻轉機構和小腿翻轉機構完成坐姿與立姿的轉換;采用后驅差速機構進行移動以確保在姿態變換時,重心偏移不會導致智能輪椅發生偏移。可實現坐、行、蹲、立和躺五種姿態,如圖2所示。

圖2 多體位自主導航智能輪椅總體位姿變換

1.2 控制系統

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

控制主體由上位機系統與下位機系統組成[4-6]。上位機系統負責處理系統綜合數據并發布指令,下位機系統主要完成底層驅動;上位機系統與下位機系統通過串口通信,下位機與遙控采用無線傳輸進行通信。其中,上位機為Mini工控機,主要負責將控制指令信息發送至下位機,進而驅動控制系統,并實時顯示智能輪椅的狀態,同時,接受來自驅動控制系統的狀態信息;上位機通過串口接收雷達的信息,實現建圖、導航及避障信息的處理。下位機的處理器使用STM32芯片,主要負責接收和發送硬件驅動的信息,為驅動控制系統提供控制信號,實時監測并且記錄系統狀態。操作遙控的處理器使用STM32芯片,主要是給下位機發送控制指令,控制原理如圖3所示。

圖3 控制系統原理結構圖

2 自主導航算法及改進

2.1 自主導航總體框架

多體位變換輪椅導航框架的核心是感知層、規劃層和控制層,信息由上層向下傳遞,共同實現多體位輪椅的導航。導航框架如圖4所示。

圖4 導航框架

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

2.2 基于DWA算法的局部路徑規劃

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

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

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

(1)

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

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

(2)

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

(3)

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

Vr=Vm∩Vn∩Vmax,

(4)

現已得到速度矢量空間,下一步是計算速度矢量空間中各個速度矢量的評價值,DWA算法評價函數表達式為

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

(5)

其中,α、β、γ均為權重系數,可根據實際需要來進行設定。heading(v,ω)用來評價在當前速度矢量下,θ表示智能輪椅到達模擬軌跡終點時的姿態角和目標姿態角之間的差值。heading(v,ω)的表達式為

(6)

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

(7)

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

(8)

可以看出,速度v的值越大velocity(v,ω)函數值越大。從評價函數的各個表達式可以看出,DWA算法是一種以目標點為導向的局部路徑規劃算法,力求盡可能快速地移動到目標點,在實際應用中,可以結合實際需求來設置權重系數,以達到最優的實時避障效果。

2.3 柵格構建地圖

本設計選擇地圖構建領域應用最廣泛的柵格地圖法,來實現全局路徑規劃算法路徑搜索,柵格地圖路徑規劃如圖5所示。根據智能輪椅的實際工作環境,將地圖分割成大小一致的二維網格圖,“0”代表自由柵格,表示當前網格沒有被障礙物占領,在網格圖中用白色填充;而“1”代表占用柵格,表示當前網格被障礙物占領,在網格圖中用黑色填充。

2.4 傳統A*算法

Dijkstra算法在二維平面上從初始節點向四周蔓延,直到蔓延到目標節點。在向四周蔓延時不會因為目標節點而改變蔓延方向和范圍,對四周的行為是一致的,沒有盲區地進行遍歷,如圖6所示。

圖6 Dijkstra算法示意圖

A*算法基于評估函數的啟發式路徑搜索算法,從當前坐標開始,采用向下搜索的方式搜索最小代價點,并把搜索到的最小代價點更新為當前位置坐標繼續搜索,直到最小代價點達到目的坐標點[13-14]。搜索公式可顯示為

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

(9)

其中,f(n)代表代價評估函數,它的含義是評估坐標起點到目的坐標的距離評估;g(n)表示的是坐標起點與當前節點之間的實際距離;h(n)為啟發函數,代表的是當前任意節點到目的坐標點的最優距離估計。

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

(10)

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

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

(11)

其中cos*(n,ngoal)是柵格節點n到目標節點的最優距離。

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

第一,如果h(n)

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

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

傳統A*算法的不足點:

1.重復尋找節點,使尋路的效率下降;

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

3.輪椅視為質點遇見障礙物時精度不夠甚至穿模。

2.5 A*算法改進理論

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

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

(12)

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

(13)

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

(14)

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

圖7 柵格節點連接關系示意圖

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

(15)

在以上的基礎上通過創建表OPEN記錄已被考察的節點,創建CLOSED表記錄已經被檢測過的節點,通過表P記錄當前節點的父節點,同時,A*算法在執行過程中還需要對上述表格進行維護。

基于改進的A*算法多體位輪椅路徑規劃流程如圖8所示。

圖8 A*算法改進流程圖

多體位變換輪椅基于改進后A*算法的路徑規劃過程為

1.建立數據表OPEN表與CLOSE表,將起點S寫入OPEN表中,將障礙點寫入CLOSE表中。

2.計算OPEN表中最小節點值f并將其納入CLOSE表中,若OPEN表為空則直接結束。

3.判斷當前節點m是否為目標節點,若是目標節點則根據m的前向路徑生成最優路徑;若不是則擴展目標節點m,生成后繼節點n。

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

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

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

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

3 A*算法改進MATLAB仿真及驗證

3.1.1 傳統A*算法路徑規劃

采用柵格法構建地圖,利用傳統A*算法規劃路徑。在傳統A*算法中啟發函數h(n)一般使用曼哈頓距離定義,則啟發函數:

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

(16)

那么多體位變換椅行駛到n點的代價函數為

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

(17)

3.1.2 仿真結果與分析

基于MATLAB進行仿真模擬,仿真環境為30 m*30 m,采用曼哈頓距離作為啟發函數對算法進行仿真,結果如圖9所示。

(a)

從仿真結果上看傳統A*算法可以規劃出一條從起點到終點的多折線路徑。在路徑規劃時,拐點與拐點優先以對角線規劃,直到對角線碰到障礙物,之后緊貼障礙物以直線規劃。此算法存在搜索空間變大、緊貼障礙物,路徑冗余及轉彎半徑大的問題。針對上述傳統A*算法的不足,通過對啟發函數的優化可提升路徑規劃性能。

3.2 改進A*算法仿真與分析

為驗證A*算法的可行性,選用MATLAB進行仿真模擬,仿真環境為30 m*30 m,任意節點采用歐式距離函數作為啟發函數,8節點連接選用切比雪夫距離,4節點的連接選用曼哈頓距離作為啟發函數對算法進行仿真驗證[15]。在構建地圖中小車從起始點出發,到達目的地后由改進A*算法規劃的路徑如圖10所示,而實際運行軌跡如圖11所示。

圖10 改進A*算法模擬運動軌跡

圖11 實際運行軌跡

從仿真結果可以看出A*算法模擬軌跡較短,經擬合平滑處理后在拐點處轉彎半徑小,折線段優化為弧線段,較符合實際情況,由此可見對改進后的A*算法是可行的。

4 結 論

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