- Email: [email protected]
Model Based Path Optimization for Valet Parking with Trailer
9th IFAC International Symposium on Advances in Automotive 9th 9th IFAC IFAC International International Symposium Symposium on on Advances Advances in in Automotive Automotive Control 9th IFAC International Symposium on Advances in Automotive Control Control 9th IFAC France, International Symposium onAvailable Advancesonline in Automotive Orléans, June 23-27, 2019 at www.sciencedirect.com Control Orléans, Orléans, France, France, June June 23-27, 23-27, 2019 2019 Control Orléans, France, June 23-27, 2019 Orléans, France, June 23-27, 2019
ScienceDirect
IFAC PapersOnLine 52-5 (2019) 85–90
Model Based Path Optimization Model Based Path Optimization Model Based Path Optimization Model Based Path Optimization Valet Parking with Trailer Valet Parking with Trailer Valet Parking with Trailer Valet Parking with Trailer ∗∗ ∗ ∗ Alexej Janz ∗ Johanna Schramm ∗ Mitja Echim ∗∗
for for for for
Alexej Janz ∗ Johanna Schramm ∗ Mitja Echim ∗∗ Alexej Janz Johanna Mitja Echim ∗∗ ∗u ∗∗ ∗∗ o del ∗∗∗ Schramm Christof B¨ skens AlexejFrank Janz ∗∗Schr¨ Johanna Schramm Mitja Echim ∗u ∗∗ o Christof skens ∗∗ AlexejFrank Janz Johanna Mitja ∗∗ Frank Schr¨ Schr¨ odel del ∗ Schramm Christof B¨ B¨ u skensEchim Frank Schr¨ o del Christof B¨ u skens ∗ ∗∗ Frank Schr¨ odel Christof B¨ uskens ∗ ∗ Development Center Chemnitz/Stollberg, IAV GmbH, 09366 ∗ Development Center Chemnitz/Stollberg, IAV GmbH, 09366 Development Center Chemnitz/Stollberg, IAV ∗ Stollberg, Germany (e-mail: {Alexej.Janz, Frank.Schroedel}@iav.de) Center Chemnitz/Stollberg, IAV GmbH, GmbH, 09366 09366 ∗ Development Stollberg, Germany (e-mail: {Alexej.Janz, Frank.Schroedel}@iav.de) Development Center Chemnitz/Stollberg, IAV GmbH, 09366 Stollberg, Germany (e-mail: {Alexej.Janz, Frank.Schroedel}@iav.de) ∗∗ ∗∗ Center of Industrial Mathematics, University of Bremen Germany Stollberg, Germany (e-mail: {Alexej.Janz, Frank.Schroedel}@iav.de) Center Industrial Mathematics, University of Bremen Germany ∗∗ Stollberg, Germany (e-mail: {Alexej.Janz, Frank.Schroedel}@iav.de) Center of of Industrial Mathematics, University of Bremen Germany ∗∗ mitja,[email protected]) Industrial Mathematics, University of Bremen Germany ∗∗ Center of(e-mail: mitja,[email protected]) Center of(e-mail: Industrial Mathematics, University of Bremen (e-mail: mitja,[email protected]) mitja,[email protected]) Germany (e-mail: (e-mail: mitja,[email protected]) Abstract: In this article, approach for parking trailer is designed. path Abstract: In this article, an an approach for parking with with trailer is designed. The The path planning planning Abstract: In this an approach for with trailer is The planning Abstract: In parking this article, article, antruck approach for parking parking with trailer is designed. designed. The path path the planning for backward of a and trailer vehicle is aa central aspect, especially path for backward parking of a truck and trailer vehicle is central aspect, especially the path Abstract: In this article, an approach for parking with trailer is designed. The path planning for backward parking of a truck and trailer vehicle is a central aspect, especially the path optimization. For calculating the path a polynomial approach is applied. The used kinematic for backward parking of a truck and trailer vehicle is a central aspect, especially the path optimization. For calculating the path a polynomial approach is applied. The used kinematic for backward parking of a truck and trailer vehicle is a central aspect, especially the path optimization. For calculating the path a polynomial approach is applied. The used kinematic single-track model and assumed smooth signals lead to a polynomial approach, where the optimization. For calculating the path a polynomial approach is applied. The used kinematic single-track model and assumed smooth signals lead to aa polynomial approach, where the optimization. For calculating the path a polynomial approach is applied. The used kinematic single-track model and assumed smooth signals lead to polynomial approach, where the single-track model and assumed smooth signals lead to a polynomial approach, where the starting and ending data like position, angle, curvature, and its derivatives are interpolated. starting and ending data like position, angle, curvature, and its derivatives are interpolated. single-track model and assumed smooth signals lead to a polynomial approach, where the starting and ending data like position, angle, curvature, and its derivatives are interpolated. In this approach there appear degrees of freedom in the solution which interpolates the data starting and ending data like position, angle, curvature, and its derivatives are interpolated. In this approach there appear degrees of freedom in the solution which interpolates the data starting and ending data like position, angle, curvature, and its derivatives are interpolated. In this approach there appear degrees of freedom in the solution which interpolates the data independently of the choice the mentioned The problem can be stated an In this approach there appearfor degrees of freedomvariables. in the solution which interpolates theas data independently of the choice for the mentioned variables. The problem can be stated an In this approach there appear degrees of freedom in the solution which interpolates theas data independently of the choice for the mentioned variables. The problem can be stated as an optimization problem and the free parameters are optimized. independently of the choice for the mentioned variables. The problem can be stated as an optimization problem and the free parameters are optimized. independently of the choice for the mentioned variables. The problem can be stated as an optimization problem problem and and the the free free parameters parameters are are optimized. optimized. optimization optimization problem and Federation the free parameters optimized. © 2019, IFAC (International of Automaticare Control) Hosting by Elsevier Ltd. All rights reserved. Keywords: Path Planning, Optimization Problems, Automated Guided Vehicles, Automobile Industry Keywords: Keywords: Path Path Planning, Planning, Optimization Optimization Problems, Problems, Automated Automated Guided Guided Vehicles, Vehicles, Automobile Automobile Industry Industry Keywords: Path Planning, Optimization Problems, Automated Guided Vehicles, Automobile Industry Keywords: Path Planning, Optimization Problems, Automated Guided Vehicles, Automobile Industry 1. INTRODUCTION and software solutions. An exemplary application of the 1. INTRODUCTION and software solutions. An exemplary application of the 1. and software solutions. An exemplary application of the mentioned topic is the research project eJIT (see RKW 1. INTRODUCTION INTRODUCTION and software solutions. An exemplary application of the mentioned topic is the research project eJIT (see RKW 1. INTRODUCTION and software solutions. An exemplary application of the mentioned topic is the research project eJIT (see RKW Sachsen Rationalisierungsund Innovationszentrum e. V. mentioned topic is the research project eJIT (see RKW Sachsen Rationalisierungsund Innovationszentrum e. V. mentioned topic is the research project eJIT (see RKW Sachsen Rationalisierungsund Innovationszentrum e. V. Highly Automated Driving (HAD) is getting more and Sachsen Rationalisierungsund Innovationszentrum e. V. im World Trade Center Dresden (June 22, 2018)), run by Highly Automated Driving (HAD) is getting more and im World Trade Center Dresden (June 22, 2018)), run by Highlyimportant Automated Drivingsociety (HAD) isincreases getting safety more and and Sachsen Rationalisierungsund Innovationszentrum e. V. Highly Automated Driving (HAD) is getting more im World Trade Center Dresden (June 22, 2018)), run by more to modern and IAV GmbH, Porsche Leipzig GmbH, Schnellecke Logistics more important to modern society and increases safety and im World Trade Center Dresden (June 22, 2018)), run by Highly Automated Driving (HAD) isincreases getting(ACC) more and IAV GmbH, Porsche Leipzig GmbH, Schnellecke Logistics more important to modern society and safety im World Trade Center Dresden (June 22, 2018)), run by IAV GmbH, Porsche Leipzig GmbH, Schnellecke Logistics comfort on the roads. Adaptive Cruise Control more important to modern society and increases safety and and Volkswagen Sachsen GmbH under the leadership of comfort on the roads. Adaptive Cruise Control (ACC) and IAV GmbH, Porsche Leipzig GmbH, Schnellecke Logistics more important to modern society andonly increases safety and Volkswagen Sachsen GmbH under the leadership of comfort on Adaptive Cruise Control (ACC) IAV GmbH, Porsche Leipzig GmbH, Schnellecke Logistics and Volkswagen Sachsen GmbH under the leadership of Lane Keeping Assistant (LKA) are two exemplary comfort on the the roads. roads. Adaptive Cruise Control (ACC) and and AMZ. It deals with fully automated tractors which start at Lane Keeping Assistant (LKA) are only two exemplary and Volkswagen Sachsen GmbH under the leadership of comfort on the roads. Adaptive Cruise Control (ACC) and AMZ. It deals with fully automated tractors which start at Lane Keeping Assistant (LKA) are only two exemplary and Volkswagen Sachsen GmbH under the leadership of Lane Keeping Assistant (LKA) are only two exemplary AMZ. It deals with fully automated tractors which start at modules in the wide field of Advanced Driver Assistant AMZ. It deals with fully automated tractors which start at a loading ramp of a company, navigate through city traffic, modules in the wide field of Advanced Driver Assistant Lane Keeping Assistant (LKA) are only two exemplary a loading ramp of a company, navigate through city traffic, modules (ADAS). in the the wide wide field of Advanced Advanced Driver Assistant AMZ. It deals with fully automated tractors which start at modules in field of Driver Assistant a loading ramp of a company, navigate through city traffic, Systems HAD is one level below fully automated finally reach the Systems (ADAS). HAD is one level belowDriver fully automated acountry loadingroads rampand of a highways company,and navigate through cityloading traffic, modules in the wide field of Advanced Assistant country roads highways and finally reach the Systems (ADAS). HAD one level fully automated acountry loading rampand of acompany. company,Therefore navigate through cityloading traffic, roads and highways and finally reach the loading driving where the driver to monitor should Systems (ADAS). HAD is isonly oneneeds level below below fully and automated ramp of another real circumstances driving where the driver only needs to monitor and should country roads and highways and finally reach the loading Systems (ADAS). HAD isonly oneneeds level below fully automated ramp of another company. Therefore real circumstances driving where the driver to monitor and should country roads and highways and finally reach the loading ramp of another company. Therefore real circumstances not take action in the driving process. The aim is on the driving where the driver only needs to monitor and should are given. not take action in the driving process. The is on ramp of another company. Therefore real circumstances driving where the only needs to monitor and are not take action indriver thethe driving process. The aim aim is should on the the ramp of another company. Therefore real circumstances not in the driving process. The aim is on the are given. given. one side to number of which are given. one take side action to decrease decrease the number of accidents accidents which are are not take action in the driving process. The aim is on the one side to decrease the number of accidents which are The aim of the project is to work on the efficiency of an eare given. one side to decrease the number of accidents which are nowadays mostly caused by human failures and on the aim of project is to efficiency of enowadays caused by human failures and on the The aim of the the project is autonomous to work work on on the the efficiency of an an eone side tomostly decrease the efficiency number of accidents which are The nowadays mostly caused by human failures and on the The aim of the project is to work on the efficiency of an edrive based vehicle and driving in the field other side to offer more in the driving process nowadays mostly caused by human failures and on the drive based vehicle and autonomous driving in the field other side to offer more efficiency in the driving process The aim of the project is to work on the efficiency of an edrive based vehicle vehicle and The autonomous drivingforin inathe the field nowadays mostly caused by human failures and on the other side to offer more efficiency in the driving process drive based and autonomous driving field of just-in-time logistic. latter stands specific and comfort the driver for example through automated other side tofor offer more efficiency in the driving process of just-in-time logistic. The latter stands for a specific and comfort for the driver for example through automated drive based vehicle and autonomous driving in the field of just-in-time logistic. The latter stands for a specific other side to offer more efficiency in the driving process and comfort for the the driver driver for example through automated delivery concept where parts developed the of just-in-time logistic. The are latter stands in for a required specific and comfort for for example through automated parking. Elementary parts of HAD are the data acquisidelivery concept where parts developed the parking. Elementary parts of HAD are the data acquisiof just-in-time logistic. The are latter stands in for a required specific delivery concept where parts are developed in the required and comfort for the driver for example through automated parking. Elementary parts of HAD are the data acquisiamount not until the demand is expressed. For an overall delivery concept where parts are developed in the required tion and processing, the high-level strategy planning and parking. Elementary parts of HAD are the data acquisiamount not until the demand is expressed. For an overall tion and processing, the high-level strategy planning and delivery concept where parts are developed in the required amount not until the demand is expressed. For an overall parking. Elementary parts of HAD are the data acquisition and processing, the high-level strategy planning and amount not until the demand is expressed. For an overall autonomous driving process, every decision is prepared, the low-level vehicle dynamic control. Information from tion and processing, the high-level strategy planning and autonomous driving process, every decision is prepared, the low-level vehicle dynamic control. Information from amount not until the demand is expressed. For an overall autonomous driving process, every decision is prepared, tion and processing, the high-level strategy planning and the low-level vehicle dynamic control. Information from autonomous driving process, every decision is prepared, made and performed without any action of the driver, sensors about the vehicle and its environment have to the low-level vehicle dynamic control. Information from made and performed without any action of the driver, sensors about the vehicle and its environment have to autonomous driving process, every decision is prepared, made and performed without any action of the driver, the low-level vehicle dynamic control. Information from sensors about the vehicle and its environment have to among other parts during the parking process. In this made and performed without any action of the driver, sensors about the vehicle and its environment have to be processed processed first. first. Then, Then, high-level high-level functions functions are are able able to among other parts during the parking process. In this be made and performed without any action of the driver, among other parts during the parking process. In this sensors about the vehicle and its environment have be processed first. Then, high-level functions are able to article, the optimization for a parking path is described among other parts during the parking process. In this plan paths for different maneuvers, like a turn or a lane be processed first. Then, high-level functions are able to article, the optimization for a parking path is described plan paths for different maneuvers, like a turn or a lane among other parts during the parking process. In this article, the optimization for a parking path is described be processed first. Then, high-level functions are able to plan paths for different maneuvers, like a turn or a lane article, the optimization for a parking path is described in more detail. The path planning for backward parking of change. After choosing the actual maneuver its trajectory plan paths for different maneuvers, like a turn or a lane in more detail. The path planning for backward parking of change. After choosing the actual maneuver its trajectory article, the optimization for a parking path is described in more detail. The path planning for backward parking of plan paths for different maneuvers, like a turn or a lane change. After choosing the actual maneuver its trajectory aa truck and trailer is aa central aspect, especially the path in more detail. The path planning for backward parking of is validated and planned. Finally, the actors perform the change. After choosing the actual maneuver its trajectory truck and trailer is central aspect, especially the path is validated and planned. Finally, the actors perform the in more detail. The path planning for backward parking of a truck and trailer is a central aspect, especially the path change. After choosing the actual maneuver its trajectory is validated and planned. Finally, the actors perform the optimization. a truck and trailer is a central aspect, especially the path is validated and planned. Finally, the actors perform the planned steering steering and and acceleration acceleration while while the the controller controller upup- optimization. planned aoptimization. truck and trailer is a central aspect, especially the path is validated and planned. Finally, the actors perform the planned steering and acceleration while the controller updates the trajectory’s coefficients at a specific sampling planned steering and acceleration while the controller up- optimization. dates the trajectory’s coefficients at a specific sampling Path planning implicates calculation of an optimal driving planned steering and acceleration while the controller up- optimization. dates the trajectory’s coefficients at a specific sampling Path planning implicates calculation of an optimal driving time. Therefore a computation unit inside the vehicle and dates the trajectory’s coefficients at a specific sampling Path planning implicates calculation of an optimal driving time. Therefore a computation unit inside the vehicle and path, which a vehicle should follow to reach its destination Path planning implicates calculation of an optimal driving dates the trajectory’s coefficients at a specific sampling time. Therefore a computation unit inside the vehicle and path, which a vehicle should follow to reach its destination Path planning implicates calculation of an optimal driving so-called CAN bus data is needed to allow automated time. Therefore a computation unit inside the vehicle and path, which a vehicle should follow to reach its destination so-called CAN bus data is needed to allow automated avoiding obstacles, while a trajectory additionally includes path, which a vehicle should follow to reach its destination time. Therefore a computation unit inside the vehicle and so-called CAN bus bus data data is is needed needed to to allow allow automated automated path, avoiding obstacles, while aa trajectory additionally includes so-called CAN which a vehicle should follow to reach its destination driving maneuvers. maneuvers. avoiding obstacles, while trajectory additionally includes avoiding obstacles, while a trajectory additionally includes driving time-dependent information information like like velocity velocity and and steering steering ananso-called CAN bus data is needed to allow automated time-dependent driving avoiding obstacles, while a trajectory additionally includes driving maneuvers. maneuvers. time-dependent information like velocity and steering angle. For calculating the path aa polynomial approach is aptime-dependent information like velocity and steering anIncluding automated systems is not only in the interest of driving maneuvers. gle. For calculating the path polynomial approach is apIncluding automated systems is not only in the interest of time-dependent information like velocity and steering angle. For calculating the path a polynomial approach is apIncluding automated systems is not not only in in the the interest of gle. plied. The used kinematic single-track model and assumed For calculating the path a polynomial approach is apIncluding automated systems is only interest of private persons and their vehicles. Especially for the indusplied. The used kinematic single-track model and assumed private persons and their vehicles. Especially for the indusgle. For calculating the path a polynomial approach is applied. The used kinematic single-track model and assumed Including automated systems is not only in the interest of private persons and their vehicles. Especially for the indussmooth signals to a polynomial approach, where conplied. The used lead kinematic single-track model and assumed try, highly or even automated driving can increase the private persons andfully their vehicles. Especially for the indussmooth signals lead to approach, where contry, highly or even automated driving can increase the plied. used kinematic single-track model and assumed smooth signals lead to aaa polynomial polynomial approach, where conprivate persons andfully their vehicles. Especially for the depots industry, highly or even fully automated driving can increase the smooth signals lead to polynomial where conditionsThe in the beginning beginning and end of ofapproach, the planned planned movelogistical efficiency. Despite the fact that industrial try, highly or even fully automated driving can increase the ditions in the and end the movelogistical efficiency. Despite the fact that industrial depots smooth signals lead to a polynomial approach, where conditions in the beginning and end of the planned movetry, highly or even fully automated driving can increase the logistical efficiency. Despite the fact that industrial depots ment like position, angle, curvature, and its derivatives ditions in the beginning and end of the planned movelogistical efficiency. Despite the fact that industrial depots are highly frequented and the amount of drivers is limited, ment like position, angle, curvature, and its derivatives are highly frequented and the amount of drivers is limited, ditions in the beginning and end of the planned movement like position, angle, curvature, and its derivatives logistical efficiency. Despite the fact that industrial depots are highly highly frequented frequented and the the amount amount of drivers drivers vehicles is limited, limited, are interpolated. First, the requirements planned ment like position, angle, curvature, andfor itsthe derivatives are and of is monitoring of the surrounding of heavy-duty is are interpolated. First, the requirements for planned monitoring of the surrounding of heavy-duty is ment likedefined position, angle, curvature, andown itsthe derivatives are interpolated. First, the requirements for the planned are highly frequented and requires the amount of developed drivers vehicles is limited, monitoring of the surrounding of heavy-duty vehicles is path are depending on the vehicle parameters. are interpolated. First, the requirements for the planned more complicated, which fully models monitoring of the surrounding of heavy-duty vehicles is path are defined depending on the vehicle own parameters. more complicated, which requires fully developed models are interpolated. First, the requirements for the planned path are are defined defined depending depending on on the the vehicle vehicle own own parameters. parameters. monitoring of the surrounding of fully heavy-duty vehicles is path more complicated, which requires developed models more complicated, which requires fully developed models path are defined depending on the vehicle own parameters. more complicated, which requires fully developed models
Copyright 85 2405-8963 © © 2019 2019, IFAC IFAC (International Federation of Automatic Control) Hosting by Elsevier Ltd. All rights reserved. Copyright © 2019 85 Copyright © under 2019 IFAC IFAC 85 Control. Peer review responsibility of International Federation of Automatic Copyright © 2019 IFAC 85 Copyright © 2019 IFAC 85 10.1016/j.ifacol.2019.09.014
2019 IFAC AAC 86 Orléans, France, June 23-27, 2019
Alexej Janz et al. / IFAC PapersOnLine 52-5 (2019) 85–90
The first layer implements a perception of the environment. Therefore the main focus is on object detection, own movement acquisition, and its prediction as well as on lane detection. Moreover, a free space (space available for maneuvering) calculation have been developed to ensure safe driving.
Them path parameters to be minimized are defined – e.g. path supposed to be smooth, short and comfortable, which means that change of the steering angle and angle between tractor and trailer should be minimized or at least remain in the tolerable ranges. Therefore the cost function could include the length of the path, the over the path summed steering angle, the summed angle between the truck and its trailer or their derivatives.
The second layer contains motion planning modules such as LKA (lane keeping assistance), ACC, ALCA (automatic lane change assistance) and of course automated parking modules. Operation of these modules is coordinated on the same layer by a module responsible for making decisions based on the acquired environment date, definition of the traffic rules and human comfort.
Another common approach besides a polynomial method is a graph search, e.g. explained in Kant and Zucker (1986). At first appearance, it seems easier to deal with obstacles while using graph theory instead of polynomial functions, but they can easily be included in the optimization process as boundaries on the x- and y-position in the polynomial approach. Krogh and Thorpe state as well a graph search in Krogh and Thorpe (1986). There, the surface and its obstacles are approximated with a grid, where the best path is evaluated. In Bry and Roy (2011) a rapidly-exploring random tree is mentioned to find a fitting path. Another approach is made in Ge and Cui (2000). Ge and Cui use potential functions where the potential is formulated such that the goal position is its minimum. The advantage of the chosen approach in this paper is the feasibility of the solution in each step of the iteration. Moreover, a graph search can have a high need for computational time. Following discussion shows that the achieved result can be integrated into an existing implementation without losing performance in computational time.
A follow-up controller represents the last layer. According to the selected driving mode – e.g. LKA or parking – different controller strategies will be used to follow the trajectory, comfortably slow-down the vehicle until a complete stop or simply execute cruise control. In this layer, the trailer angle control is also implemented, but it will not be discussed in this paper. The model-based path following algorithms located in layer two in the trailer parking module. The module itself already contains functionality to compute parking-in and parking-out scenarios depending on the incoming lane data, parking environment, and driver requests. However, for heavy-duty vehicles, an optimization of the parking path computation is required.
In order to get an introduction to the current work highly automated driving had to be illustrated in this paper (section 2). Further basic formulas and assumptions for kinematic computations are described in the section 3. The main focus in this paper has been made on the background of numerics in the optimization problem in the section 4 and the used optimization software WORHP, see B¨ uskens and Wassel (2012) is introduced in the section 5. Finally, the experiment results are discussed in the section 6 and a summary of the main facts of this paper are given in the conclusion.
This system architecture is transferred to an eJiT-truck with minor adjustments and currently being tested in closed proving ground.
2. HAD-SYSTEM
As it was already mentioned before the kinematic singletrack model is used here to describe single vehicles and towing vehicles (two-parts model: tractor and trailer). (1a) x˙ 1 = v cos(Θ0 − Θ1 ) cos Θ1 (1b) y˙ 1 = v cos(Θ0 − Θ1 ) sin Θ1 v Θ˙0 = tan δ (1c) d0 v sin(Θ0 − Θ1 ) (1d) Θ˙1 = d1 It is assumed that the side slip angle is zero and the trailer is joined with the truck at its rear axle. The most important kinematic parameters used in the equations above are listed in the table 3. (x1 , y1 ) denotes the coordinate of the middle point of the rear axle of the trailer, Θ0 is the angle of the truck and Θ1 the angle of the trailer compared to a global coordinate system xy, v describes the velocity at the middle point of the rear axle of the vehicle, δ stands for the steering angle, d0 denotes the distance of the axles of the truck and accordingly d1 denotes the distance of the rear axles of the truck and the trailer (Fig. 1). The velocity v and the steering angle δ are treated as smooth inputs and
3. VEHICLE KINEMATIC MODEL In this section, the utilized vehicle kinematic model will be introduced. For the automated driving system to remain real-time capable and to avoid unnecessary overcomplication of the designed software a trade-off between complexity and accuracy of the model had to be considered.
IAV GmbH has worked in the domain of highly automated driving for decades. During the last months, IAV GmbH has driven more than 300.000 km on highways in Germany, France, and the USA with their highly automated vehicle prototypes, which are used for the evaluation of new automation concepts. The test vehicle is equipped with additional computation hardware (dSpace micro-autobox and industry PC) in the trunk of the vehicle. These components directly receive data from the vehicle sensors and the bus systems of the vehicle. The environment sensor system plays an essential role in driver assistance systems. By using a variety of sensors, it is possible to perceive the environment of each vehicle with high availability. The software framework, which is implementing the highly automated driving functionality can handle high-speed scenarios (e.g. ACC, lane keeping and active lane change) as well as low-speed scenarios (valet parking assist). The framework consists of three layers – similar to classic robotic solutions. 86
2019 IFAC AAC Orléans, France, June 23-27, 2019
Alexej Janz et al. / IFAC PapersOnLine 52-5 (2019) 85–90
The coefficients αi and βi can be derived using the given interpolation data (3) together with equations (1). These coefficients can be stated explicitly in dependence of these degrees of freedom (cf. Ghilardelli et al. (2014)). The following path interpolates the given data regardless of the free parameters choice. Therefore they can be optimized depending on the desired result. To maintain real-time capability only two of the free parameters having the most significant effect on a planned path are optimized and the remaining are set to zero. The first parameter η1 can be expressed as the curve speed at the starting point and the second parameter η2 as the curve speed at the final point. Please do not confuse it with actual or desired speed; curve speed is neither of them - it is the parameter that behaves similarly to speed. This means it has the same tangent as the speed would have at any point of the path, and its value influence curvature in the path point. The larger η2 is the more straight becomes in the corresponding path point. They are defined as: cos ΘA (6a) p(0) ˙ = η1 sin ΘA cos ΘB (6b) p(1) ˙ = η2 sin ΘB At this point the optimization problem can be defined as follows: min2 F (η) (7a)
are not derived in this approach. Furthermore, the angle between the truck and the trailer is called trailer angle and defined as a difference of heading angles Θtr = Θ0 − Θ1 . y δ d0 v
Θ0
d1 Θ1 y1
x1
x
Fig. 1. Representation of the Towing Vehicle (cf. Ghilardelli et al. (2014)). Table 1. Parameters of the Model x1 x2 Θ0 Θ1 v δ d0 d1
x-coordinate of the middle point of approximated axle of the trailer y-coordinate of the middle point of approximated axle of the trailer heading angle of the truck heading angle of the trailer velocity of the truck in its rear axle position steering angle of front wheels wheelbase of the tractor distance between rear axle and coupling points of the trailer
η∈R+
≤ 5 pB − pA (7b) ≤ η1 ≤ η2 ≤ 5 pB − pA (7c) −π/2 ≤ δ(ui , η) ≤ π/2 (7d) −π/2 ≤ Θtr (ui , η) ≤ π/2 (7e) The cost function F (η) is optimized under some restrictions. The box constraints are based on practical experience. They have to be at least positive and different from zero to avoid uncomfortable curvature values int the planned path. On the other side, very big values of curve speeds lead to an inadequate path (e.g. very long path exceeding parking area). Therefore the bounds based on practical experience are defined in equation (7b) and equation (7c). The steering angle and trailer angle have to evaluated at discrete points ui = i/l for i = 0, . . . , l. In optimization problems, the constraints are mostly given by the problem and its physical circumstances. This is different for the cost function which is not that fixed. In the given problem the cost function can consist of terms for a minimal length, steering angle, steering angle change, trailer angle or trailer angle change. The path should guide the vehicle straight to the parking slot minimizing unnecessary turning (curvature, steering angle) and driving distance and providing a comfortable driving behavior by minimizing steering angle changes. The term for minimized driving distance is defined by the length integral s.t.
4. PROBLEM STATEMENT PATH OPTIMIZATION In this section the concrete optimization problem is derived and some resulting paths are presented. The path is denoted as x1 (u) p(u) = . (2) y1 (u) Here u is the curve parameter with u ∈ [0, 1]. The path should connect the current vehicle location with the destination parking slot considering its orientations. Hereby the path should be comfortably smooth and consider vehicle limitations such as maximal steering angle. Therefore the curvature κ of the planned path curve and its derivatives κ and κ are included in the problem statement. The following data must be interpolated with the resulting path pA , pB ∈ R2 (3a) ΘA , ΘB ∈ [0, 2π] (3b) (3c) κA , κB , κ A , κ B , κ A , κ B ∈ R. The index A corresponds to the starting point of the planned curve and the index B to its last point. The aim is to calculate a path which interpolates a starting and ending conditions. Ghilardelli et al. derived in Ghilardelli et al. (2014) in detail, that a path consisting of two polynomials of ninth order fulfills the given requirements. This means α(u) p(u) = (4) β(u) with 9 9 α(u) = α i ui β(u) = β i ui . (5) i=0
87
1 1
1
min2 sf (η) = min2
η∈R+
η∈R+
0
p(ξ) ˙ dξ.
(8)
The term for minimized trailer angle change is defined as: 1 2 min2 (ξ, η) dξ. (9) Θ˙ tr η∈R+
0
The formula for the trailer angle can be found in Ghilardelli et al. (2014) and its derivative follows:
i=0
87
2019 IFAC AAC 88 Orléans, France, June 23-27, 2019
Θ˙ tr (u, η) =
Alexej Janz et al. / IFAC PapersOnLine 52-5 (2019) 85–90
1/2 d1 α˙ 2 (u) + β˙ 2 (u)
problems with linear constraints. In general, one has to assume that F , g, and h are twice continuously differentiable. The Lagrangian
κ (u). (10) d21 κ2 (u) + 1 The derivative respectively the curve parameter u is denoted by (˙) and the derivative respectively the arch length s is denoted by ( ) . κ(u) is the curvature of the path at point u ∈ [0, 1] (cf. Ghilardelli et al. (2014)). Including the squared trailer angle change in equation (9) means to punish large values even more and additionally no signs need to be considered. An exemplary choice for the cost function is: 1 2 F (η) = λ1 sf (η) + λ2 (ξ, η) dξ (11) Θ˙ tr
L(z, λ, µ) := F (z) + λT g(z) + µT h(z)
(12)
combines objective and constraint functions taking multipliers λ and µ into account. Within the iterative process of the SQP approach the resulting subproblem in iteration k with H [k] := ∇2zz L(z [k] , λ[k] , µ[k] ) is 1 min ∇z F (z [k] )T d + dT H [k] d d 2 , ∀i = 1, ..., li , s.t. gi (z [k] ) + ∇z gi (z [k] )T d ≤ 0 hj (z [k] ) + ∇z hj (z [k] )T d = 0
0
Here λ = (λ1 , λ2 ) ∈ R is a weight on the two terms of the objective function. A good choice is e. g. λ = (0.4, 0.6). It is usual to choose weight factors which sum up to one to achieve a good numerical behavior. In a lot of methods with multi-objective functions the weight’s sum is one, e. g. in the Weighted Sum Method (Marler and Arora (2010)). In this specific case the choice combines the two aims of a minimal final length of the path and a minimal trailer angle change, where a comfortable path is considered as more important than a short path. 2
, ∀j = 1, ..., le .
is called quadratic subproblem of NLP. Often, the Hessian of the Lagrangian H [k] is replaced by an update of BFGS (named after Broyden, Fletcher, Goldfarb, and Shanno) type, which has the additional benefit that only strictly convex quadratic programs have to be solved. This strategy works well for small- to medium-sized problems, but it turns out to be infeasible for large-scale problems, since the update formula generally yield dense matrices, which in turn lead to dense Hessian approximations. To solve largescale problems, one is thus forced to fall back to the exact Hessian of the Lagrangian or to perform non-intersecting, intersecting or even multiply-intersecting block-BFGS updates which WORHP provides. The iterative scheme of the algorithm is as follows:
The objective function depends explicitly on η. In more detail sf as well as Θ˙ tr depend on polynomials α and β and their derivatives, which coefficients αi and βi (i = 0, . . . , 9) depend on η.
(1) choose (z [0] , λ[0] , µ[0] ) ∈ Rn × Rli × Rle and set k := 0. (2) if (z [k] , λ[k] , µ[k] ) fulfills some stopping criterion → STOP. (3) calculate dk ∈ Rn of the quadratic subproblem with λ[k+1] and µ[k+1] . (4) set z [k+1] := z [k] + d[k] and k := k + 1 go to (2).
Numerical results of the explained optimization problem are discussed in section 6. Prior to that, the used optimization software is introduced. 5. NONLINEAR OPTIMIZATION WITH WORHP Nonlinear optimization is a key feature for many applications in industry and science. The general question in this context is how free variables of a model must be chosen to minimize a defined objective function while maintaining certain constraints. The nonlinear optimization problem is defined as follows: let z ∈ Rn be the optimization vector. Furthermore, let F : Rn → R denote the objective function and g : Rn → Rli , h : Rn → Rle denote general nonlinear constraint functions. Then F (z) min
The main benefit of non-linear optimization with SQP method is its local convergence property. Due to the usage of second-order derivatives, one can prove that the algorithm exhibits locally quadratic convergence. WORHP provides efficient routines for computing sparse derivatives by applying graph-coloring methods to finite differences, structure-preserving sparse BFGS update techniques for Hessian approximations and sparse linear algebra to efficiently solve the underlying linear equation systems. Furthermore, it is based on reverse communication, which offers an unprecedented level of interaction between user and NLP solver. Therefore WORHP is able to solve real-world problems (e.g. HAD applications) close to real-time requirements.
z
, i = 1, ..., li (NLP) gi (z) ≤ 0 hj (z) = 0 , j = 1, ..., le is called a nonlinear program (NLP). In general, there are several different algorithms to solve such problems. All of them are some kind of specialization of Newton’s method. The solver WORHP (”We Optimize Really Huge Problems”) was especially developed for large-scale, sparse non-linear optimization problems and is the preferred NLP solver of the European space agency (ESA). WORHP uses either a sparse sequential quadratic programming method (SQP) with an interior point method for the quadratic subproblem or an interior point method on the nonlinear level. The software design was focused on high robustness and application-driven design, see B¨ uskens and Wassel (2012). s.t.
6. APPLICATION EXAMPLE In this section, results of the mentioned method from section 4 with the model from section 3 are discussed. For the optimizing process the software library WORHP is used, introduced in section 5. In the following an exemplary curve of the shape of a C is analyzed because ”rectangular” parking is a usual situation in logistic centers. At first numerical results are discussed, followed by experimental results. For the numerical part the path of the following situation is optimized:
The SQP approach is an iterative method that approximates the optimization problem locally by quadratic sub88
2019 IFAC AAC Orléans, France, June 23-27, 2019
Alexej Janz et al. / IFAC PapersOnLine 52-5 (2019) 85–90
89
0.2 0
Y
δ(u)
40
−0.2
Final Result Initial Guess
−0.4
20
0
0.2
0.4
0.6
0.8
1
u 0
Fig. 3. Resulting Steering Angle. −20
0
20
Optimizing only the total length for the same scenario results in a shorter path with intensive steering in both ends of the planned path. Then again optimizing only the trailer angle change results in a wider curve with a higher length.
X
Fig. 2. Resulting Path. pA = (0, 0)T ΘA = 0 Θtr,A = 0 vA = 0 δA = 0 δ˙A = 0
pB = (−30, 45)T ΘB = −π/2 Θtr,B = 0 vB = 0 δB = 0 δ˙B = 0.
(13a) (13b) (13c) (13d) (13e)
In the explained example 27 NLP-Iterations are needed to converge in the optimization algorithm. The cost function is evaluated 927 times. This means 927 numerical integrals are computed during the algorithm. This is related to the computational time of approximately six seconds which is heavily dependent on the used system. This amount of time suggests that the discussed method is doable in vehicles in general, even if the used code is not finally optimized concerning time efficiency and it gives only a sense or an order of magnitude for the time consumption.
(13f)
In Figure 2 the resulting trailer path is represented in red. The corresponding truck path is shown in blue. The initial guess for tractor path represented by the black dashed line. The path is designed to bring the truck and trailer from the initial position (grey) to the destination location (green). For this particular case the initial guess has been defined as ηinit = (30, 45). Experiments have shown that the distance between starting and destination points and intersection point of direction vectors in the start and destination can serve as a good initial guess. For cases, where this intersection point does not exist or badly located (e.g. too far away) half a distance between starting and destination points can be taken for both initial components. Here the starting point is 30m away of the intersection point of direction vectors and for the destination point, this distance is 45m.
Another important aspect is the dependence of the initial guess. In general in optimization processes the initial guesses are relevant. A motivation for a universal initial guess is stated above. This approach turns out to be functional in several scenarios. An analysis of the problem with random initial values leads to the result that the problem is well posed enough such that the same result is found for different initial values. Only physically relevant values are taken, in this case η ∈ [1, 90]2 . The result always differs because of numerical inaccuracies, but for the required accuracy of the solution, it is not relevant. Summed up
sf
The values of the terms of the objective function (11) for different optimization parameters are: F1 = 62.18 (14a) F1,init = 59.21 F2,init = 10.39 F2 = 3.47 (14b) F = 65.65. (14c) Finit = 69.60 Here F1 belongs to the length of the path, F2 to the summed trailer angle changes and F to the combined objective value. The initial values are displayed on the lefthand side and the final values on the right-hand side. It turns out that the path gets longer, but the trailer angle change is clearly less than at the initial guess. Figure 2 points out, that the resulting path is a feasible path that can be performed by the vehicle. The corresponding development of the steering angle of the truck with respect to the curve parameter is presented in Figure 3. It can be seen that the value of the summed steering angle along the path is clearly smaller than at its initial guess. Minimizing the trailer angle change leads to similar results as minimizing the steering angle or steering angle change. Therefore only one of these terms is mentioned in the objective function.
55
55
54.9
54.9
54.8
54.8
54.7
54.7
54.6
54.6
54.5
54.5
sf
−40
54.4
54.4
54.3
54.3
54.2
54.2
54.1
54.1 54
54 0
500 Experiment
1000
Variation Interval [5, 90]
Fig. 4. Statistical Result Exemplary for the Objective Term of the Total Length sf . 1000 Experiments with a Random Variation of η ∈ [5, 90]2 are Executed. 89
2019 IFAC AAC Orléans, France, June 23-27, 2019 90
Alexej Janz et al. / IFAC PapersOnLine 52-5 (2019) 85–90
Fig. 5. Total Length sf in Dependence of η1 , η2 .
Fig. 6. Integral of Trailer Angle Change in Dep. of η1 , η2 .
good initial guesses lead to a shorter computational time but do not change the resulting paths.
Therefore the test vehicle, as well as the kinematic model of the vehicle, were introduced firstly. Thereafter the problem statement of the path optimization was discussed in detail. The main challenges were formulating the problem in a suitable form to create comfortable driving behavior and using a suitable formulation for the numerical solver. The problem results in an NLP, which was efficiently solved by using the optimization toolkit WORHP. Finally, the proposed solution strategy was validated by using a real-world measurement data of a valet parking application with trailer. Based on this, it was shown that WORHP provides a suitable solution in order to solve the problem in real time and the proposed approach works in a suitable way. Further work will focus on a detailed validation of the proposed vehicle guidance concept in a 24/7 pilot operation. This will serve as a final stage of the eJIT project.
For validating the developed implementation the dependence on the initial guesses has to be analyzed. It appears that a random variation of η1,init and η2,init inside the interval [5, 90] leads to almost the same result, but not exactly the same values, which can be seen in Figure 4. Some outliers can be seen but the mainly derived interval for the objective value is accurate enough. The optimization with the above motivated initial guess leads to an objective value of 54.27 which is inside this interval. Another interesting aspect for understanding the optimization process is to deal with the objective function without optimizing variables. Since two variables are considered in the given problem a representation of the total length or the trailer angle change can be done as in Figure 5 and Figure 6. It can be seen that both of the cost terms look friendly for an optimization process. Wherever the initial value for the shortest length lies, the result should be very small for η1 and η2 . Similar to the trailer angle change the low region in the middle of the plot should be reached through optimization. For the combined objective function these arguments have to be combined as well.
REFERENCES Bry, A. and Roy, N. (2011). Rapidly-exploring random belief trees for motion planning under uncertainty. In Robotics and Automation (ICRA), 2011 IEEE International Conference on, 723–730. IEEE. B¨ uskens, C. and Wassel, D. (2012). The esa nlp solver worhp. In Modeling and Optimization in Space Engineering, 85–110. Springer. Ge, S.S. and Cui, Y.J. (2000). New potential functions for mobile robot path planning. IEEE Transactions on robotics and automation, 16(5), 615–620. Ghilardelli, F., Lini, G., and Piazzi, A. (2014). Path generation using η 4 -splines for a truck and trailer vehicle. IEEE Transactions on Automation Science and Engineering, 11(1), 187–203. Kant, K. and Zucker, S.W. (1986). Toward efficient trajectory planning: The path-velocity decomposition. The International Journal of Robotics Research, 5(3), 72–89. Krogh, B. and Thorpe, C. (1986). Integrated path planning and dynamic steering control for autonomous vehicles. In Robotics and Automation. Proceedings. 1986 IEEE International Conference on, volume 3, 1664– 1669. IEEE. Marler, R.T. and Arora, J.S. (2010). The weighted sum method for multi-objective optimization: New insights. Structural and multidisciplinary optimization, 41(6), 853–862. RKW Sachsen Rationalisierungs- und Innovationszentrum e. V. im World Trade Center Dresden (June 22, 2018). ejit. just electric. http://e-jit.de/.
In addition to the previous results, the implementation of the explained method is included into an existing code of an automated parking module. The mentioned approach can be applied to personal cars as well as to heavy-duty trucks towing a single trailer. In this example the objective values are: F1 = 44.66 (15a) F1,init = 42.37 F2,init = 21.27 F2 = 8.19 (15b) F = 22.78. (15c) Finit = 29.71 Here, 21 NLP-iterations are needed. As before the objective value is less than at the initial guess, but the length gets slightly greater while the trailer angle change gets essential smaller. For other shapes, this is not always the case, e. g. for an S shaped path. These results lead to the conclusion that the analyzed method is appropriate to be included in the implementation of a truck and trailer vehicle. Of course, more scenarios and critical cases must be treated in the future. 7. CONCLUSION A model based path optimization strategy for valet parking with trucks (incl. trailer) was presented in the paper. 90
ScienceDirect
IFAC PapersOnLine 52-5 (2019) 85–90
Model Based Path Optimization Model Based Path Optimization Model Based Path Optimization Model Based Path Optimization Valet Parking with Trailer Valet Parking with Trailer Valet Parking with Trailer Valet Parking with Trailer ∗∗ ∗ ∗ Alexej Janz ∗ Johanna Schramm ∗ Mitja Echim ∗∗
for for for for
Alexej Janz ∗ Johanna Schramm ∗ Mitja Echim ∗∗ Alexej Janz Johanna Mitja Echim ∗∗ ∗u ∗∗ ∗∗ o del ∗∗∗ Schramm Christof B¨ skens AlexejFrank Janz ∗∗Schr¨ Johanna Schramm Mitja Echim ∗u ∗∗ o Christof skens ∗∗ AlexejFrank Janz Johanna Mitja ∗∗ Frank Schr¨ Schr¨ odel del ∗ Schramm Christof B¨ B¨ u skensEchim Frank Schr¨ o del Christof B¨ u skens ∗ ∗∗ Frank Schr¨ odel Christof B¨ uskens ∗ ∗ Development Center Chemnitz/Stollberg, IAV GmbH, 09366 ∗ Development Center Chemnitz/Stollberg, IAV GmbH, 09366 Development Center Chemnitz/Stollberg, IAV ∗ Stollberg, Germany (e-mail: {Alexej.Janz, Frank.Schroedel}@iav.de) Center Chemnitz/Stollberg, IAV GmbH, GmbH, 09366 09366 ∗ Development Stollberg, Germany (e-mail: {Alexej.Janz, Frank.Schroedel}@iav.de) Development Center Chemnitz/Stollberg, IAV GmbH, 09366 Stollberg, Germany (e-mail: {Alexej.Janz, Frank.Schroedel}@iav.de) ∗∗ ∗∗ Center of Industrial Mathematics, University of Bremen Germany Stollberg, Germany (e-mail: {Alexej.Janz, Frank.Schroedel}@iav.de) Center Industrial Mathematics, University of Bremen Germany ∗∗ Stollberg, Germany (e-mail: {Alexej.Janz, Frank.Schroedel}@iav.de) Center of of Industrial Mathematics, University of Bremen Germany ∗∗ mitja,[email protected]) Industrial Mathematics, University of Bremen Germany ∗∗ Center of(e-mail: mitja,[email protected]) Center of(e-mail: Industrial Mathematics, University of Bremen (e-mail: mitja,[email protected]) mitja,[email protected]) Germany (e-mail: (e-mail: mitja,[email protected]) Abstract: In this article, approach for parking trailer is designed. path Abstract: In this article, an an approach for parking with with trailer is designed. The The path planning planning Abstract: In this an approach for with trailer is The planning Abstract: In parking this article, article, antruck approach for parking parking with trailer is designed. designed. The path path the planning for backward of a and trailer vehicle is aa central aspect, especially path for backward parking of a truck and trailer vehicle is central aspect, especially the path Abstract: In this article, an approach for parking with trailer is designed. The path planning for backward parking of a truck and trailer vehicle is a central aspect, especially the path optimization. For calculating the path a polynomial approach is applied. The used kinematic for backward parking of a truck and trailer vehicle is a central aspect, especially the path optimization. For calculating the path a polynomial approach is applied. The used kinematic for backward parking of a truck and trailer vehicle is a central aspect, especially the path optimization. For calculating the path a polynomial approach is applied. The used kinematic single-track model and assumed smooth signals lead to a polynomial approach, where the optimization. For calculating the path a polynomial approach is applied. The used kinematic single-track model and assumed smooth signals lead to aa polynomial approach, where the optimization. For calculating the path a polynomial approach is applied. The used kinematic single-track model and assumed smooth signals lead to polynomial approach, where the single-track model and assumed smooth signals lead to a polynomial approach, where the starting and ending data like position, angle, curvature, and its derivatives are interpolated. starting and ending data like position, angle, curvature, and its derivatives are interpolated. single-track model and assumed smooth signals lead to a polynomial approach, where the starting and ending data like position, angle, curvature, and its derivatives are interpolated. In this approach there appear degrees of freedom in the solution which interpolates the data starting and ending data like position, angle, curvature, and its derivatives are interpolated. In this approach there appear degrees of freedom in the solution which interpolates the data starting and ending data like position, angle, curvature, and its derivatives are interpolated. In this approach there appear degrees of freedom in the solution which interpolates the data independently of the choice the mentioned The problem can be stated an In this approach there appearfor degrees of freedomvariables. in the solution which interpolates theas data independently of the choice for the mentioned variables. The problem can be stated an In this approach there appear degrees of freedom in the solution which interpolates theas data independently of the choice for the mentioned variables. The problem can be stated as an optimization problem and the free parameters are optimized. independently of the choice for the mentioned variables. The problem can be stated as an optimization problem and the free parameters are optimized. independently of the choice for the mentioned variables. The problem can be stated as an optimization problem problem and and the the free free parameters parameters are are optimized. optimized. optimization optimization problem and Federation the free parameters optimized. © 2019, IFAC (International of Automaticare Control) Hosting by Elsevier Ltd. All rights reserved. Keywords: Path Planning, Optimization Problems, Automated Guided Vehicles, Automobile Industry Keywords: Keywords: Path Path Planning, Planning, Optimization Optimization Problems, Problems, Automated Automated Guided Guided Vehicles, Vehicles, Automobile Automobile Industry Industry Keywords: Path Planning, Optimization Problems, Automated Guided Vehicles, Automobile Industry Keywords: Path Planning, Optimization Problems, Automated Guided Vehicles, Automobile Industry 1. INTRODUCTION and software solutions. An exemplary application of the 1. INTRODUCTION and software solutions. An exemplary application of the 1. and software solutions. An exemplary application of the mentioned topic is the research project eJIT (see RKW 1. INTRODUCTION INTRODUCTION and software solutions. An exemplary application of the mentioned topic is the research project eJIT (see RKW 1. INTRODUCTION and software solutions. An exemplary application of the mentioned topic is the research project eJIT (see RKW Sachsen Rationalisierungsund Innovationszentrum e. V. mentioned topic is the research project eJIT (see RKW Sachsen Rationalisierungsund Innovationszentrum e. V. mentioned topic is the research project eJIT (see RKW Sachsen Rationalisierungsund Innovationszentrum e. V. Highly Automated Driving (HAD) is getting more and Sachsen Rationalisierungsund Innovationszentrum e. V. im World Trade Center Dresden (June 22, 2018)), run by Highly Automated Driving (HAD) is getting more and im World Trade Center Dresden (June 22, 2018)), run by Highlyimportant Automated Drivingsociety (HAD) isincreases getting safety more and and Sachsen Rationalisierungsund Innovationszentrum e. V. Highly Automated Driving (HAD) is getting more im World Trade Center Dresden (June 22, 2018)), run by more to modern and IAV GmbH, Porsche Leipzig GmbH, Schnellecke Logistics more important to modern society and increases safety and im World Trade Center Dresden (June 22, 2018)), run by Highly Automated Driving (HAD) isincreases getting(ACC) more and IAV GmbH, Porsche Leipzig GmbH, Schnellecke Logistics more important to modern society and safety im World Trade Center Dresden (June 22, 2018)), run by IAV GmbH, Porsche Leipzig GmbH, Schnellecke Logistics comfort on the roads. Adaptive Cruise Control more important to modern society and increases safety and and Volkswagen Sachsen GmbH under the leadership of comfort on the roads. Adaptive Cruise Control (ACC) and IAV GmbH, Porsche Leipzig GmbH, Schnellecke Logistics more important to modern society andonly increases safety and Volkswagen Sachsen GmbH under the leadership of comfort on Adaptive Cruise Control (ACC) IAV GmbH, Porsche Leipzig GmbH, Schnellecke Logistics and Volkswagen Sachsen GmbH under the leadership of Lane Keeping Assistant (LKA) are two exemplary comfort on the the roads. roads. Adaptive Cruise Control (ACC) and and AMZ. It deals with fully automated tractors which start at Lane Keeping Assistant (LKA) are only two exemplary and Volkswagen Sachsen GmbH under the leadership of comfort on the roads. Adaptive Cruise Control (ACC) and AMZ. It deals with fully automated tractors which start at Lane Keeping Assistant (LKA) are only two exemplary and Volkswagen Sachsen GmbH under the leadership of Lane Keeping Assistant (LKA) are only two exemplary AMZ. It deals with fully automated tractors which start at modules in the wide field of Advanced Driver Assistant AMZ. It deals with fully automated tractors which start at a loading ramp of a company, navigate through city traffic, modules in the wide field of Advanced Driver Assistant Lane Keeping Assistant (LKA) are only two exemplary a loading ramp of a company, navigate through city traffic, modules (ADAS). in the the wide wide field of Advanced Advanced Driver Assistant AMZ. It deals with fully automated tractors which start at modules in field of Driver Assistant a loading ramp of a company, navigate through city traffic, Systems HAD is one level below fully automated finally reach the Systems (ADAS). HAD is one level belowDriver fully automated acountry loadingroads rampand of a highways company,and navigate through cityloading traffic, modules in the wide field of Advanced Assistant country roads highways and finally reach the Systems (ADAS). HAD one level fully automated acountry loading rampand of acompany. company,Therefore navigate through cityloading traffic, roads and highways and finally reach the loading driving where the driver to monitor should Systems (ADAS). HAD is isonly oneneeds level below below fully and automated ramp of another real circumstances driving where the driver only needs to monitor and should country roads and highways and finally reach the loading Systems (ADAS). HAD isonly oneneeds level below fully automated ramp of another company. Therefore real circumstances driving where the driver to monitor and should country roads and highways and finally reach the loading ramp of another company. Therefore real circumstances not take action in the driving process. The aim is on the driving where the driver only needs to monitor and should are given. not take action in the driving process. The is on ramp of another company. Therefore real circumstances driving where the only needs to monitor and are not take action indriver thethe driving process. The aim aim is should on the the ramp of another company. Therefore real circumstances not in the driving process. The aim is on the are given. given. one side to number of which are given. one take side action to decrease decrease the number of accidents accidents which are are not take action in the driving process. The aim is on the one side to decrease the number of accidents which are The aim of the project is to work on the efficiency of an eare given. one side to decrease the number of accidents which are nowadays mostly caused by human failures and on the aim of project is to efficiency of enowadays caused by human failures and on the The aim of the the project is autonomous to work work on on the the efficiency of an an eone side tomostly decrease the efficiency number of accidents which are The nowadays mostly caused by human failures and on the The aim of the project is to work on the efficiency of an edrive based vehicle and driving in the field other side to offer more in the driving process nowadays mostly caused by human failures and on the drive based vehicle and autonomous driving in the field other side to offer more efficiency in the driving process The aim of the project is to work on the efficiency of an edrive based vehicle vehicle and The autonomous drivingforin inathe the field nowadays mostly caused by human failures and on the other side to offer more efficiency in the driving process drive based and autonomous driving field of just-in-time logistic. latter stands specific and comfort the driver for example through automated other side tofor offer more efficiency in the driving process of just-in-time logistic. The latter stands for a specific and comfort for the driver for example through automated drive based vehicle and autonomous driving in the field of just-in-time logistic. The latter stands for a specific other side to offer more efficiency in the driving process and comfort for the the driver driver for example through automated delivery concept where parts developed the of just-in-time logistic. The are latter stands in for a required specific and comfort for for example through automated parking. Elementary parts of HAD are the data acquisidelivery concept where parts developed the parking. Elementary parts of HAD are the data acquisiof just-in-time logistic. The are latter stands in for a required specific delivery concept where parts are developed in the required and comfort for the driver for example through automated parking. Elementary parts of HAD are the data acquisiamount not until the demand is expressed. For an overall delivery concept where parts are developed in the required tion and processing, the high-level strategy planning and parking. Elementary parts of HAD are the data acquisiamount not until the demand is expressed. For an overall tion and processing, the high-level strategy planning and delivery concept where parts are developed in the required amount not until the demand is expressed. For an overall parking. Elementary parts of HAD are the data acquisition and processing, the high-level strategy planning and amount not until the demand is expressed. For an overall autonomous driving process, every decision is prepared, the low-level vehicle dynamic control. Information from tion and processing, the high-level strategy planning and autonomous driving process, every decision is prepared, the low-level vehicle dynamic control. Information from amount not until the demand is expressed. For an overall autonomous driving process, every decision is prepared, tion and processing, the high-level strategy planning and the low-level vehicle dynamic control. Information from autonomous driving process, every decision is prepared, made and performed without any action of the driver, sensors about the vehicle and its environment have to the low-level vehicle dynamic control. Information from made and performed without any action of the driver, sensors about the vehicle and its environment have to autonomous driving process, every decision is prepared, made and performed without any action of the driver, the low-level vehicle dynamic control. Information from sensors about the vehicle and its environment have to among other parts during the parking process. In this made and performed without any action of the driver, sensors about the vehicle and its environment have to be processed processed first. first. Then, Then, high-level high-level functions functions are are able able to among other parts during the parking process. In this be made and performed without any action of the driver, among other parts during the parking process. In this sensors about the vehicle and its environment have be processed first. Then, high-level functions are able to article, the optimization for a parking path is described among other parts during the parking process. In this plan paths for different maneuvers, like a turn or a lane be processed first. Then, high-level functions are able to article, the optimization for a parking path is described plan paths for different maneuvers, like a turn or a lane among other parts during the parking process. In this article, the optimization for a parking path is described be processed first. Then, high-level functions are able to plan paths for different maneuvers, like a turn or a lane article, the optimization for a parking path is described in more detail. The path planning for backward parking of change. After choosing the actual maneuver its trajectory plan paths for different maneuvers, like a turn or a lane in more detail. The path planning for backward parking of change. After choosing the actual maneuver its trajectory article, the optimization for a parking path is described in more detail. The path planning for backward parking of plan paths for different maneuvers, like a turn or a lane change. After choosing the actual maneuver its trajectory aa truck and trailer is aa central aspect, especially the path in more detail. The path planning for backward parking of is validated and planned. Finally, the actors perform the change. After choosing the actual maneuver its trajectory truck and trailer is central aspect, especially the path is validated and planned. Finally, the actors perform the in more detail. The path planning for backward parking of a truck and trailer is a central aspect, especially the path change. After choosing the actual maneuver its trajectory is validated and planned. Finally, the actors perform the optimization. a truck and trailer is a central aspect, especially the path is validated and planned. Finally, the actors perform the planned steering steering and and acceleration acceleration while while the the controller controller upup- optimization. planned aoptimization. truck and trailer is a central aspect, especially the path is validated and planned. Finally, the actors perform the planned steering and acceleration while the controller updates the trajectory’s coefficients at a specific sampling planned steering and acceleration while the controller up- optimization. dates the trajectory’s coefficients at a specific sampling Path planning implicates calculation of an optimal driving planned steering and acceleration while the controller up- optimization. dates the trajectory’s coefficients at a specific sampling Path planning implicates calculation of an optimal driving time. Therefore a computation unit inside the vehicle and dates the trajectory’s coefficients at a specific sampling Path planning implicates calculation of an optimal driving time. Therefore a computation unit inside the vehicle and path, which a vehicle should follow to reach its destination Path planning implicates calculation of an optimal driving dates the trajectory’s coefficients at a specific sampling time. Therefore a computation unit inside the vehicle and path, which a vehicle should follow to reach its destination Path planning implicates calculation of an optimal driving so-called CAN bus data is needed to allow automated time. Therefore a computation unit inside the vehicle and path, which a vehicle should follow to reach its destination so-called CAN bus data is needed to allow automated avoiding obstacles, while a trajectory additionally includes path, which a vehicle should follow to reach its destination time. Therefore a computation unit inside the vehicle and so-called CAN bus bus data data is is needed needed to to allow allow automated automated path, avoiding obstacles, while aa trajectory additionally includes so-called CAN which a vehicle should follow to reach its destination driving maneuvers. maneuvers. avoiding obstacles, while trajectory additionally includes avoiding obstacles, while a trajectory additionally includes driving time-dependent information information like like velocity velocity and and steering steering ananso-called CAN bus data is needed to allow automated time-dependent driving avoiding obstacles, while a trajectory additionally includes driving maneuvers. maneuvers. time-dependent information like velocity and steering angle. For calculating the path aa polynomial approach is aptime-dependent information like velocity and steering anIncluding automated systems is not only in the interest of driving maneuvers. gle. For calculating the path polynomial approach is apIncluding automated systems is not only in the interest of time-dependent information like velocity and steering angle. For calculating the path a polynomial approach is apIncluding automated systems is not not only in in the the interest of gle. plied. The used kinematic single-track model and assumed For calculating the path a polynomial approach is apIncluding automated systems is only interest of private persons and their vehicles. Especially for the indusplied. The used kinematic single-track model and assumed private persons and their vehicles. Especially for the indusgle. For calculating the path a polynomial approach is applied. The used kinematic single-track model and assumed Including automated systems is not only in the interest of private persons and their vehicles. Especially for the indussmooth signals to a polynomial approach, where conplied. The used lead kinematic single-track model and assumed try, highly or even automated driving can increase the private persons andfully their vehicles. Especially for the indussmooth signals lead to approach, where contry, highly or even automated driving can increase the plied. used kinematic single-track model and assumed smooth signals lead to aaa polynomial polynomial approach, where conprivate persons andfully their vehicles. Especially for the depots industry, highly or even fully automated driving can increase the smooth signals lead to polynomial where conditionsThe in the beginning beginning and end of ofapproach, the planned planned movelogistical efficiency. Despite the fact that industrial try, highly or even fully automated driving can increase the ditions in the and end the movelogistical efficiency. Despite the fact that industrial depots smooth signals lead to a polynomial approach, where conditions in the beginning and end of the planned movetry, highly or even fully automated driving can increase the logistical efficiency. Despite the fact that industrial depots ment like position, angle, curvature, and its derivatives ditions in the beginning and end of the planned movelogistical efficiency. Despite the fact that industrial depots are highly frequented and the amount of drivers is limited, ment like position, angle, curvature, and its derivatives are highly frequented and the amount of drivers is limited, ditions in the beginning and end of the planned movement like position, angle, curvature, and its derivatives logistical efficiency. Despite the fact that industrial depots are highly highly frequented frequented and the the amount amount of drivers drivers vehicles is limited, limited, are interpolated. First, the requirements planned ment like position, angle, curvature, andfor itsthe derivatives are and of is monitoring of the surrounding of heavy-duty is are interpolated. First, the requirements for planned monitoring of the surrounding of heavy-duty is ment likedefined position, angle, curvature, andown itsthe derivatives are interpolated. First, the requirements for the planned are highly frequented and requires the amount of developed drivers vehicles is limited, monitoring of the surrounding of heavy-duty vehicles is path are depending on the vehicle parameters. are interpolated. First, the requirements for the planned more complicated, which fully models monitoring of the surrounding of heavy-duty vehicles is path are defined depending on the vehicle own parameters. more complicated, which requires fully developed models are interpolated. First, the requirements for the planned path are are defined defined depending depending on on the the vehicle vehicle own own parameters. parameters. monitoring of the surrounding of fully heavy-duty vehicles is path more complicated, which requires developed models more complicated, which requires fully developed models path are defined depending on the vehicle own parameters. more complicated, which requires fully developed models
Copyright 85 2405-8963 © © 2019 2019, IFAC IFAC (International Federation of Automatic Control) Hosting by Elsevier Ltd. All rights reserved. Copyright © 2019 85 Copyright © under 2019 IFAC IFAC 85 Control. Peer review responsibility of International Federation of Automatic Copyright © 2019 IFAC 85 Copyright © 2019 IFAC 85 10.1016/j.ifacol.2019.09.014
2019 IFAC AAC 86 Orléans, France, June 23-27, 2019
Alexej Janz et al. / IFAC PapersOnLine 52-5 (2019) 85–90
The first layer implements a perception of the environment. Therefore the main focus is on object detection, own movement acquisition, and its prediction as well as on lane detection. Moreover, a free space (space available for maneuvering) calculation have been developed to ensure safe driving.
Them path parameters to be minimized are defined – e.g. path supposed to be smooth, short and comfortable, which means that change of the steering angle and angle between tractor and trailer should be minimized or at least remain in the tolerable ranges. Therefore the cost function could include the length of the path, the over the path summed steering angle, the summed angle between the truck and its trailer or their derivatives.
The second layer contains motion planning modules such as LKA (lane keeping assistance), ACC, ALCA (automatic lane change assistance) and of course automated parking modules. Operation of these modules is coordinated on the same layer by a module responsible for making decisions based on the acquired environment date, definition of the traffic rules and human comfort.
Another common approach besides a polynomial method is a graph search, e.g. explained in Kant and Zucker (1986). At first appearance, it seems easier to deal with obstacles while using graph theory instead of polynomial functions, but they can easily be included in the optimization process as boundaries on the x- and y-position in the polynomial approach. Krogh and Thorpe state as well a graph search in Krogh and Thorpe (1986). There, the surface and its obstacles are approximated with a grid, where the best path is evaluated. In Bry and Roy (2011) a rapidly-exploring random tree is mentioned to find a fitting path. Another approach is made in Ge and Cui (2000). Ge and Cui use potential functions where the potential is formulated such that the goal position is its minimum. The advantage of the chosen approach in this paper is the feasibility of the solution in each step of the iteration. Moreover, a graph search can have a high need for computational time. Following discussion shows that the achieved result can be integrated into an existing implementation without losing performance in computational time.
A follow-up controller represents the last layer. According to the selected driving mode – e.g. LKA or parking – different controller strategies will be used to follow the trajectory, comfortably slow-down the vehicle until a complete stop or simply execute cruise control. In this layer, the trailer angle control is also implemented, but it will not be discussed in this paper. The model-based path following algorithms located in layer two in the trailer parking module. The module itself already contains functionality to compute parking-in and parking-out scenarios depending on the incoming lane data, parking environment, and driver requests. However, for heavy-duty vehicles, an optimization of the parking path computation is required.
In order to get an introduction to the current work highly automated driving had to be illustrated in this paper (section 2). Further basic formulas and assumptions for kinematic computations are described in the section 3. The main focus in this paper has been made on the background of numerics in the optimization problem in the section 4 and the used optimization software WORHP, see B¨ uskens and Wassel (2012) is introduced in the section 5. Finally, the experiment results are discussed in the section 6 and a summary of the main facts of this paper are given in the conclusion.
This system architecture is transferred to an eJiT-truck with minor adjustments and currently being tested in closed proving ground.
2. HAD-SYSTEM
As it was already mentioned before the kinematic singletrack model is used here to describe single vehicles and towing vehicles (two-parts model: tractor and trailer). (1a) x˙ 1 = v cos(Θ0 − Θ1 ) cos Θ1 (1b) y˙ 1 = v cos(Θ0 − Θ1 ) sin Θ1 v Θ˙0 = tan δ (1c) d0 v sin(Θ0 − Θ1 ) (1d) Θ˙1 = d1 It is assumed that the side slip angle is zero and the trailer is joined with the truck at its rear axle. The most important kinematic parameters used in the equations above are listed in the table 3. (x1 , y1 ) denotes the coordinate of the middle point of the rear axle of the trailer, Θ0 is the angle of the truck and Θ1 the angle of the trailer compared to a global coordinate system xy, v describes the velocity at the middle point of the rear axle of the vehicle, δ stands for the steering angle, d0 denotes the distance of the axles of the truck and accordingly d1 denotes the distance of the rear axles of the truck and the trailer (Fig. 1). The velocity v and the steering angle δ are treated as smooth inputs and
3. VEHICLE KINEMATIC MODEL In this section, the utilized vehicle kinematic model will be introduced. For the automated driving system to remain real-time capable and to avoid unnecessary overcomplication of the designed software a trade-off between complexity and accuracy of the model had to be considered.
IAV GmbH has worked in the domain of highly automated driving for decades. During the last months, IAV GmbH has driven more than 300.000 km on highways in Germany, France, and the USA with their highly automated vehicle prototypes, which are used for the evaluation of new automation concepts. The test vehicle is equipped with additional computation hardware (dSpace micro-autobox and industry PC) in the trunk of the vehicle. These components directly receive data from the vehicle sensors and the bus systems of the vehicle. The environment sensor system plays an essential role in driver assistance systems. By using a variety of sensors, it is possible to perceive the environment of each vehicle with high availability. The software framework, which is implementing the highly automated driving functionality can handle high-speed scenarios (e.g. ACC, lane keeping and active lane change) as well as low-speed scenarios (valet parking assist). The framework consists of three layers – similar to classic robotic solutions. 86
2019 IFAC AAC Orléans, France, June 23-27, 2019
Alexej Janz et al. / IFAC PapersOnLine 52-5 (2019) 85–90
The coefficients αi and βi can be derived using the given interpolation data (3) together with equations (1). These coefficients can be stated explicitly in dependence of these degrees of freedom (cf. Ghilardelli et al. (2014)). The following path interpolates the given data regardless of the free parameters choice. Therefore they can be optimized depending on the desired result. To maintain real-time capability only two of the free parameters having the most significant effect on a planned path are optimized and the remaining are set to zero. The first parameter η1 can be expressed as the curve speed at the starting point and the second parameter η2 as the curve speed at the final point. Please do not confuse it with actual or desired speed; curve speed is neither of them - it is the parameter that behaves similarly to speed. This means it has the same tangent as the speed would have at any point of the path, and its value influence curvature in the path point. The larger η2 is the more straight becomes in the corresponding path point. They are defined as: cos ΘA (6a) p(0) ˙ = η1 sin ΘA cos ΘB (6b) p(1) ˙ = η2 sin ΘB At this point the optimization problem can be defined as follows: min2 F (η) (7a)
are not derived in this approach. Furthermore, the angle between the truck and the trailer is called trailer angle and defined as a difference of heading angles Θtr = Θ0 − Θ1 . y δ d0 v
Θ0
d1 Θ1 y1
x1
x
Fig. 1. Representation of the Towing Vehicle (cf. Ghilardelli et al. (2014)). Table 1. Parameters of the Model x1 x2 Θ0 Θ1 v δ d0 d1
x-coordinate of the middle point of approximated axle of the trailer y-coordinate of the middle point of approximated axle of the trailer heading angle of the truck heading angle of the trailer velocity of the truck in its rear axle position steering angle of front wheels wheelbase of the tractor distance between rear axle and coupling points of the trailer
η∈R+
≤ 5 pB − pA (7b) ≤ η1 ≤ η2 ≤ 5 pB − pA (7c) −π/2 ≤ δ(ui , η) ≤ π/2 (7d) −π/2 ≤ Θtr (ui , η) ≤ π/2 (7e) The cost function F (η) is optimized under some restrictions. The box constraints are based on practical experience. They have to be at least positive and different from zero to avoid uncomfortable curvature values int the planned path. On the other side, very big values of curve speeds lead to an inadequate path (e.g. very long path exceeding parking area). Therefore the bounds based on practical experience are defined in equation (7b) and equation (7c). The steering angle and trailer angle have to evaluated at discrete points ui = i/l for i = 0, . . . , l. In optimization problems, the constraints are mostly given by the problem and its physical circumstances. This is different for the cost function which is not that fixed. In the given problem the cost function can consist of terms for a minimal length, steering angle, steering angle change, trailer angle or trailer angle change. The path should guide the vehicle straight to the parking slot minimizing unnecessary turning (curvature, steering angle) and driving distance and providing a comfortable driving behavior by minimizing steering angle changes. The term for minimized driving distance is defined by the length integral s.t.
4. PROBLEM STATEMENT PATH OPTIMIZATION In this section the concrete optimization problem is derived and some resulting paths are presented. The path is denoted as x1 (u) p(u) = . (2) y1 (u) Here u is the curve parameter with u ∈ [0, 1]. The path should connect the current vehicle location with the destination parking slot considering its orientations. Hereby the path should be comfortably smooth and consider vehicle limitations such as maximal steering angle. Therefore the curvature κ of the planned path curve and its derivatives κ and κ are included in the problem statement. The following data must be interpolated with the resulting path pA , pB ∈ R2 (3a) ΘA , ΘB ∈ [0, 2π] (3b) (3c) κA , κB , κ A , κ B , κ A , κ B ∈ R. The index A corresponds to the starting point of the planned curve and the index B to its last point. The aim is to calculate a path which interpolates a starting and ending conditions. Ghilardelli et al. derived in Ghilardelli et al. (2014) in detail, that a path consisting of two polynomials of ninth order fulfills the given requirements. This means α(u) p(u) = (4) β(u) with 9 9 α(u) = α i ui β(u) = β i ui . (5) i=0
87
1 1
1
min2 sf (η) = min2
η∈R+
η∈R+
0
p(ξ) ˙ dξ.
(8)
The term for minimized trailer angle change is defined as: 1 2 min2 (ξ, η) dξ. (9) Θ˙ tr η∈R+
0
The formula for the trailer angle can be found in Ghilardelli et al. (2014) and its derivative follows:
i=0
87
2019 IFAC AAC 88 Orléans, France, June 23-27, 2019
Θ˙ tr (u, η) =
Alexej Janz et al. / IFAC PapersOnLine 52-5 (2019) 85–90
1/2 d1 α˙ 2 (u) + β˙ 2 (u)
problems with linear constraints. In general, one has to assume that F , g, and h are twice continuously differentiable. The Lagrangian
κ (u). (10) d21 κ2 (u) + 1 The derivative respectively the curve parameter u is denoted by (˙) and the derivative respectively the arch length s is denoted by ( ) . κ(u) is the curvature of the path at point u ∈ [0, 1] (cf. Ghilardelli et al. (2014)). Including the squared trailer angle change in equation (9) means to punish large values even more and additionally no signs need to be considered. An exemplary choice for the cost function is: 1 2 F (η) = λ1 sf (η) + λ2 (ξ, η) dξ (11) Θ˙ tr
L(z, λ, µ) := F (z) + λT g(z) + µT h(z)
(12)
combines objective and constraint functions taking multipliers λ and µ into account. Within the iterative process of the SQP approach the resulting subproblem in iteration k with H [k] := ∇2zz L(z [k] , λ[k] , µ[k] ) is 1 min ∇z F (z [k] )T d + dT H [k] d d 2 , ∀i = 1, ..., li , s.t. gi (z [k] ) + ∇z gi (z [k] )T d ≤ 0 hj (z [k] ) + ∇z hj (z [k] )T d = 0
0
Here λ = (λ1 , λ2 ) ∈ R is a weight on the two terms of the objective function. A good choice is e. g. λ = (0.4, 0.6). It is usual to choose weight factors which sum up to one to achieve a good numerical behavior. In a lot of methods with multi-objective functions the weight’s sum is one, e. g. in the Weighted Sum Method (Marler and Arora (2010)). In this specific case the choice combines the two aims of a minimal final length of the path and a minimal trailer angle change, where a comfortable path is considered as more important than a short path. 2
, ∀j = 1, ..., le .
is called quadratic subproblem of NLP. Often, the Hessian of the Lagrangian H [k] is replaced by an update of BFGS (named after Broyden, Fletcher, Goldfarb, and Shanno) type, which has the additional benefit that only strictly convex quadratic programs have to be solved. This strategy works well for small- to medium-sized problems, but it turns out to be infeasible for large-scale problems, since the update formula generally yield dense matrices, which in turn lead to dense Hessian approximations. To solve largescale problems, one is thus forced to fall back to the exact Hessian of the Lagrangian or to perform non-intersecting, intersecting or even multiply-intersecting block-BFGS updates which WORHP provides. The iterative scheme of the algorithm is as follows:
The objective function depends explicitly on η. In more detail sf as well as Θ˙ tr depend on polynomials α and β and their derivatives, which coefficients αi and βi (i = 0, . . . , 9) depend on η.
(1) choose (z [0] , λ[0] , µ[0] ) ∈ Rn × Rli × Rle and set k := 0. (2) if (z [k] , λ[k] , µ[k] ) fulfills some stopping criterion → STOP. (3) calculate dk ∈ Rn of the quadratic subproblem with λ[k+1] and µ[k+1] . (4) set z [k+1] := z [k] + d[k] and k := k + 1 go to (2).
Numerical results of the explained optimization problem are discussed in section 6. Prior to that, the used optimization software is introduced. 5. NONLINEAR OPTIMIZATION WITH WORHP Nonlinear optimization is a key feature for many applications in industry and science. The general question in this context is how free variables of a model must be chosen to minimize a defined objective function while maintaining certain constraints. The nonlinear optimization problem is defined as follows: let z ∈ Rn be the optimization vector. Furthermore, let F : Rn → R denote the objective function and g : Rn → Rli , h : Rn → Rle denote general nonlinear constraint functions. Then F (z) min
The main benefit of non-linear optimization with SQP method is its local convergence property. Due to the usage of second-order derivatives, one can prove that the algorithm exhibits locally quadratic convergence. WORHP provides efficient routines for computing sparse derivatives by applying graph-coloring methods to finite differences, structure-preserving sparse BFGS update techniques for Hessian approximations and sparse linear algebra to efficiently solve the underlying linear equation systems. Furthermore, it is based on reverse communication, which offers an unprecedented level of interaction between user and NLP solver. Therefore WORHP is able to solve real-world problems (e.g. HAD applications) close to real-time requirements.
z
, i = 1, ..., li (NLP) gi (z) ≤ 0 hj (z) = 0 , j = 1, ..., le is called a nonlinear program (NLP). In general, there are several different algorithms to solve such problems. All of them are some kind of specialization of Newton’s method. The solver WORHP (”We Optimize Really Huge Problems”) was especially developed for large-scale, sparse non-linear optimization problems and is the preferred NLP solver of the European space agency (ESA). WORHP uses either a sparse sequential quadratic programming method (SQP) with an interior point method for the quadratic subproblem or an interior point method on the nonlinear level. The software design was focused on high robustness and application-driven design, see B¨ uskens and Wassel (2012). s.t.
6. APPLICATION EXAMPLE In this section, results of the mentioned method from section 4 with the model from section 3 are discussed. For the optimizing process the software library WORHP is used, introduced in section 5. In the following an exemplary curve of the shape of a C is analyzed because ”rectangular” parking is a usual situation in logistic centers. At first numerical results are discussed, followed by experimental results. For the numerical part the path of the following situation is optimized:
The SQP approach is an iterative method that approximates the optimization problem locally by quadratic sub88
2019 IFAC AAC Orléans, France, June 23-27, 2019
Alexej Janz et al. / IFAC PapersOnLine 52-5 (2019) 85–90
89
0.2 0
Y
δ(u)
40
−0.2
Final Result Initial Guess
−0.4
20
0
0.2
0.4
0.6
0.8
1
u 0
Fig. 3. Resulting Steering Angle. −20
0
20
Optimizing only the total length for the same scenario results in a shorter path with intensive steering in both ends of the planned path. Then again optimizing only the trailer angle change results in a wider curve with a higher length.
X
Fig. 2. Resulting Path. pA = (0, 0)T ΘA = 0 Θtr,A = 0 vA = 0 δA = 0 δ˙A = 0
pB = (−30, 45)T ΘB = −π/2 Θtr,B = 0 vB = 0 δB = 0 δ˙B = 0.
(13a) (13b) (13c) (13d) (13e)
In the explained example 27 NLP-Iterations are needed to converge in the optimization algorithm. The cost function is evaluated 927 times. This means 927 numerical integrals are computed during the algorithm. This is related to the computational time of approximately six seconds which is heavily dependent on the used system. This amount of time suggests that the discussed method is doable in vehicles in general, even if the used code is not finally optimized concerning time efficiency and it gives only a sense or an order of magnitude for the time consumption.
(13f)
In Figure 2 the resulting trailer path is represented in red. The corresponding truck path is shown in blue. The initial guess for tractor path represented by the black dashed line. The path is designed to bring the truck and trailer from the initial position (grey) to the destination location (green). For this particular case the initial guess has been defined as ηinit = (30, 45). Experiments have shown that the distance between starting and destination points and intersection point of direction vectors in the start and destination can serve as a good initial guess. For cases, where this intersection point does not exist or badly located (e.g. too far away) half a distance between starting and destination points can be taken for both initial components. Here the starting point is 30m away of the intersection point of direction vectors and for the destination point, this distance is 45m.
Another important aspect is the dependence of the initial guess. In general in optimization processes the initial guesses are relevant. A motivation for a universal initial guess is stated above. This approach turns out to be functional in several scenarios. An analysis of the problem with random initial values leads to the result that the problem is well posed enough such that the same result is found for different initial values. Only physically relevant values are taken, in this case η ∈ [1, 90]2 . The result always differs because of numerical inaccuracies, but for the required accuracy of the solution, it is not relevant. Summed up
sf
The values of the terms of the objective function (11) for different optimization parameters are: F1 = 62.18 (14a) F1,init = 59.21 F2,init = 10.39 F2 = 3.47 (14b) F = 65.65. (14c) Finit = 69.60 Here F1 belongs to the length of the path, F2 to the summed trailer angle changes and F to the combined objective value. The initial values are displayed on the lefthand side and the final values on the right-hand side. It turns out that the path gets longer, but the trailer angle change is clearly less than at the initial guess. Figure 2 points out, that the resulting path is a feasible path that can be performed by the vehicle. The corresponding development of the steering angle of the truck with respect to the curve parameter is presented in Figure 3. It can be seen that the value of the summed steering angle along the path is clearly smaller than at its initial guess. Minimizing the trailer angle change leads to similar results as minimizing the steering angle or steering angle change. Therefore only one of these terms is mentioned in the objective function.
55
55
54.9
54.9
54.8
54.8
54.7
54.7
54.6
54.6
54.5
54.5
sf
−40
54.4
54.4
54.3
54.3
54.2
54.2
54.1
54.1 54
54 0
500 Experiment
1000
Variation Interval [5, 90]
Fig. 4. Statistical Result Exemplary for the Objective Term of the Total Length sf . 1000 Experiments with a Random Variation of η ∈ [5, 90]2 are Executed. 89
2019 IFAC AAC Orléans, France, June 23-27, 2019 90
Alexej Janz et al. / IFAC PapersOnLine 52-5 (2019) 85–90
Fig. 5. Total Length sf in Dependence of η1 , η2 .
Fig. 6. Integral of Trailer Angle Change in Dep. of η1 , η2 .
good initial guesses lead to a shorter computational time but do not change the resulting paths.
Therefore the test vehicle, as well as the kinematic model of the vehicle, were introduced firstly. Thereafter the problem statement of the path optimization was discussed in detail. The main challenges were formulating the problem in a suitable form to create comfortable driving behavior and using a suitable formulation for the numerical solver. The problem results in an NLP, which was efficiently solved by using the optimization toolkit WORHP. Finally, the proposed solution strategy was validated by using a real-world measurement data of a valet parking application with trailer. Based on this, it was shown that WORHP provides a suitable solution in order to solve the problem in real time and the proposed approach works in a suitable way. Further work will focus on a detailed validation of the proposed vehicle guidance concept in a 24/7 pilot operation. This will serve as a final stage of the eJIT project.
For validating the developed implementation the dependence on the initial guesses has to be analyzed. It appears that a random variation of η1,init and η2,init inside the interval [5, 90] leads to almost the same result, but not exactly the same values, which can be seen in Figure 4. Some outliers can be seen but the mainly derived interval for the objective value is accurate enough. The optimization with the above motivated initial guess leads to an objective value of 54.27 which is inside this interval. Another interesting aspect for understanding the optimization process is to deal with the objective function without optimizing variables. Since two variables are considered in the given problem a representation of the total length or the trailer angle change can be done as in Figure 5 and Figure 6. It can be seen that both of the cost terms look friendly for an optimization process. Wherever the initial value for the shortest length lies, the result should be very small for η1 and η2 . Similar to the trailer angle change the low region in the middle of the plot should be reached through optimization. For the combined objective function these arguments have to be combined as well.
REFERENCES Bry, A. and Roy, N. (2011). Rapidly-exploring random belief trees for motion planning under uncertainty. In Robotics and Automation (ICRA), 2011 IEEE International Conference on, 723–730. IEEE. B¨ uskens, C. and Wassel, D. (2012). The esa nlp solver worhp. In Modeling and Optimization in Space Engineering, 85–110. Springer. Ge, S.S. and Cui, Y.J. (2000). New potential functions for mobile robot path planning. IEEE Transactions on robotics and automation, 16(5), 615–620. Ghilardelli, F., Lini, G., and Piazzi, A. (2014). Path generation using η 4 -splines for a truck and trailer vehicle. IEEE Transactions on Automation Science and Engineering, 11(1), 187–203. Kant, K. and Zucker, S.W. (1986). Toward efficient trajectory planning: The path-velocity decomposition. The International Journal of Robotics Research, 5(3), 72–89. Krogh, B. and Thorpe, C. (1986). Integrated path planning and dynamic steering control for autonomous vehicles. In Robotics and Automation. Proceedings. 1986 IEEE International Conference on, volume 3, 1664– 1669. IEEE. Marler, R.T. and Arora, J.S. (2010). The weighted sum method for multi-objective optimization: New insights. Structural and multidisciplinary optimization, 41(6), 853–862. RKW Sachsen Rationalisierungs- und Innovationszentrum e. V. im World Trade Center Dresden (June 22, 2018). ejit. just electric. http://e-jit.de/.
In addition to the previous results, the implementation of the explained method is included into an existing code of an automated parking module. The mentioned approach can be applied to personal cars as well as to heavy-duty trucks towing a single trailer. In this example the objective values are: F1 = 44.66 (15a) F1,init = 42.37 F2,init = 21.27 F2 = 8.19 (15b) F = 22.78. (15c) Finit = 29.71 Here, 21 NLP-iterations are needed. As before the objective value is less than at the initial guess, but the length gets slightly greater while the trailer angle change gets essential smaller. For other shapes, this is not always the case, e. g. for an S shaped path. These results lead to the conclusion that the analyzed method is appropriate to be included in the implementation of a truck and trailer vehicle. Of course, more scenarios and critical cases must be treated in the future. 7. CONCLUSION A model based path optimization strategy for valet parking with trucks (incl. trailer) was presented in the paper. 90