CN110361026B - 一种基于3d点云的仿人机器人路径规划方法 - Google Patents

Abstract

本发明公开了一种基于3D点云的仿人机器人路径规划方法，包括步骤：1)使用激光雷达感知地形，建立点云地图；2)在点云地图的基础上，计算地形环境信息；3)设计自适应步态集合；4)根据不同的地形环境自适应地选择不同的自适应步态集合搜索路径。本发明可有效解决仿人机器人在复杂环境下的路径规划问题，且在仿真平台和实体机器人上进行了测试，验证了此方法的有效性。

Description

一种基于3D点云的仿人机器人路径规划方法

技术领域

本发明涉及仿人机器人的技术领域，尤其是指一种基于3D点云的仿人机器人路径规划方法。

背景技术

仿人机器人因为其独特的外形结构和灵活强大的功能一直是智能机器人领域中热门的研究方向。其中步行功能为其他上层功能的开展提供了坚实的基础。双足步行功能十分灵活，适合在复杂环境下进行工作。然而，在复杂环境下，需要进形高效地路径规划搜索。已有的路径规划算法按照工作场景进行划分，可以分为两类。第一类是在简单，无障碍物的环境下规划。通常来说，算法可以根据提前设置好的参数生成适用于该环境的落脚点，进而产生出一条路径，帮助机器人在平面，斜坡，甚至是楼梯上进行规划。第二类则需要通过传感器(激光，深度相机等)感知环境，获取环境中障碍物的位置等，为规划提供更多的信息。然而现有的方法大都不太灵活。在规划的时候往往使用一个固定的步态集合。这种方法虽然比较简便，但是可能会导致无解情况的发生。如果盲目地扩大步态集合的尺寸，则会浪费资源。另外一个问题是，已有的路径规划方法会忽略地形的情况。在各种地形环境下，使用一个相同的步态集合去寻找路径，这显然是不合适的。

针对这些问题，基于地形环境的自适应路径规划路径在复杂地形环境上已被证明是行之有效的。通过计算不同的地形，生成和扩展不同的步态集合，解决了仿人机器人在复杂环境下的路径规划问题。

发明内容

本发明主要研究仿人机器人在复杂地面环境行走时的路径规划功能，针对已有的路径规划方法不能有效解决复杂环境下行走路径规划的问题，提出了一种基于3D点云的仿人机器人路径规划方法，可有效解决仿人机器人在复杂环境下的路径规划问题，且在仿真平台和实体机器人上进行了测试，验证了此方法的有效性。

为实现上述目的，本发明所提供的技术方案为：一种基于3D点云的仿人机器人路径规划方法，包括以下步骤：

1)使用激光雷达感知地形，建立点云地图；

2)在点云地图的基础上，计算地形环境信息；

3)设计自适应步态集合；

4)根据不同的地形环境自适应地选择不同的自适应步态集合搜索路径。

在步骤1)中，通过使用激光雷达感知环境信息，建立点云地图，由于在仿人机器人运动时所获取的传感器信息容易存在姿态漂移的问题，所以要用仿人机器人静止时所获取的数据建立点云地图，建立点云地图是要把点云以八叉树的形式存储起来；建立点云地图的步骤包括：获取激光扫描数据，转换成三维点云数据，滤波处理，对点云数据进行坐标变换，提取点云的高度和法向量，最后生成点云地图。

在步骤2)中，定义邻接区域，然后计算不同的地形：首先，判断这个区域的平坦情况，设邻接区域的长为L，宽为W，取邻接区域中心处所对应的点云，以R＝min(L,W)为半径，计算出法向量

其中

和

分别是法向量

的三个维度；接下来，以R/2为半径，计算出新的法向量

其中

和

分别是法向量

的三个维度，根据两个向量内积的定义：

反过来推算

和

之间的夹角θ的值：

通过判断夹角θ的值来判断平坦程度；

高度差的最大值视作相对当前仿人机器人支撑脚的高度差的绝对值的最大值；高度的最大值h_map用高度栅格地图进行计算，即：

h_map＝max{c.height-h(c.x,c.y)|c∈C}

其中，C代表生成的高度栅格地图，c是高度栅格地图中的每个像素，c.height表示高度栅格地图中像素的高度，c.x和c.y分别代表高度栅格地图像素的横坐标和纵坐标，h(c.x,c.y)代表该栅格位置的高度；

最后，崎岖程度代表了整体区域高度上的变化，采用下面这种方式来定义相邻区域里的粗糙程度I：

其中，N代表高度栅格地图中像素的个数。

在步骤3)中，设计自适应步态集合，具体如下：

3.1)可到达区域的定义

假设当前支撑脚为左脚，由于仿人机器人本身结构的特性和限制，下一步可能到达的位置并不是随意的；相反，合理的下一步到达的位置是在一个固定的区域中，这个区域根据机器人的结构人为地设置；建立坐标系，其中X轴指向机器人的前进方向，分别沿两个坐标轴以单位长度对可到达区域进行分割；其中，invMaxX和MaxX分别代表可到达区域在X轴方向的最小值和最大值；MaxY代表可达到区域在Y轴方向的最大值；

3.2)自适应步态集合的生成

设计三种不同步态集合Step_A、Step_B和Step_C；其中，Step_A只含有提前设定好的8种步态，包括在不同位置的直行步态、偏转30°和60°的转向步态；Step_A包含的步态数量少，位置单一，但是能够充分应对平面环境下简单的直行、左转和右转功能；Step_B则是能够根据环境自动展开的一种方式，足够应对大部分有障碍物的情形；Step_C也是一种提前设定好的固定的步态集合，其中包含多达27种步态，这些步态的位置和偏摆的角度更多，以便完成更加复杂的步行功能，包括不同角度的转向、后退；

其中，Step_B的生成如下：

3.2.1)先从坐标(MaxX,invMaxY)开始，检查当前步态是否在可达到区域中，如果在可到达区域中，继续检查该步态是否与地面有足够的接触点，若符合的话，则将该状态加入到步态队列中，如果上面的检查失败的话，则减小X坐标的值(MaxX-c,invMaxY)，其中c是坐标单位长度，沿X轴以固定步长向原点搜索状态空间，直到找到合法步态或者到原点；

3.2.2)如果第一步没有找到合法的步态，则偏转一个小角度δ，从可到达区域最远的位置得到新的步态的位置坐标(x,y)，此时步态的偏摆角不再是0，而是δ，参照步骤a，由远及近向原点减小固定的步长来生成新的步态来搜索空间，直到找到合法的状态；其中，δ根据小角近似原理，由下式计算得到：

上式中，c是坐标单位长度，d是新的步态位置坐标与原点的距离；使用这个角度能够近似地保证新的状态与原来的状态偏过一个单位的距离；

3.2.3)如果第一步搜索到了合法的步态，则在原来偏摆角Δ的基础上继续偏转一个角度10°+0.5Δ，然后重复步骤3.2.1)，由远及近地向坐标原点扩展当前的状态空间，直到搜索到有效的结果；

3.2.4)不断重复上述步骤，当搜索角度达到最大值Δ_max时，则停止算法的扩展，最后，还要加入两个额外的步态来保证路径搜索的结果能够成功，第一个步态是零步态，即与支撑脚在Y轴方向距离最近的偏摆角为0的步态；第二个为侧边步态，即与支撑脚在Y轴方向距离最远的偏摆角为0°的步态；因为不能保证向前直行每次都能够成功，这两个步态为规划提供了更多的可能性，所以十分重要；

在步骤4)中，根据不同的地形环境自适应地选择不同的自适应步态集合搜索路径，具体如下：

4.1)不同环境下的步态集合的挑选

首先，计算不同范围内法向量的夹角值来判断是否平坦；如果计算出来的夹角θ小于阈值θ₀，则说明在邻接区域内的地形相对平坦，环境简单，那么选择使用最小的状态集Step_A进行规划；如果地形不是平坦，存在弯曲程度，那么接下来则要判断邻接区域内的最大的高度差h_map，如果h_map超出高度的阈值h₀，则说明该片区域中存在一个或多个高的障碍物，或者是一个不平坦的斜面，这对于路径的搜索和规划将会造成影响，在这种情况下，选择最大的状态集Step_C，对空间进行大规模的搜索，从而提高成功率；如果h_map小于或等于h₀，那么下一步就要判断崎岖程度，计算方差；如果方差大于设定值的话，继续采用大的状态集Step_C；反之，使用中等程度的状态集Step_B；

4.2)自适应路径规划算法步骤

4.2.1)获取点云数据并生成相对应的数据，使用传感器感知地面环境，获取数据后对输入点云进行滤波，得到平滑后的点云，同时，计算高度信息和法向量，使用栅格地图对高度数据进行储存，使用八叉树地图分别对法向量和点云数据进行储存，为后续的规划做准备；

4.2.2)初始化ARA*算法中的参数值，准备规划的前期任务，起点设置为机器人当前的步态，在生成的点云数据上进行路径的规划与搜索；

4.2.3)对地面环境进行评估，并且在不同计算结果的基础上选择不同的状态集，为后续规划展开搜索空间；

4.2.4)展开相应的步态集合，将二维的步态集合在高度栅格地图和法向量的基础上，扩展到三维空间；

4.2.5)检查展开的状态与地面的接触情况，如果当前状态与地面接触良好，则计算相应的代价函数和启发函数的值，为后面的规划提供依据；如果当前状态与地面接触点不够，则说明该步态失效，则舍弃该状态，并把有效的步态返回到步骤4.2.2)；

4.2.6)重复步骤4.2.1)-4.2.5)，如果找到最终的结果，则说明规划成功；如果不能在规定的时间里找到合理的落脚点集合，则说明路径搜索失败。

本发明与现有技术相比，具有如下优点与有益效果：

1、本方法在已有路径规划方法的基础上，采用了自适应的思路，加快了收敛速度，提高了搜索的成功率。

2、本方法可以对地形环境进行评估，然后根据地形选择不同的步态集合，更加灵活多变，适应性强，应用广泛。

3、本方法简单易行，灵活多变，能够在在复杂环境下展开路径，帮助仿人机器人在不平整的地面上实现稳定行走，具有一定的现实意义和应用价值。

附图说明

图1为邻接区域图。

图2为可到达区域图。

图3为步态集合图。

图4为自适应步态集合生成图；其中，从左向右依次为：(a)沿X轴进行搜索；(b)如果搜索失败，则偏转一个小的角度；(c)如果搜索成功的话，则偏转一个稍大的角度；(d)成功生成步态集合的示例。

图5为步态集合挑选图。

具体实施方式

下面结合具体实施例对本发明作进一步说明。

本实施例所提供的基于3D点云的仿人机器人路径规划方法，其具体情况如下：

1)三维点云的生成

首先，需要将激光扫描数据转换成三维点云数据。收集机器人静止时的点云数据用于环境建模，这样可以保证点云数据的一致性。其次，由于传感器本身的特性以及环境的影响，从激光雷达收集的数据难免存在一定的噪声。因此，使用直通滤波器，和体素网格滤波器对点云数据进行过滤。

获得经过滤波处理的平滑点云后，需要对点云数据进行坐标系的转换，把点云从传感器的局部坐标系映射到世界坐标系下。激光雷达或者深度相机在采集数据的时候，是在传感器的局部坐标下对环境进行感知的所以，要经过变换矩阵进行计算。

其中，p_l＝[x_l y_l z_l]^T是点云在局部坐标系下的坐标，p_w＝[x_w y_w z_w]^T是点云在世界坐标系下的坐标，R是3×3的旋转矩阵，代表局部坐标系和世界坐标系变换的旋转变化；t是3×1的平移向量，表示局部坐标系和世界坐标系变换的平移变化；0＝[0 0 0]。激光扫描转换成点云之后，为了方便后续规划时的使用和计算，可以把点云数据以八叉树地图的形式储存起来。

经过上述处理之后，可以非常容易地提取地面环境的高度和法向量等信息。点云沿Z轴方向的最大值代表了该处地面对应的最高高度，把信息存储在二维的栅格地图中。需要使用最近邻的思想进一步计算所有位置的高度。点

的高度信息需要计算该点周围的k个最近邻居点

的高度的加权平均值，即：

计算某点处法向量的问题等同于估计过此点曲面切面的法向量的问题，进而转换成最小二乘平面拟合估计问题。在这种情况下，需用到主成分分析的方法具体来说，对于点云中的每一个点p_i，协方差矩阵C定义成：

其中k表示点p_i邻居点的个数，

表示p_i邻居点集的重心，λ_j是协方差矩阵的第j个特征值，

是第j个特征向量。此时，使用主成分分析所计算出来的最小的特征值所对应的特征向量为点处的法向量。因为某点处的法向量与该点处的切面垂直，包含的点云信息应该是最少的，所以对应的特征值也应该是最小的。

使用主成分分析计算法向量时，由于存在不同的计算方式，即使特征值计算出来的结果是一样的，特征向量的符号也可能不一样，即特征向量所指向的方向不一样。通常来说，没有数学的方法解决法向量的符号问题，导致了通过主成分分析计算出来的法向量的方向模糊不清。而且不同视角对应的正方向还存在一定的差异。为了解决这一问题，假设给定视角v_p，让法向量

指向视角，则法向量需要满足以下条件：

2)地形环境的计算

2.1)邻接区域

为了对地面环境进行准确地描述，首先要决定在计算过程中包括哪些区域。根据仿人机器人支撑脚的位置和姿态，图1定义了支撑脚的邻接区域R。邻接区域的范围是在仿人机器人的下一步的步长的基础上决定的，即如果仿人机器人沿直线行走，或者有轻微的转向，那么接下来的落脚点的集合将会围成一定的区域。使用包含所有步态的最小的一个矩形区域来定义为邻接区域。

2.2)地形的描述

首先，可以判断这个区域的平坦情况。在本文中，平坦程度定义为该平面的弯曲和倾斜程度。通常来说，少量缓和的凸起和凹陷并不会影响整个区域的平坦程度。可以通过比较在不同大小区域上计算出来的法向量来对邻接区域的平坦程度进行判定。计算点云地图中某点处的法向量，与选取的该点邻居点云的数量有关。利用这个思想，设邻接区域的长为L，宽为W，取邻接区域中心处所对应的点云，以R＝min(L,W)为半径，计算出法向量

其中

和

分别是法向量

的三个维度；接下来，以R/2为半径，计算出新的法向量

其中

和

分别是法向量

的三个维度。根据两个向量内积的定义：

反过来推算

和

之间的夹角θ的值：

如果夹角θ在一定的阈值θ₀范围内，则说明该区域的弯曲程度没有那么大，相对来说比较平坦；反之，如果超过阈值θ₀，则说明该区域并不平坦，可能导致仿人机器人的稳定程度受到一定的影响。

高度差的最大值可以视作在邻接区域里，相对当前仿人机器人支撑脚的高度的差的绝对值的最大值。这个性质可以来形容一个区域里的凸起或凹陷的情况。一般来说，凸起比小洞更容易对仿人机器人的行走造成影响，因为小洞并不会影响仿人机器人足底与地面的接触面积。高度的最大值h_map可以用高度栅格地图进行计算用，即：

h_map＝max{c.height-h(c.x,c.y)|c∈C}

其中，C代表生成的高度栅格地图，c是高度栅格地图中的每个像素，c.height表示高度栅格地图中像素的高度，c.x和c.y分别代表高度栅格地图像素的横坐标和纵坐标，h(c.x,c.y)代表该栅格位置的高度。

尽管上面定义了邻接区域整体的平坦情况，但是还不能够充分描述和形容邻接区域具体的崎岖程度，或者说粗糙程度。在本文中，崎岖程度代表了整体区域高度上的变化。只用邻接区域内的高度的平均值来衡量崎岖程度的话，会因为最大值或最小值的极端情况，而造成误差。众所周知，一组数据内的平均值受最值影响非常大，这是平均值的天然缺陷。为此，本文采用下面这种方式来定义相邻区域里的粗糙程度I：

其中，代表生成的高度栅格地图，c是高度栅格地图中的每个像素，c.height表示高度栅格地图中像素的高度，c.x和c.y分别代表高度栅格地图像素的横坐标和纵坐标，h(c.x,c.y)代表该栅格位置的高度，N代表高度栅格地图中像素的个数。该形式类似于数学中标准差的定义方法。标准差的意义是描述统计数据集的离散程度，所以，上式可以描述一个区域里高度的“分散程度”，也就是说，区域的崎岖程度或者粗糙程度。但是，与标准差不同的是，上式计算的不是栅格地图内每个栅格高度与平均值的差，而是与支撑脚高度的差。这使得对规划结果具有更大的参考意义。

3)自适应步态集合的生成：

3.1)可到达区域的定义

假设当前支撑脚为左脚，由于仿人机器人本身结构的特性和限制，下一步可能到达的位置并不是随意的；相反，合理的下一步可以到达的位置是在一个固定的区域中，如图2中的灰色部分。一般来说，这个区域可以根据机器人的结构人为地设置。建立坐标系，其中X轴指向机器人的前进方向，分别沿两个坐标轴以单位长度对可到达区域进行分割。其中，invMaxX和MaxX分别代表可到达区域在X轴方向的最小值和最大值；MaxY分别代表可达到区域在Y轴方向的最大值。

3.2)自适应步态集合的生成

本文设计了三种不同步态集合Step_A，Step_B和Step_C，如图3所示。其中，Step_A只含有提前设定好的8种步态，包括在不同位置的直行步态，偏转30°和60°的转向步态等。Step_A包含的步态数量较少，位置比较单一，但是能够充分应对平面环境下简单的直行，左转，和右转等功能。Step_B则是能够根据环境自动展开的一种方法，足够应对大部分有障碍物的情形。Step_C也是一种固定的步态集合，其中包含多达27种步态。这些步态的位置和偏摆的角度更多，以便完成更加复杂的步行功能，例如不同角度的转向、后退等。Step_A和Step_C是预先设置的，因此不再加以赘述。如图4所示，Step_B的生成如下：

3.2.1)先从坐标(MaxX,invMaxY)开始，检查当前步态是否在可达到区域中。如果在可到达区域中，继续检查该步态是否与地面有足够的接触点。若符合的话，则将该状态加入到步态队列中。如果上面的检查失败的话，则减小X坐标的值(MaxX-c,invMaxY)，其中c是坐标单位长度，沿X轴以固定步长向原点搜索状态空间，直到找到合法步态或者到原点，如图4中a)所示。

3.2.2)如果第一步没有找到合法的步态，则偏转一个小角度δ，如图4中b)所示，从可到达区域最远的位置得到新的步态的位置坐标(x,y)。此时步态的偏摆角不再是0，而是δ。参照步骤(1)，由远及近向原点减小固定的步长来生成新的步态来搜索空间，直到找到合法的状态。其中，δ根据小角近似原理，可以由下式计算得到：

其中，c是坐标单位长度，d是新的步态位置坐标与原点的距离。使用这个角度可以近似地保证新的状态与原来的状态偏过一个单位的距离。

3.2.3)如果第一步搜索到了合法的步态，则在原来偏摆角Δ的基础上继续偏转一个角度10°+0.5Δ，然后重复步骤3.2.1)，由远及近地向坐标原点扩展当前的状态空间，直到搜索到有效的结果，如图4中c)所示。

3.2.4)不断重复上述步骤，当搜索角度达到最大值Δ_max时，则停止算法的扩展。最后，还要加入两个额外的步态来保证路径搜索的结果能够成功。第一个步态是零步态，即与支撑脚在Y轴方向距离最近的偏摆角为0的步态；第二个为侧边步态，即与支撑脚在Y轴方向距离最远的偏摆角为0°的步态。因为不能保证向前直行每次都可以成功，这两个步态为规划提供了更多的可能性，所以十分重要。如图4中d)所示就是搜索成功的示意图。

4)自适应路径规划算法

4.1)不同环境下的步态集合的挑选

图5展示了不同步态集合的展开流程。首先计算不同范围内法向量的夹角值来判断是否平坦。如果计算出来的夹角θ小于阈值θ₀，则说明在邻接区域内的地形相对比较平坦，环境较为简单，那么则选择使用最小的状态集Step_A进行规划。如果地形不是那么平坦，存在一定的弯曲程度，那么接下来则要判断邻接区域内的最大的高度差h_map。如果h_map超出高度的阈值h₀，则说明该片区域中存在一个或多个非常高的障碍物，或者是一个不平坦的斜面。这对于路径的搜索和规划将会造成一定的影响。在这种情况下，选择最大的状态集Step_C，对空间进行大规模的搜索，从而提高成功率。如果h小于等于h₀，那么下一步就要判断崎岖程度，计算方差。如果方差非常大的话，继续采用大的状态集Step_C；反之，使用中等程度的状态集Step_B。

4.2)自适应路径规划算法步骤

4.2.1)获取点云数据并生成相对应的数据。使用传感器感知地面环境，获取数据后对输入点云进行滤波，得到平滑后的点云。同时，计算高度信息和法向量，使用栅格地图对高度数据进行储存，使用八叉树地图分别对法向量和点云数据进行储存，为后续的规划做准备；

4.2.2)初始化ARA*算法中的参数值，准备规划的前期任务。起点设置为机器人当前的步态，在生成的点云数据上进行路径的规划与搜索；

4.2.3)对地面环境进行评估，并且在不同计算结果的基础上选择不同的状态集，为后续规划展开搜索空间；

4.2.4)展开相应的步态集合，将二维的步态集合在高度栅格地图和法向量的基础上，扩展到三维空间；

4.2.5)检查展开的状态与地面的接触情况。如果当前状态与地面接触良好，则计算相应的代价函数和启发函数的值，为后面的规划提供依据；如果当前状态与地面接触点不够，则说明该步态失效，则舍弃该状态。并把有效的步态返回到步骤4.2.2)；

4.2.6)重复步骤4.2.1)-4.2.5)，如果可以找到最终的结果，则说明规划成功；如果不能在一定的时间里找到合理的落脚点集合，则说明路径搜索失败。

以上所述实施例只为本发明之较佳实施例，并非以此限制本发明的实施范围，故凡依本发明之形状、原理所作的变化，均应涵盖在本发明的保护范围内。

Claims (3)

1.一种基于3D点云的仿人机器人路径规划方法，其特征在于，包括以下步骤：

1)使用激光雷达感知地形，建立点云地图；

2)在点云地图的基础上，计算地形环境信息；

3)设计自适应步态集合，具体如下：

3.1)可到达区域的定义

假设当前支撑脚为左脚，由于仿人机器人本身结构的特性和限制，下一步可能到达的位置并不是随意的；相反，合理的下一步到达的位置是在一个固定的区域中，这个区域根据机器人的结构人为地设置；建立坐标系，其中X轴指向机器人的前进方向，分别沿两个坐标轴以单位长度对可到达区域进行分割；其中，invMaxX和MaxX分别代表可到达区域在X轴方向的最小值和最大值；MaxY代表可达到区域在Y轴方向的最大值；

3.2)自适应步态集合的生成

设计三种不同步态集合Step_A、Step_B和Step_C；其中，Step_A只含有提前设定好的8种步态，包括在不同位置的直行步态、偏转30°和60°的转向步态；Step_A包含的步态数量少，位置单一，但是能够充分应对平面环境下简单的直行、左转和右转功能；Step_B则是能够根据环境自动展开的一种方式，足够应对大部分有障碍物的情形；Step_C也是一种提前设定好的固定的步态集合，其中包含多达27种步态，这些步态的位置和偏摆的角度更多，以便完成更加复杂的步行功能，包括不同角度的转向、后退；

其中，Step_B的生成如下：

3.2.1)先从坐标(MaxX,invMaxY)开始，检查当前步态是否在可达到区域中，如果在可到达区域中，继续检查该步态是否与地面有足够的接触点，若符合的话，则将该状态加入到步态队列中，如果上面的检查失败的话，则减小X坐标的值(MaxX-c,invMaxY)，其中c是坐标单位长度，沿X轴以固定步长向原点搜索状态空间，直到找到合法步态或者到原点；

3.2.2)如果第一步没有找到合法的步态，则偏转一个小角度δ，从可到达区域最远的位置得到新的步态的位置坐标(x,y)，此时步态的偏摆角不再是0，而是δ，参照步骤a，由远及近向原点减小固定的步长来生成新的步态来搜索空间，直到找到合法的状态；其中，δ根据小角近似原理，由下式计算得到：

上式中，c是坐标单位长度，d是新的步态位置坐标与原点的距离；使用这个角度能够近似地保证新的状态与原来的状态偏过一个单位的距离；

3.2.3)如果第一步搜索到了合法的步态，则在原来偏摆角Δ的基础上继续偏转一个角度10°+0.5Δ，然后重复步骤3.2.1)，由远及近地向坐标原点扩展当前的状态空间，直到搜索到有效的结果；

3.2.4)不断重复上述步骤，当搜索角度达到最大值Δ_max时，则停止算法的扩展，最后，还要加入两个额外的步态来保证路径搜索的结果能够成功，第一个步态是零步态，即与支撑脚在Y轴方向距离最近的偏摆角为0的步态；第二个为侧边步态，即与支撑脚在Y轴方向距离最远的偏摆角为0°的步态；因为不能保证向前直行每次都能够成功，这两个步态为规划提供了更多的可能性，所以十分重要；

4)根据不同的地形环境自适应地选择不同的自适应步态集合搜索路径，具体如下：

4.1)不同环境下的步态集合的挑选

首先，计算不同范围内法向量的夹角值来判断是否平坦；如果计算出来的夹角θ小于阈值θ₀，则说明在邻接区域内的地形相对平坦，环境简单，那么选择使用最小的状态集Step_A进行规划；如果地形不是平坦，存在弯曲程度，那么接下来则要判断邻接区域内的最大的高度差h_map，如果h_map超出高度的阈值h₀，则说明该片区域中存在一个或多个高的障碍物，或者是一个不平坦的斜面，这对于路径的搜索和规划将会造成影响，在这种情况下，选择最大的状态集Step_C，对空间进行大规模的搜索，从而提高成功率；如果h_map小于或等于h₀，那么下一步就要判断崎岖程度，计算方差；如果方差大于设定值的话，继续采用大的状态集Step_C；反之，使用中等程度的状态集Step_B；

4.2)自适应路径规划算法步骤

4.2.1)获取点云数据并生成相对应的数据，使用传感器感知地面环境，获取数据后对输入点云进行滤波，得到平滑后的点云，同时，计算高度信息和法向量，使用栅格地图对高度数据进行储存，使用八叉树地图分别对法向量和点云数据进行储存，为后续的规划做准备；

4.2.2)初始化ARA*算法中的参数值，准备规划的前期任务，起点设置为机器人当前的步态，在生成的点云数据上进行路径的规划与搜索；

4.2.3)对地面环境进行评估，并且在不同计算结果的基础上选择不同的状态集，为后续规划展开搜索空间；

4.2.4)展开相应的步态集合，将二维的步态集合在高度栅格地图和法向量的基础上，扩展到三维空间；

4.2.5)检查展开的状态与地面的接触情况，如果当前状态与地面接触良好，则计算相应的代价函数和启发函数的值，为后面的规划提供依据；如果当前状态与地面接触点不够，则说明该步态失效，则舍弃该状态，并把有效的步态返回到步骤4.2.2)；

4.2.6)重复步骤4.2.1)-4.2.5)，如果找到最终的结果，则说明规划成功；如果不能在规定的时间里找到合理的落脚点集合，则说明路径搜索失败。

2.根据权利要求1所述的一种基于3D点云的仿人机器人路径规划方法，其特征在于：在步骤1)中，通过使用激光雷达感知环境信息，建立点云地图，由于在仿人机器人运动时所获取的传感器信息容易存在姿态漂移的问题，所以要用仿人机器人静止时所获取的数据建立点云地图，建立点云地图是要把点云以八叉树的形式存储起来；建立点云地图的步骤包括：获取激光扫描数据，转换成三维点云数据，滤波处理，对点云数据进行坐标变换，提取点云的高度和法向量，最后生成点云地图。

3.根据权利要求1所述的一种基于3D点云的仿人机器人路径规划方法，其特征在于：在步骤2)中，定义邻接区域，然后计算不同的地形：首先，判断这个区域的平坦情况，设邻接区域的长为L，宽为W，取邻接区域中心处所对应的点云，以R＝min(L,W)为半径，计算出法向量

其中

和

分别是法向量

的三个维度；接下来，以R/2为半径，计算出新的法向量

其中

和

分别是法向量

的三个维度，根据两个向量内积的定义：

反过来推算

和

之间的夹角θ的值：

通过判断夹角θ的值来判断平坦程度；

高度差的最大值视作相对当前仿人机器人支撑脚的高度差的绝对值的最大值；高度的最大值h_map用高度栅格地图进行计算，即：

h_map＝max{c.height-h(c.x,c.y)|c∈C}

其中，C代表生成的高度栅格地图，c是高度栅格地图中的每个像素，c.height表示高度栅格地图中像素的高度，c.x和c.y分别代表高度栅格地图像素的横坐标和纵坐标，h(c.x,c.y)代表该栅格位置的高度；

最后，崎岖程度代表了整体区域高度上的变化，采用下面这种方式来定义相邻区域里的粗糙程度I：

其中，N代表高度栅格地图中像素的个数。

Images