Combining visual, pedestrian, and collaborative navigation techniques for team based infrastructure free indoor navigation

Abstract

In this paper the authors describe the design and evaluation of a multi sensor integrated navigation system specifically targeted at teams of cooperating users operating in transient indoor conditions such as would be encountered by emergency services personnel or soldiers entering unknown buildings. Since these conditions preclude the use of dedicated indoor infrastructure the system depends on the combination of multiple self contained navigation sensors as well as dynamic networking and ranging between the users to form a decentralized cooperative navigating team. Within this paper we will discuss the design and evaluation of a system developed within a North Atlantic Treaty Organization (NATO) Science for Peace and Security (SPS) project executed by the SINTEF and the Finnish Geospatial Researcher Institute (FGI) during 2018 and 2019. The motivation of this project was to combine the expertise of the FGI in pedestrian and camera based infrastructure free navigation with the collaborative navigation and integrated navigation system design expertise of SINTEF towards the accurate navigation and continuous situational awareness of teams of cooperating users. When completed, the combined navigation system will be a shoulder mounted package which comprises a triple frequency GNSS receiver for rapid outdoor initialization, as well as a Micro Electro Mechanical System (MEMS) Inertial Measurement Unit (IMU), barometer, magnetometer, three different navigation and communication radios as well as a stereo vision plus depth sensing camera connected to and synchronized by an integrated processor platform. Two of the three radios provide for user-to-user range measurement and data exchange via each of 2.4 GHz and Ultra Wide-Band (UWB) signals to allow for collaborative navigation as well as situational awareness within the network, while the 3 rd radio provides a link to separate navigation sensors such as a foot mounted IMU pod for enhanced Pedestrian Dead Reckoning (PDR). The integrated camera provides stereo color imaging as well as structured light based infrared depth sensing, while the processor platform is responsible for data collection and processing. Introduction The motivation in pursuing infrastructure free navigation systems relates to the fact that certain classes of user including firefighters, law enforcement, soldiers and others must enter hazardous indoor environments on short notice and without detailed knowledge of the interior structure, layout or contents of these buildings. Additionally, since the building might be on fire or otherwise denied electrical power, reliance on even ad-hoc infrastructure such as Wi-Fi routers may not be a reliable source of navigation data. Assuming that the building materials block the majority of GNSS signals to the users, the remaining options are typically those sources of information that are self-contained to the individual user such as inertial sensors and visual odometry (VO) to allow each user to navigate free of infrastructure, as well as leveraging the collective network via user to user radio links to realize collaborative navigation within the team. Background The Infrastructure-free tactical situational awareness (INTACT) project, funded by the Finnish Scientific Advisory Board for Defence (MATINE) for years 2015-2017, analyzed and developed methods for infrastructure–free simultaneous localization and mapping (SLAM) and context recognition for tactical situational awareness using only measurements obtained from small and low-cost MEMS sensors mounted on the body of the user. More precisely, during the project error analysis, and estimation methods were developed for obtaining accurate and reliable horizontal position solution fusing measurements from inertial sensors and computer vision and vertical position solution from fusing barometer and sonar observations [1]. Machine learning was used for detecting the user motion and thereby adjusting the estimation parameters and thresholds for improved solution [2]. At the end of the project a proof-of-concept was carried out at the military premises in Finland by two soldiers. Computation of the fused navigation solution was complicated by exposing inertial sensors and the camera to atypical motion and harsh impacts, such as jumps, running and climbing stall bars sideways. The final result, accuracy being 1% of the travelled path, was analyzed to be comparable with state-of-the-art infrastructure-free navigation solutions made by walking forward along a largely straight path [3]. SINTEF had previously conducted multiple projects exploring the feasibility of team based navigation in outdoor-indoor building entry scenarios, and through work funded by the Norwegian Battle Lab and Experimentation (NOBLE), prototype shoulder mounted navigation systems comprising GNSS, inertial and dual user-to-user range estimating radio modules which allowed direct implementation and testing of the collaborative navigation concepts explained in the next subsection. In these initial studies the navigation performance of each individual user was enhanced, relative user to user error was reduced, and the situational awareness of the overall team status was greatly enhanced through the periodic forwarding of 3 rd party user status when performing ranging cycles, allowing a hypothetical supervisor or vehicle mounted node outside the building to serve as both a reference anchor for ranging but also to maintain knowledge of the entire team even when Line-of-Sight (LOS) communication was blocked to most team members. While the INTACT project and collaborative navigation projects both achieved respectable performance during their respective testing, the individual systems had notable drawbacks, such as requiring illumination within the environment to be relatively high for proper operation of the visual odometry within the INTACT system, and the long term systemic drift of the collaborative navigation systems when the entire team was isolated from absolute position reference for an extended period of time. Before moving further, a more detailed explanation of the techniques that are to be combined in this study are now presented. CORE TECHNIQUES The combined SINTEF FGI navigation system, and the tests conducted in this study rely on several sources of navigation data throughout a typical outdoor-indoor trajectory. While GNSS is only used during system initialization, and barometry provides only a height constraint, the primary sources of Position Velocity and Attitude (PVA) estimation are derived from Collaborative Navigation, VO, and PDR, the implementations of which are now discussed. Collaborative Navigation Collaborative navigation is based on an idea of using team members as local beacons. By measuring distance to other team members and utilizing those measurements along with shared location information, the navigation solution can be enhanced especially in GNSS-challenged environments. Collaborative navigation approaches can provide position estimate in a global coordinate frame also to team members without access to GNSS signals, given that at least some team members are able to use satellite navigation [4]. Even if the whole team has no GNSS signal available, as can happen for instance in indoor environments, they can estimate their absolute and relative positions using the range constraints along with the estimates formed by their inertial navigation sensors. The position estimation algorithm in collaborative navigation can be either centralized or de-centralized [5]. In the centralized approach, all measurements made by the team members are transmitted to some central processing unit. The unit computes the position estimates and transmits them back to the collaborators. In de-centralized approach each team member uses only measurements made locally, and position plus range estimates broadcast by other team members which can be directly communicated with. Compared to centralized position estimation, the de-centralized approach requires less communications over the network, scales better with the size of the team [5][6], and is tolerant of extended gaps in communication between individuals or groups of users. The key element in collaborative navigation is distance measurements between the team members. UWB ranging suits well for the application at hand, as it is tolerant to multipath and can also be used through walls up to some extent [7]. However, this can make sensor fusion more challenging as in Non-Line-of-Sight (NLOS) situations the UWB distance measurement error is not necessarily Gaussian [8], which is a requirement for Extended Kalman Filter (EKF) [9] commonly used in navigation applications. Overbounding Gaussian distributions can be used in the EKF but this approach does not necessarily provide optimal results [10]. Without GNSS clock synchronization between the ranging devices can be difficult to maintain, but by using Two-Way Time-of-Arrival (TOA) distance measurements or synchronizing a sufficiently stable local oscillator when GNSS is available the requirement of clock synchronization can be avoided. In this project, a completely de-centralized implementation is adopted as the target environments are those where point to point communication will be unreliable, and therefore centralized processing of data with reasonable latency is not considered feasible. In this de-centralized implementation users periodically announce their presence to other users in range, who keep an updated list of which users are recently visible and therefore considered valid targets for ranging and communication. Multiple-access for up to 32 nodes is achieved through time slicing based on user addresses, with synchronization of the mobile nodes to a common time base achieved via use of the onboard GNSS receiver during initialization, and carried forward by a local oscillator with stability sufficient to maintain valid access patterns for ove