StrojnlSkl vestnik - Journal ofMeuhanlcul Englneerlng65(2019)3, 148-160 © 2019 Jaursel of MectanlcaiEnglnesring. A||rightereserved. D0l:10.5545/!w-^me^2018.5802 Original Scientific Paper Re celvvd Vor review: 201&10-18 Received revlnedVorm: 2018-01-28 Acc epteVfvr rrubMi^;neen:^018-C^^-27 Motion Planning for Highly Automated Road Vehicles with a Hybrid Approach Using Nonlinear Optimization and Artificial Neural Networks Ferenc Hegedüs1 - Tamás Bécsi2 * - Szilárd Aradi2 - Péter Gáspár3 1 Robert Bosch, Hungary 2Budapest University of Technology and Economics, Department of Control for Transportation and Vehicle Systems, Hungary 3Hungarian Academy of Sciences, Computer and Automation Research Institute, Hungary Dv4r /h4 las/ d4cad4, maiy diff4r4i/ algorithms W4r4 d4V4loe4d for /h4 mo/ioi elaiiiig of road V4hicl4s du4 /o /h4 iicr4asiig ii/4r4s/ ii /h4 au/oma/ioi of road /raiseor/a/ioi. To b4 abl4 to 4isur4 dyiamical teasibili/y of /h4 elaii4d /raj4c/ortes, loiholoiomic dyiamics of wh44l4d V4hicl4s mus/b4 coisid4r4d. Noilii4ar oe/imiza/ioi bas4d /raj4c/ory elaii4rs ar4 erov4i /o sa/isfy /his i44d, how4V4r /his haee4is a/ /h4 4xe4is4 of iicœasiig comeu/a/ioial 4ffor/, which j4oeardiz4s /h4 r4al-im4 aeelicabili/y of th4s4 m4/hods. This eae4r er4s4i/s ai algori/hm which off4rs a solu/ioi /o /his erobl4ma/ic wi/h a hybrid aeeroach usiig artificial i4ural i4/works (ANNs). Firs/, a ioilii4ar oe/imiza/ioi bas4d /raj4c/ory elaii4r is er4sei/4d which 4isur4s /h4 dyiamical teasibili/y wi/h /h4 mod4l-bas4d er4dic/ioi of /h4 V4hicl4's mo/ioi. N4xt, ai artificial i4ural i4/work is /raii4d /o r4eroduc4 /h4 b4havior of /h4 oe/imiza/ioi bas4d elaiiiig algorithm wi/h /h4 m4/hod of sue4rvis4d tearing. Th4 g4i4ra/ioi of tiaiiiig da/a haee4is off-line, which 4limiia/4s /h4 coiœris abou/ /h4 comeu/a/ioial r4quir4m4i/s of /h4 oetimiza/ioi-bas4d m4/hod. Th4 /raii4d i4ural i4/work /h4i œelaœs /h4 origiial mo/ioi elaii4r ii oi-lii4 elaiiiig /asks which sigiificai/ly œduœs comeu/a/ioial 4ffor/ aid thus rui-/im4. Fur/h4rmor4, /h4 ou/eu/ of /h4 i4/work is sue4rvis4d by /h4 mod4l bas4d mo/ioi er4dic/ioi lay4r of /h4 origiial oe/imiza/ioi-bas4d algorithm aid cai /hus always b4 /rust4d. Fiially, /h4 e4rformaic4 of /h4 hybrid m4/hod is b4ichmark4d wi/h comeu/4r simulatiois ii terms of dyiamical teasibili/y aid rui-/im4 aid /h4 r4sul/s ar4 iv4s/iga/4d. Examiia/iois show /ha/ /h4 comeu/a/ioi /im4 cai b4 sigiificai/ly œduœd white maii/aiiiig /h4 teasibili/y of r4sul/iig V4hicl4 mo/iois. Keywords: automated driving, motion planning, trajectory planning, vehicle control, nonlinear optimization, artificial neural networks Highlights • A mod4l-bas4d, mul/i-obj4c/iv4, dyiamically teasibte /raj4c/ory elaii4r is i/roduc4d, which solv4s /h4 mo/ioi elaiiiigerobtem /hrough oilii4 ioilii4ar oe/imiza/ioi. • Although /h4 m4/hod has high e4rformaic4, i/also r4quir4s major comeu/a/ioial 4ffort, which is io/acœe/abte for r4al-/im4 aeelica/ioi. • Ai ANN bas4d aeeroach is eroeos4d /o erovid4 clos4-/o-oe/imal iii/ial valu4 for /h4 oe/imiza/ioi eroœss. • A iov4ll ANN bas4d m4/hod is eroeos4d /o œelaœ /h4 oilii4 oe/imiza/ioi eroœss, white also sue4rvisiig /h4 ou/eu/ of /h4 ANN. • Traiiiig b4ichmarks, b4havior comearisoi aid simula/ioi erov4 /h4 e4rformaic4 of /h4 d4V4loe4d solu/iois. 1 INTRODUCTION Highly automated and autonomous driving is expected to contribute to the quality of road transportation in multiple ways. The most important impact can be the improvement of road safety. Owing to the spread of passive and active electronic safety systems, the number of fatal road accidents has been reduced by 48 % between 2001 and 2015 in the European Union [1]. Higher degree of automation could further increase road safety as human factors are still responsible for the majority the remaining incidents. Energy efficiency and environmental friendliness is also an increasing social requirement. Researches show that automated vehicles could save up to 10 % to 30 % fuel by utilizing optimized route finding and driving strategies [2] and [3]. Road traffic parameters, such as average travel time and traffic flow capacity are also predicted to improve significantly [4]. The numerous expectations make autonomous driving the most important research field for both industrial and academic institutions concerning road transportation. Although there are many technical challenges to solve, one of the most important aspects is navigating the automated vehicle in the dynamic traffic environment. The decision making process generally has a hierarchical structure with 3 major levels. The highest level in the hierarchy is called route planning and is responsible for finding an appropriate route through the available road infrastructure from the current position to the required end destination. The middle level - behavior planning - navigates the selected route and interacts with the other traffic participants according to road rules. This paper addresses the lowest layer called motion planning. The 148 *Corr. Author's Address: Robert Bosch, 2 Stoczek street, 1111 Budapest, Hungary, hegedus.ferenc@mall.bme.hu Strojniski vestnik - Journal of Mechanical Engineering 65(2019)3, 148-160 motion planning layer is fed by behavior planning with some maneuver primitives (e.g. lane-change or right-turn) and generates the trajectory of the vehicle. The trajectory contains not only the desired path that the vehicle should travel along but it also includes all temporal quantities which describe the vehicle's motion e.g. the associated velocity and acceleration. The collection of these quantities is called the state or the configuration of the vehicle [5]. The trajectory of the vehicle must be safe, dynamically feasible, comfortable and customizable according to the preferences of individual passengers. A further requirement against the planning algorithm is that it must be computable fast enough to enable real-time application on state of the art hardware. Over the last decades, many different approaches have been developed to solve the motion planning problem. However, to the best of author's knowledge, no method was provided with the capability of satisfying all above requirements simultaneously. Classical motion planning approaches can be split into four major categories. Namely, there are geometric based methods, graph search based algorithms, incremental search techniques and variational methods. The first three kind of algorithms are rather suitable for path planning only, which path is then later transformed to a trajectory by assigning time information in a further step. Furthermore, the possibility to consider the vehicle's nonholonomic motion is strongly limited. Geometric methods are composing the vehicle's path from geometric curves like circular arcs, clothoids [6], polynomial splines, or polynomial function of time or arc length [7]. The parameters of the curves are usually calculated based on geometric boundary conditions (e.g. initial and final positions of the vehicle) considering the limited steering angle and side acceleration of the vehicle along the curve [8]. The advantage of these algorithms is that they are computationally cheap. Because of this, a common approach to generate a suboptimal collision-free motion is to evaluate multiple candidates into different terminal configurations and choose the best amongst safe ones for execution according to some cost function [9]. Graph search methods have been used for path finding problems since Dijkstra's algorithm have been published [10]. These algorithms are using a discretization of the vehicle's spatiotemporal environment and building a graph from the feasible and unoccupied points or motion primitives. The dimension of the state space used [11] as well as resolution of the discretization can be set adaptively to balance between computational effort and path quality [12]. With the Hybrid A* algorithm it is also possible to associate a continuous state with each discrete cell resulting a much smoother path [13]. Instead of a fixed discretization of space time, incremental search techniques such as rapidly-exploring random tree (RRT) and its extensions are building the graph by random sampling [14]. The graph begins at the initial configuration of the vehicle and is incremented by randomly sampled new configurations from its unoccupied environment. Some appropriate distance metrics (e.g. Dubins path [15]) is used to determine the vertex of the graph that the new point can be connected to. RRT can also be extended by predicting the response of a closed loop controller-vehicle model to the randomly sampled reference and using the resulting response to build the graph [16]. Variational methods are formulating the trajectory planning problem as a nonlinear constrained optimization, and often draw ideas from other optimal control techniques such as model predictive control (MPC). The aim is usually to find some appropriate input functions that drive the model of the vehicle into a prescribed end state while minimizing a cost function that represents the quality of the trajectory [17]. The problem can be discretized by using a parametrization of the input functions. These methods enable the usage of more complex dynamic models or even real measurements for the prediction of the vehicle's motion an can therefore ensure a dynamical feasibility in a much wider range of driving conditions [18]. Obstacle avoidance can also be integrated in the optimization problem formulated in form of additional constraints [19]. Beside the classical methods, an emerging interest is shown throughout the field of robotics towards artificial neural networks well known for their outstanding performance in learning, adaptation, generalization and optimization. Additionally, as the required computational time of application of a trained neural network is significantly lower than the run-time required for traditional optimizing algorithms, these networks have high potential for real-time applications. These capabilities make application of such networks worth considering in the assessment of possible solutions for complex problems featuring recognition, learning and decision-making as trajectory planning. Applications for path planning and motion control of multi-joint manipulators [20] or servo motor systems [21] are target to current researches. Motion Planning for Highly Automated Road Vehicles with a Hybrid Approach Using Nonlinear Optimization and Artificial Neural Networks 149 Strojstsklvestnik- Journal of MechanicalEngineering 65(2019)3,118-i6Q Known researches discussed the problem of collision-free trajectory planning using nonholonomic vehicle model. Some of them were to conduct obstacle avoidance in dynamic environment [22] [23]. Several works included mapless navigation with free- path detection using on-board sensors such as ultrasonic sensors [24] or laser scanner [25]. Reinforcement learning methods were also introduced both utilizing off-line [26] and on-line training [27]. Different artificial neural network aided approaches were presented using nonholonomic vehicle models in the field of control design for automated parking [28], and vehicle motion prediction, where the network is trained to replicate the dynamics of a specific vehicle [29]. However, application of artificial neural networks in safety relevant systems is only possible with post filtering by traditional algorithm. The paper is organized as follows. Section 1 introduces in details the nonlinear optimization based motion planning algorithm which is the basis of our examinations. In Section 2 the proposed hybrid approach is described which utilizes artificial neural network combined with classical methods to ensure feasible output. The performance of the algorithm is then benchmarked in Section 3, and Section 4 contains conclusion summary and offers future research directions. 1 eOTION PLANNING WITH eODEL-BASED OPTieiZATION 1.1 The Motion Planning Problem When planning a trajectory, the objective is to find the inputs (steering angle, driving or braking torque) that drive the vehicle from the current initial state to a desired end state with respect to its nonholonomic dynamics, meanwhile satisfying the requirements described in Section 0, namely; safety, dynamical feasibility, comfort, and the possibility of customization. Safety means that the trajectory must not lead to collision with fellow traffic participants. To reach dynamical feasibility, the vehicle must be able to drive along the planned trajectory with respect to its nonholonomic dynamics. Passenger comfort is strongly subjective, but researches show that it is in correlation with the magnitude of acceleration and jerk along the trajectory [30]. The passengers should also be able to influence motion characteristics within the limits of dynamical feasibility, e.g. to prefer minimal travel time over comfort and vice versa. Mathematically, the motion planning can be formulated as a constrained nonlinear optimization problem as follows: min J(X(p,t)) = VWiZi(X(p,t)), (1) p i subject to X(p, t)= f (X(p, t), u(p, t)), (2) C(X (p, t )) = 0. (3) Firstly, Eq. (1) is the objective function where arbitrary trajectory qualifiers zi(X(p,t)) can be specified depending on the state of the vehicle X( p, t) which are describing the goodness of the generated motion, and the influence of these qualifiers is weighted by factors wi. The weighting also allows some customization according to individual preferences of passengers. The applied objective function is detailed in Subsection 1.2. Secondly, Eq. (2) represents the dynamics of the vehicle introduced in Subsection 1.3. Finally, Eq. (3) represents that the vehicle must reach the required end state formulated as constraint equation C(X(p, t)), which topic is detailed in Subsection 1.4. The optimized variable p is a vector of parameters which are determining the input of the vehicle u(p,t). The input parametrization is described in Subsection 1.5. 1.2 Objective Function Main contributors to passenger discomfort are acceleration and jerk, because these quantities influence the acting force. In [31] it was shown that the application of the following cost function can effectively be used for optimization based trajectory planning to maintain passenger comfort and minimize travel time at the same time: j(x(p,t)) =wttf+wj ff[yv(t)fdt+ f Jo (4) wa [/ (t )]dt, Jo where yV and yV are the lateral jerk and acceleration of the vehicle, tf is the travel time along the trajectory, and wj, wa, and wt are weighting constants. 1.3 Model of Dynamics From the perspective of the optimization problem, the model of the vehicle dynamics can be of any kind. On one hand, to ensure a high level of dynamical feasibility, the model should precisely describe the motion of the vehicle in every governable driving scenarios. On the other hand, calculation of the vehicle's motion comes with a considerable amount of computation effort, which should be obviously kept 150 Hegedus, F. - Beesi, T.-Aradi, S.-Gaspar,P. Strojniski vestnik - Journal of Mechanical Engineering 65(2019)3, 148-160 as low as possible. The vehicle model applied in this paper is an enhanced version of the nonlinear single track model that is used in [32]. The planar multi-body model (Fig. 1) consists of a chassis with mass m and moment of inertia 9 about its vertical axis (zV), and virtual front and rear wheels with moment of inertia 9f, 9r about their own rotation axes (yf, yVr) representing the complete front and rear axes of the vehicle. The chassis can move longitudinally x and laterally y, and rotate y about its vertical axis. The wheels can rotate pf, pr about their axes as well. The center of gravity of the vehicle is considered to be constant. The radii of the wheels are noted with rf and rf, the center of gravity height of the vehicle is marked with h, and the horizontal distance between the center of gravity and the front and rear wheel centers are denoted with lf and lf respectively. The inputs of the model are the steering angle of the front wheel 8 and the total applied driving Md and braking Mb torques. In the following Subsections 1.3.1 and 1.3.2 superscripts are used to distinguish dynamic quantities in the ground's (no superscript) vehicle's (V) and in the wheel's (W) coordinate system. The coordinate transformations between the different coordinate systems are not detailed due to limited space. Furthermore, all time derivatives are noted with dot ( ). Fig. 1. Nonlinear single track vehicle model 1.3.1 Dynamics of the Wheels A dynamic wheel slip model is introduced based on [33] to increase the precision of the simulation in case of near-standstill (drive-off, full braking) situations, and to enable the usage of explicit ordinary differential equation (ODE) solvers and larger step sizes. The dynamic equations for the front wheel are the following: Pf = (Mfd - rfFjx - Mfb - Mf,rr) , sf ,x = (rf pf - xj - \xj \sf ,*) , s f,y = f-( - yj -\xj\sf ,y) . (5) (6) (7) Eq. (5) represents the motion of the wheel. In case of driving, the total driving torque is distributed by an arbitrary factor between the front Md,f and rear Md,r wheels. In case of braking, ideal break torque distribution (in sense that the longitudinal wheel slip values are equal for the two wheels) is applied to compute the wheel braking torques Mb,f and Mb,r. Eqs. (6) and (7) are used to calculate the dynamic longitudinal and lateral wheel slips. The slip dependent longitudinal relaxation length is: lf,x = max I If ,x,0 1 - C f F 3D f,x \Sf,x\^ , lf ,x,mi^J , (8) with Cf,F = Bf,xCf,xDf, where lf,x,o and lf,x,min are the longitudinal relaxation lengths at standstill and at wheel spin or lock respectively. Eq. (8) is also valid for the lateral direction lf ,y as well, with the substitution of subscripts x and y. The tire forces are calculated by the Magic Formula: Fjx =Df sin{Cf,x arctan(Bf,xSf,x- E [Bf xxSf ,x - arctan (Bf ¿Sf ,x)])}, (9) with Df = nfF^z, where ¡J.f is the static coefficient of friction between tire and road surfaces, and Bf,x, Cf,x, Ef,x are parameters. Eq. (9) is valid for the lateral direction FfW,y as well, with the substitution of subscripts x with y. To enhance the behavior of the model at very low speeds e.g. when starting from or braking to standstill, a slip damping factor is applied like following: 2kf,x,0 (l + cos (n, if xf < vtoW (10) 0, if xf > Vlow where kf,x,0 is the damping value zero velocity and vlow is the velocity at which damping is switched off. The x Motion Planning for Highly Automated Road Vehicles with a Hybrid Approach Using Nonlinear Optimization and Artificial Neural Networks 151 Strojniski vestnik- Journannf MecCanncal Engineering 65(2019)3,148-160 damping is only applied in the longitudinal direction, the damped slip values are: k sfx = sfx + f (rfPf -xW ) (11) Cf,F s f ,y = sf y. (12) The rolling resistance torque is calculated as follows: Mf ,rr =FfWzrfsign(rf pf )• [Arr + Brr\rf p f \ + Crr(rf p f )2], (13) where Arr, Brr, and Crr are rolling resistance coefficients. Eqs. (5) to (13) are all valid for the rear wheel with changing the subscripts from f to r. 1.3.2 Dynamics of the Chassis The dynamics of the chassis is now expressed in the inertial coordinate system of the ground. The equations of motion are: 1 x =— (Ff x + Frx + Fdx), m y =—(Ff y + Fry + Fd,y ), m V = q (lfFVy - lrFVy). (14) (15) (16) Aerodynamic drag force is applied to the vehicle according to the following: FVx = 2 cnAf PaxvVXv + yV, (17) FVy = 2 cnAf PAyVVxV + yV, (18) where cD is the drag coefficient and Af is the frontal area of the vehicle, and pA is the mass density of air. 1.3.3 Closed Loop Control Since the developed motion is described by the temporal course of the vehicle's state along the trajectory, a mapping is needed between wheel level inputs (steering angle 8, driving Md and braking Mb torque) to some of the state variables to facilitate the usage of input functions meaningful from the perspective of vehicle motion. This mapping is reached by applying closed loop trajectory tracking control. From the view of the optimization problem, arbitrary methods can be used to implement the control. In this paper, two independent linear quadratic regulator (LQR) controls are used. The longitudinal controller is responsible for the tracking of longitudinal velocity reference vref as well as the lateral controller follows a yaw-rate mref reference. The selection of controllers is of course in close relationship with the input functions applied in the optimization problem, described in Subsection 1.5. 1.4 Constraints The required end configuration of the vehicle must be formulated in form of constraints for the optimization problem described in Eq. (3). In this work this is happening with application of nonlinear equality constraints. The minimum feasible set of specified end state variables are position xf, yf and orientation Vf, however it is also reasonable to define the yaw rate Vf. Accordingly, the applied constraint equation is: C(X)= Xf - X(tf )= - A = 0. (19) 1.5 Input Functions In [17] it is shown that longitudinal velocity and yaw rate can efficiently (relatively small number of parameters enables feasible customization) be parametrized to describe the vehicle's motion. In current work, longitudinal velocity is always chosen constant (vref = const.) along the trajectory to reduce the number of free parameters. This choice is not unreasonable because the travel time typically remains in range of a few seconds. As the yaw-rate input signal, a polynomial spline profile shown in Fig. 2 is used. The spline is parametrized by knot points that are placed equidistantly, and by the time span. In current work, xf x(tf ) yf y(tf) Vf V (tf ) w W (tf ) a> -m cS u I ¥s ¡tf ! tf \ls tf Time Fig. 2. Yaw-rate reference signal paramefrization 152 Hegediis, F.- Bécsî, T.-Aoadî, S. -Gâspiir.R Strojniski vestnik - Journal of Mechanical Engineering 65(2019)3, 148-160 a cubic spline is used which means 4 knot point parameters and the travel time parameter additionally. The first knot point is however chosen to be exactly the initial value of yaw-rate of the vehicle fy to avoid jumps in control inputs. Accordingly, the reduced parameter vector is: p© = [© ©2 ©3 tf ]T . (20) 1.6 Solution of the Planning Problem Fig. 3 shows the architecture of how the solution p of the optimization problem represented by Eqs. (1) to (3) is found. The inputs of the optimization process are the current initial state Xi, and the required terminal state Xf of the vehicle, and an initial guess on the input function parameters p0. The initial guess is chosen to the parameters of straight drive, namely: P0 [0 0 0 Xf/Vref] . (21) The iteration step begins with the evaluation of the input function (the polynomial spline yaw rate reference) based on the parameter values. The response of the controller-vehicle system is then calculated by the following equation: X(p, t) = Xj + f f(X(p, t), u(p, T)dt, (22) 0 which can be solved numerically with the 4th Runge-Kutta method for instance. Knowing the trajectory, the objective function (Eq. (4)) and the constraint function (Eq. (19)) are evaluated, and the optimization solver checks if a feasible solution is found. If so, the motion planning is solved. Otherwise, the optimization solver calculates a correction of the parameters based on the objective and constraint values and starts a new iteration step with these corrected values. x, u(p) input generation motion predictor vehicle dynamics closed-loop controllers optimization planner u(p) parameter correction constraint calculation objective evaluation _x(p) c(x) decision logic Fig. 3. Optimization planner architecture As an optimization solver, interior point methods, trust region methods or sequential quadratic programming methods can be used. 2 HYBRID APPROACH USING ARTIFICIAL NEURAL NETWORKS 2.1 Basic Concept The nonlinear optimization based trajectory planning method presented in Section 1 introduces a sophisticated vehicle dynamics-focused approach. This comes at the price of significant computational requirements, which makes the current implementation unable to fulfill real-time applicability. Artificial neural networks have been used for function approximation since a long time. During planning, the optimization planner performs a mapping from the initial and terminal configurations of the vehicle to the appropriate input function parameters that will drive the vehicle accordingly. With the training of an artificial neural network via supervised learning to approximate this mapping, the optimization motion planning algorithm could be substituted. However, the trained neural network will always behave as a black box system. As such, it cannot be applied as a standalone algorithm, because there is no guarantee that it will produce a feasible output in all scenarios. In this paper, two possible applications of an artificial neural network are described, which are operating the network together with the classical model based methods to provide feasible motions with faster computation times. 2.2 Initial Value Generator The computation time of the optimization planner can be greatly decreased, if the initial guess of the optimized variable p0 is already close to the optimal solution. The initial value chosen in Eq. (21) is however obviously not near the optimum. The first idea is to use the neural network to provide a better initial guess instead. As the neural network is trained to provide the optimal solution, its output is expected to be at least close to the optimum considering also the estimation error. The output of the network flows through the whole original optimization loop (Fig. 3, 3rd input), which means that the process will correct a potential infeasible result. In the following, this approach will be referred to as initialized optimization planner. 2.3 Hybrid Neural Network Planner Another possibility is to completely substitute the on-line optimization with the neural network as shown in Fig. 4. The plausibility of the network's output is in this case checked with the motion predictor which is the core of the optimization algorithm. The end Motion Planning for Highly Automated Road Vehicles with a Hybrid Approach Using Nonlinear Optimization and Artificial Neural Networks 153 Strojniškivestnik- Journal of MechanicalEngineering 65(2019)3,148-i6Q state of the vehicle actually reached when driven by the input signals provided by the neural network estimator is compared with the required end state. In case the magnitude of error is not permissible, an emergency trajectory can be used. This solution will be referred to as hybrid nn planner. NN P input estimator generation Fig. 4. Hybrid NN planner architecture 2.4 Training of Networks As described in Subsections 2.2 and 2.3, our goal here is to create a network that can substitute the above mentioned trajectory planning algorithm, or at least provide a sufficient initial value guess for the optimizer. Numerous artificial neural networks with different parameter sets were trained to fulfill these tasks. The network's input consists of the vehicle's state variables selected as constraints for the optimization problem in Eq. (19) both at the initial and at the target configurations. Additionally, the initial and final state vectors are both containing the selected constant travel velocity vref described in Subsection 1.5. The network's output contains the parameters of the third-order yaw-rate reference spline defined in Eq. (20). A data set of 16000 lane changing and curved lane keeping trajectory samples for training was generated with the model-based optimization proposed in Section 1. From the start configuration of: Xi = [xi yi Wi Wi vi]J = [0 0 0 0 vref]T (23) the target state vectors are defined by the following equations and ranges: Xf 'xf ' 50 ...100 m yf -0.15xf ...0.15xf m Wf = -0.1 Wc ...1.2wc rad Vf v ,sln(W/) Vref x/ rad/s vf vref m/s _ (24) where yc = 2arctan( Xj) is the yaw angle of circular path, and vref is chosen to 20 m/s. The ranges were determined by vehicle simulations to cover the whole dynamically feasible region ahead of the vehicle, and the edges are selected to reach even extreme dynamical parameters (e.g. lateral acceleration up to near 1g). A portion of the generated sample trajectories is shown in Fig. 5. In Section 3, all the figures will show the results regarding to these trajectories, in the ascending order of maximal lateral acceleration. Fig. 5. Sample trajectories The core of the complex problem of computing dynamically feasible motions is irrelevant from the perspective of the training of an artificial neural network. The problem that the network is needed to provide a solution for is handled as a general function approximation. In order to explore the benefits of different networks to reach the best regression possible, several parameters defining the training or the network itself were used in every possible combinations. Although Levenberg-Marquandt algorithm is well-known for its superior performance in function approximation, in order to perform a more comprehensive study on the training process, Broyden-Fletcher-Goldfarb-Shanno (BFGS) and resilient back-propagation algorithms were also targets of examination. 154 Hegedus, F. - Bécsi, T. - Aradi, S. - Gâspâr, P. Strojniski vestnik - Journal of Mechanical Engineering 65(2019)3, 148-160 Regarding network architecture two variants defining the connection of the adjacent layers differently were tested: feed-forward and cascade structures. The input layer defined by the input vectors consists of 10 neurons, while the output layer using linear transfer function is of 4 neuron size, corresponding to the output vector defining the planned yaw-rate reference. Hidden layers use tangent sigmoid transfer function were examined in seven different structures: one layer sized [10] and [20], two layers sized [10; 8], [20; 8], [30; 10], three layers sized [10; 20; 8], [20; 10; 8]. The decomposition of the training data set for train, validation and test sets is also a factor worth consideration regarding the result of the training. Five different ratios were tested: [80:10:10], [70:15:15], [60:25:15], [60:15:25] and [50:25:25]. Every network with different parameter sets were trained five times with different initial values and were saved with the weights and biases culminating in the best test results, in order to exclude the effect of the potentially occurring error of finding local extremes instead of the global minimum of the error function during the training process. As it was expected, the training algorithm resulting in the fastest convergence was the Levenberg-Marquandt method, which resulted the networks to reach the performance goal in an order of magnitude faster than the networks trained with either BFGS or resilient back-propagation algorithm. Based on the test results, one hidden layer sized cascade networks and networks with multiple hidden layers provided the best performance on the test data set, while no major difference was occurring regarding the number of neurons. In respect to regression, the trained networks perform the function approximation task with mean squared error values around 10-2 on the test data set. 3 TESTING AND BENCHMARKING 3.1 Comparison of Run-time Fig. 6 shows the run-time of the motion planning algorithms normalized to the run-time of the optimization planner: kt (i) calc (i) tOPt calc (i) (25) where i is the index of the trajectory, tOpC(i) is the calculation time of the ith trajectory with the optimization planner, and t]calc(i) is the calculation time with planner j. The average planning time of the initialized optimization planner is decreased significantly by 51 %. The speed-up is even more essential in case of the hybrid nn planner, the average planning time is here decreased by 96.63 %. O optimization planner □ hybrid nn planner + initialized optimization planner 1.5 0.5 °o o o o o o ° o ° o°o n ~ o°rtO.OOCB °„ e. o ( o ° „ a> ° ° OO o OO O o 0 • - V - + ++ H+ * J- + ++■ -4+ ++ ■++■ ++ ++ " + 4* ++ ++ ++ J. + ++ + 4. -H- J. ++ 4. ++ + -H-+++ +4+ + + + -H- + + + + 10 20 30 40 50 60 70 trajectory index [1] Fig. 6. Normalized run-times As the run-time of the optimization planner is in the magnitude of 1 s, the results show, that even the current MATLAB based implementation of the hybrid nn planner may be suitable for real-time application from the perspective of planning time. 3.2 Comparison of Performance 3.2.1 Performance of Reference Parameter Estimation The deviation between the planned reference parameters normalized to the output parameters of the optimization planner shown in Fig. 7 is calculated by: kp(i) pj (i) - popt (i) popt(i) (26) where popt (i) is the parameter vector generated by the optimization planner in case of the ith trajectory, and pj (i) is the parameter vector in case of planner j. As expected, the deviation of the input function parameters is negligible in case of the initialized optimization planner. On the other hand, the hybrid nn planner produces a deviation of 4.35 % in average, that even reaches 14.76 % maximally. The values show how well neural network could be trained. It can be declared, that the goodness of the estimation is not in correspondence with the dynamical complexity of the trajectory, as it is not increasing or decreasing with growing lateral accelerations. Although the deviations are not very small, the main point is the influence of the estimation error to the motion of the vehicle. Motion Planning for Highly Automated Road Vehicles with a Hybrid Approach Using Nonlinear Optimization and Artificial Neural Networks 155 Strojnlsklvestnlk - Journal of Mechanical Engineering 65(2019)3,148-160 xlO" — 4 + + + initialized optimization planner + + + + + + + + + ++ 1 1 1 + +1 1+ + 11-H+ 1 1 l+l-H 4-1111 ++++ ++|+++ H/| ++ +++l l++ 0 0.15 0.1 0.05 20 40 60 index of trajectory [1] Q □ □ hybrid nn planner □□ D □ □ □ □ □ □ □ □ _ □ □ ° □ □ D □ □ □ "nn n □ □□ nQ □ □ □□ □ □ D □ | 20 40 60 index of trajectory [1] xlO" + initialized optimization planner ■+ . + + + + + + + + + + + + 1 + "H H-H—t—t=t)-)-l 111 hH+T+l ' 0 0.5 0.4 0.3 0.2 0.1 0 20 40 index of trajectory [1] 60 □ □ □ hybrid nn planner □ □ □ □ □ [ D ° □ □ n □ □ □ □ □ □ ■ □ D □ □ D □□ □ □ □□ □ □ d*oD tH,*® J □ J nD □ .nn 0 20 40 60 index of trajectory [1] Fig. 7. Normalized deviation of reference parameters Fig. 8. Final deviation of vehicle state 3.2.2 Deviations in Vehicle State The final value of state constraints, namely the euclidean norm of deviation between prescribed Xf and actually reached X (tf) end states of the vehicle is shown in Fig. 8. The deviation of the initialized optimization planner is again negligible, as the constraint tolerance of the nonlinear optimization solver is set to a sufficiently low value (10-4). In case of the hybrid nn planner, the norm of state constraint is noticeable. This is expected based on the results in Fig. 7. The main contributor to the total value is the deviation in the final vehicle position, so we can declare a maximal position error of approximately 40 cm after the whole planned motion («2 s), which is remarkable. However, the motions planned by the algorithm are actually only meant to be executed until the result of the next trajectory planning cycle is being calculated («50 ms), and new trajectories are expected to be planned as frequently as possible. The vehicle state deviation when considering only the executed part of the trajectory is calculated as follows: CT (X)= Xopt (Cic) - Xnn (Cic), (27) 156 where Xopt(■) and Xnn(-) are the state of the vehicle in case of the optimization planner and the hybrid nn planner respectively, and tcnanlc is the maximal calculation time with the hybrid nn planner. □ □ □ □ hybrid nn planner □ m □ □ □ □ □ □ 11 □ D □ D □ □ nn □ ^ □ D rP D □ □D ^ "to, rfW * □ D ^n rf. □ □ n 0 20 40 60 index of trajectory [1] Fig. 9. Final deviation of vehicle state considering only the driven part The results in Fig. 9 show that the state deviations are in a feasible range (with a maximal position deviation of approximately 1.3 mm) when considering only the driven part of the motion. Hegediis, F. - Bécsî, T.-Arach S. -Gâo/nâr, P Strojnlsklvestnlk- Journal of Mechanical Engineering65(2019)3,148-160 3.2.3 Influences on the Optimality Criteria As the proposed optimization planner provides an optimal motion with respect to the comfort of the passengers with the minimization of lateral acceleration yV, a corresponding comparison is also advisable and is shown in Fig. 10. The maximal value of lateral acceleration normalized to the maximal lateral acceleration developing in case of the optimization planner is calculated by the following equation: max(yV'j (i)) kay - max(yV,opt(i)) ' (28) where yV'opt(i) is the lateral acceleration of the ith trajectory in case of the optimization planner, and yV'j (i) is the lateral acceleration in case of planner j. 1.00001 "t 0.99999 0.99998 0.99997 + + -U 1 1 1 1 II III 111111 r^Lu 11.1111 + UUAJ UU* 1ULLI 1111 . - 1 -H T 1 rr 11 1 rr....... " ' + ^ + + + initialized optimization planner + + 1.1 1.05 0 20 40 60 index of trajectory [1] 0.95 □ hybrid nn planner □ □ Hi □ ] J □ □ fn" □ □ □ cP □ D # □ ^ □ □ □ % j □ □ □ □ ) 20 40 60 index of trajectory [1] Fig. 10. Normalized maximal lateral accelerations Once again, the performance of the initialized optimization planner is virtually the same, as expected. The hybrid nn planner produces a maximal increase of approximately 7.85 %, while the average stays around zero deviation. The value of maximal lateral acceleration is even smaller in several cases, which is possible by the violation of the prescribed end configuration. 3.3 Validation of Benchmark Results As the run-time of the hybrid nn planner enables real-time application, the benchmark results for this algorithm were validated with the industrial vehicle simulation software IPG CarMaker to prepare the application on a test car. The examination with a highly accurate, close-to-real environment was important, bacause the benchmark results in Section 3.2 were evaluated using the same single-track vehicle model that is employed in the optimization planner, which means that the deviations between the nominal system and a real vehicle due to unmodeled dynamics and changing environment conditions are not considered there. Fig. 11. Lane change maneuver in CarMaker Fig. 11 shows a lane change maneuver performed by the proposed planner in the CarMaker environment. The virtual test vehicle is chosen as a mid-size passenger car. The main parameters of the chassis (m, 9, lf, lr) and the wheels (rf, rr, ^f, ^r) of the vehicle motion estimator module of the planner were not adjusted exactly to the simulated vehicle to model the inevitable differences between the nominal and the actual systems (e.g. mass depending on the number of passengers). The CarMaker simulation results on Fig. 12 show that the deviations of the vehicle's final state from the planning target are higher than the ones on Fig. 8 but remain in case of almost every trajectory under a position error of 1 m. The deviation after the time frame necessary for the planning of the following trajectory is typically in a range of a few centimeters as shown in Fig. 13, which is not very small but is still in a feasible range considering that a suitable safety margin is used for collision avoidance. In terms of the highest lateral acceleration along the planned motion, Fig. 14 even shows an Motion Planning for Highly Automated Road VnNcleg wih a fybridApproach Using NoelinearOptimzation and Artificial Neural Networks 157 Strojnisklvestnik- Journal of MechanicalEngineering 65(2019)3,148-i6Q □ hybrid nn planner □ □ J 0.5 16 □ n5_dL_ -tP- Rib fa D # □□ * -n- fojfa ,, —eb-n~ □ □ 20 40 60 index of trajectory [1] Fig. 12. Final deviation of vehicle state in CarMaker 1.3 1.2 1.1 0.9 □ hybrid nn planner " f „ = □ □ IB D qfPuED □ DrA n □ □ □ □ Dn □ CP cP □ □ -Q- flP 20 40 60 index of trajectory [1] Fig. 14. Normalized maximal lateral accelerations in CarMaker 0.05 3 0.045 0.04 ^ 0.035 0.03 0.025 0 20 40 60 index of trajectory [1] Fig. 13. Final deviation of vehicle state considering only the driven part in CarMaker improvement in average. Of course, this is mainly possible due to the violation of the state constraints, but the overall performance is very similar to the one on Fig. 10. 4 CONCLUSIONS AND OUTLOOK In this paper a nonlinear optimization based trajectory planning algorithm is proposed to generate dynamically feasible, comfortable, and configurable motion for highly automated or autonomous road vehicles with the model-based prediction of the vehicle's motion. The main drawback of the developed approach is the significant computation time, which jeopardizes real-time usage even with a possible move from the current MATLAB based implementation to a faster one, e.g. in C++. To overcome this issue, two hybrid approaches are suggested with the application of an artificial neural network. Based on a dataset generated off-line with the optimization planner, the network is trained to solve the motion planning problem by the approximation 158 of the optimization planner's behavior. The first method uses the output of the neural network as an initial guess of the optimization process. This halves the run-time of the planning while maintaining the same performance regarding dynamical feasibility and motion optimality. The second algorithm skips the optimization entirely, using the neural network's output directly. The trajectory of the vehicle is calculated with the motion predictor applied in the optimization planner. The plausibility of the network's output is checked by the comparison of the prescribed and actually reached final states, which eliminates the problematic of the neural network being a black-box system. Although there are remarkable differences between the motions planned by the neural network and the optimization process when considering the whole trajectory, the deviations are in a feasible range if only the part of the motion which is meant to be actually driven is dealt with. The significantly decreased computation time can even enable the real-time usage of the proposed hybrid approach. There are multiple enhancement opportunities of the presented work. Firstly, the performance of the neural network estimator could be examined in case of the usage of more state constraint variables (input variables) as well as a greater magnitude of training data. The behavior of the algorithm should also be investigated in real operating conditions, when it is used as a continuously running motion planning task with a defined output rate to generate interconnected trajectories for long term vehicle motion. Secondly, the current work is not dealing with the generation of a safe emergency motion in case of the neural network generates a highly infeasible solution, which topic is however important for real applicability. Considering that the optimization planner must not be used online, but only to provide the learning samples for the neural Hegedüs, F. - Becsi T.-Aradi, S.-Geiopär,P. Strojniškivestnik- Journal af M echan|ca|Enggneer¡ng 65(2079)3,144-160 network, an even more accurate twin-track vehicle model could be used to evaluate the trajectories. As the authors are developing the optimization planner to include obstacle avoidance internally in the optimization problem, a corresponding extension of the presented neural network approach would also be an interesting opportunity. 5 ACKNOWLEDGEMENTS The research reported in this paper was supported by the Higher Education Excellence Program of the Ministry of Human Capacities in the frame of Artificial Intelligence research area of Budapest University of Technology and Economics (BME FIKPMI/FM). EFOP-3.6.3-VEKOP-16-2017-00001: Talent management in autonomous vehicle control technologies- The Project is supported by the Hungarian Government and co-financed by the European Social Fund. 6 REFERENCES [1] European Commission Mobility and Transport (2016). Road Safety in the European Union, from https://ec.europa.eu/ transport/road_safety/home_en,accessedon2018-10-18. [2] Mersky, A.C., Samaras, C. (2016). Fuel economy testing of autonomous vehicles. Transportation Research Part C: Emerging Technologies, vol. 65, p. 31-48, D0l:10.1016/j. trc.2016.01.001. [3] Chen,Y., Gonder, J.,Young,S.,Wood, E.(2017). Quantifying autonomous vehicles national fuel consumption impacts: A data-rich approach. Transportation Research Part A: Policy andPractice,in press, D0I:10.1016/j.tra.2017.10.012. [4] Tettamanti, T., Varga, I., Szalay, Z. (2016). Impacts of autonomous cars from a traffic engineering perspective. Periodica Polytechnica Transportation Engineering, vol. 44, no.4, p.244-250, D0I:10.3311/PPtr.9464. [5] Paden, B., Čap, M., Yong, S.Z., Yershov, D., Frazzoli, E. (2016). Asurvey of motion planningand controltechniques for self-driving urban vehicles. IEEE Transactions on Intelligent Vehicles, vol. 1, no. 1, p. 33-55, D0I:10.1109/ TIV.2016.2578706. [6] Vorobieva, H., Minoiu-Enache, N., Glaser, S., Mammar, S., Mammar, S. (2013). Geometric continuous-curvature path planningforautomatic parallel parking. 10th IEEE International Conference onNetworking,Sensing and Control, p.418-423, D0I:10.1109/ICNSC.2013.6548775. [7] Boryga, M. (2014). Trajectory planning of end-effector for path with loop. Strojniški vestnik - Journal of Mechanical Engineering, vol. 60, no. 12, p. 804-814, D0I:10.5545/sv-jme.2014.1965. [8] Minh, V.T., Pumwa, J. (2014). Feasible path planning for autonomousvehicles. MathematicalProblems in Engineering, vol.2014, art.ID 317494, D0I:10.1155/2014/317494. [9] Li, X., Sun, Z., He, Z., Zhu, Q., Liu, D. (2015). A practical trajectory planning framework for autonomous ground vehicles driving in urban environments. IEEE Intelligent Vehicles Symposium (IV), p. 1160-1166, D0l:10.1109/ IVS.2015.7225840. [10] Dijkstra, E.W. (1959). A note on two problems in connexion with graphs. NumerischeMathematik, vol. 1, no.1, p.269-271, D0I:10.1007/BF01386390. [11] Liu, S., Mohta, K., Atanasov, N., Kumar, V. (2018). Search-based motion planning for aggressive flight in SE(3). IEEE Robotics andAutomationLetters, vol.3,no.3,p. 2439-2446, D0I:10.1109/LRA.2018.2795654. [12] Likhachev, M., Ferguson, D., Gordon, G., Stentz, A., Thrun, S. (2005). Anytime dynamic A*: An anytime, replanning algorithm, The International Conference on Automated PlanningandScheduling,p. 262-271. [13] Montemerlo, M., Becker, J., Bhat, S., et al. (2008). Junior: The Stanford entry in theurbanchallenge. Journal of Field Robotics, vol.25, no.9, p.569-597, D0I:10.1002/rob.20258. [14] Jayasree, K.R.,Jayasree, P.R.,Vivek,A. (2017).SmoothedRRT techniquesfor trajectory planning. International Conference onTechnologicalAdvancementsin Power and Energy,p.1-8, D0I:10.1109/TAPENERGY.2017.8397376. [15] Dubins, L.E. (1957). On curves of minimal length with a constraint on average curvature, and with prescribed initial and terminal positions and tangents. American Journal of Mathematics, vol. 79, no. 3, p. 497-516, D0I:10.2307/2372560. [16] Arslan, O., Berntorp, K., Tsiotras, P. (2017). Sampling-based algorithms for optimal motion planning using closed-loop prediction. IEEE International Conference on Robotics and Automation, p.4991-4996, D0I:10.1109/ICRA.2017.7989581. [17] Howard, T. M., Kelly, A. (2007). Optimal rough terrain trajectory generation for wheeled mobile robots. The International Journal of Robotics Research, vol. 26, no. 2, p. 141-166, D0I:10.1177/0278364906075328. [18] Ferguson, D., Howard, T. M. Likhachev, M. (2008). Motion planninginurbanenvironments: Part I. IEEE/RSJ International Conference on IntelligentRobotsandSystems, p.1063-1069, D0I:10.1109/IR0S.2008.4651120. [19] Li, B.,Shao, Z. (2015). A unified motion planning method for parkingan autonomous vehicle inthe presence of irregularly placedobstacles. Knowledge-Based Systems, vol. 86, p. 1120, D0I:10.1016/j.knosys.2015.04.016. [20] Yang, S.X., Meng,M. (2001).Neuralnetworkapproachesto dynamiccollision-free trajectory generation. IEEETransactions on Systems, Man, andCybernetics, PartB (Cybernetics),vol. 31, no.3, p.302-318, D0I:10.1109/3477.931512. [21] Liu, L.,Hu,J., Wang,Y. Xie,Z. (2017). Neural network-based high-accuracy motion control of a class of torque-controlled motorservo systemswith input saturation. Strojniški vestnik-JournalofMechanicalEngineering,vol. 63, no.9,p. 519-528, D0I:10.5545/sv-jme.2016.4282. [22] Engedy, I.,Horvath, G. (2009). Artificial neural network based mobile robot navigation. IEEE International Symposium on Intelligent Signal Processing, p. 241-246, D0I:10.1109/ WISP.2009.5286557. Motion Planning for Highly Automated Road Vehicles with a Hybrid Approach Using Nonlinear Optimization and Artificial Neural Networks 159 Strojniski vestnik - Journal of Mechanical Engineering 65(2019)3, 148-160 [23] Qu, H., Yang, S.X., Willms, A.R., Yi, Z. (2009). Real-time robot path planning based on a modified pulse-coupled neural network model. IEEE Transactions on Neural Networks, vol. 20, no. 11, p. 1724-1739, DOI:10.1109/TNN.2009.2029858. [24] Janglova, D. (2004). Neural networks in mobile robot motion. International Journal of Advanced Robotic Systems, vol. 1, no. 1, p. 15-22, DOI:10.5772/5615. [25] Tai, L., Paolo, G., Liu, M. (2017). Virtual-to-real deep reinforcement learning: Continuous control of mobile robots for mapless navigation. IEEE/RSJ International Conference on Intelligent Robots and Systems, p. 31-36, DOI:10.1109/ IROS.2017.8202134. [26] Plessen, M.G. (2017). Automating vehicles by deep reinforcement learning using task separation with hill climbing. CoRR, vol. abs/1711.10785. [27] Velagic, J., Osmic, N., Lacevic, B. (2008). Neural network controller for mobile robot motion control. International Journal of Electrical and Computer Engineering, vol. 2, no. 11, p. 2471-2476. [28] Gorinevsky, D., Kapitanovsky, A., Goldenberg, A. (1996). Neural network architecture for trajectory generation and control of automated car parking. IEEE Transactions on Control Systems Technology, vol. 4, no. 1, p. 50-56, DOI:10.1109/87.481766. [29] Yim, Y., Oh, S.-Y. (2004). Modeling of vehicle dynamics from real vehicle measurements using a neural network with two-stage hybrid learning for accurate long-term prediction. IEEE Transactions on Vehicular Technology, vol. 53, no. 4, p. 10761084, DOI:10.1109/TVT.2004.830145. [30] Bellem, H., Schonenberg, T., Krems, J.F., Schrauf, M. (2016). Objective metrics of comfort: Developing a driving style for highly automated vehicles. Transportation Research Part F: Traffic Psychology and Behaviour, vol. 41, p. 45-54, DOI:10.1016/j.trf.2016.05.005. [31] Hegedus, F., Becsi, T., Aradi, S. (2017). Dynamically feasible trajectory planning for road vehicles in terms of sensitivity and robustness. Transportation Research Procedia, vol. 27, p. 799-807, DOI:10.1016/j.trpro.2017.12.110. [32] Hegedus, F., Becsi, T., Aradi, S., Gapar, P. (2017). Model based trajectory planning for highly automated road vehicles. IFAC-PapersOnLine, vol. 50, no. 1, p. 6958-6964, DOI:10.1016/j. ifacol.2017.08.1336. [33] Pacejka, H.B. (2012). Chapter 8 - Applications of transient tire models. Pacejka, H.B. (ed.) Tire and Vehicle Dynamics (3rd ed.) p. 355-401, Butterworth-Heinemann Oxford, DOI:10.1016/ B978-0-08-097016-5.00008-5. 160 Hegedus, F. - Bécsi, T. - Aradi, S. - Gâspâr, P.