Drift Reduction for Navigation in Harsh Environment

Ph. D Thesis

Drift Reduction for Navigation in Harsh Environment with Minimal/Low-Cost Sensors

Muhammad Ilyas Robotics and Virtual Engineering UNIVERSITY OF SCIENCE AND TECHNOLOGY August 2016


Muhammad Ilyas

A Dissertation Submitted in Partial Fulfillment of Requirements for the Degree of Doctor of Philosophy

August 2016

UNIVERSITY OF SCIENCE AND TECHNOLOGY Robotics and Virtual Engineering Supervisor: Dr. Sangdeok Park

We hereby approve the Ph.D. thesis of “Muhammad Ilyas”.

August 2016

Dr. Lee Dong-Wook Chairman of Thesis Committee Dr. Park Sangdeok Thesis Committee Member Dr. Park Hyunsub Thesis Committee Member Dr. Lee Ho-Gil Thesis Committee Member Prof. Kim Jung-Ha Thesis Committee Member Prof. Park Jaeheung Thesis Committee Member

UNIVERSITY OF SCIENCE AND TECHNOLOGY

ACKNOWLEDGEMENT First of all, thanks to God Almighty (하나님 in Korean), who gave me life, energy and opportunity to accomplish this task. Without His plan, nothing could happen. I would like to pay special thanks to my supervisor Professor Sangdeok Park for his guidance and encouragement at every step during my PhD course. I would like to thank to my dissertation defense committee members: Prof. Kim Jung-Ha from Kookmin University, Prof. Park Jaeheung fom Seoul National University, Prof. Lee Dong-Wook, Prof. Lee Ho-Gil and Prof. Park Hyunsub from UST, for spending their time in reviewing my thesis and making valuable comments on my defense presentation. The supervisor of my lab, Intelligent Sensors Laboratory (ISL), Dr Seung-Ho Baeg steered my work, technically as well as aesthetically. Thanks to my colleague, Dr Kuk Cho for his guidance for even minute things, like how to write technical papers, basics of programming languages, conduct experiments, results visualization etc. My co-researchers, Mr. Hong and Mr. Seo, for their support in experimentation, Korean language barriers, administration support etc. Dr Jong-Won Lee and Dr Hyogon Kim, from Robotics R&BD Group, had been constant help throughout my stay in Korea Institute of Industrial Technology (KITECH). No words can pay homage to the contributions and sufferings which my parents had throughout my educational career and especially during PhD. At every difficult moment in my life, I call them for prayers.

“And lower for them (parents) the wing of submission and humility through mercy, and say: ‘My Lord! Bestow on them (parents) Your Mercy as they did bring me up when I was child.” (Quran 17:24).

Lovely thanks to Azra Ilyas, Aatika Noor (14) and Shawana Noor (12); they are not merely best family, but my best friends as well.

i

ABSTRACT (Korean)

최소/저가 센서와 가혹한 환경에서 탐색을 위한 드리프트 감소

탐색(항해)은 주어진 환경에서 어떤 참조 프레임에 대한 자신의 위치, 속도 및 자세(PVA)를 결정하고 목표 위치에 대한 경로를 계획 할 수 있 는 방법을 찾는 기술이다. 모바일 에이전트는 다음과 같은 기본적인 두 가지 질문에 대한 답을 알 고 있어야 한다 : 나는 어디에 있나?, 그리고 나는 어디로 향하고 있나? 이러한 문제를 해결하기 위해 정확도는 환경에서 성공적으로 탐색할 수 있는 모바일 플랫폼의 능력을 결정한다. 이 논문에서 우리는 가혹한한 환경에서 탐색의 문제를 조사 한다: 이용할 수 있는 내비게이션 센서의 수가 극히 제한적(이거나/이고) 센서들이 환경외란 하는 경우가 있다. 그런 이유로 평범한 내비게이션 센서/시스템 및 방법들로는 가혹한 환 경에서 탐색을 위해 사용될 수 없다. 우리는 모바일 에이전트 탐색에 대해 두 종류의 가혹한 환경에 대해 연구한다. 

외계환경



육지환경

Part-I 에서 외계환경 탐색의 예로 우리는 행성탐사 로봇에 대한 통합 내비게이션 시스템을 설계했다. 행성탐사 로봇들은 매우 느린 속도와 로테이션의 특징을 가진 특별한 종류의 무인 지상차량(UGV: Unnamed Ground Vehicles) 이다. 이 탐사선은 느린 역학 때문에 쉽게 지역화 할 ii

수 있는 로봇 같지만, 불행히도 그 센서들은 지상 항법에 가능하다. 예 를 들어 GPS, RF-signals, 자기 나침반, 자이로 컴퍼스, 맵 매칭 등 은 우 주 로봇에 항상 이용할 수 없다. 이 섹션에서 내비게이션 에러 리덕션 감소의 작업과 모바일 플랫폼의 위치가 최소한의 저가 센서들 탑재로 착수되었다. 예를들면 MEMS 내부 센서, 휠 인코더, 태양센서 등. 특별한 강조는 gyroscope, Sun sensor 그리고 태도추정으로 드리프프를 감 소하는것에

대한

Inclinometer

데이터를

통합한

EASI(Estimation

of

Absolute Attitude using Sun Senser and Inclinometer) 로 불리는 절대적인 자 세추정알고리즘을 디자인 하면서 헤딩에러들을 맞게 놓는다. EASI 알고리즘은 Davenport‟s q-Method라 불리는 전통적으로 사용된 3D 자세 추정 알고리즘과 비교된다. EASI 알고리즘은 single measurement를 사용하면서 absolute attitude 를 계산하는 것으로, 즉 one shot 알고리즘 이다. 그것은 그 기능을 위해 다중 측정을 필요로 하는 q-method를 기반 한 batch-processing을 능가한다. 또한 EASI 알고리즘은 멀리있는 행성 환경에서 장기적 위치 안전성에 대한 행성탐사 로봇의 위치 정보를 생 산하기 위해

EASI-odometry라 불리는 휠인코더와 결합되어 있다. 다중

센서 데이터 융합 체계를 위한 Extended Kalman filter(EKF), Unscented Kalman filter(UFK) 같은 비선형 추정 알고리즘은 내장된 센서 데이터를 통합 대처 하기위해 비선형 attitude 측정 문제에 대응하여 비교되었다. Part-Ⅱ, 통합 내비게이션 시스템은 자기로 왜곡된 환경과 거부된 GPS의 보행자 내비게이션 시스템(PNS) 어플리케이션이 설계되었다. 예 를면 내부, 도시, 삼림 입지 등을 이용한 단일 foot mounted MEMS IMU 이다. 인간 보행 패턴으로 논문에서 가상 센서로 이용되는 일부 동작 제한을 도입했다. 3축 자력 센서는 자기 탐지기와 IMU로 데이터를 융합 하여 PNS의 absolute heading 을 고치기 위해 사용된다. 요즘 MEMS IMU 는 3개의 내비게이션 센서를 포함한다. 즉 tri-axial가속도계, tri-axial gyroscope 및 자력계, (AGM), 9DOF IMU으로 일컬어진다. iii

AGM센서를 포함하는 내부 내비게이션 시스템(INS)은 다양한 응용프로 그램과 깨끗한 환경에서 정확한 롤, 피치 및 제목정보를 제공할 수 있 다. 위치 계산을 위해 우리는 항공우주 연구 문헌에서, PNS응용 프로그 램에 대한 약간의 수정으로, 잘 알려진 INS 기계화 방정식을 빌릴수 있 다. 따라서 언제 어디서나 실내 및 실외 환경을 전체 6D 탐색정보(즉, 3D position, 3D attitude)로 얻을수 있다. 이 시스템의 중요 요소들 include: 

지구의 지역 자기장을 측정하기 위한 자력계



선형 가속도를 얻기위한 가속도계



시스템의 주기를 결정하기 위한 지로스콥

그러나, 문제는 자기계가 어떤 자석/강자성 재료의 존재로 측정 왜곡에 민감할때 나타난다.즉 자기로 왜곡된 환경에서 내비게이션 시스템을 기 반한 자기계가 정확히 heading 결정을 실패한다. 이 논문에서, 우리는 자 기장 이상을 탐지 및 보정을 함으로써 가혹한한 환경에서 heading정확도 에 자기왜곡의 효과를 완화시키기 위해 기술들을 제공한다. 개발된 알 고리즘은 자기계 데이터의 건강을 모니터 하기 위해 경사도(경사각이라 고도 함)와 자장강도의 정보를 사용하는 자기 변화 탐지기(MAD)라 한 다. 이 논문에서, 우리는 가혹한한 환경탐색을 위해 single MEMS IMU에 서 일반적으로 패키징된 내부센서의 수를 최소화 하면서 내비게이션 정 보 측정에서 drift를 줄이기 위해 기술과 알고리즘들을 제공했다.

Keywords: 추진력 감소, 가혹한 환경에서 탐색, 행성 탐사 로봇, 태양센 서, 멀티센서 융합, 보행자 내비게이션 시스템, 비선형 필터링.

iv

ABSTRACT*


Navigation is the art of determining own position, velocity and attitude (PVA) with respect to some reference frame in a given environment and then find the way to plan a path towards targeted location. A mobile agent must know the answers to the following two fundamental questions: Where am I?, and Where am I heading to? The accuracy to address these questions determines the mobile platform‟s ability to navigate successfully in its environment. In this dissertation, the issue of navigating in a harsh environment is investigated: an environment where there is very limited number of navigation sensors available and/or the sensors are prone to environmental disturbances. Hence, the conventional navigation sensor/systems and methods cannot be used for navigation in harsh environment. Navigation of mobile agents in two exemplary harsh environments is investigated; 

Extra-Terrestrial Environment



Terrestrial Environment.

In Part-I, an integrated navigation system for a planetary rovers, as an example of navigation in extra-terrestrial environment is presented. The planetary rovers are special kind of unmanned ground vehicles (UGV), characterized by their extremely *A dissertation submitted to committee of the University of Science and Technology in a partial fulfillment of the requirement for the degree of Doctor of Philosophy conferred in August 2016.

v

slow speed and rotation. These rovers seem to be easily localizable robots due to their slow dynamics but unfortunately the sensors available for terrestrial navigation, e.g.

GPS, RF-signals, magnetic compass, gyro compassing,

map-matching etc., are not always available for space robots. In this section, the task of navigation error drift reduction in attitude and position of a mobile platform is undertaken with the help of minimal onboard low-cost sensors only, e.g. MEMS inertial sensors, wheel encoders, Sun sensor etc. Special emphasis is put to correct the heading errors by designing an absolute attitude estimation algorithm, called Estimation of Absolute Attitude using Sun Sensor and Inclinometer (EASI), which integrates gyroscope, Sun sensor and Inclinometer data for reducing drift in attitude estimation. The EASI algorithm is compared with the traditionally used 3D attitude estimation algorithm, called Davenport‟s q-Method. As EASI algorithm calculates absolute attitude using single measurement, i.e. it is a one-shot algorithm; it outperforms the batch-processing based q-method, which requires multiple measurements for its operation. Further, the EASI algorithm is combined with wheel encoders, called EASI-odometry, to produce position information for planetary rover for long-term position reliability in remote planet environment. For multi-sensor data fusion, nonlinear estimation algorithms, like Extended Kalman filter (EKF), Unscented Kalman filter (UKF), has been designed and compared to cope with nonlinear attitude estimation problem incorporating data from onboard sensors only. In Part-II, as an example of navigation in terrestrial environment, an integrated navigation system is designed for pedestrian navigation application in GPS-denied and magnetically distorted environment, e.g. indoor, urban and forest vi

environment etc. using single foot-mounted MEMS IMU only. As human walking modes/patterns impose some motion constraints, which are exploited as virtual sensors in this dissertation. Moreover, a tri-axial magnetometer sensor is also used for correcting absolute heading of the Pedestrian Navigation System (PNS) by fusing data from IMU and magnetometer. The MEMS IMUs, nowadays, includes a triad of navigation sensors, i.e. tri-axial accelerometers, tri-axial gyroscope and tri-axial magnetometer, (AGM), announced as 9DOF IMU. An IMU can provide accurate roll, pitch and heading information in a variety of applications and in clean environments. Inertial navigation system (INS) mechanization equations, with little modifications for PNS application, can be used to get full 6D navigation information (i.e. 3D position, 3D attitude) anywhere/anytime, indoor and/or outdoor environment. The key components of this system include: 

Accelerometers to get linear acceleration.



Gyroscopes to determine rotation of the system.



Magnetometers to measure the Earth‟s local magnetic field

However, the issue arises when magnetometers are susceptible to measurement distortion in the presence of any magnetic/ferromagnetic material, i.e. in magnetically distorted environments, the magnetometer based navigation systems fails to determine heading accurately. This dissertation provides techniques to mitigate the effects of magnetic distortion on heading accuracy in harsh environments by detecting and compensating the magnetic field anomalies. The algorithm developed is called, Magnetic Anomaly Detection (MAD), which uses

vii

information of magnetic field strength and inclination angle (also called dip-angle) to monitor the health of magnetometer data. In this dissertation, algorithms and techniques are provided to reduce the drift in navigation information estimation by integrating minimum number of onboard sensors, normally packaged in a single MEMS IMU nowadays, for navigation in harsh environments.

Keywords: Drift reduction, Navigation in harsh environment, Planetary rover navigation, Sun sensor, Multi-sensor fusion, Pedestrian navigation system, Nonlinear filtering.

viii

Contents

ABSTRACT (Korean) ................................................................................ ii ABSTRACT* .............................................................................................. v 1

Introduction ......................................................................................... 2 1.1 Motivation .......................................................................................... 5 1.2 Problem Statement ............................................................................. 8 1.3 Navigation in Planetary Environment .............................................. 12 1.4 Navigation in GPS Denied and Magnetically Distorted Environment …………………………………………………………………..13 1.5 Related Work .................................................................................... 15 1.5.1 Navigation in Planetary Environment 15 1.5.2 Terrestrial Applications: Pedestrian Navigation System (PNS) 18 1.6 Research Goals and Contributions ................................................... 20 1.7 Dissertation Outline ......................................................................... 22

Part-I: Extra-Terrestrial Environment Application .............................. 25 Integrated Navigation System Design for Micro-Planetary Rovers ........ 25 2

Attitude Estimation in Planetary Environment ............................. 26 2.1 Introduction ...................................................................................... 26 2.2 Reference Frames ............................................................................. 27 2.3 Attitude Initialization ....................................................................... 29 2.3.1 Initialization of Roll and Pitch Angles using Inclinometer 29 2.3.2 Initialization of Heading Angle using Sun Sensor and Inclinometer 30 2.3.3 Turn-On Bias Estimation 30 2.4 Relative Attitude Estimation ............................................................ 33 ix

2.4.1 Attitude Propagation 34 2.4.2 Quaternion Method 38 2.5 Absolute Attitude Estimation ........................................................... 39 2.5.1 Sun Ephemeris 40 2.5.2 Procedure to Calculate Sun Position 42 2.5.3 The Sun Position Tracking by Sun sensor 46 2.6 Estimation of Absolute Heading Angle using Sun Sensor and Inclinometer (EASI algorithm) ........................................................ 48 2.7 Optimization-Based Absolute Attitude Estimation (q-Method) ...... 52 3

Multi-Sensor Fusion: From Bayesian to Kalman Filtering ........... 56 3.1 Introduction ...................................................................................... 56 3.2 Bayesian Filtering ............................................................................ 56 3.2.1 Probabilistic Motion Model 57 3.2.2 Probabilistic Observation Model 58 3.3 The Kalman Filter and Its Variants .................................................. 62 3.3.1 Process Model in Kalman filter 63 3.3.2 Observation Model in Kalman filter 63 3.3.3 Time Update Step or Prediction in KF 64 3.3.4 Measurement Update Step or Estimation in KF 65 3.4 Nonlinear Kalman Filters ................................................................. 66 3.4.1 Extended Kalman Filter 66 3.4.2 Time Update or Prediction in Nonlinear Filters 67 3.4.3 Measurement Update or Estimation in Nonlinear Filters 68 3.5 Unscented Kalman Filter (UKF) ...................................................... 69 3.5.1 Time Update or Prediction in UKF 70 3.5.2 Measurement Update or Estimation in UKF 70 3.6 Position Update ................................................................................ 74

4

Attitude Estimation in Planetary Environment: Experimental Results and Discussion ...................................................................... 77 4.1 Introduction ...................................................................................... 77 4.2 Hardware Description of Navigation Sensor Module for Micro x

Planetary Rover ................................................................................ 77 4.3 Experimental Set-up and Results Discussion................................... 79 4.3.1 Long-Time Static Test 80 4.3.2 Turn-in-Place (TiP) Tests 83 4.3.3 Long Time In-Motion Test 93 4.4 Planetary Rover‟s Localization ...................................................... 101 Part-II: Terrestrial Environment Application ..................................... 104 Navigation in GPS-Denied and Magnetically Distorted Environment . 104 5

Pedestrian Navigation System (PNS) Design ................................ 105 5.1 Introduction .................................................................................... 105 5.2 Foot-Mounted Inertial Navigation System .................................... 106 5.2.1 Inertial Navigation System for PNS 106 5.2.2 Foot-Mounted INS Mechanization 107 5.2.3 Foot-Mounted INS Error Model 110 5.3 INS Aided with Virtual Sensors ......................................................112 5.3.1 Zero-Velocity Update (ZUPT) 113 5.3.2 Walking Straight Paths: Straight Trajectory Heading (STH) Update 117 5.3.3 Standing Still for Long Time: PA-Locking Technique 118 5.4 EKF Design for Foot-Mounted PNS.............................................. 120 5.5 Absolute Heading Estimation using Magnetometer ...................... 122 5.5.1 Magnetometer 124 5.5.2 Magnetometer used as Compass 125 5.6 Magnetometer‟s Pre-calibration: Hard and Soft Iron Calibration . 127 5.7 Transient Magnetic Anomaly Detection (MAD) and Compensation Algorithm ....................................................................................... 131 5.8 Conclusion ..................................................................................... 132

6

Experimental Results and Discussion ............................................ 134 6.1 Introduction .................................................................................... 134 xi

6.2 Test Environments.......................................................................... 134 6.3 Experimental Set-up and Evaluation Method ................................ 136 6.4 Indoor Environment Test................................................................ 137 6.5 Outdoor Environment Tests ........................................................... 141 6.5.1 Outdoor Test with Arbitrary Walk 141 6.5.2 Urban Canyon Environment Test 143 6.5.3 Forest Canopy Environment Test 146 6.6 Experimental Set-up and Evaluation Method for MAD algorithm 148 6.6.1 Static Test 6.6.2 Moving Test in Straight Line 6.6.3 Dynamic test in PNS application 7

148 153 158

Conclusion and Future Research ................................................... 163 7.1 Conclusion ..................................................................................... 163 7.2 Future Work ................................................................................... 165

Bibliography ............................................................................................ 167

xii

List of Figures Figure 1.1: Typical motion scenarios of planetary rovers. ........................... 3 Figure 1.2: USSR‟s Lunokhod-1, 2 Lunar rovers [15]. ................................ 6 Figure 1.3: NASA‟s Sojourner and Mars Exploration Rover (MER). .......... 6 Figure 1.4: NASA‟s Curiosity (Mars rover); Chinese Yutu (Lunar rover). .. 7 Figure 1.5: Pedestrian navigation in harsh environment: Concept and applications [22]. .......................................................................................... 8 Figure 1.6: Example of different grades of IMUs. ..................................... 10 Figure 1.7: GPS-denied/magnetically distorted env.: Indoors, urban canyon, forest canopy, and planetary environments [24]. ...11 Figure 1.8: Navigation sensors availability for UGV vs planetary rovers. .11 Figure 1.9: Mars test rovers. Left: Rocky, On right: FIDO rover with Sun sensor installed on front body [52,38]. ....................................................... 17 Figure 1.10: Organizational chart of dissertation. ...................................... 24 Figure 2.1: Depiction of reference frames used in context of Planetary Rovers [69]. ................................................................................................ 29 Figure 2.2: Effect of Turn-On Bias removal on attitude propagation. ....... 32 Figure 2.3: Effect of Turn-On bias removal on 2D trajectory estimation in PNS application, in a 15 minute walk in multi-floor building. .................. 32 Figure 2.4: Effect of Turn-On bias Not-removed on 2D trajectory estimation in PNS application. ................................................................... 33 xiii

Figure 2.5: Definition of Euler angles. ....................................................... 34 Figure 2.6: Calculation of Sun position (azimuth, elevation) using Sun ephemeris data: Algorithm flow chart ........................................................ 46 Figure 2.7: Left: Mars rover use Sun sensor to determine rover‟s heading [8,77]; Right: CAD diagram of PSD type Sun sensor‟s (ISS-D60) used in this work. .................................................................................................... 47 Figure 2.8. Absolute heading and tilt angle estimation algorithm (EASI) flowchart. .................................................................................................... 49 Figure 2.9: Depiction of relation between rover heading from Sun position by Sun sensor and Sun ephemeris data. ...................................................... 52 Figure 2.10: Optimization based attitude estimation (q-Method) algorithm block diagram. ............................................................................................ 54 Figure 3.1: Time update step for the Byes filter [83]. ................................ 59 Figure 3.2: Observation update for the full Bayes filter [83]. .................... 61 Figure 3.3: Time update and measurement update steps are depicted as sequential measurements arrive. ............................................................... 62 Figure 3.4: Algorithm block diagram to integrate relative and absolute navigation sensors. ..................................................................................... 72 Figure 3.5: Flowchart of EKF and UKF, with corresponding equations against each processing block..................................................................... 73 Figure 3.6: Position calculation using wheel encoders and attitude estimate. .................................................................................................................... 74 Figure 3.7: Sensor fusion for planetary rover localization: Block diagram. xiv

.................................................................................................................... 76 Figure 4.1: Left: Ground test rover; Right: Exploded view of sensor module (KAIROSS) used for experiments. .............................................................. 79 Figure 4.2: GUI developed for KAIROSS ................................................. 79 Figure 4.3: Azimuth and Elevation angle comparison of EASI algorithm and Sun Ephemeris data. ................................................................................... 82 Figure 4.4: Rover heading angle estimation during 2.5 hours static test. Comparison of EASI algorithm and Optimization based q-Method. .......... 82 Figure 4.5: Turn-in-place motion: Left: Actual Mars rover (Sojourner) took two TiPs before reaching the targeted rock [5]. Right: Sensors set-up for TiP test (B), both CCW and CW through 0~360°. ..................................... 83 Figure 4.6: Sun sensor output during Test (B-i) with 90° rotation steps in CW and CCW direction. ............................................................................. 84 Figure 4.7: TiP with large steps (90°) in CW and CCW. Comparison of heading estimation algorithms. ................................................................... 84 Figure 4.8: Zoom-in view of start, mid and end time in Test (B-i). ........... 85 Figure 4.9: TiP with small steps (20°) in CW and CCW. EKF and UKF estimates end up with close to initial heading angle but gyro-only solution show clear divergence, more than 2° at the end of this experiment. ........... 87 Figure 4.10: Zoom-in view in TiP test (B-ii), with small increments (20°), both CW and CCW rotation through 0~360°, at start (a), mid (b) and at the end (c) point of rotation. Clearly, the q-Method has large errors as compared to EASI. ...................................................................................... 88

xv

Figure 4.11: Continuous rotation (0~360°) in CW and CCW direction in TiP test (B-iii). ................................................................................................... 90 Figure 4.12: Zoom-in view of continuous Turn-in-place test (B-iii): Start (a), mid (b) and at the end (c) of rotation in CW and CW through 0~360°rotation. The q-Method approach is noisy and has large errors in in-motion state of the sensor. ...................................................................... 91 Figure 4.13: Roll and pitch angle estimation during long in-motion test. .. 94 Figure 4.14: End trajectory Roll and pitch angle estimation during in-motion test. ............................................................................................. 94 Figure 4.15: Heading angle (unwrapped) estimated by different algorithms during one hour in-motion test. .................................................................. 96 Figure 4.16: Zoom-in view of Start (a) and End (b) heading angle estimation in dynamic test of 1 hour. The q-Method is not agreeing with other algorithm in dynamic condition. However, in static condition (e.g. before 6.5 min. and after 63.8 min.), q-Method also performs nearly well, as expected.................................................................................................. 98 Figure 4.17: Sun radiation during long-time in-motion test. Note that below a threshold of 300 W/m2, the sensor output is not reliable for heading calculation. .................................................................................................. 99 Figure 4.18: Mean error in all five tests. .................................................. 100 Figure 4.19: Standard deviation of error in all five tests. ......................... 100 Figure 4.20: 2D localization plot: Comparison among position estimation using different localization techniques for planetary rover. ..................... 102 Figure 4.21: Absolute error in position estimation. .................................. 103 xvi

Figure 4.22: Absolute error in heading estimation. .................................. 103 Figure 5.1: Inertial navigation system (INS) computation steps [57]. ......110 Figure 5.2: Example of zero-velocity event occurrence while pedestrian is walking. During time T , velocity of point „A‟ becomes absolutely zero [59]. ...................................................................................................................113 Figure 5.3: ZUPT detection and step occurrence during normal walk. The zero-velocity periods, as foot falls on ground, are shown as pink square wave and foot count is shown by small circle (black), overplayed on accelerometer and gyro signals..................................................................115 Figure 5.4 : Straight trajectory heading (STH) concept diagram. .............117 Figure 5.5: In standing-still condition, position and attitude remains constant. .....................................................................................................118 Figure 5.6. 2D Position error reduction by using PA-Locking technique. 119 Figure 5.7: Roll, pitch and heading error reduction with PA-Locking technique....................................................................................................119 Figure 5.8: EKF algorithm flowchart of PNS: Data fusion of INS, ZUPT, STH, PA-locking....................................................................................... 122 Figure 5.9: Earth‟s magnetic field. Note that true north pole and magnetic north pole have fixed deviation at a particular location on Earth‟s surface. .................................................................................................................. 124 Figure 5.10: Earth‟s magnetic field and heading calculation.  is dip-angle, 

is declination angle,  mag magnetic heading and  true is true heading with

respect to true north. ................................................................................. 125 xvii

Figure 5.11: Compass frame (red) orientation with respect to Earth surface. Local flat frame (blue) is parralel to Earth‟s surface.  , are roll and pitch angles respectively. ................................................................................... 127 Figure 5.12: Case-I: If mild distortion is present in the calibration environment. The 3D sphere and 2D circle centered at (0,0) after hard and soft iron calibration. .................................................................................. 130 Figure 5.13: Case-II: If very strong ferromagnetic distortion is present in the calibration environment. The 3D sphere and 2D circle centered at (0,0) after hard and soft iron calibration. .......................................................... 130 Figure 5.14: Effect of ferrous object on magnetic field disturbance [95] 131 Figure 6.1: Urban canyon test environment; Daengi-gil, a food street near Kitech. The IMU [97] is fitted on top of the left foot. On upper right corner, is shown large metallic manhole cover, found in many urban streets, along with other large metallic objects. .............................................................. 135 Figure 6.2: Left: a forest environment, and on right is typical indoor environment. Both environments fall in the GPS-denied and magnetically distorted environment category. ............................................................... 136 Figure 6.3: Multi-floor Indoor test. For ease of visualization, an approximate reference path manually drawn on the building image (front and top view) taken from Google Earth. .................................................. 139 Figure 6.4: 2D and 3D trajectory plot comparison in indoor test (3-floor building in KITECH). Effect of STH applied is clearly seen in red trajectory, which has less cross-track divergence and hence keeps the parallelism and perpendicularity corresponding to the building layout. .. 140 Figure 6.5: Outdoor test near KITECH, covering 1100m (aprox.) in 25 min. xviii

The reference path is manually drawn on the image taken from Google Earth. ........................................................................................................ 142 Figure 6.6: Urban environment test: The reference path (on left) is manually drawn on the image taken from Google Earth. On right is the estimated trajectory by proposed algorithm. ............................................ 145 Figure 6.7: Forest environment test: The reference path (on left) is manually drawn on the image taken from Google Earth. On right is the estimated trajectory by proposed algorithm. ............................................ 147 Figure 6.8: Roll angle estimate in long time static test. On right, is shown the zoomed-in view for visual inspection. ................................................ 149 Figure 6.9: Pitch angle estimate in long time static test. On the right, is shown the zoomed-in view for visual inspection. .................................... 149 Figure 6.10: Heading angle estimate in long time static test. On the right, is shown the zoomed-in view for visual inspection. .................................... 149 Figure 6.11: Experimental set-up for MAD algorithm verification in static test............................................................................................................. 150 Figure 6.12: Roll and pitch angle estimate in static test with permanent magnet pass. ............................................................................................. 151 Figure 6.13: Heading angle estimate in the static test with permanent magnet pass. On the right, is shown the zoomed-in view for visual inspection. ................................................................................................. 151 Figure 6.14: Car passing test scenario. The vehicle is passed three times close the sensor. ........................................................................................ 152 Figure 6.15: Roll and pitch estimation. Tilt angle estimation by K-AHRS xix

remains unaffected by magnetic distortion. .............................................. 152 Figure 6.16: Heading angle estimation maintains constant angle during magnetic distortion periods. ..................................................................... 153 Figure 6.17: Moving IMU in exactly straight line, by sliding sensor on a straight wooden support. In close-up, is shown a very precise magnetic compass for reference heading. ................................................................ 154 Figure 6.18: Roll and pitch angle estimation during quasi-static test: Move towards a car as per experimental set-up in Figure 6.17. ......................... 156 Figure 6.19: Heading angle estimate when moving sensor towards a car. On right is shown magnetic field strength and Dip-angle with chosen threshold for MAD algorithm. .................................................................................. 157 Figure 6.20: Roll and pitch angle estimation during quasi-dynamic test. 158 Figure 6.21: Heading angle estimate in quasi-dynamic test. On right is shown magnetic field strength plot with chosen threshold for MAD algorithm. .................................................................................................. 159 Figure 6.22: Effect of MAD on 2D trajectory estimation in PNS application, in a 30 minute walk in a Outdoor/Indoor multi-floor building. ............... 160 Figure 6.23: Magnetic field strength plot for mixed indoor/outdoor pedestrian walk. Magnetic field strength threshold is chosen as 0.35-0.75 G (350~750mG). .......................................................................................... 161

xx

List of Tables Table 1.1. Parameters comparison of different grades of IMUs [23]. ........ 10 Table 2.1. Reference frames definition. ...................................................... 27 Table 2.2. RMS error in position estimation in PNS: Effect of Turn-On Bias removal. ...................................................................................................... 33 Table 2.3. Estimation of Absolute heading using Sun sensor and Inclinometer (EASI algorithm)................................................................... 51 Table 2.4. Optimization based absolute attitude estimation (q-Method). ... 54 Table 4.1. Sensors parameters in KAIROSS . ............................................. 78 Table 4.2. Statistical comparison of two absolute heading estimation algorithms. .................................................................................................. 82 Table 4.3. Statistical comparison of heading estimation algorithms in continuous TiP test (B-i). ............................................................................ 86 Table 4.4. Statistical comparison of heading estimation algorithms in TiP test (B-ii). .................................................................................................... 89 Table 4.5. Statistical comparison of heading estimation algorithms in continuous Tip test (B-iii). .......................................................................... 92 Table 4.6. Statistical comparison of tilt angles estimation in long-time in-motion test. ............................................................................................. 95 Table 4.7. Statistical comparison of heading estimation algorithms in dynamic test (only end-trajectory errors are compared). ............................ 99 Table 4.8. Statistical comparison of different localization methods. ........ 102 xxi

Table 5.1. RMS error in position, if PA-Locking technique is applied in a static test. .................................................................................................. 120 Table 5.2. RMS error in attitude, if PA-Locking technique is applied in a static test. .................................................................................................. 120 Table 6.1. RMS error reduction by applying STH technique for indoor environment test. ...................................................................................... 141 Table 6.2. RMS error reduction by applying STH technique. .................. 143 Table 6.3. RMS error if STH and PA techniques are applied in urban canyon environment. ................................................................................ 146 Table 6.4. RMS error if STH and PA techniques are applied. .................. 148 Table 6.5. Maximum error observed in long time static test. ................... 150 Table 6.6. RMS error if MAD algorithm applied in a mixed indoor/outdoor environment test. ...................................................................................... 161

xxii

Acronym and Abbreviations AGM: Accelerometer, Gyro, Magnetometer. COTS: Commercial off-the-shelf Low PWC: Low power, weight, and cost. EASI: Estimation of absolute Attitude using Sun sensor and Inclinometer. SPA: Solar Position Algorithm. q-Method: Davenport's q-method. PNS: Pedestrian Navigation System INS: Inertial Navigation System ZUPT: Zero-Velocity Update. STH: Straight Trajectory Heading. PA-Locking: Position and Attitude Locking. MAD: Magnetic Anomaly Detection. K-AHRS: KITECH-Attitude Heading Reference System.

xxiii

Drift Reduction for Navigation in Harsh Environment with Minimal/Low-Cost Sensors _______________________________________________ Part-I Extra-Terrestrial Environment Application: Integrated Navigation System Design for Micro-Planetary Rovers

Part-II Terrestrial Environment Application: Pedestrian Navigation System Design for GPS-Denied and Magnetically Distorted Environment

1

Chapter 1 1Introduction Introduction

It is natural instinct in almost all living species to know where they are. They use this ability to find way to their home and food resources. This ability is living species is called „Animal Navigation‟ [1,2]. In old days, people have used different means to navigate long distances for trade or other purposes on ground and/or sea surface. Most famous methods in those days include Sun/Stars sighting, using magnetic compasses, sextant, and dead reckoning etc. [3]. For autonomous vehicles, navigation is the art of determining own position and/or attitude (roll, pitch and heading) with respect to some reference frame in a given environment and then find the way to plan a path towards some targeted location. To navigate successfully in its surroundings, a mobile agent must know the answers to the following two fundamental questions [4]: 

Where am I?,



Where am I heading to?

Autonomous platforms use some kind of sensors to estimate one or more states of the system for navigation, e.g. planetary rovers, have used high-grade IMU, wheel encoders and high-cost cameras to navigate successfully on Mars over decades. The planetary rovers are special kind of unmanned ground vehicles 2

(UGVs) which are characterized by their extremely slow speed and rotation. These rovers move in a stop-and-go fashion so that they can scan their surroundings to find their drivable path and scientific targets in the targeted exploration area. For example, NASA‟s Mars rovers have an average speed of 1 cm/s when move on hard and plane surface and even less in normal motion. The rover is programmed to drive for roughly 10 seconds, then stop to observe and understand the terrain around for 20 seconds, move safely onward for another 10 seconds and so on [5]. Typical motion scenario of a planetary rover includes straight drives, arc turns and turn-in-place maneuvers [6,7] as illustrated in Figure 1.1. No GPS

TiP

No Magnetic compass

go

go

Stop Target Rock

Turn-in-place (TiP) motion Sun sensor/IMU frame

Curved path motion

go

No Gyro-Compassing

Straight path motion

go TiP

Stop

No Maps

Vehicle frame Stop go

go TiP

Stop

Nav-frame

Figure 1.1: Typical motion scenarios of planetary rovers. Planetary rovers seem to be easily navigable mobile robots due to their slow dynamics but unfortunately the sensor suite available for ground robots are not always available onboard these rovers. Estimation of position and/or attitude (PA) information is a prerequisite for operating any kind of manned/unmanned vehicle 3

in a target environment. Planetary rovers use PA information for many other major or sub tasks as well, for example, [8] states: “Ground operators use the position and attitude of the rovers to make numerous decisions and predictions. The attitude is used to determine the expected bandwidth of UHF communications sessions, how much power the rovers can get from the solar panels due to their relative orientation to the sun, how much the rover will slip while driving, whether it is safe to use the Rock Abrasion Tool (a rock drill) without causing the rover to slip, etc. Ground operators use the position to plan drives and determine where to point the cameras and other science instruments. Other flight software uses the position and attitude as well. The navigation, driving, and Instrument Deployment Device (IDD; a robotic arm) software uses the position and attitude to plan maneuvers and perform safety checks. Standup software, which unfolded the rover after landing, used the attitude to perform safety checks. The High Gain Antenna (HGA), Pancam Mast Assembly, and Payload software use the attitude and position to point the HGA, cameras, and other science instruments”. The Second example of navigating in harsh environment, considered in this dissertation, is the pedestrian navigation in indoor, urban canyon or under forest canopy. These environments are categorized as GPS-denied environments. Other bad attribute of this kind of environment, especially indoors, is that magnetic compass cannot be used to correct heading errors, as magnetometer is badly affected by nearby ferromagnetic materials, which are very common in indoors and urban canyon. This dissertation deals with developing techniques and algorithms for reducing drift in position, velocity and attitude for navigating 4

successfully in harsh environments, using minimum number of low power, low weight and low cost , on-board sensors (PWC) only.

1.1 Motivation Finding own way in harsh (non-friendly) environment has been a key challenge in navigation community. With the advent of MEMS technology, the low cost navigation sensors are available for low power, low weight/volume applications. An effort has already being done to use MEMS IMUs for space applications [9,10,11,12,13]. However, these low cost navigation sensors have inherent large errors [14] when used alone or without data fusion techniques. Hence, the motivation of this work is how to reduce drifting errors in navigation information by multi-sensor data fusion in harsh environment, for reliable operation. In terms of „Navigationally Harsh Environment’, the planetary environment and indoor, urban and forest environments are taken as candidate harsh environments in this dissertation, for developing algorithms to reduce navigational drift. Both type of environments, are categorized as „GPS-denied‟, „magnetically distorted‟ and „unstructured‟ environments (i.e. without pre-installed navigation beacons), having very limited number of navigation sensors available and/or the sensors are prone to environmental disturbances. Hence, the conventional navigation sensor/systems and methods fail to cope with navigation accuracy requirements in harsh environment. There has been an increasing interest in planetary rover‟s mission since the space race began in later decades in last century. There are examples of many successful planetary rovers have operated/ or still operating on other planet‟s surface, millions of miles away from Earth. The first successful planetary rover 5

mission was Lunokhod-1 and Lunokhod-2 (Figure 1.2), launched by USSR in 1970s. They covered about 10km and 40km on Moon surface respectively.

Figure 1.2: USSR‟s Lunokhod-1, 2 Lunar rovers [15]. But these rovers were remotely controlled from Earth station; hence there was no autonomy on board. From NASA, many missions have been launched in effort to explore surface of other planets, among them most successful are Sojourner rover, landed in July 1997 on Mars surface. Following that, two large and more capable rovers, Spirit and Opportunity landed on the surface of the Mars in January, 2004 (Figure 1.3). Remote control for Mars rovers was not possible due to latency; hence these rovers were equipped with onboard autonomous systems. Still most of the planning is done on Earth station and execution commands are sent to the rovers once per day [16,17].

Figure 1.3: NASA‟s Sojourner and Mars Exploration Rover (MER). Most recent rover mission were launched by NASA, the Curiosity rover on 6

Mars in August, 2012, and the Yutu rover in December, 2013 (Figure 1.4) for Moon exploration by China National Space Administration (CNSA).

Figure 1.4: NASA‟s Curiosity (Mars rover); Chinese Yutu (Lunar rover). It is seen that these rovers have been playing an important role to explore the surface area closely on remote planet [18] which has not been done previously in the mankind history. These rovers have mobility ability to explore the target areas with greater details as compared to the previous orbiter or Lander missions [7]. Corresponding to such growing attention, there has been an increasing number of research activities conducted by various other institutions around the world [19], e.g. European Space Agency (ESA), Japan Aerospace Exploration Agency (JAXA), Korean Aerospace Research Institute (KARI) and Indian Space Research Organization (ISRO) etc. are preparing for planetary exploration missions. Being on remote planet, power saving is challenging and determines the mission life. Hence low power, low weight/volume navigation sensors are big candidates for future micro-rovers (300

Init. Attidtude

No

Yes

Acceleration, heading Gyro

Attitude Propagation Non-linear Filter (EKF/UKF) Gyro bias estimates

Updated Attitude

Inclinometer/ Accelerometer

Acceleration

Sun Sensor

Figure 3.4: Algorithm block diagram to integrate relative and absolute navigation sensors.

72

Initialize

xˆ 0  E (x0 )

Initialize

P0  E[(x0  xˆ 0 )(x0  xˆ 0 )T

xˆ k , Pk

Compute Syste m Jacobean Ma trix

xˆ f Fk  k x

f , Gk  k w xˆ k 1

Fk , G k

Time Update

xˆ (ki ) Pk1  k 1

xˆ

 k

 Fk P F  G k Q k G T k

T k

 f (xk , u k )

Time Update

xˆ k 1 , Pk1

Compute Meas urement Jacobe an Matrix and Kalman gain

xˆ k 1 , Pk1 H k 1 

h k 1 x

xˆ k 1

 k 1

T k 1

K k 1  P H

 k 1

(H k 1P H

T k 1

 R k 1 )

Generate new sigma points

1

Hk 1 , K k 1

Measurement Update

xˆ (ki) 1

xˆ k 1  xˆ k 1  K k 1[y k 1  h(x k1 )]

P0  E[(x0  xˆ 0 )(x0  xˆ 0 )T

  k , Pk

Generate sigma points

xˆ k

xˆ 0  E (x0 )

xˆ (ki )  xˆ k  χ (i ) χ (i )  ( NPk )T , χ ( n  i )  ( NPk )T

xˆ (ki) 1  f (xˆ (ki ) , u k ) 1 2 N (i )  xˆ 2 N i 1 k 1 1 2 N (i )   (xˆ  xˆ k1 )(xˆ (ki)1  xˆ k1 )T  Qk 2 N i 1 k 1

xˆ k 1  Pk1

xˆ (ki) 1  xˆ k1  χ (i ) χ (i )  ( NPk1 )T , χ ( n  i )  ( NPk1 )T

yˆ (ki) 1  h(xˆ (ki) 1 ) 1 2 N (i )  yˆ 2 N i 1 k 1 1 2 N (i )   (yˆ  yˆ k 1 )(yˆ (ki) 1  yˆ k 1 )T  R k 1 2 N i 1 k 1 1 2 N (i )   (x  xk1 )(yˆ (ki) 1  yˆ k 1 )T 2 N i 1 k 1

yˆ k 1 

Pk1  (I  K k 1H k 1 )Pk1

Predict measur ement and cov. matrices

xˆ k 1 , Pk1

IMU

K k 1 , Pkvv1

Measurement Update

Measurement: EASI

xˆ k 1 , Pk1

Pkvv1 Pkxy1

k k 1  Pkxy1 / Pkvv1

xˆ k 1  xˆ k 1  K k 1[y k 1  yˆ k 1 )] Pk1  Pk1  K k Pkvv1k Tk 1

Sun Sensor

Figure 3.5: Flowchart of EKF and UKF, with corresponding equations against each processing block.

73

3.6 Position Update In this dissertation, the rover position is calculated by traditional wheel-odometry method, gyro-odometry [86] and by combining wheel encoder data and output of the EASI algorithm, called EASI-odometry in this work. However, even if accurate attitude estimation is provided, the position error will grow due to errors in dead reckoning process in using wheel encoder data to get change in position. N

 k 1

N

tk 1 N

d k

E

tk

E D

Figure 3.6: Position calculation using wheel encoders and attitude estimate.

In Figure 3.6, heading angle and position of the rover is illustrated at two time instances, k and k+1. For example,  k and k 1 are absolute heading angle (w.r.t. true North) at time k and k+1 respectively, and  and p is the change in attitude and position during the sampling time dt . 74

vxwheel   xk   xk 1   y  =  y   Cn  0  dt b    k   k 1   0   zk   zk 1   

 Pk = Pk 1  P ; Pk  31

(3.39)

(3.40)

The transformation matrix ( Cbn ) can be composed of only heading angle in case of wheel-odometry, three propagated attitude angles in case of gyro-odometry [86] and stabilized attitude in case of EASI-odometry. For stabilized position estimation (i.e. non-drifting), some external absolute reference is needed to correct position errors in PositionEKF. The PositionEKF is designed to update the position only, if position measurements are available from any other external absolute sensor/system. For example, in this work, a low accuracy GPS receiver (with 5m accuracy) is used with absolute position measurements after every 30 seconds, for test purposes on ground (Earth). The measurement model for position updates in PositionEKF can be written as:

y k = h(Pk )  ηk ; ηk  31

(3.41)

Note that position measurements must be transformed to local-tangent frame (n-frame) before incorporating in estimator frame work (Section 2.2). This infrequent position measurement is chosen because on planetary rover, absolute position measurements are not frequently available as in terrestrial applications. Position is propagated using wheel encoder data and attitude estimated from EASI algorithm and position is updated, if any source of absolute position becomes available, e.g. from onboard map matching algorithm, rover tracking from orbiter circling the planet, direct rover tracking from Earth-based station etc. All of these external sources of rover position estimation cannot provide frequent position 75

measurements due to their inherent limitations, to update the localization data in PositionEKF. However, infrequent position measurements are enough if very precise and well calibrated wheel encoders are used and there is no wheel slippage or rover skidding during the motion. The divergence in localization due to odometry errors may be reduced by incorporating global position information in a PositionEKF framework, as shown in Figure 3.7. Init. Attitude: Ψ  0

ω

b ib

ω

Gyro

E(.)

Static bias Est .

Inclinometer

[,, ]

b ib

fibb

Yes

Stop

Updated Attitude

Attitude Propagation

Ψ

Ψ : Predicted Attitude -

Attitude EKF/UKF

Tilt, EASI/ q-Method [ , ]Inclino. ,[ EASI ]

fibb

,  Sun Sensor Init. position: + Lander P0

Updated Position -

P : Predicted Position

Position Propagation

Wheel Encoder

P+

Position EKF

Stop Yes

Global Position (if available)

, 

Figure 3.7: Sensor fusion for planetary rover localization: Block diagram.

76

Chapter 4

Planetary 4Attitude AttitudeEstimation Estimationinin PlanetaryEnvironment: Environment: Experimental Results Results and Experimental andDiscussion Discussion 4.1 Introduction In this chapter, in first section, the hardware used to conduct experiments in this work, is described. Then the experimental set-up to verify propositions made in Chapter 2 is provided and the result outcomes are discussed in detail. Lastly, localization results, to simulate planetary rover motion scenario, are presented.

4.2 Hardware Description of Navigation Sensor Module for Micro Planetary Rover A navigation module is developed, called Kitech Advanced Intelligent Rovers Sensor System (KAIROSS), using low cost, low weight and low power consumption sensors, which are necessary parameters for micro planetary rover to be deployed in future planetary missions (Figure 4.1). This prototype unit uses two navigation sensors; i.e. ISS-D60 (Sun sensor) and STIM300 (IMU), based on MEMS technology. The Sun sensor is a PSD type sensor with two orthogonal axes and wide field of view. The IMU consists of 3-axis accelerometer, 3-axis Gyros and 3-axis inclinometer, with onboard calibration and temperature compensation routines. The 77

main parameters of KAIROSS and sensors used are given in Table 4.1. A GUI developed for data control and visualization in shown in Figure 4.2. The KAIROSS uses OMAP-L138 processor of Texas Instruments for on-board processing. It has dual-core SoC which are ARM926EJ-S RISC MPU, and C674x DSP, with 8MB SPI flash memory for kernel image for stand-alone system, 10/100Mbps Ethernet peripheral for data and commands transfer, 3 UARTs for getting data from the sensors, USB2.0 for WiFi, I2C bus interface, and DMA etc. This system has two separated process. Getting data from the sensor is handled by DSP processor so that it ensures exact time stamps and on ARM side, it runs the Linux kernel and handles communication with main control center. KAIROSS can be operated with external/internal battery pack and it weighs only 270 grams. Table 4.1. Sensors parameters in KAIROSS . Sensors

Parameters

Wt.[g]

Pwr.[w]@5V

Size[cm]

Sun Sensor

Accuracy: 0.4°; 2-axis; FOV:120°x120°

100

0.165

r=4,h=2.1

Bias stability: 0.5 (°/hr); ARW: 0.5(°/√hr)

55

(ISS-D60) MEMS IMU (STIM 300) KAIROSS

-

270

2 3.2 (Load)

3.9x4.5x2.2 11 x 8 x 4.3

The test rover used for experiments is KYMCO MXU 500i; an off-road rover that can be remotely controlled and has been modified for autonomous mode capabilities as UGV. The navigation system is installed on top of the test rover.

78

Z

NovAtel GPS

S

IMU/SS frame: Forward, left, up Y

Main controller

S

X

S

IMU (STIM300) WiFi module

Wheel Encoder

N

Y

V

Navigation frame: E North, East, Down (NED)

X Z

V

Sun sensor (ISS-D60)

V

Rover frame: Forward, right, down

D

Figure 4.1: Left: Ground test rover; Right: Exploded view of sensor module (KAIROSS) used for experiments.

Figure 4.2: GUI developed for KAIROSS

4.3 Experimental Set-up and Results Discussion For evaluation of our proposed algorithm for integrating relative and absolute sensors for micro rover localization, possible motion scenarios are considered which are commonly encountered by planetary rovers. These include: A. Long time (e.g. hrs) stationary: Planetary rovers remain stationary most of the times unless they are commanded to move forward or perform some task. B. Turn in place (TiP) motion: This is very common motion scenario on a 79

planet with many obstacles not seen before. Rover has to avoid them by performing turn-in-place motion to reach the goal with minimum energy and time. i)

TiP in large steps: Turn-in-place rotation test of rover in steps of 90°, both clockwise (CW) and then counter-clockwise (CCW) is performed. This test is conducted to check the capabilities of the developed algorithm to track the changing absolute heading angle when the rover takes big turns to avoid obstacles on the planet surface.

ii)

TiP in small steps: This is the TiP test in steps of 20°, to check the small angle tracking of the algorithms, while rover turning.

iii)

Continuous TiP: Sometimes the rover has to turn about continuously to change direction. This test proves the algorithm capability to track absolute angle in continuous turning of the rover through 0~360°, in CW and CCW rotations.

C. With arbitrary attitude: Most of the time rovers are commanded to move forward few centimeters and then look around, and take curved path, with arbitrary attitude, around any encountered obstacle or target rock to take appropriate action.

4.3.1 Long-Time Static Test For Test (A), the sensor module was kept stationary for about 2.5 hours at a fixed location (longitude: 126.841833° and latitude: 37.293353°) in a particular direction on 27th Oct, 2014 and captured data from KAIROSS. The roll and pitch angles are 4.5° and 6°, respectively during this static test. The purpose of this experiment is to 80

test the long term stability of heading estimation algorithms, both EASI and q-Method, using onboard sensors only. The position (longitude, latitude) of the rover is taken from Google-Earth and time/date is the onboard system time. In actual planetary rovers, however, the absolute position may be acquired from other sources. The results of long time stationary condition are shown in Figures 4.3, and 4.4. In Figure 4.3, the azimuth and elevation angles, as predicted by Sun Ephemeris data with that of the Sun sensor measurements are compared, from 11:00 AM to 13:30 PM, a 2.5 hours‟ time duration. Both azimuth angles match as expected, but there is a fixed offset of about 3° in elevation angles. This offset is due to the atmospheric effect as the Sun rays are refracted when entering in the Earth atmosphere. This effect is called „Albedo Effect‟. But this has no effect of heading estimation process, i.e. EASI algorithm. Albedo Effect is absent in atmosphere free planets, as our intended application. The final output of two heading estimation algorithms is plotted in Figure 4.4. The absolute heading angle determined by EASI algorithm is quite smooth with mean error of 0.26°, standard deviation of 0.21°and maximum error less than 1°. However, the absolute heading estimate by q-Method has an error of about 2°. It proves that proposed algorithm (EASI) has better performance than optimization based q-Method in this long term static test and the heading angle of EASI is not diverging as well, though nosier as compared to gyro-only solution, which has inherent diverging nature being a dead reckoning process. A quantitative comparison of heading estimation algorithms is given in Table 4.2.

81

45

200

44

190

43

185

42

180

41

Elevation[deg]

Azimuth[deg]

Azimuth angle comparison 195

175 170

Sun Sensor:Measurement Ephemeris:Prediction

Elevation angle comparison

40 39 38

165

37

160

36

Sun Sensor:Measurement Ephemeris:Prediction

155 0.5

1

1.5 Time[hrs]

2

2.5

35

3

0.5

1

1.5 Time[hrs]

2

2.5

Figure 4.3: Azimuth and Elevation angle comparison of EASI algorithm and Sun Ephemeris data. Long Time Static Test: Comparesion of EASI and q-Method 66 EASI algo. q-Method Gyro-only

65

Heading Angle[deg]

64

63

62

61

60

59 0

0.5

1

1.5

2

2.5

Time[Hrs]

Figure 4.4: Rover heading angle estimation during 2.5 hours static test. Comparison of EASI algorithm and Optimization based q-Method. Table 4.2. Statistical comparison of two absolute heading estimation algorithms. Error[°] Mean Error

Std. dev.

Max. Error

RMS Error

Algo. q-Method

0.76

0.53

1.8

0.93

EASI algo.

0.26

0.21

0.7

0.34

Gyro-only

0.84

0.24

0.5

1.16

82

4.3.2 Turn-in-Place (TiP) Tests The second motion scenario, Turn-In-Place (TiP) is very important type of motion in planetary rover‟s perspective, as rover has to take decision to take sharp turns and avoid obstacles, not seen before, Figure 4.5.

Sojourner on Mars

Magnetic Compass

Battery

KAIROSS

Goniometer

Turn-in-place motion of Sojourner rover.

Figure 4.5: Turn-in-place motion: Left: Actual Mars rover (Sojourner) took two TiPs before reaching the targeted rock [5]. Right: Sensors set-up for TiP test (B), both CCW and CW through 0~360°. To test the TiP motion scenario (B), a sensor set up composed of KAIROSS, a Goniometer and a high precision Magnetic Compass is used, as shown in 4.5. First, the sensor module is rotated through large steps of 90°, in CW and then CCW. In each step, the rover rested for about 2 minutes before performing next rotation and completed full 0~360° rotation and back. The output of the absolute heading estimation algorithms are compared against the reference value taken from Goniometer readings and absolute heading angle measured by a very precise Magnetic Compass. The results of this test are shown in Figure 4.6~4.8 and in Table 4.3.

83

Sun Angle X 60

Sun Angle Y 60

40

40

20

Angle Y:[deg]

AngleX:[deg]

20

0

0

-20

-20

-40

-40

-60

-60

0

200

400

600

800 1000 time[s]

1200

1400

1600

1800

0

200

400

600

800 1000 time[s]

1200

1400

1600

1800

Figure 4.6: Sun sensor output during Test (B-i) with 90° rotation steps in CW and CCW direction. Heading Angle Estimation 400

(b)

q-Method EASI algo Gyro-Only EKF UKF

350

Heading Angle:[deg]

300 250 200 150

Ref. angle

100 50

(a)

(c)

0 -50

0

5

10

15 Time[Minutes]

20

25

30

Figure 4.7: TiP with large steps (90°) in CW and CCW. Comparison of heading estimation algorithms.

84

(a)

(c)

(b)

Heading Angle Estimation



2

2.5 361

1.5

Heading Angle:[deg]

0.5 0 -0.5 -1

Heading Angle:[deg]

360.6

1 Heading Angle:[deg]

2

360.8

360.4

Ref. angle

360.2 360 359.8 359.6

1.5 1 0.5 0

359.4

-0.5 359.2

-1.5

-1

359

0.5

1

1.5 2 Time[Minutes]

2.5

3

11

11.5

12 12.5 Time[Minutes]

13

23.5

24

Figure 4.8: Zoom-in view of start, mid and end time in Test (B-i).

85

24.5

25 25.5 26 Time[Minutes]

26.5

27

27.5

Table 4.3. Statistical comparison of heading estimation algorithms in continuous TiP test (B-i). Start[0°]

Error[°] Algo.

Mid[360°]

End[0°]

Mean

Std. dev.

Mean

Std. dev.

Error

Mean

Std. dev.

EASI

-0.158

0.095

359.65

0.112

0.348

-0.134

0.067

q-Method

-0.163

0.117

359.74

0.125

0.256

-0.165

0.090

EKF

-0.158

0.090

359.65

0.105

0.349

-0.134

0.063

UKF

-0.154

0.085

359.65

0.102

0.348

-0.113

0.052

Gyro-only

0.152

0.103

360.75

0.055

0.759

2.062

0.145

86

The statistical comparison at start (a), mid (b) and at the end (c) of this test shows the better performance of EASI against q-Method and little better performance of UKF as compared to EKF. The next test (B-ii) of TiP is done in small rotation angles, i.e. in steps of 20°. The purpose of this test is show that the developed algorithms are able to track the rotational motion of rover even in small steps as shown in Figure 4.9 and 4.10 and Table 4.4. Heading Angle Estimation 400

(b)


350

Heading Angle:[deg]

300 250 200 150 100 50 (a)

(c)

0 -50

0

2

4

6

8 10 12 Time[Minutes]

14

16

18

20

Figure 4.9: TiP with small steps (20°) in CW and CCW. EKF and UKF estimates end up with close to initial heading angle but gyro-only solution show clear divergence, more than 2° at the end of this experiment. The zoom-in view (a, b, c) and statistical comparison are shown in Figure 4.10 and in Table 4.4. The q-Method has large mean error and standard deviation as compared to EASI algorithm, being in-motion, the q-Method does not track heading angle well in this quasi-dynamic test, because it needs multiple measurements at the same position to perform optimization operation. 87

(a)

(b)


(c)



3 2

361

Ref. angle

2

1.5

360.5

0 -1 -2


Heading Angle:[deg]

Heading Angle:[deg]

1

360

359.5

0.5 0 -0.5 -1 -1.5

-3

359 -2

-4

-2.5

358.5

0.2

0.25

0.3 0.35 Time[Minutes]

0.4

0.45

0.5

7

7.1

7.2


7.5

7.6

7.7

17.3

17.4

17.5

17.6 17.7 17.8 Time[Minutes]

17.9

18

18.1

Figure 4.10: Zoom-in view in TiP test (B-ii), with small increments (20°), both CW and CCW rotation through 0~360°, at start (a), mid (b) and at the end (c) point of rotation. Clearly, the q-Method has large errors as compared to EASI.

88

Table 4.4. Statistical comparison of heading estimation algorithms in TiP test (B-ii). Error[°] Algo.

Start[0°] Mean

Std. dev.

Mid[360°] Mean

Std. dev.

End[0°] Error

Mean

Std. dev.

EASI

0.241

0.265

360.41

0.093

0.414

0.335

0.063

q-Method

0.605

0.625

360.86

0.180

0.862

0.995

0.148

EKF

0.255

0.1916

360.40

0.077

0.425

0.333

0.0493

UKF

0.179

0.278

360.40

0.073

0.400

0.231

0.0371

Gyro-only

0.324

0.334

360.04

0.022

0.0487

-2.073

0.0403

89

Next TiP test (B-iii) is continuous type of rotation of rover through 0~360°, both in CW and CCW direction. This scenario of motion may sometimes require turning about the rover back because of some large obstacle encountered, for example. The estimation results are presented in Figure 4.11 and Figure 4.12 and in Table 4.5. EASI algorithm output gives noisy but stable heading angle. EKF and UKF show similar performance in this short time continuous rotation experiment. The q-Method is not performing well as compared to EASI method due to sensor motion. Heading Angle Estimation 400

(b)


350

Heading Angle:[deg]

300 250 200 150 100

Ref. angle

50

(a)

(c)

0

0.5

1

1.5 2 Time[Minutes]

2.5

3

Figure 4.11: Continuous rotation (0~360°) in CW and CCW direction in TiP test (B-iii).

90

(a)

(b)


(c)



5 362

4

3

Heading Angle:[deg]

2

Ref. angle

1 0

Heading Angle:[deg]

361.5


4

361

360.5

360

1 0 -1

-1

-2

359.5

-2

2

-3

-3 0.2

0.3

0.4

0.5


0.8

0.9

1

1.35

1.4

1.45

1.5


1.65

1.7

1.75

2.3

2.4


2.7

2.8

2.9

Figure 4.12: Zoom-in view of continuous Turn-in-place test (B-iii): Start (a), mid (b) and at the end (c) of rotation in CW and CW through 0~360°rotation. The q-Method approach is noisy and has large errors in in-motion state of the sensor.

91

Table 4.5. Statistical comparison of heading estimation algorithms in continuous Tip test (B-iii). Error[°] Algo.

Start[0°] Mean

Mid [360°]

Std. dev.

Mean

End[0°]

Std. dev.

Mean

Std. dev.

EASI

0.3316

0.6762

360.1108

0.4105

0.1919

0.6570

q-Method

0.6507

0.6942

360.2965

0.4144

0.4266

0.6842

EKF

0.3262

0.6589

360.1008

0.3956

0.2854

0.6343

UKF

0.3013

0.6507

360.0265

0.3075

0.2345

0.6139

Gyro-only

0.4954

0.6976

360.6322

0.0548

0.5320

0.0219

92

4.3.3 Long Time In-Motion Test To test the algorithm performance in dynamic condition of the rover (Test-C), the sensor module is fixed on a hand-driven cart and moved slowly (e.g. 2cm/sec.) with stop-and-go fashion and arbitrary roll, pitch and heading. At the start of the dynamic test, the sensor module is kept static for some time to capture turn-on bias of the IMU. During motion, the Sun obstruction condition is also tested by blocking the Sun radiation for some time. The attitude, roll, pitch, heading estimation results by integrating Sun sensor, inclinometer and gyro are shown in Figures 4.13-4.17. The overall RMS errors and the end-trajectory attitude estimation errors are shown in Table 4.6 and Table 4.7. In this test, the sensor module was placed at certain initial orientation and it was ensured that module ends up in same orientation at the end of this long in-motion test. Hence initial orientation angles are taken as reference values for error calculations. The hand-driven cart with navigation sensor module fixed on top, was driven with different speed and arbitrary attitude for about an hour. The motion was in stop-and-go fashion in order to mimic the planetary rover‟s motion. The integrated (or fused) results should show superior performance in terms of accuracy and robustness in comparison of single navigation sensor. The EKF and UKF are compared for the accuracy and robustness by simulating a scenario of Sun sensor obstruction due to some reason, e.g. rover is under the shadow of a big rock, or Sun sensor is occluded by rover‟s self-shadow. In this case Sun radiation is below the critical value of 300W/m2 , as shown in Figure 4.17. In Sun obstruction situations, the absolute heading information from EASI or q-Method is not available to update the EKF/UKF. 93

Pitch angle

Roll angle 60

q-Method EASI Gyro-Only EKF UKF

80 60

40 30

Pitch Angle:[deg]

40

Roll Angle:[deg]


50

20 0

20 10 0 -10

-20

-20

-40 -30

-60 -40

10

20

30

40 time[s]

50

60

70

0

10

20

30

40

50

60

time[s]

Figure 4.13: Roll and pitch angle estimation during long in-motion test. Roll angle

Pitch angle q-Method EASI Gyro-Only EKF UKF

0.6 0.4

0

1

Pitch Angle:[deg]

Roll Angle:[deg]

0.2

-0.2 -0.4 -0.6


0.5

0

-0.5

-0.8 -1

-1 -1.2 64

64.2

64.4

64.6

64.8 65 time[s]

65.2

65.4

65.6

65.8

66

64

64.2

64.4

64.6

64.8 65 time[s]

65.2

65.4

65.6

65.8

Figure 4.14: End trajectory Roll and pitch angle estimation during in-motion test.

94

70

Roll and pitch angles propagated by gyro-only method has clear divergence, whereas EKF/UKF has almost same performance, as evident from the end-trajectory zoomed-in views, in Figure 4.14. The numerical comparison of roll/pitch is given in Table 4.6, below. Table 4.6. Statistical comparison of tilt angles estimation in long-time in-motion test. Error[°] Algo. EASI q-Method EKF UKF Gyro-only

Roll Stat Mean Std. dev. -0.04 0.03 -0.06 0.09 -0.27 0.03 0.27 -0.04 -0.54 0.06

Pitch

End Mean Std. dev. -0.11 0.06 -0.24 0.11 -0.41 0.05 -0.41 0.06 -0.94 0.16

Start Mean Std. dev. -1.19 0.02 -1.60 0.08 -0.98 0.03 -0.98 0.04 0.05 0.04

End Mean Std. dev. -1.09 0.02 -1.69 0.09 -0.90 0.02 -0.90 0.03 -13.88 0.07

The heading estimation by different algorithms is given in Figure 4.15. There are Sun obstructions periods of different time duration during this long-time dynamic test in order to check capability of estimators, EKF/UKF and EASI/q-Method.

95

Heading Angle Estimation 500

Sun obstruction periods 5min. (a)

0

10min.

Heading Angle:[deg]

8min.


-500

-1000 (b)

-1500

-2000

0

10

20

30 40 Time[Minutes]

50

60

70

Figure 4.15: Heading angle (unwrapped) estimated by different algorithms during one hour in-motion test. Three Sun-obstruction periods, of different time length (5, 10 and 8 minutes) are considered during the motion to check the robustness of the heading estimation; output of EKF/UKF algorithms. During Sun obstruction periods, both EASI and q-Method does not work because of the Sun sensor limitations. However fused data of IMU and EASI (i.e. EKF/UKF) are tracking heading angle in Sun obstruction periods as well. The qualitative comparison of EASI and q-Method shows that in static condition, both absolute heading estimation algorithms work well as expected, but in dynamic case, q-Method fails to estimate heading reliably, whereas EASI algorithm has good performance both in static and in in-motion situation too. The reason is that EASI is a one-shot algorithm (i.e. uses single measurement) whereas q-Method requires collecting number of

96

measurements, hence requires rover to be stationary for some time (e.g. several minutes) [87,42] before applying some kind of optimization method. Therefore, q-Method is not applicable for in-motion condition of the rover.

97

(a)

(b) Heading Angle Estimation

180 160 140

Heading Angle Estimation 2


1

Ref. angle




100 80 60 40

-1

-2

-3

20

-4

0 -20

-5 4

5

6 7 Time[Minutes]

8

9

63.8

64

64.2

64.4


65

65.2

65.4

65.6

Figure 4.16: Zoom-in view of Start (a) and End (b) heading angle estimation in dynamic test of 1 hour. The q-Method is not agreeing with other algorithm in dynamic condition. However, in static condition (e.g. before 6.5 min. and after 63.8 min.), q-Method also performs nearly well, as expected.

98

Table 4.7. Statistical comparison of heading estimation algorithms in dynamic test (only end-trajectory errors are compared). Error[°] Algo.

Mean

Std.dev.

Max. Error

EASI

-0.91

0.09

1.09

q-Method

-1.19

0.10

1.71

EKF UKF

-0.97 -0.92

0.04 0.03

1.08 1.01

Gyro –only

-1.67

0.04

1.75

Sun Radiation

1000

Radiation:[W/m 2]

800

600

400

Sun Radiation Threshold

200

0

10

20

30

40

50

60

70

time[s]

Figure 4.17: Sun radiation during long-time in-motion test. Note that below a threshold of 300 W/m2, the sensor output is not reliable for heading calculation. In Figures 4.18 and Figure 4.19, the statistical results of all five tests conducted in this study are summarized, i.e. the mean error and standard deviation of all algorithms are compared in each test.

99

Mean Error in Heading Angle Estimation 2.5 EASI q-Method EKF UKF Gyro-only

Mean Error [deg.]

2

1.5

1

0.5

0

Long_Static

TiP_i

TiP_ii Test

TiP_iii Long_Dynamic

Figure 4.18: Mean error in all five tests. Std. Dev. of Error in Heading Angle Estimation 0.8 EASI q-Method EKF UKF Gyro-only

0.7

Error Std.Dev [deg.]

0.6 0.5 0.4 0.3 0.2 0.1 0

Long_Static

TiP_i

TiP_ii Test

TiP_iii Long_Dynamic

Figure 4.19: Standard deviation of error in all five tests. Both qualitative and quantitative results confirm that the developed algorithm, EASI outperforms the conventional optimization-based q-Method, especially in dynamic condition. It is also shown that multisensory fusion has more accuracy and robustness than individual sensor/methods, in attitude estimation for planetary test rover in this work.

100

4.4 Planetary Rover‟s Localization For completeness, an integrated position estimation algorithm is also developed for planetary rover as shown in Figure 4.20. For comparison, three localization methods are considered: conventional wheel-odometry as for ground robots, gyro-odometry and EASI-odometry. In wheel-odometry, the forward position change is calculated for every motion from pre-calibrated encoder data and heading angle is calculated from steering wheel encoder data. However, this kind of motion estimation is valid for limited time and on flat surfaces only. Depending on surface roughness, this method is no longer reliable for long time operations. However, performance of wheel-odometry can be enhanced by including gyros data in attitude calculation. Instead of using only heading angle from steering wheel encoder, in gyro-odometry, 3D attitude angles are used to propagate wheel position data. This type of position estimation has good performance as compared to wheel encoders only, but still depends on the accuracy of gyros used. In long time operations, gyro-odometry also suffers from drift and position estimates are no longer usable, especially the cross-track errors grow with error in heading angle estimation. To reduce errors in gyro-odometry, attitude errors should be compensated by some absolute means, e.g. from inclinometer and Sun sensor data in this work. By combing Sun sensor and IMU, called EASI-odometry, the errors in localization of rover are also reduced much, as shown in Figure 4.20. The maximum absolute error of all three methods in x-axis and in y-axis in shown in Figure 4.21. The localization estimates are compared with high-performance NovAtel GPS module with 5m accuracy. The quantitative comparison of three position estimation methods is given in Table 4.8. 101

2D Position 80 70 60

Ref GPS EASI Odomtry Gyro Odomtry Wheel Odometry

Northing[m]

50 40 30

Stop 20 10 0 -10

Start/End

-20 -20

0

20

40 60 Easting[m]

80

100

120

Figure 4.20: 2D localization plot: Comparison among position estimation using different localization techniques for planetary rover. Table 4.8. Statistical comparison of different localization methods. Error[m] Algo.

Abs. Error(max)

2D Abs. Error

RMS Error

2D RMS Error

x

y

x2  y 2

x

y

x2  y 2

Wheel Odo.

52.1

95.3

108.6

20.2

34.3

39.8

Gyro Odo.

22.8

20.5

30.7

11.3

11.6

16.2

EASI Odo.

1.8

1.6

2.4

0.9

0.6

1.1

102

Absolute Error in X Position error X[m]

60

EASI Odo Error Gyro Odo Error Wheel Odo Error

40

20

0

0

100

200

300

400

500

600

700

500

600

700

Absolute Error in Y Position error Y[m]

100

50

0

0

100

200

300 400 Time[Sec.]

Figure 4.21: Absolute error in position estimation. The quantitative comparison of position estimates shows that position errors of EASI-odometry are less than 3m when compared with NovAtel GPS module in this 10 minute and about 200m run. Heading Angle

Heading Angle

Ref. Gyro only EASI

100

-244.5

Heading Angle:[deg]


Ref. Gyro only EASI

-244

0 -50 -100

-245

-245.5 -246 -246.5

-150

-247

-200

-247.5

-250

100

200

300

400 time[s]

500

600

700

595

600

605

610

615 time[s]

620

625

630

Figure 4.22: Absolute error in heading estimation.

The maximum heading error during this test is 5 s

Yes

No

EKF update Correction

Yes

No

STH true?

PA Lock

STH update

MAD{5.34~ 5.36} Estimate PVA errors{5.27-2} Update Error covariance{5.27-3}

Correct Predicted PVA{5.28}

updated PVA

Figure 5.8: EKF algorithm flowchart of PNS: Data fusion of INS, ZUPT, STH, PA-locking.

5.5 Absolute Heading Estimation using Magnetometer Keeping track of roll, pitch and heading angles (i.e. attitude) is very important for autonomous/semi-autonomous operations of a platform in robotics community. 122

Although gyroscopes can be used for tracking all three angles; once initialized properly, for a considerable length of time, depending on the accuracy of the sensor. But attitude angles estimated using gyros tend to drift with the passage of time, if not corrected by some absolute reference. Though roll and pitch (tilt) angles

are

easy

to

correct

due

to

gravity

vector

sensing

by

accelerometer/inclinometer, correction of the absolute heading angle is more demanding. Different techniques are employed for absolute heading angle estimation, depending on the type of sensors and the accuracy requirements; among them: Gyro compassing, Magnetometer, Sun sensor, Star trackers, Multi-antenna GPS receiver, Map matching etc. Each technique has its own limitations, merits and demerits. With the advent of modern MEMS technology, the miniaturized inertial measurements units (IMU) are emerging which mostly contains accelerometers, gyroscopes, and magnetometer, announced as 9DoF IMUs. Magnetometers in these units are used as digital compasses which, in clean environment gives very accurate absolute heading angle (~2°) with respect to magnetic north pole. This absolute heading angle is used in sensor fusion framework to correct the divergence of gyro tracked attitude, called Attitude Heading Reference Systems (AHRS). But the limitation with most of the AHRS is that they do not work reliably in magnetically distorted environment as their attitude depends on the accuracy of the magnetic heading. In this dissertation, an algorithm is developed to overcome this limitation of AHRS by detecting the magnetic distortion in the operating environment and then compensating attitude with gyro data in Extended Kalman filter framework. 123

5.5.1 Magnetometer A magnetometer is a type of sensor that measures the strength and direction of the local magnetic field. The magnetic field measured will be a combination of both the earth's magnetic field and any magnetic field created by nearby objects. The magnetic field is measured in the sensor reference frame.

Figure 5.9: Earth‟s magnetic field. Note that true north pole and magnetic north pole have fixed deviation at a particular location on Earth‟s surface. Earth’s Magnetic field, Figure 5.9, is a mathematical description of magnetic influence of the electrical currents produced by the coupling of the convective effects and rotation in the spinning liquid metallic outer core of iron and nickel in Earth‟s crust [wiki]. This mechanism is called dynamo effect. Earth‟s magnetic field is measured in units of Tesla (T) or Gauss (G). Note that 1T=10,000 G. The Earth's magnetic field is about 0.2 to 0.6 G and has a component parallel to the Earth's surface that always points toward the magnetic north pole. It is specified by a vector with direction and magnitude and is denoted by B or H.

124

5.5.2 Magnetometer used as Compass Magnetometer measures the magnetic field components of Earth in 3D. These magnetic components are used to determine heading angle in an electronic compass module with respect to Earth magnetic north. To determine the magnetic heading, the components of Earth‟s magnetic field have to be measured in two orthogonal, parallel to the Earth‟s surface, directions. The three dimensional Earth‟s magnetic field vector and its relationship with the magnetic heading angle is shown in Figure 5.10.

Hx  mag



 true

H xy



(right)

Hy

H

Hz (down)

Figure 5.10: Earth‟s magnetic field and heading calculation.  is dip-angle,  is declination angle, mag magnetic heading and true is true heading with respect to true north. The Earth‟s magnetic field vector has three components ( H x , H y , H z ), which can be measured by 3D magnetometers. The H x component is in the direction of 125

motion of the vehicle and H y is perpendicular to it in right-hand direction. Using these

two

horizontal

components,

a

combined

horizontal

component

( H xy  H x2  H y2 ) of Earth‟s magnetic field is obtained. This horizontal component is always parallel to the Earth surface and aligns itself to the magnetic North-South direction. The magnetic heading angle is obtained from horizontal components, as follows:

 mag  tan 1 (

Hy Hx

)

(5.29)

It is crucial to obtain the horizontal magnetic field components as correctly as possible, otherwise calculated heading angle will be contaminated with the tilt errors. The true heading angle and magnetic heading have some offset, depending on the location on Earth‟s surface. This offset is called Magnetic declination (  ), which is taken as positive in Eastward direction and negative in Westward direction. Then true heading of the vehicle is obtained by subtracting the declination angle from magnetic heading, i.e.

 true   mag  

(5.30)

In most of the applications, it is impractical to assume roll and pitch angles of the magnetometer as zero. Hence the magnetometer readings are compensated with the tilt angles to obtain horizontal components out of it (Figure 5.11).

126

X





Y

Gravity Vector

Z

Figure 5.11: Compass frame (red) orientation with respect to Earth surface. Local flat frame (blue) is parralel to Earth‟s surface.  , are roll and pitch angles respectively. The measurements of the 3D magnetometer should be transferred to Earth fixed plane frame, so that the horizontal components of Magnetic field are obtained to calculate magnetic heading. The roll angle (  ) and pitch angle (  ) are obtained, in our application, from onboard inclinometer or from the output of the designed attitude module in Chapter 2. Then, the tilt compensated horizontal components of Earth‟s magnetic field are obtained as; H x  mx cos( )  my sin( )sin( )  mz cos( )sin( )

(5.31)

H y  my cos( )  mz sin( )

Where ( mx , my , mz ) is magnetometer output in sensor frame.

5.6 Magnetometer‟s Pre-calibration: Hard and Soft Iron Calibration Magnetic measurements will be subjected to distortion, if sensor is used close to certain metals. These distortions are considered to fall in one of two categories; hard or soft iron. Hard iron distortions are created by objects that produce a magnetic field, e.g. ferromagnetic materials with permanent magnetic fields e.g. 127

magnets, speakers. They are time invariant. If the piece of magnetic material is physically attached to the same reference frame as the sensor, then this type of hard iron distortion will cause a permanent bias in the sensor output. Soft iron distortions are considered deflections or alterations in the existing magnetic field. It is caused by materials which can become magnetized and produce time varying magnetic field, e.g. current carrying traces on the PCB, steel shields, batteries or other magnetically soft materials. These distortions will stretch or distort the magnetic field depending upon which direction the field acts relative to the sensor. This type of distortion is commonly caused by metals such as nickel and iron. In most cases hard iron distortions will have a much larger contribution to the total uncorrected error than soft iron. The purpose of the magnetic calibration is to determine compensating parameters such as, misalignment matrix between the magnetic sensor sensing axes and the device body axes, scale factor vector, offset vector caused by hard iron distortion and the parameters caused by soft iron distortions. In general, a sensor model is used to mathematically correct for errors in scale factor, misalignment, and bias, using a linear model as;  mx  M hx  0 0  M x  1 / SCx   M   M  0 1 / SCx 0  M si  my  M hx  m   y    Mz   0 0 1 / SCx   mz  M hx 

(5.32)

Where M m is the 3  3 misalignment matrix, SCi (i  x, y, z ) is 3  1 scale factor vector, M si is a 3  3 soft-iron parameter matrix, and M hi (i  x, y, z ) is 3  1 offset caused by hard-iron effects. Then by rearranging, equation (5.32) is rewritten as:

128

 M x   M 11 M 12 M    M  y   21 M 22  Mz   M 31 M 32

M 13   mx  M 10  M 23   my  M 20  M 33   mz  M 30 

(5.33)

The goal of magnetometer hard-soft iron calibration is to estimate these 12  parameters, in order to compensate the sensor measurements to produce reliable heading angle. Different parameter estimation techniques are used to find these parameters. From simple methods as finding the minimum and maximum values to estimate the scale factors and biases of the compass, to the more expensive estimation algorithm are used, e.g. Least Squares (LS), iterative Maximum Likelihood Estimator (MLE) [93]. In this dissertation, a simple and commonly used technique is adopted, as given in [94]. The results of magnetometer calibration are shown in Figure 5.12 and Figure 5.13, for mildly and heavily distorted environments, respectively. Note that after compensating the magnetometer output with calibration parameters, the calibrated magnetometer data inscribe a sphere in 3D and a circle in 2D, centered at (0,0).

129

- Uncalibrated data - Calibrated data

0.4 0.2

0.4 0

0.2 0

-0.2

0.5

-0.2

-0.4

-0.4 -0.6

0

0.4

0.4

0.2

0

-0.2

-0.4

0.2

0

-0.2

-0.4

-0.6

-0.8

-0.5 -0.6

-0.8

3D magnetometer data


Figure 5.12: Case-I: If mild distortion is present in the calibration environment. The 3D sphere and 2D circle centered at (0,0) after hard and soft iron calibration.

- Uncalibrated data - Calibrated data 1 0.8 0.6

0.4

0.4

0.2 0

0.2

-0.2 -0.4

0

1

-0.6 0.5 1.5 1

-0.2 1.5

1

0.5

0

0

0.5 0



Figure 5.13: Case-II: If very strong ferromagnetic distortion is present in the calibration environment. The 3D sphere and 2D circle centered at (0,0) after hard and soft iron calibration.

130

5.7 Transient Magnetic Anomaly Detection (MAD) and Compensation Algorithm As gyro-only heading angle estimation has unbounded drift with time, which if not corrected by some absolute source, will produce cross-track errors in position which may lead to unpleasant consequences in some applications. Indoor environment is usually full of ferromagnetic materials, e.g. iron, steel, power lines, electrical machines etc. Hence the magnetometer readings are badly affected when it comes in the vicinity of such ferromagnetic materials or magnets itself [95] , as depicted in Figure 5.14 below. Ferrous object

Uniform(undistorted) magnetic field

Non-uniform(distorted) magnetic field

Figure 5.14: Effect of ferrous object on magnetic field disturbance [95] Therefore, it is common practice not to use magnetometers for heading calculation in such environments. However, the worth of data which magnetometer provides is far more for navigation purposes as absolute heading can be very easily calculated using this sensor, if magnetometer data is free from distortions. Hence there are efforts ongoing to use magnetometer data in such unfriendly environment too by designing such algorithms which can provide some detection and compensation in heading estimation. One such algorithm is proposed in this dissertation, dubbed as Magnetic Anomaly Detection (MAD) and compensation algorithm. The crux of the idea is that the magnetic field strength and its direction towards ground, called dip angle, remains almost constant in a 131

local area within few kilometers in a magnetically clean environment. If these factors are taken into consideration and the measured magnetic field strength and dip angle are compared with these reference values at the start of the experiment, any subsequent magnetic anomaly can easily be detected in the magnetometer readings itself. If mean of dip-angle and magnitude of magnetic field at the starting time are taken as reference values, assuming no motion and distortion during starting stationary period, then any further anomalies can be detected easily. The magnetic field strength is simply the magnitude of the three components of magnetometers and dip angle is calculated using accelerometer and magnetometer or magnetometer data alone, as shown in Equations (5.34~5.36). (5.34)

H  hx2  hy2  hz2

The dip angle calculated from accelerometer and magnetometer readings is,

  90  cos1 (g. H)

(5.35)

and the dip-angle by using magnetometer data alone is:

  tan 1 (

hz h  hy2 2 x

) (5.36)

The magnetic field strength, dip angle and declination angles in the testing area near KITECH are: 51,074.6 nT, 54°21'42", and -8°16'19" respectively [96].

5.8 Conclusion In this chapter, mathematical development of a pedestrian navigation system was presented with detailed mathematical expression and techniques to reduce inherent drift in INS. Three such techniques are focused, considering the motion 132

constraints (virtual sensors) of the normal gaiting modes of the walker, with no prior information of the user and the operating environment. It is expected that by using these virtual sensors, the drift in INS only PVA will be reduced. In the last section, the magnetometer was investigated for reducing the drift in heading angle, as virtual sensors cannot completely eliminate the heading drift in PNS. However, being harsh environment, magnetometer itself is difficult to use as its reading will be erroneous for calculation of heading angle. Hence a novel technique called MAD is proposed to check the health of magnetometer data and incorporate it in EKF framework to aid INS, when it gives healthy data. Thus, by using absolute heading information from magnetometer, the drift in navigation can further be reduced in PNS application.

133

Chapter 6 6 ExperimentalResults Results and Experimental andDiscussion Discussion

6.1 Introduction In this chapter, the experimental results are provided for PNS design and applications in harsh environments, like, indoors, urban canyons, and forest environment. First, experimental set-up and verification method of results is described, and then results obtained are presented in graphical and numerical form. In last sections, experimental results of novel MAD algorithm are presented in different motion scenarios and finally it is incorporated in PNS application.

6.2 Test Environments In order to fulfill the main aim of this dissertation, the environment chosen to test the navigation accuracy enhancement is such that they fit in the category of „Navigationally Harsh Environment’. For example, a nearby food street is chosen as urban canyon, Figure 6.1. In such kind of environments, GPS signals may not be completely blocked, but they may have large reflections, thereby reducing the chances of availability of GPS for reliable navigation. Similarly, a walking test was conducted in a forest, which is covered with tall and dense trees. 134

The GPS signals are vulnerable in such environments. The third candidate of harsh environment is indoors, Figure 6.2, with no pre-installed navigation infrastructures, i.e. R.F, Ultrasonic signals, visual land marks etc. Both absolute navigation systems/sensors, e.g. GPS and magnetometer are prone to large errors in such environments, and hence normally cannot be used. Daengi-gil: A traditional food street near Kitech.

Manhole cover, commonly found in urban streets.

MEMS IMU fitted on foot

Figure 6.1: Urban canyon test environment; Daengi-gil, a food street near Kitech. The IMU [97] is fitted on top of the left foot. On upper right corner, is shown large metallic manhole cover, found in many urban streets, along with other large metallic objects.

135

Dense and tall trees in forest environment

Metallic door and walls

Power distributor Panel

Figure 6.2: Left: a forest environment, and on right is typical indoor environment. Both environments fall in the GPS-denied and magnetically distorted environment category.

6.3 Experimental Set-up and Evaluation Method For using motion constraints of human walking as „Virtual sensors‟, a single MEMS IMU (3DM-Gx3-45) is fitted on left foot. The walking speed was kept normal during all experiments. In some experiments, user also stood still for more than 5 seconds to check the effect of PA-locking technique. The root-mean-square error (RMSE) is chosen as an accuracy measure in this dissertation. It is used to measure the difference between actual values and the output of an estimator. For quantitative comparison, only starting and final position is checked, both in 2D and 3D, in all walking tests. The starting position is assumed as (0,0,0), hence only the final estimates are inserted in RMSE formula. Mathematically RMS error is defined as:

136

n

RMS Error 

 ( y  yˆ ) i

2

i

i 1

(6.1)

n

It takes similar expression as standard deviation [98], hence gives a direct indication of accuracy of the estimator, in the same units as actual quantity. The other quantitative quality indicator is the error per 100m (i.e. percent error of the total distance travelled). Both of these indicators are given in comparison tables.

6.4 Indoor Environment Test To verify our proposed techniques of navigation drift reduction in part-II of this dissertation, the experiments are conducted in both outdoor and indoor environment, with no prior information about the test environment. The IMU (3DM-GX3) from Microstrain Inc. [97] is used for data collection in all experiments. Although this IMU provides many kinds of navigation data, but only raw accelerometer and gyro data is picked up and rest of the navigation solution and data processing is done in our own designed fusion algorithm. The IMU was fitted on the left foot and the walker walked with the normal walking speed. An indoor test was conducted in a three-floor building in KITECH, with double flight stairs to change the floor. In Figure 6.3, the approximate reference path is drawn manually in 2D and 3D for visual inspection. In Figure 6.4, the red trajectory (STH aided) keeps the parallelism and perpendicularity of the building layout, but blue trajectory is not aligned with building layout. The STH un-aided trajectory (blue line) has drift, especially in heading. The simple technique, i.e. STH greatly reduces the heading drift in this indoor environment test. The reason 137

is that most buildings have straight corridors and hence STH is more effective in indoor environment. It shows the effectiveness of simple STH method applied, without using any other sensor to reduce errors in overall localization in unstructured environments. The numerical results of the test are summarized in Table 6.1 below.

138

Figure 6.3: Multi-floor Indoor test. For ease of visualization, an approximate reference path manually drawn on the building image (front and top view) taken from Google Earth.

139

Trajectory 6 4 2 0

Start point STH-Applied STH-Not-Applied End point

100

80

100

Floor 2-to-3 80

y [m]

60

60

40 40

Floor 1-to-2

20 20 y [m]

0

0

-40

-20

0

20 x [m]

40

60

0

80

10

20

30

40

x[m]

Figure 6.4: 2D and 3D trajectory plot comparison in indoor test (3-floor building in KITECH). Effect of STH applied is clearly seen in red trajectory, which has less cross-track divergence and hence keeps the parallelism and perpendicularity corresponding to the building layout.

140

Table 6.1. RMS error reduction by applying STH technique for indoor environment test. Method STH Not applied STH applied Improvement

2D RMS Error[m] 4.01


2.12

2.50

89%

100%

Remarks Time=15 min., Distance=380m. Error/100m  0.8m

6.5 Outdoor Environment Tests Similar analysis is done in long time outdoor tests as well, as shown in following sections. First test is conducted on relatively flat ground around a lack, more than 1km long. In second test, a closed track walk is performed in an urban street to check the effectiveness of proposed algorithms in this dissertation. Third test is carried out in a thick forest, with initial and final position at the same point, and proposed techniques are applied to reduce drift in INS based PNS.

6.5.1 Outdoor Test with Arbitrary Walk In first outdoor experiment, the walker deliberately changed walking direction randomly (Figure 6.5), e.g. in segment A, walking is back-footed; in segment B, walking is side-steps, and similarly in segment C, user walked with side-steps, changing directions 180 degree randomly, in order to check the robustness of the designed algorithm to track position of the wearer.

141

Trajectory

B

Start point No STH STH End point

250

C

x [m]

200

150

A 100

50

0 -200

-150

-100

-50

0 y [m]

50

100

150

Figure 6.5: Outdoor test near KITECH, covering 1100m (aprox.) in 25 min. The reference path is manually drawn on the image taken from Google Earth.

142

In Figure 6.5, are shown the results of applying the STH algorithm as an aiding virtual sensor to drift reduction in zero velocity aided INS (i.e. ZUPT+INS). On the left in Figure 6.5, is shown the real trajectory (about 1100m) drawn manually, for visual verification and on the right, is the estimated path. It is shown that when we apply STH technique, the trajectory (red) estimated with INS+ZUPT+STH algorithm is more like to the real one as compared to the trajectory when there is no STH applied. For quantitative comparison, we checked the final position errors (RMSE) and shown in Table 6.2. Numerical results show that percent error is less than 2m of the total distance travelled, in this long outdoor experiment on relatively flat ground. Table 6.2. RMS error reduction by applying STH technique. Method STH Not applied STH applied Improvement

2D RMS Error[m] 47.01 18.32

3D RMS Error[m] 49.52 21.34

61%

56%

Remarks Time=25 min., Distance=1100m. Error/100m  1.5m

6.5.2 Urban Canyon Environment Test To test the proposed techniques, using virtual sensors, for reducing drift in ZUPT aided INS, we conducted one long-time test in the urban environment, Figure 6.6. In such environment, GPS signals are vulnerable to blockage and/or large reflections due to tall buildings and narrow passages. The start and end point was same while performing this test and the final position errors are calculated to verify the improvement achieved by proposed technique. During this long test, at some points, the user take rest for more than a minute and hence PA-locking mechanism applies as well. It is shown in Chapter 5 (Section 5.3.3) that in standing still condition, ZUPT alone cannot constraint the drift in position and 143

attitude. Hence in standing still for more than 5seconds is chosen as a condition for applying the PA-locking techniques in this dissertation. Numerical results of this test are compared in Table 6.3.

144

Trajectory 200 150

Start point STH+PA:Apply STH+PA:NotApply End point

100

x [m]

50 0 -50 -100 -150

-250

-200

-150

-100

-50 y [m]

0

50

100

150

200

Figure 6.6: Urban environment test: The reference path (on left) is manually drawn on the image taken from Google Earth. On right is the estimated trajectory by proposed algorithm.

145

Table 6.3. RMS error if STH and PA techniques are applied in urban canyon environment.

STH+PA Not applied



STH+PA applied

14.427

19.797

Improvement

84.27%

49.23%

Method

Remarks Time=45.min. Distance=1000m. Error/100m  1.5m

6.5.3 Forest Canopy Environment Test To test the proposed techniques, using virtual sensors, for reducing drift in ZUPT aided INS, we conducted one long-time test in the dense forest environment, one the harsh environment candidate in this dissertation. In such environment, GPS signals are vulnerable to blockage and/or reflections due to forest canopy; hence GPS cannot be used for reliable positioning in such harsh environment. The test environment is covered with dense and tall trees, hence well fitted in navigationally harsh environment category. For visual verification, the reference trajectory drawn manually, taken during test in forest is shown in Figure 6.7. The estimated trajectory with (red) and without (blue) using STH and PA-locking technique is shown on the right half in Figure 6.7. It is observed that STH has good impact on position estimation as compared to ZUPTs only to EKF, as the trajectory shape is more likely the actual path taken during this experiment. The quantitative comparison in terms of RMS error is given in Table 6.4.

146

Trajectory 300

Start point STH+PA:Apply STH+PA:NotApply End point

250 200 150

x [m]

100 50 0 -50 -100 -150 -200 -300

-200

-100

0 y [m]

100

200

300

Figure 6.7: Forest environment test: The reference path (on left) is manually drawn on the image taken from Google Earth. On right is the estimated trajectory by proposed algorithm.

147

Table 6.4. RMS error if STH and PA techniques are applied.

STH+PA Not applied



Time=40.min.

STH+PA applied

8.0627

20.606

Distance=11000m.

Improvement

202.9%

46.52%

Error/100m  1m

Method

Remarks

6.6 Experimental Set-up and Evaluation Method for MAD algorithm 6.6.1 Static Test In this test, we conducted two tests to check the performance of our proposed algorithm(s) with the fixed reference angle and conventionally designed AHRS (Microstrain 3DM-GX3-45) AHRS. In the first test we kept IMU stationary for more than one hour, in fixed orientation and recorded data from all three sensors, i.e. accelerometer, gyros, and magnetometer. In static test, all three techniques are applicable, i.e. ZUPT, PA-Locking and MAD as well. It is seen in Figure 6.8-6.10 and from Table 6.5, that application of these simple algorithmic techniques, without using any extra hardware, is extremely effective to reduce the drift in low-cost inertial sensor based orientation calculation. By applying these simple techniques, the error in attitude estimation is even less than the COTS Microstrain‟s AHRS. Hence these simple algorithmic approaches can reduce cost too of the application at hand.

148

Roll angle 25 Microstrain

Roll angle

Gyro-only

Microstrain

1.6

ZUPT+PA+MAD

Gyro-only 1.5

20

ZUPT+PA+MAD

1.4 1.3 1.2

Angle[deg]

Angle[deg]

15

1.1 1

10

0.9 0.8

5

0.7 0.6 0

500

1000

1500

2000

2500

3000

3500

time[s]

0 0

500

1000

1500

2000

2500

3000

3500

4000

time[s]

Figure 6.8: Roll angle estimate in long time static test. On right, is shown the zoomed-in view for visual inspection. Pitch angle 60 Microstrain Pitch angle

Gyro-only 50

0 Microstrain

ZUPT+PA+MAD

Gyro-only -0.1

ZUPT+PA+MAD

40

30 Angle[deg]

Angle[deg]

-0.2

20

-0.3

-0.4

-0.5

10 -0.6

0 0

500

1000

1500

2000

2500

3000

3500

time[s]

-10 0

500

1000

1500

2000

2500

3000

3500

4000

time[s]

Figure 6.9: Pitch angle estimate in long time static test. On the right, is shown the zoomed-in view for visual inspection. Heading angle 155 Heading angle

150

151.6 151.4

145

151.2 151

140 Angle[deg]

Angle[deg]

150.8

135

130

150.6 150.4 150.2 150

125

149.8

Microstrain Gyro-only

149.6

Microstrain

120

ZUPT+PA+MAD

Gyro-only

500

ZUPT+PA+MAD

1000

1500

2000

2500

3000

3500

time[s]

115 0

500

1000

1500

2000

2500

3000

3500

4000

time[s]

Figure 6.10: Heading angle estimate in long time static test. On the right, is shown the zoomed-in view for visual inspection.

149

Table 6.5. Maximum error observed in long time static test. Method ZUPT+PA-Lock+MAD Not-applied ZUPT+PA-Lock+MAD applied

Max. Error[deg] Roll

Pitch

Heading

20

50

32

0.1

0.1

0.2

In the 2nd static test, we kept the IMU (Microstrain‟s 3DM-Gx3-45) stationary and passed a permanent magnet across the sensor back and forth, by approaching from different directions (Figure 6.11). The permanent magnet bar was passed four times close (

Drift Reduction for Navigation in Harsh Environment

Drift Reduction for Navigation in Harsh Environment

Suggest Documents

Drift Reduction in Pedestrian Navigation System

RF-Based Location System in Harsh Environment

Ground vehicle navigation in harsh urban conditions by ... - CiteSeerX

A Harsh Environment Wireless Pressure Sensing ... - BioMedSearch

A Harsh Environment-Oriented Wireless Passive

A Gas Sensor System for Harsh Environment Applications

robust designed capacitive gas pressure sensor for harsh environment

Nozzle Classification for Drift Reduction in Orchard Spraying ...

Nozzle Classification for Drift Reduction in Orchard ... - WUR E-depot

Model 970-1036 Industrial Sensor for Harsh Environment (Throttle ...

Using Open Hardware in a Harsh Environment: A ...

soi sensing technologies for harsh environment - SOI-HITS

Cable Material Reliability for Industrial and Harsh Environment ...

System Design Issues for Harsh Environment ... - IEEE Xplore

System Design Issues for Harsh Environment ... - IEEE Xplore

harsh-environment fiber optic sensors for structural ...

Page 1 ARCTIC R&D IN NEW FOUNDLAND Harsh-environment ...

Online Environment Reconstruction for Biped Navigation - kuffner.org

SMART ENVIRONMENT FOR UBIQUITOUS INDOOR NAVIGATION

Autonomous navigation in a manufacturing environment - Robotics

Robust Navigation In GNSS Degraded Environment

Camera navigation support in a virtual environment

Harsh War, Harsh Peace - Human Rights Watch

Drift Compensated Coding Optimization for Fast Bit-rate Reduction ...