Motion Planning for Urban Autonomous Bus Considering Localization Uncertainty Using Offset-Free MPC

This study proposes a novel motion planning strategy to address localization uncertainty in autonomous buses. Conventional motion planning algorithms utilize information from high-definition (HD) maps to overcome the limited detection range of on-board sensors. However, this information contains uncertainty due to the utilization of estimated localization results during reference frame transformation. The wider dimensions of autonomous buses, compared to regular vehicles, amplify the potential dangers associated with localization uncertainty. Therefore, this research focuses on enhancing motion planning for autonomous buses by effectively addressing localization uncertainty. The investigation of manual driving data from autonomous buses highlights the need to handle three issues: heading bias, lateral position error, and longitudinal position error. Firstly, the heading bias was dealt with by implementing an Offset-free Model Predictive Control (OF-MPC) with a Moving Horizon Estimation (MHE) scheme for lateral motion planning. Secondly, the lateral position error was handled by incorporating a drivable corridor to determine the desired path. Lastly, the longitudinal position error was resolved by implementing a chance-constrained MPC for longitudinal motion planning. The proposed approach showed noticeable enhancement in path tracking performance while still securing ride comfort in lateral motion, collision safety, and prevention of stop-line violations. We evaluated the feasibility of the proposed approach through vehicle tests on a test track, and its applicability was further confirmed through fully autonomous driving tests on actual urban bus-only lanes.

SECTION I. Introduction

Recently, autonomous buses (ABs) have received great attention for their potential to improve the efficiency and safety of public transportation [1], [2], [3], [4]. Unlike taxis and private cars, ABs used in urban public transportation typically operate along specific routes, making them an ideal platform for developing and demonstrating autonomous driving systems.

With the growing adoption and practical demonstrations of autonomous vehicles (AVs), including ABs, in public transportation, ensuring passenger safety and ride comfort has become a primary concern [5], [6]. These factors are critically linked to uncertainties inherent in the data used for AVs’ motion planning and control. During these processes, AVs rely on various estimates, including their own state, their position and orientation with respect to global coordinates, and the states and future trajectories of surrounding objects. In the past, many motion planning and control approaches assumed precise knowledge of these values, neglecting potential errors in estimation and inherent uncertainties. However, after recognizing the growing importance of enhancing the safety of AVs, research has been conducted to address these uncertainties.

Research tackling uncertainties in the ego vehicle’s (EV’s) state has been pursued through two approaches: (1) estimating errors within the state and (2) utilizing assumed error bounds. The error estimation approach employs the offset-free Model Predictive Control (OF-MPC) technique when errors within the EV’s state are observable. OF-MPC estimates and mitigates these disturbances by utilizing the estimated disturbance-free state as the actual state of the EV. Chen et al. [7] applied OF-MPC to regulate the integrated brake system of AVs, accounting for model mismatches and parameter variations. Similarly, studies in [8] and [9] employed OF-MPC to address performance degradation in AV motion control due to mismatches in vehicle dynamics models.

In contrast, the approach utilizing assumed error bounds employs robust state-feedback controllers, tube-based MPC (TMPC), and stochastic MPC (SMPC) techniques. For instance, Jin et al. [10] proposed a nonlinear robust H-infinity control to improve trajectory tracking performance of AVs, considering system parameter uncertainties and nonlinearities. However, regulating transient behaviors in the presence of disturbances remains challenging for robust control methods. Consequently, optimal control approaches have been proposed. Wischnewski et al. [11] implemented TMPC to consider model uncertainty for path planning in AV racing. Hu and Cheng [12] adopted a dynamically adjustable TMPC scheme to overcome model mismatch errors in AV path tracking. Despite its advantages, the TMPC method may be overly cautious for stochastic disturbances. Consequently, SMPC incorporates uncertainties by tightening constraints based on the probabilistic distribution of uncertainties. Knaup et al. [13] presented control strategies for autonomous off-road driving scenarios, using SMPC to account for the distribution of EV trajectories subject to Gaussian noise.

Research addressing inherent uncertainties in the states and future trajectories of surrounding vehicles has been conducted by incorporating uncertainty distributions into EV motion planning and control through the aforementioned TMPC and SMPC techniques. Khaitan et al. [14] proposed a method to avoid moving obstacles, integrating TMPC to account for uncertainties in the targets’ state. Studies in [15] and [16] utilized SMPC to consider uncertainties in the predicted future trajectories of surrounding vehicles when planning the AV’s overtaking maneuver. Mosharafian and Velni [17] applied a stochastic hybrid MPC scheme to the cooperative adaptive cruise control problem, incorporating uncertainties related to the operating modes of human-driven vehicles.

While previous studies have addressed uncertainties in the EV’s state and surrounding objects’ state and future trajectories, they have largely overlooked the uncertainty associated with the global position and orientation of the EV. This oversight substantially impacts autonomous driving performance, primarily due to the utilization of High-definition (HD) maps in motion planning [8], [18], [19], [20]. Limited sensor perception range in existing AVs compared to human drivers necessitates heavy reliance on HD maps [16], [21], [22]. These maps provide key information, including centerlines, lanes, road boundaries, and stop lines for bus stops and intersections [23], [24]. The integration of this data into motion planning involves a transformation from global to body-fixed reference frames, using the estimated global position and orientation. During this transformation, errors in this estimation inherently influence motion planning results. Autonomous driving algorithms employ localization modules to estimate the EV’s global position and orientation [25], [26], highlighting the critical significance of localization accuracy.

Localization uncertainty arises from factors such as sensor signal noise, signal delays or omissions, and sensor data accuracy influenced by environmental conditions [25]. Specifically, in urban driving environments, the accuracy of GPS signals may degrade due to satellite signal blockage. While localization uncertainty is crucial for enhancing estimation performance, its impact varies based on the driving environment and the target platform. Therefore, considering localization uncertainty within the motion planning and control modules is imperative to ensure safe driving. The challenges posed by localization uncertainty become particularly notable for ABs navigating complex urban environments with narrow clearances, presenting a more demanding scenario for precise and safe control compared to private cars. Consequently, for fully autonomous driving of ABs, the uncertainties in localization results must be carefully considered.

While most of the research has concentrated on quantifying and reducing localization errors [27], only a limited number of studies have addressed the impact of localization uncertainty on motion planning and control performance. In these studies, two distinct approaches have emerged: (1) leveraging the variance of localization uncertainty and (2) adopting a cautious driving strategy based on the extent of localization uncertainty. Artunedo et al. employed a drivable corridor-based motion planning method that utilizes the variance of localization uncertainty [20]. They generated a probabilistic occupancy grid map to describe the drivable corridor, integrating road boundary data from HD maps with the propagated variance of localization uncertainty along the road boundary. Conversely, in [18] and [19], motion control methods were proposed that reduce the set speed of the EV as localization uncertainty increases. This strategy accounts for the heightened risk of collisions with nearby objects and the degradation of path-tracking performance associated with increased uncertainty.

However, the applicability of these methods to ABs is restricted for the following reasons. Firstly, the absence of compensation for uncertainty in the form of a bias can lead to impractical solutions when navigating narrow paths. Especially, the impact of heading uncertainty is more pronounced in narrowing the drivable corridor in comparison to positional uncertainties [20]. Furthermore, the probabilistic occupancy grid map-based method was designed for compact passenger vehicles with considerably narrower dimensions than the paths. Secondly, relying solely on a motion control strategy that reduces set speed based on the degree of localization uncertainty is insufficient. This uncertainty affects not only collision risks with nearby obstacles but also the accuracy of referred values used in motion planning, including stop lines. An appropriate strategy considering localization uncertainty’s varying attributes with AV’s driving conditions and changes in referred sensor status is crucial, necessitating its treatment as a variable and consideration of its distribution [26].

To mitigate the described limitations, this study proposes a novel motion planning strategy for ABs to cope with localization uncertainty. Specifically, the scope of this study involves determining the desired motions that follow the planned reference motions while ensuring safe driving in the presence of localization uncertainty. First of all, planning algorithms for lateral and longitudinal motions were separately constructed to enhance computational efficiency for implementation [16]. Both motion planning algorithms were designed to account for the localization uncertainty since the uncertainty could cause heading bias, lateral position error, and longitudinal position error. Here, “bias” refers to the error in which an estimated or measured value deviates from an actual value. The details of the proposed approach are as follows.

First, for lateral motion planning, the heading bias was assumed to be a disturbance and handled through the implementation of OF-MPC. The OF-MPC was constructed by using a Linear Parameter-varying (LPV)-MPC with a disturbance estimation method. The estimator was designed using a Moving Horizon Estimation (MHE) scheme based on a dynamic bicycle model. Using the same dynamics model, the LPV-MPC was designed to follow the desired path within the drivable corridor by optimizing the control input. Second, for longitudinal motion planning, a chance-constrained MPC was implemented to manage the longitudinal position error. The chance constraint utilized the error covariance from the localization module to restrict the EV’s longitudinal travel distance. From the proposed lateral and longitudinal motion planning approaches, path-tracking performance could be enhanced by compensating for the heading bias while securing ride comfort. In addition, safety against collisions with obstacles and stop line violations could be secured through the implemented drivable corridor and chance constraint. Finally, the feasibility and applicability of the proposed approach were confirmed through vehicle tests conducted on both a test track and actual urban driving environments.

The contribution of this research can be summarized as follows:

• We established a motion planning strategy for ABs to mitigate localization uncertainty by examining the characteristics of localization uncertainty its impact on driving performance, utilizing real-world vehicle test data. Specifically, we categorized localization uncertainty into heading bias, lateral position error, and longitudinal position error. We then devised corresponding strategies tailored to the characteristics of each component.

• By compensating for the dominant influence of heading bias using OF-MPC with an MHE scheme, which significantly affects narrowing the drivable corridor, and by incorporating variance of lateral and longitudinal errors, we achieved fully autonomous driving for ABs, enabling them to navigate relatively narrow paths than their dimensions with improved path-tracking performance and enhanced safety.

• The effectiveness of our proposed approach was validated using actual ABs, and its performance was demonstrated in both controlled test track environments and uncontrolled complex urban road scenarios.

The rest of this paper is organized as follows. Section II introduces the details of the test vehicle set-up and preliminary test results in urban driving environments, investigating the characteristics of localization uncertainty. Section III provides an overview of the proposed motion planning approach. Section IV and Section V describe the lateral and longitudinal motion planning algorithms in detail, respectively. Section VI shows the performance evaluation results of the vehicle tests, assessing the feasibility and applicability of the proposed approach. Finally, Section VII concludes the paper and provides future perspectives on this research.

SECTION II. Preliminary

This section provides details regarding the test vehicle set-up and the actual urban driving environments. Characteristics of the observed localization uncertainty are investigated using the data acquired from manual driving of the bus along the urban bus-only lanes.

A. Hardware Configuration

Fig. 1(a) shows the test vehicle, a full-sized bus for urban public transportation with a width of 2.5 m and a length of 11.0 m. Equipped with off-the-shelf on-board sensors, processors, and a control interface, the perception module utilizes measured data from LiDAR only [28]. The localization module implements data from LiDAR, cameras, and the high-precision GPS/INS system (OxTS RT3000) with Real-time Kinematic (RTK) corrections, measuring global position, orientation, speed, and acceleration [29]. The front camera and Around View Monitor (AVM) are installed for lane detection around the vehicle. In Fig. 1(b), the on-board sensors have detection ranges from 10 m to 100 m, shorter than those of typical human drivers. Consequently, it is necessary to make use of HD maps to overcome this limited detection range. The state of the test vehicle, including yaw rate, steering wheel angle, longitudinal speed, and longitudinal acceleration, is acquired from in-vehicle sensors and provided through the Controller Area Network (CAN) messages of the chassis.

FIGURE 1.

The hardware configuration of the test vehicle: full-sized, electric-driven bus (Hyundai Elec-city).

The control interface of the test vehicle is constructed using the desired steering wheel angle and the desired acceleration. After the desired motion commands are computed from the autonomous driving algorithm in an industrial PC, they are sent to the Electronic Control Units (ECUs) of the vehicle via the chassis CAN bus. These ECUs compute actuator commands for steering wheel angle, motor torque, and brake pressure to follow the desired motions. The motion control algorithm, operating within the ECUs, consists of two sub-modules: lateral and longitudinal motion control. The lateral motion control sub-module takes the desired steering wheel angle input and computes control commands to manipulate the steering wheel angle, while the longitudinal motion control sub-module calculates motor torque and brake pressure to follow the desired acceleration.

In the case of large buses’ lateral motions, there are delays in both the actual responses of the steering wheel angle to the desired steering wheel angle input and an additional delay in the vehicle’s yaw rate response to the actual steering wheel angle input. To address these delays, we developed and implemented a controller employing a sliding mode control approach to minimize the yaw rate response delay concerning the desired steering wheel angle input [30].

B. Actual Driving Environments: Urban Bus-Only Lanes

Fig. 2(a) illustrates the test course located in the bus rapid transit (BRT) lanes of Sejong City, South Korea, spanning approximately 4.5 km. This course comprises a two-way two-lane configuration with diverse curvatures and road gradients, including straight sections, curves, high-curvature regions near bus stops, underground tunnels, and overpasses. It is important to note that the presence of high buildings and roadside structures along the lanes (Fig. 2(b)) can obstruct GPS satellite signals. In addition, GPS-denied areas exist in underground tunnels, as shown in Figs. 2(c) and (d).

FIGURE 2.

Vehicle test course in BRT lanes, Sejong City, South Korea.

The urban bus-only lanes exhibit narrow gaps between the side of a bus and lane boundaries, as shown in Fig. 3(a). Considering the test vehicle’s width, which is 2.7 m inclusive of the 0.2 m sensor width, the preferable gap between each lane and the test vehicle is measured to be 0.2 m on average, as shown in Fig. 3(b). Furthermore, obstructions such as curbs, median strips, and sidewalls are present along the lanes in the underground tunnels, overpasses, and near bus stops.

FIGURE 3.

Preferable gap considering lane width of BRT course and width of the test vehicle.

We selected this specific test course due to the following reasons. First, it was essential to ensure precise path tracking and safe driving for ABs on this course. The algorithm proposed in this study is universally applicable to various driving scenarios. Despite the bus-only lane configuration with restricted driving scenarios, this course encompasses diverse road types and roadside structures. In addition, the narrow preferable gap between lanes and vehicles poses challenges. Consequently, accounting for localization uncertainty becomes necessary to ensure safe motion planning and control. Secondly, GPS-denied areas, particularly within underground tunnels, introduce considerable fluctuations in localization uncertainty. These variations provide opportune settings for illustrating the impact of localization uncertainty on motion planning and control performance and for showcasing how the proposed approach addresses and mitigates these challenges.

C. Preliminary Investigation on Characteristics of Localization Uncertainty During Manual Driving of the Bus Along the Urban Bus-Only Lanes

Before conducting the autonomous driving tests, the performance of the localization module was evaluated through manual driving along the urban bus-only lanes. This preliminary investigation aimed to determine the characteristics of localization uncertainty. The estimated values from the localization module were compared to the values acquired from the GPS/INS system. Because GPS/INS data obtained in GPS-denied areas cannot be treated as ground truth values, the focus was on reviewing the trend of the difference between the two sets of values.

During the investigation, potential dangers were observed in sections with weak or absent GPS satellite signals. Fig. 4 illustrates three such sections located near the entrance of underground tunnels at stations of approximately 780 m, 1,600 m, and 3,700 m, where the accuracy of the GPS/INS system deteriorates. Upon exiting the tunnels, the GPS signals are recovered at different locations according to repeated driving tests, as shown in Fig. 4(a). The magnitude and sign of relative errors in heading, lateral position, and longitudinal position are also different according to the tests as shown in Figs. 4(b), (c), and (d). The red dotted lines in the figures indicate the validation gates of the first test, set as three-sigma regions around the predicted observations. Here, measured values are regarded as valid in the localization module if their errors fall within the bounds of the localization module. Lateral and longitudinal position errors occasionally exceed the bounds of the validation gates, while the error in heading always remains within the bounds. From Fig. 4, it can be seen that, unlike the errors in lateral and longitudinal positions, the error in the heading is so small that it is adopted as a measurement in the localization module.

FIGURE 4.

RTK GPS availability and relative errors between the measured value of GPS/INS system and estimated value of localization module.

Fig. 5 displays the estimated deviations of heading, lateral position, and longitudinal position from the localization module. In Fig. 5(a), the maximum deviation of heading is approximately 0.59 deg at the station of 1,916 m. The maximum deviations of the lateral and longitudinal position errors are approximately 0.24 m at stations of 2,138 m and 1.15 m at the station of 1,822 m, respectively (Figs 5(b) and (c)). These estimated values are well-matched with those reported in previous research [31], where maximum errors in heading and lateral position were found to be 1.05 deg and 0.13 m, respectively.

FIGURE 5.

Estimated deviation of heading, lateral position, and longitudinal position from localization module.

Significant drifts in heading error are observed at several locations, in addition to changes in the sensor’s status, as shown in Fig. 5(a). In autonomous driving, heading bias is known to cause lateral position error, with a 1 deg disturbance in heading bias leading to lateral position errors of up to 0.35 m [8]. Assuming the proportional relationship between heading error and resulting lateral position error, the potential lateral position error due to the maximum heading bias (0.59 deg) is expected to be roughly over 0.24 m. While this heading bias is a common occurrence in localization modules, the resulting lateral position error exceeds the preferable gap in the urban bus-only lanes, highlighting the significant risks associated with localization uncertainty in autonomous operations. Consequently, it can be confirmed that a motion planning strategy considering the impact of localization uncertainty is crucial for ensuring consistent driving performance.

Furthermore, the investigations in Figs. 5(b) and (c) reveal that the evaluated deviation in lateral position error is notably smaller than that of the longitudinal position error, consistent with the observation in the recent surveys [26]. These studies have indicated that the estimated lateral position tends to be more accurate than the longitudinal position in most localization-related studies. Consequently, the lateral position can be regarded as credible in comparison to the longitudinal position.

SECTION III. Overview of the Proposed Motion Planning Approach

In this section, the outline of the proposed motion planning approach, including the interface with the localization, perception, and control modules, is described. In addition to the modules, the vehicle dynamics models adopted in the proposed motion planning approach are explained.

A. Overall Architecture of the Proposed Motion Planning Approach

The proposed motion planning approach aims to ensure safe driving in the presence of localization uncertainty. It achieves this by determining desired motions that mitigate localization uncertainty while following planned reference motions. These desired motions correspond to the desired steering wheel angle and desired longitudinal acceleration, while planned reference motions involve reference paths and reference longitudinal motions (longitudinal clearance and longitudinal speed). Fig. 6 illustrates the proposed motion planning module, which is divided into two sub-modules: lateral motion planning with OF-MPC implemented and longitudinal motion planning using chance-constrained MPC. The lateral motion planning algorithm includes disturbance estimation using MHE, drivable corridor determination, and motion optimization using LPV-MPC. The longitudinal motion planning algorithm involves constraint determination and motion optimization using linear MPC. Once desired motions are determined, the vehicle’s motion control module manipulates the steering, throttle, and brake actuators to track these desired motions.

FIGURE 6.

Outline of the proposed motion planning approach.

B. Vehicle Dynamics Model

1) Lateral Vehicle Dynamics Model

The lateral vehicle dynamics model was constructed through a combination of a dynamic bicycle model with error dynamics equations along with a reference path [33]. As shown in Fig. 7, the relevant errors of a path tracking problem are defined as the heading error ($\mathrm {e}_{\mathrm {\psi }}$ ) and lateral position error ($\mathrm {e}_{\mathrm {y}}$ ) from the center of gravity (CG) with respect to the reference path. In general lane-keeping situations, the reference path is the lane centerline.

FIGURE 7.

Definition of path tracking errors about reference path.

The state-space model of the lateral dynamics can be represented as follows: $$\begin{align*} {\dot {\mathbf {x}}}_{lat}&=\mathbf {A}_{lat\mathrm {.}C}\mathbf {x}_{lat}+\mathbf {B}_{lat\mathrm {.}C}u_{lat}+\mathbf {F}_{lat\mathrm {.}C}\rho _{path} \\ \mathrm {s.t. } \mathbf {x}_{lat}&=\left [{ {\begin{array}{cccccccccccccccccccc} \beta &\quad \gamma &\quad e_{\psi } &\quad e_{y}\\ \end{array}} }\right]^{T} \\ u_{lat}&=\delta _{FSA.des} \\ \mathbf {A}_{lat\mathrm {.}C}&=\left [{ {\begin{array}{cccccccccccccccccccc} -\frac {2C_{f}+2C_{r}}{mv_{x}} & \quad \mathrm {-1+}\frac {-2C_{f}\ell _{f}+2C_{r}\ell _{r}}{mv_{x}^{2}} & \quad 0 & \quad 0\\ \frac {-2C_{f}l_{f}+2C_{r}l_{r}}{I_{z}} & \quad -\frac {2C_{f}\ell _{f}^{2}+2C_{r}\ell _{r}^{2}}{I_{z}v_{x}} & \quad 0 & \quad 0\\ 0 & \quad 1 & \quad 0 & \quad 0\\ V_{x} & \quad 0 & \quad v_{x} & \quad 0\\ \end{array}} }\right] \\ \mathbf {B}_{lat.C}&=\left [{ {\begin{array}{cccccccccccccccccccc} {\begin{array}{cccccccccccccccccccc} \dfrac {2C_{f}}{mv_{x}}\\[0.5pc] \dfrac {2C_{f}l_{f}}{I_{z}}\\ \end{array}}\\ {\begin{array}{cccccccccccccccccccc} 0\\ 0\\ \end{array}}\\ \end{array}} }\right],\quad \mathbf {F}_{lat.C}=\left [{ {\begin{array}{cccccccccccccccccccc} {\begin{array}{cccccccccccccccccccc} 0\\ 0\\ \end{array}}\\ {\begin{array}{cccccccccccccccccccc} -v_{x}\\ 0\\ \end{array}}\\ \end{array}} }\right] \tag{1}\end{align*}$$ where $\boldsymbol {x}_{lat}$ is the state vector, which is composed of side slip angle ($\beta$ ), yaw rate ($\gamma$ ), heading error (${e}_{\psi }$ ), and lateral position error ($e_{y}$ ). The control input $u_{lat}$ represents the desired front-wheel steering angle (FSA), and $\rho _{path}$ is the curvature of the reference path. For a mild urban driving condition targeted in this research, a reasonable assumption is made that the lateral tire force is linear with respect to the side slip angle. Accordingly, constant stiffness coefficients ($C_{f}$ , $C_{r}$ ) can be adopted.

2) Longitudinal Vehicle Dynamics Model

The longitudinal vehicle dynamics model was constructed using a kinematics model, assuming that the actual acceleration of the vehicle tracks the desired acceleration with a first-order delay ($\tau _{d}$ ). The state-space representation of the longitudinal dynamics is as follows: $$\begin{align*} {\dot {\mathbf {x}}}_{lon}&=\mathbf {A}_{lon.C}\mathbf {x}_{lon}+\mathbf {B}_{lon.C}u_{lon} \\[2pt] \mathrm {s.t. } \mathbf {x}_{lon}&=\left [{ {\begin{array}{cccccccccccccccccccc} p_{x} & \quad v_{x} & \quad a_{x} \\[6pt] \end{array}} }\right]^{T} \\[6pt] u_{lon}&=a_{x.des} \\[6pt] \mathbf {A}_{lon.C}&=\left [{ {\begin{array}{cccccccccccccccccccc} 0 & \quad 1 & \quad 0\\[6pt] 0 & \quad 0 & \quad 1\\[6pt] 0 & \quad 0 & \quad -\dfrac {1}{\tau _{d}}\\[6pt] \end{array}} }\right],\quad \mathbf {B}_{lon.C}=\left [{ {\begin{array}{cccccccccccccccccccc} 0\\[4pt] 0\\[4pt] \dfrac {1}{\tau _{d}}\\[4pt] \end{array}} }\right] \tag{2}\end{align*}$$ where $\mathbf {x}_{lon}$ is the state vector, which is composed of travel distance ($p_{x}$ ), longitudinal speed ($v_{x}$ ), and longitudinal acceleration ($a_{x}$ ). $u_{lon}$ represents the control input of desired longitudinal acceleration ($a_{x.des}$ ).

To discretize the continuous-time state-space models written in Eq. (1) and Eq. (2) with a fixed sampling time ($\mathrm {\Delta }t$ ), the zero-order hold (ZOH) discretization method was applied. The parameters adopted for modeling the EV’s dynamics are listed in Table 1.

TABLE 1 Parameters of Ego Vehicle’s Lateral and Longitudinal Dynamics Models

SECTION IV. Offset-Free MPC-Based Lateral Motion Planning

In this section, the lateral motion planning algorithm to tackle both the heading bias and lateral position error is described in detail. To estimate the heading bias, the disturbance estimation method is developed using MHE. To prevent potential collisions due to lateral position error, the drivable corridor is implemented for the determination of the desired path. Considering the estimated heading bias, OF-MPC is designed to follow the desired path within the established drivable corridor.

Before delving into the algorithm, the heading bias from localization is explained in a conceptual manner. Initially, a reference path is determined using data from HD maps about a global reference frame. Then, the path is transformed from the global to the body-fixed reference frame based on the estimated global position and heading of the EV. Subsequently, the path tracking errors (heading error and lateral position error) are computed using the reference path about the body-fixed frame. During this process, the calculated path tracking error is influenced by localization uncertainty. Specifically, the heading bias from localization is directly added to the EV’s heading error about the reference path. As a result, the measured heading error, which is calculated referring to the localization result, can be represented as follows: $$\begin{equation*} e_{\psi.measured}=e_{\psi.actual}+\delta \mathrm {e}_{\psi } \tag{3}\end{equation*}$$ where $\delta \mathrm {e}_{\psi }$ represents the heading bias from the localization result, and we will hereafter refer to it as the bias of heading error to avoid confusion. Here, lateral position error is assumed to be minimal, providing a practical assumption for feasible solutions in autonomous buses navigating narrow roads. As a supplementation, we implemented drivable corridor determination to guarantee collision-free driving in the presence of surrounding obstacles.

The following three assumptions have been employed for the interworking with the localization and perception modules.

1. The localization module effectively minimizes the lateral position error, maintaining a small deviation from the actual value without sudden changes.

2. The perception module effectively recognizes roadside structures as static obstacles, represented as a probabilistic occupancy grid map named Static Obstacle Map (SOM) [28].

3. The disturbance can be accurately estimated using the nominal dynamics model, which remains relatively stable in the target driving condition.

The first assumption is based on the findings from the preliminary investigation of localization uncertainty in Section II-C. The second assumption relies on credible recognition of static obstacles, supported by previous research on the perception module [28], making the impact of perception uncertainty on motion planning negligible. The third assumption states that changes in the dynamics model parameter have insignificant effects on motion planning. Considering the proposed method’s application in mild urban driving environments, this assumption can be regarded as valid.

A. Disturbance Estimation Using MHE

This research aimed to estimate the bias of heading error using only the measured states and control input signals along with the vehicle’s lateral dynamics model. The following input values were utilized for this estimation: three measurements among the four state variables, the control input (front-wheel steering angle), and the disturbance (road curvature). Among the state’s four variables indicated in Eq. (1), three of them – yaw rate, heading error, and lateral position error, are measurable. In particular, yaw rate and the control input can be obtained through chassis sensors. Heading error, lateral position error, and the disturbance (road curvature) are computed based on the information from the desired path. However, side slip angle, which is typically not directly accessible through chassis sensors, is excluded from the measurements.

The disturbance estimator is developed using MHE, which is a receding horizon, optimization-based state estimation technique [34], [35]. For vehicle state estimation problems, MHE has shown better performance than the Kalman filter-based method in terms of convergence rate, estimation accuracy, and robustness against initial deviations [36], [37]. In addition, since MHE can handle inequality constraints on state estimation, it is beneficial for estimating values within known ranges.

To construct the MHE-based disturbance estimator, an augmented disturbance model is formulated utilizing the OF-MPC scheme. In Eq. (1), with one control input serving as the reference state for zero-offset, the lateral position error ($e_{y}$ ) is selected as the target state for achieving zero-offset. The dimension of the disturbance ($n_{d}$ ) is set to three to match the number of measurements ($p$ ), ensuring independence from the estimator’s performance [38]. In addition to the bias of heading error ($\delta e_{\psi }$ ), the bias of control input ($\delta u_{lat}$ ) and the bias of road curvature ($\delta \rho _{path}$ ) are chosen as disturbance variables. Typically, these two terms are directly used in the estimation calculation as they correspond to the control input and known disturbance. Adopting additional disturbance variables related to these terms helps mitigate the effect of potential noise on estimation accuracy. Consequently, the MHE-based disturbance estimator provides the estimation results for the current state (four variables) and the disturbance vector (three variables)

The discrete-time LPV system’s state-space equation, the adopted disturbance variables, and corresponding state and measurement matrices are written as follows: $$\begin{align*} \mathbf {x}_{lat}\left ({{k+1} \vert t}\right) &=\mathbf {A}_{lat}\left ({k \vert t}\right) \mathbf {x}_{lat}\left ({k \vert t}\right) +\mathbf {B}_{lat}\left ({k \vert t}\right) u_{lat}\left ({k \vert t}\right) \\ &\quad +\mathbf {F}_{lat}\left ({k \vert t}\right) \rho _{path}\left ({k \vert t}\right) \tag{4}\\ \mathbf {y}_{lat}\left ({k \vert t}\right) &=\mathbf {C}_{lat}\mathbf {x}_{lat}\left ({k \vert t}\right) \\ \mathbf {d}_{lat}\left ({k \vert t}\right) &=\left [{ \delta e_{\psi }\left ({\left.{ k }\right |t }\right)\mathrm {}\delta u_{lat}\left ({\left.{ k }\right |t }\right)\mathrm {}\delta \rho _{path}\left ({\left.{ k }\right |t }\right) }\right]^{T} \\ \mathbf {B}_{lat\mathrm {.}d}\left ({k \vert t}\right) &=\left [{ {\begin{array}{cccccccccccccccccccc} {0}_{\mathrm {4\times 1}} &\quad \mathbf {B}_{lat}\left ({k \vert t}\right) &\quad \mathbf {F}_{lat}\left ({k \vert t}\right) \\ \end{array}} }\right] \\ \mathbf {C}_{lat\mathrm {.}d}\left ({k \vert t}\right) &=\left [{ {\begin{array}{cccccccccccccccccccc} 0 & \quad 0 & \quad 0\\ 1 & \quad 0 & \quad 0\\ 0 & \quad 0 & \quad 0\\ \end{array}} }\right] \\ \mathbf {y}_{lat}\left ({k \vert t}\right) &\!=\!\left [{ {\begin{array}{cccccccccccccccccccc} \gamma \left ({k \vert t}\right) & e_{\psi }\left ({k \vert t}\right) \!+\!\delta \mathrm {e}_{\psi }\left ({\left.{ k }\right |t }\right) & e_{y}\left ({k \vert t}\right) \\ \end{array}} }\right]^{T} \tag{5}\end{align*}$$ where the measurement matrix ($\mathbf {C}_{lat}$ ) is given by $\begin{aligned} \mathbf {C}_{lat}\left ({k \vert t}\right) =\left [{ {\begin{array}{cccccccccccccccccccc} \mathbf {0}_{3\times 1} & \mathbf {I}_{3\times 3}\\ \end{array}} }\right] \end{aligned}$ .

By combining the lateral dynamics model stated in Eq. (4) and the disturbance model in Eq. (5), the augmented system model can be represented as follows: $$\begin{align*} \mathbf {x}_{e}\left ({{k+1} \vert t}\right)& =\mathbf {A}_{e}\left ({k \vert t}\right) \mathbf {x}_{e}\left ({k \vert t}\right) +\mathbf {B}_{e}\left ({k \vert t}\right) u_{lat}\left ({k \vert t}\right) \\ &\quad +\mathbf {F}_{e}\left ({k \vert t}\right) \rho _{path}\left ({k \vert t}\right) \\ & =f_{\left.{ k }\right |t}\left ({\mathbf {x}_{e}\left ({k \vert t}\right),u_{lat}\left ({k \vert t}\right),\rho _{path}\left ({k \vert t}\right) }\right) \\ \mathbf {y}_{lat}\left ({k \vert t}\right) &=\mathbf {C}_{e}\left ({k \vert t}\right) \mathbf {x}_{e}\left ({k \vert t}\right) \\ \mathrm {s.t. } \mathbf {x}_{e}\left ({k \vert t}\right) &=\left [{ {\begin{array}{cccccccccccccccccccc} \mathbf {x}_{lat}\left ({k \vert t}\right) & \quad \mathbf {d}_{lat}\left ({k \vert t}\right) \\ \end{array}} }\right]^{T} \\ \mathbf {A}_{e}\left ({k \vert t}\right) &=\left [{ {\begin{array}{cccccccccccccccccccc} \mathbf {A}_{lat}\left ({k \vert t}\right) & \quad \mathbf {B}_{lat\mathrm {.}d}\left ({k \vert t}\right) \\ \mathbf {0}_{\mathrm {4\times 3}} & \quad \mathbf {I}_{\mathrm {3\times 3}}\\ \end{array}} }\right] \\ \mathbf {B}_{e}\left ({k \vert t}\right) &=\left [{ {\begin{array}{cccccccccccccccccccc} \mathbf {B}_{lat}\left ({k \vert t}\right) \\ \mathbf {0}_{\mathrm {3\times 1}}\\ \end{array}} }\right] \\ \mathbf {F}_{e}\left ({k \vert t}\right) &=\left [{ {\begin{array}{cccccccccccccccccccc} \mathbf {F}_{lat}\left ({k \vert t}\right) \\ \mathbf {0}_{\mathrm {3\times 1}}\\ \end{array}} }\right] \\ \mathbf {C}_{e}\left ({k \vert t}\right) &=\left [{ {\begin{array}{cccccccccccccccccccc} \mathbf {C}_{lat}\left ({k \vert t}\right) & \quad \mathbf {C}_{lat\mathrm {.}d}\left ({k \vert t}\right) \\ \end{array}} }\right] \tag{6}\end{align*}$$

Here, the disturbance state ($\mathbf {d}_{lat}\left ({k \vert t}\right))$ is assumed as a random walk process [39].

Using the augmented system in Eq. (6), the MHE problem is formulated as follows: $$\begin{align*} &\mathop {min}\limits _{\mathbf {x}_{e. N_{e}}(t)} J\left ({\mathbf {x}_{e}}\right) \\ &\qquad =\sum \limits _{k=t-N_{e}}^{t} {\left \|{ \mathbf {y}_{lat}\left ({\left.{ k }\right |t }\right)- \boldsymbol {C}_{e}\left ({k \vert t}\right) {\hat {\mathbf {x}}}_{e}\left ({k \vert t}\right) }\right \|_{ \boldsymbol {V}_{lat}^{\mathbf {-1}}}^{2}} \\ &\qquad +\sum \limits _{k=t-N_{e}}^{t-1} \left \|{ {\hat {\mathbf {x}}}_{e}\left ({\left.{ k+1 }\right |t }\right) }\right. \\ &\qquad -\,\left.{f_{\left.{ k }\right |t}\left ({{\hat {\mathbf {x}}}_{e}\left ({k \vert t}\right),u_{lat}\left ({k \vert t}\right),\rho _{path}\left ({k \vert t}\right) }\right) }\right \|_{ \boldsymbol {W}_{lat}^{\mathbf {-1}}}^{2} \\ &\qquad +\left \|{ {\hat {\mathbf {x}}}_{e}\left ({\left.{ t-N_{e} }\right |t }\right)-{\hat {\mathbf {x}}}_{e}\left ({\left.{ t-N_{e} }\right |t-1 }\right) }\right \|_{ \boldsymbol {P}_{lat}^{\mathbf {-1}}}^{2} \\ & \mathrm {s.t. } \delta e_{\psi.min} \le \delta e_{\psi }\left ({\left.{ k }\right |t }\right)\le \delta e_{\psi.max} \tag{7}\end{align*}$$ where ${\hat {\mathbf {x}}}_{e\mathrm {.}N_{e}}\left ({t }\right)$ is the set of states to be estimated (${\hat {\mathbf {x}}}_{e\mathrm {.}N_{e}}\left ({t }\right)=\left \{{{\hat {\mathbf {x}}}_{e}\left ({\left.{ t-N_{e} }\right |t }\right)\mathrm {, \ldots,}{\hat {\mathbf {x}}}_{e}\left ({\left.{ t }\right |t }\right) }\right \})$ . $\boldsymbol {V}_{lat}^{\mathrm {- \boldsymbol {1}}}$ and $\boldsymbol {W}_{lat}^{\mathrm {-\mathbf {1}}}$ are the weight matrices of measurements and process models, which are inversely proportional to the noise covariance matrices. $\boldsymbol {P}_{lat}^{-1}$ is the arrival cost at the time ${t-N}_{e}\mathrm {.}N_{e}$ is the estimation horizon step, which is determined by dividing the estimation window length into sampling time. The constraints for the bias of heading error, denoted as $\delta e_{\psi.min}$ and $\delta e_{\psi.max}$ , are chosen as the same bound as the validation gate from the localization module.

B. Drivable Corridor Determination

The drivable corridor denotes the area where the EV can safely navigate while avoiding obstacles. To determine the desired path within this corridor, we implemented a method proposed in previous research for private cars [24]. This study extended the approach to account for the dimensions of a large bus. Specifically, we considered not only the reference path but also the future trajectories of the rear and front bumper points as the vehicle’s center of gravity follows the reference path. Here, the algorithm is briefly outlined in a conceptual manner.

The drivable corridor is defined by the left and right envelopes, which are obtained through the following sequential process. Initially, the reference path, left envelop, and right envelope are assumed to be the centerline and the left and right lanes, respectively. Then, the envelops are checked to see if the SOM within the region of interest invades them. If violations are detected, the envelopes are updated accordingly. Fig. 8 illustrates a case where a guardrail invades the right lane. After calculating the SOM for the guardrail, the right envelop is corrected using SOM information. Subsequently, the distances from the left and right envelops to the initial desired path (same as the reference path) are computed. If these distances are narrow compared to half of the vehicle’s width plus a preferable gap, the desired path in that Section is shifted to secure a preferable gap. This process of modifying the desired path is carried out repeatedly, taking into consideration the future trajectories of the rear and front bumper points.

FIGURE 8.

The determination of the desired path considering drivable corridor.

After determining the desired path, we calculate a reference speed profile, considering both the drivable corridor and the desired path’s curvature, with a focus on passenger comfort and driving safety. The speed profile for the drivable corridor is established by inversely correlating it with the distance between the vehicle’s desired path and the drivable corridor. This inverse relationship between the reference speed and the distance was heuristically estimated based on data collected from expert drivers [24]. In addition, the speed profile concerning the desired path’s curvature is computed while considering the lateral acceleration limit [30]. The reference speed profile for longitudinal motion is then determined as the smaller value between these two calculated speed profiles.

C. Motion Optimization Using LPV-MPC

MPC problem is formulated using the lateral dynamics model in Eq. (4) with the estimated state and disturbance values using MHE in Eq. (7). The optimization problem for the lateral motion is defined as follows: $$\begin{align*} &\min _{\mathbf {u}_{\text {lat. } N_{p}}} J\left ({\mathbf {x}_{\text {lat }}, u_{\text {lat }}}\right) \\ &\qquad =\left \|{ {\tilde {\mathbf {x}}}_{lat}\left ({N_{p} \vert t}\right) }\right \|_{\mathbf {P}_{lat}}^{2} \\ &\qquad +\sum \nolimits _{k\mathrm {=0}}^{N_{p}-1} \left \{{\left \|{ {\tilde {\mathbf {x}}}_{lat}\left ({{k+1} \vert t}\right) }\right \|_{\mathbf {Q}_{lat}}^{2}}\right. \\ &\qquad \qquad \qquad \quad \left.{+\left \|{ u_{lat}\left ({k \vert t}\right) }\right \|_{R_{lat}}^{2} }\right \} \tag{8a}\\ &s.t. \mathbf {x}_{lat}\left ({{0} \vert t}\right) \!=\!{\hat {\mathbf {x}}}_{lat}\left ({{0} \vert t}\right), u_{lat}\left ({{-1} \vert t}\right) \!=\!\hat {u}_{lat}\left ({{-1} \vert t}\right) \tag{8b}\\ &\qquad \mathbf {x}_{lat}\left ({{k+1} \vert t}\right) =\mathbf {A}_{lat}\left ({k \vert t}\right) \mathbf {x}_{lat}\left ({k \vert t}\right) \\ &\qquad \qquad +\mathbf {B}_{lat}\left ({k \vert t}\right) u_{lat}\left ({k \vert t}\right) \\ &\qquad \qquad \quad +\mathbf {F}_{lat}\left ({k \vert t}\right) \rho _{path}\left ({k \vert t}\right) \tag{8c}\\ &\qquad \delta _{FSA\mathrm {.}des\mathrm {.}min}\le u_{lat}\left ({k \vert t}\right) \le \delta _{FSA\mathrm {.}des\mathrm {.}max} \\ &\qquad \left |{ u_{lat}\left ({{k+1} \vert t}\right) -u_{lat}\left ({k \vert t}\right) }\right |\le dt\cdot \dot {\delta }_{FSA\mathrm {.}max} \tag{8d}\end{align*}$$ where $\mathbf {u}_{lat.N_{p}}\left ({k \vert t}\right)$ is the set of control inputs to be optimized ($\mathbf {u}_{lat.N_{p}}\left ({k \vert t}\right) =\left \{{u_{lat}\left ({{0} \vert t}\right), \ldots,u_{lat}\left ({{N_{p}-1} \vert t}\right) }\right \})$ . ${\tilde {\mathbf {x}}}_{lat}\left ({k \vert t}\right) \mathrm {:=}\mathbf {x}_{lat}\left ({k \vert t}\right) -\mathbf {x}_{lat.ref}\left ({k \vert t}\right)$ represents the reference tracking error. $\mathbf {P}_{lat}$ is the terminal cost at the time step of $N_{p}$ . The control input constraints are defined as the bounds of magnitude and jerk, following the control interface specifications. Heading and lateral position errors in the vehicle’s current state (${\hat {\mathbf {x}}}_{lat}\left ({{0} \vert t}\right))$ are linked to the reference path, resulting in an inaccurate representation of the vehicle’s state. The current state is corrected by employing the estimation results of the disturbance estimator using MHE (Eq. (8b)). Moreover, the disturbance estimator utilizing MHE corrects the external disturbance value in terms of road curvature ($\rho _{path}\left ({k \vert t}\right))$ . By eliminating the bias of heading error, the MPC formulation becomes heading offset-free. The parameters adopted for modeling the MHE and LPV-MPC are listed in Table 2.

TABLE 2 Parameters for Lateral Motion Planning Algorithm

SECTION V. Chance-Constrained MPC-Based Longitudinal Motion Planning

A. Chance Constraint for Considering the Localization Uncertainty Along Longitudinal Direction

The upper bound of the EV’s travel distance is primarily determined by the state of the targets. These targets, considered in longitudinal motion planning, consist of both those from the perception module and those virtually generated using stop lines in HD maps. The states of virtual targets are influenced by localization uncertainty. The stop line positions are defined in the global reference frame and then transformed into the EV’s body-fixed reference frame. Then a stationary, virtual target is generated on the location, as shown in Fig. 9. Therefore, errors in the virtual target’s position arise from the localization module.

FIGURE 9.

Chance-constraint for uncertainty of target’s position from localization module.

To handle the localization uncertainty in the longitudinal position error, we adopt a chance constraint as an inequality constraint for travel distance. As investigated in Section II-C, the deviation of longitudinal position error about the body-fixed reference frame is assumed to have zero mean and be normally distributed. Similarly, we assume the uncertainty of the virtual target’s position follows the same distribution. To account for this uncertainty effectively, we employ the analytic reformulation method to transform the chance constraint for the upper bound of travel distance into deterministic constraints, as shown below [40]: $$\begin{align*} p_{x}\left ({\left.{ k }\right |t }\right)&\le p_{max}\left ({\left.{ k }\right |t }\right)-\gamma \left ({\left.{ k }\right |t }\right),k\in \left \{{\mathrm {1,\ldots,}N_{p} }\right \} \\ \mathrm {s.t.}\gamma \left ({\left.{ k }\right |t }\right)&=\sqrt {2\Sigma _{\mathrm {k}}^{e}} {\mathrm {erf}}^{-1}\left ({\mathrm {1-2}\varepsilon }\right) \tag{9}\end{align*}$$ where $\gamma \left ({\left.{ k }\right |t }\right)$ is the tightening parameter. $\mathrm {\Sigma }_{\mathrm {k}}^{e}$ represents the covariance of the longitudinal position error (here, uncertainty) at $\text{k}^{\mathrm {th}}$ step. The covariance is obtained by projecting the covariance of longitudinal and lateral position errors, estimated by the localization module, onto the direction of the vehicle’s reference path. $\varepsilon$ is the chance constraint parameter which represents the probability level. erf denotes the error function. As observed in Fig. 5(c), the covariance varies with the sensor’s state. To address this variation and considering the practical challenges in accurately predicting the future set of covariance, the error covariance is assumed to keep its level within the MPC calculation interval.

B. Motion Optimization Using Chance-Constrained MPC

The chance-constrained MPC is set up as shown in Eq. (10), including the longitudinal dynamics model presented in Eq. (2) and the chance constraint detailed in Eq. (9). The objective function encompasses minimizing the reference tracking error, control input, and a slack variable related to travel distance. To ensure real-time feasibility in the MPC problem, the slack variable is introduced to soften the constraint for travel distance. The inclusion of a delay model to describe longitudinal dynamics in Eq. (2) ensures that the optimal control inputs are calculated to compensate for this model’s delay. $$\begin{align*} &\min _{\mathbf {u}_{\text {lon }. N_{p}}} J\left ({\mathbf {x}_{\text {lon }}, u_{\text {lon }}}\right) \\ &\qquad =\sum \nolimits _{k\mathrm {=0}}^{N_{p}-1} \left \{{\left \|{ {\tilde {\mathbf {x}}}_{lon}\left ({{k+1} \vert t}\right) }\right \|_{\mathbf {Q}_{lon}}^{2}}\right. \\ &\qquad \qquad +\left.{\left \|{ u_{lon}\left ({k \vert t}\right) }\right \|_{R_{lon}}^{2}+\left \|{ q_{p}\left ({{k+1} \vert t}\right) }\right \|_{Q_{p}}^{2} }\right \} \\ &s.t. \mathbf {x}_{lon}\left ({\left.{ k+1 }\right |t }\right)=\mathbf {A}_{lonD}\mathbf {x}_{lon}\left ({\left.{ k }\right |t }\right)+\mathbf {B}_{lon.D}u_{lon}\left ({\left.{ k }\right |t }\right) \\ &\qquad a_{x\mathrm {.}min}\mathrm { \le }u_{lon}\left ({\left.{ k }\right |t }\right)\le a_{x\mathrm {.}max} \\ &\qquad \mathbf {G}_{lon}^{T}\mathbf {x}_{lon}\left ({\left.{ k+1 }\right |t }\right)\le \mathbf {x}_{lon.bound}\left ({\left.{ k+1 }\right |t }\right) \\ &\qquad q_{p}\left ({\left.{ k+1 }\right |t }\right)\ge 0, \\ &\qquad k\in \left \{{ \mathrm {0,\ldots,}N_{p}-1 }\right \}, \tag{10}\end{align*}$$ where $$\begin{align*} \mathbf {G}_{lon}&=\left [{ {\begin{array}{cccccccccccccccccccc} 1 & \quad 0 & \quad 0\\ -1 & \quad 0 & \quad 0\\ \end{array}} }\right]^{T} \\ &\hspace {-2pc}\mathbf {x}_{lon.bound}\left ({\left.{ k+1 }\right |t }\right) \\ &=\left [{ {\begin{array}{cccccccccccccccccccc} p_{x.upper}\left ({\left.{ k+1 }\right |t }\right)\\ -p_{x.lower}\left ({\left.{ k+1 }\right |t }\right)\\ \end{array}} }\right] \\ &=\left [{ {\begin{array}{cccccccccccccccccccc} p_{x.max}\left ({\left.{ k+1 }\right |t }\right)-\gamma \left ({\left.{ k+1 }\right |t }\right)+q_{p}\left ({\left.{ k+1 }\right |t }\right)\\ -p_{x.min}\\ \end{array}} }\right]\end{align*}$$ where $\mathbf {u}_{lon.N_{p}}\left ({k \vert t}\right)$ is the set of control inputs to be optimized ($\mathbf {u}_{lat.N}\left ({k \vert t}\right) =\left \{{u_{lon}\left ({{0} \vert t}\right), \ldots,u_{lon}\left ({{N_{p}-1} \vert t}\right) }\right \})$ . ${\tilde {\mathbf {x}}}_{lon}\left ({k \vert t}\right) \mathrm {:=}\mathbf {x}_{lon}\left ({k \vert t}\right) -\mathbf {x}_{lon.ref}\left ({k \vert t}\right)$ is the reference tracking error. $\mathbf {x}_{lon\mathrm {.}ref}\left ({k \vert t}\right)$ is the vector of reference motions, which is composed of the travel distance ($p_{x\mathrm {.}ref}$ ) and longitudinal speed ($v_{x\mathrm {.}ref}$ ). The vehicle’s current state ($\mathbf {x}_{lon}\left ({k \vert t}\right))$ is composed of the travel distance from the current position, longitudinal speed, and longitudinal acceleration, which are unaffected by localization uncertainty. When the preceding target is a virtual target (stop line), computing the clearance between the ego vehicle and the virtual target relies on the stop line’s position relative to the body-fixed frame, thus introducing inaccuracies into the state’s reference value due to localization uncertainty. By implementing the proposed chance constraint, the constraint on travel distance is adjusted to prevent violations of stop lines. $q_{p}$ denotes the slack variable for travel distance. The prediction horizon and time step of the MPC are identical to those used for lateral motion planning. The parameters adopted for modeling the chance-constrained MPC are listed in Table 3.

TABLE 3 Parameters for Longitudinal Motion Planning Algorithm

SECTION VI. Vehicle Tests

In this section, the feasibility and applicability of the proposed approach are evaluated from the vehicle tests conducted in a test track and actual urban bus-only lanes, respectively. The test track provided a controlled environment due to restricted external vehicle access. In contrast, the actual urban bus-only lanes were accessible to other vehicles, including buses during testing.

During the vehicle tests, the autonomous driving algorithms operated on an industrial PC with an Intel Xeon E-2176G CPU (3.7 GHz), 32GB DDR4 RAM, and a GeForce RTX-2080 GPU. To handle the MPC and MHE problems within our proposed approach, we employed the CVXGEN software [41], optimization tool that generates custom C code tailored for embedding these optimization problems in real-time applications. Implementation was conducted within the Robot Operating System (ROS)-C++ environment and integrated with other interoperating algorithms.

To ensure real-time performances of our proposed MPC and MHE problems, we investigated their computational times on the industrial PC. The computation times were evaluated using urban driving data obtained from the vehicle tests. Actual computation times on this PC may vary due to other concurrent logics, impacted by the optimization of computing resources within the real-time operating system. When each algorithm operates independently, the average computational times for the MPC and MHE problems in lateral motion planning were 1.26 ms and 3.22 ms, respectively, with maximum times of around 2.33 ms and 6.58 ms. For the MPC in longitudinal motion planning, average and maximum computational times were 1.13 ms and 2.39 ms, respectively. The combined maximum computation time of the MPC and MHE problems used in lateral motion planning is 8.91 ms, which falls within the sampling times of the perception logic (50 ms) and the MPC problem (100 ms), making them suitable for practical application in an actual autonomous driving system.

A. Feasibility Test with Intentional Bias of Heading in Test Track

We aimed to investigate the effectiveness of the proposed lateral motion planning algorithm, specifically the OF-MPC with the disturbance estimator using MHE. The following three types of heading bias were considered: 1) intentional constant heading bias, 2) shift in the intentional constant bias, and 3) perturbation according to changes in road curvatures and longitudinal speed limits. The first bias was adopted to replicate the localization uncertainty in areas with degraded localization module accuracy. The second bias imitated uncertainty from inconsistent sensor accuracy. The third perturbation was investigated for assessing unexpected disturbances from changes in the dynamics model’s input variables under actual driving conditions. To implement the intentional heading bias, we added a constant value to the heading (orientation) term obtained from the localization module.

To investigate the impact of constant heading bias, a bias of −0.5 deg was added in Zone 1 (marked in Fig. 10(a)), while a bias of −1.0 deg was added in the rest of the track. This allowed a separate assessment of the impact of the intentional constant heading bias in Zone 1 and Zone 2. The shift in the intentional bias was observed near the Section between Zone 1 and Zone 2, where the effect of the perturbations on performance was also examined. Since Zone 1 featured a curved section, the bus drove at a slow speed of 15 kph. On the other hand, Zone 2 had a straight path, and the bus drove at a higher speed of 40 kph, which is the speed limit for urban roads in Section II-B.

FIGURE 10.

Vehicle test results when implementing the proposed and reference algorithms on the test track with intentional bias of heading.

To assess the feasibility of the proposed OF-MPC with MHE, path-tracking performance was evaluated. Especially, lateral position error with respect to the desired path and its convergence rate against perturbations were reviewed. To provide a comprehensive evaluation, two reference algorithms were also assessed: 1) conventional LPV-MPC without a disturbance estimator and 2) OF-MPC with a disturbance estimator using an Extended Kalman Filter (EKF). By comparing our approach to the first algorithm, the role of the OF-MPC method for coping with localization uncertainty could be confirmed. From the comparison to the second algorithm, the advantages of the proposed MHE-based approach could be demonstrated.

As shown in Fig. 10(b), the intentional constant heading bias causes lateral position errors. Without implementing the OF-MPC scheme, the averaged lateral position errors are approximately 0.2 m in Zone 1 and 0.4 m in Zone 2, respectively. This aligns with the concerns raised in Section II-C., indicating that without compensation for the heading bias, the AB may invade lane boundaries. However, by using the proposed approach, the lateral position error could be effectively maintained within the preferable gap of 0.2 m. Through the observations, the necessity of OF-MPC with a disturbance estimator for counteracting the localization uncertainty has been clearly confirmed.

The effectiveness of the proposed approach using MHE could be confirmed through a comparison with the second reference algorithm: OF-MPC with an EKF-based disturbance estimator. When using OF-MPC with EKF, the lateral position error exceeds the preferable gap of 0.2 m near the entrance of Zone 1, as shown in Fig. 10(b). On the contrary, OF-MPC with MHE shows two noticeable improvements. First, the lateral position error when using OF-MPC with MHE is smaller, with a root mean square (RMS) error of 0.061 m, reduced by 31 % compared to that of OF-MPC with EKF (RMS error of 0.088 m). Second, OF-MPC with MHE effectively reduces the offset of the lateral position error. The averaged error when using OF-MPC with MHE is −0.006 m, which is only 2% compared to OF-MPC with EKF (0.054 m). In particular, the error with MHE is mostly within the range of −0.029 m to 0.062 m, except for sections with changes in road curvatures and speeds (from 0 m to 50 m and from 150 m to 270 m). When using MHE, offsets of approximately 0.006 m and −0.013 m are observed at these stations, while EKF shows offsets of approximately 0.1 m and 0.03 m at the same stations.

The advantages of MHE implementation could also be explained in terms of convergence rates for estimating uncertainty against perturbations, particularly evident in two sections: 150 m to 160 m and from 270 m to 400 m. In the range of 150 m to 160 m, MHE shows minimal lag compared to EKF’s phase lag of 4.5 m, as shown in Fig. 10(c). Although both estimators handle the shift in heading bias (relatively large value), the difference is insignificant in this range. Conversely, from 270 m to 400 m, EKF exhibits a phase lag of 32.5 m, indicating slower convergence compared to MHE. This slower convergence rate significantly impacts lateral position error. Consequently, MHE can effectively deal with uncontrollable heading bias (relatively small value) with an improved convergence rate.

The improved performance of the proposed approach using MHE in reducing lateral position error allowed us to interpret the results of the estimated bias of heading error presented in Fig. 10(c). While MHE exhibits faster convergence than EKF, it shows lower accuracy in estimating the intentional constant heading bias. This contrasting trend can be understood according to the range of lateral position errors in Fig. 10(b), where OF-MPC with MHE displays minimal deviation and insignificant drift. It can be reasonably inferred that the estimated bias of heading error obtained from MHE is more reliable than that from EKF. Besides the assumed intentional heading bias, the estimated bias may involve external variables due to uncontrollable factors, such as changes in lane detection accuracy and EV state estimation. It could be inferred that the impact of these uncontrollable factors on offset-free performance was better mitigated in the approach using MHE than using EKF.

B. Applicability Test in Urban Bus-Only Lanes

To confirm the applicability of the proposed approach under actual localization uncertainty, fully autonomous driving tests were conducted in the urban bus-only lanes. The test course, as marked in Fig. 11(a), includes three underground tunnels. As shown in Fig. 2 and Fig. 11(d), the entrances of the first and second tunnels are located on straight sections, while their exits exhibit significant curvature due to bus stops just beyond the exits. The entrance of the third tunnel is located on a curved section, while the exit is near a straight section. As shown in Fig. 11(e), the speed limits vary along the course: 35 kph, 25 kph, and 40 kph for the three tunnels, sections near the bus stops, and the rest of the course, respectively. To minimize the impact of longitudinal speed differences between tests on performance analysis, only the test results conducted with minimal or no preceding vehicles were selected. Along the test course, there are hazardous road structures on the right side of the AB, as vehicles typically keep to the right lane.

FIGURE 11.

Vehicle test results in urban bus-only lanes when implementing the proposed and reference approaches.

To ensure safety during the applicability tests, it was necessary to develop the reference algorithm further. In the reference algorithm, conventional LPV-MPC without a disturbance estimator was adopted for the lateral motion planning algorithm. In addition, the drivable corridor had to be considered to secure safety against collisions during autonomous driving on the course. For the longitudinal motion planning algorithm, the identical MPC scheme was adopted as in the proposed algorithm, except that the chance constraint was excluded from the reference algorithm.

Before assessing performance, we examined whether the calculated control input and output responses of these MPC schemes meet specified constraints. In Fig. 12(a), lateral motion planning’s control inputs (desired front-wheel steering angle (FSA)) range from −3.9 to 2.6 degrees, within the −45.0 to 45.0 degree range in Table 2. Similarly, Fig. 12(b) shows longitudinal motion planning’s control inputs (desired longitudinal acceleration) ranging from -2.4 m/s² to 1.0 m/s², meeting the range of −5.0 to 1.0 m/s² in Table 3. Consequently, it can be confirmed that the MPC schemes satisfy the control input constraints.

FIGURE 12.

Distributions of the desired control inputs and actual responses from the vehicle tests in urban bus-only lanes.

The applicability evaluation was conducted in terms of the following three features: 1) path-tracking performance, 2) ride comfort related to lateral motion, and 3) safety against road boundaries and stop lines. Quantitative analysis assessed path-tracking and ride comfort improvements, while safety enhancements were qualitatively investigated using captured photographs due to challenges in directly measuring the actual localization error or the gap between the road boundaries and the test vehicle in actual urban environments.

1) Path-Tracking Performance

The improvement in path-tracking performance when implementing the proposed approach can be assessed by analyzing the RMS values, peaks, and ranges of lateral position error and estimated heading error. Table 4 presents a summary of the RMS values corresponding to each implemented algorithm. In addition, box plots for the chosen errors are depicted in Fig. 13. Detailed descriptions regarding the path tracking performance are as follows.

TABLE 4 Comparison of Performance in Lateral Motion Planning Algorithms Regarding Path Tracking and Ride Comfort

FIGURE 13.

Box plot for the ranges of lateral position error and estimated heading error from the vehicle tests in urban bus-only lanes.

First, the proposed approach achieves considerable reductions in RMS values of lateral position and estimated heading errors in comparison to those of the reference approach. When implementing OF-MPC with MHE, the RMS values of lateral position and estimated heading errors are 0.0741 m and 0.3195 deg for the entire course, respectively, showing reductions of 31.6 % and 25.8 % in comparison to the reference values of 0.1084 m and 0.4306 deg. Especially, near the second tunnel, the RMS values are 0.0631 m and 0.3696 deg, indicating reductions of 56.8 % and 32.9 % in comparison to LPV-MPC values of 0.1459 m and 0.5507 deg. The difference in the bias of heading error estimated near the second tunnel is approximately 0.86 deg, as shown in Fig. 11(b). Compensating for this substantial bias of heading error leads to a significant reduction in lateral position error, allowing autonomous driving to maintain the preferable gap of 0.2 m.

Second, the proposed approach exhibits a significant reduction in peaks of lateral position error in comparison to those of the reference approach. As shown in Fig. 11(a), most of the lateral position errors when using the proposed approach remain below the preferable gap of 0.2 m for the entire course. Only two out-of-tolerance exceptions are observed at sections near the stations of 1,373 m and 3,430 m, with deviations of 0.213 m and 0.262 m, respectively. These exceptions, occurring near bus stops, necessitated evasive maneuvers to avoid roadside obstacles, temporarily exceeding the preferable gap. However, these out-of-tolerance exceptions were strategically planned to secure safety throughout the entire course.

Third, the proposed approach significantly reduces the ranges of the lateral position and estimated heading errors in comparison to those when using the reference approach. In Figs. 13(a) and 13(b), the ranges are reduced by 32.2 % and 33.2 %, respectively. The most notable improvement occurs in three sections near 1,200 m, 2,100 m, and 4,050 m, where the accuracy of the GPS/INS system increases after exiting the tunnels. As shown in Fig. 11(a), the reference approach shows a rapid increase in lateral position error, around 0.3 m near the station of 1,200 m, while the proposed approach avoids this rapid change in the identical section.

2) Ride Comfort Related to Lateral Motion

The ride comfort when implementing the proposed approach can be assessed through lateral acceleration and yaw rate, which are widely used metrics for analyzing ride comfort related to lateral motion [42]. It should be reviewed due to the concern that path tracking performance when applying the proposed approach could be improved by using excessive control inputs. As shown in Table 4, the RMS values of lateral acceleration and yaw rate are slightly reduced when implementing the proposed approach compared to the reference approach. Notably, this reduction is observed in the first tunnel, where the reference algorithm’s logic necessitates additional control input in response to potential collision risks. In contrast, the proposed algorithm eliminates this need, contributing to an improvement in ride comfort by minimizing unnecessary lateral motion. Consequently, it can be confirmed that ride comfort is secured while enhancing tracking performance with the proposed approach.

3) Safety Against Road Boundaries and Stop Lines

The enhanced safety can be assessed by examining photographs taken from the AVM camera during vehicle tests, which allow us to observe whether the bus invades road boundaries and stop lines. Several distinct differences are recognized from Figs. 14, 15, 16, and 17. During the tests, the most dangerous situations occur near the underground tunnels where the accuracy of the localization module is reduced. The estimated gap between the body of the bus and the lane boundaries is less accurate than on surface roads. Therefore, displaying the top view captured by the AVM camera is effective in verifying enhanced safety against road boundaries and stop lines.

FIGURE 14.

Top view captured from AVM camera according to dangerous sections when implementing the reference approach.

FIGURE 15.

Top view captured from AVM camera according to dangerous sections when implementing the proposed approach.

FIGURE 16.

Top view near the bus stop behind the second underground tunnel when implementing the reference approach.

FIGURE 17.

Top view near the bus stop behind the second underground tunnel when implementing the proposed approach.

When driving near the bus stop behind the first underground tunnel as shown in Figs. 14(b) and 15(b), there is little difference in safety between the proposed and reference approaches. The test vehicle drifts approximately 0.2 m from the road centerline to avoid collision with the guard rail along the road. Both approaches ensure safety against roadside structures by considering the drivable corridor in common. On the contrary, noticeable improvements when implementing the proposed approach are observed in other scenarios, as shown in Figs. 14(a), (c), (d), and Figs. 15(a), (c), (d). With the reference approach, the test vehicle comes close to the right wall and lane boundaries. The vehicle attempts to maintain a preferable gap after the gap decreases due to the change in bias of heading error. In contrast, the proposed approach allows the test vehicle to drive along the centerline while maintaining the gap, as shown in Figs. 15(a), (c), and (d), providing sufficient gaps from the vehicle body to the right road boundaries.

Figs. 16 and 17 show the top view of the test vehicle when driving near the bus stop at the station of 2,700 m. As the bus stop was located right behind the second underground tunnel, the GPS satellite signal was not recovered until the bus approached the bus stop. As shown in Figs. 16(a), (b), and Figs 17(a), (b), the actual distance from the test vehicle’s front bumper to the stop line is smaller than the value estimated by the localization module. With the reference approach, the test vehicle stops close to the stop line, as shown in Fig. 16(b). However, with the proposed approach, the test vehicle maintains a clearance with a margin against the stop line, as shown in Fig. 17(b). It can be seen that the proactive motion was executed to maintain a safe distance against stop lines. Overall, the observations confirm the improvements in safety.

SECTION VII. Conclusion

This study has presented a novel motion planning strategy to address the challenges posed by localization uncertainty in ABs. After analyzing the characteristics of localization uncertainty using manual driving data, lateral and longitudinal motion planning algorithms were suggested to counteract the uncertainty. The feasibility of the proposed approach was evaluated from vehicle tests carried out on the test track, followed by fully autonomous driving tests in actual urban bus-only lanes to confirm its applicability. Based on this study, three conclusions can be drawn below.

Firstly, the characteristics of the localization uncertainty in urban bus-only lanes were observed in terms of heading bias, lateral position error, and longitudinal position error. Changes in the sensor’s status showed significant drifts in the heading error, which in turn brought potential errors in the lateral position. Although the lateral position showed higher credibility than the longitudinal position, there still exists a risk of collision with roadside structures due to the excessive width of a bus. To actualize fully autonomous driving, these characteristics have to be dealt with. Therefore, Secondly, OF-MPC with an MHE scheme has been implemented for lateral motion planning to counteract the heading bias. The effectiveness of OF-MPC with MHE was confirmed through improved path-tracking performance and enhanced ride comfort by reducing unnecessary lateral motion through the utilization of the corrected heading without bias. Lastly, the drivable corridor and chance constraint have been implemented to handle lateral and longitudinal position errors, respectively. The drivable corridor enabled planned lateral motion to effectively prevent collisions with roadside structures. With the chance constraint, the proactive longitudinal motion could be obtained with regard to stop lines. Consequently, safety against road boundaries and stop lines could be secured.

Although the proposed motion planning approach for ABs showed good performance against the localization uncertainty in actual urban driving environments, there still exists room for further performance improvements. Based on the assumption described in Section IV-A., the proposed approach could be inapplicable at moments when there are distinct changes in performances from localization and perception modules. Furthermore, system uncertainties and nonlinearities raised from significant variations in the EV’s actual dynamics from the dynamics model, as well as changes in operating conditions, could potentially deteriorate the accuracy of the disturbance estimator. As a follow-up study to address these potential problems, we plan to improve the robustness of the proposed motion planning approach through real-time estimation of the dynamics model’s parameters using learning-based MPC and adaptive tube-based nonlinear MPC schemes. In addition, while the proposed approach effectively addressed delays, particularly at lower speeds encountered on urban roads, unaddressed factors such as computational time and communication delays must be considered when applying the approach over a wider speed range.

ACKNOWLEDGMENT

This paper is based on the first author’s doctoral dissertation [30].