VISION-BASED TRACKING OF A MOVING OBJECT BY A 2 DOF

HELICOPTER MODEL: THE SIMULATION

Chayatat Ratanasawanya

October 30, 2009

OVERVIEW

Classification of visual-servo systems Components of the simulation

The non-linear model of the helicopter The LQR controller Perspective projection model & camera

calibration Coordinate systems Determination of the position of the ball

The simulation: logic and result Summary Questions/comments

VISUAL-SERVO SYSTEMS TAXONOMY

In 1980, Sanderson and Weiss introduced a taxonomy of visual servo systems. Two questions:

1. Is the control structure hierarchical, with the vision system providing set-points as input to the robot’s joint-level controller, or does the visual controller directly compute the joint-level inputs?

2. Is the error signal defined in 3D (task space) coordinates or directly in terms of image features?

4 SYSTEM STRUCTURES

Dynamic position-based look-and-move structure

Dynamic image-based look-and-move structure

4 SYSTEM STRUCTURES

Position-based visual servo structure

Image-based visual servo structure

THE SIMULATION - AN INTRODUCTION

The system being simulated can be categorized as a dynamic position-based look-and-move system. The non-linear model of the helicopter and the joint-

level LQR controller (implemented by Quanser). Perspective projection model & camera calibration. Coordinate systems Determination of the ball’s position in world frame

& in camera frame.

THE NON-LINEAR MODELOF THE HELICOPTER

A block in Simulink provided by Quanser, which captures the dynamic equations of the helicopter plant.

Quanser 2DOF Helicopter: Closed-loop simulation

Scopes

X_des

X_sim

Vm_sim

LQR = 1LQR+I = 2

Pitch open -loop = 3Yaw open -loop = 4

2

Desired Voltage

u_ol (V)

Desired Anglefrom Program

x_d (rad)

2 DOF Helicopter -Closed -loop System Simulation

X_sim_prev

x_d (rad)

u_ol (V)

control switch

X_sim

Vm _sim (V)

4

4

4

2

2

2

2

2

2

Vm_sim (V)

2

X_sim

1

LQR+ITerminator

LQRTerminator

Controller Switch

switch

u_ff _p (V)

u_lqr_p (V)

u_lqr_y (V)

u_lqr_i_p (V)

u_lqr_i_y (V)

u_ol (V)

u_pitch (V)

u_yaw (V)

2DOF HELI :Nonlinear Model

u_pitch (V)

u_yaw (V)

X_sim

V_m_sim (V)2DOF HELI :

FF+LQR+I Controller

X_d (rad)

X

reset int

u_ff _pitch (V)

u_lqr_i_pitch (V)

u_lqr_i_yaw (V)

error: theta (rad)

error: psi (rad)

2DOF HELI :FF+LQR Controller

X_d (rad)

X

u_ff _pitch (V)

u_lqr_pitch (V)

u_lqr_yaw (V)

error: theta (rad)

error: psi (rad)

control switch

4

u_ol (V)

3

x_d (rad )2

X_sim_prev1

4

2

2{2}

3{3}

4

44

2

22

2

THE LQR CONTROLLER

A controller design technique that works with the state-space representation of a plant/system.

with weighting matrices Q and R, calculate

Has the same action as a PD or a PID controller. In the simulation, it is a joint-level controller.

Quanser 2DOF Helicopter: Closed-loop simulation

Scopes

X_des

X_sim

Vm_sim

LQR = 1LQR+I = 2

Pitch open -loop = 3Yaw open -loop = 4

2

Desired Voltage

u_ol (V)

Desired Anglefrom Program

x_d (rad)

2 DOF Helicopter -Closed -loop System Simulation

X_sim_prev

x_d (rad)

u_ol (V)

control switch

X_sim

Vm _sim (V)

4

4

4

2

2

2

2

2

2

Vm_sim (V)

2

X_sim

1

LQR+ITerminator

LQRTerminator

Controller Switch

switch

u_ff _p (V)

u_lqr_p (V)

u_lqr_y (V)

u_lqr_i_p (V)

u_lqr_i_y (V)

u_ol (V)

u_pitch (V)

u_yaw (V)

2DOF HELI :Nonlinear Model

u_pitch (V)

u_yaw (V)

X_sim

V_m_sim (V)2DOF HELI :

FF+LQR+I Controller

X_d (rad)

X

reset int

u_ff _pitch (V)

u_lqr_i_pitch (V)

u_lqr_i_yaw (V)

error: theta (rad)

error: psi (rad)

2DOF HELI :FF+LQR Controller

X_d (rad)

X

u_ff _pitch (V)

u_lqr_pitch (V)

u_lqr_yaw (V)

error: theta (rad)

error: psi (rad)

control switch

4

u_ol (V)

3

x_d (rad )2

X_sim_prev1

4

2

2{2}

3{3}

4

44

2

22

2

DuCxy

BuAxx

kxu

PERSPECTIVE PROJECTION MODEL &CAMERA CALIBRATION

The projection model is used to relate the position of an object in the camera frame to the pixel coordinate of the image of that object on the image plane.

1

/

/

1cc

cc

p

p

zy

zx

KKy

x

c

c

c

z

y

x

r2

11

/

/1

p

p

cc

cc

y

x

KKzy

zx

100

)2()2(0

)1(0)1(

ccf

ccf

KK c

c

COORDINATE SYSTEMS

The world frame: stationary frame attached to the pivot point.

The helicopter frame: attached to the helicopter at the pivot point. It moves with the helicopter.

The camera frame: attached to the camera at the center of projection.

x

yz

x

y

zx

y

z

DETERMINATION OF THE BALL’S POSITION:THE SCENARIO

Initially a ping-pong ball is right in front of the camera.

r1

r2

r3

)( 213 rrRr ccc

ww

)0

0

(

013

00

00000

00000

c

cw

z

r

cs

scccs

sscsc

r

DETERMINATION OF THE BALL’S POSITION:THE SCENARIO

The ball is moved to a new position. The helicopter hasn’t moved yet.

r1r2n

r3n

)( 213 ncc

cw

nw rrRr

)(

013

00

00000

00000

cn

cn

cnc

nw

z

y

x

r

cs

scccs

sscsc

r

DETERMINATION OF THE BALL’S POSITION:THE SCENARIO

The helicopter moves to the new position to align the ball to the camera.

)( 213 sscc

cw

nw rrRr

r3n

r1r2ss

)0

0

(

013

css

cn

w

Z

r

cs

scccs

sscsc

r

dd

ddddd

ddddd

DETERMINATION OF THE BALL’S POSITION:RECAP

The ball is initially right in front of the camera. We know the pose of the helicopter (0 and 0).

The ball is moved. Get new ball position in the camera frame from inverse projection model. Use the current pose to calculate new ball position in the world frame.

Use ball position in the world frame from the previous step to calculate the desired pose (d and d).

Pass these values to the LQR controller.

PUTTING IT ALL TOGETHER:THE SIMULATION

X=[theta ;psi]

Yp limits

Xp limits

Video Viewer

VideoViewer

ImageImageImage

Terminator

Switch 1

Scopes

X_des

X_sim

Vm_sim

Raise flag when ball moves

Ball in image Flag

Projection model

cr2 [Xp; Yp]

LQR = 1LQR+I = 2

Pitch open -loop = 3Yaw open -loop = 4

2

Inverser projectionmodel

[Xp, Yp; Zc] cr2

Image From Workspace

view 320 Image

Draw Markers

Draw markers(X-mark)

Image

Pts

Determine desired angle

New ball in world

Cam position

Flag

x_d(rad)

Determine ball positionin world frame

Theta (Rad)

Psi (Rad)

cr2

Cam position

Ball in world

Determin ball position in camera frame

Theta (Rad)

Psi (Rad)

Ball in world

Cam position

cr2

Camera position expressed in helicopter frame

-4.45 5.527.35

Ball position in image[Xp ; Yp; Zc]

cc(1)cc(2) 100

2 DOF Helicopter -Closed -loop System Simulation

X_sim_prev

x_d (rad)

u_ol (V)

control switch

X_sim

Vm _sim (V)

IMPLEMENTATION

The first step towards implementation has been taken; i.e. locating the ball’s centre of gravity in real time.

frame rate

30

d/dt (Centroid [u;v])

To Video Display

To VideoDisplay

R

G

B

Thresholding

> 0.95

Memory

From Video Device

QuickCam Pro ...RGB 24_320 x240

input 1

R

G

B

Frame RateDisplay

33 .2854

Draw Markers

Draw markers(Star )

R

G

B

Pts

R

G

B

Centroid [u;v]Blob Analysis

BlobAnalysis

BW CentroidCentroidCentroidCentroidCentroidCentroidCentroidCentroidCentroidCentroidCentroidCentroid

Add

SUMMARY

Visual-servo systems taxonomy Components of the simulation

The non-linear dynamic model The controller Projection model Coordinate systems Locating the ball

The simulation First step towards implementation

QUESTIONS/COMMENTS

7th annual UVS Canada conference 2009

Victoria, BC. November 2-5, 2009