Low Cost solution for Pose Estimation of Quadrotor - - PowerPoint PPT Presentation

▶
low cost solution for pose estimation of quadrotor
SMART_READER_LITE
LIVE PREVIEW

Low Cost solution for Pose Estimation of Quadrotor - - PowerPoint PPT Presentation

Apr 01, 2024 21 likes •441 views

Introduction Approach Pose Estimation using UWB sensor IIT Kanpur WiFi based Solutions Conclusion Low Cost solution for Pose Estimation of Quadrotor mangal@iitk.ac.in https://www.iitk.ac.in/aero/mangal/ Intelligent Guidance and Control

slide-1
SLIDE 1

Introduction Approach Pose Estimation using UWB sensor WiFi based Solutions Conclusion

IIT Kanpur

Low Cost solution for Pose Estimation of Quadrotor

mangal@iitk.ac.in https://www.iitk.ac.in/aero/mangal/

Intelligent Guidance and Control Laboratory Indian Institute of Technology, Kanpur

Mangal Kothari January 7, 2018 IIT Kanpur 1 / 42

slide-2
SLIDE 2

Introduction Approach Pose Estimation using UWB sensor WiFi based Solutions Conclusion

IIT Kanpur

Outline

Introduction Approach Pose Estimation using UWB sensor WiFi based Solutions Conclusion

Mangal Kothari January 7, 2018 IIT Kanpur 2 / 42

slide-3
SLIDE 3

Introduction Approach Pose Estimation using UWB sensor WiFi based Solutions Conclusion

IIT Kanpur

Introduction

  • Our goal is to make robust systems capable of Navigating in

GPS denied environments.

  • Exploring the enormous scope of Indoor Navigation

(Surveillance, Disaster Management or systems for first response).

  • System which can be used Ubiquitously overcoming

nonuniform environmental conditions.

Mangal Kothari January 7, 2018 IIT Kanpur 3 / 42

slide-4
SLIDE 4

Introduction Approach Pose Estimation using UWB sensor WiFi based Solutions Conclusion

IIT Kanpur

Introduction

Why No to GPS!!

  • GPS signal are highly dependent on the operating conditions.

Localization

  • The major milestone for autonomous navigation is localization.
  • Recently, SLAM based techniques are showing promising

results.

  • Our major focus is on localization working on range based

sensors like UWB, Wi-Fi and augment with IMU (accelerometer, gyroscope and magnetometer) and optical flow camera.

Mangal Kothari January 7, 2018 IIT Kanpur 4 / 42

slide-5
SLIDE 5

Introduction Approach Pose Estimation using UWB sensor WiFi based Solutions Conclusion

IIT Kanpur

Previous approaches

Attitude estimation

  • Estimation of IMU and MARG orientation using a gradient

descent algorithm (Madgwik, 2011).

  • Experimental comparison of sensor fusion algorithms for

attitude estimation (Cavallo, 2014).

SLAM approaches

  • ORB-SLAM: a versatile and accurate monocular SLAM

system (Mur-Artal, 2015).

  • Towards a navigation system for autonomous indoor flying

(Grzonka, 2009). A laser based SLAM approach.

Mangal Kothari January 7, 2018 IIT Kanpur 5 / 42

slide-6
SLIDE 6

Introduction Approach Pose Estimation using UWB sensor WiFi based Solutions Conclusion

IIT Kanpur

Previous approaches

Challenges

  • Vision and Lidar SLAM approaches require sensor with heavy

payload and are computationally inefficient.

  • The attitude estimation approaches are computationally

efficient citing the usage of micro-controllers, but loses accuracy.

Our approach

  • We make use of on-board computers along with bringing

down the computation involved in SLAM processes and assigning more computation to attitude estimation.

Mangal Kothari January 7, 2018 IIT Kanpur 6 / 42

slide-7
SLIDE 7

Introduction Approach Pose Estimation using UWB sensor WiFi based Solutions Conclusion

IIT Kanpur

Our Approach

Why range based solutions (UWB sensors)

  • Payload efficient: requires just 25-30gm of additional payload.
  • Processing efficient: SLAM based solutions require higher

computational cost which in process requires powerful and heavy processors.

  • Cost efficient: These solutions are cheaper. Wifi systems are

becoming common to lots of Places.

Mangal Kothari January 7, 2018 IIT Kanpur 7 / 42

slide-8
SLIDE 8

Introduction Approach Pose Estimation using UWB sensor WiFi based Solutions Conclusion

IIT Kanpur

Pose Estimation using UWB sensor

  • An EKF based solution to estimate the position and attitude
  • f the system.
  • Uses gyroscope, accelerometer and magnetometer data for

quaternion estimation.

  • Fusion of Sonar with accelerometer for height estimation.
  • Fusion of velocity from optical flow camera with the

accelerometer data for position estimation.

  • A SLAM based approach for the UWB sensor position

estimation and simultaneously correcting for system’s position.

Mangal Kothari January 7, 2018 IIT Kanpur 8 / 42

slide-9
SLIDE 9

Introduction Approach Pose Estimation using UWB sensor WiFi based Solutions Conclusion

IIT Kanpur

Quaternion Estimates

  • Gyroscopic data is main input in the prediction step of the

Kalman fusion process for acquiring quaternion.

  • Gyroscopic data suffers from bias and an integrating solution

can thus result in erroneous output in long run.

  • Assuming that the accelerometer data in the body frame

when operated by the predicted quaternion will result in gravity vector.

  • Thus the accelerometer serve as measurement correction.

Mangal Kothari January 7, 2018 IIT Kanpur 9 / 42

slide-10
SLIDE 10

Introduction Approach Pose Estimation using UWB sensor WiFi based Solutions Conclusion

IIT Kanpur

Quaternion Estimates

Prediction Step for Quaternion

Sω = [0 ωx ωy ωz] , ˙ q = 1 2 q ⊗ Sω ˙ qω,t = 1 2 qω,t−1 ⊗ Sω, qω,t = qω,t−1 + ˙ qω,t∆t

Accelerometer Update

Eg = [0 1], Ba = [0 ax ay az] Ba = q∗

ω,t ⊗ E b g ⊗ qω,t

ea = z − ˆ za =   ax − 2(q1q3 − q0q2) ay − 2(q0q1 + q2q3) az − 2( 1

2 − q2 1 − q2 2)

 

Mangal Kothari January 7, 2018 IIT Kanpur 10 / 42

slide-11
SLIDE 11

Introduction Approach Pose Estimation using UWB sensor WiFi based Solutions Conclusion

IIT Kanpur

Quaternion Estimates

Accelerometer transformation

ab =   cosθcosψ cosθsinψ −sinθ −cosφsinψ + sinφsinθcosψ cosθcosψ + sinφsinθsinψ sinφcosθ sinφsinψ + cosφsinθcosψ −sinφcosψ + cosφsinθsinψ cosφcosθ     1  

Magnetometer Update

  • The accelerometer however cannot correct for the yaw motion

as the rotation about yaw parallels the gravity direction.

  • Based on the magnetic field of the earth we can find the north

direction.

  • Our approach uses a Magnetic distortion compensation model

(Madgwick’s AHRS) for the yaw estimation.

Mangal Kothari January 7, 2018 IIT Kanpur 11 / 42

slide-12
SLIDE 12

Introduction Approach Pose Estimation using UWB sensor WiFi based Solutions Conclusion

IIT Kanpur

Quaternion Estimates

Magnetometer Measurement Update

Bm = [0 mx my mz] Eh = [0 hx hy hz] = qB

E ⊗ Bm ⊗ q∗B E

Eb =

  • h2

x + h2 y

hz

  • = [0

bx bz] em = z − ˆ zm =   mx − 2bx( 1

2 − q2 2 − q2 3) + 2bz(q1q3 − q0q2)

my − 2bx(q1q2 − q0q3) + 2bz(q0q1 + q2q3) mz − 2bx(q0q2 + q1q3) + 2bz( 1

2 − q2 1 − q2 2)

 

Mangal Kothari January 7, 2018 IIT Kanpur 12 / 42

slide-13
SLIDE 13

Introduction Approach Pose Estimation using UWB sensor WiFi based Solutions Conclusion

IIT Kanpur

Quaternion Estimates EKF

State Vector and Observation Vector

νt =

  • q0

q1 q2 q3 mx my mz x y z Vx Vy Vz xd yd zd T

t

zt =

  • ax

ay az mx my mz Vx,B Vy,B hB R T

t

Measurement Update

ˆ zMARG = ˆ za ˆ zm

  • KMARG

= HMARG ˆ Σ(HMARG ˆ ΣHT

MARG + Q)

νt = ˆ νt + K(z − ˆ z) Σt = (I − KMARGHMARG)ˆ Σt

Mangal Kothari January 7, 2018 IIT Kanpur 13 / 42

slide-14
SLIDE 14

Introduction Approach Pose Estimation using UWB sensor WiFi based Solutions Conclusion

IIT Kanpur

Attitude estimates (roll)

Figure: Estimated roll

Mangal Kothari January 7, 2018 IIT Kanpur 14 / 42

slide-15
SLIDE 15

Introduction Approach Pose Estimation using UWB sensor WiFi based Solutions Conclusion

IIT Kanpur

Attitude estimates (roll)

Figure: Estimated roll

Mangal Kothari January 7, 2018 IIT Kanpur 15 / 42

slide-16
SLIDE 16

Introduction Approach Pose Estimation using UWB sensor WiFi based Solutions Conclusion

IIT Kanpur

Attitude estimates (pitch)

Figure: Estimated pitch

Mangal Kothari January 7, 2018 IIT Kanpur 16 / 42

slide-17
SLIDE 17

Introduction Approach Pose Estimation using UWB sensor WiFi based Solutions Conclusion

IIT Kanpur

Attitude estimates (pitch)

Figure: Estimated pitch

Mangal Kothari January 7, 2018 IIT Kanpur 17 / 42

slide-18
SLIDE 18

Introduction Approach Pose Estimation using UWB sensor WiFi based Solutions Conclusion

IIT Kanpur

Attitude estimates (yaw)

Figure: Estimated yaw

Mangal Kothari January 7, 2018 IIT Kanpur 18 / 42

slide-19
SLIDE 19

Introduction Approach Pose Estimation using UWB sensor WiFi based Solutions Conclusion

IIT Kanpur

Attitude estimates (yaw)

Figure: Estimated yaw

Mangal Kothari January 7, 2018 IIT Kanpur 19 / 42

slide-20
SLIDE 20

Introduction Approach Pose Estimation using UWB sensor WiFi based Solutions Conclusion

IIT Kanpur

Attitude estimates (yaw)

Figure: Estimated yaw

Mangal Kothari January 7, 2018 IIT Kanpur 20 / 42

slide-21
SLIDE 21

Introduction Approach Pose Estimation using UWB sensor WiFi based Solutions Conclusion

IIT Kanpur

Attitude estimates (yaw)

Figure: Estimated yaw

Mangal Kothari January 7, 2018 IIT Kanpur 21 / 42

slide-22
SLIDE 22

Introduction Approach Pose Estimation using UWB sensor WiFi based Solutions Conclusion

IIT Kanpur

Attitude estimates (yaw)

Figure: Estimated yaw

Mangal Kothari January 7, 2018 IIT Kanpur 22 / 42

slide-23
SLIDE 23

Introduction Approach Pose Estimation using UWB sensor WiFi based Solutions Conclusion

IIT Kanpur

Attitude estimates

  • The roll, pitch and yaw estimates approximates the ground

truth results.

  • The roll and pitch estimates show better results as compared

to Madgwick’s AHRS.

  • The convergence of yaw estimates are fast as compared to

pixhawk’s EKF.

  • The magnetic distortion compensation does not require user

predefined direction.

Mangal Kothari January 7, 2018 IIT Kanpur 23 / 42

slide-24
SLIDE 24

Introduction Approach Pose Estimation using UWB sensor WiFi based Solutions Conclusion

IIT Kanpur

Position Estimation

  • Fusion of accelerometer data with the raw velocity

measurement from optical flow camera.

  • All vision based solution suffer from drift and in the long run

diverges from ground truth results.

  • However, for short duration flights result accuracy matches

vision based ORB SLAM solution.

Mangal Kothari January 7, 2018 IIT Kanpur 24 / 42

slide-25
SLIDE 25

Introduction Approach Pose Estimation using UWB sensor WiFi based Solutions Conclusion

IIT Kanpur

Position Estimation

  • Fusing the Sonar data and the accelerometer data along with

quaternion operations to account for non linearity.

  • Sonar data is precise with an accuracy of ± 5cm but suffers

from irregularities.

  • High dependence on sonar can lead to noisy and inaccurate

estimates of height.

  • We pass the sonar raw estimates through a median filter,

which sorts out the outlier values.

Mangal Kothari January 7, 2018 IIT Kanpur 25 / 42

slide-26
SLIDE 26

Introduction Approach Pose Estimation using UWB sensor WiFi based Solutions Conclusion

IIT Kanpur

Position Estimation

Prediction Update

ˆ xt = xt−1 + Vt−1∆t ˆ Vt = Vt−1 + (RE

B a − [0, 0, g]T)∆t

Measurement Update

ˆ zPX4 =    ˆ Vx,B ˆ Vy,B

ˆ ν(10)t (q2

0+q2 3−q2 1−q2 2)

   KPX4 = HPX4 ˆ Σ(HPX4 ˆ ΣHT

PX4 + Q)

νt = ˆ νt + KPX4(zPX4 − ˆ zPX4) Σt = (I − KPX4HPX4)ˆ Σt

Mangal Kothari January 7, 2018 IIT Kanpur 26 / 42

slide-27
SLIDE 27

Introduction Approach Pose Estimation using UWB sensor WiFi based Solutions Conclusion

IIT Kanpur

Height Estimation

Figure: Estimated Height

Mangal Kothari January 7, 2018 IIT Kanpur 27 / 42

slide-28
SLIDE 28

Introduction Approach Pose Estimation using UWB sensor WiFi based Solutions Conclusion

IIT Kanpur

Height Estimation

Figure: Estimated Height

Mangal Kothari January 7, 2018 IIT Kanpur 28 / 42

slide-29
SLIDE 29

Introduction Approach Pose Estimation using UWB sensor WiFi based Solutions Conclusion

IIT Kanpur

Height Estimation

Figure: Estimated Height

Mangal Kothari January 7, 2018 IIT Kanpur 29 / 42

slide-30
SLIDE 30

Introduction Approach Pose Estimation using UWB sensor WiFi based Solutions Conclusion

IIT Kanpur

Position Estimation

Figure: Estimated Position Only px4flow vs ORB SLAM

Mangal Kothari January 7, 2018 IIT Kanpur 30 / 42

slide-31
SLIDE 31

Introduction Approach Pose Estimation using UWB sensor WiFi based Solutions Conclusion

IIT Kanpur

Range Only SLAM

  • Range only data does not allow the other UWB sensor to be

localized until we have accurate estimate of system position.

  • Our approach make use of velocity-accelerometer fusion for

initial measurements.

  • Once the system is able to localize the UWB sensor the weight
  • n the estimates from the UWB sensor is given more weight.

Mangal Kothari January 7, 2018 IIT Kanpur 31 / 42

slide-32
SLIDE 32

Introduction Approach Pose Estimation using UWB sensor WiFi based Solutions Conclusion

IIT Kanpur

Range Only SLAM

ˆ zD =

  • (ˆ

vt(8) − ˆ vt(14))2 + (ˆ vt(9) − ˆ vt(15))2 + (ˆ vt(10) − ˆ vt(16))2 KD = HD ˆ Σ(HD ˆ ΣHT

D + Q)

νt = ˆ νt + KD(zD − ˆ zD) Σt = (I − KDHD)ˆ Σt

Mangal Kothari January 7, 2018 IIT Kanpur 32 / 42

slide-33
SLIDE 33

Introduction Approach Pose Estimation using UWB sensor WiFi based Solutions Conclusion

IIT Kanpur

Position Estimation

Figure: Position Estimate

Mangal Kothari January 7, 2018 IIT Kanpur 33 / 42

slide-34
SLIDE 34

Introduction Approach Pose Estimation using UWB sensor WiFi based Solutions Conclusion

IIT Kanpur

Position Estimation

Figure: Position Estimate

Mangal Kothari January 7, 2018 IIT Kanpur 34 / 42

slide-35
SLIDE 35

Introduction Approach Pose Estimation using UWB sensor WiFi based Solutions Conclusion

IIT Kanpur

Position Estimation

5 −4 −2 2 4 6 8 10 −3 −2 −1 1 2 3 Y (m) X (m) Z (m) Ground Truth for Quadrotor External UWB range sensor pose External UWB sensor true location EKF

Figure: Position Estimate

Mangal Kothari January 7, 2018 IIT Kanpur 35 / 42

slide-36
SLIDE 36

Introduction Approach Pose Estimation using UWB sensor WiFi based Solutions Conclusion

IIT Kanpur

Wifi Triangulation for Localization

  • Better initialization of router position leads to better accuracy

in position estimates.

  • First interval involves data gathering and applying least

squares to estimate router positions.

  • The estimate router position serve as an initial guess to the

EKF.

Mangal Kothari January 7, 2018 IIT Kanpur 36 / 42

slide-37
SLIDE 37

Introduction Approach Pose Estimation using UWB sensor WiFi based Solutions Conclusion

IIT Kanpur

Figure: EKF Localization

Mangal Kothari January 7, 2018 IIT Kanpur 37 / 42

slide-38
SLIDE 38

Introduction Approach Pose Estimation using UWB sensor WiFi based Solutions Conclusion

IIT Kanpur

Figure: EKF SLAM

Mangal Kothari January 7, 2018 IIT Kanpur 38 / 42

slide-39
SLIDE 39

Introduction Approach Pose Estimation using UWB sensor WiFi based Solutions Conclusion

IIT Kanpur

WiFi RSSI Fingerprinting

  • A pre-calibration is done to extract a fingerprint of the RSSI

signal.

  • Based on the distribution we extract the position estimates.
  • KNN and WKNN methods are used applying discrete or

guassian distribution.

Mangal Kothari January 7, 2018 IIT Kanpur 39 / 42

slide-40
SLIDE 40

Introduction Approach Pose Estimation using UWB sensor WiFi based Solutions Conclusion

IIT Kanpur

Figure: Data Gathering

Mangal Kothari January 7, 2018 IIT Kanpur 40 / 42

slide-41
SLIDE 41

Introduction Approach Pose Estimation using UWB sensor WiFi based Solutions Conclusion

IIT Kanpur

Figure: EKF Localization

Mangal Kothari January 7, 2018 IIT Kanpur 41 / 42

slide-42
SLIDE 42

Introduction Approach Pose Estimation using UWB sensor WiFi based Solutions Conclusion

IIT Kanpur

Conclusion

  • We presented solutions which do not require high

computation cost.

  • The presented sensor solutions are light weight allowing UAVs

to have higher payload.

  • The performance of the solution performs comparable to the

state of the art techniques.

Mangal Kothari January 7, 2018 IIT Kanpur 42 / 42