The Research On Multi-sources 3D Mapping And Cooperative Soldier Navigation Algorithm

Soldier navigation is an important part of soldier combat system.In order to further improve the accuracy of soldier navigation and build 3D battlefield map in real time,combined with the characteristics of soldier’s movement and the characteristics of new lidar,this paper studies the 3D mapping and positioning algorithm for soldier,cooperative navigation algorithm and multi-sensors fusion algorithm.The main contents of this paper are as follows.Firstly,according to the characteristics of the new lidar and the requirements of soldier navigation,a 3D mapping and positioning algorithm for soldier is designed.Aiming at the problem of closed-loop detection,a soldier gait assisted key frame construction algorithm of laser point cloud and a closed-loop detection algorithm based on soldiers’ pose and descriptor similarity are designed.The two algorithms are combined to form the closed-loop detection module which make the positioning and mapping accuracy of SLAM algorithm for soldier is further improved.Secondly,in order to further improve the navigation accuracy,the soldiers cooperative navigation algorithm based on Kalman filter is studied by using the cooperative information individual soldiers;In order to fuse the position and attitude information of cooperative navigation with the position and attitude calculated by laser odometer,it is necessary to add the cooperative navigation information as a factor node to the back-end optimization framework of soldier SLAM.Therefore,the soldier cooperative navigation algorithm based on factor graph is studied,and the real-time individual soldier cooperative navigation system is implemented based on ROS.Finally,in order to make full use of the navigation information between soldier team members and further improve the accuracy of positioning and mapping,a multi-sensors fusion algorithm based on factor graph is designed to realize multi-sources positioning and 3D mapping system for soldier.Several groups of experiments were carried out indoors and outdoors.The experimental results show that the soldier multi-source positioning and 3D mapping algorithm designed in this paper has higher positioning accuracy and stronger adaptability than inertial navigation algorithm based on zero speed correction,and can achieve good3 D scene mapping.