1. Introduction
As unmanned ground vehicles (UGVs) are increasingly used in complex and open terrains, especially in unstructured terrains (e.g., mines, forests, deserts, etc.), 2D raster map navigation methods commonly used in indoor environments are no longer applicable. In addition, it is challenging to accurately parameterize the obstacle point cloud and predict the motion trajectory during navigation, which involves a large amount of data with a high degree of uncertainty. This leads to difficulties in ensuring safety and speed in practical applications, especially in dynamic obstacle avoidance in unstructured terrain [Reference Mohamed, Yin and Liu1]. Thus, this paper is dedicated to proposing a 3D point cloud dynamic obstacle avoidance strategy for unstructured terrain, which enables UGVs to have all-terrain autonomous navigation capability.
As the manufacturing costs of LiDAR decrease, it is increasingly being researched for its use in obstacle detection and tracking. MacLachlan et al. [Reference MacLachlan and Mertz2] introduced feature-based Kalman filter tracking, Vatavu et al. [Reference Vatavu, Rahm, Govindachar, Krehl, Mantha, Bhavsar, Schier, Peukert and Maile3] proposed a particle filtering (PF)-based obstacle estimation method, and Maurer et al. [Reference Jung and Bae4] expanded upon this with a hierarchical system using adaptive ground evaluation and interactive multi-model extended Kalman filtering (IMM-EKF). Brito et al. [Reference Brito, Floor, Ferranti and Alonso-Mora5] used linear Kalman filtering (KF) for trajectory prediction of a moving obstacle modeled as a minimal Minkowski sum ellipsoid. Jian et al. [Reference Jian, Yan, Lei, Lu, Lan, Wang and Liang6] built on this, combining density-based spatial clustering of applications with noise (DBSCAN) and KF for obstacle trajectory prediction, evaluating prediction results with uncertainty.
In terms of the method to deal with the problem of vehicle obstacle avoidance, He S et al. [Reference He, Dai, Zhao, Zou and Ma7] proposed a prescribed performance control strategy to ensure that the error between vehicles converges faster than the predetermined value, which ensures the anti-collision and connectivity between adjacent vehicles, and also ensures the obstacle avoidance of static obstacles. However, it does not solve the problem of fault-tolerant control of uncertain marine surface vehicle (MSV) with unknown inertia matrix under random disturbance. Compared to conventional dynamic obstacle avoidance methods, such as the artificial potential field [Reference Hogan and Rodriguez8] and the dynamic window approach (DWA) [Reference Ge and Cui9], model predictive control (MPC) has better predictive, constraint handling, and mathematical optimization capabilities. Additionally, it can handle complex obstacle avoidance problems with high nonlinearity and constraints, despite having higher computational complexity, ultimately achieving superior overall performance. It is now widely used in robot operating systems [Reference Lee, Lee, Ahn, Shi and Lim10, Reference Scianca, De Simone, Lanari and Oriolo11]. Zeng et al. [Reference Zeng, Zhang and Sreenath12] developed a safety-centric MPC strategy using a discrete-time control barrier function (CBF) or MPC-CBF, but found it to underperform in dynamic environments. Jian et al. [Reference Jian, Yan, Lei, Lu, Lan, Wang and Liang6] proposed a dynamic control barrier function (D-CBF), integrating barrier uncertainty into MPC optimization, improving system safety and enabling dynamic obstacle avoidance. However, the direct approach of using a constant as an extend class
$K_{\infty }$
function
$\gamma$
of the CBF will affect the flexibility and feasibility of the system during operation.
Addressing identified issues, we introduce an adaptive dynamic control barrier function model predictive control (AD-CBF-MPC) method. This algorithm, depicted in Figure 1, uses blue and red numbers to represent UGV and obstacles, respectively, with positions incrementing over time. AD-CBF adapts feasible domains based on obstacle positions and sizes, reflected in the
$\gamma$
value. We use a 2D Gaussian function, considering the UGV-obstacle distance and obstacle size, to replace the constant CBF’s
$K_{\infty }$
function
$\gamma$
, improving system feasibility during obstacle avoidance. We employ LiDAR for obstacle state information, stabilize the obstacle point cloud using smoothing filtering and hierarchical density-based spatial clustering of applications with noise (HDBSCAN), and propose unscented Kalman filtering (UKF) for tracking and predicting obstacle states. This information is integrated into the MPC optimization solution constraints, leading to a reliable UGV obstacle avoidance system.
Schematic diagram of the AD-CBF-MPC algorithm.
The contributions of this work are summarized as follows:
-
An effective solution is proposed for the state identification and prediction of complex random obstacles. The method innovatively utilizes HDBSCAN to realize real-time parameterization of obstacle states, and then passes into the UKF state prediction algorithm to complete the advanced prediction of obstacle trajectories.
-
In contrast to the CBF-MPC methods in [Reference Jian, Yan, Lei, Lu, Lan, Wang and Liang6] and [Reference Zeng, Zhang and Sreenath12], the system usually has only point-by-point feasibility for a fixed
$\gamma$
. In this paper, we propose an adaptive control barrier function (A-CBF), which combines the correlation of each physical quantity between the unmanned vehicle and the obstacle in obstacle avoidance in the form of a 2D Gaussian function to obtain the formula as the value of
$\gamma$
, to realize the lasting feasibility of the system. -
Combining the first contribution and the second contribution, the current state and the predicted state of the obstacle obtained by the HDBSCAN-UKF algorithm are used as the dynamic obstacle avoidance constraints of the A-CBF method, and the adaptive dynamic control barrier function (AD-CBF) is proposed, which takes into account the uncertainty of the dynamic obstacle and improves the safety of the obstacle avoidance.
2. Preliminaries
This section defines the kinematic model of the UGV and presents the optimal control problem to be solved. Additionally, it rewrites the optimization form of the MPC-CBF to prepare for the AD-CBF-MPC algorithm presentation in Section 3.3.
If MPC is employed for controlling vehicle motion, a vehicle kinematics or dynamics model that is reasonably accurate must be developed. Since the application scenario of this study involves low-speed automated driving, utilizing solely the vehicle kinematics model is adequate. Additionally, the commonly utilized chassis for UGVs are the Ackermann and the four-wheel differential. Previous studies have widely discussed applying MPC on the Ackermann chassis [Reference Yu, Duan, Taheri, Cheng and Qi13]; however, this research specifically focuses on the application of MPC and its optimization algorithms on a four-wheel differential chassis. It should be noted that the results presented in this study are equally applicable to other types of UGV chassis. For the discrete system, the dynamics model of the UGV can be described as
${\boldsymbol{x}}_{t+1}=f({\boldsymbol{x}}_{t},{\boldsymbol{u}}_{t})$
, where
${\boldsymbol{x}}_{t}\in X\subset \mathrm{\mathbb{R}}^{n}$
is the state of the system at time
$t\in \mathbf{\mathbb{Z}}^{+}, {\boldsymbol{u}}_{t}\in U\subset \mathrm{\mathbb{R}}^{m}$
denotes the control input, and to simplify the model, the four-wheel differential chassis is restricted to keeping the same speed of the same side wheels, which is equivalent to a two-wheel differential model. The final kinematic model of the UGV is:
\begin{equation} {\boldsymbol{x}}_{t+1}=f\left({\boldsymbol{x}}_{t},{\boldsymbol{u}}_{t}\right)={\boldsymbol{x}}_{t}+\left[\begin{array}{l@{\quad}l} \cos \theta _{t} & 0\\[5pt] \sin \theta _{t} & 0\\[5pt] 0 & 1 \end{array}\right]{\boldsymbol{u}}_{t}\Delta t \end{equation}
where
${\boldsymbol{x}}_{t}=[x_{t},y_{t},\theta _{t}]^{T}$
contains the position and cornering state of the UGV at moment
$t$
, and
${\boldsymbol{u}}_{t}=[v_{t},\omega _{t}]^{T}$
contains the linear and angular velocity inputs to the system at moment
$t$
.
Thus, the MPC optimal control problem can be expressed as [Reference Rosolia, De Bruyne and Alleyne14, Reference Zhang, Liniger and Borrelli15]:
\begin{equation} J_{t}^{*}\left({\boldsymbol{x}}_{t}\right)=\min _{{\boldsymbol{u}}_{t\colon t+N-1|t}}p\left({\boldsymbol{x}}_{t+N|t}\right)+\sum _{k=0}^{N-1}q\left({\boldsymbol{x}}_{t+k|t},{\boldsymbol{u}}_{t+k|t}\right) \end{equation}
\begin{equation} s.t.\; {\boldsymbol{x}}_{t+k+1|t}=f\left({\boldsymbol{x}}_{t+k|t},{\boldsymbol{u}}_{t+k|t}\right),k=0,\ldots, N-1 \end{equation}
\begin{equation} {\boldsymbol{x}}_{t+k|t}\in X,{\boldsymbol{u}}_{t+k|t}\in U,k=0,\ldots, N-1 \end{equation}
\begin{equation} {\boldsymbol{x}}_{t|t}={\boldsymbol{x}}_{t},{\boldsymbol{x}}_{t+N|t}\in X_{f} \end{equation}
\begin{equation} g\left({\boldsymbol{x}}_{t+k|t}\right)\geq 0,k=0,\ldots, N-1 \end{equation}
here
${\boldsymbol{x}}_{t+k|t}$
denotes the state vector at time step
$t+k$
predicted at time step
$t$
obtained by starting from the current state
${\boldsymbol{x}}_{t}$
(2d), and applying the input sequence
${\boldsymbol{u}}_{t\colon t+N-1|t}$
to the system dynamics (2b),
$q({\boldsymbol{x}}_{t+k|t},{\boldsymbol{u}}_{t+k|t})$
as stage cost,
$p({\boldsymbol{x}}_{t+N|t})$
as terminal cost. (2b) is the system dynamics constraint, (2c) encompasses permissible system and input states, and (2d) outlines initial and terminal states. (2e), the collision avoidance constraint, uses
$N$
as prediction step size and
$g(\!\cdot\!)$
as the Euclidean constraint, abbreviating the method as MPC-DC.
In MPC-DC, Euclidean-based constraints initiate UGV motion regulation near obstacles. To start avoidance from a greater distance, larger obstacle avoidance fields of view are needed in MPC, but this increases optimal solution time. To solve this, CBF constraints have been proposed for MPC, providing a larger, set-invariant obstacle avoidance field and addressing the set-invariant optimal obstacle avoidance issue with an overshooting field of view [Reference Son and Nguyen16, Reference Rosolia and Ames17].
Define the safe set
$C$
of the optimal control as the set of superlevels of a continuously differentiable function
$h\colon X\subset \mathrm{\mathbb{R}}^{n}\rightarrow \mathrm{\mathbb{R}}$
:
\begin{equation} C=\left\{{\boldsymbol{x}}\in X\subset \mathrm{\mathbb{R}}^{n}\colon h\left({\boldsymbol{x}}\right)\geq 0\right\}. \end{equation}
if for all
${\boldsymbol{x}}\in \partial C$
there is
$\partial h/\partial {\boldsymbol{x}}\neq 0$
and for system (1) there exists an extend class of
$K_{\infty }$
functions
$\gamma$
, then
$h$
is a CBF function which
$h$
satisfies:
\begin{equation} \exists\; u\; s.t.\; \dot{h}\left({\boldsymbol{x}},{\boldsymbol{u}}\right)\geq -\gamma \left(h\left({\boldsymbol{x}}\right)\right),\; \gamma \in K_{\infty } \end{equation}
Generalization to discrete-time domain:
\begin{equation} \Delta h\left({\boldsymbol{x}}_{k},{\boldsymbol{u}}_{k}\right)\geq -\gamma \left(h\left({\boldsymbol{x}}_{k}\right)\right),\; \gamma \in K_{\infty }. \end{equation}
where
$\Delta h({\boldsymbol{x}}_{k},{\boldsymbol{u}}_{k})=h({\boldsymbol{x}}_{k+1})-h({\boldsymbol{x}}_{k})$
. Here instead of defining
$\gamma$
as a scalar, as in the literature [Reference Jian, Yan, Lei, Lu, Lan, Wang and Liang6] and [Reference Zeng, Zhang and Sreenath12], it is defined as a
$K_{\infty }$
function. For the discrete-time domain, it can be assumed that
$0\lt \gamma (h({\boldsymbol{x}}_{k}))\leq h({\boldsymbol{x}}_{k})\leq 1$
is satisfied for any
$h({\boldsymbol{x}}_{k})$
.
Thus, for the definition of the MPC-CBF optimal control problem the collision avoidance control constraint (2e) in MPC-DC can be replaced by:
\begin{equation} \Delta h\left({\boldsymbol{x}}_{k},{\boldsymbol{u}}_{k}\right)\geq -\gamma \left(h\left({\boldsymbol{x}}_{k}\right)\right),k=0,\ldots, N-1. \end{equation}
3. Implementation
Existing obstacle trajectory prediction algorithms work best in structured settings with simple kinematic models. We propose a novel method for complex, unstructured environments, such as forests or mines. Knowledge of obstacle trajectories can boost UGV obstacle avoidance reaction speed, improving safety. Our proposed HDBSCAN-UKF trajectory prediction method addresses these needs.
Traditional MPC algorithms with Euclidean constraints are slow and ineffective for dynamic obstacles, demanding more adaptable avoidance constraints. We initially merge MPC with CBF for improved static and partial dynamic obstacle avoidance. Then, we introduce and incorporate the AD-CBF into the MPC, enabling rapid and stable dynamic obstacle avoidance.
3.1. HDBSCAN obstacle recognition
HDBSCAN, a refinement of DBSCAN, creates a hierarchical structure instead of choosing clusters based on a global core point radius
$Eps$
threshold. The minimal cluster size is determined by integrating all possible
$Eps$
values with the minimum number of covered points
$\textit{MinPts}$
, reducing the impact of clustering parameters on DBSCAN’s clustering accuracy [Reference Dhanushree, Priya, Aruna and Bhavani18]. Unlike DBSCAN, HDBSCAN does not identify each trajectory point but selects some core points for cluster generation. This decreases the number of queries and eases data analysis and processing.
HDBSCAN defines the core distance (
$d_{\textit{mreach}-k}$
) as the distance between an object and its
$\textit{MinPts}$
closest neighbors. A hierarchy can be constructed based on two objects using a distance of:
\begin{equation} d_{\textit{mreach}-k}\left(a,b\right)=\max \left\{core_{k}\left(a\right),core_{k}\left(b\right),d\left(a,b\right)\right\} \end{equation}
where
$d(a,b)$
denotes the Euclidean distance according to the chosen measure.
$core_{k}(a)$
denotes the Euclidean distance from point
$a$
to the kth core point [Reference Neto, Sander, Campello and Nascimento19].
HDBSCAN provides numerous benefits. It retains DBSCAN’s advantages and is better suited for high-dimensional, complex datasets. HDBSCAN requires only the minimum cluster size, eliminating the need for manual parameter setting as it automatically uncovers clustering clusters and adapts to the dataset. This makes HDBSCAN useful for segmenting nearby obstacles and a sensible choice for recognizing obstacles with significant length-width ratio differences.
Performing the HDBSCAN clustering process on the set of obstacles point cloud (
$C=\{X,Y,Z\}$
) input from LiDAR generates a set (
$O_{obs}^{c}=\{O_{1}^{c},O_{2}^{c},\ldots, O_{n}^{c}\}$
) of current states of n clusters of obstacles, each including the position and radius of the obstacle.
A comparison of the two algorithms‘ recognition effects is shown in Figure 2, where the green cylinder represents the obstacle’s position and size as determined by each algorithm. Figure 2a shows that DBSCAN struggles with irregular obstacles, like complex stacking, leading to over-inflation of the green cylinder and occupying the feasible region. Conversely, the HDBSCAN results, depicted in Figure 2b, demonstrate clear advantages in reasonably segmenting and recognizing small-interval and complex-shaped obstacles, making it suitable for outdoor unstructured spaces.
(a) Shows the result of obstacle recognition using the DBSCAN algorithm, while (b) Displays the result using the HDBSCAN algorithm.
3.2. UKF obstacle trajectory prediction
The Kalman filter (KF) is widely used for system state estimation and prediction but works best with Linear Gaussian (LG) systems. The extended Kalman filter (EKF) is useful when dealing with non-linear mappings but can increase system error and complexity. The unscented Kalman filter (UKF), used for state estimation in non-linear LiDAR observing systems, outperforms in handling non-linear issues, offering superior probability distribution approximation.
The measurement model of LiDAR in the Cartesian coordinate system can be nonlinear, with a system state vector of
$X=[x,y,z]^{T}$
representing the position of the measurement target in Cartesian coordinates. The measurement vector, denoted as
$Z=[r,\theta, \varphi ]^{T}$
, consists of three components: the distance
$r$
, horizontal angle
$\theta$
, and vertical angle
$\varphi$
, all of which are measured by the LiDAR. The measurement model can be expressed as follows:
\begin{equation} Z=\left[\begin{array}{l} r\\[5pt] \theta \\[5pt] \varphi \end{array}\right]=H\left(X\right)=\left[\begin{array}{l} \sqrt{x^{2}+y^{2}+z^{2}}\\[5pt] \arctan \left(\dfrac{y}{x}\right)\\[10pt] \arcsin \left(\dfrac{z}{\sqrt{x^{2}+y^{2}+z^{2}}}\right) \end{array}\right] \end{equation}
let
$H(X)$
be the nonlinear mapping from the state vector
$X$
to the observation vector
$Z$
.
The nonlinear system described above can be solved by UKF using the unscented transform (UT), with the mean and variance being approximated through sampling and weight assignment according to a specific regularity. The procedure for applying UT to this problem is described as follows [Reference Wan and Van Der Merwe20]:
In the discrete-time domain, assuming that the prediction model is a variable acceleration period motion model
$X_{k+1|k}=F(X_{k|k})$
and the LiDAR nonlinear observation model is
$Z_{k+1|k}=H(X_{k+1|k})$
, and knowing that the state mean value at moment
$k$
is
$\overline{X}_{k|k}$
, the observation value is
$z_{k}$
, and the variance is
$P_{k|k}$
, the statistical properties of the system can be obtained by UT constructing the
$2n+1 \textit{Sigma}$
-points
$\chi _{k+1|k}^{i}$
, and at the same time, constructing the corresponding weights
$W_{k+1|k}^{i}$
, which are
$\chi _{k+1|k}^{i}$
’s corresponding weights [Reference Yu, Duan, Taheri, Cheng and Qi21]:
\begin{equation} \chi _{k|k}^{0}=\overline{X}_{k|k} \end{equation}
\begin{equation} \chi _{k|k}^{i}=\overline{X}_{k|k}+\left(\sqrt{\left(n+\lambda \right)P_{k|k}}\right)_{i} i=1,\ldots, n \end{equation}
\begin{equation} \chi _{k|k}^{i}=\overline{X}_{k|k}-\left(\sqrt{\left(n+\lambda \right)P_{k|k}}\right)_{i-n} i=n+1,\ldots, 2n \end{equation}
\begin{equation} W_{m}^{0}=\frac{\lambda }{n+\lambda } \end{equation}
\begin{equation} W_{c}^{0}=W_{m}^{0}+\left(1-\alpha ^{2}+\beta \right) \end{equation}
\begin{equation} W_{m}^{i}=W_{c}^{i}=\frac{1}{2\left(n+\lambda \right)}i=1,\ldots, 2n \end{equation}
where
$\lambda =\alpha ^{2}(n+\kappa )-n$
is the scale factor.
$(P^{1/2})^{T}(P^{1/2})=P, (P^{1/2})_{i}$
denote the ith column of the square root of the matrix.
The distribution state of the
$\textit{Sigma}$
-points around
$\overline{X}$
is determined by
$\alpha$
. Adjusting
$\alpha$
reduces the effect of higher order terms on the system, and is usually set to a small positive number
$0\leq \alpha \leq 1$
, here
$\alpha =0.001$
. There is no specific restriction on the value of
$\kappa$
, but at least ensure that the matrix
$(n+\lambda )P$
is a semipositive definite matrix, and is usually set to
$\kappa =3-n$
, and
$\kappa$
is a non-negative number, here
$\kappa =0$
.The state distribution parameter
$\beta \geq 0$
, by setting
$\beta$
, the accuracy of the variance can be improved, and for Gaussian distribution,
$\beta =2$
is optimal [Reference Gao, Gao, Zhong, Hu and Gu22].
For the system prediction model, the predicted state value
$X_{k+1|k}$
and variance
$P_{k+1|k}$
can be obtained through the UT prediction process:
\begin{equation} X_{k+1|k}=\sum _{i=0}^{2n}W_{m}^{i}\chi _{k+1|k}^{i}, \chi _{k+1|k}^{i}=F\left(\chi _{k|k}^{i}\right)\quad i=0,\ldots, 2n \end{equation}
\begin{equation} P_{k+1|k}=\sum _{i=0}^{2n}W_{c}^{i}\left(\chi _{k+1|k}^{i}-X_{k+1|k}\right)\left(\chi _{k+1|k}^{i}-X_{k+1|k}\right)^{T}+Q_{k} \end{equation}
The implementation of the HDBSCAN-UKF obstacle trajectory prediction algorithm is shown in Alg. 1. Assuming the sampling time
$\Delta t s$
, the obstacle trajectory prediction for
$\Delta t\cdot N s$
can be realized by Alg. 1, which leads to the set of future states
$O_{obs}^{p}=\{O_{1}^{p},O_{2}^{p},\ldots, O_{n}^{p}\}$
of
$n$
clusters of obstacles.
HDBSCAN-UKF Trajectory Prediction.
For example, a variable acceleration periodic motion of an obstacle position over time satisfies:
\begin{equation} \left\{\begin{array}{l} x=x_{0}-A\sin \left(2\pi \omega t\right)\\[5pt] y=y_{0}-A\cos \left(2\pi \omega t\right) \end{array}\right. \end{equation}
Given the initial obstacle state
$(x_{0},y_{0},r_{0})=(8.5,2.0,0.48)$
, amplitude
$A=2.5m$
, frequency
$\omega =0.015$
, sampling time
$\Delta t=0.5\,s$
, and prediction step size
$N=30$
, the algorithm can predict the obstacle’s motion 3 s ahead. Figure 3a and Figure 3b depicts the actual path of the obstacle along the x- and y-axes, as well as the tracking and predicted trajectories generated by the HDBSCAN and UKF algorithms, respectively.
(a) and (b) Show the actual, tracked, and predicted obstacle trajectories in the x and y directions.
In addition, the root-mean-square error (RMSE, Eq. 12) can be used to judge the effectiveness of the trajectory tracking of the HDBSCAN algorithm, as well as the effectiveness of the trajectory prediction of the UKF algorithm. The calculation results are shown in Table I.
Where
$x_{i}, x_{i}^{tra}$
, and
$x_{i}^{pre}$
are the actual trajectory, tracking trajectory, and predicted trajectory of the obstacle in the x-axis direction at the ith step, respectively, and
$n$
is the total number of samples, and the same for the y-axis.
\begin{equation} \left\{\begin{array}{l} RMSE_{x}^{tra}=\sqrt{\sum\limits_{i=0}^{n-1}\left(x_{i}-x_{i}^{tra}\right)/n}\\[8pt] RMSE_{x}^{pre}=\sqrt{\sum\limits_{i=0}^{n-1}\left(x_{i}-x_{i}^{pre}\right)/n} \end{array}\right. \end{equation}
3.3. AD-CBF-MPC control strategy
Section 2 explains the basic principle and implementation process of the MPC-CBF algorithm. While the algorithm is effective in avoiding static obstacles, it struggles to avoid dynamic obstacles that change their trajectories frequently.
In order to make up for the shortcomings of this algorithm, this section firstly proposes to fuse the current state set
$O_{obs}^{c}$
of the obstacle obtained in Section 3.1, with the future state set
$O_{obs}^{p}$
of the obstacle obtained in Section 3.2, and the resulting
$O_{obs}=\{O_{obs}^{c},O_{obs}^{p}\}$
is used as the avoidance target of the MPC-CBF algorithm to improve the dynamic performance of the algorithm, and to realize the dynamic control barrier function (D-CBF). Where each element of
$O_{obs}$
is of the form
$O_{i}=[x_{obs}^{i},y_{obs}^{i},r_{obs}^{i}](i=1,\ldots, 2n)$
, the current and future states of the obstacle at discrete time
$k$
can be represented by
$O_{i}(k)=[x_{obs}^{i}(k),y_{obs}^{i}(k),r_{obs}^{i}(k)]$
and the center position of the UGV is represented by
$p(k)=[x(k),y(k)]$
.
Then the functional form of D-CBF can be chosen as:
\begin{equation} h\left(X_{k}^{i}\right)=\left(x\left(k\right)-x_{obs}^{i}\left(k\right)\right)^{2}+\left(y\left(k\right)-y_{obs}^{i}\left(k\right)\right)^{2}-\left(r_{c}+r_{obs}^{i}\left(k\right)+r_{s}\right)^{2} \end{equation}
\begin{equation} \Delta h\left(X_{k}^{i},u_{k}\right)\geq -\gamma \left(h\left(X_{k}^{i}\right)\right), k=0,\ldots, M_{CBF}-1;\quad i=1,\ldots, 2n \end{equation}
where the admissible state is
$X_{k}^{i}=\{p(k),O_{i}(k)\}, u_{k}$
is the optimal input to the MPC solution,
$r_{c}$
and
$r_{s}$
are the UGV body radius and safety distance, respectively,
$M_{CBF}$
is the CBF constraint step, and
$n$
is the number of obstacles.
Error analysis table for HDBSCAN-UKF prediction and tracking.
The flexible selection of
$\gamma (h(\!\cdot\!))$
is crucial for the CBF planning process. It usually takes a smaller value (
$0\lt \gamma \leq 1$
), indicating the control performance trade-off for system stability. Higher
$\gamma$
values increase stability but may decrease control performance [Reference Zeng, Li and Sreenath23]. Therefore, it is important to choose the value of
$\gamma$
in a rational and dynamic manner to balance stability and control performance under different obstacles.
When considering the actual situation of UGV obstacle avoidance, the selection of
$\gamma$
is influenced by two aspects:
-
The distance,
$d^{i}(k)$
, between the position of the UGV at moment
$k$
and the center position of the ith obstacle. As
$d^{i}(k)$
decreases,
$\gamma (h(\!\cdot\!))$
also decreases and this relationship is positively correlated. -
The size of the radius of the obstacle,
$r_{obs}^{i}(k)$
. As
$r_{obs}^{i}(k)$
becomes smaller,
$\gamma (h(\!\cdot\!))$
increases, and this relationship is negatively correlated.
According to Eq. (13a),
$h(X_{k}^{i})$
already includes both
$d^{i}(k)$
and
$r_{obs}^{i}(k)$
attributes. The final adaptive functional form of
$\gamma (h(\!\cdot\!))$
can be written as:
\begin{align} \gamma \left(h\left(X_{k}^{i}\right)\right)&=A\left(1+\dfrac{erf\left(d^{i}\left(k\right)-d_{\mathrm{E }}\right)}{\sqrt{2}\sigma _{d}}\right)\left(1-\dfrac{erf\left(r_{obs}^{i}\left(k\right)-r_{\mathrm{E }}\right)}{\sqrt{2}\sigma _{r}}\right)\nonumber \\[5pt] & \quad k=0,\ldots, M_{CBF}-1;\quad i=1,\ldots, 2n \end{align}
The equation defines
$A$
as the amplitude, with
$d_{\mathrm{E }}$
and
$r_{\mathrm{E }}$
representing the expected means of
$d$
and
$r$
, respectively.
$\sigma _{d}$
and
$\sigma _{r}$
indicate the standard deviation of
$d$
and
$r$
, respectively, and
$erf(\!\cdot\!)$
is the error function. Given the environmental conditions, obstacle features, and dimensions of UGVs, the parameter
$[A,d_{\mathrm{E }},r_{\mathrm{E }},\sigma _{d},\sigma _{r}]=[0.5,2.0,0.4,0.6,0.2]$
is chosen.
The UGV was kept stationary, and obstacle 1 with a radius of 0.4 m was set to approach from 4.0 m away from the UGV, and obstacle 2 with a radius of 0.6 m was set to move away from 1.0 m away from the UGV. Figure 4 depicts the
$\gamma (h)$
value distribution for this case. Figure 4a shows the total
$\gamma (h)$
distribution for all revealed obstacle motion features, and Figure 4b shows the distribution of
$\gamma (h)$
in the 50 results solved for two obstacles with different states from the beginning of AD-CBF planning. Eq. (14) allows
$\gamma (h)$
to autonomously select a suitable value based on the obstacle and UGV’s relative state, ensuring a balance between system safety and feasibility.
(a) Is a plot of the total distribution of
$\gamma (h)$
values for the test case, and (b) Shows the distribution of
$\gamma (h)$
values corresponding to the 50 steps of obstacle states backward from the start of AD-CBF planning.
Ultimately, the associative formulations (2), (13), and (14), such that
$M_{CBF}=N$
, can be obtained as the control strategy of AD-CBF-MPC:
\begin{equation} J_{t}^{*}\left(x_{t}\right)=\min _{u_{t\colon t+N-1|t}}p\left(x_{t+N|t}\right)+\sum _{k=0}^{N-1}q\left(x_{t+k|t},u_{t+k|t}\right) \end{equation}
\begin{equation} s.t.\; x_{t+k+1|t}=f\left(x_{t+k|t},u_{t+k|t}\right), k=0,\ldots, N-1 \end{equation}
\begin{equation} x_{t+k|t}\in X, u_{t+k|t}\in U, k=0,\ldots, N-1 \end{equation}
\begin{equation} x_{t|t}=x_{t}, x_{t+N|t}\in X_{f} \end{equation}
\begin{equation} X_{t+k|t}^{i}=\left\{x_{t+k|t},O_{i}\left(k\right)\right\}, k=0,\ldots, N-1;\quad i=1,\ldots, 2n \end{equation}
\begin{equation} h\left(X_{t+k|t}^{i}\right)=\left(x\left(k\right)-x_{obs}^{i}\left(k\right)\right)^{2}+\left(y\left(k\right)-y_{obs}^{i}\left(k\right)\right)^{2}-\left(r_{c}+r_{obs}^{i}\left(k\right)+r_{s}\right)^{2} \end{equation}
\begin{equation} \gamma \left(h\left(X_{t+k|t}^{i}\right)\right)=A\left(1+\frac{erf\left(d^{i}\left(t+k\right)-d_{\mathrm{E }}\right)}{\sqrt{2}\sigma _{d}}\right)\left(1-\frac{erf\left(r_{obs}^{i}\left(t+k\right)-r_{\mathrm{E }}\right)}{\sqrt{2}\sigma _{r}}\right) \end{equation}
\begin{equation} \Delta h\left(X_{t+k|t}^{i},u_{t+k|t}\right)\geq -\gamma \left(h\left(X_{t+k|t}^{i}\right)\right), k=0,\ldots, N-1;\quad i=1,\ldots, 2n \end{equation}
4. Experiments
In the experiments, several high-performance methods are combined to create a safe and reliable UGV navigation framework (Figure 5) for collision-free navigation. Through the data fusion between LiDAR, IMU, and wheel odometer, the environment is 3D mapped and saved as a Point Cloud Data (PCD) using Fast-lio-lc [Reference Xu and Zhang24, Reference Xu, Cai, He, Lin and Zhang25], and handed over to the Removert [Reference Kim and Kim26] algorithm for the removal of dynamic trajectories, and finally a static PCD point cloud is obtained; subsequently, the Amcl-3 dl [Reference Zhao, Fritsche, Yin, Gunnarsson and Gustafsson27] algorithm performs the position by calling the point cloud map data and comparing it with the current surrounding point cloud of the UGV for position estimation; after that, the estimated position is used as the reference position of the UGV for the PF-RRT* algorithm [Reference Fankhauser, Bloesch and Hutter28–Reference Jian, Lu, Zhou, Lan, Xiao, Wang and Liang30], which is given as the target point of the UGV on the point cloud a priori map, and global path planning is carried out to obtain the reference path of the UGV to reach the target point.
Framework for UGV navigation systems.
In addition, the improved HDBSCAN-UKF algorithm [Reference Neto, Sander, Campello and Nascimento19, Reference Gao, Gao, Zhong, Hu and Gu22] is used to parameterize the obstacles around the unmanned vehicle and predict its future trajectory, which is passed to the AD-CBF-MPC [Reference Jian, Yan, Lei, Lu, Lan, Wang and Liang6, Reference Zeng, Zhang and Sreenath12] local planning algorithm for obstacle avoidance, while the MPC algorithm obtains the reference path of global path planning in real time for following until it finally arrives at the target point safely.
(a) Shows the Gazebo dynamic obstacle avoidance simulation environment. (b) Shows the trajectory comparison of multiple obstacle avoidance algorithms in the simulation environment.
4.1. Simulation scenarios
In order to evaluate the performance of the proposed method, a simulation environment with complex terrain and dynamic obstacles is built in Gazebo as shown in Figure 6a. The blue solid arrows show the global reference path (35.869 m), red dashed arrows indicate obstacle movement, and yellow dashed boxes mark static obstacles. The terrain is a bumpy mud slab. A simulated UGV, same size as the actual vehicle, is used. The AD-CBF-MPC obstacle avoidance navigation algorithm is compared with four other benchmark methods in this environment:
-
MPC-DC: Model predictive controllers with Euclidean safety distance constraints.
-
MPC-CBF: Model predictive controller with safety prioritized control barrier function, where
$\gamma =0.1$
. -
D-CBF-MPC: Safety-first obstacle avoidance model predictive controller with HDBSCAN-UKF obstacle trajectory prediction, where
$\gamma =0.1$
. -
A-CBF-MPC: Model predictive controller with A-CBF.
The performance of five algorithms was compared using the following metrics: Nav-time: Time for UGV to travel from start to end. Min-dist: Minimum distance between UGV and obstacle during navigation. Ref-time: Time from obstacle detection to initiation of avoidance behavior. Vel-var: Velocity variance when avoiding obstacles. Path-len: Total distance traveled by the UGV from start to end.
These performance metrics are presented in Table II.
Each algorithm’s driving trajectories are shown in different colors in Figure 6b.
-
MPC-DC (red) does not have advance avoidance constraints or obstacle prediction and therefore is not able to perform safe avoidance behaviors for dynamic obstacles and collides with the first dynamic obstacle.
-
MPC-CBF (blue) does not consider the influence of future trajectories of obstacles and CBF is only point-by-point feasible, which leads to the algorithm’s difficulty in avoiding dynamic obstacles, small safety distances, large speed variations, and steep driving trajectories.
-
D-CBF-MPC (green) takes into account the future trajectory of the obstacle and is able to avoid dynamic obstacles safely and in advance, and has the shortest navigation time compared to the other four algorithms, but does not have the global persistent feasibility of the CBF, which leads to the planning of the obstacle avoidance paths is not optimal, and the final length of the traveled path is on the large side.
-
A-CBF-MPC (black) is the opposite case of the D-CBF-MPC method, where the availability of the global persistent feasibility of CBF leads to an improvement in navigation time and velocity change for obstacle avoidance (with respect to MPC-CBF), but the absence of future position constraints on the trajectory of the dynamic obstacle leads to algorithmic results in terms of minimum safe distances, velocity changes, and path lengths that do not reach the ideal case.
-
The method proposed in this paper, AD-CBF-MPC (purple), possesses both the global persistent feasibility of CBF and the future position constraints of dynamic obstacle trajectories, which makes our method reach the optimal case compared with other methods, except for the navigation time due to the adaptive solution, which is slightly higher than that of the D-CBF-MPC method, which makes the unmanned vehicle navigate the process kind of the unmanned vehicle, which can safely and quickly arrive at the target point, and keep a safer and minimum distance with dynamic obstacles on the way, and also ensure the unmanned vehicle’s speed to smoothly change, and generate the traveling trajectory with the shortest distance.
4.2. Real-world scenarios
Real scenario experiments are performed on the UGV platform depicted in Figure 7. The algorithms will operate under the ROS Noetic operating system. The LiDAR’s scanning frequency is 10 Hz with a planning frequency of 30 Hz. Additionally, an industrial computer with 8-core Arm 64-bit 2.2 GHz CPU, 32 GB video memory, and 64 GB storage is employed as the algorithm solver and simulation operating system.
Comparison of AD-CBF-MPC with other baseline methods.
Schematic of UGV platform hardware.
Dynamic obstacle avoidance experiments in real environments.
A forest with uneven terrain was used for real-world navigation and obstacle avoidance testing. Figure 8 shows pedestrians randomly obstructing the UGV’s path. The UGV’s local perception range is set to
$8\times 8\,m$
with a 0.1m map resolution. HDBSCAN-UKF and AD-CBF-MPC both have a predicted step size of 30. The UGV has a body radius of
$r_{c}=0.3\,m$
and a safety distance of
$r_{d}=0.2\,m$
.
Figure 8 illustrates the real-time obstacle avoidance process. The AD-CBF-MPC algorithm produces a local obstacle avoidance path (green line), and the PF-RRT* algorithm generates a global reference trajectory (blue line). Green and blue cylinders represent the current and predicted obstacle positions, respectively. Red and yellow numbers correspond to the obstacle and UGV states at discrete moments.
Figure 8a displays the algorithm’s real-time solutions at discrete moments, while Figure 8b shows state changes during navigation. The algorithm effectively avoids obstacles in complex outdoor environments. Upon detecting an obstacle, its state is parameterized by HDBSCAN (green column) and its future state is predicted by UKF (blue column). These states form the AD-CBF-MPC’s obstacle avoidance constraints, generating a safe, smooth local path.
As shown in Figure 8b, the UGV avoids obstacles by moving in the opposite direction, with the trajectory prediction and A-CBF constraints greatly reducing the cost of optimal solution, enhancing safety and providing smoother, faster trajectories.
5. Conclusion and future work
The CBF-MPC and D-CBF-MPC algorithms are enhanced to handle complex terrain navigation and dynamic obstacle avoidance. Obstacle states are parameterized by HDBSCAN and their trajectories are predicted using the UKF algorithm for real-time tracking and position prediction. These positions are integrated into CBF constraints to form the D-CBF. An adaptive version, AD-CBF, is proposed to solve the point-by-point feasibility issue in classical CBF, ensuring global persistent feasibility.
The proposed algorithm’s effectiveness is demonstrated in both simulated and real environments. It outperforms others in all performance metrics, especially compared to the D-CBF-MPC algorithm. The improved AD-CBF-MPC algorithm increases the minimum safe distance by 13% and reduces speed variations by 41%, enhancing safety, speed, and smoothness of UGV navigation.
Because AD-CBF algorithm needs to adaptively calculate the control performance parameter
$\gamma$
of CBF before obstacle avoidance planning, this method has limitations in real-time performance and is only suitable for low-speed motion models. In the future, it is planned to develop more reasonable, effective, and robust obstacle avoidance strategies by further improving the adaptive functional form of CBF and taking into account end-to-end methods such as deep learning.
Author contributions
All authors contributed to the study’s conception and design. Material preparation, data collection, and analysis were performed by Liang Guo and Suyu Zhang. The first draft of the manuscript was written by Liang Guo and all authors commented on previous versions of the manuscript. All authors read and approved the final manuscript.
Competing interests
The authors declare no competing interests exist.
Ethical approval
None.
