单球驱动自平衡机器人的互补滤波姿态解算

Ａｔｔｉｔｕｄｅ ａｌｇｏｒｉｔｈｍ ｂａｓｅｄ ｏｎ ｃｏｍｐｌｅｍｅｎｔａｒｙ ｆｉｌｔｅｒ ｆｏｒ ｓｅｌｆ－ｂａｌａｎｃｉｎｇ ｒｏｂｏｔ ｄｒｉｖｅｎ ｂｙ ｓｉｎｇｌｅ ｂａｌｌ
ＹＵ Ｙｉ １，ＺＨＡＮＧ Ｚｈｅｎｇ１，２
（１．Ｓｃｈｏｏｌ ｏｆ Ｉｎｆｏｒｍａｔｉｏｎ Ｓｃｉｅｎｃｅ ａｎｄ Ｅｎｇｉｎｅｅｒｉｎｇ，Ｗｕｈａｎ Ｕｎｉｖｅｒｓｉｔｙ ｏｆ Ｓｃｉｅｎｃｅ ａｎｄ Ｔｅｃｈｎｏｌｏｇｙ，Ｗｕｈａｎ ４３００８１，Ｃｈｉｎａ； ２．Ｅｎｇｉｎｅｅｒｉｎｇ Ｒｅｓｅａｒｃｈ Ｃｅｎｔｅｒ ｏｆ Ｍｅｔａｌｌｕｒｉｃａｌ Ａｕｔｏｍａｔｉｏｎ ａｎｄ Ｍｅａｓｕｒｅｍｅｎｔ Ｔｅｃｈｎｏｌｏｇｙ ｏｆ Ｍｉｎｉｓｔｒｙ ｏｆ Ｅｄｕｃａｔｉｏｎ， Ｗｕｈａｎ Ｕｎｉｖｅｒｓｉｔｙ ｏｆ Ｓｃｉｅｎｃｅ ａｎｄ Ｔｅｃｈｎｏｌｏｇｙ，Ｗｕｈａｎ ４３００８１，Ｃｈｉｎａ）
ຫໍສະໝຸດ Baidu０ 引 言
近十年来，一类以单球体作为驱动轮的轮式自平衡机 器 人 逐 渐 引 起 了 国 内 外 学 者 的 关 注 。 ［１－１０］ 该 类 自 平 衡 机 器 人利用万向轮与底部支撑球体的摩擦产生驱动力，实现机
器人的自平衡、自旋以及全方位移动，即使在狭窄区域内 机器人也能灵活 移 动。 根 据 单 球 驱 动 自 平 衡 机 器 人 （ｓｅｌｆ－ ｂａｌａｎｃｉｎｇ ｒｏｂｏｔ ｄｒｉｖｅｎ ｂｙ ｓｉｎｇｌｅ ｂａｌｌ，ＳＲＤＳＢ） 的 动 力 学 模 型［８］分析可 知， 这 一 类 机 器 人 具 有 多 变 量 、 强 耦 合 、 欠 驱 动 等 非 线 性 特 征 。 ［１１］ 在 有 效 的 轮 球 式 驱 动 结 构 基 础 上 ， 为
２０１９ 年 ４ 月 第 ４０ 卷 第 ４ 期
计算机工程与设计
ＣＯＭＰＵＴＥＲ ＥＮＧＩＮＥＥＲＩＮＧ ＡＮＤ ＤＥＳＩＧＮ
Ａｐｒ．２０１９ Ｖｏｌ．４０ Ｎｏ．４

单球驱动自平衡机器人的互补滤波姿态解算
余 义１，章 政１，２
（１．武汉科技大学 信息科学与工程学院，湖北 武汉 ４３００８１； ２．武汉科技大学 冶金自动化与检测技术教育部工程研究中心，湖北 武汉 ４３００８１）
摘 要：针对单球驱动自平衡机器人的低成本、高精度、实时姿态估计需求，设计一种基于比例－积分 （ＰＩ） 型 互 补 滤 波 的 姿态估计算法。在四元数姿态角更新方程的基础 上， 通 过 融 合 陀 螺 仪／加 速 度 计 的 姿 态 信 息 实 现 对 机 器 人 水 平 姿 态 信 息 的 估计，实现多传感器间频率点的切换。考虑到磁力计易受到干扰的情况，采用陀螺仪和磁力计单独对自平衡机器人的偏航 角姿态解算，避免磁力计参与四元数姿态解算引起的水平姿态误差。实验结果表明，该互补滤波算法有效融合了陀螺仪的 高 频 特 性 、 加 速 度 计 和 磁 力 计 的 低 频 特 性 ， 系 统 的 姿 态 估 计 精 度 在 ±０．１５°内 。 关 键词：单球驱动自平衡机器人；互补滤波；姿态解算；陀螺仪／加速度计／磁力计；四元数 中图法分类号：ＴＰ２７ 文献标识号：Ａ 文章编号：１０００－７０２４ （２０１９）０４－９６９－０７ ｄｏｉ：１０．１６２０８／ｊ．ｉｓｓｎ１０００－７０２４．２０１９．０４．０１２
Ａｂｓｔｒａｃｔ：Ａｎ ａｔｔｉｔｕｄｅ ｅｓｔｉｍａｔｉｏｎ ａｌｇｏｒｉｔｈｍ ｗｉｔｈ ａ ｐｒｏｐｏｒｔｉｏｎａｌ－ｉｎｔｅｇｒａｌ （ＰＩ）ｃｏｍｐｌｅｍｅｎｔａｒｙ ｆｉｌｔｅｒ ｗａｓ ｐｒｏｐｏｓｅｄ ｆｏｒ ｔｈｅ ｓｅｌｆ－ ｂａｌａｎｃｉｎｇ ｒｏｂｏｔ ｄｒｉｖｅｎ ｂｙ ｓｉｎｇｌｅ ｂａｌｌ（ＳＲＤＳＢ），ｗｈｉｃｈ ｗａｓ ｏｐｅｒａｔｉｎｇ ｗｉｔｈ ｌｏｗ－ｃｏｓｔ，ｈｉｇｈ－ｐｒｅｃｉｓｉｏｎ ａｎｄ ｒｅａｌ－ｔｉｍｅ ａｔｔｉｔｕｄｅ．Ｏｎ ｔｈｅ ｂａｓｉｓ ｏｆ ｔｈｅ ｑｕａｔｅｒｎｉｏｎ ｒｅｎｅｗａｌ ｅｑｕａｔｉｏｎｓ ｏｆ ｔｈｅ ａｔｔｉｔｕｄｅ ａｎｇｌｅｓ，ｔｈｅ ａｔｔｉｔｕｄｅ ｅｓｔｉｍａｔｉｏｎ ｏｆ ＳＲＤＳＢ ｗａｓ ｉｍｐｌｅｍｅｎｔｅｄ ｂｙ ｆｕｓｉｎｇ ｔｈｅ ｏｕｔｐｕｔｓ ｏｆ ｇｙｒｏｓｃｏｐｅｓ ａｎｄ ａｃｃｅｌｅｒｏｍｅｔｅｒｓ，ｗｈｏｓｅ ｆｒｅｑｕｅｎｃｙ ｓｗｉｔｃｈｉｎｇ ｐｏｉｎｔ ｗａｓ ｒｅｇｕｌａｔｅｄ ｔｈｒｏｕｇｈ ａ ＰＩ ｃｏｍｐｏｎｅｎｔ．Ｔｈｅ ｇｙｒｏ－ ｓｃｏｐｅ ａｎｄ ｔｈｅ ｍａｇｎｅｔｏｍｅｔｅｒ ｗｅｒｅ ｆｕｓｅｄ ｔｏ ｃａｌｃｕｌａｔｅ ｔｈｅ ｙａｗ ａｔｔｉｔｕｄｅ ｉｎｄｅｐｅｎｄｅｎｔｌｙ ｔｏ ａｖｏｉｄ ｔｈｅ ａｄｖｅｒｓｅ ｉｎｆｌｕｅｎｃｅ ｏｆ ｔｈｅ ｍａｇｎｅ－ ｔｏｍｅｔｅｒ ｏｎ ｔｈｅ ｈｏｒｉｚｏｎｔａｌ ａｔｔｉｔｕｄｅ．Ｅｘｐｅｒｉｍｅｎｔａｌ ｒｅｓｕｌｔｓ ｓｈｏｗ ｔｈａｔ ｔｈｅ ｐｒｏｐｏｓｅｄ ｃｏｍｐｌｅｍｅｎｔａｒｙ ｆｉｌｔｅｒ ｉｓ ｃｏｍｂｉｎｅｄ ｔｈｅ ｈｉｇｈ ｆｒｅ－ ｑｕｅｎｃｙ ｏｆ ｇｙｒｏｓｃｏｐｅ ｗｉｔｈ ｔｈｅ ｌｏｗ ｆｒｅｑｕｅｎｃｙ ｏｆ ａｃｃｅｌｅｒｏｍｅｔｅｒ ａｎｄ ｍａｇｎｅｔｏｍｅｔｅｒ，ｍｅａｎｗｈｉｌｅ，ｉｔ ｉｓ ｆｅａｓｉｂｌｅ ｆｏｒ ｔｈｅ ｒｅａｌ－ｔｉｍｅ ｃｏｎ－ ｔｒｏｌ ｒｅｑｕｉｒｅｍｅｎｔ ｏｆ ＳＲＤＳＢ ｗｉｔｈ ｔｈｅ ｄｙｎａｍｉｃ ｅｓｔｉｍａｔｉｏｎ ｅｒｒｏｒ ｃｏｎｔｒｏｌｌｅｄ ｗｉｔｈｉｎ ±０．１５°． Ｋｅｙ ｗｏｒｄｓ：ｓｅｌｆ－ｂａｌａｎｃｉｎｇ ｒｏｂｏｔ ｄｒｉｖｅｎ ｂｙ ｓｉｎｇｌｅ ｂａｌｌ；ｃｏｍｐｌｅｍｅｎｔａｒｙ ｆｉｌｔｅｒ；ａｔｔｉｔｕｄｅ ａｌｇｏｒｉｔｈｍ；ｇｙｒｏｓｃｏｐｅ／ａｃｃｅｌｅｒｏｍｅｔｅｒ／ｍａｇ－ ｎｅｔｏｍｅｔｅｒ；ｑｕａｔｅｒｎｉｏｎ