Two-stage active instant positioning and mapping algorithm based on graph topology

Abstract

The invention discloses a two-stage active instant positioning and mapping algorithm based on graph topology. The algorithm is divided into a global stage and a local stage. The method comprises the following steps: firstly, acquiring a current pose, a pose map and a navigation map of an unmanned platform by an algorithm, and calculating local leading edge points and candidate loopback points; if the points exist, entering a local exploration stage, constructing a local topological graph through a fast random spanning tree algorithm, solving a shortest path to any vertex of the local topological graph by using a Dijkstra algorithm, scoring a path set according to a local exploration and active loopback joint objective function, solving an optimal path, and finally obtaining the optimal path. And updating the global topological graph and the global frontier points according to the local topological graph and the local frontier points. On the contrary, when no local leading edge point exists in the local range and no loopback requirement exists, the algorithm enters a global exploration stage, the unmanned platform goes to the unexplored area to conduct local exploration again according to the global topological graph, and after exploration of the designated area is completed, the unmanned platform returns to the starting point, and the exploration task is completed.

Description

technical field [0001] The invention belongs to the technical field of perception and autonomous planning of ground unmanned platforms, and in particular relates to a two-stage active real-time positioning and mapping algorithm based on graph topology. Background technique [0002] The Unknown Environment Autonomous Exploration technology refers to the unmanned platform autonomously searching and detecting the unknown environment in the absence of prior information, collecting environmental information, and providing favorable support for subsequent disposal. It can be widely used in urban drainage. It is used in dangerous scenarios such as explosion and risk removal, nuclear biochemical environmental survey, mine rescue, and asteroid development. The significance is that the complex environment can be surveyed without relying on external positioning devices (such as Beidou positioning) or personnel manipulation, which greatly reduces the risk of detection tasks. Simultaneo...

AI Technical Summary

Problems solved by technology

[0006] Scheme 3: Literature (Chen Y, Huang S, Zhao L, et al. Cramér–Rao Bounds and Optimal Design Metrics for Pose-Graph SLAM [J]. IEEE Transactions on Robotics, 2021, PP(99): 1-15.) This literature proposes to use the Fisher information matrix and the minimum spanning tree number to measure the uncertainty of the pose graph in the SLAM algorithm, but it does not apply the scale index to active exploration.

Images