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

权利要求书

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

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

步骤2：计算边集合Set_E({E₁...,E_i...,E_m})，E_i表示边集合Set_E中的第i个元素，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(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；

步骤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}}。

2.根据权利要求1所述的基于离散位姿的路径规划方法，其特征在于，步骤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}具有相同含义。