没有合适的资源?快使用搜索试试~ 我知道了~
首页IMU惯性导航姿态算法
IMU惯性导航姿态算法
4星 · 超过85%的资源 需积分: 47 451 下载量 10 浏览量
更新于2023-03-16
评论 33
收藏 1.47MB PDF 举报
惯性导航应用中,关于加速度传感器、陀螺传感器、磁罗盘传感器的姿态滤波算法原理与实现。
资源详情
资源评论
资源推荐
An efficient orientation filter for inertial and
inertial/magnetic sensor arrays
Sebastian O.H. Madgwick
April 30, 2010
Abstract
This report presents a novel orientation filter applicable to IMUs consisting of
tri-axis gyroscopes and accelerometers, and MARG sensor arrays that also include
tri-axis magnetometers. The MARG implementation incorporates magnetic distortion
and gyroscope bias drift compensation. The filter uses a quaternion representation,
allowing accelerometer and magnetometer data to be used in an analytically derived
and optimised gradient-descent algorithm to compute the direction of the gyroscope
measurement error as a quaternion derivative. The benefits of the filter include: (1)
computationally inexpensive; requiring 109 (IMU) or 277 (MARG) scalar arithmetic
operations each filter update, (2) effective at low sampling rates; e.g. 10 Hz, and (3)
contains 1 (IMU) or 2 (MARG) adjustable parameters defined by observable system
characteristics. Performance was evaluated empirically using a commercially available
orientation sensor and reference measurements of orientation obtained using an optical
measurement system. A simple calibration method is presented for the use of the
optical measurement equipment in this application. Performance was also benchmarked
against the propriety Kalman-based algorithm of orientation sensor. Results indicate
the filter achieves levels of accuracy exceeding that of the Kalman-based algorithm;
< 0.6
◦
static RMS error, < 0.8
◦
dynamic RMS error. The implications of the low
computational load and ability to operate at low sampling rates open new opportunities
for the use of IMU and MARG sensor arrays in real-time applications of limited power
or processing resources or applications that demand extremely high sampling rates.
1
Contents
1 Introduction 3
2 Quaternion representation 4
3 Filter derivation 6
3.1 Orientation from angular rate . . . . . . . . . . . . . . . . . . . . . . . . . . 6
3.2 Orientation from vector observations . . . . . . . . . . . . . . . . . . . . . . 6
3.3 Filter fusion algorithm . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 10
3.4 Magnetic distortion compensation . . . . . . . . . . . . . . . . . . . . . . . . 11
3.5 Gyroscope bias drift compensation . . . . . . . . . . . . . . . . . . . . . . . 12
3.6 Filter gains . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 13
4 Experimentation 14
4.1 Equipment . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 14
4.2 Orientation from optical measurements . . . . . . . . . . . . . . . . . . . . . 14
4.3 Calibration of frame alignments . . . . . . . . . . . . . . . . . . . . . . . . . 16
4.4 Experimental proceedure . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 18
5 Results 19
5.1 Typical results . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 19
5.2 Static and dynamic performance . . . . . . . . . . . . . . . . . . . . . . . . 21
5.3 Filter gain vs. performance . . . . . . . . . . . . . . . . . . . . . . . . . . . . 22
5.4 Sampling rate vs. performance . . . . . . . . . . . . . . . . . . . . . . . . . . 23
5.5 Gyroscope bias drift . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 23
6 Discussion 24
7 Conclusions 25
A IMU filter implementation optimised in C 29
B MARG filter implementation optimised in C 30
2
1 Introduction
The accurate measurement of orientation plays a critical role in a range of fields includ-
ing: aerospace [1, 2, 3], robotics [4, 5], navigation [6, 7] and human motion analysis [8, 9]
and machine interaction [10]. Whilst a variety of technologies enable the measurement of
orientation, inertial based sensory systems have the advantage of being completely self con-
tained such that the measurement entity is constrained neither in motion nor to any specific
environment or location. An IMU (Inertial Measurement Unit) consists of gyroscopes and
accelerometers enabling the tracking of rotational and translational movements. In order to
measure in three dimensions, tri-axis sensors consisting of 3 mutually orthogonal sensitive
axes are required. A MARG (Magnetic, Angular Rate, and Gravity) sensor is a hybrid IMU
which incorporates a tri-axis magnetometer. An IMU alone can only measure an attitude
relative to the direction of gravity which is sufficient for many applications [4, 2, 8, 1]. MARG
systems, also known as AHRS (Attitude and Heading Reference Systems) are able to provide
a complete measurement of orientation relative to the direction of gravity and the earth’s
magnetic field.
A gyroscope measures angular velocity which, if initial conditions are known, may be inte-
grated over time to compute the sensor’s orientation [11, 12]. Precision gyroscopes, ring laser
for example, are too expensive and bulky for most applications and so less accurate MEMS
(Micro Electrical Mechanical System) devices are used in a majority of applications [13]. The
integration of gyroscope measurement errors will lead to an accumulating error in the calcu-
lated orientation. Therefore, gyroscopes alone cannot provide an absolute measurement of
orientation. An accelerometer and magnetometer will measure the earth’s gravitational and
magnetic fields respectively and so provide an absolute reference of orientation. However,
they are likely to be subject to high levels of noise; for example, accelerations due to motion
will corrupt measured direction of gravity. The task of an orientation filter is to compute
a single estimate of orientation through the optimal fusion of gyroscope, accelerometer and
magnetometer measurements.
The Kalman filter [14] has become the accepted basis for the majority of orientation filter
algorithms [4, 15, 16, 17] and commercial inertial orientation sensors; xsens [18], micro-strain
[19], VectorNav [20], Intersense [21], PNI [22] and Crossbow [23] all produce systems founded
on its use. The widespread use of Kalman-based solutions are a testament to their accuracy
and effectiveness, however, they have a number of disadvantages. They can be complicated to
implement which is reflected by the numerous solutions seen in the subject literature [3, 4, 15,
16, 17, 24, 25, 26, 27, 28, 29, 30, 31, 32]. The linear regression iterations, fundamental to the
Kalman process, demand sampling rates far exceeding the subject bandwidth; for example, a
sampling rate between 512 Hz [18] and 30 kHz [19] may be used for a human motion caption
application. The state relationships describing rotational kinematics in three-dimensions
typically require large state vectors and an extended Kalman filter implementation [4, 17, 24]
to linearise the problem.
These challenges demand a large computational load for implementation of Kalman-
based solutions and provide a clear motivation for alternative approaches. Many previous
approaches to address these issues have implemented either fuzzy processing [2, 5] or fixed
filters [33] to favour accelerometer measurements of orientation at low angular velocities and
the integrated gyroscope measurements at high angular velocities. Such an approach is simple
3
but may only be effective under limited operating conditions. Bachman et al [34] proposed
an alternative approach where the filter achieves an optimal fusion of measurements data at
all angular velocities. However, the process requires a least squares regression, which also
brings in an associated computational load. Mahony et al [35] developed the complementary
filter which is shown to be an efficient and effective solution; however, performance is only
validated for an IMU.
This report introduces novel orientation filter that is applicable to both IMUs and MARG
sensor arrays addressing issues of computational load and parameter tuning associated with
Kalman-based approaches. The filter employs a quaternion representation of orientation (as
in: [34, 17, 24, 30, 32]) to describe the coupled nature of orientations in three-dimensions and
is not subject to the problematic singularities associated with an Euler angle representation
1
.
A complete derivation and empirical evaluation of the new filter is presented. Its performance
is benchmarked against an existing commercial filter and verified with optical measurement
system. Innovative aspects of the proposed filter include: a single adjustable parameter
defined by observable systems characteristics; an analytically derived and optimised gradient-
descent algorithm enabling performance at low sampling rates; an on-line magnetic distortion
compensation algorithm; and gyroscope bias drift compensation.
2 Quaternion representation
A quaternion is a four-dimensional complex number that can be used to represent the ori-
entation of a ridged body or coordinate frame in three-dimensional space. An arbitrary
orientation of frame B relative to frame A can be achieved through a rotation of angle θ
around an axis
A
ˆ
r defined in frame A. This is represented graphically in figure 1 where the
mutually orthogonal unit vectors
ˆ
x
A
,
ˆ
y
A
and
ˆ
z
A
, and
ˆ
x
B
,
ˆ
y
B
and
ˆ
z
B
define the principle axis
of coordinate frames A and B respectively. The quaternion describing this orientation,
A
B
ˆ
q,
is defined by equation (1) where r
x
, r
y
and r
z
define the components of the unit vector
A
ˆ
r in
the x, y and z axes of frame A respectively. A notation system of leading super-scripts and
sub-scripts adopted from Craig [37] is used to denote the relative frames of orientations and
vectors. A leading sub-script denotes the frame being described and a leading super-script
denotes the frame this is with reference to. For example,
A
B
ˆ
q describes the orientation of
frame B relative to frame A and
A
ˆ
r is a vector described in frame A. Quaternion arithmetic
often requires that a quaternion describing an orientation is first normalised. It is therefore
conventional for all quaternions describing an orientation to be of unit length.
A
B
ˆ
q =
q
1
q
2
q
3
q
4
=
cos
θ
2
−r
x
sin
θ
2
−r
y
sin
θ
2
−r
z
sin
θ
2
(1)
The quaternion conjugate, denoted by
∗
, can be used to swap the relative frames described
by an orientation. For example,
B
A
ˆ
q is the conjugate of
A
B
ˆ
q and describes the orientation of
frame A relative to frame B. The conjugate of
A
B
ˆ
q is defined by equation (2).
A
B
ˆ
q
∗
=
B
A
ˆ
q =
q
1
−q
2
−q
3
−q
4
(2)
1
Kuipers [36] offers a comprehensive introduction to this use of quaternions.
4
ˆ
y
A
ˆ
y
B
A
ˆ
r
ˆ
x
B
ˆ
x
A
θ
ˆ
z
A
ˆ
z
B
Figure 1: The orientation of frame B is achieved by a rotation, from alignment with frame
A, of angle θ around the axis
A
r.
The quaternion product, denoted by ⊗, can be used to define compound orientations.
For example, for two orientations described by
A
B
ˆ
q and
B
C
ˆ
q, the compounded orientation
A
C
ˆ
q
can be defined by equation (3).
A
C
ˆ
q =
B
C
ˆ
q ⊗
A
B
ˆ
q (3)
For two quaternions, a and b, the quaternion product can be determined using the
Hamilton rule and defined as equation (4). A quaternion product is not commutative; that
is, a ⊗ b 6= b ⊗ a.
a ⊗ b =
a
1
a
2
a
3
a
4
⊗
b
1
b
2
b
3
b
4
=
a
1
b
1
− a
2
b
2
− a
3
b
3
− a
4
b
4
a
1
b
2
+ a
2
b
1
+ a
3
b
4
− a
4
b
3
a
1
b
3
− a
2
b
4
+ a
3
b
1
+ a
4
b
2
a
1
b
4
+ a
2
b
3
− a
3
b
2
+ a
4
b
1
T
(4)
A three dimensional vector can be rotated by a quaternion using the relationship de-
scribed in equation (5) [36].
A
v and
B
v are the same vector described in frame A and frame
B respectively where each vector contains a 0 inserted as the first element to make them 4
element row vectors.
B
v =
A
B
ˆ
q ⊗
A
v ⊗
A
B
ˆ
q
∗
(5)
The orientation described by
A
B
ˆ
q can be represented as the rotation matrix
A
B
R defined
by equation (6) [36].
A
B
R =
2q
2
1
− 1 + 2q
2
2
2(q
2
q
3
+ q
1
q
4
) 2(q
2
q
4
− q
1
q
3
)
2(q
2
q
3
− q
1
q
4
) 2q
2
1
− 1 + 2q
2
3
2(q
3
q
4
+ q
1
q
2
)
2(q
2
q
4
+ q
1
q
3
) 2(q
3
q
4
− q
1
q
2
) 2q
2
1
− 1 + 2q
2
4
(6)
Euler angles ψ, θ and φ in the so called aerospace sequence [36] describe an orientation
of frame B achieved by the sequential rotations, from alignment with frame A, of ψ around
5
剩余31页未读,继续阅读
joey_wu
- 粉丝: 7
- 资源: 3
上传资源 快速赚钱
- 我的内容管理 收起
- 我的资源 快来上传第一个资源
- 我的收益 登录查看自己的收益
- 我的积分 登录查看自己的积分
- 我的C币 登录后查看C币余额
- 我的收藏
- 我的下载
- 下载帮助
会员权益专享
最新资源
- RTL8188FU-Linux-v5.7.4.2-36687.20200602.tar(20765).gz
- c++校园超市商品信息管理系统课程设计说明书(含源代码) (2).pdf
- 建筑供配电系统相关课件.pptx
- 企业管理规章制度及管理模式.doc
- vb打开摄像头.doc
- 云计算-可信计算中认证协议改进方案.pdf
- [详细完整版]单片机编程4.ppt
- c语言常用算法.pdf
- c++经典程序代码大全.pdf
- 单片机数字时钟资料.doc
- 11项目管理前沿1.0.pptx
- 基于ssm的“魅力”繁峙宣传网站的设计与实现论文.doc
- 智慧交通综合解决方案.pptx
- 建筑防潮设计-PowerPointPresentati.pptx
- SPC统计过程控制程序.pptx
- SPC统计方法基础知识.pptx
资源上传下载、课程学习等过程中有任何疑问或建议,欢迎提出宝贵意见哦~我们会及时处理!
点击此处反馈
安全验证
文档复制为VIP权益,开通VIP直接复制
信息提交成功
评论20