[发明专利]一种基于离散位姿的路径规划方法

说明书

本发明公开了一种基于离散位姿的路径规划方法，包括以下步骤：步骤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：给定目标位置，并进行路径规划。本发明基于离散位姿的路径规划方法，给定基于关键帧的地图，在该地图进行快速路径规划，并尽可能使规划后的路径靠近关键帧，解决了基于离散位姿的路径规划，保证机器人使用本发明可以运行的稳定可靠。

技术领域

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

背景技术

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

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

发明内容

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

本发明所述地图包括关键帧集合与三维点集合Set_MN_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中。