High-speed autonomous navigation system for heavy vehicles

1 High-speed autonomous navigation system for heavy vehicles A. Rodriguez-Castaño, G. Heredia and A. Ollero Robotics, Vision and Control Group, http://grvc.us.es Engineering School, University of Seville Camino de los Descubrimientos s/n, 41092 Sevilla (Spain). Corresponding author: G. Heredia , [email protected] Camino de los Descubrimientos s/n, 41092, Sevilla, Spain Tel: +34-954486035 Fax: +34-954487340 Abstract: This paper presents techniques for GPS based autonomous navigation of heavy vehicles at high speed. The control system has two main functions: vehicle position estimation and generation of the steering commands for the vehicle to follow a given path autonomously. Position estimation is based on fusion of measurements from a carrier-phase differential GPS system and odometric sensors using fuzzy logic. A Takagi-Sugeno fuzzy controller is used for steering commands generation, to cope with different road geometry and vehicle velocity. The presented system has been implemented in a 13 tons truck, and fully tested in very demanding conditions, i.e. high velocity and large curvature variations in paved and unpaved roads. Keywords: autonomous vehicles; navigation; heavy vehicles; fuzzy control; Differential GPS; sensor data fusion. 1. Introduction Autonomous guidance of ground vehicles has been an active research and development topic in the last 30 years. For example, several systems have been implemented in experimental outdoor autonomous vehicles such as the Navlab family at Carnegie Mellon University [1], the ARGO autonomous vehicle [2], the ROMEO vehicles [3]. The interest in autonomous vehicles’ technologies has grown specially in the US, where DARPA has organized the Grand Challenges and the Urban Challenge from 2004 to 2007 [4][5], which remarkably promoted the technologies of intelligent vehicles around the world. Reference [6] presents the main autonomous vehicles developed in the last years in the US. Other remarkable examples in the last years are the VisLab autonomous vehicles [7][8]. Most of these vehicles are the result of the adaptation of conventional cars or vans. However, the number of references presenting experimental results with autonomous heavy vehicles, such as trucks, is lower. Interest in heavy vehicle autonomous guidance has grown from the nineties in the framework of the initiatives on intelligent transportation systems (ITS) and automated highway systems (AHS). Published results on autonomous heavy vehicles come mainly from the California PATH program [9][10][11] and University of Minnesota’s SAFETRUCK [12][13][14]. Other Preprint version, the final publication is available at http://dx.doi.org/10.1016/j.asoc.2016.02.026 Rodriguez-Castaño A, Heredia G and Ollero A (2016), High-speed autonomous navigation system for heavy vehicles, Applied Soft Computing, jun 2016, Vol. 43, pp. 572-582. 2 work on autonomous heavy vehicles has been done with TERRAMAX [15][16], designed for off-road navigation, and by Isuzu Motors in Japan [17]. Recently, special attention has been devoted to vehicle platooning or semi-autonomous “road trains” [18], composed by a leader vehicle with a human driver with one or several autonomous vehicles following it closely, for its potential in fuel and emissions savings and increase of road capacity. The SARTRE European Project has made a road demonstration of a platoon with a leader truck and several vehicles following it [19][20]. Tests with a three truck platoon have been performed within the Energy ITS Project in Japan [21][22]. Other examples include the German KONVOI project which analyzes the implications of truck platoons in autobahns [23] and the Swedish research program on Intelligent Vehicles [24]. Many of these autonomous vehicle researches have been done at medium or low speeds. High speed navigation of autonomous vehicles is still a challenging application due to the requirements on reliability and safety. The choice of the sensors used for position estimation in autonomous navigation may have important practical requirements. Some sensors require auxiliary guidance mechanisms in or around the field of interest [25], as is the case with most of PATH program work [13], which uses magnetic markers buried in the road. The work in [17] also uses magnetic markers, but it stores a “map” of the markers for preview control. This paper deals with GPS based autonomous navigation by implementing a sensor data fusion and path following techniques. With the advent of modern GPS receivers, kinematic centimeter-level absolute position estimation and attitude measurements of the vehicle are available using carrier-phase Differential Global Positioning System (DGPS) with position accuracy up to few centimeters. Carrier-phase DGPS techniques are being used for straight line tracking and heading control of golf carts and farm vehicles at low velocities [25], and adapted to conventional cars [26]. The TerraMax autonomous heavy vehicle [15] uses 3 different DGPS receivers for improved reliability. However, the use of GPS receivers requires operation environments with good sky visibility. Furthermore, in practical implementations, also arise other sources of errors like communication loss of differential correction messages, output variable latency and receiver errors [13]. Some of them can be detected taking into account the vehicle dynamics, using model-based fault detection and identification techniques, as is described in [14], and in [27] for an aerial vehicle. It has been also recognized that sensor data fusion can improve significantly the reliability of position estimation. Sensor data fusion for vehicle position estimation can be solved by means of statistical approaches, such as Kalman filtering (see, for example, [28]). These techniques require a stochastic state-space representation of the vehicle model and of the measurement process. A kinematic model of the vehicle and knowledge of measuring equipment are typically used to derive the state-space representation, although for nonlinear systems an Extended Kalman Filter formulation is needed. However, a main drawback of Kalman filtering is that sensor noise is modelled as white noise. Although this can be accurate for several sensors, it is clearly not true for DGPS. Moreover, the implementation of Kalman Filtering requires estimations of the measurement covariance matrix, which can be obtained from the technical characteristics of the sensor equipment, and 3 the process covariance matrix, that represents the model inaccuracies, which is much more difficult to obtain. It has been also shown that a poor estimation of input noise statistics may seriously degrade Kalman filter performance and even cause filter divergence [29]. In this paper fuzzy logic is applied to position estimation without the need for precise information, which can be difficult or expensive to obtain. The path following component of a vehicle controller has the mission of generating the vehicle’s steering to track a previously defined path, by taking into account the vehicle’s actual position and orientation and the constraints imposed by the vehicle and its low-level motion controller. Path following, which is sometimes referred to as “lateral control“ or “path tracking”, is directly related to the lateral vehicle motion and steering control. Vehicle control also involves speed control. Both are coupled problems. However, path following has been usually studied for constant velocity. Thus, the path following algorithm implements a steering control law by using the error between the current estimated vehicle position/ orientation and the path to follow. The inputs of the path tracker are variables defining the state of the vehicle with respect to the path, and the output is the steering command to be executed by the low-level motion controller. Linear control methods have been successfully applied for vehicle automated steering [30][31]. However, if the linearization conditions are violated or nonlinearities in the steering mechanism or in the motion sensors exist, the tracking deteriorates. Nonlinear path following techniques, as Takagi-Sugeno (TS) fuzzy control, have shown good performance [26]. TS fuzzy systems are a special type of fuzzy systems in which the consequent part of the rules is not defined by a fuzzy membership function but by affine linear dynamic systems. A TS fuzzy model will approximate a nonlinear system by smoothly interpolating these affine local models. Fuzzy control can also be used to integrate the driving knowledge acquired in the form of if-then rules from an experienced driver, and TS driving control laws directly extracted from sensor data recorded while the vehicle is operated by a human driver [32]. Furthermore, design techniques that guarantee stability based on Lyapunov functions and other methods can be also applied [33-37]. Most path following methods have parameters related to the selection of a goal point, or a particular segment of the path to follow, which are required to compute the signal error in the path following loop. This has been integrated with path tracking through the concept of preview control. TS fuzzy logic also provides an efficient framework for integrating this parameter adaptation in the steering controller [38] and could be also implemented to tune automatically the parameters according to the current navigation conditions. This paper presents a Takagi-Sugeno fuzzy path following controller for heavy vehicles at high speeds, including fuzzy sensor data fusion for vehicle position estimation. The remaining of the paper is organized as follows. Section 2 introduces the main characteristics of GPS-based path following. In section 3 the Takagi-Sugeno fuzzy path following strategy is presented. Section 4 describes the experiments carried out with a 13 Tons truck. Finally section 5 presents the Conclusions. 4 2. GPS based path following 2.1. Position estimation GPS receivers provide low-frequency position information which is clearly insufficient to control a vehicle at high velocities. It is necessary to use additional high-frequency sensors to improve the performance and integrity of GPS navigation systems. A position estimation algorithm should use low-frequency sensor information for correcting low-frequency drift error in high frequency sensors and should use high frequency sensor information to decorrelate the errors in low-frequency sensors. Kalman filtering is one of the most widely used sensor data fusion technique in autonomous navigation. This paper proposes the application of fuzzy sensor data fusion to consider the heuristic knowledge involved in the estimation problem. This technique is based on the use of a fuzzy system for the on-line fusion of the measurements from a tachometer, a gyroscope and a carrier-phase DGPS receiver. The proposed fuzzy position estimation system initially showed similar performance to the Extended Kalman filter in experimental tests, but the fuzzy position estimation was much more flexible and easily tuned than the Kalman filter. This flexibility comes from the fact that a human designer can easily understand what each rule does and it is much easier to tune them to match experimental results. The fuzzy position estimation system uses the data from the available sensors to obtain an estimation of the position and orientation of the truck in real time. The DGPS receiver provides data on latitude and longitude of the receiver antenna, which is then converted to UTM (Universal Transverse Mercator) coordinates. On the other hand, the gyroscope provides an estimation of the angular velocity of the vehicle, and the tachometer sensor readings are converted to linear velocity of the vehicle. With these measurements it is possible to obtain an incremental estimation of the position and orientation of the vehicle using the kinematic model. At a first approximation and for short distances, the truck can be considered to move on a plane, and a simplified 2D model can be used. For navigation in 2D, the position and orientation of the vehicle is given by (x, y, ) (see Figure 1), where x and y are the vehicle’s cartesian coordinates, is the orientation angle, and (v, ) are the linear and angular velocities of the truck, respectively.  5 Figure 1 Vehicle kinematics Then, the kinematic model is given by the following differential equations [3]: (1) If the differential equations (1) are discretized using the Euler method, the following expressions are obtained for the position and heading at instant k+1: (2) where x(k) and y(k) are the absolute vehicle position in UTM coordinates, (k) is the orientation at time k, v(k) is the vehicle velocity, is the yaw rate, and T is the sampling period. This method is usually known as dead reckoning (DR) or odometry. It has a high performance for short distances and can be computed very quickly, but the growth of position uncertainty is unbounded since it is an incremental method. At last there are two independent estimations. The GPS estimation is a very accurate, absolute position estimation, but the sampling frequency is low, 5-10 Hz at most. Furthermore, in some situations the estimations can be degraded due to satellite loss or other factors (i.e. in urban canyons). Luckily most receivers provide an indicator of the accuracy of the estimation (i.e. meter, submeter, centimeter), which can be used in the position estimation algorithms. In some cases the GPS receiver may provide one or a group of few outlier points that are far from the previous estimations. These outlier points are not compatible with the vehicle kinematics, and can be detected comparing them with the DR estimations. On the other hand, the DR estimation is relative, it is very fast (up to 200 Hz or more) and it is very accurate for short distances. However, for larger periods the errors accumulate and grow unboundly, and then a good indicator of its accuracy is the distance from the last absolute correction. The fuzzy position estimation uses the complementary characteristics of both to obtain an y X Y x v  x ·v–sin= y ·vcos=  ·= xk 1+()xk()v–kTksin= yk 1+yk() vkTkcos+= k1+()k() k()T+= k() 6 estimation. The fuzzy system can be split up into two subsystems [40], a position fuzzy system and an orientation fuzzy system (see Figure 2). Figure 2 Sensor data fusion block diagram Then, the position is computed by means of: Position = pos_GPS_factor * GPS_position + (1 - pos_GPS_factor) * DR_position where DR_position is the vehicle position computed using dead reckoning, and pos_GPS_factor is the position fuzzy system output, which gives the confidence in the sensors. This factor should be very close to 1.0 most of the time, as shown from statistics obtained in tests (GPS receiver position error less than 2 cm with 95% probability), and it should be close to 0.0 when the GPS measurement is too far from the DR estimation, assuming DR estimation uncertainty is low for short intervals. Although this assumption is true most of the time, it could fail due to loss of GPS measurements. The fuzzy sensor data fusion method takes it into account. The position fuzzy system inputs are the level of accuracy (meter, submeter, centimeter) provided by the DGPS receiver, the distance travelled by the vehicle in the DR estimation since last GPS update, and the position error between the DR estimation and the GPS position. The rules are as: IF accuracy IS high AND pos_error IS low THEN pos_GPS_factor IS high where the membership functions of the inputs and the output are defined in Figure 3. A similar scheme is used in the orientation fuzzy system. Figure 3 Membership functions This fuzzy position and heading estimation system has been implemented and tested in a test heavy vehicle (which is described in Section 4), with good performance: it is able to achieve track lateral errors of less than 7 cm in curved paths, and less than 3 cm in straight paths. It is able to work at high output frequencies (it has been tested at up to 200 Hz). The position and heading fuzzy estimation system was tuned in the following way. Several experiments were done in one of the tracks described in section 4 (track A). The “true” Gyroscope GPS Tachometer Processing and DR estimation Orientation Fuzzy System Position Fuzzy System yaw rate accuracy position error heading error dist_travelled or_GPS_factor pos_GPS_factor LM H 30 60 90 10 25 40 H L 100 40 70 1 0 0.5 M dist_travelled (m) HLM LMH 0 00 pos_error (cm) accuracy (cm) pos_GPS_factor 0 11 0 1 0 1 0 7 position and heading were obtained with a human driving the test truck along track A. The vehicle position and heading were recorded by the onboard computer, and they were postprocessed first to remove outliers. Then the path was obtained from the data using principal curves [39], which are curves that pass through the “middle” of the data distribution, and are self-consistent (i.e., a point on the curve is the average of all data points that project onto it). Then, the output of the fuzzy position and heading estimation system was compared to the “true” solution; the rules were tuned in the track sections with larger errors. Although there may be a considerable number of rules, only very few of them are active at a given instant. Therefore, the candidate rules can be easily selected and, as the rules are self-explanatory, they can be easily modified to reduce the error. This process is repeated iteratively until the position and heading error is reduced to a specified level. 2.2. Path following control loops The path following controller generates the vehicle´s steering to track a previously defined path, based on the estimated errors between the current vehicle position and the path to follow. As shown in Figure 4, the path following vehicle controller has two components, a motion low level controller and a path following strategy. In this Figure RP is the reference path, e is the error between actual vehicle position and the path to follow, is the desired curvature and p is the actual vehicle position. Figure 4 Path following controller architecture The low level controller commands the steering wheel motor for the vehicle to get the curvature given by the high level controller. PID motion controllers are usually applied in this low motion control level, which requires a curvature to steering wheel angle mapping. PID controllers provides good results in most cases if a satisfactory feedback signal is available. The objective of the path following high level controller (or simply path following controller, as is usually referred to) is to generate control commands for the vehicle to follow a previously defined path by taking into account the actual position and the constraints imposed by the vehicle and its low level controller. Path following is a nonlinear control problem. There are many algorithms that have been used but there is no general solution that can guarantee robustness when the curvature of the path varies. Fuzzy logic is a suitable technique to apply to path following strategies defined by means of rules. The resulting controllers are known as direct fuzzy controllers. The controller inputs are the variables defining the state of the vehicle with respect to the path. The output of  error computation high level control low level control vehicle position estimation  e RP p 8 the fuzzy controller is the steering command to be executed by the low level motion controller. Several methods have parameters related to the selection of a goal point on the path to follow. This goal point is required to compute the signal error in the path following loop. These parameters are related to the gain of the closed loop system and have a significant effect on the tracking performance. Thus, oscillations or even instability can arise for some values of the controller parameters. These instability conditions are related to both the vehicle´s characteristics and navigation conditions including speed, path to follow and terrain. Usually these algorithms select a goal point P on the path at a fixed distance from the vehicle that is called the lookahead distance L, as shown in Figure 5. Using this goal point three values can be obtained that are the inputs to the path tracker. These inputs are the lateral position error in vehicle coordinates , the orientation error with respect to the goal point and the curvature error with respect to the goal point . The expressions of and are: (3) where and are the orientation and curvature of the path at the goal point. Figure 5 Path following. These algorithms are usually efficient, but a common problem is that the results are critically dependent on the appropriate tuning of the lookahead distance for the current navigation conditions. For example, at a given velocity if the lookahead is too long the vehicle may cut corners and if too short oscillations may arise. 3. TS Fuzzy path following for high speed navigation In this section, a Takagi-Sugeno (TS) fuzzy path following system for heavy autonomous vehicles at high speeds is presented. TS fuzzy systems are a special type of fuzzy systems in which the consequent part of the rules is not defined by a fuzzy membership function but by affine linear dynamic systems [41]. A TS fuzzy model will approximate a nonlinear system by smoothly interpolating these affine local models. Each local model contributes to the global model in a fuzzy subset of the input space. The TS fuzzy model has recently found wide applicability in fuzzy model based control. From a control engineering perspective the use of local affine (or local linear) models bridges the gap between fuzzy control and conventional control. Many existing tools and theories in linear  e e e e e path –= e path –= path path X Y  Path  L path P x y 9 systems theory can be partially applied to TS fuzzy models and controllers. Furthermore, general TS fuzzy systems have been shown to be universal approximators, and they can be used to effectively model a wide class of nonlinear systems. Stability analysis of TS systems also benefit from its internal structure. Sufficient stability conditions for this class of systems rely on the existence of a common quadratic Lyapunov function [33] or a piecewise-quadratic (PWQ) Lyapunov function for less conservative stability conditions [34]. The search for the Lyapunov function can be stated as a convex optimization problem in terms of linear matrix inequalities (LMI) for which efficient solving methods and software exist. Stability conditions for uncertain systems have also been developed [35]. Other approaches seek to reduce the conservativeness of the stability conditions obtaining local domains of attraction with fuzzy Lyapunov functions [36]. Alternative stability analysis based on LaSalle theorem has also been proposed [37]. The path following controller for heavy vehicles should fulfill the conditions explained in the previous section: (i) it should be able to drive the vehicle at a large range of velocities, following the path without cutting corners and without oscillations; (ii) it should drive the vehicle through path sections with different curvatures and curvature variations; (iii) the controller should be able to manage the different dynamic regimes that appear in heavy vehicle guidance; and (iv), in heavy vehicle autonomous guidance control signals must be smooth, specially at high velocities; control signals with fast variations can cause oscillations. A satisfactory path-tracking needs a fast response of the steering system when the vehicle has to change its curvature to manage a turn. This fast response is necessary because the vehicle’s curvature set point is continuously updated. If the steering response is slow, the cross track error will increase in these situations, and then control signals should be strong. The proposed TS fuzzy controller is an ideal candidate to deal with all these conditions. The TS fuzzy controller is composed of two sets of rules. One of them make an estimation of the lookahead distance for each driving conditions, and it is derived from heuristic knowledge from experienced drivers and vehicle experiments. The second set provide the control input to the low level controller, and it is derived from input/output data obtained in path following experiments with a human driver. Lookahead distance selection depends on path characteristics (curvature and curvature change), as have been derived from human driver and prior experiment heuristics. In general, drivers reported that they used a closer reference point on the path and applied a tight control when the curvature was changing between straight and curved path segments. Furthermore, reference points used by drivers where closer in curves than in straight segments. From that, the idea that has been implemented in the lookahead selector fuzzy set of rules is to increase the controller gain (decrease lookahead distance, since gain is inversely proportional to the lookahead distance) when the curvature of the path is changing (typically entering or leaving a turn) and have a lower gain (higher lookahead distance) when the vehicle must follow a constant curvature segment (straight or turn). Also, lookahead distance will be larger in straight segments than in curved segments. A typical lookahead variation can be seen in Figure 6, for a road composed of a straight section, a curved section and another straight section. The vehicle moves along a straight path with a lookahead L1; when a goal point is 16 (). The  R controller has as additional input the lateral deviation from the path ( ). . In Point 1 the truck is approaching the 30 m turning radius curve in track A (see Figure 10). The vehicle is moving at 35.8 km/h speed, it is in a straight segment (NTR=1000, =0.0) and the goal point is inside a transition area (GTR=142, =0.8). In this case, the output of the lookahead controller L is decreased to 15.8 m, and the lateral error is low (-0.12 m). In this situation the control output generated by the curvature fuzzy controller is -0.0112 m-1, corresponding to approximately 3 degrees in the steering wheels. In Point 2 the test truck is already inside the turn (NTR = 30, = 0.0, GTR = 30 and = 0.0). This case corresponds to a point inside the turn and it will continue in the same turn in the near future, as GTR and are the same than NTR and respectively. The output of the lookahead controller L is increased to 18.2 m and the curvature control command is increased to 0.0504 (corresponding to 14 degrees in the steering column), due to the lower velocity (26.2 km/h) and larger lateral error ( = 0.41 m). Another example is Point 3, which corresponds to the middle point of the straight segment marked as “C” in Figure 10 (NTR = 1000, = 0.0, GTR = 1000 and = 0.0, the truck is in a straight segment and it will continue in a straight segment in the near future). In this case, for a 48.1 km/h speed an output lookahead L = 27.2 is obtained. Even though the speed is not far from the case shown for Point 1, the lookahead value is nearly doubled. This is due to the fact that in Point 1 the vehicle is approaching a turn, while in this case the truck is on a completely straight segment. The curvature command output is lower (-0.0112 vs 0.0074 m-1) due to the fact that the controller is smoother as speed increases. If this example is compared with Point 2, the lookahead L is lower (18.2 vs. 27.2 m) for lower speed (26.1 vs. 48.1 km/h) and lower turning radius (30 vs. 1000 m). Straight line-tracking tests at high speed (80-100 km/h) have been made in track B, which is a unpaved road composed of straight sections and sections of large turning radius (between 600 and 750 m), which can be considered as a long straight road. Figure 13 shows the speed record of one of these tests in a 2 km long section of track B. The maximum cross-track error on the straight segments was 28 cm. and the standard deviation was 10 cm (see Figure 13). The maximum cross-track error on the straight segments was 28 cm. and the standard deviation was 10 cm (see Figure 13). Several tests to verify the performance of the proposed control system have been done also in tracks C and D, shown in Figure 14. Tracks C and D are unpaved roads, and they are composed of sections of different turning radius connected with straight sections. Minimum turning radius is 40 m. in both tracks. V (km/h) NTR (m) GTR (m) (m) L (m)  R (1/m) Point 1 35.8 1000 0.0 142 0.8 -0.12 15.8 -0.0112 Point 2 26.2 30 0.0 30 0.0 0.41 18.2 0.0504 Point 3 48.1 1000 0.0 1000 0.0 0.10 27.2 0.0074 Table 2: Numeric values of inputs and outputs of the fuzzy controllers at selected points in track A G  N G  N G N G G N  N G 17 The results of a 45 minutes test run in track C are shown in Table 3. The vehicle moves clock wise for 7 laps at a constant velocity of 45 km/h. The standard deviation obtained is similar to the tests in track A, while the maximum cross track error has increased from 50 to 70 cm. This is due to the greater speed in turns of severe curvature and the different road surface; track A is made of asphalt while track C is unpaved dirt so the vehicle slips much more entering and leaving the turns. Figure 13 Straight line tracking speed record and cross-track error (track B). In track D, one test was done at a constant speed of 30 km/h, with a cross-track error standard deviation of 14 cm and a maximum value of 46 cm. Another test was done at a constant speed of 45 km/h. In this case, the standard deviation was 16 cm, and the maximum cross-track error 010 20 30 40 50 60 70 80 75 80 85 90 95 100 tim e (se c) Speed (Km/h) 010 20 30 40 50 60 70 80 -0.4 -0.3 -0.2 -0.1 0 0.1 0.2 0.3 0.4 tim e (se c) C ross track error (m ) Std: 0.18 m time (s) V (km/h) e (m) time (s) 18 was 71 cm. The results are also shown in Table 3. Figure 14 Tracks C (left) and D (right). Numbers close to curves indicate turning radius (m) I 4.1. Comparison of results It is important to mention that the comparison with the results of other methods published in the literature is qualitative, since there are many factors as the characteristics of the vehicles, the conditions of the experimental tests and the implementation of the controllers that can affect the performance of the experiments and are not reported in the papers or are not easily comparable. Most of the autonomous navigation experimental results that can be found in the literature have been done with small vehicles or cars, and at low speeds. Then, it is difficult to compare the proposed navigation system for heavy vehicles. The most relevant published experimental results with heavy vehicles have been done in the framework of California PATH and University of Michigan's SAFETRUCK programs. In [10] results of the autonomous navigation of a heavy truck using a robust linear controller are presented. The tests have been done on an asphalt road composed of straight segments and curved segments with a radius of 800 m. The maximum velocity is between 65 and 75 km/h. Track C Max. error (cm) Std. dev. (cm) Track D Max. error (cm) Std. dev. (cm) Full test 71.2 16.17 Test 1 (30 km/h) 46.4 14.10 Lap 1 63.7 15.21 Test 2 (45 km/h) 71.1 16.56 Lap 2 69.1 14.19 Test 2-Lap 1 70.6 16.68 Lap 3 70.3 14.88 Test 2-Lap 2 71.1 16.35 Lap 4 69.7 16.43 Lap 5 71.2 16.29 Lap 6 71.0 16.02 Lap 7 70.7 16.37 Table 3: Results of test runs on tracks C and D 19 The lateral error is less than 0.15 m in straight segments and 0.35 m in the curved segments, but are greater than 0.5 m in transitions between segments. These results have to be compared to the autonomous truck tests in track B, since we have considered as roads with radius larger than 600 m as straight roads (in fact, track B combines straight segments with curved segments of radius between 600 and 750 m). These tests were done at speeds of between 80 and 100 km/h on an unpaved road, and the maximum error was less than 0.28 m, with std. dev. less than 0.1 m, which gives better results than [10], especially in transition segments. Both controllers show similar performance in straight and constant curvature segments, but the fuzzy controller proposed in this paper performs remarkably better in transitions between segments since it adapts smoothly to roads of changing curvature. In [12], a modified pursuit method is used to drive a heavy vehicle on an asphalt road. In a straight segment the maximum error is around 0.25 m at a speed of up to 65 km/h. This controller obtains slightly better maximum lateral error than the proposed fuzzy navigation system (0.28 m at speeds of up to 100 km/h), but at considerably lower speeds. In an S-shaped road segment with radius 85 m, the controller in [12] gets a maximum lateral error of about 0.45 m at a speed of 35 km/h. A comparable road test for the test truck is the road segment on track A around the "C" point (radius of 83 m), where the fuzzy navigation system achieves a maximum lateral error of 0.32 m at around 45 km/h, which gives a better performance. In [22] a 11 ton. autonomous truck is controlled at speeds between 60 and 80 km/h in a 3 km test track with 700 m. straight sections and 800 m oval sections. The lateral error is within 0.35 m at 60 km/h (in oval sections) and 0.5 m at 80 km/h (in straight sections). With the fuzzy controller proposed in this paper the maximum lateral error is smaller for speeds from 80 to 98 km/h in straight paths (0.30 m in track B) and larger in curved sections (0.50 m in track A), but it is difficult to compare because the minimum turning radius of the oval sections are unknown. Moreover, the controller in [22] is unable to correct the lateral error during half of each curved section (400 m), keeping a steady lateral error within 0.20 to 0.30 m. Overall, the fuzzy controller shows better tracking performance in straight and curved roads compared to [12] and [22]. The controller developed in [43] for the guidance of an electric power van is tested in a 2 km road inside the campus with a typical square shape. The vehicle is guided at an average speed of 26 km/h and a maximum speed of 46 km/h using a partially speed-dependent lookahead ranging between 8 to 14 m. They achieve a mean lateral error of 0.13 m, a standard deviation of 0.15 m and a maximum lateral error about 0.90 m in a 6 laps experiment. Even though there are no details about the turning radius of the corners and the surfaces of the tracks are different (paved vs unpaved) this experiment could be compared to the results presented in this paper for tracks C and D (see table 2) given the similar range of speeds and shape of the track. As can be seen the results are similar in terms of standard deviation (0.14 m for 30 km/h and 0.17 m for 45 km/h) but the maximum lateral error is larger in [43] (0.90 m vs 0.71 m). Comparing the results, the fuzzy controller has similar performance to [43] in straight paths, and better performance in the curved sections. This behavior can be due to the fuzzy controller addressing better road curvature changes, since the controller in [43], although adapting the lookahead to the vehicle velocity seems not to address well changes in road curvature. 20 In the SARTRE European project [20], experimental tests with a mixed platoon of trucks and cars have been performed. Unfortunately, only the data for one of the cars is presented in [20]. The experiments show a sedan car traveling at 90 km/h in straight paths. The lateral error is within 0.35 m, that is quite similar to the 0.30 m obtained with the fuzzy controller presented in this paper, and the standard deviation is 0.10 m, better than the 0.18 m obtained in track B. The same vehicle in a lightly curved road (470 m turning radius) keeps an average error of - 0.01 m with a standard deviation of 0.09 m and the maximum error within 0.25 m. In [20] the vehicle tends to go slightly separated from the path in the straight sections with an average error of 0.09 m, but the performance is better in the slightly curved sections. There are no details about the control method to explain the difference in performance between the curved and straight sections. Furthermore, there are main differences in the experiments, the surface of the tracks (paved vs unpaved) and the weight of the vehicles (14 ton truck vs a sedan car), what makes difficult the comparison. One common effect that can be observed in some sections of the experiments in [20] and [22] is that the average lateral error is larger than zero what indicates a lack of integral action in the controllers, what is properly addressed in the fuzzy controller in this paper. As a conclusion, and keeping in mind that all the comparisons in this section have to be considered as qualitative since there are no sufficient details in the literature on the vehicles, the roads and the controller implementation to make a direct quantitative comparison, the proposed fuzzy controller has similar or slightly better performance tracking straight and constant curvature paths. The main advantage of the fuzzy controller is that it adapts much better to road segments with changes in curvature, and thus it is more flexible and can be used in a wider set of roads in real conditions. 5. Conclusions This paper proposes a path following system designed and implemented for heavy unmanned vehicles. A TS fuzzy control system can be used to control heavy vehicles at different driving conditions. A good performance is achieved following paths with very different curvatures and velocities with this single controller. The controller is able to drive the truck at high speed (up to 100 km/h) on unpaved roads, and it can also control the vehicle on curved roads with a turning radius of 30-40 m. The conditions in which the driving tests have been done are very demanding, because in most of them legal speed limits have been exceeded. Moreover, most of the tests have been done on unpaved gravel roads. The main advantage of the presented controller is its capability of adaptation to road segments of different curvatures and the transitions between them. Furthermore, its structure allows the easy addition of new rules considering for example the slip angle or other factors to increase the envelope of working conditions of the controller. Future improvements can be done considering slip in the motion of the vehicle and applying on-line estimation of slip angle to correct the - mapping in the low-level control. References   21 [1] Pomerleau, D. (1993). Neural Network Perception for Mobile Robot Guidance. Kluwer Academic Publishing. [2] Broggi, A., Bertozzi, M., Fascioli, A. and Conte, G. (1999). Automatic Vehicle Guidance: The Experience of The ARGO Autonomous Vehicle.World Scientific Co. Publisher. [3] G. Heredia and A. Ollero (2007). Stability of autonomous vehicle path tracking with pure delays in the control loop. Advanced Robotics. Vol. 21, No. 1, pp. 23-50. [4] Iagnemma, K. and M. Buehler, Eds. (2006). Special Issue on the DARPA Grand Challenge, Parts 1 and 2. Journal of Field Robotics, Vol. 23, No. 8 and 9. [5] Buehler, M., Iagnemma, K. and Singh, S. (Eds.) (2009). The DARPA Urban Challenge - Autonomous Vehicles in City Traffic. Springer. [6] Cheng, H. (2011). Autonomous Intelligent Vehicles: Theory, Algorithms, and Implementation. Springer-Verlag London. [7] Bertozzi, M., Broggi, A., Coati, A. and Fedriga, RI. (2013). A 13,000 km Intercontinental Trip with Driverless Vehicles: The VIAC Experiment. IEEE Intelligent Transportation System Magazine, Vol. 5, No. 1, pp. 28-41. [8] Broggi, A., Cerri, P., Debattisti, S., Laghi, MC., Medici, P., Panciroli, M. and Prioletti, A. (2014). PROUD-Public road urban driverless test: Architecture and results, In Procs. IEEE Intelligent Vehicles Symposium 2014, pages 684-654, Dearbon, MI, USA. [9] Chen, C. and Tomizuka, M. (2000). Lateral Control of Commercial Heavy Vehicles. Vehicle System Dynamics, Vol. 33 , pp. 391-420. [10] J.Y. Wang and M. Tomizuka (1999), “Robust Hinf lateral control for heavy-duty vehicles in automated highway system”. Proc. of American Control Conf., pp. 3671-3675, San Diego (CA), USA. [11] Shladover, S. (2007). PATH at 20-History and Major Milestones. IEEE Trans. on Intelligent Transportation Systems, Vol. 8, No. 4, pp. 584-592. [12] Alexander, L. and Donath, M. (1999a). Differential GPS based control of a heavy vehicle. Proc. of the Int. Conf. on Intelligent Transportation Systems, 5-8 Oct., pp. 662-667. [13] Alexander, L. and Donath, M. (1999b). Differential GPS based control of a heavy vehicle. Minnesota Department of Transportation Technical Report MN/RIC-2000-05. [14] Ganguli, A. and Rajamani, R. (2003). Fault Diagnostics for GPS-based Lateral Vehicle Control. Vehicle System Dynamics, Vol. 39, No. 2, pp. 99–120. [15] Braid, D., A. Broggi and G. Schmiedel (2006). The TerraMax Autonomous Vehicle. Journal of Field Robotics, 23(9), pp. 693–708. [16] Chen, Y. , V. Sundareswaran, C. Anderson, A. Broggi, P. Grisleri, P.P. Porta, P. Zani and J. Beck (2008). TerraMax: Team Oshkosh Urban Robot. Journal of Field Robotics 25(10), pp. 841–860. [17] Ukawa, H., Idonuma, H. and Fujimura, T. (2002). A study of the autonomous driving system of heavy duty vehicle. Int. J. of Vehicle Design, Vol. 29, N. 1/2, pp. 65-82. 22 [18] Bergenheim, C., Shladover, S. and Coelingh, E. (2012). Overview of platooning systems. Proceedings of the 19th ITS World Congress, Oct 22-26, Vienna, Austria. [19] Chan, E. (2014). SARTRE Automated Platooning Vehicles. Transport Research Arena 2014 Conference, April 14-17, Paris, France. [20] Chan, E., Gilhead, P., Jelinek, P., Krejci, P. and Robinson, T. (2012). Cooperative control of SARTRE automated platoon vehicles. 19th ITS World Congress, Vienna (Austria), 2226 October. [21] Tsugawa, S. (2013). An Overview on an Automated Truck Platoon within the Energy ITS Project. 7th IFAC Symposium on Advances in Automotive Control, September 4-7, Tokyo, Japan. [22] Yoshida, J., Sugimachi, T. and Fukao, T. (2011). Autonomous Driving of a Truck Based on Path Following Control. Trans. of the Japan Society of Mechanical Engineers, Vol. 77, pp. 4125-4135. [23] Deutschle, S., Keßler, G., Hakenberg, M. and Abel, D. (2010). The KONVOI Project - Development and Investigation of Truck Platoons on Highways. Proc. of the FISITA 2010 World Automotive Congress, May 30-June 4, Budapest, Hungary. [24] Alam, A., Besselink, B., Turri, V., Mårtensson, J. and Johansson, K. H. (2015). Heavyduty vehicle platooning towards sustainable freight transportation. IEEE Control Systems Magazine (accepted). 2015. [25] O’Connor M. L., Elkaim G. H., and B. W. Parkinson (1995). Kinematic GPS for closedloop control of farm and construction vehicles. Proc. of ION GPS-95, Palm Springs, CA, Sept. 1995, pp 1261-1268. [26] Naranjo, J., C. Gonzalez, R. Garcia and T. de Pedro (2007). Using Fuzzy Logic in Automated Vehicle Control. Intelligent Systems, Vol. 22, No. 1, pp. 36-45. [27] Heredia, G., Ollero, A., Bejar, M. and Mahtani, R. (2008). Sensor and Actuator Fault Detection in Small Autonomous Helicopters. Mechatronics. Vol. 18, No. 2, pp. 90-99. [28] Nebot, E.M., Durrant-Whyte, H.F., and Scheding, S. (1997). Frequency domain modeling of aided GPS with application to high-speed vehicle navigation systems. Proceedings of the 1997 Conference on Robotics and Automation, New Mexico, USA, April 1997. [29] A. Jazwinsky (1970). Stochastic processes and filter theory. New York, Academic Press. [30] Cormier W.H. and R. Fenton (1980). On the steering of automated vehiclesa velocityadaptive controller. IEEE Trans. AC, pp. 375-385, 1980. [31] Ollero A., and O. Amidi (1991). Predictive path tracking of mobile robots. Applications to the CMU Navlab. Proc. of the Fifth Int. Conf. on Advanced Robotics, Pisa, pp. 10811086. [32] Ollero, A., J. Ferruz, O. Sánchez and G. Heredia (2001). Mobile Robot Path Tracking and Visual Target Tracking Using Fuzzy Logic. In Fuzzy Logic Techniques for Autonomous Vehicle Navigation. Ed. D. Driankov and A. Saffiotti, Physica-Verlag, pp. 51-72. 23 [33] .Wang, H., Tanaka, K. and Griffin, M. (1996). An approach to fuzzy control of nonlinear systems: stability and design issues. IEEE Transactions on Fuzzy Systems, Vol. 4, No. 1, pp. 14-23. [34] M. Johansson, A. Rantzer and K.E. Arzen (1999), “Piecewise Quadratic Stability of Fuzzy Systems”, IEEE Trans. Fuzzy Syst., Vol. 7, No. 6, pp. 713-722. [35] Lam, H.K., Leung, F.H.F. and Tam, P. K.S. (2000). Stable and robust fuzzy control for uncertain nonlinear systems, IEEE Transactions on Systems, Man and Cybernetics, Part A: Systems and Humans, Vol. 30, No. 6, pp. 825-840. [36] Lee, D.H., Joo, Y.H. and Tak, M.H. (2014). Local stability analysis of continuous-time Takagi-Sugeno fuzzy systems: A fuzzy Lyapunov function approach. Information Sciences, Vol. 257, pp. 163-175. [37] Precup, R.E., Tomescu, M., Radac, M.B., Petriu, E., Preitl, S. and Dragos, C.A. (2012). Iterative performance improvement of fuzzy control systems for three tank systems. Expert Systems with Applications, Vol. 39, No. 9, pp. 8288-8299. [38] Ollero A., A. G. Cerezo and J. Martinez (1994). Fuzzy supervisory path tracking of autonomous vehicles. Control Engineering Practice. Vol. 2, No. 2, pp. 313-319. [39] Zhang, J., Chen, D. and Kruger, U. (2008). Adaptive Constraint K-Segment Principal Curves for Intelligent Transportation Systems. IEEE Trans. on Intelligent Transportation Systems, Vol. 9, No. 4, pp. 666-677. [40] Rodriguez-Castaño A., Heredia G. and A. Ollero (2000). Fuzzy path tracking and position estimation of autonomous vehicles using differential GPS. Mathware & Soft Computing, Vol. VII, No. 3, pp. 257-264. [41] Takagi, T. and Sugeno, M. (1985). Fuzzy identification of systems and its application to modeling and control. IEEE Trans. Syst., Man, Cybern., Vol. 15, pp. 116–132. [42] Cheng, H. (2011). Intelligent Autonomous Vehicles: Theory, Algorithms, and Implementation, Springer-Verlag London. [43] Broggi, A., Medici, P., Zani, P., Coati, A. and Panciroli, M. (2012). Autonomous vehicles control in the VisLab Intercontinental Autonomous Challenge. Annual Reviews in Control, Vol. 36, pp. 161-171.