TruckTrix ® Path-Planning in the helyOS Operating System for Yard Automation

In this paper we brieﬂy introduce the helyOS operating system for remote operation of autonomous mobile machines and more precisely the TruckTrix-Path Service, which is used for path planning for yard automation solutions, introduced by Fraunhofer Institute for Transportation and Infrastructure Systems. TruckTrix-Path provides the path planning for complex vehicle structures through critical narrows. Its extensions, developed specially for yard automation problems, adopt calculated trajectories for to be driven by a real automated truck. Alongside with other products of the TruckTrix-Family: TruckTrix-Coop, TruckTrix-Control and TruckTrix-Field it provides a fast and simple solution to control several automated vehicles on closed yards or ﬁelds


I. INTRODUCTION
The topics connected with automated driving are becoming very prominent within the last years.One of the most important scenarios in this field is the automation of closed areas, where it is possible to automate all participating vehicles.For instance, this closed area can be a port, an airport, a logistics center, factory yards or agricultural complexes.Our group works in this field of yard automation and has architected the Highly Efficient online Yard Operating System (helyOS -see [1]), which can be used for every kind of yard automation.
One of the key technologies for all automated driving problems is the collision free path planning, which should take carefully into account the vehicle geometry.This task tends to be much more difficult in the case of the complex vehicle geometries and corresponding complex kinematics, when the vehicle consists of a truck with one or several trailers introducing articulation angles between the vehicle parts.
The solution of such a kind of path planning problem has been implemented in the TruckTrix ® Algorithm (see [2]).Since that time the TruckTrix ® background has been significantly improved and several new features for the path post-processing have been provided.In the following we are going to discuss these achievements and show our results.For instance, in Fig. 1 one can see the path planning result for a complex vehicle structure shifting loads, which drives around the Gaudy Avenue corner in Barcelona, Spain.In this work we are going to show how this result was obtained with the help of TruckTrix ® -Path.
In the paper we are going to briefly discuss the architecture of the helyOS system (section II), then we present the backgrounds of TruckTrix ® -Path algorithm for path planning (section III) and look a bit behind the corresponding cost functional (section IV).The second part of the paper is dedicated to the path post-processing for the TruckTrix ® solution and its certain methods used in TruckTrix ® -Path: trajectory smoothing and the computation of a precise solution at the last steps of the path (section V).In the summary of this paper we will present the step by step creation and improvement of the TruckTrix ® -Path solution for the example of the vehicle simulation on the streets of Barcelona.

II. ARCHITECTURE OF HELYOS
As we have already mentioned, the helyOS system has been created for the operation and control of one or several vehicles (or generally, robots) in some closed area.It can be any area or yard isolated from general public traffic.We have introduced the architecture of helyOS and have applied it in several projects: AutoTruck, where we already successfully demonstrated the functionality (single truck automation on logistic yard -see [3]), Fraunhofer lighthouse project "Cognitive Agriculture" (short: "COGNAC" -see [4,5]), etc.The helyOS framework can be easily fit into any kind of yard automation needs just by providing an additional external service or services for it.In Fig. 2 one can see the architecture overview of helyOS.The system includes HCT (helyOS Control Tower), which is responsible for the yard logistic and for the creation of complex missions for one or several agents, and HAL (helyOS Automation Layer), which provides solutions for an agent (vehicle, sensor or other actors on the yard).Both parts, HCT and HAL, are universal and must be extended for each specific application with the help of plug-in external services.For instance, for the HCT part it can be the path planning service, cooperative driving service, live map updates, e.t.c.
In the present work we will be talking more about the HCT part (left) of helyOS, since our main goal is to discuss the use of path planning services in helyOS.
HCT consists of an internal database and a backend providing the general functionality of helyOS.Upon user input, e.g.via a web frontend, a request (called work process) for some new activity on the yard is written to the database and dispatched to the external service, which corresponds to the specific yard automation application.For some application a work process can consist only of one single service, for example, Trucktrix ® -Path for a single truck driving from A to B. For some other applications there can involve multiple different sequential or parallel services.For instance, if we have a work process request for the trip of one truck under consideration of other work processes already being executed for other trucks, the request should be processed sequentially by the TruckTrix ® -Path and TruckTrix ® -Coop services.The latter service adds time stamps for every point of the trajectory, in order to completely exclude possible collisions with other Fig. 3. Logistics Yard in Autotruck project trucks.For some other projects, e.g.like agricultural processes, it can be necessary to have an external service which provides the common planning for several actors and uses the result of a dynamical map service.We are applying helyOS in two smart agricultural projects and are using the TruckTrix ® -Field service to plan optimal paths for weed and pest control on agricultural fields in the COGNAC project [4,5], as well as other path planning services provided by third-party companies in the project Feldschwarm [6].
However, if we return from agriculture problems to the large class of yard, port or airport logistics automation cases, the key service would be the path planning service for one vehicle or robot on the yard.Therefore, in the following, we will focus on the TruckTrix ® -Path external service as the first one fully applicable external helyOS service.The first helyOS application in combination with the TruckTrix ® external service was the already mentioned AutoTruck project.In Fig. 3 one can see the TruckTrix ® trajectory calculated and driven by the real AutoTruck-vehicle in the the AutoTruck project.

III. TRUCKTRIX ® ALGORITHM
In this section we will briefly describe the TruckTrix ® algorithm.It has been firstly presented in [2].Here, as a basis, we use the A-Star graph search algorithm [7] in multi-dimensional space (2 coordinates, orientation of the truck, as well as all joint angles of all vehicle modules).On the each step we are trying to "drive" the vehicle system (simulate a driving maneuver of the vehicle model) with different steering angles of all steered axles.Note that for complex vehicle structures, several active steering axles are possible.The algorithm takes that into account.These steering angles belong to a discrete set of possible angles.By "drive" we mean the integration of kinematic equations for the vehicle.
The "best" of the calculated steps is set to be the next step and we continue with the calculation from this next step.If certain step the vehicle has a collision with an obstacle, we go one step backwards and continue with the second best step.And so on (see Fig. 4).Therefore, one of the crucial points of the A-Star algorithm is how to choose the "best" step.Intuitively, it could be just the geographically closest step to the target.However, we want the vehicle combination to reach the target with a specific orientation and joint angles between vehicle parts.Furthermore, because of the complex geometry and kinematics, this "best" step choice should take into account the necessary space for vehicle maneuvers.
For this reason we need to have the perfect cost functional, that can point out which of the next steps is the best one.We have derived a special cost functional based on the Hamilton-Jacobi equation.In [8] the Hamilton-Jacobi equation has been used to derive the cost-functional for a simple car with the only one steering axle and without any trailers.We have extended this method for our problem of complex vehicle geometry and complex vehicle kinematics.It will be discussed in the next section.

IV. TRUCKTRIX ® COST-FUNCTIONAL
In this section we are going to look behind the costfunctional used in TruckTrix ® -Path.We will show the derivation of the main terms of the cost-functional.Our functional also has some additional terms, which help us to provide more real-looking trajectory.However, this lies out of the scope of this article.
Let us consider a vehicle, that contains 2 modules: a truck with length L 1 and 2 axles (the first one of them is steerable) and a semitrailer (length L 2 ) with only 1 driver steered axle.Its position on the map can be described by just 4 values: two coordinates x, y, truck orientation θ 0 , and angle between truck and trailer θ 1 .
The cost-functional F ( − → X (t)) should provide for each 4dimensional point − → X (t) = (x(t), y(t), θ 0 (t), θ 1 (t)) an estimation about how far it is from the 4-dimensional target in the sense of an optimal path.For the case of the unit velocity we can write for the cost-functional F ( − → X (t)): where δt is a small time ( = distance, because of the unit velocity) shift.By the limit δt → 0 we will receive: or Firstly, we should deal with the first two terms of this equation.They can be easily rewritten as: However, the x and y coordinates should be considered together and translated in "geographical" distance estimation, that does not care about vehicle geometry.For this reason we should firstly introduce an initial distance function l(x, y) which provides a good estimation of the distance to target.If we have this function, then the first two terms of the equation 4 can be written as follows: where ϑ is the angle between the x axle and the tangential to l(x, y) function.
For the derivation of the last two terms of equation 4 we should notice, that: where ρ min is the minimal radius for the vehicle rotation.This radius can be estimated from the total vehicle length (sum of lengths for truck and trailer L 1 + L 2 ) and maximal steering angle of the first truck axle φ max 1 : The third term of the equation 4 corresponds to the rotation of the truck with the minimal possible radius: For the last term of the equation 4 one should try to reach the maximal possible joint angle change and finally receive: where with φ max 2 as the maximal value of the trailer steering angle.

Before smoothing
After smoothing Therefore, we receive equation for the cost-functional in the following form: (10) The equation 10 is written in the form of the Hamilton-Jacobi equation for effective Hamiltonian F and is frequently used for integration of motion equations in very different physical problems (see [9]).In TruckTrix ® -Path service we use a more complicated equation with additional terms, that are responsible for other aspects important for a smooth trajectory appearance.For more complicated structures with more steerable axles in one or several trailers, we have derived and solved analogous equations.
However, there can still be some artifacts of discrete steering angles and finite step length within the TruckTrix ® calculation.Those results are suitable for passability checks for complex vehicle structures, but can appear to be not always suitable for automated vehicles.To obtain a solution, that can be easily used by automated vehicles we need a smooth trajectory, that precisely reaches the target position.To cover these two restrictions, we have introduced trajectory post-processing that is based on two main procedures, which are introduced in the next two sections of this paper.This post-processing is also part of the TruckTrix ® -Path external service and is used in the helyOS system.

V. POST-PROCESSING OF TRUCKTRIX ® TRAJECTORIES FOR THE USE-CASE OF AUTOMATED DRIVING
As we have mentioned in the previous section, the pure TruckTrix ® results are suitable for the passability checks for complex vehicle structures.In order to make them applicable also for automated vehicles, we need to smooth received trajectory and to develop the solution, that exactly reaches the target position.
Firstly, we should smooth the result of the TruckTrix ® algorithm.The method used in TruckTrix ® -Path is based on approximation through spline-interpolation of the received trajectory (see Fig. 5).Therefore, we can receive smoothed trajectory, that also has a smooth derivative, which is an important requirement for the trajectory following controller of automated vehicles.
The second very important post-processing procedure is connected to the recalculation of the last points of the tra-Fig.6. TruckTrix ® solution with higher resolution at the final steps of trajectory jectory with a much higher resolution (much smaller distance between A-Star points).This is necessary, if at the end of the path the vehicle is going to reach the gate of logistics yard or some other target which should be achieved exactly.
The pure TruckTrix ® solution can have the small deviation of the last steps from the target position.This happens because of limits set on target achievement tolerance.These limits are strongly connected with the path-planning resolution (length of the A-Star step).Therefore, to beat this problem, we have created a corresponding post-processing procedure, which takes a number of the last steps of the TruckTrix ® path solution (for instance, maximum N = 10 steps, but no more than the number of steps since the last point of the driving direction change), and provides a new much more precise solution for this part of the trajectory.For instance, it can be a 10 times more precise solution, which means, that the corresponding step length is 10 times smaller than that of the first TruckTrix ® -calculation.
In Fig. 6 one can see the resulting improvement due to this procedure.Now the vehicle reaches the target with higher precision and can be docked automatically.

VI. SUMMARY: TRUCKTRIX ® FOR COMPLEX VEHICLE
SYSTEM IN BARCELONA In the present paper we have briefly discussed the yard automation issues and its needs for a suitable and reliable path planner.One of these path planners is the TruckTrix ® -Path service.It is based on the A-Star algorithm with a strong mathematical background for the cost-functional and provides many features, that help the algorithm to create the trajectories for complex vehicle combinations, which can be suitable both for passability analyses, as well as for automatic driving.
As a conclusion we would like to return to Fig. 1 and show how this path is going to be improved because of TruckTrix ® -Path features.This can be seen in the Fig.

Fig. 1 .
Fig. 1.Truck-Trailer combination carrying cargo container simulated on Barcelona streets.Green color shows the drivable area, red -obstacles, black lines -traces of wheels on the road.The simulation has been performed with the help of TruckTrix ® -Path.Visualisation is provided by the HeavyGoods ® system (product of the CodeSquare GmbH).

Fig. 4 .
Fig. 4. Schema for A-Star algorithm.Displayed here with only 3 possibilities for the next step (drives forward only and has 3 possible steering angles).Note, that there are more possibilities in TruckTrix ® .Obstacles are shown orange, red arrows mark possibilities with collisions, green arrows denote a feasible way.

7 .
Here we can see a step by step solution change: (a) shows the elementary path function used for calculation of the cost-functional -l(x, y) (see Eq. 5); (b) shows the pure TruckTrix ® solution; (c) shows the TruckTrix ® solution after post-processing -the vehicle is ready to be unloaded near the Sagrada Familia.