Trajectory Optimization And State Selection For Urban .
Artificial Life and Robotics (2018) 4-4ORIGINAL ARTICLETrajectory optimization and state selection for urban automateddrivingKeisuke Yoneda1 · Toshiki Iida1 · TaeHyon Kim1 · Ryo Yanase1 · Mohammad Aldibaja1 · Naoki Suganuma1Received: 30 April 2018 / Accepted: 26 July 2018 / Published online: 22 September 2018 The Author(s) 2018AbstractThe automated driving is an emerging technology in which a car performs recognition, decision making, and control. Thedecision-making system consists of route planning and trajectory planning. The route planning optimizes the shortest pathto the destination like an automotive navigation system. According to static and dynamic obstacles around the vehicle, thetrajectory planning generates lateral and longitudinal profiles for vehicle maneuver to drive the given path. This study isfocused on the trajectory planning for vehicle maneuver in urban traffic scenes. This paper proposes a trajectory generationmethod that extends the existing method to generate more natural behavior with small acceleration and deceleration. Thispaper introduces an intermediate behavior to gradually switch from the velocity keeping to the distance keeping. The proposed method can generate smooth trajectory with small acceleration/deceleration. Numerical experiments show that thevehicle generates smooth behaviors according to surrounding vehicles.Keywords Automated vehicle · Path planning · Frenet-frame1 IntroductionAutomated vehicle is recently developed in the field of automotive engineering, robotics, and artificial intelligence forthe purpose of commercially available vehicle [1, 2]. Theautomated vehicle is expected to reduce traffic accidents,traffic jam and driver’s burden. Nowadays, some drivingassistant systems such as adaptive cruise control, lane keeping assist and emergency braking system, are available forexpressway driving and emergency system.To achieve urban automated driving, it is important togenerate appropriate behaviors according to surrounding circumstances. The automated vehicles are generally equippedwith various sensors including light detection and ranging(LiDAR), radar, camera, global navigation satellite system/inertial navigation system (GNSS/INS) as shown in Fig. 1a.These sensors data are processed to percept surroundingThis work was presented in part at the 2nd InternationalSymposium on Swarm Behavior and Bio-Inspired Robotics,Kyoto, October 29–November 1, 2017.* Keisuke hi, Kanazawa, Ishikawa 920‑1192, Japan13Vol:.(1234567890)environment, estimate a precise location, optimize trajectories and control the vehicle. To implement a robust vehiclesystem, one common approach is to achieve such perception and decision-making systems using a precise predefinedmap. These approaches generally refer to highly accuratelandmark information (e.q., lane markings, intersections)from the predefined map based on the current vehicle poseas shown in Fig. 1b. This study is focused on a behaviorplanning for vehicle maneuver in urban traffic scenes.The behavior planning in automated driving consists ofroute planning and trajectory planning. The route planningoptimizes the minimum cost path from the current positionto the destination. Generally, Dijkstra’s and A* algorithmsare able to find the shortest route in structured environmentsfrom digital map like an automotive navigation system. Onthe other hand, in unstructured environments, heuristicmethods are able to optimize collision-free and smooth pathin the free region [3–5]. For the obtained path, the trajectoryplanning is performed to optimize the minimum cost polynomial function [6–9]. In the trajectory planning, the 4th/5thorder polynomial functions enable to generate continuoustrajectories with the minimum acceleration/jerk. Primitivedriving patterns such as distance keeping and velocity keeping, was described in [6]. The obtained behaviors were thenevaluated qualitatively by numerical simulation. Although
Artificial Life and Robotics (2018) 23:474–480475numerical experiments and Sect. 5 concludes with theobtained results.2 Behavior model2.1 Decision‑making system(a)(b)In our automated vehicle system, the decision-making system consists of multi-level of behavior planners as shownin Fig. 1c. It consists of a route navigation (high level), adriving permission planning according to traffic rules (middle level) and a trajectory planning (low level). The highlevel planner has the role of the route planning using thedigital map. The middle-level planner manages the trafficrules (e.g., velocity limit, permission of transiting intersection and passing lanes) for the obtained path. The low-levelplanner then generates a trajectory according to static anddynamic obstacles around the vehicle. Therefore, lateral andlongitudinal profiles are optimized based on the obtainedpath from the high-level planner, and driving permissionsfrom the middle-level planner.2.2 Sensing data(c)Fig. 1 Overview of experimental vehicle and decision makingscheme. a Experimental automated vehicle, b digital map data, cdecision making schemethe primitive driving patterns could behave appropriatelywhile smoothly avoiding obstacles in the surroundings, itwas not a natural driving behavior because it was too aggressive. In other words, there is a problem that a large acceleration/deceleration occurs at the time of switching betweenthe distance keeping and the velocity keeping. Drivingwith large acceleration/deceleration is an unnatural behavior. Therefore, this paper proposes a trajectory generationmethod that extends the existing method described in [6].The proposed method can generate smooth trajectory withsmall acceleration/deceleration by introducing an intermediate behavior. Numerical experiments show that the vehiclegenerates smooth behaviors according to the surroundingvehicles.The rest of this paper is composed as follows. Section 2introduces the behavior model. Section 3 explains the proposed trajectory generation method. Section 4 describesThe above-mentioned planners require sensing informationabout ego-vehicle and surrounding objects. GNSS/INS andthe self-localization module provide precise position, orientation, velocity and acceleration for the ego-vehicle [10–12].Surrounding objects are recognized using the onboard sensors such as LiDAR, radar and camera. For dynamic objects,information about position, orientation, rectangular size,velocity, and acceleration are estimated using LiDAR orradar (the size is only estimated by LiDAR). Static objectsare estimated as an occupancy grid map by LiDAR.3 Trajectory planning3.1 Frenet‑frame coordinateTraveling path is defined by parametric curves such asBezier curve and B-Spline curve. To generate trajectoriesalong the shape of the given path, the Frenet-frame is commonly used in [6]. On the Frenet coordinate system, normaltrajectory d(t) and tangential trajectory s(t) are defined forthe given curve as shown in Fig. 2a. Let d(t) and s(t) be aoffset pattern and a distance pattern, respectively. The offset pattern corresponds to the amount of lateral movementwithin the traveling lane. On the other hand, the distancepattern corresponds to the acceleration/deceleration profile.Therefore, the vehicle trajectory is formulated in the following equation.13
476Artificial Life and Robotics (2018) 23:474–480t1 ( t1 t0 𝛥Td or 𝛥Ts ). Let 𝛥T be a preview time(𝛥Tmin 𝛥T 𝛥Tmax , 𝛥Tmin 2.0[s], 𝛥Tmax 6.0[s]).The offset pattern d(t) is defined using a quintic functionto control the lateral position precisely. The distance patterns(t) is defined using a quartic or quintic functions for velocity keeping and distance keeping, respectively. To control avelocity smoothly, the distance pattern is generated by solving a quartic function (in this case, b5 0.0). On the otherhand, to control a distance to the leading vehicle precisely,a quintic function provides the distance pattern.3.3 Offset pattern generationTo generate collision-free trajectories for static and dynamicobjects, trajectories of several terminal conditions are generated as candidates according to the lane with. The trajectoryis defined between the initial condition [d0 , ḋ 0 , d̈ 0 ] at time t0and the terminal condition [d1 , ḋ 1 , d̈ 1 , 𝛥Td ] at time t1 using aquintic function Eq. (2). Considering a comfortable driving, velocity and acceleration for lateral direction should beminimized. Therefore, the terminal condition is [d1 , ḋ 1 , d̈ 1 ,𝛥Td ] [𝛥d, 0, 0, 𝛥Td ]. Several patterns of 𝛥d and 𝛥Td areselected to generate candidate trajectories.(a)3.4 Distance pattern generation(b)Fig. 2 Trajectory generation. a Trajectory generation in the Frenetframe, b offset patterns and distance patterns(1)In practical use, the optimum trajectory is searched whileadjusting the various types of terminal condition for eachpattern as shown in Fig. 2b.x ⃗(s(t), d(t)) ⃗r(s(t)) d(t) n⃗r (s(t)).3.2 Polynomial trajectoryThe vehicle trajectory is a combination of an offset patternd(t) and a distance pattern s(t). Both patterns are generallyformulated as a 4th/5th order polynomial function. The 4thorder function enables to generate the minimum accelerationtrajectory. The 5th order function enables to generate the minimum jerk trajectory. The offset pattern d(t) and the distancepattern s(t) are defined as follows.d(t) a0 a1 t a2 t2 a3 t3 a4 t4 a5 t5 ,(2)(3)where variable ai and bi are computed according tothe initial conditions [d0 , ḋ 0 , d̈ 0 ] [s0 , ṡ 0 , s̈ 0 ] and terminal conditions [d1 , ḋ 1 , d̈ 1 , 𝛥Td ] [s1 , ṡ 1 , s̈ 1 , 𝛥Ts ] at times(t) b0 b1 t b2 t2 b3 t3 b4 t4 b5 t5 ,13The distance pattern has the role of velocity profile according to the surrounding objects. Distance keeping and velocity keeping behaviors are generated depending on the distance to the leading vehicle and so on. In [6], track mode,stop mode and cruise mode was proposed as primitivebehavior patterns. The track and stop mode are basic distance keeping behaviors which precisely adjust vehiculargap to the leading vehicle and distance to the stop line usingquintic functions in Eq. (3). In the distance keeping, it isnecessary to consider continuity of position, velocity andacceleration. The initial condition [s0 , ṡ 0 , s̈ 0 ] at time t0 andterminal condition [s1 𝛥s, ṡ 1 , s̈ 1 , 𝛥Ts ] at time t1 are thendetermined to generate trajectories. Let 𝛥s be a small valueof displacement around the terminal condition.On the other hand, the cruise mode is defined as a velocitykeeping behavior. In the velocity keeping, it is not necessaryto adjust the terminal position precisely. Because it leads toquick acceleration and deceleration. Therefore, when thereis no preceding vehicle nearly, the distance pattern is generated using a quartic function in Eq. (3) under b5 0.0. Theinitial condition [s0 , ṡ 0 , s̈ 0 ] at time t0 and terminal condition[ṡ 1 𝛥s,̇ s̈ 1 , 𝛥Ts ] at time t1 are then determined to generatetrajectories.Werlind et al. [6] demonstrated the collision-free maneuverby numerical simulation. However, the obtained behavior wasnot a natural driving behavior because a large acceleration anddeceleration occurs at the time of switching from the velocity
Artificial Life and Robotics (2018) 23:474–480477keeping to the distance keeping. Therefore, this paper proposes a trajectory generation method that extends the existingmethod in [6] to generate more natural behavior with smallacceleration and deceleration. This paper introduces an intermediate behavior “adjust mode” to gradually switch from thevelocity keeping to the distance keeping.Figure 3 illustrates an overview of distance patterns. Figure 4 indicates a flowchart of the mode selection. Leadingvehicle is extracted for each offset pattern. Suitable modecandidates are extracted based on the position, velocity andacceleration for the ego-vehicle and the leading vehicle. Trajectories is generated according to each mode. If multiplemode is selected, the model with the closest terminal positionis extracted. In Fig. 4, ṡ tgt is the target velocity. It is the speedlimit value of the current road obtained from the digital map.Details of each distance pattern are described as follow.3.4.1 Distance keeping: track modeThe track mode generates distance keeping profile for thepreceding vehicle as shown in Fig. 3a. The following equation keeps a vehicular gap Ddes (t1 ) between the ego-vehicleand the preceding vehicle while aligning the velocity and theacceleration. s1 slv (t1 ) Ddes (t1 ) ṡ ṡ (t ) 𝜏 s̈ (t )lv 1lv 1 1 s̈ 1 s̈ lv (t1 ) Ddes (t1 ) D0 𝜏 ṡ lv (t1 ), 2 slv (t1 ) slv (t0 ) ṡ lv (t0 )ΔTs s̈ lv (t1 )ΔTs 2 ṡ (t ) ṡ (t ) s̈ (t )ΔTlv 0lv 0s lv 1 s̈ lv (t1 ) s̈ lv (t0 ) t1 t0 ΔTs(a)(b)(4)Fig. 4 Distance pattern mode selectionwhere slv is a position of the preceding vehicle. D0 is theminimum vehicular gap ( D0 5 m) and 𝜏 is an inter-vehicletime (𝜏 2 s).(c)(d)Fig. 3 Distance patterns (𝛥s 0.0 and 𝛥ṡ 0.0). a Track mode, b stop mode, c cruise mode, d adjust mode13
478Artificial Life and Robotics (2018) 23:474–4803.4.2 Distance keeping: stop modeTo park the vehicle at the stop line or the destination, thestop model generates a deceleration profile as shown inFig. 3b. sstop is a distance to the parking place which is givenfrom the middle-level planner.{s1 sstop(5)ṡ 1 s̈ 1 0.0.(a)3.4.3 Velocity keeping: cruise modeThe cruise mode keeps the ego-velocity to the given velocity ṡ tgt as shown in Fig. 3c. ṡ tgt is determined from the speedlimit.{ṡ 1 ṡ tgt(6)s̈ 1 (ṡ 1 ṡ 0 ) ΔTs(b)3.4.4 Velocity keeping: adjust modeWhen the preceding vehicle is at a long distance, the adjustmode gradually reduces the vehicular gap to Ddes (t1 ) asshown in Fig. 3d. It searches the maximum velocity ṡ adj withthe maximum preview time 𝛥Ts under Ddes (t1 ) D(t1 ). ṡ 1 ṡ adj s̈ 1 0.0 Tmin t0 ΔTmin T max t0 ΔTmax .Fig. 5 Collision detection. a Collision detection for dynamic object, bcollision detection for static object using occupancy grid mapis selected from these trajectories [13]. The cost function C isdefined in Eqs. (8)–(10).(7)iC kpath Cpath kd Cd ks Cs ,Cd kjd3.5 Collision detectionFor all generated trajectory set, it is determined whether ornot each trajectory is drivable. For dynamic objects, it isconfirmed the overlapping of bounding boxes at each timestep as shown in Fig. 5a. When the rectangular boxes of theego-vehicle and the moving object overlap, it is judged thata collision has occurred. In a similar way, a collision detection with static object is performed using the occupancy gridmap. Each bounding box of the ego-trajectory is confirmeda collision with the obstacle grid cell of the occupancy gridmap as shown in Fig. 5b.3.6 Optimal trajectory selectionCollision-free trajectory set are generated based on theabove-mentioned procedures. The minimum cost trajectory13{Cs t0t1d⃛2 (t)dt kxd 𝛥Td kyd 𝛥d2 ,kjs t 1 ⃛s2 (t)dt kxs 𝛥Ts kys 𝛥s20tkjs t 1 ⃛s2 (t)dt kxs 𝛥Ts kys 𝛥ṡ 2t0(8)(9)(if distance keeping)(if velocity keeping) ,(10)where, Cd and Cs are costs about the offset and the distancepatterns, respectively. Both terms evaluate integral of jerks,ipreview times and displacements of terminal condition. Cpathiis a weight of drivable lanes for the i-th lane. Cpathis deter-mined from the middle-level planner to enable to change01lanes for a multiple-lane road. In case of Cpath, the Cpathtrajectory traveling in 0-th lane tends to be selected as shownin Fig. 6. On the other hand, the trajectory to change the lane01is likely to be selected in case of Cpath. Cpath
Artificial Life and Robotics (2018) 23:474–480479(a)(a)(b)Fig. 6 Multiple lane driving4 Simulation and evaluation(b)4.1 ConditionsTwo types of experiments are carried out to evaluate theeffectiveness of the proposed method. The first experimentquantitatively evaluates the maximum value of acceleration/deceleration depending on whether or not the proposed intermediate mode (adjust mode) is introduced. As one of thescenes where large deceleration occurs, the scene approaching the stopped vehicle while traveling at the maximumspeed of the general road (60 km/h) is evaluated as shownin Fig. 7a. The second evaluation qualitatively observes thebehavior for typical driving scenes. The obtained behaviorsare observed with typical driving maneuvers of followingand overtaking scenarios as shown in Fig. 8a, b.4.2 ResultsFigure 7b, c indicate experimental results for the approaching driving. In the case where the adjust mode is not used,it can be confirmed that a large deceleration occurs immediately after switching from the velocity keeping to thedistance keeping. In Fig. 7b, the maximum decelerationvalue was 3.94 m s2 . On the other hand, by introducingthe adjust mode, it was confirmed that deceleration startedfrom a few seconds before and the maximum decelerationvalue was suppressed to 1.71 m s2 as shown in Fig. 7c.Therefore, the results showed that the introduction of theproposed method is possible to reduce the maximum deceleration value to 43.4% of the conventional one.Figure 8c, d shows experimental results for typical driving maneuvers. In the following scenario, the precedingvehicle starts to decelerate after 10 s and stops as shownin Fig. 8c. While the gap is increasing, the ego-vehicledrives the cruise mode to keep the target velocity. When the(c)Fig. 7 Experimental results. a Scenario, b results: w/o adjust mode, cresults: w/ adjust modepreceding vehicle starts decelerating, the ego-vehicle gradually reduces the gap using the adjust mode. The track modethen accurately controls the gap. In a series of state transitions, it is observed that the ego-velocity and the gap aresmoothly updating. On the other hand, the preceding vehicleparks by the side of the road in the overtaking scenario.As shown in Fig. 8d, the ego-vehicle smoothly controls thelateral offset and velocity during overtaking. These resultsshow that the proposed planner controls the vehicle properlyaccording to surroundings.5 ConclusionA method of trajectory optimization is proposed for vehicle maneuvers in urban road. Polynomial curves generatethe minimum jerk curve in given conditions. According tothe static and dynamic objects, the ego-vehicle switchesmotion pattern smoothly. The proposed method introduces13
480Artificial Life and Robotics (2018) 23:474–480(a)Open Access This article is distributed under the terms of the Creative Commons Attribution 4.0 International License (http://creat iveco mmons .org/licen ses/by/4.0/), which permits unrestricted use, distribution, and reproduction in any medium, provided you give appropriatecredit to the original author(s) and the source, provide a link to theCreative Commons license, and indicate if changes were made.References(b)(c)(d)Fig. 8 Experimental results. a Following scenario, b overtaking scenario, c results: following scenario, d results: overtaking scenariothe intermediate behavior to smoothly switch from thevelocity keeping behavior to the distance keeping behavior.Experimental results showed that by introducing the proposed method, it is possible to halve the deceleration valuethat occurs when the ego-vehicle is approaching a stationaryvehicle.Additionally, the proposed trajectory planner has beeninstalled in our automated vehicle and conducted a selfdriving test in real traffic scenes. Further quantitative evaluations in dense traffic scenes in actual automated drivingare future works.131. Franke U, Pfeiffer D, Rabe C, Knoeppel C, Enzweiler M, Stein F,Herrtwich RG (2013) Making Bertha see. Proceedings of ICCVworkshop on computer vision for autonomous driving2. Kato S, Takeuchi E, Ishiguro Y, Ninomiya Y, Takeda K, HamadaT (2015) An open approach to autonomous vehicles. IEEE Micro35(6):60–693. Dolgov D, Thrun S, Montemerlo M, Diebel J (2008) Pr
Traveling path is defined by parametric curves such as Bezier curve and B-Spline curve. To generate trajectories along the shape of the given path, the Frenet-frame is com-monly used in [6 ]. On the Frenet coordinate system, normal trajectory d(t) and tangential trajectory
(like hiking and dining) or different transportation modes, such as walking and driving. We show examples of trajectory classification in Section 7. Trajectory Outlier Detection: Different from trajectory patterns that frequently occur in trajectory data, trajectory ou
Efficient constraint handling – Gradients guide search – Robust & efficient formulation Proven approach in EMTG software (Evolutionary Mission Trajectory Generator) Low-Thrust Trajectory Optimization From Sims and Flanaga
St-Toolkit: A Framework for Trajectory Data Warehousing 3 Our contributions are twofold: a semantic model for trajectory data warehouse and a middleware for loading, designing and querying a spatio-temporal data warehouse. The model is based on the conceptual view on trajectories introduced by Spaccapietra et al. 2007. A trajectory is a segment
Since the eld { also referred to as black-box optimization, gradient-free optimization, optimization without derivatives, simulation-based optimization and zeroth-order optimization { is now far too expansive for a single survey, we focus on methods for local optimization of continuous-valued, single-objective problems.
Structure topology optimization design is a complex multi-standard, multi-disciplinary optimization theory, which can be divided into three category Sizing optimization, Shape optimization and material selection, Topology optimization according to the structura
testing on a glider range optimization problem, this method was applied to the solar aircraft trajectory optimization problem. The system developed is robust, computationally efficient, and can be used to optimize and control multi-purpose solar aerial vehicles. Plans are underway to design and build a s
Several benefits of using the NASA General Mission Analysis Tool (GMAT) for trajectory design are also demonstrated. Specific analysis using GMAT includes an investigation of the Earth-Moon-Sun dynamics as they apply to the initial lunar-flyby trajectory, the use of the Sun-Earth L1 Lagrange point to transition from an Earth-
ASTM E 989: 1989Classification for determination of impact VM1 2.0.1 insulation class (IIC) International Standards Organisation ISO 140/VII: 1978 Field measurements of impact sound VM1 2.0.1 insulation of floors Amend 2 Dec 1995 Amend 2 Dec 1995 Amend 2 Dec 1995. 9 AIRBORNE AND IMPACT SOUND DEPARTMENT OF BUILDING AND HOUSING July 1992 Definitions G6/VM1 & AS1 Adequate Adequateto achieve the .
