CN110057362B - 有限单元地图的移动机器人路径规划方法 - Google Patents

Abstract

本发明公开一种有限单元地图的移动机器人路径规划方法，首先将连续的可行域离散为有限的单元组合体，用有限单元地图的节点集合与长度不相等的单元边的集合建立赋权无向循环图，然后根据赋权无向循环图用Dijkstra搜索算法搜索从起始位置到结束位置的所有目标点，通过DouglasPeucker算法对属于冗余节点的非转角点的边缘节点进行删除，提取关键路标，最后用三次自然样条函数拟合提取到的关键路标得到机器人的移动路径。本发明方法能够根据机器人所处运行环境的障碍物位置的不同，找到规划的机器人的无碰撞移动路径，与实际更相符，尤其适用于在狭长通道的地图上进行机器人移动路径规划。

Description

有限单元地图的移动机器人路径规划方法

技术领域

本发明涉及路径规划领域，尤其涉及一种有限单元地图的移动机器人路径规划方法。

背景技术

机器人路径规划是机器人研究的一个重要领域，也是机器人技术的重要组成部分。机器人路径规划问题的重点是寻找从初始配置到目标配置的无碰撞路径。近年来，该问题通过路线图、单元分解、势场方法、基于抽样的规划和智能算法得到了很好的解决，但在不同的地图上，特别是在狭长通道的地图上，往往不能很好地解决这一问题。

为此，本发明给出一种有限单元地图的移动机器人路径规划方法，为得到最优的无碰撞路径，建立的有限单元地图模型，让其能够无碰撞地通过长而窄的通道，与实际情况更相符，为移动机器人逃脱各种障碍，规划最优路径提供了一种新思路。

发明内容

本发明的目的在于提供一种有限单元地图的移动机器人路径规划方法，用于寻找从初始配置到目标配置的无碰撞路径，为寻找最优的机器人移动路径提供了保障。

为了达成上述目的，本发明的解决方案是：

有限单元地图的移动机器人路径规划方法，包括以下步骤(1)～(7)：

(1)机器人运行环境是一个二维封闭平面空间，将机器人可以自由移动的除去障碍物及边缘的区域称为可行域，记为C，平面空间C中的障碍物集合为C_Obs＝{Obstacle(1),Obstacle(2),…,Obstacle(n)}，其中，n为障碍物个数；

(2)按照步骤(1)中可行域集合形状的变化，用普通三角形将连续的可行域离散为有限的单元组合体，用单元边的集合表示机器人的可行路径，建立有限单元地图，将单元地图中的节点及单元进行编号，并将节点编号转为节点坐标；

(3)根据步骤(2)中有限单元地图对各个单元进行组装，将单元矩阵元素按照相关节点的编号放到整体节点关联矩阵中，用有限单元地图的节点集合与长度不相等的单元边的集合建立赋权无向循环图；

(4)根据步骤(3)中的赋权无向循环图将路径节点分为网格地图边缘节点和网格地图内部节点，网格地图边缘节点又分为处于转角位置的节点和处于非转角位置的节点；

(5)根据步骤(3)中的赋权无向循环图，任意确定初始位姿点B₁和终点B_u，用Dijkstra搜索算法依次求取点B₂，B₃，……一直到B_u-1，然后依次连接目标点B₁，B₂，B₃，……一直到B_u其中，u为确定的搜索出的目标点的个数；

(6)根据步骤(5)中搜索出的目标点，采用Douglas-Peucker算法提取机器人行走的关键路标K₁，K₂，……K_a，其中步骤(4)中非转角点的边缘节点是冗余节点，不作为关键路标，直接剔除，a为提取的关键路标的个数；

(7)采用三次自然样条函数拟合步骤(6)中提取的关键路标，得到机器人的移动路径。

本发明的有限单元地图的移动机器人路径规划方法，所述步骤(2)中用普通三角形将连续的可行域离散为有限的单元组合体，其中，三角形剖分的有限单元地图用节点矩阵及单元连接矩阵表示，记剖分后的可行域有m个节点和k个单元，第i个节点P_i的坐标为(x_i,y_i)，则节点矩阵P可以表示为

单元连接矩阵可以表示为

其中

表示第j个单元的第i个节点的编号。此外单元连接矩阵还表示单元连接节点的关系，即在第j个单元中，

与

连接，

与

连接，

与

连接。

本发明的有限单元地图的移动机器人路径规划方法，所述步骤(3)将单元矩阵元素按照相关节点的编号放到整体节点关联矩阵中，由单元连接矩阵，构造节点关联矩阵T与S，T＝[t₁ t₂ … t_n]，S＝[s₁ s₂ … s_n]，节点连接矩阵T与S中的元素均是网格节点编号，T中的第i个节点t_i与S中的第i个节点s_i相互关联，且表示节点t_i与节点s_i连接，由于任意两节点之间的距离不同，故定关联矩阵T与S的距离矩阵D＝[d₁ d₂ … d_n]，D中的元素d_i表示节点关联矩阵T中的第i个节点t_i与S中的第i节点s_i的距离，节点关联矩阵T、S与节点距离矩阵D可以构成稀疏矩阵表示的赋权无向连通矩阵G＝sparse(Τ,S,D)，基于有限单元的地图矩阵map表示为节点矩阵P与无向连通图矩阵组成的关联矩阵map＝(P,G)。

本发明的有限单元地图的移动机器人路径规划方法，所述步骤(6)中从步骤(4)得到的非转角点的边缘节点是冗余节点，不作为关键路标，直接剔除，对剔除非转角点后的边缘集合点采用Douglas-Peucker算法提取关键路标点，Douglas-Peucker算法为现有成熟方法，此处不再赘述。

附图说明

图1是有限单元地图的移动机器人路径规划方法流程图。图2与图3分别是用本发明方法得到的实验结果图。

具体实施方式

下面结合附图对本发明的技术方案进行详细说明。

本发明提供一种有限单元地图的移动机器人路径规划方法，其总体思路为：首先将有限单元地图离散建立赋权无向循环图，然后使用Dijkstra搜索算法搜索出从初始位姿点B₁到目标点B_u的目标点，通过Douglas-Peucker算法在搜索出的目标点之间提取关键路标，最后采用三次自然样条函数对关键路标进行拟合，得到无碰撞的光滑规划路径。

如图1所示，本发明的有限单元地图的移动机器人路径规划方法，具体实施包括以下步骤(1)～(7)：

(1)机器人运行环境是一个二维封闭平面空间，将机器人可以自由移动的除去障碍物及边缘的区域称为可行域，记为C，平面空间C中的障碍物集合为C_Obs＝{Obstacle(1),Obstacle(2),…,Obstacle(n)}，其中，n为障碍物个数。

(2)按照步骤(1)中可行域集合形状的变化，用普通三角形将连续的可行域离散为有限的单元组合体，用单元边的集合表示机器人的可行路径，建立有限单元地图，将单元地图中的节点及单元进行编号，并将节点编号转为节点坐标；具体实现为：

用普通三角形将连续的可行域离散为有限的单元组合体，其中，三角形剖分的有限单元地图用节点矩阵及单元连接矩阵表示，记剖分后的可行域有m个节点和k个单元，第i个节点P_i的坐标为(x_i,y_i)，则节点矩阵P可以表示为

单元连接矩阵可以表示为

其中

表示第j个单元的第i个节点的编号。此外单元连接矩阵还表示单元连接节点的关系，即在第j个单元中，

与

连接，

与

连接，

与

连接。

(3)根据步骤(2)中有限单元地图对各个单元进行组装，将单元矩阵元素按照相关节点的编号放到整体节点关联矩阵中，用有限单元地图的节点集合与长度不相等的单元边的集合建立赋权无向循环图；具体实现为：

将单元矩阵元素按照相关节点的编号放到整体节点关联矩阵中，由单元连接矩阵，构造节点关联矩阵T与S，T＝[t₁ t₂ … t_n]，S＝[s₁ s₂ … s_n]，节点连接矩阵T与S中的元素均是网格节点编号，T中的第i个节点t_i与S中的第i个节点s_i相互关联，且表示节点t_i与节点s_i连接，由于任意两节点之间的距离不同，故定关联矩阵T与S的距离矩阵D＝[d₁ d₂ …d_n]，D中的元素d_i表示节点关联矩阵T中的第i个节点t_i与S中的第i节点s_i的距离，节点关联矩阵T、S与节点距离矩阵D可以构成稀疏矩阵表示的赋权无向连通矩阵G＝sparse(Τ,S,D)，基于有限单元的地图矩阵map表示为节点矩阵P与无向连通图矩阵组成的关联矩阵map＝(P,G)。

(4)根据步骤(3)中的赋权无向循环图将路径节点分为网格地图边缘节点和网格地图内部节点，网格地图边缘节点又分为处于转角位置的节点和处于非转角位置的节点。

(5)根据步骤(3)中的赋权无向循环图，任意确定初始位姿点B₁和终点B_u，用Dijkstra搜索算法依次求取点B₂，B₃，……一直到B_u-1，然后依次连接目标点B₁，B₂，B₃，……一直到B_u其中，u为确定的搜索出的目标点的个数。

(6)根据步骤(5)中搜索出的目标点，采用Douglas-Peucker算法提取机器人行走的关键路标K₁，K₂，……K_a，其中步骤(4)中非转角点的边缘节点是冗余节点，不作为关键路标，直接剔除，a为提取的关键路标的个数；具体实现为：

从步骤(4)得到的非转角点的边缘节点是冗余节点，不作为关键路标，直接剔除，对剔除非转角点后的边缘集合点采用Douglas-Peucker算法提取关键路标点，Douglas-Peucker算法为现有成熟方法，此处不再赘述。

(7)采用三次自然样条函数拟合步骤(6)中提取的关键路标，得到机器人的移动路径。

以上实施例仅为说明本发明的技术思想，不能以此限定本发明的保护范围，凡是按照本发明提出的技术思想，在技术方案基础上所做的任何改动，均落入本发明保护范围之内。

Claims (4)

1.一种有限单元地图的移动机器人路径规划方法，其特征在于，包括以下步骤：

(1)机器人运行环境是一个二维封闭平面空间，将机器人可以自由移动的除去障碍物及边缘的区域称为可行域，记为C，平面空间C中的障碍物集合为C_Obs＝{Obstacle(1),Obstacle(2),…,Obstacle(n)}，其中，n为障碍物个数；

(2)按照步骤(1)中可行域集合形状的变化，用普通三角形将连续的可行域离散为有限的单元组合体，用单元边的集合表示机器人的可行路径，建立有限单元地图，将单元地图中的节点及单元进行编号，并将节点编号转为节点坐标；

(3)根据步骤(2)中有限单元地图对各个单元进行组装，将单元矩阵元素按照相关节点的编号放到整体节点关联矩阵中，用有限单元地图的节点集合与长度不相等的单元边的集合建立赋权无向循环图；

(4)根据步骤(3)中的赋权无向循环图将路径节点分为网格地图边缘节点和网格地图内部节点，网格地图边缘节点又分为处于转角位置的节点和处于非转角位置的节点；

(5)根据步骤(3)中的赋权无向循环图，任意确定初始位姿点B₁和终点B_u，用Dijkstra搜索算法依次求取点B₂，B₃，……一直到B_u-1，然后依次连接目标点B₁，B₂，B₃，……一直到B_u其中，u为确定的搜索出的目标点的个数；

(6)根据步骤(5)中搜索出的目标点，采用Douglas-Peucker算法提取机器人行走的关键路标K₁，K₂，......K_a，其中步骤(4)中非转角点的边缘节点是冗余节点，不作为关键路标，直接剔除，a为提取的关键路标的个数；

(7)采用三次自然样条函数拟合步骤(6)中提取的关键路标，得到机器人的移动路径。

2.如权利要求1所述有限单元地图的移动机器人路径规划方法，其特征在于，所述步骤(2)中用普通三角形将连续的可行域离散为有限的单元组合体，其中，三角形剖分的有限单元地图用节点矩阵及单元连接矩阵表示，记剖分后的可行域有m个节点和k个单元，第i个节点P_i的坐标为(x_i,y_i)，则节点矩阵P可以表示为

单元连接矩阵可以表示为

其中

表示第j个单元的第i个节点的编号；此外单元连接矩阵还表示单元连接节点的关系，即在第j个单元中，

与

连接，

与

连接，

与

连接。

3.如权利要求1所述有限单元地图的移动机器人路径规划方法，其特征在于，所述步骤(3)将单元矩阵元素按照相关节点的编号放到整体节点关联矩阵中，由单元连接矩阵，构造节点关联矩阵T与S，T＝[t₁ t₂ … t_n]，S＝[s₁ s₂ … s_n]，节点连接矩阵T与S中的元素均是网格节点编号，T中的第i个节点t_i与S中的第i个节点s_i相互关联，且表示节点t_i与节点s_i连接，由于任意两节点之间的距离不同，故定关联矩阵T与S的距离矩阵D＝[d₁ d₂ … d_n]，D中的元素d_i表示节点关联矩阵T中的第i个节点t_i与S中的第i节点s_i的距离，节点关联矩阵T、S与节点距离矩阵D可以构成稀疏矩阵表示的赋权无向连通矩阵G＝sparse(Τ,S,D)，基于有限单元的地图矩阵map表示为节点矩阵P与无向连通图矩阵组成的关联矩阵map＝(P,G)。

4.如权利要求1所述有限单元地图的移动机器人路径规划方法，其特征在于，所述步骤(6)中从步骤(4)得到的非转角点的边缘节点是冗余节点，不作为关键路标，直接剔除，对剔除非转角点后的边缘集合点采用Douglas-Peucker算法提取关键路标点。

Images