Research On Multi-sensor Integrated Navigation Algorithm And Its Realization In Embedded System

Autonomous navigation is the basic prerequisite for the robots to carry out the task independently,and it is also the key problem that the robots need to be solved in various fields.It has become a hotspot in the field of robot technology.At present,the commonly used autonomous navigation methods are inertial navigation,visual navigation and odometer navigation and so on.They directly use the sensors equipped with robots to percept movement without the exchange of information with the outside world,so these systems have a strong autonomy.However,a single autonomous navigation method can only be used in specific conditions,and cannot meet the higher requirements of mobile robots for navigation performance.Therefore,the combination of a variety of navigation systems is a solution to solve the problem of robot autonomous navigation.On the one hand,it can improve the navigation accuracy;on the other hand,it enhances the robustness and reliability of integrated navigation system.Therefore,according to the requirements of autonomous navigation of mobile robots,this paper studies the odometer-aided visual inertial integrated navigation algorithm in theory.Also,the embedded integrated navigation system based on ARM/GPU is designed at the practical application level.The main research work and contributions of this paper are summarized as follows:(1)The odometer-aided visual inertial integrated navigation algorithm based on cascade filtering is studied.Firstly,the system state equation is established according to the inertial navigation principle,and the system error state equation is deduced.Also,the odometer information is used as velocity observation,and the velocity error observation equation is deduced.It is input to the Kalman filter(KF)together with the inertial navigation results to complete the inertial/odometer combination based on KF.Secondly,the images acquired by camera at four adjacent times are taken as the visual input,and the SIFT features are extracted and matched respectively.The visual observation equation is composed of the geometric constraints(the epipolar geometry and trifocal tensor)between the images.The inertial/odometer combination results and visual observation are input to the Unscented Kalman Filter(UKF)to complete the status update to achieve visual/inertial/odometer integrated navigation.Finally,we use the KITTI open source dataset and the vehicle data collected by our research group to verify the feasibility and effectiveness of the proposed algorithm.(2)SIFT features extraction and matching method based on GPU implementation is researched.In this paper,CUDA programming is used to extract and match SIFT features based on GPU.Firstly,according to the principle of SIFT algorithm,GPU parallel acceleration code is constructed in four steps: building scale space,locating key points,calculating main direction and calculating feature description vector.Secondly,using the vector angle as the feature similarity measure method,the feature point sets of the two images to be matched are expressed as matrices,and the feature matching is realized by matrix multiplication and searching in the result matrix.At the same time,the code is verified by the actual scene pictures captured by the camera.(3)Based on ARM/GPU heterogeneous computing platform TX1,an embedded visual/inertial/odometer integrated navigation algorithm is implemented.First,the inertial navigation,KF filter,UKF filter and other functions in addition to SIFT feature extraction and matching function code are programmed using C/C++ language to complete the visual/inertial/odometer integrated navigation;Secondly,the performance of the embedded integrated navigation system is verified by using the KITTI open source dataset and the vehicle data collected by our research group.