- Email: [email protected]
Cooperative vehicle path generation during merging using model predictive control with real-time optimization
Control Engineering Practice 34 (2015) 98–105
Contents lists available at ScienceDirect
Control Engineering Practice journal homepage: www.elsevier.com/locate/conengprac
Cooperative vehicle path generation during merging using model predictive control with real-time optimization Wenjing Cao a,n, Masakazu Mukai b, Taketoshi Kawabe c, Hikaru Nishira d, Noriaki Fujiki d a
Powertrain Engineering Division 1, Nissan Motor Co., Ltd., 560-2 Okatsukoku, Atsugi, Kanagawa 243-0192, Japan Department of Electrical Engineering, Faculty of Engineering, Kogakuin University, 2-24-1 Nishishinjuku, Shinjuku-ku, Tokyo 163-8677, Japan c Department of Automotive Science, Graduate School of Integrated Frontier Sciences, Kyushu University, 744 Motooka, Nishi-ku, Fukuoka 819-0395, Japan d Nissan Research Center, Nissan Motor Co., Ltd., 1-1 Morinosatoaoyama, Atsugi, Kanagawa 243-0123, Japan b
art ic l e i nf o
a b s t r a c t
Article history: Received 15 October 2013 Accepted 15 October 2014
This paper proposed a cooperative merging path generation method for vehicles to merge smoothly on the motorway using a Model Predictive Control (MPC) scheme which optimizes the motions of the relevant vehicles simultaneously. The cooperative merging is a merging in where the most relevant vehicle in the main lane would accelerate or decelerate slightly to let the merging vehicle merge in easily. The proposed path generation algorithm can generate the merging path ensuring the merging vehicle can access the whole acceleration area, and do not exceed it. We have introduced a state variable to the optimization problem by which the merging point for the merging vehicle is optimized. The simulation results showed that the cooperative merging path can be successfully generated under some typical traffic situations without re-adjustment of the optimization parameters. & 2014 Elsevier Ltd. All rights reserved.
Keywords: Automotive control Cooperative merging Model predictive control (MPC) Optimal control Path generation
1. Introduction With the advent of vehicle-to-vehicle (V2V), vehicle sensors for advanced driver assistance (ADAS) and by-wire vehicle controls, it is now possible to conceive of fully autonomous driving systems. One problem of interest is the merging of a vehicle into another lane which is filled with traffic. This is an important issue because of the following factors: merging is one of the difficult maneuvers. In particular, nonsmooth merging would cause congestion in the merging section (Papageorgiou, Papamichail, Spiliopoulou, & Lentzakis, 2008). In addition, the increase of vehicles in the slow lanes of motorways would increase the mental workload of the drivers of the vehicles in the merging lane (De Waard, Kruizinga, & Brookhuis, 2008). Elderly drivers will also increase and they will keep on driving till older ages than before (Waller, 1991; Wood, 2002). It has been shown that, it is more demanding for elderly drivers to merge into traffic than for young drivers (De Waard, Dijksterhuis, & Broohuis, 2009). De Waard et al. (2009) indicated that both an in-car support system and an extended acceleration lane will be helpful.
n
Corresponding author: Tel.: þ 81 50 3789 7944; fax: þ81 46 282 8887. E-mail addresses: [email protected] (W. Cao), [email protected] (M. Mukai), [email protected] (T. Kawabe), [email protected] (H. Nishira), [email protected] (N. Fujiki). http://dx.doi.org/10.1016/j.conengprac.2014.10.005 0967-0661/& 2014 Elsevier Ltd. All rights reserved.
Athans considered the problem of merging strings of vehicles into a single lane as a linear optimal regulator problem (Athans, 1969). Milanés proposed a control algorithm to decide the best time for the merging vehicle to enter the main road and validated this method both in simulations and experiments (Milanés, Godoy, Villagrá, & Perez, 2011). Lu proposed a longitudinal control problem and established a unified model for different road layouts (Lu & Hedrick, 2003). Kachroo developed a merging control system, in which both a longitudinal controller and a lateral controller were included (Kachroo & Li, 1997). Recently, Hidas (2005) has introduced the concept of cooperative merging. In the cooperative merging the most relevant vehicle in the main lane would accelerate or decelerate slightly to let the merging vehicle merge in easily. The cooperative merging phenomenon has never been considered in the above previous researches about automated merging and is the focus of this paper. To merge cooperatively and smoothly, a merging path generation method based on Model Predictive Control (MPC) scheme is proposed. The merging problem is formulated into an optimization problem to optimize the motions of the relevant vehicles simultaneously. A state variable related to the merging path is introduced to the optimization problem, so that the merging path and the merging point of the merging vehicle can be optimized according to the motion of the main lane vehicle. An appropriate available moving area, which has a similar shape to the actual acceleration area, is designed to constrain the
W. Cao et al. / Control Engineering Practice 34 (2015) 98–105
movement of the merging vehicle. Using the state variable and the constraints of the motion of the merging vehicle, an optimization problem is formulated. As a result, the merging vehicle would move on the centerline of the merging lane before merging, modify its merging path in consideration of the motion of the main lane vehicle, merge at the optimal merging point, and move on the centerline of the main lane after merging. The upper and lower bounds of the accelerations of the relevant vehicles are constrained, so that the relevant vehicles can move smoothly. Three traffic scenes are computer-simulated to validate the effectiveness of the proposed method. The simulation results showed that the proposed method can generate cooperative and mild paths for the relevant vehicles in some typical situations without re-adjustment of the optimization parameters. This paper is organized as follows: The formulation of the optimization problem used to solve the merging problem and the process of the MPC method are described in Section 2. In Section 3 we present the simulation results and analysis. And then, two possible applications of the proposed method are discussed. Section 4 contains some conclusions and some ideas of the future works.
2. Cooperative merging path generation method using MPC with real-time optimization We consider merging of two vehicles here for simplicity (see Fig. 1). One vehicle is a merging vehicle running on the merging lane; the other is a mane lane vehicle running on the main lane. We assume that the positions and the velocities of the vehicle are measured by a sensor system installed in the road side. The control inputs for the vehicles are calculated by a computer on the road side and sent to the vehicles by some communication devices. Another possible implementation is that the positions and velocities are sent to the vehicles and calculated by on-board computers installed in the vehicles using the same algorithm on the computers. The merging vehicle and the main lane vehicle are called as the relevant vehicles of the merging problem.
99
merging is considered to start; the velocity of Vehicle2 is chosen as the X-axis. As a result the expressions of l1, l2, and l3 are as (1), (2), and (3). The definitions of the symbols in these equations are shown in Table 1. l1 : y ¼ 0;
ð1Þ
l2 : y ¼ kðx βÞ;
l3 : y ¼
y Z 0;
ð2Þ
k α1=2 ðx βÞ ðx βÞ2 : 2 k
ð3Þ
2.1.2. Vehicle dynamics In this paper Vehicle1 and Vehicle2 are simplified as two particles. Vehicle2 is assumed to run on l1 during the merging maneuver. We assume that if no vehicle exists on the main lane, the path of Vehicle1 coincides with l3. However, in actual merging with main lane vehicles, the merging vehicle may depart from the centerline of the lane in the merging section. To generate an adjustable merging path, we introduce a variable b into (3). b will be employed as a state variable x in (5). The x-coy-coordinate of Vehicle1 is denoted as x1x, and the y-coordinate of it is denoted as x1y in this paper. As a result the relationship of x1x and x1y turns out to be x1y ¼
k α1=2 ðx1x β bÞ ðx1x β bÞ2 : 2 k
ð4Þ
x1y depends on x1x and b. b can vary the value of x1y independently of x1x. Therefore, the merging path can be modified using b. For example an effect of b is shown in Fig. 3. Although b is a variable, b is set as a constant in Fig. 3. When b¼0, the merging path coincides the line l3. The positive value of b shifts the merging path to the right of l3. The negative value of b shifts the merging path to the left of l3.
2.1. Modeling of the road and the vehicles 2.1.1. Road model To optimize the paths of the relevant vehicles under some constraints, we used MPC to solve the merging problem. The formulation of the optimization problem and the MPC scheme will be described in the following subsections. To design paths for the relevant vehicles according to the road shape, the merging lane and the main lane have to be modeled. Just as in paper (Cao, Mukai, Kawabe, Nishira, & Fujiki, 2013c), in consideration of simplicity, the centerline of the main lane and the centerline of the merging lane are modeled as l1, and l2 in Fig. 2. l3 is a smooth line which converges to l2 on the left side and converges to l1 on the right side. In Fig. 2 and the rest parts of this paper, we denote the merging vehicle as “Vehicle1” and denote the main lane vehicle as “Vehicle2”. The positions of Vehicle1 is plotted as “◯” and Vehicle2 is plotted as “ ”. To get the mathematical expressions of the lines, the coy-coordinate system is set as follows: the origin is set at the position of Vehicle2 when the
Fig. 1. The situation of the merging problem in this paper.
Fig. 2. Approximation method of the road.
Table 1 Definitions of the symbols included in l1, l2, l3. Symbols
Definitions
x y k β α
The The The The The The
x-coy-coordinate y-coordinate slope of l2 interception of l2 on the Y-axis design parameter which determines smoothness of the curvature
Fig. 3. The effect of the variable b (constant case).
100
W. Cao et al. / Control Engineering Practice 34 (2015) 98–105
By approximating the relevant vehicles with the particle approximation method, we obtain the state space equation as follows: 2 3 2 0 3 v1x 7 6v 7 6 0 7 6 2x 7 6 7 6 7 6 0 7 6 vb 7 6 6 7 6 7 ; ð5Þ x_ ¼ 6 7 þ 6 a1x 7 7 6 0 7 6 6 7 6 7 6 7 4 0 5 4 a2x 5 ab 0 where z ¼ ½x1x x2x b v1x v2x vb T ; a ¼ ½a1x a2x ab T :
ð6Þ ð7Þ
Here, z is the state vector and a is the control input of the system. They are expressed as (6) and (7). x1x is the x-coordinates of Vehicle1 and x2x is the x-coordinates of Vehicle2. v1x and v2x are the velocities of Vehicle1 and Vehicle2 in the X-axis direction, while a1x and a2x are the accelerations. vb is the differential of b, while the differential of vb is noted as ab. ab would influence the acceleration of Vehicle1 in the Y-axis direction. Vehicle2 has a2x as the control input in X-axis direction only, so it will always move on l1 during merging. The acceleration of Vehicle1 in the Y-axis, a1y, can be calculated as the second time derivative of x1y along the trajectories of (5). 2.2. Formulation of the optimization problem To obtain a standard merging for drivers the control method has to fulfill the following requirements. (a) The relative distances between the merging vehicle and the main lane vehicle must be kept above an appropriate value to merge safely. (b) The merging vehicle should move on the centerline of the road before and after the merging maneuver. It should also be able to access the whole acceleration area and not depart from the lane. (c) Vehicles should run at their desired speed. (d) The accelerations and decelerations should be as smooth as possible. The relevant vehicles should not accelerate or decelerate frequently. To meet the above requirements, a penalty function is created for each requirement. For requirement (a): Relative distance between the two vehicles is kept above r min with the term Lr. The expression of Lr is as (8). In (8) r is the relative distance between Vehicle1 and Vehicle2 in the two-dimensional space. It is calculated with (9). Lr ¼
1 ; ðr 2 r 2min Þ
r ¼ ððx1x x2x Þ2 þ ðx1y 0Þ2 Þ1=2 :
ð8Þ
appropriately, the available moving area can be modified to have a similar shape to an actual acceleration area. Fig. 4 shows the shapes of l4 and l5. They should converge to l2 on the left side, converge to l1 on the right side, and form a similar shape to the acceleration area around the merging intersection. In this paper, to express the boundary of the available moving area with barrier functions, (4) is extent to obtain the expressions of l4 and l5. They are expressed as (10) and (11) in the x y plane. k α1=2 l4 : y ¼ x β bmax f 1 ðxÞ ðx β bmax f 1 ðxÞÞ2 ; ð10Þ 2 k l5 : y ¼
k α1=2 x β bmin f 2 ðxÞ ðx β bmin f 2 ðxÞÞ2 ; 2 k
ð11Þ
where bmin and bmax are parameters related to the size of the acceleration area. The expression of f1 and f2 in (10) and (11) is as (12). The definitions of the parameters in (12) are shown in Table 2. The relationship between the value of the parameters and the shape of f1 and f2 is shown in Fig. 5. 1 f i ðxÞ ¼ kbi 1 þ expð αbi ðx βbli ÞÞ 1 1 ; i ¼ 1; 2; ð12Þ þ 1 þ expðαbi ðx β bri ÞÞ where βbl1 and βbl2 are related to the shape of the entrance of the acceleration area. βbr1 and βbr2 are related to the shape of the exit of the acceleration area. The other parameters are related to the shape of the available moving area. If the value of b becomes too large or too small during the merging, Vehicle1 would run out of the range of the road. To ensure the path of Vehicle1 does not exceed the boundaries of the available moving area, Lbl and Lbr are included in the penalty function. Since (4), (10), and (11) have the same constructions, the requirement of keeping Vehicle1 moving within the boundaries can be ensured by Lbl and Lbr expressed as (13) and (14). Lbl ensures that Vehicle1 does
Fig. 4. Shape of l4 and l5.
Table 2 Definitions of the parameters in fi(x) ði ¼ 1; 2Þ. Parameters
Meanings
kbi αbi βbli
Maximum value of fi(x) Parameter which decide the steepness of shift part x-coy-coordinate of the point around which the function shift from 0 to kbi x-coy-coordinate of the point around which the function shift from kbi to 0
βbri
ð9Þ
rmin is the safe margin of the relative distance between Vehicle1 and Vehicle2. With this term included in the penalty function, the optimum control input is calculated under the condition that the relative distance r is kept larger than rmin when the initial value of r for optimizing calculation is larger than rmin and the optimization problem is feasible. When the initial value of r for the calculation is smaller than rmin by some reasons, e.g. calculation/discretizing error of the control input, the term is expected to work so that r is expanded to rmin. For requirement (b): To bound an available moving area for Vehicle1, l4 and l5 are introduced. With parameters set
Fig. 5. Relationship between the parameters and the shape of f i ðxÞði ¼ 1; 2Þ.
W. Cao et al. / Control Engineering Practice 34 (2015) 98–105
not exceed l5. Lbr makes Vehicle1 do not exceed l4. Lbl ¼ lnðb bmin f 1 ðx1x ÞÞ;
ð13Þ
Lbr ¼ lnðbmax f 2 ðx1x Þ bÞ:
ð14Þ
For requirement (c): To make vehicles run as closely as possible at their desired speed, terms Lv1 and Lv2 expressed as (15) and (16) are included. Lv1 ¼ ðv1x v1d Þ2 ;
ð15Þ
Lv2 ¼ ðv2x v2d Þ2 ;
ð16Þ
where v1d and v2d are the desired velocities of Vehicle1 and Vehicle2. For requirement (d): La1 and La2 are included to avoid abrupt and severe accelerations in the X-axis direction. ab would influence the acceleration of Vehicle1 in the Y-axis direction, so Lab is included to avoid abrupt and severe acceleration of Vehicle1 along Y-axis. The penalty functions are La1 ¼ a21x ;
La2 ¼ a22x ;
Lab ¼ a2b :
ð17Þ
The overall penalty function (18) is established as to make the relevant vehicles meet these requirements. LðzðτÞ; aðτÞÞ ¼ ωr Lr ωbl Lbl ωbr Lbr þ ωv1 Lv1 þ ωv2 Lv2 þ ωa1 La1 þ ωa2 La2 þ ωab Lab :
ð18Þ
where ωr, ωbl, ωbr, ωv1, ωv2, ωa1, ωa2, and ωab are weights for each term. The merging trajectories can be determined by solving the optimization problem (19). J is expressed as (20), in which t is the current time, T is the prediction horizon, LðzðτÞ; aðτÞÞ is a penalty function, and τ is the virtual time for prediction. The optimization problem (19) is defined to minimize the performance index J subject to the state space equation (5) and the acceleration constraints (21). a1x, a2x, and ab are optimized simultaneously which leads to cooperative maneuvers of Vehicle1 and Vehicle2. ð19Þ
min J: a
Z J¼
tþT t
LðzðτÞ; aðτÞÞ dτ :
8 > < a1xmin r a1x r a1xmax a2xmin r a2x r a2xmax > : a ra r a : bmin
b
ð20Þ
ð21Þ
bmax
Here a1xmin , a2xmin , and abmin are the lower boundaries of a1x, a2x, and ab. a1xmax , a2xmax , and abmax are the upper boundaries of them. These acceleration constraints are established to make the relevant vehicles move smoothly. The acceleration of Vehicle1 in the Y-axis, a1y, can be observed during merging.
optimal input vectors at time step i and T is the prediction horizon used in (20). Step 3 : Update the input aðtÞ≔aτ . Step 4 : Set t≔t þ h, and go back to Step 2. With a solver which is fast enough, the process of MPC can be executed in real-time. C/GMRES method proposed by Ohtsuka (2004) can be used for the execution. The C/GMRES method can solve a nonlinear model predictive control problem within an acceptable computational time. Since our problem includes a nonlinear penalty function, the C/GMERES method is employed in the following section. 3. Computer simulation and analysis To validate the effectiveness of the proposed method, computer simulations of three traffic scenes were conducted. It was assumed that data used to update the state vector were obtained without any noises or delay, and the two relevant vehicles always followed the optimal inputs without any errors. As a real-time optimization method, the C/GMRES method, proposed by Ohtsuka (2004), was chosen to solve the problem. The simulation was conducted on a personal computer (CPU: Core™i5, 2.50 GHz). In order to obtain a typical merging, we recorded traffic scenes of merging from a helicopter in Fukuoka, Japan. We analyzed the time history of the data, and obtained the initial conditions of the merging maneuver and the paths of the relevant vehicles. A traffic scene was chosen as a representative of cooperative merging. The time history of this merging showed that the main lane vehicle decelerated to let the merging vehicle merge in more easily. The scene was compared with the computer simulation results. The beginning of the merging is shown in Fig. 6. The parameters depend on the road shape: α, k, β, bmin, bmax, kb1, αb1, βblef1, βbrig1, kb2, kb2, αb2, βblef2, βbrig2 have to design for each intersection on the road. The parameters were designed using a picture shown in Fig. 6. The limitation of the accelerations: a1xmin , a2xmin , abmin, a1xmax , a2xmax , and abmax were set to be similar to the acceleration limitation of these of adoptive cruise control devices. The weights: ωi, i ¼ 1; …7 are manually turning parameter depending on the shape of the road aiming for smooth motion of the vehicles. The weights can be recorded on a road side system and sent to the vehicles when they are close to the intersection. The typical initial merging conditions are showed as follows: x1x ¼ 59 m, x2x ¼ 0 m, b ¼ 0 m, v1x ¼ 9:9 m=s, v2x ¼ 20 m=s, a1x ¼ 0 m=s2 , a2x ¼ 0 m=s2 . To reduce the accelerations, α was set to be 40. We set the other parameters in the expressions of l1, l2, and l3 according to the road shape as follows: k ¼ 0:156, β ¼ 159:8. The acceleration area was successfully bounded by l4 and l5 using parameters: bmin ¼ 13:07, bmax ¼ 80:10, kb1 ¼ 2:15, αb1 ¼ 0:05, βblef 1 ¼ 140, βbrig1 ¼ 180, kb2 ¼ 1, αb2 ¼ 0:05, βblef 2 ¼ 180,
2.3. MPC with real-time optimization The process of MPC was described in paper (Cao, Mukai, & Kawabe, 2013a). MPC is a control method in which the current control input is obtained by solving a finite horizon optimal control problem. Using the current measured state the initial state in the optimal control problem the optimal input sequence is calculated at each sampling instant. The first element of the sequence is applied to the system. The optimization problem is discretized with sampling period h. The process of MPC can be described as follows: Step 1: Set τ : ¼ t, measure z(t), and set zðτ Þ≔zðtÞ. Step 2: Solve the optimization problem to get the optimal input vectors aτ ; aτ þ h ; aτ þ 2h ; …; aτ þ T h which can minimize the performance index J. Here ai is the discretized time
101
Fig. 6. The typical merging scene shot on a helicopter.
Fig. 7. Approximation result of the road shape.
102
W. Cao et al. / Control Engineering Practice 34 (2015) 98–105
βbrig2 ¼ 840. For mild merging, the lower and upper bounds of the
accelerations were set as: a1xmin ¼ a2xmin ¼ abmin ¼ 3 m=s2 , a1xmax ¼ a2xmax ¼ abmax ¼ 3 m=s2 . The model of the road is shown in Fig. 7. Weights in the penalty functions were set so that simulation results would be similar to actual merging results: ω1 ¼ 15:0, ω2 ¼ 0:001, ω3 ¼ 0:0011, ω4 ¼ 0:004, ω5 ¼ 0:004, ω6 ¼ 0:024, ω7 ¼ 0:0241, ω8 ¼ 0:001. According to the traffic condition, r min was set as 14 m, and v1d and v2d were set to be 16:7 m=s. The sampling time h was set as 0.01 s. The prediction horizon T was set as 3 s. The positions of the two vehicles at t i ¼ 2i s; ði ¼ 0; 1; 2; …; 9Þ, were plotted in the generated merging paths. The unit for all the speeds is [m/s]. The unit for the accelerations is [m/s2]. The unit for b and r is [m]. To verify that the merging vehicle was kept in the available moving area, boundaries of the available moving area, l4 and l5 were also shown in the figures of the generated merging paths. To ensure there were no excessive accelerations, the acceleration in Y-axis direction of Vehicle1, a1y, was also calculated and shown in the simulation results.
3.1. Case 1: The cooperative merging Cao et al. (2013a) As a direct comparison with the actual merging, the initial conditions of this case were set to be exactly the same with the initial conditions of the representative cooperative merging. The actual merging path is shown in Fig. 8 and the generated merging path is shown in Fig. 9. The time histories of variables during the simulation are shown in Fig. 10. From Fig. 9 it can be known that, just as the actual motions shown in Fig. 8, Vehicle1 merged successfully to the front of Vehicle2 and kept an appropriate distance with Vehicle2. This means that the relative positions of the two vehicles in the simulation results are consistent with the actual merging result. The cooperative merging phenomenon was seen in the simulation results. It can also be seen from Fig. 10 that the Vehicle2 decelerated a little to let Vehicle1 merge in more easily. To show the modification of the merging path clearly, the zoomed merging path is shown in Fig. 11. As expected to keep relative distance, Vehicle1 ran apart from l3, but always ran inside the available moving area bounded by l4 and l5. As shown in Fig. 10, during the merging maneuver all accelerations were kept between 3 m=s2 and 3 m=s2 . The relative distance was always kept above r min ¼ 14 m. This means the corresponding constraints were not violated. The vehicles' speeds tended to the desired value, 16.7 m/s, during and after merging. As a comparison, the simulation of this case is also conducted with the method proposed in Cao, Mukai, Kawabe, Nishira, and Fujiki (2013b), in which b was fixed at 0. The time histories of the variables are shown in Fig. 12.
Fig. 10. Time histories of main variables during merging in Case 1.
Fig. 11. Zoomed generated paths of Case 1.
Fig. 8. Merging path of the actual merging scene.
Fig. 9. Generated paths of Case 1.
Fig. 12. Time histories of main variables in Case 1 when the merging path is fixed.
W. Cao et al. / Control Engineering Practice 34 (2015) 98–105
As can be seen, the variation of the accelerations in Fig. 10 was milder than that in Fig. 12, while the relative distance was almost the same. The merging vehicle moved to the right of l3. This motion increased the relative distance between the relevant vehicles. This is considered to be the benefit of the introduction of the state variable b.
3.2. Case 2: Relaxed initial conditions which motivate the main lane vehicle to be the leading vehicle–verification of the universality of the weights To investigate whether the proposed method can generate realistic paths without adjustment of parameters in the performance index for the vehicles when the main lane vehicle should become the leading vehicle, we changed the initial conditions as: x1x ¼ 40 m, x2x ¼ 40 m, b ¼ 0 m, v1x ¼ 9:9 m=s, v2x ¼ 20 m=s, a1x ¼ 0 m=s2 , a2x ¼ 0 m=s2 . Since x1x ¼ x2x , v1x o v2x , a1x ¼ a2x ¼ 0, normally, drivers of a merging vehicle would merge to the behind of the main lane vehicle to avoid excessive accelerations and unnecessary risks. The generated paths in Fig. 13 show that Vehicle1 merged behind of Vehicle2 and kept an appropriate distance with Vehicle2. In this case, all constrains and barriers were kept in the simulation too. The simulation results show the expected results.
Fig. 13. Generated paths of Case 2.
103
3.3. Case 3: Collision case In this case the initial conditions were set as: x1x ¼ 0 m, x2x ¼ 0 m, b ¼ 0 m, v1x ¼ 9:9 m=s, v2x ¼ 20 m=s, a1x ¼ 0 m=s2 , a2x ¼ 0 m=s2 . It is obvious that x1x ¼ x2x , v1x ¼ v2x , a1x ¼ a2x ¼ 0. Therefore without control, the motions in the X-axis direction of Vehicle1 and Vehicle2 would be exactly the same, and they would collide with each other at the intersection point. The optimization was able to avoid a collision. Fig. 15 shows that Vehicle1 decelerated a little to let Vehicle2 go ahead. The generated paths in Fig. 14 show that Vehicle1 merged behind Vehicle2 and followed it with an appropriate relative distance. The variation of the variables in Fig. 15 shows that Vehicle1 and Vehicle2 kept the constraints. The relative distance was always kept above r min ¼ 14 m. In this case, decrease of a1x and increase of a2x after t¼ 2 s were cooperatively generated to avoid collision. They worked so well that b did not have to be changed actively to generate a smooth merging trajectory, and therefore Vehicle1 did not move significantly away from the centerline in this case.
3.4. The influence of noise A real control system will have imprecise sensors that may give a range of estimates for velocity and position of vehicles. To test the influence of noise 0.02 m random noise for position sensors of Vehicle1 and Vehicle2 and 0.02 m/s random noise for velocity sensors of Vehicle1 and Vehicle2 are added to the simulation. In this case the initial conditions were set as: x1x ¼ 50 m, x2x ¼ 0 m, b ¼ 0 m, v1x ¼ 16:7 m=s, v2x ¼ 16:7 m=s, a1x ¼ 0 m=s2 , a2x ¼ 0 m=s2 . The prediction horizon is 6 s. Fig. 16 and Fig. 17 show that merging succeeds without collision for this magnitude of noise. However if the amplitude of the noise
Fig. 14. Generated paths of Case 3. Fig. 16. Generated paths of the case with the noises.
Fig. 15. Time histories of main variables during merging in Case 3.
Fig. 17. Time histories of main variables during merging in the case with the noises.
104
W. Cao et al. / Control Engineering Practice 34 (2015) 98–105
Fig. 18. Distribution of the given initial conditions (500 initial conditions).
was grater, calculation of the control input became impossible. The main factor that made calculation impossible was that the augments of logarithm functions used in Eqs. (13) and (14) became negative because of the noise. This issue may be solved by replacing the equations with soft constrains conditions. 3.5. Tests for the dependence of the initial condition To investigate the proposed method for more merging cases, 500 initial conditions were tested. rmin was set to be 5 m in this simulation. The initial positions and the velocities of vehicles are shown in Fig. 18. These initial conditions were generated by using a random function in MATLAB within appropriate boundaries. The boundaries were set so that realistic situations were simulated. The initial conditions of Vehicle1 were distributed larger than those of Vehicle2, considering that the initial speed and the position of the merging vehicle are more likely to be affected by the shape of the merging lane than that of the main lane vehicle. Fig. 18 shows a histogram of the minimum relative distances occurred in the simulation. From Fig. 19 it is observed that the all cases of merging were succeeded and the minimum distance is never less than 10 m in all cases. 3.6. Discussion of application Two possible applications can be considered for the proposed method. In one application, both the optimal inputs for the merging vehicle and the main lane vehicle are used for the motion control of both vehicles. In this case the cooperative merging phenomenon can be reproduced, and the relevant vehicles can cooperate with each other, traveling on smooth paths while completing the merge. In the other application, only the merging vehicle is controlled. The optimal input for the merging vehicle is used for its motion control while the optimal input for the main lane vehicle is only used for the prediction of its motion. Since the state vector and the optimal inputs are updated every time step, the discrepancy between the predicted motion of the main lane vehicle and its
Fig. 19. The histogram of the minimum relative distances.
actual motion can be taken into account and the control input for Vehicle1 will be modified owing to the update. Furthermore, large value of the weight for the acceleration of vehicle2 reduces the amplitude of the optimal acceleration /deceleration of Vehicle2. The behavior of vehicle2 generated in this manner is little affected by the motion of Vehicle1, as if Vehicle2 was not cooperatively driven ignoring existence of Vehicle1. In other words, the proposed method can be used in the case when the main lane vehicle does not consider the merging vehicle. In this way, as long as the merging vehicle moves appropriately, there will not be any accident and the main lane vehicle would not have to accelerate excessively. As a result, in this way, the merging vehicle can merge successfully without forcing the main lane vehicle to produce unacceptable accelerations. In this application, with given initial conditions, motions of the vehicles involved in a merging can be forecasted. Therefore, it can
W. Cao et al. / Control Engineering Practice 34 (2015) 98–105
also be used as a description of the cooperative merging phenomenon and modeling of the merging maneuver.
4. Conclusions and future works 4.1. Conclusions In this paper a cooperative merging path generation method for cooperative and mild merging based on MPC is proposed. In this method the merging problem is formulated as an optimization problem. A state variable that modifies the merging trajectory is introduced to the optimization problem, by which the merging point of the merging vehicle is optimized according to the motion of the main lane vehicle. An available moving area is designed for the merging vehicle to restrict the movement of it. As a result, the merging vehicle would run on the centerline of the lanes before and after merging, adjusting its path, and merge at the optimal merging point. Computer simulations are conducted to validate the effectiveness of the proposed method. To be realistic, the initial conditions and parameters in the simulation were set according to a traffic scene recorded from a helicopter. The results proved that, as long as the initial conditions were reasonable, the proposed method can generate cooperative merging path. 4.2. Future works In the future the effectiveness of the proposed method in the case when there are multiple vehicles on the main lane will be investigated. Whether the merging vehicle can find the optimal gap will be researched. Motions of the main lane vehicles will also be observed in that case. We will also research the robustness of
105
the proposed method against unexpected motions of the main lane vehicle and disturbances. References Athans, M. (1969). A unified approach to the vehicle-merging problem. Transportation Research, 3, 123–133. Cao, W., Mukai, M., & Kawabe, T. (2013a). Two-dimensional merging path generation using model predictive control. Artificial Life and Robotics, 17, 350–356. Cao, W., Mukai, M., Kawabe, T., Nishira, H., & Fujiki, N. (2013b). Automotive longitudinal speed pattern generation with acceleration constraints aiming at mild merging using model predictive control method. In 2013 9th Asian control conference (ASCC) (pp. 1–6). Istanbul, Turkey. Cao, W., Mukai, M., Kawabe, T., Nishira, H., & Fujiki, N. (2013c). Mild merging path generation method with optimal merging point based on mpc. In T. Kawabe (Ed.), 7th IFAC symposium on advances in automotive control 7 (pp. 756–761). Tokyo, Japan: National Olympics Memorial Youth Center. De Waard, D., Dijksterhuis, C., & Broohuis, K. A. (2009). Merging into heavy motorway traffic by young and elderly drivers, accident analysis and prevention. Accident Analysis and Prevention, 41, 588–597. De Waard, D., Kruizinga, A., & Brookhuis, K. A. (2008). The consequences of an increase in heavy goods vehicles for passenger car drivers' mental workload and behavior: A simulator study. Accident Analysis and Prevention, 40, 818–828. Hidas, P. (2005). Modelling vehicle interactions in microscopic simulation of merging and weaving. Transportation Research Part C, 13, 37–62. Kachroo, P., & Li, Z. (1997). Vehicle merging control design for an automated highway system. In IEEE conference on ITSC'97 (pp. 224–229). Boston, MA. Lu, X. Y., & Hedrick, J. K. (2003). Longitudinal control algorithm for automated vehicle merging. International Journal of Control, 76, 193–202. Milanés, V., Godoy, J., Villagrá, J., & Perez, J. (2011). Automated on-ramp merging system for congested traffic situations. IEEE Transactions on Intelligent Transportation Systems, 12, 500–508. Ohtsuka, T. (2004). A continuation/GMRES method for fast computation of nonlinear receding horizon control. Automatica, 40, 563–574. Papageorgiou, M., Papamichail, I., Spiliopoulou, A. D., & Lentzakis, A. (2008). Realtime merging traffic control with applications to toll plaza and work zone management. Transporation Research Part C, 16, 535–553. Waller, P. F. (1991). The older driver. Human Factors, 33, 499–505. Wood, J. M. (2002). Age and visual impairment decrease driving performance as measured on a closed-road circuit. Human Factors, 44, 482–494.
Contents lists available at ScienceDirect
Control Engineering Practice journal homepage: www.elsevier.com/locate/conengprac
Cooperative vehicle path generation during merging using model predictive control with real-time optimization Wenjing Cao a,n, Masakazu Mukai b, Taketoshi Kawabe c, Hikaru Nishira d, Noriaki Fujiki d a
Powertrain Engineering Division 1, Nissan Motor Co., Ltd., 560-2 Okatsukoku, Atsugi, Kanagawa 243-0192, Japan Department of Electrical Engineering, Faculty of Engineering, Kogakuin University, 2-24-1 Nishishinjuku, Shinjuku-ku, Tokyo 163-8677, Japan c Department of Automotive Science, Graduate School of Integrated Frontier Sciences, Kyushu University, 744 Motooka, Nishi-ku, Fukuoka 819-0395, Japan d Nissan Research Center, Nissan Motor Co., Ltd., 1-1 Morinosatoaoyama, Atsugi, Kanagawa 243-0123, Japan b
art ic l e i nf o
a b s t r a c t
Article history: Received 15 October 2013 Accepted 15 October 2014
This paper proposed a cooperative merging path generation method for vehicles to merge smoothly on the motorway using a Model Predictive Control (MPC) scheme which optimizes the motions of the relevant vehicles simultaneously. The cooperative merging is a merging in where the most relevant vehicle in the main lane would accelerate or decelerate slightly to let the merging vehicle merge in easily. The proposed path generation algorithm can generate the merging path ensuring the merging vehicle can access the whole acceleration area, and do not exceed it. We have introduced a state variable to the optimization problem by which the merging point for the merging vehicle is optimized. The simulation results showed that the cooperative merging path can be successfully generated under some typical traffic situations without re-adjustment of the optimization parameters. & 2014 Elsevier Ltd. All rights reserved.
Keywords: Automotive control Cooperative merging Model predictive control (MPC) Optimal control Path generation
1. Introduction With the advent of vehicle-to-vehicle (V2V), vehicle sensors for advanced driver assistance (ADAS) and by-wire vehicle controls, it is now possible to conceive of fully autonomous driving systems. One problem of interest is the merging of a vehicle into another lane which is filled with traffic. This is an important issue because of the following factors: merging is one of the difficult maneuvers. In particular, nonsmooth merging would cause congestion in the merging section (Papageorgiou, Papamichail, Spiliopoulou, & Lentzakis, 2008). In addition, the increase of vehicles in the slow lanes of motorways would increase the mental workload of the drivers of the vehicles in the merging lane (De Waard, Kruizinga, & Brookhuis, 2008). Elderly drivers will also increase and they will keep on driving till older ages than before (Waller, 1991; Wood, 2002). It has been shown that, it is more demanding for elderly drivers to merge into traffic than for young drivers (De Waard, Dijksterhuis, & Broohuis, 2009). De Waard et al. (2009) indicated that both an in-car support system and an extended acceleration lane will be helpful.
n
Corresponding author: Tel.: þ 81 50 3789 7944; fax: þ81 46 282 8887. E-mail addresses: [email protected] (W. Cao), [email protected] (M. Mukai), [email protected] (T. Kawabe), [email protected] (H. Nishira), [email protected] (N. Fujiki). http://dx.doi.org/10.1016/j.conengprac.2014.10.005 0967-0661/& 2014 Elsevier Ltd. All rights reserved.
Athans considered the problem of merging strings of vehicles into a single lane as a linear optimal regulator problem (Athans, 1969). Milanés proposed a control algorithm to decide the best time for the merging vehicle to enter the main road and validated this method both in simulations and experiments (Milanés, Godoy, Villagrá, & Perez, 2011). Lu proposed a longitudinal control problem and established a unified model for different road layouts (Lu & Hedrick, 2003). Kachroo developed a merging control system, in which both a longitudinal controller and a lateral controller were included (Kachroo & Li, 1997). Recently, Hidas (2005) has introduced the concept of cooperative merging. In the cooperative merging the most relevant vehicle in the main lane would accelerate or decelerate slightly to let the merging vehicle merge in easily. The cooperative merging phenomenon has never been considered in the above previous researches about automated merging and is the focus of this paper. To merge cooperatively and smoothly, a merging path generation method based on Model Predictive Control (MPC) scheme is proposed. The merging problem is formulated into an optimization problem to optimize the motions of the relevant vehicles simultaneously. A state variable related to the merging path is introduced to the optimization problem, so that the merging path and the merging point of the merging vehicle can be optimized according to the motion of the main lane vehicle. An appropriate available moving area, which has a similar shape to the actual acceleration area, is designed to constrain the
W. Cao et al. / Control Engineering Practice 34 (2015) 98–105
movement of the merging vehicle. Using the state variable and the constraints of the motion of the merging vehicle, an optimization problem is formulated. As a result, the merging vehicle would move on the centerline of the merging lane before merging, modify its merging path in consideration of the motion of the main lane vehicle, merge at the optimal merging point, and move on the centerline of the main lane after merging. The upper and lower bounds of the accelerations of the relevant vehicles are constrained, so that the relevant vehicles can move smoothly. Three traffic scenes are computer-simulated to validate the effectiveness of the proposed method. The simulation results showed that the proposed method can generate cooperative and mild paths for the relevant vehicles in some typical situations without re-adjustment of the optimization parameters. This paper is organized as follows: The formulation of the optimization problem used to solve the merging problem and the process of the MPC method are described in Section 2. In Section 3 we present the simulation results and analysis. And then, two possible applications of the proposed method are discussed. Section 4 contains some conclusions and some ideas of the future works.
2. Cooperative merging path generation method using MPC with real-time optimization We consider merging of two vehicles here for simplicity (see Fig. 1). One vehicle is a merging vehicle running on the merging lane; the other is a mane lane vehicle running on the main lane. We assume that the positions and the velocities of the vehicle are measured by a sensor system installed in the road side. The control inputs for the vehicles are calculated by a computer on the road side and sent to the vehicles by some communication devices. Another possible implementation is that the positions and velocities are sent to the vehicles and calculated by on-board computers installed in the vehicles using the same algorithm on the computers. The merging vehicle and the main lane vehicle are called as the relevant vehicles of the merging problem.
99
merging is considered to start; the velocity of Vehicle2 is chosen as the X-axis. As a result the expressions of l1, l2, and l3 are as (1), (2), and (3). The definitions of the symbols in these equations are shown in Table 1. l1 : y ¼ 0;
ð1Þ
l2 : y ¼ kðx βÞ;
l3 : y ¼
y Z 0;
ð2Þ
k α1=2 ðx βÞ ðx βÞ2 : 2 k
ð3Þ
2.1.2. Vehicle dynamics In this paper Vehicle1 and Vehicle2 are simplified as two particles. Vehicle2 is assumed to run on l1 during the merging maneuver. We assume that if no vehicle exists on the main lane, the path of Vehicle1 coincides with l3. However, in actual merging with main lane vehicles, the merging vehicle may depart from the centerline of the lane in the merging section. To generate an adjustable merging path, we introduce a variable b into (3). b will be employed as a state variable x in (5). The x-coy-coordinate of Vehicle1 is denoted as x1x, and the y-coordinate of it is denoted as x1y in this paper. As a result the relationship of x1x and x1y turns out to be x1y ¼
k α1=2 ðx1x β bÞ ðx1x β bÞ2 : 2 k
ð4Þ
x1y depends on x1x and b. b can vary the value of x1y independently of x1x. Therefore, the merging path can be modified using b. For example an effect of b is shown in Fig. 3. Although b is a variable, b is set as a constant in Fig. 3. When b¼0, the merging path coincides the line l3. The positive value of b shifts the merging path to the right of l3. The negative value of b shifts the merging path to the left of l3.
2.1. Modeling of the road and the vehicles 2.1.1. Road model To optimize the paths of the relevant vehicles under some constraints, we used MPC to solve the merging problem. The formulation of the optimization problem and the MPC scheme will be described in the following subsections. To design paths for the relevant vehicles according to the road shape, the merging lane and the main lane have to be modeled. Just as in paper (Cao, Mukai, Kawabe, Nishira, & Fujiki, 2013c), in consideration of simplicity, the centerline of the main lane and the centerline of the merging lane are modeled as l1, and l2 in Fig. 2. l3 is a smooth line which converges to l2 on the left side and converges to l1 on the right side. In Fig. 2 and the rest parts of this paper, we denote the merging vehicle as “Vehicle1” and denote the main lane vehicle as “Vehicle2”. The positions of Vehicle1 is plotted as “◯” and Vehicle2 is plotted as “ ”. To get the mathematical expressions of the lines, the coy-coordinate system is set as follows: the origin is set at the position of Vehicle2 when the
Fig. 1. The situation of the merging problem in this paper.
Fig. 2. Approximation method of the road.
Table 1 Definitions of the symbols included in l1, l2, l3. Symbols
Definitions
x y k β α
The The The The The The
x-coy-coordinate y-coordinate slope of l2 interception of l2 on the Y-axis design parameter which determines smoothness of the curvature
Fig. 3. The effect of the variable b (constant case).
100
W. Cao et al. / Control Engineering Practice 34 (2015) 98–105
By approximating the relevant vehicles with the particle approximation method, we obtain the state space equation as follows: 2 3 2 0 3 v1x 7 6v 7 6 0 7 6 2x 7 6 7 6 7 6 0 7 6 vb 7 6 6 7 6 7 ; ð5Þ x_ ¼ 6 7 þ 6 a1x 7 7 6 0 7 6 6 7 6 7 6 7 4 0 5 4 a2x 5 ab 0 where z ¼ ½x1x x2x b v1x v2x vb T ; a ¼ ½a1x a2x ab T :
ð6Þ ð7Þ
Here, z is the state vector and a is the control input of the system. They are expressed as (6) and (7). x1x is the x-coordinates of Vehicle1 and x2x is the x-coordinates of Vehicle2. v1x and v2x are the velocities of Vehicle1 and Vehicle2 in the X-axis direction, while a1x and a2x are the accelerations. vb is the differential of b, while the differential of vb is noted as ab. ab would influence the acceleration of Vehicle1 in the Y-axis direction. Vehicle2 has a2x as the control input in X-axis direction only, so it will always move on l1 during merging. The acceleration of Vehicle1 in the Y-axis, a1y, can be calculated as the second time derivative of x1y along the trajectories of (5). 2.2. Formulation of the optimization problem To obtain a standard merging for drivers the control method has to fulfill the following requirements. (a) The relative distances between the merging vehicle and the main lane vehicle must be kept above an appropriate value to merge safely. (b) The merging vehicle should move on the centerline of the road before and after the merging maneuver. It should also be able to access the whole acceleration area and not depart from the lane. (c) Vehicles should run at their desired speed. (d) The accelerations and decelerations should be as smooth as possible. The relevant vehicles should not accelerate or decelerate frequently. To meet the above requirements, a penalty function is created for each requirement. For requirement (a): Relative distance between the two vehicles is kept above r min with the term Lr. The expression of Lr is as (8). In (8) r is the relative distance between Vehicle1 and Vehicle2 in the two-dimensional space. It is calculated with (9). Lr ¼
1 ; ðr 2 r 2min Þ
r ¼ ððx1x x2x Þ2 þ ðx1y 0Þ2 Þ1=2 :
ð8Þ
appropriately, the available moving area can be modified to have a similar shape to an actual acceleration area. Fig. 4 shows the shapes of l4 and l5. They should converge to l2 on the left side, converge to l1 on the right side, and form a similar shape to the acceleration area around the merging intersection. In this paper, to express the boundary of the available moving area with barrier functions, (4) is extent to obtain the expressions of l4 and l5. They are expressed as (10) and (11) in the x y plane. k α1=2 l4 : y ¼ x β bmax f 1 ðxÞ ðx β bmax f 1 ðxÞÞ2 ; ð10Þ 2 k l5 : y ¼
k α1=2 x β bmin f 2 ðxÞ ðx β bmin f 2 ðxÞÞ2 ; 2 k
ð11Þ
where bmin and bmax are parameters related to the size of the acceleration area. The expression of f1 and f2 in (10) and (11) is as (12). The definitions of the parameters in (12) are shown in Table 2. The relationship between the value of the parameters and the shape of f1 and f2 is shown in Fig. 5. 1 f i ðxÞ ¼ kbi 1 þ expð αbi ðx βbli ÞÞ 1 1 ; i ¼ 1; 2; ð12Þ þ 1 þ expðαbi ðx β bri ÞÞ where βbl1 and βbl2 are related to the shape of the entrance of the acceleration area. βbr1 and βbr2 are related to the shape of the exit of the acceleration area. The other parameters are related to the shape of the available moving area. If the value of b becomes too large or too small during the merging, Vehicle1 would run out of the range of the road. To ensure the path of Vehicle1 does not exceed the boundaries of the available moving area, Lbl and Lbr are included in the penalty function. Since (4), (10), and (11) have the same constructions, the requirement of keeping Vehicle1 moving within the boundaries can be ensured by Lbl and Lbr expressed as (13) and (14). Lbl ensures that Vehicle1 does
Fig. 4. Shape of l4 and l5.
Table 2 Definitions of the parameters in fi(x) ði ¼ 1; 2Þ. Parameters
Meanings
kbi αbi βbli
Maximum value of fi(x) Parameter which decide the steepness of shift part x-coy-coordinate of the point around which the function shift from 0 to kbi x-coy-coordinate of the point around which the function shift from kbi to 0
βbri
ð9Þ
rmin is the safe margin of the relative distance between Vehicle1 and Vehicle2. With this term included in the penalty function, the optimum control input is calculated under the condition that the relative distance r is kept larger than rmin when the initial value of r for optimizing calculation is larger than rmin and the optimization problem is feasible. When the initial value of r for the calculation is smaller than rmin by some reasons, e.g. calculation/discretizing error of the control input, the term is expected to work so that r is expanded to rmin. For requirement (b): To bound an available moving area for Vehicle1, l4 and l5 are introduced. With parameters set
Fig. 5. Relationship between the parameters and the shape of f i ðxÞði ¼ 1; 2Þ.
W. Cao et al. / Control Engineering Practice 34 (2015) 98–105
not exceed l5. Lbr makes Vehicle1 do not exceed l4. Lbl ¼ lnðb bmin f 1 ðx1x ÞÞ;
ð13Þ
Lbr ¼ lnðbmax f 2 ðx1x Þ bÞ:
ð14Þ
For requirement (c): To make vehicles run as closely as possible at their desired speed, terms Lv1 and Lv2 expressed as (15) and (16) are included. Lv1 ¼ ðv1x v1d Þ2 ;
ð15Þ
Lv2 ¼ ðv2x v2d Þ2 ;
ð16Þ
where v1d and v2d are the desired velocities of Vehicle1 and Vehicle2. For requirement (d): La1 and La2 are included to avoid abrupt and severe accelerations in the X-axis direction. ab would influence the acceleration of Vehicle1 in the Y-axis direction, so Lab is included to avoid abrupt and severe acceleration of Vehicle1 along Y-axis. The penalty functions are La1 ¼ a21x ;
La2 ¼ a22x ;
Lab ¼ a2b :
ð17Þ
The overall penalty function (18) is established as to make the relevant vehicles meet these requirements. LðzðτÞ; aðτÞÞ ¼ ωr Lr ωbl Lbl ωbr Lbr þ ωv1 Lv1 þ ωv2 Lv2 þ ωa1 La1 þ ωa2 La2 þ ωab Lab :
ð18Þ
where ωr, ωbl, ωbr, ωv1, ωv2, ωa1, ωa2, and ωab are weights for each term. The merging trajectories can be determined by solving the optimization problem (19). J is expressed as (20), in which t is the current time, T is the prediction horizon, LðzðτÞ; aðτÞÞ is a penalty function, and τ is the virtual time for prediction. The optimization problem (19) is defined to minimize the performance index J subject to the state space equation (5) and the acceleration constraints (21). a1x, a2x, and ab are optimized simultaneously which leads to cooperative maneuvers of Vehicle1 and Vehicle2. ð19Þ
min J: a
Z J¼
tþT t
LðzðτÞ; aðτÞÞ dτ :
8 > < a1xmin r a1x r a1xmax a2xmin r a2x r a2xmax > : a ra r a : bmin
b
ð20Þ
ð21Þ
bmax
Here a1xmin , a2xmin , and abmin are the lower boundaries of a1x, a2x, and ab. a1xmax , a2xmax , and abmax are the upper boundaries of them. These acceleration constraints are established to make the relevant vehicles move smoothly. The acceleration of Vehicle1 in the Y-axis, a1y, can be observed during merging.
optimal input vectors at time step i and T is the prediction horizon used in (20). Step 3 : Update the input aðtÞ≔aτ . Step 4 : Set t≔t þ h, and go back to Step 2. With a solver which is fast enough, the process of MPC can be executed in real-time. C/GMRES method proposed by Ohtsuka (2004) can be used for the execution. The C/GMRES method can solve a nonlinear model predictive control problem within an acceptable computational time. Since our problem includes a nonlinear penalty function, the C/GMERES method is employed in the following section. 3. Computer simulation and analysis To validate the effectiveness of the proposed method, computer simulations of three traffic scenes were conducted. It was assumed that data used to update the state vector were obtained without any noises or delay, and the two relevant vehicles always followed the optimal inputs without any errors. As a real-time optimization method, the C/GMRES method, proposed by Ohtsuka (2004), was chosen to solve the problem. The simulation was conducted on a personal computer (CPU: Core™i5, 2.50 GHz). In order to obtain a typical merging, we recorded traffic scenes of merging from a helicopter in Fukuoka, Japan. We analyzed the time history of the data, and obtained the initial conditions of the merging maneuver and the paths of the relevant vehicles. A traffic scene was chosen as a representative of cooperative merging. The time history of this merging showed that the main lane vehicle decelerated to let the merging vehicle merge in more easily. The scene was compared with the computer simulation results. The beginning of the merging is shown in Fig. 6. The parameters depend on the road shape: α, k, β, bmin, bmax, kb1, αb1, βblef1, βbrig1, kb2, kb2, αb2, βblef2, βbrig2 have to design for each intersection on the road. The parameters were designed using a picture shown in Fig. 6. The limitation of the accelerations: a1xmin , a2xmin , abmin, a1xmax , a2xmax , and abmax were set to be similar to the acceleration limitation of these of adoptive cruise control devices. The weights: ωi, i ¼ 1; …7 are manually turning parameter depending on the shape of the road aiming for smooth motion of the vehicles. The weights can be recorded on a road side system and sent to the vehicles when they are close to the intersection. The typical initial merging conditions are showed as follows: x1x ¼ 59 m, x2x ¼ 0 m, b ¼ 0 m, v1x ¼ 9:9 m=s, v2x ¼ 20 m=s, a1x ¼ 0 m=s2 , a2x ¼ 0 m=s2 . To reduce the accelerations, α was set to be 40. We set the other parameters in the expressions of l1, l2, and l3 according to the road shape as follows: k ¼ 0:156, β ¼ 159:8. The acceleration area was successfully bounded by l4 and l5 using parameters: bmin ¼ 13:07, bmax ¼ 80:10, kb1 ¼ 2:15, αb1 ¼ 0:05, βblef 1 ¼ 140, βbrig1 ¼ 180, kb2 ¼ 1, αb2 ¼ 0:05, βblef 2 ¼ 180,
2.3. MPC with real-time optimization The process of MPC was described in paper (Cao, Mukai, & Kawabe, 2013a). MPC is a control method in which the current control input is obtained by solving a finite horizon optimal control problem. Using the current measured state the initial state in the optimal control problem the optimal input sequence is calculated at each sampling instant. The first element of the sequence is applied to the system. The optimization problem is discretized with sampling period h. The process of MPC can be described as follows: Step 1: Set τ : ¼ t, measure z(t), and set zðτ Þ≔zðtÞ. Step 2: Solve the optimization problem to get the optimal input vectors aτ ; aτ þ h ; aτ þ 2h ; …; aτ þ T h which can minimize the performance index J. Here ai is the discretized time
101
Fig. 6. The typical merging scene shot on a helicopter.
Fig. 7. Approximation result of the road shape.
102
W. Cao et al. / Control Engineering Practice 34 (2015) 98–105
βbrig2 ¼ 840. For mild merging, the lower and upper bounds of the
accelerations were set as: a1xmin ¼ a2xmin ¼ abmin ¼ 3 m=s2 , a1xmax ¼ a2xmax ¼ abmax ¼ 3 m=s2 . The model of the road is shown in Fig. 7. Weights in the penalty functions were set so that simulation results would be similar to actual merging results: ω1 ¼ 15:0, ω2 ¼ 0:001, ω3 ¼ 0:0011, ω4 ¼ 0:004, ω5 ¼ 0:004, ω6 ¼ 0:024, ω7 ¼ 0:0241, ω8 ¼ 0:001. According to the traffic condition, r min was set as 14 m, and v1d and v2d were set to be 16:7 m=s. The sampling time h was set as 0.01 s. The prediction horizon T was set as 3 s. The positions of the two vehicles at t i ¼ 2i s; ði ¼ 0; 1; 2; …; 9Þ, were plotted in the generated merging paths. The unit for all the speeds is [m/s]. The unit for the accelerations is [m/s2]. The unit for b and r is [m]. To verify that the merging vehicle was kept in the available moving area, boundaries of the available moving area, l4 and l5 were also shown in the figures of the generated merging paths. To ensure there were no excessive accelerations, the acceleration in Y-axis direction of Vehicle1, a1y, was also calculated and shown in the simulation results.
3.1. Case 1: The cooperative merging Cao et al. (2013a) As a direct comparison with the actual merging, the initial conditions of this case were set to be exactly the same with the initial conditions of the representative cooperative merging. The actual merging path is shown in Fig. 8 and the generated merging path is shown in Fig. 9. The time histories of variables during the simulation are shown in Fig. 10. From Fig. 9 it can be known that, just as the actual motions shown in Fig. 8, Vehicle1 merged successfully to the front of Vehicle2 and kept an appropriate distance with Vehicle2. This means that the relative positions of the two vehicles in the simulation results are consistent with the actual merging result. The cooperative merging phenomenon was seen in the simulation results. It can also be seen from Fig. 10 that the Vehicle2 decelerated a little to let Vehicle1 merge in more easily. To show the modification of the merging path clearly, the zoomed merging path is shown in Fig. 11. As expected to keep relative distance, Vehicle1 ran apart from l3, but always ran inside the available moving area bounded by l4 and l5. As shown in Fig. 10, during the merging maneuver all accelerations were kept between 3 m=s2 and 3 m=s2 . The relative distance was always kept above r min ¼ 14 m. This means the corresponding constraints were not violated. The vehicles' speeds tended to the desired value, 16.7 m/s, during and after merging. As a comparison, the simulation of this case is also conducted with the method proposed in Cao, Mukai, Kawabe, Nishira, and Fujiki (2013b), in which b was fixed at 0. The time histories of the variables are shown in Fig. 12.
Fig. 10. Time histories of main variables during merging in Case 1.
Fig. 11. Zoomed generated paths of Case 1.
Fig. 8. Merging path of the actual merging scene.
Fig. 9. Generated paths of Case 1.
Fig. 12. Time histories of main variables in Case 1 when the merging path is fixed.
W. Cao et al. / Control Engineering Practice 34 (2015) 98–105
As can be seen, the variation of the accelerations in Fig. 10 was milder than that in Fig. 12, while the relative distance was almost the same. The merging vehicle moved to the right of l3. This motion increased the relative distance between the relevant vehicles. This is considered to be the benefit of the introduction of the state variable b.
3.2. Case 2: Relaxed initial conditions which motivate the main lane vehicle to be the leading vehicle–verification of the universality of the weights To investigate whether the proposed method can generate realistic paths without adjustment of parameters in the performance index for the vehicles when the main lane vehicle should become the leading vehicle, we changed the initial conditions as: x1x ¼ 40 m, x2x ¼ 40 m, b ¼ 0 m, v1x ¼ 9:9 m=s, v2x ¼ 20 m=s, a1x ¼ 0 m=s2 , a2x ¼ 0 m=s2 . Since x1x ¼ x2x , v1x o v2x , a1x ¼ a2x ¼ 0, normally, drivers of a merging vehicle would merge to the behind of the main lane vehicle to avoid excessive accelerations and unnecessary risks. The generated paths in Fig. 13 show that Vehicle1 merged behind of Vehicle2 and kept an appropriate distance with Vehicle2. In this case, all constrains and barriers were kept in the simulation too. The simulation results show the expected results.
Fig. 13. Generated paths of Case 2.
103
3.3. Case 3: Collision case In this case the initial conditions were set as: x1x ¼ 0 m, x2x ¼ 0 m, b ¼ 0 m, v1x ¼ 9:9 m=s, v2x ¼ 20 m=s, a1x ¼ 0 m=s2 , a2x ¼ 0 m=s2 . It is obvious that x1x ¼ x2x , v1x ¼ v2x , a1x ¼ a2x ¼ 0. Therefore without control, the motions in the X-axis direction of Vehicle1 and Vehicle2 would be exactly the same, and they would collide with each other at the intersection point. The optimization was able to avoid a collision. Fig. 15 shows that Vehicle1 decelerated a little to let Vehicle2 go ahead. The generated paths in Fig. 14 show that Vehicle1 merged behind Vehicle2 and followed it with an appropriate relative distance. The variation of the variables in Fig. 15 shows that Vehicle1 and Vehicle2 kept the constraints. The relative distance was always kept above r min ¼ 14 m. In this case, decrease of a1x and increase of a2x after t¼ 2 s were cooperatively generated to avoid collision. They worked so well that b did not have to be changed actively to generate a smooth merging trajectory, and therefore Vehicle1 did not move significantly away from the centerline in this case.
3.4. The influence of noise A real control system will have imprecise sensors that may give a range of estimates for velocity and position of vehicles. To test the influence of noise 0.02 m random noise for position sensors of Vehicle1 and Vehicle2 and 0.02 m/s random noise for velocity sensors of Vehicle1 and Vehicle2 are added to the simulation. In this case the initial conditions were set as: x1x ¼ 50 m, x2x ¼ 0 m, b ¼ 0 m, v1x ¼ 16:7 m=s, v2x ¼ 16:7 m=s, a1x ¼ 0 m=s2 , a2x ¼ 0 m=s2 . The prediction horizon is 6 s. Fig. 16 and Fig. 17 show that merging succeeds without collision for this magnitude of noise. However if the amplitude of the noise
Fig. 14. Generated paths of Case 3. Fig. 16. Generated paths of the case with the noises.
Fig. 15. Time histories of main variables during merging in Case 3.
Fig. 17. Time histories of main variables during merging in the case with the noises.
104
W. Cao et al. / Control Engineering Practice 34 (2015) 98–105
Fig. 18. Distribution of the given initial conditions (500 initial conditions).
was grater, calculation of the control input became impossible. The main factor that made calculation impossible was that the augments of logarithm functions used in Eqs. (13) and (14) became negative because of the noise. This issue may be solved by replacing the equations with soft constrains conditions. 3.5. Tests for the dependence of the initial condition To investigate the proposed method for more merging cases, 500 initial conditions were tested. rmin was set to be 5 m in this simulation. The initial positions and the velocities of vehicles are shown in Fig. 18. These initial conditions were generated by using a random function in MATLAB within appropriate boundaries. The boundaries were set so that realistic situations were simulated. The initial conditions of Vehicle1 were distributed larger than those of Vehicle2, considering that the initial speed and the position of the merging vehicle are more likely to be affected by the shape of the merging lane than that of the main lane vehicle. Fig. 18 shows a histogram of the minimum relative distances occurred in the simulation. From Fig. 19 it is observed that the all cases of merging were succeeded and the minimum distance is never less than 10 m in all cases. 3.6. Discussion of application Two possible applications can be considered for the proposed method. In one application, both the optimal inputs for the merging vehicle and the main lane vehicle are used for the motion control of both vehicles. In this case the cooperative merging phenomenon can be reproduced, and the relevant vehicles can cooperate with each other, traveling on smooth paths while completing the merge. In the other application, only the merging vehicle is controlled. The optimal input for the merging vehicle is used for its motion control while the optimal input for the main lane vehicle is only used for the prediction of its motion. Since the state vector and the optimal inputs are updated every time step, the discrepancy between the predicted motion of the main lane vehicle and its
Fig. 19. The histogram of the minimum relative distances.
actual motion can be taken into account and the control input for Vehicle1 will be modified owing to the update. Furthermore, large value of the weight for the acceleration of vehicle2 reduces the amplitude of the optimal acceleration /deceleration of Vehicle2. The behavior of vehicle2 generated in this manner is little affected by the motion of Vehicle1, as if Vehicle2 was not cooperatively driven ignoring existence of Vehicle1. In other words, the proposed method can be used in the case when the main lane vehicle does not consider the merging vehicle. In this way, as long as the merging vehicle moves appropriately, there will not be any accident and the main lane vehicle would not have to accelerate excessively. As a result, in this way, the merging vehicle can merge successfully without forcing the main lane vehicle to produce unacceptable accelerations. In this application, with given initial conditions, motions of the vehicles involved in a merging can be forecasted. Therefore, it can
W. Cao et al. / Control Engineering Practice 34 (2015) 98–105
also be used as a description of the cooperative merging phenomenon and modeling of the merging maneuver.
4. Conclusions and future works 4.1. Conclusions In this paper a cooperative merging path generation method for cooperative and mild merging based on MPC is proposed. In this method the merging problem is formulated as an optimization problem. A state variable that modifies the merging trajectory is introduced to the optimization problem, by which the merging point of the merging vehicle is optimized according to the motion of the main lane vehicle. An available moving area is designed for the merging vehicle to restrict the movement of it. As a result, the merging vehicle would run on the centerline of the lanes before and after merging, adjusting its path, and merge at the optimal merging point. Computer simulations are conducted to validate the effectiveness of the proposed method. To be realistic, the initial conditions and parameters in the simulation were set according to a traffic scene recorded from a helicopter. The results proved that, as long as the initial conditions were reasonable, the proposed method can generate cooperative merging path. 4.2. Future works In the future the effectiveness of the proposed method in the case when there are multiple vehicles on the main lane will be investigated. Whether the merging vehicle can find the optimal gap will be researched. Motions of the main lane vehicles will also be observed in that case. We will also research the robustness of
105
the proposed method against unexpected motions of the main lane vehicle and disturbances. References Athans, M. (1969). A unified approach to the vehicle-merging problem. Transportation Research, 3, 123–133. Cao, W., Mukai, M., & Kawabe, T. (2013a). Two-dimensional merging path generation using model predictive control. Artificial Life and Robotics, 17, 350–356. Cao, W., Mukai, M., Kawabe, T., Nishira, H., & Fujiki, N. (2013b). Automotive longitudinal speed pattern generation with acceleration constraints aiming at mild merging using model predictive control method. In 2013 9th Asian control conference (ASCC) (pp. 1–6). Istanbul, Turkey. Cao, W., Mukai, M., Kawabe, T., Nishira, H., & Fujiki, N. (2013c). Mild merging path generation method with optimal merging point based on mpc. In T. Kawabe (Ed.), 7th IFAC symposium on advances in automotive control 7 (pp. 756–761). Tokyo, Japan: National Olympics Memorial Youth Center. De Waard, D., Dijksterhuis, C., & Broohuis, K. A. (2009). Merging into heavy motorway traffic by young and elderly drivers, accident analysis and prevention. Accident Analysis and Prevention, 41, 588–597. De Waard, D., Kruizinga, A., & Brookhuis, K. A. (2008). The consequences of an increase in heavy goods vehicles for passenger car drivers' mental workload and behavior: A simulator study. Accident Analysis and Prevention, 40, 818–828. Hidas, P. (2005). Modelling vehicle interactions in microscopic simulation of merging and weaving. Transportation Research Part C, 13, 37–62. Kachroo, P., & Li, Z. (1997). Vehicle merging control design for an automated highway system. In IEEE conference on ITSC'97 (pp. 224–229). Boston, MA. Lu, X. Y., & Hedrick, J. K. (2003). Longitudinal control algorithm for automated vehicle merging. International Journal of Control, 76, 193–202. Milanés, V., Godoy, J., Villagrá, J., & Perez, J. (2011). Automated on-ramp merging system for congested traffic situations. IEEE Transactions on Intelligent Transportation Systems, 12, 500–508. Ohtsuka, T. (2004). A continuation/GMRES method for fast computation of nonlinear receding horizon control. Automatica, 40, 563–574. Papageorgiou, M., Papamichail, I., Spiliopoulou, A. D., & Lentzakis, A. (2008). Realtime merging traffic control with applications to toll plaza and work zone management. Transporation Research Part C, 16, 535–553. Waller, P. F. (1991). The older driver. Human Factors, 33, 499–505. Wood, J. M. (2002). Age and visual impairment decrease driving performance as measured on a closed-road circuit. Human Factors, 44, 482–494.