基于改进EKF的柔性绳索自动升降装置姿态估计.pdfVIP

基于改进ＥＫＦ的柔性绳索

自动升降装置姿态估计

摘要：提出了一种柔性绳索自动升降装置姿态估计算法，使用姿态四元数表示柔性绳索自动升降装置的姿态，使用扩展

卡尔曼滤波器融合磁传感器、角速率传感器和重力传感器的优点，保证了姿态估计的精度，避免了估计误差导致的自动

升降装置侧翻等安全性问题。首先，选择陀螺仪的姿态四元数和漂移偏差来构建状态向量，并基于陀螺仪的运动学模

型建立状态方程。其次，利用正交化方法从加速度计和磁力计的输出中获得单位姿态四元数，使磁场矢量垂直于测得

的重力矢量，避免地磁干扰，用于ＥＫＦ的测量。接着，使用ＥＫＦ更新方程确定柔性绳索自动升降装置的姿态。最后，通

过静态实验和动态实验证明了所提方法的有效性，静态偏差控制在０．２°之内，在保持平滑的前提下收敛速度也快于传

统ＥＫＦ算法。

关键词：柔性绳索；自动升降装置；扩展卡尔曼滤波；四元数；姿态估计

ＡｔｔｉｔｕｄｅＥｓｔｉｍａｔｉｏｎｏｆＦｌｅｘｉｂｌｅＲｏｐｅＡｕｔｏｍａｔｉｃＬｉｆｔｉｎｇＤｅｖｉｃｅ

ＢａｓｅｄｏｎＩｍｐｒｏｖｅｄＥＫＦ

Ａｂｓｔｒａｃｔ：Ａｎａｔｔｉｔｕｄｅｅｓｔｉｍａｔｉｏｎａｌｇｏｒｉｔｈｍｆｏｒｆｌｅｘｉｂｌｅｒｏｐｅａｕｔｏｍａｔｉｃｌｉｆｔｉｎｇｄｅｖｉｃｅｗａｓｐｒｏｐｏｓｅｄ．Ｔｈｅ

ａｌｇｏｒｉｔｈｍｕｓｅｄｔｈｅａｔｔｉｔｕｄｅｑｕａｔｅｒｎｉｏｎｔｏｒｅｐｒｅｓｅｎｔｔｈｅａｔｔｉｔｕｄｅｏｆｔｈｅｆｌｅｘｉｂｌｅｒｏｐｅａｕｔｏｍａｔｉｃｌｉｆｔｉｎｇｄｅｖｉｃｅ，

ａｎｄｕｓｅｄｔｈｅｅｘｔｅｎｄｅｄＫａｌｍａｎｆｉｌｔｅｒ（ＥＫＦ）ｔｏｆｕｓｅｓｅｎｓｏｒｓｄａｔａ．Ａｓｔａｔｅｅｑｕａｔｉｏｎｗａｓｅｓｔａｂｌｉｓｈｅｄｂａｓｅｄ

ｏｎｔｈｅｋｉｎｅｍａｔｉｃｍｏｄｅｌｏｆｔｈｅｇｙｒｏｓｃｏｐｅａｎｄｔｈｅａｔｔｉｔｕｄｅｖｅｃｔｏｒｑｕａｔｅｒｎｉｏｎｏｆｔｈｅｇｙｒｏｓｃｏｐｅ．Ｔｈｅｏｒｔｈｏｇｏｎａｌ

ａｔｔｉｔｕｄｅｍｅｔｈｏｄｗａｓｕｓｅｄｔｏｏｂｔａｉｎｔｈｅｕｎｉｔａｔｔｉｔｕｄｅｑｕａｔｅｒｎｉｏｎｆｒｏｍｔｈｅｏｕｔｐｕｔｏｆｔｈｅａｃｃｅｌｅｒｏｍｅｔｅｒａｎｄ

收稿日期：２０２０－０２－１４；修回日期：２０２０－０３－０９

张富春，等：基于改进ＥＫＦ的柔性绳索自动升降装置姿态估计２３１



ｍａｇｎｅｔｏｍｅｔｅｒ．ＴｈｅＥＫＦｕｐｄａｔｅｅｑｕａｔｉｏｎｗａｓｕｓｅｄｔｏｄｅｔｅｒｍｉｎｅｔｈｅａｔｔｉｔｕｄｅｏｆｔｈｅｆｌｅｘｉｂｌｅｒｏｐｅａｕｔｏｍａｔｉｃ

ｌｉｆｔｉｎｇｄｅｖｉｃｅ．Ｔｈｅｅｆｆｅｃｔｉｖｅｎｅｓｓｏｆｔｈｅｐｒｏｐｏｓｅｄｍｅｔｈｏｄｗａｓｖｅｒｉｆｉｅｄｂｙｓｔａｔｉｃｅｘｐｅｒｉｍｅｎｔｓａｎｄｄｙｎａｍｉｃ

ｅｘｐｅｒｉｍｅｎｔｓ．Ｔｈｅｓｔａｔｉｃｄｅｖｉａｔｉｏｎｉｓｓｍａｌｌｅｒｔｈａｎ０．２°，ａｎｄｔｈｅｃｏｎｖｅｒｇｅｎｃｅｓｐｅｅｄｉｓｆａｓｔｅｒｔｈａｎｔｈｅ

ｔｒａｄｉｔｉｏｎａｌＥＫＦａｌｇｏｒｉｔｈｍｗｈｉｌｅｍａｉｎｔａｉｎｉｎｇｓｍｏｏｔｈｎｅｓｓ．

Ｋｅｙｗｏｒｄｓ：ｆｌｅｘｉｂｌｅｒｏｐｅ；ａｕｔｏｍａｔｉｃｌｉｆｔｉｎｇｄｅｖｉｃｅ；ｅｘｔｅｎｄｅｄＫａｌｍａｎｆｉｌｔｅｒ；ｑｕａｔｅｒｎｉｏｎ；ａｔｔｉｔｕｄｅｅｓｔｉｍａ

ｔｉｏｎ

高空检修作业是电网稳定运行的必要手段。近年来，新导航坐标系中的方位，考虑到处理的方便性，采用四元数表

建线路均采用高跨设计，而且大部分线路地处偏远、环境恶［１５］

示。定义四元数ｑ为：

劣、交通不便，这对线路高空维护检修及其安全性带来了巨ｃｏｓ（／２）

α