Visual-Inertial-AidedNavigationforHigh-DynamicMotioninBuiltEnvironments资源-CSDN文库

SLAM

需积分: 10 131 浏览量 2020-04-06 11:10:28 上传评论收藏 1.18MB PDF 举报

资源推荐

资源详情

资源评论

IEEE TRANSACTIONS ON ROBOTICS, VOL. 28, NO. 1, FEBRUARY 2012 61

Visual-Inertial-Aided Navigation for High-Dynamic

Motion in Built Environments Without

Initial Conditions

Todd Lupton and Salah Sukkarieh

Abstract—In this paper, we present a novel method to fuse ob-

servations from an inertial measurement unit (IMU) and visual

sensors, such that initial conditions of the inertial integration,

including gravity estimation, can be recovered quickly and in a

linear manner, thus removing any need for special initialization

procedures. The algorithm is implemented using a graphical simul-

taneous localization and mapping like approach that guarantees

constant time output. This paper discusses the technical aspects

of the work, including observability and the ability for the system

to estimate scale in real time. Results are presented of the system,

estimating the platforms position, velocity, and attitude, as well as

gravity vector and sensor alignment and calibration on-line in a

built environment. This paper discusses the system setup, describ-

ing the real-time integration of the IMU data with either stereo or

monocular vision data. We focus on human motion for the pur-

poses of emulating high-dynamic motion, as well as to provide a

localization system for future human–robot interaction.

Index Terms—Field robots, localization, search-and-rescue

robots, sensor fusion.

I. INTRODUCTION

HE motivation of this paper was inspired by the need to

develop a human-mounted localization system for ﬁrst re-

sponse units, such as ﬁre ﬁghters, counter-terrorism groups, and

search-and-rescue operators. The system had to keep track of

the location of the operator in an unknown building in real

time. The system could not use any purpose built localiza-

tion infrastructure and had to provide information that could

be shared with the future incorporation of robotic systems in the

mission.

A human-mounted localization system for ﬁrst responders

operating in built environments poses a number of difﬁculties.

There is the lack of (or poor) global positioning system (GPS)

signals, unreliable magnetic readings, high-dynamic motion,

Manuscript received September 7, 2010; revised April 28, 2011; accepted

September 21, 2011. Date of publication November 29, 2011; date of current

version February 9, 2012. This paper was recommended for publication by

Associate Editor J. Neira and Editor D. Fox upon evaluation of the reviewers’

comments. This work was supported in part by the Australian Research Council

(ARC) Centre of Excellence programme, funded by the ARC and the New South

Wales State Government, and by ARC Discovery under Grant DP0665439.

T. Lupton was with The University of Sydney, Sydney, N.S.W. 2006,

Australia. He is now with Silverbrook Research, Balmain, N.S.W. 2041,

Australia (e-mail: tlup8791@uni.sydney.edu.au).

S. Sukkarieh is with The University of Sydney, Sydney, N.S.W. 2006,

Australia (e-mail: salah@acfr.usyd.edu.au).

Color versions of one or more of the ﬁgures in this paper are available online

at http://ieeexplore.ieee.org.

Digital Object Identiﬁer 10.1109/TRO.2011.2170332

and the lack of any internal localization infrastructure. A local-

ization system would, thus, need to use whatever features were

available in the environment, but the closeness of objects in the

buildings, as well as the presence of walls, means that landmark

features may only be observable for short periods of time. The

system must also be able to quickly initialize and handle periods

when few or no external landmarks are observed.

Cameras can work with landmarks of opportunity that al-

ready exist in the environment; therefore, no external infras-

tructure is required. Indoor Simultaneous Localization and Map-

ping (SLAM) using a single camera has been developed in the

past [4]; however, this implementation had to operate at a high

frame rate, i.e., up to 200 Hz [9], and even then, only slow dy-

namics and certain restricted motions could be used. In addition,

as a single camera only provides bearing observations, special-

ized landmark initialization techniques are required as range is

not immediately observable, and the scale of the environment

and, therefore, the motions cannot be determined.

The use of a stereo camera pair for SLAM [17], [18] or visual

odometry [11] gives very promising results for this kind of

application. In addition to providing more constrained motion

estimates, the true map scale can be observed and scale drift over

the trajectory is eliminated. The close proximity to landmarks

and visually rich environments contribute greatly to its success.

The main shortcoming of a system that relies solely on visual

observations is that it will fail when sufﬁcient visual landmarks

are not observable, even if for just a short period of time. This

could easily be the case for human-mounted systems that are

considered in this paper. For example, loss of distinct visual

features can occur in dark areas of buildings, due to motion

blur, if smoke is present, or if the cameras are very close to

blank walls such as in narrow corridors or staircases.

The use of an inertial measurement unit (IMU) could help

in these momentary periods where visual observations may not

be available [19]. Even a low-cost IMU can observe the high

dynamics of such a system and can constrain the estimated lo-

cation for short periods once properly initialized. The difﬁculty

in using an IMU in these applications is one of obtaining proper

initialization.

In this paper, the development and analysis of a novel system

to process and fuse inertial observations with observations from

other body frame sensors, such as cameras, is presented. This

system is inspired by the idea that inertial observations can

be integrated in an arbitrary frame between poses to form a

single pseudo-observation, as presented in [16]. This core idea is

expanded in this paper with integration into a graphical ﬁlter that

62 IEEE TRANSACTIONS ON ROBOTICS, VOL. 28, NO. 1, FEBRUARY 2012

allows automatic inertial initialization and map management to

produce a robust visual-inertial navigation system.

The main contribution of this paper is that no explicit ini-

tialization stage or large uncertainty priors are required, and the

initial conditions are automatically recovered in a linear manner.

This is achieved by performing the inertial observation mathe-

matical integration in a body ﬁxed frame that moves with the

vehicle. Postintegration, both gravity and initial velocity are lin-

early corrected in the navigation frame, the orientation of which

is ﬁxed to the initial vehicle orientation. This way, the grav-

ity vector is estimated directly instead of estimating initial roll

and pitch, thus removing the major source of nonlinearity from

inertial navigation.

As a consequence, the initial velocity and gravity vector in

this frame can be recovered quickly and linearly after the ob-

servations have been integrated. These developments lead to a

quickly initializing and accurate navigation solution that is toler-

ant to faults and can quickly recover from errors automatically.

Section II of this paper provides a background on inertial nav-

igation and SLAM techniques and discusses their relevance to

the considered application. Section III details the development

of the new inertial observation processing algorithm, which we

call inertial preintegration, while Section IV explains the cal-

culation of its covariance and Jacobian components that are

required for implementation.

In Section V, the concept of performing inertial navigation

in a body referenced navigation frame instead of the traditional

globally referenced frame is discussed. The reasons why this

approach works for the considered application are explained

as well as the advantages gained from it. Section VI gives

an overview of the proposed inertial navigation technique as

a whole discussing how all the components work together to

make the initial conditions linearly observable.

Section VII details the implementation of the developed

visual-inertial navigation system as a whole. While Section VIII

provides a number of experimental results analyzing the perfor-

mance of the system in real-world environments.

A conclusion is then provided in Section IX-A and future

work in Section IX-B.

II. A

IDED INERTIAL NAVIGATION

A. Inertial Navigation

IMUs were primarily developed and used for navigation in

the aerospace community [21]. In these applications, a globally

referenced and ﬁxed navigation frame is used with aiding obser-

vations, such as GPS and magnetometer observations, and the

navigation solution is computed in this frame.

Acceleration and rotation rate observations are taken from the

IMU and transformed into this navigation frame before being

integratedto provide an estimate of the current position, velocity,

and attitude of the platform. In order for these estimates to

be obtained, the initial position, velocity, and attitude of the

platform in the frame must ﬁrst be determined. This is known

as the initial condition requirement.

This process is illustrated in Fig. 1, where a number of poses of

interest are shown. The dotted line between the poses represents

Fig. 1. Inertial navigation in a globally referenced navigation frame. Dotted

line represents the trajectory of the vehicle and the inertial observations taken

at these points. Inertial observations are transformed into the navigation frame

shown by the dashed lines.

the trajectory of the vehicle and the inertial observations taken at

these points. The inertial observations are then transformed into

the navigation frame shown by the dashed lines before being

integrated to obtain the navigation solution.

The initial attitude estimate is particularly important in order

to conduct gravity compensation because of its nonlinear effect

on the inertial observation integration. This implies that either

an initialization procedure or method of obtaining these initial

conditions is required.

These initial conditions can be obtained by an initialization

routine that places the vehicle at a known position, velocity,

and attitude to use as a starting point [3]. Other options also

include collecting IMU and aiding observations, such as GPS,

over a period of time and then performing a nonlinear batch

initialization to obtain the initial conditions or to just start with

a large uncertainty and try to converge to the true state of the

vehicle over time [13], [20].

B. Using Inertial Navigation in Simultaneous

Localization and Mapping

The algorithms for inertial navigation have been adapted for

use in SLAM applications [3], [8], [12], where observations

are made in a body referenced frame, such as with camera

observations.

These applications are usually only conducted over small

areas, where the curvature of the earth can be ignored and low-

cost IMUs are used such that earth rotation does not have a

signiﬁcant effect. As a result, the inertial integration that is used

for navigation can be simpliﬁed; see (1)–(3), shown below, for

the position, velocity, and attitude estimates, respectively.

The C

and E

terms are the rotation and rotation rate matri-

ces, respectively, from the current body frame to the navigation

frame using the current vehicle orientation φ

= p

+(t2 − t1)v



− bias

obs

)+g

)dt

(1)

LUPTON AND SUKKARIEH: VISUAL-INERTIAL-AIDED NAVIGATION FOR HIGH-DYNAMIC MOTION 63

= v



− bias

obs

)+g

)dt (2)

= φ



(ω

− bias

obs

)dt. (3)

As IMUs provide discrete time samples of the accelerations

and rotation rates experienced, these equations are used in their

discrete form. Algorithm 1 shows an example implementation

if used for state prediction in an extended Kalman ﬁlter (EKF).

For SLAM applications, both the integrated inertial naviga-

tion solution and the estimate of the uncertainty of that solution

are required. The uncertainty is needed so that when observa-

tions from other sensors are combined with the inertial observa-

tions, they can be weighted to give the optimal estimate of the

vehicle states.

Algorithm 2 shows how the uncertainty of the inertial nav-

igation solution, i.e., P

, can be calculated given the initial

vehicle state uncertainty P

, and the IMU sensor observation

noise covariance matrix Q.

TABLE I

ERIVATIVES OF THE INERTIAL PREDICTION EQUATIONS WITH RESPECT TO

ESTIMATED STATES

C. Inertial Simultaneous Localization and

Mapping Initialization

The navigation implementations that are discussed are com-

puted in a globally referenced frame. The inertial body frame

and visual observations are transformed into this frame before

being fused, and therefore, the initial condition requirement still

exists. This is shown in Algorithm 1, where the requirement for

, v

, and φ

is provided at the beginning.

Table I shows the derivatives of the standard inertial integra-

tion equations [see (1)–(3)] with respect to the vehicle states. It

can be seen from this table that the majority of the nonlinearity

comes from the vehicle attitude terms (with some nonlinearity

with respect to the IMU bias terms as well). This is because of

the nonlinear sine and cosine terms in the rotation and rotation

rate matrices.

The position and velocity states have a perfectly linear in-

teraction and, therefore, do not pose a problem for a linear

estimator.

The major problem is with unknown initial attitude due to the

nonlinear effects it has on the estimate. This causes errors when

a linear estimator, such as an EKF, is used, and slow convergence

or even instability when a linearized batch solver is used.

In [3], the initial orientation and attitude of the unmanned

aerial vehicle was obtained by keeping it stationary on the run-

way, while GPS observations were made of its position, and

internal tilt sensors were used to get the roll and pitch. The use

of such an initialization routine is common, but for the situations

that are considered in this paper, it is inconvenient to the user.

In [13], the aircraft would ﬂy with GPS and inertial obser-

vations being made over a period of time until sufﬁcient in-

formation was available for the navigation solution to converge.

This large initial angle uncertainty technique, when applied with

an EKF or other nonrelinearizing implementation, suffers from

accumulated linearization errors from uncertainties in the ve-

hicle states. These are normally handled by adding additional

stabilizing noise to the ﬁlter by inﬂating the observation or pro-

cess model uncertainties. This procedure is often unstable for

high-dynamic applications, like the one considered in this paper,

where the initial attitude has a large range of possible values,

not just close to ﬂat and level for aircraft applications.

As a result, a batch initialization routine is probably the best

suited to the target application as it does not require the user

to perform a specialized initialization routine, and it provides a

more numerically stable and accurate initialization.

III. I

NERTIAL PREINTEGRATION THEORY

Regardless of the initialization procedure chosen, all the IMU

observations are traditionally transformed into the navigation

剩余15页未读，继续阅读

评论收藏

内容反馈

山喵

粉丝: 14
资源: 19

Visual-Inertial-Aided Navigation for High-Dynamic Motion in Buil...

最新资源

Visual-Inertial-Aided Navigation for High-Dynamic Motion in Buil...

High-Precision, Consistent EKF-based Visual-Inertial Odometry.pdf

On-Manifold Preintegration for Real-Time Visual-Inertial Odometry.pdf

Selective Sensor Fusion for Neural Visual-Inertial Odometry.pdf

Visual-Inertial Monocular SLAM with Map Reuse.pdf

Keyframe-Based Visual-Inertial Odometry Using Nonlinear Optimization.pdf

IEKF-based Visual-Inertial Odometryusing Direct Photometric Feedback ROVIO

Visual-Inertial Monocular SLAM with Map Reuse

梁伟Visual C++网络编程经典案例详解源代码 9-14

VINS（Visual-Inertial Navigation System）简介

7_27am_李明杨_Visual-Inertial SLAM 理论和算法1

SLAM-基于关键帧的视觉惯性SLAM-Visual-Inertial-优质项目实战-毕业设计.zip

SLAM-适配ROS的基于关键帧的开放式视觉惯性SLAM算法-Visual-Inertial-优质项目实战.zip

ygz-stereo-inertial（立体匹配优化算法）

vins（Visual-Inertial Navigation System）.docx

Visual-Inertial+SLAM算法设计1

Quantitative Trajectory Evaluation for Visual(-Inertial) Odometry.pdf

Keyframe-Based Visual-Inertial SLAM Using Nonlinear Optimization

Tightly-Coupled Monocular Visual-Inertial Fusion for AutonomousFlight

Low-Cost GPS Aided Inertial Navigation System

VINS SLAM教程（Visual-Inertial Navigation: A Tutorial January 15, 2017）

Visual-lidar+odometry+and+mapping_+low-drift,+robust,+and+fast.pdf

Estimation Techniques for Low-Cost Inertial Navigation

博客中聚类算法（K-means、FCM、DBSCAN、DPC）的数据集（免积分）

机器学习期末复习题及答案

神经网络回归预测--气温数据集

最新资源