\section{Introduction}\label{sec:introduction}
Since dense traffic and cluttered environments bring uncertainties and interaction challenges with other road users, one of the most challenging tasks in automated driving is safe and comfortable urban driving. Amongst others, trajectory planning is a far from trivial task due to the varying road layouts, different (types of) road users, and highly dynamic scenarios. The task of planning a trajectory for the automated vehicle (AV) to follow has gained increasing attention and focus over the last years \cite{Paden2016}\cite{Claussmann2019}.

To compute fast (near-)optimal trajectories in terms of safety and comfort, several types of algorithms exist, falling in one of these categories: \textit{optimization-based}, \textit{path primitive-based} and \textit{sampling-based}. The first approach formulates the environment, such as the road layout and other traffic participants, as artificial potential fields or hard constraints and tries to solve the optimization problem analytically. For instance, \cite{Siebenrock2020} tries to find an optimal trajectory by formulating a Model Predictive Control (MPC) problem. However, usually such methods are limited in flexibility of environment representations due to the analytical formulation, limiting their applicability in real-world autonomous driving systems. The second approach generates a set of path primitives and calculates an optimal velocity and/or acceleration profile per path primitive, after which the most optimal trajectory in terms of safety and comfort is selected. In \cite{Artunedo2019}, a real-time motion planner is developed based on sampling path primitives as $5^{th}$-order Bezier curves and calculating suitable velocity profiles based on the motion prediction of dynamic objects. However, the fixed mathematical description of the path primitives can limit the flexibility of the solution, especially in complex road layouts and highly cluttered environments. Moreover, the decoupling of the velocity profile from the path primitives causes problems for online decision making such as overtaking a slower vehicle versus cruising behind it.

To overcome the limited flexibility of the aforementioned approaches, the trajectory planning problem can be solved by using a \textit{sampling-based} approach, which has shown great potential in several robotics applications \cite{Arab2016}\cite{Littlefield2018}. In \cite{Li2014}, an algorithm called \textit{Stable-Sparse-RRT (SST)} is introduced where piecewise-constant control samples are generated following a stochastic distribution and propagated through a kinematic model. Sampling and propagating control inputs guarantee that the solution adheres to the vehicles non-holonomic kinematic constraints. Furthermore, by applying forward propagation of the kinematic system in time, a time space can be mapped on the system states. A time-bounded collection of vehicle states will be henceforth referred to as a trajectory. Mapping calculated vehicle states in time enables the planner to take time-bounded motion prediction of other road users into account. However, due to the inherently exploring nature of such sampling based algorithms, convergence to high quality trajectories is slow and (sub-)optimality is often not guaranteed \cite{Zheng2021}. To overcome these limitations and enable SST solutions to be applied for AV trajectory planning, initial state space exploration of high potential regions such as the lane center or around a previously found solution is enforced, resulting in higher quality solutions while maintaining the same planning update rate.

The main contributions of this work are as follows:
\begin{enumerate}
    \item We apply the generic algorithm introduced in \cite{Li2014} in a real-time automated driving trajectory generation application

    \item We present a novel extension to the trajectory planner, improving its efficiency, by providing an initial exploration method of high potential regions such as the lane center and previous solutions.

\end{enumerate}
The proposed trajectory planner is implemented in a realistic simulation environment and validated based on several urban-driving scenarios, showing its capability of providing safe and comfortable trajectories in the case of complex road layouts and other static and dynamic traffic participants.

This article is outlined as follows. In Section \ref{sec:problemstatement}, the problem statement and preliminaries are defined. Section \ref{sec:method} focuses on the method used for solving the trajectory planning problem. This method is proven in a simulation study in Section \ref{sec:validation} and future work is presented in Section \ref{sec:conclusion}.

\section{Preliminaries}\label{sec:problemstatement}
To improve on-road safety in extended operational design domains, novel motion planning algorithms are developed in the EU Horizon Safe-up Project. Based on prediction models of other traffic participants' behaviour and advanced risk estimation, motion planners and vehicle control strategies need to show improved safety and capabilities in various use cases, such as bad weather, faulty sensors or dynamic urban environments including pedestrians or cyclists.

The focus of this work is to find safe, comfortable and feasible vehicle trajectories in an urban environment. Due to the highly dynamic nature of this environment, the trajectory planner should be able to find suitable solutions in real-time with a sufficient update rate. Furthermore, the solution should take into account the kinodynamic constraints of the vehicle, the road layout and the predicted motion of other traffic participants.
\subsection{Problem statement}
To define the constraints and goals of the planner, first consider the state of the vehicle at time $k$ denoted by $\textbf{x}_k=(x_k,y_k,\theta_k,v_k)$, where $x$ and $y$ represent the vehicle ground-plane position, $\theta$ is the vehicle planar orientation and $v$ is the vehicle longitudinal velocity. To allow for safe and comfortable driving in urban environments, the vehicle needs to proactively account for the predicted motion of other traffic participants. Often these predictions are neglected which can lead to reactive and thus more aggressive vehicle behavior. To account for predicted motions, assume a set of $O$ objects, where the measured state of an object at time $k$ is given by its planar coordinates denoted by $\textbf{o}^o_k=(x^{o}_k, y^{o}_k, \theta^{o}_k), \forall o \in O$. Furthermore, assume the predicted trajectories of these dynamic objects to be available with no noise or inaccuracies. The challenge is then to include these into the trajectory planning problem in a (near) optimal way by balancing progress along the road and minimize driving risks. In the absence of static or dynamic obstacles, the vehicle is expected to follow the lane centers of the road network. To model that, the road infrastructure is described by a set of $L$ lanes, where each lane is described as a collection of lane center points, together with a lane width. A lane is denoted by $l^i = ( r^1, r^2, \hdots, r^R, \omega ), \forall i \in [1 \hdots L]$, where $r^j \forall j \in R$ is a lane center point described by its planar position, $(x^r, y^r)$ and $\omega$ is the lane width. To lead the vehicle along a global route, the trajectory planner receives global goals to plan towards. Here, the goal is represented by a goal space $\mathbb{G} \subset \mathbb{R}^4$, and it is computed such that its position components $(x,y)$ span over all available lanes at a fixed distance $g^d$ along the vehicles position, with longitudinal threshold $g^t$. Furthermore, the orientation component $\theta$ and velocity component $v$ are unbounded, meaning that a specific orientation and velocity are not required for the goal to be reached. The schematic overview of the goal space is depicted in Figure \ref{fig:goalspace}.
\begin{figure}[h!]
    \centering
    \includegraphics[clip, trim=75mm 60mm 75mm 60mm, width=1\columnwidth]{Figures/goalArea.pdf}
    \caption{\footnotesize{Ground plane visualization of goal space}}
    \label{fig:goalspace}
\end{figure}
Considering the aforementioned requirements for safe and comfortable driving, the trajectory planning problem at time $k$ requires finding a trajectory $ \xi_k  =  \begin{bmatrix}
\textbf{x}_0 \;
\textbf{x}_1
\hdots
\textbf{x}_f \\
\end{bmatrix}$, which satisfies the kinodynamic constraints of the vehicle, is collision-free given the predicted object states and brings the vehicle to the goal space such that $\textbf{x}_f \in \mathbb{G}$. Besides these constraints, the planner should also minimize the risk of collision for the vehicle as well as other traffic participants by maintaining sufficient distance to others, follow the appropriate lane center for a given road network whenever possible and maximize comfort.
\subsection{State and control space definition}
Most sampling-based planners generate search trees by sampling new states in a selected state space. Such state spaces can be $\mathbb{R}^2$, Dubins or Reeds-Shepp \cite{Rathinam2018}. However, planning in such spaces either puts a limit on the movement of the car (e.g., the constant curve radius in Dubins), does not satisfy the differential and/or non-holonomic kinematic constraints of the vehicles motion model (e.g., instantaneous discrete change of vehicle heading) or require a specific steering function for the system. To facilitate the integration of kinematic and differential constraints, while maintaining flexibility on the reachable space of the vehicle, control space sampling poses an alternative. Here, control inputs are sampled and propagated in the system over a specified time. Given a fixed initial state (sample), this results in a new state space sample that satisfies the kinematic constraints.
As often used in motion planning designs and also in this work, a non-linear kinematic bicycle model as shown in Figure \ref{fig:kinematicmodel} is used to allow for real-time trajectory planning for larger time horizons, i.e., 4-10 seconds \cite{Paden2016}.
The model is described by its discretized kinematic equations, using Euler discretization, as:
\begin{equation}
\label{eq:vehiclemodel}
\begin{split}
    x_{k+1} =& x_k+t^sv_k\cos{\theta_k},\\
    y_{k+1} =& y_k+t^sv_k\sin{\theta_k},\\
    \theta_{k+1} =& \theta_k+t^s\frac{v_k}{L^w}\tan{\delta_k},\\
    v_{k+1} =& v_k+t^sa_k,
\end{split}
\end{equation}
where $t^s$ is the integration time, $a$ denotes the longitudinal acceleration of the vehicle, $\delta$ denotes the steering angle and $L^w$ denotes the wheel base of the vehicle.
\begin{figure}
    \centering
    \includegraphics[width=0.8\columnwidth]{Figures/Vehicle.pdf}
    \caption{\footnotesize{Schematic representation of the kinematic bicycle model.}}
    \label{fig:kinematicmodel}
\end{figure}
Due to physical constraints of the vehicle, such as a maximum velocity, acceleration and/or steering angle, the state is bounded by $\textbf{x} \in \mathbb{X} \subset \mathbb{R}^4 $ and the input to the system is denoted by  $\textbf{u} = [ a,  \delta] \in \mathbb{U} \subset \mathbb{R}^2 $.

\subsection{Stable Sparse RRT (SST)} \label{SST}
To plan safe and comfortable trajectories for autonomous urban driving, this work builds upon the anytime-planner introduced in \cite{Li2014}, where a sampling-based planning method called \textit{Stable Sparse RRT (SST)} is introduced. Given an initial state, SST explores the state space by generating a state tree, following an iterative \textit{selection-propagation-pruning} procedure (as shown in Figure \ref{fig:SSTProcedure}), briefly described here and more extensively in \cite{Li2014}.

\textit{i) Selection}. First, a random state $\textbf{x}$ is sampled in $\mathbb{X}$. Then a set of states in the current tree are selected within distance $d^n$ of this random state. If no states are available within $d^n$, the closest state in the state tree is returned. If more than one state are found in the vicinity of the random state, the state is returned with the lowest motion cost-to-go from the start state. A detailed description of the motion costs applied in this work is given in Section \ref{optimization}.

\textit{ii) Propagation}. A control input is sampled and the selected state is propagated for propagation time $t^p$ to achieve a new state. Details on the input sampling procedure can be found in Section \ref{inputsampling}.

\textit{ iii) Pruning}. Finally, the new state is added to the tree only if it passes several validity checks, as described in Section \ref{validitychecking}, and the closest state is either more than threshold $d^p$ away, or the motion cost-to-go is lower than that of the set of states in its vicinity. If the new state is added to the tree, the states in the vicinity are moved to an inactive subset and thus removed from further consideration in the planning iteration.

\begin{figure}[h!]
    \centering
    \includegraphics[clip, trim=80mm 30mm 80mm 30mm, width=\columnwidth]{Figures/SSTOverview.pdf}
    \caption{\footnotesize{Simplified overview of the SST \textit{selection-propagation-pruning procedure}}}
    \label{fig:SSTProcedure}
\end{figure}
\section{Trajectory generation method}\label{sec:method}
To find a fast, safe and comfortable vehicle trajectory, the SST algorithm, developed previously for robotic applications, is applied in this work in an automated driving architecture, and enhanced with specific methods for sampling the control inputs, checking the validity of vehicle states and calculating the motion costs of travelling to successive states. Furthermore, a novel method is proposed expanding the SST algorithm, to force initial exploration of high potential state space regions.
\subsection{Input sampling}\label{inputsampling}
The SST algorithm applies piece-wise constant control inputs to the kinodynamic model to steer the system from a selected state to a new state. Here, a control input sampler is proposed which takes into account the physical limitations of the system in terms of achievable acceleration, deceleration and steering angle by bounding the input samples. The input components are sampled following a zero-mean Gaussian distribution with standard deviation $\sigma^a$ and $\sigma^{\delta}$, for the acceleration and steering angle respectively. If the sample falls outside of the input bounds, $[\underline{a}, \overline{a}]$ and $[\underline{\delta}, \overline{\delta}]$ respectively, a new sample is drawn until a valid input is found. A Gaussian distribution is chosen to promote the probability of samples around the zero-mean, but still allows for steering, braking and accelerating up to the specified limits whenever required. The sampled inputs are applied to the system by forward propagation during time $t^p$. Note that propagation time $t^p$ can be equal to, but also be a multitude of integration step time $t^s$.
\subsection{State validity checking}\label{validitychecking}
To prevent the planner from steering the system to states which are not valid, a state validity checker is proposed in this section. To keep the planner as efficient as possible, non-valid states are not taken into consideration in the search tree generation. According to the trajectory planning problem definition, a state is considered valid if it is within specific state bounds, within the lanes and the full vehicle geometry is not in collision with other traffic participants.

\textit{i) State bounds:} The validity checker checks for two different types of state bounds. Position state bounds on the $x$ and $y$ state are added to prevent exploration in undesired regions of $\mathbb{X}$, such as positions far beyond the ground plane area of $\mathbb{G}$, thereby stimulating faster convergence to high-quality paths. Furthermore, a bound is added to the velocity state based on the local legal speed limit or vehicle physical limitation.

\textit{ii) Road layout:} The vehicle should remain within the driveable space, spanned by the lanes, at all times. To check whether a vehicle is within a lane, a penalty grid map $P(x,y)$ is constructed. In this grid map, the cell penalties increase linear along the distance to the closest lane center point, up to a maximum value $\overline{P}$, as follows:
\begin{equation}
    P(x,y) =
    \begin{cases}
    \frac{2\overline{P} d^{r}}{\omega}, & \text{if } d^{r} < \frac{\omega}{2} \\
    \overline{P}, & \text{otherwise,}
    \end{cases}
\end{equation}
where $d^{r}$ is the Euclidean distance from the cell center to the closest lane center point in $L$.

The ground plane position part, $(x,y)$, of the vehicle state under consideration is mapped on a cell in a penalty grid map. If the penalty of this cell is equal to threshold $\overline{P}$, the vehicle state is considered invalid.
An example of the penalty grid map is depicted in Figure \ref{fig:gridmap}, where the lane center is given a low penalty (purple) and the penalty increases towards the lane edges, until it reaches its maximum value $\overline{P}$ (red).

\begin{figure}[h!]
    \centering
    \includegraphics[clip, trim=10mm 10mm 10mm 20mm, width=0.9\columnwidth]{Figures/2RoundaboutGrid4.png}
    \caption{\footnotesize{Grid map based on lane information on a roundabout with red lines representing lane markings.}}
    \label{fig:gridmap}
\end{figure}

\textit{iii) Object collision:} Any vehicle state where the ego vehicle geometry, denoted by length $L$ and width $W$, overlaps with the bounding box of another traffic participants predicted state at the same time instance is considered as a collision. For a given vehicle pose at time $k$, $(x_k, y_k, \theta_k)$, a polygon is created on the ground plane considering the length and width of the vehicle. Similar polygons are created for all predicted target poses at time $k$, under the assumption that the geometry of any target is considered known. If the vehicle polygon overlaps with at least one of the predicted target polygons, the state is considered invalid. The polygon overlap is checked using the \textit{ray casting} algorithm \cite{Kularathne2018}.

\subsection{Optimization objectives}\label{optimization}
Due to the \textit{anytime}-nature of the planner, every planning iteration is allowed to run for a fixed amount of time, $t^q$, tuning which is a trade-off between planner update rate and quality of the found trajectory. To determine the most optimal solution, costs are awarded either to states, \textit{state costs}, or the transition from one state to the other, \textit{motion costs}. Resulting motion costs are calculated from state costs of successive states by approximating the integral of the cost using the trapezoidal approximation function. Henceforth, any state cost of state $\textbf{x}$ will be referred to as $c(\textbf{x})$, whereas the motion cost from state $\textbf{x}_n$ to state $\textbf{x}_{n+1}$ will be denoted by $m(\textbf{x}_n,\textbf{x}_{n+1})$.

The total motion cost of travelling from a starting state $\textbf{x}_0$ to a state $\textbf{x}_N$ is a summation of the successive motion costs:
\begin{equation}
m(\textbf{x}_0,\textbf{x}_N) = \sum^{N-1}_{n=0} m(\textbf{x}_n,\textbf{x}_{n+1}).
\end{equation}
In this work, each individual motion cost is a weighted summation of four separate optimization objectives: \textit{\textbf{p}ath \textbf{l}ength}, \textit{\textbf{d}esired \textbf{v}elocity}, \textit{\textbf{p}enalty \textbf{g}rid} and \textit{\textbf{t}arget \textbf{c}learance}:
\begin{equation}
    \tilde{m}(\textbf{x}_n, \textbf{x}_{n+1}) = \sum_{i \in W} w^i m^i(\textbf{x}_n, \textbf{x}_{n+1}), W = \{pl, dv, pg, tc\},
\end{equation}
where $\tilde{m}(\cdot)$ is the total multi-objective motion cost and $w^i$ is the weighting of the individual optimization objectives, which are detailed in the remainder of this section.

\paragraph{Path length}
Where comfort and safety allow it, the vehicle should attempt to reach its goal as efficient as possible in terms of driving distance. Therefore, this optimization objective tries to minimize the geometric length of the trajectory, i.e., the distance along the ground plane path, by adding a cost based on the Euclidean distance between the positions of successive states:
\begin{equation}
    m^{pl}(\textbf{x}_n, \textbf{x}_{n+1}) = \sqrt{(x_{n+1}-x_n)^2 + (y_{n+1}-y_n)^2}.
\end{equation}

\paragraph{Desired velocity}
In nominal traffic, the vehicle should try to drive as fast as possible while remaining safe, comfortable and obliging to traffic rules. Therefore, an objective is added to push the vehicle velocity towards a desired speed, which is (near) the legal speed limit or at another situation-appropriate speed advice. The state cost of the desired velocity objective is defined as:
\begin{align}
    \label{eq:cds}
    c^{dv}(\textbf{x}_n) = |v_n - v^{d}|,
\end{align}
where the desired velocity is denoted by $v^d$.
\paragraph{Penalty grid}
Besides rendering states invalid, the penalty grid as described in section \ref{validitychecking} also penalizes undesired states, using the cost term:
\begin{equation}
    c^{pg}(\textbf{x}_n) = P(x_n, y_n).
\end{equation}
This optimization objective makes it attractive for the vehicle to drive on the lane center. When a deviation from the lane center is required, for instance for a static object near the side of the road, this optimization objective ensures that the lateral displacement with respect to the center of the lane is as minimal as safety allows.

\paragraph{Target clearance}
Due to the dynamic nature of the urban environment, trajectory planning should be able to take into account the predicted motion of other traffic participants. Besides collision avoidance by rendering a state invalid when its position collides with a predicted target position, the vehicle should steer clear of other traffic participants with some margin. Therefore, a repulsive artificial potential field is introduced around the time-dependent target position. The potential field of a perceived object is described using a two-dimensional Gaussian function as follows:
\begin{align}
    c^{tc}(\textbf{x}_k)=&\sum_{n=1}^{N}A^{o}e^{-f(\mathbf{x}_k,\mathbf{o}^o_k)}, \\
    \text{where }f=&\frac{\left(x_k-x^{o}_k\right)^2}{\left(\sigma^{o,x}\right)}+\frac{\left(y_k-y^{o}_k\right)^2}{\left(\sigma^{o,y}\right)},
\end{align}
where $x^o_k$, $y^o_k$ represent respectively the predicted x-position and y-position at time $k$ of object $o$. Furthermore, $A^o$ represents the maximum magnitude of the repulsive field of the object. Finally, $\sigma^{o,x}$ and $\sigma^{o,y}$ represent the longitudinal and lateral standard deviations of the potential field distribution respectively.

\subsection{Domain knowledge-based exploration branches}
The SST algorithm focuses on a trade-off between fast exploration of the available state space and fast convergence to high-quality paths. However, due to the lack of domain-specific knowledge on the state space, exploration can be slow or come at the expense of the final solution optimality. To overcome this, an informed exploration of the state space based on the road layout can be used. As the vehicle is expected to predominantly follow a (virtual) lane center while driving in a structured environment, an initial exploration branch of states is computed in the search tree, starting at tree-root $\mathbf{x}_0$, which steers the vehicle towards the lane center. This algorithm, for which a schematic overview is provided in Figure \ref{fig:InitialSolution}, picks a point on the current ego lane center at a fixed distance, $d^{la}$. Then, for a fixed number of $N$ iterations, inputs are sampled and applied to the current state to achieve a new state sample. After $N$ iterations, the sample closest to the lane center point, and corresponding control inputs, are saved. The initial state is set to the newly achieved state and the process is repeated until the goal space is reached, no valid state can be found or the distance from the starting state to the latest state sample exceeds a threshold.

Furthermore, the original SST algorithm attempts to solve each planning query without knowledge of the previous planning iterations, i.e., previously computed trajectories and/or decisions are not taken into account. However, in case of a sufficiently high planner update rate and vehicle trajectory tracking capability, the vehicle is likely to be somewhere along the previously calculated trajectory. In this work, the search tree is augmented with a branch created from the trajectory calculated in the previous iteration, provided that the starting state, tree root $\mathbf{x}_0$, is sufficiently close to a given state on the (interpolated) previous solution. The schematic overview of the algorithm for embedding the computed trajectory of the previous planning iteration is depicted in Figure \ref{fig:LastSolution}. Here, the state of the previous trajectory is selected closest to the tree root. If this state is sufficiently close to the tree root, the states of the previous trajectory after the selected state are iteratively checked for validity and, if valid, added to the branch. If a state is invalid, the process is aborted. This does not only allow the planner to more quickly converge to high-quality paths as it can refine the previous iteration result, but it also raises the probability that any decision taken in a previous iteration (e.g., overtake versus the following predecessor) is adhered to in later iterations.S

The augmented planner as introduced in this section will henceforth be referred to as \textit{Domain-Knowledge-Informed-SST}, or in short \textit{DKI-SST}. The original algorithm will be referred to as \textit{base SST}.

\begin{figure} [h!]
    \centering

    \includegraphics[trim=12mm 0mm 5mm 0mm, clip, width=1.0\columnwidth]{Figures/flowchart.pdf}
    \caption{\footnotesize{Algorithm flowchart for adding road-layout based exploration branch.}}
    \label{fig:InitialSolution}
\end{figure}
\begin{figure}[h!]
    \centering
      \includegraphics[trim=10mm 0mm 7mm 0mm, clip, width=1.0\columnwidth]{Figures/flowchart2.pdf}
    \caption{\footnotesize{Algorithm flowchart for adding the previous solution based exploration branch. The trajectory found in the previous iteration is denoted by $\tilde{\xi}$}}
    \label{fig:LastSolution}
\end{figure}

\section{Scenario definition and validation}\label{sec:validation}
In this section, the method described in Section \ref{sec:method} will be proven based on several simulation studies. First, the performance of the DKI-SST planner is compared to the base SST planner in two simple scenarios to demonstrate the performance gain by introducing domain knowledge. Second, the performance of the DKI-SST planner is benchmarked based on three common urban driving scenarios, which include various road-layouts and dynamic traffic participants. A schematic overview of the scenarios is given in Figure \ref{fig:scenarios}.

\begin{figure}[h!]
    \centering
    \includegraphics[trim=5mm 40mm 5mm 50mm, clip, width=1.0\columnwidth]{Figures/scenarios.pdf}
    \caption{\footnotesize{Scenario definition for validation. I): following a straight road. II): overtaking a static predecessor. III): navigating a roundabout. IV): evasive maneuvering based on VRU motion prediction. V): speed adjustment based on VRU motion prediction}}
    \label{fig:scenarios}
\end{figure}

The automated vehicle is modeled as kinodynamic system with three degrees of freedom, as described in \cite{Pacejka2005}. This system is modeled using the Gazebo simulator \cite{Koenig2004}, where the road networks are described in the OpenDrive format \cite{Dupuis2010} and the dynamic actors are defined using the OpenScenario standard \cite{OpenScenario}. For vehicle actuation based on the computed trajectories, a feedback tracking controller is employed based on the deviation from the path and accompanying speed. The base SST planner is implemented in the OMPL framework \cite{OMPL2012}.
The parametrization of the DKI-SST planner is depicted in Table \ref{table:pars}.

\begin{table}
\begin{tabular}{p{0.9cm} p{4.2cm} p{1.3cm} p{0.5cm}}\toprule
\textbf{Parameter}              & \textbf{Description} & \textbf{Value} & \textbf{Unit}\\
\toprule
$fq$ & Update rate & 2 & Hz\\
$t^q$ & Query solving time & 0.3 & s\\
$g^d$ & Fixed goal distance & 30.0 & m\\
$g^t$ & Longitudinal goal space threshold & 2.0 & m\\
$t^s$ & Integration time & 0.04 & s \\
$d^n$ & Nearest neighbour selection distance & 0.2 & - \\
$t^p$ & Propagation step & 0.4 & s\\
$d^p$ & Pruning distance & 0.1 & -\\
$\sigma^a$ & Acceleration input standard deviation & 0.8 & $\text{ms}^{-2}$\\
$\sigma^{\delta}$ & Steering angle input standard deviation & 0.2 & rad\\
$\underline{v},\overline{v}$&Min/max velocity&$[0.0,6.0]$&$\text{ms}^{-1}$\\
$\underline{a},\overline{a}$&Min/max acceleration&$[-0.8,0.8]$&$\text{ms}^{-2}$\\
$\underline{\delta},\overline{\delta}$&Min/max steering angle&$[-0.4,0.4]$&$\text{rad}$\\
$\overline{P}$ & Maximum penalty grid cell value & 100 & - \\
$\overline{p}$ & State invalid cell value & 99 & - \\
$w^{pl}$ & Path length penalty weight & 0.05 & - \\
$w^{ds}$ & Desired speed penalty weight & 0.5 & - \\
$w^{pg}$ & Penalty grid penalty weight & 0.2 & - \\
$w^{tc}$ & Target clearance penalty weight & 2.0 & - \\
$v^d$&Desired velocity&5.0&$\text{ms}^{-1}$\\
$A_O $ &Object risk amplitude& 100 &-\\
$\sigma^{o,x}$&Longitudinal risk deviation&3.0&$\text{m}$\\
$\sigma^{o,y}$&Lateral risk deviation&2.0&$\text{m}$\\
$d^{la}$&Road layout look ahead distance&3.0&$\text{m}$\\
$d^{i}$&Threshold distance for road layout exploration branch& 40.0 &$\text{m}$\\
$N$ & Max iterations for sampling road layout exploration state & 100 & - \\
$d^{m}$&Threshold distance between start state and previous iteration closest state & 1.0 & - \\
\toprule
\end{tabular}
\caption{\footnotesize{Simulation parameters for all considered cases.}}
\label{table:pars}
\end{table}

\subsection{Comparison to base SST planner}
The performance gained by introducing domain knowledge is demonstrated based on two simple scenarios: driving on a straight road and overtaking a static object. For the first case, the vehicle should stay in the middle of the lane as much as possible while both longitudinal and lateral accelerations should be limited to drive comfortably. Furthermore, the vehicle should drive as close as possible to the desired velocity, as it is not limited by safety-critical environmental factors. To circumvent the possible inaccuracies of a tracking controller in this scenario, the vehicle is manually driven at exactly the lane center, at exactly the desired speed ($5m/s$). For each calculated trajectory, the mean absolute values for the deviation from the desired velocity, accelerations and deviation from closest lane center are taken. The simulation is run for a period of minimum 10 sec.
For the second  considered case, when overtaking a static object, it is important that the vehicle initiates a timely overtaking maneuver, thus preventing coming too close to the object or causing a collision. Therefore, the minimal distance to the object is taken as a performance indicator from this scenario. As vehicle actuation is required in this scenario, the tracking controller is tuned for optimal performance for the Base SST and DKI-SST algorithms separately.

The overview of the results of both planners, shown in Table \ref{table:comparisonToBase} clearly show the benefits of the DKI-SST method. This leads to 75\% performance gain in the use of acceleration and more than 95\% performance gain in the deviation from desired speed.
\begin{table}[h!]
\small
\begin{tabular}{ p{3.4cm} p{1.0cm} p{1.0cm} p{1.5cm}}
\toprule
\textbf{Performance criteria} & \textbf{Base SST} & \textbf{DKI-SST} & \textbf{Performance gain [\%]} \\
\toprule
 Mean absolute trajectory longitudinal acceleration $[\text{ms}^{-2}]$ & 0.52 & 0.13 & 75.0  \\
 Mean absolute trajectory deviation from desired speed $[\text{ms}^{-1}]$ & 1.52 & 0.051 & 96.6 \\
 Mean absolute trajectory deviation from closest lane center $[\text{m}]$ & 0.33 & 0.057 & 82.7 \\
 Minimum resulting distance to target $[\text{m}]$ & 3.87 & 3.94 & 1.8\\
 \toprule
\end{tabular}
\caption{\footnotesize{Performance indicators for the base SST algorithm versus DKI-SST.}}
\label{table:comparisonToBase}
\end{table}
\subsection{Roundabout}
The third scenario under consideration is navigating a roundabout. The discrete change in heading angle of the road when entering the roundabout, together with the smooth curve on the roundabout itself, make this a challenging road layout for urban driving. In this scenario, the vehicle is guided to take the roundabout at the third exit. In Figure  \ref{fig:UC3TrajectoriesAlt}, it is shown that the ground plane positions of all computed trajectories (blue) are navigating the vehicle within the lane over the roundabout. This results in a safe and comfortable driven path, depicted in red.
\begin{figure}[h!]
    \centering
    \includegraphics[trim=0mm 0mm 0mm 0mm, clip, scale=0.25]{Figures/UC3/trajec_alt.jpg}
    \caption{\footnotesize{Planned trajectories (blue) and final driven path (red) on a roundabout}}
    \label{fig:UC3TrajectoriesAlt}
\end{figure}
\subsection{Preventive steering for Vulnerable Road Users}
To determine the trajectory planners capability of accounting for object motion prediction, two VRU-crossing scenarios are evaluated. In scenario \rom{4}, the VRU has merely started crossing the two-lane street when the automated vehicle approaches. In this case, a preemptive lane change is a safe and valid option. The resulting calculated trajectories are depicted in Figure \ref{fig:27b} at four different time instances. Here, it is shown that the vehicle makes are comfortable lane change, well before the current VRU repulsive field requires to do so. The preemptive lane change is initiated due to the expected trajectory of the VRU, which is expected to be at a sufficient lateral distance for the vehicle to safely pass after a lane change.

\begin{figure}[h!]
        \centering
        \begin{subfigure}[b]{0.90\columnwidth}
            \centering
            \includegraphics[trim=30mm 0mm 30mm 0mm, clip, width=\columnwidth]{Figures/UC27b/1.jpg}
            \caption[]%
            {{\footnotesize t=51.8s}}
            \label{fig:27b_1}
        \end{subfigure}
        \begin{subfigure}[b]{0.95\columnwidth}
            \centering
            \includegraphics[trim=30mm 0mm 30mm 0mm, clip, width=\columnwidth]{Figures/UC27b/2.jpg}
            \caption[]%
            {{\footnotesize t=55.3s}}
            \label{fig:27b_2}
        \end{subfigure}
        \vskip\baselineskip
        \begin{subfigure}[b]{0.95\columnwidth}
            \centering
            \includegraphics[trim=30mm 0mm 30mm 0mm, clip, width=\columnwidth]{Figures/UC27b/3.jpg}
            \caption[]%
            {{\footnotesize t=59.8s}}
            \label{fig:27b_3}
        \end{subfigure}
        \hfill
        \begin{subfigure}[b]{0.95\columnwidth}
            \centering
            \includegraphics[trim=30mm 0mm 30mm 0mm, clip, width=\columnwidth]{Figures/UC27b/4.jpg}
            \caption[]%
            {{\footnotesize t=62.8s}}
            \label{fig:27b_d}
        \end{subfigure}
        \caption[]
        {\footnotesize{Planned trajectory of the vehicle and predicted trajectory of the pedestrian, including its current repulsive potential field (blue). Trajectories range from current time (purple) to 5 seconds ahead (yellow)}. }
        \label{fig:27b}
\end{figure}

\subsection{Preventive braking for Vulnerable Road Users}
In the second VRU-crossing scenario, the VRU is well into crossing the road when the vehicle approaches. Due to the movement of the VRU, solely evasive steering is not a safe option. Figure \ref{fig:27c} shows the computed paths again at four different time instances, together with the velocity profile along this path. The vehicle reduces its speed along the computed trajectories, to allow the VRU to safely cross the road, before speeding up again. The resulting vehicle speed is shown in Figure \ref{fig:UC27cVelocity}.
\begin{figure}
        \centering
        \begin{subfigure}[b]{0.95\columnwidth}
            \centering
            \includegraphics[trim=30mm 0mm 30mm 0mm, clip, width=\columnwidth]{Figures/UC27c/1.jpg}
            \caption[]%
            {{\footnotesize t=49.5s}}
            \label{fig:27c_1}
        \end{subfigure}
        \begin{subfigure}[b]{0.95\columnwidth}
            \centering
            \includegraphics[trim=30mm 0mm 30mm 0mm, clip, width=\columnwidth]{Figures/UC27c/2.jpg}
            \caption[]%
            {{\footnotesize t=52.5s}}
            \label{fig:27c_2}
        \end{subfigure}
        \vskip\baselineskip
        \begin{subfigure}[b]{0.95\columnwidth}
            \centering
            \includegraphics[trim=30mm 0mm 30mm 0mm, clip, width=\columnwidth]{Figures/UC27c/3.jpg}
            \caption[]%
            {{\footnotesize t=55.0s}}
            \label{fig:27c_3}
        \end{subfigure}
        \hfill
        \begin{subfigure}[b]{0.95\columnwidth}
            \centering
            \includegraphics[trim=30mm 0mm 30mm 0mm, clip, width=\columnwidth]{Figures/UC27c/4.jpg}
            \caption[]%
            {{\footnotesize t=59.5s}}
            \label{fig:27c_d}
        \end{subfigure}
        \caption[]
        {\footnotesize{Planned trajectory of the vehicle, together with the current repulsive potential field of the pedestrian. The coloring of the trajectory indicates the computed speed at that point, ranging from black (5$m/s$) to yellow (1$m/s$)}.  }
        \label{fig:27c}
\end{figure}

\begin{figure}[h!]
    \centering
    \includegraphics[width=0.9\columnwidth]{Figures/UC27c/27cLongVelocity.jpg}
    \caption{\footnotesize{Longitudinal vehicle velocity over time of scenario \rom{5}}}
    \label{fig:UC27cVelocity}
\end{figure}

\section{Conclusions}\label{sec:conclusion}
In this work, a real-time and high-quality trajectory planner is introduced for safe and comfortable automated driving in dynamic urban environments which takes explicitly into account the road layout and the predicted motion of other traffic participants. Building on a previous SST algorithm, additional road-layout and previous trajectories knowledge are embedded to optimize the generation and exploration of the search tree branches.
Results show significant performance gains of this new method when compared to the baseline on several criteria.
Furthermore, using several urban scenarios, the planner has shown to be able to navigate various road layouts, as well as be able to adapt both planned path and speed profile to the predicted motion of other traffic participants.
Future work includes shaping the repulsive artificial potential function based on object information to include even more realistic information, e.g. estimation uncertainty and object type, to include more complex context awareness and validate these through real-life experiments.

\section{Acknowledgments}\label{sec:acknowledgements}
This work is supported by the EU Horizon 2020 R{\&}D program under grant agreement No. 861570, project SAFE-UP (proactive SAFEty systems and tools for a constantly UPgrading road environment).

\printbibliography

\end{document}


\section{Introduction}
Modern process industry is characterized by high complexity due to strong coupling among different units and thus efficient control is critical for maintaining high-level operations. Among various process control strategies, PID control has received widespread attention owing to its simplicity (with only three tunable parameters) in design and effectiveness in delivering high control performance for numerous real-world applications \cite{wang2007proposal}. However, these PID parameters require careful tuning for yielding superior control performance. One class of naive PID tuning methods is based on the trial-and-error approach, where different PID parameters are tested and the one giving the best control performance is deployed to the system. However, this trial-and-error approach is time-consuming and may result in sub-optimal PID parameters \cite{dogru2022reinforcement}. Another category of PID tuning techniques is the rule-based methods where the models of the process are often required \cite{seborg2016process}. However, such models may not be always available, especially for complex processes, which restricts the applicability of these methods. To address this issue, data-driven techniques based on, e.g., evolution optimization \cite{nyberg2017optimizing}, genetic algorithm \cite{mitsukura1999design}, and neural networks (NNs) \cite{d2007use}, have been developed for PID tuning without requiring a process model. However, these methods need a large quantity of labeled data for training due to their model-free nature, making them sample inefficient \cite{guan2021design}.

Recently, reinforcement learning (RL) has seen a surge in popularity in the control of dynamical systems \cite{zheng2020constrained}. RL is essentially a sequential decision-making process for optimizing a black-box objective function  \cite{sutton2018reinforcement}. Unlike supervised learning, model-free RL can learn the best policy to optimize the objective function from direct interactions with the environment and does not require prior knowledge about the environment. On the other hand, the PID tuning can be seen as a black-box optimization problem where the relation between PID parameters and the resultant control performance is unknown. On this aspect, RL has great potential to be an effective technique for solving the PID tuning problem through the sequential decision-making process \cite{lakhani2021stability}.

In light of these observations, research studies have been reported on using RL-based algorithms for PID tuning \cite{wang2007proposal,dogru2022reinforcement,guan2021design,zheng2020constrained,lakhani2021stability, younesi2019q, shi2020adaptive, lawrence2020reinforcement}. For example, Q-learning, a popular RL algorithm, has been used to design PID controllers that are adaptive to system operating condition changes \cite{younesi2019q}. However, the Q-learning algorithm works only on discrete state-action spaces, and cannot handle the continuous space \cite{shi2020adaptive}. One solution to this problem is to discretize the states and actions into bins before applying Q-learning \cite{lawrence2020reinforcement}. Nonetheless, such discretizations may lead to errors due to the finite resolution, and eventually result in poor control performance \cite{shi2020adaptive, noel2014control}. To this end, actor-critic-based algorithms have been utilized where the policy and Q-values are approximated by parameterized functions \cite{sutton2018reinforcement,wang2007proposal,dogru2022reinforcement}. In particular, the deep deterministic policy gradient (DDPG) algorithm proposed by DeepMind \cite{silver2014deterministic}, which is sample-efficient and effective in handling continuous state-action space, has been adapted to the PID tuning problem \cite{lakhani2021stability}. An updated version of DDPG with lower variance, known as twin delayed DDPG (TD3), has also been applied to PID tuning for nonlinear systems \cite{shi2020adaptive}. Although the TD3 algorithm has demonstrated improved sample efficiency compared to the other existing methods such as DDPG \cite{fujimoto2018addressing}, it often performs poorly in high-dimensional state-action space due to its inherent lack of exploration \cite{haarnoja2018soft}, which limits its efficacy in PID tuning.

In this article, we present a novel algorithm for facilitating the PID tuning, termed as entropy-maximizing TD3 (EMTD3), to overcome the issue of insufficient exploration associated with the traditional TD3 algorithm. Specifically, the developed EMTD3 algorithm deploys an entropy-based stochastic actor to ensure significant exploration at the beginning, followed by a deterministic actor based on the TD3 algorithm. For our method, the stochastic actor can well ensure sufficient explorations initially and the deterministic actor can facilitate fast convergence once knowledge about the environment is acquired. Moreover, the off-policy nature of the EMTD3 algorithm enables it to use past experience and makes this method sample efficient. The proposed method is applied to the PID tuning of a second-order system. Simulation results show that our method outperforms traditional TD3 significantly in improving the sampling efficiency and discovering optimal PID parameters.

This paper is organized as follows. Section \ref{sec: preliminaries} presents fundamentals about PID control and the TD3 algorithm, followed by our proposed EMTD3 method elaborated in \ref{sec: EMTD3}. The numerical case study is presented in Section \ref{sec: simulation} with our conclusions given in Section \ref{sec: conclusion}.

\section{Preliminaries}
\label{sec: preliminaries}
\subsection{PID Control}

The typical form of a PID controller has three adjustable parameters and can be expressed as \cite{seborg2016process}:
\begin{equation}
		u(t_{n}) = K_{p}\left[e(t_{n}) + \frac{1}{\tau_{I}} \sum_{i=1}^{n}e(t_i) + \tau_{D} \frac{e(t_n)-e(t_{n-1})}{\Delta t}\right], \label{eq: PID}
\end{equation}
where $t_n$ is the $n$-th time instant, $u(t_n)$ is the manipulated variable (MV), $e(t_n)$ is the error between the setpoint $y_{sp}$ and the controlled variable (CV) $y(t_n)$, and $\Delta t$ is the sampling interval. $K_p$, $\tau_I$, and $\tau_D$ are the PID parameters. In this work, we use the following anti-reset windup to accommodate the potential windup issue from the integral part  \cite{lakhani2021stability}:
\begin{equation}
	sat(u)=
	\begin{dcases}
		u_{min}, \quad & \text{if} \quad  u < u_{min}, \\
		u, \quad & \text{if} \quad  u_{min} \le u \le u_{max}, \\
		u_{max}, \quad & \text{if} \quad  u > u_{max},
	\end{dcases} \label{eq: Saturation}
\end{equation}
where $u_{min}$ and $u_{max}$ are the lower and upper bounds of the MV, respectively.

\subsection{Reinforcement Learning}
RL formulates the optimal control problem as a Markov decision process expressed as $(\mathbb{S},\mathbb{A},\mathbb{P},R)$, where $\mathbb{S}$ is the state space, $\mathbb{A}$ is the action space, $\mathbb{P}$ is the transition probability matrix, and $R$ is the reward signal \cite{sutton2018reinforcement}. In RL, an agent observes the state $s_t$ and deploys an action $a_t$ under current policy to the environment. The environment then evolves one step forward under $a_t$, generating a reward signal $r_t:=R(s_t, a_t)$  to the agent. To assess the action $a_t$ taken at state $s_t$, the Q-value $Q^{\pi}(s_t, a_t)$ is defined as the expected sum of future discounted rewards from time $t$ onward:
\begin{equation}
	Q^{\pi}(s_t,a_t) = \mathbb{E}\left[\sum_{t=0}^{\infty}\gamma^tr_{t+1} | s=s_t, a=a_t,\pi \right], \label{eq: q_value}
\end{equation}
where $\gamma\in[0,1]$ is the discount factor and $\pi(a_t|s_t):s_t\to a_t$ is the underlying policy mapping state $s_t$ to action $a_t$. The objective of the RL problem is to maximize the total cumulative reward by finding the optimal policy $\pi^{*}(a_t|s_t)$ from the optimal Q-value $Q^*(s_t, a_t)$
\begin{equation}
	\pi^{*}(a_t|s_t)=\begin{dcases}
		1, \quad &\text{if} \quad a_t=\arg \max_{a_t\in\mathbb{A}}~ Q^{*}(s_t, a_t),\\
		0, \quad &\text{otherwise}.
	\end{dcases}   \label{eq: optimal_policy}
\end{equation}
For a deterministic policy $\mu(a_t|s_t)$, the Q-value can be described by the Bellman expectation condition as
\begin{equation}
Q^{\mu}(s_t, a_t)=\mathbb{E} \left[ r_t +  \gamma Q^{\mu}(s_{t+1}, \mu(s_{t+1}))\right]. \label{eq:DeterministicQ}
\end{equation}
For a high-dimensional RL problem, it is challenging to learn the Q-value for each state-action pair individually. Under such circumstances, a function approximator (FA), generally a NN $Q_\theta(s_t,a_t)$, parameterized by $\theta$, is used to learn the true Q-value. The objective of the FA is to minimize the approximation error by finding the optimal parameter $\theta^{*}$:
\begin{equation}
\theta^{*}=\arg\min_{\theta}~\mathbb{E}\left[(y_t-Q_\theta(s_t,a_t))^2\right], \label{eq:CriticLoss}
\end{equation}
where $y_t= r_t+\gamma \max_{a}Q_\theta(s_{t+1},a)$ is the temporal-difference (TD) target.

In addition to the value-based method above, another important RL category is policy-based methods, where the RL  searches directly the optimal policy $\mu_\phi(s_t)$, parameterized by $\phi$, to maximize the accumulated reward $J(\mu_\phi)=\mathbb{E}_{\mu_\phi}\left[\sum_t R(s_t,a_t)\right]$. This class of methods uses gradient ascent to update $\phi$  \cite{lakhani2021stability}
\begin{equation}
\phi_{t+1}\leftarrow \phi_{t} + \alpha \nabla_{\phi} J(\mu_\phi)|_{\phi=\phi_t}, \label{eq: PG_update}
\end{equation}
where $\alpha$ is the learning rate, and the gradient of the cost $J(\mu_\phi)$, using the policy-gradient theorem \cite{sutton2018reinforcement}, is
\begin{equation}
\nabla_{\phi} J(\mu_\phi) = \mathbb{E}_{\mu_\phi}\left[\nabla_{a} Q^{\mu}(s_t,a)|_{a=\mu_{\phi}(s_t)}\nabla_{\phi}\mu_{\phi}(s_t)\right]. \label{eq: DPG_gradient}
\end{equation}

The actor-critic method combines both value-based and policy-gradient methods where the actor generates actions depending on the current state and the critic evaluates the action that was taken \cite{wang2007proposal, dogru2022reinforcement, lakhani2021stability}.

\subsection{DDPG and TD3}
As a model-free off-policy actor-critic method, the DDPG framework employs a deterministic policy and thus is sample-efficient \cite{silver2014deterministic}. The application of DDPG to PID tuning has been reported in \cite{lakhani2021stability}. However, the critic network in the DDPG suffers from the issue of overestimation bias due to the maximizing operation for calculating the TD target in \eqref{eq:CriticLoss}. To address this problem, the TD3 method is proposed where two different target Q networks, parameterized by $\theta_i^{\prime}$, $i=1,2$, are used for estimating two Q-values and the smaller one is then used for computing the TD target \cite{fujimoto2018addressing}:
\begin{equation}
y_t= r_t+\gamma \min_{i=1,2} Q_{\theta_i ^\prime}(s_{t+1},\mu_{\phi^{\prime}}(s_{t+1})), \label{eq:q_target}
\end{equation}
where $\phi^{\prime}$ is the parameter of the target actor network. Moreover, the parameters of the actor and target networks are updated less frequently to reduce the accumulation of function approximation error and hence reduce the variance of Q-value estimate.
In addition, a clipped noise is added to the target action to enable better exploration by adding uncertainty to the policy, further reducing the variance. Then the new target value is formulated as \cite{fujimoto2018addressing}
\begin{align}
y_t= & r_t+\gamma   Q_{\theta^\prime}(s_{t+1},\mu_{\phi^\prime}(s_{t+1})+\epsilon),
\end{align}
where $\epsilon\sim clip(\mathcal N (0,\sigma), -c, +c)$ is the added Gaussian noise, $\theta^\prime$ and $\phi^\prime$ are the parameters of the selected target critic and  actor networks giving smaller Q-value in \eqref{eq:q_target}, and $c$ is the bound on the target action. Details on the DDPG and TD3 algorithms can respectively be found in \cite{silver2014deterministic}, \cite{fujimoto2018addressing}.

\section{The Proposed EMTD3 for PID Tuning}
\label{sec: EMTD3}
\subsection{Entropy-Maximizing TD3 (EMTD3)}
In this paper, we propose a novel hybrid RL algorithm, termed as EMTD3, consisting of a stochastic actor for exploration and a deterministic actor for exploitation. At the initial stage, an entropy regularization term is added to the RL objective to simultaneously maximize the entropy along with the total cumulative reward:
\begin{equation}
J(\pi)=\sum_{t=0}^{T}\mathbb{E}_{(s_t,a_t)\sim\rho_{\pi}}[r_t+\beta\mathbb{H}(\pi(\cdot|s_t))], \label{eq:em_rl}
\end{equation}
where $\pi$ is the stochastic policy, the entropy  $\mathbb{H}(\pi(\cdot|s_t))=-\int_{a_t}\pi(a_t|s_t)\log\pi(a_t|s_t)$,  $\rho_{\pi}$ is the state-action distribution, and $\beta$ is the parameter controlling the relative weight between the two terms. The added entropy-maximizing term encourages the agent to sufficiently explore the state-action space and avoid convergence to the local solution. With the RL objective in \eqref{eq:em_rl}, based on the Bellman equation, the new Q-value can be obtained by expanding \eqref{eq: q_value} into
\begin{align}
Q^\pi(s_t,a_t)=& \sum_{t=0}^{T}\mathbb{E}_{(s_t,a_t)\sim\rho_{\pi_{\phi_1}}}[r_t+\nonumber\\&\gamma(Q^\pi(s_{t+1},a^{\prime})-\beta\log \pi(a^{\prime}|s_{t+1}))], \label{eq:q_em_rl_2}
\end{align}
where $a^{\prime}\sim \pi(\cdot|s_{t+1})$ and is sampled from $\pi$. As discussed in Section \ref{sec: preliminaries}, in the continuous space, we use FAs, parameterized by $\theta$ and $\phi_1$, respectively, to estimate the  Q-value and policy. The optimal parameter $\theta^{*}$ in the FA for the Q-value can be obtained by minimizing the loss function
\begin{equation}
\theta^{*}=\arg\min_{\theta}~\mathbb{E}\left[(y_t-Q_\theta(s_t,a_t))^2\right].
\end{equation}
Similar to the deep Q network, we compute the TD target $y_t$ by using a separate NN, parameterized by $\theta^{\prime}$,
\begin{align}
y_t= r_t+ \gamma (Q_{\theta ^\prime}(s_{t+1},a^{\prime})-\beta \log \pi_{\phi_{1}}(a^{\prime }|s_{t+1})).
	\label{eq:target_em}
\end{align}
The parameter $\phi_1$ of the stochastic policy can be calculated utilizing the re-parameterization trick introduced in \cite{haarnoja2018soft}.
\begin{figure} [thpb]
	\centering
	\includegraphics[width=\columnwidth]{EMTD3}
	\caption{The structure of automatic  PID tuning on the  EMTD3 framework.}
	\label{fig: EMTD3}
\end{figure}
Adding the entropy-maximizing term to the RL objective assists the stochastic policy in exploring the action space. However, it may result in  overestimation of the entropy and hence demonstrate unstable behaviors during training \cite{ding2021averaged,ali2021reducing}. In addition,  stochastic policy methods often require a large number of samples for training in continuous action space and may show slow convergence. On the other hand, deterministic policy methods do not suffer from above issues. However, the critic networks in deterministic methods often generate peaks for some actions and remain stuck in that region without finding the global optimum \cite{ali2021reducing}.

In the proposed EMTD3 method, we combine entropy-maximizing stochastic policy and deterministic policy together to preserve the respective advantages. Fig. \ref{fig: EMTD3} shows the structure of the proposed EMTD3 algorithm. It consists of two critic networks, two target critic networks, a deterministic, a stochastic, and a target actor network. The developed EMTD3 method is divided into two stages. For the first stage (warm-up period), we implement the entropy-maximizing stochastic actor focusing on the exploration, which reduces the action peaks by adding uncertainty and assists in effectively exploring the action space \cite{ali2021reducing}. After the first stage, the policy shifts to a deterministic one (TD3) to focus on exploitation and accelerate the convergence. Note that both stages share the same critic networks and critic target networks, but the actor networks differ. In other words, once the first stage is complete, these critic networks and their target networks will have been preliminarily trained. This can greatly reduce the variance associated with Q-value estimates in the second stage, which is beneficial for speeding up the learning process of the deterministic actor. In addition, we use (\ref{eq:target_em}) and (\ref{eq:q_target}) respectively to calculate the TD target $y_t$ for stochastic and deterministic policies, where two different target networks are used to compute two Q-values and the smaller one is selected to alleviate the overestimation error. The parameters of the critic networks can be updated by using the gradient of the following cost function that quantifies the approximation error:
 \begin{equation}
 J(Q_{\theta_i})=\frac{1}{|B|} \sum\left(y_t-Q_{\theta_{i}}(s,a) \right)^2,
 \end{equation}
where $i=1,2$ represents $i$-th critic network and $B$ is the batch size sampled from the replay buffer that stores the experience $(s_t,a_t,r_t,s_{t+1})$. The parameters of the stochastic and deterministic actor networks ($\phi_1$ and $\phi_2$, respectively) can be updated using the gradient of the following costs:
\begin{align}
	J(\pi_{\phi_{1}})=& \frac{1}{|B|} \sum_{s \in B}(\min_{i=1,2}Q_{\theta_i}(s,a_{\phi_{1}}^{\prime }(s))-\nonumber\\&\beta \log \pi_{\phi_{1}}(a_{\phi_1}^{\prime}(s)|s)), \label{eq:em_update}
\end{align}
\begin{equation}
	J(\mu_{\phi_{2}})=\frac{1}{|B|} \sum_{s \in B}Q_{\theta_1}(s,\mu_{\phi_2}(s_t)). \label{eq:td3_update}
\end{equation}
To reduce the variance, we update the deterministic actor and target networks less frequently than the critics \cite{fujimoto2018addressing}. The target network parameters are updated by ($\rho$ is close to 1):
\begin{align}
\phi_2^\prime\leftarrow \rho\phi_2^\prime+(1-\rho)\phi_2,~ \theta_{i}^\prime\leftarrow \rho \theta_{i}^\prime+(1-\rho)\theta_{i}.
\end{align}

\begin{table*}[tbh]
	\centering
	\caption{Pseudo code for EMTD3-based PID tuning} \label{Algo_EMTD3_PID_Tuning}
	\begin{tabular}{ll}
		\hline
		\multicolumn{2}{l}{\textbf{Algorithm: EMTD3 for PID tuning}} \\ \hline
		1: & \textbf{Input:} initial Q-value FA parameters $\theta_1$, $\theta_2$, stochastic and deterministic policy parameter $\phi_1$, $\phi_2$, and empty replay buffer $\mathcal{D}$ \\
		2: & Initialize target networks $\theta_1^{\prime}\leftarrow\theta_1$, $\theta_2^{\prime}\leftarrow\theta_2$, $\phi_1^{\prime}\leftarrow\phi_1$,
		$\phi_2^{\prime}\leftarrow\phi_2$ \\
		3:&\textbf{Repeat} \\
		4:& If time-step $<$ warm-up period:\\
		5:&\qquad \qquad Observe state $s$ and select action $a\sim \pi_{\phi_1}(\cdot|s)$ \\

		6:& If time-step $\ge$ warm-up period:\\
		7:&\qquad \qquad Observe state $s$ and select $a=clip(\mu_{\phi_2}(s)+\epsilon, a_{low}, a_{high})$ , where $\epsilon$ is random noise.\\
		8:& Execute $a$ in the environment \\
		9:& Observe the next state $s^{\prime}$ and compute the episodic reward $r$ using the closed-loop data \\
		10:& Store the transition $\left(s,a,r,s^{\prime}\right)$ into the replay buffer $\mathcal{D}$ \\
		11:& If $s^\prime$ is terminal, reset environment\\
		12:&  \textbf{Update RL agent parameters:} \\
		13:&  Randomly select a batch of transitions $B=\left\{\left(s,a,r,s^{\prime}\right)\right\}$ from $\mathcal{D}$ with
		the number of transitions as $|B|$ \\
		14: & Compute the target for the Q-value:\\
		15:& $\qquad $ If time-step $<$ warm-up period:\\
		16: & \qquad \qquad $y= r+ \gamma (\min_{i=1,2} Q_{\theta_i ^\prime}(s^\prime,a^{\prime})-\alpha \log \pi_{\phi_{1}}(a^{\prime}|s^\prime))$, where $a^{\prime}\sim \pi(.|s^\prime)$ and is sampled from $\pi_{\phi_1}$  \\
		17: & $\qquad $ If time-step $\ge$ warm-up period:\\
		18: & \qquad \qquad $y=r+\gamma (\min_{i=1,2} Q_{\theta_i ^\prime}(s^\prime,a^{\prime}(s^\prime)))$, where $a^{\prime}(s^\prime)=clip(\mu_{\phi^\prime_2}(s^\prime))+clip(\epsilon,a_{low},a_{high}))$\\
		19:& Update the Q-value by one step of gradient descent using cost function w.r.t. $\theta_i$: \\
		& \qquad \qquad  $\nabla_{\theta_i}\frac{1}{|B|} \sum_{\left(s,a,r,s^{\prime}\right)\in B}\left(Q_{\theta_{i}}(s,a) - y \right)^2$ \\
		20: &  If time-step $<$ warm-up period:\\
		21: &  \qquad \qquad Update the policy by one step of gradient descent using cost function w.r.t. $\phi_1$:\\
		22:&  \qquad \qquad $\nabla_{\phi_1}\frac{1}{|B|}\sum_{\left(s,a,r,s^{\prime}\right) \in B}(\min_{i=1,2}Q_{\theta_i}(s,a_{\phi_{1}}^{\prime }(s))-\alpha \log \pi_{\phi_{1}}(a_{\phi_1}^{\prime }(s)|s))$, where $a_{\phi_1}^{\prime}(s)$   is sampled from $\pi_{\phi_1}(\cdot|s)$\\
		23: &\qquad \qquad Update the parameters $\theta_i^{\prime}$  in the target networks\\

		24: &  \qquad \qquad $\theta_{i}^\prime\leftarrow \rho \theta_{i}^\prime+(1-\rho)\theta_{i}$\\
		25: & If  time-step $\ge$ warm-up period: \\

		26: &  \qquad  If $time-step \% d == 0$ where $d$ is the update period, then\\
		27: &  \qquad \qquad$\nabla_{\phi_2}\frac{1}{|B|}\sum_{\left(s,a,r,s^{\prime}\right) \in B}Q_{\theta_1}(s, \mu_{\phi_2}(s))$\\
		28: &  \qquad \qquad  Update target networks with:\\
		29: &  \qquad \qquad  $\theta_{i}^\prime\leftarrow \rho \theta_{i}^\prime+(1-\rho)\theta_{i}$\\
		30:&  \qquad \qquad  $\phi_2^\prime\leftarrow \rho\phi_2^\prime+(1-\rho)\phi_2$\\
		31:& \textbf{Until Convergence then End Repeat}  \\
		\hline
	\end{tabular
\end{table*}

\subsection{EMTD3 on PID Tuning}
In the proposed EMTD3-based PID  tuning framework (see Fig. \ref{fig: EMTD3}), the environment comprises the closed-loop system that can generate MVs, CVs, and setpoint from a closed-loop simulation.
For this framework, each closed-loop simulation represents \textit{one agent-environment interaction} in the EMTD3 algorithm. To reduce the dimension, we define the environment state $\mathbf{s}_k$ after the $k$-th closed-loop simulation to be a subset of the  trajectories of process variables:
\begin{align}
&\mathbf{s}_k=\left[\tilde{\mathbf{y}}_k, \tilde{\mathbf{u}}_k, \tilde{\mathbf{y}}^{sp}_k\right]^\mathsf{T},~~\tilde{\mathbf{y}}_k=[y_{0}^{k},y_{i}^{k},y_{2i}^{k},\ldots]^\mathsf{T}, ~~\nonumber\\ &\tilde{\mathbf{u}}_k=[u_{0}^{k},u_{i}^{k},u_{2i}^{k},\ldots]^\mathsf{T}, ~~\tilde{\mathbf{y}}_k^{sp}=[y_{0}^{sp,k}, y_{i}^{sp,k}, y_{2i}^{sp,k},\ldots]^\mathsf{T},\nonumber \label{eq: Env_state}
\end{align}
where $k$ stands for the $k$-th closed-loop operation and $i$ is the sampling interval. The setpoint $\tilde{\mathbf{y}}^{sp}_k$ is included in the state to provide the agent information about the control target. In this work, we have kept the setpoint fixed throughout the closed-loop simulation. We define the PID parameters as the RL action
\begin{equation}
\mathbf{a}_k = \left[K_p, \tau_I, \tau_D\right].
\end{equation}
The reward function $r_k$ is defined as the total accumulated tracking error  throughout one closed-loop simulation:
\begin{equation}
r_k = -\sum_{s=0}^{K} e_s^{2}, \label{eq: ISE}
\end{equation}
where $e_s = \tilde y_{s}^{sp}-\tilde y_{s}$ is the tracking error at time $s$ and $K$ is the length of the closed-loop simulation. The pseudo code of the proposed EMTD3 method for PID tuning is illustrated in Table \ref{Algo_EMTD3_PID_Tuning}.

\section{Simulation}
\label{sec: simulation}
In this section, we test the proposed EMTD3-based PID tuning via the following second-order process
\begin{equation}
	G(s) = \frac{0.3}{25s^2+10s+1}e^{-10s}. \label{eq: model}
\end{equation}
The MV in the simulation is bounded between $u_{min}=-20$ to $n_{max}=100$. The setpoint $y^{sp}$ is fixed at 7.5. Table \ref{table: Hyperparameters} lists the hyperparameters adopted by the EMTD3 algorithm.
\begin{table}[h]
	\centering
	\caption{List of hyperparameters used by the EMTD3 algorithm}
	\label{table: Hyperparameters}
	\begin{tabular}{lll}
		\hline
		Parameters & Values (Case I) & Values (Case II)\\ \hline
		Learning rate $\alpha$ for actors & 0.02 & 0.02\\
		Learning rate $\beta$ for critics & 0.0005 & 0.008 \\
		Warm-up period & 70 & 100\\
		Batch size $B$ & 40 & 40 \\
		Episode length $T$ &  200 & 200\\
		Environment state dimension $n_s$ & 30 & 30 \\
		Discount factor $\gamma$ in the return & 0.99 & 0.99\\
		Polyak averaging coefficient $\rho$ & 0.006 & 0.006\\
		Initial temperature $\beta$  & 2 & 2\\
		$\frac{1}{\beta}$ incremental factor & 0.005 & 0.0001\\
		Initial noise variance $\sigma^2$  & 0.05 & 0.08\\
		Noise decay factor & 0.005 & 0.0045\\ \hline
	\end{tabular
\end{table}

\subsection{Case Study I}
For this case study, we specify the ranges of the PID tuning parameters to be $K_P \in [0,15]$, $\tau_{I}\in[0,15]$, and $\tau_D\in[0,10]$, respectively. The developed EMTD3 algorithm in Table \ref{Algo_EMTD3_PID_Tuning} is applied to identify the optimal PID parameters for \eqref{eq: model}. Specifically, in the warm-up stage, we gradually decrease the weight $\beta$ in \eqref{eq:em_rl} to diminish the entropy effect and thus reduce the exploration, followed by introducing a decaying zero-mean Gaussian noise after the warm-up stage. For comparison, we also use traditional TD3 with the same hyperparameters as in Table \ref{table: Hyperparameters} for the PID tuning, where a decaying zero-mean Gaussian noise is added to the action each time. Fig. \ref{fig: small_action_exploration} shows the distribution of the attempted PID parameters by the traditional TD3 (red) and our EMTD3 (blue) method, where the pentagram shows the optimal PID parameters for \eqref{eq: model}. It is observed that the TD3 method does not well explore the space and thus converges to local solutions far from the optimum. In contrast, the EMTD3 method explores the action space uniformly and converges closer to the optimum. Fig. \ref{fig: small_action} shows the learning curves between these two methods, where the EMTD3 method can settle down rapidly whereas the TD3 method needs much more training episodes. Thus, the EMTD3 show superior sample efficiency that TD3. Moreover, the discovered PID parameters from EMTD3 yield a larger reward and thus better control performance than those from TD3, which is also indicated by the step response in Fig. \ref{fig: small_action_cv}. The obtained PID parameters from EMTD3 can make the CV track the setpoint much faster without significant overshoots.

\begin{figure} [thpb]
	\centering
	\includegraphics[width=0.8\columnwidth]{small_action_exploration-eps-converted-to}
	\caption{Exploration of the PID parameter space by TD3 (red dot) and the proposed EMTD3 algorithm (blue dot).}
	\label{fig: small_action_exploration}
\end{figure}
\begin{figure} [thpb]
	\centering
	\includegraphics[width=0.8\columnwidth]{learning_curve_small_action-eps-converted-to}
	\caption{Learning curves with TD3 and EMTD3 for Case I.}
	\label{fig: small_action}
\end{figure}
\begin{figure} [thpb]
	\centering
	\includegraphics[width=0.8\columnwidth]{final_control_small_action-eps-converted-to}
	\caption{The PID control performance from TD3 and EMTD3 for Case I.}
	\label{fig: small_action_cv}
\end{figure}

\subsection{Case Study II}
\begin{figure} [thpb]
	\centering
	\includegraphics[width=0.8\columnwidth]{medium_action_exploration-eps-converted-to}
	\caption{Exploration of the PID parameter space by TD3 (red dot) and EMTD3 (blue dot) for Case II.}
	\label{fig: medium_action exploration}
\end{figure}
For this case study, we specify a much larger range for PID parameters than Case I to further assess the performance of EMTD3 and TD3 for PID tuning: $K_P\in [0,20]$, $\tau_{I}\in[0,20]$, and  $\tau_D\in[0,20]$. The exploration and exploitation schemes for both the TD3 and EMTD3 algorithms are the same as those in Case I. Fig. \ref{fig: medium_action exploration} illustrates the distribution of attempted PID parameters from these methods where the EMTD3 method again shows better exploration and thus converges closer to the optimum than TD3. Note that this action space is much larger and forms a more challenging problem than Case I. The learning curves in Fig. \ref{fig: medium_action} illustrate the convergence rate of the EMTD3 and TD3 algorithms. Similar to Case I, the EMTD3 method is more sample efficient and can deliver better ultimate reward than TD3, as shown in the inner plot in Fig. \ref{fig: medium_action}. The last-episode tracking performance based on the PID parameters delivered from EMTD3 and TD3 is shown in \ref{fig: medium_action_cv}, which indicates that the discovered PID parameters from EMTD3 can enable faster tracking performance with less tracking error. This is consistent with the observation with Fig. \ref{fig: medium_action exploration} where the solution from EMTD3 is much closer to the optimum than that from TD3.

\begin{figure} [thpb]
	\centering
	\includegraphics[width=0.8\columnwidth]{learning_curve_medium_action-eps-converted-to}
	\caption{Learning curve with TD3 and EMTD3 for Case II.}
	\label{fig: medium_action}
\end{figure}
\begin{figure} [thpb]
	\centering
	\includegraphics[width=0.8\columnwidth]{final_control_medium_action-eps-converted-to}
	\caption{The PID control performance from TD3 and EMTD3 for Case II.}
	\label{fig: medium_action_cv}
\end{figure}

\section{Conclusion}
\label{sec: conclusion}
We proposed a sample-efficient entropy-maximizing TD3 method with an application to facilitate the automatic PID tuning. For the proposed method, an entropy-maximizing stochastic actor is adopted at the beginning to enable sufficient exploration, followed by a deterministic actor to focus on exploitation. We further developed a framework to formulate the PID tuning as a RL problem where the proposed EMTD3 is used to rapidly discover optimal PID parameters. Simulation case studies are provided to verify the sample efficiency and effectiveness of the proposed EMTD3 method in finding global optimum against the traditional TD3 method. Future work includes improving the algorithms to enable online adaptive PID tuning for nonlinear systems.

\addtolength{\textheight}{-12cm}

\section*{ACKNOWLEDGMENT}
M. A. Chowdhury acknowledges the support of Distinguished Graduate Student Assistantships (DGSA) from the Texas Tech University. Q. Lu acknowledges the new faculty startup funds from the Texas Tech University.

\bibliographystyle{ieeetr}