Design of Multi-Sensor-Based Open Architecture Integrated Navigation System for Localization of UGV

Abstract

The UGV is one of the special field robot developed for mine detection, surveillance and transportation. To achieve successfully the missions of the UGV, the accurate and reliable navigation data should be provided. This paper presents design and implementation of multi-sensor-based open architecture integrated navigation for localization of UGV. The presented architecture hierarchically classifies the integrated system into four layers and data communications between layers are based on the distributed object oriented middleware. The navigation manager determines the navigation mode with the QoS information of each navigation sensor and the integrated filter performs the navigation mode-based data fusion in the filtering process. Also, all navigation variables including the filter parameters and QoS of navigation data can be modified in GUI and consequently, the user can operate the integrated navigation system more usefully. The conventional GPS/INS integrated system does not guarantee the long-term reliability of localization when GPS solution is not available by signal blockage and intentional jamming in outdoor environment. The presented integration algorithm, however, based on the adaptive federated filter structure with FDI algorithm can integrate effectively the output of multi-sensor such as 3D LADAR, vision, odometer, magnetic compass and zero velocity to enhance the accuracy of localization result in the case that GPS is unavailable. The field test was carried out with the UGV and the test results show that the presented integrated navigation system can provide more robust and accurate localization performance than the conventional GPS/INS integrated system in outdoor environments.

1. INTRODUCTION

Unmanned ground vehicle (UGV) is a vehicle that is controlled without a person, but can be controlled remotely from a station. Also, by using information about surrounding terrains, the UGV is the automatic driving system which can plan its route (Pinn & Scheding 2010). UGV is expected to be useful in variety of tasks that requires extreme caution, such as mine detection, repetitive surveillance work and transportation. In order to successfully accomplish such tasks, accurate and reliable navigation data of an UGV should be provided, and the accuracy of location data plays managera crucial role when the UGV is set to an automatic driving mode (Chena 2010).

Just as a typical system, Satellite Navigation using a global positioning system (GPS) receiver or GPS/INS integrated navigation can be considered as a possible means to provide the location data of an UGV. However, it is impossible to obtain location data from a GPS-based system when the satellite signal reception is interfered, and in an environment where GPS is disabled, using GPS/INS integrated navigation system to overcome such disadvantage also is prone to increased error over time due to continuous jamming. Therefore, for a reliable localization of an UGV in an outdoor environment where the terrains can interfere with satellite signal reception, or in a field of war where jamming effect must put in the consideration, it is necessary to build a navigation system that is not dependent on GPS. UGV is equipped with 3D laser detection and ranging (LADAR), frequency-modulated continuous wave (FMCW) radar and camera for its automatic driving mode and the surveillance tasks (Madhavan & Messina 2003). If such sensor information is combined with GPS/INS integrated navigation system, it is possible to obtain a reliable localization data even in an environment where GPS cannot function properly (Park et. al. 2009).

This paper presents a design and implementation of multi-sensor-based open architecture integrated navigation for localization of an UGV. Mainly, a integrated navigation system for land vehicles includes a line-replaceable unit (LRU) that incorporates inertial measurement unit (IMU), GPS receiver and navigation computer, and an odometer in the manner of synchronizing with the LRU through outer interface (Hadfield 1988). This type of navigation system has an advantage of optimizing its size and weight, but is difficult to modify or reconstruct its hardware and software when adding and integrating other navigation sensors. Therefore, in order to effectively integrate localization data, which is obtained by 3D LADAR, FMCW Rader and Camera installed in an UGV, velocity information and azimuth data obtained from magnetic compass and odometer, it is necessary to build an open architecture navigation system. For this purpose, this paper proposed an open architecture integrated navigation system with layers. Algorithms for each navigation software component was devised and implemented for each layers. The data communication between the layers and each component is operated based on a distributed object oriented middleware, which is a software layer between the OS and the program in the distributed computing environment on the networks.

The integrated navigation system proposed in this paper has adapted the concept of navigation manager in order to manage and integrate data obtained from variety of sensors. A navigation manager is built on a GUI basis, and includes functions to set parameter, showing real-time data, changing navigation mode, and post-processing for its functionality analysis to effectively operate unit navigation and integrated navigations. An integrated filter was devised based on a Federated Kalman Filter to combine multi-sensor data (Carlson 1990, Carlson 1998, Gu & Fang 2009), and in order to enhance the reliability of the system, fault detection and isolation (FDI) algorithm was included.

The devising of an open architecture integrated navigation system is explained in Section 2. In this Section, the structure of open-architecture integrated navigation system and integrated navigation algorithm based on Federated Kalman Filter was explained, and the devising of navigation manager was explained as well. In Section 3, the result for performance test is included, and Section 4 concludes this paper.

 

2. DESIGN OF OPEN-ARCHITECTURE INTEGRATED NAVIGATION SYSTEM

2.1 Open-Architecture Integrated Navigation Structure

In multi-sensor integration, using sensors that has different characteristics of operation can improve the signal strength of integrated navigation, and by using pre-obtained information such as a map can improve the accuracy of the navigation greatly. This paper suggests an open-architecture integrated navigation structure using variety of navigation sensors installed in an UGV, as appear in Table 1. The suggested Integrated Navigation Structure is devised to enable continuous integrated navigation even in an environment without GPS availability by using localization data that is obtained by synchronizing data obtained from 3D LADAR and camera, and pre-obtained environmental database (DB), such as digital elevation model (DEM), image map.

Table 1. The operating characteristics of navigation sensors installed in an UGV.

Navigation Sensor	Operating Characteristics
(D) GPS   IMU (INS)   Camera   3D LADAR   FMCW Rader   Magnetic Compass   Odometer	· Radio navigation · Position and velocity · Self-contained · Position, velocity, and attitude · Self-contained · Position using image matching with a prior MAP · Self-contained · Position using 3D range registration with DEM · Self-contained · Range and relative position · Self-contained · Direction of magnetic north · Self-contained · Velocity (longitudinal direction)

 

According to the operating characteristics of each sensors (indicated in Table 1), the open architecture integrated navigation structure was proposed, as appeared in Fig. 1. The integrated navigation structure is classified into 4 layers, which includes unit sensor layer, unit navigation layer, integrated navigation layer, and navigation service layer according to its operative characteristics. The unit navigation layer is composed of navigation sensor hardware, but other layers are composed of software components for computing navigation algorithm. The data that is obtained from unit sensor layer is sent to unit navigation layer for integration at the integration filter, after the data has been added of time information that is based on the data acquirement time of IMU. Unit navigation layer is composed of sensor-based navigation, vision-based navigation, terrain-based navigation, and SLAM after considering the characteristics of sensor data, algorithm, and computation amount. Each component computes the algorithm by using pre-obtained environment DB and navigation information from higher component. In integrated navigation layer, the navigation manager component determines Navigation mode either by the user setting, or after combining road description file (RDF), which includes the road information and QoS information from each sensor, and the Integration Filter does the data integration according to the decision of navigation mode by the navigation manager. Navigation service layer sends the navigation results that were calculated from the integrated navigation layer to automatic driving components such as world modeling, path planning, terrain perception, autonomous control, and integrated control.

Fig. 1. Open architecture integration structure.

Each sensor in unit sensor layer and components in unit navigation layer, as indicated in Fig. 1, manage the communication via physical interfaces such as universal asynchronous receiver transmitter (UART), synchronous data link control (SDLC), universal serial bus (USB), and Ethernet. The sensor navigation, vision navigation, terrain navigation and SLAM component in Unit Navigation Layer is operated at an independent single board computer (SBC) node, with each SBC connected to gigabyte Ethernet hub. Data communication among each component and between each component and integrated navigation layer is operated via the distributed object oriented middleware. Therefore, each layer and component communicate the data only via their network interface in a distributed computer environment, and this enables each component of independent addition/modification of Unit sensor and Modification/Amendment of Component Algorithm, thus enhancing the openness of integrated navigation.

As indicated in Fig. 2, Navigation Manager in Integrated Navigation Layer uses velocity information of the UGV, and distinguishes the navigation modes into either zero-velocity update (ZUPT) mode or Non-ZUPT mode. At this time, a velocity meter measures the RPM of in-wheel motor of the vehicle to determine if the velocity of UGV is zero. Therefore, using the velocity information provided by the velocity meter as the filtering cycle of integrated filter, the filter state-variable and covariance are integrated by determining ZUPT or Non-ZUPT mode.

Fig. 2. ZUPT mode and non-ZUPT mode switch.

Fig. 3 indicates the navigation mode in Non-ZUPT mode. Localization mode is classified into DGPS/INS, GPS/INS, and integration mode, depends on the QoS condition of the navigation data. In Non-ZUPT Mode, the navigation manager carries out data integration using QoS information obtained from the navigation data. (D)GPS provides QoS information such as dilution of precision (DOP), but other sensors do not provide QoS information. Therefore, in order to determine QoS of each sensor, chi-square test was performed. Generally, the QoS of navigation data in an outdoor environment varies greatly according to its surrounding terrains and DB accuracy. Therefore, in order to compensate for the variability, the critical value that has been determined through repetitive tests was set. The vision navigation and terrain navigation component can estimate current location of UGV by matching with the environmental DB even though the GPS is disabled. However, if the environmental DB is inaccurate, due to increased error in localization, it is impossible to estimate the exact location of the UGV. In such situation, the navigation manager switches localization mode into SLAM mode to determine the location of UGV using relative location estimation.

Fig. 3. Navigation mode in non-ZUPT mode.

Usually, in the outdoor environment, the initial location of UGV is determined as the output of GPS receiver when INS is initially aligned. However, when the satellite signal is cut off, then it is impossible to determine the initial location based on GPS. Therefore, the proposed integrated navigation structure utilizes a method that is indicated in Fig. 4 to determine the initial location of the UGV. If the navigation manager was unable to obtain the initial location via GPS while the INS is initially aligned, it will request information of initial location of UGV to vision component in unit navigation layer, and then to the terrain navigation component. If the unit navigation layer is unable to set its initial layer, then the navigation manager automatically sets the last location where the UGV finished its navigation previously as the initial location, then initiates the integrated Navigation, and as soon as the GPS gets available, it uses GPS localization data to carry on the integrated navigation.

Fig. 4. Setting initial location of UGV.

2.2 Integrated Navigation Algorithm

Fig. 5 indicates the integrated navigation algorithm that is proposed in this paper. This utilizes Strapdown INS (SDINS) as the main navigation system, and it utilizes (D)GPS, vision (stereo/panoramic camera), 3D LADAR, Velocity sensor, and Magnetic Compass as supplementary information. Integrated Navigation Algorithm is built with NR (No-Reset) Mode Federated Filter, which is easy to detect defects while enabling enhanced system integration, and consists of 6 local filters (with ZUPT) and one master filter. Also, FDI algorithm was applied to ensure the stability of Integrated Navigation System from malfunctioning or defect of the sensor.

Fig. 5. Federated Kalman filter-based integrated navigation algorithm.

SDINS, the main navigation system computes location, velocity, and position information by integrating acceleration and angular velocity that are obtained from IMU. In order to combine INS and supplementary sensors using integrated filter, INS navigation error model was necessary. In this discussion, psi (\(\psi\)) angle error model was used, which is a widely accepted model for a prolonged navigation. The equation for location, velocity, and position error can be expressed as following in the navigation frame (Benson, Jr. 1975, Goshen-Meskin & Bar-Itzhack 1992).

\(\delta \dot{\mathbf{p}}^n = -\boldsymbol{\omega}_{en}^n \times \delta \mathbf{p}^n + \delta \mathbf{v}^n\)                                                                                                  (1)

\(\delta \dot{\mathbf{v}}^n=\mathbf{f}^n \times \delta \Psi^n -(2\boldsymbol{\omega}_{ie}^n + \boldsymbol{\omega}_{en}^n ) \times \delta \mathbf{v}^n + \mathbf{C}_b^n\delta \mathbf{f}^b\)                                                                                 (2)

\(\delta \dot{\boldsymbol{\Psi}}= - \boldsymbol{\omega}_{in}^n \times \delta \boldsymbol{\Psi} - \mathbf{C}_b^n \delta \boldsymbol{\omega}_{ib}^b\)                                                                                                (3)

The symbols \(\delta \mathbf{p}^n , \delta \mathbf{v}^n \) and \(\delta\boldsymbol{\Psi}\) are symbols for the location error, velocity error, and position error, respectively. \(\mathbf{f}^n\) is acceleration vector, \(\delta \mathbf{f}^b\) is acceleration error vector, \(\delta \boldsymbol{\omega}_{ib}^b\) is gyro error vector, \(\mathbf{C}_b^n\) is coordinate transformation matrix from body frame (b) to navigation frame (n), \(\boldsymbol{\omega}_{ie}^n\) is earth rotation rate, \(\boldsymbol{\omega}_{en}^n\) is craft rate, and \(\boldsymbol{\omega}_{in}^n\) is angular velocity of navigation frame according to inertial frame. Modeling after setting IMU error as "random bias" in Eqs. (2) and (3) enables the error equation as the following.

\( \left [ \begin{matrix} \dot{\mathbf{x}}_{nav} \\ \dot{\mathbf{x}}_{IMU} \end{matrix} \right ] = \left [ \begin{matrix} \mathbf{F}_{11} & \mathbf{F}_{12} \\ \mathbf{0}_{6 \times 9} & \mathbf{0}_{6 \times 6} \end{matrix} \right ] \left [\begin{matrix} {\mathbf{x}}_{nav} \\ {\mathbf{x}}_{IMU} \end{matrix} \right ] + \left [\begin{matrix} {\mathbf{w}}_{nav} \\ {\mathbf{w}}_{IMU} \end{matrix} \right ] \)                                                                             (4)

At this equation \(\mathbf{x}_{nav} , \mathbf{x}_{IMU} ,\mathbf{F}_{11},\) and \(\mathbf{F}_{12}\) are defined as below.

\( \mathbf{x}_{nav} \equiv \left [ \begin{matrix} \delta p_{_N} & \delta p_{_E} & \delta p_{_D} & \delta v_{_N} & \delta v_{_E} & \delta v_{_D} & \delta \alpha & \delta \beta & \delta \gamma \end{matrix} \right ]^T \)                                                                   (5)

\( \mathbf{x}_{imu} \equiv \left [ \begin{matrix} \delta f_x & \delta f_y & \delta f_z & \delta \omega_x & \delta \omega_y & \delta \omega_z \end{matrix} \right ]^T \)                                                                                 (6)

\( \mathbf{F}_{11} = \left [ \begin{matrix} 0 & \rho_{_D} & -\rho_{_E} & 1 & 0 & 0 & 0 & 0 & 0 \\ -\rho_{_D} & 0 & \rho_{_N} & 0 & 1 & 0 & 0 & 0 & 0 \\ \rho_{_E} & -\rho_{_N} & 0 & 0 & 0 & 1 & 0 & 0 & 0 \\ 0 & 0 & 0 & 0 & 2\Omega_{_D}+\rho_{_D} & -(2\Omega_{_E}+\rho_{_E}) & 0 & -f_{_D} & f_{_E} \\ 0 & 0 & 0 & -(2\Omega_{_D}+\rho_{_D}) & 0 & 2\Omega_{_N}+\rho_{_N} & f_{_D} & 0 & f_{_N} \\ 0 & 0 & 0 & 2\Omega_{_E}+\rho_{_E} & -(2\Omega_{_N}+\rho_{_N}) & 0 & -f_{_E} & f_{_N} & 0 \\ 0 & 0 & 0 & 0 & 0 & 0 & 0 & \Omega_{_D}+\rho_{_D} & -(\Omega_{_E}+\rho_{_E}) \\ 0 & 0 & 0 & 0 & 0 & 0 & -(\Omega_{_D}+\rho_{_D}) & 0 & \Omega_{_N}+\rho_{_N} \\ 0 & 0 & 0 & 0 & 0 & 0 & \Omega_{_E}+\rho_{_E} & -(\Omega_{_N}+\rho_{_N}) & 0 \\ \end{matrix} \right ] \)                         (7)

\( \omega_{en}^n = \left [ \begin{matrix} \rho_{_N} & \rho_{_E} & \rho_{_D} \end{matrix} \right ]^T = \left [ \begin{matrix} { v_{_E} \over R_{_p} +h } & - { v_{_N} \over R_{_m} +h } & - { v_{_E} \tan L \over R_{_p} +h } \end{matrix} \right ]^T \)                                                                           (8)

\( \boldsymbol{\omega}_{ie}^n = \left [ \begin{matrix} \Omega_N & \Omega_E & \Omega_D \end{matrix} \right ]^T = \left [ \begin{matrix} \Omega \cos L & 0 & -\Omega \sin L \end{matrix} \right ]^T \)                                                                               (9)

\( \mathbf{F}_{12} = \left [ \begin{matrix} \mathbf{0}_{3 \times 3} & \mathbf{0}_{3 \times 3} \\ \mathbf{C}_b^n & \mathbf{0}_{3 \times 3} \\ \mathbf{0}_{3 \times 3} & -\mathbf{C}_b^n \end{matrix} \right ] \)                                                                                                   (10)

In this equation, \(L\) is latitude, \(\Omega\) is earth rotation rate.

The measuring equation for a local filter is expressed as the following.

\(\mathbf{Z}_i(k)=\mathbf{H}_i(k)\mathbf{x}_i(k)+\mathbf{v}_i(k)\)                                                                                            (11)

In this equation, \(i\) indicates \(i^{\textrm {th}}\) local filter, \(\mathbf{z}_i(k)\) is the measured value, \(\mathbf{x}_i(k)\) is state-variable, \(\mathbf{H}_i(k)\) is a measuring matrix that combines measured value and state-variable, and \(\mathbf{v}_i (k)\) is measurement noise. For the 1^st local filter’s devising, since \(\mathbf{z}_i (k)\) is the difference of GPS and INS position and velocity, thus we have devised the 1st local filter as following.

\( \mathbf{X}_1 (k) = \left [ \begin{matrix} \mathbf{X}_{nav} & \mathbf{X}_{IMU} \end{matrix} \right ]^T \)                                                                                            (12)

\( \mathbf{z}_1 (k) = \left [ \begin{matrix} \delta p_N & \delta p_E & \delta p_D & \delta v_N & \delta v_E & \delta v_D \end{matrix} \right ]^T \)                                                                            (13)

\( \mathbf{H}_1 (k) = \left [ \begin{matrix} \mathbf{I}_{6 \times 6} & \mathbf{0}_{6 \times 9} \end{matrix} \right ] \)                                                                                              (14)

Since \(\mathbf{z}_2 (k)\) and \(\mathbf{z}_3 (k)\) is the difference of position between vision/3D LADAR based localization system and INS in navigation frame, 2^nd and 3^rd local filters are built as the following.

\( \mathbf{X}_2 (k) = \mathbf{X}_3 (k) = \left [ \begin{matrix} \mathbf{X}_{nav} & \mathbf{X}_{IMU} \end{matrix} \right ]^T \)                                                                                  (15)

\( \mathbf{z}_2 (k) = \mathbf{z}_3 (k) = \left [ \begin{matrix} \delta p_N & \delta p_E & \delta p_D \end{matrix} \right ]^T \)                                                                                  (16)

\( \mathbf{H}_2 (k) = \mathbf{H}_3 (k) = \left [ \begin{matrix} \mathbf{I}_{3 \times 3} & \mathbf{0}_{3 \times 12} \end{matrix} \right ] \)                                                                                  (17)

Since \(\mathbf{z}_4 (k)\) is the velocity difference of velocity between odometer and INS in navigation frame, the 4^th local filter is built as the following.

\( \mathbf{X}_4 (k) = \left [ \begin{matrix} \mathbf{X}_{nav} & \mathbf{X}_{IMU} & \delta v_{SF} \end{matrix} \right ]^T \)                                                                                            (18)

\( \mathbf{z}_4 (k) = \left [ \begin{matrix} \delta v_N & \delta v_E & \delta v_D \end{matrix} \right ]^T \)                                                                                      (19)

\( \mathbf{H}_4 (k) = \left [ \begin{matrix} \mathbf{0}_{3 \times 3} & \mathbf{I}_{3 \times 3} & \left [ \begin{matrix} 0 & v_D & -v_E \\ -v_D & 0 & v_N\\ v_E & -v_N & 0 \end{matrix} \right ] & \mathbf{0}_{3 \times 6} & - \left [ \begin{matrix} v_x C_{11} \\ v_x C_{21} \\ v_x C_{31} \end{matrix} \right ] \end{matrix} \right ] \)                                                            (20)

In this equation \({\delta v}_{SF}\) is the error of scale factor in the odometer. In Eq. (20), the velocity of the odometer is calculated as the following using the RPM of in-wheel motor.

\( v_x = {r \over 4n} \times {2\pi \over 60 } \times \left [ \sum\limits_{i=1}^6 \omega_i (t) - \left ( \omega_{\max}(t) + \omega_{\min}(t) \right ) \right] \)                                                                        (21)

\(r\) is the wheel’s radius, \(n\) is deceleration ratio, and \(\omega\) is the RPM of the motor, and the maximum and minimum RPM have been excluded. The UGV used in the discussion uses 6×6 in-wheel motor system, so 6 motor RPMs are used.

Since \(\mathbf{z}_5 (k)\) is the difference of azimuth between magnetic compass and INS in navigation frame, 2^nd and 3^rd local filters are built as the following (Rogers 2000).

\( \mathbf{X}_5 (k) = \left [ \begin{matrix} \mathbf{X}_{nav} & \mathbf{X}_{IMU} & \delta \psi_B \end{matrix} \right ]^T \)                                                                                            (22)

\( \mathbf{z}_5 (k) = \delta \psi \)                                                                                                                (23)

\( \mathbf{H}_5 (k) = \left [ \begin{matrix} \mathbf{0}_{1 \times 6} & -\tan \theta \cos \psi & -\tan \theta \sin \psi & -1 & \mathbf{0}_{1 \times 7} \end{matrix} \right ] \)                                                                       (24)

\({\delta\psi}_B\) is the Bias Error of Magnetic Compass’ Azimuth.

Local filters are expressed using the linearized Kalman filter expressed below. Eqs. (25) and (26) indicate time propagation, and Eqs. (27-29) indicate measurement update.

\( \mathbf{x}_i (t_k^-) = \Phi_i(t_k,t_{k-1}) \hat{\mathbf{x}}_i (t_{k-1}^+) \)                                                                                              (25)

\( \mathbf{P}_i (t_k^-) = \Phi_i(t_k,t_{k-1}) \mathbf{P}_i (t_{k-1}^+) \mathbf{\Phi}_i^T (t_k,t_{k-1}) + \mathbf{Q}_i(t_{k-1}) \)                                                                            (26)

\( \mathbf{K}_i (t_k) = \mathbf{P}_i (t_k^-) \mathbf{H}_i^t (t_k) \left [ \mathbf{H}_i (t_k) \mathbf{P}_i (t_k^-) \mathbf{H}_i^T (t_k) + \mathbf{R}_i (t_k) \right ]^{-1} \)                                                                      (27)

\( \hat{\mathbf{x}}_i (t_k^+) = \hat{\mathbf{x}}_i (t_k^-) + \mathbf{K}_i (t_k) \left [ \mathbf{z}_i (t_k) - \mathbf{H}_i(t_k) + \hat{\mathbf{x}}_i (t_k^-) \right ] \)                                                                            (28)

\( \mathbf{P}_i (t_k^+) = \left [ \mathbf{I} - \mathbf{K}_i (t_k) \mathbf{H}_i(t_k) \right ] \mathbf{P}_i (t_k^-) \)                                                                                    (29)

\(​​\mathbf{P}\) is covariance matrix, \(\mathbf{K}\) is Kalman gain, and the upper exponent ⁺ means a posteriori filter value, and the upper exponent ¯ means a priori filter estimate value.

The Master filter uses state-variable and covariance that is estimated from local filtersas quasi-observable value, and combines them as follows.

\( \mathbf{P}_M (t_k^+) = \left [ \sum\limits_{i=1}^N \{ \mathbf{A}_i^T \mathbf{P}_i^{-1} (t_k^+) \mathbf{A}_i \} \right ]^{-1} \)                                                                                            (30)

\( \hat{\mathbf{x}}_M (t_k^+) = \mathbf{P}_M (t_k^+) \left [ \sum\limits_{i=1}^N \{ \mathbf{A}_i^T \mathbf{P}_i^{-1} (t_k^+) \hat{\mathbf{x}}_i (t_k^+) \} \right ] \)                                                                                      (31)

In these equations, \(M\) indicates the master filter, and \(\mathbf{A}_i\) indicates a matrix consists of 0 and 1 to match the dimensions of state-variables between nth local filter and the master filter. Therefore, the master filter includes every single state-variables, and \(\mathbf{x}_M\) and \(\mathbf{A}_i\) are equal as the equation indicated below.

\( \mathbf{X}_M (k) = \left [ \begin{matrix} \mathbf{X}_{nav} & \mathbf{X}_{IMU} & \delta v_{SF} & \delta \psi_B \end{matrix} \right ]^T \)                                                                                            (32)

\( \mathbf{A}_1 = \mathbf{A}_2 = \mathbf{A}_3 = \left [ \begin{matrix} \mathbf{I}_{15 \times 15} & \mathbf{0}_{15 \times 2} \end{matrix} \right ] \)                                                                                              (33)

\( \mathbf{A}_4 = \left [ \begin{matrix} \mathbf{I}_{15 \times 15} & \mathbf{0}_{15 \times 1} & \mathbf{0}_{15 \times 1} \\ \mathbf{0}_{1 \times 15} & 1 & 0 \end{matrix} \right ] \)                                                                                                   (34)

\( \mathbf{A}_5 = \left [ \begin{matrix} \mathbf{I}_{15 \times 15} & \mathbf{0}_{15 \times 1} & \mathbf{0}_{15 \times 1} \\ \mathbf{0}_{1 \times 15} & 0 & 1 \end{matrix} \right ] \)                                                                                                   (35)

Eqs. (36-38) are indicative of FDI algorithm of each local filter that is based on a chi-square test. FDI algorithm in each local filter senses distribution of the residual, and determines any deviances in output of supplementary sensors (Brumback 1987).

\(\zeta_k = \sum\limits_{i=k-N+1}^k \mathbf{v}_i^T \mathbf{S}_i^{-1} \mathbf{v}_i \sim \chi_{Np}^2\)                                                                                          (36)

\(\mathbf{v}_i=\mathbf{z}_i(t_k)-\mathbf{H}_i(t_k)\mathbf{\hat{x}}_i(t_k^-)\)                                                                                             (37)

\(\mathbf{S}_i =\mathbf{H}_i(t_k) \mathbf{P}_i(t_k^-) \mathbf{H}_i^T(t_k)+\mathbf{R}_i(t_k)\)                                                                                       (38)

\(\zeta_k\) is a test statistics with chi-square distribution, \(\mathbf{v}_i\) is residual, and \(\mathbf{S}_i\) indicates the distribution of the residual. In this discussion, we set the window size \(N\) to be 1 to have 95% validity.

2.3 Navigation Manager

Navigation Manager enables users to set parameter, and it is also able to show the real-time data, change navigation mode, and post-process for its functionality analysis. The GUI for navigation manager is composed of the navigation status on the left side, a window that shows real time/post-process navigation selection, a window that indicates if each navigation component and navigation manager that are operated in a distributed computer environment are connected together. The right side tab is for setup for integrated navigation, data list for each navigation component, and a map window that provides a real time data of location, velocity, and azimuth. Fig. 6 is the navigation manager and the map window. Integrated navigation and Unit navigation results can be expressed in a same map using different colors. Also, the indicated location and map scale can easily be changed in 2D location, altitude, velocity, and azimuth graphs.

Fig. 6. Navigation manager and map window.

Fig. 7 indicates the setup and data list window. Through the setup window, the user can modify INS operation period, integrated filter operation period, filter parameters, measurement noise for each unit navigation, and threshold and QoS value for FDI algorithm operation. The data list shows real-time result of integrated navigation and each of unit navigations.

Fig. 7. Setup and data list window.

3. TESTING RESULTS

In order to confirm the performance of proposed integrated navigation system, a test using a UGV platform that is indicated in Fig. 8 was done. The sensors used in the performance test are indicated in Table 2, and DGPS is used as a reference navigation system to measure the accuracy of localization by integrated navigation.

Fig. 8. UGV platform.

Table 2. UGV navigation sensor.

Item	Navigation sensor
GPS DGPS IMU 3D LADAR Magnetic Compass Odometer	NovAtel, OEMV2-L1 Navcom, SF-2050M Honeywell, HG1700AG58 Velodyne, HDL-64E S2 Honeywell, HMR3500 In-Wheel Motor RPM sensor

 

Fig. 9 showed a navigation result integrating IMU, 3D LADAR, odometer and magnetic compass. Though GPS information was not used, the actual route of UGV and the reference route showed similar results. Fig. 10 indicates the horizontal localization differences compared with the reference. 3D LADAR-based terrain navigation had 5.198 m RMS, maximum 13.624 m of error, and the integrated navigation showed 4.859 m RMS, maximum 11.486 m of difference. According to the test result, though GPS was not available, using information obtained from supplementary sensors such as 3D LADAR, the system was able to correct INS error to avoid navigation error and estimate the accurate location of the UGV. Fig. 11 is the result that integrates also DGPS navigation information, which is used as a reference in Fig. 9’s test. As shown in Fig. 12, the off-track difference of integrated system is much reduced. This is because of the weighting factors of local filters by Eqs. (30) and (31). From this experiment, the presented integration algorithm can perform a reliable estimation of UGV location in an outdoor environment where the GPS is not available, and the integrated system can come up with the more effective navigation result with the multi-sensor fusion.

Fig. 9. Comparison of localization result when GPS information was not available.

Fig. 10. Horizontal localization difference when GPS information was not available.

Fig. 11. Comparison of localization result after applying also DGPS Information.

Fig. 12. Horizontal localization difference after applying also DGPS information.

4. CONCLUSION

This paper proposed an open architecture integrated navigation system that is based on multi-sensor to enable a reliable and trustworthy localization of an UGV, and tested the system derived from the proposal. The integrated navigation structure was classified into 4 different layers, and by operating software components of each layer independently, the openness of integrated navigation was achieved. The Integrated Algorithm that is based on distribution Kalmal Filter in Integrated Navigation Layer can set the navigation mode based on QoS of the Unit Navigation Data, and by computing the data available from every sensor, the system was able to provide the most effective navigation result. GUI-based navigation manager can monitor unit navigation data using gigabyte Ethernet under distributed computer environment, and the openness of the integrated navigation system has been enhanced by enabling the users to modify every variable to operate the integrated navigation via GUI navigation manager. A performance test utilizing a UGV platform equipped with (D)GPS, INS, 3D LADAR, magnetic compass and odometer was done, and the result showed that the integrated navigation system is able to localize the UGV regardless of the availability of the GPS by effectively integrating sensor data. The effectiveness of the integrated navigation structure proposed in this paper is only verified in the localization mode, but further experiment and research should enhance the performance of integrated navigation by combining SLAM mode.