基于惯性测量单元旋转的陀螺漂移估计和补偿方法

ＩＳＳＮ１０００００５４
－
／
ＣＮ１１２２２３Ｎ
－
清华大学学报 （自 然科学版）
２
年 第
０１４５４
卷 第
９
期

（），，，
ＮＪＴｓｉｎｈｕａＵｎｉｖ Ｓｃｉ＆ Ｔｅｃｈｎｏｌ２０１４Ｖｏｌ．５４ｏ．９
ｇ
／
４２５
１１４３１１４７
－




基于惯性测量单元旋转的陀螺漂移估计和补偿方法

吴哲明
１
，
孙振国
１
，
张文增
１
，
陈 强
１
，
２

（
，
嘉兴
）
清 华大学 机械工程系
，
先进成形制造教育部重点实验室
，
北京
１．１０００８４
；
２．
浙江清华长三角研究院
３１４００６



摘 要
：
陀螺仪的漂移误差是影响惯性导航系统的姿态测
量精度的重要因素
。
常用的陀螺漂移估计 及航向角误差修
正方法需要用磁强计或
ＧＰＳ
等 外部传感器的辅助数据来实
现
，
存 在室内
、
磁 干扰等环境下应用 受 限 的 问 题
。
对 此 该 文
提出了惯性测量单元旋转的改进方法
。
该 方法在测量过程
中对陀螺仪施加独立于物体运动的特定旋转
，
通 过旋转前后
不同姿态下陀螺仪和加速度计输出的倾角数值
，
得 到惯性测
量单元漂移估计
。
实验表明
，
该估计算法可有效地提 高航向
角的测量精度
。

关键词：姿态测量；陀 螺 漂 移；惯 性 测 量 单 元 （
ＩＭＵ
）旋 转；
航向角修正
中图分类号 ：
Ｖ２４９．３２
文献标志码 ：
Ａ
文章编号
：
１０ ００
－
００５４
（
２０１４
）
０９
－
１１ ４３
－
０５


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

Ｗ ＵＺｈｅｍｉｎ
ｇ
１
，
ＳＵＮＺｈｅｎ
ｇ
ｕｏ
１< br>，
ＺＨＡＮＧ Ｗｅｎｚｅｎ
ｇ
１
，
ＣＨＥＮＱｉａｎ
ｇ
１
，
２

（
１．Ｋｅ
ｙ
Ｌａｂｏｒａ ｔｏｒ
ｙ
ｆｏｒＡｄｖａｎｃｅｄ ＭａｔｅｒｉａｌｓＰｒｏｃｅｓｓｉｎ
ｇ
Ｔｅｃｈｎｏｌｏ
ｇｙ
ｏｆＭｉ
ｎｉｓｔｒ
ｙ
ｏｆＥｄｕｃａｔｉｏｎ
，
Ｄｅ
ｐ
ａ ｒｔｍｅｎｔｏｆＭｅｃｈａｎｉｃａｌ Ｅｎ
ｇ
ｉｎｅｅｒｉｎ
ｇ
，
Ｔｓｉ
ｎ
ｇ
ｈｕａＵｎｉｖｅｒｓｉｔ
ｙ
，
Ｂｅｉ
ｊ
ｉｎ
ｇ
１０００８４
，
Ｃｈｉｎａ
；
２．Ｙ ａｎ
ｇ
ｔｚｅＤｅｌｔａＲｅ
ｇ
ｉｏｎＩ
ｎｓｔｉｔｕｔｅｏｆＴｓ ｉｎ
ｇ
ｈｕａＵｎｉｖｅｒｓｉｔ
ｙ
，
Ｊｉａｘｉｎ
ｇ
３１４００６
，
Ｃｈｉｎａ
）
< br>Ａｂｓｔｒａｃｔ
：
Ｇ
ｙ
ｒｏｓｃｏ
ｐ
ｅｂｉａｓｉ ｓａ ｍａ
ｊ
ｏｒｆａｃｔｏｒａｆｆｅｃｔｉｎ
ｇ
ｔｈｅａｃｃｕｒａｃ< br>ｙ
ｏｆ ａ
ｔｔｉｔｕｄｅｅｓｔｉｍａｔｅｓｉｎｉｎｅｒｔｉａｌｎａｖｉ
ｇ
ａｔｉｏｎｓ
ｙ
ｓｔｅｍｓ．Ｅｘｉｓｔｉｎ
ｇ
ｍｅｔｈｏｄｓｔ ｏ ｅｓ
ｔｉｍａｔｅｔｈｅ
ｇｙ
ｒｏｓｃｏ
ｐ
ｅ ｂｉａｓ ａｎｄ ｈｅａｄｉｎ
ｇ
ｅｒｒｏｒ ｃｏｒｒｅｃｔｉｏｎ ｎｅｅｄ ａｄｄ
ｉｔｉｏｎａｌｍｅａｓｕｒｅｍｅｎｔｓｆｒｏｍ ａ ｍａ
ｇ
ｎｅｔｏｍｅ ｔｅｒ
，
ｇ
ｌｏｂａｌ
ｐ
ｏｓｉｔｉｏｎｉｎ
ｇ
ｓ
ｙ
ｓｔｅｍ
（
ＧＰＳ
）
ｏｒｏｔｈｅｒ ｅｘｔｅｒｎａｌｓｅｎｓｏｒｓ
，
ｓｏｔｈｅ
ｙ
ａｒｅｏｆｌｉｍｉｔｅｄ ｕｓｅｉｎ ｓｏｍｅ ａ
ｐｐ
ｌｉ
ｃａｔｉｏｎｓ ｓｕｃｈ ａｓ ｉｎｄｏｏｒ ｎａｖｉ
ｇ
ａｔｉｏｎ ｓ
ｙ
ｓｔｅｍｓ ｏｒｉｎ ｔｈｅ
ｐ
ｒｅｓｅｎｃｅｏｆ
ｓｔｒｏｎ
ｇ
ｍａ
ｇ
ｎｅｔ ｉｃｄｉｓｔｕｒｂａｎｃｅｓ．Ａｂｉａｓｅｓｔｉｍａｔｉｏｎ ｍｅｔｈｏｄ ｉｓ
ｇ
ｉ ｖｅｎｈｅｒｅ
ｂａｓｅｄｏｎｒｏｔａｔｉｏｎｓｏｆｔｈｅｉｎｅｒｔｉａｌ ｍｅａｓｕｒｅｍｅｎｔｕｎｉｔ． Ｔｈｅ
ｇｙ
ｒｏｓｃｏ
ｐ
ｅ ｂｉａｓｉｓｅｓｔｉｍａｔｅｄｆｒｏｍ ｔｈｅ ｄｉｆｆｅｒｅｎｃｅ ｂｅｔｗｅｅｎｔｈｅ ｉｎｃｌ ｉｎａｔｉｏｎａｎ
ｇ
ｌ
ｅｓｏｆｔｈｅ
ｇｙ
ｒｏｓｃｏ
ｐ
ｅｓａｎｄａｃｃｅｌｅｒｏｍｅｔｅｒｓａｔｄｉｆｆｅｒｅｎｔ ａｔｔｉｔｕｄｅｓ
，
ｗｈｉｃｈａ
ｒｅ ｍｅａｓｕｒｅｄ
ｊ
ｕｓｔｂｅｆｏｒｅａｎｄａｆｔｅｒｒｏｔａｔｉｏｎｏｆｔｈｅ ｕｎ ｉｔ．Ｔｅｓｔｓｓｈｏｗｔｈａ
ｔｔｈｉｓｍｅｔｈｏｄｅｆｆｅｃｔｉｖｅｌ
ｙ
ｉ ｍ
ｐ
ｒｏｖｅｓｔｈｅａｃｃｕｒａｃ
ｙ
ｏｆｔｈｅａｔｔｉｔｕｄｅｅｓ ｔｉｍ
ａｔｅ
，
ｅｓ
ｐ
ｅｃｉａｌｌ
ｙ
ｔｈｅｈｅ ａｄｉｎ
ｇ
ａｎ
ｇ
ｌｅ．

Ｋｅ
ｙ
ｗｏｒｄｓ
：
ａｔｔｉｔｕｄｅ ｍｅａｓｕｒｅｍｅｎｔ
；
ｇｙ
ｒｏｓｃｏ
ｐ
ｅ ｂｉａｓ
；
ｒｏｔａｔｉｏｎ ｏｆ
ｉｎｅｒｔｉａｌ ｍｅａｓｕｒｅｍｅｎｔ ｕｎｉｔ
（
ＩＭＵ
）；
ｈｅａｄｉｎ
ｇ
ａｎ
ｇ
ｌｅ
ｃｏ
ｒｒｅｃｔｉｏｎ
陀螺仪作为一种常见的姿态传感器
，
广 泛应用 于
移动机器人
、
无人机
、
航天器等需要对运动物体进
行姿
态测量和控制的领域
。
通过对
３
轴陀螺仪所测
量的旋
转角速度进行积分能够得到姿态角
。
但是
，
由于陀
螺仪存在漂移问题
，
长时间累积计算会产生 较大误差
，
需要利 用辅助传感器来对陀螺漂移进行 估计和误差补
偿
［
１
］
。

惯性测量单元一般包含了陀螺仪和加速度计
。

［
通过加速度计能够获得惯性测量单元的倾角
２
］
，
相

当于倾角传感器
。
结合陀螺仪和加速度计的计算结 果
可以获得较高精度的倾角信息
。
国内外学者采用
［
Ｋａｌｍａｎ
滤波
３
－
４
］

、
互 补 滤 波
［
５
－
６
］

等 数 据 融 合 方 法 来
估计和补偿陀螺仪漂移以提高 动 态 倾 角 的 测 量 精
度
。
但加速度计无法直接提供姿态角中的航向角
ψ

的信息
［
７
］
。
Ｃｈｕｒｃｈｉｌｌ
［
８
］
提出了利用加速 度计估计陀
螺处于铅垂方向时的漂移以修正航向角的方法
。
该 方
法需要确保旋转是绕水平方向以保证修正精度
，
不
适用于惯性测量单元< br>（
ｉｎｅｒｔｉａｌｍｅａｓｕｒｅｍｅｎｔｕｎｉｔ
，
ＩＭ
Ｕ
）
姿态角处于任意值的情形
。

目前
，
修 正陀螺仪漂移造成的航向角误差的方
法
，
大都需要在磁强计
［
９
］
或
ＧＰＳ
［
１０
］
等 辅助传感器下
实现
，
通过磁强计或
ＧＰＳ
来对漂移造 成的平行于铅
垂线方向上的误差进行补偿
。
由于磁强计不适合在
周围有较强磁场干扰的环境下应用
，
而
ＧＰＳ
方法不
适合在
ＧＰＳ
信 号微弱的环境下
（
如 室 内
、
煤 矿 等 场
合
）
应用
，
因此 这些方法经常会受到限制
。
例如
，
一
种面向三峡水轮机叶片表面检测的爬壁机器人
［
１１
］
，
它就处于一个空间相对封闭
、
磁场干扰强
、
周围无稳
收稿日期
：
２０１３
－
１２
－
３１

基金项目
：
国家自然科学基金项目
（
５１４７５２５９
）

作者简介
：
吴哲明
（
１９８５
—），
男
（
汉
），
福 建
，
博士研究生
。

通信作者
：
孙振国
，
副教授
，
Ｅ
－ｍａｉｌ
：
ｓｕｎｚｈ
ｇ＠
ｔｓｉｎ
ｇ
ｈｕａ．ｅｄｕ ．ｃｎ

１１４４

清 华 大 学 学 报 （自 然 科 学 版） ，（）
２０１４５４９

定特征点且安放信号标志点困难的特殊作业环境
。
本文提出了一种通过
ＩＭＵ
旋转实现陀螺漂移

估计和补偿的方法
。
该方法在测量过程中对陀螺仪
施加独立物体运动的特定旋转
，
通 过旋转前后不同
姿态下陀螺仪和加速度计输出的倾角数值
，
得 到惯
性测量单元漂移估计
。
首先
，
基于 加速度计的倾角
测量模型
，
建立了 倾角误差和陀螺漂移之间的数学
模型
；
然后
，
通过控制
ＩＭＵ
旋转获取不同姿态下的
倾角误差来估 计 陀 螺 漂 移
；
最 后
，
设 计 和 实 现 相 应
算法
，
并对所提的估计算法进行了实验验证
。


采用的
Ｅｕｌｅｒ
角定义为
：
导航坐标系
ｏｘｚ
ｎ
ｙ
ｎｎ
绕
ｚ
ｎ
轴
转
－
ｘ
′′
ｚ
′
，
ｏｘ
′′
ｚ
′
绕< br>ｘ
′
轴转
θ
角度得
ｏｘ
″
ｙｙ
ψ
角度得
ｏ
得
Ｉ
如
″
ｚ
″
，
ｏｘ
″″
ｚ
″
再绕
ｙ
″轴转

角度
，
ＭＵ
坐标系
，
ｙｙ
图
１
所示
。

１
姿态运动学描述
导航坐标系
ｏ
定义为
：
原点为
ｏ
，
轴指
ｘｚ
ｘ
ｎ
ｙ
ｎｎ
ｎ
，，（。
向东
（
轴指向北
（
轴指向天 惯性
Ｅ
）
Ｎ
）
ｚ
Ｕ
）
ｙ
ｎ ｎ
测量单元
（
坐 标系为
ｏ
的姿态可
ＩＭＵ
）
ｘｚＩＭＵ
ｂ
ｙ
ｂｂ
。
由 示
，
即 航向角 仰 角
θ
、
横 滚 角
。
Ｅｕｌｅｒ
角表
ψ
、
俯
，），／／），
∈
（，）。
本文

－
π
－
π
２
，
２－
π
π
θ∈
（
ππψ
∈
（
图
１ ＩＭＵ
坐标系和导航坐标系定义

在此 定 义 下
，
从 导 航 坐 标 系 到 标 系 的
ＩＭＵ
坐
［］
转换矩阵
Ｒ
就是
ＩＭＵ
姿态矩阵
４
，

Ｒ
＝
ｃｏｓｃｏｓｓｉｎｓｉｎｓｉｎｃｏｓｓｉｎｓｉｎｃｏｓｓｉｎｓｉｎｃｏｓ
＋θ －＋θ －θ
ψψψψ
熿燄
（）
． １
ｓｉｎｃｏｓｃｏｓｃｏｓｓｉｎ
θ θ θ
ψψ
ｓｉｎｃｏｓｃｏｓｓ ｉｎｓｉｎｓｉｎｓｉｎｃｏｓｃｏｓｓｉｎｏｓｃｏｓ
－θ －－θ
ｃ
θ
燅

燀ψψψψ
，
加 重 力 加 速 度 影 响 时 速 度 计 的 测 量 值 为
［］
旋转 角 速 度 和
ＩＭＵ Ω＝ Ω
ｘ
Ω
ｙ
Ω
ｚ
Ｅｕｌｅｒ
仅考虑
·
· ·
力 矢 量 在 标 系 中 的 坐 标 为
Ａ
ｍ
，
即重
ＩＭＵ
坐
Ａ
ｍ
＝

］
４
］
角速率
［
的关系为
［

θ
ψ
［］。

Ａ
ｍ
Ａ
ｍｘ
Ａ
ｍｚ
ｙ
·
ｓｉｎｔａｎｃｏｓｔａｎΩ
ｘ

θ
１
－θ
烄烌
烄烌
熿 燄
·
（）
２
Ω
ｙ
．
ｏｓ

０ ｓｉｎ


θ
＝
ｃ
·
ｓｉｎｓｅｃｏｓｓｅｃ
θ
０
－
ｃ
θ
烎
Ω
ｚ
燅

燀
烆
ψ
烆
烎
通过式
（
２
）
对
３
轴陀螺仪测量的旋转角速度进行积


分即可计算 出 姿 态 角
。
但 是
，
陀 螺 仪 存 在

ＩＭＵ
的
漂移
，
其漂移模型为
（
３
）
Ω
ｍ
＝ Ω ＋
ｂ．
：［，
Ω
ｍ
式中 是 真 实 的 角 速 度
Ω＝ Ω
ｘ
ΩΩ
ｚ
］
ＩＭＵ
ｙ
，［］
是实际 测 量 的 陀 螺 仪 输 出 是 陀 螺

ｂ＝ ｂ
ｘ
ｂ
ｚ
ｙ
ｂ
漂移
。

陀 螺 漂 移 是 个 随 机 缓 变 的 过 程
［
５
］
，
而 通 过 式
（）
计 算 姿 态 角 的 过 程 中 需 要 对 角 速 度 进 行 积 分
。
２
因此
，
漂移短时间造成的姿态测量误差影响较小
，
但
长时间会导致较大的累积误差
。
对漂移估计并补偿
修正的目的便是提高长时间的姿态估计精度
。


图
２
通过加速度计测量加速度来估计姿态

，
重力矢量的坐标为 在导航 坐 标 系 中
－
ｇ
［
００
Ｔ
］，
因此根据坐标变换公式有
１
０
ｃｏｓｓｉｎ

燄
θ
熿 燄
熿
ｇ
Ａ
ｍ
＝－ｇ
Ｒ ０
＝
－
ｓｉｎ
ｇθ
．
１
ｃｏｓｃｏｓ
燅
燀燅
燀
－ｇθ
（）
４
从而根据加速度计测量值可以计算倾角
（
包 括
横滚角和俯仰角
），
相当于倾角传感器
，

（
ａｔａｎ２
Ａ
－
Ａ

）

ｍｘ
，

ｍｚ
烄


烄
ｍ
烌
Ａ

烌
．
（）
５
－
ｍ
ｙ
＝
ａｓｉｎ

烆
θ
ｍ
烎
‖
Ａ
ｍ
‖
烎

烆
加速度计长时间测量结果稳定
，
因 此通过互补
２
加速度计的倾角测量模型

加速度计测量的是载体运动时加速度的大小
，

、
载体 其测量结果包括重力加速度 沿某方向运动时
的加速度以及噪声等
。
使用加速度计估计姿态时
，
一般利用其提供的重力加速度部分
。
如图
２
所示
，

（ ）
滤波等方法可以获得较高精度的倾角信息
［
４
］
。

从式
（
５
）
可以看出
，
加速度计无法直接提供航向

吴哲明，等： 基于惯性测量单元旋转的陀螺漂移估计和补偿方法


１１４５

５
］
。

角
ψ
的信息
，
存在无法修正航向的局限性
［
３
惯性测量单元旋转算法
３．１
陀螺漂移和倾角误差的关系

某一姿态
ｋ
下
，
设 惯性测量单元的姿态倾角为

［，
假 设 在 时 间 间 隔 内 姿 态 倾 角 变 化 速 率
ｔ
θ
ｋｋ
］
Δ
一定
，
则通过陀螺 仪测量的角速率计算得到的倾角 变化为
：

烄
Δ
ｋ
烌
＝
Ｖ
ｋ
（
Ω ＋
ｂ
）
．
（）
６

烆
Δθ
ｋ
烎
ｇ

Ｖ
烄
ｓｉｎ
ｋ
ｔａｎ
θ
ｋ
１
－
ｃｏｓ
ｋ
ｔａｎ
θ
ｋ
烌

ｋ
＝
Δ
ｔ．

烆
ｃｏｓ
ｋ
０
－
ｃｏｓ
ｋ
ｓｅｃ
θ
ｋ
烎

通过加速度计测量的加速度值计算得到倾角变
化为
［
ΔΔθ
］
ａ
，

则陀螺仪和加速度计分别输出的倾
角变化的差值为
ｒ
ｋ
＝
烄
Δ
ｋ
烌

－
烄
Δ
ｋ
烌

．
（
７
）
烆
Δθ

ｋ
烎
烆
Δθ
ｋ
ｇ

烎
由于加速度计长时间测量结果稳定
ａ

，
可 认为根
据加速度计计算获得的结果为精确的倾角值
，

烄
Δ
ｋ
烌

＝
Ｖ
ｋ
Ω
．
（
８
）

烆
Δθ
ｋ
烎
ａ
根据式
（
６
）
和
（
８
），
陀螺漂 移和两者倾角变化差
值的关系为
ｒ
ｋ
＝
Ｖ
ｋ
ｂ．
（
９
）

每个姿态 倾 角
［
θ
ｋ ｋ
］
下 根 据 倾 角 之 差
ｒ
ｋ
＝
［
Δ

Δθ
］
ｋ
只能确 定 式
（
９
）
中 的 两 个 方 程
，
而 陀 螺 仪 漂 移
有
３
个未知量且相互不相关
，
因此 在通常情况下无
法计算出陀螺仪
３
个轴的漂移值
。

３．２
方法原理

陀螺漂移是一个缓慢变化的随机过程
，
在 小于
１０ｓ
的时间内可 认为没有变化
［
６
］
。
本文采用如图
３
所示的惯性测量单元旋转的方法
，
通过旋转机构控
制惯性测量单元绕轴
ｕ＝
［
ｕ
ｘ
ｕ
ｙ
ｕ
ｚ
］
产生一个独立于
物体运动的
Δα
角 旋 转
，
于是惯 性 测量单元相对于
导航坐标系发生了姿态变化
。

根据
ｕ
和
Δα
可以确定旋转后的惯性测量单元
坐标系
ｏｘ
′
ｂ
ｙ
′
ｂ
ｚ
′
ｂ
到旋转前坐标系

ｏｘ
ｂ
ｙ
ｂ
ｚ
ｂ
的旋转矩

阵
Ｓ
为

Ｓ
＝
Ｉｃｏｓ
Δα＋
ｓｉｎ
Δα
［
ｕ
］
×
＋
（
１
－
ｃｏｓ
Δα
）
ｕ ｕ．

（
１０
）
式中
：［
ｕ
］
×
为
ｕ
的 反 对 称 矩 阵
，
Ｉ
是 单 位 矩 阵
，
为点积
。

图
３
旋转
ＩＭＵ
的方法示意

根据旋转矩阵
Ｓ
，
由旋转 前的姿态矩阵
Ｒ
计算
旋转后新的姿态矩阵
Ｒ′
，

Ｒ
′
＝
Ｓ
Ｔ
Ｒ．
（
１１
）

根据姿态矩阵
Ｒ′
得 到惯性测量单元的 新 倾 角

［
θ
′ ′ ６ ８
ｋ ｋ
］，
进而由式
（ ）

和
（ ）
获得包含陀螺漂移和
倾角误差的新方程
，

' ′
ｒ
ｋ

ｋ
＝
Ｖｂ．
（
１２
）

对 于
ｎ
个 不 同 倾 角
｛［
θ
ｋ

ｋ
］

ｋ
＝１
～
ｎ

｝，
根 据 式
（
１２
）
可以获得
２ｎ
个方程
，
用矩阵形式表示为

ｒ
＝
Ｖｂ．
（
１３
）

式中
，
ｂ＝
［
ｂ
ｘ

ｂ
ｙ
ｂ
ｚ
］
即为待估计的陀螺漂移
。

旋转前后陀螺漂移
ｂ
认为不变
，
式
（
１２
）
即 为以
ｂ
为 未知量的超定方程 组
。
通 过 最 小 二 乘 法 求
解
ｂ
，

ｂ
＝
（
Ｖ
Ｔ
Ｖ
）
－１
Ｖ
Ｔ
ｒ．
（
１４
）

当
ｎ
≥
２
时
，
也 就是旋转包含两个姿态以上
，
即
能够解出陀螺
３
个轴的漂移
。

通过所估计的陀螺漂移进行补偿
，
就 能减小长
时间的姿态估计中受漂移导致的姿态角尤其是航向
角的积分误差
。

３．３
算法流程

基于以上原理
，
设计的算法流程如图
４
所示
。
算
法中惯性测量单元的初始姿态已知
，
旋 转轴
所在方向向量和旋转的角度已预先标定
惯性测量单元的姿态
、
旋 转轴
ｕ＝
［
ｕ ｕ

ｕ
，
因

此利用
和

Δα
，

ｘ
ｙ
ｚ
］


通过式
（
１１
）
就能够计算出惯性测量单元所固接的运
动
物体的姿态
。

１１４６
清 华 大 学 学 报 （自 然 科 学 版） ，（）
２０１４５４９



图
４
旋转惯性测量单元的算法流程

４
实验结果
本文以 荷 兰
Ｘｓｅｎ
公 司 的
ＭＥＭＳ
惯 性 测 量 单
元
ＭＴｉ
－
１００
为对 象
，
基 于 工 业 机 器 人
ＭＯＴＯＭＡＮ
ＳＫ６
进行实 验验证
。
ＭＴｉ
－
１００
集成了
３
轴陀螺仪和
３
轴加速度计
。
其技术参数指标如表
１
所示
。

表
１
惯性测量单元
ＭＴｉ－１００
的 技术参数
［
１２
］

型号 偏置稳定度 角度随机游走 输出噪声
ＭＴｉ
－
１００ １０
（
°
）／
ｈ ２．０ｍ
／
ｓ
０．０１< br>（
°
）／（
ｓ
·
槡
Ｈｚ
）

实 验 中 将
ＩＭＵ
固 定 在 工 业 机 器 人 的 末 端 关
节
，
通过工业机 器人末端旋转轴实现旋转
。
设定工
业机器人末端关节以旋转间隔时间为
５ｓ
带 动
ＩＭＵ
做往复的旋 转 运 动
，
Δα
＝９０°
。
航 向 角 和 横 滚 角 的
初始设定为
０
，
在 初始 俯 仰 角 为
θ
＝０°
、
３０°
、
６０°
下
各采集 了
１０
组数据
，
采集时间为
３６００ｓ
，
采 样频率
为
１００Ｈｚ
。

图
５
为俯仰角初始值为
３０°
条 件下对陀螺仪
Ｘ
轴漂移的一组估计结果
，
横坐标为采样时间
，
纵坐标
为漂移值
。
从图
５
中结果可以发现
，
漂 移的估计值
能够一直保持跟踪漂移的实际值
，
且平均误差小于
３
（
°
）／
ｈ
。
Ｙ
轴

和
Ｚ
轴的结果也类似
。

图
６
为利用估计的陀螺漂移来补偿姿态测量误

差后的航向角修正结果
。
从图
６
中可以发现
，
经 过

１ｈ
的姿态定位 计算
，
航 向角误差由大于
２０°
减 小至

４°
左右
，
显著减 小了因 陀 螺 漂 移 造 成 的 航 向 角 测 量
误差
。

图
５
旋转
ＩＭＵ
方法对陀螺仪
Ｘ
轴漂移的估计结果
图
６
方法对陀螺漂移补偿后的姿态测量结果

所有
３０
组数据的姿态测量结果如表
２
所示
。

表
２
通过本文方法补偿漂移前后的姿态定位结果对比

俯 仰 角 误 差／（
°
）横 滚 角 误 差／（
°
） 航 向 角 误 差／（
°
）

最 大 值 平 均 值 最 大 值 平 均 值 最 大 值 平 均 值
未修正
１７．０ ６．３ １６．３ ５．５ ３２．７ １１．９
修正后
２．９ １．６ １．８ ０．９ ４．２ ２．０
从表
２
中可以看出
，
基于本文方法对姿态角进
行修正
，
在通过陀 螺漂移补偿后
，
俯 仰角
、
横 滚角的
误差减小至
３
（
°
）／
ｈ
以内
，
尤其是将航向角的误差修
正后降至
５
（
°
）／
ｈ
以内
。
结果表明
，
所提出的方法能
够克服现有技术需要在磁强 计 或
ＧＰＳ
等 外 部 传 感
器的辅助下实现航向角的误差修正的问题
，
可 应用
于室内
、
磁干扰等环境下的姿态测量
。


结 论
本文提出的基于惯性测量单元旋转的陀螺仪漂
移估计方法具有如下特点
：

１
）
相对于常用的 以磁强计或
ＧＰＳ
辅 助修正陀














５

吴哲明，等： 基于惯性测量单元旋转的陀螺漂移估计和补偿方法
１１４７


螺仪的漂移造成姿态估计积累误差的方法
，
该 方法
不受室内
、
磁干扰等环境的限制
。

２
）
通过控制惯性测量单 元旋转
，
利 用旋转前后
不同姿态下陀螺仪和加速度计结果计算的倾角变化
的差值
，
可以计算并估计出陀螺漂移值
。

３
）
实验结果表明
，
漂移补 偿修正后姿态估计精
度有了明显提高
，
尤其是显著地减小了航向角误差
。
该方 法 能 为 长 时 间 的 姿 态 测 量 提供较为精确的
信息
。



参考文献
（
Ｒｅｆｅｒｅｎｃｅｓ
）

［
１
］
刘兴川
，
张盛
，
李丽哲，
等
．
基于四元数的
ＭＡＲＧ
传感器姿 态
测量算法
［
Ｊ
］
．
清 华大学学报
：
自然科学版
，
２０１２
，
５２
（
２
）：
６２７
－
６３１．

ＬＩＵ Ｘｉｎ
ｇ
ｃｈｕａｎ
，
ＺＨＡＮＧ Ｓｈｅｎ
ｇ
，
ＬＩ Ｌｉｚｈｅ
，
ｅｔ ａｌ． Ｑ
ｕａｔｅｒｎｉｏｎ
－
ｂａｓｅｄａｌ
ｇ
ｏｒｉｔｈｍ ｆｏｒｏｒｉｅｎｔａｔｉｏｎ ｅｓｔｉｍａｔｉｏｎｆｒｏｍ

ＭＡＲＧｓｅｎｓｏｒｓ< br>［
Ｊ
］
．ＪＴｓｉｎ
ｇ
ｈｕａＵｎｉｖ
：
Ｓ ｃｉａｎｄＴｅｃｈ
，
２０１２
，

５２
（
２）：
６２７
－
６３１．
（
ｉｎＣｈｉｎｅｓｅ
）

［
２
］
ＭａｒｔｉｎＰ
，
ＳａｌａｕｎＥ．Ｔｈ ｅｔｒｕｅｒｏｌｅｏｆａｃｃｅｌｅｒｏｍｅｔｅｒｆｅｅｄｂａｃｋ
ｉ
ｎ
ｑｕａｄｒｏｔｏｒｃｏｎｔｒｏｌ
［
Ｃ
］／／
ＩＥＥＥＣｏｎｆｅｒｅｎ ｃｅｏｎＲｏｂｏｔｉｃｓａｎｄ Ａ
ｕｔｏｍａｔｉｏｎ．Ａｎｃｈｏｒａ
ｇ
ｅ：
ＩＥＥＥ
，
２０１０
：
１６２３
－
１６２９ ．

［
３
］
ＳｅｔｏｏｄｅｈＰ
，
Ｋｈａｙ
ａｔｉａｎＡ
，
Ｆｒａ
ｊ
ａｈＥ．Ａｔｔｉｔｕｄｅｅｓｔｉ ｍａｔｉｏｎｂ
ｙ
ｓｅ
ｐ
ａｒａｔｅ
－
ｂｉａｓＫａｌｍ ａｎｆｉｌｔｅｒ
－
ｂａｓｅｄｄａｔａｆｕｓｉｏｎ
［
Ｊ
］
．Ｊｏｕｒｎａｌｏ
ｆ
Ｎ
ａｖｉ
ｇ
ａｔｉｏｎ
，
２００４
，
５７
（
２
）：
２６１
－
２７ ３．

［
４
］
Ｄｅ ＭａｒｉｎａＨ Ｇ
，
ＰｅｒｅｄａＦＪ
，
Ｇｉｒｏｎ
－
ＳｉｅｒｒａＪ Ｍ
，
ｅｔａｌ．ＵＡＶ
ａｔｔｉｔｕｄｅｅｓｔｉｍａｔｉｏｎｕｓｉｎ
ｇ
ｕｎｓｃｅｎｔｅｄＫａｌｍａｎｆｉｌｔｅｒａｎｄＴＲＩＡＤ

［
Ｊ
］
． ＩＥＥＥ Ｔｒａｎｓ Ｉｎｄｕｓｔｒｉａｌ Ｅｌｅｃｔｒｏｎｉｃｓ
，
２０１２
，

５９
（
１１
）：
４４６５
－
４４７４．

［
５
］
ＹｏｏＴ Ｓ
，
Ｈｏｎ
ｇ
Ｓ Ｋ
，
Ｙｏｏｎ Ｈ Ｍ
，
ｅｔａｌ．Ｇａｉｎ
－
ｓｃｈｅｄｕｌｅｄ ｃ
ｏｍ
ｐ
ｌｅｍｅｎｔａｒ
ｙ
ｆｉｌｔｅｒｄｅｓｉ
ｇ
ｎｆｏｒａ ＭＥＭＳｂａｓｅｄａｔｔｉｔｕｄｅａｎｄ ｈｅ
ａｄｉｎ
ｇ
ｒｅｆｅｒｅｎｃｅ ｓ
ｙ
ｓｔｅｍ
［
Ｊ
］
． Ｓｅｎｓｏｒｓ
，
２０１１
，
１１
（
４
）：

３８１６
－
３８３０．

［
６
］
Ｍａｈｏｎ
ｙ
Ｒ
，
Ｅｕｓｔｏｎ Ｍ
，
ＫｉｍＪ
，
ｅｔａｌ．Ａ ｎｏｎ
－
ｌｉｎｅａｒｏｂｓｅｒｖｅｒ
ｆｏｒａｔｔｉｔｕｄｅｅｓｔｉｍａｔｉ ｏｎｏｆａｆｉｘｅｄ
－
ｗｉｎ
ｇ
ｕｎｍａｎｎｅｄａｅｒｉａｌｖｅｈｉｃ ｌｅ

ｗｉｔｈｏｕｔ ＧＰＳ ｍｅａｓｕｒｅｍｅｎｔｓ
［
Ｊ
］
． Ｔｒａｎｓａｃｔｉｏｎｓ ｏ
ｆ
ｔｈｅ Ｉｎｓｔ
ｉｔｕｔｅ ｏ
ｆ
Ｍｅａｓｕｒｅｍｅｎｔ ａｎｄ Ｃｏｎｔｒｏｌ
，
２０１１
，
３３
（
６
）：

６９９
－
７１７．

［
７
］
ＭｅｒｃｋｅｒＴ Ｈ
，
ＡｋｅｌｌａＭ Ｒ．Ｒｉ
ｇ
ｉｄ
－
ｂｏｄ
ｙ
ａｔｔｉｔｕｄｅｔｒａｃｋｉｎ
ｇ
ｗｉｔｈ ｖ
ｅｃｔｏｒｍｅａｓｕｒｅｍｅｎｔｓａｎｄｕｎｋｎｏｗｎ
ｇｙ
ｒｏｂｉａｓ
［< br>Ｊ
］
．Ｊｏｕｒｎａｌｏ
ｆ
Ｇｕ
ｉｄａｎｃｅ
，
Ｃｏｎｔｒｏｌ
，
ａｎｄ Ｄ
ｙ
ｎａｍｉｃｓ
，
２０１１
，
３４
（
５
）：

１４７４
－
１４８４．

［
８
］
Ｃｈｕｒｃｈｉｌｌ Ｄ Ｌ．Ｉｎｅｒｔｉａｌ Ｍｅａｓｕｒｅｍｅｎｔ Ｓ
ｙ
ｓｔｅｍ ｗｉｔｈ Ｓｅｌｆ
Ｃ
ｏｒｒｅｃｔｉｏｎ
［
Ｐ
］
．Ｎｏ．８０１０３０８．ＵＳＡ
，
２０１１．
［
９
］
Ｒｅｎ Ｈ
，
ＫａｚａｎｚｉｄｅｓＰ．Ｉｎｖｅ ｓｔｉ
ｇ
ａｔｉｏｎｏｆａｔｔｉｔｕｄｅｔｒａｃｋｉｎ
ｇ
ｕｓｉｎ
ｇ
ａ
ｎ ｉｎｔｅ
ｇ
ｒａｔｅｄｉｎｅｒｔｉａｌａｎｄ ｍａ
ｇ
ｎｅｔｉｃ ｎａｖｉ
ｇ
ａｔｉｏｎ ｓ
ｙ
ｓｔｅｍ ｆｏｒ

ｈａｎｄ
－
ｈｅｌｄ ｓｕｒ
ｇ
ｉｃａｌｉｎｓｔｒｕｍｅｎｔｓ
［
Ｊ
］
．ＩＥＥＥ
／
ＡＳＭＥ Ｔｒａｎｓ Ｍｅ
ｃｈａｔｒｏｎｉｃｓ
，
２０１２
，
１７
（
２
） ：
２１０
－
２１７．

［
１０
］
Ａ
ｇ
ｈｉｌｉＦ
，
ＳａｌｅｒｎｏＡ．Ｄｒｉｆｔｌｅｓｓ３
－
Ｄａ ｔｔｉｔｕｄｅｄｅｔｅｒｍｉｎａｔｉｏｎａｎｄ

ｐ
ｏｓｉｔｉｏｎｉｎ
ｇ
ｏｆｍｏｂｉｌｅｒｏｂｏｔｓｂ
ｙ
ｉｎｔｅ
ｇ
ｒａｔｉｏｎｏｆ ＩＭＵ ｗｉｔｈｔｗｏ
ＲＴＫ ＧＰＳｓ
［
Ｊ
］
．ＩＥＥＥ
／
ＡＳＭＥ Ｔｒａｎｓ Ｍｅｃｈａｔｒｏｎｉｃｓ
，
２０１３
，
１８
（
１
）：
２１
－
３１．

［
１１
］
ＷｕＺ
，
ＣｈｅｎＱ
，
Ｓｕｎ Ｚ
，
ｅｔａｌ．Ｄｅｓｉ
ｇ
ｎｏｆａｗｈｅｅｌｅｄｃｌｉｍｂｉｎ
ｇ
ｒ
ｏｂｏｔｆｏｒａｕｔｏｍａｔｅｄｃｕｒｖｅｄ ｓｕｒｆａｃｅｉｎｓ
ｐ
ｅｃｔｉｏｎ ｏｆｈ
ｙ
ｄｒａｕｌｉｃ ｔｕ
ｒｂｉｎｅｂｌａｄｅ
［
Ｃ
］／／
Ｃｈａｎ
ｇ
Ｆ
－
Ｋ
，
ｅｄ．Ｐｒｏｃｅｅｄｉｎ
ｇ
ｓ ｏｆ ８ｔｈ Ｉｎｔ
ｅｒｎａｔｉｏｎａｌ Ｗｏｒｋｓｈｏ
ｐ
ｏｎ Ｓｔｒｕｃｔｕｒａｌ Ｈｅａｌｔｈ Ｍｏｎｉｔｏｒｉｎ
ｇ
． Ｓｔａ
ｎｆｏｒｄ
：
ＤＥＳｔｅｃｈＰｕｂｌｉｃａｔｉｏｎｓ
，
２０１１
：
８６４
－
８７１．

［
１２
］
Ｘｓｅｎｓ Ｔｅｃｈｎｏｌｏ
ｇ
ｉｅｓ． ＭＴｉ Ｕｓｅｒ Ｍａｎｕａｌ
［
Ｍ
／
ＯＬ
］
．

［
２０１３< br>－
１０
－
１２
］
．ｈｔｔ
ｐ
：／／
ｗｗｗ．ｘｓｅｎｓ．ｃｏｍ
／
．