(19)中华人民共和国国家知识产权局
(12)发明专利申请
(10)申请公布号 (43)申请公布日 (21)申请号 201910484762.6
(22)申请日 2019.06.05
(71)申请人 华南理工大学
地址 510640 广东省广州市天河区五山路
381号
(72)发明人 毕盛 刘云达 董敏 闵华清
(74)专利代理机构 广州市华学知识产权代理有
限公司 44245
代理人 冯炳辉
(51)Int.Cl.
G01C 21/34(2006.01)
(54)发明名称
法
(57)摘要
本发明公开了一种基于3D点云的仿人机器
人路径规划方法，包括步骤：1)使用激光雷达感
知地形，建立点云地图；2)在点云地图的基础上，
根据不同的地形环境自适应地选择不同的自适
应步态集合搜索路径。本发明可有效解决仿人机
器人在复杂环境下的路径规划问题，且在仿真平
台和实体机器人上进行了测试，验证了此方法的
有效性。权利要求书3页 说明书8页 附图3页CN 110361026 A 2019.10.22
C N 110361026
A
1.一种基于3D点云的仿人机器人路径规划方法，其特征在于，包括以下步骤：
1)使用激光雷达感知地形，建立点云地图；
2)在点云地图的基础上，计算地形环境信息；
3)设计自适应步态集合；
4)根据不同的地形环境自适应地选择不同的自适应步态集合搜索路径。
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代表高度栅格地图中像素的个数。
4.根据权利要求1所述的一种基于3D点云的仿人机器人路径规划方法，其特征在于：在步骤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)不同环境下的步态集合的挑选
首先，计算不同范围内法向量的夹角值来判断是否平坦；如果计算出来的夹角θ小于阈值θ0，则说明在邻接区域内的地形相对平坦，环境简单，那么选择使用最小的状态集Step A进行规划；如果地形不是平坦，存在弯曲程度，那么接下来则要判断邻接区域内的最大的高度
差h map，如果h map超出高度的阈值h0，则说明该片区域中存在一个或多个高的障碍物，或者是一个不平坦的斜面，这对于路径的搜索和规划将会造成影响，在这种情况下，选择最大的状态集Step C，对空间进行大规模的搜索，从而提高成功率；如果h map小于或等于h0，那么下一步就要判断崎岖程度，计算方差；如果方差大于设定值的话，继续采用大的状态集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)，如果到最终的结果，则说明规划成功；如果不能在规定的时间里到合理的落脚点集合，则说明路径搜索失败。
一种基于3D点云的仿人机器人路径规划方法技术领域
[0001]本发明涉及仿人机器人的技术领域，尤其是指一种基于3D点云的仿人机器人路径规划方法。
背景技术
[0002]仿人机器人因为其独特的外形结构和灵活强大的功能一直是智能机器人领域中热门的研究方向。其中步行功能为其他上层功能的开展提供了坚实的基础。双足步行功能十分灵活，适合在复杂环
境下进行工作。然而，在复杂环境下，需要进形高效地路径规划搜索。已有的路径规划算法按照工作场景进行划分，可以分为两类。第一类是在简单，无障碍物的环境下规划。通常来说，算法可以根据提前设置好的参数生成适用于该环境的落脚点，进而产生出一条路径，帮助机器人在平面，斜坡，甚至是楼梯上进行规划。第二类则需要通过传感器(激光，深度相机等)感知环境，获取环境中障碍物的位置等，为规划提供更多的信息。然而现有的方法大都不太灵活。在规划的时候往往使用一个固定的步态集合。这种方法虽然比较简便，但是可能会导致无解情况的发生。如果盲目地扩大步态集合的尺寸，则会浪费资源。另外一个问题是，已有的路径规划方法会忽略地形的情况。在各种地形环境下，使用一个相同的步态集合去寻路径，这显然是不合适的。
[0003]针对这些问题，基于地形环境的自适应路径规划路径在复杂地形环境上已被证明是行之有效的。通过计算不同的地形，生成和扩展不同的步态集合，解决了仿人机器人在复杂环境下的路径规划问题。
发明内容
[0004]本发明主要研究仿人机器人在复杂地面环境行走时的路径规划功能，针对已有的路径规划方法不能有效解决复杂环境下行走路径规划的问题，提出了一种基于3D点云的仿人机器人路径规划方法，可有效解决仿人机器人在复杂环境下的路径规划问题，且在仿真平台和实体机器人上进行了测试，验证了此方法的有效性。
[0005]为实现上述目的，本发明所提供的技术方案为：一种基于3D点云的仿人机器人路径规划方法，包括以下步骤：
[0006]1)使用激光雷达感知地形，建立点云地图；
[0007]2)在点云地图的基础上，计算地形环境信息；
[0008]3)设计自适应步态集合；
[0009]4)根据不同的地形环境自适应地选择不同的自适应步态集合搜索路径。
[0010]在步骤1)中，通过使用激光雷达感知环境信息，建立点云地图，由于在仿人机器人运动时所获取的传感器信息容易存在姿态漂移的问题，所以要用仿人机器人静止时所获取的数据建立点云地图，建立点云地图是要把点云以八叉树的形式存储起来；建立点云地图的步骤包括：获取激光扫描数据，转换成三维点云数据，滤波处理，对点云数据进行坐标变换，提取点云的高度和法向量，最后生成点云地图。
说 明 书
1/8页CN 110361026 A
豫公网安备41160202000603
站长QQ:729038198
关于我们
投诉建议