Abstract This paper presents a robust multi-sensor fusion framework, Leg-KILO (Kinematic-Inertial-Lidar Odometry). When lidar-based SLAM is applied to legged robots, high-dynamic motion (e.g., trot gait) introduces frequent foot impacts, leading to IMU degradation and lidar motion distortion. Direct use of IMU measurements can cause significant drift, especially in the z-axis direction. To address these limitations, we tightly couple leg odometry, lidar odometry, and loop closure module based on graph optimization. For leg odometry, we propose a kinematic-inertial odometry using an on-manifold error-state Kalman filter, which incorporates the constraints from our proposed contact height detection to reduce height fluctuations. For lidar odometry, we present an adaptive scan slicing and splicing method to alleviate the effects of high-dynamic motion. We further propose a robot-centric incremental mapping system that enhances map maintenance efficiency. Extensive experiments are conducted in both indoor and outdoor environments, showing that Leg-KILO has lower drift performance compared to other state-of-the-art lidar-based methods, especially during high-dynamic motion. To benefit the legged robot community, a lidar-inertial dataset containing leg kinematic data and the code are released.
2024.07.31: The code is released.2024.07.20: The paper is accepted by RA-L 2024!
Related datasets have been released in link
The related video can be watched on Youtube.
Currently our code is tested on
- Ubuntu 18.04
- ROS melodic
- gtsam 4.0.3
- pcl 1.8
- opencv 3.2
- unitree_legged_msgs (has included in the project)
You can refer to the configuration process of LIOSAM.
We provide the ubuntu18.04 installation process, other versions may require some changes. We recommend that you install the ros desktop version, as this contains most of the required dependencies.
Ros
apt-get update
sudo apt-get install ros-melodic-desktop-full
sudo rosdep init
rosdep update
echo "source /opt/ros/melodic/setup.bash" >> ~/.bashrc
source ~/.bashrc
Gtsam
sudo apt-get install software-properties-common
sudo add-apt-repository ppa:borglab/gtsam-release-4.0
sudo apt install libgtsam-dev libgtsam-unstable-dev
cd ~/legkilo_ws/src
git clone https://github.com/ouguangjun/Leg-KILO.git
cd ..
catkin_make
Download the dataset from link
source devel/setup.bash
roslaunch legkilo run.launch
rosbag play xxxx.bag
Our project is developed on LIOSAM and retains the part of lidar optimization and loop closure detection , thanks very much to the authors for their excellent open source work. And also thanks to fast lio for the ikd-tree, and A1-QP-MPC-Controller.
The code is under BSD 3-Clause License, the same as LIOSAM.
- With Docker configuration
- Test on other ubuntu version
- The matlab code for evaluation
- Test on other open source legged robot's datasets