Motion Planning for Autonomous Vehicles in Unanticipated Obstacle Scenarios at Intersections Based on Artificial Potential Field

Featured Application

This work designed a motion planning algorithm for autonomous vehicles in unanticipated obstacle scenarios. In standard driving scenarios, the proposed motion planning algorithm plans a trajectory that complies with intersection regulations, including lane-marking, recommended turning lane, traffic light, right-of-way, and no-parking rules. In unanticipated obstacle scenarios, after the necessity of obstacle avoidance is identified, the ego vehicle would break the rules temporarily to ensure the safety and mobility of autonomous vehicles.

Abstract

In unanticipated obstacle scenarios at intersections, the safety and mobility of autonomous vehicles (AVs) are negatively impacted due to the conflict between traffic law compliance and obstacle avoidance. To solve this problem, an obstacle avoidance motion planning algorithm based on artificial potential field (APF) is proposed. An APF-switching logic is utilized to design the motion planning framework. Collision risk and travel delay are quantified as the switching triggers. The intersection traffic laws are digitalized and classified to construct compliance-oriented potential fields. A potential violation cost index (PVCI) is designed according to theories of autonomous driving ethics. The compliance-oriented potential fields are reconfigured according to the PVCI, forming violation cost potential fields. A cost function is designed based on compliance-oriented and violation cost potential fields, integrated with model predictive control (MPC) for trajectory optimization and tracking. The effectiveness of the proposed algorithm is verified through simulation experiments comparing diverse traffic law constraint strategies. The results indicate that the algorithm can help AVs avoid obstacles safely in unanticipated obstacle scenarios at intersections.

1. Introduction

Autonomous vehicles (AVs) have developed rapidly in recent years. For a foreseeable period, autonomous driving will coexist with human drivers on the roads [1]. Differences in behavioral rules between them will lead to mutual misunderstanding and mistrust [2], which can cause suspicion, confusion, and even lead to danger. Consequently, AVs should be regulated by the same traffic regulations as human drivers.

Road traffic laws establish good driving behaviors that vehicles should follow, but the regulatory factors are sometimes broken by other factors such as moral factors and contingency [3,4]. The uncontrollable behaviors of other traffic participants cause them to become unanticipated obstacles that negatively impact the safe passage of the surrounding vehicles [5], making it necessary for surrounding vehicles to take illegal actions to avoid obstacles [6]. Rigidly obeying traffic laws will not optimally achieve the ultimate goal of integrating AVs into the road traffic system [7].

In 2021, German legislation acknowledged that human drivers occasionally need to violate specific traffic laws to meet other driving requirements [8]. The European Union Horizon Commission Expert Group recognized that pursuing greater road safety sometimes necessitates non-compliance behaviors. The group recommended that AVs can decide to violate traffic laws if they can provide explanations, leaving the justice system to determine if the violation was reasonable [9]. In the context of Level 4 and above-level autonomous vehicles, a human driver is not required to take charge when faced with some specific driving scenarios. Addressing unanticipated obstacle scenarios necessitates trajectory planning that incorporates regulatory factors for both extrication and obstacle avoidance. This imperative ensures the continuous regulatory awareness of autonomous vehicles across diverse driving environments, posing a significant challenge in the practical realization of autonomous driving systems. This paper primarily investigates the development of a trajectory planning framework to address the aforementioned functionalities, integrating regulatory considerations into both general and obstacle avoidance trajectory planning.

1.1. Related Works

The existing traffic laws compliance motion planning approaches can be broadly divided into AI-based, rule-based, and optimization-based approaches. Most AI-based approaches represent traffic laws as constraints or reward functions. Constrained Markov decision processes are employed to train decision strategies, including methods based on Lagrange multipliers [10,11] and constraint strategy optimization [12]. These methods can effectively handle straightforward traffic laws. Moreover, considering penalties for violating traffic laws in the reward function is also a common approach [13]. However, due to the black-box nature inherent in AI-based approaches, their ability to meet the stringent reliability requirements of autonomous driving systems necessitates further research. Compared to AI-based approaches, rule-based approaches offer superior controllability and interpretability. Rule-based approaches incorporate traffic laws into motion planning and have already found several successful applications. These predominantly encompass discrete input space planning based on collision detection [14], finite state machine [15], constraint optimization [16], and reachable sets [17]. Lu et al. [18] utilized static traffic laws as the foundation for generating a set of candidate behaviors. It comprehensively considers factors such as efficiency and safety, allowing vehicles to determine the optimal decision while complying with traffic laws and having high consistency with manual decision results. Ma et al. [19] combined rule-based approaches with optimization-based approaches, formulating strategies for compliance state transitions. The constraints and references in motion planning are modified based on predictions of non-compliant behaviors, enabling AVs to obey traffic laws on the highway. However, most rule-based approaches need typical hand-crafted rule design, which adds a degree of difficulty to devising decision-making solutions in complex scenarios, especially for some unforeseen scenarios.

Optimization-based approaches integrate multiple optimization objectives into a cost function, adjusting weights accordingly, thus prioritizing diverse driving goals [20,21]. Maurer et al. incorporate traffic laws into the cost function, while obstacle avoidance serves as the constraint in motion planning [22,23]. Wang et al. [24], Bae et al. [25], and Liu et al. [26] utilized the artificial potential field (APF) to represent traffic laws and integrate them into the cost function. This approach allows for the successful execution of the driving task while considering various factors. These methods consider traffic laws as high-cost factors, particularly in the context of prohibited regulations, such as solid line and stop line adherence. Esterle et al. [27] utilized game theory to optimize multi-objective interactive behaviors, considering collision and violation in a reward function. Vosahlik et al. [28] proposed a minimum violation planning framework that assigns higher weights to collision avoidance and lower weights to traffic law compliance in the cost function. This permits AVs to breach traffic laws to ensure safety. However, assigning lower weights to traffic law compliance potentially leads to violations, even in non-conflict scenarios. For example, the vehicle could have evaded the collision by abruptly braking within the lane but instead opted for a trajectory that transgressed the solid line.

Table 1. Relative research comparison.

Consider Factors	Prioritize Safety	Prioritize Traffic Laws	Balance Motion Planning in Conflict Scenarios
Study	Wang et al. (2019) [24]	Lu et al. (2022) [18]	Maurer et al. (2016) [22,23]
	Bae et al. (2019) [25]	Liu et al. (2023) [13]	Esterle et al. (2020) [27]
	Manzinger et al. (2020) [17]	Ma et al. (2023) [19]	Vosahlik et al. (2022) [28]
	Muraleedharan et al. (2021) [20]	Liu et al. (2023) [26]
	Dini et al. (2022) [21]

lists recent trajectory planning research, offering an analysis of the factors taken into account by each method. To summarize, most of the research regards traffic laws as strong constraints or high-cost factors and does not account for behaviors that extend beyond traffic rules, thereby reducing the effectiveness of decision-making in conflict scenarios. Additionally, a limited number of methods address the intricate balance required between compliance and other driving requirements when confronted with specific scenarios, and also neglect to investigate the regulatory factors pertinent to obstacle avoidance behavior in this scenario.

1.2. Contributions

To solve the above issues, this paper focuses on the unanticipated obstacle scenarios at intersections, utilizing APF-switching logic to design the motion planning framework. Time to Collision (TTC) and obstructed delay time are quantified as switching triggers. The intersection traffic laws are digitized and classified to extract basic violation propositions. Compliance constraints are proposed according to the types of these propositions, including lane line constraints, stop line constraints and speed constraints. Compliance-oriented potential fields are constructed to accomplish the constraint tasks. According to theories of autonomous driving ethics, the potential violation cost index (PVCI) is designed, which includes the consideration of collision risk, violation degree, and violation penalties. Violation cost potential fields are constructed by truncating the compliance-oriented potential field according to the PVCI. A cost function is designed based on compliance-oriented and violation cost potential fields, integrated with model predictive control (MPC) for trajectory optimization and tracking. The main contributions of this paper are summarized below.

Compliance constraints are generated based on the standardized mathematical description process, which digitizes natural language traffic laws into mathematical logic language. To incorporate traffic law compliance into motion planning, compliance-oriented potential fields are designed according to compliance constraints and relevant traffic laws.

This paper proposes a potential violation cost index to assess the impact of necessary violations. Then, the high-cost compliance-oriented potential field is truncated according to the PVCI, forming a lower-cost violation cost potential field. This allows the vehicle to plan an effective obstacle avoidance path in unanticipated obstacle scenarios with lower violation costs.

1.3. Paper Organization

The remainder of this paper is structured as follows. An unanticipated obstacle scenario is illustrated and the obstacle avoidance principle in this scenario is proposed in Section 2. The obstacle avoidance motion planning framework is designed in Section 3. The obstacle avoidance motion planning method based on APF is proposed in Section 4. The proposed algorithm is validated in Section 5. Finally, this study is concluded in Section 6.

2. Unanticipated Obstacle Scenario and Obstacle Avoidance Principle

2.1. Unanticipated Obstacle Scenario

Figure 1 illustrates a realistic and common unanticipated obstacle scenario at the intersection inlet lane. The global path planning for the ego vehicle is to turn left. According to the road sign requirements, the vehicle enters the left-turn guidance lane delineated by a solid white line on its right side and a double solid yellow line on its left side, as shown in Figure 1a. Since the traffic light is red at this point and there are already other vehicles waiting in the lane, the ego vehicle queues up, as shown in Figure 1b. When the traffic light turns green, vehicles enter the intersection in order. At this moment, the vehicle in front of the ego vehicle suddenly breaks down and cannot move. It becomes a road obstacle that blocks the passage of the ego vehicle, as shown in Figure 1c.

This scenario involves multiple driving requirements, including safety, mobility, and traffic law compliance. Different decision priorities lead to different behaviors. One engineering design option is to program the ego vehicle to prioritize the ability to obey traffic laws. This means continuous waiting behind the obstacle vehicle, which negatively impacts the mobility and safety of surrounding vehicles. Another approach is to program the vehicle to prioritize the ability to continue moving. This means entering adjacent lanes and proceeding around obstacles while remaining safe. This action will cause the ego vehicle to briefly violate the lane marking rules. Unanticipated obstacles could lead to a conflict between traffic law compliance and obstacle avoidance. AVs have to weigh the tension between safety, mobility, and traffic law compliance in this scenario [29].

2.2. Obstacle Avoidance Principle

For AVs, the dual primary objectives of obstacle avoidance are to guarantee vehicle safety and fulfill transportation tasks successfully. The factors that need to be considered in motion planning for AVs in unanticipated obstacle scenarios include:

• Driving safety: Safety takes precedence when carrying out any driving task, especially when obstacle avoidance involves a violation. It is critical to avoid collision caused by the violation.
• Impact on the transportation environment: Indefinite standstills can lead to traffic congestion and negatively impact traffic flow. Violations undertaken to address conflicts can impact the normal operation of surrounding vehicles. The extent of violations should be minimized.
• Legal liability: Violations will result in appropriate penalties that are closely related to traffic safety. Less penalized violations should be prioritized to accomplish obstacle avoidance.

Maurer [22] incorporates ethical considerations into motion planning and control. Traffic law compliance and obstacle avoidance represent two crucial aspects of autonomous vehicles (AVs), corresponding to consequentialism and deontology, respectively. Based on this idea, the obstacle avoidance principle in unanticipated scenarios is proposed. First of all, it is essential to ensure the safety of AVs. Secondly, the impact on the traffic environment is taken into account. Finally, the legal liability associated with AVs should be considered.

The same principle is equally applicable to the vehicles that play different social roles, taking an on-duty ambulance as an illustrative example. While the ambulance has the discretion to deviate from traffic laws, it does not imply that reckless driving is permissible. Ensuring safety remains imperative. Moreover, its actions should consider the potential implications on the traffic ecosystem, which indirectly influences its safety and efficiency.

3. Obstacle Avoidance Motion Planning Framework

The planning framework is shown in Figure 2. It consists of a model predictive controller and an APF-switching trigger. The compliance-oriented potential fields and the violation cost potential fields are integrated with the cost function of the MPC. TTC and obstructed delay time are quantified as the APF-switching triggers. In normal driving scenarios, the compliance-oriented potential fields are constructed based on environmental information for compliance motion planning. When obstacle avoidance becomes necessary, the system transitions from compliance-oriented potential fields to violation cost potential fields for obstacle avoidance motion planning.

Collision risk arises from violations or aggressive driving behaviors of other vehicles. In this study, TTC is utilized as the indicator for assessing collision risk. The trajectories of the ego vehicle and surrounding vehicles are obtained from the predictive information. An obstacle avoidance is necessary when the TTC is smaller than a certain threshold. The judgment is based on:

$$\mathrm{T}\mathrm{T}\mathrm{C}<t_{\mathrm{safe}}$$

(1)

where t_safe denotes the minimum safe time and is set to 1.6 s according to [30].

The accumulation of obstructed delay time begins when the vehicle decelerates due to interference from other traffic participants and continues until it resumes normal driving. The obstructed delay time is determined according to the pseudocode Algorithm 1 provided.

Algorithm 1 Determination of Obstructed Delay Time

Input: TrafficLight, Ego, Tgtf, vego, vtgtf, tnow, tstop Output: tobs 1: tobs = 0; tstop = 0; 2: If (TrafficLight = G) then 3:   If (vego = 0) then 4:     If (exist Tgtf ∧ vtgtf = 0) then 5:       tobs = tnow − tstop; 6:     end if 7:   else 8:    tobs = tobs; 9:    end if 10: else 11:   tobs = tobs; 12: end if 13: return tobs;

where TrafficLight denotes the traffic light state, and G denotes the green light. Ego and Tgt_f denote the ego vehicle and the other vehicle in front. v_ego and v_tgtf denote the speed of the Ego and Tgtf, respectively. t_now and t_stop denote the current time and the time at which the vehicle first stops because an obstacle vehicle is in front. The judgment is based on:

$$t_{\mathrm{obs}}>t_{\mathrm{con}}$$

(2)

where t_obs and t_con denote the obstructed delay time and the threshold of congestion. In this study, t_con is set to 55 s according to China’s related requirements, namely, the Evaluation Methods for Road Traffic Congestion Levels [31].

The above two conditions are related to the safety and mobility of vehicles, respectively. The logical formula for potential field switching is designed as:

$$U_{\mathrm{law}}=\left\{\begin{array}{cc}{U}_{\mathrm{com}},& (\mathrm{T}\mathrm{T}\mathrm{C}>t_{\mathrm{safe}})\wedge (t_{\mathrm{obs}}<t_{\mathrm{con}})\\ U_{\mathrm{vio}},& \mathrm{else}\end{array}\right.$$

(3)

where U_law represents the potential fields associated with traffic laws involved in motion planning. U_com and U_vio represent the compliance-oriented and violation cost potential fields, respectively.

4. Obstacle Avoidance Motion Planning Method Based on APF

4.1. Vehicle Dynamic Model

A bicycle model is employed for modeling vehicle dynamics. The corresponding equations of the vehicle dynamic model are as follows:

$$\begin{array}{c}m\left(\dot{u}-vr\right)=F_{x_T}\end{array}$$

(4)

$$\begin{array}{c}m\left(\dot{v}+ur\right)=F_{y_f}+F_{y_r}\end{array}$$

(5)

$$\begin{array}{c}{I}_z\dot{r}=l_f{F}_{y_f}-l_r{F}_{y_r}\end{array}$$

(6)

$$\begin{array}{c}\dot{\theta }=r\end{array}\hspace{1em}\begin{array}{c}\dot{\theta }=r\end{array}$$

(7)

$$\begin{array}{c}\dot{X}=u\mathit{cos}\theta -v\mathit{sin}\theta \end{array}$$

(8)

$$\begin{array}{c}\dot{Y}=v\mathit{cos}\theta +u\mathit{sin}\theta \end{array}$$

(9)

where u and v are the longitudinal velocity and lateral velocity. r is the yaw rate at the center of gravity (CG); X and Y are the longitudinal position and lateral position in the global coordinate; θ is the heading angle of the vehicle; m is the mass of the vehicle; I_z is the moment of inertia of the vehicle around its vertical axis; l_f and l_r are the distances from the front axle to CG and from the rear axle to CG, respectively; ${\mathit{F}}_{{\mathit{x}}_{\mathit{T}}}$ is the total longitudinal force of the tires; ${\mathit{F}}_{{\mathit{y}}_{\mathit{f}}}$ and ${\mathit{F}}_{{\mathit{y}}_{\mathit{r}}}$ are the total lateral force of the front and rear tires, which are defined as:

$$\begin{array}{c}{F}_{y_f}=C_f{\alpha }_f=C_f\left(\delta -\frac{v + l_{f}r}{u}\right)\end{array}$$

(10)

$$\begin{array}{c}{F}_{y_r}=C_r{\alpha }_r=C_r\left(\delta -\frac{v + l_{r}r}{u}\right)\end{array}$$

(11)

where C_f and C_r are the cornering stiffness values of the front and rear tires; α_f and α_r are the sideslip angles of the front and rear tires; δ is the front wheel steering angle.

By linearizing Equations (4)–(9) around the vehicle’s operating point, the vehicle linear dynamics can be written in state space form as follows:

$$\begin{array}{c}\dot{x}=Ax+Bu\end{array}$$

(12)

$$x=[XuYv\theta r{]}^T$$

(13)

$$u_c=[F_{x_T}\delta {]}^T$$

(14)

where x is the state vector; u_c is the input vector; A and B are the state matrix and input matrix, respectively. The model is discretized by the zero-order hold method, to be utilized as the model of the model predictive path planning controller.

4.2. Compliance-Oriented Potential Field

Passing through intersections involves multiple traffic laws. Although specific provisions in the traffic laws differ across countries and regions, a significant degree of consistency in fundamental principles is shared. Through analysis, seven common types of violations at intersections are categorized: speeding, failure to stop, failure to yield, improper turn, marked lane violation, illegal stopping, and illegal reversing. Based on the standardized mathematical description process proposed in the previous work [32], the traffic laws involved at intersections are digitized to form logical propositions for violation judgments. The basic violation propositions of common violations are extracted. The expressions are shown in

Table 2. Basic violation propositions and their digital representations.

Basic Violation Proposition	Digitized Expression
Crossing the lane line	$overlap\left(area\right(ego),laneline)$
Crossing the stop line	$overlap\left(line\right(mfrntp\left(ego\right),crdn\left(ego\right)),stopline)$
Speed violation	$\left(vx\right(ego)>v{x}_{upper})\vee \left(vx\right(ego)<v{x}_{lower})$

Where ego denotes the ego vehicle; the atomic proposition crdn(obj) denotes the geometric center coordinates of the traffic participant obj; mfrntp(obj) denotes the coordinates of the midpoint of obj’s front end; vx(obj) denotes the longitudinal velocity of obj; vx_upper and vx_lower denote the upper and lower bounds of the speed limit, respectively; area(obj) denotes the area structure that includes the vertex coordinate structure of obj; line(p₁, p₂) denotes the line between points p₁ and p₂; overlap(r₁, r₂) judges the overlap status of areas r₁ and r₂; stopline and laneline denote stop line and lane line, respectively.

.

Three basic violation propositions are constrained by three types of compliance-oriented potential fields: lane-marking potential field, stop-line potential field, and speed-limit potential field. In this study, the design of the compliance-oriented potential field is based on China’s traffic law, specifically, the Regulation on the Implementation of the Law of the People’s Republic of China on Road Traffic Safety [33] and the Road traffic signs and markings—Part 3: Road traffic markings [34]. The overall compliance-oriented potential field is the sum of the compliance-oriented potential functions (PF):

$$U_{\mathrm{com}}=\sum _i{U}_{{\mathrm{laneline}}_i}+\sum _j{U}_{{\mathrm{stopline}}_j}+\sum _m{U}_{{\mathrm{speed}}_m}$$

(15)

where U_laneline, U_stopline, and U_speed are lane marking potential PF, stop line PF, and speed limit PF; i, j, and m denote the index of the U_laneline, U_stopline, and U_speed, respectively.

4.2.1. Lane-Marking Potential Field

Types of lane markings include solid lines, broken lines, and virtual lane lines. According to [34], solid lines are non-crossable, and broken lines allow vehicles to cross for short periods when appropriate. Virtual lane lines are used to guide vehicles along the optimal path during a turn at an intersection, according to [33]. Optimal turning path information can be provided by high-definition maps [35,36]. In order to handle the driving constraints of different types of lane markings in an autonomous driving system, a non-crossable potential field is used to model the solid line, and a crossable potential field is used to model the broken line and virtual lane line.

The non-crossable potential field ensures that the vehicle stays within the lane. The distance from the front tires to the solid line is used as the independent variable of the PF. The distance can be determined as:

$$\begin{array}{c}{d}_L^{\left(t+k,t\right)}=\left\{\begin{array}{cc}{d}_{\mathrm{NC},\mathrm{l}}-d_{\mathrm{fl}}^{\left(t+k,t\right)},& \mathrm{left}\\ d_{\mathrm{fr}}^{\left(t+k,t\right)}-d_{\mathrm{NC},\mathrm{r}},& \mathrm{right}\end{array}\right.\end{array}$$

(16)

where (t + k, t) denotes the kth time step ahead of the current time t in the prediction horizon; “left” and “right” indicate the lane markings on the left and right of the vehicle; d_fl and d_fr are the lateral positions of the left and right front wheels, respectively; d_NC,l and d_NC,r are the lateral positions of the left and right non-crossable lane markings, respectively. When d_L decreases to a certain extent, the cost should increase rapidly with the decreasing distance. Therefore, the inverse proportional function and power function are utilized to design the PFs for the non-crossable lane markings. The design of the non-crossable lane-marking potential field is based on the method proposed in [26]. The PF of the non-crossable lane marking is as follows:

$$U_{\mathrm{N}{\mathrm{C}}_i}^{\left(t+k,t\right)}=\left\{\begin{array}{cc}{A}_{\mathrm{NC}},& d_{L_i}^{\left(t+k,t\right)}\le d_{\mathrm{m},\mathrm{NC}}\\ \frac{a_{\mathrm{NC}}}{(d_{L_i}^{\left(t+k,t\right)}{)}^{b_{\mathrm{NC}}}}-e_{\mathrm{s},\mathrm{NC}},& d_{\mathrm{m},\mathrm{NC}}<d_{L_i}^{\left(t+k,t\right)}<d_{\mathrm{in},\mathrm{NC}}\\ 0,& d_{\mathrm{in},\mathrm{NC}}\le d_{L_i}^{\left(t+k,t\right)}\end{array}\right.,$$

(17)

where d_m,NC and d_in,NC are the minimal distance and the influence distance; a_NC and b_NC are the intensity and shape parameters of the PF. The smooth parameters guarantee the continuity of the PF value when ${\mathrm{d}}_{{\mathrm{L}}_{\mathrm{i}}}^{\left(\mathrm{t}+\mathrm{k},\mathrm{t}\right)}$ switches to another condition. The smooth parameter e_s,NC and the maximum amplitude A_NC are defined as:

$$e_{\mathrm{s},\mathrm{NC}}=\frac{a_{\mathrm{NC}}}{D_{\mathrm{in},\mathrm{NC}}^{b_{\mathrm{NC}}}}$$

(18)

$$A_{\mathrm{NC}}=\frac{a_{\mathrm{NC}}}{D_{\mathrm{m},\mathrm{NC}}^{b_{\mathrm{NC}}}}-e_{\mathrm{s},\mathrm{NC}}$$

(19)

The visualization of the non-crossable lane-marking potential field is shown in Figure 3, where the minimal and influence distance is set to 0.1 m and 1.5 m, respectively. The intensity and shape parameters of the solid line PF are set to 0.2 and 2.0, respectively.

The crossable lane-marking potential field is used to constrain the vehicle to travel within the lane and, at the same time, is crossable for the vehicle to change lanes. Therefore, the crossable lane-marking potential field is designed based on Gaussian-like functions. The PF of the crossable lane marking is as follows:

$$U_{C,i}^{(t+k,t)}=A_C\mathit{exp}\left(-\frac{(d_L^{(t+k,t)}-d_{C,i})}{2{\sigma }^2}\right)$$

(20)

where A_C is the maximum amplitude of the crossable lane markings PF; d_C is the lateral position of the lane markings; σ is the rate of rise that is related to the lane width. The visualization of the crossable lane-marking potential field is shown in Figure 4, where A_C is set to 2 and σ is set to 0.3 times the lane width.

4.2.2. Stop-Line Potential Field

Types of stop lines include marked stop lines and virtual stop lines. The virtual stop line is adopted as the basis for the right-of-way yielding line and the safety following line. When the trajectory of the ego vehicle intersects with that of the vehicle possessing the right-of-way, a virtual yield line is created perpendicular to the ego vehicle’s direction at the point of intersection. Subsequently, a potential field for the yield stop line is established based on this virtual line.

The distance between the foremost part of a vehicle and the stop line is used as the independent variable of the stop line PF, which is determined as:

$$\begin{array}{c}{s}_S^{\left(t+k,t\right)}=s_f^{\left(t+k,t\right)}-s_s\end{array}$$

(21)

where s_f and s_s are the longitudinal position of the foremost part of the vehicle and the longitudinal position of the stop line, respectively. The stop-line potential field is also regarded as non-crossable. The inverse proportional function and power function are utilized. The stop line PF is designed as:

$$U_{\mathrm{SL}}^{\left(t+k,t\right)}=\left\{\begin{array}{cc}{A}_{\mathrm{SL}},& s_S^{\left(t+k,t\right)}\le s_{\mathrm{m},\mathrm{SL}}\\ \frac{a_{\mathrm{SL}}}{(s_S^{\left(t+k,t\right)}{)}^{b_{\mathrm{SL}}}}-e_{\mathrm{s},\mathrm{SL}},& s_{\mathrm{m},\mathrm{SL}}<s_S^{\left(t+k,t\right)}<s_{\mathrm{in},\mathrm{SL}}\\ 0,& s_{\mathrm{in},\mathrm{SL}}\le s_S^{\left(t+k,t\right)}\end{array}\right.$$

(22)

where a_SL and b_SL are the intensity and shape parameters of the PF; s_m,SL is the minimal distance and is set to 0.1 m; the influence distance s_in,SL is defined according to the length of the guide lane. The smooth parameter e_s,NC and the maximum amplitude A_NC are defined as:

$$\begin{array}{c}{e}_{\mathrm{s},\mathrm{SL}}=\frac{a_{\mathrm{SL}}}{S_{\mathrm{in},\mathrm{SL}}^{b_{\mathrm{SL}}}}\end{array}$$

(23)

$$A_{\mathrm{SL}}=\frac{a_{\mathrm{SL}}}{S_{\mathrm{m},\mathrm{SL}}^{b_{\mathrm{SL}}}}-e_{\mathrm{s},\mathrm{SL}}$$

(24)

The visualization of the stop-line potential field is shown in Figure 5, where the intensity and shape parameters of the stop line PF are set to 0.2 and 2.0, respectively.

When driving in a queue sequence, it is necessary to maintain a safe following distance with the vehicle in front. The independent variable of the car-following PF is defined as:

$$\begin{array}{c}{s}_F^{\left(t+k,t\right)}=s_f^{\left(t+k,t\right)}-s_{\mathrm{m},\mathrm{CF}}\end{array}$$

(25)

where s_f is the longitudinal position of the foremost part of the ego vehicle; s_m,CF is the longitudinal position of the safe following line.

The foundation of car-following PF is the Yukawa potential, with a slower rate of rise at a moderate distance. This property allows for a smoother transition between different driving regimes, such as free flow, synchronized flow, and congested flow. The car-following PF is defined as:

$$\begin{array}{c}{U}_{\mathrm{CF}}^{\left(t+k,t\right)}=A_{\mathrm{CF}}\frac{e^{-a_{\mathrm{CF}}{s}_F^{\left(t+k,t\right)}}}{s_F^{\left(t+k,t\right)}}\end{array}$$

(26)

where A_CF and a_CF are the maximum amplitude and the scale factor of the car-following PF. The obstacle PF employed in this study is based on [37]. The visualization of the car and car-following potential field is shown in Figure 6, where A_CF and a_CF are set to 2.0 and 0.5, respectively.

4.2.3. Speed-Limit Potential Field

For violations of exceeding the upper speed limit and falling below the lower speed limit at intersections, the speed-limit potential field is constructed using increasing and decreasing exponential functions, respectively.

Vehicle speed is used as the independent variable of the speed limit PF, which is defined as:

$$U_{\mathrm{speedlimit}}=\eta (\mathit{exp}(v^{(t+k,t)}-v_{\mathrm{limit}})-1)$$

(27)

where $\mathsf{\eta }$ is a scaling factor. v_limit is the speed limit of the road. This function results in less impact on vehicle speed when it is under the speed limit. When exceeding the speed limit, there is a rapid escalation in the cost incurred.

Moreover, no-parking zones and no-reversing zones are distinctly designated within crosswalks and intersections. The speed constraint PF in no-parking and no-reversing zones is defined as:

$$U_{\mathrm{propel}}=\eta \cdot a^v$$

(28)

where a and $\mathsf{\eta }$ are the base of the function and the scaling factor. In this study, a is set to 0.8. The value of the potential field increases as the vehicle approaches a stop.

4.3. Violation Cost Potential Field

The violation cost potential field aims to reduce the violation cost during obstacle avoidance under necessary violations. The compliance-oriented potential field of the corresponding traffic law is truncated according to the PVCI, forming the violation cost potential field, which is defined as:

$$U_{vlaneline}=\mathit{min}(\mathrm{P}\mathrm{V}\mathrm{C}{\mathrm{I}}_{laneline},U_{laneline})$$

(29)

$$U_{vstopline}=\mathit{min}(\mathrm{P}\mathrm{V}\mathrm{C}{\mathrm{I}}_{stopline},U_{stopline})$$

(30)

$$U_{vspeed}=\mathit{min}(\mathrm{P}\mathrm{V}\mathrm{C}{\mathrm{I}}_{speed},U_{speed})$$

(31)

$$U_{vio}=\sum _i{U}_{vlanelin{e}_i}+\sum _j{U}_{vstoplin{e}_i}+\sum _m{U}_{vspee{d}_m}$$

(32)

where PVCI_laneline, PVCI_stopline, and PVCI_speed are the calculated PVCI values for the crossing lane line, crossing stop line, and speed violation; U_vlaneline, U_vstopline, and U_vspeed are the violation cost potential field of the lane line, stop line, and vehicle speed; i, j, and m denote the index of the U_vlaneline, U_vstopline, and U_vspeed, respectively; U_vio is the overall violation cost potential field. In this way, the non-crossable compliance-oriented potential field is changed to a crossable violation cost potential field, allowing the vehicle to make effective obstacle avoidance motion planning in unanticipated obstacle scenarios.

4.3.1. Collision Risk

The predicted collision risk is determined based on TTC. The vehicle trajectory intersections are obtained by considering the current and predicted motion states of the ego vehicle and other traffic participants. The predicted collision risk R in PVCI is defined as:

$$R=\frac{\mathrm{T}\mathrm{T}{\mathrm{C}}_{\mathrm{s}\mathrm{t}\mathrm{d}}}{\mathrm{T}\mathrm{T}{\mathrm{C}}_i}$$

(33)

where TTC_i is the collision time between the ego vehicle and the i^th obstacle vehicle. TTC_std is the standard collision time. The TTC_std values are defined as 2.5 s, 3 s, and 3.5 s when the relative speed falls below 10 m/s, between 10–15 m/s, and exceeds 15 m/s, respectively [18].

4.3.2. Violation Degree

The degree of violation is quantified by measuring the states of vehicles that deviate from the compliance range. Data normalization of the degree of violation is performed to enable effective comparisons between different types and magnitudes of violations. The measurement methods for different types of violations are shown in

Table 3. Measurement methods of violation degree.

Violation Type	Degree Calculation
Speeding	$\frac{v_x\left(ego\right)-v{x}_{upper}}{v{x}_{upper}}$
Failure to stop	$\left\{\begin{array}{cc}\frac{s\left(ego\right)+\frac{1}{2}l\left(ego\right)-s\left(stopline\right)}{l\left(ego\right)}& \mathrm{stopline}\\ \frac{s\left(ego\right)+\frac{1}{2}l\left(ego\right)-\left(s\right(tg{t}_f)-l(tg{t}_f)}{d_{\mathrm{safe}}}& \mathrm{follow}\end{array}\right.$
Failure to yield	$\frac{s\left(ego\right)+\frac{1}{2}l\left(ego\right)-s\left(vstopline\right)}{l\left(ego\right)}$
Marked lane violation	$\left\{\begin{array}{cc}\frac{d\left(ego\right)+\frac{1}{2}w\left(ego\right)-d\left(lanelin{e}_l\right)}{w\left(ego\right)}& \mathrm{left}\\ \frac{d\left(lanelin{e}_r\right)-d\left(ego\right)+\frac{1}{2}w\left(ego\right)}{w\left(ego\right)}& \mathrm{right}\end{array}\right.$
Improper turn	$\left\{\begin{array}{cc}\frac{d\left(ego\right)+\frac{1}{2}w\left(ego\right)-d\left(vlanelin{e}_l\right)}{w\left(ego\right)}& \mathrm{left}\\ \frac{d\left(vlanelin{e}_r\right)-d\left(ego\right)+\frac{1}{2}w\left(ego\right)}{w\left(ego\right)}& \mathrm{right}\end{array}\right.$
Illegal reversing	$\frac{s_{reverse}\left(ego\right)}{l\left(ego\right)}$
Illegal stopping	$\frac{s\left(ego\right)+\frac{1}{2}l\left(ego\right)-s\left(noparking\right)+\frac{1}{2}l\left(noparking\right)}{l\left(ego\right)}$

Where s(obj) and d(obj) denote the longitudinal and lateral coordinates of the object obj, respectively; l(obj) and w(obj) denote the geometrical length and width of the obj, respectively; tgt_f denotes the other traffic participant located directly in front of the ego vehicle; d_safe denotes the safe following distance. vstopline denotes the virtual stop line; laneline_r and laneline_l denote the right and left lane lines of the lane where the obj is located; vlaneline_r and vlaneline_l denote the right and left lane lines of the virtual lane where the obj is located; s_reverse(obj) denotes the distance that the obj is reversing; noparking denotes the no-parking zone.

.

4.3.3. Violation Penalty

The severity of a violation is closely related to the type of violation, and an intuitive indicator is the penalty that corresponds to the type of violation. To incorporate the violation penalty into autonomous driving motion planning, the basic violation propositions are validated in the first place. Then, the violation objects are identified to determine the specific violations. The consequences of different violations are specified according to the relevant penalty regulations.

This study consults the penalties for violations specified in the relevant Chinese laws, the Measures for the Administration of Points Assigned for Road Traffic Violations [38] and the Road Traffic Safety Violations, Codes, and Penalty Scoring Standards [39]. The penalties for violations are cumulative. Certain violations do not result in the assignment of demerit points, instead resulting in warnings and monetary penalties. This study solely focuses on the deduction of driving license points. Specific violations and their penalties are listed in

Table 4. Penalties for violations.

Violation Object	Violation Type	Penalties
Lane line
Solid write line	Lane markings violation	1
Double solid yellow line	Lane markings violation Wrong-way driving	1 3
Broken line	Long-time drive on line	0
Virtual line	Improper turn	0
Stop line
Marked stop line	Run a red light	6
Virtual stop line	Fail to yield Tailgating	3 0
speed
$v{x}_{upper}<vx\left(ego\right)$	Speeding	0–6
$vx\left(ego\right)<0$	Reverse at intersection	0
$vx\left(ego\right)=0$	Stop at no-parking zone	0

.

Collision risk, violation degree, and violation penalty are extracted to constitute the PVCI, based on the principle of obstacle avoidance in unanticipated obstacle scenarios. The PVCI is defined as:

$$\mathrm{P}\mathrm{V}\mathrm{C}\mathrm{I}=f(R,D,P)=k_{R}R+k_{D}D+k_{P}P$$

(34)

where k_R, k_D, and k_P are the weights of PVCI related to predicted collision risk R, violation degree D, and violation penalty P, respectively.

4.4. Optimization

A model predictive controller is used for iterative optimization of vehicle dynamics, compliance and obstacle avoidance paths, and trajectory tracking over the predicted time horizon.

Before engaging in trajectory planning using MPC, it is essential to establish a global path that outlines the desired lane and desired speed for the MPC. The desired output to be tracked is defined as:

$$y_{des}=[Y_{\mathrm{des}}{u}_{\mathrm{des}}{]}^T$$

(35)

where y_des is the output matrix; Y_des and u_des are the desired lateral position and desired longitudinal velocity.

The driving behaviors of the vehicle are categorized into two scenarios according to the planning framework, each corresponding to a distinct optimization equation. The optimization problem of motion planning is:

$$\underset{u,\epsilon }{min}\sum _{k=1}^{Np}{U_{\mathrm{com}}}^{(t+k,t)}+U_{\mathrm{obs}}^{(t+k,t)}+{‖y^{(t+k,t)}-y_{\mathrm{des}}^{(t+k,t)}‖}_Q^2+{‖u^{(t+k-1,t)}‖}_R^2+{‖u^{(t+k-1,t)}-u^{(t+k-2,t)}‖}_S^2+{‖{\epsilon }_k‖}_P^2$$

(36)

$$\begin{gathered} \underset{u,\epsilon }{min}\sum _{k=1}^{Np}{U_{\mathrm{vio}}}^{(t+k,t)}+U_{\mathrm{obs}}^{(t+k,t)}+{‖y^{(t+k,t)}-y_{\mathrm{des}}^{(t+k,t)}‖}_Q^2+{‖u^{(t+k-1,t)}‖}_R^2+{‖u^{(t+k-1,t)}-u^{(t+k-2,t)}‖}_S^2+{‖{\epsilon }_k‖}_P^2 \\ s.t.\left(k=1,\dots ,N_p\right) \end{gathered}$$

(37)

$$x^{\left(t+k,t\right)}=A_d{x}^{\left(t+k-1,t\right)}+B_d{u}^{\left(t+k-1,t\right)}$$

(38)

$$y^{\left(t+k,t\right)}=C{x}^{\left(t+k,t\right)}+D{u}^{\left(t+k,t\right)}$$

(39)

$$u_{c\text{\_}\min }<u_c^{\left(t+k-1,t\right)}<u_{c\text{\_}\max }$$

(40)

$$\mathsf{\Delta }{u}_{c\text{\_}\min }<u_c^{\left(t+k-1,t\right)}-u_c^{\left(t+k-2,t\right)}<\mathsf{\Delta }{u}_{c\text{\_}\max }$$

(41)

where N_p is the prediction horizon, set to 20; N_c is the control horizon, set to 5; ε_k is the vector of slack variables at the kth step. The states are predicted through Equation (38), where A_d and B_d are the discrete state and input matrices by discretizing Equation (12). The tracking outputs are calculated by Equation (39), in which C and D are the output and feedforward matrices. The control inputs and their changes are constrained in Equations (40) and (41). ${\mathit{u}}_{\mathit{c}\text{\_}\min }$ and ${\mathit{u}}_{\mathit{c}\text{\_}\max }$ are the matrices of the lower and upper bounds of the control input; $\mathsf{\Delta }{\mathit{u}}_{\mathit{c}\text{\_}\min }$ and $\mathsf{\Delta }{\mathit{u}}_{\mathit{c}\text{\_}\max }$ are the matrices of the lower and upper bounds of the control input changes; Q, R, S, and P are weighting matrices.

In normal driving scenarios, Equation (36) is utilized as the cost function within the MPC for compliance motion planning. In unanticipated obstacle scenarios, Equation (37) is utilized for obstacle avoidance motion planning. The PFs integrated with the optimization problem are nonlinear and non-convex. The method proposed by Rasekhipour et al. [37] is employed to convert the optimization problem into an approximately quadratic convex.

5. Case Study

MATLAB/Simulink 2021a is used to code the motion planning algorithm. The computer used has an 11th Gen Intel(R) Core(TM) i7-11800H processor and 32 GB of memory (Intel Corporation, Santa Clara, CA, USA). The scenario illustrated in Section 2 and its extension are simulated. To verify the proposed algorithm, comparative tests are designed by changing the traffic law constraint strategy of the ego vehicle. The parameters relevant to the path planning controller for the ego vehicle are extracted from a Chevrolet Equinox model within the CarSim 2021.0 software, and the parameters are shown in

Table 5. Parameters of the ego vehicle.

Parameter	Symbol	Value	Unit
Vehicle mass	m	2271	kg
Length	l	4.796	m
Width	w	1.814	m
Moment of Inertia	Iz	4600	kg·m2
Distance from front axle to CG	lf	1.421	m
Distance from rear axle to CG	lr	1.434	m
Front cornering stiffness	Cf	132,000	N·rad−1
Rear cornering stiffness	Cr	136,000	N·rad−1

. The geometry of the surrounding vehicles is the same as that of the ego vehicle. The speed of the ego vehicle entering the intersection is set to 20 km/h. The width of the lanes is 3.5 m. The traffic light signal stages of green, red and yellow light in the simulation are set to 26 s, 31 s, and 3 s, according to an actual recorded two-lane intersection in the SinD dataset [40].

Software-in-the-loop (SIL) testing is conducted to verify if the C++ code corresponding to the controller model accurately executes identical control actions when subjected to equivalent stimuli. The hardware target of the C++ code generation is bound to ARM Cortex-M. A simple obstacle avoidance scenario is designed for SIL testing. An obstacle vehicle is stationary ahead of the ego vehicle in the same lane, with the road boundary on the left and a white dashed line on the right. The ego vehicle travels at a speed of 20 km/h on the road, navigating an avoidance path by considering obstacle and lane-marking potential fields in the MPC cost function. The SIL testing compares the MPC outputs, represented by vehicle speed and steering angle in this evaluation. Figure 7 depicts the state and error of the ego vehicle in both normal and SIL tests. It can be seen that the error is negligible.

5.1. Scenario 1: Unanticipated Obstacle Scenario at Intersection Inlet Road

Scenario 1 is the unanticipated obstacle scenario illustrated in Section 2, where the obstacle vehicle is located in the center of the lane 30 m in front of the ego vehicle, and the traffic light is red. The ego vehicle enters the left-turn lane marked by solid lines on both sides, comes to a stop behind the obstacle vehicle, and waits in the lane. The traffic light turns green at 20 s, and the front vehicle remains stationary.

In the first comparison test, the ego vehicle is programmed to adhere to traffic laws strictly. The experiment’s results are shown in Figure 8. The trajectories of ego (red) and obstacle (blue) vehicles are shown in Figure 8a. The speed and front wheel steering angles of the ego vehicle are shown in Figure 8b,c. The ego vehicle remains stationary and continues to wait due to the constraints of the solid-line potential field and the car-following potential field. The obstructed delay time continues to accumulate beyond the congestion threshold after two red light stages. This could lead to an infinite stop.

In the second comparison test, the ego vehicle can identify the necessity of obstacle avoidance through programming, yet it completely ignores traffic laws during the obstacle avoidance process. The compliance-oriented potential field is set to zero. The simulation result is presented in Figure 9. The ego vehicle starts to overtake the obstacle vehicle when the obstructed delay time equals the congestion threshold. The vehicle only considers collision avoidance during the planning process and chooses to cross the double-solid yellow line. The maximum occupancy of the opposite-direction lane is 2.12 m. The obstructed delay time is 148.25 s. The behavior of crossing the double-solid yellow line not only has high violation penalties but also results in driving in the wrong direction, which has a serious impact on traffic environment.

After implementing the proposed algorithm, the result is shown in Figure 10 according to the obstacle avoidance motion planning framework. The compliance-oriented potential fields in the cost function are replaced by the violation cost potential fields. Crossing double-solid yellow lines results in driving in the wrong direction; thereby, the initial value of the PVCI of the double-solid yellow line is higher. The ego vehicle crosses the solid white line to pass the obstacle vehicle. As the vehicle crosses the line, the violation degree in PVCI increases as the distance by which the vehicle exceeds the compliant range increases. This prompts the ego vehicle to plan a trajectory that is closer to the original lane while maintaining safety. The maximum occupancy of the adjacent lane is 1.78 m. The obstructed delay time is 148.68 s. After obstacle avoidance is completed, the compliance-oriented potential fields are utilized in the cost function to plan a compliant trajectory. The detailed PVCI of the ego vehicle is shown in Figure 10d.

5.2. Scenario 2: Fast Approaching Vehicle Passed by During Obstacle Avoidance

Scenario 2 is an extension of scenario 1, and the setting of this scenario is shown in Figure 11. After the mobility of the vehicle is negatively impacted by the unanticipated obstacle vehicle, the compliance-oriented potential field in the cost function switches to the violation cost potential field for obstacle avoidance motion planning. The ego vehicle is about to cross the solid white line to overtake the obstacle vehicle. At this moment, there is another vehicle approaching at a speed of 30 km/h from the adjacent lane in the same direction. The results can be observed in Figure 12. Due to the predicted collision risk, the PVCI for the solid white line and the car following increases rapidly, and the violation cost potential field is elevated, leading the ego vehicle to halt its evasive maneuver. After the vehicle from the adjacent lane passes and is safe, the ego vehicle continues its obstacle avoidance. The maximum occupancy of the adjacent lane is 1.83 m. The obstructed delay time is 151.35 s. The detailed PVCI of the ego vehicle is shown in Figure 12d.

5.3. Effectiveness Analysis of the Designed Algorithm

In this section, the effectiveness of the algorithm is evaluated. The performances of the ego vehicle in simulation testing are shown in

Table 6. Vehicle performance in simulation testing.

Scenarios	Obstructed Delay Time (s)	Violation Type	Occupancy of Adjacent Lane (m)
Scenario 1.1	\	\	0
Scenario.1.2	148.25	Crossing the double solid yellow line	2.12
Scenario.1.3	148.68	Crossing the solid white line	1.78
Scenario.2	151.35	Crossing the solid white line	1.83

. With the proposed motion planning scheme, the ego vehicle maintains mobility instead of coming to an infinite stop in an unanticipated scenario. After the obstructed delay time reaches the congestion threshold, the compliance-oriented potential fields are truncated based on PVCI to transform into the violation cost potential fields. In the primary objective of ensuring safety, obstacle avoidance is accomplished. According to the profile report of the simulation, the optimization problem can be resolved within approximately 4.8 ms when the motion planning does not involve potential fields. However, the resolution time increases to about 6 ms when potential fields are integrated into the optimization problem. Therefore, the proposed potential fields do not impose a significant additional computational burden. The preceding matrix computations in the MPC module exhibited an average computation time of 0.37 ms. The MPC meets strict real-time requirements, especially in emergency situations.

6. Conclusions

To solve the problem that safety and mobility of AVs are negatively impacted due to the conflict between traffic law compliance and obstacle avoidance in unanticipated obstacle scenarios at intersections, an obstacle avoidance motion planning method based on APF is proposed in this study. The core of this method lies in the construction of compliance-oriented potential fields and violation cost potential fields. Intersection traffic laws are digitalized and classified to construct compliance-oriented potential fields. The PVCI is designed according to theories of autonomous driving ethics. Violation cost potential fields are constructed by truncating compliance-oriented potential fields according to PVCI. A cost function is designed based on compliance-oriented and violation cost potential fields, integrated with MPC for trajectory optimization and tracking. Simulation tests are performed in the MATLAB/Simulink environment. Results indicate that, in unexpected obstacle scenarios, the proposed algorithm can plan trajectories with relatively lower penalties, achieving obstacle avoidance with a minimal degree of violation, thus ensuring both vehicle safety and mobility.

The algorithm will be further optimized to meet the requirements of on-road testing. Future research will focus on further exploration to more accurately identify conflict moments and strive for a reasonable balance between driving demands. A more precise description of non-compliant behaviors to better assess violation costs will be established. More complex scenarios will be considered to enhance the adaptability of the motion planning method.