动态多行人环境中基于最优交互避碰的机器人导航①

张栋翔(上海英集斯自动化技术有限公司 软件研发部, 上海 201108)

动态多行人环境中基于最优交互避碰的机器人导航①

张栋翔
(上海英集斯自动化技术有限公司 软件研发部, 上海 201108)

在动态的多行人环境中, 服务机器人仅依赖于自身传感器、以第一人称视角自主导航时. 机器人自主定位的不确定性以及对周围行人运动状态估计的不确定性均增加, 这给机器人导航决策带来了困难. 为解决这个问题, 提出一种基于最优交互避碰的机器人自主导航法. 本方法采用一种改进的粒子PHD滤波法即NP-PHDF法跟踪多个行人的状态. NP-PHDF法结合了卡尔曼粒子滤波及PHD滤波优点, 因此它可以跟踪数目变化的多个目标, 能够跟踪突然的加减速以及急转弯运动, 并且能够抵抗遮挡. 同时, 与基于粒子滤波的机器人自主定位法类似, NP-PHDF法使得行人运动状态的不确定性能够以粒子的分布来度量. 为降低状态估计的不确定性, 本文提出一种“圈粒子”的粒子圈存法从粒子的分布中提取机器人和行人的真实状态. 算法的有效性在实际场景实验中得到了验证.

移动机器人; 自主导航; 动态的多行人环境; 最优交互避碰

导航是服务机器人一项重要的基础性功能[1,2], 然而目前机器人导航还不能很好地适应一些高度动态的环境. 例如在城市街道、商场、医院、地铁、火车站等大型公共场所中, 存在大范围的走动的行人, 如何在这种高动态环境中和谐地与人共存, 在执行任务时安全地避开周围走动的行人, 顺利到达目标点是现阶段服务机器人导航的一个难题, 所以开展服务机器人在多行人环境中的导航研究具有重要的理论价值与应用意义.

传统的机器人导航算法是把行人当做动态的障碍物, 采用反应式控制法避开行人. 这些方法包括人工势场法[3]、方向区间柱图法(VFH)[4]和动态窗(DWA)法等. 反应式控制法虽然计算简单有效, 但是一旦把机器人放入新的环境中, 就需要人工手动调节参数. 例如人工势场法需要繁琐地调节吸引场和排斥场, 使两种场共同作用产生理想的避障结果; VFH法同样需要调整柱状图的平滑参数和障碍物对机器人的影响因子.考虑到行人是具有主动意识的智能体, 除了本身会自主地选择移动策略到达目标地点外, 还能对机器人的行为产生反应, 在遇到机器人时也会采取“积极主动”的策略避开机器人, 甚至在某些情况下行人与机器人还进行复杂的人机交互. 因此, 本文提出了一种基于最优交互避碰(optimal reciprocal collision avoidance, ORCA)法的导航策略, 机器人仅依赖于自身的传感器以第一人称视角在多行人环境中移动, 同时考虑行人的主动意识这一因素, 从而提高机器人的导航策略的有效性.

本研究的灵感来自D. Hennes等人[5]提出的基于最优交互避碰的多机器人导航研究, 称为不确定自定位下的避碰法(collision avoidance under localization uncertainty, CALU). 在参考文献[5]中, 机器人的数目是已知的, 并且所有机器人通过无线通信共享各自的状态信息. 然而对于在人群中移动的机器人来说, 机器人周围行人的数目是未知并且不断变化的. 本文研究服务机器人以第一人称视角在动态的多行人环境中导航问题. 机器人完全依靠自身的传感器感知环境,得到周围行人的数目、位置与速度等信息. 本研究提出的方法易于部署, 具有较高的实用价值.

1 系统概述

一般地, 服务机器人在动态的多行人环境中安全有效地导航, 需要具备三个因素: i)可靠的自主定位; ii)对周围行人运动状态的估计; iii)有效的路径规划和避障算法. 机器人以第一人称视角感知环境, 行人之间的遮挡经常发生, 故而机器人自定位和多行人跟踪偏离了实际的状态. 当机器人状态估计以及行人状态估计所包含的区域大于实际状态所处区域时, 碰撞就不可避免. 图1说明了当机器人自身状态和多行人状态估计的不确定性扩大时, 如何阻碍了机器人导航.

图1 机器人在动态的多行人环境中移动时, 各目标的状态估计相互“重叠”妨碍了机器人导航决策

王勇等人[6]提出了一种定性定量描述机器人自定位结果可靠性的方法, 指明机器人在什么时候可以信赖自身的传感器信息来自主定位, 从而在地铁、工厂等环境中取得了良好的导航效果. 本文提出了一种人群中导航算法框架(如图2所示), 本方法的关键在于限制机器人自定位和行人状态估计的不确定性. 为此本文提出了一种抽取具有高置信度的粒子的“剥洋葱”法来得到机器人和行人的真实状态.

图 2 多行人环境中机器人导航框架

2 多行人环境中导航算法

2.1 最优交互避碰法

最优交互避碰法是用于解决多个移动的智能体相互避碰问题的方法, 它是基于速度障碍(velocity obstacle)的协作式碰撞避免法. 在最优交互避碰法中,一个智能体A对周围的每个智能体生成一个速度障碍,所有的速度障碍组成了可能发生碰撞时A速度的取值空间. 如果A以此空间之外的速度运动则不会发生碰撞. 最终每个智能体在不妨碍对方运动的同时, 自身也能达到目标地点, 所有的智能体以互惠的方式避免了碰撞. 在此过程中, 每个智能体的最优速度可以通过低纬度的线性规划计算得到.

假设环境中有一个机器人和一个行人, 机器人以完整约束(holonomic)的方式移动. 在某时刻, 机器人期望以速度朝着目标地点移动. 我们的目的是从某时刻到之后的时刻τ之前这一段时间内, 在不发生碰撞的前提下, 机器人尽可能地以期望速度运动,即无限趋近于对机器人A来说, 最经济的运动方式自然是保持当前运动速度不变,可以设为当前速度. A对行人B生成的速度障碍为我们定义向量u的起点为, u的终点是向量到区域的边缘最近的点, 也即:

这意味着当机器人与行人负担同等的避碰责任时, 机器人至少需要移动u才能避免碰撞. 我们用ORC表示不发生碰撞时, 机器人可以采取的移动速度组成的空间.由生成, 即:

其中, 变量c表示双方各自所负的避碰责任. c=1表示机器人承担所有的避碰责任.c=1 2表示行人与机器人各自承担一半责任.

对于在多行人环境中移动的机器人, 机器人对每个行人均可得到一个线以及相应的半平面(见图3), 所有半平面的交集便组成了机器人在多行人环境中的不发生碰撞时的速度集合, 即:

其中D（0,vmax）是机器人自身条件所允许的速度范围.

R图3中的多边形区域表示机器人无碰撞时速度的取值范围.

上述方法可以推广到非完整约束机器人(non-holonomic robots), Alonso-mora等人[7]提出了非完整约束最优交互避碰法(NH-ORCA)处理所有以非完整约束方式移动的机器人. NH-ORCA的关键在于假设非完整约束机器人能够以误差ξ按照完整约束机器人的方式移动, 也就是说通过把完整约束运动映射到非完整约束运动, 可以使差速传动机器人的运动达到全向轮运动同样的效果. 在NH-ORCA中, 通过把半径为ri的非完整约束机器人当做一个半径为ri+的完整约束机器人, 非完整约束机器人的和可同样地由完整约束机器人得到. 同样地,行人的半径设为rp+, 我们假设==. 增加ξ的值会得到非完整约束机器人的避碰速度, ξ的值依赖于机器人自身的运动能力. 最后, 同样根据(见图3)得到非完整约束机器人的避碰运动速度. 在动态多行人环境中移动的机器人生成多条ORCA线如图3(b), 图中的虚线区域是机器人的无碰撞速度取值空间机器人的最优速度是中最接近的点.

图3 动态多行人环境中移动的机器人生成的多条ORCA线

2.2 多行人跟踪

智能机器人仅依赖于自身携带的传感器跟踪周围的行人仍然是一个值得研究的开放性课题. 这个问题的难点之一在于由第一人称视角的局限导致的行人之间复杂的动态遮挡; 另一个难点在于数目随时间变化的多行人跟踪. 随着机器人和行人的移动, 不断地有行人出现或者消失在机器人的视野中. 我们扩展了噪声估计粒子PHD滤波法(noise-estimate particle PHD filter, NP-PHDF)[8], 采用粒子PHD滤波结合行人的常速度模型(constant velocity model)来跟踪机器人周围行人的状态. 因此, 我们有状态转移矩阵:

这里Δt是采样间隔, I是单位矩阵. 观测矩阵H=（I0）. 值得注意的是, 虽然基于行人的常速运动模型, 我们的状态转移矩阵和观测矩阵都很简单,但是由于NP-PHDF算法的优点, 结合我们的“圈粒子”方法(见算法1)提取目标的真实状态, 算法仍然取得了很好的效果.

在卡尔曼粒子滤波中(Kalman particle filter), 目标的状态由卡尔曼滤波得到, 而系统噪声和观测噪声由粒子滤波实时地优化. 这样卡尔曼滤波的参数可以通过观测在线调整, 从而使得卡尔曼粒子滤波可以跟踪突然的加减速以及急转弯运动. 另一方面, PHD滤波是基于随机有限集(random finite sets, RFS)的多目标跟踪方法. 随机有限集(RFS)在数学上提供了解决数目和状态均变化的多目标跟踪问题的理论基础. 即使在存在动态的遮挡和状态估计不确定性的情况下, 仍然可以从随机有限集中得到一个最优贝叶斯(Bayesian)解. NP-PHDF结合了随机有限集与卡尔曼粒子滤波两者的优点. 通过NP-PHDF, 每个被跟踪目标的状态估计的不确定性可以表示为一组粒子, 每个粒子的权值可以表示为:

2.3 机器人自主定位

本研究中, 机器人通过自适应蒙特卡罗法(adaptive Monte-Carlo localization, AMCL)[9]自主定位.在自适应蒙特卡罗法中, 每个粒子表示机器人状态的一种假设, 粒子的集合表示机器人状态估计的不确定性. 自适应蒙特卡罗法根据粒子的重要性进行重采样.粒子的数目随着当前环境发生变化. 自适应蒙特卡罗法已经实现在ROS(Robot Operating System)中.

2.4 状态估计的不确定性

本研究的关键在于限制多行人状态估计和机器人自主定位的不确定性. 从前文可知, 机器人和行人的状态均可以用粒子的集合近似表示, 也就是说状态估计的后验概率分布p）可以用粒子的加权和表示. 这意味着在k时刻, 一组粒子的平均位置μ=（μx,μy）由下式得到:）可以表示为表示为算法所估计的机器人和行人的位置. 根据似然度计算公式(4), 粒子的权值正比于粒子自身到μ的距离. 这意味着: 首先, 一些粒子的位置偏离于真实值; 其次, 可以舍弃那些权值小的远离真实值的粒子. 这样, 最后留下来的粒子的分布接近于行人和机器人所占有的区域. 本文提出了一种“圈粒子”法删除低权值的粒子, 从而得到目标的真实足迹. 算法1阐述了“圈粒子”法的细节以及如何根据保留的粒子生成ORCA线.

算法1 “圈粒子”法

“圈粒子”法的计算是直观而有效的. 我们假设目标处于某个状态的概率置信度为1-ε, 从粒子集中找到距离均值μ最近的粒子（si,ωi）, 把粒子（si,ωi）加入到粒子集C中, 同时增大bound的值. 这个过程反复地迭代运行直至bound＞1-ε. C中的粒子分布表示状态估计的不确定性. 图4中展示了舍弃低权值粒子的“圈粒子”过程.图中虚线圆表示圈粒子过程中粒子集C的外接圆. 实线圆表示迭代过程结束后粒子集C的外接圆. 实线圆外部的所有粒子被舍弃. 从C中我们可以近似得到目标实际所处区域的外接圆, 基于闵可夫斯基和(Minkowski sum), ORCA线由这些外接圆得到. 机器人下一步的移动速度由这些ORCA线围成的多边形区域以及期望速度得到(见2.1小节).

图4 舍弃低权值粒子的“圈粒子”过程

3 实验结果

我们在实体机器人上进行了实验, 实验在4.94.5mm×房间内进行. 机器人和行人的初始位置如图5所示. 实验使用一台安装了Hokuyo UTM30LX-EW激光器的Kobuki差分驱动机器人, 实验平台如图5所示. 其中Kinect用于从机器人的视角监控周围的环境, Hokuyo激光器是用于感知周围的行人, Hokuyo激光器的视角设为270°. 在实验中, 行人的检测和感知是用基于AdaBoost分类器的双腿识别算法[10]识别行人的, 多个行人状态的跟踪和预测用NH-PHDF算法, 机器人自身的运动由NH-ORCA算法控制. 全部算法模块是在Intel®CoreTMi5 CPU 2.50GHz, 8.0GB内存的笔记本电脑上运行.

图5 实验环境和机器人平台

在实验中, Kobuki机器人被取消了避开障碍物的能力. 这意味着在实验中机器人必须识别并准确预测行人的运动才能避开行人, 否则机器人除了可能撞上行人外, 还有可能撞上墙壁. 为了检测算法的鲁棒性,算法中的参赛c设置为0.5, 这意味着行人也要负担一半的避碰责任. 需要指出的是, 实验中行人检测仅依赖于2D激光传感器扫描, 行人感知存在一定的漏警率, 后续可以采用多源传感器跟踪行人的方法来提高算法的整体效果.

图6 实际场景实验一例: 顶视图

如图5中的箭头所示, 三个行人和Kobuki机器人互相走向对方. 实验中行人经常故意挡在机器人面前.实验进行了30次, 总共发生了4次碰撞, 其中3次是因为没有准确地检测到周围的行人. 1次是行人故意走到机器人移动的正前方, 机器人为了避开行人撞上了它无法识别的墙壁.

其中一次实验如图6所示, Kobuki机器人从左下角开始向右上角移动. 它首先避开了对向而来的行人(图(d)), 注意在此过程中对向而来的行人故意没有改变方向, 机器人承担了所有的避碰责任. 在避开对向来的行人后, 机器人发现正前方突然出现一个行人(图(e)). 由于Kobuki机器人只依靠激光器的2D扫描感知行人, 这位行人因对向而来的行人遮挡(图(c))没有被检测到. 虽然如此, Kobuki机器人及时发现并避开了他. 最终顺利到达目标点(图(h)).

4 结语

本文提出了一种动态多行人环境中的机器人导航方法, 机器人仅依赖于自身的传感器. 与传统的方法不同, 基于ORCA我们引入了交互避碰模型. 由于第一人称视角感知的局限性, 基于粒子分布表达的机器人和行人的状态估计的物理范围大于实际状态的物理范围. 为了提取目标的高可信度的状态, 我们提出了一种层递式“圈粒子”法(encircling particles)来去除冗余的粒子. 算法的有效性在仿真实验和实体实验中均得到了验证.

1 张铮,李振波,胡寿伟,等.混合传感器网络中的移动机器人导航方法.机器人,2014,36(3):316–321.

2 陈洋,赵新刚,韩建达.移动机器人三维路径规划方法综述.机器人,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 王勇,陈卫东,王景川,肖鹏.面向动态高遮挡环境的移动机器人自适应位姿跟踪算法,机器人,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

上海市闵行区产学研项目(2016MH018)

2016-07-05;收到修改稿时间:2016-08-08

10.15888/j.cnki.csa.005665