CN117111617A - 考虑感知盲区碰撞不确定性的机器人路径规划方法及系统 - Google Patents

Abstract

本发明公开了一种考虑感知盲区碰撞不确定性的机器人路径规划方法及系统，涉及路径规划技术领域，该方法包括：获取机器人的起始位置和目标位置信息，规划全局路径，机器人沿全局路径进行导航与运动；在机器人运动过程中，获取机器人当前位置信息、速度信息和周围环境点云信息，进行实时局部路径规划；基于采样速度生成若干条预测轨迹，结合机器人当前周围环境点云信息，采用改进的评价函数对每条预测轨迹进行评价，通过遍历选择最优预测轨迹，根据最优预测轨迹确定对应的速度数据，控制机器人的局部运动。本发明改进评价函数，通过修正的避障函数和新增的震荡函数对预测轨迹进行修正，降低机器人在已探测区域与盲区交接处的碰撞风险。

Description

考虑感知盲区碰撞不确定性的机器人路径规划方法及系统

技术领域

本发明涉及路径规划技术领域，尤其涉及一种考虑感知盲区碰撞不确定性的机器人路径规划方法及系统。

背景技术

随着自主机器人相关技术的发展，具备自主导航功能的行走机器人被广泛应用于智能物流、智能家居、紧急救援等实际场景中。目前可通过多种算法实现机器人的全局路径规划，即规划出一条从起始位置到目标位置的路径，然而，当机器人在执行具体任务时，环境中的各种动态因素会影响或限制机器人的稳定运行，此时需要腿足机器人搭载传感器（如激光雷达、摄像头等）进行环境感知，实时获取周围障碍物的位置信息用以自主导航与避障。即，现有机器人一般是以全局路径规划作为指引进行局部路径规划，以此实现自主导航和避障。

然而，在某些特殊工作场景中，机器人上的机载传感器视场受到诸如走廊拐角及障碍物遮挡等无法感知后方区域的限制，导致机器人载体无法获取盲区中障碍物的确定性位置。因此，当机器人运动至已探测区域与盲区交接处时，感知盲区中障碍物位置的不确定性会给机器人带来无法评估的碰撞风险。传统的局部路径规划算法，如动态窗口算法(Dynamic Window Approach,DWA)、人工势场法等，在已知环境中躲避动静态障碍物具有较高的灵活性，但这一类方法基于已探明的障碍分布及可评估的碰撞系数，仅是在机器人当前所处的已知环境中预测下一时刻的运动轨迹，以此实现避障；但是，当机器人运动到已探测区域和被遮挡的感知盲区交接处时，由于盲区中障碍物位置的不确定性，传统的局部路径规划算法计算的预测轨迹容易因突然出现的障碍物失效，机器人无法及时规避盲区中突然出现的障碍物，导致碰撞风险上升。

发明内容

为解决上述现有技术的不足，本发明提供了一种考虑感知盲区碰撞不确定性的机器人路径规划方法及系统，解决上述腿足机器人在感知盲区中因障碍物位置的不确定性导致碰撞风险上升的问题，通过在机器人局部运动路径的规划过程中同等考虑已知障碍物以及被遮挡的感知盲区，当机器人运动至已探测区域和盲区交接处时，通过提高感知盲区的代价系数，对局部规划路径进行修正，使机器人能够规避感知盲区中的潜在障碍物，得到预测避障的无碰撞轨迹，在感知盲区中具有灵活性高、适应性好、抗干扰能力强的优点。

第一方面，本公开提供了一种考虑感知盲区碰撞不确定性的机器人路径规划方法。

一种考虑感知盲区碰撞不确定性的机器人路径规划方法，包括：

获取机器人的起始位置信息和目标位置信息，规划全局路径，机器人沿规划的全局路径进行导航与运动；

在机器人运动过程中，获取机器人当前的位置信息、速度信息和周围环境点云信息，进行实时局部路径规划，模拟生成最优运动轨迹，根据最优运动轨迹确定对应的速度数据，控制机器人的局部运动；

其中，在局部路径规划的过程中，基于机器人当前的速度信息，进行速度采样，生成多组速度组合，基于多组速度组合对应生成若干条预测轨迹，结合机器人当前的周围环境点云信息，采用改进的评价函数对每条预测轨迹进行评价，通过遍历选择最优预测轨迹，以此作为最优运动轨迹。

第二方面，本公开提供了一种考虑感知盲区碰撞不确定性的机器人路径规划系统。

一种考虑感知盲区碰撞不确定性的机器人路径规划系统，包括：

全局路径规划模块，用于获取机器人的起始位置信息和目标位置信息，规划全局路径，机器人沿规划的全局路径进行导航与运动；

局部路径规划模块，用于在机器人运动过程中，获取机器人当前的位置信息、速度信息和周围环境点云信息，进行实时局部路径规划，模拟生成最优运动轨迹，根据最优运动轨迹确定对应的速度数据，控制机器人的局部运动；

其中，在局部路径规划的过程中，基于机器人当前的速度信息，进行速度采样，生成多组速度组合，基于多组速度组合对应生成若干条预测轨迹，结合机器人当前的周围环境点云信息，采用改进的评价函数对每条预测轨迹进行评价，通过遍历选择最优预测轨迹，以此作为最优运动轨迹。

以上一个或多个技术方案存在以下有益效果：

1、本发明提供了一种考虑感知盲区碰撞不确定性的机器人路径规划方法及系统，在机器人局部运动路径的规划过程中同等考虑已知障碍物以及被遮挡的感知盲区，当机器人运动至已探测区域和盲区交接处时，通过提高感知盲区的代价系数，对局部规划路径进行修正，使机器人能够规避感知盲区中的潜在障碍物，得到预测避障的无碰撞轨迹，在感知盲区中具有灵活性高、适应性好、抗干扰能力强的优点。

2、本发明通过改进的基于预测控制理论的次优方法，根据腿足机器人当前环境推算实时运动轨迹，通过修正的避障函数和新增的震荡函数对机器人的预测轨迹进行修正，降低机器人在运动至已探测区域与盲区交接处时的碰撞风险，实现腿足机器人在障碍物位置不确定性环境中的自主导航与预测避障。

附图说明

构成本发明的一部分的说明书附图用来提供对本发明的进一步理解，本发明的示意性实施例及其说明用于解释本发明，并不构成对本发明的不当限定。

图1为本发明实施例所述考虑感知盲区碰撞不确定性的机器人路径规划方法的流程图；

图2为本发明实施例所述考虑感知盲区碰撞不确定性的机器人路径规划方法的具体流程框图；

图3为本发明实施例中机器人在走廊拐角的感知盲区中预测避障的场景模式示意图。

具体实施方式

应该指出，以下详细说明都是示例性的，旨在对本发明提供进一步的说明。除非另有指明，本文使用的所有技术和科学术语具有与本发明所属技术领域的普通技术人员通常理解的相同含义。

需要注意的是，这里所使用的术语仅是为了描述具体实施方式，而非意图限制根据本发明的示例性实施方式。如在这里所使用的，除非上下文另外明确指出，否则单数形式也意图包括复数形式，此外，还应当理解的是，当在本说明书中使用术语“包含”和/或“包括”时，其指明存在特征、步骤、操作、器件、组件和/或它们的组合。

实施例一

本实施例提供了一种考虑感知盲区碰撞不确定性的机器人路径规划方法，利用激光雷达感知周围环境，构建全局地图并规划全局路径，在获取全局路径规划的引导路径后，采用改进的基于预测控制理论的次优方法进行局部路径规划，具体的，当腿足机器人沿全局路径规划的引导路径运动到已探测区域与感知盲区交接处时，通过修正次优方法的评价函数中避障函数及其权重，提高被遮挡区域（即感知盲区）的代价系数，推算腿足机器人运动到感知盲区和障碍物区域之间的已知区域的轨迹；同时在该评价函数中增加震荡函数，将全局地图栅格化，每个栅格有1个记录该推算轨迹与障碍物区域和感知盲区的靠近程度的代价值，通过评价函数确定该推算轨迹扫过的所有栅格的总代价值，以此进行轨迹评价，通过这一新增的震荡函数，能够实现对路径相似但速度不同的轨迹的评分；最后，根据评分选出评价最优的轨迹，以该最优轨迹所对应的速度作为移动机器人运动速度，帮助机器人躲避潜在障碍物，实现机器人更安全、碰撞风险更低的局部路径规划。

本实施例所提出的考虑感知盲区碰撞不确定性的机器人路径规划方法，如图1所示，包括：

获取机器人的起始位置信息和目标位置信息，规划全局路径，机器人沿规划的全局路径进行导航与运动；

在机器人运动过程中，获取机器人当前的位置信息、速度信息和周围环境点云信息，进行实时局部路径规划，模拟生成最优运动轨迹，根据最优运动轨迹确定对应的速度数据，控制机器人的局部运动；

其中，在局部路径规划的过程中，基于机器人当前的速度信息，进行速度采样，生成多组速度组合，基于多组速度组合对应生成若干条预测轨迹，结合机器人当前的周围环境点云信息，采用改进的评价函数对每条预测轨迹进行评价，通过遍历选择最优预测轨迹，以此作为最优运动轨迹。

在本实施例中，首先在机器人运动的起始阶段，获取机器人的起始位置信息和目标位置信息，利用如A*算法等多种算法进行全局路径规划，机器人沿规划的全局路径进行导航和运动。而在机器人运动过程中，已知定位信息的腿足机器人利用激光雷达获取当前的周围环境点云信息，同时该腿足机器人获取自身的状态信息（包括腿足机器人的位姿信息和速度信息），以此进行实时局部路径规划以及发布速度。

在本实施例中，根据激光雷达的点云可见性信息，将机器人所在环境分为可通行的安全区域、无法通行的障碍物区域以及无法感知探测障碍物位置的不确定性区域（即感知盲区）。

在机器人局部路径规划的过程中，采用改进的基于预测控制理论的次优方法，对机器人的实时运动轨迹进行预测和修正，以此实现局部路径规划。在本实施例中，首先在局部路径规划的过程中，仅考虑腿足机器人的前进和旋转方向，其控制信号为线速度v和角速度ω。基于上述获取的机器人速度信息，并根据腿足机器人的运动模型和机械特性的约束，构建速度采样空间，对机器人的线速度v和角速度ω进行采样，生成多组速度组合(v,ω)，由一系列的选定速度的动作生成模拟轨迹。实际上，每组速度组合相当于一条预测轨迹，之后基于改进后的评价函数对生成的预测轨迹进行评价，获得评分最高的预测轨迹，执行最优速度。

具体的，在任意t时刻，机器人的速度信息(v,ω)在二维空间中可以形成无穷多组运动轨迹，即构成一个初始的速度搜索空间，但是受腿足机器人的运动学约束和环境约束等，如机器人本身的速度极限、机器人驱动的电机性能及摩擦影响（即匀加速、减速运动的启停动作）、避障（需要保证以最大加速度能在最近的障碍物前停下来）等，可以使采样速度缩小至更加精确的区域，此时获取采样速度，为后续局部路径规划提供预测轨迹。

通常，利用传统的基于预测控制理论的次优方法进行局部路径规划，传统的基于预测控制理论的次优方法对速度组合(v,ω)设计3种约束，得到采样窗口，根据采样的速度组合对应生成若干条预测轨迹，之后，通过评价函数对每条预测轨迹进行评分，通过遍历所有的预测轨迹，确定最高分的预测轨迹，输出最高分轨迹对应的速度组合，以此控制机器人的局部运动。其中，传统的评价函数如公式1-1所示：

（1-1）

上式中，为方位角评价函数，/>为避障函数，/>为速度评价函数，α、β、γ为各评价函数的权重，σ表示归一化。在对预测轨迹进行评价时，若预测轨迹遇到障碍物，由于上述评价函数中设有避障函数，基于该避障函数对预测轨迹距离障碍物的距离进行评价，同时结合对预测轨迹方位角和速度的评价，通过遍历选出最优的轨迹并记录位置和速度；反之，则基于对预测轨迹方位角和速度的评价，通过遍历选出最优的轨迹并记录位置和速度。

由于上述传统方法只能根据评价指标选择在某一时刻最优的轨迹，缺乏应对复杂场景的能力，存在评价函数机制不合理、机器人易陷入局部最优等问题。当机器人运动至已探测区域（包括安全区域和障碍物区域）与盲区（即不确定性区域）交接处时，盲区中障碍物的不确定性给机器人带来碰撞风险。针对感知盲区中障碍物位置不确定性对腿足机器人运动规划的影响，对传统的基于预测控制理论的次优方法进行改进。如图2所示，当机器人运动至已探测区域与盲区的交接处时，即机器人所在环境存在不确定性区域时，采用改进的基于预测控制理论的次优方法，对机器人的实时运动轨迹进行预测和修正，通过评价预测轨迹来选出最优轨迹，以此实现局部路径规划。

上述改进后的评价函数如公式1-2所示：

（1-2）

上式中，为修正的避障函数，/>为新增的震荡函数，Ω为评价函数（即震荡函数）的权重；/>为方位角评价函数，/>为速度评价函数，α、β、γ为各对应的评价函数的权重，σ表示归一化。

其中，修正的避障函数为：

（1-3）

上式中，表示从起点至位置o处累计时间间隔/>的个数，/>为起点至/>个时间间隔后机器人位置，/>为障碍物区域或感知盲区，/>为预测轨迹上起点至/>个时间间隔后机器人位置与障碍物的最短距离，/>为预设的安全半径；/>为障碍物距离上限；/>为该预测轨迹被舍弃，/>为取最小值函数；其中，时间间隔个数的设置与离开起点的距离有关，先确定一个移出距离/>，根据机器人当前线速度/>估算时间间隔个数，即/>为整数，计算公式为：

（1-4）

其中，为取整函数。

修正后的避障函数能够提高感知盲区的代价系数，除了原始评价轨迹的功能外，还考察每条轨迹从起点至/>个时间间隔后的位置这两者之间的部分。具体的，当预测轨迹（即速度组合）距离障碍物区域或感知盲区/>的最短距离小于预设的安全半径时，该轨迹因存在碰撞风险而被舍弃；当预测轨迹距离障碍物区域或感知盲区/>的最短距离大于等于预设的安全半径时，则将最短距离与障碍物距离上限/>进行比较，选择远离障碍物的预测轨迹，即该避障函数倾向于远离障碍物的轨迹。

同时，将全局地图栅格化，每个栅格有1个记录该预测轨迹（即速度组合）与障碍物区域或感知盲区的靠近程度的代价值，新增震荡函数，依据预测轨迹扫过的所有栅格的总代价值，进行评分。新增/>震荡函数如公式1-5所示：

（1-5）

上式中，为预测轨迹扫过所有栅格代价值之和，σ表示归一化。

当多条预测轨迹中存在路径相似但速度不同的两条轨迹时，采用新增的震荡函数，对这两条预测轨迹进行评分，选择速度更优的预测轨迹，以该更优预测轨迹所对应的速度作为机器人的运动速度，控制机器人运行。

即，采用改进的评价函数对每条预测轨迹进行评价，舍弃距离障碍物最短距离小于预设安全半径的轨迹，推算机器人运动到感知盲区和障碍物区域之间的预测轨迹；并根据预测轨迹扫过全局地图所有栅格的总代价值进行评分，选择评分更高的预测轨迹为最优预测轨迹。其中，每个栅格中记录预测轨迹与障碍物区域和感知盲区的靠近程度的代价值。

最后，采用改进后的评价函数，调整评价函数的权重，选出评价最优的运动轨迹，该最优运动轨迹所对应的速度数据（包括线速度和角速度）来作为移动机器人运动速度，帮助机器人逃离局部最优，增强该方法在障碍物位置不确定性环境下的路径规划能力，帮助腿足机器人躲避潜在障碍物。

如图3所示，当机器人到达已探测区域和感知盲区交接处时，通过传统方法和本实施例所述方法获取局部规划路径，基于传统方法获取的路径或轨迹为轨迹1，而基于本实施例所述方法获取的路径或轨迹为轨迹2，感知盲区中存在障碍物，明显的，轨迹1存在碰撞风险，轨迹2能够提高机器人的可见度，降低碰撞风险。

本实例通过改进的基于预测控制理论的次优方法，根据腿足机器人当前环境推算实时运动轨迹，通过修正的避障函数和新增的震荡函数对机器人的预测轨迹进行修正，降低机器人在运动至已探测区域与盲区交接处时的碰撞风险，实现腿足机器人在障碍物位置不确定性环境中的自主导航与预测避障。

实施例二

本实施例提供了一种考虑感知盲区碰撞不确定性的机器人路径规划系统，包括：

全局路径规划模块，用于获取机器人的起始位置信息和目标位置信息，规划全局路径，机器人沿规划的全局路径进行导航与运动；

局部路径规划模块，用于在机器人运动过程中，获取机器人当前的位置信息、速度信息和周围环境点云信息，进行实时局部路径规划，模拟生成最优运动轨迹，根据最优运动轨迹确定对应的速度数据，控制机器人的局部运动；

其中，在局部路径规划的过程中，基于机器人当前的速度信息，进行速度采样，生成多组速度组合，基于多组速度组合对应生成若干条预测轨迹，结合机器人当前的周围环境点云信息，采用改进的评价函数对每条预测轨迹进行评价，通过遍历选择最优预测轨迹，以此作为最优运动轨迹。

以上实施例二中涉及的各步骤与方法实施例一相对应，具体实施方式可参见实施例一的相关说明部分。

以上所述仅为本发明的优选实施例而已，并不用于限制本发明，对于本领域的技术人员来说，本发明可以有各种更改和变化。凡在本发明的精神和原则之内，所作的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。

上述虽然结合附图对本发明的具体实施方式进行了描述，但并非对本发明保护范围的限制，所属领域技术人员应该明白，在本发明的技术方案的基础上，本领域技术人员不需要付出创造性劳动即可做出的各种修改或变形仍在本发明的保护范围以内。

Claims (10)

1.一种考虑感知盲区碰撞不确定性的机器人路径规划方法，其特征是，包括：

获取机器人的起始位置信息和目标位置信息，规划全局路径，机器人沿规划的全局路径进行导航与运动；

在机器人运动过程中，获取机器人当前的位置信息、速度信息和周围环境点云信息，进行实时局部路径规划，模拟生成最优运动轨迹，根据最优运动轨迹确定对应的速度数据，控制机器人的局部运动；

其中，在局部路径规划的过程中，基于机器人当前的速度信息，进行速度采样，生成多组速度组合，基于多组速度组合对应生成若干条预测轨迹，结合机器人当前的周围环境点云信息，采用改进的评价函数对每条预测轨迹进行评价，通过遍历选择最优预测轨迹，以此作为最优运动轨迹。

2.如权利要求1所述的考虑感知盲区碰撞不确定性的机器人路径规划方法，其特征是，所述改进的评价函数包括速度评价函数、方位角评价函数、修正的避障函数和新增的震荡函数。

3.如权利要求2所述的考虑感知盲区碰撞不确定性的机器人路径规划方法，其特征是，基于修正的避障函数对每条预测轨迹进行评价，包括：

当预测轨迹距离障碍物区域或感知盲区的最短距离小于预设的安全半径时，舍弃该预测轨迹；

当预测轨迹距离障碍物区域或感知盲区的最短距离大于等于预设的安全半径时，则将最短距离与障碍物距离上限进行比较，选择远离障碍物的预测轨迹。

4.如权利要求3所述的考虑感知盲区碰撞不确定性的机器人路径规划方法，其特征是，所述修正的避障函数为：

其中，表示从起点至位置o处累计时间间隔/>的个数，/>为起点至/>个时间间隔后机器人位置，/>为障碍物区域或感知盲区，/>为预测轨迹上起点至/>个时间间隔后机器人位置与障碍物的最短距离，/>为预设的安全半径；/>为障碍物距离上限；/>为该预测轨迹被舍弃，/>为取最小值函数，v表示机器人的线速度，ω表示机器人的角速度。

5.如权利要求2所述的考虑感知盲区碰撞不确定性的机器人路径规划方法，其特征是，基于新增的震荡函数对每条预测轨迹进行评价，包括：

根据预测轨迹扫过全局地图所有栅格的总代价值进行评分，选择评分更高的预测轨迹为最优预测轨迹。

6.如权利要求5所述的考虑感知盲区碰撞不确定性的机器人路径规划方法，其特征是，所述新增的震荡函数为：

其中，为预测轨迹扫过所有栅格的代价值之和，σ表示归一化，v表示机器人的线速度，ω表示机器人的角速度。

7.一种考虑感知盲区碰撞不确定性的机器人路径规划系统，其特征是，包括：

全局路径规划模块，用于获取机器人的起始位置信息和目标位置信息，规划全局路径，机器人沿规划的全局路径进行导航与运动；

局部路径规划模块，用于在机器人运动过程中，获取机器人当前的位置信息、速度信息和周围环境点云信息，进行实时局部路径规划，模拟生成最优运动轨迹，根据最优运动轨迹确定对应的速度数据，控制机器人的局部运动；

其中，在局部路径规划的过程中，基于机器人当前的速度信息，进行速度采样，生成多组速度组合，基于多组速度组合对应生成若干条预测轨迹，结合机器人当前的周围环境点云信息，采用改进的评价函数对每条预测轨迹进行评价，通过遍历选择最优预测轨迹，以此作为最优运动轨迹。

8.如权利要求7所述的考虑感知盲区碰撞不确定性的机器人路径规划系统，其特征是，所述改进的评价函数包括速度评价函数、方位角评价函数、修正的避障函数和新增的震荡函数。

9.如权利要求8所述的考虑感知盲区碰撞不确定性的机器人路径规划系统，其特征是，基于修正的避障函数对每条预测轨迹进行评价，包括：

当预测轨迹距离障碍物区域或感知盲区的最短距离小于预设的安全半径时，舍弃该预测轨迹；

当预测轨迹距离障碍物区域或感知盲区的最短距离大于等于预设的安全半径时，则将最短距离与障碍物距离上限进行比较，选择远离障碍物的预测轨迹。

10.如权利要求8所述的考虑感知盲区碰撞不确定性的机器人路径规划系统，其特征是，基于新增的震荡函数对每条预测轨迹进行评价，包括：

根据预测轨迹扫过全局地图所有栅格的总代价值进行评分，选择评分更高的预测轨迹为最优预测轨迹。