Transcript Localization of the NTUA Emulator Space Robot Using a

National Technical University of Athens
Mechanical Engineering Department
Control Systems Laboratory
Localization of the NTUA Emulator Space
Robot Using a Discrete Extended Filter, Data
Fusion & Feedback Delay Compensation
Aris Kalgreadis, Iosif S. Paraskevas, Thaleia Flessa, Prof. Evangelos
Papadopoulos
1
Introduction/Motivation
On-Orbit Servicing and Active Debris Removal can benefit
by the development of space robotic systems
The NTUA space emulator was created to emulate the
operation of free-flying robotic servicers
Main research goals are to study :
 Docking Strategies
 Cooperative manipulation
 Control methodologies in the presence of flexibilities
 Robot localization
2
NTUA Emulator
Two robots that hover over a granite
table of minimum roughness. The
design includes :
• Three porous air-bearings
• Six thrusters
• A reaction wheel
• On board optical sensors
• Overhead camera
• Processing units
• Batteries
• Manipulator
3
Localization Problems
•
•
•
•
•
Non-linear kinematics
Measurement noise
Drift over time of the optical sensors
Image distortion errors
Image processing delays
Proposed Solution :
• New calibration techniques and image processing software
• A Discrete Extended Kalman Filter with :
 Fusion of measurements from sensor systems
 Compensation for delayed measurements
4
Kinematic Model
The equations describing the
kinematic model are :
é M × ax
ê
ê M × ay
ê
êë J ×q
ù é Fx
ú ê
ú = ê Fy
ú ê
úû êë M z
ù
ú
ú = R (q ) × A × u
ú
úû
Fa = F1 - F2
Simplified thruster forces :
Fb = F3 - F4
Fc = F5 - F6
Rotation matrix between CS{w}
and CS{r}
é cos (q ) - sin (q ) 0 ù
ê
ú
R (q ) = ê sin (q ) cos (q ) 0 ú
ê
ú
0
0
1
êë
úû
Simplified input vector
u = é Fa
ë
Fb
Fc
T ù
û
Transmission matrix from
thruster forces and reaction
wheel torque to Fx , Fy , M z
T
( )
( )
é sin 60 0
ê
A = ê - cos 60 0
ê
ê
-R
ë
1
( )
cos ( 60 )
-R
R
0
sin 60 0
0
0 ù
ú
0 úú
1 úû
5
Optical Sensors
• Relative localization sensors that
provide δ𝑥 and 𝛿𝑦 displacement
measurements
• Two sensors produce four values per
instant and the three unknown
parameters 𝑥, 𝑦, 𝜃 can be calculated
Main advantages :
• High sampling rate
• Compact size
• Good accuracy
• Low cost
Main disadvantage :
• Odometric error accumulated
over time
6
Camera (I)
• Absolute localization
measurements
• Position and orientation are
calculated by tracking five LEDs
on the top surface of the robot
Image processing software that :
Main disadvantages :
•
•
•
•
• Low sampling rate
• Image processing delays
• Distortion error
Identifies the LED pattern
Removes lens distortion
Find the LED coordinates
Calculates the position and orientation of
the robot
7
Camera (II)
Image processing algorithm operation principles :
1. Image threshold to isolated LED pixel coordinates  x do , y do 
2. Offset coordinates to make the center of distortion the new origin
 x di , y di    x do
 o xi , y do  o yi 
3. Remove aspect ratio by scaling the x-axis :
x di    x di
4. Calculate undistorted LED coordinates
x ui  x di   1    rdi
2
y ui  y di   1    rdi
rdi 
x di  y di
2
2


Parameters oxi ,oyi ,h,k
were calculate with a
calibration method
that required a single
image of a calibration
grid
2
8
Camera (III)
5. Transform the undistorted coordinates to 𝑚𝑚 that refer to the CS{w}
xw = é xui
ë
yw = é xui
ë
yui
yui
é ax ù
ê
ú
1 ù × ê bx ú
û
ê c ú
êë x úû
é ay ù
ê
ú
ù
1 × ê by ú
û ê
ú
êë cy úû
Since the robot movement is on a plane
the z-coordinate is constant and can be
removed from the transformation
6. With respect to the real geometry identify
LED pattern and determine the position and
orientation of the robot
Parameters ax ,bx ,cx ,ay ,by ,cy
were calculated
using linear regression algorithm with dataset
the nodes of the calibration grid
9
Discrete Extended Kalman Filter (I)
• The state vector :
x k   xk
yk
k
xk
k 
yk
T
• The control input :
u k   Fa , k
Fb , k
Fc , k
T k 
T
• Non-linear kinematic model :
x k  f k 1 ( x k 1 , u k 1 )  w k
wk
N  0, Q k

10
Discrete Extended Kalman Filter (II)
• Measurement Vector :
z k   xk
• Measurement Model :
z k  H  x k  vk
where :
1

H  0

 0
0
0
0
k 
yk
(i )
0
1
0
0
0
0
1
0
0
0

0

0 
(i)
vk
T
(i)
N  0, R k
(i)

Time Update Equations
Initial state estimate :
xˆ k (  )  f k 1 ( xˆ k 1 (  ), u k 1 )
associated covariance: P (  ) 
k
Ak 1  Pk 1 (  )  Ak 1  Q k 1
T
where :
Ak 1 
f ( x, u )
x
x  xˆ k  1 (  )
11
Measurements Handling
Operation frequency of sensor systems :
• Camera has a frequency of 7 Hz but the receive data
frequency is lowered to 4 Hz due to processing
delays
• Optical Sensors have a frequency of 125 Hz
The DEKF frequency is set equal to the frequency of the
Optical Sensors.
Two states of filter operation can be distinguished :
• At time 𝑘 only a measurement from optical
sensors is available
• At time 𝑘 ′ measurements from both sensors are
available
12
Measurement Update
Measurement update equations when only optical sensors
measurement is available :
  H  Pk (  )  H
Kalman gain :
K k  Pk (  )  H
Updated state estimate :
x k ( )  x k ()  K k   zk
Update state covariance:
Pk (  )   I  K k  H   Pk (  )
T
(o)
T
 Rk
(o)

1
 H  xˆ k (  ) 
13
Sensor Fusion
Measurement update when measurements from both sensor
systems are available :
Centralized Fusion Scheme
Fused measurement model :
( c ) 1
( o ) 1
R k    R k 
 R k  
1
( c ) 1
( o ) 1
H k   R k    R k   H  R k   H 
( c ) 1
(c)
( o ) 1
(o)
z k   R k    R k   z k   R k   z k  
14
Delay Compensation (I)
Camera measurements are delayed due to :
1. Image processing algorithm
2. Wireless communication time between
external computer and robot
Delay time is approximately
0.3𝑠 that corresponds to 38
filter time steps.
Camera measurement :
 taken at time 𝑠
 available at a later time
𝑘 due to the time delay
Basic assumptions :
• The delay period 𝑁 = 𝑘 − 𝑠 is known and constant
• The measurement variance R s( c )* of the delayed measurement is
known at time step s
15
Delay Compensation (II)
Two parallel filter are created :
 The main filter is a simple EKF filter that uses only measurements from
optical sensors
 The parallel filter is a fusion filter that works only in the period N of the time
delay
At the parallel filter :
• At time s only the fused measurement covariance matrix H k  is
updated
( c )*
• At time k the delayed measurement z k
is fused and the
following quantity is added to the state estimate :
 xˆ k  M *  K s   z
( c )*
k
 H xˆ s 
N 1
where
M* 
 (I  K 
k i
i0
 H k  i )   k  i 1
16
Simulations
To test the validity of the proposed DEKF a Simulink/Matlab model was
created with the following features :
• Filter and Optical sensor frequency : Ts = 0.008s
• Camera frequency : Tcam = 0.304s
• Delay time : 0.3sec
Measurement noise variance :
Measurement variance matrix :
sv
sv
k
k
(c)
= [0.001m 0.001m 0.0175rad]T
(o)
= [0.005m 0.005m 0.0349rad]T
Rk(c) = diag(s v ( c ) 2 )
Rk(o) = diag(s v ( o ) 2 )
(
)
k
Process variance matrix :
Initial conditions :
k
Q = diag éë 1 1 1.75 1 1 1.75 ùû ×10-6
x 0 = éë 0 0 0.7854 0 0 0 ùû
P0 = 0.5 × I
T
17
Simulation Results (I)
18
Simulation Results (II)
19
Experiments
• The robot executed and open loop trajectory with thrusting while the
camera and the optical sensors where tracking the location of the
robot
• The robot was also tracked by a PhaseSpace mocap for evaluation of
the estimated location accuracy
•
The measurement files were imported to the Simulink model that
was modified in the following way :
 Filter and Optical sensor frequency : T s  0.004 s
 Camera frequency : Tcam  0.2640 s
 Delay time : 0.2640 sec
 Initial conditions :
x 0 = éë 0.8731 0.5821 1.7704 0 0 0 ùû
P0 = 0.5 × I
T
20
Experimental Results (I)
21
Experimental Results (II)
22
Summary
This study addressed the problem of fusing data from two different
sensors ( relative & absolute )
The main topics covered :
•
The hardware subsystems of the NTUA Space Emulator were
presented with emphasis on the localization subsystems
•
The procedure to reduce camera distortion was analyzed
•
The theoretical development of a DEKF with data fusion and
time delay was shown
•
Simulation and experimental results have shown its validity
23
For more information
Website :
http://csl-ep.mech.ntua.gr
Contact :
Prof. Evangelos Papadopoulos
[email protected]
Aris Kalgreadis
[email protected]
Iosif S. Paraskevas
[email protected]
Thaleia Flessa
[email protected]
24