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

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 Laboratory Indian Institute of Technology, Kanpur Mangal Kothari January 7, 2018 IIT Kanpur 1 / 42

Introduction Approach Pose Estimation using UWB sensor IIT Kanpur WiFi based Solutions Conclusion Outline Introduction Approach Pose Estimation using UWB sensor WiFi based Solutions Conclusion Mangal Kothari January 7, 2018 IIT Kanpur 2 / 42

Introduction Approach Pose Estimation using UWB sensor IIT Kanpur WiFi based Solutions Conclusion 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

Introduction Approach Pose Estimation using UWB sensor IIT Kanpur WiFi based Solutions Conclusion 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

Introduction Approach Pose Estimation using UWB sensor IIT Kanpur WiFi based Solutions Conclusion 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

Introduction Approach Pose Estimation using UWB sensor IIT Kanpur WiFi based Solutions Conclusion 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

Introduction Approach Pose Estimation using UWB sensor IIT Kanpur WiFi based Solutions Conclusion 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

Introduction Approach Pose Estimation using UWB sensor IIT Kanpur WiFi based Solutions Conclusion Pose Estimation using UWB sensor • An EKF based solution to estimate the position and attitude of 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

Introduction Approach Pose Estimation using UWB sensor IIT Kanpur WiFi based Solutions Conclusion 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

Introduction Approach Pose Estimation using UWB sensor IIT Kanpur WiFi based Solutions Conclusion Quaternion Estimates Prediction Step for Quaternion q = 1 S ω = [0 ω x ω y ω z ] , ˙ 2 q ⊗ S ω q ω, t = 1 ˙ q ω, t = q ω, t − 1 + ˙ q ω, t ∆ t 2 q ω, t − 1 ⊗ S ω , Accelerometer Update E g = [0 0 0 1] , B a = [0 a x a y a z ] B a = q ∗ ω, t ⊗ E b g ⊗ q ω, t   ax − 2( q 1 q 3 − q 0 q 2 ) ay − 2( q 0 q 1 + q 2 q 3 ) e a = z − ˆ z a =   az − 2( 1 2 − q 2 1 − q 2 2 ) Mangal Kothari January 7, 2018 IIT Kanpur 10 / 42

Introduction Approach Pose Estimation using UWB sensor IIT Kanpur WiFi based Solutions Conclusion Quaternion Estimates Accelerometer transformation     cos θ cos ψ cos θ sin ψ − sin θ 0 a b = − cos φ sin ψ + sin φ sin θ cos ψ cos θ cos ψ + sin φ sin θ sin ψ 0 sin φ cos θ     sin φ sin ψ + cos φ sin θ cos ψ − sin φ cos ψ + cos φ sin θ sin ψ 1 cos φ cos θ 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

Introduction Approach Pose Estimation using UWB sensor IIT Kanpur WiFi based Solutions Conclusion Quaternion Estimates Magnetometer Measurement Update B m = [0 m x m y m z ] h z ] = q B E ⊗ B m ⊗ q ∗ B E h = [0 h x h y E � � � h 2 x + h 2 E b = 0 0 h z = [0 b x 0 b z ] y mx − 2 b x ( 1 2 − q 2 2 − q 2  3 ) + 2 b z ( q 1 q 3 − q 0 q 2 )  e m = z − ˆ z m = my − 2 b x ( q 1 q 2 − q 0 q 3 ) + 2 b z ( q 0 q 1 + q 2 q 3 )   mz − 2 b x ( q 0 q 2 + q 1 q 3 ) + 2 b z ( 1 2 − q 2 1 − q 2 2 ) Mangal Kothari January 7, 2018 IIT Kanpur 12 / 42

Introduction Approach Pose Estimation using UWB sensor IIT Kanpur WiFi based Solutions Conclusion Quaternion Estimates EKF State Vector and Observation Vector � T � ν t = q 0 q 1 q 2 q 3 m x m y m z x y z V x V y V z x d y d z d t � T � z t = a x a y a z m x m y m z V x , B V y , B h B R t Measurement Update � ˆ � z a z MARG ˆ = z m ˆ H MARG ˆ Σ( H MARG ˆ Σ H T = MARG + Q ) K MARG ν t = ˆ ν t + K ( z − ˆ z ) ( I − K MARG H MARG )ˆ Σ t = Σ t Mangal Kothari January 7, 2018 IIT Kanpur 13 / 42

Introduction Approach Pose Estimation using UWB sensor IIT Kanpur WiFi based Solutions Conclusion Attitude estimates (roll) Figure: Estimated roll Mangal Kothari January 7, 2018 IIT Kanpur 14 / 42

Introduction Approach Pose Estimation using UWB sensor IIT Kanpur WiFi based Solutions Conclusion Attitude estimates (roll) Figure: Estimated roll Mangal Kothari January 7, 2018 IIT Kanpur 15 / 42

Introduction Approach Pose Estimation using UWB sensor IIT Kanpur WiFi based Solutions Conclusion Attitude estimates (pitch) Figure: Estimated pitch Mangal Kothari January 7, 2018 IIT Kanpur 16 / 42

Introduction Approach Pose Estimation using UWB sensor IIT Kanpur WiFi based Solutions Conclusion Attitude estimates (pitch) Figure: Estimated pitch Mangal Kothari January 7, 2018 IIT Kanpur 17 / 42

Introduction Approach Pose Estimation using UWB sensor IIT Kanpur WiFi based Solutions Conclusion Attitude estimates (yaw) Figure: Estimated yaw Mangal Kothari January 7, 2018 IIT Kanpur 18 / 42

Introduction Approach Pose Estimation using UWB sensor IIT Kanpur WiFi based Solutions Conclusion Attitude estimates (yaw) Figure: Estimated yaw Mangal Kothari January 7, 2018 IIT Kanpur 19 / 42

Introduction Approach Pose Estimation using UWB sensor IIT Kanpur WiFi based Solutions Conclusion Attitude estimates (yaw) Figure: Estimated yaw Mangal Kothari January 7, 2018 IIT Kanpur 20 / 42

Introduction Approach Pose Estimation using UWB sensor IIT Kanpur WiFi based Solutions Conclusion Attitude estimates (yaw) Figure: Estimated yaw Mangal Kothari January 7, 2018 IIT Kanpur 21 / 42

Introduction Approach Pose Estimation using UWB sensor IIT Kanpur WiFi based Solutions Conclusion Attitude estimates (yaw) Figure: Estimated yaw Mangal Kothari January 7, 2018 IIT Kanpur 22 / 42

Introduction Approach Pose Estimation using UWB sensor IIT Kanpur WiFi based Solutions Conclusion 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