一种基于改进Dijistra算法单舵轮机器人路径规划方法

技术领域

本发明涉及路径规划技术领域，具体涉及一种基于改进Dijistra算法单舵轮机器人路径规划方法。

背景技术

舵轮机器人是一种智能控制移动以进行各种任务的设备。随着人工智能的不断发展，舵轮机器人已经逐渐能够代替人类做一部分工作，相比人类而言，机器人具有能长时间、高效率工作的优势。舵轮机器人，尤其是服务型的舵轮机器人，例如，扫地机器人、送货机器人等，在日常生活中逐渐变得很常见。

在舵轮机器人的技术领域中，规划控制是一个重要的研究领域，较为常用的规划算法有A-Star算法和Dijistra算法；而Dijistra算法与A-Star算法相比，从起到移动到终点形成的路径更短。

如在中国申请号为202211581793.1，公开日为2023.2.24的专利文献公开了一种移动机器人的全局移动方法，在初始节点向目标节点靠近的过程，搜索出代价值最小的节点，进而移动机器人在代价值最小的节点之前移动；移动机器人的移动距离短，移动时间短；同时对于已知的障碍物，对障碍节点以外的节点的代价值进行计算；使移动机器人在移动使避开障碍节点，实现对已知障碍物进行避让；对于新出现的障碍物，在移动机器人向新出现的障碍物靠近的过程中，规划出避障路径，实现对未知障碍物进行避让。

该移动方法，在对新出现的障碍物进行避让时，移动机器人偏离设定轨迹避开障碍物，然后规划避开障碍物后的当前节点与路径终点之间的最短路径，由于其每次在遇到障碍物时都需要重新规划路径，从而影响导航效率。

发明内容

本发明提供一种基于改进Dijistra算法单舵轮机器人路径规划方法，舵轮机器人沿原始路径自初始节点向目标节点靠近，靠近障碍节点时，生成子路径绕开障碍节点后再次进入到原始路径。

为达到上述目的，本发明的技术方案是：一种基于改进Dijistra算法单舵轮机器人路径规划方法，包括以下步骤：

S1、预设地图，在地图中构建全局坐标系，全局坐标系中的每个坐标点为都为节点。

预设初始节点和目标节点，初始节点为舵轮机器人的起点，目标节点为舵轮机器人的终点。

S2、预设初始节点和目标节点之间的原始路径，原始路径为初始节点与目标节点之间的最短路径；原始路径上的障碍节点以外的节点为原始节点。

S3、舵轮机器人沿原始路径移动，从初始节点经历原始节点向目标节点靠近。

S4、判断舵轮机器人是否遇到原始路径上的障碍物，若是，则进行S5；若否，则沿原始路径继续移动。

S5、以舵轮机器人所在的当前节点为中心设置搜索区域。

S6、利用Dijkstra算法，并根据当前节点作为起始节点计算搜索区域内除障碍节点以外的其余节点的代价值，舵轮机器人移动到搜索区域内代价值最小的节点上。

S7、从当前代价值最小的节点移动到下一原始节点。

S8、舵轮机器人沿原始路径剩余的原始节点继续向目标节点移动；在移动过程中，重复S4-S8判断是否遇到障碍物；当舵轮机器人到达目标节点，进行S9。

S9、通过舵轮机器人经历的原始节点和代价值最小的节点生成规划路径。

以上方法，以初始节点为固定点，对原始路径进行局部优化，在原始路径上延伸出子路径，舵轮机器人绕开障碍节点，从障碍节点的上一原始节点移动到障碍节点的下一原始节点；利用Dijkstra算法将当前节点设置为起始节点，从而无需从原始起点重新规划路径，只需要与障碍物保持安全距离下绕开并回到原始路径到达终点。舵轮机器人沿原始路径移动，若遇到障碍节点后，离开原始路径绕开障碍节点，然后再重新进入原始路径，并沿原始路径继续移动，这样不需要计算规划机器人从当前节点直接移动到目标节点的另一路径，减少计算量，提升导航效率。

进一步的，S6还包括，预设距离阈值，将距离差小于等于距离阈值的相邻障碍物判断为一个障碍节点，舵轮机器人同时避开一个以上的障碍物。

以上方法，将判断范围内的多个障碍物判断为一个群障碍节点，进而舵轮机器人绕开该障碍节点时，同时避开多个障碍物，减少舵轮机器人避障时间，缩短规划路径的长度。

进一步的，S9之后还包括，对规划路径进行平滑处理。

以上方法，对规划路径进行平滑处理，提高规划路径的平滑性，提升导航效率。

进一步的，S6具体为：通过公式F(n) = G(n) + H(n)；计算搜索区域内除障碍节点以外的其余节点的F(n)值；G(n)为从当前原始节点到一节点的代价值，H(n)为一节点到下一原始节点的估计代价值；F(n)为估价函数。

以上方法，通过舵轮机器人未到达的下一原始节点与一节点的坐标计算出从一节点到达下一原始节点的估计代价值，这样能计算出下一原始节点与一节点之间的直线距离；进而能计算出F(n)值更小的节点；当舵轮机器人绕开障碍节点后，快速向原始路径中的下一原始节点移动。

进一步的，通过公式

以上设置，通过两点之间的距离公式进行得到预估值，确保准确性。

附图说明

图1为本发明绕开障碍物后再次进入到始路径的示意图。

图2为本发明绕开多个障碍物的示意图。

图3为本发明的流程图。

图4为本发明中改进Dijistra算法有向网格图。

具体实施方式

下面结合附图和具体实施方式对本发明做进一步详细说明。

如图1-3所示；一种基于改进Dijistra算法单舵轮机器人路径规划方法，舵轮机器人具有视觉识别装置，所述视觉识别装置为摄像头。舵轮机器人通过全局路径规划和局部路径规划实现移动；全局路径规划实现舵轮机器人在初始节点1与目标节点2之间按最短的原始路径4移动。局部路径规划实现舵轮机器人按原始路径4移动过程中绕开障碍物后再次进入到原始路径4中移动。

单舵轮机器人路径规划方法，包括以下步骤：

S1、预设地图，在地图中构建全局坐标系，全局坐标系中的每个坐标点为都为节点。

预设初始节点1和目标节点2，初始节点1为舵轮机器人的起点，目标节点2为舵轮机器人的终点。

S2、预设初始节点1和目标节点2之间的原始路径4，原始路径4为初始节点1与目标节点2之间的最短路径；原始路径4上的障碍节点3以外的节点为原始节点40，原始节点40设有一个以上。

S3、舵轮机器人沿原始路径4移动，从初始节点1经历N个原始节点40向目标节点2靠近。

S4、判断舵轮机器人是否遇到原始路径4上的障碍物31，若是，则进行S5；若否，则沿原始路径4继续移动。

S5、以舵轮机器人所在的当前节点为中心设置搜索区域。在本实施例中，

S6、通过公式

在本实施例中，G(n)通过当前原始节点的坐标与一节点的坐标进行计算得到。如（x

本实施例中，通过改进Dijkstra算法求出当前节点的F（n）和G（n），具体如下：在有向图网格中设置两个以上节点，确定邻接矩阵中使用Dijkstra算法搜索节点i到其它节点的最优路径的步骤如下：

步骤1：将当前节点设置为起点0，计算邻接矩阵中从起点0出发到达周围临近节点k的代价值

步骤2：计算刚加入节点k的邻近节点l的距离，不包含集合H收录的节点。若节点k的距离与节点k到节点l的距离之和小于节点l的距离，更新节点l的距离，即为G（n），并将其父节点更新为k。

如图4所示，对于Dijkstra算法来说，若具有5个节点，其中有向图网络如4所示，其邻接矩阵如下式所示：

用序号0~5表示路径上的节点i，若从节点i可以到达节点j，则用箭头表示该路径，用

如图1所示，当前原始节点40下通过改进Dijkstra算法到一节点的代价值G（n），然后通过H（n）公式计算出估计代价值，最小节点6为移动到搜索区域内代价值最小的节点。

通过舵轮机器人未到达的下一原始节点40与一节点的坐标计算出从一节点到达下一原始节点40的估计代价值，这样能计算出下一原始节点40与一节点之间的直线距离；进而能计算出F(n)值更小的节点；当舵轮机器人绕开障碍节点3后，快速向原始路径4中的下一原始节点40移动。

S7、从当前代价值最小的节点6移动到下一原始节点40。

S8、舵轮机器人沿原始路径4剩余的原始节点40继续向目标节点2移动；在移动过程中，重复S4-S8判断是否遇到障碍物；当舵轮机器人到达目标节点2，进行S9。

S9、通过舵轮机器人经历的原始节点40和代价值最小的节点生成规划路径7。然后对规划路径7进行平滑处理，对规划路径7进行平滑处理，提高规划路径7的平滑性，提升导航效率。

本实施例中，对规划路径7进行平滑处理具体包括：

在Dijkstra算法中设定筛选规则选择特定节点，删除两个节点下多余无用的过渡点，实现跳跃性前进。

本发明的原理：以初始节点1为固定点，对原始路径4进行局部优化，在原始路径4上延伸出子路径，舵轮机器人绕开障碍节点3，从障碍节点3的上一原始节点40移动到障碍节点3的下一原始节点40；与障碍物保持安全距离下绕开并回到原始路径4到达终点。舵轮机器人沿原始路径4移动，若遇到障碍节点3后，离开原始路径4绕开障碍节点3，然后再重新进入原始路径4，并沿原始路径4继续移动，这样不需要计算规划机器人从当前节点直接移动到目标节点2的另一路径，减少计算量，提升导航效率。

上述方法中，

S6还包括，预设距离阈值，将距离差小于等于距离阈值的相邻障碍物判断为一个障碍节点3，舵轮机器人同时避开一个以上的障碍物。将判断范围内的多个障碍物判断为一个群障碍节点3，进而舵轮机器人绕开该障碍节点3时，同时避开多个障碍物，减少舵轮机器人避障时间，缩短规划路径7的长度。

参照图2所示，预设距离阈值为两个节点，将距离差小于等于距离阈值的相邻障碍物30判断为一个障碍节点，舵轮机器人绕开当前的障碍节点时，舵轮机器人同时避开两个障碍物30。

在形成规划路径之后，通过模型预测控制算法全称为Model Predictive Control（MPC）进行轨迹跟踪以及优化，从而使得导航路径实现规划，MPC算法为现有技术在此不再累述。