Pose Estimation Based on Wheel Speed Anomaly Detection in Monocular Visual-Inertial SLAM

摘要

Considering the adverse impact of speed measurement on the accuracy of pose estimation after a mobile robot slips, collides, or abducts, this paper proposes a monocular inertial simultaneous localization and mapping algorithm that includes wheel speed anomaly detection. The algorithm adds wheel speed measurement to the least squares problem in a tightly coupled manner and uses a nonlinear optimization method to maximize the posterior probability to solve the optimal state estimation. For the speed control of the Mecanum wheel, because the existing closed-loop speed control method cannot calculate the motion constraint error, this paper reports a design of a control method of the Mecanum wheel moving chassis based on torque control, which can use the motion constraint error to estimate the credibility of the wheel speed measurement to detect whether the chassis movement status is abnormal; meanwhile, to prevent the chassis speed measurement error from adversely affecting the robot pose estimation, this paper uses three methods to actively detect whether the chassis movement is abnormal, and analyze the chassis movement status in real time. When it is determined that the chassis has abnormal motion, the wheel odometer pre-integration measurement of the current frame is actively removed from the state estimation equation, thereby ensuring the accuracy of the pose estimation. Experimental results show the feasibility and effectiveness of the method proposed in this paper, and the algorithm is robust.