6.4.1. Localization

To test our new localization module (described in Section 5) we followed different paths through the campus. These routes were recorded in manual driving using a remote control. To test the fusion of the different localization subsystems these paths pass both within a grid map, previously obtained using the architecture shown in Figure 2, and outside it. We obtained this map through the datasets recorded for the experiments of our previous work [42]. Using this map, we are able to locate our vehicle in the environment using AMCL. However, it has been six months since the data was captured. This means that in the map may appear dynamic objects that were in the environment at the time the map was made and that might not be in the environment when the AMCL runs. We can see this map in Figure 11. The paths we followed in the experiments are those shown in Figures 12 and 13. In the path *R*1 (Figure 12 *left*) we circulated entirely within the map. In the path *R*2 (Figure 12 *right*) we circulated in part out of the map, in this way we tested how the system behaves when using

only GNSS. In the path *R*3 (Figure 13 *left*) we circulated approximately the half of the route out of map. The difference with respect to the path *R*2 is that in this case we went out and entered the map describing a rectilinear trajectory, which can be more favorable to the relocation because AMCL suffers in the turns. The last route was the *R*4 (Figure 13, right), in which we circulated most of the journey off-map to test the non-divergence of the system. In this route, we found very unfavorable areas for the GNSS because of high trees and nearby buildings. All the paths were done twice, once with GNSS standalone, and another using the GNSS-RTK in floating mode (details about the single-frequency RTK system used can be found in [42]).

**Figure 12.** (**left**) Path *R*1 around the building located in the upper right quadrant. This path runs entirely through the map shown in Figure 11. (**right**) path *R*2 around the two buildings on the right side of the image. This path leaves the map in the area of the building in the lower right quadrant of the image.

**Figure 13.** (**left**) path *R*3 around the two buildings on the right side of the image. This path leaves the map shown in Figure 11 in the area of the building in the lower right quadrant of the image. (**right**) Path *R*4 around the two buildings on the right side of the image, and the building on the lower left side of the image. This is the path that makes the longest journey off the map.

In Figure 14 we can see the trajectories obtained when following the path *R*1, with GNSS standalone (left), and with GNSS RTK floating (right). This is the only journey that we made entirely within the map. In these conditions, the AMCL localization proved to be very robust. This translates into very small variances what makes the KF output to be very similar to the output of the AMCL. As we can see in Figure 14, the red line (KF) is superimposed on the green line (AMCL), as both provide almost the same location. In both cases, we see that the blue line (GNSS) differs from the fusion, although to a lesser extent in the case of RTK.

Figure 15 shows the paths obtained for *R*2. In this case, the behavior in the map area is the same as shown in the previous case. However, in the area where we travel off-map (*x* > 75 m) the fusion adapts to the observations provided by the GNSS-based subsystem. In this area, when the AMCL uncertainty grows exceeding the thresholds, the Kalman filter output corrects the AMCL state estimation so that when the robot re-enters to the mapped area the AMCL subsystem converges again.

**Figure 14.** Trajectories through the *R*1 path expressed by localization using the fusion described in Section 5. (**left**) Using GNSS standalone. (**right**) Using GNSS floating point. In both cases, the fusion gives more weight to AMCL, because the robot is inside the map during the whole trajectory.

**Figure 15.** Trajectories through the *R*2 path expressed by localization using the fusion described in Section 5. (**left**) Using GNSS standalone. (**right**) Using GNSS floating point. In both cases, the loop closure of the right quadrant is performed off-map. We see that thanks to the fusion of GNSS, we can maintain the trajectory to re-enter the map area.

Figure 16 illustrates the trajectories made for the path *R*3. In this case, we can observe a behavior similar to that of the path *R*2 where the fusion adapts to the GNSS system in the off-map area (*x* > 75 m). In the case of the path made using the RTK system, we see that the low variances makes the fusion to closely follow the observations. For this reason, we see how the red line (AMCL) is superimposed to the rest of the subsystems in Figure 16 right.

Figure 17 shows the result of performing manual driving through the path *R*4. In this case, the trajectories run off the map in the whole area defined by *x* > 75 m and *y* < −90 m. We see that the

behavior is similar to the cases described for the paths *R*2 and *R*3 with the difference that in this case most of the journey is off-map and verifying that the re-entry can be done successfully even after a long journey without mapped references.

**Figure 16.** Trajectories through the *R*3 path expressed by localization using the fusion described in Section 5. (**left**) Using GNSS standalone. (**right**) Using GNSS floating point. We see that in the case of the fusion with GNSS real-time kinematic RTK floating point (**right**), in the off-map area the fusion adapts to these observations.

**Figure 17.** Trajectories through the *R*4 path expressed by localization using the fusion described in Section 5. (**left**) Using GNSS standalone. (**right**) Using GNSS floating point. In this case, the whole area *x* > 50 m is off-map. We see that in both cases we can make the re-entry on the map. However, in the case of the GNSS RTK floating point (**right**) the trajectory seems more consistent except in the curve located in the lower right quadrant, where conditions were very unfavorable due to occlusions and multi-path produced by trees and buildings.

#### 6.4.2. Autonomous Navigation

In the previous section, we described the experiments carried out to test the localization but in that case, the control loop closure was being performed by the users who handle the remote control. Because of this, we are influencing the localization by using our own human localization at the cognitive level. In order to test how location affects control, we integrated into our vehicle the architecture shown in Figure 3 to navigate autonomously in closed circuit. Using the paths recorded during the previous experiments we generated two maps of trajectories through the routes *R*3 and *R*4 (Section 6.4.1), defining two different circuits. For these experiments, the application is configured to run through the specified circuit indefinitely. In this way, we will be able to observe the repetitiveness of the trajectories in each lap. With these conditions, we carried out four different experiments. First, we performed the autonomous navigation along the circuit defined by the path *R*3 using the GNSS in standalone mode. Next, we used the circuit described by the path *R*4 also with the GNSS configured

in standalone mode. In order to see how the differential corrections of GNSS can affect navigation, we repeated the two experiments previously described but using the RTK in floating mode.

The system behavior in the paths *R*3 and *R*4 with the standalone GNSS configuration was similar. In both cases, the errors of the standalone GNSS system caused failures in the zones marked with yellow circles in Figure 18. These errors resulted in emergency stops, thanks to the system described in Section 4.2.4. However, it was possible to demonstrate the system recoverability since, after skipping these areas in manual driving we were able to complete the circuit autonomously. During the RTK experiments, the system was able to complete the circuit without interruptions including the off-map areas. The behavior in these experiments was very similar to the described in the previous section: the system shown repeatability and a smooth and stable control without oscillations. However, in these experiments we were not able to circulate indefinitely, since in the second lap, in the areas marked with yellow circles in Figure 18 we lost connection with the base station, disabling the RTK mode and forcing to interrupt the autonomous navigation. As in the previous case, the system proved to be recoverable as it returned to autonomous navigation after passing these critical points.

**Figure 18.** Representation of GNSS fixes in RTK floating point mode, recorded during autonomous navigation. (**right**) Through the path *R*3. (**right**) Through the path *R*4. In both cases, if the link to the base station is available, the closed loop is completed without problems. Areas marked with yellow circles indicate problem zones for the link to the base station.
