机器人状态估计：矩阵李群方法解析

需积分: 9 3 浏览量更新于2024-07-18 收藏 4.44MB PDF 举报

《STATE ESTIMATION FOR ROBOTICS》是一本备受机器人领域推崇的著作，由Timothy D. Barfoot撰写，版权于2016年，尚未正式出版但已在SLAM（同时定位与地图构建）社区广受关注。本书聚焦于机器人状态估计的核心理论，特别强调矩阵李群方法在该领域的应用，这在机器人技术尤其是自主导航系统中至关重要。书中的主要内容包括深入探讨李代数，这是一种在机器人运动学和动力学建模中广泛应用的数学工具，它帮助处理复杂的空间变换和姿态估计问题。作者通过滤波器的视角，讲解了诸如卡尔曼滤波（Kalman Filter）、扩展卡尔曼滤波（Extended Kalman Filter, EKF）和粒子滤波（Particle Filter）等经典估计方法，这些技术对于实时处理传感器数据、融合多种信息源以及在机器人环境中进行精确导航非常关键。在第三章，作者新增了一节关于递归离散时间滤波器的内容，揭示它们与批量解（batch solution）之间的联系，并对早期存在的错误进行了修正。第四章中，作者补充了斯坦利·施密特在EKF发展历史上的贡献，增强了对这一技术历史背景的理解。书中还强调了概率论的贝叶斯观点，解释了如何将概率密度函数应用于状态估计，尽管经典方法通常采用概率分布的形式。在第七章，作者介绍了雅可比公式在矩阵指数运算中的应用，这对于理解系统的动态行为和状态转换十分有用。值得注意的是，书中在章节中对马氏距离（Mahalanobis distance）的定义进行了修订，确保与实际定义相符，并在4月8日的版本中增加了对概率密度函数使用的注释，体现了作者严谨的学术态度。《STATE ESTIMATION FOR ROBOTICS》是一本深度解析机器人状态估计的实用指南，不仅覆盖了理论基础，还提供了丰富的实践案例和历史沿革，对于想要深入了解机器人技术特别是SLAM和自主导航的读者来说，这是一本不可或缺的参考文献。

xvi Foreword

my primary area of interest is navigation of robots in outdoor environ-

ments, I have often been faced with vehicles operating in three dimen-

sions. Accordingly, I have attempted to provide a detailed look at how

to approach state estimation in three dimensions. In particular, I show

how to treat rotations and poses in a simple and practical way using

matrix Lie groups. The reader should have a background in under-

graduate linear algebra and calculus, but otherwise this book is fairly

standalone. I hope readers of these pages will ﬁnd something useful; I

know I learned a great deal while creating them.

I have provided some historical notes in the margins throughout

the book, mostly in the form of biographical sketches of some of the

researchers after whom various concepts and techniques are named;

I primarily used Wikipedia as the source for this information. Also,

the ﬁrst part of Chapter 6 (up to the alternate rotation parameteriza-

tions), which introduces three-dimensional geometry, is based heavily

on notes originally produced by Prof. Chris Damaren at the University

of Toronto Institute for Aerospace Studies.

This book would not have been possible without the collaborations of

many fantastic graduate students along the way. Paul Furgale’s PhD

thesis extended my understanding of matrix Lie groups signiﬁcantly

by introducing me to their use for describing poses; this led us on

an interesting journey into the details of transformation matrices and

how to use them eﬀectively in estimation problems. Paul’s later work

led me to become interested in continuous-time estimation. Chi Hay

Tong’s PhD thesis introduced me to the use of Gaussian processes in

estimation theory and he helped immensely in working out the de-

tails of the continuous-time methods presented herein; my knowledge

in this area was further improved through collaborations with Simo

S¨arkk¨a from Aalto University while on sabbatical at the University

of Oxford. Additionally, I learned a great deal by working with Sean

Anderson, Patrick Carle, Hang Dong, Andrew Lambert, Keith Leung,

Colin McManus, and Braden Stenning; each of their projects added to

my understanding of state estimation. Colin, in particular, encouraged

me several times to turn my notes from my graduate course on state

estimation into this book.

I am indebted to Gabriele D’Eleuterio, who set me on the path of

studying rotations and reference frames in the context of dynamics;

many of the tools he showed me transferred eﬀortlessly to state estima-

tion; he also taught me the importance of clean, unambiguous notation.

Finally, thanks to all those who read and pointed out errors in the

drafts of this book, particularly Marc Gallant, who found more typos

than everybody else combined.

Tim Barfoot

June 23, 2016

Toronto

Introduction

Robotics inherently deals with things that move in the world. We live

in an era of rovers on Mars, drones surveying the Earth, and soon

self-driving cars. And, although speciﬁc robots have their subtleties,

there are also some common issues we must face in all applications,

particularly state estimation and control.

The state of a robot is a set of quantities, such as position, orien-

tation, and velocity, that if known fully describe that robot’s motion

over time. Here we will focus entirely on the problem of estimating

the state of a robot, putting aside the notion of control. Yes, control

is essential, as we would like to make our robots behave in a certain

way. But, the ﬁrst step in doing so is often the process of determining

the state. Moreover, the diﬃculty of state estimation is often underes-

timated for real-world problems and thus it is important to put it on

an equal footing with control.

In this book, we will introduce the classic estimation results for linear

systems corrupted by Gaussian measurement noise. We will then ex-

amine some of the extensions to nonlinear systems with non-Gaussian

noise. In a departure from typical estimation texts, we will take a de-

tailed look at how to tailor general estimation results to robots oper-

ating in three-dimensional space, advocating a particular approach to

handling rotations: matrix Lie groups.

The rest of this introduction will provide a little history of estimation,

discuss types of sensors and measurements, and introduce the problem

of state estimation. We conclude with a breakdown of the contents of

the book and provide some other suggested reading.

1.1 A Little History

About 4,000 years ago, the early seafarers were faced with a vehicular

state estimation problem: how to determine a ship’s position while at

sea. Early attempts to develop primitive charts and make observations

of the sun allowed local navigation along coastlines. However, it was

not until the ﬁfteenth century that global navigation on the open sea

became possible with the advent of key technologies and tools. The

Mariner’s compass, an early form of the magnetic compass, allowed

2 Introduction

crude measurements of direction to be made. Together with coarse

nautical charts, the compass made it possible to sail along rhumb lines

between key destinations (i.e., following a compass bearing). A seriesFigure 1.1

Quadrant. A tool

used to measure

angles.

of instruments were then gradually invented that made it possible to

measure the angle between distant points (i.e., cross-staﬀ, astrolabe,

quadrant, sextant, theodolite) with increasing accuracy.

These instruments allowed latitude to be determined at sea fairly

readily using celestial navigation. For example, in the Northern hemi-

sphere the angle between the North Star, Polaris, and the horizon pro-

vides the latitude. Longitude, however, was a much more diﬃcult prob-

lem. It was known early on that an accurate timepiece was the missing

piece of the puzzle for the determination of longitude. The behaviours

of key celestial bodies appear diﬀerently at diﬀerent locations on the

Earth. Knowing the time of day therefore allows longitude to be in-

ferred. In 1764, British clockmaker John Harrison built the ﬁrst accu-Figure 1.2

Harrison’s H4. The

ﬁrst clock able to

keep accurate time

at sea, enabling

determination of

longitude.

rate portable timepiece that eﬀectively solved the longitude problem;

a ship’s longitude could be determined to within about ten nautical

miles.

Estimation theory also ﬁnds its roots in astronomy. The method of

least squares was pioneered

by Gauss, who developed the technique to

Carl Friedrich

Gauss (1777-1855)

was a German

mathematician

who contributed

signiﬁcantly to

many ﬁelds

including statistics

and estimation.

minimize the impact of measurement error in the prediction of orbits.

Gauss reportedly used least squares to predict the position of the dwarf

planet Ceres after passing behind the Sun, accurate to within half a

degree (about nine months after it was last seen). The year was 1801

and Gauss was 23. Later, in 1809, he proved that the least-squares

method is optimal under the assumption of normally distributed errors.

Most of the classic estimation techniques in use today can be directly

related to Gauss’ least-squares method.

The idea of ﬁtting models to minimize the impact of measurement

error carried forward, but it was not until the middle of the twenti-

eth century that estimation really took oﬀ. This was likely correlated

with the dawn of the computer age. In 1960, K´alm´an published two

Rudolf Emil

K´alm´an (1930-) is

a Hungarian-born

American electrical

engineer,

mathematician,

and inventor.

landmark papers that have deﬁned much of what has followed in the

ﬁeld of state estimation. First, he introduced the notion of observability

(Kalman, 1960a), which tells us when a state can be inferred from a

set of measurements in a dynamic system. Second, he introduced an

optimal framework for estimating a system’s state in the presence of

measurement noise (Kalman, 1960b); this classic technique for linear

systems (whose measurements are corrupted by Gaussian noise) is fa-

mously known as the Kalman ﬁlter, and has been the workhorse of esti-

mation for the more than 50 years since its inception. Although used in

many ﬁelds, it has been widely adopted in aerospace applications. Re-

There is some debate as to whether Adrien Marie Legendre might have come up with

least squares before Gauss.

1.2 Sensors, Measurements, and Problem Deﬁnition 3

searchers at National Aeronautics and Space Administration (NASA)

were the ﬁrst to employ the Kalman ﬁlter to aid in the estimation of

spacecraft trajectories on the Ranger, Mariner, and Apollo programs.

In particular, the on-board computer on the Apollo 11 lunar module,

the ﬁrst manned spacecraft to land on the surface of the Moon, em-

ployed a Kalman ﬁlter to estimate the module’s position above the

lunar surface based on noisy radar measurements.

Early Estimation

Milestones

1654 Pascal and

Fermat lay

foundations

of probability

theory

1764 Bayes’ rule

1801 Gauss uses

least-squares

to estimate

the orbit of

the planetoid

Ceres

1805 Legendre pub-

lishes “least-

squares”

1913 Markov chains

1933 (Chapman)-

Kolmogorov

equations

1949 Wiener ﬁlter

1960 Kalman

(Bucy) ﬁlter

1965 Rauch-Tung-

Striebel

smoother

1970 Jazwinski

coins “Bayes

ﬁlter”

Many incremental improvements have been made to the ﬁeld of state

estimation since these early milestones. Faster and cheaper computers

have allowed much more computationally complex techniques to be

implemented in practical systems. However, until about ﬁfteen years

ago, it seemed that estimation was possibly waning as an active research

area. But, something has happened to change that; exciting new sensing

technologies are coming along (e.g., digital cameras, laser imaging, the

global positioning satellites) that pose new challenges to this old ﬁeld.

1.2 Sensors, Measurements, and Problem Deﬁnition

To understand the need for state estimation is to understand the na-

ture of sensors. All sensors have a limited precision. Therefore, all mea-

surements derived from real sensors have associated uncertainty. Some

sensors are better at measuring speciﬁc quantities than others, but even

the best sensors still have a degree of imprecision. When we combine

various sensor measurements into a state estimate, it is important to

keep track of all the uncertainties involved and therefore (hopefully)

know how conﬁdent we can be in our estimate.

In some sense, state estimation is about doing the best we can with

the sensors we have. This, however, does not prevent us from in parallel

improving the quality of our sensors. A good example is the theodolite Figure 1.3

Theodolite. A

better tool to

measure angles.

sensor that was developed in 1787 to allow triangulation across the

English Channel. It was much more precise than its predecessors and

helped show that much of England was poorly mapped by tying mea-

surements to well-mapped France.

It is useful to put sensors into two categories: interoceptive and exte-

roceptive. These are actually terms borrowed from human physiology,

but they have become somewhat common in engineering. Some deﬁni-

tions

in·tero·cep·tive [int-@-r¯o-’sep-tiv], adjective: of, relating to, or being

stimuli arising within the body.

ex·tero·cep·tive [ek-st@-r¯o-’sep-tiv], adjective: relating to, being, or

activated by stimuli received by an organism from outside.

Typical interoceptive sensors are the accelerometer (measures transla-

Merriam-Webster’s Dictionary.

4 Introduction

tional acceleration), gyroscope (measures angular rate), wheel odome-

ter (measures angular rate). Typical exteroceptive sensors are the cam-

era (measures range/bearing to a landmark or landmarks) and time-

of-ﬂight transmitter/receiver (e.g., laser rangeﬁnder, pseudolites, global

positioning system (GPS) transmitter/receiver). Roughly speaking, we

can think of exteroceptive measurements as being of the position and

orientation of a vehicle while interoceptive ones are of a vehicle’s ve-

locity or acceleration. In most cases, the best state estimation concepts

make use of both interoceptive and exteroceptive measurements. For

example, the combination of a GPS receiver (exteroceptive) and an in-

ertial measurement unit (3 linear accelerometers and 3 rate gyros; inte-

roceptive) is a popular means of estimating a vehicle’s position/velocity

on Earth. And, the combination of a sun/star sensor (exteroceptive)

and three rate gyros (interoceptive) is commonly used to carry out pose

determination on satellites.

Now that we understand a little bit about sensors, we are prepared

to deﬁne the problem that will be investigated in this book:

Estimation is the problem of reconstructing the underlying state of a system given a

sequence of measurements as well as a prior model of the system.

There are many speciﬁc versions of this problem and just as many

solutions. The goal will be to understand which methods work well in

which situations, in order to pick the best tool for the job.

1.3 How This Book is Organized

The book is broken into three main parts:

I: Estimation Machinery

II: Three-Dimensional Machinery

III: Applications

The ﬁrst part, Estimation Machinery, presents classic and state-of-the-

art estimation tools, without the complication of dealing with things

that live in three-dimensional space (and therefore translate and ro-

tate); the state to be estimated is assumed to be a generic vector.

For those not interested in the details of working in three-dimensional

space, this ﬁrst part can be read in a standalone manner. It covers both

recursive state estimation techniques as well as batch methods (less

common in classic estimation books). As is commonplace in robotics

and machine learning today, we adopt a Bayesian approach to estima-

tion in this book. We contrast (full) Bayesian methods with maximum

a posteriori (MAP) methods, and attempt to make clear the diﬀerence

between these when faced with nonlinear problems. The book also con-

nects continuous-time estimation with Gaussian process regression from

剩余381页未读，继续阅读

clifftree1

粉丝: 0
资源: 4

机器人状态估计：矩阵李群方法解析

2017版机器人状态估计权威指南：SLAM进阶必读

机器人学状态估计详解：线性与非线性系统方法

2017年机器人状态估计：矩阵李群方法的进阶指南

state estimation for robotics

state estimation for Robotics

STATE ESTIMATION FOR ROBOTICS pdf

State Estimation for Robotics20181128

STATE ESTIMATION FOR ROBOTICS 2017

State estimation for Robotics.pdf

State Estimation for Robotics_20180512

最新资源