CN105974928B - 一种机器人导航路径规划方法 - Google Patents

Abstract

本发明涉及一种机器人导航路径规划方法，包括：设定一个起始路标，以该起始路标为基础将所述机器人所处场景内设置的所有路标进行层级划分，并且将相邻层级直属关系路标之间的位置关系进行保存，作为路标索引；输入机器人当前路标与目标路标；确定当前路标与目标路标的层级；搜索当前路标与目标路标的共同关系路标；确定所述路径为当前路标到共同关系路标与共同关系路标到目标路标的路径，路径规划完成。本技术方案的一种机器人导航路径规划方法，采用多叉树结构将路标划分层级，将路标层级与相邻层级直属关系的路标之间的位置关系保存为路标索引，使得路径规划中算法简单、效率高，规划的路径唯一，很适合在嵌入式设备上运行。

Description

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

技术领域

本发明属于机器人视觉导航领域，特别涉及一种机器人导航路径规划方法。

背景技术

为了实现机器人视觉导航,需要设置路标和导航模块，以及利用路标与导航模块创建机器人视觉导航地图。所述路标可以设置在室内的天花板上；所述导航模块为了拍摄与识别所述路标，一般包括相机、图像处理单元与计算单元，所述相机设置在机器人头部顶端，所述图像处理单元、计算单元设置在机器人内部；所述地图创建一般是指机器人利用所述相机拍摄路标，并且利用导航模块内的图像处理单元、计算单元等进行路标识别、标定、计算与确定各路标之间的位置关系。

在创建了机器人视觉导航地图的基础上，导航过程中根据导航指令，还需要进行机器人导航路径的规划。

现有的机器人导航路径的规划方法一般采用智能算法、可视图法、自由空间法、人工势场法等，这些方法各有优缺点，也常常结合起来使用，但无一例外，计算过程都很复杂，对于某些导航路径规划简单的机器人来说，效率低，不便于嵌入式设备使用。

发明内容

本发明的目的在于解决现有技术中的机器人导航路径规划方法计算复杂、效率低、不便于嵌入式设备使用的问题。

为实现上述目的，一种机器人导航路径规划方法，包括：

步骤100：设定一个起始路标，以该起始路标为基础将所述机器人所处场景内设置的所有路标进行层级划分，并且将相邻层级直属关系路标之间的位置关系进行保存，作为路标索引；

步骤200：输入所述机器人的当前路标与目标路标；

步骤300：搜索所述当前路标与所述目标路标的共同关系路标；

步骤400：确定所述路径为当前路标经所述共同关系路标到目标路标的路径，路径规划完成。

进一步地，所述步骤100包括：

步骤110：设置路标的数量，并设定一个起始路标，所述起始路标为第一层级路标；

步骤120：所述机器人从起始路标开始识别路标，对路标进行相机标定，并对每个路标进行层级划分。

步骤130：计算所有相邻层级的具有直属关系的两个路标坐标系的转换关系矩阵。

进一步地，所述步骤100中以路标为节点，采用多叉树结构对路标进行层级划分，所述起始路标为第一层级路标。

进一步地，所述机器人从已划分层级的路标出发，能够同时识别到所述已划分层级的路标与其他未划分层级路标时，将所述其他未划分层级路标的层级数划分为比所述已划分层级的路标大一个层级。

进一步地，所述直属关系是指，如果两个路标之间的层级相差一个层级，且层级较大的路标的层级是根据层级较小的路标的层级而直接确定的，则这两个路标具有直属关系。

进一步地，所述相机标定的时刻是机器人能够稳定并且准确的反映出识别的路标的时刻。

进一步地，所述步骤130中，设OXY、O'X'Y'是两个相邻路标的坐标系， O'在OXY中的坐标为(tx,ty),由X轴到X'轴的角度为θ，则所述转换关系矩阵 A是一个可逆矩阵，即：

进一步地，所述路标包括：

第一坐标标记，用于确定路标坐标系的X轴；

第二坐标标记，用于确定路标坐标系的Y轴；

区域标记，用于确定路标所在的空间位置，所述区域标记包括多个等距离的信息块，所述尺寸是所述信息块之间的距离或者信息块的长度。

进一步地，所述机器人使用所述尺寸将实际空间与相机坐标系下的尺寸进行转换。

进一步地，所述步骤300包括：

步骤310：确定当前路标与目标路标的层级；

步骤320：比较当前路标层级与路标目标层级是否相同，若相同,执行步骤330，否则执行步骤340；

步骤330：分别逐层级搜索当前路标与目标路标的上层级路标，直到搜索到同一个路标，所述搜索到的同一个路标为共同关系路标，结束搜索；

步骤340：比较当前路标层级与目标路标层级的大小；若当前路标层级大于目标路标层级，执行步骤350，否则执行步骤360；

步骤350：搜索当前路标的上层级路标，直到搜索到与目标路标同一层级的路标，如果搜索到的该同一层级路标是目标路标，则所述目标路标为共同关系路标，结束搜索；否则逐层搜索该同一层级路标和目标路标的上层级路标，直到搜索到同一个路标作为共同关系路标，结束搜索；

步骤360：搜索目标路标的上层级路标，直到搜索到与当前路标同一层级的路标，如果搜索到的该同一层级路标是当前路标，则所述当前路标为共同关系路标，结束搜索；否则逐层搜索该同一层级路标和当前路标的上层级路标，直到搜索到同一个路标作为共同关系路标，结束搜索。

与现有技术相比，本技术方案具有以下优点：

本技术方案的一种机器人导航路径规划方法，采用多叉树结构将路标划分层级，将路标层级与相邻层级直属关系的路标之间的位置关系保存为路标索引，使得路径规划中算法简单、效率高，规划的路径唯一，很适合在嵌入式设备上运行。

附图说明

图1为本发明的一种机器人路径规划方法的流程图。

图2为本发明的路标多叉树结构的示意图。

图3为本发明搜索当前路标和目标路标的共同关系路标的流程图。

图4-图6为本发明划分路标层级的示意图。

图7为本发明的路标结构示意图。

(注意：附图中的所示结构只是为了说明本发明特征的示意，并非是要依据附图所示结构。)

具体实施方式

为使本发明的上述目的、特征和优点能够更为明显易懂，下面结合附图对本发明的具体实施例进行详细的说明。

本发明提供了一种机器人导航路径规划方法。

所述的机器人导航路径规划方法建立在机器人视觉导航地图的基础上。

所述的机器人视觉导航地图的创建一般是指利用设置在机器人导航模块中的相机拍摄机器人所处场景内设定的路标，并且利用设置在机器人导航模块中的图像处理单元、计算单元等进行路标识别、标定、计算与确定各路标之间的位置关系，并保存在机器人中，地图创建完成。

参考图1，所述的机器人导航路径规划方法，包括：

步骤100，设定一起始路标，以起始路标为基础将机器人所处场景内设置的所有路标进行层级划分，并且将相邻层级直属关系路标之间的位置关系进行保存，作为路标索引。

所述直属关系是指，如果两个路标之间的层级相差一个层级，且层级较大的路标的层级是根据层级较小的路标的层级而直接确定的，则这两个路标具有直属关系。

参考图2，假设所述路标的数量根据导航需求设置为7个，采用多叉树结构以路标为节点，将7个路标划分为三个层级，起始路标E为第一层级路标，第四路标D、第六路标F为第二层级路标，第一路标A、第二路标B、第三路标C、第七路标G为第三层级路标，并且保存7个路标的层级与相邻层级直属关系路标之间的位置关系，作为路标索引。具体的层级划分方法在后文说明。

所述多叉树结构常用于计算机领域，包括节点与树枝；树枝将节点连接起来就构成了多叉树结构。

本实施例中，所述节点为路标，所述相邻层级直属关系路标之间的位置关系为树枝。

步骤200，输入机器人当前路标与目标路标。

所述当前路标是指与机器人当前所处位置相对应的路标，所述目标路标，是指与机器人导航目的地位置相对应的路标。

参考图2，假设机器人位于第七路标G相对应的位置，根据指令要去到第一路标A相对应的位置，则，第七路标G为当前路标，第一路标A为目标路标。

所述当前路标与目标路标的输入，是将路标的ID值输入，每个路标具有不同的ID值，ID值根据需要设定是数值；

所述当前路标与目标路标的输入，只要是给每个路标设定了不同的代码，代码可以是任意形式，能够输入机器人，并且机器人能够识别即可。

步骤300，搜索当前路标与目标路标的共同关系路标。

参考图3，所述共同关系路标的确定，包括：

步骤310，确定当前路标与目标路标的层级。

根据步骤100中设置的路标索引，能够知道所有路标的层级，因而也可以轻松的确定当前路标的层级与目标路标的层级。

步骤320，比较当前路标层级与路标目标层级，若相同,执行步骤330，若不同执行步骤340。

所述的比较当前路标与目标路标的层级有两种结果：一是当前路标与目标路标层级相同，二是当前路标与目标路标层级不同。

所述的当前路标与目标路标层级不同也分为两种情况，一是当前路标层级大于目标路标，二是当前路标层级小于目标路标。

所述的层级大小是指，若N大于M，则第N层级路标的层级大于第M 层级路标的层级。

比如，第三层级路标大于第二层级路标与第一层级路标，第二层级路标大于第一层级路标。

步骤330，分别逐层级搜索当前路标与目标路标的上层级路标，直到搜索到同一个路标，所述搜索到的同一个路标为共同关系路标，结束搜索。

所述当前路标的上层级路标，是指与当前路标具有直属关系的层级逐层级减小的所有路标；以此类推任意路标。

所述当前路标的上层级路标包含上一层级路标，上一层级路标的层级比当前路标小一层级，上二层级路标的层级比当前路标小二层，上N层级路标的层级比当前路标小N层级；以此类推任意路标。

参考图2，第三层级的第七路标G的上层级路标包括第六路标D与起始路标E，具体的说，第六路标D是当前路标的上一层级路标，层级为第二层级比第三层级小一层，起始路标E是当前路标的上二层级路标，层级为第一层级比第三层级小二层。

参考图2，假设第七路标G为当前路标，第一路标A为目标路标，两个路标都是第三层级路标，层级相同；分别逐层对第七路标G和第一路标A做上层级路标搜索，搜索到第七路标G的上一层级路标为第六路标F，第一路标A的上一层级路标为第四路标D,未找到共同关系路标；对第六路标F和第四路标D继续做上一层级路标的搜索，第六路标F和第四路标D的上一层级路标都是起始路标E,则起始路标E为第六路标G和第一路标A的共同关系路标，结束搜索。

步骤340，比较当前路标的层级与目标路标的层级的大小，若当前路标层级大于目标路标层级，执行步骤350，否则执行步骤360。

步骤350：搜索当前路标的上层级路标，直到搜索到与目标路标同一层级的路标，如果搜索到的该同一层级路标是目标路标，所述目标路标确定为共同关系路标，结束搜索；否则逐层搜索该同一层级路标和目标路标的上层级路标，直到搜索到同一个路标作为共同关系路标，结束搜索。

在本实施例中，参考图2，假设第七路标G为当前路标，其层级为第三层级，第四路标D为目标路标，其层级为第二层级，第七路标G的层级大于第四路标D的层级；逐层搜索层级大的第七路标G的上层级路标，直到搜索到与第四路标D同层级的路标，搜索到了第七路标G的上一层级路标第六路标F,第六路标F与第四路标D同级，此时，分别逐层搜索第六路标F与第四路标D的上层级路标，搜索到了第六路标F与第四路标D的上一层级路标起始路标E，找到了共同关系路标起始路标E，结束搜索。

在本实施例中，参考图2，假设第七路标G为当前路标，其层级为第三层级，第六路标F为目标路标，其层级为第二层级，第七路标G的层级大于第六路标F的层级；逐层搜索层级大的第七路标G的上层级路标，直到搜索到与第六路标F同层级的路标，搜索到了第七路标G的上一层级路标第六路标F,此时，找到了共同关系路标，即目标路标第六路标F，结束搜索。

步骤360：搜索目标路标的上层级路标，直到搜索到与当前路标同一层级的路标，如果搜索到的该同一层级路标是当前路标，所述当前路标确定为共同关系路标，结束搜索；否则逐层搜索该同一层级路标和当前路标的上层级路标，直到搜索到同一个路标作为共同关系路标，结束搜索。

在本实施例中，参考图2，假设第七路标G为目标路标，其层级为第三层级，第四路标D为当前路标，其层级为第二层级，第七路标G的层级大于第四路标D的层级；逐层搜索层级大的第七路标G的上层级路标，直到搜索到与第四路标D同层级的路标，搜索到了第七路标G的上一层级路标第六路标F,第六路标F与第四路标D同级，此时，分别逐层搜索第六路标F与第四路标D的上层级路标，搜索到了第六路标F与第四路标D的上一层级路标起始路标E，找到了共同关系路标起始路标E，结束搜索。

在本实施例中，参考图2，假设第七路标G为目标路标，其层级为第三层级，第六路标F为当前路标，其层级为第二层级，第七路标G的层级大于第六路标F的层级；逐层搜索层级大的第七路标G的上层级路标，直到搜索到与第六路标F同层级的路标，搜索到了第七路标G的上一层级路标第六路标F,此时，找到了共同关系路标，即当前路标第六路标F，结束搜索。

步骤400，确定所述路径为当前路标经所述共同关系路标到目标路标的路径，路径规划完成。

根据步骤100中确定的路标索引，能够知道所述相邻层级直属关系路标之间的位置关系，则可以知道当前路标到共同关系路标的路径，也可以知道共同关系路标到目标路标的路径，两段路径结合在一起，路径规划完成。

在本实施例中，参考图2，假设第七路标G为当前路标，第一路标A为目标路标，找到第七路标G与第一路标A的共同关系路标后，根据索引中的相邻层级直属路标之间的位置关系，能够知道当前路标到共同关系路标的路径，即第七路标G到第六路标F与第六路标F到起始路标E的路径，也能够知道共同关系路标到目标路标的路径，即第一路标A到第四路标D与第四路标D到起始路标E的路径，导航路径规划完成。

上述方法如果基于与以路标为节点的多叉树结构的机器人视觉导航地图，会具有工作量小，效率高的优点，因此所述的机器人路径规划方法建立在所述多叉树结构的机器人视觉导航地图的基础上，步骤100是根据多叉树结构的机器人视觉导航地图来确定路标索引，包括以下步骤：

步骤1，设置路标的数量，设定一起始路标。

在本实施例中，参考图2，所述的机器人视觉导航地图的创建，是指以路标为节点构建多叉树结构，将此多叉树结构作为机器人视觉导航地图并保存。

在本实施例中，参考图2，所述路标的数量根据导航需求设置为7个，包括起始路标E，第二层级一路标D,第二层级二路标F，第三层级一路标A,第三层级二路标B，第三层级三路标C，第三层级四路标G。

在其他实施例中，所述路标的数量根据导航的需求可以设置任意数量，比如2个，比如3个,比如100个，只要所述路标数量能够满足机器人视觉导航需求即可。

所述起始路标可以设定为所有路标中的任意一个，但一般选取位于所有路标所处空间中央位置的路标。

因为在多叉树地图构建过程中，相机标定与坐标转换关系矩阵计算都存在不可避免的误差，并且误差是相乘叠加的；假设起始路标E与第二层级一路标D的相机标定与坐标转换关系计算误差为Δ1，第二层级一路标D与第三层级一路标A的相机标定与坐标转换关系计算误差为Δ2，则起始路标E 到第三层级一路标A间的标定与计算误差为Δ0＝Δ1*Δ2，因而，一般选择位于所有所述路标所处空间的中央位置的路标为起始路标，这样，在使用本发明所述方法时，由起始路标E从其周围开始构建多叉树结构地图，可以使得多叉树结构地图的层数少，以尽可能的减小误差。

在本实施例中，所述起始路标E位于7个路标所处空间的中央位置。

在其他实施例中，所述起始路标可以是所有路标中的任意一个，比如参考本实施例，可以是第二层级一路标D,或者第二层级二路标F，或者第三层级二路标B。

所述步骤1还包括设置所述路标中的尺寸，使用所述尺寸可以将实际空间与相机坐标系下的尺寸进行转换。

在本实施例中，所述路标采用如图7中所示的路标，所述路标包括第一坐标标记11，用于确定坐标X轴；第二坐标标记12，用于确定坐标Y轴；还包括区域标记100，区域标记设置于第一坐标标记11与第二坐标标记12 确定的坐标内，用于确定路标所在的空间位置；所述区域标记100内包括等距离的信息块，每个路标包含的所述信息块的数量和位置不同，以区别路标；图2中所述的路标包括信息块110、信息块120、信息块130、信息块140、信息块150、信息块160、信息块170、信息块180和信息块190，相邻两两信息块之间的距离为5cm。

在其他实施例中，所述路标区域标记100内若只包括信息块110、信息块130和信息块190，则，信息块110与信息块130之间的距离为10cm, 也就是2个5cm,信息块130和信息块190之间的距离为10cm,也是2个 5cm，信息块110和信息块190之间的距离为在与路标坐标系X轴与Y轴分别平行的两个方向上均为10cm。

在其他实施例中，采用如图7中所述的路标，所述的信息块110、信息块120、信息块130、信息块140、信息块150、信息块160、信息块170、信息块180和信息块190，相邻两两信息块之间的距离为10cm。

在其他实施例中，采用如图7中所述的路标，所述的信息块110、信息块120、信息块130、信息块140、信息块150、信息块160、信息块170、信息块180和信息块190，相邻两两信息块之间的距离为8cm。

在其他实施例中，采用如图7中所述的路标，所述的区域标记100内的所述信息块可以不是3行3列，可以是4行4列或其他数量的行与其他数量的列。

在本实施例中，根据路标中设置的尺寸可以将所述实际空间中的尺寸与相机坐标系下的尺寸进行转换，比如，可以将实际空间中的宽度与相机坐标系下的宽度进行转换，或者也可以将实际空间中的距离与相机坐标系下的距离进行转换；本实施例中，所述信息块之间的距离为5cm，假设拍摄所述路标后，所述信息块之间的距离在所述相机坐标系下为1mm，即可以确定所述相机坐标系下10mm的距离在实际空间中距离为50cm，或者比如所述信息块的长度为1cm,所述信息块的长度在所述相机坐标系下为0.2mm,即可以确定所述相机坐标系下10mm的长度在实际空间中长度为50cm。

在其他实施例中，所述路标也可以采用诸如条形码、二维码等路标，只要确定并设置好所述路标中的尺寸，能够使用所述尺寸可以将实际空间与相机坐标系下的尺寸进行转换。

步骤2，机器人从起始路标开始识别路标，对路标进行相机标定，并对每个路标进行层级划分。

所述相机标定常用于机器视觉研究，在本发明中，是指将所述路标的坐标系都转化为所述相机的坐标系，并且得知所述路标在所述实际空间与相机坐标系下的尺寸的转换数学关系，找出其定量的联系。

所述路标的层级划分由小到大，确定起始路标为第一层级路标，然后依次将所有路标进行层级划分。

在本实施例中，所述起始路标E为第一层级路标，然后以起始路标E开始将所有路标进行层级划分。

在其他实施例中，所述路标的层级划分也可以由大到小，比如讲起始路标E划分为的第五层路标，或者第N层路标，N可以为任意数，也可以是字母等。

所述路标的层级大小是指：假设有第M层级路标与第N层级路标，M大于N,则，第M层级路标的层级大于第N层级路标的层级。

在本实施例中，起始路标E为第一层级路标，所述第二层级一路标D的层级数大于起始路标E。

在其他实施例中，所述路标的层级大小可做其他规定，比如假设有第M 层级路标与第N层级路标，M大于N,则，第N层级路标的层级大于第M层级路标的层级。

所述路标的层级数确定采用如下方法：机器人从已划分层级的路标出发，能够同时识别到所述已划分层级的路标与其他未划分层级路标时，将所述其他未划分层级路标的层级数划分为比所述已划分层级的路标大一个层级。

在本实施例中，所述机器人从起始路标开始识别路标，划分起始路标为第一层级路标，在人工干预下，机器人从起始路标出发，在某一位置能够同时识别到起始路标与其他路标时，将其他路标划分为第二层级路标，并对划分好层级的路标进行相机标定；所述第一层级路标分别与所有的所述第二层级路标具有直属关系。

所述路标的直属关系是指，所述两个路标之间的层级相差一个层级，且层级较大的路标的层级是根据层级较小的路标的层级而直接确定的；所述具有直属关系的相邻层级两个路标在多叉树结构中直接相连。

然后，机器人从第二层级路标出发，在任何位置均不能够同时识别到起始路标与其他路标，而只能在某一位置同时识别到第二层级路标与其他路标时，将其他路标划分为第三层级路标，并对划分好层级的路标进行相机标定；采用同样的方法划分第四层级与第四层级以上的路标。

在本实施例中，参考图4，所述机器人1在人工干预下，从起始路标E 开始行走，通过所述导航模块中的相机2进行路标图像拍摄与识别，当所述导航模块中的相机2能够同时拍摄到起始路标E与第二层级一路标D，并且准确且稳定的反映出拍摄到的是起始路标E与第二层级一路标D时，人工干预，给机器人1一个信号，机器人1对起始路标E与第二层级一路标D均进行相机标定，分别计算起始路标E坐标系、第二层级一路标D坐标系和所述相机坐标系的转换关系矩阵，并且，将所述第二层级一路标D划分为第二层级路标。

然后，参考图5，机器人1在人工干预下从第二层级一路标D出发沿着第三层级二路标B的方向行走，通过所述导航模块中的相机2进行路标图像拍摄与识别，在此过程中，当机器人1在任意位置不能同时识别到起始路标E 与第三层级二路标B，而在某一位置能够准确且稳定的反映出同时识别到的是第二层级一路标D与第三层级二路标B时，人工干预，给机器人1一个信号，机器人1对第二层级一路标D与第三层级二路标B均进行相机标定，分别计算第二层级一路标D坐标系、第三层级二路标B坐标系和所述相机坐标系的转换关系矩阵，并且，将所述第三层级二路标B划分为第三层级路标。

最后，人工干预机器人回到第二层级一路标D，沿第三层级一路标A方向行走，当过程中机器人能够准确并稳定的识别出第二层级一路标D与第三层级一路标A时，人工干预，给机器人一个信号，机器人对第二层级一路标 D与第三层级一路标A均进行相机标定，分别计算第二层级一路标D坐标系、第三层级一路标A坐标系和所述相机坐标系的转换关系矩阵，并且，将所述第三层级一路标A划分为第三层级路标。

在其他实施例中，参考图6，所述机器人1在人工干预下，从起始路标E 开始行走，通过所述导航模块中的相机2进行路标图像拍摄与识别，当所述导航模块中的相机2能够同时拍摄到起始路标E、第二层级一路标D与第二层级二路标F，并且准确且稳定的反映出拍摄到的是起始路标E、第二层级一路标D与第二层级二路标F时，人工干预，给机器人一个信号，机器人对起始路标E、第二层级一路标D、与第二层级二路标F均进行相机标定，分别计算起始路标E坐标系、第二层级一路标D坐标系、第二层级二路标F和所述相机坐标系的转换关系矩阵，并且，将所述第二层级一路标D、与第二层级二路标F划分为第二层级路标。

在其他实施例中，采用同样的方法划分第四层级与第四层级以上的路标，即机器人在任意位置均不能同时识别到路标与起始路标、或第一层级路标、或第二层级路标，而在某一位置能够同时识别到路标与第三层级路标时，将路标划分为第四层级路标；第四层级以上路标的层级划分也采用同样的方法。

在本实施例中，参考图2，采用的是多叉树结构进行地图的创建，所述起始路标E为第一层级路标，1张地图只有一个第一层级路标，所述第二层级一路标D划分为第二层级路标，层级数比起始路标E多一层，所述第三层级一路标A、第三层级二路标B划分为第三层级路标。

在其他实施例中，可以采用其他分级或分类方法对路标进行划分，比如，也可以将起始路标E与第二层级一路标D均确定为第一层级路标，其余路标再划分为第二层级或第二以上层级路标。

在其他实施例中，所述机器人可以从起始路标E开始沿任意方向行走，比如沿第二层级二路标F方向开始行走。

在本实施例中，机器人进行相机标定的时刻是机器人能够稳定并且准确的反映出识别的路标时，一般设定为机器人能够在持续在3秒的时间内稳定并且准确的识别路标；并且，本实施例中还要求机器人处于两个路标距离的二分之一处。

在其他实施例中，时间可以根据需要进行调整为4秒或5秒或6秒及以上，只要满足需要且能够实现即可；并且只要满足此条件即可，不需要机器人一定要处于两个路标距离的二分之一处。

在本实施例中，对所述路标进行相机标定，主要是为了能够将所述路标的坐标系转化为所述照相机坐标系，每一个路标的坐标系都已经转换到了所述相机坐标系，便于步骤3中的两两路标之间的关系运算，计算简单。

在本实施例中，所述相机标定方法采用张正友基于针孔相机模型特定的平面标定方法。

在其他实施例中，根据需要可以采用其他相机标定方法，只要能够将所述路标坐标系与所述相机坐标系进行转换即可，这样，每一个路标的坐标系都已经转换到了所述相机坐标系，便于步骤3中的两两路标之间的关系运算，计算简单。

步骤3，计算所有相邻层级的具有直属关系的两个路标坐标系的转换关系矩阵。

本实施例中，计算的是多叉树结构中所有具有直属关系的相邻层级的两个路标坐标系的转换关系矩阵，比如起始路标E与第一层级一路标D之间的转换关系矩阵，比如起始路标E与第二层级二路标F之间的转换关系矩阵。

本实施例中，设OXY、O'X'Y'是两个相邻路标的坐标系，O'在OXY中的坐标为(tx,ty),由X轴到X'轴的角度为θ,坐标转换公式是:x＝x'cosθ-y'sin θ+tx,y＝x'sinθ+y'cosθ+ty。

即，转换矩阵如下所示：

为了更进一步简化公式，便于坐标系之间的转换计算，引入齐次坐标形式，则得到以下公式：

由此可得到相应的路标坐标系之间的可逆转换矩阵A：

由于相邻层级的具有直属关系的两个路标的坐标系均已转换为相机坐标系，两个路标坐标系原点在所述相机坐标系下的坐标计算简单，两个路标坐标系的旋转角θ也计算简单，因而计算出转换矩阵中的参数，则能够确定相邻层级的具有直属关系的坐标转换关系，并将转换关系矩阵保存。

所述当划分层级的路标数量与设置的路标数量相同时，已完成以上步骤 1、步骤2、步骤3后，所述机器人视觉导航地图的创建完成，在本实施例中，得到的是一个机器人存储着的，以路标为节点的多叉树结构，相邻层级的路标坐标系的转换关系矩阵也存在机器人中作为多叉树结构的相邻层级的具有直属关系的两个路标之间的链，这样，导航时，机器人可以将相邻层级的具有直属关系的路标之间的实际距离计算出来，以完成导航。

所述机器人导航路径规划方法就基于上述的以多叉树结构创建的机器人视觉导航系统。

本技术方案的一种机器人导航路径规划方法，采用多叉树结构将路标划分层级，将路标层级与相邻层级直属关系的路标之间的位置关系保存为路标索引，使得路径规划中算法简单、效率高，规划的路径唯一，很适合在嵌入式设备上运行。

以上所述，仅为本发明的较佳实施例而已，并非用于限定本发明的保护范围，凡在本发明的精神和原则之内所作的任何修改、等同替换和改进等，均应包含在本发明的保护范围之内。

Claims (9)

1.一种机器人导航路径规划方法，其特征在于，包括：

步骤100：设定一个起始路标，以该起始路标为基础将所述机器人所处场景内设置的所有路标进行层级划分，并且将相邻层级直属关系路标之间的位置关系进行保存，作为路标索引；

步骤200：输入所述机器人的当前路标与目标路标；

步骤300：搜索所述当前路标与所述目标路标的共同关系路标；

步骤400：确定所述路径为当前路标经所述共同关系路标到目标路标的路径，路径规划完成；

其中，所述步骤300包括：

步骤310：确定当前路标与目标路标的层级；

步骤320：比较当前路标层级与路标目标层级，若相同执行步骤330，否则执行步骤340；

步骤330：分别逐层级搜索当前路标与目标路标的上层级路标，直到搜索到同一个路标，所述搜索到的同一个路标为共同关系路标，结束搜索；

步骤340：比较当前路标层级与目标路标层级的大小；若当前路标层级大于目标路标层级，执行步骤350，否则执行步骤360；

步骤350：搜索当前路标的上层级路标，直到搜索到与目标路标同一层级的路标，如果搜索到的该同一层级路标是目标路标，则所述目标路标为共同关系路标，结束搜索；否则逐层搜索该同一层级路标和目标路标的上层级路标，直到搜索到同一个路标作为共同关系路标，结束搜索；

步骤360：搜索目标路标的上层级路标，直到搜索到与当前路标同一层级的路标，如果搜索到的该同一层级路标是当前路标，则所述当前路标为共同关系路标，结束搜索；否则逐层搜索该同一层级路标和当前路标的上层级路标，直到搜索到同一个路标作为共同关系路标，结束搜索。

2.根据权利要求1所述的一种机器人导航路径规划方法，其特征在于，所述步骤100包括：

步骤110：设置路标的数量，并设定一个起始路标，所述起始路标为第一层级路标；

步骤120：所述机器人从起始路标开始识别路标，对路标进行相机标定，并对每个路标进行层级划分；

步骤130：计算所有相邻层级的具有直属关系的两个路标坐标系的转换关系矩阵。

3.根据权利要求1所述的一种机器人导航路径规划方法，其特征在于，所述步骤100中以路标为节点，采用多叉树结构对路标进行层级划分，所述起始路标为第一层级路标。

4.根据权利要求1所述的一种机器人导航路径规划方法，其特征在于，所述机器人从已划分层级的路标出发，能够同时识别到所述已划分层级的路标与其他未划分层级路标时，将所述其他未划分层级路标的层级数划分为比所述已划分层级的路标大一个层级。

5.根据权利要求1所述的一种机器人导航路径规划方法，其特征在于，所述直属关系是指，如果两个路标之间的层级相差一个层级，且层级较大的路标的层级是根据层级较小的路标的层级而直接确定的，则这两个路标具有直属关系。

6.根据权利要求2所述的一种机器人导航路径规划方法，其特征在于，所述相机标定的时刻是机器人能够稳定并且准确的反映出识别的路标的时刻。

7.根据权利要求2所述的一种机器人导航路径规划方法，其特征在于，所述步骤130中，设OXY、O'X'Y'是两个相邻路标的坐标系，O'在OXY中的坐标为(tx,ty),由X轴到X'轴的角度为θ，则所述转换关系矩阵A是一个可逆矩阵，即：

8.根据权利要求1所述的一种机器人导航路径规划方法，其特征在于，所述路标包括：

第一坐标标记，用于确定路标坐标系的X轴；

第二坐标标记，用于确定路标坐标系的Y轴；

区域标记，用于确定路标所在的空间位置，所述区域标记包括多个等距离的信息块，所述路标的尺寸是所述信息块之间的距离或者信息块的长度。

9.根据权利要求8所述的一种机器人导航路径规划方法，其特征在于，所述机器人使用所述尺寸将实际空间与相机坐标系下的尺寸进行转换。