IMPROVED REFERENCE KEY FRAME ALGORITHM

The autonomous vehicles, such as wheeled robots and drones, efficiently contribute in the search and rescue operations. Specially for indoor environments, these autonomous vehicles rely on simultaneous localization and mapping approach (SLAM) to construct a map for the unknown environment and simultaneously to estimate the vehicle’s position inside this map. The result of the scan matching process, which is a key step in many of SLAM approaches, has a fundamental role of the accuracy of the map construction. Typically, local and global scan matching approaches, that utilize laser scan rangefinder, suffer from accumulated errors as both approaches are sensitive to previous history. The reference key frame (RKF) algorithm reduces errors accumulation as it decreases the dependency on the accuracy of the previous history. However, the RKF algorithm still suffers; as most of the SLAM approaches, from scale shrinking problem during scanning corridors that exceed the maximum detection range of the laser scan rangefinder. The shrinking in long corridors comes from the unsuccessful estimation of the longitudinal movement from the implemented RKF algorithm and the unavailability of this information from external source as well. This paper proposes an improvement for the RKF algorithm. This is achieved by integrating the outcomes of the optical flow with the RKF algorithm using extended Kalman filter (EKF) to overcome the shrinking problem. The performance of the proposed algorithm is compared with the RKF, iterative closest point (ICP), and Hector SLAM in corridors that exceed the maximum detection range of the laser scan rangefinder.


INTRODUCTION
The autonomous vehicles, such as wheeled robot and drones, efficiently participate in search and rescue operations. These vehicles save humans lives by performing the hazardous missions and tasks. Typically for indoor missions, navigation of these vehicles, in unknown environments, is addressed by the simultaneous localization and mapping approach (SLAM) (Santos et al., 2013). This is accomplished by constructing a map for the unknown environment and simultaneously estimating the vehicle's position inside this map (Thrun and Leonard, 2008) (Bresson et al., 2017) and (Aulinas et al., 2008). SLAM approaches that utilize laser rangefinders depend on a scan matching method of the successive scans (Mohamed et al., 2016). The scan matching process is a key step in the SLAM approaches. This process can adopt a local or global scan matching (Diosi and Kleeman, 2005) and (Bosse and Zlot, 2008). The result of the scan matching process has a fundamental role of the accuracy of the map construction. Both local and global scan matching approaches suffer from accumulated errors because both approaches are affected by the matching errors that occur in previous scans (Hess et al., 2016). Various examples, such as iterative closest point (ICL) (Besl and McKay, 1992), iterative matching range point (IMRP) (Lu and Milios, 1994), iterative dual correspondence (IDC) (Lu and Milios, 1994), polar scan matching (PSM) (Diosi and Kleeman, 2005), iterative closest line (ICL) (Alshawa, 2007), and Hector SLAM (Kohlbrecher et al., 2011), represent local and global scan matching methods and algorithms. All these methods and algorithms suffer from error accumulation over time.
On the other hand, reference key frame (RKF) algorithm proposed 2D real-time scan matching algorithm that depends on a sole sensor, laser scan rangefinder, and does not require external aided sensors (Mohamed et al., 2017). The RKF algorithm attempts to mitigate the accumulated errors that contaminate the local and global scan matching. The RKF algorithm reduces the dependency on the accuracy of the previous result of the scan matching process. This is achieved by computing the transformation matrix parameters with respect to the previous key frame instead of the previous scan as discussed later in Section 2.
However, laser scan matching process in long corridors, that exceed the maximum detection range of the laser scan rangefinder, is a challenging task for vehicle's navigation within unknown environment (Diosi and Kleeman, 2005). The orientation and the lateral movement of the vehicle can be estimated from the scan matching process, while the estimation of the longitudinal movement is challenging as there is no clue for the forward movement. Although the RKF algorithm mitigates the accumulated errors, the algorithm suffers from scale shrinking problem during long corridors. This is due to the shortage of detection of two non-parallel lines as required by the algorithm. As a result, the RKF algorithm depends on the ICP algorithm alone during the non-parallel lines-outage period. Thus, the shrinking in some corridors comes from the unsuccessful estimation of the longitudinal movement from the implemented ICP algorithm and the unavailability of this information from external source as well. This paper proposes an improved version of the RKF algorithm (IRKF) to overcome the shrinking problem. This is achieved by integrating the outcomes of the optical flow with the RKF algorithm using extended Kalman filter (EKF). This paper is organized as follows: Section 2 summarizes the RKF algorithm. Section 3 illustrates an overview of the integrated navigational system. The used methodology is demonstrated in Section 4. The experimental results are presented in Section 5 and finally the conclusion is given in Section 6. (Mohamed et al., 2017) proposed the reference key frame (RKF) algorithm. This algorithm depends on extracting lines from the laser scanner point cloud. Then, two non-parallel lines are elected from the extracted lines to be the mapping reference lines. The scan matching process is performed with the current reference lines and their matching lines in each successive scan. As a result of the vehicle's movement, new two non-parallel lines are elected when the previous reference lines are not detected anymore. Hence, transformation parameters between the previous and current reference lines are computed to preserve the mapping continuity. Thus, the scan matching process is performed with the kernel reference lines during the entire mission. This process is carried out as long as nonparallel lines are detected. In case of lines outage, the ICP algorithm is conducted as a point-to-point scan matching technique. In addition to the computation of the transformation parameters of the entire map, the transformation parameters during the ICP period are computed separately. This is performed to preserve continuity for the new reference lines, once detected, with the entire map. The algorithm switches back to the RKF algorithm once linear features are detected as shown in Figure 1.

OVERVIEW OF THE INTEGRATED NAVIGATIONAL SYSTEM
Orientation-compensated translational velocities in metric scale are provided using optical flow sensor called PX4FLOW (Honegger et al., 2013). These velocity values aid the proposed IRKF algorithm to estimate the longitudinal movement of the unmanned aerial vehicle (UAV) especially in the long corridors. Therefore, the laser scan rangefinder and optical flow sensors are fused in an integrated navigational system of the UAV. As a result, the estimated 2D position and heading of the UAV from the laser scan rangefinder are fused with the velocities computed from the optical flow sensor by EKF to provide 2D navigational solution of the UAV and the bias of the optical flow sensor as shown in Figure 2.

UAV Hardware System Architecture
A quadcopter of X-configuration is utilized during the test experiments. The proposed hardware architecture consists of two main sub-systems, namely navigation and autopilot subsystems as shown in Figure 3. The navigation sub-system includes low-cost 2D laser scan rangefinder (LS), optical flow sensor (OF) accompanied with sonar, and mini PC, while the autopilot subsystem comprises NAVIO 2 autopilot and laser rangefinder (LRF) attached to raspberry pi 2 B embedded system board. Figure 3. The two main sub-systems.

Navigation sub-system:
The utilized low-cost laser scan rangefinder is RPLIDAR 360°. This sensor is developed by RoboPeak and produces 2D point cloud data. The maximum detection range is 6m, and the field of view is 360°. The employed optical flow sensor is PX4FLOW (pixhawk). The PX4FLOW comprises imaging sensor (camera), ultrasonic rangefinder (sonar), and low-cost MEMS 3-axes gyro. An ARM Cortex M4 processor is also mounted on the sensor board in order to process the optical flow approach at a subsampled resolution of 64x64 pixels. The optical flow sensor provides 2D velocities with respect to the body frame (Honegger et al., 2013). The optical flow measurements are computed between the consecutive image frames. The sum of absolute differences (SAD) algorithm is utilized to find the similarity between two blocks of the same size (Niitsuma and Maruyama, 2010). The data acquisition is implemented in the mini PC.

Autopilot sub-system:
The NAVIO 2 autopilot board is equipped with dual IMU (MPU9250 9DOF and LSM9DS1 9DOF), magnetometer, barometer (MS5611), U-blox M8N GPS/Glonass/Beidou receiver. This autopilot board requires external embedded system, for instance Raspberry Pi, for code execution and development. The altitude of the UAV is estimated using PulsedLight laser rangefinder. This laser rangefinder sensor is composed of a single laser beam with maximum detection range of 40 m.

Data Fusion by Extended Kalman Filter (EKF)
Extended Kalman Filter (EKF) (Gelb, 1974) have been employed to integrate sensory information from laser scan rangefinder and optical flow sensor, to provide an improved 2D navigational solution over using individual sensor information.
The EKF describes the kinematic model of the UAV using the following first order state equation: The two terms on the right-hand side in Equation (1) respectively define the dynamic model and the stochastic model. The dynamic model describes how states develop over time, while the stochastic model defines the uncertainty in the dynamic model. In this research work, eight states are estimated, as presented in Equation (2); each state is represented with respect to the mapping frame.
Therefore, the corresponding discrete-time linear system can be expressed as follows: where = estimated state vector , −1 = transition matrix The laser scan rangefinder and the optical flow sensors are utilized as external observations (i.e. updates) for the system. The discrete form of the measurement equation that describes the relationship between the observations and the estimated state vector is defined as: where = observations vector = design matrix = estimated state vector = observations noise = current epoch Figure 4 depicts the work flow diagram of the EKF algorithm.
The navigation system has two updates. The first update measurements ( , , ) are provided by the RKF algorithm using the laser scan rangefinder, while the second update measurements ( , ) are provided by the optical flow sensor.
Since the utilized RKF algorithm alternates between two algorithms; namely RKF and ICP algorithms, the covariance matrix of the measurement noise ( , ) changes according to the selected algorithm. Moreover, both algorithms have the same design matrix . The optical flow update design matrix ( ) and covariance matrix of the measurement noise ( ) are given in Equations (32, 34). Otherwise, the navigation system is idle until receiving updates. The inputs of the EKF such as transition matrix, covariance matrix of the system noise, covariance matrix of the measurement noise, design matrices for both updates, and the measurements vectors, should be prepared as described in the next sub-sections.

Computation During Laser Scan Rangefinder (LS) Update
The where = design matrix of the LS

Covariance Matrix of RKF algorithm:
As mentioned before, the proposed RKF algorithm comprises two methods, namely RKF and ICP algorithms. According to the experiments results in (Mohamed et al., 2017), the RKF algorithm exhibits more robustness than the ICP algorithm. Therefore, each method has its corresponding covariance matrix of the measurement noise ( ). Since the RKF algorithm shows lower uncertainty in measurements, the RMSE between the successive scans, after performing the scan matching process, are used to parameterize the covariance matrix of the measurement noise ( ).
where = covariance matrix of the measurement noise of the RKF algorithm = root mean square error between successive scans

Covariance Matrix of ICP algorithm:
The ICP algorithm is employed during the unavailability of two nonparallel lines, for instance in corridors, or lines outage environment. In contrast, the ICP algorithm shows high uncertainty in measurements especially in the longitudinal direction and low uncertainty in measurements elsewhere. This is due to, in contrary to feature-to-feature scan matching, the point-to-point scan matching is prone to outlier association (Aghamohammadi and Taghirad, 2007).
Furthermore, the lack of evidence that supports the longitudinal movement negatively affects the certainty of the measurements. As a result, for the localization, the new positions of the UAV appear to be stationary, which is not the real situation. Thus, the next point clouds are approximately accumulated at the same position, causing shrinking in the corridor, until a new evidence of the forward direction is caught. In order to assist in tackling the shrinking problem of the long corridor, the Eigenvalues and Eigenvectors of one of the two parallel lines (i.e., corridor) are used to compute the covariance matrix of the line after transforming the Eigenvectors from the body frame to the mapping frame. The computed covariance matrix is lent to the covariance matrix of the position measurement noise as shown in Figure 5. In this figure, the yellow rectangle represents the unchanged position of the UAV (i.e., shrinking area), while the blue dashed oval represents the lent covariance matrix. The implemented trajectory of the UAV is represented by the red dashed line. Figure 5 also demonstrates that the corridor is not necessarily aligned with the mapping frame axes. Thus, the covariance matrix of the position measurement noise is fully populated as given in Equation (24). The covariance matrix of the position measurement noise is computed using the Eigenvalue and Eigenvector equation as follows: where 1 = the corresponding Eigenvector of the largest Eigenvalue 2 = the corresponding Eigenvector of the smallest Eigenvalue 1 = the largest Eigenvalue corresponds to the longitudinal movement (high uncertainty) 2 = the smallest Eigenvalue corresponds to the lateral movement (low uncertainty) = the covariance matrix Therefore, the covariance matrix of the measurement noise is given as: where = covariance matrix of the measurement noise of the ICP algorithm = root mean square error between successive scans From Equation (28), the scale of the heading variance is manually tuned according to the environment due to the high uncertainty of the scan matching result from the ICP algorithm.

Computation During Optical Flow (OF) Update
On the other hand, the computed velocities from the optical flow sensor are used as second external observation. Since the velocities are computed in the body frame, the optical flow observations are transformed to the mapping frame using the computed transformation matrix from the RKF algorithm. Thereafter, the transformed velocities are used as updates according to: The quality strength is encapsulated in one byte; the quality value ranges from 0 to 255. The highest quality is 255 and the quality reduces until reach 0. Therefore, the corresponding covariance matrix of the measurement noise is computed as follows: where = covariance matrix of the measurement noise of the OF = the received quality from the sensor = variance adjustment

EXPERIMENTAL RESULTS
A comparison has been performed to study the performance of the proposed algorithm amongst various algorithms, such as ICP algorithm, Hector SLAM, corner registration, and RKF algorithm, in corridors that exceed the maximum detection range of the laser scan rangefinder, 6m. The dataset is gathered at the University of Calgary CCIT building. All the experimental figures are to scale representation. The actual length of the corridor is 25 m.
The mapping and position results of the ICP algorithm are illustrated in Figure 6. The blue points represent the constructed map, while the red asterisks represent the vehicle's trajectory. The ICP algorithm suffers from the shrinking problem because of the lack of evidence for longitudinal movement. Furthermore, the impact of the accumulated errors leads up to inclination in the corridor. The ICP algorithm estimates 70% of the actual length. Three level multi-resolution map representations, using grid cell dimensions 20, 10, and 5 cm, have been utilized to evaluate Hector SLAM approach. Figure 7 shows the mapping and position results using Hector SLAM of resolution 5 cm. The blue points represent the constructed map, while the red asterisks represent the vehicle's trajectory. It also suffers from the same problem and for the same reason. The Hector SLAM method estimates 65.2% of the actual length.  The blue points represent the constructed map, while the red asterisks represent the vehicle's trajectory. Although the RKF algorithm exhibits improvement in the corridor scale, it also suffers from the shrinking problem. The improvement comes from the idea of matching of two non-parallel lines (i.e. reference frame), the black arrows show the places that the RKF algorithm gets the second line with the main line of the corridor. The RKF algorithm estimates 75.2% of the actual length.  Figure 9 shows the mapping and position results for the proposed IRKF algorithm. The blue points represent the constructed map, while the red asterisks represent the vehicle's trajectory. It is obvious that the shrinking problem does not occur in the vertical corridor since the optical flow sensor aids the proposed IRKF algorithm by providing measurements of the longitudinal movement especially in the corridor. Furthermore, the certainty of the laser scan rangefinder update has also been maintained for the lateral direction only. The proposed algorithm estimates 99.2% of the actual length.

CONCLUSION
In long corridors that exceeds the maximum detection range of the laser scan rangefinder, the probability of detecting nonparallel lines is typically low. Thus, the RKF algorithm suffers from scale shrinking problem during scanning such corridors. The shrinking in long corridors comes from the lack of evidence in the forward direction and the unavailability of this information from external source as well. Consequently, the next detected point clouds are approximately accumulated at the same position during the outage of two non-parallel lines. Once a forward clue is detected by the laser scan rangefinder, the estimated position of the UAV starts to move forward again. Thus, this outage period causes a shrink in the corridor structure and therefore affects the entire dimensions of the map. Therefore, extra source such as optical flow sensor is added to the navigational system. The optical flow sensor provides orientation compensated velocities in the x and y directions in the body frame. After transforming the compensated velocities to the mapping frame, EKF algorithm is used to fuse the computed velocities with the estimated 2D position and the heading of the UAV from the RKF algorithm using laser scan rangefinder. The computed velocities aid in estimating the position of UAV especially during navigation in the long corridors. Therefore, the integration between the RKF algorithm and the observations of the optical flow sensor overcomes the shrinking problem. The percentage of the estimation error of the proposed algorithm is 0.8%, while the estimation error of the ICP algorithm, corner registration, Hector SLAM, and RKF are 30%, 29.6%, 34.8%, 24.8% respectively.