[发明专利]一种基于3D点云的仿人机器人路径规划方法

权利要求书

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)，如果找到最终的结果，则说明规划成功；如果不能在规定的时间里找到合理的落脚点集合，则说明路径搜索失败。