一种具备全范围信息恢复能力的机器人导航避障方法

1.本公开涉及机器人避障技术领域，具体涉及一种具备全范围信息恢复能力的机器人导航避障方法及系统。

背景技术：

2.本部分的陈述仅仅是提供了与本公开相关的背景技术信息，不必然构成在先技术。
3.避障是机器人导航必不可少的功能之一。避障算法可分为基于地图(map-based)和反应式(reactive)两种，基于地图的算法由于需要实时更新障碍物地图，往往计算量较大，而且对于动态障碍物的躲避效果相对较差；反应式算法对从传感器获得的信息直接分析计算，得出机器人控制量，对于动态障碍物的实时避障效果有不错的提升，并且对不同环境的适应能力较强。现有的反应式避障算法往往只考虑当前时刻感知范围内的障碍物情况，如果在移动过程中某动态障碍物从感知范围内离开，但此时若其仍然在机器人周边一定范围内还是会对机器人的移动产生影响，此类情况会导致机器人发生二次碰撞，引发机器人移动安全问题。

技术实现要素：

4.本公开为了解决上述问题，提出了一种具备全范围信息恢复能力的机器人导航避障方法，能够在考虑当前观测的基础上，结合一定范围内的历史观测数据，将已经通过但仍然会对其之后动作产生影响的障碍物保留一定程度的记忆，避免出现刚刚避过某一障碍物却因为躲避另一方向上的障碍物而导致的与历史障碍物发生二次碰撞的情况。
5.根据一些实施例，本公开采用如下技术方案：
6.一种具备全范围信息恢复能力的机器人导航避障方法，训练步骤为：
7.在仿真环境三维空间中建立机器人坐标系，并在一定区域内随机生成障碍物的位置、以及机器人的起始点和目标点；
8.获取周围环境数据，基于状态空间、动作空间、奖励函数、状态转移概率、观测空间以及观测概率分布建立环境模型；
9.获取原始雷达和当前雷达的观测数据，构建历史观测数据队列，将当前最新的雷达观测数据以及位姿插入历史队列中，使历史观测数据与当前观测数据融合，形成机器人周围360
°
范围内环境的新的观测感知结果；
10.利用新的感知结果在优化更新的策略函数网络中输出的概率分布来选择机器人下一步的避障动作，直至到达目标点。
11.进一步的，建立的观测空间包括机器人对于原始数据补全后的感知结果、机器人当前速度、目标点在机器人局部坐标系下的坐标位置。
12.与现有技术相比，本公开的有益效果为：
13.本公本使用基于actor-critic框架的ppo强化学习算法作为训练基础算法，建立
机器人避障导航模型，并在此算法的观测数据获取以及奖励设置步骤中进行了改进。为观测数据增加历史信息，对多帧原始的雷达数据进行相应处理与融合，得到含有一定记忆功能的，可表示机器人周围360
°
范围内的障碍物距离信息的感知信息，使其具备全范围信息恢复能力，其中还包括了相关距离信息的时间特征，并将此感知信息送入深度学习网络中得到观测到动作的映射关系。实现了机器人在未知环境下的自主导航避障，提升了传统反应式避障算法对动态障碍物的避障性能；改善了雷达感知受限的情况，避免出现由于机器人的移动导致的障碍物离开感知区域，但其仍可能对机器人之后的移动造成影响而带来的二次碰撞问题。
附图说明
14.构成本公开的一部分的说明书附图用来提供对本公开的进一步理解，本公开的示意性实施例及其说明用于解释本公开，并不构成对本公开的不当限定。
15.图1为本公开雷达当前帧与历史雷达帧融合得到感知结果的流程图。
16.图2为本发明中避障网络模型的结构图。
17.图3为本发明实例中当前帧雷达与融合历史帧感知结果的雷达可视化效果对比，蓝矩形代表机器人。右图显示了机器人从
①
号位置点走到
②
号位置点的过程中，保持了对历史观测数据的记忆。
具体实施方式：
18.下面结合附图与实施例对本公开作进一步说明。
19.应该指出，以下详细说明都是例示性的，旨在对本公开提供进一步的说明。除非另有指明，本文使用的所有技术和科学术语具有与本公开所属技术领域的普通技术人员通常理解的相同含义。
20.需要注意的是，这里所使用的术语仅是为了描述具体实施方式，而非意图限制根据本公开的示例性实施方式。如在这里所使用的，除非上下文另外明确指出，否则单数形式也意图包括复数形式，此外，还应当理解的是，当在本说明书中使用术语“包含”和/或“包括”时，其指明存在特征、步骤、操作、器件、组件和/或它们的组合。
21.实施例1
22.本公开公开了一种具备全范围信息恢复能力的机器人导航避障方法，包括以下训练步骤：
23.步骤1：在仿真环境三维空间中建立机器人坐标系，并在一定区域内随机生成障碍物的位置、以及机器人的起始点和目标点；
24.步骤2：获取周围环境数据，基于状态空间、动作空间、奖励函数、状态转移概率、观测空间以及观测概率分布建立环境模型；
25.步骤3：获取原始雷达和当前雷达的观测数据，构建历史观测数据队列，将当前最新的雷达观测数据以及位姿插入历史队列中，使历史观测数据与当前观测数据融合，形成机器人周围360
°
范围内环境的新的观测感知结果；
26.步骤4：利用新的感知结果在优化更新的策略函数网络中输出的概率分布来选择机器人下一步的避障动作，直至到达目标点。
27.具体的，步骤2中建立的环境模型包括状态空间s、动作空间a、奖励函数r、状态转移概率p、观测空间ω、观测概率分布o。
28.进一步的，建立的观测空间包括机器人对于原始数据补全后的感知结果、机器人当前速度、目标点在机器人局部坐标系下的坐标位置。
29.步骤3中包括对于原始数据补全后的感知结果，具体包括融合历史观测数据与当前观测数据后的周围360
°
范围内障碍物的距离信息以及对应的时间信息；机器人当前速度包括差速机器人当前的线速度和角速度；坐标位置包括目标点在机器人局部坐标系下的坐标值组成的向量。
30.具体的，建立的观测空间ω包括机器人对于雷达获取的原始数据进行360
°
补全后的感知结果机器人当前速度目标点在机器人局部坐标系下的坐标位置其中，包括了融合历史雷达帧与当前雷达帧后的周围360
°
范围内障碍物的距离信息以及其对应的时间信息，包括了差速机器人当前的线速度和角速度，包括了目标点在机器人局部坐标系下的(x,y)坐标值组成的向量。
31.步骤3中具体包括了将原始雷达数据转化为代表机器人周围360
°
范围内观测感知结果的处理方法，如图1所示：
32.(1)将获取的原始雷达数据转化为极坐标系并进行归一化；
33.(2)建立存储一定帧数历史观测数据的队列和与历史观测数据相对应的机器人位姿的队列，位姿包含在世界坐标系下机器人的笛卡尔坐标系下的坐标以及机器人的偏航角；
34.(3)根据一定判定条件，更新历史观测数据队列；
35.具体的，处理方法(3)中的判定条件为：
36.每次将历史位姿队列中队尾的位姿与当前位姿比较，设置位置差允许范围的阈值以及偏航角的阈值，如果上一次更新时的位姿与当前位姿的距离差大于所述设定的阈值或者偏航角差大于所述设定的阈值，就将当前观测数据与当前位姿分别插入历史观测与历史位姿队列的队尾，将队列中处于队头的数据删除。
37.(4)根据之前获得的历史观测队列与机器人当前最新观测数据，将其融合成为一个可表示机器人周围360
°
范围内环境的新的感知结果，360
°
的感知结果中包括当前观测数据以及根据历史观测数据记忆下来的带有一定不确定性的“回忆观测数据”；所述“回忆观测数据”为根据历史观测数据记忆下来的带有一定不确定性的观测数据。并且根据历史队列中的数据与当前时刻的时间差的大小，分别为其赋予一维时间特征，这个特征代表机器人对于观测信息的置信程度。
38.处理方法(4)中对于历史观测队列与机器人当前最新观测数据的处理过程包括以下步骤：
39.将历史观测队列中观测数据统一转换到当前坐标系下，即先将历史观测数据由极坐标系转换到各自时刻的笛卡尔坐标系下，然后再将对应点转换到当前时刻的笛卡尔坐标系下，最后再进行一步笛卡尔坐标系到极坐标系的转换；
40.将当前观测数据与历史观测数据连接成一组完整观测；将360
°
的范围切分为n份，用观测数据的极角来计算其在360
°
范围内对应的id值；
41.对数据进行筛选后，根据观测数据与当前时刻的时间差的大小，赋予一维特征；然后将生成的360
°
范围的雷达数据与其对应的时间特征结合到一起返回。
42.上述步骤4的具体实施为，将原始雷达数据转化为代表机器人周围360
°
范围内观测感知结果的处理方法过程中，将传入的原始雷达数据转化为极坐标系并进行归一化，以下所述观测数据均为经过极坐标转化后的结果。
43.建立一个存储一定帧数历史观测数据的队列和一个与数据相对应的机器人位姿的队列，位姿包含在世界坐标系下机器人的笛卡尔坐标系下的(x，y)坐标以及机器人的偏航角yaw。
44.根据一定判定条件，更新历史观测数据队列。
45.所述的判定条件为：每次将历史位姿队列中队尾的(即上一次更新时插入的)位姿与当前位姿比较，设置一定的阈值dis
update
和theta
update
，如果上一次更新时的位姿与当前位姿的距离差大于dis
update
或者偏航角差大于theta
update
，那么就将当前观测数据与当前位姿分别插入历史观测与历史位姿队列队尾，将队列中处于队头的较旧的数据删除。
46.根据之前获得的历史观测队列与机器人当前最新观测数据，将其融合成为一个可表示机器人周围360
°
范围内环境的新的感知结果，360
°
的感知结果中包括了绝对正确的当前观测数据以及根据历史观测数据记忆下来的带有一定不确定性的“回忆观测数据”；并且根据历史队列中的数据与当前时刻的时间差的大小，分别为其赋予一维时间特征t_feature，这个特征同时也代表了机器人对于观测信息的置信程度。具体的处理过程为：
47.将历史观测队列中观测数据统一转换到当前坐标系下，由于历史观测数据均为极坐标系，而在极坐标系下无法直接进行旋转平移转换，所以先将历史观测数据由极坐标系转换到各自时刻的笛卡尔坐标系下，然后再将对应点转换到当前时刻的笛卡尔坐标系下，最后再进行一步笛卡尔坐标系到极坐标系的转换。
48.将当前观测数据与历史观测数据连接成一组完整观测，并根据以下公式，
49.id＝θ/(2π/n)
50.其中id代表某一观测数据在360
°
范围内对应的位置，n代表将360
°
范围切分的份数。
51.将360
°
的范围切分为n份，用观测数据的极角来计算其在360
°
范围内对应的i d值，也就是判断雷达点落在360
°
范围的这n份中的哪一部分。
52.由于历史观测数据与当前观测数据有一部分重叠，也就是说来自不同时间的观测数据可能会被赋予相同的id，所以对数据做进一步的筛选：
①
当前观测数据是完全确定的，所以全部保留；
②
历史观测数据如果对应id相同，则根据雷达点与机器人的远近，留下距离较近的数据。
③
最终得到筛选过后的代表机器人周围360
°
范围内障碍物情况的观测数据。
53.根据历史观测数据与当前时刻的时间差的大小，为其赋予一维特征，具体的，
①
离当前时刻由远及近，特征值从0到1.0递增；
②
未知区域(即历史数据也从未探测到的区域)的特征值为-1。
54.将生成的360
°
雷达数据与其对应的时间特征结合到一起返回。
55.在步骤2中，所述的建立的空间动作包括再连续空间内的一系列可达到的速度，差速机器人的速度包括了线速度和角速度，即
56.a
t
＝[v
t
,ω
t
]。
[0057]
基于观测和策略选择动作，机器人采取动作后与环境交互获取新的观测，并计算得到的奖励，将观测、动作、奖励、机器人的终止状态存储下来，以供样本采样训练；
[0058]
对机器人是否到达目标点建立奖励函数，建立的奖励函数设计为：
[0059][0060]
其中r
t
代表在时刻t接收的奖励函数，由和组成。
[0061]
具体的，
①
机器人到达目标会被奖励：
[0062][0063]
其中，r
arrival
为一个可调参数，代表当机器人到达目标点时对其的奖励；ωg也是一个可调参数，用来对机器人的相对目标点未知的靠近给出奖励；p
t-1
和p
t
分别表示机器人上一时刻与当前时刻的位置；g代表目标点的位置。
[0064]
②
当机器人和环境中的障碍物相撞了，被惩罚：
[0065][0066]
其中，r
collision
为一可调参数，代表当机器人发生碰撞后对其的惩罚值，p
t
表示机器人当前时刻的位置，bk表示障碍物k所处位置，当机器人与障碍物的位置距离小于r则判定其为碰撞状态。
[0067]
③
另外，加入一个主动避障的惩罚项：
[0068]
首先计算dis
ca
范围内所有雷达点xi的主动避障代价
[0069][0070]
然后将每个雷达点映射到-π/2～π/2的某个角度αi中：
[0071]
αi＝π*i/l
data-π/2
[0072][0073][0074]
l
data
即为雷达数据的长度，每个雷达数据xi代表传感器返回的该点的距离。coeff是一个可调的参数；cost
sum
就是一帧雷达总的代价，最终对于主动避障的惩罚就是cost
sum
取负值后的结果。
[0075]
④
为了鼓励机器人平滑移动，一个小的惩罚被用来惩罚大的旋转速度：
[0076][0077]
设置r
arrival
＝17，ωg＝2.7，r
collision
＝-17，dis
ca
＝0.2，coeff＝0.001，w
ω
＝-0.15。l
data
即为雷达数据的长度，每个雷达数据xi代表传感器返回的该点的距离。
[0078]
步骤4中提到的，利用新的感知结果在优化更新的策略函数网络中输出的概率分
布来选择机器人下一步的避障动作，直至到达目标点。
[0079]
具体的，建立机器人导航避障神经网络模型，如图2所示，融合后的雷达数据形状为2x1080，首先输入一个一维卷积层，其输出为32个通道，并使用大小为5*5的卷积核，然后再输入一个输出为32个通道的一维卷基层，卷积核大小为3*3；然后输入一个输出层大小为256的全连接层；接着将上一步的输出与另外两个输入机器人当前速度目标点在机器人局部坐标系下的坐标位置连接起来，然后一起输入一个输出层大小为128的全连接层；最后将上一步的输出分别输入到两个输出层大小为128的全连接层中得到一个大小为2的值再由两个值得到对应的从一个服从的正分布中采样得到最终的动作值a
t
。
[0080]
其中利用基于actor-critic框架的ppo强化学习算法，根据观测数据，在每一步执行actor网络产生的动作去和环境进行交互，得到环境反馈的奖励和新的状态，并把一定次数的交互数据保存下来，然后由critic网络用这些数据对动作进行评估得到动作价值，并根据获得的状态、奖励和动作价值为actor和critic网络进行参数更新；
[0081]
根据优化更新后的策略函数网络输出的概率分布选择动作。对多帧原始的雷达数据进行相应处理与融合，得到含有一定记忆功能的，可表示机器人周围360
°
范围内的障碍物距离信息的感知信息，使其具备全范围信息恢复能力，其中还包括了相关距离信息的时间特征，并将此感知信息送入深度学习网络中得到观测到动作的映射关系。
[0082]
具体的实施中，将当前帧雷达与融合历史帧感知结果的雷达可视化效果进行对比，如图3所示，过程为：
[0083]
将小车从位置
①
移动到位置
②
，此时处于位置
②
。图三中左图所示为原始的180
°
激光雷达数据可视化的效果，除小车模型外其余黑的点均为雷达数据点；右图所示为经过当前数据与历史帧数据融合后的可视化效果，在小车模型前方180
°
范围内的为当前帧数据，其余为在从位置
①
到位置
②
的过程中记忆下来的历史帧数据。可以看出，此时机器人不仅有当前可以观测到的数据，还有之前保留下来的历史数据可供其决策，提高了决策的安全性。
[0084]
本领域内的技术人员应明白，本公开的实施例可提供为方法、系统、或计算机程序产品。因此，本公开可采用完全硬件实施例、完全软件实施例、或结合软件和硬件方面的实施例的形式。而且，本公开可采用在一个或多个其中包含有计算机可用程序代码的计算机可用存储介质(包括但不限于磁盘存储器、cd-rom、光学存储器等)上实施的计算机程序产品的形式。
[0085]
本公开是参照根据本公开实施例的方法、设备(系统)、和计算机程序产品的流程图和/或方框图来描述的。应理解可由计算机程序指令实现流程图和/或方框图中的每一流程和/或方框、以及流程图和/或方框图中的流程和/或方框的结合。可提供这些计算机程序指令到通用计算机、专用计算机、嵌入式处理机或其他可编程数据处理设备的处理器以产生一个机器，使得通过计算机或其他可编程数据处理设备的处理器执行的指令产生用于实现在流程图一个流程或多个流程和/或方框图一个方框或多个方框中指定的功能的装置。
[0086]
这些计算机程序指令也可存储在能引导计算机或其他可编程数据处理设备以特
定方式工作的计算机可读存储器中，使得存储在该计算机可读存储器中的指令产生包括指令装置的制造品，该指令装置实现在流程图一个流程或多个流程和/或方框图一个方框或多个方框中指定的功能。
[0087]
这些计算机程序指令也可装载到计算机或其他可编程数据处理设备上，使得在计算机或其他可编程设备上执行一系列操作步骤以产生计算机实现的处理，从而在计算机或其他可编程设备上执行的指令提供用于实现在流程图一个流程或多个流程和/或方框图一个方框或多个方框中指定的功能的步骤。
[0088]
以上所述仅为本公开的优选实施例而已，并不用于限制本公开，对于本领域的技术人员来说，本公开可以有各种更改和变化。凡在本公开的精神和原则之内，所作的任何修改、等同替换、改进等，均应包含在本公开的保护范围之内。
[0089]
上述虽然结合附图对本公开的具体实施方式进行了描述，但并非对本公开保护范围的限制，所属领域技术人员应该明白，在本公开的技术方案的基础上，本领域技术人员不需要付出创造性劳动即可做出的各种修改或变形仍在本公开的保护范围以内。

技术特征：

1.一种具备全范围信息恢复能力的机器人导航避障方法，其特征在于，训练步骤包括：在仿真环境三维空间中建立机器人坐标系，并在一定区域内随机生成障碍物的位置、以及机器人的起始点和目标点；获取周围环境数据，基于状态空间、动作空间、奖励函数、状态转移概率、观测空间以及观测概率分布建立环境模型；获取原始雷达和当前雷达的观测数据，构建历史观测数据队列，将当前最新的雷达观测数据以及位姿插入历史队列中，使历史观测数据与当前观测数据融合，形成机器人周围360
°
范围内环境的新的观测感知结果；利用新的感知结果在优化更新的策略函数网络中输出的概率分布来选择机器人下一步的避障动作，直至到达目标点。2.如权利要求1所述的一种具备全范围信息恢复能力的机器人导航避障方法，其特征在于，建立的观测空间包括机器人对于原始数据补全后的感知结果、机器人当前速度、目标点在机器人局部坐标系下的坐标位置。3.如权利要求2所述的一种具备全范围信息恢复能力的机器人导航避障方法，其特征在于，所述对于原始数据补全后的感知结果包括融合历史观测数据与当前观测数据后的周围360
°
范围内障碍物的距离信息以及气对应的时间信息；机器人当前速度包括差速机器人当前的线速度和角速度；坐标位置包括目标点在机器人局部坐标系下的坐标值组成的向量。4.如权利要求1所述的一种具备全范围信息恢复能力的机器人导航避障方法，其特征在于，将原始雷达数据转化为代表机器人周围360
°
范围内观测感知结果的处理方法为：(1)将获取的原始雷达数据转化为极坐标系并进行归一化；(2)建立存储一定帧数历史观测数据的队列和与历史观测数据相对应的机器人位姿的队列，位姿包含在世界坐标系下机器人的笛卡尔坐标系下的坐标以及机器人的偏航角；(3)根据一定判定条件，更新历史观测数据队列；(4)根据之前获得的历史观测队列与机器人当前最新观测数据，将其融合成为一个可表示机器人周围360
°
范围内环境的新的感知结果，360
°
的感知结果中包括当前观测数据以及根据历史观测数据记忆下来的带有一定不确定性的“回忆观测数据”；并且根据历史队列中的数据与当前时刻的时间差的大小，分别为其赋予一维时间特征，这个特征代表机器人对于观测信息的置信程度。5.如权利要求4所述的一种具备全范围信息恢复能力的机器人导航避障方法，其特征在于，处理方法(3)中的判定条件为：每次将历史位姿队列中队尾的位姿与当前位姿比较，设置位置差允许范围的阈值以及偏航角的阈值，如果上一次更新时的位姿与当前位姿的距离差大于所述设定的阈值或者偏航角差大于所述设定的阈值，就将当前观测数据与当前位姿分别插入历史观测与历史位姿队列的队尾，将队列中处于队头的数据删除。6.如权利要求4所述的一种具备全范围信息恢复能力的机器人导航避障方法，其特征在于，处理方法(4)中对于历史观测队列与机器人当前最新观测数据的处理过程包括以下步骤：将历史观测队列中观测数据统一转换到当前坐标系下，即先将历史观测数据由极坐标
系转换到各自时刻的笛卡尔坐标系下，然后再将对应点转换到当前时刻的笛卡尔坐标系下，最后再进行一步笛卡尔坐标系到极坐标系的转换；将当前观测数据与历史观测数据连接成一组完整观测；将360
°
的范围切分为n份，用观测数据的极角来计算其在360
°
范围内对应的id值；对数据进行筛选后，根据观测数据与当前时刻的时间差的大小，赋予一维特征；然后将生成的360
°
范围的雷达数据与其对应的时间特征结合到一起返回。7.如权利要求1所述的一种具备全范围信息恢复能力的机器人导航避障方法，其特征在于，建立的动作空间包括再连续空间内的一系列可达到的速度，差速机器人的速度包括了线速度和角速度，即a
t
＝[v
t
,ω
t
]。8.如权利要求1所述的一种具备全范围信息恢复能力的机器人导航避障方法，其特征在于，对机器人是否到达目标点建立奖励函数，建立的奖励函数设计为：其中r
t
代表在时刻t接收的奖励函数，由和组成。9.如权利要求7所述的一种具备全范围信息恢复能力的机器人导航避障方法，其特征在于，具体的，机器人到达目标会被奖励：当机器人和环境中的障碍物相撞了，被惩罚：其中，r
collision
为一可调参数，代表当机器人发生碰撞后对其的惩罚值，p
t
表示机器人当前时刻的位置，b
k
表示障碍物k所处位置，当机器人与障碍物的位置距离小于r则判定其为碰撞状态。10.如权利要求7所述的一种具备全范围信息恢复能力的机器人导航避障方法，其特征在于，加入一个主动避障的惩罚项：首先计算dis
ca
范围内所有雷达点x
i
的主动避障代价的主动避障代价然后将每个雷达点映射到-π/2～π/2的某个角度α
i
中：α
i
＝π*i/l
data-π/2π/2
l
data
即为每帧雷达数据的长度，每个雷达数据x
i
代表传感器返回的该点的距离；coeff是一个可调的参数；cost
sum
就是一帧雷达总的代价，最终对于主动避障的惩罚就是cost
sum
取负值后的结果。

技术总结

本公开提供了一种具备全范围信息恢复能力的机器人导航避障方法，包括获取周围环境数据，获取原始雷达和当前雷达的观测数据，构建历史观测数据队列，将当前最新的雷达观测数据以及位姿插入历史队列中，使历史观测数据与当前观测数据融合，形成机器人周围360

技术研发人员：

张伟 曹睿 李晓磊 谢世宽 赵越男 李传鹏 李宇

受保护的技术使用者：

山东大学

技术研发日：

2022.07.21

技术公布日：

2022/10/11