CN114035569A - 一种航站楼载人机器人路径拓展通行方法 - Google Patents

Abstract

本发明属于民用航空技术领域，尤其是涉及一种航站楼载人机器人路径拓展通行方法。载人机器人运行办法步骤为分析群体环境中的目标运动模型；激光雷达获取目标位置信息；根据位置信息推断出目标运动信息；建立目标危险评估函数；考虑目标速度，修改评估函数；将评估函数加入到TEB惩罚函数中；设计随众、探测和超越三种通行模式。本发明具有的优点为可以实时实现对行人的运动趋势的估计，分析时变的行人运动状况，快速做出导航决策和规划路径，以此实现路径的拓展，这对实现载人机器人在人群环境中快速、安全的通行具有实用价值和现实意义。

Description

一种航站楼载人机器人路径拓展通行方法

技术领域

本发明属于民用航空技术领域，尤其是涉及一种基于行人感知的航站楼载人机器人路径拓展通行方法。

背景技术

航站楼中存在大量旅客，且具有流动性人群，同一区域不同时间下人群分布位置和密度存在较大差异，机器人在通行过程中需考虑行人的干扰。行人的随机运动、周围行人的影响以及复杂的运动模式使得行人路径难以准确预测。载人机器人识别行人、规划路径、控制运动需要一定的时间，反应慢，容易与行人发生碰撞。因此，航站楼载人机器人有必要实时实现对行人的运动趋势的预估，分析时变的行人运动状况，快速做出导航决策和规划路径，以此实现路径的拓展，这对实现载人机器人在人群环境中快速、安全的通行具有实用价值和现实意义。

人群中自主通行路径拓展是基于对行人感知的基础上进行路径拓展，是路径规划问题的延伸。路径规划作为移动机器人众多研究领域中的一个重要分支，尤其是在人机共处的动态场景下，合理有效的路径规划是机器人正常运行的基本保障。

发明内容

为解决上述技术问题，在人机共存的公共环境下安全顺畅的将旅客送达目的地，本发明提供一种航站楼载人机器人路径拓展通行方法。

本发明采用的技术方案是：一种航站楼载人机器人路径拓展通行方法，其特征在于，所述方法包括步骤：

分析环境目标的运动模型；

获取所述环境目标的位置信息；

根据所述环境目标的位置信息推断所述环境目标的运动信息；

建立所述环境目标的运动信息函数；

基于所述环境目标的运动信息函数设计机器人的通行模式。

优选地，所述建立所述环境目标的运动信息函数包括步骤：

建立所述环境目标的危险评估函数；

对所述环境目标的危险评估函数优化得到代价地图函数；

将所述代价地图函数加入到TEB惩罚函数中。

优选地，所述设计机器人的通行模式包括步骤：

建立随众模式：所述随众模式是根据对所述环境目标的运动特征的分析，采用与所述环境目标保持运动方向一致的行进模式；和/或，

建立探测模式：所述探测模式是机器人在所述随众模式过程中，对范围内的所述环境目标位置及危险度进行判别；和/或，

建立超越模式：所述超越模式是所述机器人在运行过程中根据运行轨迹的判断加速超过所述环境目标的过程。

优选地，所述建立所述环境目标的危险评估函数包括步骤：

建立所述环境目标相对于所述机器人的相对速度函数，公式如下：

其中x_t和y_t分别为t时刻的x方向和y方向上的所述环境目标的坐标值，x_t+1和y_t+1分别为t+1时刻的x方向和y方向上的所述环境目标的坐标值，T为扫描周期，

为所述环境目标相对于所述机器人的相对速度；

根据所述相对速度函数建立所述环境目标的危险评估函数，公式如下：

式中，Φ为危险评估值，λ为危险参数调节因子，

为所述环境目标相对于所述机器人的位置向量，

为所述环境目标相对于所述机器人的相对速度。

优选地，所述对所述环境目标的危险评估函数优化得到代价地图函数包括公式如下：

式中，Φ为危险评估值，λ为危险参数调节因子，

为所述环境目标相对于所述机器人的位置向量，

为所述环境目标相对于所述机器人的相对速度，当所述Φ小于0时，所述机器人可与所述环境目标保持安全距离。

优选地，所述将所述代价地图函数加入到TEB惩罚函数中包括步骤：

建立个人空间模型，公式如下：

其中θ的公式为：

其中d的公式为：

式中，f_i ^p(x,y)为个人空间模型函数，Φ^p为所述环境目标产生的危险代价函数值，d为所述环境目标距离所述机器人的直线距离，θ为所述机器人与所述环境目标的位置连线与x轴的夹角，

为根据所述环境目标在x轴和y轴方向上的运动速度来控制f_i ^p(x,y)沿x轴和y轴的衰减速度；

结合所述个人空间模型，调整TEB路径点与障碍物之间的距离，建立所述环境目标与所述机器人的最小危险距离公式如下：

式中，f_i ^p(x,y)为个人空间模型函数，f_ob为所述环境目标与所述机器人的最小危险距离，Φ^p为所述环境目标产生的危险代价函数值，d为所述环境目标距离所述机器人的直线距离，θ为所述机器人与所述环境目标的位置连线与x轴的夹角，

为根据所述环境目标在x轴和y轴方向上的运动速度来控制f_i ^p(x,y)沿x轴和y轴的衰减速度；λ为所述环境目标代价地图的边界距离与所述环境目标距离的动态调整参数，r_omin为系统常量，S是形变因子；n是多项式系数，默认值为2；Γ为时间间隔序列。

优选地，所述随众模式包括对所述机器人在运行过程中产生的斥力与引力进行分类，分为：

使所述机器人不偏离全局路径的目标力，和/或，使所述机器人跟随所述环境目标运动不会脱落的对齐力，和/或，使所述环境目标与所述机器人保持距离的分离力；

所述目标力公式如下：

其中，

为所述目标力，，K为比例系数，

为所述环境目标的当前位置坐标，

为所述环境目标的当前位置坐标的下一备选全局路径检查点的位置坐标；

所述对齐力公式如下：

式中，

为所述机器人受到的对齐力的矢量转向力，K为比例系数，

为所述机器人运动方向的角度矢量，

为设定范围内平均运动方向的角度矢量，N为所述机器人探测的所述环境目标的数量。

所述分离力公式如下：

式中，

为分离力矢量，K为比例系数，

为当前所述环境目标的位置矢量，

为相邻所述环境目标的位置矢量。

优选地，所述超越模式包括：

随众超越，所述随众超越是所述机器人探测范围内的所述环境目标并发出警示提醒，所述机器人再执行超越所述环境目标的行为。

拓展超越，所述拓展超越是所述机器人探测范围内的所述环境目标，所述机器人执行无警示提醒下的超越所述环境目标的行为。

优选地，所述目标力、所述对齐力和所述分离力的合力是所述机器人受到的基本作用力，所述合力为：

将受到的所述合力根据所述机器人的运动方程进行分解，得到社会力模型函数，公式如下：

式中，m为机器人的质量，

为机器人的速度，I为机器人的转动惯量，

为机器人受到的目标力，

为分离力矢量，

为机器人受到的对齐力的矢量转向力，

为机器人转弯的角度，t为转弯时间。

本发明具有的优点和积极效果是：

本发明可以实时实现对行人的运动趋势的估计，分析时变的行人运动状况，快速做出导航决策和规划路径，以此实现路径的拓展，这对实现载人机器人在人群环境中快速、安全的通行具有实用价值和现实意义。

附图说明

图1是本发明实施例的流程示意图；

图2是本发明实施例的目标运动区域示意图；

图3是本发明实施例的目标行走示意图；

图4是本发明实施例的目标行走示意图；

图5是本发明实施例的目标选择通行路径示意图；

图6是本发明实施例的TEB障碍物构建模型示意图；

图7是本发明实施例的优化后障碍物惩罚参数路径规划图；

图8是本发明实施例的随众策略行为中的改进类鸟群工作规划示意图；

图9是本发明实施例的仿真环境示意图；

图10是本发明实施例的无行人通行环境路径规划示意图；

图11是本发明实施例的行人同行具备通行条件的路径规划示意图；

图12是本发明实施例的穿行行人环境的路径规划示意图；

图13是本发明实施例的路径拓展实验仿真示意图；

图14是本发明实施例的机器人探测模式切换超越模式路径示意图；

图15是本发明实施例的机器人超越模式切换随众模式路径示意图；

图16是本发明实施例的机器人随众模式切换探测模式路径示意图；

具体实施方式

需要说明的是，在不冲突的情况下，本发明中的实施例及实施例中的特征可以相互结合。下面将参考附图并结合实施例来详细说明本发明。

为了使本领域技术人员更好地理解本发明方案，下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分的实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都应当属于本发明保护的范围。

在本实施例中提供了一种航站楼载人机器人路径拓展通行方法，如图1所示，根据本发明实施例提供的航站楼载人机器人路径拓展通行方法的流程图。机器人运行办法如下：

分析群体中的环境目标的运动模型；

激光雷达获取环境目标的位置信息；

建立环境目标的危险评估函数；

对环境目标的危险评估函数优化得到代价地图函数；

将危险评估函数与代价地图函数加入到TEB惩罚函数中，建立环境目标的运动信息函数；

根据运动信息函数设计随众、探测和超越三种通行模式；

结合通行模式，实验验证Risk-TEB路径规划算法在不同环境目标的通行状态下的有效性。

下面结合附图和具体实施例对本发明提供的一种航站楼载人机器人路径拓展通行方法进行详细说明。

分析群体中的环境目标的运动模型：

由于人群在机场航站楼的运动具有目的性，在低密度人群中，行人多选择以舒适的心理范围进行快速离开；在高密度人群中，由于行人的年龄、身体状况、人群挤压效应等影响，会出现走走停停、人群快速移动、自组织的朝向目标行走等运动方式。

行人在不走动的情况下，根据道路通行能力手册(HCM2000)，行人在地面的投影大小为0.6m×0.5m。与此同时，行人运动时需求的空间与心理需求十分紧密，人与人之间的距离分类如表1所述，一般情况下，当行人在拥挤环境中行走时，个人需求空间无法满足，此时行人的心理需求将会降低。

表1人与人之间的距离分类

大多数情况下，当采用固定半径的圆描述行人区域时，计算简单，但没有充分考虑行人的行走规律。通过对人群中行人运动的观察，行人在运动过程中，运动路径并不是一条直线，而是存在一定的偏移，相应的行人区域也应该符合基本的行走规律。

人群中由于受到周围行人运动空间的挤压效应和目的地吸引效应，行人前行时的运动方向总趋势是向前，行人前行时的运动范围或者路径范围大约在行人正前方左右30°内，基于对行人运动范围和路径范围的计算，行人行进路径正前方的概率为95％，行进路径偏移30°的概率为5％，而对于行人急转弯的概率为20％。

如图2所示，在人群中，由于周围行人与行人之间的行进影响，行人在行走的过程中急停的状态很小，但是机器人为了避免“追尾”行人，设定了机器人与行人的安全距离。

行人在确定起始点与目标点之后，通常所规划出来的全局路径是一条可通行的区域，在此区域内，行人通过对前方行走区域的观察，根据障碍物的不同，分为：

1、在无行人环境中，行人会在通行区域内按照自身所能承受的最大速度前进；

2、当前方有一个目标时，根据前方行人相对于自己前方的位置和相对行走速度进行判断；

如图3所示，规定以前方行人为参考点，相对速度表示为：v_r＝v_i-v_o，其中v_r为相对速度，v_i为自身速度，v_o为前方行人速度；

当v_r>0时：若行人在自己左侧，则进行右侧通行；若在自己正前方，根据行走规则，一般情况下，从左侧进行通行；若行人在自己右侧，则进行左侧通行。

如图4所示，当v_r≤0时：无论行人在什么位置，均选择以稳定速度前行。

3、根据行人在短距离行进时所关注前方多个行人的判断行为，以及所处位置和相对行走速度进行判断。

规定以前方每个行人为参考点，相对速度表示为：v_ri＝v_i-v_oi，其中v_ri为相对第i个行人的相对速度，v_i为自身速度，v_oi为前方第i个行人的速度。

当行人以视野范围内第一排的行人作为参照，为了思考超越的可能性，会对第二排行人的运动状态进行判断；

当行人以第一排中2-5个行人的运动状态进行观测，由于前方视野内的每个行人的运动速度不尽相同，当v_i>min[v_oi]时，由于速度快的行人在行进时，会致使所通行后方区域出现空闲，此时此处行程将被视为可通行区域，行人的运动将多向此运动；当v_i<min[v_oi]，行人则保持当前运动状态。

如图5所示，当前方有三个行人行进时，开始时刻处于1、2、3号行人身后，此时三个行人速度v₁>v₃>v₂，随着时间的推移，1号行人后方出现可通行区域，根据运动行为实验对象对朝向1号行人身后区域运动，此时，3号行人将不再出现在自己的视野中，进而由4号行人将3号行人代替出现在实验对象视野中，由此进行循环往复。

在跟随人群前进的过程中，行人自身有一个全局通行区域进行指引，若自身的运动规划方向与前方人群的运动方向不一致时，随即由跟随状态进入脱离跟随人群状态，同时将时刻保持沿全局规划的通行区域进行通行。

4、当前方有行人或人群横行时，通过对行人的运动速度和运动方向估测，根据相对速度进行判别直行是否产生危险，若产生，则向行人运动的反方向运动，以期达到通行的目的。

Risk-TEB算法：

如图6所示，在一般的路径规划算法中，为了简化障碍物模型的构建，均是将障碍物膨胀为圆形，基于此传统的TEB路径规划算法在对障碍物避障时采用的也是将障碍物的半径膨胀为圆的方法。

然而，行人模型不完全等同于一般障碍物，行人有属于自己的运动模型。当激光雷达数据侦测观察到行人位置点(x_t,y_t)的改变时，将计算出关于该点的运动信息，行人相对于机器人的速度：

式中，x_t和y_t分别为t时刻的x方向和y方向上的行人的坐标值，x_t+1和y_t+1分别为t+1时刻的x方向和y方向上的行人的坐标值，T为扫描周期，

为行人相对于机器人的相对速度；

根据行人相对于机器人的速度建立行人危险评估函数：

式中，Φ为危险评估值，λ为危险参数调节因子，

为行人相对于机器人的位置向量，

为行人相对于机器人的相对速度；

为了满足在人群环境中路径规划考虑到行人的速度问题，当危险评估值小于0时，与此同时行人的代价地图函数也将随之改变，行人代价地图函数是对危险评估函数的优化而得，公式如下：

式中，Φ为危险评估值，λ为危险参数调节因子，

为行人相对于机器人的位置向量，

为行人相对于机器人的相对速度，当危险评估值小于0时，机器人可与行人保持安全距离；

建立个人空间模型，公式如下：

其中θ的公式为：

其中d的公式为：

式中，f_i ^p(x,y)为个人空间模型函数，Φ^p为环境目标产生的危险代价函数值，d为环境目标距离所述机器人的直线距离，θ为机器人与行人的位置连线与x轴的夹角，

为根据行人在x轴和y轴方向上的运动速度来控制f_i ^p(x,y)沿x轴和y轴的衰减速度；

结合个人空间模型，调整TEB路径点与障碍物之间的距离，建立行人与机器人的最小危险距离公式如下：

式中，f_i ^p(x,y)为个人空间模型函数，f_ob为行人与机器人的最小危险距离，Φ^p为行人产生的危险代价函数值，d为行人距离机器人的直线距离，θ为机器人与行人的位置连线与x轴的夹角，

为根据行人在x轴和y轴方向上的运动速度来控制f_i ^p(x,y)沿x轴和y轴的衰减速度；λ为行人代价地图的边界距离与行人距离的动态调整参数，r_omin为系统常量，S是形变因子；n是多项式系数，默认值为2；Γ为时间间隔序列；即为计算机器人探测并计算与行人保持最小危险距离的函数公式。

设计不同的通行模式：

机器人路径拓展是为了实现机器人能够像人类一样能够在任意的人群环境下安全通行，由于路径拓展不是一个单一的通行环节，需要根据行人在人群中的运动模型，对载人机器人的通行模式进行设计；由于航站楼环境内行人的分布随时都会发生变化，因此载人机器人所处区域的可通行状态也可能发生变化。实现在不同环境下的路径拓展，机器人需要根据区域内行人的位置来判断通行状态，来制定相应的通行策略。

如表2所示，通行状态根据人群中行人的位置分布可分为穿行状态、缓慢通行状态和拥堵状态，由此定义航站楼载人机器人的集中通行模式：随众模式、探测模式和超越模式。

将全局路径用n个节点划分为距离相对较小的路径段，这里的节点称作为检查点，载人机器人根据当前坐标位置计算与下一个检查点的相对位置，若角度过大，为了舒适性要求，防治机器人路径跟随时出现震荡，则适当选择将检查点向后移动。

表2不同通行状态下的通行模式表

随众模式：

如图8所示，通过对行人运动特征的分析，当机器人遇到因人群密度大造成拥堵现象时，局部范围内，机器人可能无法完全按照全局路径行进，为了保证机器人不与行人碰撞，采用的策略为保持和人群运动方向一致，此时局部路径会与行人运动路径相重合。

当机器人摄像头摄像视野范围内的行人为单行人时，根据行人特征分析中的特征描述，将机器人视为后方行人，通过人的行走策略进行跟随，此时斥力和引力主要分为目标力、对齐力和分离力。

目标力：

使机器人不偏离全局路径的目标力公式如下：

式中，

为分离力矢量，K为比例系数，

为当前行人的位置矢量，

为相邻行人的位置矢量。

目标力的大小同当前机器人到检查位置的距离成正比，若机器人远离检查点位置，则在目标力的作用下，会产生较大的加速度，从而使机器人能够以较快的速度向目标位置运动。

对齐力：

使机器人跟随行人运动不会脱落的对齐力，公式如下：

式中，

为机器人受到的对齐力的矢量转向力，K为比例系数，

为机器人运动方向的角度矢量，

为设定范围内平均运动方向的角度矢量，N为机器人探测的行人的数量。

若机器人的运动方向通群体的平均运动方向同群体的平均运动方向存在较大偏角时，对齐力的作用就会增强，从而产生较大的角加速度使机器人的运动方向同人群的运动方向保持一致。

分离力：

行人与机器人保持距离的分离力，公式如下：

式中，

为分离力矢量，K为比例系数，

为当前行人的位置矢量，

为相邻行人的位置矢量，行人的位置矢量与相邻行人的位置矢量的差值即为两者之间的距离矢量，N为当前对象视野内的行人数量。

基本作用力的合成和运动解算：

目标力，对齐力和分离力的合力就是机器人受到的基本作用力，即

将收到的合力根据机器人的运动方程进行分解可以得到如下社会力模型函数公式：

式中，m为机器人的质量，

为机器人的速度，I为机器人的转动惯量，

为机器人受到的目标力，

为分离力矢量，

为机器人受到的对齐力的矢量转向力，

为机器人转弯的角度，t为转弯时间。

社会力模型为行人运动时的方向在自我认知为安全舒适的条件下所受自身心理力，也是同其他物体保持适当的安全距离；为了防止外界事物对自己的影响所产生的的物理力和朝向目标点运动的驱动力影响。

如图所示为行人趋利避模型示意图，

图中对力的分析可得如下公式：

其中，α_psy、β_psy为行人自身的心理力参数，r_p为自身设定的安全间距，d_p为与其他行人之间的间距，n_psy表示心理力的方向，与移动物体的位置方向相反。α_phy、β_phy为物理力参数，d_r为与移动机器人的间距，v_r为机器人的移动速度，m_phy表示距离力的方向，与机器人的运动方向相反，n_phy表示物理力方向，该力的方向与速度方向垂直，并且与m_phy夹角为锐角。s_dri为在目标点方向上行人的个数，v_dri为自身运动的平均速度，v_p为行人的自身速度，n_dri表示目标点对自身的驱动力方向，始终朝向目标点。

由于力的运算关系，则行人得到的合力为：

F＝F_psy+F_phy+F_dri

由该公式可知，在该场景中外界对自己的影响F_phy起主要作用，也即是行人会向远离碰撞方向运动，产生避让行为。因此，在人群环境中，机器人可通过改变F_phy从而改变行人的运动以此实现避让。

探测与超越模式:

机器人在随众过程中，时刻对探测视野中的行人位置及危险度进行判别，如果没有可行的路径，机器人将一直不间断的进行探测，检查是否满足其他状态的行进条件。如果是，则切换至相应状态下的通行模式，若没有合适行进条件则机器人会选择随众模式。

超越模式根据具体需求，可以分为随众超越和拓展超越。

随众超越是机器人发出警示提醒前方行人的行为，依然无法让行人产生避让行为，在保证安全的前提下采取伺机行动，侦测可以突破超越前方行人的行为；

拓展超越是根据机器人在行进过程中出现的前方行人的位置和危险度的判定，可以实现在无警示刺激下就可以进行超越的行为。

实施例1：路径拓展仿真实验

对于人群中自主通行路径拓展进行仿真，场景中对行人的模拟采用基于社会力模型的pedsim ros行人仿真包进行生成，机器人采用双轮差速动力学模型，同时机器人配备了一个模拟激光雷达传感器，仿真中全局路径规划均通过A*算法进行生成，仿真环境通过RVIZ进行可视化，仿真环境基于ubuntu 16.04ROS机器人操作系统进行。

Risk-TEB路径规划仿真实验：

为了评估Risk-TEB路径规划在不同人群通行状态下路径规划的可行性，在如图9所示的仿真环境中进行实验。仿真环境中，分别给定机器人的运动目标点，行人的运动弯曲根据pedsim ros行人仿真包进行随机分配，并且行人的运动也具有随机性，环境中的不同行人会按照不同的运动速度进行运动，以此在环境中形成不同的通行区域。

如图10所示，在仿真环境地图中，将机器人的目标点设置到右侧无行人区域，此时左侧的行人不会对机器人规划的路径产生影响，根据机器人的探测，规划出一条平滑的路径并沿着该路径进行前行。

如图11所示，机器人在目前位置前往设定的重点位置时，机器人右前方存在两个行人，左前方存在可通行的区域，机器人基于对行人的位置探测和运动趋势进行评估，生成通行路径达到目标终点。

如图12所示，机器人准备穿行于两个静止的行人之间，根据机器人的探测，行人此时均为静止状态，不对机器人的运动产生影响，随即机器人规划出穿行于行人环境的路径。

在相似场景下对传统TEB算法做同样的实验，评价标准为所规划路径距离行人的最近距离，结果如表3所示：

表3行人最近距离对比表

[注]：“-”代表无数据

从表中可以得出，原始TEB算法在没有添加行人危险评价函数对障碍物惩罚函数进行调整时，所规划出的路径比添加之后的更近，导致行人在运动过程中有危险的发生，因此为了验证两种算法在三种场景中通行成功且不与行人相碰的次数，同类型的场景实验均以10次测试为标准，如表4进行了统计：

表4通行成功次数统计表

由于每次仿真时，行人的运动均是随机的，完全模仿现实中行人的运动，所以每次的实验数据均不相同，通过统计来进行分析，消除了随机误差，三种同类型场景下可以看出，当在路径约束函数中加入行人危险评价后，通行成功的次数高，相较于传统的TEB更有优势。

机器人路径拓展仿真：

为了验证载人机器人在路径拓展中的可靠性和安全性，通过如图13所示的地图进行实验，实验场景中模拟了狭窄通道和宽阔环境下的通行场景，场景中的行人同样采用pedsim_ros行人仿真包进行随机生成，实验过程中，机器人将经过宽阔环境、狭窄通道，在行驶过程中将遇到不同的行人。

如图14所示，机器人启动，采用全局路径规划偏向靠近行人，然而通过机器人的探测发现行人在视野中，由于目标点的约束作用，导致局部路径规划靠近临时全局路径检查点时比较靠近检查点，但是由于行人的存在，以及前方存在可通行的区域，故机器人在路径拓展时采用由探测模式切换到超越模式，以期达到快速通行。

如图15所示，当机器人越过人群，来到狭窄区域，此时，区域符合超越条件，将一直保持超越的状态进行通行，当探测到前方行人的时候，机器人随机从超越模式切换到随众模式，并保持跟随，由于此时行人对机器人的排斥作用较小，机器人跟随人群行走的速度比行人大。

如图16所示，在机器人靠近行人之后，机器人从随众模式中切换到探测模式，并规划出一条绕过行人运动的局部路径，由于行人的运动速度小于机器人，此时，行人对机器人的危险度较小，所以此时局部路径规划的点在行人正前方，机器人选择朝向全局路径点前进。

实施例2：路径拓展实验

将路径拓展算法部署于载人机器人系统中进行物理验证

路径拓展实验的软件框架为基于ROS系统的导航框架move base，该框架主要由全局规划器、局部规划器、代价地图和恢复行为四个部分构成。关于机器人的位姿确定，movebase会通过订阅由SLAM实时定位与建图节点发布的机器人位姿信息进行确定，通过坐标转化TF树来实时获得建图节点与机器人、传感器的数据等之间相对于建图节点的位姿关系。

在给出一个机器人需要导航的目标点后，全局路径规划器采用A*算法，将根据已经订阅的全局静态的地图和经传感器数据构建的全局代价地图，为机器人准确规划一条完整的全局无障碍路径；在实际的路径规划过程中，除了需要完整的全局路径外，由于障碍物是动态的，因此结合机器人进行路径拓展时的通行模式，不同环境下可以进行切换不同路径的规划器，通过路径规划器规划出来的速度、角速度信息传递给机器人执行机构完成机器人的运动，最终实现路径拓展。

狭窄空间路径拓展实验：

由于航站楼载人机器人在运行过程当中会遇到各式各样的工作环境，通过SLAM建图模块建立的走廊地图，在狭窄走廊里机器人从地图起点运行到终点，以此验证机器人在狭窄空间遇到行人后的自主通行能力。

机器人以起点A至终点B行进，走廊全长108米，走廊宽度为2.4米，机器人长0.8米，宽0.6米。行人在机器人周围随机移动，在此过程中实验人员分别对机器人不遮挡、部分遮挡、完全遮挡进行测试。

狭窄走廊空间中，机器人处于完全可通行的环境，此时命令机器人运行到目标位置，此时，路径拓展通行模式由探测模式切换为超越模式，由于走廊比较狭窄，局部代价地图会将墙壁障碍物进行膨胀，仅留下走廊中间最安全区域，此时超越模式中Risk-TEB路径规划算法会选择中间区域进行路径规划，保证了机器人的安全运行。机器人继续执行往目标位置行进，在行驶过程中遇到1位行人在其运动路径的前方对其行进路线进行部分遮挡，此时路径拓展通行模式由超越模式切换到探测模式，根据行人所在位置和运动速度，进行危险度判断，根据判断结果，切换到超越模式，此时在行人与墙壁中间通行区域最大的一侧将会是Risk-TEB路径规划算法的最优选择，以保证机器人能够顺利通行。

当行人在机器人正前方，处于完全遮挡状态，此时机器人的路径拓展模式处于探测模式中，探测模式会根据机器人的配置参数判断剩余空间是否允许通行，若不允许，则将探测模式切换到随众模式。

由走廊宽度和机器人宽度，在人与墙壁之间留有1米空间，剩余空间允许通行，因此此时将有探测模式切换到超越模式，可以看到载人机器人由Risk-TEB实时进行路径调整，而且此时规划所发布的速度指令将比较缓慢，防止撞到障碍物。

通过测量走廊宽度和机器人宽度可以得知，在行人与墙壁之间留有部分空间，剩余空间允许通行，因此此时机器人的探测模式切换到超越模式，人机器人由Risk-TEB实时进行路径调整，此时规划所发布的速度指令将比较缓慢，防止撞到障碍物。

多行人路径拓展实验：

在航站楼场景中，行人存在不同的运动状态，为了验证机器人在具有多行人的环境中的行进效果，设计宽阔的连廊环境进行多行人路径拓展实验。

机器人朝着目标点开始行进，此时经过探测发现前方行人无法超越，因此开启随众模式；由于机器人在进行对行人人群进行跟随时，虽然探测视野中的行人会有一个ID号，但是行人对机器人的影响则没有对行人ID进行绑定，所以随众过程中行人位置进行交换不会影响机器人的跟随。

机器人在行进过程中，发现前方行人之间构成的空间具备超越的可能，随即将机器人正在执行的探测模式切换到超越模式，根据超越模式中局部路径规划器Risk-TEB对路径进行重规划，以确保能够安全通行。

本发明的优点：

可以实时实现对行人的运动趋势的估计，分析时变的行人运动状况，快速做出导航决策和规划路径，以此实现路径的拓展，这对实现载人机器人在人群环境中快速、安全的通行具有实用价值和现实意义。

以上对本发明的实施例进行了详细说明，但所述内容仅为本发明的较佳实施例，不能被认为用于限定本发明的实施范围。凡依本发明申请范围所作的均等变化与改进等，均应仍归属于本发明的专利涵盖范围之内。

Claims (9)

1.一种航站楼载人机器人路径拓展通行方法，其特征在于，所述方法包括步骤：

分析环境目标的运动模型；

获取所述环境目标的位置信息；

根据所述环境目标的位置信息推断所述环境目标的运动信息；

建立所述环境目标的运动信息函数；

基于所述环境目标的运动信息函数设计机器人的通行模式。

2.根据权利要求1所述的一种航站楼载人机器人路径拓展通行方法，其特征在于，所述建立所述环境目标的运动信息函数包括步骤：

建立所述环境目标的危险评估函数；

对所述环境目标的危险评估函数优化得到代价地图函数；

将所述代价地图函数加入到TEB惩罚函数中。

3.根据权利要求1所述的一种航站楼载人机器人路径拓展通行方法，其特征在于，所述设计机器人的通行模式包括步骤：

建立随众模式：所述随众模式是根据对所述环境目标的运动特征的分析，采用与所述环境目标保持运动方向一致的行进模式；和/或，

建立探测模式：所述探测模式是机器人在所述随众模式过程中，对范围内的所述环境目标位置及危险度进行判别；和/或，

建立超越模式：所述超越模式是所述机器人在运行过程中根据运行轨迹的判断加速超过所述环境目标的过程。

4.根据权利要求2所述的一种航站楼载人机器人路径拓展通行方法，其特征在于，所述建立所述环境目标的危险评估函数包括步骤：

建立所述环境目标相对于所述机器人的相对速度函数，公式如下：

式中，x_t和y_t分别为t时刻的x方向和y方向上的所述环境目标的坐标值，x_t+1和y_t+1分别为t+1时刻的x方向和y方向上的所述环境目标的坐标值，T为扫描周期，

为所述环境目标相对于所述机器人的相对速度；

根据所述相对速度函数建立所述环境目标的危险评估函数，公式如下：

式中，Φ为危险评估值，λ为危险参数调节因子，

为所述环境目标相对于所述机器人的位置向量，

为所述环境目标相对于所述机器人的相对速度。

5.根据权利要求2或4所述的一种航站楼载人机器人路径拓展通行方法，其特征在于，所述对所述环境目标的危险评估函数优化得到代价地图函数包括公式如下：

式中，Φ为危险评估值，λ为危险参数调节因子，

为所述环境目标相对于所述机器人的位置向量，

为所述环境目标相对于所述机器人的相对速度，当所述Φ小于0时，所述机器人可与所述环境目标保持安全距离。

6.根据权利要求2所述的一种航站楼载人机器人路径拓展通行方法，其特征在于，所述将所述代价地图函数加入到TEB惩罚函数中包括步骤：

建立个人空间模型，公式如下：

其中θ的公式为：

其中d的公式为：

式中，f_i ^p(x,y)为个人空间模型函数，Φ^p为所述环境目标产生的危险代价函数值，d为所述环境目标距离所述机器人的直线距离，θ为所述机器人与所述环境目标的位置连线与x轴的夹角，

为根据所述环境目标在x轴和y轴方向上的运动速度来控制f_i ^p(x,y)沿x轴和y轴的衰减速度；

结合所述个人空间模型，调整TEB路径点与障碍物之间的距离，建立所述环境目标与所述机器人的最小危险距离公式如下：

式中，f_i ^p(x,y)为个人空间模型函数，f_ob为所述环境目标与所述机器人的最小危险距离，Φ^p为所述环境目标产生的危险代价函数值，d为所述环境目标距离所述机器人的直线距离，θ为所述机器人与所述环境目标的位置连线与x轴的夹角，

为根据所述环境目标在x轴和y轴方向上的运动速度来控制f_i ^p(x,y)沿x轴和y轴的衰减速度；λ为所述环境目标代价地图的边界距离与所述环境目标距离的动态调整参数，r_omin为系统常量，S是形变因子；n是多项式系数，默认值为2；Γ为时间间隔序列。

7.根据权利要求3所述的一种航站楼载人机器人路径拓展通行方法，其特征在于，所述随众模式包括对所述机器人在运行过程中产生的斥力与引力进行分类，分为：

使所述机器人不偏离全局路径的目标力，和/或，使所述机器人跟随所述环境目标运动不会脱落的对齐力，和/或，使所述环境目标与所述机器人保持距离的分离力；

所述目标力公式如下：

其中，

为所述目标力，，K为比例系数，

为所述环境目标的当前位置坐标，

为所述环境目标的当前位置坐标的下一备选全局路径检查点的位置坐标；

所述对齐力公式如下：

式中，

为所述机器人受到的对齐力的矢量转向力，K为比例系数，

为所述机器人运动方向的角度矢量，

为设定范围内平均运动方向的角度矢量，N为所述机器人探测的所述环境目标的数量。

所述分离力公式如下：

式中，

为分离力矢量，K为比例系数，

为当前所述环境目标的位置矢量，

为相邻所述环境目标的位置矢量，N为当前所述机器人探测视野内的所述环境目标数量。

8.根据权利要求3所述的一种航站楼载人机器人路径拓展通行方法，其特征在于，所述超越模式包括：

随众超越，所述随众超越是所述机器人探测范围内的所述环境目标并发出警示提醒，所述机器人再执行超越所述环境目标的行为。

拓展超越，所述拓展超越是所述机器人探测范围内的所述环境目标，所述机器人执行无警示提醒下的超越所述环境目标的行为。

9.根据权利要求7所述的一种航站楼载人机器人路径拓展通行方法，其特征在于，所述目标力、所述对齐力和所述分离力的合力是所述机器人受到的基本作用力，所述合力为：

将受到的所述合力根据所述机器人的运动方程进行分解，得到社会力模型函数，公式如下：

式中，m为机器人的质量，

为机器人的速度，I为机器人的转动惯量，

为机器人受到的目标力，

为分离力矢量，

为机器人受到的对齐力的矢量转向力，

为机器人转弯的角度，t为转弯时间。

Images