提出了一种柔性绳索自动升降装置姿态估计算法,使用姿态四元数表示柔性绳索自动升降装置的姿态,使用扩展卡尔曼滤波器融合磁传感器、角速率传感器和重力传感器的优点,保证了姿态估计的精度,避免了估计误差导致的自动升降装置侧翻等安全性问题.首先,选择陀螺仪的姿态四元数和漂移偏差来构建状态向量,并基于陀螺仪的运动学模型建立状态方程.其次,利用正交化方法从加速度计和磁力计的输出中获得单位姿态四元数,使磁场矢量垂直于测得的重力矢量,避免地磁干扰,用于EKF的测量.接着,使用EKF更新方程确定柔性绳索自动升降装置的姿态.最后,通过静态实验和动态实验证明了所提方法的有效性,静态偏差控制在0.2°之内,在保持平滑的前提下收敛速度也快于传统EKF算法.
柔性绳索 自动升降装置 扩展卡尔曼滤波 四元数 姿态估计