Intersection Vehicle Turning Control for Fully Autonomous Driving Scenarios

sensors Article Intersection Vehicle Turning Control for Fully Autonomous Driving Scenarios Zhizhong Ding, Chao Sun, Momiao Zhou *, Zhengqiong Liu and Congzhong Wu School of Computer and Information, Hefei University of Technology, Hefei 230009, China; zzding@hfut.edu.cn (Z.D.); sunchao7610@mail.hfut.edu.cn (C.S.); zqliu@hfut.edu.cn (Z.L.); wcz114773@hfut.edu.cn (C.W.) * Correspondence: mmzhou@hfut.edu.cn Citation: Ding, Z.; Sun, C.; Zhou, M.; Liu, Z.; Wu, C. Intersection Vehicle Turning Control for Fully Autonomous Driving Scenarios. Sensors 2021, 21, 3995. https:// doi.org/10.3390/s21123995 Academic Editor: Felipe Jiménez Abstract: Currently research and development of autonomous driving vehicles (ADVs) mainly consider situation whereby manual driving vehicles and ADVs run simultaneously on lanes. In order to acquire information of vehicle itself and environment necessary for decisionmaking and controlling, ADVs that are under development now are normally equipped with a lot of sensing units, for example, high precision global positioning systems, various types of radar, and video processing systems. Obviously, current advanced driver assistance systems (ADAS) or ADVs still have some problems concerning high reliability of driving safety, as well as vehicle s cost and price. It is certain, however, that in future re will be some roads, areas or cities where all vehicles are ADVs, i.e., without any human driving vehicles in traffic. For such scenarios, methods of environment sensing, traffic instruction indicating, and vehicle controlling should be different from that of situation mentioned above if reliability of driving safety and production cost expectation is to be improved significantly. With anticipation that a more sophisticated vehicle ad hoc network (VANET) should be an essential transportation infrastructure for future ADV scenarios, problem of vehicle turning control based on vehicle to everything (V2X) communication at road intersections is studied. The turning control at intersections mainly deals with three basic issues, i.e., target lane selection, trajectory planning and calculation, and vehicle controlling and tracking. In this paper, control strategy, model and algorithms are proposed for three basic problems. A model predictive control (MPC) paradigm is used as vehicle upper layer controller. Simulation is conducted on CarSim-Simulink platform with typical intersection scenes. Keywords: autonomous driving; vehicle turning control; VANET; model predictive control Received: 14 May 2021 Accepted: 7 June 2021 Published: 9 June 2021 Publisher s Note: MDPI stays neutral with regard to jurisdictional claims in published maps and institutional affiliations. Copyright: 2021 by authors. Licensee MDPI, Basel, Switzerland. This article is an open access article distributed under terms and conditions of Creative Commons Attribution (CC BY) license (https:// creativecommons.org/licenses/by/ 4.0/). 1. Introduction Currently, research and development of advanced driver assistance systems (ADAS) mainly focus on oncoming market demand [1]. It is conducted by companies such as Tesla, Google, Baidu, etc., and traffic situation y envision is usually that of humans driving vehicles, and with autonomous driving vehicles (ADV) or driverless vehicles occurring concurrently on roads. In order to acquire information of vehicle itself and environment, which is necessary for decision-making and controlling, ADVs under development now are normally equipped with a lot of sensing systems, e.g., globe positioning system, lidar, millimeter wave radar, infrared radar, video system or vision system, and so on. The sensor information is processed by advanced or intelligent processing units. The current solutions of ADAS or ADV still have some problems [2]. Firstly, those equipped sensing systems might not absolutely guarantee reliability of driving safety due to ir performance degradation caused by bad wear, lack of light, obstacles, blind areas, etc. Secondly, it might take too long for advanced or intelligent algorithms to extract needed information from sensed signals such as video due to computational complexity. For example, if we want a vehicle with a speed of 160 km/h to make decisions for autonomous driving in a time interval that vehicle moves every Sensors 2021, 21, 3995. https://doi.org/10.3390/s21123995 https://www.mdpi.com/journal/sensors

Sensors 2021, 21, 3995 2 of 16 0.5 m distance, all computations have to be finished in 22.5 milliseconds. Thirdly, those sensing systems, particularly lidar, increase cost of vehicle production considerably. It could be anticipated that in future re are some roads, areas or cities where all vehicles are totally ADVs, i.e., without any manual driving vehicles in traffic. To improve driving safety and to reduce production cost, in such scenarios, way of environment sensing, traffic instruction indicating and vehicle controlling might be different from what is used in current ADAS or ADV. We assume that a more sophisticated VANET (vehicle ad hoc network) should be an essential infrastructure for future ADV transportation since it can provide much more information with much more efficiency by V2X communications (i.e., vehicle-to-vehicle, vehicle-to-infrastructure, vehicle-to-pedestrian, etc.). This anticipation motivates work of article. The statistical data of traffic accidents shows that many accidents are caused by vehicle turning at road intersections [3]. Therefore, it is an important issue to control vehicle behavior properly in such situations, moreover, controlling of vehicle turning at a road intersection is one of most complicated problems to be solved in an ADV scenario. The problem mainly consists of three basic issues, i.e., target lane selection, trajectory planning and calculation, and vehicle controlling and tracking. In this paper, strategy, model and algorithms concerning intersection vehicle turning control of ADVs are proposed for above three fundamental problems. The MPC (model predictive control) [4] paradigm is used as vehicle upper layer controller. Simulation is conducted on CarSim-Simulink platform with typical intersection scenes [5]. The main contributions of this paper are to: 1. Propose an approach to problem of controlling turning maneuver at intersections for ADV scenarios, which is based on V2X communication instead of various sensing systems, such as lidar, millimeter radar, and video system. It could be expected that cost of cars could be reduced significantly with such a solution. 2. Propose a simple and feasible strategy for target lane selection considering characteristics of fully ADV scenarios. Target lane selection is a relatively difficult problem in a non-fully ADV scenario. 3. Design and implement an MPC-based upper layer controller for vehicle self-driving and conduct extensive simulations by CarSim-Simulink cross platform. The rest of this article is organized as follows: The research works related to ours are explored in Section 2. Section 3 discusses ADV scenarios and turning control. In Section 4, simulation verification and data analysis of proposed method are carried out. Finally, conclusion and furr research direction are given. 2. Related Work The pioneer manufacture of ADV now develops cars that will drive toger with human-driving vehicles on roads. They do not reveal detail of ir solutions, such as trajectory calculation and vehicle control. However, it could be speculated that y prefer a general solution suitable to all road scenarios, rar than considering particular road shapes such as cross road or intersection [6]. This is due to two factors. First, re is no easy way currently to acquire enough traffic instruction information and to detect road shape reliably before VANETs are deployed or highly detailed electronic maps are available. Secondly, random events or behaviors of human driving make it quite difficult to make control decisions that are effective in a whole procedure, for example, target lane selection when ADVs are turning. In order to generate a correct vehicle turning which can prevent risk of collision, most important issue is geographical trajectory planning and calculation for vehicles. There are a lot of literatures reporting works related to turning trajectory, but y are mainly for transportation or road design [7 10]. For example, [7,8] use chart analysis to show that trajectories might be quite different for different drivers according to actual traffic data. By analyzing and clustering highly diversifying real turning tracks of human driving, [9] models turning track by a Euler curve connected with a straight line at each end, to obtain continuously varying curvature. Later a five-segment turning

Sensors 2021, 21, 3995 3 of 16 trajectory is proposed, by inserting a circular arc between two Euler curves [10]. To form a driving trajectory from geographical track, relationship of velocity or acceleration versus time has to be established. The study [10] presents mean velocity profiles for different vehicle types and road geometry by clustering method. A cubic function of speed change at turning intersection is proposed in [11], many constraint conditions determine each parameter in this cubic function, for example, residual coefficient, vehicle speed and acceleration, and unknown quantities. These unknown quantities reflect behavioral differences caused by individual characteristics and inter attributes of drivers. These differences are modeled as random variables, and finally, parameters are determined by statistical method. In order to solve problem of multilane vehicle path conflict, [12] makes each turning vehicle choose a fixed lane by setting left turn guide lines. In [13], lane selection behavior of traffic flow from branch road to trunk road at four continuous intersections of urban trunk road is measured. Lane selection behaviors are divided into two parts: temporary lane selection behavior and target lane selection behavior. Considering different factors that influence two parts above, such as drivers characteristics, lane attributes, expected maximum utility of direct lane, etc., a joint probability model is constructed to select lane with highest probability for each vehicle. The third issue is controlling of vehicle. The popular structure of controlling unit is hierarchical [14 21]. That is, upper control layer is mainly to generate appropriate control variables, usually including acceleration and front-wheel angle. The lower control layer is a vehicle dynamics controller that converts outputs of upper layer to actuator input variables, such as throttle opening, steering wheel angle, etc. The work of this paper mainly focuses on design of upper controller, letting lower control layer be implemented on a matured vehicle dynamic simulation platform such as CarSim. The main target of upper layer controller is to track its reference input. Currently, commonly used methods include classical proportional integral differential (PID) control [14,15], sliding mode control [16,17] and model predictive control (MPC) [4,18 21]. The paper [14] uses PID to realize vehicle s longitudinal upper control on automatic highway. The study in [15] designs a forward control strategy and completes formation coordination adaptive cruise system by improving PID proportion coefficient. Neverless, PID upper layer controller normally considers only error feedback and has a relatively fixed structure. Therefore, performance control effect is not satisfactory when external conditions change. Hence, a scheme called sliding mode control has been proposed that does not require a fixed system structure and has advantages of fast response and insensitive to disturbance. The work in [16] uses a cascade control system in which inner loop uses sliding mode control to ensure trajectory tracking of formation. In [17], a coupled sliding mode control method is proposed to improve control performance and stability of two-way platoon. However, this method needs to overcome chattering when approaching equilibrium point. The most recent works [4,18 21] are based on MPC, which is a class of optimization-based control paradigms with flexible structure and objective function. The study in [4] constructs a quadratic objective function to minimize trajectory error, and objective functions of [18] and [19] are based on fuel efficiency, and [20,21] proposes a predictive controller derived from infinite norm with aim to ensure that distance between front and rear vehicles is always greater than minimum safe distance [20]. Several or works have introduced data-driven methods to predict turning vehicle trajectories and vehicle control. The host vehicle determines turning trajectory by training front vehicle trajectory in data set [22,23] and relies on cameras, radar, GPS, and or sensors to finish vehicle control [24]. Generally speaking, this kind of vehicle turning control relying on a large number of sensors has high complexity, accuracy of perceived peripheral information cannot be guaranteed, and cost is also significantly higher than that based on V2X communication [25].

GPS, and or sensors to finish vehicle control [24]. Generally speaking, this kind of vehicle turning control relying on a large number of sensors has high complexity, accuracy of perceived peripheral information cannot be guaranteed, and cost is also Sensors 2021, 21, 3995 significantly higher than that based on V2X communication [25]. 4 of 16 To best of our knowledge, however, re is no literature focusing on fully ADV scenarios where VANET could be expected to be an essential infrastructure of future transportation system. Such an expectation motivates us to address issue with different To trains best of thought our knowledge, and different however, solutions. re is no literature focusing on fully ADV scenarios where VANET could be expected to be an essential infrastructure of future 3. Turning transportation Control for system. ADV Such Scenarios an expectation motivates us to address issue with different trains of thought and different solutions. As mentioned, turning control at intersection mainly has three basic problems to 3. solve, Turning i.e., Control target lane for ADV selection, Scenarios trajectory planning and calculation, and vehicle controlling and tracking, which are addressed in this section. It should be clarified first that As mentioned, turning control at intersection mainly has three basic problems to design or approach proposed in this paper is based on following considerations or solve, i.e., target lane selection, trajectory planning and calculation, and vehicle controlling assumptions: and tracking, which are addressed in this section. It should be clarified first that design The or computational approach proposed load of algorithms in this paper is as islow based possible on following so that y considerations could run on or assumptions: inexpensive embedded systems, while maintaining real time processing capability. The By computational using a dedicated load positioning of algorithms system, is as low instead as possible of GPS so or that mentioned y could sensors, run on inexpensive future VANET embedded could provide systems, position while maintaining information real-time for all processing vehicles as capability. accurate as By to using tens of acentimeters dedicated positioning (actually, such system, a positioning instead of device GPS or is under mentioned developed sensors, [26,27]). future Information VANETexchanged could provide over position or provided information by VANET for all should vehicles be as asless accurate as possible as to tens since of centimeters wireless channel (actually, capability such a positioning would be device a bottleneck is under of developed ADV application [26,27]). in Information crowded traffic exchanged cases. over or provided by VANET should be as less as possible since wireless channel capability would be a bottleneck of ADV application in crowded 3.1. traffic Scenario cases. Description and Problem Formulation As shown in Figure 1, we consider a signalized intersection with multiple turning 3.1. lanes Scenario and multiple Description target andlanes. Problem Road Formulation side units (RSUs) are distributed among both sides of Asroad, shownand in Figure all vehicles 1, weon consider road a signalized are ADVs intersection which are equipped with multiple with on board turning lanes units and (OBUs). multiple The target communication lanes. Roaddelay side units and transmission (RSUs) are distributed contents loss among can both be not sides considered road, in and all turning vehicles scene. on For road se areconditions, ADVs which this are paper equipped designs witha on-board turning control units of (OBUs). system The that communication can achieve delay following and transmission three functions. contents loss can be not considered in (1) turning According scene. to For se driving conditions, requirement, this paper vehicles designs entering a turning intersection control system can that realize can achieve automatic following control three of functions. turning left, turning right and turning around. Figure 1. A typical multi lane intersection scenario. Figure 1. A typical multi-lane intersection scenario. (1) According to driving requirement, vehicles entering intersection can realize automatic control of turning left, turning right and turning around. (2) When turning vehicles are released from multiple turning lanes of signalized intersection, turning control system needs to solve problem of path conflict among turning vehicles to balance exit traffic flow as much as possible, and to consider as few lane changes as possible after turning.

Sensors 2021, 21, 3995 5 of 16 Sensors 2021, 21, 3995 (2) When turning vehicles are released from multiple turning lanes of signalized 5 ofin tersection, turning control system needs to solve problem of path conflict 16 among turning vehicles to balance exit traffic flow as much as possible, and to consider as few lane changes as possible after turning. (3) In process of turning, vehicles should shorten distance from vehicles in (3) In process of turning, vehicles should shorten distance from vehicles in front as far as possible to improve traffic efficiency of intersection with front as far as possible to improve traffic efficiency of intersection with condition of ensuring safe distance from vehicles in front. condition of ensuring a safe distance from vehicles in front. To realize vehicle control more effectively, vehicles approaching intersections can can re receive essential messages from fromrsus. RSUs. Basic Basic motion motion states states and and some some additional additional data data of each of each vehicle vehicle can be canalso be also transferred transferred to each to each or or by byobus. OBUs. In In this this paper, RSUs are are only only required to to send send messages to to OBUs without receiving back back from from OBUs, which will will significantly reduce workload of of RSUs. RSUs. The The format format and and content of of messages from from RSUs RSUs and and OBUs OBUs are are shown shown in in Table Table 1. 1. In an RSU message, number of starting lanes, target lanes and coordinates of Table 1. Messages defined for RSU and OBU. stop line of each road will be broadcast. The state can be eir yes or no to indicate wher access RSU in this Message direction. The remaining time (RT) OBU is Message remaining time till state changes. The combination of direction, state and remaining time can indicate Number of lanes Vehicle ID remaining time of green light in each direction. Location of Stop line Location In an OBU message, Direction vehicle ID, location, and velocity are Velocity basic information of vehicles. The status State indicates behavior that host vehicle is Status expected to take, whose value can be 1, Remaining 0, or 1, representing Time decelerating to stop, keeping Moment running in current motion states, or accelerating till desired velocity, respectively. Moment tells when to change In an RSU behavior, message, which number is related ofto starting status. lanes, It stands target for lanes decelerating and coordinates moment of when stop linestatus of each equals road 1, will and befor broadcast. accelerating The state moment can bewhen eir yes statues or no equals to indicate 1. wher access in this direction. The remaining time (RT) is remaining time till Table 1. Messages defined for RSU and OBU. state changes. The combination of direction, state and remaining time can indicate remaining time RSU of green Message light in each direction. OBU Message In an OBU Number message, of lanes vehicle ID, location, and velocity are Vehicle basic ID information of vehicles. The status indicates behavior that host vehicle is expected to take, whose Location of Stop line Location value can be 1, 0, or 1, representing decelerating to stop, keeping running in current Direction Velocity motion states, or accelerating till desired velocity, respectively. Moment tells when to change behavior, State which is related to status. It stands for decelerating Status moment when status equals Remaining 1, and for Time accelerating moment when Moment statues equals 1. 3.2. Driving Control Frame of of Turning Vehicle The turning control frame proposed in this paper is is shown in in Figure 2. 2. It It is is mainly composed of five modules: target lane selection, driving trajectory planning, controller reference input calculation, MPC controller, and plant. In order to to solve problem of of vehicles trajectory conflict in in multiple turning lanes, this paper first designs a target lane selection algorithm that generates coordinates of of exit exit point for for each vehicle. Figure 2. System structure of turning vehicle for ADV. Figure 2. System structure of turning vehicle for ADV. The trajectory planning module establishes geographical movement track from turning starting point to endpoint. Then reference input controller calculation module generates geographical motion trajectory with a timestamp, to form reference trajectory. In this paper, a hierarchical design is adopted for path tracking. The upper layer controller uses MPC module to generate control variables, acceleration, and front-wheel direction. The lower layer controller completes vehicle dynamics control, relying on dynamic simulation platform CarSim mainly, which is plant module in

Sensors 2021, 21, 3995 6 of 16 block diagram. The peripheral vehicle detection module in our system needs just information of position, speed, and heading values of or vehicles to detect wher surrounding environment is abnormal in process of vehicle turning, which could be acquired through vehicle to vehicle (V2V) communications, but needed ego vehicle state information such as pose, can be provided by local OBU. The v-state planning module broadcasts state of car that mainly refers to when car in front changes its state. For example, status = 1 and Moment = t Ad indicate that car in front starts to accelerate at time t Ad, which is convenient for car behind to plan trajectory. 3.3. Target Lane Selection As mentioned above, purpose of target lane selection is to solve multilane turning vehicle path conflict, ensure traffic efficiency, and make vehicles change lanes as few as possible after turning. Therefore, our lane selection algorithm is applied before vehicles enter intersection, and it mainly depends on direction of motion of vehicles at next intersection and lane selection of vehicles ahead. The specific design taking left turn as an example is as follows: assume that re are M left turning lanes and N target lanes at current intersection (se data are obtained by RSU broadcast at intersection). The core idea of lane selection scheme is to transform M-to-N lane selection problem into a 1-to-N lane selection problem, and vehicles in each left source lane do not affect each or, so scheme needs to receive road selection results of front vehicle in current left turn lane only. In addition, few vehicles turn left at next intersection when turning left at current intersection, so allocation of target lane should be inclined from second lane. The specific steps are as follows: Step 1: The target lane is divided into M parts averagely, and average number of lanes corresponding to each source lane is k = N/M. Step 2: Since it is not exactly evenly divided, remaining margin of target lane is T = N k*m. Step 3: Each vehicle builds a queue of length M in its own OBU. Firstly, initial value of each queue is k, that is [k, k, k k], where number of elements is M. Then, margin of target lane is allocated from second lane. Finally, following queue can be obtained in all OBUs, that is: [k, k + 1, k + 1, k + 1, k k] (1) }{{} T } {{ } M Step 4: Each OBU converts queue to target lane required by current host vehicle. Step 5: Call lane selection function of 1-to-N, and choose empty lane close to its target lane each time. As for lane selection function of 1-to-N, it is relatively simple since it is not involving problem of path conflict. The vehicles are firstly grouped according to N, and rear vehicle selects lane that front vehicle does not select but that is closest to desired lane. For example, if vehicle will turn right at next intersection, it expects lane closest to right naturally. If ahead vehicles in same group did not select rightmost lane, current turning vehicle will select that lane. 3.4. Trajectory Planning and Calculation Driving track planning only considers geographical track that vehicle should follow. On this track, when and where vehicle starts and stops, speed of vehicle for each position is analyzed and controlled as separate problems. Here ideal track of a turning vehicle can be assumed to be an arc combined with a straight line. Its rationality mainly lies in two points: (I) driver s turning track is close to arc in most turning scenes; and (II) steering wheel angle is basically fixed in actual turning (that is, front

Sensors 2021, 21, 3995 7 of 16 wheel angle is basically same). According to Ackerman s steering geometry idea [28], we have R = L/δ,where L denotes longitudinal distance in meters between center of front and rear wheels, δ denotes front wheel deflection in radians, and R denotes turning radius in meters. If speed is basically fixed, its corresponding track is a certain arc. However, if terrain is too limited to follow arc, or if vehicle detects a danger to surrounding vehicles while making turn, we need to replan trajectory (see Figure 2). Here this paper focuses on revealing a scenario in which a turn can be completed in an arc combined with a straight line. Corresponding to our algorithm, OBU receives four key point coordinates broadcasted by RSUs, including location coordinates of starting road stop point X s (x 0,y 0 ), starting road extension line one point X s1 (x 1,y 1 ), ending road stop point X f (x N0,y N0 ), and one point of ending road extension line X f1 (x N1,y N1 ). Here, extended line point is not arbitrarily selected, but preselected by RSU. It can represent yaw angle of road combined with stop line point. The coordinates here are GPS positions. Taking X s as an example, x 0 is longitude of position while y 0 is latitude, and ors are similar. The outputs are position of starting road stop X s, ending road stop X f, arc start A arc end B center of a circle T and corner radius R start road yaw angle ϕ s, terminal road yaw angle ϕ f. The yaw angle here refers to angle between vehicle s main body direction and north pole direction. Moreover, it should be noted that point A and point X s, point B and point X f may coincide. The specific algorithm implementation is shown as follows. Step 1: A straight line is determined according to two points. Thus, yaw angle of starting road is determined from stop point of starting road and extension line of road. The yaw angle of terminal road is determined from stop point of terminal road and extension line of road. Step 2: Calculate linear equation of starting road and ending road. They are L 1,L 2. Step 3: Calculate linear equation of stop line of start road and end road. Step 4: Calculate intersection coordinate M(x M, y M ) of two roads. Save intersection point if re is an intersection. Orwise, two roads are parallel. Step 5: The distance between center of circle and two roads is equal, and determined circle must be inscribed to lane line, so unique circle is determined. Thus, center of circle and radius of arc can be obtained. (T, R) = f (L 1, L 2 ) (2) Step 6: Since arc track is determined, starting point and ending point of arc in whole turning process are obtained. The above algorithm is applicable to left turn, right turn, and U-turn. When difference between ϕ s and ϕ f is π or π, it means a U-turn. Similarly, if difference is between 0 and π, or between 2π and π, it means turning left; orwise, it means turning right. The driving track planning above has planned out geographical movement track, turning speed limit and acceleration of turning vehicle at current intersection should be considered next to make geographical path time stamped. Therefore, this paper will introduce controller reference input calculation algorithm from above two aspects respectively as follows. 3.4.1. Maximum Speed of Turning Vehicle The maximum speed limit of turning vehicles mainly depends on terrain and performance of vehicles. The speed limit that most vehicles can reach will be adopted here. At this speed, re will be no sideslip during turning process of vehicles. Corresponding to our algorithm, it is minimum of following two values: (I) speed limit for turning vehicles at current intersection, and (II) literature [28,29] give speed limit under steady-state steering characteristics by using a vehicle model with two

Sensors 2021, 21, 3995 8 of 16 degrees of freedom [28,29]. The relation between speed and turning radius is formulated as follows. R = L δ (1 + Kv2 ) (3) Transformed from Formula (3), relation between speed and turning radius can be obtained: R δ/l 1 v = (4) K where K is called stability factor of a vehicle, and it is defined as follows. K = m L 2 ( l f k 2 l r k 1 ) (5) where k 1, k 2 denote cornering stiffness of front and rear wheels; l f and l r are distance from center of mass to center of front and rear wheels, L is wheelbase length of vehicle, and m is its mass. Suppose all parameters of vehicle can be obtained from its electronic units and turning radius can be obtained from RSU, Equation (4) will determine one speed limit v h for turning vehicle. On or hand, re might be a turning speed limitation v l given also by intersection RSU, refore an actual maximal value could be obtained by selecting a minimal one, that is: v cmax = min(v l, v h ) (6) The calculated result is basically consistent with actual driving turning speed. 3.4.2. Acceleration Model of Turning Vehicle Generally speaking, when entering intersection, initial speed v 0 is not more than maximum turning speed v cmax. In this way, our problem is transformed into speed change from v 0 to v cmax. This paper uses idea of clustering seen in Reference [9], which clusters average acceleration of vehicles at multiple traffic intersections, and n fits relationship between average acceleration value a and road turning radius R to construct function. a = g(r) (7) At this time, using calculated mean acceleration to replace acceleration change of whole intersection can not only simplify vehicle control process, but also be easier to achieve in era of electric vehicles. Thus, we can get speed change process of whole intersection. v(t) = v 0 + at (8) When vehicle speed reaches v cmax, vehicle passes through intersection at a constant velocity v cmax. 3.5. Vehicle Controlling and Tracking Since vehicle does not follow our reference trajectory without error at time, application of MPC in this paper is mainly to assist upper dynamic adjustment control of vehicle. The control principle of MPC is shown in Figure 3. Firstly, real-time state value and expected state value of controlled object (notice that expected trajectory is discretized to get known) are taken as input of MPC controller. Then, prediction module in MPC controller calculates state values of future N p time points according to state update equation as Formula (9). Finally, optimization module of MPC controller establishes loss function according to minimum error value between predicted state value and expected state value and solves control input value applied to controlled object.

Sensors 2021, 21, 3995 and expected state value of controlled object (notice that expected trajectory is discretized to get known) are taken as input of MPC controller. Then, prediction module in MPC controller calculates state values of future Np time points according to state update equation as Formula (9). Finally, optimization module of MPC controller establishes loss function according to minimum error value between predicted state value and expected state value and solves control input value applied to controlled object. 9 of 16 Figure 3. MPC control principle. Figure 3. MPC control principle. Here, state values this paper selects include lateral position error, longitudinal Here, state values this paper selects include lateral position error, longitudinal position error and yaw angle error and ir derivative, which is expressed by ξ = position error and yaw angle error and ir derivative, which is expressed by ξ y,x,ψ,ψ.,y,x, outputted control variables are current front wheel angle and acceleration, ẋ, ψ, ψ, Y, expressed X) T, as outputted uδ, a. Referring controlto variables vehicle are dynamics current equation front [30 32], wheel angle and (ẏ, acceleration, following state expressed transfer equation as u(δ, is a). obtained: Referring to vehicle dynamics equation [30 32], following state transfer equation is obtained: ( k1) A( k) Bu( k) D (9a) ξ(k + 1) = Aξ(k) + Bu(k) + D 1 T 0 0 0 0 T*(2* Cf 2* Cr) T*(2* Cf 2* Cr) T*(2* Cf * lf 2* Cr * l ) 1 T 0 0 r 0 0 1 000 m* vx m m* v x 0 1 T (2 C f +2 C r ) T (2 C f +2 C r ) T (2 C f l f 2 C r l r ) m v x m m v x 0 0 0 0 1 T 0 0 A A = 0 0 1 T 0 0 2 T*(2* Cf * lf 2* Cr * lr) T*(2* Cf * lf 2* Cr * lr) T*(2* Cf * lf 2* Cr * lr ) T (2 C 0 1 0 0 0 f l f 2 C r l r ) T (2 C f l f 2 C r l r ) I z v x I z 1 T (2 C f l 2 f +2 C r lr 2 ) I z v x 0 0 Iz * vx Iz Iz * v x 0 0 0 0 1 T 0 0 0 0 0 1 T 0 0 0 0 1 0 0 0 0 1 0 0 0 (9b) v x T 0 T (2 C f l f 2 C r l) 2 C f T m v x 0 m 0 0 T*(2* Cf * lf 2* Cr * l) vx * T 2* Cf * T D = 0 0 0 T (2 C m* v 0 f l f 2 C r l r ) ; B = 2 T C f l f. x m I z v x I z 0 0 0 0 0 0 0 D T*(2* Cf * lf 2* Cr * lr) ; B. T 2* T* Cf * 0lf T 0 The cost Iz * function vx is constructed according I z to principle of minimum error. 0 0 0 T N p 0 T min J(k) = ξ(k + i 2 k) u(k) Q + N c u(k + i t) 2 R + ρε2 i=1 i=1 s.t. u min (k + j) < u(k + j) < u max (k + j) u min (k + j) < u(k + j) < u max (k + j) where j = 0,1,2,... Nc 1 and i k stands for ith prediction step at time step k. N p represents prediction horizon length. N c represents control horizon length. The parameters Q, R, and ρ [0, 1] are chosen in order to have a good trade-off among reference trajectory, gap policy tracking and actuators excitation. The parameter ε is relaxation factor, which makes optimization function solvable. After solving Equation (10) in each control cycle, a series of control input increments in control time domain are obtained: (9a) (9b) (10) U t = [ u t, u t+1, u t+2,, u t+n c 1] T (11) The first element in control sequence is acted on system as actual control input increment, that is: u(t) = u(t 1) + u t (12)

in control time domain are obtained: * * * * * Ut ut, ut 1, ut2,, u tnc 1 (11) The first element in control sequence is acted on system as actual control Sensors 2021, 21, 3995 10 of 16 input increment, that is: ut () ut ( 1) ut After entering next control cycle, it it repeats above process, so as so to asrealize to realize trajectory tracking control of vehicle. 4. 4. Simulink and Experimental Results In this section, we make some simulation experiments to test our algorithm as aswell as as MPC MPC controller CarSim-Simulink. on CarSim Simulink. The The quality quality of of communication is is assumed ideal, ideal, traffic scenes traffic scenes are simulated are simulated to testto lane test selection lane selection algorithm algorithm and vehicle and vehicle turning turning control with control right-hand with right hand traffic rules traffic such rules assuch China, as China, United United States, States, etc. etc. 4.1. Target Lane Selection Simulation As shown in Figure 4, re are seven vehicles from three three different lanes lanes in in same same direction. The width of each lane is isset to tobe be3.5 3.5 m, m, and and three three left left lanes lanes and and five five target target lanes lanes are to be selected. Taking first car in in first lane lane as as an an example, Y axis Y-axis coordinate of vehicle traveling on left-side left side through lane is set 0, X-axis X axis coordinate is also set 0. set More 0. More vehicle vehicle fundamental parameters and and initial initial conditions are are shown shown in Tables in Tables 2 and 2 3 respectively. and 3 respectively. The performance The performance parameters parameters of vehicle of vehicle selected selected in CarSim in CarSim are shown are in Table shown 3, in where Table L, 3, l f where, l r, k 1 and L, lf, klr, k1 2 areand k2 same are as same meaning as meaning formula in formula (5) and (5) I z denotes and I moment denotes of inertia moment about of inertia Z-axis. about Z axis. z T * (12) Figure 4. The initial scene of multi lane simulation. Figure 4. The initial scene of multi-lane simulation. Table 2. Vehicle starting positions in multilane simulation. Vehicle No. 1 2 3 4 5 6 7 X(m) 0 10 8 18 0 10 20 Y(m) 0 0 3.5 3.5 7 7 7 Table 3. Vehicle dynamic parameters. Vehicle Mass Body Size L l f l r k 1 k 2 I z 1723 kg 5 m 2.6 m 1.232 m 1.468 m 66,900 N/rad 42,700 N/rad 4175 kg m 2 The simulation results are shown in Figure 5. All vehicles in leftmost lane choose target lane 1, red vehicle C3 and purple vehicle C4 in second left turn lane choose target lane 2 and 3 respectively, green vehicle C5 in third left turn lane chooses target lane 5, yellow vehicle C6 and blue vehicle C7 in third left turn lane choose target lane 4. According to previous target lane selection strategy, 3- to 5-lane selection scheme should correspond to following: first left turn lane corresponds to target lane 1 only, second left turn lane corresponds to target lane 2 and 3, and third left turn lane corresponds to target lane 4 and 5. The results in Figure 5 correspond to expected behavior of vehicles precisely.

target lane 2 and 3 respectively, green vehicle C5 in third left turn lane chooses target lane 5, yellow vehicle C6 and blue vehicle C7 in third left turn lane choose target lane 4. According to previous target lane selection strategy, 3 to 5 lane selection scheme should correspond to following: first left turn lane corresponds to target lane 1 Sensors 2021, 21, 3995 only, second left turn lane corresponds to target lane 2 and 3, and third left turn 11 of 16 lane corresponds to target lane 4 and 5. The results in Figure 5 correspond to expected behavior of vehicles precisely. Figure 5. Track coordinates of all seven vehicles. Figure 5. Track coordinates of all seven vehicles. 4.2. 4.2. Turning Maneuver Simulation According to results of of target target lane lane selection selection above, above, M to N M-to-N lane selection lane selection prob problem can be cantransformed be transformed into a 1 to N into a 1-to-N lane selection lane selection problem, problem, and vehicles and in different vehicles in different lanes do lanes not affect do not each affect or, each refore, or, this refore, paper furr this paper verifies furr turning verifiesmaneuver turning maneuver control of control two vehicles of two in vehicles same in lane. The same speed lane. of The speed front vehicle of front is v1, vehicle target is lane v 1, target is roadselect1, lane is roadselect1, speed of speed rear of vehicle rear is v2, vehicle and is v target lane is roadselect2. Notice, 2, and target lane is roadselect2. Notice, roadselect1 roadselect1 can be can same be as same or different as or different from roadselect2. from roadselect2. The performance The performance metrics chosen to compare control configurations are error of tracking reference trajectory and metrics chosen to compare control configurations are error of tracking reference trajectory safe distance. and safe distance. The control configurations analyzed in comparisons are three turning scenarios. The control configurations analyzed in comparisons are three turning scenarios. The following turning scenarios are considered. The following turning scenarios are considered. 1. L TC: left turn control scenario. The driving behaviors of both front and rear vehicles are to turn left, and rear vehicle needs to make a strategic judgment on wher it can pass intersection at current moment and control safe distance. If it can pass current intersection, and its speed is less than maximum speed of left turn scenario, it accelerates to maximum turning speed with fixed acceleration, and n drives at a constant speed. Considering control of waiting area included in left turn control scenario here, this paper distinguishes it by flag bit of strategy judgment, that is flagleftturn (one has to note that re is red light control for left turn by default.). If flagleftturn = 1, it means that vehicles can turn left through current intersection, and if flagleftturn = 2, it means that vehicle stops in waiting area, orwise, it means that it cannot pass. 2. R-TC: right turn control scenario. The driving behaviors of both front and rear vehicles are turning right. Here we need to consider wher re is red light control scenario, and this paper uses flag bit flagrightturn to distinguish. Generally, re is no waiting area for right turn, so flag bit flagrightturn = 1 indicates that vehicle can turn right through current intersection. On contrary, it means that it cannot pass. 3. U-TC: U-turn control scenario. It is similar to right turn scenario. Considering wher re is red light control, flaguturn = 1 indicates that vehicle can turn around and pass current intersection, orwise it cannot pass. Furrmore, in order to more accurately reflect performance of algorithm designed in this paper, we also need to consider performance of control algorithm in different road scenarios, such as different front and rear vehicle speeds, different distances between two vehicles when front and rear vehicles just enter intersection, and different turning radii of road. Here, this paper intercepts several typical intersection

Sensors 2021, 21, 3995 12 of 16 road scenarios of Hefei City as virtual experimental roads. Through field measurement, turning radius at intersection of Danxia Road and Bainiao road is 11 m, that at intersection of Jinzhai South Road and Ziyun road is 25 m, and that at intersection of Fanhua Avenue and Jinzhai south road is 35 m. The selection of distance and speed of front and rear vehicles are shown in Table 4 below. Additionally, it is worth noting that rear vehicle here is host vehicle. Table 4. Front and host vehicle speed and distance between host vehicle and front vehicle. vpre vego d1 d2 vl R Direction 0 km/h 40 km/h 0 km/h 20 km/h 40 km/h 0 km/h 0m 80 m 0m 17 m 15 m 1.5 m 40 km/h 40 km/h 30 km/h 35 m 25 m 6m Left Right U-turn where v pre denotes speed of front vehicle, vego denotes host vehicle speed, d1 represents distance from front vehicle to stop line, d2 represents distance from host vehicle to front vehicle, and R is road radius. vl denotes max velocity of vehicle on road. 4.2.1. Verify Effectiveness Sensors 2021, 21, 3995 Sensors 2021, 21, 3995 The verification of effectiveness for proposed model is mainly conducted under CarSim17, which is a widely used platform to simulate vehicle motion. This paper uses 13 of 16 CarSim17 and Simulink for CO-Simulation, and uses XY_Graph, scope, matlabfunction to 13 of 16 process simulation data. The specific results are shown in Figures 6 8. Here, this paper selects position, speed of car behind, and distance from front vehicle under track as observation measures to test effectiveness of algorithm. (a) (b) (a)turn left test result: (a) rear track image; (b) front and rear(b) Figure 6. vehicle speed. Figure 6. Turn leftleft testtest result: (a) rear track image; (b) (b) front andand rearrear vehicle speed. Figure 6. Turn result: (a) rear track image; front vehicle speed. (a) (b) Figure 7.(a) Turn right test result: (a) rear track image; (b) front and rear(b) vehicle speed. Figure 7. Turn right test result: (a) rear track image; (b) front and rear vehicle speed. Figure 7. Turn right test result: (a) rear track image; (b) front and rear vehicle speed.

(a) (b) Sensors 2021, 21, 3995 13 of 16 Figure 7. Turn right test result: (a) rear track image; (b) front and rear vehicle speed. (a) (b) Figure Figure 8. U turn 8. U-turn test test result: result: (a) rear (a) rear track track image; image; (b) front (b) front and and rear rear vehicle vehicle speed. speed. The The front front vehicle vehicle in Figure in Figure 6 starts 6 starts from from stop stop line, line, distance distance between between rear rear car car and and front front car car is 17 ism, 17and m, and speed speed of ofrear rear vehicle vehicle is 20 is km/h. 20 km/h. However, However, in Figure in Figure 7, 7, initial initial speed speed of of front and front rear andvehicle rear vehicle speed speed is 40 km/h, is 40 km/h, distance distance between between two is two 15 m, is 15 m, distance distance between between front front vehicle vehicle and and stop stop line line is 80 ism. 80 m. Neverless, front vehicle starts from stop line just right, and rear vehicle is also still, with a distance of 1.5 m from front vehicle in Figure 8. Figure 6 shows that behavior of car behind is to keep a constant speed for a period of time, n decelerate until a state where distance from vehicle in front is equal to fixed safety time distance, and speed is same as that of car in front, n keep a constant speed for t h seconds at current velocity, and n accelerate to maximum speed limit. According to formulas (4) and (7), when turning radius is 6 m, acceleration is 0.5 m/s 2, and maximum turning speed is 10.0 km/h. When turning radius is 25 m, acceleration is 1 m/s 2 and maximum turning speed is 28.0 km/h, and when radius is 35 m, acceleration is 1.25 m/s 2 and maximum turning speed is 36.2 km/h. In behavior depicted in Figure 7, it firstly maintains current speed for 8 s, n decelerates evenly for 1.3 s, whose acceleration is 1 m/s 2, which means that it decelerates until stop line is just equal to v cmax, and n passes intersection at a constant speed. Because initial distance between vehicles ahead and behind is greater than fixed time interval, front vehicle begins to decelerate after sending deceleration time stamp to rear car. At this time, rear car calculates its behavior according to current distance. As for behavior depicted in Figure 8, since static safety distance is fixed at beginning, rear car first stops for t h seconds, n accelerates to maximum speed limit, while car in front accelerates directly to maximum speed limit. 4.2.2. Test Performance of Proposed Method As shown in Figures 6 8, furr analysis of error between actual track and reference track can be made. This paper takes rear car as object of explanation and uses scope function to draw error between actual track and reference track at each moment. As shown in Figures 9 and 10, we can see that maximum error at driving with uniform speed is 0.35 m, and maximum error at driving with variable speed is 0.61 m. This paper maintains consistency of system parameters including turning left, turning right, and U-turn. Furrmore, this paper analyzes safety of algorithm. Taking left turn as an example shown in Figure 11, distance between host car and front car is described, where blue solid line refers to actual distance. The orange dash dot line represents fixed time interval distance, and red dotted line represents THW distance which is defined by minimum safety distance of forwarding collision, which is d min = v rel * T thw, here T thw is 1.2 s [33]. The actual distance curve is always

safety distance is fixed at beginning, rear car first stops for th seconds, n accel erates to maximum speed limit, while car in front accelerates directly to maximum speed limit. 4.2.2. Test Performance of Proposed Method Sensors 2021, 21, 3995 14 of 16 As shown in Figures 6 8, furr analysis of error between actual track and reference track can be made. This paper takes rear car as object of explanation and uses scope function to draw error between actual track and reference higher track at than each moment. minimum As shown distance in curve. Figures In 9 a and word, 10, we security can see that of algorithm maximum can error be guaranteed. at driving with Moreover, uniform speed distance 0.35 between m, and front maximum and rear error vehicles at driving controlled with variable by control speed is algorithm 0.61 m. This close paper to maintains fixed time consistency interval distance, of system which is parameters distance including control strategy turning left, commonly turning adopted right, and by U turn. formation now [14]. Sensors 2021, 21, 3995 Figure 9. Tracking range error under uniform speed. 15 of 16 Furrmore, this paper analyzes safety of algorithm. Taking left turn as an example shown in Figure 11, distance between host car and front car is described, where blue solid line refers to actual distance. The orange dash dot line represents fixed time interval distance, and red dotted line represents THW distance which is defined by minimum safety distance of forwarding collision, which is dmin = vrel * Tthw, here Tthw is 1.2 s [33]. The actual distance curve is always higher than minimum distance curve. In a word, security of algorithm can be guaranteed. Moreover, distance between front and rear vehicles controlled by control algorithm is close to fixed time interval distance, which is distance control strategy Figure commonly 10. Tracking adopted range by error formation under variable now speed. [14]. Figure 11. Distance image of front and rear vehicles. 5. Conclusions 5. Conclusions This paper studies a practical turning vehicle control algorithm based on internet of This paper studies a practical turning vehicle control algorithm based on internet vehicles. According to information obtained by V2V and V2I communication, including of vehicles. According to information obtained by V2V and V2I communication, including surrounding vehicles, road information, etc., this article designs turn lane surrounding vehicles, road information, etc., this article designs turn lane selection scheme, and establishes trajectory planning and upper MPC vehicle control model. selection scheme, and establishes trajectory planning and upper MPC vehicle control Finally, in order to verify reliability and efficiency of algorithm, this paper uses model. Finally, in order to verify reliability and efficiency of algorithm, this paper simulation platform CarSim to carry out simulation test. The experimental results show uses simulation platform CarSim to carry out simulation test. The experimental that algorithm has good reliability and robustness. results show that algorithm has good reliability and robustness. There is still more interesting work to be furr studied. As this paper experiments There is still more interesting work to be furr studied. As this paper experiments only on our simulation platform, we will furr consider real vehicle test and improve only control on our performance. simulation platform, Through we will actual furr vehicle consider data, corresponding real vehicle test parameters and improve of algorithm control performance. will be furr Through debugged. actual vehicle data, corresponding parameters of algorithm will be furr debugged. Author Contributions: Conceptualization, Z.D.; methodology, Z.D., C.S. and M.Z.; software, C.S, Z.L. and C.W.; formal analysis, Z.D., C.S. and M.Z.; writing original draft preparation, Z.D. and C.S.; writing review and editing, Z.D., C.S. and M.Z.; funding acquisition, Z.D. All authors have