National Technical University of Athens Mechanical Engineering Department

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 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 : Simplified thruster forces : Rotation matrix between CS{w} Simplified input vector and CS{r} Transmission matrix from thruster forces and reaction wheel torque to 5

Optical Sensors 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 2. Offset coordinates to make the center of distortion the new origin 3. Remove aspect ratio by scaling the x-axis : 4. Calculate undistorted LED coordinates Parameters were calculate with a calibration method that required a single image of a calibration grid 8

Camera (III) 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 were calculated using linear regression algorithm with dataset the nodes of the calibration grid 9

Discrete Extended Kalman Filter (I) • The state vector : • The control input : • Non-linear kinematic model : 10

Discrete Extended Kalman Filter (II) • Measurement Vector : • Measurement Model : where : Time Update Equations Initial state estimate : where : associated covariance: 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 12

Measurement Update Measurement update equations when only optical sensors measurement is available : Kalman gain : Updated state estimate : Update state covariance: 13

Sensor Fusion Measurement update when measurements from both sensor systems are available : Centralized Fusion Scheme Fused measurement model : 14

Delay Compensation (I) Camera measurements are delayed due to : 1. Image processing algorithm 2. Wireless communication time between external computer and robot 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 is updated • At time k the delayed measurement is fused and the following quantity is added to the state estimate : where 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 : • Camera frequency : • Delay time : Measurement noise variance : Measurement variance matrix : Process variance matrix : Initial conditions : 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 Phase. Space 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 : § Camera frequency : § Delay time : § Initial conditions : 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 egpapado@central. ntua. gr Aris Kalgreadis akalgre@gmail. com Iosif S. Paraskevas isparas@mail. ntua. gr Thaleia Flessa tflessa@mail. ntua. gr 24