1. Introduction
The advanced development of science and technology has made many researchers put their efforts to enhance safety, energy-saving, comfort, and agility in road traffic through vehicle automation. Since the last two decades, many researchers have addressed how the raw images from environment and the steering control states of a vehicle could be used to drive a vehicle autonomously in real time [
1]. Nearly 30 years have passed, and we still have not seen the commercialization of autonomous vehicles without a steering wheel. Various advanced driver assistance system (ADAS) modules have been put on the market, such as adaptive cruise control (ACC), autonomous emergency braking system (AEB), lane keeping system (LKS), etc. Although these features can help drivers reduce the burden of driving and improve road safety, they may still not able to solve congestion and parking problems in urban areas.
Recently, research has also focused on parking assist technologies, such as vision-based parking assist systems [
2,
3], ultrasonic-based auto parking systems [
4], laser scanner radar-based parking systems [
5], and parking assist systems using hybrid method [
6]. While these research efforts ended with the function of assisted parking, more advanced challenges remain, for instance, to build a system that includes path planning and fully automated parking. First, the path planning must convey to the vehicle the knowledge of its surrounding environment so that the vehicle control unit can command its actuators with appropriate actions in real time. A variety of methods of path planning were based on the requirements of the problem to be solved, such as rapidly-exploring random trees (RRT) [
7], heuristically-guided RRT [
8], potential field algorithm [
9], and A-star [
10]. By adding kinematic constraints to the A-star, a hybrid A-star algorithm was proposed by Dolgov et al. [
11], which had been used by the Stanford team to execute U-turns on blocked roads and to navigate vehicles in parking lots in a grand challenge project supported by the Defense Advanced Research Projects Agency (DARPA).
After the path planning task sends signals to command a vehicle to track the expected path, the vehicle control unit will drive the vehicle by considering its coupled longitudinal and lateral dynamic behaviors. For a small low-speed harvester-assisting vehicle, Loukatos [
12] simply used its geometrical model to calculate speed controls to drive motors to reach a specific turning angle. For high-speed normal vehicles with nonlinear and/or time-varying dynamics, traditional PID controller could scarcely be used to maneuver such vehicles of non-holonomic motions. Alternative methods of gain scheduling, parameter adaptation, and auto-tuning might be able to upgrade PID control performances [
13] with or without vehicle models. More advanced techniques, such as sliding mode control [
14], pure pursuit control [
15,
16], optimal predictive control [
17], iterative linear quadratic regulator (LQR) [
18], nonlinear model predictive control [
19], and robust control [
20] have been proposed to control the vehicle with complex dynamic behaviors.
One of the most promising methods of autonomous vehicle control is model predictive control (MPC), which accommodates both kinematic and dynamic vehicle models to improve the vehicle tracking performance at low and high speeds [
21]. Either nonlinear MPC or time-varying linear MPC is attractive to the application of trajectory tracking for self-driving vehicles, and has been proved stable in the sense of Lyapunov theorem [
22]. Vougioukas [
23] applied a nonlinear model predictive tracking controller to a mobile robot in the presence of obstacles. Only its kinematic model was used to compute an optimal M-step-ahead control sequence in real time, by minimizing the cost of tracking errors and control efforts. However, the computational load may impede fast applications to self-driving vehicles [
24].
This paper combines hybrid A-star path planning and time varying linear MPC with vehicle dynamic models for an electric van (e-van) to simulate self-driving and parking on a hardware-in-the-loop (HiL) simulation platform. The HiL simulation technique is often used for testing the proposed path planning and model predictive control strategy before it is implemented on a real vehicle in the real-world environment. In this paper, the hardware part has power steering wheel and motor, accelerator and brake pedals and their control unit, and computers. Other vehicle physical parts, components, subsystems, and vehicle dynamics are replaced by mathematical models coded in the software platform on computers. Since simulations are performed on a virtual plant, no physical plant could be damaged, and no people could be injured.
Section 2 establishes the kinematic and dynamic vehicle models.
Section 3 briefs the hybrid A-star path planning algorithm.
Section 4 presents the MPC by minimizing a cost function that describes vehicle performance in terms of safety, energy-saving, comfort, and agility.
Section 5 introduces experimental results with the HiL platform and
Section 6 concludes the research.
3. Hybrid A-Star Algorithm
A crucial part for a self-driving vehicle is the path planning system, which typically encompasses different abstract layers, such as mission and motion planning. Mission planning consists of trajectory planning and path planning. The path planning generates a collision free path in an obstacle environment by optimizing it with regard to some rules, while the trajectory planning schedules the vehicle movement on the planned path. The motion planner, on the other hand, operates at a lower level to avoid obstacles while progressing towards local goals.
This paper applies the hybrid A-star algorithm [
11,
25] of path planning for an auto parking system. The first phase of this approach is different from the well-known A-star algorithm, which uses 3D kinematic states of the vehicle, but we modify the state-update rule to obtain the continuous-state data on the discrete searching grids on a map. The traditional A-star path planning only allows reaching centers of cells connected with piecewise linear lines. However, the hybrid A-star allows expansion towards any continuous point on the grid as a state, as shown in
Figure 3. The state is described by x = (
x,
y,
ψ), in which
x and
y denote the position of the mass center
C and
ψ the heading of the vertex or the yaw angle of the vehicle in
Figure 1.
The non-holonomic constraints of the vehicle are considered for the vertex expansion by taking one of three actions—maximum steering left, maximum steering right, or no steering. In
Figure 4, the vertex search starts from the current vehicle state
xs to the goal state
xg which are provided by the PreScan simulation platform. Then, the hybrid A-star will generate six successor vertices, three of which are driving forward, while three are driving in reverse (not shown in the figure). Each vertex expansion is generated by the minimal turning radius
based on the vehicle parameters in
Figure 1 to make sure that the resulting paths are always drivable. When a new vertex is expanded to a cell already occupied by other vertices, the ones with the higher cost will be deleted and the vertex with the least cost remains.
Any point
xn on the planned path satisfies the optimal value of the cost function
where
g(
xn) is the distance or cost between
xs and
xn,
h(
xn) is the predicted distance or cost from
xn to
xg, namely, the heuristic function.
The heuristic function used in the hybrid A-star path planning consists of a constrained heuristic and an unconstrained heuristic. The constrained heuristic ignores environmental obstacles but incorporates the geometric and dynamic constraints of the vehicle. The unconstrained heuristic disregards vehicle constraints but only accounts for obstacles.
In this paper, the Reeds-Shepp curve [
26] was used as the constrained heuristic function for searching a path, which referred to the shortest curve that considered the heading and curvature restrictions. Taking the curvature limit of our vehicle into account, the Reeds-Shepp curve could generate drivable way points for the vehicle. As to the unconstrained heuristic function, the traditional A-star with Euclidean distance was introduced. By combining both constraint and unconstraint heuristic functions, the final path was generated to satisfy geometry constraints as well as to avoid obstacles.
In experiments, the size of each grid cell was 0.5 × 0.5 m2 to compose the scene of the test field on the PreScan simulation platform. As the vehicle was entering the parking lot of a size about 65 × 45 m2, it was switched to the self-driving mode. The Nvidia AutoChauffeur PX2, which had a Cortex A57 microarchitecture, took about 0.5–1.5 s to plan the path from the entrance of the parking lot to the selected parking space. During the movement, the vehicle speed was under 10 km/h; the control actions were determined in a sampling period of 100 ms after collecting the real-time information of sensors and waypoints along the path.
Though a vehicle model used to expand vertices might result in an excessive steering action, it can be easily solved by the proposed MPC optimization.
Figure 5 illustrates a visualization process of path planning on the parking lot of testing site created in the graphical user interface (GUI) of the ROS visualization tool (RViz).
4. Model Predictive Control
The MPC that imitates the working pattern of the human brain is here applied to autonomous vehicles under various constraints.
Figure 6 illustrates the conceptual diagram of MPC. The measured or estimated system states are sent to the MPC block, where the optimal control action is calculated by a system model and constraints to track the pre-filtered set points by minimizing a cost function.
4.1. Vehicle States Prediction Model
The system model in
Figure 6 is responsible for vehicle states prediction. The kinematic model (5) can be linearized about a reference state
and a reference control input
by Taylor series expansion after neglecting high-order terms:
where
.
Under the assumptions of a small steering angle,
, and
, and small changing rate of side slip angle,
dβ/
dt = 0, dynamic Equations (6)–(12) can be simplified with the Magic Formula tire model (4) as
where 2
Ct(
Nf +
Nr) represents the driving force or braking force exerted between tires and ground. Define a new state
z = [
vy,
vx,
ψ,
,
Y,
X]
T, and input
u2 = [
δ,
p], where
p = 2
Ct(
Nf +
Nr)/
m denotes the pedal angle of accelerator or brake. The linearized state-space model of (17)–(21) is derived as
where
4.2. Cost Function and Constraints
The cost function for the MPC is defined in a quadratic form:
where
is the cross-tracking error from reference path,
is the heading error from reference path, as shown in
Figure 7. The weighting factors
w’s account for the importance of vehicle safety, energy-saving, comfort, and agility. For safety and agility,
weighs the cross-tracking error from reference path, and
the heading error from reference path. For energy-saving,
weighs the pedal angle of accelerator or brake, and
the change of the pedal angle of accelerator or brake. For comfort and safety,
weighs the steering angle,
the change of steering angle, and
the coupling effect of vehicle speed and steering angle. For agility,
weighs the vehicle speed deviation from the reference.
It is interesting to devise a rule of choosing reasonable values for the eight weighting factors. First, the eight weighting factors can be grouped into two categories: the weightings , , , and on the importance of vehicle states, and the weightings , , , and on the importance of control variables. The former category relates more about vehicle safety by evaluating the trajectory tracking and heading errors as well as speed errors, while the latter relates more about energy-saving and somewhat of driving comfort. Agility has something to do with fast response. In this paper, when we say that the self-driving vehicle is agile, it means that the vehicle returns quickly back to the planned reference trajectory from which the vehicle may deviate. Therefore, the weightings on , and care both tracking errors and fast response to diminish the errors.
Safe driving is always the first rule for a self-driving vehicle. It is suggested in this paper that, in any cases, 60% or more weightings should be assigned to the vehicle states category which emphasizes safe driving, but less weightings be put to the control variables category to consider moderately the energy saving and comfort of driving.
The inequality constraints of control input
and
are hard constraints, while the predicted outputs are usually soft constraints, as follows:
5. Experiments and Results
The HiL simulation platform for experiments encompasses two parts as shown in
Figure 8. The hardware part in quadrants 1 and 2 consists of an Nvidia AutoChauffeur PX2, a power steering wheel with motor, and an accelerator and brake pedal and its controller (Logitech G29). The software part includes:
- (1)
ROS on the Nvidia PX2 in quadrant 1, where the proposed hybrid A-star path planning algorithm and MPC algorithm are executed;
- (2)
Vehicle kinematic and dynamic models model coded in the Matlab/Simulink platform in quadrant 3;
- (3)
Sensor models of GPS, camera, radar, and lidar, and the world map for simulation provided by PreScan platform in quadrant 4.
The test field was on the ITRI campus, whose map was planned off-line by the hybrid A-star algorithm in the PreScan simulation platform. In each sampling period of 100 ms, the proposed MPC took from the map of planned path 20–40 waypoints ahead of the current position to calculate a new control action by applying an open source Interio Point Optimizer (Ipopt) [
27]. The experimental setup is illustrated in
Figure 9, and parts of the test field are illustrated in
Figure 10. This section verifies the feasibility of the hybrid A-star path planning and the proposed MPC strategy through three different simulation scenarios.
5.1. Self-driving Performance
In order to verify the independence and intuitiveness of the weighting factors in the cost function of the MPC strategy, two sets of weighting factors are assigned, as shown in
Table 1. As was suggested, that in any cases 60% or more weighting should be assigned to the vehicle states category that emphasized safe driving. Setting 1 has 85% weighting and Setting 2 has 62.5% weighting on the vehicle states category. For more insight into the weighting factors on the tracking and heading errors, Setting 1 puts 27.5% each on
and
, but Setting 2 puts only 17.5% each on
and
. In other words, Setting 1 puts more importance on safety than Setting 2, in terms of 55% versus 35% on the weighting factors
and
.
In contrast, less importance is suggested for the category of control variables, considering the energy saving and comfort of the vehicle. For energy saving, Setting 2 assigns 20% but Setting 1 assigns only 10% weighting to and , which means that more importance on energy saving is given in Setting 2 than in Setting 1. For the importance of comfort, Setting 2 allocates 32.5% while Setting 1 allocates only 15% weighting to , , and , which means that Setting 2 may cause more driving comfort than Setting 1.
Figure 11, marked with driving ordering numbers, shows that on the prescribed route in
Figure 10, Setting 1 has better tracking and heading performances than Setting 2, where more erroneous offsets are observed, especially at corners, from the prescribed route.
Figure 12 also shows that the self-driving vehicle with Setting 1 (
at 20%) follows the prescribed tracking speed at 10 km/h better than that of Setting 2 (
at 12.5%). However,
Figure 13 shows that the lateral acceleration curve of Setting 2 is much smoother and therefore more comfortable for passengers in the vehicle, than that of Setting 1.
5.2. Double Lane Change (DLC)
The DLC experiment investigated the driving performance of the proposed MPC strategy and compared it to the traditional proportional–integral–derivative (PID) controller. Both the driver’s model and the ISO 3888-2 DLC test scenario were built in in the PreScan simulation platform.
Figure 14 shows that the tracking error of the vehicle on the DLC route by the MPC strategy was smaller than that of the traditional PID controller. Similarly, from
Figure 15, the MPC strategy resulted in a better DLC performance than the PID controller in terms of vehicle lateral acceleration.
5.3. Energy Efficiency Simulation
Energy saving is often considered a secondary requirement for self-driving vehicles. With the same scenario of the planned path in
Figure 11, the energy efficiency is investigated with 10 sets of prescribed weighting factors in
Table 2. The weightings on the vehicle states were still assigned with a high percentage within 45–55% to emphasize safe driving. In addition, it is noticed that the weightings on
and
were given at the same value, and the resultant weightings on
and
were assigned within a narrow range between 20.5 and 24, so that the agility and driving comfort were equally important for all these cases. Thus, the major difference was between the weightings
and
on the tracking and heading errors and the weightings
and
on the energy saving. The range of the resultant values of
and
was between 23 and 34.5.
Figure 16 shows the time histories of the accelerator pedal angle in percentage (%) for the traditional PID controller and for the three selected cases (1, 3, and 10) to investigate energy efficiency by the proposed MPC strategy for self-driving vehiclecontrol. Since the sampling frequency was 10 Hz in the simulation, the energy consumption was estimated by summing 1200 data of pedal angles, each of which corresponds to an equivalent amount of power. Here, the equivalent power per 1% of pedal angle is assumed to be
Q in W/% the total energy consumption can be calculated by
It is easy to understand that the summation is the estimate of energy consumption that is simply the enclosed area under the curve
p(
t). The energy efficiency improvement was then calculated by
where
was 8.96Q (Wh). The
and
of each case are shown in
Table 2. The relationship bewteen the energy efficiency improvement and the weighting (
) on energy saving is given in
Figure 17. As was predicted, the energy efficiency was better if the weighting on energy saving was higher.
5.4. Integral Test of Path Planning and Auto-Parking
In this experiment, we tested manual driving mode, self-driving mode, dynamic path planning, obstacle avoidance, and automatic parking. The parking lot map (
Figure 10) was created in the PreScan simulation platform. In
Figure 18, a driver on the HiL platform drove the “virtual” vehicle manually to the entrance to the parking lot in about 100 s. After selecting a parking space on the graphical user interface in ROS (RViz), the driver switched to self-driving mode. The vehicle automatically followed the prescribed reference path without any obstacle. Around 200 s, the vehicle detected an obstacle via RADAR and made an emergency stop. At this point, the hybrid A-star recalculated and updated a new path to avoid the obstacle. Finally, the vehicle passed the obstacle and parked in the selected space. The corresponding curves of the steering angle, yaw rate, and brake pedal signal during the path planning and auto-parking scenarios are shown, respectively, in
Figure 19,
Figure 20 and
Figure 21, where ordering numbers are provided in correspondence with those in
Figure 18.
6. Conclusions
This paper applied a hybrid A-star algorithm for path planning and the MPC strategy for a self-driving e-van on the HiL simulation platform. The hardware of the platform consisted of an electric power steering system, accelerator, and brake pedals, and an Nvidia drive PX2 on ROS. The vehicle dynamics model, sensors, controller, and test field map were virtually built with the PreScan simulation platform. In comparison with the traditional PID control, the path tracking performance was improved by the proposed MPC strategy by minimizing the prescribed cost function which accounted for vehicle safety, energy-saving, comfort, and agility. Three self-driving scenarios were presented. Self-driving performance was first investigated by two different sets of weighting factors for the cost function of MPC, each accounting for different levels of importance on vehicle safety, energy-saving, comfort, and agility. Second, the stability and tracking performance of the ISO 38888-2 DLC test by the proposed MPC was verified to be better than that by the traditional PID controller. Finally, an integral test was executed on the HiL platform to demonstrate the feasibility of the shift from manual to self-driving mode, obstacle avoidance, dynamic path planning, and auto-parking. This HiL platform can be used for future development of path planning, sensing, perception, decision-making, and control for any kind of self-driving vehicles.