3D Motion Tracking by Inertial and Magnetic sensors with or without GPS

Transcription

1 3D Motion Tracking by Inertial and Magnetic sensors with or without GPS Junping Cai M.Sc. E. E, PhD Centre for Product Development (CPD) Mads Clausen Institute (MCI) University of Southern Denmark(SDU)

2 Outline Short introduction of Junping Cai Coordinate Systems Inertial Motion Tracking Working Principle On-site Calibration Extended Kalman Filter without GPS Extended Kalman Filter with GPS Experiment results Application examples Discussions

3 Short introduction of Junping Come from China , B.Sc., Automatic Control, Department of Automatic Control, Nanjing University of Aeronautics and Astronautics, China , Production Engineer, Canon (Tianjin) Ltd., China 996-, Purchasing Engineer, Danfoss (Tianjin) Ltd., China -4, M.Sc., Mechatronics Engineering, Mads Clausen Institute (MCI),University of Southern Denmark (SDU), Denmark 4-7, PhD Student, Automation & Control, Department of Electronic Systems, Aalborg University, Denmark, Supervisors: Professor Jakob Stoustrup 8 9, Lead Development Engineer, Automatic Controls Laboratories, Danfoss A/S, Denmark 9 now, R & D Engineer, Centre for Product Development (CPD),MCI,SDU

4 Coordinate systems Earth-Centered Earth-Fixed Frame (ECEF, e-frame) Inertial Frame (i-frame) Local Geodetic Frame (t-frame) Local Tangent Plane (LTP) (N-frame, global) North- East- Down (NED) East- North- Up (ENU) Body Frame (b-frame,local) Forward Right Down Relations between ECEF-frame (e), local geodetic-frame (t) and inertial-frame (i).

5 Coordinate systems World coordinates WGS84 and LTP Earth Centered Earth Fixed ECEF WGS 84 parameters: a=6,378,37 m b=6,356,75 m Spherical coordinates - LLA Longitude Latitude h Altitude Earth-Centered Earth Fixed (ECEF) Coordinate System Definition of Ellipsoidal Coordinates (Latitude, Longitude, Altitude) in WGS-84 Ellipsoid

6 Inertial motion tracking working principle 3-axis Gyro Angular Rate 3 -axis Accelerometer Acceleration Integral Projection to global Orientation Correct gravity Initial Linear Velocity Acceleration Integral Velocity Initial Position Integral Position Gyroscope Accelerometer ω a x x ω a y y ω a z z Euler Angles Quaternion Rotation vector Coordinate Transformation local system The Devil is in the details Up North East Global system Velocity V Position P X X V Y P Y V P Orientation Z Z

7 Quaternion attitude representation-for example ) ( ) ( )) ( ( ) ( ), ( ) ( ) ( ) ( Quaternion update ) ( ) ( to Local) Transformation matrix (Global Quaternion 4 4 ) ( ) ( ) ( z y x z x y y x z x y z k L G t k L G k k k L G t L G t L G t L G T L G t q t q t t t q q q t q qq q q I q q C q k q j q q i q C(,)) C(,), ( atan Yaw C(,3)) asin(- Pitch C(3,3)) C(,3), ( atan Roll gravity - g position velocity a on accelerati linear ) ( L G L G L G L G L G linear tf t tf t linear ENU z y x L G U N E Vdt P P dt a V V g a a a a q C a a a Transformation

8 Calibration Why: Low cost Suffer from the time drift Sensitivity to the environmental parameters Purpose is to in-field determine : The Bias The Scale factor (Gain) The Orientation (misalignment) Requirement Time Complexity Instruments... Sensors case 3-axis Acc 3-axis Gyro 3-axis Mag Support base

9 Orientation -Misalignment Misalignment: Nominal sensitivity axis vs. Actual sensitivity axis r Actual sensitivity axis r Unit vector r, r, r r r r r r r r r r r r r r cos sin cos sin sin

10 8 6 4 Accelerometer calibration example a x a y a z a x,f a y,f a z,f 9,6 Ka 98,8 93, Data sheet specification [min typ max] [7 3 33] a x a y x 4 Raw and filtered data.5 a z a tot Ra,9997,49,5,5,9999,5,5,98,9996 Ba,4985,588, Calibrated 6 8 data 4 x 4

11 On-field fast calibration -Magnetometers Method I: Using Earth Magnetic Field as Known Input, Publication ION Method II: 3D Ellipsoid Curve Fitting Method III: D Mapping (boat & car) Magnetic declination is the angle between magnetic north (the direction the north end of a compass needle points) and true north(north pole). The declination is positive when the magnetic north is east of true north. Alsion for example (WMM ) Latitude = 54.9; % Degree N Longitude = 9.78; % Degree E Altitude =.; % Km Date =.; F = e-5; % nt Gauss H = 753.6e-5; % nt Gauss X = 756.3e-5; % nt Gauss Y = 55.e-5; % nt Gauss Z = e-5; % nt Gauss Decl = + 39/6; % (Degree East) Incl = 69+ 5/6; % (Down) (Dip)

12 On-field fast calibration -Magnetometers Method III : 3D mapping F h 555 x b k x Magnetic field raw data 56 x hy b k y y 53 h z b k z Magnetic field calibrated data z Requirement: Many different orientations as possible. e.g. keep the object still for a few seconds in at least significantly different orientations, preferably more At least 3 meters from large ferromagnetic objects such as radiators and iron desks

13 On-field fast calibration -Magnetometers D mapping example Magnetic Field Raw Data Magnetic[Gauss] B R B P Magnetic [Gauss].5 Magnetic Field Calibrated Data B Y B tot B tot,t [Gauss].. -. Horizontal Projection before after true Time [s] [Gauss] Vertical Projection [Gauss] time [s]

14 On-field calibration No special instrument is needed No special training is needed No strict sensor alignment (when mounting) is need First we do the factory calibration Then user do the on-site (on-use) calibration. User need to stand still (if mounted) or hold the sensor set still for a few seconds. User turns around 36 degree (if mounted) or rotate the sensor set in space Automatic and fast!

15 Extended Kalman Filter without GPS Event detection and Constraints Closed loop Error Estimates IMU Navigation Processor Position Velocity Attitude Event detector Error estimates Kalman Filter Constraints N-Navigation coordinate (NED) b- sensor body coordinate Magnetometer

16 Extended Kalman Filter with GPS Loosely coupled integration strategy Closed loop Error Estimates IMU Navigation Processor Position Velocity Attitude Error estimates GPS Kalman Filter Constraints N-Navigation coordinate (NED) b- sensor body coordinate Magnetometer

17 Experiments (indoor) Position Forward [m] Right[m] Walking a straight line Acceleration [m/s ] Acceleration [m/s ] Velocity [m/s] A R Acceleration measurement in RPY A P A Y A tot time [sec] Linear Acceleration in NED A N A E A D 3 time [sec] Velocity 5 V N V E V D 3 time [sec]

18 Indoor pedestrian tracking

19 Experiment (outdoor) Landyacht with GPS Alsion parking lot

20 Experiment Results 5 Roll angle [degree] Pitch angle [degree] Yaw angle [degree] - 5 5

21 Experiment Results Linear Acceleration.5 Acceleration [m/s ].5 3 Velocity in NED V N V E V D time [s] Velocity [m/s] time [s]

22 Experiment Results Position in NED P N P E 5 P D Position in NED Position [m] Position [m] time [sec] INS: Fs= Hz GPS: Fs= Hz -6 Black line -INS. Colored line-gps time [sec] Zoomed in

23 Experiment results Position and Velocity direction - P EN V cal V GPS -4 North [m] East [m]

24 What during GPS signal blockage (outage) periods The consequence of GPS outage Bridging algorithm Optimal Backward Smoothing (OBS) DBM algorithm Position NED error between INS and GPS Positioning errors in INS/GPS navigation applications 5 Position [m] 4 3 Positioning (North) errors between INS/GPS with GPS outage at 4s-5s and 3s-5s time [sec]

25 Optimal backward smoothing (OBS) algorithm Three classes of OBS algorithms fixed-interval smoother [ N] fixed-point (single-point ) smoother j k fixed-lag smoother k k m j [ k N] N Application dependent: Post-mission: the fixed-interval Near real-time: the fixed-lag Initial condition: the fixed- point Categories of OBS algorithms adapted from Nassar 3

26 DBM algorithm Acceleration error parameter during GPS outage a i ( r ( t The expected position error during the outage DBM r r i, e i, b i r r e end i, e e t i, INS, e i, INS, b r b i ) r i, b a ( t r e ) t i, GPS, e b i, GPS, b ) b begining Position [m] Position in NED time [sec] Scenario shows the effect of backward smoothing (Simulated data with manipulated GPS signal) red solid line-when GPS outage. red dotted line- GPS true position, black line -smoothed data

27 Uniqueness of the product Compact design: 7.9 x 9.5 x 4.8 mm3, gram + battery Battery (rechargeable) driven, battery time: 4 - hour SIM card data storage + USB data transfer/battery charging 5 sensors hardware and software 3D gyro, 3D accelerometer, 3D magnetometer, temperature and internal voltages. Heading, roll and pitch, velocity, position, angular velocity, acceleration. Time stamping RF data transmission /receiving Advanced batch and data processing and filtering

28 Applications Medical /Biomechanical study Rehabilitation Sport training.

29 Application example: medical Remote Monitoring of Patients With Parkinson s Disease (PD) Y 3D Acceleration 3D Angular velocity 3D (Magnetic) Kalman Filter + Translation + Pattern recognition Frequency (Hz) Amplitude (g) Power ratio (%) 6 4 Spectrogram of test signal 5 5 Time (sec) Original test signal Time (sec) 5 5 Time (sec) Hyperkinesias Normal Dystonia Slow Freezing Movement pattern of PD patients normal movements slowing of movements hyperkinesias (exaggerated abnormal movements) dystonia (abnormal tone in a limb) freezing (no movements) X

30 Remote Monitoring of PD Patients -Preliminary Experiment -Accelerometer 4 a x Acceleration [g] a y a z a tot Left Hand Time [s] 4 a x Acceleration [g] Time [s] a y a z a tot Right Hand

31 Remote Monitoring of PD Patients -Preliminary Experiment Gyroscope Angular velocity [degree/s] Angular velocity [degree/s] Time [s] Time [s] w x w y w z Left Hand w x w y w z Right Hand

32 Application example: Medical/ biomechanical study Life/KU (Copenhagen University) Detection and quantification of lameness in horses Symmetric /Asymmetric Structure and Motion Laboratory, The Royal Veterinary College, UK Royal National Orthopaedic Hospital, UK Study of locomotion Displacement data for x (craniocaudal), y (lateral) and z (dorsoventral) movement m, and orientation data, for optical motion capture (blue) and inertialsensor (red) for a series of strides at canter (9m/s) Structure and Motion laboratory, UK

33 Application example: Sport training

34 Application example: Rehabilitation/welfare

35 Application example: Rehabilitation/welfare

36 Applications: Sports/rehabilitation Depiction of the 5 segments comprising stick figure for human body Physical segment model and the definition of its orthogonal frame

37 Applications: sports/rehabilitation Relation between the measurements in segment (i) and segment (i+) T - Transpose H - magnetic field g - gravity joint angle the is i the rotation vector from segment i is ), ( ), ( i i T i z i y i x i i T i z i y i x T i z i y i x i i T i z i y i x K where H H H K Rot H H H g g g K Rot g g g The whole body movement can be calculated by a series of Translation & Rotation. There is no need for strict sensor alignment.

38 Discussions Do we need so many sensors? What are the cost of sensors What is the accuracy of the measurement?

39 Discussion Do we need so many sensors? Sensor dependent Gyroscope roll pitch yaw Accelerometer Magnetometer Gyroscope Optical (Sagnac Effect ) Ring Laser Gyroscope (RLG) Fiber Optic Gyroscope (FOG) Mechanical MEMS (Micro-Electro-Mechanical Systems) Gyroscope

40 Discussion cont. Do we need so many sensors Application dependent When the time frequency matter accelerometer only is enough Frequency (Hz) Amplitude (g) Power ratio (%) Spectrogram of test signal Time (sec) Original test signal 5 5 Time (sec) Time (sec)

41 Discussion cont. Do we need so many sensors Application dependent When the angle and linear acceleration matter: gyroscope+ accelerometer + magnetometer

42 Discussion What are the cost of sensors Accelerometers Magnetometers

43 Discussion 3 What is the accuracy of the measurement? Periodic (cyclic) movement Walking (on one plane / on multi-plane ) Running Hurdles Pole jumping (vaulting) No cyclic / Random movement (outdoors with GPS ) No cyclic / Random movement (indoors, e g upper limb movement) - biomechanical model is need

44 Summary We can fuse gyroscopes, accelerometers, magnetometers (and GPS data ) to deliver accurate and reliable motion information, and output: Quaternion/ Transformation matrix /Rotation vector Heading, pitch, and roll Linear acceleration Velocity Position Hardware Application knowledge Software Mathematic On-site calibration Data fusion Biomechanical models

45 Some inspirations: Click and play the video