保證智能輪椅平滑通過狹窄通道的路徑曲率優(yōu)化算法

陳靈 王森 胡豁生 麥當勞-麥爾克勞斯 費敏銳

保證智能輪椅平滑通過狹窄通道的路徑曲率優(yōu)化算法

陳靈1王森2胡豁生3麥當勞-麥爾·克勞斯3費敏銳1

為了使用戶更感舒適，智能輪椅應該能在具有復雜幾何約束的室內環(huán)境下平滑地通過狹窄通道.本文提出一個基于路徑曲率優(yōu)化的算法以確保智能輪椅平滑地通過狹窄過道.考慮到路徑平滑度取決于路徑曲率及其變化率，在通過傳感器數(shù)據(jù)計算出狹窄通道相對于輪椅的位置后，算法以貝塞爾曲線的曲率及其變化率最小為優(yōu)化目標，以輪椅過通道時的方向及貝塞爾多邊形應為凸多邊形作為約束，規(guī)劃出一條平滑的最優(yōu)路徑，然后控制輪椅實時跟蹤這條路徑.上述過程動態(tài)循環(huán)運行，實現(xiàn)了智能輪椅平滑通過狹窄通道.仿真中將本文算法同基于A*的路徑規(guī)劃導航算法進行了對比，結果表明本文提出的基于曲率優(yōu)化的算法可以實現(xiàn)比A*算法路徑曲率更小且更加平滑的過狹窄通道過程,并且即使在沒有全局地圖和定位信息情況下，算法也能控制輪椅平滑地通過狹窄過道.實驗中詳細闡述了算法的實現(xiàn)過程，實驗結果也證實了算法的有效性.

智能輪椅,曲率優(yōu)化,貝塞爾曲線,過通道

DOI10.16383/j.aas.2016.c160185

室內環(huán)境往往具有很多狹窄的過道,這給輪椅的正常工作帶來較大挑戰(zhàn).另一方面,輪椅用戶通常是行動不便者和老人,他們不能很好地控制輪椅.因此,一個巨大的需求在于智能輪椅能自動穿過較窄的過道,以便輪椅用戶能感到安全和舒適[1].

智能輪椅過通道很大程度上依賴于其運動控制算法,可以分為兩個部分:路徑規(guī)劃[2?5]和運動導航[6?7].路徑規(guī)劃用于計算一條穿過狹窄通道的可行的路徑而不碰撞障礙物.傳統(tǒng)的路徑規(guī)劃算法,如A*[8?9]和它的變體FHD*[10]運用優(yōu)化的方法去找最短的避障路徑.盡管這些算法已經(jīng)成功用于通用移動機器人,它們卻不能直接用于智能輪椅上而規(guī)劃一條穿過狹窄通道的平滑路徑,因為它們沒有考慮環(huán)境的物理空間限制和用戶舒適度.

由于通道寬度與輪椅寬度的比值較小(如英國標準研究院[11]所規(guī)定的,輪椅兩邊與通道墻之間只有300mm.圖1(a)展示了在作者實驗室里的相同的情況,留給輪椅通過過道的空隙兩邊共只有26cm),對輪椅來說很難使其在任意的航向角位置通過狹窄的門,如圖1(b)所示,只有當輪椅的航向角垂直于門線時輪椅才能通過.而對于這樣的物理空間的約束以及如何在此約束中平滑地通過通道,許多有關研究文獻并沒有考慮到.如Cheein等在文獻[12]提出一種整合同時定位與地圖構建算法的邊界點方法用于輪椅通過過道.然而,它需要連續(xù)地產生平均邊界點,從而使得輪椅的軌跡不平滑而且耗時.Peula等[13]提出基于案例推理的輪椅通過過道的方法,同樣地,其也沒有考慮規(guī)劃路徑的平滑性.

路徑的平滑性一定程度上決定了輪椅用戶在輪椅過狹窄通道時的舒適性,因此研究者提出了其他考慮路徑平滑性的路徑規(guī)劃方法,如基于圓弧[14?15]、樣條[1,16]和貝塞爾曲線[17?20]的方法.文獻[1]提出基于B樣條的方法,讓一臺仿真輪椅在仿真環(huán)境中通過一個狹窄的通道,而并沒用真實輪椅進行測試.另外,文獻[17]提出基于貝塞爾曲線的路徑規(guī)劃方法,用于一般的移動機器人,Choi等[21]也將類似方法用于自主駕駛汽車上,其約束由航路點和過道寬度產生.同樣地,這些方法很難直接用在智能輪椅上.

本文提出一個基于路徑曲率優(yōu)化的算法以確保智能輪椅平滑地通過狹窄過道.算法以三次貝塞爾曲線的曲率及其變化率最小為優(yōu)化目標,以通過傳感器數(shù)據(jù)測得的過道相對于輪椅的位置、輪椅過通道時的方向及貝塞爾多邊形應為凸多邊形作為約束條件,實時計算滿足優(yōu)化目標和約束的三次貝塞爾曲線以作為期望路徑,然后控制輪椅速度以跟蹤此期望路徑,從而實現(xiàn)輪椅光滑地通過狹窄通道的目的.相比于傳統(tǒng)的A*算法及其變體,本算法使輪椅通過通道時路徑更加平滑;而相比于其他考慮平滑性的路徑規(guī)劃方法,由于本算法利用在線測得的通道位置實時計算期望路徑,因此不需要構建全局地圖以及基于地圖的全局定位,本算法更加簡單和更易實現(xiàn).

1 過通道策略

1.1 問題描述

過狹窄通道的問題可以看作是一個路徑規(guī)劃的問題.本文方法的目的是首先基于曲率優(yōu)化產生一個平滑路徑,然后控制輪椅去跟蹤它.只要產生的期望路徑能平滑通過狹窄過道且輪椅能夠精確地跟蹤它,那么過狹窄通道的問題就能成功解決.選擇輪椅上的某個點作為控制的參考點.為了實現(xiàn)過通道,算法策略應該滿足以下要求:

1)輪椅的實際路徑必須是優(yōu)雅且平滑的,沒有震蕩和模棱兩可的情形.

2)由于輪椅的幾何約束,路徑的曲率應該限制在某個范圍內.

3)當輪椅到達狹窄過道位置時,其航向角應該垂直于狹窄過道的平面.

圖2所示為輪椅過狹窄通道的原理描述.為了產生平滑的過通道的路徑,需要考慮輪椅的幾何特征和大小.檢測過道位置的傳感器所在位置或者后輪的中心可以選為操作點.因此兩個本地坐標系分別由{WL}(虛線)和{WC}(實線)表示,它們的原點分別是為全局坐標系G下狹窄通道的中心位置.和分別為輪椅的當前與期望位置,以及它們相對應的航向角Hs和Hd.除非特別聲明,全文所有的位置和航向角都是相對于坐標系{WC}的.

1.2 算法結構

圖3為提出的輪椅過狹窄通道的算法結構圖.算法主要有三個模塊,分別是:通道定位、路徑規(guī)劃和輪椅控制.在通道定位模塊中,采用激光測距儀或者3D攝像頭(如微軟Kinect傳感器),可以檢測出相對于輪椅坐標系的通道中間位置.一旦通道位置被精確找到,它就被路徑規(guī)劃模塊用于通過曲率優(yōu)化產生一條期望的貝塞爾曲線路徑.在輪椅控制模塊中,期望的路徑與輪椅當前位置之間的誤差輸入到PID控制器計算出輪椅二輪的速度,從而控制輪椅跟蹤這個路徑.這個過程反復實時運行.

圖2 輪椅過狹窄通道過程原理圖Fig.2 The schematic description of the narrow gap passing process

每當檢測到狹窄過道,算法動態(tài)產生一條平滑的路徑,因此,過狹窄通道的策略可以連續(xù)不斷進行而不需要里程計或者全局定位信息.這樣可以大大減少來自輪椅和不確定環(huán)境帶來的各種干擾,使得過狹窄通道過程更安全和有效.每個模塊的詳細描述如下.

1.2.1 通道定位

通道定位模塊目的在于計算通道在輪椅坐標系里的相對位置(包括位置Pd和方向Hd,即過道法向量的方向),然后作為路徑規(guī)劃模塊的輸入.通道定位具體算法取決于所用的傳感器類型.當使用二維激光測距儀時,結合分割和直線擬合的方法可以用來定位通道.后文實驗部分將會詳細介紹此方法.當使用深度攝像頭時,紅-綠-藍顏色的圖像數(shù)據(jù)首先通過分割算法被用于通道的識別,然后帶有深度信息的深度圖像用于精確找到通道相對于攝像頭的位置.在攝像頭的外部參數(shù)標定好的情況下,計算出來的結果可以很簡單地通過平移旋轉運算計算出通道相對于輪椅的位置.

1.2.2 路徑規(guī)劃

路徑規(guī)劃的目的在于采用優(yōu)化方法確定貝塞爾曲線的控制點,使得由這些控制點決定的貝塞爾曲線路徑具有最小的曲率變化率.最平滑的貝塞爾曲線滿足各種來自輪椅和環(huán)境的約束.由于輪椅和過道之間的相對位置可實時計算得到,路徑規(guī)劃也連續(xù)不斷地進行著.圖2表示了弧線PsPd的控制點為Pi,i∈0,1,2,3,也就是三次貝塞爾曲線的控制點[21].

圖3 輪椅過狹窄通道算法結構圖Fig.3 Strategy architecture of narrow gap passing

然后,三次貝塞爾曲線便可生成,以連接點Ps和Pd,方向分別為Hs和Hd.也就是說,在檢測到狹窄通道后,算法通過對貝塞爾曲線的曲率的優(yōu)化,利用通道的中間點位置Ps及其對應的方向Hd進行控制點的計算.因為Ps和Pd分別已經(jīng)對應了已知的控制點P0和P3,那么參數(shù)估計僅僅需要計算P1和P2,至此,在已知P0和P3情況下,整個貝塞爾曲線就被完全確定了.需要注意的是Hd期望是垂直于墻壁的.

為了滿足前述的各項要求,根據(jù)環(huán)境和實際情況,為優(yōu)化設計了兩類約束:

1)方向.因為路徑規(guī)劃是從航向角Hs開始的,期望的貝塞爾曲線在位置P0處切線的斜率應該等于Hs.類似地,為了克服輪椅與通道寬度之間比值小的問題,點P3處的切線應該是通道的中間位置處的法向方向.因此,點P1和P2的可選范圍進一步減小為一維.也就是說,由于貝塞爾曲線的第二個屬性,點P1和P2必須分別處于點P0和P3的切線上.并且,P1需要沿著Hs的方向尋找,而P2只可能出現(xiàn)在Hd的反方向.因此,有關于方向的約束為

其中,Pc是直線P0P1和P2P3的交點,‖·‖表示二范數(shù).這兩個約束保證了由點P1和P2確定的貝塞爾曲線可以連接起點終點即P0和P3,并且對應的方向分別為Hs和Hd.

2)完全凸性.如前所述對曲率的約束要求曲線沒有曲率的突然變化.于是,貝塞爾曲線必須是凸的.已經(jīng)知道,如果貝塞爾曲線的控制點構成的多邊形是凸的,那么貝塞爾曲線也呈完全凸性.因此,P1和P2必須在保證曲線的完全凸性的情況下進行約束.這里采用向量叉乘的方法來實現(xiàn)此約束.向量的叉乘ω1和ω2分別為

故而,完全凸性的約束表示為

其中,sgn(ωi),i=1,2為向量ωi的旋轉方向.

帶約束的優(yōu)化是為了找到控制點P1和P2使得曲線盡量光滑,也就是曲率及其變化率應當盡量小.由于這里只考慮二維環(huán)境,且為了平衡精度與算法復雜度,采用三次貝塞爾曲線,曲線坐標表示為B(t)=(x(t),y(t)),其中

其中,(Ai,Bi)表示控制點Pi的坐標.那么貝塞爾曲線相對于變量t的曲率為

其中,ρ(t)為曲線半徑.因此,P1和P2可由如下受約束于式(1)和式(3)的優(yōu)化問題求得:

當輪椅到達狹窄的過道時,期望的輪椅航向角應該垂直于過道.但是由于干擾和控制的誤差,最終的輪椅位置和方向可能有所偏離期望值.為了滿足上述第3個要求,在輪椅通過過道前,輪椅的航向角必須調整為垂直于過道.

1.2.3 輪椅控制

為了使得輪椅能夠跟蹤規(guī)劃好的路徑,采用PID反饋控制器.輪椅實際的位置與參考路徑之間的誤差作為PID控制器的輸入.如圖4所示,沿著輪椅的航向角距離輪椅為Z的一點F用來定義位置誤差,然后將點F投影到參考路徑于點S,使得垂直于點S的切線.位置誤差由點F和S之間的距離Derr表示.交叉跟蹤誤差Cerr由期望路徑和輪椅重心位置(xr,yr)之間的最短距離表示.

圖4 位置誤差的計算Fig.4 The calculation of the position error

在本文中,假定徑向速度V為常量,角速度ω作為PID控制器的輸出量.數(shù)字PID控制可以描述為

2 仿真和分析

2.1 仿真結果

首先,進行仿真來驗證算法的核心部分,也就是基于貝塞爾曲線的曲率優(yōu)化的路徑規(guī)劃算法.假設通道位置已知.進行了2種場景的仿真,分別為場景A(目標位置在通道左邊)和場景B(目標位置在通道右邊),如圖5所示.

對于每一種情形,路徑規(guī)劃算法算出四個不同的最優(yōu)參考軌跡.每一個參考路徑對應不同的參數(shù)如輪椅的起始位置及其航向角,目標位置及其航向角.

表1列出了每一條參考路徑對應的所需參數(shù),其中,Ps,Pd和Pt分別表示輪椅在走廊里的起始坐標、通道中間點的坐標和在房間里的目標點位置坐標,單位為m.i(Hs,Hd)表示路徑i的起始位置和通道中間位置對應的航向角(單位為度),i(Hd,Ht)表示路徑i的通道中間位置和穿過通道之后的目標點位置對應的航向角.以路徑1為例,對于場景A,對應輪椅的起始位姿由起始坐標Ps及航向角Hs構成,即(0.1,1.3,?20),終點位姿由通道中間點的坐標Pd及航向角Hd構成,即(1.6,1.8,90),路徑2與路徑1類似;再以路徑3為例,對應輪椅的起始位姿由通道中間點的坐標Pd及航向角Hd構成,即(1.6, 1.8,90),終點位姿由目標點位置坐標Pt及航向角Ht構成,即(0.0,3.5,160),路徑4與路徑3類似.以這些參數(shù)作為輸入,路徑規(guī)劃算法便可計算出平滑路徑然后由PID控制器控制輪椅進行跟蹤.PID控制器輸出驅動輪椅行進的轉速.控制器各系數(shù)選擇為kp=0.01,ki=0.001和kd=0.001.

圖5展示了計算出的參考路徑及其對應的輪椅實際路徑,各參考路徑對應的參數(shù)如表1給出.由圖5可以得出如下結論:

表1 每條路徑的輸入?yún)?shù)Table 1 The input parameters for each path

圖5 輪椅的參考路徑和實際路徑Fig.5 Reference path and actual path of wheelchair

1)所有的最優(yōu)參考路徑滿足在路徑規(guī)劃部分提到的約束:首先,輪椅始終在走廊里;其次,貝塞爾曲線在起始位置的切線方向等于輪椅的航向角,在目標位置也是如此;再次,計算出的貝塞爾控制多邊形是凸的.

2)所有最優(yōu)參考路徑是平滑的.

3)盡管有小的跟蹤誤差(從圖中放大部分可以看出),輪椅的實際軌跡能較精確地跟蹤參考路徑.

因此,仿真驗證了過狹窄通道算法的有效性.

為了分析輪椅實際跟蹤路徑的平滑性和精確度,如圖6(a)和圖6(b)所示,分別展示了場景A仿真中的輪椅角速度ω和距離誤差Derr相對于貝塞爾曲線參數(shù)t的變化曲線.由圖6可知,角速度沒有呈現(xiàn)出突變,表明了輪椅運動的平滑性.最大的跟蹤誤差約為55mm,都在合理和接受的范圍,表明跟蹤精度較高.

2.2 與其他算法的比較

為了驗證本算法相對于其他主流室內路徑規(guī)劃算法的在過狹窄通道情況下的優(yōu)越性,進行了另一個仿真以比較本文算法和基于A*[22]的開源算法在路徑曲率(光滑度)上的表現(xiàn).如圖7所示,實驗場景為包含2個80cm狹窄通道的室內環(huán)境,仿真平臺為基于機器人操作系統(tǒng)(Robot operating system,ROS[23])的Gazebo[24]三維仿真器.仿真中,本文算法的核心部分在Matlab中運行,通過Matlab的Robotics工具箱可以實現(xiàn)與ROS和Gazebo系統(tǒng)的數(shù)據(jù)通信,一方面來自Gazebo中輪椅上的激光測距儀數(shù)據(jù)可以傳入Matlab中通道定位模塊,實現(xiàn)通道位置計算;另一方面Matlab中輪椅控制模塊計算出的輪椅速度值可以發(fā)送給Gazebo中的輪椅,實現(xiàn)輪椅的控制.其中路徑曲率優(yōu)化算法采用Matlab優(yōu)化工具箱中的內點法優(yōu)化方法.而基于A*路徑規(guī)劃導航算法實現(xiàn)采用ROS的開源軟件包Navigation Stack[25].仿真時,兩種方法均控制輪椅從起點位姿(1.5m,0m,0rad)穿過2個狹窄通道并到達終點位姿(0.5m,3.5m,3.14rad),線速度設定為0.15m/s.其中位姿第3項表示輪椅的航向角.

圖6 在場景A中每一條路徑對應的輪椅的角速度和跟蹤誤差Fig.6 The angular rate and the tracking errors for each path in ScenarioA

圖7 算法比較的仿真環(huán)境Fig.7 Simulation environment for algorithm comparison

圖8分別展示了由本文方法和A*算法產生的輪椅路徑,由圖8可以定性地看出A*算法產生的路徑在2個狹窄通道處有“急轉彎”,即有較大曲率,而本文提出的基于曲率優(yōu)化方法產生的路徑則沒有“急轉彎”,轉彎過程較均勻的分配在整個路徑中,而不會在通道處出現(xiàn)較大曲率,使路徑較平滑.

圖8 基于本文方法和A*算法的輪椅路徑Fig.8 Actual trajectories generated by the proposed method and A*algorithm

圖9展示了兩種方法產生的路徑的曲率,可以看出,A*產生的路徑的曲率具有多次跌宕起伏,最大曲率達到2.2,而基于曲率優(yōu)化方法產生的路徑的曲率只有2次起伏,且最大曲率僅為1.1,遠小于A*產生的路徑的曲率.因此,圖9進一步說明了本文提出的基于曲率優(yōu)化的算法可以實現(xiàn)比A*算法路徑曲率更小且更加平滑的過狹窄通道過程.

圖9 基于本文方法和A*算法的路徑曲率的比較Fig.9 The comparison of trajectory curvature generated by the proposed method and A*algorithm

另外,需要指出的是基于A*的路徑規(guī)劃導航算法需要事先構建好環(huán)境地圖,然后再運行基于此地圖的全局定位算法,而本文算法只需要根據(jù)傳感器信息識別并計算狹窄過道相對于輪椅坐標系的相對坐標,而并不需要構建地圖和全局定位,因此本文方法更簡單且更易實現(xiàn).

2.3 實時性分析

第2.2節(jié)仿真中基于內點法的優(yōu)化方法計算復雜度較高,較大程度上影響了算法的實時性.為了評估本文算法的實時性,圖10展示了算法每次循環(huán)中各模塊運行時間及總時間.由圖10可以看出,每次循環(huán)中基于內點法的路徑規(guī)劃模塊所需運行時間在0.15s左右,而每次循環(huán)算法總時間約為0.2s,即5Hz,在輪椅線速度為0.15m/s時滿足實時性要求.仿真運行的軟硬件配置為:Intel i5 CPU,4GB RAM,操作系統(tǒng)為Linux 14.04,Matlab軟件版本為R2015b.需要指出的是仿真算法是運行在解釋性語言Matlab中,如果算法由編譯性語言如C++等實現(xiàn),算法運行時間將會更小,實時性可以更好.

圖10 算法每次循環(huán)中各模塊運行時間及總時間Fig.10 Consuming time of each algorithm module and their total value for each iteration

3 實驗

為了進一步驗證和評估輪椅過狹窄通道的算法,用真實輪椅進行了實驗.如圖11(a)和圖11(b)所示,輪椅的長度和寬度分別為85cm和65cm.采用激光測距儀測算通道位置.由于單個激光測距儀的視角不足以覆蓋所有的安全區(qū)域,結合兩個激光測距儀一起以構成更廣視角.兩個激光測距儀和后輪的距離為76cm,對應的偏向角θ1和θ2分別為46.36°和?56.50°.由于過道寬度為80cm,每一邊留給輪椅的自由空間只有7.5cm.在實驗之前,首先對兩個激光測距儀之間的相對位置關系進行標定以使二者的數(shù)據(jù)具有一致性.

圖11 輪椅和激光的配置Fig.11 Confguration of the wheelchair and the lasers

圖12描述了實驗場景(5m×5m),其中有一些墻壁和一間屋子.實線框住的是輪椅,其分別從A,B,C和D四個位置出發(fā),穿過過道進入到屋子.輪椅的全局位置坐標由位置跟蹤系統(tǒng)VICON獲得,以評估算法的有效性.過通道算法的實現(xiàn)細節(jié)描述如下.

3.1 通道定位

利用來自兩個激光測距儀的數(shù)據(jù),通道定位算法計算通道中心位置及其對應的航向角Pd和Hd,其中航向角應垂直于過道平面,可由墻壁的斜率表示,即Hd=arctan(?1/Kw).Pd和Hd則作為產生貝塞爾曲線的輸入?yún)?shù).主要采用分割和直線擬合兩個步驟計算Pd和Hd.

圖12 實驗場景Fig.12 Experimental scenario

3.1.1 分割

分割的目的在于將激光掃描數(shù)據(jù)分成幾個不同的子集使得每個子集點屬于一條直線.例如,如圖12所示,一次完整的激光掃描數(shù)據(jù)點被分成6個子集.首先,采用算法Adaptive breakpoint detector (ABD)[26]通過檢測斷點來找到所有連續(xù)點集.如圖13所示,共有三個點集:{1,2},{3}和{4,5,6}.然后,采用算法Iterative end-point ft(IEPF)[27]將每個連續(xù)點集分成不同的子集,以使每個子集的點都屬于同一條直線.對每兩個子集,計算第1個子集的末尾點(Pr)與第2個子集的起始點(Pf)之間的距離Drf,如果Drf等于過道的寬度,那么就找到了過道,并且Pd=(Pr+Pf)/2.如圖9所示,通道在子集2和子集4之間找到了.

圖13 一次激光掃描數(shù)據(jù)點及其分段Fig.13 A full scan of laser data points and its expected segmentations

3.1.2 直線擬合

在通道位置找到之后,接下來是要計算找到的通道所倚靠的墻壁的斜率Kw以確定Hd.Kw的計算可通過擬合對應于墻壁的所有激光數(shù)據(jù)點構成的直線來實現(xiàn).直線擬合的目的是確定直線的參數(shù).直線表示方法有多種,其中一個表示直線的參數(shù)形式為

其中,q和k分別為直線y的截距和斜率.假設有一個需要擬合的點集R={(xi,yi)|i=1,···,k},那么參數(shù)k和q可通過最小二乘直線擬合方法計算為

其中,式(9c)為直線斜率.

對于如圖12所示的場景,激光點集2和點集4為構成墻壁的點.把這些點的笛卡爾坐標代入式(9a)～(9d)便計算出斜率Kw.

盡管路徑規(guī)劃算法已經(jīng)設計好,但在實際中還不能直接用于輪椅,尤其是在輪椅大體積的情況下.下面就通過選擇合適的起點P0和目標點P3,也就是Ps和Pd來解決這個問題.改進后的算法能產生在實際中令人滿意的路徑,且兼具光滑性和實用性.

3.2 控制點的選擇

當新的貝塞爾曲線路徑產生時,控制點P0的坐標總被設置為(0,0).然而,如圖2所示,P0的位置可以是一般而言,應該選擇因為有關過道的位置檢測都是基于激光的位置.當輪椅作為一個質點或者激光和具有驅動力的輪子之間的距離很小時,選擇是合理的.然而,由于輪椅的大體積,這兩種情形都不適用于真實輪椅.而且,很難控制輪椅后輪使裝在前面的激光跟蹤規(guī)劃好的路徑.因此,在規(guī)劃貝塞爾路徑時,選為控制點如圖2圓點所示.對應地,整個過通道算法策略都是基于坐標系{WC}.同時,選擇比選擇對于輪椅控制誤差來說更具魯棒性,因為對于相同的控制誤差造成的偏差,的情形要多于因為Pcs距離Pls較遠,在坐標系{WL}中計算出的通道的坐標需要平移至坐標系{WC}中.這個平移量只有沿X軸的量,且平移量為Pcs和Pls之間的距離,也就是如圖2所示的L.

3.3 期望位置的選擇

當輪椅到達過道中心時,激光位置可能恰好和墻壁共線,它們可能不能精確地檢測到過道的位置甚至根本檢測不到.同時由于現(xiàn)實情況下的各種干擾,輪椅的航向角不能恰好到達期望值.因此,輪椅需要在過通道時調整其航向角以使其垂直于過道平面,從而減小碰撞到過道框架的可能性.

為了克服這個問題,在確定Pd時,在過道定位環(huán)節(jié)中計算出來的值的基礎上沿著Hd的方向設置一個偏移量,以保證激光有足夠的前方距離來檢測墻壁.于是,輪椅可以在位置而不是Pd調整其航向角垂直于過道平面,如圖2所示.設偏移量為do,則新的Pd可表示為然而,由于貝塞爾曲線的控制點為偏移之后的點要被進一步偏移到才被當作控制點P3進而產生貝塞爾路徑.可由下式計算出:

其中,(xPd,yPd)為通道位置Pd的坐標,為的坐標,dos為偏移量大小,包括實際偏移量do和輪椅的長度L.

從圖14可以看出,由于輪椅尺寸大,單個激光測距儀不能檢測整個通道的范圍,這就可能造成計算出錯誤的路徑和不合適的控制.而結合兩個激光測距儀的方法可以為通道定位提供完整范圍.

圖14 兩個激光測距儀相對于單個的優(yōu)勢Fig.14 Advantage of two lasers compared with single laser

圖15展示了4個不同起點位置對應的輪椅的軌跡.對于每個起始位置,進行了2次試驗.可以顯然地看到,在每個起點,輪椅都能找到通道位置,然后平滑而精確地通過過道而不碰撞到通道兩邊邊緣,證實了本文所提出算法的精確度和有效性.

圖15 輪椅軌跡Fig.15 Trajectories of the wheelchair

圖16分別展示了輪椅在起點位置A,B,C和D對應的到達期望位置時的實際位置.可以看出,幾乎所有的實際位置都在期望位置的附近,只略有較小誤差,并且多數(shù)航向角都按期望的那樣垂直于通道平面,盡管有些輕微地偏離了期望值.這些誤差主要有兩個原因:1)輪椅前面的隨動輪在方向上造成較大干擾;2)選擇Pd過程中的長偏移量放大了通道定位的誤差.而實際位置和期望位置之間的水平誤差都小于7.5cm,表明本文所提算法能減小來自現(xiàn)實的干擾和不確定性,從而使輪椅成功通過即使只有狹窄自由間隙的通道.這歸功于根據(jù)輪椅位置連續(xù)不斷地進行路徑規(guī)劃的策略.值得一提的是,如果不是采用這種策略,而采用只在第1次檢測到通道時規(guī)劃一條路徑再控制輪椅跟蹤這條路徑的方法,會使輪椅不能到達期望位置,因為輪椅在跟蹤此路徑時需實時估計自身位置,而如果只依賴輪椅上的航位推算法而不借助其他定位算法,由于航位推算位置估算方法無限地累計誤差會使跟蹤誤差越來越大,從而造成跟蹤失敗.

圖17為每次試驗在三個不同階段的角速度變化曲線.在第一個階段,角速度遞增直到輪椅快要到達過道中間的位置;在第二個階段,輪椅調整其航向角以保證航向角垂直于過道;在第三個階段,輪椅徑直走向屋子.需要指出的是在所有試驗的第一個階段,角速度平滑遞增而沒出現(xiàn)突變現(xiàn)象,再一次證明了算法成功實現(xiàn)了平滑過通道的目的.另外,對于同一個起始位置的兩次不同試驗,對應輪椅的角速度曲線重疊,表明算法的高可重復性.

圖16 輪椅從不同起點到達終點位置時的實際位置Fig.16 Actual wheelchair poses when arriving at the desired pose with diferent starting posesA,B,CandD

圖17 輪椅的角速度Fig.17 Angular rate of the wheelchair

圖18為輪椅從起點A和D出發(fā)的軌跡截圖(因為相似性,起點B和C的截圖在此省略),包括了幾個重要的位置,如起點和在過通道前后的位置.在兩個起點的第5張截圖中,輪椅幾乎在通道的中間位置,航向角也幾乎垂直于通道平面,證實了算法的有效性.第6張截圖也表明輪椅能以期望的航向角穿過通道.

圖18 輪椅從起點A和D出發(fā)過通道軌跡截圖((a)～(f)對應于起點A,(g)～(l)對應于起點D)Fig.18 Snapshots of the wheelchair's trajectories when passing gap,starting from poseAandD((a)～(f), (g)～(l)are with poseAandD,respectively.)

4 結論

本文提出了一種智能輪椅通過狹窄通道的新穎方法.狹窄通道指自由空間只有幾厘米的門或者過道.所提出的路徑規(guī)劃算法基于貝塞爾曲線曲率優(yōu)化,從而產生平滑的路徑以控制輪椅跟蹤此路徑.算法按照“定位通道”→ “產生貝塞爾曲線路徑”→“跟蹤產生的路徑”的順序連續(xù)執(zhí)行.算法考慮輪椅和環(huán)境的各種約束,能夠在沒有全局坐標或者里程計的情況下平滑地穿過狹窄通道.

首先,進行了仿真來驗證算法的核心部分,即基于貝塞爾曲線的曲率優(yōu)化的路徑規(guī)劃算法.然后仿真比較了本文算法與基于A*的路徑規(guī)劃開源導航算法在路徑平滑度上的表現(xiàn),分析了算法實時性.最后用裝有兩個激光測距儀的真實輪椅進行對算法的實驗驗證,結果證實了整個算法的可行性和有效性.算法不僅適用于輪椅,還可用于其他各類想要通過狹窄通道的移動機器人.未來的工作主要測試輪椅在不同復雜室內環(huán)境下的過通道算法,比如辦公室、購物中心、居家和醫(yī)院等,主要解決過不規(guī)則障礙物的問題.

1 Elbanhawi M,Simic M,Jazar R N.Continuous path smoothing for car-like robots using B-spline curves.Journal of Intelligent and Robotic Systems,2015,80(1):23?56

2 Yoon S,Yoon S E,Lee U,Shim D H.Recursive path planning using reduced states for car-like vehicles on grid maps.IEEE Transactions on Intelligent Transportation Systems, 2015,16(5):2797?2813

3 Pan Z,Li J Q,Hu K M,Zhu H.Intelligent vehicle path planning based on improved artifcial potential feld method.Applied Mechanics and Materials,2014,742:349?354

4 Zhang Hao-Jie,Gong Jian-Wei,Jiang Yan,Xiong Guang-Ming,Chen Hui-Yan.Research on incremental heuristic path planner with variable dimensional state space.Acta Automatica Sinica,2013,39(10):1602?1610 (張浩杰,龔建偉,姜巖,熊光明,陳慧巖.基于變維度狀態(tài)空間的增量啟發(fā)式路徑規(guī)劃方法研究.自動化學報,2013,39(10):1602?1610)

5 Zu Di,Han Jian-Da,Tan Da-Long.LP-based path planning method in acceleration space for mobile robot.Acta Automatica Sinica,2007,33(10):1036?1042 (祖迪,韓建達,談大龍.加速度空間中基于線性規(guī)劃的移動機器人路徑規(guī)劃方法.自動化學報,2007,33(10):1036?1042)

6 Park J W,Im W S,Kim D Y,Kim J M.Safe driving algorithm of the electric wheelchair with model following control.In:Proceedings of the 16th European Conference on Power Electronics and Applications.Lappeenranta,Finland: IEEE,2014.1?10

7 Sinyukov D A,Padir T.Adaptive motion control for a differentially driven semi-autonomous wheelchair platform.In: Proceedings of the 2015 International Conference on Advanced Robotics.Istanbul,Turkey:IEEE,2015.288?294

8 Zhang Z Y,Zhao Z P.A multiple mobile robots path planning algorithm based on a-star and Dijkstra algorithm.International Journal of Smart Home,2014,8(3):75?86

9 Bhadoria A,Singh R K.Optimized angular a star algorithm for global path search based on neighbor node evaluation.International Journal of Intelligent Systems and Applications, 2014,6(8):46?52

10 Seder M,Mostarac P,PetroviI.Hierarchical path planning of mobile robots in complex indoor environments.Transactions of the Institute of Measurement and Control,2011,33(3?4):332?358

11 BSI.Design of Buildings and Their Approaches to Meet the Needs of Disabled People.Code of Practice.Standard Number BS 8300:2009+A1:2010,ISBN 978 0 580 70730 8，British Standards Institute,2009.

12 Cheein F,De La Cruz C,Guimaraes E,Bastos-Filho T, Carelli R.Navigation system for UFES's robotic wheelchair.Devices for Mobility and Manipulation for People with Reduced Abilities.Boca Raton,FL:CRC Press,2014.41?93

13 Peula J M,Urdiales C,Herrero I,Fernandez-Carmona M, Sandoval F.Case-based reasoning emulation of persons for wheelchair navigation.Artifcial Intelligence in Medicine, 2012,56(2):109?121

14 Rastelli J R,Lattarulo R,Nashashibi F.Dynamic trajectory generation using continuous-curvature algorithms for door to door assistance vehicles.In:Proceedings of the 2014 IEEE Intelligent Vehicles Symposium.Dearborn,USA: IEEE,2014.510?515

16 Chu C H,Hsieh H T,Lee C H,Yan C Y.Spline-constrained tool-path planning in fve-axis fank machining of ruled surfaces.The International Journal of Advanced Manufacturing Technology,2015,80(9?12):2097?2104

17 Simba K R,Uchiyama N,Sano S.Real-time smooth trajectory generation for nonholonomic mobile robots using Bzier curves.Robotics and Computer-Integrated Manufacturing, 2016,41(1):31?42

18 Yang L,Song D L,Xiao J Z,Han J D,Yang L Y,Cao Y.Generation of dynamically feasible and collision free trajectory by applying six-order Bezier curve and local optimal reshaping.In:Proceedings of the 2015 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS).Hamburg,Germany:IEEE,2015.643?648

20 Chen Cheng,He Yu-Qing,Bu Chun-Guang,Han Jian-Da.Feasible trajectory generation for autonomous vehicles based on quartic Bzier curve.Acta Automatica Sinica, 2015,41(3):486?496 (陳成,何玉慶,卜春光,韓建達.基于四階貝塞爾曲線的無人車可行軌跡規(guī)劃.自動化學報,2015,41(3):486?496)

21 Choi J W,Curry R,Elkaim G.Piecewise Bezier curves path planning with continuous curvature constraint for autonomous driving.Machine Learning and Systems Engineering.Netherlands:Springer,2010.31?45

22 Hart P E,Nilsson N J,Raphael B.A formal basis for the heuristic determination of minimum cost paths.IEEE Transactions on Systems Science and Cybernetics,1968,4(2):100?107

23 Open Source Robotics Foundation(OSRF).Robot operating system[Online],available:http://www.ros.org,May 21, 2016

24 Open Source Robotics Foundation(OSRF).Gazebo[Online],available:http://www.gazebosim.org/,May 21,2016

25 Open Source Robotics Foundation(OSRF).Global planner of navigation stack in ROS[Online],available:http://wiki. ros.org/global_planner?distro=indigo,May 21,2016

26 Borges G A,Aldon M J.Line extraction in 2D range images for mobile robotics.Journal of Intelligent and Robotic Systems,2004,40(3):267?297

27 Duda R O,Hart P E.Pattern Classifcation and Scene Analysis.New York:Wiley,1973.

陳 靈上海大學機電工程與自動化學院助理研究員.主要研究方向為機器人定位與導航.本文通信作者.

E-mail:lcheno@shu.edu.cn

(CHEN LingAssistant professor at the School of Mechatronics Engineering and Automation,Shanghai University. His research interest covers localization and navigation of robots.Corresponding author of this paper.)

王 森英國牛津大學計算機科學學院助理研究員.主要研究方向為機器人定位和多傳感器融合.

E-mail:sen.wang@cs.ox.ac.uk

(WANG SenAssistant professor in the Department of Computer Science, University of Oxford,Oxford,UK.His research interest covers robot localization and multiple sensor fusion.)

胡豁生英國埃塞克斯大學計算機科學與電子系統(tǒng)學院教授,機器人實驗室主任.主要研究方向為自主機器人和網(wǎng)絡化機器人.E-mail:hhu@essex.ac.uk

(HU Huo-ShengProfessor at the School of Computer Science and Electronic Engineering,University of Essex, UK,leading robotics research.His research interest covers autonomous robots and networked robots.)

麥當勞-麥爾·克勞斯英國埃塞克斯大學計算機科學與電子系統(tǒng)學院教授.主要研究方向為嵌入式系統(tǒng)和機器人應用.E-mail:kdm@essex.ac.uk

(MCDONALD-MAIER KlausProfessor at the School of Computer Scienceand ElectronicEngineering, University of Essex,UK.His research interest covers embedded systems and application of robotics.)

費敏銳上海大學機電工程與自動化學院教授.主要研究方向為網(wǎng)絡化控制系統(tǒng),智能機器人系統(tǒng).

E-mail:mrfei@staf.shu.edu.cn

(FEIMin-RuiProfessor at the SchoolofMechatronicsEngineering and Automation,Shanghai University. His research interest covers networked control system and intelligent robot system.)

Novel Path Curvature Optimization Algorithm for Intelligent Wheelchair to Smoothly Pass a Narrow Space

CHEN Ling1WANG Sen2HU Huo-Sheng3MCDONALD-MAIER Klaus3FEI Min-Rui1

This paper presents a novel algorithm to address the smooth narrow pass traversing issue,which is based on optimizing the curvature of the wheelchair path.Being aware of the fact that the path smoothness is determined by the path curvature and its change rate,after calculating the position of the narrow pass relative to the base frame of the wheelchair from perception sensor data,the algorithm takes the curvature and its change rate of Bezier curve as the optimal objective,and the wheelchair heading and the condition that the Bezier curve polygon should be convex polygon as constraints,and plans a smooth and optimal path for the controlled wheelchair to follow.This process is iterated dynamically to enable the intelligent wheelchair to traverse the narrow pass smoothly.Simulation is frstly conducted to compare the performances of our method and the A*-based path planning navigation algorithm,which shows that the proposed algorithm is able to achieve more smooth path with smaller curvature when the wheelchair traverses narrow path.Furthermore,the algorithm can control the wheelchair to traverse narrow pass smoothly even without any global map and localization.Real experiment with detailed explanation of algorithm implementation is also given to verify the efectiveness of the proposed algorithm.

Intelligent wheelchair,curvature optimization,Bezier curve,pass traversing

陳靈,王森,胡豁生,麥當勞-麥爾·克勞斯,費敏銳.保證智能輪椅平滑通過狹窄通道的路徑曲率優(yōu)化算法.自動化學報,2016,42(12):1874?1885

Chen Ling,Wang Sen,Hu Huo-Sheng,Mcdonald-Maier Klaus,Fei Min-Rui.Novel path curvature optimization algorithm for intelligent wheelchair to smoothly pass a narrow space.Acta Automatica Sinica,2016,42(12):1874?1885

2016-03-01 錄用日期2016-07-11

Manuscript received March 1,2016;accepted July 11,2016

上海市科委揚帆人才計劃項目(16YF1403700),上海市科委重點項目(15411953502,14JC1402200),上海高校青年教師培養(yǎng)資助計劃(ZZ SD15088)資助

Supported by Shanghai Sailing Program(16YF1403700),Key Project of Science and Technology Commission of Shanghai Municipality(15411953502,14JC1402200),and Shanghai Colleges and Universities Young Teachers Training Funding Scheme(ZZS D15088)

本文責任編委程龍

Recommended by Associate Editor CHENG Long

1.上海大學機電工程與自動化學院上海200072中國 2.牛津大學計算機與電子工程學院牛津OX1 3QD英國 3.艾塞克斯大學計算機與電子工程學院科爾徹斯特CO4 3SQ英國

1.School of Mechatronics Engineering and Automation,Shanghai University,Shanghai 200072,China 2.Department of Computer Science,University of Oxford,Oxford OX1 3QD,UK 3.School of Computer Science and Electronic Engineering,University of Essex,Colchester CO4 3SQ,UK