基于捷联惯导的蛇形管道机器人定位算法

Localization Algorithm of Snake Shaped Pipeline Robot Based on Strapdown Inertial Navigation

为解决蛇形管道探测机器人因处于地下管道,难以定位的问题,基于牛顿第二定律为蛇形管道探测机器人设计了一种高精度的捷联式惯性导航定位系统(strapdown inertial navigation system,SINS)。系统搭载九轴惯性测量器件(inertial measurement unit,IMU)对蛇形机器人的加速度及角速率信息进行测量,融合卡尔曼滤波算法对定位系统进行误差补偿,采用四元数法构造捷联式惯性导航系统姿态矩阵进行姿态更新,再积分得到速度和位移。利用MATLAB软件根据上述算法对机器人的速度、位移进行分析计算,并与机器人真实的速度位移做比较,验证算法的可靠性。实验结果表明:所提算法的定位误差最大不超过4.7%,能够为管道探测机器人提供准确地速度和位置信息,具有较高的实用价值和意义。

In order to solve the problem that the snake shaped pipeline detection robot is difficult to locate because it is in the underground pipeline,a high-precision strapdown inertial navigation system(SINS)was designed for the snake shaped pipeline detection robot based on Newton's second law.The system was equipped with nine axis inertial measurement unit(IMU)to measure the acceleration and angular rate information of snake robot,integrated Kalman filter algorithm to compensate the error of positioning system,constructed strapdown inertial navigation system attitude matrix by quaternion method,updated the attitude,and then integrated to obtain velocity and displacement.The speed and displacement of the robot were analyzed and calculated by MATLAB software according to the above algorithm,and compared with the real speed and displacement of the robot to verify the reliability of the algorithm.The experimental results show that the maximum positioning error of the proposed algorithm is no more than 4.7%,which can provide accurate speed and position information for the pipeline detection robot,and has high practical value and significance.