Introduction UAV Navigation: Airborne Inertial SLAM Jonghyuk Kim Faculty of Engineering and Information Technology Australian National University, Australia Salah Sukkarieh ARC Centre of Excellence in Autonomous Systems The University of Sydney, Australia In this presentation we are interested in large and fast outdoor vehicles working in unstructured environments which typically require state information at high update rates. For such systems we need to go beyond thinking of SLAM as just a form of information about a vehicle and a map, but to also see SLAM as the feedback mechanism to the Guidance, Navigation and Control (GNC) system. 1 Jonghyuk KIM 2 Jonghyuk KIM Introduction Introduction We are focusing on systems that: Require complete 6DoF state information for control and navigation purposes; Requires this information at high update rates (>50Hz) for low level control (even greater for measuring higher order vehicle dynamics); Require the localisation system to have a level of information redundancy, that is not solely relying on a single sensor, but instead fusing information easily from various sources. A significant important property of aided inertial navigation system is that they are independent of the platform kinematics since they only measure its dynamics, that is, a true black-box localisation system. In this presentation we will focus on how SLAM is used to constrain the drift of an INS, thus also providing localisation autonomy as well as platform independency. Given the use of we also open up the vast body of literature on this topic which has been generated over the past 50 years, and ask further questions on computational efficiency, observability and optimal trajectories. 3 Jonghyuk KIM 4 Jonghyuk KIM
Introduction Introduction If one looks at how localisation systems for such vehicles have been formulated in the past, the core sensing device has been an Inertial Measurement Unit (IMU). These units measure acceleration and rotation rates of a platform at high update rates, which can then be transformed and processed to provide the position, velocity and attitude of a platform, resulting in an System (INS). The data presented by the INS is fed to the GNC system to achieve autonomy. However, the solutions from the INS, just like any other form of dead-reckoning system, drifts with time and hence requires other absolute sensory information to be fused with the INS solution in order to constrain this drift, resulting in Aided Inertial Navigation. Examples include GPS and terrain maps. A significant important property of aided inertial navigation system is that they are independent of the platform kinematics since they only measure its dynamics, that is, a true black-box localisation system. In this presentation we will focus on how SLAM is used to constrain the drift of an INS, thus also providing localisation autonomy as well as platform independency. Given the use of we also open up the vast body of literature on this topic which has been generated over the past 50 years, and ask further questions on computational efficiency, observability and optimal trajectories. 5 Jonghyuk KIM 6 Jonghyuk KIM Presentation Outline Inertial Sensors and Inertial Measurement Units Inertial Sensors and Inertial Measurement Units Aided Structure Example Systems come in various shapes and size, ranging from accuracies of 0.001km/wk to 10km/hr depending on the application types. In such systems, the sensing devices are three accelerometers and three gyros mounted orthogonally to each others. The gyros measure the rotation rate of the platform from which the navigation system can determine what the attitude of the platform is with respect to some navigational reference frame. The accelerometers measure the body acceleration of the platform which can then be transformed to the navigation frame by using attitude information. This acceleration can then be twice integrated to provide the velocity and position of the platform within navigation frame. 7 Jonghyuk KIM 8 Jonghyuk KIM
INS Functional Block Diagram Frame Mechanisation 9 Jonghyuk KIM 10 Jonghyuk KIM Aided Structures Aided Structures The aiding of inertial navigation can take on two structures, direct or indirect. The direct structure fuses the inertial navigation solutions generally the position and velocity data with position and/or velocity information from the external source. An Extended Kalman Filter is what is used to fuse the data. The indirect implementation perturbs the inertial equations to formulate inertial error equations. These result in linearised model and hence a standard linear Kalman Filter can be used. Three significant benefits arise when using the indirect implementation. The equations have slower dynamics then when compared to the raw inertial state solution since we are now dealing with errors, thus requiring prediction less often. The Kalman filter implementation also allows for a significant reduction in computational resources. Given the linear nature of the system, we can now ask questions about the observability structure of SLAM, and more readily implement SLAM in autonomous decision making for Guidance and Navigation. We will begin our formulation of using the EKF as this is what is commonly used in SLAM research. We will then look at the indirect implementation. 11 Jonghyuk KIM 12 Jonghyuk KIM
Presentation Outlines Vehicle and Map State Vectors Inertial Sensors and Inertial Measurement Units Aided Structure Example Augmented State and Covariance Matrix 13 Jonghyuk KIM 14 Jonghyuk KIM Navigation Frame Mechanisation Vehicle Model: The Observations Feature Location: Observation: Map Model: 15 Jonghyuk KIM 16 Jonghyuk KIM
New Feature Augmentation Innovation Gate State Augmentation Covariance Augmentation 17 Jonghyuk KIM 18 Jonghyuk KIM Presentation Outlines Example Inertial Sensors and Inertial Measurement Units Aided Structure Example Start at unknown location with no a priori map information. Predict motion through INS. Make relative observations to local features and build a map through these observations. Predict and re-observe features which are in the map and begin to correlate. Correlation assists in constraining drift in INS. Update the vehicle and feature estimates at each observation. 19 Jonghyuk KIM 20 Jonghyuk KIM
Example Example 21 Jonghyuk KIM 22 Jonghyuk KIM Example Example Post-Processed Results 23 Jonghyuk KIM 24 Jonghyuk KIM
Example Example Real-Time Results 25 Jonghyuk KIM 26 Jonghyuk KIM Example Example 27 Jonghyuk KIM 28 Jonghyuk KIM
Example Example Inertial Sensors and Inertial Measurement Units Aided Structure Example 29 Jonghyuk KIM 30 Jonghyuk KIM The previous slides illustrated the process and results of using inertial SLAM on an airborne system. As part of the process of demonstrating this system, questions were asked of the accuracy required from the sensors in order to constrain the drift of the inertial system, and for certain accuracy requirements of the map. This led to asking from where particular error sources crept into the system and ho how they could be constrained. This section of the presentation will provide an overview of some of this work. 31 Jonghyuk KIM 32 Jonghyuk KIM
33 Jonghyuk KIM 34 Jonghyuk KIM Presentation Outlines Inertial Sensors and Inertial Measurement Units Aided Structure Example 35 Jonghyuk KIM 36 Jonghyuk KIM
In the previous slides which focused on sensor characteristics we were provided with the opportunity of perturbing the inertial equations. The equations developed allowed us to see how errors in the inertial navigation system propagate through the rest of the initialised feature positions. More importantly however, the process of perturbing the equations and developing this error formulation allows us to pose the question of whether a SLAM formulation can be developed from these equations. The answer is yes, and the reason why one would like to conduct such a process is because of the computational efficiency and the insight that such a process provides. The next set of slides will highlight how this efficiency takes place, and and the following set of slides on the insight such a formulation provides. 37 Jonghyuk KIM 38 Jonghyuk KIM Indirect SLAM flowchart Comparison between Direct and Indirect 39 Jonghyuk KIM 40 Jonghyuk KIM
Presentation Outlines Comparison between Reduced Rates Indirect Inertial Sensors and Inertial Measurement Units Aided Structure Example 41 Jonghyuk KIM 42 Jonghyuk KIM The indirect formulation of the SLAM algorithm opens up the door to the vast literature of linear control theory. We now have a linear time varying system which can be modelled as a piecewise constant system. Thus, we can now ask what the observability of SLAM is in a more mathematically precise fashion. This not only provides us with insight into the structure of the information flow, but also allows us to venture into the areas of control theory for looking at optimal technique for closing-the-loop and for observability enhancement. State Model (single feature) Observation Model Acceleration Matrix 43 Jonghyuk KIM 44 Jonghyuk KIM
45 Jonghyuk KIM 46 Jonghyuk KIM From the structure of Q SOM (j) the following can be observed: The observability of SLAM at the j th segment, Q SOM (j), depends on all preceding segments including the current one. The order in which the piecewise constant segments are arranged has no effect on the final observability of the system. The first and second rows in Q SOM (j) provide six linearly independent rows and repeating these at later segments does not provide any further linearly independent rows, thus does not enhance the observability of the system. [f n j ] in each segment can provide only two linearly independent rows. From a simple physical interpretation between the velocity and attitude errors, the latter can be decomposed into both an observable and an unobservable state. The observable part of the attitude error is perpendicular to the acceleration vector and can effect the velocity error, while the unobservable part is parallel to the acceleration vector. When the vehicle is either stationary (such as in the alignment process) or when it is not accelerating, the acceleration is just due to gravity (hence vertical). In this case, the unobservable state corresponds to the heading error. 47 Jonghyuk KIM However, when the vehicle undergoes acceleration along the other axes, the unobservable state is no longer in the vertical axis and is the combination of the three Euler angles. That is, the unobservable state is in the direction of the acceleration vector. Accordingly H ip [f n j ] can provide only two linearly independent rows at each segments. However, if [f n j ] changes between separate segments, for example through a platform maneuver, it can enhance the observability. What is interesting is that the range, bearing and elevation observation in SLAM can only provide a rank of 8 or 9 when the vehicle undergoes sufficient maneuvers such as accelerating, decelerating or turning. Since 6DoF SLAM always has a rank deficiency of three regardless of the number of landmarks within the filter, additional uncorrelated position observations that can provide three independent rows to the SOM can make SLAM fully observable. 48 Jonghyuk KIM
Presentation Outlines Inertial Sensors and Inertial Measurement Units Aided Structure Example 49 Jonghyuk KIM 50 Jonghyuk KIM Given a number of previously observed features and a collection of un-visited regions, how does the vehicle decide to go next? Decision making will be based on maximising the entropic mutual information gain (I) in both the vehicle pose and map location estimates. Information gain for taking an action can be calculated from the covariance of both vehicle pose and map states: In order to find the expected mutual information gain (I) of visiting an unknown area or re-visiting a feature, the current estimation covariance must be propagated (through the discrete Ricatti equation) along the expected path to the time of expected feature observation Prediction Propagation: Covariance propagation from initial time (t 0 ) to time of observation (t obs ). That is P(t 0 t 0 ) to P(t obs t 0 ). Observation Update: Covariance update for expected observation at t obs. That is, P(t obs t 0 ) to P(t obs t obs ). where, P(t 0 t 0 ) is the covariance of the estimates at the time of deciding an action to take, P(t obs t obs ) is the covariance of the estimates once the action has been taken, x is the vector of vehicle and map estimates and z is a feature observation. 51 Jonghyuk KIM 52 Jonghyuk KIM
P(t obs t obs ) is calculated by applying an information update for the expected feature observation. When re-observing a previoulsy seen feature, the covariance is calculated via: When visiting an un-explored region, expected P(t obs t obs ) is calculated by the equations for augmenting a new feature into the estimator, the location of which is given at the center of the region. When comparing to the prior covariance P(t obs t 0 ) before initializing the expected feature, P(t obs t obs ) remains same dimension and thus the information gain from the time of decision (t 0 ) will only be the information loss in the vehicle state while travelling to the region. In order to quantify some information gain in visiting an un explored region and initializing a feature, the initial covariance P(t 0 t 0 ) is augmented to account for low initial information about a yet to be seen feature: where U m are appropriately chosen large, unseen feature position uncertainties. 53 Jonghyuk KIM 54 Jonghyuk KIM 55 Jonghyuk KIM 56 Jonghyuk KIM
Conclusion and Future Works Further Readings The focus of this presentation was on introducing you to the area of inertial SLAM. Inertial SLAM poses a number of significant advantages as it provides for the capability of platform independence as a localisation system. In this presentation we saw the formulation of the SLAM problem in the standard EKF approach along with its indirect implementation. This indirect approach allowed for significant reductions in computation with minimal loss in accuracy, and also provided a more through understanding of how SLAM works. We also saw the beginnings of higher level decision theory in ins application to closing the loop. Immediate areas of interest include linking the observability measure closer into the decision making and improving the consistency in large scale application. Kim J.. Autonomous Navigation for Airborne Applications. PhD Thesis, Australian Centre for Field Robotics, The University of Sydney, Australia, 2004. Bryson M, Kim J. and Sukkarieh S., Information and Observability Metrics of Inertial SLAM SLAM for On-line Path-planning on an Aerial Vehicle, In IEEE International Conference on Robotics and Automation (ICRA 05), Barcelona, Spain, April 18-22, 2005. Kim J., S. Ong, E. Nettleton and S. Sukkarieh. Decentralised approach to unmanned aerial vehicle navigation without the use of global positioning system and preloaded map. In Journal of Aerospace Engineering,Institute of Mechanical Engineering, vol. 218, Part (G), Pages: 399-416, 2004. Kim J. and S. Sukkarieh. Autonomous Airborne Navigation in Unknown Terrain Environments. In IEEE Transactions on Aerospace and Electronic Systems, 40(3):1031-1045, July 2004. Kim J. and S. Sukkarieh. Recasting SLAM - Towards Improving Efficiency and Platform Independency. In International Symposium on Robotics Research (ISRR 03), Siena, Italy, 2003. 57 Jonghyuk KIM 58 Jonghyuk KIM