基于曲线拟合和目标点邻域规划的边界探索自主建图方法
阅读：103发布：2020-05-24
专利汇可以提供基于曲线拟合和目标点邻域规划的边界探索自主建图方法专利检索，专利查询，专利分析的服务。并且本 发明 提供的一种基于曲线拟合和目标点邻域规划的边界探索自主建图方法，对已构建地图的边界以曲线拟合方式筛选目标观测区，同 时针 对 机器人 不可达目标观测区，采取邻域规划方法建立新目标观测点，利用同步 定位 与建图技术，引导 机器人自主导航 至下一个观测区，完成对未知环境的自主探索。本发明提供的一种基于曲线拟合和目标点邻域规划的边界探索自主建图方法，以较少的探索次数，更高的探索效率和更少的探索时间完成对未知室内复杂场景的自主探索建图，提高了机器人完成建图的自主性和智能化，不需人工干预就可完成搜索建图，降低了人 力 、物力成本。，下面是基于曲线拟合和目标点邻域规划的边界探索自主建图方法专利的具体信息内容。
1.基于曲线拟合和目标点邻域规划的边界探索自主建图方法，其特征在于，包括以下步骤：
S1：机器人构建地图；
S2：根据边界提取算法对机器人已构建的地图提取边界，判断是否有边界，如有则生成边界候选组，否则执行步骤S8；
S3：对边界候选组中的边界进行曲线建模，筛选出安全边界；
S4：选择与机器人最近距离的安全边界作为目标观测点；
S5：判断目标观测点是否为不可达观测点，若是则执行步骤S6，否则执行步骤S7；
S6：对不可达观测点使用边界邻域规划算法，提取新目标观测点；
S7：机器人使用movebase节点A*路径规划，导航至目标观测点，执行步骤S2；
S8：机器人完成自主建图。
2.根据权利要求1所述的基于曲线拟合和目标点邻域规划的边界探索自主建图方法，其特征在于，所述步骤S1具体为：为ROS系统中，利用里程计节点获取机器人初始位姿，机器人全程使用Gmapping算法构建地图。
3.根据权利要求2所述的基于曲线拟合和目标点邻域规划的边界探索自主建图方法，其特征在于，所述Gmapping算法采用的地图模型为栅格地图；所述栅格地图通过传感器的距离信息直接得到环境的占用状态，提供环境特征数据；栅格地图中每个栅格有三种状态，包括空闲状态、占用状态和未知状态；其中：
所述空闲状态表示该栅格处没有障碍物；
所述占用状态表示该栅格处有障碍物，机器人路径规划时需避开；
所述未知状态表示该栅格未被机器人感知到，属于未知环境。
4.根据权利要求3所述的基于曲线拟合和目标点邻域规划的边界探索自主建图方法，其特征在于，所述步骤S2中，通过提取栅格地图的地图边界，将栅格地图划分为空闲空间和未知空间，从而生成候选边界组。
5.根据权利要求4所述的基于曲线拟合和目标点邻域规划的边界探索自主建图方法，其特征在于，所述步骤S3具体包括：
S31：建立边界候选组的局部边界坐标系；
S32：将边界从地图世界坐标系转化到局部边界坐标系上；
S33：利用局部边界坐标系进行边界曲线建模，得到边界拟合曲线；
S34：根据边界拟合曲线去除不安全边界，筛选出安全边界。
6.根据权利要求5所述的基于曲线拟合和目标点邻域规划的边界探索自主建图方法，其特征在于，所述步骤S4具体包括：
S41：求边界候选组中每个边界拟合曲线的根，得到多组根解；
S42：计算每组根解的距离；
S43：比较得到的距离与机器人的直径关系，若距离不小于直径，则为安全边界；否则为不安全边界；
S44：在所有安全边界中选取与机器人距离最近的边界作为观测区，并将该边界的质心作为目标观测点。
7.根据权利要求6所述的基于曲线拟合和目标点邻域规划的边界探索自主建图方法，其特征在于，所述步骤S6具体为：当边界观测区朝向地图未知区域弯曲，使得质心处于未知区域一侧，为防止机器人放弃该边界的探索，使用边界邻域规划算法提取新目标观测点。
基于曲线拟合和目标点邻域规划的边界探索自主建图方法
技术领域
背景技术
[0002] 随着机器人技术的快速发展和社会需求的变革，自主移动机器人越来越受到工程界和学术界的关注。为了自主移动机器人能够在非结构化、非确定的环境中自主地帮助人们完成日常生活任务，比较关键的技术是当机器人处于未知环境时，建立外界环境在机器人内部表示的地图，用于后续导航。但是传统机器人建图的方式是人工手动或使用键盘、游戏杆控制机器人移动，在面对大而复杂的室内环境时会浪费时间、人力、物力。因此机器人脱离人为控制，实现自主探索建图具有重要意义，一方面节约人力物力资源，另一方面，机器人根据自身感知、收集和处理环境信息，实时做出建图决策，提高机器人的自主性和智能化。
[0003] 在自主探索领域，Yamauchi[1]提出的边界探索方法是目前大多数探测算法的基础。Yamauchi将边界定义为地图上空闲区域和未知区域之间的边界，并不断将探索的目标设定为距离机器人最近的边界。该方法可以使得机器人完整遍历未知室内环境，但没有考虑路径最优，在未知环境面积比较大的时候，机器人探索会耗费大量时间。所以为了降低机器人的路径成本，陈明建[2]等人提出基于滚动窗口的机器人自主构图算法，其中心思想是按照左下右上的牛耕遍历方法和A*路径规划算法实现遍历整个环境，有效缩短了探索建图中机器人探索路径长度，但是该算法在探索未知环境地图的过程中地图重复率很高，需要更多时间成本。为了降低时间成本，Topiwala[3]等人于2018年提出基于WFD(Wavefront Frontier Detector)算法的探测方法，在寻找地图边界时，该算法改进Frontier-base边界探索算法，只扫描栅格地图的已知区域，无需扫描整个地图栅格。该方法虽然降低了边界探索算法的时间，但是对候选边界的评估有所欠缺，容易产生无效不可达边界。
[0004] 自主移动机器人自主探索建图的实现受定位、地图、路径规划等多因素的影响，上述国内外学者针对不同方面的探索问题提出了相应的解决方案，进一步优化了机器人自主建图功能。但是还存在几个问题：一、探索候选点选取条件欠缺。探索候选点的选取依据边界算法选取，未考虑候选点的可达性和安全性。二、探索建图时间长，当机器人不能导航到探索目标点时，缺乏有效的实时解决方案，往往会拉长建图时间。
发明内容
[0006] 为解决上述技术问题，本发明的技术方案如下：
[0007] 基于曲线拟合和目标点邻域规划的边界探索自主建图方法，包括以下步骤：
[0008] S1：机器人构建地图；
[0009] S2：根据边界提取算法对机器人已构建的地图提取边界，判断是否有边界，如有则生成边界候选组，否则执行步骤S8；
[0010] S3：对边界候选组中的边界进行曲线建模，筛选出安全边界；
[0011] S4：选择与机器人最近距离的安全边界作为目标观测点；
[0012] S5：判断目标观测点是否为不可达观测点，若是则执行步骤S6，否则执行步骤S7；
[0013] S6：对不可达观测点使用边界邻域规划算法，提取新目标观测点；
[0014] S7：机器人使用movebase节点A*路径规划，导航至目标观测点，执行步骤S2；
[0015] S8：机器人完成自主建图。
[0017] 其中，所述Gmapping算法采用的地图模型为栅格地图；所述栅格地图通过传感器的距离信息直接得到环境的占用状态，提供环境特征数据；栅格地图中每个栅格有三种状态，包括空闲状态、占用状态和未知状态；其中：
[0018] 所述空闲状态表示该栅格处没有障碍物；
[0019] 所述占用状态表示该栅格处有障碍物，机器人路径规划时需避开；
[0020] 所述未知状态表示该栅格未被机器人感知到，属于未知环境。
[0021] 其中，所述步骤S2中，通过提取栅格地图的地图边界，将栅格地图划分为空闲空间和未知空间，从而生成候选边界组。
[0022] 其中，所述步骤S3具体包括：
[0023] S31：建立边界候选组的局部边界坐标系；
[0024] S32：将边界从地图世界坐标系转化到局部边界坐标系上；
[0025] S33：利用局部边界坐标系进行边界曲线建模，得到边界拟合曲线；
[0026] S34：根据边界拟合曲线去除不安全边界，筛选出安全边界。
[0027] 其中，所述步骤S4具体包括：
[0028] S41：求边界候选组中每个边界拟合曲线的根，得到多组根解；
[0029] S42：计算每组根解的距离；
[0030] S43：比较得到的距离与机器人的直径关系，若距离不小于直径，则为安全边界；否则为不安全边界；
[0031] S44：在所有安全边界中选取与机器人距离最近的边界作为观测区，并将该边界的质心作为目标观测点。
[0032] 其中，所述步骤S6具体为：当边界观测区朝向地图未知区域弯曲，使得质心处于未知区域一侧，为防止机器人放弃该边界的探索，使用边界邻域规划算法提取新目标观测点。
[0033] 与现有技术相比，本发明技术方案的有益效果是：
[0034] 本发明提供的一种基于曲线拟合和目标点邻域规划的边界探索自主建图方法，对已构建地图的边界以曲线拟合方式筛选目标观测区，同时针对机器人不可达目标观测区，采取邻域规划算法建立新目标观测点，引导机器人自主导航至下一个观测区，完成对未知环境的自主探索，实现了以较少的探索次数，更高的探索效率和更少的探索时间完成对未知室内复杂场景的自主探索建图，提高了机器人完成建图的自主性和智能化，不需人工干预就可完成搜索建图，降低了人力、物力成本。附图说明
[0035] 图1为边界探索自主建图方法流程示意图；
[0036] 图2为栅格地图状态表示图；
[0037] 图3为边界提取算法示例图；
[0038] 图4为局部边界坐标系；
[0039] 图5为坐标系转换关系图；
[0040] 图6为不可达目标观测点示意图；
[0041] 图7为不可达目标观测点邻域规划图。
具体实施方式
[0043] 为了更好说明本实施例，附图某些部件会有省略、放大或缩小，并不代表实际产品的尺寸；
[0044] 对于本领域技术人员来说，附图中某些公知结构及其说明可能省略是可以理解的。
[0045] 下面结合附图和实施例对本发明的技术方案做进一步的说明。
[0046] 实施例1
[0047] 如图1所示，基于曲线拟合和目标点邻域规划的边界探索自主建图方法，包括以下步骤：
[0048] S1：机器人构建地图；
[0049] S2：根据边界提取算法对机器人已构建的地图提取边界，判断是否有边界，如有则生成边界候选组，否则执行步骤S8；
[0050] S3：对边界候选组中的边界进行曲线建模，筛选出安全边界；
[0051] S4：选择与机器人最近距离的安全边界作为目标观测点；
[0052] S5：判断目标观测点是否为不可达观测点，若是则执行步骤S6，否则执行步骤S7；
[0053] S6：对不可达观测点使用边界邻域规划算法，提取新目标观测点；
[0054] S7：机器人使用movebase节点A*路径规划，导航至目标观测点，执行步骤S2；
[0055] S8：机器人完成自主建图。
[0056] 在具体实施过程中，机器人在观测点根据建图算法构建的地图信息提取边界，然后使用观测区选取策略选择下一目标观测点，并针对不可达观测区规划新目标观测点，最后将机器人导航至目标点。以此循环，直到自主建图完成。该方法通过对地图候选边界曲线拟合方案和处理探索目标点的路径信息方案，使得机器人自主建图时每一个探索目标点均是当前最优可到达点，并且在出现不可达目标观测点时，生成最优目标替代点，增强了机器人自主建图的鲁棒性和安全性，减少了机器人自主建图的时间。
[0057] 实施例2
[0058] 更具体的，所述步骤S1具体为：为ROS系统中，利用里程计节点获取机器人初始位姿，机器人全程使用Gmapping算法构建地图。
[0059] 更具体的，所述Gmapping算法采用的地图模型为栅格地图；所述栅格地图通过传感器的距离信息直接得到环境的占用状态，提供环境特征数据；栅格地图中每个栅格有三种状态，包括空闲状态、占用状态和未知状态；其中：
[0060] 所述空闲状态表示该栅格处没有障碍物；
[0061] 所述占用状态表示该栅格处有障碍物，机器人路径规划时需避开；
[0062] 所述未知状态表示该栅格未被机器人感知到，属于未知环境。
[0063] 在具体实施过程中，机器人在未知室内环境进行自主探索时，需要不断获取Gmapping建图算法构建的地图，来制定下一步探索策略。Gmapping算法是一种基于粒子滤波的SLAM(Simultaneous Localization and Mapping)算法：首先收集激光数据、里程计数据估计机器人的位姿，再对环境建图，然后根据建立的地图修正机器人位姿，从而构建准确的地图模型。Gmapping算法采用的地图模型是栅格地图。栅格地图能够通过传感器的距离信息直接得到环境的占用状态，提供详细的环境特征数据，适合非结构化环境的空间表示，是机器人导航和路径规划的重要基础。栅格地图中每个栅格有三种状态：空闲、占用、未知，如图2所示，分别由白色、黑色、灰色表示。空闲表示该栅格处没有障碍物，占用表示栅格处有障碍物，机器人路径规划时应避开，未知表示栅格还未被机器人感知到，属于未知的环境。
[0064] 更具体的，所述步骤S2中，通过提取栅格地图的地图边界，将栅格地图划分为空闲空间和未知空间，从而生成候选边界组。
[0065] 在具体实施过程中，当机器人在当前位置构建了环境地图后，需要决策下一步的建图观测区，以此不断引导机器人构建完整的室内环境地图。本方法中，建图观测区从实时地图的边界中选取，首先需要通过已知地图提取地图的边界，生成候选边界组。地图边界是空闲空间与未知空间的边界区域。在栅格地图中，地图边界是由一系列相邻地图边界栅格组成，地图边界栅格是与未知栅格相邻的空闲栅格。因此，通过访问地图栅格占用状态，可以寻找到地图边界，如图3所示，具体为：
[0066] 1)获取机器人已构建的栅格地图和机器人位姿，并将机器人位姿从世界坐标转化为地图栅格索引P0；
[0067] 2)P0加入队列E，扫描E中第i个(i从1开始)栅格的8邻域栅格，将所有未访问过的空闲栅格加入E中，若搜寻到边界栅格，则执行第3)步，否则搜寻E中第i+1个栅格的8邻域栅格；按照此方式寻找到图3中第一个边界栅格r1；
[0068] 3)扫描r1的8邻域，并将所有未被加入任何边界的边界栅格加入到边界F中；然后扫描F中第二个边界栅格的8邻域栅格中寻找符合的边界栅格，并加入到F中，以此类推，直到找不到新的边界栅格；按照此方式寻找到图3中相邻边界栅格f1,f2,f3,f4,f5,f6组成的边界F；
[0069] 4)搜寻空闲栅格队列E中第i+1个栅格的8邻域栅格，将访问到的且未加入E中的空闲栅格加入E，并找新的边界栅格。若找到则至执行第3)步，否则若E为空则，执行第5)步，若E不空，重复第4)步；
[0070] 5)地图扫描结束；将栅格数目大于阈值的边界F加入到候选边界组A中，用于后续的边界曲线建模。
[0071] 实施例3
[0072] 在具体实施过程中，候选边界组中有很多短小、细长、空间狭窄的边界，这些边界无法容纳机器人，导致机器人进入狭窄区域发生碰撞，或者使得机器人长时间执行路径规划，却无路径到达，增加了自主建图时间。所以针对上述缺陷，本方法提出对候选边界组中的边界通过曲线建模的方式，去除不安全边界，存留安全有效的边界，具体为：
[0073] S31：建立边界候选组的局部边界坐标系：候选边界组中的边界是一系列基于机器人地图坐标系的二维坐标点，不包含边界的方向信息，因此对边界进行曲线建模时首先需要建立边界的局部坐标系，如图4所示，构建局部边界坐标系步骤如下：
[0074] 1)坐标系原点：边界质心O；
[0075] 2)坐标系Y轴：连接边界两端边界点P,Q为一条直线，找到距离该直线最远的边界点N作为顶点，连接质心与顶点的直线为Y轴，正方向为质心到顶点的方向；
[0076] 3)坐标系X轴：坐标系Y轴正向顺时针旋转90度方向为X轴正向。
[0077] S32：将边界从地图世界坐标系转化到局部边界坐标系上：如图5所示，O-XY是机器人地图坐标系，O'是边界质心，O'-X'Y'是局部边界坐标系。P是边界顶点，S为一条边界中任一边界点。已知世界坐标系下O'(X1,Y1)，P(X2,Y2)，S(Xi,Yi)，可得边界局部坐标系下O'与P的坐标：
[0078] O'(x1,y1)＝(0,0)
[0079]
[0080] 则该边界的角度θ为：
[0081]
[0082] 由二维坐标系位移旋转公式(2)得到边界局部坐标下边界点S(xi,yi)为：
[0083]
[0084] 简化得：
[0085]
[0086] S33：利用局部边界坐标系进行边界曲线建模，得到边界拟合曲线：设局部边界坐标系下的一条边界为F((x1,y2),(x2,y2),...,(xn,yn))，其中xi，yi分别表示边界F中第i个(i＝1,2,...,n)边界点的横纵坐标。设边界F的拟合曲线多项式f(x)如下：
[0087] f(x)＝c0+c1x+c2x2 (4)
[0088] 其中c0,c1,c2为多项式未知常量系数。
[0089] 计算边界F中所有边界点到拟合曲线的偏差平方和：
[0090]
[0091] 其中L是偏差平方和，n为边界F中边界点的个数，xi,yi分别表示边界点的横纵坐标值，令：
[0092] X＝(x1,x2,...,xi)T
[0093] Y＝(y1,y2,...,yi)T
[0094] W＝(c1,c2,c3)T
[0095] 则公式(5)表示为：
[0096] L＝(Y-XW)T(Y-XW) (6)
[0097] 对公式(6)中W求偏导：
[0098]
[0099] 令偏导 则：
[0100] XTY＝XTXW (8)
[0101] 求得：
[0102] W＝(XTX)-1XTY (9)
[0103] 由于边界F中边界点的横纵坐标X,Y为已知条件，所以可求得W，故得常量系数c0,c1,c2，因而求得边界F的曲线拟合函数f(x)，即得到边界拟合曲线；
[0104] S34：根据边界拟合曲线去除不安全边界，筛选出安全边界。
[0105] 实施例4
[0106] 更具体的，在实施例3的基础上，机器人自主探索建图需要在初始边界候选组中选择一个目标探索点，进行下一步建图；在对边界候选组的边界进行曲线拟合建模后，本文采用以下方案来筛选最佳目标探索点：
[0107] 求解边界组中每个边界拟合曲线的根，即根据公式(4)，令f(x)为零得到：
[0108] c0+c1x+c2x2＝0
[0109] 解得根x1，根x2为：
[0110]
[0111]
[0112] 计算两个根解的距离D：
[0113] D＝|x1-x2|
[0114] 比较距离D与机器人的直径
[0115]
[0116] 其中r为机器人的半径值；最后在所有安全边界中选取与机器人距离最近的边界作为观测区，并将该边界的质心作为目标观测点。
[0117] 实施例5
[0118] 更具体的，所述步骤S6具体为：当边界观测区朝向地图未知区域弯曲，使得质心处于未知区域一侧，为防止机器人放弃该边界的探索，使用边界邻域规划算法提取新目标观测点。
[0119] 在具体实施过程中，由于室内环境地图复杂、激光雷达为发散式扫描，因而有时会出现如图6所示的情况，边界观测区朝向地图未知区域弯曲，使得质心处在未知区域一侧。此时虽然质心作为目标观测点，但是需要机器人在原地经过长时间导航规划，确定导航失败后，再放弃此探索边界。不但加长了自主探索建图时间，而且使得机器人遗漏该探索区域，使得建图不完整。因此本文提出边界邻域规划的方法来解决上述问题。
[0120] 在具体实施过程中，为了解决上述问题，本文提出探索目标点邻域规划的方式，在短时间内寻找探索目标点的替代探索点。如图6所示，边界F的质心C在未知空间内，机器人无法规划导航，为了防止机器人放弃该边界的探索，在边界区域再寻找一个合适探索点代替质心的探索；方案如下：
[0121] 1)建立以质心为原点，半径为一米的四分之一圆面，如图7所示，作为滑动圆面；
[0122] 2)滑动圆面逆时针滑动并扫描地图，依次扫描S1,S2,S3,S4四个区域，并计算每个区域空闲栅格的数量
[0123] 3) 其中R为表示机器人面积，d表示栅格地图分辨率，R/d为机器人在地图内占有的栅格数量；
[0124] 4)计算符合上述条件区域的重心，并将该质心作为此边界的替代探索目标点。
[0125] 5)机器人导航，并记录机器人与替代探索目标点的全局路径P，该路径由一系列不断接近目标点的离散点组成(p1,p2,......pn-1,pn)；
[0126] 6)若替代目标点出现故障，则选择P中倒数第二个离散点代替目标点，以此不断接近边界，达到探索边界的目的。
[0127] 如图7所示，由上述方法得到，质心C的替代探索目标点为S3中的C'。
[0128] 在具体实施过程中，本发明提供的一种基于曲线拟合和目标点邻域规划的边界探索自主建图方法，对已构建地图的边界以曲线拟合方式筛选目标观测区，同时针对机器人不可达目标观测区，采取邻域规划算法建立新目标观测点，引导机器人自主导航至下一个观测区，完成对未知环境的自主探索，实现了以较少的探索次数，更高的探索效率和更少的探索时间完成对未知室内复杂场景的自主探索建图，提高了机器人完成建图的自主性和智能化，不需人工干预就可完成搜索建图，降低了人力、物力成本。
[0129] 显然，本发明的上述实施例仅仅是为清楚地说明本发明所作的举例，而并非是对本发明的实施方式的限定。对于所属领域的普通技术人员来说，在上述说明的基础上还可以做出其它不同形式的变化或变动。这里无需也无法对所有的实施方式予以穷举。凡在本发明的精神和原则之内所作的任何修改、等同替换和改进等，均应包含在本发明权利要求的保护范围之内。
[0130] [1]B.Yamauchi,"Afrontier-based approach for autonomous exploration,"Computational Intelligence in Robotics and Automation,1997.CIRA'97.,Proceedings.,1997IEEE International Symposium on,Monterey,CA,1997,pp.146-151.doi:10.1109/CIRA.1997.613851
[0131] [2]陈明建,林伟,曾碧.基于滚动窗口的机器人自主构图路径规划[J].计算机工程,2017(2).
[0132] [3]Topiwala A,Inani P,Kathpal A.Frontier Based Exploration for Autonomous Robot[J].2018.
| 标题 | 发布/更新时间 | 阅读量 |
|---|---|---|
| 一种基于遗传算法的视觉惯性组合的SLAM方法 | 2020-05-23 | 486 |
| 一种基于多传感器的目标态势融合感知方法和系统 | 2020-05-15 | 532 |
| 数据处理方法、装置、电子设备及存储介质 | 2020-05-08 | 634 |
| 一种石化工厂巡检机器人定位系统和方法 | 2020-05-14 | 610 |
| 一种基于输变电设备的智能巡检管理方法 | 2020-05-12 | 476 |
| 一种室内同步定位与建图方法 | 2020-05-14 | 739 |
| 一种基于点云分割匹配闭环校正的同步定位与构图算法 | 2020-05-20 | 150 |
| 一种基于卫星辅助的机器视觉定位系统及方法 | 2020-05-08 | 145 |
| 一种基于激光雷达的SLAM闭环检测方法及检测系统 | 2020-05-21 | 824 |
| 一种移动机器人同步定位与地图构建方法 | 2020-05-14 | 512 |
高效检索全球专利专利汇是专利免费检索,专利查询,专利分析-国家发明专利查询检索分析平台，是提供专利分析，专利查询，专利检索等数据服务功能的知识产权数据服务商。
我们的产品包含105个国家的1.26亿组数据，免费查、免费专利分析。
分析报告
专利汇分析报告产品可以对行业情报数据进行梳理分析，涉及维度包括行业专利基本状况分析、地域分析、技术分析、发明人分析、申请人分析、专利权人分析、失效分析、核心专利分析、法律分析、研发重点分析、企业专利处境分析、技术处境分析、专利寿命分析、企业定位分析、引证分析等超过60个分析角度，系统通过AI智能系统对图表进行解读，只需1分钟，一键生成行业专利分析报告。
QQ群二维码
意见反馈
意见反馈