v ision -b ased t racking of a m oving o bject by a 2 dof h elicopter m odel : t he s imulation...
Post on 21-Jan-2016
216 Views
Preview:
TRANSCRIPT
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
top related