Integrated Perception and Planning for Autonomous Vehicle Navigation: An Optimization-Based Approach
We propose an optimization-based integrated perception and planning framework for autonomous vehicle navigation that achieves real-time state estimation and path planning with high accuracy and robustness. Our Simultaneous Localization And Mapping (SLAM) module is based on Error-State Extended Kalman Filter (ES-EKF) for LiDAR-Inertial sensor fusion. The SLAM system generates a cost map using Euclidean Distance Transform (EDT) that directly encodes environmental constraints as a cost map. A non-linear trajectory optimization problem is formulated with the cost function and solved in real-time using the direct collocation approach. Our results on the KITTI dataset demonstrate the effectiveness of our framework.