基于环境可通过性分析的四足机器人路径规划方法及系统

技术领域

本公开涉及移动机器人环境感知技术与导航技术领域，具体涉及基于环境可通过性分析的四足机器人路径规划方法及系统。

背景技术

本部分的陈述仅仅是提供了与本公开相关的背景技术信息，不必然构成在先技术。

移动机器人从结构上来讲可以分为三大类-轮式机器人、腿足式机器人和履带式机器人。腿足式机器人相比于轮式机器人与履带式机器人，对崎岖地形的适应能力更强，可应用于复杂地形的物质运输、野外勘测、灾祸救援及高危环境作业。现有的路径规划方法大多针对轮式机器人和履带式机器人，对腿足式机器人研究很少。

面向崎岖复杂的野外环境，四足机器人的路径规划首要解决的是环境可通过性问题。针对腿足式机器人可通过性地图的构建，发表于IROS上的2009年论文《Stereo Camer aBased Navigation of Mobile Robots on Rough Terrain》、2016年《NavigationPlanning for Legged Robots in Challenging Terrain》均利用几何特征-坡度、粗糙度和高度差以构建可通过性地图；针对路径规划问题，采用基于图搜索或基于采样的路径规划算法实现。

但是发明人发现，以上方法存在的问题主要有：

(1)维数较少的几何特征对于非结构化环境的特征不能很好的表征，存在信息丢失问题；

(2)现有方法的距离传感器大多使用机械激光雷达或深度相机，前者存在的弊端是点云相对稀疏，环境特征易丢失，后者结合ORB－SLAM2算法可构建稠密点云地图，但深度相机在室外工作易受强光影响存在过曝问题且夜晚丧失工作能力，实际应用存在一定的局限性；

(3)针对地图的可通过性估计，现有方法大多基于栅格地图或可占据栅格地图进行分析，但地图的生成没有考虑地形的复杂度和机器人本身的越障能力，且可通过性没有考虑障碍物的高度梯度值，比如台阶、沟壑等地形多被划分为不可通行区域，而这些区域并非四足机器人的不可通行区域；针对路径规划方法，基本采用基于图搜索和采样的算法实现，但路径生成的过程对复杂地形的可通过性分析不够充分；

(4)针对稠密点云地图的运算量大问题，现有的一些方法采用增强GPU性能处理，但是所需的成本太高。

发明内容

本公开为了解决上述问题，提出了基于环境可通过性分析的四足机器人路径规划方法及系统，使用固态激光雷达提供稠密点云，构建局部稠密高程地图；提出基于四维几何特征的地图可通过性判断方法及基于TM-RRT*算法的路径规划方法实现四足机器人的自主路径导航。

根据一些实施例，本公开采用如下技术方案：

基于环境可通过性分析的四足机器人路径规划方法，包括：

获取四足机器人的实时稠密点云数据以及位姿信息，并对所有数据预处理；

基于预处理后的稠密点云数据以及位姿信息构建始终以四足机器人为中心的局部栅格高程地图；

将地图每个栅格的表面法向量和高度差作为局部区域的几何特征评估每个栅格的可通过性，获取每个栅格的可通过值；

将每个栅格的可通过值映射为占用概率值，获取以四足机器人为中心的可通过性地图，基于可通过性地图，利用可通过性地图的快速搜索随机树算法实现对四足机器人的路径规划，获取一条在起始位置与期望位置之间的无碰撞路径，基于P ID控制跟踪方法控制四足机器人自主导航运动。

根据一些实施例，本公开采用如下技术方案：

基于环境可通过性分析的四足机器人路径规划系统，包括：

数据获取模块，用于获取四足机器人的实时稠密点云数据以及位姿信息，并对所有数据预处理；

高程地图构建模块，用于基于预处理后的稠密点云数据以及位姿信息构建始终以四足机器人为中心的局部栅格高程地图；

可通过性判断模块，用于将地图每个栅格的表面法向量和高度差作为局部区域的几何特征评估每个栅格的可通过性，获取每个栅格的可通过值；

将每个栅格的可通过值映射为占用概率值，获取以四足机器人为中心的可通过性地图；

路径规划模块，用于基于可通过性地图，利用可通过性地图的快速搜索随机树算法实现对四足机器人的路径规划，获取一条在起始位置与期望位置之间的无碰撞路径；

控制运动模块，用于基于P ID控制跟踪方法控制四足机器人自主导航运动。

根据一些实施例，本公开采用如下技术方案：

一种非暂态计算机可读存储介质，所述非暂态计算机可读存储介质用于存储计算机指令，所述计算机指令被处理器执行时，实现所述的基于环境可通过性分析的四足机器人路径规划方法。

根据一些实施例，本公开采用如下技术方案：

一种电子设备，包括：处理器、存储器以及计算机程序；其中，处理器与存储器连接，计算机程序被存储在存储器中，当电子设备运行时，所述处理器执行所述存储器存储的计算机程序，以使电子设备执行实现所述的基于环境可通过性分析的四足机器人路径规划方法。

与现有技术相比，本公开的有益效果为：

本公开提供了一种基于环境可通过性分析的四足机器人路径规划方法，使用固态激光雷达提供稠密点云，为了减轻处理器的运算负担同时保持点云的形状特征，对实时稠密点云进行预处理，构建局部稠密高程地图，去除了干扰产生噪声以及减少了点云运算时间。

附图说明

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

图1为本公开实施例的系统整体框架图；

图2为本公开实施例的构建的坐标系示意图；

图3为本公开实施例的可通过性地图的生成过程示意图；

图4为本公开实施例的相邻八邻域栅格地图示意图。

具体实施方式

下面结合附图与实施例对本公开作进一步说明。

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

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

实施例1

本公开的一种实施例中提供了一种基于环境可通过性分析的四足机器人路径规划方法，具体包括以下步骤：

步骤一：获取四足机器人的实时稠密点云数据以及位姿信息，并对所有数据预处理；

步骤二：基于预处理后的稠密点云数据以及位姿信息构建始终以四足机器人为中心的局部栅格高程地图；

步骤三：将地图每个栅格的表面法向量和高度差作为局部区域的几何特征评估每个栅格的可通过性，获取每个栅格的可通过值；

步骤四：将每个栅格的可通过值映射为占用概率值，获取以四足机器人为中心的可通过性地图；

步骤五：基于可通过性地图，利用可通过性地图的快速搜索随机树算法实现对四足机器人的路径规划，获取一条在起始位置与期望位置之间的无碰撞路径；

步骤六：基于P ID控制跟踪方法控制四足机器人自主导航运动。

作为一种实施例，本公开的一种基于环境可通过性分析的四足机器人路径规划方法的具体实施方式如下：

步骤1：使用传感系统实时稠密点云数据以及位姿信息，所述传感系统包括固态激光雷达以及惯导系统，使用固态激光雷达获取稠密点云，使用惯导系统获取四足机器人位姿信息；

为了减轻处理器的运算负担同时保持点云的形状特征，对实时稠密点云以及位姿信息进行预处理，主要包括：1)离群点滤波，以去除干扰产生噪声；2)体素格滤波，以减少点云运算时间。

步骤2：基于预处理后的稠密点云数据以及位姿信息构建始终以四足机器人为中心的局部栅格高程地图，具体的：

(1)坐标系定义

如图2所示，该方法共有4个坐标系：惯性坐标系W，机器人机身坐标系B，传感器坐标系S和地图坐标系M。地图坐标系通过传感器坐标系S定义。

(2)建图过程

首先进行数据收集，使用预处理后的三维点云和四足机器人的位姿实时更新机器人运动时的地图；

其次是数据处理：将得到的高程图数据中每个栅格单元i的数据

高程图数据包括平均高度估计

基于空间协方差

步骤3：将地图每个栅格的表面法向量和高度差作为局部区域的几何特征评估每个栅格的可通过性，获取每个栅格的可通过值，包括：当获得高程地图后，使用RANSAC算法拟合一个平面S

(1)计算地图的可通过性

当获取含高程图数据

其中，

通过奇异值分解，可以获得矩阵A

s(x,y)＝a cos(n

其中，s为坡度值；r为粗糙度；c为曲率。

以图4相邻八邻域栅格地图为例，高度差为

结合4维几何特征使用可行域评估公式：

获取每个栅格可通过值，其中(x,y)表示栅格所在位置。w

(2)将每个栅格的可通过值映射为占用概率值，获取以四足机器人为中心的可通过性地图，包括：通过设置四足机器人可通过值的最小和最大阈值将每个栅格的可通过值映射为占用概率值，根据每个栅格的占用概率值和分辨率，结合四足机器人本身的外形尺寸，考虑四足机器人的构型空间，对地图作膨胀处理，结合四足机器人对崎岖环境的自适应通过能力，对环境的安全区域和危险区域加以定义，获取以四足机器人为中心的可通过性地图。具体的：

通过式(3-7)将每个栅格的可通过值映射为占用概率值，其中data

当获得以四足机器人为中心的可通过性地图(OccupancyGrid)M

对M

结合四足机器人本身的外形尺寸，考虑机器人的构型空间(C-space)，对地图作膨胀处理，

M'

其中，Size(l,w)为膨胀尺寸。

结合四足机器人对崎岖环境的自适应通过能力，对环境的安全区域(可通过)和危险区域(不可通过)加以定义：

以图4相邻八邻域栅格地图为例，occVal

如果slopedgt

步骤4：基于可通过性地图，利用基于可通过性地图的快速搜索随机树算法(TM-RRT*，traversable map-based RRT*)实现对四足机器人的路径规划，获取一条在起始位置与期望位置之间的无碰撞路径，包括：

针对地图的可通过性，提出了TM-RRT*算法。相对于传统的RRT*算法，针对可占据栅格地图的可通过性生成新节点X

Step1:初始化随机数T，将起始位置设为随机树的根节点进行构造；

Step2：在地图M'

Step3:将Step2中得到的最近节点X

定义X

则定义步长

则朝X

因此可得节点X

其中，pixelVcur为当前点的像素值(单位：像素)，pixelVpre为上一个点的像素值(单位：像素)。

根据式(3-10)得

Step4:以X

Step5:重复进行上述步骤，直到能够在随机树中搜索到目标位置，从而得到一条起始位置与期望位置之间的无碰撞路径。

步骤5：基于PID控制跟踪方法控制四足机器人自主导航运动。

为了使四足机器人拟合规划好的路径，考虑到四足机器人的运动控制策略，提出基于PID的控制跟踪方法，发送速度控制命令给运动控制器：

其中，w为机器人运动的角速度，k

实施例2

本公开的一种实施例中提供了一种基于环境可通过性分析的四足机器人路径规划系统，包括：

数据获取模块，用于获取四足机器人的实时稠密点云数据以及位姿信息，并对所有数据预处理；

高程地图构建模块，用于基于预处理后的稠密点云数据以及位姿信息构建始终以四足机器人为中心的局部栅格高程地图；

可通过性判断模块，用于将地图每个栅格的表面法向量和高度差作为局部区域的几何特征评估每个栅格的可通过性，获取每个栅格的可通过值；

将每个栅格的可通过值映射为占用概率值，获取以四足机器人为中心的可通过性地图；

路径规划模块，用于基于可通过性地图，利用可通过性地图的快速搜索随机树算法实现对四足机器人的路径规划，获取一条在起始位置与期望位置之间的无碰撞路径；

控制运动模块，用于基于P ID控制跟踪方法控制四足机器人自主导航运动。

实施例3

本公开的一种实施例中提供了一种非暂态计算机可读存储介质，所述非暂态计算机可读存储介质用于存储计算机指令，所述计算机指令被处理器执行时，实现所述的基于环境可通过性分析的四足机器人路径规划方法。

实施例4

本公开的一种实施例中提供了一种电子设备，包括：处理器、存储器以及计算机程序；其中，处理器与存储器连接，计算机程序被存储在存储器中，当电子设备运行时，所述处理器执行所述存储器存储的计算机程序，以使电子设备执行实现所述的基于环境可通过性分析的四足机器人路径规划方法。

本公开是参照根据本公开实施例的方法、设备(系统)、和计算机程序产品的流程图和/或方框图来描述的。应理解可由计算机程序指令实现流程图和/或方框图中的每一流程和/或方框、以及流程图和/或方框图中的流程和/或方框的结合。可提供这些计算机程序指令到通用计算机、专用计算机、嵌入式处理机或其他可编程数据处理设备的处理器以产生一个机器，使得通过计算机或其他可编程数据处理设备的处理器执行的指令产生用于实现在流程图一个流程或多个流程和/或方框图一个方框或多个方框中指定的功能的装置。

这些计算机程序指令也可装载到计算机或其他可编程数据处理设备上，使得在计算机或其他可编程设备上执行一系列操作步骤以产生计算机实现的处理，从而在计算机或其他可编程设备上执行的指令提供用于实现在流程图一个流程或多个流程和/或方框图一个方框或多个方框中指定的功能的步骤。

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