CN112882469A - 一种融合全局训练的深度强化学习避障导航方法 - Google Patents

Abstract

本发明公开了一种融合全局训练的深度强化学习避障导航方法。根据已知信息规划初始路径；机器人从初始点沿着初始路径向目标点运动；选择临时目标向着临时目标运动；设置累计期望奖励惩罚值，不断多次导航，机器人导航过程中每一帧给予奖励惩罚值；从传感器获得交互元组；输入深度学习网络训练；将待导航的交互元组输入训练后的深度学习网络，输出最优的路径及累计期望奖励惩罚值，按照最优路径运动。本发明能够有效地提高深度强化学习的收敛速度，在导航过程中提高机器人的导航效率，同时使得机器人的运动具有环境友好性，将对周边环境的影响降至最低。

Description

一种融合全局训练的深度强化学习避障导航方法

技术领域

本发明涉及机器人的动态环境避障导航方法，特别是涉及了一种融合全局训练的深度强化学习避障导航方法。

背景技术

对诸如送货机器人，室内服务机器人等动态环境下工作的自动导航机器人来说，避障导航是其中十分重要的功能。机器人必须在避开各种障碍物的同时安全快速地到达目标点。对于静态障碍物的避障研究相对成熟，对动态障碍物的避障导航问题则更加复杂，因为它需要对未知的周围动态(如行人，车辆或其他机器人)进行预测。随着周围环境复杂性的增加，传统导航方法的可通行区域会越来越小，直到出现机器人无法规划可行的路径，即出现冻结机器人问题。此外，当移动障碍物是行人时，不仅要考虑避免碰撞，还要考虑机器人的运动对行人的影响。

传统方法更多的是基于几何模型进行避障规划，这些模型可以被看作是没有任何认知过程的低级智能。因此，传统方法对传感器数据的精确度要求较高，数据的误差对避障导航算法的性能有较大的影响，在实际环境中无法处理较为复杂的情况。而深度强化学习既可以解决传统方法的缺点，同时也不需要事先准备大量数据集，被认为是动态避障问题中一种十分有潜力和应用价值的技术。但深度强化学习收敛性差，环境适应能力弱等问题也制约了该方法的推广应用。本发明结合传统方法和深度强化学习，利用部分已知的环境信息规划初始路径作为全局，加快深度强化学习的学习速度，同时设计深度强化学习的区别化奖励函数，使得机器人的避障导航具有环境友好性，降低对周围环境的影响。

发明内容

为了解决背景技术中存在的问题，本发明的目的在于提供一种融合全局训练的深度强化学习避障导航方法，适用于部分环境信息已知，如室内、小区等情况下的机器人导航。

本发明在轮速里程计以及SLAM系统的支持下，可以获得车辆当前的位置信息、运动信息以及与目标的相对位置关系。利用传统算法规划出由一系列路点组成的初始路径，作为深度强化学习训练，使得机器人能够更好地学习到如何在躲避障碍物的同时有效导航。当训练完成后，深度强化学习的避障导航器则能独立工作，不再需要全局辅助。

本发明采用的技术方案的步骤如下：

深度强化学习训练阶段包括如下步骤：

(1)根据环境中部分已知的静态障碍物信息以及机器人的初始点与目标点，利用传统算法规划出由一系列路点组成的初始路径；

(2)机器人从初始点沿着初始路径向目标点运动；

(3)在机器人实时运动过程中，根据机器人当前所处的位置，选择初始路径中的路点作为机器人的临时目标，机器人向着临时目标运动；

在机器人向着临时目标运动过程中，设置累计期望奖励惩罚值，不断进行机器人的多次导航，机器人导航过程中每一帧具有环境给予的奖励惩罚值，从初始点到目标点的奖励惩罚值累计处理为累计期望奖励惩罚值，将当前帧的奖励惩罚值加入机器人的交互元组；

将连续多帧的交互元组输入深度学习网络作为训练数据，深度学习网络预测输出机器人从初始点到目标点的多种路径及其对应的累计期望奖励惩罚值，以累计期望奖励惩罚值最大化进行训练优化，直到深度学习网络满足精度要求；

然后针对待导航的机器人，将机器人当前帧的交互元组输入到训练后的深度学习网络中，输出最优的路径及其对应的累计期望奖励惩罚值，控制待导航的机器人按照最优的路径运动以实现避障导航。

所述步骤(1)中，用传统的快速探索随机树算法规划一条由一系列路点组成的初始路径，在深度强化学习训练过程中发挥全局作用，提高训练收敛速度。

在环境中，存在边界、静态障碍物以及动态障碍物，边界和部分静态障碍物的信息已知，或前期可借助传感器获取相关信息，利用边界和部分静态障碍物的信息构建全局地图，全局地图输入到快速探索随机树算法模块中生成一条由多个路点组成的初始路径。

所述步骤(2)具体为：根据机器人在全局地图中的位置，结合步骤(1)给出的初始路径，在初始路径中根据机器人与各个路点的距离远近情况选择一个位于机器人和目标点间的路点作为机器人的临时目标；机器人将根据自身与临时目标的相对位置关系，进行运动控制，向着临时目标运动。

临时目标应当满足如下几点要求：

a)初始路径中每个路点应当只使用一次，不应在经过选择后再次选择回该路点；

b)初始路径中的路点不要求逐个使用，应根据机器人当前的位置按需选择；

c)临时目标具有唯一性，机器人在导航过程中有且仅有一个临时目标。

所述步骤(3)中，每帧的交互元组包括当前机器人获得机器人当前帧采集的雷达点云、当前帧的动作控制量和当前帧的奖励惩罚值；动作控制量是指机器人的旋转角速度和运动线速度。初始化累计期望奖励惩罚值为零，奖励惩罚值分为包含运动奖励、导航时间奖励、轨迹平滑奖励、路点奖励、到达目标奖励以及碰撞发生时的惩罚。

所述的累计期望奖励惩罚是由运动奖励、导航时间奖励、轨迹平滑奖励、路点奖励、到达目标奖励以及碰撞发生时的惩罚累计而成。

在所述步骤(3)中，将对机器人的每一步运动做出奖励或惩罚，包括运动奖励、导航时间惩罚、轨迹平滑奖励、路点奖励、到达目标奖励和碰撞发生时的惩罚；

A)运动奖励

临时目标对机器人的运动起到了全局控制的作用，机器人的航向角和机器人与临时目标的方位角相差越小代表机器人越向着临时目标运动。增加以下运动奖励，即对机器人向着临时目标运动的行为做出奖励：

其中，

表示运动奖励值，angle表示了机器人的航向角和机器人与临时目标的方位角之间的差值，Φ(·)表示类余弦函数，类余弦函数Φ(·)的输出值与输入值成反比，输入值越小输出值越大；

B)导航时间惩罚

本发明将机器人的线速度设定为一个固定值，机器人的导航时间转化为机器人的运动轨迹长度。当机器人运动轨迹长度大于初始路径的长度，认为机器人没有以较优的方式运动，增加以下导航时间惩罚，给予机器人惩罚：

其中，

表示导航时间惩罚值，

为初始路径的长度，由初始路径上相邻路点间的距离相加得到；

为机器人从初始点运动开始的运动轨迹长度；γ_t为预先设定的时间调节参数；

C)轨迹平滑奖励

机器人圆滑的运动轨迹能缩短导航时间，且也有利于躲避障碍物；同时考虑到机器人的驱动方式，频繁的转向会导致机器人驱动模块工作效率低下。按照以下公式的设置对角速度的突然大变化增加轨迹平滑奖励，给予机器人一个惩罚，有助于在导航时获得平滑的轨迹：

其中，

表示轨迹平滑奖励值，ω_t表示了t时刻机器人的角速度，γ_osc为预先设定的角速度调节参数，

为预先设定的角速度阈值；

D)路点奖励

按照以下公式设置路点奖励，当机器人每次到达一个临时目标时，增加以下路点奖励，给予机器人一个奖励，增强初始路径的全局引导：

其中，

表示路点奖励值，

为预先设定的距离阈值；

E)同时，针对机器人每次导航结束的不同情形，增加机器人区别化的奖励和惩罚：

当机器人在沿初始路径运动过程中，会躲避障碍物，机器人根据障碍物的运动状态做出不同的运动响应，通过传感器数据实施分析障碍物的运动状态，进而判断是否设置奖励和惩罚：

E.1)2D激光雷达的传感器围绕机器人一圈发射激光并返回圆周各个角度的距离信息，根据返回圆周各个角度的距离信息处理获得机器人周围环境中各个障碍物相对于机器人的坐标距离；

E.2)在t₁时刻，探测获得各个障碍物相对于机器人的坐标距离，再结合机器人自身的全局坐标，得到t₁时刻环境中各个障碍物的全局坐标；

E.3)在t₁时刻之后紧邻的t₂时刻，按照步骤相同方式处理获得t₂时刻环境中各个障碍物的全局坐标；

E.4)t₁、t₂两个时刻间隔较短，通过预设的间隔距离阈值，将t₁、t₂两个时刻的障碍物进行一一配对，从而得到障碍物的运动信息；已知t₁时刻机器人的运动指令，可以在新的全局坐标中减去机器人的位置变化，对t₁时刻的障碍物位置进行预测。

若在t₁、t₂两个时刻，障碍物的全局坐标的变化小于预设的移动阈值，则视为障碍物没有运动，该障碍物为静态障碍物；

若在t₁、t₂两个时刻，障碍物的全局坐标的变化大于预设的移动阈值，且在预设的移动范围内，则视为障碍物是运动的，该障碍物为动态障碍物；

E.5)实时将机器人与静态障碍物之间的距离和预设的碰撞距离阈值进行比较，若机器人与静态障碍物之间的距离小于预设的碰撞距离阈值，则机器人与静态障碍物间发生碰撞；否则机器人与静态障碍物间未发生碰撞；

在机器人与静态障碍物发生碰撞时，本次导航直接结束，机器人不再向目标点运动，并增加一个固定的静态碰撞惩罚：

其中，

表示静态碰撞惩罚值，P_datum为设定的静态碰撞固定惩罚值；

E.6)根据已知t₁、t₂两个时刻障碍物的全局坐标的变化处理得到动态障碍物的运动速度和运动方向，实时获得动态障碍物的位置，将机器人与动态障碍物之间的距离和预设的碰撞距离阈值进行比较：

若机器人与动态障碍物之间的距离小于预设的碰撞距离阈值，则机器人与动态障碍物间发生碰撞；否则机器人与动态障碍物间未发生碰撞；

在机器人与动态障碍物发生碰撞时，根据不同碰撞角度增加动态碰撞惩罚，给予机器人不同的惩罚，以减少对环境的影响，让机器人的导航不会对行人等造成干扰：

P_dynamic＝-P_datum-γ_dynamic*cos(θ_obs-θ_robot)

其中，P_dynamic表示动态碰撞惩罚值，θ_obs和θ_robot分别为障碍物和机器人的运动方向，γ_dynamic为提前设定的调节参数。

E.7)机器人已到达目标点，导航结束：若机器人到达目标点，则代表机器人在导航过程中没有发生任何碰撞，顺利完成导航，本次导航结束，增加机器人到达目标奖励。

机器人无论是与静态障碍物发生碰撞，还是与与动态障碍物发生碰撞，均导致本次导航结束。碰撞后发起新的导航增加训练数据。

本发明首先利用传统路径规划方法中的快速探索随机树算法，根据部分已知的环境地图信息规划出一条初始路径，再将该初始路径作为深度强化学习的全局控制训练机器人的导航行为。在训练过程中，每一时刻都将根据机器人的动作指令、导航轨迹以及初始路径，给予机器人奖励惩罚值，进一步约束机器人的行为。同时本发明针对机器人发生碰撞的情形，提出了一种区别化的惩罚方法，根据机器人与障碍物两个运动方向的夹角给予机器人不同大小的惩罚，从而驱使机器人从障碍物运动方向的后方绕行，提高机器人导航过程的环境友好性。强化学习训练结束后，机器人工作时不再需要全局路径。

与背景技术相比，本发明具有的有益效果是：

(1)本发明对传感器精度要求较低，仅使用成本较低的2D激光雷达即可完成导航及避障任务；

(2)本发明利用传统路径规划算法对深度强化学习训练进行全局，能够有效加快深度强化学习的收敛速度；

(3)本发明具有较好的鲁棒性，能够有效处理各种复杂场景，只需将已知的环境地图信息输入快速探索随机树模块即可得到初始路径；

(4)本发明能够让机器人的避障导航过程具有更高的环境友好性，能减少对环境以及行人等的干扰。

综合来说，本发明能够有效利用传统算法得到的初始路径，发挥全局作用，加快深度强化学习收敛速度。通过本发明设计的奖励函数，使得机器人在导航过程中能够大致沿着初始路径快速抵达目标点，且具有较平滑的轨迹以及较高的导航效率。同时本发明提出的区别化惩罚机制，使得机器人在躲避动态障碍物(例如行人)时，更倾向于从障碍物运动方向后方绕行，极大提高了环境友好性，减少对环境的干扰。

附图说明

图1是实施例的走廊环境地图示意图；

图2是快速探索随机树算法模块输出的初始路径示例；

图3是初始路径的全局作用示意图；

图4是动态障碍物的四种碰撞情况示意图；

图5是仿真实验中的静态障碍物避障环境；

图6是仿真实验中的动态障碍物避障环境。

具体实施方式

下面结合附图和实施例对本发明做进一步说明。

在深度强化学习的训练阶段，按照本发明内容完整方法实施的实施例如下：

(1)根据已知的部分环境信息，建立全局地图，如图1所示，白色栅格代表机器人的可通行区域，黑色栅格代表环境边界或者动态障碍物；

(2)通过SLAM模块，获得机器人的初始点，将该位置与目标点同时输入快速探索随机树算法模块中，得到机器人导航任务的初始路径，如图2所示；

(3)将初始路径输入深度强化学习模块中作为机器人导航的全局控制，对机器人每个时刻的行为给予奖励惩罚值，包括运动奖励、导航时间惩罚、轨迹平滑奖励、路点奖励、到达目标奖励以及碰撞发生时的惩罚。

下面以一个简单的例子来表述全局控制的思想与具体实施步骤。图3(a)是根据部分已知地图、机器人初始点和目标点得到的初始路径。在这条初始路径中包含四个路点，第四个路点为机器人初始点，第一个路点为目标点，中间两个路点则为机器人需抵达的临时目标。

开始导航后，由于机器人从初始点出发，所以第四个路点当即从初始路径中删除，第三个路点(9.22，-5.97)成为机器人当前的临时目标，机器人向着临时目标运动。

经过一段时间的运动，机器人与临时目标的距离小于设定的阈值，于是该路点也被从初始路径中删除，第二个路点(8.01，-1.59)随即成为机器人的临时目标，指导机器人前往该位置。当机器人与第二个路点间的距离同样小于阈值时，执行与上个路点相同的操作，此时第一个路点(即目标)成为机器人被指引前往的位置。

当机器人的导航任务结束时，根据机器人是否达到目标点以及与何种障碍物发生碰撞，给予机器人区别化的奖励或惩罚。

(3.1)机器人已经到达目标点，导航结束，代表机器人在导航过程中没有发生任何碰撞，顺利完成导航任务，给予机器人正向奖励。

(3.2)机器人与静态障碍物发生碰撞，导致本次导航结束，给予机器人一个固定的惩罚：

(3.3)机器人与动态障碍物发生碰撞，导致本次导航结束，根据不同碰撞角度给予机器人不同的惩罚，驱使机器人从障碍物运动方向的后方绕行：

下面通过对机器人与动态障碍物发生碰撞的不同情形进行分析，具体说明区别化奖励的原理。如图4所示，红色圆形代表机器人，黑色圆形代表障碍物，根据发生碰撞时机器人在障碍物的前方或后方，机器人与障碍物的速度夹角呈锐角或钝角，可以将机器人与动态障碍物发生碰撞分为四种情形：

a.机器人在障碍物的后方，且机器人与障碍物的速度夹角呈锐角。

由于希望的是机器人在导航过程中尽可能减少对行人等动态障碍物的干扰，那么机器人在遇到动态障碍物时从障碍物速度方向的后方绕行是最佳的结果。如图4(a)所示，障碍物此时正在向右方运动，那么希望机器人从障碍物的左边绕行。而此时，机器人与障碍物不仅发生了碰撞，而且从机器人的速度方向可以看出机器人并没有躲避障碍物的意图，或者估计的障碍物速度过小，导致机器人认为该时刻障碍物应在更右的位置。因此希望在这种情形下，给予机器人更大的惩罚。

b.机器人在障碍物的前方，且机器人与障碍物的速度夹角呈锐角。

图4(b)所示的情形为机器人估计的障碍物速度过大，导致机器人认为该时刻障碍物应在更左的位置，从而机器人与障碍物发生了碰撞。同样，希望在这种情形下，给予机器人更大的惩罚。

c.机器人在障碍物的前方，且机器人与障碍物的速度夹角呈钝角。

当机器人与障碍物发生碰撞的情形如图4(c)所示时，代表机器人已经学习到了从动态障碍物的后方绕行，但是由于机器人本身运动能力有限或者由于深度网络未收敛导致机器人未能及时做出转向指令等其他因素导致机器人无法避开障碍物。所以希望在这种情形下，给予机器人更小的惩罚。

d.机器人在障碍物的后方，且机器人与障碍物的速度夹角呈钝角。

图4(d)的情形与图4(c)类似，机器人学习到了具备环境友好性的导航方式，所以给予机器人更小的惩罚。

通过对图4中四种情形的分析，可以看到在机器人与障碍物的速度夹角呈锐角是，均希望给予更大的惩罚，而呈钝角时给予更小的惩罚。所以在存在一个基准惩罚P_datum时，简单的cos(·)即可帮助实现区别化奖励。

机器人当前帧采集的雷达点云、当前帧的动作控制量和当前帧的累计期望奖励惩罚值组成当前帧的交互元组。将连续多帧的交互元组输入深度学习网络训练。待导航的机器人当前帧的交互元组输入训练后的深度学习网络，输出最优的路径及其累计期望奖励惩罚值，机器人将按照最优的路径运动。

在Gazebo仿真环境中测试评估了本方法的效果，以2D激光雷达作为输入，比较使用本方法前后的机器人导航效果。仿真环境中的静态障碍物地图与动态障碍物地图如图5与图6所示，所有环境中仅地图边界事先已知的，矩形静态障碍物与圆形动态障碍物均未知，圆形障碍物的箭头代表动态障碍物的运动方向。

表1列出了机器人使用本方法前后的导航效果数据对比。Original DQN代表未使用本方法的深度强化学习导航算法，DQN_guidance在深度强化学习方法中加入了全局，而DQN_friendly在此基础上进一步加上了碰撞的区别化惩罚。比较的指标包括三个：成功率、平均导航时间与平均绕路百分比。

成功率＝机器人到达目标次数/机器人总导航次数，是机器人导航任务中最重要的指标。平均导航时间统计机器人到达目标的平均时间，平均绕路百分比＝(机器人实际运动的距离-初始路径长度)/初始路径长度，这两个指标能够有效表示机器人的导航效率。

表1机器人使用本方法前后的导航效果比较

可以看出，采用本发明方法后，机器人在导航任务中明显提高了导航效率，且大幅提升了导航成功率。

由此可见，本发明结合传统算法和深度强化学习，能够有效地提高深度强化学习的收敛速度，在导航过程中提高机器人的导航效率，同时使得机器人的运动具有环境友好性，将对周边环境的影响降至最低。

Claims (6)

1.一种融合全局训练的深度强化学习避障导航方法，其特征在于，包括如下步骤：

(1)根据环境中部分已知的静态障碍物信息以及机器人的初始点与目标点，规划出由一系列路点组成的初始路径；

(2)机器人从初始点沿着初始路径向目标点运动；

(3)在机器人实时运动过程中，根据机器人当前所处的位置，选择初始路径中的路点作为机器人的临时目标，机器人向着临时目标运动；

在机器人向着临时目标运动过程中，设置累计期望奖励惩罚值，不断进行机器人的多次导航，机器人导航过程中每一帧具有环境给予的奖励惩罚值，从初始点到目标点的奖励惩罚值累计处理为累计期望奖励惩罚值，将当前帧的奖励惩罚值加入机器人的交互元组；

将连续多帧的交互元组输入深度学习网络作为训练数据，深度学习网络预测输出机器人从初始点到目标点的多种路径及其对应的累计期望奖励惩罚值，以累计期望奖励惩罚值最大化进行训练优化，直到深度学习网络满足精度要求；

然后针对待导航的机器人，将机器人当前帧的交互元组输入到训练后的深度学习网络中，输出最优的路径及其对应的累计期望奖励惩罚值，控制待导航的机器人按照最优的路径运动以实现避障导航。

2.根据权利要求1所述的一种融合全局训练的深度强化学习避障导航方法，其特征在于：所述步骤(1)中，用传统的快速探索随机树算法规划一条由一系列路点组成的初始路径。

3.根据权利要求3所述的一种融合全局训练的深度强化学习避障导航方法，其特征在于：在环境中，利用边界和部分静态障碍物的信息构建全局地图，全局地图输入到快速探索随机树算法模块中生成一条由多个路点组成的初始路径。

4.根据权利要求1所述的一种融合全局训练的深度强化学习避障导航方法，其特征在于：所述步骤(2)具体为：根据机器人在全局地图中的位置，结合步骤(1)给出的初始路径，在初始路径中选择一个位于机器人和目标点间的路点作为机器人的临时目标；机器人将根据自身与临时目标的相对位置关系，进行运动控制，向着临时目标运动。

5.根据权利要求1所述的一种融合全局训练的深度强化学习避障导航方法，其特征在于：所述步骤(3)中，每帧的交互元组包括当前机器人获得机器人当前帧采集的雷达点云、当前帧的动作控制量和当前帧的奖励惩罚值；初始化累计期望奖励惩罚值为零，奖励惩罚值分为包含运动奖励、导航时间奖励、轨迹平滑奖励、路点奖励、到达目标奖励以及碰撞发生时的惩罚。

6.根据权利要求1所述的一种融合全局训练的深度强化学习避障导航方法，其特征在于：在所述步骤(3)中，将对机器人的每一步运动做出奖励或惩罚，包括运动奖励、导航时间惩罚、轨迹平滑奖励、路点奖励、到达目标奖励和碰撞发生时的惩罚；

A)运动奖励

增加以下运动奖励：

其中，

表示运动奖励值，angle表示了机器人的航向角和机器人与临时目标的方位角之间的差值，Φ(·)表示类余弦函数；

B)导航时间惩罚

当机器人运动轨迹长度大于初始路径的长度，认为机器人没有以较优的方式运动，增加以下导航时间惩罚：

其中，

表示导航时间惩罚值，L_{init_path}为初始路径的长度，L_{current_path}为机器人从初始点运动开始的运动轨迹长度；γ_t为时间调节参数；

C)轨迹平滑奖励

按照以下公式的设置对角速度的突然大变化增加轨迹平滑奖励：

其中，

表示轨迹平滑奖励值，ω_t表示了t时刻机器人的角速度，γ_osc为角速度调节参数，ω_threshold为角速度阈值；

D)路点奖励

按照以下公式设置路点奖励，当机器人每次到达一个临时目标时，增加以下路点奖励：

其中，

表示路点奖励值，d_threshold为距离阈值；

E)同时，针对机器人每次导航结束的不同情形，增加机器人区别化的奖励和惩罚：

当机器人在沿初始路径运动过程中，会躲避障碍物，机器人根据障碍物的运动状态做出不同的运动响应，通过传感器数据实施分析障碍物的运动状态，进而判断是否设置奖励和惩罚：

E.1)2D激光雷达的传感器围绕机器人一圈发射激光并返回圆周各个角度的距离信息，根据返回圆周各个角度的距离信息处理获得机器人周围环境中各个障碍物相对于机器人的坐标距离；

E.2)在t₁时刻，探测获得各个障碍物相对于机器人的坐标距离，再结合机器人自身的全局坐标，得到t₁时刻环境中各个障碍物的全局坐标；

E.3)在t₁时刻之后紧邻的t₂时刻，按照步骤相同方式处理获得t₂时刻环境中各个障碍物的全局坐标；

E.4)通过预设的间隔距离阈值，将t₁、t₂两个时刻的障碍物进行一一配对，从而得到障碍物的运动信息；

若在t₁、t₂两个时刻，障碍物的全局坐标的变化小于预设的移动阈值，则视为障碍物没有运动，该障碍物为静态障碍物；

若在t₁、t₂两个时刻，障碍物的全局坐标的变化大于预设的移动阈值，且在预设的移动范围内，则视为障碍物是运动的，该障碍物为动态障碍物；

E.5)实时将机器人与静态障碍物之间的距离和预设的碰撞距离阈值进行比较，若机器人与静态障碍物之间的距离小于预设的碰撞距离阈值，则机器人与静态障碍物间发生碰撞；否则机器人与静态障碍物间未发生碰撞；

在机器人与静态障碍物发生碰撞时，本次导航直接结束，机器人不再向目标点运动，并增加一个固定的静态碰撞惩罚：

其中，

表示静态碰撞惩罚值，P_datum为设定的静态碰撞固定惩罚值；

E.6)根据已知t₁、t₂两个时刻障碍物的全局坐标的变化处理得到动态障碍物的运动速度和运动方向，实时获得动态障碍物的位置，将机器人与动态障碍物之间的距离和预设的碰撞距离阈值进行比较：

若机器人与动态障碍物之间的距离小于预设的碰撞距离阈值，则机器人与动态障碍物间发生碰撞；否则机器人与动态障碍物间未发生碰撞；

在机器人与动态障碍物发生碰撞时，根据不同碰撞角度增加动态碰撞惩罚：

P_dynamic＝-P_datum-γ_dynamic*cos(θ_obs-θ_robot)

其中，P_dynamic表示动态碰撞惩罚值，θ_obs和θ_robot分别为障碍物和机器人的运动方向，γ_dynamic为提前设定的调节参数。

E.7)机器人已到达目标点，导航结束：若机器人到达目标点，则代表机器人在导航过程中没有发生任何碰撞，本次导航结束，增加机器人到达目标奖励。

Images