Sensing and Estimation

1
 Sensing
motion capture
cameras

reflective markers

2
Onboard State Estimation

3
Microsoft Hokuyo
Kinect Laser
Scanner

Operation in Unstructured
Environments
4
Shaojie Shen,Yash Mulgaonkar, Nathan Michael and Vijay Kumar, “Multi-Sensor Fusion
for Robust Autonomous Flight in Indoor and Outdoor Environments with a
Rotorcraft MAV,” Proceedings of IEEE International Conference on Robotics and
Automation (ICRA), 2014.
5
 Simultaneous Localization and Mapping
also Structure from Motion
Features
f1
f0 f2
f3
z00 z01 z32
f4

z02 z11 z12

x0 z33 z34
z23 f6

z36
x1 = f (x0 , u01 , t01 ) x1 x3

x2
Robot x2 = f (x1 , u12 , t12 ) x3 = f (x2 , u23 , t23 )
Poses

Flows on SE(3) 6
Estimation and Control Architecture
position
GPS (10Hz) User
velocity
Interface
Laser
Laser Scanner odometry
(20 Hz) Pose Graph
SLAM
Altitude
Pressure estimator Multi-Sensor
Altimeter (20 Hz) Unscented
Kalman
Planner
Visual Filter (100 Hz) (20 Hz)
Stereo odometry
Camera (25 Hz)

Map Trajectory
Downward Velocity Generator (20 Hz)
estimator Refine (20 Hz)
Camera (30Hz)

Controller
IMU (100 Hz)
(100 Hz)

7
 Onboard State Estimation

S. Shen, N. Michael and V. Kumar, “Autonomous navigation in confined indoor

environments with a micro-aerial vehicle,” IEEE Robotics and Automation Magazine, 2013 8
 ½ km, 1.5 m/s, indoor/outdoor
Shaojie Shen, Yash Mulgaonkar, Nathan Michael and Vijay Kumar, “Multi-Sensor Fusion for
Robust Autonomous Flight in Indoor and Outdoor Environments with a Rotorcraft MAV,”
Proceedings of IEEE International Conference on Robotics and Automation (ICRA), 2014. 9
Indoor Navigation and Mapping

10
 80 gm

370 gm

1,750 gms
220 gm

200 W/kg
11
 Systems Design Considerations
● Larger vehicles are more capable (better
sensors, processors)
● Larger vehicles can exhibit longer missions
(bigger batteries)

● Smaller vehicles can navigate in more

constrained environments
● Smaller vehicles are more agile and
maneuverable

12
Nonlinear Control

1
 Limitations of Linear Control
● Assumption: roll and
pitch angles, and all
velocities are close to
zero

2
 Nonlinear Control

Control the robot at states far

away from the equilibrium
(hover) state

3
 Trajectory Tracking
Given rT (t), ṙT (t), r̈T (t)
rdes (t), ṙdes (t), r̈des (t)
des
(t), ˙ des (t), ¨des (t)
rT (t)

rdes r desired trajectory

des
(position, yaw)

2 3
x(t)
6 y(t) 7
ep = rT (t) r rT (t) = 6 7
4 z(t) 5
ev = ṙT (t) ṙ (t)
Want (r̈T (t) r̈c ) + kd,x ev + kp,x ep = 0

Commanded acceleration, calculated by the controller 4

 rdes u1
Trajectory ψdes Position Motor Rigid body
Planner controller controller dynamics
Attitude u2
Rc controller
(ϕc, θc, ψc) R, Ω (ϕ, θ, ψ, p, q, r)
r, ṙ
2 3 2 3
0 0
mr̈ = 4 0 5 + R 4 0 5
mg F 1 + F 2 + F 3 + F4
u1
2 3 2 3 2 3 2 3
ṗ L(F2 F4 ) p p
I 4q̇ 5 = 4 L(F3 F1 ) 5 4q 5 ⇥ I 4q 5
ṙ M 1 M 2 + M 3 M4 r r

u2 5
 Trajectory Tracking
2
x (t)des
3 ψdes
6 y des (t) 7
rT (t) = 6 7
4 z des (t) 5
des
b3
(t)
t
t b2
⇥
0
⇤ 0 ⌅
des t R des 1
R b3 =
ktk
= des
eR (Rdes , R)
b1
u2 = ! ⇥ I! + I ( KR eR K ! e! )

6
 des
How to determine R ?
You are given two pieces of information
des t
⇥
R b3 =
0 ktk
⇤ 0 ⌅
1 = des

You know that the rotation matrix has the form

2 3
c c✓ s s s✓ c s c s✓ + c✓s s
R = 4c✓s + c s s✓ c c s s✓ c✓s c 5
c s✓ s c c✓
You should be able to find the roll and pitch angles.
7
 des
How to calculate the error eR (R , R)?

●  Cannot simply take the difference of two rotation

matrices

What is the magnitude of the rotation required to go

from the current orientation to the desired
orientation?
des
R!R
The required rotation is
T des
R=R R
The angle and axis of rotation can be determined
using Rodrigues formula 8
 Stability
Large basin of attraction* ⇤ ⌅
2 1 ⇥
tr[I (R ) R] < 2
des T ⇤e (0)⇤2 ⇥ kR 1 tr I (Rdes )T R
min (I) 2

*T. Lee, M. Leoky, andN. H. McClamroch, Geometric tracking control of a quadrotor UAV on SE(3), IEEE Conference
on Decision and Control, 2010. 9
Smaller, safer …

10
 Pico Quadrotor
11 cm Safer
20 g,
6.5 Watts More maneuverable
Max speed 6m/s

Y. Mulgaonkar, G. Cross and V. Kumar, “Design of

small, safe and robust quadrotor swarms,” IEEE
International Conference on Robotics and
Automation (ICRA), Seattle WA, May 2015.

11
Recovery from mid air
Smaller, safer … collisions

12 12
 basin of 1
attraction
⇠ 5
L 2

D. Mellinger and V. Kumar, “Minimum Snap Trajectory

Generation and Control for Quadrotors,” Proc. IEEE
International Conference on Robotics and Automation. Shanghai,
China, May, 2011.

Y. Mulgaonkar, G. Cross and V. Kumar, “Design of small, safe

and robust quadrotor swarms,” in IEEE International
Conference on Robotics and Automation (ICRA), Seattle
WA, May 2015.
13
14
Sequential Nonlinear

Composition Linear
Control
Trajectory
Controller
for
Hover Attitude
only
control

15
 Trajectory Planning ⇥
⇥ F1
Inputs 4 0 1 0 1 ⇧ ⌃
F2 ⌃
u1, u2
u1 = Fi u2 = L ⇤ 1 0 1 0 ⌅⇧
⇤F3 ⌅
i=1 µ µ µ µ
2 3 F4
State r 2 3
acceleration in the
jerk in the vertical
vertical direction
direction
(q, q̇) 6v 7 q
6 7 u1 = m(a3 g.b3 )
6a7 6 q̇ 7 u̇1 = mj3
position 6 7 6 7
yaw jerk 6 j 7 6 u1 7 ü1 = ms3 + u1 (q 2 + rp)
6 7$6 7
snap 6 s 7 6 u̇1 7 mj2
6 7 6 7 p=
u1
yaw 6 7 4 ü1 5
6 7 q=
mj1
2 3
4 ˙5 u2 snap in the
vertical
u1 p
direction ! = 4q 5
¨ r
16
 Planar Quadrotor
2 3
Inputs u 1 = F2 + F 4 y
u1, u2 q = 4z 5
u2 = (F2 F4 )L
State
(q, q̇)

Equations of motion

17
 Differential Flatness
All state variables and the inputs can be written as smooth functions
of flat outputs and their derivatives (and the other way around)

Planar Quadrotor

18
 Planar Quadrotor
The flat outputs and their derivatives can be written as a function of
the state, the inputs, and their derivatives
Flat outputs State Input
 2 3 
y y u1
z 6z 7 u2
6 7
6 7
6 7
6 ẏ 7
6 7
4 ż 5
  ˙
1
ÿ m sin
= 1 u1
z̈ cos
 
m
y (iii) 1 u1 ˙ cos u̇1 sin
=
z (iii) m u1 ˙ sin + u̇1 cos
 (iv)  u1  
y 1 sin cos ü 1 1 2 u̇ 1
˙ cos + u1 ˙ 2 sin
Ixx
(iv) = u1 + ˙ sin ˙ 2 cos
z m cos Ixx sin u 2 m 2 u̇ 1 u 1
19
 Planar Quadrotor
The state, the inputs, and their derivatives can be written as a function
of the flat outputs and their derivatives
Flat outputs State Input
 2 3 
y y u1
z 6z 7 u2
6 7
6 7
6 7
6 ẏ 7
6 7
4 ż 5
˙
u1 = m ÿ 2 + z̈ 2 ü1 = ...
✓ ◆
mÿ mz̈
= atan2 , ¨ = ...
u1 u1
u̇1 = m( y (iii) sin + z (iii) cos ) u2 = ...
m ⇣ ⌘
˙= y (iii) cos + z (iii) sin
u1 20
 Differential Flatness
All state variables and the inputs can be written as smooth functions
of flat outputs and their derivatives (and the other way around)

Planar Quadrotor

Diffeomorphism

21
 Differential Flatness
All state variables and the inputs can be written as smooth
functions of flat outputs and their derivatives
2 3
3-D Quadrotor r 2 3
6v 7 q
6 7
6a7 6 q̇ 7
6 7 6 7
6j7 6 u1 7
6 7$6 7
6s7 6 u̇1 7
6 7 6 7
6 7 4 ü1 5
6 7
4 ˙5 u2
¨
22
 b3

ω3
a3 ω2 b 2
ω4 C
r
ω1

O b1
a2
The three-dimensional quadrotor is
differentially flat.
a1

23
 Trajectory Planning
B
2 3
x
6y 7
SE(3) 6 7
[x, y, z, ψ] 6z 7
6 7
6✓ 7
6 7
6 7
B
6 7
A A 6 7
6 7
6 ẋ 7
6 7
6 ẏ 7
6 7
6 ż 7
6 7
6 ✓˙ 7
6 7
Minimum snap trajectory 4 ˙5
Z T
.... ¨(t)2 dt ˙
min ↵k r (t)k2 +
(t) 0 24
25
Robots for Emergency Response

Swarms

1
2
3
4
5
 Three Organizing Principles for
Collective Behavior
● Each individual acts independently

● Actions are based on local information

● Anonymity in coordination

6
Example: Transportation and Construction

7
 Complexity
n robots, m obstacles
●  Dimensionality of the state space increases
linearly with n O(n)

●  Number of potential interactions with neighbors

increases as n2 O(mn+n2)
●  Number of potential interactions with obstacles
increases as mn

●  Number of assignments of robots to goal O(n!)

positions

8
 factorial
3 (t)
Assignment of robots to goals
( gg3
1 if robot i is assigned to goal j
i,j =
0 otherwise s1
R

1 (t)
Planning trajectories
exponential
2 3
x1 (t)
6 x2 (t) 7 gg2
X(t) = 6
4 ... 5
7 s3
R
xN (t)
gg1
(t) : [t0 , tf ] ! X(t) 2 (t)
s2
Safety Optimality
 Z tf
inf ||xi (t) xj (t)|| 2R > 0
?
(t) = argmin L( (t))dt
i6=j2I,t2[t0 ,tf ] (t) t0

9
Four Key Ideas

10
● Concurrent assignment of goals and trajectories

● Leader-follower networks

● Anonymity

● Sharing information

11
 1. Assignment of Goals and
Collision Free Trajectories
start

goal

goal
start

12
Concurrent Assignment and Planning of
Trajectories: CAPT

13
 CAPT

φ2

φ1

14
 Concurrent Assignment and Planning
Assumption p
||si gj || > 2R 2 8i 2 N , j 2 M
gg3

R
s1
Theorem

Assignments and trajectories that gg2

minimize the sum of square of distances
Z tf
s3
minimize Ẋ(t)T Ẋ(t)dt
, (t) t0 gg1
will be safe (no collisions) R

||xi (t) xj (t)|| > 2R

s2
15
 CAPT
2
10
Hungarian Algorithm
Quadratic (N2)
Time to Compute (s)

1 Cubic (N3)
10

0
10

100 200 300 500 700 1000

Number of Robots

16
2. Leader-Follower Networks

robot i

robot j

si,j (t) = xj (t) xi (t)

17
Leader-Follower Networks

PBS NOVA: Making Stuff Wilder (Hosted by David Pogue) 18

 3. Anonymity

PBS NOVA: Making Stuff Wilder (Hosted by David Pogue) 19

Control of Formation Shape and Group Motion

(Turpin, Michael, and Kumar, 2013) 20

Control of Formation Shape and Group Motion

21
 Robot First Responders

Kartik Mohta, Matthew Turpin, Alex Kushleyev, Daniel Mellinger, Nathan Michael, and Vijay Kumar,
“QuadCloud: A Rapid Response Force with Quadrotor Teams,” Int. Symp. on Experimental Robotics (ISER), 2014.
22
Kartik Mohta, Matthew Turpin, Alex Kushleyev, Daniel Mellinger, Nathan Michael, and Vijay Kumar, “QuadCloud: A Rapid
Response Force with Quadrotor Teams,” Int. Symp. on Experimental Robotics (ISER), 2014. 23
Enabling Cooperation

24
 Active Mapping
Autonomously create 3D map of an unknown environment
with ground and aerial robots

25
 Control Policy
Reduce uncertainty of map by maximizing information gain

Map Measurements

ICS [m; z⌧ | x⌧ ] Information

x⇤⌧ = arg max
x⌧ 2X D(x⌧ ) Duration

Paths

Change I_CS to IG (information gain)

26
 Active Mapping

Robot’s location

Robot’s view
20x

27
Active Mapping

28
Search and Rescue
N. Michael, S. Shen, K. Mohta,Y. Mulgaonkar,V. Kumar, K. Nagatani,Y.
Okada, S. Kiribayashi, K. Otake, K.Yoshida, K. Ohno, E. Takeuchi, and S.
Tadokoro, “Collaborative mapping of an earthquake-damaged building
via ground and aerial robots,” J. Field Robotics, vol. 29, no. 5, pp. 832–
841, 2012.

[Michael et al, 2012] 29

Final Map

30
3 floors of a 9 story building 31
Swarms!
32