[发明专利]用于移动机器人在非平面环境自主探索的导引点选取方法

权利要求书

1.一种用于移动机器人在非平面环境自主探索的导引点选取方法，其特征在于：包括以下步骤：

步骤一：按顺序获取3D激光雷达采集的激光帧数据并确定运动轨迹，得到包含运动轨迹的激光帧序列；将第一帧激光雷达作为初始帧，选取一定数量的连续的激光雷达帧构建局部子地图；且所述局部子地图的第一帧与最后一帧的位姿的运动平移距离大于预设距离或旋转角度大于预设角度；

步骤二：根据激光帧与其对应位姿构建机器人坐标系下的高程地图，且该高程地图的每个栅格信息要包含栅格状态；所述栅格状态为占据、空闲与未知中的任一种；

步骤三：根据高程地图计算初始导引点集合以及每个栅格的坡度与粗糙度信息；

步骤四：对初始导引点集合进行mean-shift聚类，得到聚类导引点，并计算当前的机器人中心到每个聚类导引点的路径；

步骤五：根据聚类导引点以及地图几何信息生成最优导引点。

2.根据权利要求1所述的用于移动机器人在非平面环境自主探索的导引点选取方法，其特征在于，所述步骤二具体通过如下的子步骤来实现：

创建以机器人中心为基点，机器人朝向前左上分别对应X-Y-Z构建坐标系；构建以机器人中心为中心的高程地图，该高程地图的每个栅格包含的信息为

；其中，为第c个栅格的中心点坐标，h_c为落在该栅格的所有3D点的最大的z值，s_c为该栅格的状态，其状态包括占据、空闲与未知三种，分别用occ，free，un表示；所述高程地图的边长的上限为机器人使用传感器的最大感知距离，下限为所述预设距离。

3.根据权利要求2所述的用于移动机器人在非平面环境自主探索的导引点选取方法，其特征在于，所述步骤三具体通过如下的子步骤来实现：

（1）遍历步骤二中高程地图的每一个栅格，取出以该栅格为中心的4邻域与8邻域，若该栅格状态为空闲且其4邻域中至少有一个的状态为未知，则该栅格被标记为初始候选导引点，最终得到初始候选导引点集合；

其中，，其中， W为整个高程地图的X方向的长度，H为整个高程地图的Y方向的长度，r为每个栅格的分辨率，c表示第c个栅格，；

（2）将每个栅格与其8邻域组成的共9个点的集合用于PCA主成分解，将其中分解的第三个奇异值对应的反余弦值作为坡度信息，用k_c表示；同时以9个点集合计算该栅格的平均高度h_mean，并通过下式计算粗糙度r_c：

其中，abs()为取绝对值；

（3）更新每个栅格包含的信息。

4.根据权利要求1所述的用于移动机器人在非平面环境自主探索的导引点选取方法，其特征在于，所述步骤四中，采用D^*路径规划算法计算当前的机器人中心到每个聚类导引点的路径。

5.根据权利要求1所述的用于移动机器人在非平面环境自主探索的导引点选取方法，其特征在于，所述步骤五具体通过如下的子步骤来实现：

遍历聚类导引点与其对应路径的集合中的二元组，通过下式计算每个聚类导引点的最终收益，并将收益最大的聚类导引点作为最优导引点：

其中，为三个控制参数，为导引点的信息增益，其定义为以为中心的栅格中状态为未知的格子的个数；为机器人中心与聚类导引点的欧式距离，为对应的路径中所有点对应栅格的坡度与粗糙度之和求平均， ； 为机器人当前位置,。