Algorithms For Graph Based Autonomous Navigation

In order to solve the problem of autonomous navigation of ground robot in an unknown and unstructured environment without Global Navigation Satellite System(GNSS),this paper focus on the autonomous navigation method based on stereo cameras,Miniature Inertial Measurement Unit(MIMU)and polarized skylight compass,the main research and innovation points are as follows:Research on how to apply the graph theory in autonomous navigation was given,and graph nodes and edges based navigation information representation method was proposed.In this method,the graph nodes contain a set of poses(position and heading)information,while the graph edges carry constraint information.The constraint graph edges are divided into two categories,namely relative constraint edges and absolute constraint edges.The two kinds of constraints are represented by ordinary edges and annular edges respectively.Basing on the representation method presented here,an improved graph optimization algorithm was developed which is able to make full used of both relative and absolute constraint edges.Simulation results show that the algorithm has better optimization performance than existing approach.After the depth analysis of error model of polarized skylight compass,we proposed an Iterative Least Square(ILS)based calibration method and a three-channels and arbitrary installation angles based polarization angle computation method.Simulation and experimental results show that the proposed methods have higher accuracy than existing methods.To solve the calibration problem of stereo camera/MIMU system,we presented a calibration method by measuring the vertical direction vector on multiple static positions and analyzed the error propagation characteristics.The proposed method is able to provide an accurate relative attitude between camera and MIMU.Experimental results evaluated the feasibility and accuracy of the proposed calibration method.Two tightly coupled stereo cameras / MIMU integration algorithms are presented,one is an Iterative Extended Kalman Filter(IEKF)for far and near features based stereo cameras/inertial navigation,and another is a Square Root Unscented Kalman Filter for multiple view geometry based stereo cameras/inertial navigation.A self-developed binocular/inertial system was used to evaluate the IEKF based algorithm,the outdoor experimental results show that the proposed algorithm has more multiple orders of magnitude improvement than the inertial-only solution,and has a more precise and smooth motion estimation than the visual-only solution.Besides,the SRUKF based approach has been applied to a publicly available date sets,the KITTI data.The results and comparison show that the proposed algorithm has a better precise location and heading estimation than the stereo visual odometry and UKF based monocular/inertial approach.A graph node based dead reckoning algorithm for binocular/MIMU/skylight compass integrated navigation was developed.The detailed algorithm flow was presented.Furthermore,a skylight compass aided visual loop closure detection and place recognition algorithm was provided which is able to improve the accuracy and computational performance of loop closure detection and place recognition.In order to test the proposed algorithm,a self-developed multi-sensors platform was employed to an outdoor vehicle experiment.The results and comparisons show that compared with traditional navigation algorithms,the positioning error of proposed algorithm is not affected by initial alignment error and does not accumulate over time.