CN111552288B - 一种移动机器人路径平滑方法 - Google Patents

Abstract

本发明公开了一种移动机器人路径平滑方法，首先介绍了拐点和无关点的概念和确定方法，以及拐点关系矩阵建立步骤，并在此基础上设计了路径平滑方法，能够实现初始路径的平滑，让路径变得更短、更流畅。同时，本发明提供的路径平滑方法能够与其他任何路径规划算法进行结合，只要给定的一条路径上存在不必要的转折点即可实现路径平滑，具有较高的实用性。此外，本发明在路径平滑方法中引入了交叉算子，增加了找到潜在更短路径的可能性。

Description

一种移动机器人路径平滑方法

技术领域

本发明属于路径规划技术领域，具体涉及一种移动机器人路径平滑方法的设计。

背景技术

在互联网与人工智能技术的迅猛发展的大背景下，智能移动机器人技术也在不断创新，其中路径规划算法是移动机器人研究中的一个重要关键所在。目前，关于移动机器人路径规划算法的研究内容丰富多样，但一些算法受自身性能的影响导致规划得到的路径存在一些不必要的转折点，通过一定的方法对这些不必要的转折点进行平滑，实际上不仅能够进一步挖掘更短的路径，而且还能使路径变得更加流畅。因此，对路径平滑算法的研究也是目前研究路径规划技术的一个重要研究方向。

发明内容

本发明的目的是提出一种移动机器人路径平滑方法，对移动机器人的初始路径进行平滑，使路径更短且更流畅。

本发明的技术方案为：一种移动机器人路径平滑方法，包括以下步骤：

S1、获取一条移动机器人的初始路径，并确定初始路径上的拐点和无关点。

S2、将初始路径上的无关点删除，保留拐点，并依次连接拐点得到拐点路径。

S3、获取无关点的数量m。

S4、对无关点进行遍历，每次选取一个无关点并按照初始路径节点的实际顺序将其插入到拐点路径中，得到m条插入路径。

S5、采用拐点平滑算法对m条插入路径进行平滑，得到m条拐点平滑路径。

S6、以m条拐点平滑路径作为父代路径，使用相同点交叉算子对父代路径进行交叉操作，得到若干条子代路径。

S7、计算所有子代路径的长度，并将长度最短的子代路径作为最终平滑路径。

进一步地，步骤S1中确定初始路径上的拐点和无关点的具体方法为：

A1、从初始路径上任意选取相邻的3个节点。

A2、将3个节点中每相邻的两个节点相连，得到两条连线。

A3、计算两条连线的斜率值。

A4、判断两个斜率值是否相等，若相等则将位于中间位置的节点确定为无关点，否则将位于中间位置的节点确定为拐点。

A5、重复步骤A1-A4，直到初始路径中除起点和终点外的所有节点都确定为拐点或无关点，进入步骤A6。

A6、将初始路径中的起点和终点确定为拐点。

进一步地，步骤S5中采用拐点平滑算法对插入路径进行平滑的具体方法为：

B1、获取一条插入路径，确定插入路径上的拐点和无关点，将无关点删除，保留拐点，得到拐点路径。

B2、对拐点路径中的拐点进行排序，确定拐点关系矩阵。

B3、设置计数值i＝1，根据拐点关系矩阵确定第i个拐点的关系集合。

B4、记录第i个拐点，并在第i个拐点的关系集合中按照逆序的方式找到第1个不等于0的关系值gd_ij，确定该关系值gd_ij所在列的序号j。

B5、令计数值i＝j，判断第i个拐点是否为终点，若是则进入步骤B6，否则返回步骤B4。

B6、顺序连接步骤B4中记录的所有拐点，得到拐点平滑路径。

进一步地，步骤B2中确定拐点关系矩阵的具体方法为：

C1、按照拐点排序结果依次计算拐点路径中每一个拐点与其他所有拐点之间的关系值。

C2、将每一个拐点与其他所有拐点之间的关系值构成该拐点的关系集合。

C3、将每一个拐点的关系集合作为一个行向量，构建拐点关系矩阵。

进一步地，步骤C1中任意两个拐点GD_i与GD_j的关系值gd_ij的计算方法为：

(1)当i＝j时，gd_ij＝0，表示拐点不与自身建立关系。

(2)当i>j时，gd_ij＝0，表示拐点不与其自身所在位置之前的拐点建立关系。

(3)当i<j时，确定拐点GD_i与拐点GD_j的连线所在直线的直线方程l_ij，通过计算障碍物到直线l_ij的距离，判断拐点GD_i与拐点GD_j的连线是否与障碍物发生交叉或者与障碍物的最近距离小于预设的安全距离，若是则gd_ij＝0，否则gd_ij＝GD_j，表示gd_ij取值为拐点GD_j的编号。

进一步地，拐点GD_i与拐点GD_j的连线所在直线的直线方程l_ij表示为：

其中k＝k_ij表示直线方程l_ij的斜率，b表示直线方程l_ij的截距，(x_i,y_i)为拐点GD_i的坐标，(x,y)为直线方程l_ij的坐标。

进一步地，步骤S6中使用相同点交叉算子对父代路径进行交叉操作的具体方法为：

D1、随机选取两条父代路径，记录两条父代路径上除起点与终点之外的相同节点。

D2、依次对每个相同节点后的两条父代路径结构进行交叉互换，生成2n条子代路径；n为两条父代路径上除起点与终点之外的相同节点数量。

D3、重复步骤D1-D2，直到每条父代路径都与其余所有父代路径完成交叉操作，得到若干条子代路径。

本发明的有益效果是：

(1)本发明定义了拐点和无关点的概念与判断方法，并给出了拐点关系矩阵的建立方法，在拐点关系矩阵的基础上设计了路径平滑方法，能够实现路径的平滑，让路径变得更短、更流畅。

(2)本发明提供的路径平滑方法能够与其他任何路径规划算法进行结合，只要给定的一条路径上存在不必要的转折点即可实现路径平滑，具有较高的实用性。

(3)本发明在路径平滑方法中引入了交叉算子，增加了找到潜在更短路径的可能性。

附图说明

图1所示为本发明实施例提供的一种移动机器人路径平滑方法流程图。

图2所示为本发明实施例提供的拐点路径示意图。

图3所示为本发明实施例提供的初始路径和拐点平滑路径对比示意图。

图4所示为本发明实施例提供的实验用图。

图5所示为本发明实施例提供的蚁群算法规划的初始路径示意图。

图6所示为本发明实施例提供的最终平滑路径示意图。

具体实施方式

现在将参考附图来详细描述本发明的示例性实施方式。应当理解，附图中示出和描述的实施方式仅仅是示例性的，意在阐释本发明的原理和精神，而并非限制本发明的范围。

本发明实施例提供了一种移动机器人路径平滑方法，如图1所示，包括以下步骤S1～S7：

S1、获取一条移动机器人的初始路径，并确定初始路径上的拐点和无关点。

本发明实施例中，确定初始路径上的拐点和无关点的具体方法包括以下步骤A1～A6：

A1、从初始路径上任意选取相邻的3个节点。

A2、将3个节点中每相邻的两个节点相连，得到两条连线。

A3、计算两条连线的斜率值。

A4、判断两个斜率值是否相等，若相等则将位于中间位置的节点确定为无关点，否则将位于中间位置的节点确定为拐点。

例如，从一条路径上任取3个相邻节点：a、b、c，通过计算得到线段L_ab和线段L_bc的斜率分别为k_ab和k_bc，如果k_ab≠k_bc，则节点b为拐点；如果k_ab＝k_bc，则节点b为无关点。

A5、重复步骤A1-A4，直到初始路径中除起点和终点外的所有节点都确定为拐点或无关点，进入步骤A6。

A6、将初始路径中的起点和终点确定为拐点。

S2、将初始路径上的无关点删除，保留拐点，并依次连接拐点得到拐点路径。

实际上，删除一条路径上的无关点，保留拐点，然后依次连接拐点得到拐点路径与原路径是等效的。如图2所示，一条从起点栅格12到终点栅格70的路径：(12-13-24-25-36-47-48-59-70)，根据拐点和无关点的定义可知，拐点集合为：{12,13,24,25,47,48,70}，无关点集合为：{36,59}，剔除无关点之后的拐点路径为：(12-13-24-25-47-48-70)，如图2所示，通过连接拐点得到的拐点路径与原路径完全等效。

S3、获取无关点的数量m。

S4、对无关点进行遍历，每次选取一个无关点并按照初始路径节点的实际顺序将其插入到拐点路径中，得到m条插入路径。

S5、采用拐点平滑算法对m条插入路径进行平滑，得到m条拐点平滑路径。

本发明实施例中，采用拐点平滑算法对插入路径进行平滑的具体方法包括以下步骤B1～B6：

B1、获取一条插入路径，确定插入路径上的拐点和无关点，将无关点删除，保留拐点，得到拐点路径。

B2、对拐点路径中的拐点进行排序，确定拐点关系矩阵。

本发明实施例中，以图2为例，其中拐点路径(12-13-24-25-47-48-70)中共有7个拐点，拐点排序如表1所示：

表1

则确定拐点关系矩阵的具体方法包括以下步骤C1～C3：

C1、按照拐点排序结果依次计算拐点路径中每一个拐点与其他所有拐点之间的关系值。

本发明实施例中，任意两个拐点GD_i与GD_j的关系值gd_ij的计算方法为：

(1)当i＝j时，gd_ij＝0，表示拐点不与自身建立关系。

(2)当i>j时，gd_ij＝0，表示拐点不与其自身所在位置之前的拐点建立关系。

(3)当i<j时，设拐点GD_i的二维坐标为(x_i,y_i)，拐点GD_j的二维坐标为(x_j,y_j)。

①当x_i≠x_j时，已知两点坐标，可以确定过这两拐点连线所在直线l_ij的斜率k_ij，见公式(1)：

由求解直线方程的点斜式方法可以确定过这两拐点的直线l_ij的方程，见公式(2)和公式(3)：

y-y_i＝k_ij(x-x_i) (2)

y-y_j＝k_ij(x-x_j) (3)

通过对公式(2)和公式(3)进行左右移项变换可得公式(4)和公式(5)：

y＝k_ijx-k_ijx_i+y_i (4)

y＝k_ijx-k_ijx_j+y_j (5)

②当x_i＝x_j时，直线l_ij垂直于二维坐标系中的横坐标轴，直线l_ij不存在斜率，直线方程可以写成公式(6)所示形式：

x＝x_i (6)

因此，根据上述分析可以得到拐点GD_i与拐点GD_j的连线所在直线的直线方程l_ij表示为：

其中k＝k_ij表示直线方程l_ij的斜率，b表示直线方程l_ij的截距，(x_i,y_i)为拐点GD_i的坐标，(x,y)为直线方程l_ij的坐标。

通过计算障碍物到直线l_ij的距离，判断拐点GD_i与拐点GD_j的连线是否与障碍物发生交叉或者与障碍物的最近距离小于预设的安全距离，若是则gd_ij＝0，否则连线不与障碍物发生交叉且与障碍物的最近距离不小于预设的安全距离，此时gd_ij＝GD_j，表示gd_ij取值为拐点GD_j的编号。

C2、将每一个拐点与其他所有拐点之间的关系值构成该拐点的关系集合。

例如，拐点GD₁的关系集合如表2所示：

表2

C3、将每一个拐点的关系集合作为一个行向量，构建拐点关系矩阵。

最终得到的拐点关系矩阵如表3所示：

表3

B3、设置计数值i＝1，根据拐点关系矩阵确定第i个拐点的关系集合。

B4、记录第i个拐点，并在第i个拐点的关系集合中按照逆序的方式(即从关系集合的右端向左的方向)找到第1个不等于0的关系值gd_ij，确定该关系值gd_ij所在列的序号j。

B5、令计数值i＝j，判断第i个拐点是否为终点，若是则进入步骤B6，否则返回步骤B4。

B6、顺序连接步骤B4中记录的所有拐点，得到拐点平滑路径。

下面通过一个示例演示拐点平滑算法的操作过程：

首先，基于图2所示的栅格地图，确定起点为12号栅格，终点为70号栅格，使用蚁群算法规划一条从起点到终点的路径：(12-13-24-25-36-47-48-59-70)。然后，根据拐点定义对初始路径上的节点进行拐点识别，识别结果见表4，删除无关点之后的拐点路径为(12-13-24-25-47-48-70)。

表4

接下来在得到拐点路径之后，检测拐点间连线与障碍物的关系，得到拐点关系矩阵，如表5所示：

表5

有了拐点关系矩阵之后，进行拐点平滑算法的核心部分。首先，赋值i＝1，记录第1号拐点12；然后，找到第1号拐点12的关系集合，如表6所示；按照逆序的方式(即从关系集合的右端向左的方向)找到第1个不等于0的关系值为gd₁₂＝13，gd₁₂所在列的序号2。

表6

赋值i＝2，记录第2号拐点13；然后，找到第2号拐点13的关系集合，如表7所示；按照逆序的方式找到第1个不等于0的关系值为gd₂₆＝48，gd₂₆所在列的序号6。

表7

赋值i＝6，记录第6号拐点48；然后，找到第6号拐点48的关系集合，如表8所示；按照逆序的方式找到第1个不等于0的关系值为gd₆₇＝70，gd₆₇所在列的序号7。

表8

赋值i＝7，记录第7号拐点70，此时发现拐点70是拐点路径的终点，算法结束，得到拐点平滑路径为(12-13-48-70)。

如图3所示，图3(a)是利用蚁群算法规划的初始路径轨迹，图3(b)是删除无关点之后的拐点路径轨迹，对比图3(a)与图3(b)可知初始路径轨迹与拐点路径轨迹完全一样，互为等效路径。图3(c)是使用拐点平滑算法平滑之后得到的拐点平滑路径。通过统计可知，初始路径轨迹上的转弯次数为5，长度为10.071，拐点平滑路径的转弯次数为2，长度为9.659，说明经过拐点平滑算法平滑后的路径不仅转弯次数变少、路径更加平滑，而且路径长度也更加短。

S6、以m条拐点平滑路径作为父代路径，使用相同点交叉算子对父代路径进行交叉操作，得到若干条子代路径。

在遗传算法中有一个非常重要的环节就是交叉，所谓交叉是指把两个父代个体的部分结构进行交换、重组生成新的个体的操作。根据编码方式的不同，可以分为两大类：实值重组和二进制交叉。

在二进制交叉中最常用的交叉算子为单点交叉，具体操作是：在个体染色体中随机设定一个交差点，然后，对该点前或后的两个个体的部分结构进行交叉互换，最终生成两个新个体。下面通过一个例子介绍单点交叉的操作流程，如表9所示，两个父代染色体在第4基因位置发生断裂，每个父代染色体断裂成前段和后段，每个父代染色体保持各自前的段染色体不变，然后通过交换后段染色体得到两个新的子代染色体，单点交叉操作完成。

表9

本发明实施例中，引入了遗传算法中单点交叉算子的思想，使用的是相同点交叉算子，使用相同点交叉算子对父代路径进行交叉操作的具体方法包括以下步骤D1～D3：

D1、随机选取两条父代路径，记录两条父代路径上除起点与终点之外的相同节点。

D2、依次对每个相同节点后的两条父代路径结构进行交叉互换，生成2n条子代路径；n为两条父代路径上除起点与终点之外的相同节点数量。

D3、重复步骤D1-D2，直到每条父代路径都与其余所有父代路径完成交叉操作，得到若干条子代路径。

下面给出一个相同点交叉的例子，假设在起点与终点之间存在两条不同的路径(1-23-44-132-563-700-721-801-820)和(1-3-12-44-213-721-755-801-806-820)，两条路径上除了起点1与终点820之外的相同节点包括{44,721,801}，对每一个相同点进行相同点交叉操作，得到的结果如表10所示。

表10

S7、计算所有子代路径的长度，并将长度最短的子代路径作为最终平滑路径。

下面通过一个具体实验例说明本发明提出的移动机器人路径平滑方法的效果。

如图4所示为本实验例采用的地图，起点为1号栅格，终点为900号栅格。首先通过蚁群算法得到一条初始路径，如图5所示；然后采用本发明提供的移动机器人路径平滑方法对初始路径进行平滑处理，得到的最终平滑路径如图6所示。根据图5和图6可以得到初始路径和最终平滑路径的路径节点和路径长度如表11所示。

表11

由表11可知，按照本发明提供的移动机器人路径平滑方法得到的平滑路径不仅在长度上优于初始路径，而且在路径平滑程度上更加流畅。

本领域的普通技术人员将会意识到，这里所述的实施例是为了帮助读者理解本发明的原理，应被理解为本发明的保护范围并不局限于这样的特别陈述和实施例。本领域的普通技术人员可以根据本发明公开的这些技术启示做出各种不脱离本发明实质的其它各种具体变形和组合，这些变形和组合仍然在本发明的保护范围内。

Claims (4)

1.一种移动机器人路径平滑方法，其特征在于，包括以下步骤：

S1、获取一条移动机器人的初始路径，并确定初始路径上的拐点和无关点；

S2、将初始路径上的无关点删除，保留拐点，并依次连接拐点得到拐点路径；

S3、获取无关点的数量m；

S4、对无关点进行遍历，每次选取一个无关点并按照初始路径节点的实际顺序将其插入到拐点路径中，得到m条插入路径；

S5、采用拐点平滑算法对m条插入路径进行平滑，得到m条拐点平滑路径；

S6、以m条拐点平滑路径作为父代路径，使用相同点交叉算子对父代路径进行交叉操作，得到若干条子代路径；

S7、计算所有子代路径的长度，并将长度最短的子代路径作为最终平滑路径；

所述步骤S5中采用拐点平滑算法对插入路径进行平滑的具体方法为：

B1、获取一条插入路径，确定插入路径上的拐点和无关点，将无关点删除，保留拐点，得到拐点路径；

B2、对拐点路径中的拐点进行排序，确定拐点关系矩阵；

B3、设置计数值i＝1，根据拐点关系矩阵确定第i个拐点的关系集合；

B4、记录第i个拐点，并在第i个拐点的关系集合中按照逆序的方式找到第1个不等于0的关系值gd_ij，确定该关系值gd_ij所在列的序号j；

B5、令计数值i＝j，判断第i个拐点是否为终点，若是则进入步骤B6，否则返回步骤B4；

B6、顺序连接步骤B4中记录的所有拐点，得到拐点平滑路径；

所述步骤B2中确定拐点关系矩阵的具体方法为：

C1、按照拐点排序结果依次计算拐点路径中每一个拐点与其他所有拐点之间的关系值；

C2、将每一个拐点与其他所有拐点之间的关系值构成该拐点的关系集合；

C3、将每一个拐点的关系集合作为一个行向量，构建拐点关系矩阵；

所述步骤C1中任意两个拐点GD_i与GD_j的关系值gd_ij的计算方法为：

(1)当i＝j时，gd_ij＝0，表示拐点不与自身建立关系；

(2)当i>j时，gd_ij＝0，表示拐点不与其自身所在位置之前的拐点建立关系；

(3)当i<j时，确定拐点GD_i与拐点GD_j的连线所在直线的直线方程l_ij，通过计算障碍物到直线l_ij的距离，判断拐点GD_i与拐点GD_j的连线是否与障碍物发生交叉或者与障碍物的最近距离小于预设的安全距离，若是则gd_ij＝0，否则gd_ij＝GD_j，表示gd_ij取值为拐点GD_j的编号。

2.根据权利要求1所述的移动机器人路径平滑方法，其特征在于，所述步骤S1中确定初始路径上的拐点和无关点的具体方法为：

A1、从初始路径上任意选取相邻的3个节点；

A2、将3个节点中每相邻的两个节点相连，得到两条连线；

A3、计算两条连线的斜率值；

A4、判断两个斜率值是否相等，若相等则将位于中间位置的节点确定为无关点，否则将位于中间位置的节点确定为拐点；

A5、重复步骤A1-A4，直到初始路径中除起点和终点外的所有节点都确定为拐点或无关点，进入步骤A6；

A6、将初始路径中的起点和终点确定为拐点。

3.根据权利要求1所述的移动机器人路径平滑方法，其特征在于，所述拐点GD_i与拐点GD_j的连线所在直线的直线方程l_ij表示为：

其中k＝k_ij表示直线方程l_ij的斜率，b表示直线方程l_ij的截距，(x_i,y_i)为拐点GD_i的坐标，(x,y)为直线方程l_ij的坐标。

4.根据权利要求1所述的移动机器人路径平滑方法，其特征在于，所述步骤S6中使用相同点交叉算子对父代路径进行交叉操作的具体方法为：

D1、随机选取两条父代路径，记录两条父代路径上除起点与终点之外的相同节点；

D2、依次对每个相同节点后的两条父代路径结构进行交叉互换，生成2n条子代路径；n为两条父代路径上除起点与终点之外的相同节点数量；

D3、重复步骤D1-D2，直到每条父代路径都与其余所有父代路径完成交叉操作，得到若干条子代路径。

Images