動態(tài)多行人環(huán)境中基于最優(yōu)交互避碰的機器人導(dǎo)航①

張棟翔(上海英集斯自動化技術(shù)有限公司 軟件研發(fā)部, 上海 201108)

動態(tài)多行人環(huán)境中基于最優(yōu)交互避碰的機器人導(dǎo)航①

張棟翔
(上海英集斯自動化技術(shù)有限公司 軟件研發(fā)部, 上海 201108)

在動態(tài)的多行人環(huán)境中, 服務(wù)機器人僅依賴于自身傳感器、以第一人稱視角自主導(dǎo)航時. 機器人自主定位的不確定性以及對周圍行人運動狀態(tài)估計的不確定性均增加, 這給機器人導(dǎo)航?jīng)Q策帶來了困難. 為解決這個問題, 提出一種基于最優(yōu)交互避碰的機器人自主導(dǎo)航法. 本方法采用一種改進的粒子PHD濾波法即NP-PHDF法跟蹤多個行人的狀態(tài). NP-PHDF法結(jié)合了卡爾曼粒子濾波及PHD濾波優(yōu)點, 因此它可以跟蹤數(shù)目變化的多個目標(biāo), 能夠跟蹤突然的加減速以及急轉(zhuǎn)彎運動, 并且能夠抵抗遮擋. 同時, 與基于粒子濾波的機器人自主定位法類似, NP-PHDF法使得行人運動狀態(tài)的不確定性能夠以粒子的分布來度量. 為降低狀態(tài)估計的不確定性, 本文提出一種“圈粒子”的粒子圈存法從粒子的分布中提取機器人和行人的真實狀態(tài). 算法的有效性在實際場景實驗中得到了驗證.

移動機器人; 自主導(dǎo)航; 動態(tài)的多行人環(huán)境; 最優(yōu)交互避碰

導(dǎo)航是服務(wù)機器人一項重要的基礎(chǔ)性功能[1,2], 然而目前機器人導(dǎo)航還不能很好地適應(yīng)一些高度動態(tài)的環(huán)境. 例如在城市街道、商場、醫(yī)院、地鐵、火車站等大型公共場所中, 存在大范圍的走動的行人, 如何在這種高動態(tài)環(huán)境中和諧地與人共存, 在執(zhí)行任務(wù)時安全地避開周圍走動的行人, 順利到達目標(biāo)點是現(xiàn)階段服務(wù)機器人導(dǎo)航的一個難題, 所以開展服務(wù)機器人在多行人環(huán)境中的導(dǎo)航研究具有重要的理論價值與應(yīng)用意義.

傳統(tǒng)的機器人導(dǎo)航算法是把行人當(dāng)做動態(tài)的障礙物, 采用反應(yīng)式控制法避開行人. 這些方法包括人工勢場法[3]、方向區(qū)間柱圖法(VFH)[4]和動態(tài)窗(DWA)法等. 反應(yīng)式控制法雖然計算簡單有效, 但是一旦把機器人放入新的環(huán)境中, 就需要人工手動調(diào)節(jié)參數(shù). 例如人工勢場法需要繁瑣地調(diào)節(jié)吸引場和排斥場, 使兩種場共同作用產(chǎn)生理想的避障結(jié)果; VFH法同樣需要調(diào)整柱狀圖的平滑參數(shù)和障礙物對機器人的影響因子.考慮到行人是具有主動意識的智能體, 除了本身會自主地選擇移動策略到達目標(biāo)地點外, 還能對機器人的行為產(chǎn)生反應(yīng), 在遇到機器人時也會采取“積極主動”的策略避開機器人, 甚至在某些情況下行人與機器人還進行復(fù)雜的人機交互. 因此, 本文提出了一種基于最優(yōu)交互避碰(optimal reciprocal collision avoidance, ORCA)法的導(dǎo)航策略, 機器人僅依賴于自身的傳感器以第一人稱視角在多行人環(huán)境中移動, 同時考慮行人的主動意識這一因素, 從而提高機器人的導(dǎo)航策略的有效性.

本研究的靈感來自D. Hennes等人[5]提出的基于最優(yōu)交互避碰的多機器人導(dǎo)航研究, 稱為不確定自定位下的避碰法(collision avoidance under localization uncertainty, CALU). 在參考文獻[5]中, 機器人的數(shù)目是已知的, 并且所有機器人通過無線通信共享各自的狀態(tài)信息. 然而對于在人群中移動的機器人來說, 機器人周圍行人的數(shù)目是未知并且不斷變化的. 本文研究服務(wù)機器人以第一人稱視角在動態(tài)的多行人環(huán)境中導(dǎo)航問題. 機器人完全依靠自身的傳感器感知環(huán)境,得到周圍行人的數(shù)目、位置與速度等信息. 本研究提出的方法易于部署, 具有較高的實用價值.

1 系統(tǒng)概述

一般地, 服務(wù)機器人在動態(tài)的多行人環(huán)境中安全有效地導(dǎo)航, 需要具備三個因素: i)可靠的自主定位; ii)對周圍行人運動狀態(tài)的估計; iii)有效的路徑規(guī)劃和避障算法. 機器人以第一人稱視角感知環(huán)境, 行人之間的遮擋經(jīng)常發(fā)生, 故而機器人自定位和多行人跟蹤偏離了實際的狀態(tài). 當(dāng)機器人狀態(tài)估計以及行人狀態(tài)估計所包含的區(qū)域大于實際狀態(tài)所處區(qū)域時, 碰撞就不可避免. 圖1說明了當(dāng)機器人自身狀態(tài)和多行人狀態(tài)估計的不確定性擴大時, 如何阻礙了機器人導(dǎo)航.

圖1 機器人在動態(tài)的多行人環(huán)境中移動時, 各目標(biāo)的狀態(tài)估計相互“重疊”妨礙了機器人導(dǎo)航?jīng)Q策

王勇等人[6]提出了一種定性定量描述機器人自定位結(jié)果可靠性的方法, 指明機器人在什么時候可以信賴自身的傳感器信息來自主定位, 從而在地鐵、工廠等環(huán)境中取得了良好的導(dǎo)航效果. 本文提出了一種人群中導(dǎo)航算法框架(如圖2所示), 本方法的關(guān)鍵在于限制機器人自定位和行人狀態(tài)估計的不確定性. 為此本文提出了一種抽取具有高置信度的粒子的“剝洋蔥”法來得到機器人和行人的真實狀態(tài).

圖 2 多行人環(huán)境中機器人導(dǎo)航框架

2 多行人環(huán)境中導(dǎo)航算法

2.1 最優(yōu)交互避碰法

最優(yōu)交互避碰法是用于解決多個移動的智能體相互避碰問題的方法, 它是基于速度障礙(velocity obstacle)的協(xié)作式碰撞避免法. 在最優(yōu)交互避碰法中,一個智能體A對周圍的每個智能體生成一個速度障礙,所有的速度障礙組成了可能發(fā)生碰撞時A速度的取值空間. 如果A以此空間之外的速度運動則不會發(fā)生碰撞. 最終每個智能體在不妨礙對方運動的同時, 自身也能達到目標(biāo)地點, 所有的智能體以互惠的方式避免了碰撞. 在此過程中, 每個智能體的最優(yōu)速度可以通過低緯度的線性規(guī)劃計算得到.

假設(shè)環(huán)境中有一個機器人和一個行人, 機器人以完整約束(holonomic)的方式移動. 在某時刻, 機器人期望以速度朝著目標(biāo)地點移動. 我們的目的是從某時刻到之后的時刻τ之前這一段時間內(nèi), 在不發(fā)生碰撞的前提下, 機器人盡可能地以期望速度運動,即無限趨近于對機器人A來說, 最經(jīng)濟的運動方式自然是保持當(dāng)前運動速度不變,可以設(shè)為當(dāng)前速度. A對行人B生成的速度障礙為我們定義向量u的起點為, u的終點是向量到區(qū)域的邊緣最近的點, 也即:

這意味著當(dāng)機器人與行人負擔(dān)同等的避碰責(zé)任時, 機器人至少需要移動u才能避免碰撞. 我們用ORC表示不發(fā)生碰撞時, 機器人可以采取的移動速度組成的空間.由生成, 即:

其中, 變量c表示雙方各自所負的避碰責(zé)任. c=1表示機器人承擔(dān)所有的避碰責(zé)任.c=1 2表示行人與機器人各自承擔(dān)一半責(zé)任.

對于在多行人環(huán)境中移動的機器人, 機器人對每個行人均可得到一個線以及相應(yīng)的半平面(見圖3), 所有半平面的交集便組成了機器人在多行人環(huán)境中的不發(fā)生碰撞時的速度集合, 即:

其中D（0,vmax）是機器人自身條件所允許的速度范圍.

R圖3中的多邊形區(qū)域表示機器人無碰撞時速度的取值范圍.

上述方法可以推廣到非完整約束機器人(non-holonomic robots), Alonso-mora等人[7]提出了非完整約束最優(yōu)交互避碰法(NH-ORCA)處理所有以非完整約束方式移動的機器人. NH-ORCA的關(guān)鍵在于假設(shè)非完整約束機器人能夠以誤差ξ按照完整約束機器人的方式移動, 也就是說通過把完整約束運動映射到非完整約束運動, 可以使差速傳動機器人的運動達到全向輪運動同樣的效果. 在NH-ORCA中, 通過把半徑為ri的非完整約束機器人當(dāng)做一個半徑為ri+的完整約束機器人, 非完整約束機器人的和可同樣地由完整約束機器人得到. 同樣地,行人的半徑設(shè)為rp+, 我們假設(shè)==. 增加ξ的值會得到非完整約束機器人的避碰速度, ξ的值依賴于機器人自身的運動能力. 最后, 同樣根據(jù)(見圖3)得到非完整約束機器人的避碰運動速度. 在動態(tài)多行人環(huán)境中移動的機器人生成多條ORCA線如圖3(b), 圖中的虛線區(qū)域是機器人的無碰撞速度取值空間機器人的最優(yōu)速度是中最接近的點.

圖3 動態(tài)多行人環(huán)境中移動的機器人生成的多條ORCA線

2.2 多行人跟蹤

智能機器人僅依賴于自身攜帶的傳感器跟蹤周圍的行人仍然是一個值得研究的開放性課題. 這個問題的難點之一在于由第一人稱視角的局限導(dǎo)致的行人之間復(fù)雜的動態(tài)遮擋; 另一個難點在于數(shù)目隨時間變化的多行人跟蹤. 隨著機器人和行人的移動, 不斷地有行人出現(xiàn)或者消失在機器人的視野中. 我們擴展了噪聲估計粒子PHD濾波法(noise-estimate particle PHD filter, NP-PHDF)[8], 采用粒子PHD濾波結(jié)合行人的常速度模型(constant velocity model)來跟蹤機器人周圍行人的狀態(tài). 因此, 我們有狀態(tài)轉(zhuǎn)移矩陣:

這里Δt是采樣間隔, I是單位矩陣. 觀測矩陣H=（I0）. 值得注意的是, 雖然基于行人的常速運動模型, 我們的狀態(tài)轉(zhuǎn)移矩陣和觀測矩陣都很簡單,但是由于NP-PHDF算法的優(yōu)點, 結(jié)合我們的“圈粒子”方法(見算法1)提取目標(biāo)的真實狀態(tài), 算法仍然取得了很好的效果.

在卡爾曼粒子濾波中(Kalman particle filter), 目標(biāo)的狀態(tài)由卡爾曼濾波得到, 而系統(tǒng)噪聲和觀測噪聲由粒子濾波實時地優(yōu)化. 這樣卡爾曼濾波的參數(shù)可以通過觀測在線調(diào)整, 從而使得卡爾曼粒子濾波可以跟蹤突然的加減速以及急轉(zhuǎn)彎運動. 另一方面, PHD濾波是基于隨機有限集(random finite sets, RFS)的多目標(biāo)跟蹤方法. 隨機有限集(RFS)在數(shù)學(xué)上提供了解決數(shù)目和狀態(tài)均變化的多目標(biāo)跟蹤問題的理論基礎(chǔ). 即使在存在動態(tài)的遮擋和狀態(tài)估計不確定性的情況下, 仍然可以從隨機有限集中得到一個最優(yōu)貝葉斯(Bayesian)解. NP-PHDF結(jié)合了隨機有限集與卡爾曼粒子濾波兩者的優(yōu)點. 通過NP-PHDF, 每個被跟蹤目標(biāo)的狀態(tài)估計的不確定性可以表示為一組粒子, 每個粒子的權(quán)值可以表示為:

2.3 機器人自主定位

本研究中, 機器人通過自適應(yīng)蒙特卡羅法(adaptive Monte-Carlo localization, AMCL)[9]自主定位.在自適應(yīng)蒙特卡羅法中, 每個粒子表示機器人狀態(tài)的一種假設(shè), 粒子的集合表示機器人狀態(tài)估計的不確定性. 自適應(yīng)蒙特卡羅法根據(jù)粒子的重要性進行重采樣.粒子的數(shù)目隨著當(dāng)前環(huán)境發(fā)生變化. 自適應(yīng)蒙特卡羅法已經(jīng)實現(xiàn)在ROS(Robot Operating System)中.

2.4 狀態(tài)估計的不確定性

本研究的關(guān)鍵在于限制多行人狀態(tài)估計和機器人自主定位的不確定性. 從前文可知, 機器人和行人的狀態(tài)均可以用粒子的集合近似表示, 也就是說狀態(tài)估計的后驗概率分布p）可以用粒子的加權(quán)和表示. 這意味著在k時刻, 一組粒子的平均位置μ=（μx,μy）由下式得到:）可以表示為表示為算法所估計的機器人和行人的位置. 根據(jù)似然度計算公式(4), 粒子的權(quán)值正比于粒子自身到μ的距離. 這意味著: 首先, 一些粒子的位置偏離于真實值; 其次, 可以舍棄那些權(quán)值小的遠離真實值的粒子. 這樣, 最后留下來的粒子的分布接近于行人和機器人所占有的區(qū)域. 本文提出了一種“圈粒子”法刪除低權(quán)值的粒子, 從而得到目標(biāo)的真實足跡. 算法1闡述了“圈粒子”法的細節(jié)以及如何根據(jù)保留的粒子生成ORCA線.

算法1 “圈粒子”法

“圈粒子”法的計算是直觀而有效的. 我們假設(shè)目標(biāo)處于某個狀態(tài)的概率置信度為1-ε, 從粒子集中找到距離均值μ最近的粒子（si,ωi）, 把粒子（si,ωi）加入到粒子集C中, 同時增大bound的值. 這個過程反復(fù)地迭代運行直至bound＞1-ε. C中的粒子分布表示狀態(tài)估計的不確定性. 圖4中展示了舍棄低權(quán)值粒子的“圈粒子”過程.圖中虛線圓表示圈粒子過程中粒子集C的外接圓. 實線圓表示迭代過程結(jié)束后粒子集C的外接圓. 實線圓外部的所有粒子被舍棄. 從C中我們可以近似得到目標(biāo)實際所處區(qū)域的外接圓, 基于閔可夫斯基和(Minkowski sum), ORCA線由這些外接圓得到. 機器人下一步的移動速度由這些ORCA線圍成的多邊形區(qū)域以及期望速度得到(見2.1小節(jié)).

圖4 舍棄低權(quán)值粒子的“圈粒子”過程

3 實驗結(jié)果

我們在實體機器人上進行了實驗, 實驗在4.94.5mm×房間內(nèi)進行. 機器人和行人的初始位置如圖5所示. 實驗使用一臺安裝了Hokuyo UTM30LX-EW激光器的Kobuki差分驅(qū)動機器人, 實驗平臺如圖5所示. 其中Kinect用于從機器人的視角監(jiān)控周圍的環(huán)境, Hokuyo激光器是用于感知周圍的行人, Hokuyo激光器的視角設(shè)為270°. 在實驗中, 行人的檢測和感知是用基于AdaBoost分類器的雙腿識別算法[10]識別行人的, 多個行人狀態(tài)的跟蹤和預(yù)測用NH-PHDF算法, 機器人自身的運動由NH-ORCA算法控制. 全部算法模塊是在Intel?CoreTMi5 CPU 2.50GHz, 8.0GB內(nèi)存的筆記本電腦上運行.

圖5 實驗環(huán)境和機器人平臺

在實驗中, Kobuki機器人被取消了避開障礙物的能力. 這意味著在實驗中機器人必須識別并準(zhǔn)確預(yù)測行人的運動才能避開行人, 否則機器人除了可能撞上行人外, 還有可能撞上墻壁. 為了檢測算法的魯棒性,算法中的參賽c設(shè)置為0.5, 這意味著行人也要負擔(dān)一半的避碰責(zé)任. 需要指出的是, 實驗中行人檢測僅依賴于2D激光傳感器掃描, 行人感知存在一定的漏警率, 后續(xù)可以采用多源傳感器跟蹤行人的方法來提高算法的整體效果.

圖6 實際場景實驗一例: 頂視圖

如圖5中的箭頭所示, 三個行人和Kobuki機器人互相走向?qū)Ψ? 實驗中行人經(jīng)常故意擋在機器人面前.實驗進行了30次, 總共發(fā)生了4次碰撞, 其中3次是因為沒有準(zhǔn)確地檢測到周圍的行人. 1次是行人故意走到機器人移動的正前方, 機器人為了避開行人撞上了它無法識別的墻壁.

其中一次實驗如圖6所示, Kobuki機器人從左下角開始向右上角移動. 它首先避開了對向而來的行人(圖(d)), 注意在此過程中對向而來的行人故意沒有改變方向, 機器人承擔(dān)了所有的避碰責(zé)任. 在避開對向來的行人后, 機器人發(fā)現(xiàn)正前方突然出現(xiàn)一個行人(圖(e)). 由于Kobuki機器人只依靠激光器的2D掃描感知行人, 這位行人因?qū)ο蚨鴣淼男腥苏趽?圖(c))沒有被檢測到. 雖然如此, Kobuki機器人及時發(fā)現(xiàn)并避開了他. 最終順利到達目標(biāo)點(圖(h)).

4 結(jié)語

本文提出了一種動態(tài)多行人環(huán)境中的機器人導(dǎo)航方法, 機器人僅依賴于自身的傳感器. 與傳統(tǒng)的方法不同, 基于ORCA我們引入了交互避碰模型. 由于第一人稱視角感知的局限性, 基于粒子分布表達的機器人和行人的狀態(tài)估計的物理范圍大于實際狀態(tài)的物理范圍. 為了提取目標(biāo)的高可信度的狀態(tài), 我們提出了一種層遞式“圈粒子”法(encircling particles)來去除冗余的粒子. 算法的有效性在仿真實驗和實體實驗中均得到了驗證.

1 張錚,李振波,胡壽偉,等.混合傳感器網(wǎng)絡(luò)中的移動機器人導(dǎo)航方法.機器人,2014,36(3):316–321.

2 陳洋,趙新剛,韓建達.移動機器人三維路徑規(guī)劃方法綜述.機器人,2010,32(4):568–576.

3 Pradhan N, Burg T, Birchfield S. Robot crowd navigation usingpredictive position fields in the potential function framework. Proc. of the American Control Conference. 2011, 5991384. 4628–4633.

4 Ulrich I, Borenstein J. VFH*: Local obstacle avoidance with look-ahead verification. Proc. of the International Conference of Roboticsand Automation (ICRA). 2000, 3. 2505–2511.

5 Hennes D, Claes D, Meeussen W, Tuyls K. Multi-robot collision avoidance with localization uncertainty. Proc. of the 11th International Conference on Autonomous Agentsand Multiagent Systems. 2012, 2. 672–679.

6 王勇,陳衛(wèi)東,王景川,肖鵬.面向動態(tài)高遮擋環(huán)境的移動機器人自適應(yīng)位姿跟蹤算法,機器人,2015,(1):241–252.

7 Alonso-Mora J, Breitenmoser A, Rufli M, Beardsley PA, Siegwart R. Optimal reciprocal collision avoidance for multiple non-holonomic robots. Distributed Autonomous Robotic Systems-The 10th International Symposium. Lausanne, Switzerland. 2010. 203–216.

8 Ishibashi M, Iwashita Y, Kurazume R. Noise-estimate particle phd filter. Proc. IEEE World Automation Congress(WAC). Waikoloa, HI. 2014. 784–789.

9 Fox D. Adapting the sample size in particle filters through kld-sampling. The International Journal of Robotics Research, 2003, 22(12): 985–1003.

10 Arras KO, Mozos OM, Burgard W. Using boosted features for the detection of people in 2nd range data. IEEE International Conference on Robotics and Automation. 2007. 3402–3407.

Robot Navigation in Dynamic Human Environments Based on Optimal Reciprocal Collision Avoidance

ZHANG Dong-Xiang
(Software Department, Shanghai Ingenious Automation Technology Co. Ltd., Shanghai 201108, China)

In dynamic multi-pedestrian environments, while a service robot navigates only relying on its own sensors with the first-person perspective, both the uncertainties of robot localization and the estimation of people’s states are increased, which hinder the navigation decision of a service robot. To solve this problem, a local collision avoidance method based on the optimal reciprocal collision avoidance (ORCA) is proposed. In this method, the states of multiple pedestrians are estimated by a variant of particle-PHD filter, i.e. NP-PHDF for multi-target tracking. NP-PHDF is a combination of Kalman particle filter and PHD filter and has their advantages. So it can not only track time-varying number of targets with sudden motion changes such as abrupt acceleration/deceleration or steep turn, but also resist block among pedestrians. Meanwhile, similar as robot localization which uses particle filter, the uncertainties of estimation for pedestrians can be represented by the distribution of particles. To reduce the uncertainties, an encircling-particles method is proposed to refine the true states of robot and pedestrians from the probabilistic particle distribution. The effectiveness of the proposed technique is demonstrated through experiments in real environments.

mobile robot; autonomous navigation; dynamic multi-pedestrian environments; optimal reciprocal collision avoidance

上海市閔行區(qū)產(chǎn)學(xué)研項目(2016MH018)

2016-07-05;收到修改稿時間:2016-08-08

10.15888/j.cnki.csa.005665