Graduate Seminar, July 2017

Yair Ben Elisha

Under the supervision of Asst. Prof. Vadim Indelman

•Introduction

•Related work

•Contribution

•Single Robot Approach

•Multi-robot Approach

•Conclusion

Autonomous cars(google)

Military Mines/Tunnels

Exploration

Space Exploration

Sub-marine

explorationIndoor Operation

•Navigation and mapping in unknown, GPS-deprived,

environment

•Key components for autonomous operation include:

▪ Estimation and Perception – Where am I? What is the environment?

• Simultaneous localization and mapping (SLAM)

▪ Planning – What is the best action to do next?

• Traditional planning approaches

• Belief Space Planning (BSP)

Perception

Planning

Estimation‘Passive’

‘Active’

Inertial sensor

offline calibration

Camera

offline calibration

Inertial Sensor

Online calibration (pre-determined maneuvers)

Sensors

Calibration

Goal

“GPS”

“Sensors

Calibration”

• Aerial cooperative multi-robot

• Sensors:

▪ Inertial Measurements Unit (IMU)

▪ Monocular downward-looking camera

• Vision inertial navigation system (VINS)

• Partially unknown environment

• GPS-deprived environment

• Centralized architecture

Planning optimal trajectories for cooperative

robots to achieve online IMU calibration and

accurate navigation

• Online Calibration

(V. Indelamn, 2012), (J. Maye, 2016)

▪ SLAM considering IMU and extrinsic parameters calibration

▪ Without planning

• Belief Space Planning (BSP)

(V. Indelman, 2013), (G. A. Hollinger, 2014) , (V. Indelman, 2015)

▪ Performance improvement in SLAM

▪ Not considering IMU measurements

• Planning Considering Online Calibration

▪ Extrinsic parameters calibration (transformation between frames)

(W. Achtelik, 2013), (D.J. Webb, 2014), (J. Maye, 2016)

▪ IMU calibration assuming GPS availability

(K. Hausman, 2016)

• Multi-robot Belief Space Planning

(A. Kim, 2014, IJRR) (V. Indelman, 2015, IJRR) (V. Indelman, 2015, ISRR)

▪ Operation in unknown environments

▪ Cooperation via mutual landmark observations or observing other robots

(V. Indelman, 2015, ISRR)

• Incorporating online sensor calibration into belief space planning

(BSP) for visual-inertial navigation systems

▪ Allows to consider, within planning, reduction of navigation estimation

uncertainty and reduction of the uncertainty evolution rate

• Approach for cooperative multi-robot BSP using future indirect

constraints given past correlation between robots

▪ Introduce concept of “expendable” robots used for updating other robots

• Incorporate recently developed concept of IMU pre-integration into BSP

Robot’s Nav.

State

0{ , , }r r r

k kX x x

IMU Sensors

Calibration

0{ , , }r r r

k kC c c

Perceived

Environment

0{ , , }nkL l l

Joint State

{ , , }k k k kX C L

Control

Actions

0: 1 0 1{ , , }r r r

k kU u u

Sensors

Measurements

1: 1{ , , }r r r

k kZ z z

1

Rr

k k rX X

1

Rr

k k rC C

Random

Varia

ble

sG

iven

• Joint probability distribution function (pdf)

1: 0: 1

1 1 1 1

1

| ,

· | , , · | |

k k k k

kIMU o

i i i i i i i i

i

b p Z U

priors p x x c z p c c p z

Motion ModelObservation

ModelCalibration

Model

1 1 1| , , IMU

i i i ip x x c z

Motion Model:

Strapdown Equations:

1 1 1, ,

1

2

i i

IMU T m a

i i i i i i i

m g

i i i i

p v

f x c z v C q a b n g

q d n q

1 1 1 1

1

[ ]

, ,

: ~ 0,

i i i i

IMU

i i i i i

i w

x p v q

x f x c z w

Gaussian Noise w N

Calibration Model:

Random constant model:

1|i ip c c

1 1i i ic c e

1 1

1

[ ]

: ~ 0,

i i i

i i i

i e

c d b

c g c e

Gaussian Noise e N

Single Observation Model:

General Observation Model:

| o

i ip z

, ,

: ~ 0,

,

i j i j i

i v

i j

z h x l v

Gaussian Noise v N

h x l Pinhole Camera Model

,| | ,

:

oj k

o

k k k j k j

l

o

k k

p z p z x l

Landmarks observed from xk

1 1 1 1| , , | |IMU o

i i i i i i i ip x x c z p c c p z

Motion ModelObservation

Model

Calibration

Model

1 1 1 1

1

[ ]

, ,

~ 0,

i i i i

IMU

i i i i i

i w

x p v q

x f x c z w

w N

1 1 1 1

1

[ ]

~ 0,

i i i

i i i i i

i e

c d b

c g c e c e

e N

,

,

,

~ 0,

| | ,o

j k

i j i j i

i v

o

k k k j k j

l

o

k k

z h x l v

v N

p z p z x l

• Factorization of a joint pdf in terms

of process and measurement models

▪ Vertices represent the variables

▪ Nodes represent constrains between

variables, the factors

• Computationally efficient

probabilistic inference

• The belief at the lth look ahead time is defined as:

•Maximum a posteriori (MAP) inference:

0: 0: 1

1 1 1 1 1| , ,

|

|

,

|

k l k l k l k l

o

k l k l k l k l k l k l k l k l k l

b p Z

b p x x u c p c c p z

U

,k l k l k lb

•Challenge:

▪ High rate IMU measurements

▪ Updating the graph at high rate

▪ High complexity

▪ No real time performance in inference, limits planning horizon in BSP

•Solution:

▪ Integrate IMU measurements into a single factor

▪ Add corresponding factor at the frequency of other sensors (e.g.

camera)

▪ Previous works use this concept within inference only

▪ Additionally use this concept within BSP to increase planning horizon

[T. Lupton, 2012]

[V. Indelman, 2013]

• The objective function over a planning horizon of L steps:

•Optimal control:

• Choice of cost function

1

: 1

0

, ,L

k k L k k L l k l k l L k L

l

J b U cf b u cf b

: 1

: 1 : 1argmin ,k k L

kU

k L k k L k k LU J b U

penalizes joint state uncertaintypenalizes control actionspenalizes reaching the goal

,u

Goal T

k l k l k l k l k lMMcf b u X X u tr M M

, k lcf M

Cost function

•Optimal control:

• Discrete Methods:

▪ Choose best path from a set of candidate paths

▪ Sampling methods (e.g. RRT or PRM)

• Continuous Methods:

▪ Direct optimization or Gradient descent methods

: 1

: 1 : 1argmin ,k k L

kU

k L k k L k k LU J b U

•Matlab simulation using GTSAM library

• Synthetic IMU measurements and camera observations

• Assuming data association is solved

• Assuming heading angle control only

• A priori-known regions with different uncertainty levels

• Theorem: Full observability requires the camera-IMU

platform to undergo rotation about at least two IMU axes

and acceleration along two IMU axes

• Conclusion: Heading angle control is not sufficient for full

IMU calibration

• Alternatives: Using a priori known regions with different

levels of uncertainty to calibrate accelerometers only

[Achtelik13icra]

Position Covariance Accel. Calib. Covariance

Known Region

Known Region

• Environment

▪ Randomly scattered unknown landmarks

▪ Goal location within area without landmarks at all (“dark corridor”)

▪ A priori-known regions with different uncertainty levels

• Discrete method – generate candidate paths

▪ Shortest path to goal

▪ Shortest paths to clusters of mapped/known landmarks

• Approach 1 – ‘BSP-Calib’ (our approach)

▪ Incorporate sensor calibration states within the belief

▪ “Trade-off” uncertainty cost function

• Approach 2 – ‘BSP’

▪ Does not incorporate sensor calibration states within the belief

• Approach 3 – ‘Shortest-Path’

▪ Uncertainty cost function set to zero

TO

c c x x

T T

k l k lcf tr M M tr M M

BSP-Calib BSP Shortest-Path

c c

u

G G T

k l k l k l k lMMlcf E u tr M M

Uncertainty = 10m Uncertainty = 1e-5m - Covariance

Accel. Calibration CovarianceKnown Region

28.5

21.1

Position Covariance“Dark Corridor”

•Online active self-calibration of IMU in GPS-deprived

environment using BSP

• Improved navigation accuracy

• Incorporated IMU pre-integration concept within BSP for

longer planning horizons

• Cooperative multi-robot:

▪ Robust and faster exploration/mapping

▪ Higher accuracy in a multi robot collaborative framework

Cooperation via mutual

landmark observationsCooperation via

observing other robots

Cooperation via

observing same landmark

Cooperation via

observing other robots

Indirect cooperation

given past correlation

Given correlation,

all robots are updated when a

single robot observes a (known)

landmark

• Relaxing previous cooperation constraints

• Requires initial/past correlation between robots

•Given prior correlation, informative observation made by one

robot, impacts also the states of other robots

Note: Study of correlation decay with time or sensitivity to correlation

magnitude – not part of this work

Given correlation,

all robots are updated when a single robot

observes a (known) landmark

• Study case:

▪ Two robots, r1 and r2 , starting with some initial correlation

▪ r1 observes a prioiri known landmark

▪ r2 does not make any observations

• Definition of covariance matrix Σ or the information matrix I

• R - The square root information matrix

11 121

21 22

,TI R R I

• r1 observes the known region at tk

2

1

1 1

1 1

. # .

0

,

X n

rJacob n Robots o

Tr r

erv

r

bs

A h a

X x x z

a a

h x v

11 12

21 22

k k

k

k k

• Calculation of Σ22

2

2 11

22 22 2 2 21 1

22 11 1 12

k

k

k k k

rr

r r a r

11 12

21 22

k k

k

k k

•Examined Trajectories:

“Shortest-Path” “Future-Correlation”“Higher-Correlation”

Low Prior Correlation High Prior Correlation No Prior Correlation

•Position Accuracy (errors and covariance):

“Shortest-Path” “Future-Correlation”“Higher-Correlation”

• The belief of R robots is defined as:

• Extendable to BSP for the lth look ahead time

, , . ,

1 1 1 1

1 1

| , , | |R k

r r r r r IMU r r r cam r o

k i i i i i i i i

r i

b priors p x x c z p c c p Z

•General objective function

• Cost function

1

: 1

0

1

, ,L

k k L k k L l k l k l L k L

l

Ri

i

J b U cf b u cf b

cf cf

1

,r r

ru

Ri Goal r r

k l k l k lM Mr

cf u X X cf M

• Simulation – same as single robot

• Partial unknown environment

▪ Mostly unknown environment

▪ A priori-known regions with different uncertainty levels

• Discrete method – PRM

• Re-planning every 8 steps

• Initial Correlation is created by observing same landmark

1 1

1

11 2 2 1,,G

l k l l k lM

cf X c cf MX f

Uncertainty = 1e-5m - Covariance

Accel. Calibration CovariancePosition Covariance & Error

1 1 1

1

1 2 2 1, ,G G

l k l k l l k lM

cf E cf cf M

•Online self-calibration of IMU in GPS-deprived

environment using BSP for improving navigation accuracy

•Cooperative multi-robot using indirect updates within BSP