Surface Exploration with a Robot-Trailer System for Autonomous Subsurface Scanning

Autonomous surface exploration with mobile robots entails the problem of simultaneous trajectory planning and path tracking, while also on-board robot localization and kinematics control are essential. When it comes to articulated robotic setups, planning and control is more challenging since the towed component that typically bears a tool or a sensing mechanism should closely follow a specific trajectory in order to accurately complete its task. The paper at hand presents a holistic surface exploration method with an articulated mobile robot that tows a two-wheeled trailer. A variation of Boustrophedon global path planing module has been developed considering robot's embodiment for full field coverage, integrated with a real-time Model Predictive Controller (MPC) to ensure trailer's path following. Articulated robot's state estimation is provided with stereo-based visual odometry. The system has been evaluated both in simulation and in realistic environment, proving its ability to perform dense surface exploration that in our particular case, eventually enables a Ground Penetrating Radar (GPR) mounted on the trailer to autonomously scan the entire target area.


I. INTRODUCTION
Large scale surface/subsurface scanning and navigation is essential in contemporary construction sites, where automated solutions are important to decrease manual labor costs. Up to now, several technological advancements have been achieved in the domain of surface and subsurface perception with the development of an abundance of sensors suitable for a variety of application scenarios. Yet, the integration of such sensors with a robotic system that will enable autonomous large scale scanning and navigation still necessitates further research endeavors. Specific challenges in autonomous surface/subsurface exploration hold this initiative back, such as the requirement for dense overlapping passes for the robots or the towed systems, the structured motion of the robot and the challenging outdoor conditions that hinder the robot's localization accuracy.
Autonomous exploration with robot-trailer systems is prominent in transportation, agriculture and exploration, while use of autonomous navigation for various types of vehicles is rapidly increasing. Manual teleoperation of such systems is challenging, time consuming and lacks repeatability. In agriculture, several articulated robot-trailer systems have been developed for the autonomous plowing of the fields [1], [2], yet the most challenging task is the control of the trailer 1 All authors are with the Centre for Research and Technology Hellas -Information Technologies Institute (CERTH / ITI), 6 th Km Charilaou-Thermi Road, Thessaloniki, Greece, 57100 {etsiogas, gkostave, gkouros, akargakos, dgiakoum, Dimitrios.Tzovaras}@iti.gr to follow straight lines. Moreover, in subsurface inspection, autonomous scanning of large fields with Ground Penetrating Radars (GPR) is a demanding task which necessitates full coverage of the field with several parallel passes of the GPR [3]. In articulated robot setups, utilization of model predictive control (MPC) is widely used since main advantages of MPC are the accuracy of the controlled motion (that can't be achieved with manual navigation) as well as the reliability of the system. As an example, authors in [4] utilized a linear MPC controller that tracks a simulated robot-trailer system and provided sufficiently small lateral position error along a structured trajectory generated from a Dubins path [5]. Such approaches can significantly increase the articulated system's autonomy.
The paper at hand examines the research conducted towards an automatic GPR [6] data collection with an articulated robot and processing pipeline, in order to allow full coverage of the target area. The method presents a complete pipeline of structured global planning, robot and trailer state estimation and path tracking for local navigation through a model predictive controller. The minimization of path tracking error allows for the rover's and, thus, the trailer's structured motion that maximizes the coverage of the subsurface environment using the GPR sensor which is mounted on the trailer. It should be stressed that the adopted MPC design follows an approach similar to the one described in [4], however specific modifications have been applied in order to satisfy the existing robot kinematic model. Our robot-trailer system, illustrated in Fig. 1, consists of an outdoor exploration rover with a customdesigned trailer on which a GPR antenna is mounted. The selected rover is a Robotnik skid-steering SUMMIT-XL that is suitable for outdoors navigation. The robot also possesses a built-in PTZ camera that is reversed to point at a flat sheet of printed AprilTags and a forward looking ZED stereo camera. The trailer system holds the GPR in a tangential and flat position with respect to the surface, while at the same time the free joint that connects the trailer with the robot allows their in-between articulation on the horizontal plane.
The rest of the paper is organized as follows. State-ofthe-art methods are outlined in Sect. II and an analytic description of the robot's kinematic model is included in Sect. III-A. Sect. III-B presents the overall functionality of the boustrophedon path planner and robot state estimation is described in Sect. III-C. Sect. III-D describes the local navigation approach, while results and conclusions regarding the method's performance are drawn in Sect. IV and V, respectively.

II. RELATED WORK
Automatic exploration of the subsurface comprises the incorporation of GPR antenna motion information in the processing loop, which necessitates integration with its robotic carrier [7]. Authors in [8], introduced a novel robotic system integrated with a GPR for automated bridge inspection. The collected GPR data were later analyzed for the inspection of rebars used for the construction in the subsurface of a bridge, however the control of the GPR motion was constrained by its mounting base. The developed method was able to identify defective spots in the subsurface of a bridge, however the mobile robot was utilized only as the mean to carry the GPR antenna for the collection of the data and, thus, robot's locomotion data was not considered during the process. Authors in [9], [10], demonstrated an integrated robotic system consisted of an all-terrain mobile robotic platform integrated with a GPR antenna to collect data for surface and subsurface inspection of a bridge, where again the antenna's motion was controlled by the operator's visual inspection. In another application, the authors in [11] employed an all-terrain mobile platform to carry a GPR antenna, a system specifically hardened to operate in extreme temperatures of South Pole station of Antarctica. It is revealed that a common characteristic among the above mentioned works is that the robotic topologies were designed to operate in relatively flat terrain where instantaneous changes of robot attitude were absent, while robot motion was not directly associated with the desired antenna path.
Several path planners and motion tracking controllers have been implemented for articulated robotic systems. The main constraint of GPR imaging is the fact that the antenna should follow straight paths in order to concatenate the measurements and to obtain a radargram that corresponds to the straight traveled distance. To achieve this, the robot should be capable of following the straight paths with maximum accuracy. Authors in [12] developed a non-linear centralized model predictive controller approach based on the moving horizon estimation using the ACADO toolkit [13]. This allowed the introduction of sufficiently small Euclidean errors for straight line trajectories. A similar work presented by Kayacan et al. [14] utilized a tube-based linear MPC to track a tractor-trailer system resulting in increased Euclidean errors for straight line paths. In [15] another non-linear MPC method has been proposed aiming to keep both the tractor and the trailer on a reference path. This approach utilized GPS, IMU, EKF and a 2D laser scanner to provide accurate state estimation of the system and managed to keep the lateral position error of the trailer to minimal levels. Authors in [16], analyzed applications of MPC both in simulation and real environments comparing a kinematic model with a dynamic bicycle model. Backman et al. [17] implement a multivariable nonlinear model predictive controller to improve the tracking accuracy of a trailer for use in farming context. A highlight of this work is the online alternation of the prediction horizon length to regulate the computation time and achieve the optimal horizon size. In [18], nonlinear MPC is also applied to develop a 6-DOF control model that tracks the reference trajectory of a flying tethered airfoil. The robot's kinematic model is presented herein to clarify the system setup. Specifically, the studied robotic topology consists of a skid-steering robot-trailer kinematic model as illustrated in Fig. 2 which includes constant and variable parameters. Variables φ r and φ t are defined as the heading angles for the robot and trailer respectively while their difference defines the hitch angle θ = φ r − φ t . In practice, θ is directly measured using the Apriltags visual fiducial system and the backwards facing omnidirectional PTZ camera that is mounted on the robot's chassis. The above definition is essential to formulate the system's equations in Sect. III-D. Variables x t and y t are the trailer's longitudinal and lateral position respectively. It should be stressed that since the robot's kinematic model uses skid-steering instead of explicit Ackermann steering, the steering angle does not exist and is not considered a parameter herein. Thus, the robot's angular velocity ω is considered the control input of MPC as it directly regulates the robot's orientation. Constant parameter l h = -0.23 m corresponds to the distance from hitch to the rear wheel of the robot. The negative value is justified by the employed hitching system in situation where to topology is off-axle with positive offset. This is the opposite of the traditional off-axle (with negative offset) hitch, where the joint is located beyond the vehicle's rear axle [19]. Finally, constant parameter l t = 1.125 m corresponds to the distance from hitch to the rear tire of the trailer while variables v r and v t express the robot's and trailer's longitudinal velocity respectively. Due to the size limitations of the GPR antenna, direct scanning of the entire surface area is not feasible. Thus, multiple passes are required and in order to ensure full coverage of the surface area with the GPR antenna towed by the surface rover, a parametric path should be calculated that takes into consideration both the articulated kinematic model constraints and the embodiment of the GPR antenna. The boustrophedon cellular decomposition (BCD) [20] is a method used in robotics and artificial intelligence for the purposes of coverage path planning, which is the specification of a path to be followed by a vehicle in order to pass over each point in an environment. The motion applied to the mobile robot adopts the boustrophedon-like motion (that BCD is based on) in accordance to which the robot performs multiple parallel passes through the examined area. Considering the fact that the employed surface rover can operate in relatively uneven terrain, small surface obstacles are easily surpassed and, thus, a variation of the BCD method that does not necessitate obstacle avoidance has been implemented. In order to initiate the planner, the operator roughly selects four points (vertices) that define a rectangular area within which the Boustrophedon path are calculated. The path to be followed by the GPRequipped trailer is then constructed as a Dubins path that utilizes the boustrophedon motion although differing from a standard boustrophedon path since it involves unidirectional instead of bidirectional turns.

B. Boustrophedon path planning
To enable surface scanning with the GPR antenna, measurements are taken into account only when hitch angle θ is close to zero. This means that real-time processing during data acquisition simply ignores GPR measurements taken while the system drives a curved path segment. For this reason, the method aims to minimize the path tracking error only when the trailer follows straight line path segments where the GPR's consecutive samples are properly aligned. Fig. 3 shows two paths created using the proposed method (green color) and two typical boustrophedon paths (blue color). From the illustration, it is evident that the proposed approach generates paths that have wider curvatures compared to common boustrophedon paths where all turns have the same radius of curvature. The practical need for this arises from the physical limitations of the system's hitch angle. Considering this, it evident that the smaller a turn's curvature the harder it gets for the robot to keep the trailer in the path. Specifically, in real conditions, when the radius of curvature is too small the robot "violates" the reference path by taking a wider turn to achieve timely alignment with the forthcoming straight path segment. This phenomenon is also observed later in Fig. 6(b). Overall, the presented path planning method ensures wider turns for the surface rover, thus, resulting in the best possible coverage of the exploration area by the GPR. The sole planner's parameter is the radius of minimum curvature R min that is the radius of circle A. Then, if D is defined as the diameter of circle A, longer paths are created to cover greater areas using intersecting circles with diameters that are multiples of D, excluding the START/END circles which are used only to denote the starting and ending point of the path.

C. State estimation with visual odometry
State estimation of the robot's motion among consecutive frames is performed through a stereo based visual odometry algorithm, suitable to operate in outdoor environments [21]. In each iteration loop, a disparity image is computed through a stereo correspondence algorithm based on local block matching accompanied with global disparity space optimization. The output disparity map utilized to obtain the depth value z(x, y) coordinates is calculated through triangulation for each pixel (x, y) of the left camera image (kept as reference frame), as z(x, y) = f · B/disp(x, y), where f is the camera focal length, B is the stereo camera baseline and disp(x, y) is the estimated disparity value for each pixel of the reference image. By applying the same procedure on all the pixels of the stereo pair, a dense 3D point cloud of the scene is obtained. However, for rough motion estimation between the consecutive frames, only a subset of the initial point cloud is required. In particular, among the potential detectors suitable for localization, the current system employs the well known Speed Up Robust Features (SURF) [22] algorithm, which detects and matches the most prominent 2D points within two consecutive frames. By evaluating the depth information at the respective 2D images, we obtain a set of point-wise 3D correspondences among the consecutive frames. Let us assume that the robot observes a specific point P t in the 3D space, such as P t = [x t , y t , z t ] T . In the next time instance, t + 1 the robot undergoes a specific motion with rotation matrix R t+1 and translation vector T t t+1 = [T x , T y , T z ] T , so the corresponding point P t in now observed as P t+1 = [x (t+1) , y (t+1) , z (t+1) , ] T . The transformation from point P t to P t+1 is given as follows: Considering the 3D registration of all the detected SURF points, a rigid body transformation is computed that expresses the overall camera (and thus robot) motion estimation in each execution loop. The required incremental rigid body transformation typically conforms with a sum of quadratic differences minimization criterion, resulting to a singular value decomposition (SVD) optimization problem. However, taking into account that errors are introduced, mainly stemming from erroneous SURF features matching, depth estimation of the stereo correspondence algorithm and the native camera reprojection error obtained from the stereo pair calibration, RANSAC outlier rejection is applied and the remaining inlier points are used along the SVD to compute the rover's motion estimation. Robot poses and transformations are stored as nodes and edges in the graph respectively, representing the robot's trajectory. Having expressed the rover's location estimates as a graph of nodes, a global graph optimization step is applied through g2o [23] optimization when the robot revisits the same location, i.e. observes the same SURF features. The applied Dubins path motion eases the revisit of areas and allows loop closure optimization. The spacing between parallel robot traverses was kept approximately to 0.5 m, enabling frequent loop closures and respective error correction, as well as significant overlap among the GPR measurements since the antenna's width is approximately equal to 1.2 m. Fig. 4 illustrates the Dubins path and the robot-trailer system located near the end of the exploration phase along with the constructed surface map. Note that the incremental motion estimations obtained from visual odometry are utilized in local planning and control method to measure deviation from the global path.

D. Local navigation with model predictive controller
For the local navigation and in order to closely follow the Dubins path, a model predictive controller (MPC) has been implemented as an optimal approach to solve the path tracking problem for the robot-trailer system. MPC is a control method used to control a process while satisfying a set of constraints and in our approach, a linear model has been implemented in order to enhance real-time performance. The goal of MPC, quite common in robot-trailer systems, is the elimination of the trailer's lateral position error. The constraints of the system are described by the following inequalities: where θ, v r and ω are the hitch angle and the robot's longitudinal and angular velocity respectively. Incorporating the above constraints into MPC we ensure the avoidance of undesired effects such as the jackknife phenomenon. After the calculation of the global path as a set of waypoints with a resolution of 20 cm using the boustrophedon path planner (Sect. III-B), a local path planner is applied taking as input the robot's visual odometry described in Sect. III-C. The local planner uses the underlying model predictive controller that runs at a specified frequency and calculates the appropriate values for the longitudinal and angular velocity of the robot, so the latter can guide the trailer to the next waypoint from the global path. The system's motion parameters (described in Sect. III-A) are modeled by the following equations: When the heading angles φ r and φ t are equal to the values indicated by the reference path, the sine function is simplified according to the small-angle approximation: sinα ≈ α. After applying this in the above equations, they are rearranged as a state-space model in the following way: where ω (angular velocity of the robot) is the control input of MPC, φ r φ t y t T is the state vector and y is the output vector containing the trailer's lateral position and heading angle. These parameters are utilized to control the robot's motion comparing the current lateral displacement of the trailer from the closest waypoint in the Dubins path depending on the deviation measured in Euclidean distance, where the respective set of velocities is applied to the robot' wheels. Finally, the cost function of MPC is defined in the following way: (12) where N p is the prediction horizon, Q is a positive definite weighting matrix on the system's output vector y, R is a weighting parameter on the control input vector u and r is the reference vector. The experiments have been conducted both in a simulation as well as in a realistic environment as illustrated in Fig. 5. The ability of the trailer to accurately follow the Dubins path was examined. The accuracy depends both on the performance of the visual odometry while estimating the state of the robot, as well as the ability of the model predictive controller to keep the trailer on track of the global path. Overall, the performance of visual odometry in simulation as well as in the real world experiments was 5 cm in lateral displacement, while the heading angle error was less than 2 degrees. In both scenarios, the Dubins path has been calculated sufficiently and the distance among the waypoints was 20 cm. Fig. 6 illustrates the different trajectories in real and simulated environment. As expected, it is revealed that larger deviations appear in real conditions. Higher error is observed on the circular fragments of the trajectory e.g. turns, where the controller is not able to impose the trailer to accurately follow the curve. However, during straight line traversals, less deviations from the desired paths are observed. For the evaluation of the controller, two metrics have been utilized as indicators of the trailer's path tracking error. The first one comprises the Euclidean distance between the trailer's actual position and the reference position in the path and the second is the more common trailer's lateral position error. For the simulation scenario, the Euclidean distance deviation is exhibited in Fig.  7(a) and it reveals that in straight line segments, essential for the GPR measurements, the error is well below 5 cm while the absolute lateral deviation is less than 25 cm as illustrated in Fig. 7(b). However, in the real environment, the performance decreases and the Euclidean deviation in the worst pass is approximately 1 m in straight lines as shown in Fig. 7(c), however after loop closure optimization in visual odometry this error decreases within the acceptable errors of 30 cm. The absolute lateral position error in the real environment during trailer's straight lines traversal remains under 10 cm as depicted in Fig. 7(d). Considering that the GPR antenna width is 1.2 m, the aforementioned errors allow overlapping of the parallel passes of the trailer and, thus the antenna, enabling full coverage of the examined field during scanning in real conditions. The control loop of the model predictive controller was running at 5 Hz constrained mainly from the bottleneck of the visual odometry state estimation component. V. CONCLUSIONS In this work a complete pipeline for field exploration through an articulated robot-trailer system has been presented. Global path planning is performed with Dubins path generation and trailer's state estimation is inherited through the system's kinematic model from stereo-based visual odometry. Local navigation and path tracking is applied through a variation of MPC, specifically designed for a skid-steering robot towing a two-wheeled trailer. The application that this method targets is the full coverage of the area of interest from a GPR antenna for dense subsurface scanning, taking into consideration the robot kinematic model and the antenna's embodiment in order to pass from all the spots in the field. Experiments have been conducted in both simulation and realistic environment. In simulation, the error levels were lower due to constant and controllable environmental parameters. Despite the higher errors observed in real conditions, their values were smaller the antenna's width, ensuring overlapping among the parallel passes of the trailer's GPR during exploration.