CN108614552A - 一种基于离散位姿的路径规划方法 - Google Patents

Abstract

本发明公开了一种基于离散位姿的路径规划方法，包括以下步骤：步骤1：将关键帧转化为顶点，将关键帧集合Set_K中的每一个关键帧K_i加入到顶点集合Set_V中；步骤2：计算边集合Set_E({E₁...,E_i...,E_m})，E_i包含信息{V_c1,V_c2,C}，其中，V_c1与V_c2为Set_K中的任意两个不相同的顶点，C表示两个顶点相连的代价；步骤3：给定目标位置，并进行路径规划。本发明基于离散位姿的路径规划方法，给定基于关键帧的地图，在该地图进行快速路径规划，并尽可能使规划后的路径靠近关键帧，解决了基于离散位姿的路径规划，保证机器人使用本发明可以运行的稳定可靠。

Description

一种基于离散位姿的路径规划方法

技术领域

本发明涉及路径规划方法技术领域，具体涉及一种基于离散位姿的路径规划方法。

背景技术

视觉SLAM系统可以对环境进行地图构建，得到的地图形式通常为一系列关键帧和空间点的集合。该地图可以保存并进一步应用于机器人定位导航。机器人导航则需要实时的基于地图进行路径规划。

基于视觉SLAM构图算法得到的地图有一定的特殊性，即在关键帧位置附近的区域，定位精度和对环境变化的抗干扰能力比较好，因此，在机器人进行自主导航时，为了保证高可靠性，需要保证机器人在关键帧附近工作。本发明解决了基于离散位姿地图的路径规划问题，任意指定机器人目标位置，可以快速得到从当前位置到目标位置的路径，并且使得路径尽量在关键帧位置上，从而在满足路径规划需求的同时，使机器人尽量稳定可靠的运行。

发明内容

本发明提供了一种基于离散位姿的路径规划方法，给定基于关键帧的地图，在该地图进行快速路径规划，并尽可能使规划后的路径靠近关键帧。

本发明所述地图包括关键帧集合与三维点集合Set_M N_k，N_M分别表示两个集合的元素数目，N_k为关键帧集合的元素数目，N_M为三维点集合Set_M的元素数目。本发明只关注关键帧集合Set_K。K_i表示第i个关键帧，K_i包含下列信息{t_i,R_i,I_i}，t_i表示关键帧K_i在世界坐标系中的位置，为3×1矩阵，R_i表示关键帧K_i的朝向，为3×3矩阵，I_i为采集到的图像。

本发明所述图Graph包括顶点集合Set_V({V₁...,V_i...,V_n})，V_i由上述地图中的K_i得到，V_i包含信息{t_i,R_i}；边集合Set_E({E₁...,E_i...,E_m})，E_i包含信息{V_c1,V_c2,C}，其中V_c1与V_c2为Set_K中的任意两个不相同的顶点，C表示两个顶点相连的代价，计算方式如下所示。

本发明采用的技术方案如下：

一种基于离散位姿的路径规划方法，包括以下步骤：

步骤1：将关键帧转化为顶点，将关键帧集合Set_K中的每一个关键帧K_i加入到顶点集合Set_V中。

步骤1中，通过现有的视觉SLAM系统可以对环境进行地图构建，得到的地图包括：关键帧集合N_k为关键帧集合的元素数目，K₁表示第1个关键帧，表示第N_k个关键帧，K_i表示第i个关键帧，K_i包含下列信息{t_i,R_i,I_i}，t_i表示关键帧K_i在世界坐标系中的位置，为3×1矩阵，R_i表示关键帧K_i的朝向，为3×3矩阵，I_i为采集到的图像；

顶点集合Set_V({V₁...,V_i...,V_n})，V_i包含信息{t_i,R_i}，V_i中的{t_i,R_i}和K_i中的{t_i,R_i}具有相同含义。

步骤2：计算边集合Set_E({E₁...,E_i...,E_m})，E_i包含信息{V_c1,V_c2,C}，其中，V_c1与V_c2为Set_K中的任意两个不相同的顶点，C表示两个顶点相连的代价，采用下列步骤计算任意{V_c1,V_c2}之间的代价C。

步骤2.1：V_c1包含信息{t_c1,R_c1}，V_c2包含信息{t_c2,R_c2}，计算t_c1与t_c2之间的距离Dist_{c1_c2}：

Dist_{c1_c2}＝norm(t_c1-t_c2)；

其中，norm(v)为计算向量v的模，norm(t_c1-t_c2)为计算向量t_c1-t_c2的模，norm为计算函数；

如果Dist_{c1_c2}大于阈值DisThres，则C设置为+∞，将{V_c1,V_c2,C}加入到Set_E，否则执行步骤2.2(即如果Dist_{c1_c2}小于等于阈值DisThres，则执行步骤2.2)；

步骤2.2：计算V_c1,V_c2之间的朝向误差DistAngle_{c1_c2}：

该步骤计算V_c1,V_c2之间的朝向误差DistAngle_{c1_c2}。继续执行步骤2.3.

步骤2.3：计算V_c2在V_c1中的位置，并得到路径角度误差DistPath_{c1_c2}。

Oc＝R_c1×t_c2+t_c1

DistPath_{c1_c2}＝1-Oc(3)/norm(Oc)

其中Oc表示V_c2相对于V_c1的位置，为3×1矩阵，Oc(i)表示获取Oc的第i个元素，Oc(3)表示获取Oc的第3个元素，norm为函数。

步骤2.4：计算C.

C＝(DistAngle_{c1_c2}+DistPath_{c1_c2}+α)×Dist_{c1_c2}

其中，α为人为设定阈值，将{V_c1,V_c2,C}加入到Set_E。

步骤3：给定目标位置，并进行路径规划。

步骤3.1：给定目标位置{t_d,R_d}，在Set_V中查找距离目标位置最近的顶点V_end，即遍历Set_V中的元素，找到V_end使得下列公式最小：

dist_min＝norm(t_end-t_d)；

V_end包含信息{t_end,R_end}。

步骤3.2：获取当前位置{t_c,R_c}，在Set_V中查找距离当前位置最近的顶点V_start，即遍历Set_V中的元素，找到V_start使得下列公式最小：

dist_min＝norm(t_start-t_c)；

V_start包含信息{t_start,R_start}。

步骤3.3：对Set_V和Set_E使用Dijkstra算法(迪杰斯特拉算法)在地图中查找最短路径，使得V_start与V_end之间距离最小，得到顶点集合List_path＝{V_start...V_pi...V_end}，其中V_pi∈Set_V。

步骤3.4：将当前位置和目标位置加入到List_path，最终得到路径List_path＝{{t_c,R_c},V_start...V_pi...V_end,{t_d,R_d}}。

综上所述，由于采用了上述技术方案，本发明的有益效果是：

本发明基于离散位姿的路径规划方法，给定基于关键帧的地图，在该地图进行快速路径规划，并尽可能使规划后的路径靠近关键帧，解决了基于离散位姿的路径规划，保证机器人使用本发明可以运行的稳定可靠。

附图说明

图1为本发明基于离散位姿的路径规划方法的流程示意图。

具体实施方式

本说明书中公开的所有特征，或公开的所有方法或过程中的步骤，除了互相排斥的特征和/或步骤以外，均可以以任意方式组合。

实施例一：

步骤1：将关键帧转化为顶点。将地图中的每一个关键帧K_i加入到顶点集合Set_V中。

步骤1中，通过现有的视觉SLAM系统可以对环境进行地图构建，得到的地图包括：关键帧集合N_k为关键帧集合的元素数目，K₁表示第1个关键帧，表示第N_k个关键帧，K_i表示第i个关键帧，K_i包含下列信息{t_i,R_i,I_i}，t_i表示关键帧K_i在世界坐标系中的位置，为3×1矩阵，R_i表示关键帧K_i的朝向，为3×3矩阵，I_i为采集到的图像；

顶点集合Set_V({V₁...,V_i...,V_n})，V_i包含信息{t_i,R_i}，V_i中的{t_i,R_i}和K_i中的{t_i,R_i}具有相同含义。

步骤2：计算边集合Set_E({E₁...,E_i...,E_m})，E_i包含信息{V_c1,V_c2,C}，其中，V_c1与V_c2为Set_K中的任意两个不相同的顶点，C表示两个顶点相连的代价，采用下列步骤计算任意{V_c1,V_c2}之间的代价C。

步骤2.1：V_c1包含信息{t_c1,R_c1}，V_c2包含信息{t_c2,R_c2}，计算t_c1与t_c2之间的距离Dist_{c1_c2}。

Dist_{c1_c2}＝norm(t_c1-t_c2)

其中，norm(v)为计算向量v的模，norm(t_c1-t_c2)为计算向量t_c1-t_c2的模，norm为计算函数；

如果Dist_{c1_c2}大于阈值DisThres，则C设置为+∞，将{V_c1,V_c2,C}加入到Set_E，否则执行步骤2.2。

步骤2.2：计算V_c1,V_c2之间的朝向误差DistAngle_{c1_c2}。

该步骤计算V_c1,V_c2之间的朝向误差DistAngle_{c1_c2}，继续执行步骤2.3.

步骤2.3：V_c1包含信息{t_c1,R_c1}，V_c2包含信息{t_c2,R_c2}，计算V_c2在V_c1中的位置，并得到路径角度误差。

Oc＝R_c1×t_c2+t_c1

DistPath_{c1_c2}＝1-Oc(3)/norm(Oc)；

其中Oc表示V_c2相对于V_c1的位置，为3×1矩阵，Oc(i)表示获取Oc的第i个元素，Oc(3)表示获取Oc的第3个元素，norm为函数。

步骤2.4：计算C.

C＝(DistAngle_{c1_c2}+DistPath_{c1_c2}+α)×Dist_{c1_c2}

其中α为人为设定阈值。将{V_c1,V_c2,C}加入到Set_E。

步骤3：给定目标位置，并进行路径规划。

步骤3.1：给定目标位置{t_d,R_d}，在Set_V中查找距离目标位置最近的顶点V_end，即遍历Set_V中的元素，找到V_end使得下列公式最小：

dist_min＝norm(t_end-t_d)

V_end包含信息{t_end,R_end}。

步骤3.2：获取当前位置{t_c,R_c}，在Set_V中查找距离目标位置最近的顶点V_start，如步骤3.1中所述。即遍历Set_V中的元素，找到V_start使得下列公式最小：

dist_min＝norm(t_start-t_c)；

V_start包含信息{t_start,R_start}。

步骤3.3：对Set_V和Set_E使用Dijkstra算法在构建的地图查找最短路径，使得V_start与V_end之间距离最小。得到顶点集合List_path＝{V_start...V_pi...V_end}，其中V_pi∈Set_V。

步骤3.4：将当前位置和目标位置加入到List_path，最终得到路径List_path＝{{t_c,R_c},V_start...V_pi...V_end,{t_d,R_d}}。

Claims (4)

1.一种基于离散位姿的路径规划方法，其特征在于，包括以下步骤：

步骤1：将关键帧转化为顶点，将关键帧集合Set_K中的每一个关键帧K_i加入到顶点集合Set_V中；

步骤2：计算边集合Set_E({E₁...,E_i...,E_m})，E_i包含信息{V_c1,V_c2,C}，其中，V_c1与V_c2为Set_K中的任意两个不相同的顶点，C表示两个顶点相连的代价；

步骤3：给定目标位置，并进行路径规划。

2.根据权利要求1所述的基于离散位姿的路径规划方法，其特征在于，

步骤1中，通过视觉SLAM系统对环境进行地图构建，得到的地图包括：关键帧集合Set_K({K₁...,K_i...,K_Nk})，N_k为关键帧集合的元素数目，K₁表示第1个关键帧，K_Nk表示第N_k个关键帧，K_i表示第i个关键帧，K_i包含下列信息{t_i,R_i,I_i}，t_i表示关键帧K_i在世界坐标系中的位置，为3×1矩阵，R_i表示关键帧K_i的朝向，为3×3矩阵，I_i为采集到的图像；

顶点集合Set_V({V₁...,V_i...,V_n})，V_i包含信息{t_i,R_i}，V_i中的{t_i,R_i}和K_i中的{t_i,R_i}具有相同含义。

3.根据权利要求1所述的基于离散位姿的路径规划方法，其特征在于，

步骤2中，采用下列步骤计算任意{V_c1,V_c2}之间的代价C，具体包括：

步骤2.1：V_c1包含信息{t_c1,R_c1}，V_c2包含信息{t_c2,R_c2}，计算t_c1与t_c2之间的距离Dist_{c1_c2}：

Dist_{c1_c2}＝norm(t_c1-t_c2)；

其中，norm(t_c1-t_c2)为计算向量t_c1-t_c2的模，norm为计算函数；

如果Dist_{c1_c2}大于阈值DisThres，则C设置为+∞，将{V_c1,V_c2,C}加入到Set_E，否则执行步骤2.2；

步骤2.2：计算V_c1,V_c2之间的朝向误差DistAngle_{c1_c2}：

该步骤计算V_c1,V_c2之间的朝向误差DistAngle_{c1_c2}，继续执行步骤2.3；

步骤2.3：计算V_c2在V_c1中的位置，并得到路径角度误差DistPath_{c1_c2}；

Oc＝R_c1×t_c2+t_c1

DistPath_{c1_c2}＝1-Oc(3)/norm(Oc)

其中Oc表示V_c2相对于V_c1的位置，为3×1矩阵，Oc(i)表示获取Oc的第i个元素，Oc(3)表示获取Oc的第3个元素，norm为函数；

步骤2.4：计算C；

C＝(DistAngle_{c1_c2}+DistPath_{c1_c2}+α)×Dist_{c1_c2}

其中，α为人为设定阈值，将{V_c1,V_c2,C}加入到Set_E。

4.根据权利要求1所述的基于离散位姿的路径规划方法，其特征在于，

步骤3中，给定目标位置，并进行路径规划，具体包括：

步骤3.1：给定目标位置{t_d,R_d}，在Set_V中查找距离目标位置最近的顶点V_end，即遍历Set_V中的元素，找到V_end使得下列公式最小：

dist_min＝norm(t_end-t_d)；

V_end包含信息{t_end,R_end}；

步骤3.2：获取当前位置{t_c,R_c}，在Set_V中查找距离当前位置最近的顶点V_start，即遍历Set_V中的元素，找到V_start使得下列公式最小：

dist_min＝norm(t_start-t_c)；

V_start包含信息{t_start,R_start}；

步骤3.3：对Set_V和Set_E使用Dijkstra算法在地图中查找最短路径，使得V_start与V_end之间距离最小，得到顶点集合List_path＝{V_start...V_pi...V_end}，其中V_pi∈Set_V；

步骤3.4：将当前位置和目标位置加入到List_path，最终得到路径List_path＝{{t_c,R_c},V_start...V_pi...V_end,{t_d,R_d}}。