[发明专利]基于视觉拓扑节点和惯性导航的定位方法

权利要求书

1.一种基于视觉拓扑节点和惯性导航的定位方法，所述方法包括以下步骤：

第一步骤(S1)中，读取地图以获得各个拓扑节点信息，记作N₁，N₂……N_n，拓扑节点具有以下数据结构：

N_n：{

E_mn，

NP_n，

KPT：{KPT₁，KPT₂……KPT_n}，

DES：{DES₁，DES₂……DES_n}

}，其中，E_mn表示从拓扑节点N_m到拓扑节点N_n这个方向是可以行驶到达的；NP_n表示拓扑节点所在世界坐标系的坐标；KPT_k表示第k个视觉特征点的特征信息，每个特征点信息由两个数字表示，(m，n)，其表示图片中第m行n列对应的像素点为特征点；DES_k表示第k个视觉特征点的描述符，

第二步骤(S2)中，基于拓扑路径规划得到驶向最终目标点的全局拓扑路径；

第三步骤(S3)中，当前机器人所在位置为P_t，当前在所述全局拓扑路径中由拓扑节点N_n驶向拓扑节点N_m的过程中，在全局路径规划下基于机器人所在环境进行局部路径规划，其中，基于惯性导航得到机器人定位信息，基于视觉定位方法更新机器人定位信息，基于机器人定位信息和障碍物信息局部路径规划得到机器人控制信息；

第四步骤(S4)中，机器人接受并执行所述控制信息，执行结束后进行状态判断，当前的机器人定位信息与当前目标拓扑节点N_m进行比较，若判断未达到目标拓扑节点，继续进行局部路径规划和控制；若由当前的定位信息判断机器人目前已到达拓扑节点，调用视觉定位，利用视觉定位结果强制更新机器人定位，修正在拓扑节点N_n到N_m运行过程中产生的漂移，完成定位更新后，判断当前是否已达最终目标点，若未达到，回到局部路径规划，继续产生规划路径进行控制；若判断达到最终目标点，定位结束。

2.根据权利要求1所述的方法，其中，拓扑地图的拓扑节点信息包括拓扑节点的位姿信息和视觉信息。

3.根据权利要求2所述的方法，其中，所述位姿信息包括三维位置信息及三维转向角信息，所述视觉信息包括视觉的特征点及所述特征点的描述符。

4.根据权利要求1所述的方法，其中，第一步骤(S1)中，读取地图时，将机器人启动时的位置作为世界坐标系原点，记作P₀。

5.根据权利要求1所述的方法，其中，第二步骤(S2)中，基于路径规划算法如：Dijkstra得到驶向最终目标点的全局拓扑路径。

6.根据权利要求1所述的方法，其中，第三步骤(S3)中，基于惯性导航得到机器人定位信息时，机载惯性测量单元IMU在预定固定频率f下采样机器人的瞬时加速度并对测量值积分得到机器人相对于出发原点P₀的相对位移ΔP，得到的机器人在世界坐标系下的位置P_t＝P₀+ΔP。