ATTITUDE ESTIMATION APPARATUS AND METHOD IN IN-PIPE ROBOT

摘要

The present invention measures an environmental status of a robot, and sets a quaternion expressing an attitude of a robot by reflecting the measured values. Next, an error quaternion, which is an error value between a gravitational vector extracted from the set quaternion and a prestored actual gravitational vector, is calculated. Afterwards, the calculated error quaternion is inputted in an attitude estimation function to calculate an estimation vector of the error quaternion. The quaternion is corrected using the estimation vector to correct the error between the actual environment and the measured value. Next, the corrected value is extracted to reset the quaternion using the corrected value. The attitude of the robot is then determined based on the reset quaternion. Accordingly, the present invention prevents a gimbal lock phenomenon from occurring when estimating the attitude of the robot, and corrects the attitude of the robot using an estimated value on an error between a status value measured by a sensor and the actual environment; thereby determining the attitude of the robot by estimating noise.