High-Order Control Lyapunov–Barrier Functions for Real-Time Optimal Control of Constrained Non-Affine Systems

Abstract

This paper presents a synthesis of higher-order control Lyapunov functions (HOCLFs) and higher-order control barrier functions (HOCBFs) capable of controlling nonlinear dynamic systems while maintaining safety. Building on previous Lyapunov and barrier formulations, we first investigate the feasibility of the Lyapunov and barrier function approach in controlling a non-affine dynamic system under certain convexity conditions. Then we propose an HOCLF form that ensures convergence of non-convex dynamics with convex control inputs to target states. We combine the HOCLF with the HOCBF to ensure forward invariance of admissible sets and guarantee safety. This online non-convex optimal control problem is then formulated as a convex Quadratic Program (QP) that can be efficiently solved on board for real-time applications. Lastly, we determine the HOCLBF coefficients using a heuristic approach where the parameters are tuned and automatically decided to ensure the feasibility of the QPs, an inherent major limitation of high-order CBFs. The efficacy of the suggested algorithm is demonstrated on the real-time six-degree-of-freedom powered descent optimal control problem, where simulation results were run efficiently on a standard laptop.

1. Introduction

Since the mid-1950s, fast technological advancements have been significantly influenced by the demands of the automotive, robotics, and aerospace industries for precision, reliability, and robustness. With this advancement, modern control theory emerged with multiple techniques to handle complex uncertain systems, leveraging digital computers to perform complex calculations. Unlike classical control theory, which relies heavily on model simplifications and frequency domain techniques, modern control theory focuses on using state-space methods and various estimation techniques like optimal and adaptive control to control complex dynamic systems. Modern techniques were enhanced to be more robust and reliable over time, such as nonlinear model predictive control or model-free adaptive control [1,2,3,4]. Nevertheless, these techniques still have some shortcomings such as model dependency, computational complexity, and implementation challenges.

Researchers have since worked on new forms of model predictive control which leverage the use of a sequence of techniques like linearization and discretization to generate an optimal control problem that can be iteratively solved like model predictive control. These techniques offer excellent reliability and robustness but still suffer from computational cost and complexity in real-time implementation. The work in [5,6,7] synthesizes the second-order cone program (SOCP), conditionally enforced constraints, and linearization and discretization procedures to transform highly complex nonlinear systems with non-convex state and control into a convexified problem and demonstrates its performance on the 6DoF powered descent guidance problem as a feed-forward trajectory generation problem.

Although successive convexification (SCvx) can provide a real-time control solution for complex systems, it has some significant drawbacks. First, linearization and discretization in a real-time implementation abstract the higher-order features of the problem, rendering the convex model inadequate outside the nominal states. This abstraction possibly raises convergence issues, lowers the confidence in maintaining safety constraints, or even leads to unstable control behavior. Second, SCvx can encounter infeasibility and thus requires good initialization, which can be challenging to acquire for some nominal states, especially for systems with fast dynamics, making SCvx vulnerable to disturbances and model uncertainties. Ultimately, as SCvx requires iteratively solving a sequence of convex optimization problems at each step, real-time implementation can impose a significant computational burden when handling complex problems with large state and control spaces, which may lead to delays in decision-making.

In recent years, a new theory emerged that shows the capability of controlling nonlinear systems while ensuring safety despite non-convex constraints. Based on the work in [8,9,10], which lays the mathematical foundation for Lyapunov functions and their application in null-asymptotic controllability, researchers have synthesized a control Lyapunov function and control barrier function approach to design safe, deployable controllers. The work in [11,12] uses barrier functions as a novel methodology for specifying and strictly enforcing safety conditions. When unified with control Lyapunov functions in the context of a quadratic program, CLFs and CBFs provide real-time optimization-based controllers capable of controlling and ensuring the safety of nonlinear dynamics and non-convex safety constraints in conjunction with performance objectives. This approach proved effective in numerous applications, including automotive control problems like adaptive cruise control and lane-keeping. This approach was extended for higher-order systems [13] and infers asymptotic stability by examining the second- and third-order derivatives of the Lyapunov functions and argues the crucial role that higher-order derivatives play in establishing analytical stability. Recently, this result has been adapted and generalized to higher-order derivatives and time-varying systems by parameterizing Lyapunov functions and imposing some conditions on their higher-order derivatives [14], while, for safety, the authors of [15,16] proposed a sequence of time-varying barrier functions to ensure the intersection of all subsets of the safety constraint remains forward invariant. This approach extends the concept of Lyapunov and barrier functions to higher-order control Lyapunov barrier functions (HOCLBFs), designed specifically for safely constraining higher-order systems subject to high-relative-degree constraints.

Adopting the control Lyapunov and barrier functions approach proves advantageous for designing online real-time controllers that can enforce safety constraints and manage non-convex constraints and dynamics, as the control problem reduces solving a straightforward, computationally efficient quadratic program at each time step. Nonetheless, the existing literature requires a control affine system for using CLFs and CBFs. Additionally, the unification of these control functions suffers several limitations when subject to different constraints, which renders the quadratic program infeasible or inaccurate or results in divergent behaviors. Finally, this approach has been adopted for simple low-dimensional problems such as 2D robots or adaptive cruise control and has yet to prove its efficacy in complex systems such as aerospace applications with highly nonlinear dynamics and uncertain environments that demand real-time, adaptive, and reliable control systems.

1.1. Motivation and Contributions

Our motivation in this work is to attract more research on the foundation of the Lyapunov and barrier functions and their feasibility in controlling complex systems, as generalizing the approach to a broader scope of problems such as non-affine systems can reveal more insights into the theory and design of robust controllers in addition to the broader scope of problems that can be tackled using this approach. In this work, we aim to enhance the current advances and overcome the drawbacks of this method. Finally, we aim to demonstrate the feasibility of this approach in controlling a highly complex control problem. In this paper, we start initially by investigating the applicability of CLFs and CBFs to non-affine systems and then contemplate the current forms of CLFs and CBFs and enhance the formulation to require less tunable parameters. Finally, we apply this approach to the 6DoF powered descent problem to showcase the ability of CLFs and CBFs to control high-dimensional complex systems. Our contribution can be summarized in the following:

1.

We establish sufficient conditions for generalizing CLFs and CBFs to non-affine control systems.

2.

We propose a computationally efficient higher-order CLF formulation and address the different infeasibilities that arise from unifying high-order CLFs and CBFs due to the large number of tunable parameters using fewer parameters and a heuristic approach to tune them.

3.

We illustrate the efficacy of our approach by devising an online real-time optimal controller for six-degree-of-freedom powered descent with non-affine inputs, considering a range of non-convex safety constraints.

This work puts an effort toward the unification of the CLF and CBF theory and achieving a concise robust form, inviting more future research in developing robust controllers and coupling these controllers with machine learning algorithms. Figure 1 illustrates the HOCLBF approach in the general context of optimal control theory.

1.2. Organization

The rest of the paper is organized as follows. Section 2 reviews the mathematical background for control Lyapunov and barrier functions. In Section 3, we expand the scope of the CLFs and CBFs beyond control affine systems, suggest a new high-order CLF formulation, synthesize the CLF-CBF approach, and address the potential infeasibility sources using both heuristic and classical approaches. In Section 4, we demonstrate the efficiency of the HOCLBF approach in controlling 6-DOF powered descent, along with numerical simulation results presented in Section 4.1.5. Conclusions and possible future directions of this work are detailed in Section 5.

2. Preliminaries

This section briefly reviews some fundamental definitions of stabilizing control Lyapunov and barrier functions, while we refer the reader to [8,9,10,11,12,13,14,15,16] for a deeper understanding of these concepts. Consider the nonlinear control-affine system:

$$\dot{x}\left(t\right)=f\left(x\left(t\right)\right)+g\left(x\left(t\right)\right)u\left(t\right)$$

(1)

where f and g are globally Lipschitz, $x\in {\mathbb{R}}^n$ and $u\in {\mathbb{R}}^m$ are the states and control inputs, respectively, constrained in closed sets, with initial condition $x\left(t_0\right)=x_0$.

Definition 1

([10,11,12]). A continuous differentiable function $V:{\mathbb{R}}^n\to \mathbb{R}+$ is an exponentially stabilizing control Lyapunov function if it is positive, proper, infinitesimally decreasing, and there exists $c_1,c_2,c_3>0$, such that for any solution $x\left(t\right)$:

$$\begin{array}{c}{c}_1{∥x_1∥}^2⩽V\left(x\right)⩽c_2{\parallel x\parallel }^2\\ {\displaystyle \underset{u\in U}{inf}}[L_{f}V\left(x\right)+L_{g}V\left(x\right)u+c_{3}V\left(x\right)]⩽0\end{array}$$

(2)

The Lyapunov function yields a feedback controller that exponentially stabilizes and drives the system to the desired output/zero dynamics, with $c_3$ representing the rate of convergence of the solution:

$$\parallel x\left(t\right)\parallel ⩽\sqrt{{\displaystyle \frac{c_2}{c_1}}}{e}^{-{\displaystyle \frac{c_{3}t}{2}}}\parallel x\left(0\right)\parallel ,$$

(3)

and this controller is equivalent to a min-norm controller that can be solved via a quadratic program, which proved efficient in experimental application [17,18].

$$\begin{array}{cc}\hfill & \underset{u\in U}{min}{u}^{⊺}u\hfill \\ \hfill & \mathrm{s}.\mathrm{t}.L_{f}V\left(x\right)+L_{g}V\left(x\right)u+c_{3}V\left(x\right)⩽0\hfill \end{array}$$

(4)

Definition 2

([19]). The relative degree of a differentiable function refers to the number of differentiations required to obtain the control input u explicitly shown in the control function.

Since most systems are not first-order in control inputs, we need to ensure stability using the higher-order derivatives of the Lyapunov function. We can infer asymptotic stability by examining up to the third derivative of the Lyapunov function.

Definition 3

([13]). If the function $V\left(x\right)$ is three times differentiable, and there exist $a_1,a_2>0$ such that:

$$a_1\stackrel{⃛}{V}\left(x\right)+a_2\ddot{V}\left(x\right)+\dot{V}\left(x\right)⩽0$$

(5)

then $V\left(x\right)$ is an asymptotic stabilizing Lyapunov function.

In addition to stability requirements, the nonlinear system (1) is often subject to critical safety measures. These safety measures can be stated as a set C defined by some continuous function $h\left(x\right)$ as:

$$\begin{array}{c}\begin{array}{cc}\hfill C& =\left\{x\in {\mathbb{R}}^n:h\left(x\right)⩾0\right\}\hfill \\ \hfill 𝜕C& =\left\{x\in {\mathbb{R}}^n:h\left(x\right)=0\right\}\hfill \\ \hfill Int\left(C\right)& =\left\{x\in {\mathbb{R}}^n:h\left(x\right)>0\right\}\hfill \end{array}\end{array}$$

(6)

where $𝜕C$ is the boundary of the set representing the critical safety limits, and $Int\left(C\right)$ is the interior of the set constituting all the permissible/safe states. We can use control barrier functions to ensure the solutions $x\left(t\right)$ will not leave the safe set C, $\forall t\ge 0$ [20]. The CBF is a new mathematical technique used to guarantee the safety and stability of a dynamic system while achieving the desired goals. CBFs are scalar functions that quantify the system’s safety in the current state. The control law is then designed to keep the system within the safety region defined by the CBFs. Mathematically, consider the nonlinear control affine system (1).

Definition 4

([12]). If there exists a continuous function $\alpha (•):[-b,a]\in \mathbb{R}\to \mathbb{R}$ that is strictly increasing and $\alpha \left(0\right)=0$, then α belongs to the extended $\mathcal{K}$ class functions.

Definition 5

([12]). The function $h:{\mathbb{R}}^n\to \mathbb{R}$ is a zeroing barrier function for the safe set $C=\left\{x\in {\mathbb{R}}^n:h\left(x\right)⩾0\right\}$ if there exists an extended $\mathcal{K}$ class function $\alpha (•)$ such that:

$$\begin{array}{c}{\displaystyle \underset{u\in U}{sup}}[L_{f}h\left(x\right)+L_{g}h\left(x\right)u+\alpha \left(h\left(x\right)\right)]⩾0\\ {\displaystyle \underset{int\left(C\right)}{inf}}\left[\alpha \left(h\left(x\right)\right)\right]⩾0\mathit{and}{\displaystyle \underset{𝜕C}{lim}}\alpha \left(h\left(x\right)\right)=0\end{array}$$

(7)

The existence of the CBF $h\left(x\right)$ renders the set C forward invariant.

In the same way as Lyapunov functions, control barrier functions require that the control input u appears in $h\left(x\right)$. In [16], they propose a higher-order CBF to handle higher-relative-order constraints.

Definition 6

([16]). For the nonlinear system (1) with the $m^{th}$ differentiable constraint $h\left(x\right)$, we define a sequence of functions ${\psi }_i$ starting from ${\psi }_0=h\left(x\right)$ and a sequence of safe sets $C_i\left(t\right)$ associated with each ${\psi }_i$, with $i\in \{1,2,...,m\}$:

$$\begin{array}{cc}\hfill {\psi }_i(x,t)& ={\dot{\psi }}_{i-1}(x,t)+{\alpha }_i\left({\psi }_{i-1}(x,t)\right)\hfill \\ \hfill C_i\left(t\right)& =\left\{x\in {\mathbb{R}}^n:{\psi }_{i-1}(x,t)⩾0\right\}\hfill \end{array}$$

(8)

The function ${\psi }_m$ is a high-order control barrier function if there exist extended $\mathcal{K}$ class functions ${\alpha }_i(•)$ such that:

$$\underset{u\in U}{sup}[L_f^{m}h\left(x\right)+L_g{L}_f^{m-1}h(x,t)u+{\displaystyle \frac{{𝜕}^{m}h(x,t)}{𝜕t^m}}+\mathcal{O}\left(h(x,t)\right)+{\alpha }_m\left({\psi }_{m-1}(x,t)\right)]⩾0$$

(9)

$\forall x\left(t\right)\in C_1\left(t\right)\cap ,\dots ,\cap C_m\left(t\right),\forall t>0$, under the condition that $x\left(0\right)\in C_1\left(0\right)\cap ,\dots ,\cap C_m\left(0\right)$.

Ensuring ${\psi }_m⩾0$ guarantees the forward invariance of the set $C_{m+1}$ and, subsequently, the forward invariance of all the sets $C_{m-i}\left(t\right)$, hence ensuring the system’s safety within the set $C_1\left(t\right)$, similar to Definition 5. To guarantee the feasibility of the HOCBF, the authors of [16] suggested multiplying the class $\mathcal{K}$ functions $\alpha (•)$ by a penalty term $p_i$ and solving iteratively to relax the problem when needed:

$${\psi }_i={\dot{\psi }}_{i-1}+p_i{\alpha }_i\left({\psi }_{i-1}\right)$$

(10)

3. A QP Formulation Based on High-Order Control Lyapunov–Barrier Functions

In this section, we synthesize HOCLFs and HOCBFs to control nonlinear systems. We first establish some conditions to extend the use of CLFs and CBFs for controlling and constraining non-affine nonlinear systems. Next, we propose a new form for higher-order CLFs inspired by the analytical first-order CLF in [10,11,12], explicitly displaying the rate of convergence and the computational high-order CLF proposed in [13] incorporating the higher-order derivative of the Lyapunov function. Finally, we suggest a classical controller to address the contrast between the CLFs and a heuristic approach to handle the infeasibility in the QP arising from CLF and CBF conflicts.

Starting from the first formulation of Lyapunov functions [8], Artstein’s theorem considers a general dynamic system of the form $\dot{x}=f(x,u)$ and argues its global asymptotic stability under the condition of finding a continuous function $V:{\mathbb{R}}^n\to \mathbb{R}$ such that:

$$\begin{array}{cc}\hfill V\left(0\right)& =0x=0\hfill \\ \hfill V\left(x\right)& >0\forall x\ne 0\hfill \\ \hfill \underset{u\in U}{inf}[\nabla V·f(x,u)]& <0\forall x\ne 0\hfill \end{array}$$

(11)

A smooth mapping $v\left(x\right):\mathbb{R}\to U$ is constructed to achieve the global asymptotic stability of the system with the feedback law $\dot{x}=f(x,v\left(x\right))$, where $v\left(x\right)$ belongs to a convex set and U is a convex polytope $U=\left\{u\in {\mathbb{R}}^m\mid A_{0}u⩽b_0\right\}$. It is also suggested to consider a piece-wise linear function to ensure Lipschitz continuity and guarantee the existence of a control law $v\left(x\right)$. The linearity in input was further established in the work conducted in [9], where a universal Artstein theorem is constructed for systems with affine inputs. Since Artstein’s theorem guarantees the stability of a general dynamic system, we argue that we can use analytical Lyapunov and barrier functions to stabilize non-affine nonlinear systems using convex optimization under certain conditions.

In this work, we consider a general dynamic system of the form:

$$\dot{x}\left(t\right)=f\left(x\left(t\right)\right)+g(x\left(t\right),u\left(t\right))$$

(12)

where $x\in {\mathbb{R}}^n$ and $u\in {\mathbb{R}}^m$ are the states and control inputs, respectively. f and g are globally Lipschitz, and g is at least twice differentiable. We also consider a continuously differentiable positive-definite function $V:{\mathbb{R}}^n\to \mathbb{R}+$ to be an exponentially stabilizing control Lyapunov function yielding the following controller:

$$\begin{array}{cc}\hfill & \underset{u\in U}{min}{u}^{⊺}u\hfill \\ \hfill & \mathrm{s}.\mathrm{t}.[\nabla V·\dot{x}+cV\left(x\right)]⩽0\hfill \end{array}$$

(13)

Theorem 1.

For a general Lyapunov function $V:{\mathbb{R}}^n\to \mathbb{R}+$, the min-norm controller (13) is a convex quadratic program if $[\nabla V(x\left)·g\right(x,u\left)\right]$ is convex, requiring that $\nabla V\left(x\right)$ and $g(x,u)$ are both convex and positive functions (or concave and negative) and have the same monotonicity. Since $\nabla V\left(x\right)$ is a function of the states only, it is sufficient that $[\nabla V\left(x\right)·{\displaystyle \frac{{𝜕}^{2}g(x,u)}{𝜕u^2}}]⩾0$. That is, $\nabla V\left(x\right)$ is positive and $g(x,u)$ is convex (or negative and concave) in $dom\left(V\right)\cap dom\left(g\right)$, respectively.

Proof. 

We prove the first statement by verifying Jensen’s inequality for the product $[\nabla V(x\left)·g\right(x,u\left)\right]$. If $\nabla V\left(x\right)$ and $g(x,u)$ are positive, convex, and have the same monotonicity, then for $0\le \theta \le 1$, we find:

$$\nabla V\left(x\right)g(x,\theta u_1+(1-\theta )u_2)\le \theta \nabla V\left(x\right)g(x,u_1)+(1-\theta )\nabla V\left(x\right)g(x,u_2)$$

We prove the second statement by verifying the second-order convexity condition. Taking the first derivative of $[\nabla V\left(x\right)·\dot{x}]$ with respect to the control input eliminates $[\nabla V(x\left)·f\right(x\left)\right]$ and yields

$${\displaystyle \frac{\mathrm{d}[\nabla V\left(x\right)·\dot{x}]}{\mathrm{d}u}}={\displaystyle \frac{𝜕\nabla V\left(x\right)}{𝜕u}}·g(x,u)+\nabla V\left(x\right)·{\displaystyle \frac{𝜕g(x,u)}{𝜕u}}$$

Taking the second derivative yields

$${\displaystyle \frac{{\mathrm{d}}^2[\nabla V\left(x\right)·\dot{x}]}{\mathrm{d}{u}^2}}={\displaystyle \frac{{𝜕}^2\nabla V\left(x\right)}{𝜕u^2}}·g(x,u)+\nabla V·{\displaystyle \frac{{𝜕}^{2}g(x,u)}{𝜕u^2}}+2{\displaystyle \frac{𝜕\nabla V\left(x\right)}{𝜕u}}·{\displaystyle \frac{𝜕g(x,u)}{𝜕u}},$$

and canceling the term involving $𝜕\nabla V\left(x\right)$, we obtain

$${\displaystyle \frac{{𝜕}^2[\nabla V\left(x\right)·\dot{x}]}{𝜕u^2}}=\nabla V\left(x\right)·{\displaystyle \frac{{𝜕}^{2}g(x,u)}{𝜕u^2}}.$$

Therefore, the convexity of the feasible set in the quadratic program depends on the sign of $\nabla V\left(x\right)·{\displaystyle \frac{{𝜕}^{2}g(x,u)}{𝜕u^2}}$.    □

Remark 1.

With similar reasoning, we can prove that the CBF in (7) defines a convex feasible set for the quadratic program if $[\nabla h\left(x\right)·{\displaystyle \frac{{𝜕}^{2}g(x,u)}{𝜕u^2}}]⩽0$.

Remark 2.

A control affine system is a special case as $[\nabla V·{\displaystyle \frac{{𝜕}^{2}g(x,u)}{𝜕u^2}}]=[\nabla h\left(x\right)·{\displaystyle \frac{{𝜕}^{2}g(x,u)}{𝜕u^2}}]=0$.

Example 1.

The following example demonstrates an extreme case where both $[\nabla V(x\left)·g\right(x,u\left)\right]$ change sign within $dom\left(V\right)\cap dom\left(g\right)$ while keeping the convexity condition valid. Consider the system

$$\dot{x}=10x^2-5x\sqrt{u}$$

(14)

We can drive and stabilize this system from an arbitrary $x_0$ to the unstable equilibrium point at $x_f=\left[00\right]$ by using the Lyapunov function $V\left(x\right)=x^2$. This results in

$$\nabla V\left(x\right)=2x,{\displaystyle \frac{{𝜕}^{2}g(x,u)}{𝜕u^2}}={\displaystyle \frac{x}{4\sqrt{u^3}}}.$$

By examining $\nabla V\left(x\right)$ and ${\displaystyle \frac{{𝜕}^{2}g(x,u)}{𝜕u^2}}$, we can see a change of sign between $x\in {\mathbb{R}}_{+}^n$ and $x\in {\mathbb{R}}_{-}^n$; nevertheless, $[\nabla V\left(x\right)·{\displaystyle \frac{{𝜕}^{2}g(x,u)}{𝜕u^2}}]\ge 0$ for $x\in {\mathbb{R}}^n$. Hence, we conclude controllability via the control Lyapunov function

$$CLF:=20x^3-10x^2\sqrt{u}+c{x}^2⩽0$$

Figure 2 shows the state trajectory, control trajectory, and phase portrait of the unforced system.

Next, we propose a new computationally efficient form for higher-order CLFs that can exponentially stabilize nonlinear systems. Starting from the HOCLF proposed in [13], we ensure the satisfaction of positiveness and zero dynamics by the initial choice of the Lyapunov function, e.g., taking a quadratic function of the difference between the desired and actual solution guarantees $V\left(x\right)>0,\forall t$ and $V\left(0\right)=0$.

$$V(x,x_d)={(x_{desired}\left(t_f\right)-x\left(t\right))}^2$$

(15)

We also omit the scalars ${\tau }_i$ and add the Lyapunov function multiplied by a scalar c that represents the rate of convergence of the CLF to the sum of Lyapunov derivatives and obtain the following HOCLF:

$$\underset{u\in U}{inf}[V^m+V^{m-1}+\cdots +\dot{V}+cV]⩽0$$

(16)

Combining high-order Lyapunov and barrier functions can often encounter infeasibilities. The first source of infeasibility comes from the conflicts of goals between the CLFs and CBFs. This problem was overcome in [11,12,21] by adding a relaxing (softening) term $\delta$ to the CLFs, which allows them to diverge from the desired states to guarantee the satisfaction of CBF constraints. The second infeasibility arises from the HOCLF and HOCBF coefficients when their constraints conflict with the constraint of the control input bounds. Although [16] suggested an iterative approach to change the parameters until they make the QP feasible, it relaxes the safety constraints and has a marginally incremental impact on computation time and power. Alternatively, we adopt a heuristic approach where the quadratic program solves for approximate solutions when an optimal solution is impractical or impossible to find, and this is achieved by maximizing the rates of convergence for the CLF and the Kappa function coefficients with a tunable upper bound while minimizing the relaxing terms for the CLF. The last infeasibility to overcome stems from conflicts between the multiple goals of CLFs; e.g., a position CLF driving the system towards a desired position conflicts with the velocity CLF trying to attenuate the velocity to zero, resulting in a quadratic program infeasibility. To overcome this issue, we suggest using state-varying desired states. In other words, instead of using the final desired states in the CLFs, we compute intermediate desired states between the initial and final states. We suggest two methods for determining intermediate states.

Navigational Planner: We can use a navigation algorithm between the initial and final states and obtain a sequence of plausible intermediate states; nonetheless, this method has some distinctive drawbacks. Employing the navigation algorithm offline leaves it myopic to the current state and trajectory; therefore, its intermediate states can become unreachable as the current trajectory progresses, especially for highly nonlinear coupled dynamics. On the other hand, using the algorithm online can generate excellent intermediate states; however, it requires significantly higher computational power and time.

PID Planner: We can use an online PID planner to generate the subsequent intermediary desired state based on our current and final state. We can define the error between the current and final state as $e_x\left(t\right)=x_{desired}\left(t_f\right)-x\left(t\right)$, yielding a PID planner as follows:

$$x_{desired}(t+dt)=K_{\mathrm{P}}{e}_x\left(t\right)+K_{\mathrm{I}}{\int }_0^{t_f}{e}_x\left(\tau \right)d\tau +K_{\mathrm{D}}{\displaystyle \frac{d{e}_x\left(t\right)}{dt}}$$

(17)

where $K_{\mathrm{P}},K_{\mathrm{I}},K_{\mathrm{D}}$ are tunable parameters. We adopt this approach in our work as it provides an exceptional compromise between generating online, plausible, reachable intermediate states and considerable computational efficacy. The final form of the optimal quadratic program controller we suggest is as follows:

$$\begin{array}{cc}\hfill \underset{u,\delta ,q,c}{min}:& u^{T}H\left(x\right)u+{\delta }^{T}Q\left(x\right)\delta -P^T\left(x\right)q-R^T\left(x\right)c\hfill \\ \hfill \mathrm{s}.\mathrm{t}:& V_i^m\left(x_{PID}\right)+V_i^{m-1}\left(x_{PID}\right)+\cdots +{\dot{V}}_i\left(x_{PID}\right)+C_i{V}_i\left(x_{PID}\right)\hfill & \hfill ⩽{\delta }_i& \mathrm{HOCLF}\hfill \\ \hfill & {\dot{V}}_j\left(x_{PID}\right)+C_j{V}_j\left(x_{PID}\right)\hfill & \hfill ⩽{\delta }_j& \mathrm{CLFs}\hfill \\ \hfill & {\psi }_{m,0,k}\left(x_{PID}\right)+{\psi }_{m,1,k}\left(x_{PID}\right)+{\alpha }_{m,k}\left({\psi }_{m-1,k}\right)\left(x_{PID}\right)\hfill & \hfill ⩾0& \mathrm{HO}\text{-}\mathrm{ZCBFs}\hfill \\ \hfill & {\Phi }_{0,0,p}\left(x_{PID}\right)+{\Phi }_{0,1,p}\left(x_{PID}\right)+{\alpha }_p\left({\Phi }_0\right)\left(x_{PID}\right)\hfill & \hfill ⩾0& \mathrm{ZCBFs}\hfill \\ \hfill & A_{0}u\hfill & \hfill ⩽b_0& \mathrm{Control}\mathrm{limitation}\hfill \\ \hfill & \mathrm{Other}\mathrm{DCP}\mathrm{constraints}\hfill \end{array}$$

(18)

where $H,Q,P,\mathrm{and}R$ are weighing coefficients used to tune the system’s behavior.

4. Simulations and Results

4.1. 6-DOF Optimal Powered Descent

To demonstrate the proposed HOCLBF framework on a more complex system, we address the problem of controlling a 6-DOF powered descent. The model we use in this paper is inspired by [7,22,23] but tailored to accommodate our application. In our model, we assume a single 2D gimbal engine that can continuously vary between minimum and maximum thrusts; the rocket is a rigid body with a constant CG and inertia and we neglect any deformations. We assume the dynamics are mainly governed by inertial and thrust forces and neglect aerodynamic forces based on the findings of [7]. Lastly, we ignore planetary motion and assume a constant gravity throughout the rocket trajectory.

4.1.1. Dynamic Model

To fully describe the dynamics of the 6-DOF rocket, similar to [7,23] we assume the fuel dynamics are governed by an affine function of the thrust magnitude and neglect the atmospheric back pressure:

$$\dot{m}\left(t\right)=-{\alpha }_{\dot{m}}{∥T_B∥}_2$$

(19)

where ${\alpha }_{\dot{m}}={\displaystyle \frac{1}{I_{sp}{g}_0}}$. $I_{sp}$ is the vacuum specific impulse of the engine, and $g_0$ is standard Earth gravity. We describe the translational and rotational dynamics with six differential equations as follows.

$$\begin{array}{cc}\hfill {\dot{r}}_I\left(t\right)& =C_{I\leftarrow B}\left(q_I\left(t\right)\right)v_b\hfill \\ \hfill {\dot{v}}_B\left(t\right)& ={\displaystyle \frac{1}{m\left(t\right)}}[T\left(t\right)+A\left(t\right)]+E_{B\leftarrow I}\left(q_I\left(t\right)\right)g_I-{\omega }_B\times v_B\hfill \\ \hfill {\ddot{r}}_I\left(t\right)& =C_{I\leftarrow B}\left(q_I\left(t\right)\right){\dot{v}}_B+{\dot{C}}_{I\leftarrow B}\left(q_I\left(t\right)\right)v_B\hfill \\ \hfill {\dot{q}}_I\left(t\right)& =E_{I\leftarrow B}\left(q_I\left(t\right)\right){\omega }_B\hfill \\ \hfill {\dot{\omega }}_B\left(t\right)& ={\tilde{J}}_B^{-1}\left[r_{T,B}\times T_B\left(t\right)+r_{cp,B}\times A\left(t\right)\right]-{\tilde{J}}_B^{-1}\left({\omega }_B\times \tilde{J}{\omega }_B\right)\hfill \\ \hfill {\ddot{q}}_I\left(t\right)& =E_{I\leftarrow B}\left(q_I\left(t\right)\right){\dot{\omega }}_B+{\dot{E}}_{I\leftarrow B}\left(q_I\left(t\right)\right){\omega }_B\hfill \end{array}$$

(20)

We add the second derivative of the position and attitude in the inertial frame. We make these adjustments to accommodate the Lyapunov approach efficiently.

4.1.2. High-Order Control Lyapunov Functions

To drive the states to the desired ones, we need to formulate positive Lyapunov functions that are twice differentiable as the order of the position dynamics is $m=2$. Hence, we use the following quadratic Lyapunov function which satisfies positiveness and the zero dynamics point and yields:

$$\begin{array}{cc}\hfill V_i& ={\displaystyle \frac{1}{2}}{\parallel s_i\left(t\right)-s_{id}\parallel }_2^2\hfill \\ \hfill {\dot{V}}_i& ={\dot{s}}_i\left(t\right)\left(s_i\left(t\right)-s_{id}\right)\hfill \\ \hfill {\ddot{V}}_i& ={\ddot{s}}_i\left(t\right)\left(s_i\left(t\right)-s_{id}\right)+{\dot{s}}_i^2\left(t\right)\hfill \\ \hfill CLF& :={\ddot{V}}_{r_I}+{\dot{V}}_{r_I}+c_{r_I}{V}_{r_I}⩽{\delta }_{r_I}\hfill \end{array}$$

(21)

$$\begin{array}{cc}\hfill & \mathrm{Position}\mathrm{CLF}:\hfill \\ \hfill {\ddot{V}}_{r_I}=& {\displaystyle \frac{1}{m\left(t\right)}}\left(r_I\left(t\right)-r_d\right)C_{I\leftarrow B}\left(q_I\left(t\right)\right)T\left(t\right)+\left(r_I\left(t\right)-r_d\right)C_{I\leftarrow B}\left(q_I\left(t\right)\right)\hfill \\ \hfill & [{\displaystyle \frac{1}{m\left(t\right)}}A\left(t\right)+E_{B\leftarrow I}\left(q_I\left(t\right)\right)g_I-{\omega }_B\left(t\right)\times v_B\left(t\right)]+\left(r_I\left(t\right)-r_d\right)\hfill \\ \hfill & {\dot{C}}_{I\leftarrow B}\left(q_I\left(t\right)\right)v_B\left(t\right)+{\dot{r}}_I{\left(t\right)}^2\hfill \\ \hfill & \mathrm{Velocity}\mathrm{CLF}:\hfill \\ \hfill {\dot{V}}_{v_B}=& {\displaystyle \frac{1}{m\left(t\right)}}\left(v_B\left(t\right)-v_d\right)T\left(t\right)+\left(v_B\left(t\right)-v_d\right)\hfill \\ \hfill & \left[E_{B\leftarrow I}\left(q_I\left(t\right)\right)g_I-{\omega }_B\left(t\right)\times v_B\left(t\right)\right]\hfill \\ \hfill & \mathrm{Attitude}\mathrm{CLF}:\hfill \\ \hfill {\ddot{V}}_{q_I}=& \left(q_I\left(t\right)-q_d\right)E_{B\leftarrow I}\left(q_I\left(t\right)\right){\tilde{J}}^{-1}\left(r_T\times T\left(t\right)\right)+(q_I\left(t\right)-q_d)E_{B\leftarrow I}\left(q_I\left(t\right)\right)\hfill \\ \hfill & [{\tilde{J}}^{-1}(r_{cp}\times A\left(t\right)-{\omega }_B\left(t\right)\times \tilde{J}{\omega }_B\left(t\right))]+(q_I\left(t\right)-q_d){\dot{E}}_{B\leftarrow I}\left(q_I\left(t\right)\right){\omega }_B\left(t\right)\hfill \\ \hfill & +{\dot{q}}_I{\left(t\right)}^2\hfill \\ \hfill & \mathrm{Angular}\mathrm{velocity}\mathrm{CLF}:\hfill \\ \hfill {\dot{V}}_{{\omega }_B}=& \left({\omega }_B\left(t\right)-{\omega }_d\right){\tilde{J}}^{-1}\left(r_T\times T_B\left(t\right)\right)+\left({\omega }_B\left(t\right)-{\omega }_d\right)\hfill \\ \hfill & \left[{\tilde{J}}^{-1}\left(r_{cp}\times A\left(t\right)-{\omega }_B\left(t\right)\times \tilde{J}{\omega }_B\left(t\right)\right)\right]\hfill \end{array}$$

(22)

Formulating all the Lyapunov functions with the relaxing terms and PID planner ensures convergence to the desired states while maintaining flexibility to diverge when a conflict of goals happens based on the weights of the relaxing terms. Now that smooth convergence is guaranteed, it is crucial to maintain the states within their designated safe spaces, as described by the safety constraints. This is accomplished through implementing control barrier functions without relaxation to enforce forward invariance for the safe sets.

4.1.3. Safety Constraints

To ensure the safety of the descent, we need to ensure the satisfaction of several non-convex constraints; we start by setting a lower bound on the vehicle mass:

$$m\left(t\right)⩾m_{dry}\in R_{++}\Rightarrow m\left(t\right)-m_{dry}⩾0$$

(23)

We also want the rocket to stay inside a glide slope cone with half angle ${\gamma }_{gs}\in (0,{90}^{\circ }]$ and an origin that coincides with the inertial frame:

$${\widehat{e}}_3·{\overrightarrow{r}}_I\left(t\right)⩾cot\left({\gamma }_{gs}\right){∥{\widehat{H}}_{\gamma }{\overrightarrow{r}}_I\left(t\right)∥}_2\Rightarrow r_I{M}_{gs}{r}_I⩾0$$

(24)

where ${\widehat{H}}_{\gamma :}={\left[{\widehat{e}}_1,{\widehat{e}}_2\right]}^T$ and $M_{gs}=\left[{[-cot{\left({\gamma }_{gs}\right)}^{2}00]}^T{[0-cot{\left({\gamma }_{gs}\right)}^{2}0]}^T{\left[001\right]}^T\right]$.

To constrain the angular velocities of the rocket, we set an upper bound ${\omega }_{max}$:

$${∥{\omega }_B\left(t\right)∥}_2⩽{\omega }_{max}\Rightarrow {\omega }_{max}^2-\left|\right|{\omega }_B\left(t\right){\left|\right|}_2^2⩾0$$

(25)

Since there are physical limitations on the controls, we need to set upper and lower bounds on the thrust generated by the engine and ensure that the thrust remains within a gimbal angle dictated by the engine:

$$\begin{array}{c}\hfill 0⩽T_{min}⩽{∥T_B∥}_2⩽T_{max}\\ \hfill T_z⩾cos\left({\delta }_{\max }\right){∥T_B∥}_2\end{array}$$

(26)

Next, we can specify different boundary conditions reflecting several scenarios for our rocket trajectory. We start from a specified initial rocket mass, position, velocity, and angular velocity:

$$m\left(0\right)=m_{in},r_I\left(0\right)=r_{I_{in}},v_B\left(0\right)=v_{B_{in}},{\omega }_B\left(0\right)={\omega }_{B_{in}}$$

(27)

and we aim to control the rocket to satisfy the desired final conditions:

$$r_I\left(t_f\right)=r_{I_{LS}},v_B\left(t_f\right)=v_{B_{LS}},q_I\left(t_f\right)=q_{I_{LS}}{\omega }_B\left(t_f\right)={\omega }_{B_{LS}}$$

(28)

4.1.4. Safety Constraints as Control Barrier Functions

In this section, we formulate a set of first-order and second-order zeroing control barrier functions to confine the system’s states within the designated safe sets. We define the extended class $\mathcal{K}$ function as a linear function of the safety function itself ${\alpha }_i\left({\psi }_j\right)={\psi }_j$ with a coefficient of 1, as it will be multiplied by an optimized parameter to ensure feasibility. Formulating the highest order CBF yields:

$$\begin{array}{cc}\hfill {\psi }_{1_m}:=& -{\alpha }_{\dot{m}}{∥T_B\left(t\right)∥}_2⩾-p_{1_m}\left(m\left(t\right)-m_{dry}\right)\hfill \\ \hfill {\psi }_{1_{{\omega }_B}}:=& -{\omega }_B\left(t\right)·{\tilde{J}}^{-1}\left(r_T\times T\left(t\right)\right)⩾-{\tilde{J}}^{-1}\left({\omega }_B\left(t\right)\times \tilde{J}{\omega }_B\right)·{\omega }_B\left(t\right)-{\displaystyle \frac{p_{1_{{\omega }_B}}}{2}}\left({\omega }_{max}^2-{∥{\omega }_B∥}_2^2\right)\hfill \\ \hfill {\psi }_{2_{gs}}:=& {\displaystyle \frac{1}{m\left(t\right)}}{r}_I{M}_{gs}{C}_{I\leftarrow B}\left(q_I\left(t\right)\right)T\left(t\right)⩾r_I{M}_{gs}{C}_{I\leftarrow B}\left(q_I\left(t\right)\right)\left[{\omega }_B\left(t\right)\times v_B\left(t\right)-E_{B\leftarrow I}\left(q_I\left(t\right)\right)g_I\right]\hfill \\ \hfill & -r_I{M}_{gs}{C}_{I\leftarrow B}\left(q_I\left(t\right)\right)v_B-{\dot{r}}_I{M}_{gs}{\dot{r}}_I-\left({\displaystyle \frac{p_{1_{gs}}}{2}}+p_{2_{gs}}\right){\dot{\psi }}_{0_{gs}}-{\displaystyle \frac{p_{1_{gs}}{p}_{2_{gs}}}{2}}\left({\psi }_{0_{gs}}\right)\hfill \end{array}$$

(29)

The last step for the barrier functions is replacing the sum and multiplications of parameters $\left({\displaystyle \frac{p_{1_{gs}}}{2}}+p_{2_{gs}}\right)$ and ${\displaystyle \frac{p_{1_{gs}}{p}_{2_{gs}}}{2}}$ with single parameters $\{p_{1_{gs}},p_{2_{gs}}\}$, respectively. That is to keep the barrier functions convex in parameters too, enabling the optimization of the parameters inside the quadratic program. Algorithm 1 summarizes the HOCLBF method of solving a sequence of online convex quadratic programs to control the optimal 6-DOF powered descent in real-time. Model parameters were taken from [7,22,23], with additional parameters either manually tuned or heuristically optimized to ensure adequate performance. Note that we strongly suggest using an adaptive fourth Runge-Kutta for time integration to minimize higher-order truncation errors.

Algorithm 1 HOCLBF for 6-DOF powered descent control.

1: Initialize: $x=x_{initial},x_{final},p_{i,min},c_{i,min},p_{i,max},c_{i,max},dt,\epsilon ,K_{P_i},K_{I_i},K_{D_i}$   2: while  $(\parallel x_{final}-x{\parallel }_2 ⩾\epsilon )$  do   3:     $e_x=x_{final}-x$   4:     $x_{desired}=K_{\mathrm{P}}{e}_x+K_{\mathrm{I}}\int e_{x}d\tau +K_{\mathrm{D}}{\displaystyle \frac{d{e}_x}{dt}}$   5:     $u^{*}={arg\; min}_{u,c,\delta ,p}:u^{T}Hu+{\delta }^{T}Q\delta -P^{T}p-R^{T}c$   6:     s.t:   7:           ${\ddot{V}}_{r_I}+{\dot{V}}_{r_I}+c_{r_I}{V}_{r_I}⩽{\delta }_{r_I}$                                                    ▹ Position HOCLF (22)   8:           ${\dot{V}}_{v_B}+c_{v_B}{V}_{v_B}⩽{\delta }_{V_{v_B}}$                                                               ▹ Velocity CLF (22)   9:           ${\ddot{V}}_{q_I}+{\dot{V}}_{q_I}+c_{q_I}{V}_{q_I}⩽{\delta }_{q_I}$                                                   ▹ Attitude HOCLF (22) 10:           ${\dot{V}}_{{\omega }_B}+c_{{\omega }_B}{V}_{{\omega }_B}⩽{\delta }_{{\omega }_B}$                                               ▹ Angular velocity CLF (22) 11:           $c_{{(r,v,q,\omega )}_{max}}⩽I_4{c}_{(r,v,q,\omega )}⩽c_{{(r,v,q,\omega )}_{max}}$                          ▹ Convergence rate bounds 12:           ${\psi }_{\dot{m},1}+p_{1_m}{\psi }_{\dot{m},0}⩾0$                                                               ▹ Dry mass CBF (23) 13:           ${\psi }_{gs,2}+p_{1_{gs}}{\psi }_{gs,1}+p_{2_{gs}}{\psi }_{gs,0}⩾0$                                    ▹ Glide slope HOCBF (24) 14:           ${\psi }_{{\omega }_B,1}+p_{1_{{\omega }_B}}{\psi }_{{\omega }_B,0}⩾0$                                               ▹ Angular velocity CBF (25) 15:           ${\widehat{e}}_3·u⩾cos\left({\delta }_{\max }\right){∥u∥}_2$                                       ▹ Gimbal thrust constraint (26) 16:           $A_{T}u⩽b_T$                                                         ▹ Thrust magnitude bounds (26) 17:     $x=x+\dot{x}(x,u^{*})dt$ or $x=x+R{K}_4(\dot{x},x,u^{*},dt)$     ▹ Time propagation (19) and (20) 18: end while

4.1.5. Numerical Results

In this section, we assess the performance of the HOCLBF in comparison to the base CLF-CBF and the successive convexification approach. Our goal is to demonstrate the advantage of using the HOCLBF in generating feasible solutions compared to the base CLF-CBF with constant parameters, as well as compare its trajectory to the robust SC algorithm. The comparison is carried out under the condition that all other parameters are kept identical to ensure a consistent assessment. We run our algorithm on a standard laptop using a commercial solver “ECOS” [24] suited for embedded applications and obtain an average runtime of less than 1.1 ms. We start by comparing the high-velocity entry and landing between the three approaches, starting from $r_I\left(0\right)=[3.0,3.0,4.0]$ and landing on a circular pad $C([0,0,0],{\rho }_v=0.5)$ in the presence of a 60° glide slope constraint. In Figure 3, we can see that the base CLF-CBF generates infeasible solutions as soon as it gets closer to the glide slope constraint, while the HOCLBF managed to reach the landing site successfully. We can also see some fluctuations in the HOCLBF compared to the smooth SC trajectory.

Figure 4 illustrates the 3D trajectories where we can see that the HOCLBF reaches the landing site successfully, while the base CLF-CBF faces infeasibilities arising from the fixed parameters.

To demonstrate the efficiency of the CBF approach in ensuring the safety of the rocket landing, we generated a trajectory with no glide slope ${\gamma }_{gs}={90}^{\circ }$, then for ${\gamma }_{gs}={45}^{\circ },{60}^{\circ },{75}^{\circ }$ (Figure 5). For the tighter glide slopes, we can see that despite starting outside the safe region, the rocket moves horizontally at a slower descent rate until it enters the safe region and then starts descending faster and successfully reaches the landing site with no infeasibilities in the QP while keeping the safety constraints satisfied.

4.1.6. Limitations and Future Work

In this work, we attempt to generalize the use of high-order CLFs and CBFs to a wider set of problems and demonstrate the potential of HOCLBFs in safely controlling complex high-dimensional problems. In this work, we overcome the infeasibility issues that arise from the coupling of CLFs and CBFs by using heuristic parameters and we obtain successful results comparable to the robust successive convexification algorithm. Nonetheless, this approach still requires adequate analytical design for the Lyapunov and barrier functions, and some parameters still need initial tuning and are prone to diverge in the presence of high disturbances. Therefore, our next goal is to incorporate some machine learning algorithms alongside differentiable optimization to enhance the performance of the HOCLBF and make it robust to parameter tuning and environmental disturbances.

5. Conclusions

In this paper, we generalize control Lyapunov and barrier functions to general nonlinear dynamic systems with convex input under some conditions and suggest a new formulation of the higher-order control Lyapunov function that guarantees convergence for higher-order dynamics. We unite control Lyapunov and barrier functions using a heuristic approach and an online PID planner ensuring the feasibility of the Quadratic Program. We showcase the efficacy of our HOCLBF approach by devising a real-time six-degree-of-freedom powered descent controller as a convex Quadratic Program. Simulation results for different maneuvers under different safety settings were presented to demonstrate the effectiveness of the HOCLBF in controlling a highly nonlinear coupled system while maintaining safety. In this work, we overcome the infeasibility issues that arise from the coupling of CLFs and CBFs by using heuristic parameters, and we obtain successful results comparable to the robust successive convexification algorithm. Nonetheless, this approach still requires an adequate analytical design for the Lyapunov and barrier functions; some parameters still need initial tuning and are prone to diverge in the presence of high disturbances. Therefore, The next phase of this research will focus on leveraging differentiable convex optimization and reinforcement learning approaches to achieve automatic parameter tuning and intelligent safety constraints. We are also looking into distributional robust optimization to make the HOCLBF robust to model uncertainty and environmental disturbances.