CN104599588B - 一种栅格地图通行成本的计算方法 - Google Patents

Abstract

本发明提供了一种栅格地图通行成本的计算方法，所述方法包括：获取激光传感器数据并对其数据进行预处理；对有效数据进行栅格化，生成初始栅格地图；对初始栅格地图进行膨胀处理；根据机器人平台尺寸重新计算每个小栅格的通行成本；通过该方法减少了路径搜索的搜索面积，在保证获得路径可通过的同时，提高了搜索效率。

Description

一种栅格地图通行成本的计算方法

技术领域

本发明涉及一种计算方法，具体涉及一种栅格地图通行成本的计算方法。

背景技术

环境认知及建模是实现机器人平台自主导航的基本前提。机器人平台在运动过程中依靠传感器获取对环境的感知信息，同时创建环境地图。一个合理的环境地图有益于减少用于路径搜索的环境区域面积，提高路径搜索的效率。

一种表示环境的模型栅格地图将环境分解成一系列离散的栅格，通常采用0、1值定义栅格的通行(0)和不通行(1)两种属性。采用这种方法生成的栅格通行成本地图，在定义栅格通行成本时并未考虑机器人平台的尺寸信息，导致在进行路径搜索时需要根据机器人平台尺寸确定栅格的通过性，同时搜索环境区域增大，搜索效率低。

申请号为200810194812.9的中国专利公开了一种基于栅格数据的空间成本距离计算方法，该方法主要用于计算空间内任意位置到达另一空间位置所需要的最小空间成本距离值，而对栅格单元的数值没有给出明确的计算方法。申请号为200810143537.8的中国专利公开了一种实时数据融合的移动机器人平台栅格地图创建方法，该方法将栅格定义为空闲、被占用和不确定三种状态，仅计算栅格置信度，完成栅格地图创建，在地图创建过程中并未考虑栅格通行成本值的具体计算方法。

发明内容

本发明的目的是提供一种栅格地图通行成本的计算方法，该方法根据机器人平台尺寸计算每个栅格的通行成本，以数值化表示，能适用于为各种规则、不规则机器人平台建立栅格地图，栅格通行成本分配合理，计算过程简单。

本发明的目的是采用下述技术方案实现的：

一种栅格地图通行成本的计算方法，所述方法包括：

获取激光传感器数据并对其数据进行预处理；

对有效数据进行栅格化，生成初始栅格地图；

对初始栅格地图进行膨胀处理；

根据机器人平台尺寸重新计算每个小栅格的通行成本。

优选的，所述激光传感器数据包括，以激光传感器的重心为极坐标原点建立极坐标系，探测前方10m且角度在-45°～225°以内的物体与极坐标原点之间弧长和角度值；其中，所述激光传感器为安装在机器人平台前方的单线激光传感器。

优选的，所述预处理包括：通过以太网将所述激光传感器数据发送至工控机，所述工控机根据激光传感器和机器人平台之间的坐标转换将激光传感器数据投影到机器人平台。

进一步地，所述坐标转换包括：以表示任一激光传感器数据点在极坐标系下的坐标，根据激光传感器直角坐标系和极坐标系之间的坐标转换关系计算该激光传感器数据点在直角坐标系下的坐标(x_i，y_i)；

以(l_x(t)，l_y(t))表示激光传感器坐标系和大地坐标系之间的坐标转换关系，利用将激光传感器数据点在直角坐标系下的坐标(x_i，y_i)转换为大地坐标系下的坐标(x_gi，y_gi)。

优选的，所述有效数据包括，随着机器人平台移动，不断对激光传感器数据点进行坐标转换，生成大地坐标系下的坐标集合。

优选的，所述对有效数据进行栅格化包括：将占据栅格通行成本设定为254，自由栅格通行成本设定为0，存储在工控机内；在每个小栅格(x_si，y_si，cost_si)中，x_si为行数，y_si为列数，cost_si为通行成本；所述占据栅格为存在激光传感器数据点的栅格；所述自由栅格为不存在激光传感器数据点的栅格。

优选的，所述对初始栅格地图进行膨胀处理包括，设定机器人平台外形的内切圆半径为r，外接圆半径为R，初始栅格地图中任一自由栅格为G_si(x_si,y_si,0)；

在栅格膨胀区域半径P的范围内，遍历G_si到占据栅格点的欧几里得距离，计算G_si到占据栅格的最小距离d_min。

进一步地，根据d_min(G_si)与机器人平台外形的内切圆半径r和外接圆半径R之间的大小关系，重新计算该栅格点的栅格通行成本cost_si，生成栅格地图

式中，f为控制栅格通行成本衰减的比例因子，f越大，栅格通行成本衰减越快。

与现有技术相比，本发明提供的方法达到以下有益效果：

本发明采用激光传感器数据获取初始栅格地图，依据机器人尺寸信息对初始栅格地图进行膨胀处理，计算自由栅格的通行成本；利用该方法建立的栅格地图进行路径搜索时，可以减少搜索区域的面积，提高搜索效率，保证获得路径的可通过性。

附图说明

图1是本发明一种栅格地图通行成本的计算方法的流程图

图2是本发明的机器人***结构示意图

其中，1.机器人平台2.激光传感器3.网线4.工控机

图3是本发明建立的初始栅格地图示意图

图4是本发明的机器人平台外形尺寸及内切圆、外接圆示意图；

图5是本发明建立的栅格地图示意图

具体实施方式

下面结合附图和具体实施例对本发明作进一步的详细说明。

如图1所示，一种栅格地图通行成本的计算方法，所述方法包括：

获取激光传感器数据并对其数据进行预处理；所述激光传感器数据包括，以激光传感器的重心为极坐标原点建立极坐标系，探测前方10m且角度在-45°～225°以内的物体与极坐标原点之间弧长和角度值；其中，如图2所示，所述激光传感器为安装在机器人平台前方的单线激光传感器。所述机器人平台为任一规则、不规则形状的机器人。

所述预处理包括：通过以太网将所述激光传感器数据发送至工控机，所述工控机根据激光传感器和机器人平台之间的坐标转换将激光传感器数据投影到机器人平台。所述工控机用于遥控机器人运动，在运动过程中采集激光传感器数据和轮速码盘数据。

所述坐标转换包括：以表示任一激光传感器数据点在极坐标系下的坐标，根据激光传感器直角坐标系和极坐标系之间的坐标转换关系计算该激光传感器数据点在直角坐标系下的坐标(x_i，y_i)；

激光传感器坐标系和机器人坐标系之间的坐标转换关系是固定的，而机器人坐标系和大地坐标系之间的坐标转换关系随机器人平台姿态变化调整，这可通过GPS或航迹推算方法获得，因此激光传感器坐标系和大地坐标系之间的坐标转换关系是动态变化的：以(l_x(t)，l_y(t))表示激光传感器坐标系和大地坐标系之间的坐标转换关系，利用将激光传感器数据点在直角坐标系下的坐标(x_i，y_i)转换为大地坐标系下的坐标(x_gi，y_gi)。

对有效数据进行栅格化，如图3所示，生成初始栅格地图；所述有效数据包括，随着机器人平台移动，不断对激光传感器数据点进行坐标转换，生成大地坐标系下的坐标集合。

所述对有效数据进行栅格化包括：将占据栅格通行成本设定为254，自由栅格通行成本设定为0，存储在工控机内；在每个小栅格(x_si，y_si，cost_si)中，x_si为行数，即x_si按照地图栅格分辨率的圆整数，y_si为列数，即y_si按照地图栅格分辨率的圆整数，cost_si为通行成本；，所述占据栅格为存在激光传感器数据点的栅格；所述自由栅格为不存在激光传感器数据点的栅格。

对初始栅格地图进行膨胀处理；所述对初始栅格地图进行膨胀处理包括，设定机器人平台外形的内切圆半径为r，外接圆半径为R，初始栅格地图中任一自由栅格为G_si(x_si,y_si,0)；

在栅格膨胀区域半径P的范围内，遍历G_si到占据栅格点的欧几里得距离，计算G_si到占据栅格的最小距离d_min。

根据机器人平台尺寸重新计算每个小栅格的通行成本。

如图4所示，根据d_min(G_si)与机器人平台外形的内切圆半径r和外接圆半径R之间的大小关系，重新计算该栅格点的栅格通行成本cost_si，生成栅格地图；

式中，f为控制栅格通行成本衰减的比例因子，f越大，栅格通行成本衰减越快。

实施例：选取参数P为0.55m，f为10，按照上式对栅格通行成本重新计算后，最终获得的栅格地图如图5所示，图5中的每个栅格可用(x_si，y_si，cost_si)表示，其中x_si代表每个小栅格的行数，y_si代表每个小栅格的列数，cost_si代表小栅格的通行成本，每个栅格通行成本的设定按照它到障碍物栅格的距离设定，取值范围为[0-254]。根据栅格通行成本的设定可知，254和253表示机器人不可通行栅格，1-252表示机器人可以以特定姿态通行的栅格，需要根据机器人姿态进行判定，0表示机器人可以以任意姿态通行。

最后应该说明的是，结合上述实施例仅说明本发明的技术方案而非对其限制。所属领域的普通技术人员应当理解到：本领域技术人员可以对本发明的具体实施方式进行修改或者等同替换，但这些修改或变更均在申请待批的权利要求保护范围之中。

Claims (8)

1.一种栅格地图通行成本的计算方法，其特征在于，所述方法包括：

获取激光传感器数据并对其数据进行预处理；

对有效数据进行栅格化，生成初始栅格地图；

对初始栅格地图进行膨胀处理；

根据机器人平台尺寸重新计算每个小栅格的通行成本。

2.如权利要求1所述的一种栅格地图通行成本的计算方法，其特征在于，所述激光传感器数据包括，以激光传感器的重心为极坐标原点建立极坐标系，探测前方10m且角度在-45°～225°以内的物体与极坐标原点之间弧长和角度值；其中，所述激光传感器为安装在机器人平台前方的单线激光传感器。

3.如权利要求1所述的一种栅格地图通行成本的计算方法，其特征在于，所述预处理包括：通过以太网将所述激光传感器数据发送至工控机，所述工控机根据激光传感器和机器人平台之间的坐标转换将激光传感器数据投影到机器人平台。

4.如权利要求3所述的一种栅格地图通行成本的计算方法，其特征在于，所述坐标转换包括：以表示任一激光传感器数据点在极坐标系下的坐标，根据激光传感器直角坐标系和极坐标系之间的坐标转换关系计算该激光传感器数据点在直角坐标系下的坐标(x_i，y_i)；

以(l_x(t)，l_y(t))表示激光传感器直角坐标系和大地坐标系之间的坐标转换关系，利用将激光传感器数据点在直角坐标系下的坐标(x_i，y_i)转换为大地坐标系下的坐标(x_gi，y_gi)。

5.如权利要求1所述的一种栅格地图通行成本的计算方法，其特征在于，所述有效数据包括，随着机器人平台移动，不断对激光传感器数据点进行坐标转换，生成大地坐标系下的坐标集合。

6.如权利要求1所述的一种栅格地图通行成本的计算方法，其特征在于，所述对有效数据进行栅格化包括：将占据栅格通行成本设定为254，自由栅格通行成本设定为0，存储在工控机内；在每个小栅格(x_si，y_si，cost_si)中，x_si为行数，y_si为列数，cost_si为通行成本；所述占据栅格为存在激光传感器数据点的栅格；所述自由栅格为不存在激光传感器数据点的栅格。

7.如权利要求6所述的一种栅格地图通行成本的计算方法，其特征在于，所述对初始栅格地图进行膨胀处理包括，设定机器人平台外形的内切圆半径为r，外接圆半径为R，初始栅格地图中任一自由栅格为G_si(x_si,y_si,0)；

在栅格膨胀区域半径P的范围内，遍历G_si到占据栅格点的欧几里得距离，计算G_si到占据栅格的最小距离d_min(G_si)。

8.如权利要求7所述的一种栅格地图通行成本的计算方法，其特征在于，根据d_min(G_si)与机器人平台外形的内切圆半径r和外接圆半径R之间的大小关系，重新计算该栅格点的栅格通行成本cost_si，生成栅格地图

${\mathrm{cost}}_{si}=\left\{\begin{array}{cc}254& d_{\min }\left(G_{si}\right)=0\\ 253& 0<d_{\min }\left(G_{si}\right)\&le;r\\ e^{-f\&times;\left(d_{\min }\right(G_{si})-r)}\&times;253& r<d_{\min }\left(G_{si}\right)\&le;R\\ 0& R<d_{\min }\left(G_{si}\right)\&le;P\end{array}\right.$

式中，f为控制栅格通行成本衰减的比例因子，f越大，栅格通行成本衰减越快。