一种机器人导航路径规划方法

一种机器人导航路径规划方法
【专利摘要】本发明涉及一种机器人导航路径规划方法，包括：设定一个起始路标，以该起始路标为基础将所述机器人所处场景内设置的所有路标进行层级划分，并且将相邻层级直属关系路标之间的位置关系进行保存，作为路标索引；输入机器人当前路标与目标路标；确定当前路标与目标路标的层级；搜索当前路标与目标路标的共同关系路标；确定所述路径为当前路标到共同关系路标与共同关系路标到目标路标的路径，路径规划完成。本技术方案的一种机器人导航路径规划方法，采用多叉树结构将路标划分层级，将路标层级与相邻层级直属关系的路标之间的位置关系保存为路标索引，使得路径规划中算法简单、效率高，规划的路径唯一，很适合在嵌入式设备上运行。
【专利说明】
一种机器人导航路径规划方法
技术领域
[0001] 本发明属于机器人视觉导航领域，特别涉及一种机器人导航路径规划方法。
【背景技术】
[0002] 为了实现机器人视觉导航，需要设置路标和导航模块，以及利用路标与导航模块 创建机器人视觉导航地图。所述路标可以设置在室内的天花板上;所述导航模块为了拍摄 与识别所述路标，一般包括相机、图像处理单元与计算单元，所述相机设置在机器人头部顶 端，所述图像处理单元、计算单元设置在机器人内部;所述地图创建一般是指机器人利用所 述相机拍摄路标，并且利用导航模块内的图像处理单元、计算单元等进行路标识别、标定、 计算与确定各路标之间的位置关系。
[0003] 在创建了机器人视觉导航地图的基础上，导航过程中根据导航指令，还需要进行 机器人导航路径的规划。
[0004] 现有的机器人导航路径的规划方法一般采用智能算法、可视图法、自由空间法、人 工势场法等，这些方法各有优缺点，也常常结合起来使用，但无一例外，计算过程都很复杂， 对于某些导航路径规划简单的机器人来说，效率低，不便于嵌入式设备使用。

【发明内容】

[0005] 本发明的目的在于解决现有技术中的机器人导航路径规划方法计算复杂、效率 低、不便于嵌入式设备使用的问题。
[0006] 为实现上述目的，一种机器人导航路径规划方法，包括：
[0007] 步骤100:设定一个起始路标，以该起始路标为基础将所述机器人所处场景内设置 的所有路标进行层级划分，并且将相邻层级直属关系路标之间的位置关系进行保存，作为 路标索引；
[0008] 步骤200:输入所述机器人的当前路标与目标路标；
[0009] 步骤300:搜索所述当前路标与所述目标路标的共同关系路标；
[0010] 步骤400:确定所述路径为当前路标经所述共同关系路标到目标路标的路径，路径 规划完成。
[0011] 进一步地，所述步骤100包括：
[0012] 步骤110:设置路标的数量，并设定一个起始路标，所述起始路标为第一层级路标；
[0013] 步骤120:所述机器人从起始路标开始识别路标，对路标进行相机标定，并对每个 路标进行层级划分。
[0014] 步骤130:计算所有相邻层级的具有直属关系的两个路标坐标系的转换关系矩阵。
[0015] 进一步地，所述步骤100中以路标为节点，采用多叉树结构对路标进行层级划分， 所述起始路标为第一层级路标。
[0016] 进一步地，所述机器人从已划分层级的路标出发，能够同时识别到所述已划分层 级的路标与其他未划分层级路标时，将所述其他未划分层级路标的层级数划分为比所述已 划分层级的路标大一个层级。
[0017] 进一步地，所述直属关系是指，如果两个路标之间的层级相差一个层级，且层级较 大的路标的层级是根据层级较小的路标的层级而直接确定的，则这两个路标具有直属关 系。
[0018] 进一步地，所述相机标定的时刻是机器人能够稳定并且准确的反映出识别的路标 的时刻。
[0019] 进一步地，所述步骤130中，设0ΧΥ、0'X'Y'是两个相邻路标的坐标系，0'在0ΧΥ中的 坐标为(tx，ty)，由X轴到X'轴的角度为Θ，则所述转换关系矩阵Α是一个可逆矩阵，即：
[0020]
[0021] 进一步地，所述路标包括：
[0022] 第一坐标标记，用于确定路标坐标系的X轴；
[0023] 第二坐标标记，用于确定路标坐标系的Y轴；
[0024] 区域标记，用于确定路标所在的空间位置，所述区域标记包括多个等距离的信息 块，所述尺寸是所述信息块之间的距离或者信息块的长度。
[0025] 进一步地，所述机器人使用所述尺寸将实际空间与相机坐标系下的尺寸进行转 换。
[0026] 进一步地，所述步骤300包括：
[0027]步骤310:确定当前路标与目标路标的层级；
[0028]步骤320:比较当前路标层级与路标目标层级是否相同，若相同，执行步骤330,否 则执行步骤340;
[0029]步骤330:分别逐层级搜索当前路标与目标路标的上层级路标，直到搜索到同一个 路标，所述搜索到的同一个路标为共同关系路标，结束搜索；
[0030]步骤340:比较当前路标层级与目标路标层级的大小;若当前路标层级大于目标路 标层级，执行步骤350，否则执行步骤360;
[0031]步骤350:搜索当前路标的上层级路标，直到搜索到与目标路标同一层级的路标， 如果搜索到的该同一层级路标是目标路标，则所述目标路标为共同关系路标，结束搜索；否 则逐层搜索该同一层级路标和目标路标的上层级路标，直到搜索到同一个路标作为共同关 系路标，结束搜索；
[0032]步骤360:搜索目标路标的上层级路标，直到搜索到与当前路标同一层级的路标， 如果搜索到的该同一层级路标是当前路标，则所述当前路标为共同关系路标，结束搜索；否 则逐层搜索该同一层级路标和当前路标的上层级路标，直到搜索到同一个路标作为共同关 系路标，结束搜索。
[0033] 与现有技术相比，本技术方案具有以下优点：
[0034] 本技术方案的一种机器人导航路径规划方法，采用多叉树结构将路标划分层级， 将路标层级与相邻层级直属关系的路标之间的位置关系保存为路标索引，使得路径规划中 算法简单、效率高，规划的路径唯一，很适合在嵌入式设备上运行。
【附图说明】
[0035] 图1为本发明的一种机器人路径规划方法的流程图。
[0036] 图2为本发明的路标多叉树结构的不意图。
[0037] 图3为本发明搜索当前路标和目标路标的共同关系路标的流程图。
[0038] 图4-6为本发明划分路标层级的示意图。
[0039]图7为本发明的路标结构示意图。
[0040](注意：附图中的所示结构只是为了说明本发明特征的示意，并非是要依据附图所 示结构。）
【具体实施方式】
[0041] 为使本发明的上述目的、特征和优点能够更为明显易懂，下面结合附图对本发明 的具体实施例进行详细的说明。
[0042] 本发明提供了一种机器人导航路径规划方法。
[0043] 所述的机器人导航路径规划方法建立在机器人视觉导航地图的基础上。
[0044] 所述的机器人视觉导航地图的创建一般是指利用设置在机器人导航模块中的相 机拍摄机器人所处场景内设定的路标，并且利用设置在机器人导航模块中的图像处理单 元、计算单元等进行路标识别、标定、计算与确定各路标之间的位置关系，并保存在机器人 中，地图创建完成。
[0045] 参考图1，所述的机器人导航路径规划方法，包括：
[0046] 步骤100,设定一起始路标，以起始路标为基础将机器人所处场景内设置的所有路 标进行层级划分，并且将相邻层级直属关系路标之间的位置关系进行保存，作为路标索引。
[0047] 所述直属关系是指，如果两个路标之间的层级相差一个层级，且层级较大的路标 的层级是根据层级较小的路标的层级而直接确定的，则这两个路标具有直属关系。
[0048] 参考图2,假设所述路标的数量根据导航需求设置为7个，采用多叉树结构以路标 为节点，将7个路标划分为三个层级，起始路标E为第一层级路标，第四路标D、第六路标F为 第二层级路标，第一路标A、第二路标B、第三路标C、第七路标G为第三层级路标，并且保存7 个路标的层级与相邻层级直属关系路标之间的位置关系，作为路标索引。具体的层级划分 方法在后文说明。
[0049]所述多叉树结构常用于计算机领域，包括节点与树枝;树枝将节点连接起来就构 成了多叉树结构。
[0050]本实施例中，所述节点为路标，所述相邻层级直属关系路标之间的位置关系为树 枝。
[0051 ]步骤200，输入机器人当前路标与目标路标。
[0052]所述当前路标是指与机器人当前所处位置相对应的路标，所述目标路标，是指与 机器人导航目的地位置相对应的路标。
[0053]参考图2,假设机器人位于第七路标G相对应的位置，根据指令要去到第一路标A相 对应的位置，则，第七路标G为当前路标，第一路标A为目标路标。
[0054]所述当前路标与目标路标的输入，是将路标的ID值输入，每个路标具有不同的ID 值，ID值根据需要设定是数值；
[0055]所述当前路标与目标路标的输入，只要是给每个路标设定了不同的代码，代码可 以是任意形式，能够输入机器人，并且机器人能够识别即可。
[0056]步骤300,搜索当前路标与目标路标的共同关系路标。
[0057]参考图3,所述共同关系路标的确定，包括：
[0058]步骤310,确定当前路标与目标路标的层级。
[0059] 根据步骤100中设置的路标索引，能够知道所有路标的层级，因而也可以轻松的确 定当前路标的层级与目标路标的层级。
[0060] 步骤320，比较当前路标层级与路标目标层级，若相同，执行步骤330，若不同执行 步骤340。
[0061 ]所述的比较当前路标与目标路标的层级有两种结果:一是当前路标与目标路标层 级相同，二是当前路标与目标路标层级不同。
[0062] 所述的当前路标与目标路标层级不同也分为两种情况，一是当前路标层级大于目 标路标，二是当前路标层级小于目标路标。
[0063] 所述的层级大小是指，若N大于Μ，则第N层级路标的层级大于第Μ层级路标的层级。 [0064]比如，第三层级路标大于第二层级路标与第一层级路标，第二层级路标大于第一 层级路标。
[0065]步骤330,分别逐层级搜索当前路标与目标路标的上层级路标，直到搜索到同一个 路标，所述搜索到的同一个路标为共同关系路标，结束搜索。
[0066] 所述当前路标的上层级路标，是指与当前路标具有直属关系的层级逐层级减小的 所有路标;以此类推任意路标。
[0067] 所述当前路标的上层级路标包含上一层级路标，上一层级路标的层级比当前路标 小一层级，上二层级路标的层级比当前路标小二层，上Ν层级路标的层级比当前路标小Ν层 级；以此类推任意路标。
[0068]参考图2,第三层级的第七路标G的上层级路标包括第六路标D与起始路标Ε，具体 的说，第六路标D是当前路标的上一层级路标，层级为第二层级比第三层级小一层，起始路 标Ε是当前路标的上二层级路标，层级为第一层级比第三层级小二层。
[0069] 参考图2，假设第七路标G为当前路标，第一路标Α为目标路标，两个路标都是第三 层级路标，层级相同；分别逐层对第七路标G和第一路标A做上层级路标搜索，搜索到第七路 标G的上一层级路标为第六路标F，第一路标A的上一层级路标为第四路标D，未找到共同关 系路标;对第六路标F和第四路标D继续做上一层级路标的搜索，第六路标F和第四路标D的 上一层级路标都是起始路标E，则起始路标E为第六路标G和第一路标A的共同关系路标，结 束搜索。
[0070] 步骤340,比较当前路标的层级与目标路标的层级的大小，若当前路标层级大于目 标路标层级，执行步骤350，否则执行步骤360。
[0071]步骤350:搜索当前路标的上层级路标，直到搜索到与目标路标同一层级的路标， 如果搜索到的该同一层级路标是目标路标，所述目标路标确定为共同关系路标，结束搜索； 否则逐层搜索该同一层级路标和目标路标的上层级路标，直到搜索到同一个路标作为共同 关系路标，结束搜索。
[0072] 在本实施例中，参考图2,假设第七路标G为当前路标，其层级为第三层级，第四路 标D为目标路标，其层级为第二层级，第七路标G的层级大于第四路标D的层级;逐层搜索层 级大的第七路标G的上层级路标，直到搜索到与第四路标D同层级的路标，搜索到了第七路 标G的上一层级路标第六路标F，第六路标F与第四路标D同级，此时，分别逐层搜索第六路标 F与第四路标D的上层级路标，搜索到了第六路标F与第四路标D的上一层级路标起始路标E， 找到了共同关系路标起始路标E，结束搜索。
[0073] 在本实施例中，参考图2，假设第七路标G为当前路标，其层级为第三层级，第六路 标F为目标路标，其层级为第二层级，第七路标G的层级大于第六路标F的层级;逐层搜索层 级大的第七路标G的上层级路标，直到搜索到与第六路标F同层级的路标，搜索到了第七路 标G的上一层级路标第六路标F，此时，找到了共同关系路标，即目标路标第六路标F，结束搜 索。
[0074]步骤360:搜索目标路标的上层级路标，直到搜索到与当前路标同一层级的路标， 如果搜索到的该同一层级路标是当前路标，所述当前路标确定为共同关系路标，结束搜索； 否则逐层搜索该同一层级路标和当前路标的上层级路标，直到搜索到同一个路标作为共同 关系路标，结束搜索。
[0075] 在本实施例中，参考图2，假设第七路标G为目标路标，其层级为第三层级，第四路 标D为当前路标，其层级为第二层级，第七路标G的层级大于第四路标D的层级;逐层搜索层 级大的第七路标G的上层级路标，直到搜索到与第四路标D同层级的路标，搜索到了第七路 标G的上一层级路标第六路标F，第六路标F与第四路标D同级，此时，分别逐层搜索第六路标 F与第四路标D的上层级路标，搜索到了第六路标F与第四路标D的上一层级路标起始路标E， 找到了共同关系路标起始路标E，结束搜索。
[0076] 在本实施例中，参考图2，假设第七路标G为目标路标，其层级为第三层级，第六路 标F为当前路标，其层级为第二层级，第七路标G的层级大于第六路标F的层级;逐层搜索层 级大的第七路标G的上层级路标，直到搜索到与第六路标F同层级的路标，搜索到了第七路 标G的上一层级路标第六路标F，此时，找到了共同关系路标，即当前路标第六路标F，结束搜 索。
[0077] 步骤400,确定所述路径为当前路标经所述共同关系路标到目标路标的路径，路径 规划完成。
[0078] 根据步骤100中确定的路标索引，能够知道所述相邻层级直属关系路标之间的位 置关系，则可以知道当前路标到共同关系路标的路径，也可以知道共同关系路标到目标路 标的路径，两段路径结合在一起，路径规划完成。
[0079] 在本实施例中，参考图2，假设第七路标G为当前路标，第一路标A为目标路标，找到 第七路标G与第一路标A的共同关系路标后，根据索引中的相邻层级直属路标之间的位置关 系，能够知道当前路标到共同关系路标的路径，即第七路标G到第六路标F与第六路标F到起 始路标E的路径，也能够知道共同关系路标到目标路标的路径，即第一路标A到第四路标D与 第四路标D到起始路标E的路径，导航路径规划完成。
[0080] 上述方法如果基于与以路标为节点的多叉树结构的机器人视觉导航地图，会具有 工作量小，效率高的优点，因此所述的机器人路径规划方法建立在所述多叉树结构的机器 人视觉导航地图的基础上，步骤1〇〇是根据多叉树结构的机器人视觉导航地图来确定路标 索引，包括以下步骤：
[0081 ]步骤1，设置路标的数量，设定一起始路标。
[0082] 在本实施例中，参考图2,所述的机器人视觉导航地图的创建，是指以路标为节点 构建多叉树结构，将此多叉树结构作为机器人视觉导航地图并保存。
[0083] 在本实施例中，参考图2,所述路标的数量根据导航需求设置为7个，包括起始路标 E，第二层级一路标D，第二层级二路标F，第三层级一路标A，第三层级二路标B，第三层级三 路标C，第三层级四路标G。
[0084] 在其他实施例中，所述路标的数量根据导航的需求可以设置任意数量，比如2个， 比如3个，比如100个，只要所述路标数量能够满足机器人视觉导航需求即可。
[0085] 所述起始路标可以设定为所有路标中的任意一个，但一般选取位于所有路标所处 空间中央位置的路标。
[0086] 因为在多叉树地图构建过程中，相机标定与坐标转换关系矩阵计算都存在不可避 免的误差，并且误差是相乘叠加的；假设起始路标E与第二层级一路标D的相机标定与坐标 转换关系计算误差为A 1，第二层级一路标D与第三层级一路标A的相机标定与坐标转换关 系计算误差为A 2，则起始路标E到第三层级一路标A间的标定与计算误差为Δ 〇 = Δ 1* Δ 2， 因而，一般选择位于所有所述路标所处空间的中央位置的路标为起始路标，这样，在使用本 发明所述方法时，由起始路标E从其周围开始构建多叉树结构地图，可以使得多叉树结构地 图的层数少，以尽可能的减小误差。
[0087] 在本实施例中，所述起始路标E位于7个路标所处空间的中央位置。
[0088] 在其他实施例中，所述起始路标可以是所有路标中的任意一个，比如参考本实施 例，可以是第二层级一路标D，或者第二层级二路标F，或者第三层级二路标B。
[0089] 所述步骤1还包括设置所述路标中的尺寸，使用所述尺寸可以将实际空间与相机 坐标系下的尺寸进行转换。
[0090] 在本实施例中，所述路标采用如图7中所示的路标，所述路标包括第一坐标标记 11，用于确定坐标X轴;第二坐标标记12，用于确定坐标Y轴;还包括区域标记100，区域标记 设置于第一坐标标记11与第二坐标标记12确定的坐标内，用于确定路标所在的空间位置； 所述区域标记100内包括等距离的信息块，每个路标包含的所述信息块的数量和位置不同， 以区别路标；图2中所述的路标包括信息块110、信息块120、信息块130、信息块140、信息块 150、信息块160、信息块170、信息块180和信息块190,相邻两两信息块之间的距离为5cm。
[0091] 在其他实施例中，所述路标区域标记100内若只包括信息块110、信息块130和信息 块190,则，信息块110与信息块130之间的距离为10cm,也就是2个5cm,信息块130和信息块 190之间的距离为10cm，也是2个5cm，信息块110和信息块190之间的距离为在与路标坐标系 X轴与Y轴分别平行的两个方向上均为l〇cm。
[0092] 在其他实施例中，采用如图7中所述的路标，所述的信息块110、信息块120、信息块 130、信息块140、信息块150、信息块160、信息块170、信息块180和信息块190,相邻两两信息 块之间的距离为1 〇cm 〇
[0093] 在其他实施例中，采用如图7中所述的路标，所述的信息块110、信息块120、信息块 130、信息块140、信息块150、信息块160、信息块170、信息块180和信息块190,相邻两两信息 块之间的距离为8cm 〇
[0094] 在其他实施例中，采用如图7中所述的路标，所述的区域标记100内的所述信息块 可以不是3行3列，可以是4行4列或其他数量的行与其他数量的列。
[0095] 在本实施例中，根据路标中设置的尺寸可以将所述实际空间中的尺寸与相机坐标 系下的尺寸进行转换，比如，可以将实际空间中的宽度与相机坐标系下的宽度进行转换，或 者也可以将实际空间中的距离与相机坐标系下的距离进行转换;本实施例中，所述信息块 之间的距离为5cm，假设拍摄所述路标后，所述信息块之间的距离在所述相机坐标系下为 1mm，即可以确定所述相机坐标系下10mm的距离在实际空间中距离为50cm，或者比如所述信 息块的长度为lcm，所述信息块的长度在所述相机坐标系下为0.2mm，即可以确定所述相机 坐标系下l〇mm的长度在实际空间中长度为50cm。
[0096] 在其他实施例中，所述路标也可以采用诸如条形码、二维码等路标，只要确定并设 置好所述路标中的尺寸，能够使用所述尺寸可以将实际空间与相机坐标系下的尺寸进行转 换。
[0097] 步骤2，机器人从起始路标开始识别路标，对路标进行相机标定，并对每个路标进 行层级划分。
[0098] 所述相机标定常用于机器视觉研究，在本发明中，是指将所述路标的坐标系都转 化为所述相机的坐标系，并且得知所述路标在所述实际空间与相机坐标系下的尺寸的转换 数学关系，找出其定量的联系。
[0099] 所述路标的层级划分由小到大，确定起始路标为第一层级路标，然后依次将所有 路标进行层级划分。
[0100] 在本实施例中，所述起始路标E为第一层级路标，然后以起始路标E开始将所有路 标进行层级划分。
[0101] 在其他实施例中，所述路标的层级划分也可以由大到小，比如讲起始路标E划分为 的第五层路标，或者第N层路标，N可以为任意数，也可以是字母等。
[0102] 所述路标的层级大小是指:假设有第Μ层级路标与第N层级路标，Μ大于N，则，第Μ层 级路标的层级大于第Ν层级路标的层级。
[0103] 在本实施例中，起始路标Ε为第一层级路标，所述第二层级一路标D的层级数大于 起始路标Ε。
[0104] 在其他实施例中，所述路标的层级大小可做其他规定，比如假设有第Μ层级路标与 第Ν层级路标，Μ大于Ν，则，第Ν层级路标的层级大于第Μ层级路标的层级。
[0105] 所述路标的层级数确定采用如下方法:机器人从已划分层级的路标出发，能够同 时识别到所述已划分层级的路标与其他未划分层级路标时，将所述其他未划分层级路标的 层级数划分为比所述已划分层级的路标大一个层级。
[0106] 在本实施例中，所述机器人从起始路标开始识别路标，划分起始路标为第一层级 路标，在人工干预下，机器人从起始路标出发，在某一位置能够同时识别到起始路标与其他 路标时，将其他路标划分为第二层级路标，并对划分好层级的路标进行相机标定;所述第一 层级路标分别与所有的所述第二层级路标具有直属关系。
[0107] 所述路标的直属关系是指，所述两个路标之间的层级相差一个层级，且层级较大 的路标的层级是根据层级较小的路标的层级而直接确定的;所述具有直属关系的相邻层级 两个路标在多叉树结构中直接相连。
[0108] 然后，机器人从第二层级路标出发，在任何位置均不能够同时识别到起始路标与 其他路标，而只能在某一位置同时识别到第二层级路标与其他路标时，将其他路标划分为 第三层级路标，并对划分好层级的路标进行相机标定;采用同样的方法划分第四层级与第 四层级以上的路标。
[0109] 在本实施例中，参考图4,所述机器人1在人工干预下，从起始路标E开始行走，通过 所述导航模块中的相机2进行路标图像拍摄与识别，当所述导航模块中的相机2能够同时拍 摄到起始路标E与第二层级一路标D，并且准确且稳定的反映出拍摄到的是起始路标E与第 二层级一路标D时，人工干预，给机器人1 一个信号，机器人1对起始路标E与第二层级一路标 D均进行相机标定，分别计算起始路标E坐标系、第二层级一路标D坐标系和所述相机坐标系 的转换关系矩阵，并且，将所述第二层级一路标D划分为第二层级路标。
[0110]然后，参考图5,机器人1在人工干预下从第二层级一路标D出发沿着第三层级二路 标B的方向行走，通过所述导航模块中的相机2进行路标图像拍摄与识别，在此过程中，当机 器人1在任意位置不能同时识别到起始路标E与第三层级二路标B，而在某一位置能够准确 且稳定的反映出同时识别到的是第二层级一路标D与第三层级二路标B时，人工干预，给机 器人1 一个信号，机器人1对第二层级一路标D与第三层级二路标B均进行相机标定，分别计 算第二层级一路标D坐标系、第三层级二路标B坐标系和所述相机坐标系的转换关系矩阵， 并且，将所述第三层级二路标B划分为第三层级路标。
[0111] 最后，人工干预机器人回到第二层级一路标D，沿第三层级一路标A方向行走，当过 程中机器人能够准确并稳定的识别出第二层级一路标D与第三层级一路标A时，人工干预， 给机器人一个信号，机器人对第二层级一路标D与第三层级一路标A均进行相机标定，分别 计算第二层级一路标D坐标系、第三层级一路标A坐标系和所述相机坐标系的转换关系矩 阵，并且，将所述第三层级一路标A划分为第三层级路标。
[0112] 在其他实施例中，参考图6,所述机器人1在人工干预下，从起始路标E开始行走，通 过所述导航模块中的相机2进行路标图像拍摄与识别，当所述导航模块中的相机2能够同时 拍摄到起始路标E、第二层级一路标D与第二层级二路标F，并且准确且稳定的反映出拍摄到 的是起始路标E、第二层级一路标D与第二层级二路标F时，人工干预，给机器人一个信号，机 器人对起始路标E、第二层级一路标D、与第二层级二路标F均进行相机标定，分别计算起始 路标E坐标系、第二层级一路标D坐标系、第二层级二路标F和所述相机坐标系的转换关系矩 阵，并且，将所述第二层级一路标D、与第二层级二路标F划分为第二层级路标。
[0113] 在其他实施例中，采用同样的方法划分第四层级与第四层级以上的路标，即机器 人在任意位置均不能同时识别到路标与起始路标、或第一层级路标、或第二层级路标，而在 某一位置能够同时识别到路标与第三层级路标时，将路标划分为第四层级路标;第四层级 以上路标的层级划分也采用同样的方法。
[0114] 在本实施例中，参考图2,采用的是多叉树结构进行地图的创建，所述起始路标E为 第一层级路标，1张地图只有一个第一层级路标，所述第二层级一路标D划分为第二层级路 标，层级数比起始路标E多一层，所述第三层级一路标A、第三层级二路标B划分为第三层级 路标。
[0115] 在其他实施例中，可以采用其他分级或分类方法对路标进行划分，比如，也可以将 起始路标E与第二层级一路标D均确定为第一层级路标，其余路标再划分为第二层级或第二 以上层级路标。
[0116] 在其他实施例中，所述机器人可以从起始路标E开始沿任意方向行走，比如沿第二 层级二路标F方向开始行走。
[0117] 在本实施例中，机器人进行相机标定的时刻是机器人能够稳定并且准确的反映出 识别的路标时，一般设定为机器人能够在持续在3秒的时间内稳定并且准确的识别路标;并 且，本实施例中还要求机器人处于两个路标距离的二分之一处。
[0118] 在其他实施例中，时间可以根据需要进行调整为4秒或5秒或6秒及以上，只要满足 需要且能够实现即可；并且只要满足此条件即可，不需要机器人一定要处于两个路标距离 的二分之一处。
[0119] 在本实施例中，对所述路标进行相机标定，主要是为了能够将所述路标的坐标系 转化为所述照相机坐标系，每一个路标的坐标系都已经转换到了所述相机坐标系，便于步 骤3中的两两路标之间的关系运算，计算简单。
[0120] 在本实施例中，所述相机标定方法采用张正友基于针孔相机模型特定的平面标定 方法。
[0121] 在其他实施例中，根据需要可以采用其他相机标定方法，只要能够将所述路标坐 标系与所述相机坐标系进行转换即可，这样，每一个路标的坐标系都已经转换到了所述相 机坐标系，便于步骤3中的两两路标之间的关系运算，计算简单。
[0122] 步骤3,计算所有相邻层级的具有直属关系的两个路标坐标系的转换关系矩阵。
[0123] 本实施例中，计算的是多叉树结构中所有具有直属关系的相邻层级的两个路标坐 标系的转换关系矩阵，比如起始路标E与第一层级一路标D之间的转换关系矩阵，比如起始 路标E与第二层级二路标F之间的转换关系矩阵。
[0124] 本实施例中，设0ΧΥ、0 ' X ' Y '是两个相邻路标的坐标系，0 '在0ΧΥ中的坐标为（tx， ty)，由X轴到X'轴的角度为Θ，坐标转换公式是:x = x'cos0-y'sin0+tx，y = x'sin0+y'cos0+ ty〇
[0125] SP，转换矩阵如下所示：
[0126]
[0127] 为了更进一步简化公式，便于坐标系之间的转换计算，引入齐次坐标形式，则得到 以下公式：
[0128]
[0129 ]由此可得到相应的路标坐标系之间的可逆转换矩阵A:
[0130]
[0131] 由于相邻层级的具有直属关系的两个路标的坐标系均已转换为相机坐标系，两个 路标坐标系原点在所述相机坐标系下的坐标计算简单，两个路标坐标系的旋转角Θ也计算 简单，因而计算出转换矩阵中的参数，则能够确定相邻层级的具有直属关系的坐标转换关 系，并将转换关系矩阵保存。
[0132] 所述当划分层级的路标数量与设置的路标数量相同时，已完成以上步骤1、步骤2、 步骤3后，所述机器人视觉导航地图的创建完成，在本实施例中，得到的是一个机器人存储 着的，以路标为节点的多叉树结构，相邻层级的路标坐标系的转换关系矩阵也存在机器人 中作为多叉树结构的相邻层级的具有直属关系的两个路标之间的链，这样，导航时，机器人 可以将相邻层级的具有直属关系的路标之间的实际距离计算出来，以完成导航。
[0133] 所述机器人导航路径规划方法就基于上述的以多叉树结构创建的机器人视觉导 航系统。
[0134] 本技术方案的一种机器人导航路径规划方法，采用多叉树结构将路标划分层级， 将路标层级与相邻层级直属关系的路标之间的位置关系保存为路标索引，使得路径规划中 算法简单、效率高，规划的路径唯一，很适合在嵌入式设备上运行。
[0135] 以上所述，仅为本发明的较佳实施例而已，并非用于限定本发明的保护范围，凡在 本发明的精神和原则之内所作的任何修改、等同替换和改进等，均应包含在本发明的保护 范围之内。
【主权项】
1. 一种机器人导航路径规划方法，其特征在于，包括： 步骤100:设定一个起始路标，W该起始路标为基础将所述机器人所处场景内设置的所 有路标进行层级划分，并且将相邻层级直属关系路标之间的位置关系进行保存，作为路标 索引； 步骤200:输入所述机器人的当前路标与目标路标； 步骤300:捜索所述当前路标与所述目标路标的共同关系路标； 步骤400:确定所述路径为当前路标经所述共同关系路标到目标路标的路径，路径规划 完成。2. 根据权利要求1所述的一种机器人导航路径规划方法，其特征在于，所述步骤100包 括： 步骤110:设置路标的数量，并设定一个起始路标，所述起始路标为第一层级路标； 步骤120:所述机器人从起始路标开始识别路标，对路标进行相机标定，并对每个路标 进行层级划分。 步骤130:计算所有相邻层级的具有直属关系的两个路标坐标系的转换关系矩阵。3. 根据权利要求1所述的一种机器人导航路径规划方法，其特征在于，所述步骤100中 W路标为节点，采用多叉树结构对路标进行层级划分，所述起始路标为第一层级路标。4. 根据权利要求1所述的一种机器人导航路径规划方法，其特征在于，所述机器人从已 划分层级的路标出发，能够同时识别到所述已划分层级的路标与其他未划分层级路标时， 将所述其他未划分层级路标的层级数划分为比所述已划分层级的路标大一个层级。5. 根据权利要求1所述的一种机器人导航路径规划方法，其特征在于，所述直属关系是 指，如果两个路标之间的层级相差一个层级，且层级较大的路标的层级是根据层级较小的 路标的层级而直接确定的，则运两个路标具有直属关系。6. 根据权利要求2所述的一种机器人导航路径规划方法，其特征在于，所述相机标定的 时刻是机器人能够稳定并且准确的反映出识别的路标的时刻。7. 根据权利要求2所述的一种机器人导航路径规划方法，其特征在于，所述步骤130中， 设OXY、0 'X ' Y '是两个相邻路标的坐标系，0 '在OXY中的坐标为(tx，ty)，由X轴到X '轴的角度 为9，则所述转换关系矩阵A是一个可逆矩阵，即：8. 根据权利要求1所述的一种机器人导航路径规划方法，其特征在于，所述路标包括： 第一坐标标记，用于确定路标坐标系的X轴. 第二坐标标记，用于确定路标坐标系的Y轴. 区域标记，用于确定路标所在的空间位置，所述区域标记包括多个等距离的信息块，所 述尺寸是所述信息块之间的距离或者信息块的长度。9. 根据权利要求8所述的一种机器人导航路径规划方法，其特征在于，所述机器人使用 所述尺寸将实际空间与相机坐标系下的尺寸进行转换。10. 根据权利要求1所述的一种机器人导航路径规划方法，其特征在于，所述步骤300包 括： 步骤310:确定当前路标与目标路标的层级； 步骤320:比较当前路标层级与路标目标层级，若相同执行步骤330，否则执行步骤340; 步骤330:分别逐层级捜索当前路标与目标路标的上层级路标，直到捜索到同一个路 标，所述捜索到的同一个路标为共同关系路标，结束捜索； 步骤340:比较当前路标层级与目标路标层级的大小;若当前路标层级大于目标路标层 级，执行步骤350，否则执行步骤360; 步骤350:捜索当前路标的上层级路标，直到捜索到与目标路标同一层级的路标，如果 捜索到的该同一层级路标是目标路标，则所述目标路标为共同关系路标，结束捜索;否则逐 层捜索该同一层级路标和目标路标的上层级路标，直到捜索到同一个路标作为共同关系路 标，结束捜索； 步骤360:捜索目标路标的上层级路标，直到捜索到与当前路标同一层级的路标，如果 捜索到的该同一层级路标是当前路标，则所述当前路标为共同关系路标，结束捜索;否则逐 层捜索该同一层级路标和当前路标的上层级路标，直到捜索到同一个路标作为共同关系路 标，结束捜索。
【文档编号】G01C21/34GK105974928SQ201610614320
【公开日】2016年9月28日
【申请日】2016年7月29日
【发明人】乔徽, 张腾飞
【申请人】哈尔滨工大服务机器人有限公司