Spatially Formulated Connected Automated Vehicle Trajectory Optimization with Infrastructure Assistance

Yi, Ran; Zhou, Yang; Wang, Xin; Liu, Zhiyuan; Li, Xiaotian; Ran, Bin

doi:https://doi.org/10.1155/2022/6184790

Journal of Advanced Transportation

On this page

Abstract Introduction Results Conclusion Data Availability Conflicts of Interest Acknowledgments References Copyright Related Articles

Special Issue

Management and Control Methods of Mixed Traffic Flow with Connected Automated Vehicles

View this Special Issue

Research Article | Open Access

Volume 2022 | Article ID 6184790 | https://doi.org/10.1155/2022/6184790

Spatially Formulated Connected Automated Vehicle Trajectory Optimization with Infrastructure Assistance

Ran Yi,¹Yang Zhou,¹Xin Wang,²Zhiyuan Liu,³Xiaotian Li,¹and Bin Ran¹

Academic Editor: Alessandro Severino

Received05 Oct 2021

Accepted05 Apr 2022

Published20 May 2022

Abstract

This paper presents a constrained connected automated vehicles (CAVs) trajectory optimization method on curved roads with infrastructure assistance. Specifically, this paper systematically formulates trajectory optimization problems in a spatial domain and a curvilinear coordinate. As an alternative of temporal domain and Cartesian coordinate formulation, our formulation provides the constrained trajectory optimization flexibility to describe complex road geometries, traffic regulations, and road obstacles, which are usually spatially varying rather than temporal varying, with assistances vehicle to infrastructure (V2I) communication. Based on the formulation, we first conducted a mathematical proof on the controllability of our system, to show that our system can be controlled in the spatial domain and curvilinear coordinate. Further, a multiobjective model predictive control (MPC) approach is designed to optimize the trajectories in a rolling horizon fashion and satisfy the collision avoidances, traffic regulations, and vehicle kinematics constraints simultaneously. To verify the control efficiency of our method, multiscenario numerical simulations are conducted. Suggested by the results, our proposed method can provide smooth vehicular trajectories, avoid road obstacles, and simultaneously follow traffic regulations in different scenarios. Moreover, our method is robust to the spatial change of road geometries and other potential disturbances by the road curvature, work zone, and speed limit change.

1. Introduction

The CAVs equipped with advanced sensing and communication technologies enable their self-driving tasks and connection with other vehicles as well as infrastructures. Under such a connected and automated environment, CAVs can obtain precise ambient information via sensing and communication to make decisions and control more efficiently and safely than human-driven vehicles. Due to the potentials, CAV technologies have experienced a fast development in very recent years [1–5]. Among these technologies, trajectory planning serves as a critical component to plan the vehicles’ future movement to avoid hazards and make the CAVs operate in a safe, comfortable, and efficient manner. Rather than treating the highway as a straight line, there are many curved roads in the real world such as mountain area that requires ego-CAV trajectory planning algorithm to render both longitudinal and lateral movements to make CAVs pass safely.

Due to the importance, two-dimensional CAVs trajectory planning algorithms have been widely researched in recent years [6–14]. The state of art of CAV trajectory planning algorithms can be generally divided into four different approaches: (i) the graph search-based approach [14, 15]; (ii) the interpolating curve planner [16–18]; (iii) the sampling-based approach [19, 20]; and (iv) the function optimization approach [21, 22]. The graph search-based approach divides the feasible vehicle travel region into multiple grids, and by that, finding the optimal trajectory can be equivalently treated as finding the shortest path among nodes of feasible grids. By the shortest path formulation, this type of approach (e.g., [23, 24]) usually applied dynamic programming and A-star algorithm. By the nature of the shortest path problem, this type of approach needs to tradeoff between computation and precision. When the size of lattice is small and number of lattices are big, this type of algorithm can be slow computing. Whereas when the size of the grid is big, the trajectory can be discontinuous and nonsmooth. On the other hand, the interpolating curve planner, which uses predefined curve functions to generate reference points from the available space [18, 25, 26], is comparatively computational less expensive. The abovementioned approaches largely ignore vehicle kinematics by assuming the physical positions are attainable regardless of vehicle acceleration and angular speed constraints that may induce implementation infeasibility is the real-world application. The sampling-based approach executes a random search over the vehicle feasible state space (e.g., position, speed, and acceleration) and finds an optimal sequence of state heuristically by comparing these randomly sampled state spaces according to a predefined objective function (e.g., minimize travel delay and intensive acceleration or brake). Though effective, these approaches usually render suboptimal solutions suggested by [27] due to the searching heuristics nature. Further, if the state dimension is large, the computation complexity will also increase significantly. As an opposite, the function optimization approach formulates the trajectory planning of CAV as a constrained optimization problem that is flexible in handling constraints from the environment [8, 13, 28, 29], which can minimize a multiobjective cost function meanwhile satisfying the vehicle dynamics and hazard avoidances constraints. Further, the constrained optimization approach is usually implemented in a rolling horizon fashion to be against unexpected disturbances, and it is also known as MPC in the control theory field, which is widely applied in CAV car following control [30–32].

Though promising, the constrained optimization-based CAVs trajectory planning still faces some challenges. One challenge is to describe the road geometries in the formulation explicitly. Differed in the utilized coordinates, they can be further categorized as (i) CAV trajectory constrained optimization on a Cartesian coordinate [8, 13, 28, 29] and (ii) CAV trajectory constrained optimization on a curvilinear coordinate [9, 10, 33, 34]. As a most widely applied coordinate, the Cartesian coordinate is more suited to describe open spaces. However, road geometry is usually composed of complex and composite curves, which means using Cartesian coordinates will be technically challenging to formulate the road boundaries as constraints. On the other hand, the curvilinear coordinate system is born to describe geometries formed by curved lines [35]. Especially for the road geometry description, one axle of the curvilinear coordinate can be the lane centerline, while the other axle can be perpendicular to the centerline tangent. By that, the road geometry and boundaries can be simply formulated, reducing a great formulation and computation complexity.

Nevertheless, most of the vehicle trajectory optimization in the literature is vastly built by a temporal formulation [6–11], assuming that the road geometry characteristics (e.g., curvature, lane tangent directions) remain unchanged over time, that has not fully exploited the potentials of the curvilinear coordinate. The road geometries characteristics, traffic regulations (e.g., speed limit), and hazards are spatially varying rather than temporally varying, which serve as an unobservable exogenous disturbance impacting the performances that further exert a negative impact on the desired planning objectives. Though the real-time information of road attributes can be conveyed to CAVs via V2I, with the increasing maturity of the vehicle to infrastructure (V2I) communication, explicitly incorporating these spatially varying characteristics in the temporal domain is still forbiddingly hard. However, the very recent works by [36, 37], which utilize the spatial formulation for one-dimensional CAV car following control and curvilinear coordinate based human driven vehicles modeling in an intersection, respectively, provide a new angle to formulate the trajectory planning problems in a spatial domain. Inspired by their works, we found that the spatial domain formulation can explicitly incorporate the optimal formulation of spatially varying attributes on a curvilinear coordinate. Hence, an infrastructure assisted spatially constrained optimal ego-CAV trajectory planning algorithm based on a curvilinear coordinate is provided in this paper, which can deal with any types of curved road safely and efficiently. Especially, we contribute to design a spatially formulated constrained CAV trajectory optimization in a curvilinear coordinate that is capable of: (i) greatly simplifying the formulation compared with ones formulated in the time domain and Cartesian coordinates; (ii) quickly converging to the desired vehicle operation state (e.g., target speed, following lane centerline), and robust to the spatially disturbances (e.g., lane curvature change) by the infrastructure communication and assistance and (iii) providing a multi-objective optimization (e.g., smoothness of vehicle control, less deviation from desired state) framework which explicitly incorporates the spatial characteristics (e.g., lane curvature) and constraints (e.g., speed limit, obstacles, and lane width) while avoiding obstacles.

The organization of this paper is as follows. Section 2 provides detailed problem descriptions and gives assumptions of our paper. Section 3 describes the system by a state space and presents the design of MPC for the system. Section 4 presents the designed simulation experiments to verify the effect of the proposed model. The conclusion of the paper and future work is summarized and discussed in Section 5.

2. Problem Description

We provide an illustrative example to motivate our work on trajectory planning first. For our modeling, we focus on two-dimensional ego-CAV trajectory planning on a highway without any intersections. Specifically, our planning target is to ensure ego-CAV travel as fast as possible and meanwhile keep to the targeted road center line. When a CAV drives on a complex traffic scenario such as on a winding lane with or without a potential obstacle, as shown in Figure 1, an infinite number of paths are possible for a CAV, but limited time is given for the decision. It is assumed that the CAV can collect vehicle dynamics from its own sensors. However, road attributes (road geometry, traffic control devices) are obtained from communicating with roadside units (RSUs) [38], by which CAVs know exact traffic environment information in advance. We provide detailed geometries via V2I communication because road geometries are relatively static and can be easily measured by infrastructure. If we rely on vehicular sensing, we will be unable to obtain and utilize road geometries that are not within the sensing range. Additionally, when a CAV is traveling at a high speed, the road geometry information obtained via vehicular sensing may be noisy or inaccurate. The purpose of the trajectory planning algorithm is to find the optimal path from an infinite number of possible paths based on the CAV’s location in a very short time to ensure safety, control efficiency, comfort requirements, and traffic regulation obeyance. Before the detailed modeling, we made the following assumptions for the involved system. Assumption 1: the vehicle is treated as a point. Assumption 2: all actuation and communication delays are negligible. Assumption 3: the vehicle is above automation level 3 according to Society of Automotive Engineering standard [39]. Assumption 4: all vehicle dynamics effects such as suspension movement, road inclination, and aerodynamic forces are negligible. Assumption 5: the road geometry characteristics, spatially traffic regulations and control (e.g., speed limit sign), and obstacle position information can be measured by RSU and communicate to CAVs.

3. Methodology

3.1. State Space Formulation

In this section, we describe the system by a spatial domain state space in a curvilinear coordinate and prove its controllability. One axle of a curvilinear coordinate for a path is usually defined as its centerline, while the other axle can be perpendicular to the lane tangent. By that, road geometry and its boundaries can be simply formulated.

Let represents the centerline of the lane as shown in Figure 2. The lane centerline, with respect to the global frame that is represented by the , where its position is given by and its orientation in the global frame is represented by . Let be the mapped length of the curvature along the lane centerline that vehicle has traveled. Based on that, we define represents the vehicle body-fixed reference frame with its pace , the reciprocal of the vehicular velocity, represents the vehicle’s forward speed in space domain, and its orientation in the global frame is represented by .

To describe the vehicle dynamics over the established curvilinear coordinate, we construct a state space system and define the system state for each space aswhere is the lateral deviation, which equals to signed orthogonal distance from the CAV to the closest point on the lane centerline , in meters; , where is the angular deviation, is the angle between the CAV heading and the x-axis in the global frame {G} and is the angle between the tangent of the lane centerline and the x-axis in the global frame {G}, in radians; , where is the pace deviation, is the reciprocal of the road speed limit ( is a nondifferentiable function of s, which can be treated as a constant), in second per meter. Note that, and are spatially varying and got in a real-time manner by the infrastructure. By V2I, and can be directly obtained by vehicle and used for modeling. Specifically, the vehicle dynamics are modeled as a nonlinear state space system aswhere , which is the relative angular spatial change rate that controlled by the steering wheel. is the curvature of the vehicle trajectory; is the curvature of the lane centerline, in radian per meter; and is relative moderation that indicates acceleration of the CAV, which can be treated as the control of the brake or throttle pedal (negative value indicates accelerating), in second per meter squared. and compose our “road characteristics compensated” control input , given as .

However, the road speed limit can be changed based on geometric road design in a real-world situation, which means cannot be treated as a spatially invariant constant when speed limit change occurs. Moreover, the speed limit change for the road is a unit jump, and is still a nondifferentiable function of s. To solve this problem, we introduce a “buffer zone” in front of the speed limit changing area where a changed speed limit is posted at the same length as our MPC model’s prediction horizon. The “buffer zone” section divides speed limit difference by the “buffer zone” length to smoothly form a discrete velocity transit. With is sufficiently small, the velocity change in the “buffer zone” is close enough to be treated as a continuous system. Thus, becomes a differentiable function of inside the “buffer zone,” and is the parameter indicating acceleration of the road is the derivative of , in second per meter squared. The new nonlinear function with the “buffer zone” introduced is , where , where is the parameter indicating acceleration of the CAV.

Table 1 shows brief equations and their associated units of measurement

Given the state space defined above, we first analyzed its controllability that describes the ability of any control variables that can move the state of a system from any initial state to any final state in a finite time interval. Hence, we applied the following definitions and theorems to approve the controllability.

Theorem 1. Nonlinear system small-time locally controllability [40]: the linear test: if a nonlinear , whose linearized control system over an equilibrium point : is controllable, then it is small-time locally controllable at .

Definition 1. Equilibrium point: for a nonlinear differential equation , where is a function mapping . A point is called an equilibrium point if there is a specific such that .
By the Definition 1, it is trivial to find that the equilibrium point of the system as equation (2) occurs if and only if ,

Theorem 2. Linear time-invariant system controllability [40]: For a linear time-invariant system with the form , the controllability matrix can be written asIf , this linear system is controllable.
Based on the Definition 1, Theorems 1 and 2, we can have the following proposition:

Proposition 1. The state space formulated as equation (2) is small-time locally controllable at the equilibrium point , .

Proof. With Taylor series, equation (2) at the equilibrium point can be rewritten asAccording to the Taylor series, a representation of a function as an infinite sum of terms that are calculated from the values of the function’s derivatives at a single point. In our case, only 1^st order derivative needs to be considered for the linearized control system. Moreover, the equilibrium in our system occurs when the CAV drives along the lane centerline, which means that there is no deviation between the CAV and the lane centerline. The 1^st order derivative of equation (4) at the equilibrium point can be written asand equation (5) can be simplified aswhere , .
Based on equation (3), the controllability matrix of equation (6) can be written aswhich gives us the rank of the controllability matrix:

Remark 1. The above linearization process can also be proved by the small-angle approximation. Specifically, when the angle is relevant small and , the small-angle approximation can be applied. The same state-space model, as equation (6), can be derived based on the small-angle approximation from equation (2).
To reflect control frequency in the real world, we applied the zero^th-order hold (ZOH) for control input for discretization. Specifically, the control input is assumed to be a constant during each update spatial interval , and when is sufficiently small, the discretization process can be treated as a continuous system [41]. The discrete version of equation (5), according to equation (6), is shown asFor notation brevity, we use to represent for the rest of the paper.

3.2. MPC Formulation

In this section, we provide an MPC formulation due to its great capability to systematically deal with system state and control constraints and meanwhile handling multiobjectives. Further, it is robust to be against system disturbances due to its rolling horizon implementation. Based on the discretized control Equation (9a), a linear MPC formulation is formulated in our study. In our MPC framework as illustrated by Figure 3, at each current space step , we solved a constrained trajectory optimization problem over a fixed finite prediction horizon with spatial length to calculate the optimal control input and state sequences within the horizon. The controller only implements the first step optimal control input at space step , and the algorithm continues this process repetitively until the end of algorithm, as shown in Figure 3.

To better illustrate the algorithm, we use to denote the optimal control vectors for CAV obtained at space for the prediction horizon, to ; to denote the predicted future states for CAV obtained at space for the prediction space horizon, and to ; to denote the realized states for CAV by space , which can be seen as the optimal solution where is the initial state when the control introduces.

By carefully considering the control efficiency and driving smoothness, an optimal control strategy can be obtained by solving the optimal control problem given aswhere is the prediction space horizon; is the running cost consists of the CAV penalties on the deviation from equilibrium point and driving discomfort; and is the terminal cost that refers to the final stage of the prediction horizon. Equation (10c) is the initial state for the prediction horizon at space step ; and equation (10d) is the state constraint used to guarantee the vehicle states at each space point. Equation (10e) is the control constraint used to regulate that the steering wheel rotation and acceleration/deceleration are within a reasonable range of .

For the objective function formulated in equation (10a), the running cost is specified as follows:where and are diagonal positive definite matrices, usually designed as the diagonal matrix below:

Especially, if we want to regulate that the vehicles stick to the lane centerline for safety concern, we can predefine and with large values, while on the other hand, if we are willing to provide the trajectories with flexibility, we can set .

We further specify the terminal cost as:where , .

In equations (10d) and (10e), by considering vehicle’s physical limits, CAV’s state constraint determined at space is formulated as

Equation (13a) is the constraint to make sure that the CAV drives within the drivable lane, where , and and . Where and are lower bound and upper bound that CAV can pass through within the obstacle zone, respectively; and are spatial positions where obstacle start and end, which can be obtained from infrastructure through V2I in advance. is the half of the lane width; equation (13b) is the constraint that the physical limits of allowable angular deviation for CAV to make sure that the CAV’s driving direction will not deviate much from the lane centerline direction, where , and indicates the lower and upper bound of allowable angular deviation; equation (13c) is the constraint to ensure that the CAV does not exceed the speed limit, where , and this constraint also makes sure that the CAV cannot drive backward; equation (13d) is the constraint that the realized moving path of CAV is bounded by a given minimum turning radius, where , is the minimum turning radius of the CAV; equation (13e) is the constraint based on the physical limits of vehicle’s acceleration/deceleration rate . In this constraint, and , where is the CAV’s maximum acceleration limit. Similarly, , where is the CAV’s maximum deceleration limits.

To be noted that the above formulation can be expanded in the scenario with speed limit change or stop sign by introducing the concept of “buffer zone,” and correspondingly, equation (13e) changes to . The information , and can be hardly measured by vehicle due to the limit of sensing range, whereas this static information can be readily measured and communication by the infrastructure. As can be found that, differed from the method merely based on vehicular sensors, the infrastructure-assisted approach can provide necessary information to further improve control. Furthermore, the information that is transmitted by vehicle-to-vehicle (V2V) communication is extremely dependent on traffic volume and the distance between the controlled vehicle and its leading vehicle. If the volume of traffic is low, there may be no other vehicles to provide geometric data. Although the volume of traffic is not necessarily low, V2V communication cannot provide information if the distance between vehicles is large, as the vehicle will not save much geometric road information as it passes.

4. Experiment and Results

To test our optimal control model, we create a one-lane road segment with a series of curves as the numerical simulation environment shown in Figure 4. The road comprises three continuous curves with a total length of 1600 m and 3.6 m lane width, which means a 1.8 m width from lane centerline to left/right lane boundary. The first two curves have the same radius of 300 m, and the radius of the third curve is 200 m. The speed limit of the road is set as 54 (15 ).

To validate our proposed method, simulation experiments of multiple traffic scenarios are performed on MATLAB since the field test is expensive and beyond the scope of this paper. The parameter setting for the CAV trajectory optimization as equations (10a) to (10e) is given in Table 2, according to [37, 42].

Five different simulation experiments of multiple traffic scenarios are summarized in Table 3.

4.1. Scenario 1: A Continuous Curvy Road Segment with the Constant Speed Limit

For the initial condition, we set the vehicle’s lateral deviation from the lane centerline to be 1 m, the angular deviation to be , and the pace deviation to be (equivalently desired speed difference 5 in our case). To better analyze the convergence behavior of the proposed algorithm, we plot the proposed controller performance of the first 50 m in Figure 5, which demonstrates the proposed trajectory optimization method’s performance without the “buffer zone” introduced. To be noted that system states and control inputs maintain very closely to the system equilibrium point with nearly no oscillation from 50 m to 1600 m. Figures 5(a)–5(e) indicate the system state evolution, including lateral deviation, angular deviation, and pace deviation, and how the control inputs change in the space domain as the CAV moves by our control, respectively. As can be seen, similar trends are observed for different system states and the control input values. To be more specific, the lateral deviation exhibits some reasonable fluctuation initially: CAV moves 1.386 m laterally from lane centerline’s left side to the right side in the first 6 m along the road, where the positive sign indicates that the CAV has been off the track and is at the left side of the lane centerline and the negative sign indicates that the CAV has off the track and is at the right side of the lane centerline. Then it quickly turns back from the right side to the lane centerline in the next 6 m and keeps driving on the lane centerline afterward. Angular deviation and relative angular spatial change rates converge in a nearly similar fashion as lateral deviation. As for the pace deviation and relative moderation converges to zero gradually. These results show that the algorithm quickly finds the difference between the vehicle states and the lane centerline and calculates the optimal control inputs to achieve the system equilibrium dynamically. Moreover, even though the and evolve spatially, the proposed controller shows great robustness to these disturbances mentioned above. Rather than the time-domain approach (e.g., [43]), the proposed method can better handle the space varying and , which shows the superiority of proposed algorithm by incorporating the road geometric attributes via infrastructure to vehicle communication.

(a)

(b)

(c)

(d)

(e)

To gain further insight into the proposed trajectory optimization method, we convert , , and in the spatial domain into speed, acceleration, and angular deviation rate in the time domain, as shown in Figure 6. The CAV starts with and keeps a high acceleration for the first 15 m and then it decreased gradually until it achieves equilibrium points, as shown in Figure 6(a). These acceleration changes lead to a speed increase in the first 50 m and make the speed converge to the desired speed, as shown in Figure 6(b). We can also find that the angular deviation rate is within a reasonable range initially and gradually converges to zero.

(a)

(b)

(c)

4.2. Scenario 2: A Continuous Curvy Road Segment with Speed Limit Change

The results in the previous sections show that the proposed trajectory optimization method is efficient and stable without speed limit change to be considered. However, in order to test how the proposed trajectory optimization method works in a real-world situation, we change the speed limit of the road segment from 1104 m to 1600 m to be 10 and create a “buffer zone” of the same length as . In other words, we now pay our attention to a new situation where the road segment from 1024 m to 1104 m is replaced by the “buffer zone” with equals . The illustration for the road trajectory with a “buffer zone” is given in Figure 7.

The results in Figure 8 give us a generalized illustration of how the proposed trajectory optimization method performs with the “buffer zone.” Although the “buffer zone” is introduced, the system state evolution and how the control input change in the space domain, as shown in Figures 8(a)–8(e) respectively, is stable and show great robustness to the real-world disturbance. The change of angular deviation rate that indicates the relative turning speed and direction of the CAV shown in Figure 8(h) shows a similar trend as that in Figure 6(c). Furthermore, as expected, CAV’s speed and acceleration oscillate around the “buffer zone.”

(a)

(b)

(c)

(d)

(e)

(f)

(g)

(h)

Figure 9 gives us a detailed illustration of CAV’s speed and acceleration changes inside the “buffer zone.” Figure 9(a) shows that the algorithm quickly detects the speed limit change in the “buffer zone” and makes the deceleration decision to achieve the system equilibrium dynamically, which leads to a smooth speed transition, as shown in Figure 9(b).

(a)

(b)

4.3. Scenario 3: A Curved Road with Two Different Desired Driving Behaviors

The previous sections demonstrated the performance of our proposed trajectory optimization method. In this section, we further conduct a comparison to see the and weight impact on the obedience to the lane centerline. To see the flexibility of CAV’s decision, we set an obedient driving behavior with and as the default case and a flexible one with weights approaches to zero. To better visualize, we create a new one-lane 150 m curvy road with the same CAV initial conditions and adjust the reference line’s weight. The dash-dotted line in Figure 10 shows the result with original weight, and the dotted line shows the result of all , , , and changed to 0 which means that there is no reference line to follow.

The results shown in Figure 10 illustrate that CAV makes much more aggressive decisions with the obedient driving behavior than those with the flexible driving behavior. As we can see, CAV with the obedient driving behavior makes sharp turning decisions and quickly turns back to the lane centerline. In contrast, CAV makes smooth turning decisions and needs more time to reach the lane centerline with the flexible driving behavior. Moreover, unlike CAV with obedient driving behavior tightly follows the reference line as long as it reaches the reference line, CAV with the flexible driving behavior can deviate from the reference line when the curve occurs.

4.4. Scenario 4: A Curved Road with an Obstacle

We further conducted a simulation experiment using the same initial condition and one-lane road as shown in Figure 7 to test our proposed trajectory optimization method with a 10 m obstacle created. The and during the obstacle section. To meet both safety and driving comfort requirements, a joint driving mode is designed, which means a flexible driving behavior before and during the obstacle and an obedient driving behavior afterward.

As we can see from Figure 11, the CAV quickly detects the obstacle and makes smooth and comfortable control decisions to avoid the obstacle. The flexible driving behavior makes the CAV drive in the center to prevent any potential collision between the CAV and the obstacle during the obstacle section. In contrast, the obedient driving behavior lets the CAV quickly turn back and keep to the lane centerline after the CAV passes the obstacle section.

4.5. Scenario 5: A Curved Road with an Obstacle and System Disturbances

To show the robustness of our proposed method, we conduct an experiment with external system disturbance [44] based on Scenario 4 to demonstrate that our method is capable of providing efficient and stable control. Specifically, we add normally distributed random generated disturbances to each of the system states. The details of disturbances are given in Figure 12, where the range of disturbance for the lateral deviation, the angular deviation, and the pace deviation is within , , and per meter, respectively.

(a)

(b)

(c)

As we can see from Figure 13, the resulted trajectory is also smooth and within the feasible drivable area, which suggests that our method is also efficient with system disturbance.

5. Conclusion

Vehicular trajectory optimization plays an essential role in ensuring vehicles’ travel safety and efficiency. Traditional trajectory planning algorithms and methods are primarily formulated in the time domain by detecting the road information with a limited range and optimizing the trajectory myopically, which cannot competently handle the spatially varying road geometric change, obstacles, and traffic regulations. To remedy that, this paper provided a new angle to plan long-term trajectories in a spatial domain with the help of infrastructure. Specifically, this paper systematically formulates trajectory optimization in a spatial domain and on a curvilinear coordinate, enabling our method to flexibly formulate spatially varying complex road geometries, traffic regulations, and road obstacles whose information can be obtained through V2I communication. For rigor, the controllability of the state space was mathematically proved by using both the linear test and the small-angle approximation. Considering vehicle travel efficiency and trajectory smoothness while satisfying the collision avoidance and vehicle kinematics constraint, a multiobjective MPC was constructed, which can be efficiently solved by the state of arts optimization methods.

To demonstrate the usefulness and wide applications of our proposed trajectory planning optimization algorithm, multiscenarios numerical simulations were conducted, which include five parts: (i) a continuous curvy road segment with the constant speed limit, (ii) a continuous curvy road segment with speed limit change, (iii) a curved road with two different desired driving behaviors, (iv) a curved road with an obstacle, and (v) a curved road with an obstacle and system disturbances. As the results suggested, the proposed trajectory optimization method could guarantee satisfactory performance for all scenarios under different types of disturbances. Further, the algorithm is capable of providing stable trajectory control with nearly no oscillation by utilizing the information provided by the infrastructure and newly presented formulation.

Some directions can be extended on the current framework in the future. For example, future studies could apply a more complex four-wheel vehicle control model and incorporate uncertainties in vehicle dynamics. Moreover, we will extend the MPC framework to include ambient vehicle movement and cooperatively plan the trajectories of CAVs for more applicable scenarios, such as intersections and on-ramp scenarios [45].

Data Availability

We do not use any field collected data in this paper. The numerical experiment code will be shared based on the requests.

Conflicts of Interest

The authors ensure that there are no conflicts of interest for this paper.

Acknowledgments

This research was supported by Wisconsin Traffic Operation and Safety (TOPS) Laboratory.

References

D. Cao, J. Wu, J. Wu, B. Kulcsár, and X. Qu, “A Platoon Regulation Algorithm to Improve the Traffic Performance of Highway Work Zones,” Computer-Aided Civil and Infrastructure Engineering, vol. 36, 2021.
View at: Publisher Site | Google Scholar
Y. Wang, S. Hou, and X. Wang, “Reinforcement Learning-Based Bird-View Automated Vehicle Control to Avoid Crossing Traffic,” Computer-Aided Civil and Infrastructure Engineering, vol. 36, no. 7, pp. 890–901, 2020.
View at: Publisher Site | Google Scholar
Z. Wang, X. Zhao, Z. Xu, X. Li, and X. Qu, “Modeling and Field Experiments on Autonomous Vehicle Lane Changing with Surrounding Human-Driven Vehicles,” Computer-Aided Civil and Infrastructure Engineering, vol. 36, no. 7, pp. 877–889, 2020.
View at: Publisher Site | Google Scholar
A. Severino, S. Curto, S. Barberi, F. Arena, and G. Pau, “Autonomous vehicles: an analysis both on their distinctiveness and the potential impact on urban transport systems,” Applied Sciences, vol. 11, no. 8, p. 3604, 2021.
View at: Publisher Site | Google Scholar
F. Arena, G. Pau, A. Severino, S. Trubia, and S. Curto, “Future connected cars through the evolution of telematics and infotainment,” International Journal on Engineering Applications (IREA), vol. 9, no. 2, p. 49, 2021.
View at: Publisher Site | Google Scholar
B. Gutjahr, L. Groll, and M. Werling, “Lateral vehicle trajectory optimization using constrained linear time-varying MPC,” IEEE Transactions on Intelligent Transportation Systems, vol. 18, no. 6, pp. 1–10, 2016.
View at: Publisher Site | Google Scholar
F. Mensing, R. Trigui, and E. Bideaux, “Vehicle trajectory optimization for application in ECO-driving,” in Proceedings of the 2011 IEEE Vehicle Power and Propulsion Conference, pp. 1–6, Chicago, IL, USA, September 2011.
View at: Publisher Site | Google Scholar
H. Guo, C. Shen, H. Zhang, H. Chen, and R. Jia, “Simultaneous trajectory planning and tracking using an MPC method for cyber-physical systems: a case study of obstacle avoidance for an intelligent vehicle,” IEEE Transactions on Industrial Informatics, vol. 14, no. 9, pp. 4273–4283, 2018.
View at: Publisher Site | Google Scholar
S. Manzinger, C. Pek, and M. Althoff, “Using Reachable Sets for Trajectory Planning of Automated Vehicles,” IEEE Transactions on Intelligent Vehicles, vol. 6, 2020.
View at: Publisher Site | Google Scholar
M. G. Plessen, “Trajectory Planning of Automated Vehicles in Tube-like Road Segments,” in Proceedings of the 2017 IEEE 20th International Conference on Intelligent Transportation Systems (ITSC), pp. 1–6, Yokohama, Japan, October 2017.
View at: Publisher Site | Google Scholar
M. Werling, J. Ziegler, S. Kammel, and S. Thrun, “Optimal Trajectory Generation for Dynamic Street Scenarios in a Frenet Frame,” in Proceedings of the 2010 IEEE International Conference on Robotics and Automation, pp. 987–993, Anchorage, AK, USA, May 2010.
View at: Publisher Site | Google Scholar
E. Frazzoli, M. A. Dahleh, and E. Feron, “Real-time motion planning for agile autonomous vehicles,” Journal of Guidance, Control, and Dynamics, vol. 25, no. 1, pp. 116–129, 2002.
View at: Publisher Site | Google Scholar
F. Gritschneder, K. Graichen, and K. Dietmayer, “Fast Trajectory Planning for Automated Vehicles Using Gradient-Based Nonlinear Model Predictive Control,” in Proceedings of the 2018 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS), pp. 7369–7374, Madrid, Spain, October 2018.
View at: Publisher Site | Google Scholar
J. Bohren, T. Foote, J. Keller et al., “Little ben: the ben franklin racing team’s entry in the 2007 DARPA urban challenge,” Journal of Field Robotics, vol. 25, no. 9, pp. 598–614, 2008.
View at: Publisher Site | Google Scholar
R. Kala and K. Warwick, “Multi-level planning for semi-autonomous vehicles in traffic scenarios based on separation maximization,” Journal of Intelligent and Robotic Systems, vol. 72, no. 3-4, pp. 559–590, 2013.
View at: Publisher Site | Google Scholar
J. Reeds and L. Shepp, “Optimal paths for a car that goes both forwards and backwards,” Pacific Journal of Mathematics, vol. 145, no. 2, pp. 367–393, 1990.
View at: Publisher Site | Google Scholar
T. Fraichard and A. Scheuer, “From reeds and shepp’s to continuous-curvature paths,” IEEE Transactions on Robotics, vol. 20, no. 6, pp. 1025–1035, 2004.
View at: Publisher Site | Google Scholar
L. Labakhua, U. Nunes, R. Rodrigues, and F. S. Leite, “Smooth trajectory planning for fully automated passengers vehicles: spline and clothoid based methods and its simulation,” Informatics in Control Automation and Robotics, Springer, Berlin, Germany, 2008.
View at: Publisher Site | Google Scholar
Y. Kuwata, S. Karaman, J. Teo, E. Frazzoli, J. P. How, and G. Fiore, “Real-time motion planning with applications to autonomous urban driving,” IEEE Transactions on Control Systems Technology, vol. 17, no. 5, pp. 1105–1118, 2009.
View at: Publisher Site | Google Scholar
S. Karaman, M. R. Walter, A. Perez, E. Frazzoli, and S. Teller, “Anytime Motion Planning Using the RRT,” in Proceedings of the 2011 IEEE International Conference on Robotics and Automation, pp. 1478–1483, Shanghai, China, May 2011.
View at: Publisher Site | Google Scholar
L. B. Cremean, T. B. Foote, J. H. Gillula et al., “Alice: an information-rich autonomous vehicle for high-speed desert navigation,” Journal of Field Robotics, vol. 23, no. 9, pp. 777–810, 2006.
View at: Publisher Site | Google Scholar
D. Kogan and R. Murray, “Optimization-based Navigation for the DARPA Grand Challenge,” in Proceedings of the Conference on Decision and Control (CDC), San Diego, CA, USA, December 2006.
View at: Google Scholar
J. Wu, B. Kulcsár, S. Ahn, and X. Qu, “Emergency vehicle lane pre-clearing: from microscopic cooperation to routing decision making,” Transportation Research Part B: Methodological, vol. 141, pp. 223–239, 2020.
View at: Publisher Site | Google Scholar
J. Wu, S. Ah, Y. Zhou, P. Liu, and X. Qu, “The Cooperative Sorting Strategy for Connected and Automated Vehicle Platoons,” 2020, https://arxiv.org/abs/2003.06481.
View at: Google Scholar
T. Berglund, A. Brodnik, H. Jonsson, M. Staffanson, and I. Soderkvist, “Planning smooth and obstacle-avoiding B-spline paths for autonomous mining vehicles,” IEEE Transactions on Automation Science and Engineering, vol. 7, no. 1, pp. 167–172, 2010.
View at: Publisher Site | Google Scholar
J. Pérez, J. Godoy, J. Villagrá, and E. Onieva, “Trajectory Generator for Autonomous Vehicles in Urban Environments,” in Proceedings of the 2013 IEEE international conference on robotics and automation, pp. 409–414, Karlsruhe, Germany, May 2013.
View at: Publisher Site | Google Scholar
M. Elbanhawi and M. Simic, “Sampling-based robot motion planning: a review,” IEEE Access, vol. 2, pp. 56–77, 2014.
View at: Publisher Site | Google Scholar
M. Nolte, M. Rose, T. Stolte, and M. Maurer, “Model predictive control based trajectory generation for autonomous vehicles—an architectural approach,” in Proceedings of the 2017 IEEE Intelligent Vehicles Symposium (IV), pp. 798–805, Los Angeles, CA, USA, June 2017.
View at: Publisher Site | Google Scholar
A. Heilmeier, A. Wischnewski, L. Hermansdorfer, J. Betz, M. Lienkamp, and B. Lohmann, “Minimum Curvature Trajectory Planning and Control for an Autonomous Race Car,” Vehicle System Dynamics, vol. 58, pp. 1–31, 2019.
View at: Publisher Site | Google Scholar
J. Wang, S. Gong, S. Peeta, and L. Lu, “A real-time deployable model predictive control-based cooperative platooning approach for connected and autonomous vehicles,” Transportation Research Part B: Methodological, vol. 128, pp. 271–301, 2019.
View at: Publisher Site | Google Scholar
Y. Zheng, S. E. Li, K. Li, F. Borrelli, and J. K. Hedrick, “Distributed model predictive control for heterogeneous vehicle platoons under unidirectional topologies,” IEEE Transactions on Control Systems Technology, vol. 25, no. 3, pp. 899–910, 2017.
View at: Publisher Site | Google Scholar
S. Gong, J. Shen, and L. Du, “Constrained optimization and distributed computation based car following control of a connected and autonomous vehicle platoon,” Transportation Research Part B: Methodological, vol. 94, pp. 314–334, 2016.
View at: Publisher Site | Google Scholar
T. D. Barfoot and C. M. Clark, “Motion planning for formations of mobile robots,” Robotics and Autonomous Systems, vol. 46, no. 2, pp. 65–78, 2004.
View at: Publisher Site | Google Scholar
A. Khalifa, O. Kermorgant, S. Dominguez, and P. Martinet, “An observer-based longitudinal control of car-like vehicles platoon navigating in an urban environment,” in Proceedings of the 2019 IEEE 58th Conference on Decision and Control (CDC), pp. 5735–5741, Nice, France, December 2019.
View at: Publisher Site | Google Scholar
E. Héry, S. Masi, P. Xu, and P. Bonnifait, “Map-based Curvilinear Coordinates for Autonomous Vehicles,” in Proceedings of the 2017 IEEE 20th International Conference on Intelligent Transportation Systems (ITSC), pp. 1–7, Yokohama, Japan, October 2017.
View at: Publisher Site | Google Scholar
Y. Zhang, Y. Bai, M. Wang, and J. Hu, “Cooperative Adaptive Cruise Control with Robustness against Communication Delay: An Approach in the Space Domain,” IEEE Transactions on Intelligent Transportation Systems, vol. 22, no. 9, pp. 5496–5507, 2020.
View at: Publisher Site | Google Scholar
J. Zhao, V. L. Knoop, and M. Wang, “Two-dimensional vehicular movement modelling at intersections based on optimal control,” Transportation Research Part B: Methodological, vol. 138, pp. 1–22, 2020.
View at: Publisher Site | Google Scholar
B. Ran, T. Chen, Y. Cheng et al., “System and methods for partially instrumented connected automated vehicle highway systems,” Google Patents, Jul., vol. 30, 2020.
View at: Google Scholar
S. S. Shadrin and A. A. Ivanova, “Analytical review of standard Sae J3016 «taxonomy and definitions for terms related to driving automation systems for on-road motor vehicles» with latest updates,” Avtomobil’ Doroga Infrastruktura, vol. 3, no. 21, p. 10, 2019.
View at: Google Scholar
M. S. Mahmoud, Advanced Control Design with Application to Electromechanical Systems, Butterworth-Heinemann, Oxford, UK, 2018.
B. M. Chen, Z. Lin, and Y. Shamash, Linear systems theory, Springer, Berlin, Germany, 2004.
Y. Zhou, S. Ahn, M. Wang, and S. Hoogendoorn, “Stabilizing mixed vehicular platoons with connected automated vehicles: an H-infinity approach,” Transportation Research Part B: Methodological, vol. 132, pp. 152–170, 2020.
View at: Publisher Site | Google Scholar
B.-C. Chen, B.-C. Luan, and K. Lee, “Design of lane keeping system using adaptive model predictive control,” in Proceedings of the 2014 IEEE International Conference on Automation Science and Engineering (CASE), pp. 922–926, New Taipei, Taiwan, August 2014.
View at: Publisher Site | Google Scholar
Y. Zhou, S. Ahn, M. Chitturi, and D. A. Noyce, “Rolling horizon stochastic optimal control strategy for ACC and CACC under uncertainty,” Transportation Research Part C: Emerging Technologies, vol. 83, pp. 61–76, 2017.
View at: Publisher Site | Google Scholar
T. Chen, M. Wang, S. Gong, Y. Zhou, and B. Ran, “Connected and automated vehicle distributed control for on-ramp merging scenario: a virtual rotation approach,” Transportation Research Part C: Emerging Technologies, vol. 133, Article ID 103451, 2021.
View at: Publisher Site | Google Scholar

Copyright

Copyright © 2022 Ran Yi et al. This is an open access article distributed under the Creative Commons Attribution License, which permits unrestricted use, distribution, and reproduction in any medium, provided the original work is properly cited.

PDF Download Citation

Download other formats

Order printed copies

Views

302

Downloads

371

Citations