一种步长可变地图生成方法
【技术领域】
[0001] 本发明属于计算机图形技术和机器人导航领域，特别是一种步长可变地图生成方 法。
【背景技术】
[0002] 机器人感知环境的一种有效方法是利用一张先验地图把环境给存储起来，然后根 据传感器对机器人自身定位，从而确定机器人与环境的关系。目前地图表示主要有两种，格 子地图和合成地图。
[0003] 格子地图，就是将周围的环境用一个个格子表示，如果有障碍物，那就是1，如果 没，那就是0。但是考虑到传感器信号不是那么精确，现在普遍使用的是Certainty Grid Map(确定性格子地图），传感器信号返回后并不是直接转换成0，1，而是一个概率值，表示这 个格子有障碍物的可能性，如果几次传感器的读数都表面同一个格子里面有障碍物，则这 个格子有障碍物的可能性就很高。这样做的好处是可以避免传感器误差。格子地图的好处 是比较精确，对机器人周围的环境表示比较完整。但是缺点是太耗内存，比较耗计算资源。 尤其是一些处理速度和存储容量都有限制的微控制器。
[0004] 合成地图综合了格子地图和节点地图。在局部用格子表示，在全局用节点表示。是 目前广泛使用的地图表示法。但是这种表示法有个问题就是如何有效的将格子地图和节点 地图融合在一起。
【发明内容】
[0005] 本发明的目的在于克服现有技术的不足，提供一种步长可变地图生成方法，该方 法将大数据量的三维地图压缩成小数据量的二维地图，方便路径规划算法在其基础上的实 现非障碍点地图是步长可变地图，数据量小，运算量小，提高数据处理的实时性非障碍点地 图具有节点间联系，大大简化路径规划计算量。
[0006] 本发明的目的是通过以下技术方案来实现的：一种步长可变地图生成方法，它包 括如下步骤：
[0007] si:三维地图分层处理，三维地图的输入之后，根据机器人的高度，将机器人分为 底盘部分，躯干部分，头部三层，将三维地图也按这种分法分为3层三维地图，分别为底盘部 分地图，躯干部分地图，头部地图；
[0008] S2:二维障碍点地图生成，将躯干部分地图映射到二维地图，形成二维障碍点地 图；
[0009] S3:非障碍点地图生成，由二维障碍点地图生成非障碍点地图；
[0010] S4:建立非障碍节点间联系，构成一张完整的步长可变地图。
[0011] 所述的三维地图分层处理步骤包括如下子步骤：
[0012] S11:利用坐标跟踪和转换库对三维环境地图进行坐标变换，使得三维环境地图的 坐标系的x〇y平面与地面平行；
[0013] S12:通过KD-tree方法对三维环境地图的数据组织和排序；
[0014] S13:根据机器人的高度将机器人分为底盘部分，躯干部分以及头部；
[0015] S14:将三维环境地图按机器人的分层进行水平切割，分成三层不同的三维环境地 图。
[0016] 所述的二维障碍点地图生成步骤包括如下子步骤：
[0017] S21:将步骤S1中得到的三层三维环境地图中的躯干部分三维图映射到二维平面；
[0018] S22:对得到的二维平面地图进行去噪处理，以二维地图上的每个障碍点作为圆 点，以阈值半径r0作为半径画圆，判断在该圆内的障碍点个数是否大于阈值K，如果大于，则 保留该点，否则去处；
[0019] S23:对去噪处理后的二维地图作进一步的降采样处理，得到一张二维障碍点地 图。
[0020] 所述的非障碍点地图生成包括如下子步骤：
[0021] S31:在一张障碍图中，给定一个初始的非障碍点，计算最靠近该非障碍点的障碍 点，非障碍点到障碍点的距离为r，已知机器人底盘半径为R，如果r大于R，则说明该初始的 非障碍点有效，否则，重新选取初始的非障碍点，再次重复以上过程；
[0022] S32:以选取的非障碍点作为圆点，r为半径画初始圆，将初始圆作k等分，k>l，除 障碍点外的等分点为非障碍点候选点；
[0023] S33:选取一个等分点编号为A，按字母表顺序逆时针编号；
[0024] 534:定义一个11 = 1;
[0025] S35:确定了第i代的非障碍点之后，计算第i+Ι代的非障碍点，按照编号顺序规则， 计算第i + Ι代第1个非障碍候选点到最近障碍点的距离r1+1>1，判断r1+1>1是否大于R小于4R， 如果满足，则将该点定为非障碍点，并以该点为圆心，r 1+1>1为半径画圆，做k等分，判断哪些 等分点包含在已知圆内，并抛弃这些包含在已知圆内的点，如果r i+1>1不满足大于R小于4R， 则抛弃该点；
[0026] S36:将第i+Ι代所有的非障碍候选点按照步骤1方法运行；
[0027] S37:i = i+1，重复 S35 和 S36。
[0028] 所述的步骤S32中k等分一般为16等分，k越大，生成的非障碍点越多;将初始圆做k 等分时以最靠近该障碍非障碍点的障碍点为一个等分点。
[0029] 所述步骤S33中选取的等分点最好为位于经过障碍点与初始非障碍点的直径上不 是障碍点的等分点。
[0030] 所述的建立非障碍节点间联系包括:连接两个非障碍点〇1〇2,在〇1〇2中寻找Q1Q1' 和Q2Q2'，使得叭01'=0202'=21?，？1和？2分别为叭01'和0202'与〇1〇2的交点，？1和？2求解 如下：
[0031] 已知：
[0032]
[0033] 贝1J:
[0038] 当向量plp2和向量〇1〇2方向相反时，〇1和〇2可以相互连接，即机器人可以在〇1和 〇2间通行；当向量plp2和向量〇1〇2同向时，计算plp2的中点〇3,并求该点到障碍物的最近距 离r c，若rc大于R，ol和〇2可以相互连接，否则，〇1和〇2不可以相互连接。
[0039] 所述的中点〇3的计算方法为:设〇3的坐标为(X3，y3)，则
[0040]
[0041] 本发明的有益效果是:本发明提供了一种步长可变地图生成方法，该方法将大数 据量的三维地图压缩成小数据量的二维地图，方便路径规划算法在其基础上的实现非障碍 点地图是步长可变地图，数据量小，运算量小，提高了数据处理的实时性非障碍点地图具有 节点间联系，大大简化了路径规划计算量。
【附图说明】
[0042]图1为本发明的方法流程图；
[0043]图2为初始非障碍点生成示意图；
[0044] 图3为非障碍点生成示意图a;
[0045] 图4为非障碍点生成示意图b;
[0046] 图5为非障碍点生成示意图c;
[0047]图6为非障碍点生成示意图d;
[0048] 图7为建立非障碍节点联系示意图a;
[0049] 图8为建立非障碍节点联系示意图b。
【具体实施方式】
[0050] 下面结合附图进一步详细描述本发明的技术方案，但本发明的保护范围不局限于 以下所述。
[0051] 如图1所示，从即时定位和建图（SLAM)系统中得到室内场景的三维环境地图，由于 三维地图数据量太大，需经过三维地图分层处理模块进行三维环境地图分层，通过二维障 碍点地图生成模块将数据压缩，选取机器人躯干部分三维图作为压缩源，映射为二维地图， 即生成了二维障碍物地图，该模块不但对数据降维，而且还确保了机器人躯干部分三维图 信息不丢失，便于机器人的路径规划和避障。在给定一个初始的非障碍点之后，非障碍点地 图生成模块会根据该点，计算并生成所有非障碍点，形成一张非障点地图。通过建立非障碍 节点间联系模块，确立了非障碍点之间的连接关系，最后将障碍点地图和非障碍点地图保 存，形成一张先验的步长可变地图，作为机器人的导航地图。
[0052] -种步长可变地图生成方法，它包括如下步骤：
[0053] S1:三维地图分层处理，三维地图的输入之后，根据机器人的高度，将机器人分为 底盘部分，躯干部分，头部三层，将三维地图也按这种分法分为3层三维地图，分别为底盘部 分地图，躯干部分地图，头部地图；
[0054] S2:二维障碍点地图生成，将躯干部分地图映射到二维地图，形成二维障碍点地 图；
[0055] S3:非障碍点地图生成，由二维障碍点地图生成非障碍点地图；
[0056] S4:建立非障碍节点间联系，构成一张完整的步长可变地图。
[0057] 所述的三维地图分层处理步骤包括如下子步骤：
[0058] SI 1:利用坐标跟踪和转换库对三维环境地图进行坐标变换，使得三维环境地图的 坐标系的x〇y平面与地面平行；
[0059] S12:通过KD-tree方法对三维环境地图的数据组织和排序，可采用PCL库里面的函 数来实现的；
[0060] S13:根据机器人的高度将机器人分为底盘部分，躯干部分