RESUMO
This paper proposes a method to solve the problem of localization and mapping of a two-wheeled inverted pendulum (TWIP) robot on approximately flat ground using a Lidar-IMU-Odometer system. When TWIP is in motion, it is constrained by the ground and suffers from motion disturbances caused by rough terrain or motion shaking. Combining the motion characteristics of TWIP, this paper proposes a framework for localization consisting of a Lidar-IMU-Odometer system. This system formulates a factor graph with five types of factors, thereby coupling relative and absolute measurements from different sensors (including ground constraints) into the system. Moreover, we analyze the constraint dimension of each factor according to the motion characteristics of TWIP and propose a new nonholonomic constraint factor for the odometry pre-integration constraint and ground constraint factor in order to add them naturally to the factor graph with the robot state node on SE(3). Meanwhile, we calculate the uncertainty of each constraint. Utilizing such a nonholonomic constraint factor, a complete lidar-IMU-odometry-based motion estimation system for TWIP is developed via smoothing and mapping. Indoor and outdoor experiments show that our method has better accuracy for two-wheeled inverted pendulum robots.