future internet Article
A Driving Behavior Planning and Trajectory Generation Method for Autonomous Electric Bus Lingli Yu *
ID
, Decheng Kong and Xiaoxin Yan
School of Information Science and Engineering, Central South University, Changsha 410083, China;
[email protected] (D.K.);
[email protected] (X.Y.) * Correspondence:
[email protected] Received: 6 May 2018; Accepted: 8 June 2018; Published: 10 June 2018
Abstract: A framework of path planning for autonomous electric bus is presented. ArcGIS platform is utilized for map-building and global path planning. Firstly, a high-precision map is built based on GPS in ArcGIS for global planning. Then the global optimal path is obtained by network analysis tool in ArcGIS. To facilitate local planning, WGS-84 coordinates in the map are converted to local coordinates. Secondly, a double-layer finite state machine (FSM) is devised to plan driving behavior under different driving scenarios, such as structured driving, lane changing, turning, and so on. Besides, local optimal trajectory is generated by cubic polynomial, which takes full account of the safety and kinetics of the electric bus. Finally, the simulation results show that the framework is reliable and feasible for driving behavior planning and trajectory generation. Furthermore, its validity is proven with an autonomous bus platform 12 m in length. Keywords: autonomous electric bus; path planning; high-precision map building; trajectory generation
1. Introduction Autonomous driving technology improves active traffic safety and brings convenience to human beings. It has received much attention from researchers and scholars. Compared with unmanned cars, autonomous busses drive on relatively fixed routes, at relatively slow speeds, and for shorter distances in urban areas. At the same time, the role of bus drivers is transformed into a bus safety officer to decrease manpower fatigue. Thus, autonomous busses are more likely to come about earlier than driverless cars in everyday life. However, driving behavior planning and trajectory generation are still a challenging problem for autonomous vehicle technology. During the global planning stage, global routes are determined by a high-precision digital map and a localization system [1]. The global planning methods include the artificial potential field (APF) [2], cell decomposition [3], visibility graph [4], Voronoi diagram [5], and probabilistic road maps [6]. All these methods establish the model of the environmental map by grid or nodes and then compute the weights to search the optimal path. They are time consuming, especially in complex environments. Many scholars focus on sampling-based algorithms for on-line path planning such as the probabilistic roadmap method (PRM) [7] and the rapidly-exploring random tree (RRT) [8]. Unfortunately, these methods produce solutions that are suboptimal or even far from optimal and the resulting path is not continuous. Nowadays, more and more researchers begin to apply intelligence computation to solve this problem. For instance, [9] proposes a global path planning of wheeled robots by multi-objective memetic algorithms (MOMAs). A modified particle swarm optimization (PSO) algorithm is utilized to obtain a high-quality global path for a mobile robot in [10]. An intelligence algorithm could find the global optimal solution, but it needs more computation time. Local planning contains two parts, driving behavior planning, and trajectory generation. Driving behavior of the autonomous bus is determined by global planning and perception information [11]. Future Internet 2018, 10, 51; doi:10.3390/fi10060051
www.mdpi.com/journal/futureinternet
Future Internet 2018, 10, 51
2 of 14
For example, [12] introduces a driving decision mechanism based on decision tree, and the multiple criteria decision making is utilized in [13]. Reference [14] presents an integrated behavioral inference and decision-making, it simulates vehicle behavior for both autonomous vehicle and nearby vehicles. Meanwhile, [15] proposes a human-like decision-making algorithm to address decision making problem in dynamic, uncertain scenarios such as the simple intersections and roundabouts. However, these methods often explore unlikely regions of the belief space and increase the calculation time. Finite state machine (FSM) is expressed by the sequences of driving behavior [16,17], it is a practical discrete sample method which is usually applied to driverless vehicle system. However, it is more suitable for specific and simplified situations and not robust to a varied world. Generally, trajectory planning is an essential and key part of robotics motion planning. Nowadays, many researchers usually rest on some simple assumptions about the vehicle kinematics. The trajectory is expressed by a variety of methods, including polynomial [18], Bezel curve [19], Spline curve [20], and so on. A quaternary fourth order polynomial and the dynamic bicycle model are adopted in [6] to describe the vehicle motion equation. Reference [21] uses a family of parameterized trajectories, which contains geometric constraints and performance index. These methods are usually converted into constrained optimization, which increase the complexity greatly. Extensive studies have been conducted on autonomous vehicles recently, but few have studied the technology of autonomous busses. In this paper, we mainly propose a framework that is applied to an autonomous electric bus successfully. For global planning, we apply ArcGIS to model a road network and plan a global optimal path. ArcGIS is convenient to build and edit a road network for autonomous bus, such as setting road properties, traffic facilities and so on. Besides, it is more efficient and stable to search an optimal path by ArcGIS for the road network. To overcome shortcomings of traditional FSM, a double-layer FSM is utilized to reduce the complexity of driving behavior transfer and enhance the stability in different driving scenarios. Local trajectory generation requires a faster reaction time than global planning to deal with obstacles effectively, and the trajectory should satisfy curvature continuity. Cubic polynomial curves meet the demand and its few paraments reduce calculation time. The remainder of this paper is organized as follows. Firstly, the framework of path planning for autonomous bus system is introduced in Section 2. Next, map-building and global planning are described in Section 3. Section 4 presents the method of driving behavior planning and trajectory generation is introduced in Section 5. Section 6 provides simulations and results of field test in real-world scenarios. Finally, some conclusions of this paper follow in Section 7. 2. Framework Path planning provides safe and reliable maneuvers under various driving environment, and it is the basis for autonomous vehicle to drive without human intervention. Figure 1 shows the framework of the path planning system. An integrated navigation system (GPS/IMU) provides vehicle speed, heading angle, and global position. The perception layer provides environmental information including static or dynamic obstacles and road information for path planning. Global planning is employed to find the optimal path from the current position to the destination in the high-precision GIS map form. Local planning plans appropriate driving behaviors and generates a smooth trajectory for the autonomous bus to follow the route. In this paper, global planning is based on ArcGIS. Firstly, a high-precision GIS map is built according to original GPS data of path, which represents the road geometry. Then, the network analysis tool of ArcGIS is utilized to search for the optimal route in the high-precision map based on the current location which can be obtained by GPS/IMU. Local planning contains two parts: driving behavior planning and optimal trajectory generation. Driving behavior planning imitates an experienced driver to give reasonable driving behaviors (e.g., lane changing, overtaking or following the vehicle ahead) based on perception information and global waypoints. Double-layer FSM (finite state machine) is proposed to execute a rule-based decision process under different driving scenarios. The trajectory generation module generates a safe
Global planning Map-building Future Internet 2018, 10, 51
GPS/IMU
3 of 14
Global path planning
and feasible path for autonomous bus. Here, a cubic polynomial is used to express optimal trajectory reference smoothly, which fully considers the kineticsGlobal and safety of the bus.
Future Internet 2018, 10, x FOR PEER REVIEW
points Local planning Global planning Driving behavior planning Map-building
Perception information GPS/IMU
3 of 15
Optimal trajectory generation Global path planning Local Global reference trajectory points Local planning
Figure 1. Framework of path planning for autonomous bus. Driving behavior planning Perception
Local planning contains two parts: driving behavior planning and optimal trajectory generation. information Driving behavior planning imitates an experienced driver to give reasonable driving behaviors (e.g., lane changing, overtaking or following the vehicleOptimal ahead) trajectory based on perception information and generation global waypoints. Double-layer FSM (finite state machine) is proposed to execute a rule-based decision process under different driving scenarios. Local The trajectory generation module generates a safe and feasible path for autonomous bus. Here, a cubic polynomial is used to express optimal trajectory trajectory smoothly, which fully considers the kinetics and safety of the bus. Figure 1. Framework Framework of of path planning for autonomous bus.
3. Map Building and Global Path Planning Local planning contains driving behavior planning and optimal trajectory generation. 3. Map Building and Globaltwo Pathparts: Planning Map-building is one of the core driverless technologies, which is crucial for positioning, Driving behavior planning imitates an experienced driver to give reasonable driving behaviors (e.g., Map-building is one the core driverless is crucialof forsensor positioning, navigation navigation and control ofof autonomous electrictechnologies, bus. With thewhich development technology, now lane changing, overtaking or following the vehicle ahead) based on perception information and and control of autonomous bus. With development sensor technology, now accurate accurate positioning of the electric lane through the the GPS/IMU can beofobtained. In this section, a highglobal waypoints. Double-layer FSM (finite state machine) is proposed to execute a rule-based positioning of the lane through GPS/IMU can be obtained. In this section, a high-precision map is precision map is constructed bythe ArcGIS. decision process under different driving scenarios. The trajectory generation module generates a safe constructed ArcGIS. ArcGIS by offers a unique set of capabilities for applying location-based analysis to business and feasible path for autonomous bus. Here, a cubic polynomial is used to express optimal trajectory ArcGIS offers a unique capabilities for applying location-based to business practices. It is widely used set foroftopographic map building [22] and mapanalysis data analysis [23].practices. Here, a smoothly, which fully considers the kinetics and safety of the bus. It is widely usedinto for topographic map are building [22] and map data analysis [23]. Here, a path is divided path is divided two types which the general path and special path, respectively. The path in into contains two types which areedge the general path and special respectively. The path in GISwith contains two GIS two parts: and node. All nodes are path, connected by the edges in order no break 3. Map Building and Global Path Planning parts: edge and node. All nodes are connected by the edges in order with no break point. point. Map-building is one of the core driverless technologies, which is crucial for positioning, 3.1. Map-Building in General General Path navigation and control of autonomous electric bus. With the development of sensor technology, now 3.1. Map-Building in Path accurate positioning of the lane through the GPS/IMU can turn. be obtained. In this section, a highThe general general path data is is imported to The path refers refers to to aastraight straightlane laneand andhas hasno nolarge large turn.Original OriginalGPS GPS data imported precision map is constructed by ArcGIS. ArcGIS to build road network. TheThe path in the GISGIS map is connected by by edges in order. To reduce the to ArcGIS to build road network. path in the map is connected edges in order. To reduce ArcGIS offers a unique set of capabilities for applying location-based analysiswhich to business amount number of GPS data, original data are filtered by setting the distance interval, means the amount number of GPS data, original data are filtered by setting the distance interval, which practices. It is widelynodes used ford,topographic building [22] from and map data analysis [23]. Here, a the distance between as is shown in map Figure (d ranges 5m to 10 m here). means the distance between is nodes d, as shown in 2Figure 2 (d ranges from 5m to 10 m here). path is divided into two types which are the general path and special path, respectively. The path in GIS contains two parts: edge and node. All nodes are connected by the edges in order with no break Node Edge GPS point point. 3.1. Map-Building in General Path The general path refers to a straight lane and has no large turn. Original GPS data is imported to ArcGIS to build road network. The path in the by edges in order. To reduce d GIS map is connected Lane centerline the amount number of GPS data, original data are filtered by setting the distance interval, which Figure Map-building in general means the distance between nodes is d,2. shown in Figure 2 (d path. ranges Figure 2.asMap-building in general path. from 5 m to 10 m here).
Node Edge GPS point
d
Lane centerline
Figure 2. Map-building in general path.
Future Internet 2018, 10, 51
4 of 14
Future Internet 2018, 10, x FOR PEER REVIEW
4 of 15
3.2. 3.2. Map-Building Map-Building in in Special Special Path Path The The special specialpath pathrefers refersto toaapath path with with aa corner corner of of aa relatively relatively large large angle, angle, such such as as intersections intersections or or ramps. Besides, there is no lane line can be detected by perception layer. The GPS data ramps. Besides, there is no lane line can be detected by perception layer. The GPS data of of special special path path isissaved saved in in special special path path database. database. Meanwhile, Meanwhile, special specialmark mark points pointsare are respectively respectivelyset seton onthe thetwo two ends ends of the special path which are entry point and exit point. They are the key points to distinguish of the special path which are entry point and exit point. They are the key points to distinguish the the general (Figure 3). 3). TheThe yellow lineline is only to keep path integrity in ArcGIS while generalpath pathand andspecial specialpath path (Figure yellow is only to keep path integrity in ArcGIS the GPS points are theare truethe global while the GPS points true path. global path.
Intersection
Exit point
GPS point Entry point Figure 3. Map-building in special path. Figure 3. Map-building in special path.
Finally, two databases are established: GIS database and special path database. GIS database Finally, two databases are established: GISArcGIS database path database database.stores GIS database stores the road network information created by andand the special special path GPS data stores the road network information created by ArcGIS and the special path database stores GPS data of ramps or intersections. of ramps or intersections. 3.3. Global Path Planning Based on ArcGIS 3.3. Global Path Planning Based on ArcGIS The road network map consists of two basic elements which are edges and nodes respectively, The road network map consists of two basic elements which are edges and nodes respectively, which forms a topology network structure. The direction of road, markers, and the other rules are all which forms a topology network structure. The direction of road, markers, and the other rules are all created in advance in the map. The global planning is to find the best path from the initial position to created in advance in the map. The global planning is to find the best path from the initial position to the destination based on such a network topology. the destination based on such a network topology. ArcGIS supports the network analysis very well based on the Geometric Network. So the global ArcGIS supports the network analysis very well based on the Geometric Network. So the global path planning is adopted by network analysis tool of ArcGIS. Then we can get the optimal path and path planning is adopted by network analysis tool of ArcGIS. Then we can get the optimal path and all all features (edges and nodes) of the path. The nodes in the global optimal path are the reference features (edges and nodes) of the path. The nodes in the global optimal path are the reference points points for local planning. Besides, instead of planning one specific path to the destination, the global for local planning. Besides, instead of planning one specific path to the destination, the global planning planning plans the path dynamically based on the current location of the bus. plans the path dynamically based on the current location of the bus. 3.4. Conversion of the Coordinate System 3.4. Conversion of the Coordinate System The built map is based on WGS-84 coordinate system, which is not suitable for local planning The built map is based on WGS-84 coordinate system, which is not suitable for local planning and control. WGS-84 coordinate system is transferred to local coordinate. Figure 4 displays the three and control. WGS-84 coordinate system is transferred to local coordinate. Figure 4 displays the three types of coordinate systems. ‘East–north–up’ coordinates are a coordinate system where the X axis is types of coordinate systems. ‘East–north–up’ coordinates are a coordinate system where the X axis is lattitudinal, the Y axis is longitudinal, and the Z axis vertical. lattitudinal, the Y axis is longitudinal, and the Z axis vertical.
Future Internet 2018, 10, 51 Future Internet 2018, 10, x FOR PEER REVIEW
5 of 14 5 of 15
Y Y
H B
o
Equator
Y
O
L
X
O
X
X
prime meridian
Z
WGS-84
East-North-Up coordinate
Local coordinate
Figure 4. Transformation of coordinate systems. Figure 4. Transformation of coordinate systems. ( B, L, H ) represents the GPS data received from GPS/IMU which is the coordinate origin. B, L, and representslatitude, the GPSand data received fromin GPS/IMU is the system. coordinate origin. B, L, ( B, L, H )longitude, H represent geodetic height geocentricwhich coordinate Then convert it and H represent longitude, latitude,Pand geodetic height in geocentric coordinate system. Then convert ( X , Y , Z ) P P P to rectangular coordinate system by Equations (1)–(3) it to rectangular coordinate system P( XP , YP , ZP ) by Equations (1)–(3) X P ( Rn + H ) cos B cos L XP Y = ((RRn++HH ) cos B cos ) cos B sin L L (1) n P = (1) ( Rn + H ) cos B sin L YP Z [( R 2 P n + H ) − e R2n ]sin B ZP [( Rn + H ) − e Rn ] sin B
a/(/a(2acos cos2 BB++bb2 sin sin2 BB)) n =a2 RnR= 2
2
2
2
2
1/2 1/2
(2) (2)
e = ( a2 − b2 )/a
(3) e = (a − b ) / a (3) where Rn is radius of curvature in prime vertical and e is the first eccentricity of ellipsoid. a, b are semimajor axis of ellipsoid and semiminor axis of ellipsoid, respectively. P( xQ , yQ , zQ ) is a point in R a, b rectangular Then, the point ( xQPvertical , yQP , zQP ) ineeast–north–up coordinatesofisellipsoid. given by (4). where n iscoordinate. radius of curvature in prime and is the first eccentricity 2
2
P( xQ, yQ , zQ ) are semimajor axis of ellipsoid and − semiminor axiscos of ellipsoid, is a point xQP sin L L 0respectively. XQ − XP ( xQP , yQP , zQP ) = −the (4) in rectangular coordinate. − YP is given by (4). sinpoint B cos L − sin B sininLeast–north–up cos B YQcoordinates yQP Then, zQP cos B cos L cos B sin L sin B ZQ − ZP xQP − sin L cos L 0 XQ − XP 0 0 yQP (= B cos L coordinate − sin B sin L by cos(5) B and YP (4) Finally, we can get the x , − y sin ) in local the ( x 0 , y0 ) for local YQ −provide z cos B cos L cos B sin L sin B Z − Z path planning. P Q QP ( x 0 = xop ∗ cos yaw − yop ∗ sin yaw (5) Finally, we can get the ( x' , y' )y0in (5) and provide the ( x' , y' ) for local path = local yop ∗ coordinate cos yaw + xby op ∗ sin yaw planning. where, yaw is the heading angle of the bus.
x ' = xop *cos yaw − yop *sin yaw 4. Driving Behavior Planning Based on Double-Layer FSM y ' = yop *cos yaw + xop *sin yaw 4.1. Top-Layer FSM Design
(5)
yaw the heading angle of the bus. where, The mapisdata of general path and special path comes from GIS database and special path database, respectively. In addition, the driving behavior under the two types of paths is different; for instance, 4. Driving Behavior Planning Basedpath on Double-Layer overtaking is not allowed in special so as to ensureFSM safe driving for bus. Therefore, the state of top-layer FSM is devised as {general path (R1) and special path (R2)}. The type of road is distinguished 4.1.the Top-Layer FSMand Design by entry point exit point which are marked in global map (see Figure 3). The trigger events as follows: The map data of general path and special path comes from GIS database and special path T1: The bus passesIn the entry point. database, respectively. addition, the driving behavior under the two types of paths is different; for T2: The bus passes theallowed exit point. instance, overtaking is not in special path so as to ensure safe driving for bus. Therefore, the Thus, the top-layer state transition shows in Figure state of top-layer FSM is devised as {general path (R1)5.and special path (R2)}. The type of road is distinguished by the entry point and exit point which are marked in global map (see Figure 3). The trigger events as follows:
Future Internet 2018, 10, x FOR T2: The bus passes thePEER exitREVIEW point.
6 of 15
Thus, state transition T1:the Thetop-layer bus passes the entry point.shows in Figure 5. T2: The bus passes the exit point. Future Internet 2018, 10, 51
6 of 14
Thus, the top-layer state transition shows in Figure 5.
T1
General path
T1
Special path
General path
Special path
T2 T2
Figure 5. Top-layer state transition. Figure 5. 5. Top-layer Top-layer state Figure state transition. transition.
4.2. Bottom-Layer FSM Design
4.2. Bottom-Layer FSM Driving scenarios forDesign driverless bus are relatively simple and bus usually travels in a fixed route.
scenarios for driverless bus relatively simple and bus usually travels indriving aroute. fixed(Q2), In this section, the drivingfor state set is designed as {structured driving (Q1), unstructured Driving scenarios driverless bus areare relatively simple and bus usually travels in a fixed route. In this section, the driving state set is designed as {structured driving (Q1), unstructured In this (Q3), section, the driving(Q4), state set is designed as {structured driving (Q1), unstructured driving (Q2), following overtaking buffer adjustment (Q5), and special path (Q6)}. The corresponding driving (Q2), following (Q3), overtaking (Q4), buffer adjustment (Q5), and special path (Q6)}. following (Q3), overtaking (Q4), buffer adjustment (Q5), and special path (Q6)}. The corresponding external trigger events are devised based on the above states, which mainly divided into the following The corresponding external trigger events areabove devised based onmainly the above states, mainly external trigger events are devised based on the states, which divided intowhich the following categories:
• •
divided into the following categories: categories:
E1/E2: The moving vehicle in front of the bus enters/disappears in detection area.
•• E1/E2: The E1/E2: Themoving movingvehicle vehiclein infront frontof ofthe thebus busenters/disappears enters/disappearsinindetection detectionarea. area. E3/E4: The lane information from perception is valid/invalid. •• E3/E4: The E3/E4: Thelane laneinformation informationfrom fromperception perceptionisisvalid/invalid. valid/invalid. • •E5: The bus enters the buffered road. E5: The road. • E5: The bus bus enters enters the the buffered buffered road. • •E6: The stationary obstacle in front of entersinindetection detection area. E6: The ofthe the bus bus enters enters area. • E6: The stationary stationary obstacle obstacle in in front front of the bus in detection area. • •E7: The process of overtaking completes. E7: The • E7: The process process of of overtaking overtaking completes. completes. • •E8: The bus enters the special E8: The bus enters the specialpath. path. • E8: The bus enters the special path. • •E9: The busbus finishes thethe special E9: The finishes specialpath. path. • E9: The bus finishes the special path. • E10: The bus enters the area of destination. • E10: The bus enters the area of destination. • E10: The bus enters the area of destination. According to the statesand andthe thetrigger trigger events, events, the FSM possesses six states, as as According to the states thebottom-layer bottom-layer FSM possesses six states, According to the states and the trigger events, the bottom-layer FSM possesses six states, as shown shown in Figure 6. our In our system,different differentbehaviors behaviors also thethe local reference points which shown in Figure 6. In system, alsodetermine determine local reference points which in used Figure In our system, different behaviors also determine the local reference points which is used is for6.trajectory generation. is used for trajectory generation. for trajectory generation. E5
Q3
E5
Q3 E2↓ E1↑
E2↓ E1↑ Mission start
Mission start
Q1
Q1 E4↓ E3↑
E4↓ E3↑ Q2
E5
Q5
E5
Q5
E6↓ E7↑
E10
E8↓
E6↓ E7↑ E9↑ Q4
E10
E9↑
Mission complete
Mission complete
E8↓ Q6
Q4 Q6 Q2 Figure 6. Bottom-layer state transition block diagram.
•
•
6.6.Bottom-layer state transition block diagram. Structured driving:Figure The bus drives in structured road and thediagram. lane recognition is valid. It is an Figure Bottom-layer state transition block initial state in the bottom-layer FSM. We mainly utilize the results of lane recognition in this bus drives in road and thelane lane recognition is generated valid. It is an stage to driving: guide theThe bus tobus drive in this stage. So the local optimal reference points Structured driving: The drives instructured structured road and the recognition is are valid. It is an •Structured by state thestate results of bottom-layer lane detection.FSM. initial in in the FSM.We We mainly utilize the results lane recognition in this initial the bottom-layer mainly utilize the results of laneof recognition in this stage
stage guide to drive instage. this stage. theoptimal local optimal are by generated to to guide thethe busbus to drive in this So the So local referencereference points arepoints generated the results of lane detection. by the results of lane detection.
Future Internet 2018, 10, x FOR PEER REVIEW
7 of 15
Future Internet 2018, 10, 51
• •
7 of 14
Following: It is a state that the autonomous bus can follow the vehicle ahead only when the lane detection is valid. The speed varies with the vehicle ahead. • Following: It is a state that the autonomous bus can follow the vehicle ahead only when the lane Overtaking: autonomous overtakes vehicle ahead which drives slowly (speed of the detectionThe is valid. The speedbus varies with thethe vehicle ahead. does notThe exceed 2 m/s) in this stage. It divided intowhich three drives steps: slowly (a) lane(speed change; •vehicle Overtaking: autonomous bus overtakes theisvehicle ahead of (b) overtake; (c) lane Safe2 m/s) spaceinisthis considered avoidinto collisions with(a)other vehicles. If the vehicle doesreturn. not exceed stage. It isto divided three steps: lane change; (c) lane return. Safe is considered to avoid collisions with other vehicles. d1 (b) dth ,overtake; the bus changes the lane ( dspace 1 represents the distance between two vehicles, d th is the
d1 < dth , the bus changes the lane (d1 represents the distance between two vehicles, dth is the safe Ifspace). At this time, the local reference points are shifted from the current lane to another safe space). At this time, the local reference points are shifted from the current lane to another
•
•
•
lane.lane. TheThe busbus willwill return the overtakesthe thevehicle vehicle and return thelane laneagain againuntil until it it overtakes and d >d1d . dth . 1
th
adjustment: It is intermediate thebus busneeds needstoto adjust speed especially when •Buffer Buffer adjustment: It an is an intermediatestage stage that that the adjust speed especially when the bus is around a ramp ororintersection. ofenter enterand andexit exit can obtained the bus is around a ramp intersection. The The signs signs of can be be obtained fromfrom the the global map. global map. •Special Special path: a stage afterbuffer bufferadjustment adjustment and with special terrain suchsuch as as path: It isItaisstage after andused usedtotodeal deal with special terrain intersections. The local reference points come from the special path database that is established in intersections. The local reference points come from the special path database that is established Section3.2. 3.2. in Section •Unstructured Unstructured driving: Lane linesfail failto tobe be identified identified by layer in this stage. A check driving: Lane lines byperception perception layer in this stage. A check zone is set manually and detect the threaten obstacles in this area. Then check whether a dangerous zone is set manually and detect the threaten obstacles in this area. Then check whether a obstacle exists in the check zone. For instance, in Figure 7, C1 is a threat for the bus while C2 dangerous obstacle exists in the check zone. For instance, in Figure 7, C1 is a threat for the bus is not.
C1
Check Zone
H
while C2 is not.
C2
Global reference points
Figure 7. Check zone in unstructured driving. Figure 7. Check zone in unstructured driving.
5. Optimal Trajectory Generation 5. Optimal Trajectory Generation The The global reference cannotprovide provide a feasible fortoa follow bus tosince follow they are global referencepoints points cannot a feasible path path for a bus they since are usually usually sparse and unconstrained. It is proved that the mobile vehicle’s trajectory generation sparse and unconstrained. It is proved that the mobile vehicle’s trajectory generation from start to endfrom solved using polynomial parameterizations To balance between computing complexity start can to be end canbybe solved by using polynomial [24]. parameterizations [24]. To balance between and constraints, cubic polynomial is applied to generate the optimal trajectory which is smooth and computing complexity and constraints, cubic polynomial is applied to generate the optimal trajectory consistent with theconsistent bus kineticwith constraints. which is smooth and the bus kinetic constraints. In Figure 8, l1 is the fitted global path based on global reference points, the point G ( x f , y f , θ ) is a G( x f , y f , ) In Figure 8, l1 is the fitted global path based on global reference points, the point target point that is selected in global reference points to determine the expression of cubic polynomial. is a θtarget is selected inrepresents global reference to determine expression cubic is the point global that desired heading, T the cubic points polynomial curve, l0 isthe parallel to the X of axis. The cubic polynomial can be written as l
polynomial. is the global desired heading, T represents the cubic polynomial curve, 0 is parallel to the X axis. The cubic polynomial asa3 x3 Y ( X ) = can a0 +be a1 xwritten + a2 x 2 + (6)
) = a0with + a1 xthe + acenter x the headstock. In the established (6) where the coordinate system XOYYis( Xfixed 2 x + a3of coordinate system, the initial position is coordinate origin and initial heading of the bus is zero, that is, where the coordinate system XOY is fixed with the center of the headstock. In the established the constraints are expressed as coordinate system, the initial position is coordinate and initial heading of the bus is zero, Y (0) =origin 0 (7) that 2
3
is, the constraints are expressed as
Y (0) = 0
(7)
Future Internet 2018, 10, x FOR PEER REVIEW
8 of 15
dY
Future Internet 2018, 10, 51
=0
dx
8 of 14
dY/dx = constant 0 combined with Equations (6)–(8), we can obtain the coefficients in (9).
(8)
(8)
combined with Equations (6)–(8), we can obtain the constant coefficients in (9).
a0 = a1 = 0
(9)
a0 = a1 = 0
(9)
Y
Global reference point
G ( x f , y f , )
yoff
set
d
l0
l1
T
O
X
Figure 8. Trajectory Trajectorygeneration. generation. Figure 8.
To ensure the globalreference reference points finally, the trajectory inGpoint G is tangent To ensure thebus bustototrack track the the global points finally, the trajectory in point is tangent to
.The first of cubic polynomial in the target G ( xpoint i.e., l1 . the Combined ( x fthe , y fslope , ) of to l1l1.The firstderivative derivative of cubic polynomial in thepoint target i.e., slope of f , y f , θ )G
l1 .
with Equations (6) and (9), the equation is expressed in (10) Combined with Equations (6) and (9), the equation is expressed in (10) 2 y0 (yx'f()x )== 2a 2a22xxf f ++ 3a 3a33xx ff 2 f = tan θ
(10)
= tan
(10)
Then, yo f f set (the offset of the target point) is written as (11).
Then,
yoffset
(the offset of the target point) is written as (11). yo f f set = y f cos θ + x f sin θ
(11)
yoffset = y f cos + x f sin
(11)
The target point ( x f , y f , θ ) is also a point on the cubic trajectory, so it satisfies the function
The target point
( x f , y f , )
2 3 trajectory, so it satisfies the function is also a point the cubic Y f = aon (12) 2 x f + a3 x f
combined with Equations (10)–(14) are obtained Y = aasx f
a2 =
2
2 f
3yo f f set
combined with Equations (10)–(14) are obtained asθ x f 2 cos
+ a3 x3f
−
(12)
4 tan θ xf
(13)
3 yoffset 2yo4f tan 3 tan θ f set a3a= − −3 2 = 22 xf θ x ff cos x f cos
(14) (13)
Therefore, the cubic polynomial is related to the target point G ( x f , y f , θ ), that is, different target 2 yoffset 3 tan points determine different trajectories. aFor instance, the values of x f and y f are fixed but (14) − when 3 = 2 x f in Figure x f 3 cos Trajectory clusters of different target θ ∈ (0 ∼ 45◦ ), the trajectory clusters are showed 9a. points are illustrated in Figure 9.
Therefore, the cubic polynomial is related to the target point
G( x f , y f , )
points determine different trajectories. For instance, when the values of
xf
, that is, different target and
yf
are fixed but
（0~45 ）, the trajectory clusters are showed in Figure 9a. Trajectory clusters of different target
points are illustrated in Figure 9.
Future Internet 2018, 10, x FOR PEER REVIEW
9 of 15
Future Internet 2018, 10, 51 10 Future Internet 2018, 10, x FOR PEER REVIEW
8 10
6
6 8
4
4
Y
8
6
6
2
2
4
4
Y
Y
Y
8 10
0
0
2
2
-2 0
-2 0
5
10
15
20
25
30
35
40
20
25
30
35
40
X
(a) -2
9 of 14 9 of 15
10
0
5
10
0
-2
15
0
5
10
15
0
5
10
15
20 X
25
30
35
40
25
30
35
40
(b) 20
X Figure 9. Trajectory clusters under different target points; (a) (30, 10, 0~45°);X (b) (20, 10, 0~60°).
(a)
(b)
Therefore, an appropriate point istarget essential optimal trajectory. Besides, several Figure clusterstarget under different different points;for (a) (30, (30, 10, 0~45 0~45°); (b) (20, (20, 10, 10, 0~60°). ◦ ); (b) ◦ ). Figure 9. 9. Trajectory Trajectory clusters under target points; (a) 10, 0~60 constraints should be satisfied. On one hand, the maximum steering angle of the front wheel is limited for bus. an Theappropriate bus can travel through theessential target point with maximum front wheelseveral angle. Therefore, target point is for optimal trajectory. Besides, Therefore, an appropriate target point is essential for optimal trajectory. Besides, several According Figurebe 10,satisfied. turning radius R can be the written as (15). steering angle of the front wheel is constraintstoshould On one hand, maximum constraints should be satisfied. On one hand, the maximum steering angle of the front wheel is limited limited for bus. The bus can travel through the target point with maximum front wheel angle. for bus. The bus can travel through the target point with front wheel angle. According to d / sin f asmaximum According to Figure 10, turning radius R canRbe= written (15). (15) Figure 10, turning radius R can be written as (15). where
Rg [
f
R== dd// sin (15) sin δf f (15) is front wheel angle and d meansRthe wheelbase. For the bus, the range of the front wheel
,
]
f max wheel angle and d means the wheelbase. For the bus, the range of the front wheel where δffminis front . From (15), the minimum turning radius is obtained by (16). where f is front wheel angle and d means the wheelbase. For the bus, the range of the front wheel Rg ∈ [δ f min , δ f max ]. From (15), the minimum turning radius is obtained by (16).
Rg [ f min , f max ]
Rmin = d / sin f max is obtained by (16). . From (15), the minimum turning radius Rmin = d/ sin δ f max = d / sin f max
f max is the maximum angle of front min where δf max where is the maximum angle of frontwheel. wheel.
R
where
f max
(16) (16) (16)
is the maximum angle of front wheel.
d d
f
f
R R
f
O
f wheel. Figure 10. Steering kinematics mode front
Figure 10. Steering kinematics mode front wheel.
O
On the other hand, the faster of bus travels, the mode longer of trajectory should be. In this paper, 10.the Steering kinematics front wheel. On the other hand, the Figure faster of the bus travels, the longer of trajectory should be. In this paper, a piecewise turning radius is defined by the equation a piecewise turning radius is defined by the equation On the other hand, the faster of the bus travels, the longer of trajectory should be. In this paper, Rmin v vth La the =( equation a piecewise turning radius is defined by (17) R Rmin + Kla v vv
