chapter 13uavbook.byu.edu/lib/exe/fetch.php?media=lecture:chap13.pdfbeard & mclain, “small...

Beard & McLain, “Small Unmanned Aircraft,” Princeton University Press, 2012, Chapter 13: Slide 1

Chapter 13

Vision Based Guidance


Architecture w/ Camera

vision-based guidance

path manager

path following

autopilot

unmanned aircraft

waypoints

on-board sensors

position error

tracking error

status

targets to track/avoid

servo commands state estimator

wind

path definition

airspeed, altitude, heading,

commands

x(t)

camera


Gimbal and Camera Reference Frames

Gimbal-1 frame: Fg1= (ig1, jg1,kg1

)

Gimbal frame: Fg= (ig, jg,kg

)

Camera frame: Fc= (ic, jc,kc

)

Gimbal-1 frame:

Rotate body frame about kbaxis

by gimbal azimuth angle, ↵az

Rg1b (↵az)

4=

0

@cos↵az sin↵az 0

� sin↵az cos↵az 0

0 0 1

1

A


Gimbal Reference Frames

Gimbal frame:

Rotate gimbal-1 frame about jg1 axis

by gimbal elevation angle, ↵el

Rgg1(↵el)

4=

0

@cos↵el 0 � sin↵el

0 1 0

sin↵el 0 cos↵el

1

A

Rgb = Rg

g1Rg1b =

0

@cos↵el cos↵az cos↵el sin↵az � sin↵el

� sin↵az cos↵az 0

sin↵el cos↵az sin↵el sin↵az cos↵el

1

A


Camera Reference Frame

Camera frame:

ic points to the right in the image

jc points down in the image

kcpoints along the optical axis

Rcg =

0

@0 1 0

0 0 1

1 0 0

1

A


Pinhole Camera Model

• Describes mathematical relationship between

coordinates of 3-D point and its projection

onto 2-D image plane

• Assumes point aperture, no lenses

• Good 1st order approximation


Pinhole Camera Model

image plane

Goal:

From pixel location (✏x

, ✏y

) in image plane and camera

parameters, determine location of object in world `c

Approach:

Use similar triangles geometry


Camera Model

image plane

f : focal length in pixels

P : converts pixels to meters

M : width of square pixel array

v: field of view

`c: 3-D location of point

f =

M

2 tan

��

2

�

Location of projection of object

in camera frame: (P ✏x

, P ✏y

, Pf)Pixel location (in pixels): ✏

x

, ✏y

Distance from origin of camera frame

to object image:

F =

qf2

+ ✏2x

+ ✏2y


Camera Model

image plane

By similar triangles:

`cx

L =

P ✏x

PF=

✏x

F

Similarly:

`cy

/L = ✏y

/F and `cz

/L = f/F

Combining:

`c =

0

@`cx

`cy

`cz

1

A=

LF

0

@✏x

✏y

f

1

A

Problem: L is unknown...


Camera Model

image plane

Unit direction vector to target:

`c

L =

1

F

0

@✏x

✏y

f

1

A=

1q✏2x

+ ✏2y

+ f2

0

@✏x

✏y

f

1

A

Define notation:

ˇ`4=

0

@ˇ`x

ˇ`y

ˇ`z

1

A 4=

`

L


Gimbal Pointing

• Two scenarios

– Point gimbal at given world coordinate

• “Point to this GPS location”

– Point gimbal so that optical axis aligns with certain

point in image plane

• “Point at this object”

• Gimbal dynamics

– Assume rate control inputs


Scenario 1: Point Gimbal at World Coordinate

piobj

: known location of object in inertial frame (world coordinate)

piMAV

= (pn, pe, pd)>: location of MAV in inertial frame

Objective:Align optical axis of camera with desired relative position vector

`id4= pi

obj

� piMAV

Body-frame unit vector that points in desired direction of specified

world coordinates

ˇ`bd =

1��`id��R

bi `

id


Scenario 2: Point Gimbal at Object in Image

Objective:Maneuver gimbal so that object at pixel location ✏ is pushed to center of image

Desired direction of optical axis:

ˇ`c

d

=

1qf2

+ ✏2x

+ ✏2y

0

@✏x

✏y

f

1

A

Desired direction of optical axis expressed in body frame:

ˇ`b

d

= Rb

g

Rg

c

ˇ`c

d


Gimbal Pointing Angles

We know direction to point gimbal

What are corresponding azimuth and elevation angles?

Optical axis in camera frame = (0, 0, 1)>

Objective: Select commanded gimbal angles ↵c

az and ↵c

el so that

ˇbd

4=

0

@ˇbxd

ˇbyd

ˇbzd

1

A = Rb

g

(↵c

az,↵c

el)Rg

c

0

@001

1

A

=

0

@cos↵c

el cos↵c

az � sin↵c

el � sin↵c

el cos↵c

az

cos↵c

el sin↵c

az cos↵c

az � sin↵c

el sin↵c

az

sin↵c

el 0 cos↵c

el

1

A

0

@0 0 11 0 00 1 0

1

A

0

@001

1

A

=

0

@cos↵c

el cos↵c

az

cos↵c

el sin↵c

az

sin↵c

el

1

A


Gimbal Pointing Angles

Objective: Select commanded gimbal angles gimbal angles ↵c

az and ↵c

el so that

0

@ˇ`bxd

ˇ`byd

ˇ`bzd

1

A=

0

@cos↵c

el cos↵c

az

cos↵c

el sin↵c

az

sin↵c

el

1

A

Solving for ↵c

el and ↵c

az gives desired azimuth and elevation angles:

↵c

az = tan

�1

ˇ`byd

ˇ`bxd

!

↵c

el = sin

�1�ˇ`bzd

�

Gimbal servo commands can be selected as:

uaz = kaz(↵c

az � ↵az)

uel = kel(↵c

el � ↵el)

kaz and kel are positive control gains


GeolocationRelative position vector between target and MAV: ` = p

obj

� pMAV

Define:

L = k`k (range to target)

ˇ` = `/L (unit vector pointing from aircraft to target)

From geometry:

piobj

= piMAV

+RibRb

gRgc`

c

= piMAV

+ L⇣Ri

bRbgRg

cˇ`c⌘

where

piMAV

= (pn, pe, pd)>

Rib = Ri

b(�, ✓, )

Rbg = Rb

g(↵az

,↵el

)

L is unknown ! solving geolocation problem reduces to estimating range to target L


Range to Target – Flat Earth Model

From geometry: cos' =

h

LFrom Cauchy-Schwartz equality: cos' = ki · ˇ`i = ki · Ri

bRbgRg

cˇ`c

Equating: L =

h

ki · RibRb

gRgcˇ`c

From before: piobj

= piMAV

+ L⇣Ri

bRbgRg

cˇ`c⌘

Therefore: piobj

=

0

@pnpepd

1

A+ h

RibRb

gRgcˇ`c

ki · RibRb

gRgcˇ`c


Geolocation Errors

The position of the object is

piobj

=

0

@pnpepd

1

A+ h

RibRb

gRgcˇ`c

ki · RibRb

gRgcˇ`c

Equation is highly sensitive to measurement errors

• attitude estimation errors

• gimbal pointing angle errors

• gimbal/autopilot alignment errors

Two types of errors

• bias errors (address with calibration)

• random errors (address with EKF)


Geolocation Using EKF

The states are x = (pi>

obj

, L)>. Need propagation equations x = f(x, u).

Assuming the object is stationary:

˙piobj

= 0

Also:

˙L =

d

dt

q(pi

obj

� piMAV

)

>(pi

obj

� piMAV

)

=

(piobj

� piMAV

)

>(

˙piobj

� ˙piMAV

)

L

= �(pi

obj

� piMAV

)

>˙piMAV

L

where

piMAV

= piobj

� L⇣Ri

bRbgRg

cˇ`c⌘

and for constant-altitude flight:

˙piMAV

=

0

@ˆ

Vg cos �

ˆ

Vg sin �

0

1

A


Geolocation Using EKF

Prediction equations:

˙

ˆpiobj

˙

ˆL

!=

0

� (

ˆpiobj

�ˆpiMAV

)

>˙

ˆpiMAV

ˆL

!

Jacobian of prediction equation:

@f

@x

=

0 0

�˙

ˆpiTMAV

ˆL(

ˆpiobj

�ˆpiMAV

)

>˙

ˆpMAV

ˆL2

!

Measurement equation: piMAV

= piobj

� L⇣Ri

bRbgRg

cˇ`c⌘

Jacobian of measurement equation:

@h

@x

=

�I Ri

bRbgRg

cˇ`c�


GPS smoother

geo-location

attitude estimation

vision processing

gimbal

Geolocation Architecture


Target Motion in Image Plane

• The objective is to track targets seen in the image plane.

• Feature tracking, or other computer vision algorithms, are used to track

pixels in image plane.

• Feature tracking is noisy, and so pixel location must be low pass filtered.

• Motion of the target in the image plane is due to two sources:

– Self motion, or ego motion. Primarily due to rotation of the MAV.

– Relative target motion.

• Since we are primarily interested in the relative motion, we must subtract

the pixel movement due to ego motion.


Pixel LPF and Differentiation

Define the following

¯✏ = (✏x

, ✏y

)

>, Raw pixel measurements,

✏ = (✏x

, ✏y

)

>, Filtered pixel location,

˙✏ = (✏x

, ✏y

)

>, Filtered pixel velocity

Basic idea: LPF

¯✏ to obtain ✏, and use dirty derivative to obtain

˙✏:

✏(s) =1

⌧s+ 1

¯✏, ˙✏(s) =s

⌧s+ 1

¯✏.


Digital Approximation

Tustin approximation s 7! 2

Ts

z � 1

z + 1

,

Converting to the z-domain gives

✏[z] =1

2⌧Ts

z�1z+1 + 1

¯✏ =

Ts2⌧+Ts

(z + 1)

z � 2⌧�Ts2⌧+Ts

¯✏

˙✏[z] =2Ts

z�1z+1

2⌧Ts

z�1z+1 + 1

¯✏ =

22⌧+Ts

(z � 1)

z � 2⌧�Ts2⌧+Ts

¯✏.

Taking the inverse z-transform gives the di↵erence equations

✏[n] =

✓2⌧ � Ts

2⌧ + Ts

◆✏[n� 1] +

✓Ts

2⌧ + Ts

◆(

¯✏[n] + ¯✏[n� 1])

˙✏[n] =

✓2⌧ � Ts

2⌧ + Ts

◆˙✏[n� 1] +

✓2

2⌧ + Ts

◆(

¯✏[n]� ¯✏[n� 1]) ,

where ✏[0] = ¯✏[0] and ˙✏[0] = 0.


Digital Approximation

Define � = (2⌧ � Ts)/(2⌧ + Ts), and note that 1� � = 2Ts/(2⌧ + Ts). Then

✏[n] = � ✏[n� 1] + (1� �)

✓¯✏[n] + ¯✏[n� 1]

2

◆

| {z }Average of last two samples

˙✏[n] = � ˙✏[n� 1] + (1� �)

✓¯✏[n]� ¯✏[n� 1]

Ts

◆

| {z }Euler approximation of derivative

,

where ✏[0] = ¯✏[0] and ˙✏[0] = 0.


Apparent Motion

Let

ˇ`4= `/L = (p

obj

� pMAV

)/ kpobj

� pMAV

k be the normalized relative

position vector.

The Coriolis formula (expressed in camera frame) gives

dˇ`c

dti=

dˇ`c

dtc+ !c

c/i ⇥ ˇ`c.

Motion of target

in image plane

True relative

translational motion

between target and MAV

Apparent motion

due to rotation of

MAV and gimbal


Apparent Motion, cont.

Motion of target in image plane:

dˇ`c

dtc

=

d

dtc

0

@✏x

✏y

f

1

A

F=

F

0

@✏x

✏y

0

1

A� ˙F

0

@✏x

✏y

f

1

A

F 2=

F

0

@✏x

✏y

0

1

A� ✏

x

✏

x

+✏

y

✏

y

F

0

@✏x

✏y

f

1

A

F 2

=

1

F 3

0

@F 2 � ✏2

x

�✏x

✏y

�✏x

✏y

F 2 � ✏2y

�✏x

f �✏y

f

1

A✓✏x

✏y

◆=

1

F 3

0

@✏2y

+ f2 �✏x

✏y

�✏x

✏y

✏2x

+ f2

�✏x

f �✏y

f

1

A˙✏

= Z(✏) ˙✏,

where

Z(✏)4=

1

F 3

0

@✏2y

+ f2 �✏x

✏y

�✏x

✏y

✏2x

+ f2

�✏x

f �✏y

f

1

A˙✏.


Apparent Motion, cont.

Ego Motion, !c

c/i

⇥ ˇ`c

:

!c

c/i

= !c

c/g|{z}0

+ !c

g/b|{z}

RcgR

gb

0

BB@

� sin(↵az)↵el

cos(↵az)↵el

↵az

1

CCA

+!c

b/i|{z}0

BB@

pqr

1

CCA

.

Therefore

˙

ˇ`capp4= !c

c/i

⇥ ˇ`c

=

1

F

2

4Rc

g

Rg

b

0

@p� sin(↵az)↵el

q + cos(↵az)↵el

r + ↵az

1

A

3

5⇥

0

@✏x

✏y

f

1

A


Total Motion in Image Plane

dˇ`c

dti

= Z(✏) ˙✏| {z }Target Motion

+

1

F

2

4Rc

g

Rg

b

0

@p� sin(↵

az

)↵el

q + cos(↵az

)↵el

r + ↵az

1

A

3

5⇥

0

@✏x

✏y

f

1

A

| {z }Ego Motion


Time to CollisionFor all objects in the camera field of view, it is possible to compute the time

to collision to that obstacle, defined as:

The time to collision if the vehicle were to proceed along the line-of-

sight vector to the obstacle at the current closing velocity.

Therefore, for each obstacle, the time to collision is given by

tc4= �L

˙L

Impossible to compute using only a monocular camera because of scale am-

biguity.

Two techniques:

• Looming Requires target size.

• Flat earth approximation Requires height above ground.


Time to Collision - LoomingFrom similar triangles:

Sobj

L =

P ✏s

PF=

✏s

F

If Sobj

is not changing, then

˙LL =

LSobj

"✏s

F

˙F

F� ✏

s

F

#

=

F

✏s

"✏s

F

˙F

F� ✏

s

F

#

=

˙F

F� ✏

s

✏s

=

✏x

✏x

+ ✏y

✏y

F� ✏

s

✏s

,

the inverse of which is the time to collision tc

.


Time to Collision – Flat Earth

L =

h

cos'

Di↵erentiation gives

˙LL =

˙h

h+ ' tan'.

From geometry we have

cos' =

ˇìz.

Di↵erentiate and solve for ':

' = � 1

sin'

d

dtiˇìz

Therefore,

' tan' = � 1

cos'

d

dtiˇìz = � 1

ˇìz

d

dtiˇìz


Precision Landing

For precision landing, we use the guidance model

pn = Vg cos� cos �

pe = Vg sin� cos �

pd = �Vg sin �

� =

g

Vgtan�

˙� = u1

� = u2.

Define:

piMAV =

�pn, pe, pd

�>

viMAV =

�Vg cos� cos �, Vg sin� cos �, �Vg sin �

�>

vv2MAV =

�Vg, 0, 0

�>


Proportional Navigation (PN)

`

pMAV

vMAV

˙ = vobj − vMAV

vobj

pobj

aMAV

Camera-pronav

Proportional Navigation: align

the line-of-sight rate

˙` with the

negative line-of-sight vector �`.Define

⌦? =

ˇ`⇥˙`

L .

Assuming constant velocity, we can

only accelerate in the plane per-

pendicular to the velocity vector.

Therefore, to zero ⌦?, the de-

sired acceleration is

aMAV = N⌦? ⇥ vMAV,

whereN > 0 is the (tunable) nav-

igation constant.


Acceleration CommandTo implement the acceleration command, must convert to vehicle-2 frame as

av2MAV = µN⌦v2? ⇥ vv2

MAV

= µN

0

@⌦

v2?,x

⌦

v2?,y

⌦

v2?,z

1

A⇥

0

@Vg

0

0

1

A

=

0

@0

µNV ⌦

v2?,z

�µNV ⌦

v2?,y

1

A ,

where

⌦v2? = Rv2

b

Rb

g

Rg

c

⌦c

?

⌦c

? =

ˇ`c ⇥

˙`c

L˙`c

L =

˙LLˇ`c

+ Z(✏) ˙✏+ ˙

ˇ`capp

where the inverse of time to collision

˙L/L can be estimated using looming or

flat earth model.


Polar Converting Logic

Use polar converting logic to convert acceleration command av2 to roll angle

and flight path angle commands:

�c = tan�1

av2y�av2z

!

Vg �c =

q(av2y )2 + (av2z )2.

�c = tan�1

av2yav2z

!

Vg �c = �

q(av2y )2 + (av2z )2.


Polar Converting Logic, cont.General rule:

�c= tan

�1

av2y|av2z |

!

�c= �sign(av2z )

1

Vg

q(av2y )

2+ (av2z )

2

Note discontinuity in �cat (av2y , av2z ) = (0, 0). When av2z = 0, then

• When av2y > 0 ! �c= ⇡/2

• When av2y < 0 ! �c= �⇡/2

Remove discontinuity as

�c= �(av2y ) tan

�1

av2y|av2z |

!

where

�(av2y ) = sign(av2y )

1� e�kav2y

1 + e�kav2y

chapter 13uavbook.byu.edu/lib/exe/fetch.php?media=lecture:chap13.pdfbeard & mclain, “small...

Documents