IEEE Control Systems Letters paper presented at

2023 American Control Conference (ACC)

San Diego, CA, USA. May 31 - June 2, 2023

Iterative Convex Optimization for Model Predictive Control

with Discrete-Time High-Order Control Barrier Functions
Shuo Liu∗1 , Jun Zeng∗2 , Koushil Sreenath2 and Calin A. Belta1

Abstract— Safety is one of the fundamental challenges in have been introduced in [4]. In this paper, we address
control theory. Recently, multi-step optimal control problems the above challenges with a proposed convex MPC with
for discrete-time dynamical systems were formulated to en- linearized, discrete-time CBFs, under an iterative approach.
force stability, while subject to input constraints as well as
safety-critical requirements using discrete-time control barrier In contrast with the real-time iteration (RTI) approach in-
functions within a model predictive control (MPC) framework. troduced in [5], which solves the problem through iterative
Existing work usually focus on the feasibility or the safety for Newton steps, our approach solves the optimization problem
the optimization problem, and the majority of the existing work formulated by a convex MPC iteratively for each time step.
restrict the discussions to relative-degree one control barrier We show that the proposed approach can significantly reduce
functions. Additionally, the real-time computation is challenging
when a large horizon is considered in the MPC problem for the computational time, compared to the state of the art
relative-degree one or high-order control barrier functions. In introduced in [4], even for CBFs with high relative-degree,
this paper, we propose a framework that solves the safety- without sacrificing the controller performance. The feasibllity
critical MPC problem in an iterative optimization, which is rate of our proposed method also outperforms that of the
applicable for any relative-degree control barrier functions. In baseline method in [4] for large horizon lengths.
the proposed formulation, the nonlinear system dynamics as
well as the safety constraints modeled as discrete-time high- B. Related work
order control barrier functions (DHOCBF) are linearized at
each time step. Our formulation is generally valid for any 1) Model Predictive Control (MPC): MPC is widely used
control barrier function with an arbitrary relative-degree. in modern control systems, such as controller design in
The advantages of fast computational performance with safety robotic manipulation and locomotion [6], [7] to obtain a
guarantee are analyzed and validated with numerical results.
control strategy as a solution to an optimization problem.
I. I NTRODUCTION Stability was achieved in [8] by incorporating discrete-time
A. Motivation control Lyapunov functions (DCLFs) into a general MPC-
based optimization problem to realize real-time control on a
Safety-critical optimal control is a central problem in robotic system with limited computational resources. More
robotics. For example, reaching a goal while avoiding ob- and more recent work like [9] emphasizes safety in robot
stacles and minimizing energy can be formulated as a design and deployment since it is an important criterion for
constrained optimal control problem by using continuous- real-world tasks. Some works consider safety criteria through
time control barrier functions (CBFs) [1], [2]. By dividing the introduction of additional repelling functions [1], [10]
the timeline into small intervals, the problem is reduced to while some works regard obstacle avoidance as one concrete
a (possibly large) number of quadratic programs, which can scenario in terms of safety criteria for robots [11]–[13].
be solved at real-time speeds. However, this approach can be Those safety criteria are usually formulated as constraints in
too aggressive due to the lack of predicting ahead. optimization problems. This paper can be seen in the context
Model predictive control (MPC) with CBFs [3] considers of MPC with safety constraints.
the safety problem in the discrete-time domain, and provides 2) Continuous-Time CBFs: It has recently been shown
a smooth control policy as it involves future state information that to stabilize an affine control system while also satisfying
along a receding horizon. However, the computational time safety constraints and control limitations, CBFs can be
is relatively large and increases dramatically with a larger unified with control Lyapunov functions (CLFs) to form
horizon, since the optimization itself is usually nonlinear a sequence of single-step optimization programs [1], [2],
and non-convex. An additional issue of this nonlinear model [14], [15]. If the cost is quadratic, the optimizations are
predictive formulation is the feasibility of the optimization. quadratic programs (QP), and the solutions can be deployed
For CBFs with relative-degree one, relaxation techniques in real time [1], [16]. Adaptive, robust and stochastic ver-
∗ Authors contributed equally. sions of safety-critical control with CBFs were introduced
This work was supported in part by the NSF under grants IIS-2024606 in [17]–[21]. For safety constraints expressed using functions
and CMMI-1931853. with high relative degree with respect to the dynamics of
1 S. Liu and C. Belta are with the department of Mechanical Engi-
neering, Boston University, Brookline, MA, 02215, USA {liushuo, the system, exponential CBFs [22] and high-order CBFs
cbelta}@bu.edu. 2 J. Zeng and K. Sreenath are with the Uni- (HOCBFs) [23]–[25] were proposed.
versity of California, Berkeley, CA, 94720, USA {zengjunsjtu, 3) Discrete-Time CBFs: Discrete-time CBFs (DCBFs)
koushils}@berkeley.edu
Implementation code is released on https://github.com/ were introduced in [26] as a means to enable safety-critical
ShockLeo/Iterative-MPC-DHOCBF. control for discrete-time systems. They were used in a

979-8-3503-2806-6/$31.00 ©2023 AACC 3368

Authorized licensed use limited to: Shanghai Univ of Electric Power. Downloaded on April 01,2024 at 03:17:37 UTC from IEEE Xplore. Restrictions apply.
nonlinear MPC (NMPC) framework to create NMPC-DCBF time, given that it is initialized in C. We consider the set C
[3], wherein the DCBF constraint was enforced through as the superlevel set of a discrete-time function h : Rn → R:
a predictive horizon. This method was also utilised in a
C := {xt ∈ Rn : h(xt ) ≥ 0}. (1)
multi-layer control framework in [27], where DCBFs with
longer horizons were considered in the MPC problem serving We consider a discrete-time control system in the form
as a mid-level controller to guarantee safety. Generalized
discrete-time CBFs (GCBFs) and discrete-time high-order xt+1 = f (xt , ut ), (2)
CBFs (DHOCBFs) were proposed in [28] and [29] respec- where xt ∈ X ⊂ Rn represents the state of system (2) at
tively, where the DCBF constraint only acted on the first time step t ∈ N, ut ∈ U ⊂ Rq is the control input, and
time-step, i.e., a single-step constraint. MPC with DCBF has function f is locally Lipschitz.
been used in various fields, such as autonomous driving [30]
and legged robotics [31]. For the work above, the CBF Definition 1 (Relative degree [33]). The output yt = h(xt )
constraints are either limited to be activated at the first time- of system (2) is said to have relative degree m if
step [26], [28], [29] to improve the optimization feasibility at yt+i = h(f¯i−1 (f (xt , ut ))), i ∈ {1, 2, . . . , m},
the cost of sacrificing the safety performance, or for multiple ∂yt+m ∂yt+i (3)
or all steps [27], [30], [31] with additional performance s.t. ̸= 0q , = 0q , i ∈ {1, 2, . . . , m − 1},
∂ut ∂ut
optimization from other modules, such as multi-layer con-
trol [27], [30] or planning [31], which needs to specified for i.e., m is the number of steps (delay) in the output yt in
different platforms. A decay-rate relaxing technique [32] was order for the control input ut to appear.
introduced for NMPC with DCBF [4] for all time-steps to In the above definition, we use f¯(xt ) to denote the
enhance the safety and feasibility at the same time, but the uncontrolled state dynamics f (xt , 0). The subscript i of
computation itself is overall still nonlinear and non-convex function f¯(·) denotes the i-times recursive compositions of
which could be computationally slow for large horizons and f¯(·), i.e., f¯i (xt ) = f¯(f¯(. . . , f¯(f¯0 (xt )))) with f¯0 (xt ) = xt .
nonlinear dynamical systems, and the discussion in [4] is | {z }
i-times
limited to relative-degree one. In this paper, we generalize We assume that h(xt ) has relative degree m with respect
relaxing technique for DHOCBF and largely optimize the to system (2) based on Def. 1. Starting with ψ0 (xt ) := h(xt ),
computational time compared to all existing work. we define a sequence of discrete-time functions ψi : Rn →
R, i = 1, . . . , m as:
C. Contributions
ψi (xt ) := △ψi−1 (xt , ut ) + αi (ψi−1 (xt )), (4)
We propose a novel approach to the NMPC with discrete-
time CBFs that is significantly faster than existing ap- where △ψi−1 (xt , ut ) := ψi−1 (xt+1 ) − ψi−1 (xt ), and
proaches. In particular, the contributions are as follows: αi (·) denotes the ith class κ function which satisfies
αi (ψi−1 (xt )) ≤ ψi−1 (xt ) for i = 1, . . . , m. A sequence
• We present a model predictive control strategy for
of sets Ci is defined based on (4) as
safety-critical tasks, where the safety-critical constraints
can be enforced by DHOCBFs. The decay rate in each Ci := {xt ∈ Rn : ψi (xt ) ≥ 0}, i = {0, . . . , m − 1}. (5)
constraint can be relaxed to enhance the feasibility in
Definition 2 (DHOCBF [29]). Let ψi (xt ), i ∈ {1, . . . , m}
optimization and to ensure forward invariance of the
be defined by (4) and Ci , i ∈ {0, . . . , m − 1} be defined by
intersection of a series of safety sets.
(5). A function h : Rn → R is a Discrete-Time High-Order
• We propose an optimal control framework for guaran-
Control Barrier Function (DHOCBF) with relative degree m
teeing safety, where the DHOCBF constraints as well
for system (2) if there exist ψm (xt ) and Ci such that
as the system dynamics are linearized at each iteration,
and considered as constraints in a convex optimization ψm (xt ) ≥ 0, ∀xt ∈ C0 ∩ · · · ∩ Cm−1 . (6)
solved iteratively.
• We show through numerical examples that the proposed Theorem 1 (Safety Guarantee [29]). Given a DHOCBF
framework is significantly faster than existing methods, h(xt ) from Def. 2 with corresponding sets C0 , . . . , Cm−1
without sacrificing safety and feasibility. defined by (5), if x0 ∈ C0 ∩ · · · ∩ Cm−1 , then any Lipschitz
controller ut that satisfies the constraint in (6), ∀t ≥ 0
II. P RELIMINARIES renders C0 ∩ · · · ∩ Cm−1 forward invariant for system (2),
i.e., xt ∈ C0 ∩ · · · ∩ Cm−1 , ∀t ≥ 0.
In this section, we introduce some definitions and results
Remark 1. The function ψi (xt ) in (4) is called a ith order
on CBF and MPC.
discrete-time control barrier function (DCBF) in this paper.
Since satisfying the ith order DCBF constraint (ψi (xt ) ≥
A. Discrete-Time High-Order Control Barrier Function
0) is a sufficient condition for rendering C0 ∩ · · · ∩ Ci−1
(DHOCBF)
forward invariant for system (2) as shown above, it is not
In this work, safety is defined as forward invariance of a necessary to formulate DCBF constraints up to mth order as
set C, i.e., a system is said to be safe if it stays in C for all (6) if the control input ut could be involved in some optimal

3369
Authorized licensed use limited to: Shanghai Univ of Electric Power. Downloaded on April 01,2024 at 03:17:37 UTC from IEEE Xplore. Restrictions apply.
control problem, which allows us to choose an appropriate Algorithm 1 iMPC-DHOCBF
order for the DCBF constraint to reduce the computation. In Input: System dynamics (2), candidate CBF constraint, obstacle
other words, the highest order for DCBF could be mcbf with configurations, initial state x(0).
mcbf ≤ m. We can simply define a ith order DCBF ψi (xt ) Output: Safety-critical optimal control for obstacle avoidance.
1: Set initial guess Ū00 = 0 at t = 0.
in (4) as 2: Propagate with system dynamics to get initial guess of states
X̄00 from initial state x(0).
ψi (xt ) := △ψi−1 (xt , ut ) + γi ψi−1 (xt ), (7) 3: for t ≤ tsim − 1 do
4: Initialize j = 0.
where 0 < γi ≤ 1, i ∈ {1, . . . , mcbf }. 5: while Iteration j (not converged OR j < jmax ) do
6: Linearize system dynamics / constraints with X̄jt , Ūjt .
The expression in (7) follows the format of the first order 7: Solve a convex finite-time constrained optimal control
DCBF proposed in [26] and could be used to define a problem (CFTOC) with linearized dynamics / constraints
DHOCBF with arbitrary relative degree. and get optimal values of states and inputs X∗,j ∗,j
t , Ut .
j+1 ∗,j j+1 ∗,j
8: X̄t = Xt , Ūt = Ut , j = j + 1
B. Model Predictive Control 9: end while
10: Extract optimized states and inputs X∗t = X∗,j ∗
t , Ut = Ut
∗,j

Consider the problem of regulating to a target state xr for from last iteration and extract u∗t from U∗t .
the discrete-time system (2) while making sure that safety is 11: Apply u∗t with respect to system dynamics (2) to get xt+1 =
guaranteed by ensuring ψ0 (xt ) = h(xt ) ≥ 0. The following f (xt , u∗t ), and record x(t + 1) = xt+1 .
12: Update Ū0t+1 with U∗t and propagate to calculate X̄0t+1 .
optimal control problem takes future N states into account 13: t = t + 1.
as prediction at each time step t: 14: end for
15: return closed-loop trajectory [x(0), . . . , x(tsim )]
NMPC-DCBF:
N
X −1
min p(xt,N ) + q(xt,k , ut,k , ωt,k ) (8a)
Ut ,Ωt
k=0
A. Iterative Convex Optimization
s.t. xt,k+1 = f (xt,k , ut,k ), k={0, . . . , N −1} (8b) The algorithm described in Alg. 1 contains an iterative
ut,k ∈ U , xt,k ∈ X , ωt,k ∈ R, k={0, . . . , N −1} (8c) optimization at each time step t, which is denoted as
h(xt,k+1 ) ≥ ωt,k (1 − γ)h(xt,k ), 0 < γ ≤ 1, (8d) iterative MPC-DHOCBF (iMPC-DHOCBF). Our iterative
k = {0, . . . , N −1}, optimization problem contains three parts for each iteration
j: (1) solve a convex finite-time optimal control (CFTOC)
problem with linearized dynamics and DHOCBF, (2) check
where xt,k+1 denotes the state at time step k + 1 predicted convergence criteria, (3) update state and input vectors for
at time step t obtained by applying the input vector ut,k next iteration. Notice that the open-loop trajectory with
to the state xt,k . In (8a), q(·) and p(·) denote stage and updated states X̄jt = [x̄jt,0 , . . . , x̄jt,N −1 ] and inputs Ūjt =
terminal costs, respectively, and ωt,k is a slack variable. [ūjt,0 , . . . , ūjt,N −1 ] is passed between iterations, which al-
The discrete-time dynamics is represented by (8b) and the lows iterative linearization for both system dynamics and
constraints of state and control input along the horizon are DHOCBF locally. As discussed before, “high-order” implies
captured by (8c). The DCBF constraint in (8d) is proposed that the relative degree should be larger or equal to one.
in [26] and is designed to ensure the forward invariance The iteration is finished when the convergence error func-
of the set C based on (1). The above formulation was first tion e(X∗,j ∗,j j j
t , Ut , X̄t , Ūt ) is within a user-defined normal-
proposed in [3], and then later generalized in [4], where the ized convergence criteria, where X∗,j t = [x∗,j ∗,j
t,0 , . . . , xt,N ],
decay rate (1 − γ) of the CBF was relaxed by slack variable ∗,j ∗,j ∗,j
Ut = [ut,0 , . . . , ut,N −1 ] represent optimized states and
ωt,k to enhance safety and feasibility. inputs at iteration j. To restrict the number of iterations, we
The optimal solution to (8) at time t is U∗t = limit j < jmax , where jmax denotes the maximum numbers
[ut,0 , . . . , u∗t,N −1 ] and Ω∗t = [ωt,0
∗ ∗ ∗
, . . . , ωt,N −1 ]. The first of iterations. Therefore, the iterative optimization stops when
∗
element of Ut is applied to (2) as the cost function reaches a local optimal minimum, whose
iteration number is denoted as jt,conv . The optimized states
xt+1 = f (xt , u∗t ) (9)
X∗t and inputs U∗t are passed to the iMPC-DHOCBF for-
to get the new state xt+1 . The constrained finite-time optimal mulation for the next time instant. At each time, we record
control problem (8) is solved at time step t+1, and all future the updated state propagated by the system dynamics with a
time steps based on the new state xt+1 , yielding a safety- given discretization time, which allows to extract the output
critical receding horizon control strategy. closed-loop trajectory with our proposed iMPC-DHOCBF.
B. Linearization of Dynamics
III. I TERATIVE C ONVEX MPC WITH DHOCBF
At iteration j, an improved vector ujt,k is considered by
In this section, we present an iterative convex MPC for linearizing the system around x̄jt,k , ūjt,k :
DCBF, which works for general DHOCBFs defined in Sec.
II-A. xjt,k+1 −x̄jt,k+1 =Aj (xjt,k −x̄jt,k ) + B j (ujt,k −ūjt,k ), (10)

3370
Authorized licensed use limited to: Shanghai Univ of Electric Power. Downloaded on April 01,2024 at 03:17:37 UTC from IEEE Xplore. Restrictions apply.
where 0 ≤ j < jmax ; k and j represent open-loop time step
and iteration indices, respectively. We also have
Aj = Dx f (x̄jt,k , ūjt,k ), B j = Du f (x̄jt,k , ūjt,k ), (11)
where Dx and Du denote the Jacobian of the system dynam-
ics f (x, u) with respect to the state x and the input u. This
approach allows to linearize the system at (x̄jt,k , ūjt,k ) locally
between iterations. The convex system dynamics constraints
are provided in (10) since all nominal vectors (x̄jt,k , ūjt,k ) in
current iteration are constant and constructed from previous Fig. 1: Linearization of DHOCBF: h∥ (xjt,k |x̃jt,k ) ≥ 0 represents
iteration j − 1. the linearized safe set locally and is colored in green. Note that
h∥ (xjt,k |x̃jt,k ) ≥ 0 guarantees h(xjt,k ) ≥ 0 (colored in blue plus
C. Linearization of DCBF & DHOCBF green), which ensures collision avoidance (outside the grey region).
In this section, we show how to linearize the DCBF up
to the highest order. At iteration j, in order to linearize
ψ̃0 (·), . . . , ψ̃mcbf −1 (·) are more conservative than the original
h(xjt,k ), an explicit line is projected in the state space to
forms ψ0 (·), . . . , ψmcbf −1 (·). This problem can occur when
the nearest point x̃jt,k on the boundary of the obstacle from the horizon is too large, or the linearization is too conser-
each state x̄jt,k . Note that x̄jt,k is the nominal state vector vative. In order to handle this issue, we introduce a slack
from iteration j − 1 for the linearization at iteration j, which variable ωt,k,i j
with a corresponding decay rate (1 − γi ):
means x̄jt,k = xj−1 t,k . The tangent line passing through the
j
nearest point x̃t,k is denoted as h∥ (xjt,k |x̃jt,k ). This allows us ψ̃i−1 (xjt,k+1 ) ≥ ωt,k,i
j
(1 − γi )ψ̃i−1 (xjt,k ), ωt,k,i
j
∈ R, (13)
to define a linearized safe set by h∥ (xjt,k |x̃jt,k ) ≥ 0, ∀t ∈ N j
where i ∈ {1, . . . , mcbf }. The slack variable ωt,k,i is se-
as shown in Fig. 1 by the green region. lected by minimizing a cost function term to satisfy DCBF
Remark 2. Note that x̃jt,k represents the optimized value constraints at initial condition at any time step [4].
of the minimum distance problem with distance function Another challenge induced by the DCBF linearization is
h(·) between x̄jt,k and safe set C. For common smooth and that the constraints in (13) could be non-convex, since now
j
differentiable CBFs, the expression of x̃jt,k as a function of ωt,k,i and xjt,k are both optimization variables. Note that
j j
x̄jt,k is explicit [34], [35]. For example, when h(·) describes ψ̃0 (xt,0 ) are constant, thus we can only place ωt,k,i in front
a l2 -norm function with the obstacle being a circular shape, of ψ̃0 (xjt,0 ) and move the other optimization variables to the
x̃jt,k is exactly the intersection point between x̄jt,k and the other side of the inequalities. This motivates us to provide the
center of the obstacle. Notice that x̃jt,k could be implicit following form for reformulating (13) as convex constraints:
for general elliptic calculations [36], but it could still be i
X
numerically approximated as the values of x̄jt,k known at ψ̃i−1 (xjt,k ) + Zν,i (1 − γi )k ψ̃0 (xjt,ν ) ≥
iteration j before the linearization. ν=1
(14)
j
The relative degree of h∥ (xjt,k |x̃jt,k )
with respect to system ωt,k,i Z0,i (1 − γi )k ψ̃0 (xjt,0 ),
j
(2) is still m when the relative degree of h(xjt,k ) is m. j ≤ jmax ∈ N+ , i ∈ {1, . . . , mcbf }, ωt,k,i ∈ R.
Thus, in order to guarantee safety with forward invariance In the above, Zν,i is a constant that can be obtained recur-
based on Thm. 1 and Rem. 1, two sufficient conditions need sively by reformulating ψ̃i−1 (·) back to ψ̃0 (·) given ν ∈
to be satisfied: (1) the sequence of linearized DHOCBF {0, .., i}. We define Zν,i as follows. When 2 ≤ i, ν ≤ i − 2,
ψ̃0 (·), . . . , ψ̃mcbf −1 (·) is larger or equal to zero at the initial we have
condition xt , and (2) the highest-order DCBF constraint
lmax
ψ̃mcbf (x) ≥ 0 is always satisfied, where ψ̃i (·) is defined as: X
Zν,i = [(γζ1 − 1)(γζ2 − 1) · · · (γζi−ν−1 − 1)]l ,
(15)
ψ̃0 (xjt,k ) :=h∥ (xjt,k |x̃jt,k ) l=1
(12) ζ1 < ζ2 < · · · < ζi−ν−1 , ζs ∈ {1, 2, . . . , i − 1},
ψ̃i (xjt,k ) :=ψ̃i−1 (xjt,k+1 )−ψ̃i−1 (xjt,k )+γi ψ̃i−1 (xjt,k ).
where [·]l denotes the lth combination of the product of the
Here, we have 0 < γi ≤ 1, i ∈ {1, . . . , mcbf }, and mcbf ≤ m i−1


elements in parenthesis, therefore we have lmax = i−ν−1 .
(as in (7)).
ζs denote all ζ in (15). For the case ν = i − 1, if 2 ≤ i, we
Remark 3. From Rem. 1, it follows that that mcbf is not define Zν,i = −1; if i = 1, we define Zν,i = 1. Beside that,
necessarily equal to m. A detailed discussion on this can be we define Zν,i = 0 for the case ν = i.
found in [4], [28].
Remark 4. The decay rate in (14) used by the iMPC-
An important issue is feasibility. It is possible that DHOCBF is partially relaxed compared to the one in (13)
ψi (x0t,k ) ≥ 0, 1 ≤ i ≤ mcbf − 1, with k ∈ {0, . . . , N } due to the requirement of the linearization. This can affect
is not satisifed since the linearized DHOCBF functions the feasibility of the optimization.

3371
Authorized licensed use limited to: Shanghai Univ of Electric Power. Downloaded on April 01,2024 at 03:17:37 UTC from IEEE Xplore. Restrictions apply.
D. CFTOC Problem where xt = [xt , yt , θt , vt ]T captures the 2-D location, head-
In Secs. III-B and III-C, we have illustrated the lineariza- ing angle, and linear speed; ut = [u1,t , u2,t ]T represents
tion of system dynamics as well as the safety constraints angular velocity (u1 ) and linear acceleration (u2 ), respec-
with DHOCBF. This allows us to consider them as con- tively. The system is discretized with ∆t = 0.1. System (17)
straints into a convex MPC formulation at each iteration, is subject to the following state and input constraints:
which we call convex finite-time constrained optimization X = {xt ∈ R4 : −10 · I4×1 ≤ xt ≤ 10 · I4×1 },
control (CFTOC). This is solved at iteration j with op- (18)
U = {ut ∈ R2 : [−7, −5]T ≤ ut ≤ [7, 5]T }.
timization variables Ujt = [ujt,0 , . . . , ujt,N −1 ] and Ωjt,i =
j j
[ωt,0,i , . . . , ωt,N,i ], where i ∈ {1, . . . , mcbf }. 2) System Configuration: The initial state is [−3, 0, 0, 0]T
and the target state is xr = [3, 0.01, 0, 0]T , which are marked
CFTOC of iMPC-DHOCBF at iteration j: as blue and red diamonds in Fig. 2. The circular obstacle
N
X −1 is centered at (0, 0) with r = 1,, which is displayed in
min p(xjt,N ) + q(xjt,k , ujt,k , ωt,k,i
j
) (16a) orange. The other reference vectors are ur = [0, 0]T and
j j j
Ut ,Ωt,1 ,...,Ωt,m
cbf k=0
ωr = [1, 1]T . We use the offset y = 0.01m in xr to prevent
s.t. xjt,k+1 −x̄jt,k+1 =Aj (xjt,k − x̄jt,k )+B j (ujt,k − ūjt,k ), (16b) singularity of the optimization problem.
ujt,k ∈ U , xjt,k ∈ X , ωt,k,i
j
∈ R, (16c) 3) DHOCBF: As a candidate DHOCBF function ψ0 (xt ),
i
X we choose a quadratic distance function for circular obstacle
ψ̃i−1 (xjt,k ) + Zν,i (1 − γi )k ψ̃0 (xjt,ν ) ≥ avoidance h(xt ) = (xt − x0 )2 + (yt − y0 )2 − r2 , where
ν=1 (x0 , y0 ) and r denote the obstacle center location and radius,
respectively. The linearized DHOCBF ψ̃0 (xjt,k ) in (12) is
j
ωt,k,i Z0,i (1 − γi )k ψ̃0 (xjt,0 ), (16d)
defined as ψ̃0 (xjt,k ) := h∥ (xjt,k |x̃jt,k ), with
In the CFTOC, the linearized dynamics constraints in (10) h∥ (xjt,k |x̃jt,k ) = (x̃jt,k − x0 )xjt,k + (ỹt,k
j j
− y0 )yt,k
and the linearized DHOCBF constraints in (14) are enforced (19)
with constraints (16b) and (16d) at each open loop time −(r2 − x20 − y02 + x̃jt,k x0 + ỹt,k
j
y0 ),
step k ∈ {0, . . . , N − 1}. The state and input constraints are
where h∥ (xjt,k |x̃jt,k ) is the linearized boundary, whose rel-
considered in (16c). The slack variables are unconstrained
as the goal of the optimization itself is to minimize the ative degree is 2; (x̃jt,k , ỹt,k
j
) denotes the tangent point of
deviation from the nominal DHOCBF constraints with the circular boundary h(xt ). From (15), we have Z0,2 =
j
cost term q(·, ·, ωt,k,i ), while ensuring feasibility of the γ1 − 1, Z1,2 = −1, Z0,1 = 1, Z2,2 = Z1,1 = 0.
optimization, as discussed in [32]. Note that, for ensuring 4) MPC Design: The cost function of thePMPC problem
N −1
the safety guarantee established by the DHOCBF, the consists of stage cost q(xjt,k , ujt,k , ωt,k
j
) = k=0 (||xjt,k −
j j
constraints (16d) are enforced with i ∈ {0, . . . , mcbf }, where xr ||2Q + ||ut,k − ur ||2R + ||ωt,k − ωr ||2S ) and terminal cost
Zν,i ∈ R is as defined in (14) with ν ∈ {0, .., i}. The p(xjt,N ) = ||xjt,N − xr ||2P , where Q = P = 10 · I4 , R = I2
optimal decision variables of (16) at iteration j is a list of and S = 1000 · I2 .
control input vectors as U∗,j ∗,j ∗,j
t = [ut,0 , . . . , ut,N −1 ] and a list 5) Convergence Criteria: We use the following absolute
∗,j ∗,j ∗,j and relative convergence functions as convergence criteria
of slack variable vectors as Ωt,i = [ωt,0,i , . . . , ωt,N −1,i ].
The CFTOC is solved iteratively in our proposed iMPC- mentioned in Alg. 1:
DHOCBF and the solution can be extracted once the eabs (X∗,j ∗,j ∗,j
− X̄∗,j ||
t , Ut ) = ||X
convergence criteria or the maximum iteration number jmax (20)
is reached, as shown in Alg. 1. erel (X∗,j ∗,j j j
t , Ut , X̄t , Ūt ) = ||X
∗,j
− X̄∗,j ||/||X̄∗,j ||.
The iterative optimization stops when eabs < εabs or erel <
IV. C ASE S TUDY
εrel , where εabs = 10−4 , εrel = 10−2 and the maximum
In this section, we present numerical results to validate iteration number is set as jmax = 1000.
our proposed approach using a unicycle model. We provide a To make a fair comparison with NMPC-DHOCBF, the
performance comparison with the baseline NMPC-DHOCBF hyperparameters P, Q, R, S remain unchanged for all setups.
approach. The NMPC-DHOCBF is simply extended by using 6) Solver Configurations and CPU Specs.: For iMPC-
relaxed DHOCBF based on (8d) in NMPC-DCBF (8), as DHOCBF, we used OSQP [37] to solve the convex opti-
discussed in [4, Rem. 4]. mizations at all iterations. The baseline approach NMPC-
A. Numerical Setup DHOCBF is open-source, and was solved using IPOPT [38]
with the modeling language Yalmip [39]. We used a Win-
1) System Dynamics: Consider a discrete-time unicycle dows desktop with Intel Core i7-8700 (CPU 3.2 GHz)
model in the form running Matlab for all computations.
     
xt+1 −xt vt cos(θt )∆t 0 0 
 yt+1 −yt   vt sin(θt )∆t   0 0
 B. Performance
 = +  u1,t , (17)
 θt+1 −θt   0  ∆t 0  u2,t 1) Iterative Convergence: The iterative convergence is
vt+1 −vt 0 0 ∆t shown in Figs. 2a, 4 and 3. Fig. 2a shows the closed-loop

3372
Authorized licensed use limited to: Shanghai Univ of Electric Power. Downloaded on April 01,2024 at 03:17:37 UTC from IEEE Xplore. Restrictions apply.
 (a) iMPC-DHOCBF when N = 24, (b) iMPC-DHOCBF with mcbf = 2. (c) NMPC-DHOCBF with mcbf = 2. (d) iMPC-DHOCBF and NMPC-
γ1 = γ2 = 0.4. DHOCBF with mcbf = 1.
Fig. 2: Open-loop and closed-loop trajectories with controllers iMPC-DHOCBF (solid lines) and NMPC-DHOCBF (dashed lines): (a)
several open-loop trajectories at different iterations predicted at t = 6 and one closed-loop trajectory with controller iMPC-DHOCBF;
(b) closed-loop trajectories with controller iMPC-DHOCBF with different choices of N and γ; (c) closed-loop trajectories with controller
NMPC-DHOCBF with different choices of N and γ. Note that two trajectories stop at t = 13 and t = 33 because of infeasibility; (d)
closed-loop trajectories with controllers iMPC-DHOCBF and NMPC-DHOCBF with mcbf = 1. Both methods work well for safety-critical
navigation.

(a) Location x (b) Location y (c) Orientation θ (d) Speed v

Fig. 3: Iterative convergence of all states at converged iteration j6,conv = 32 with N = 24, mcbf = 2, γ1 = γ2 = 0.4. iMPC does help to
optimize the cost function to reach local optimal minimum.

trajectory (the black line) generated by solving the iMPC- Based on black, red, blue and magenta lines with the highest
DHOCBF until the converged iteration jt,conv from t = 0 order of CBF constraint mcbf = 2 in Fig. 2b and 2c, as
to t = tsim = 100 and open-loop iteratively converging γ1 , γ2 become smaller, the system tends to turn further away
trajectories (colored lines) at different iterations at t = 6. from the obstacle when it is getting closer to obstacle, which
Fig. 3 presents more details on the iterative convergence of indicates a safer control strategy. From the lines in Fig. 2d
states at different iterations at t = 6 with number of iterations where mcbf = 1, we can see that the system can still safely
jt,conv = 32. We note that, after around 10 iterations, the navigate around the obstacle, although it turns away from the
converging lines for the states (red lines) nearly overlap obstacle later than when having one more CBF constraint in
with the converged line (blue line) in Fig. 3. This verifies Fig. 2b and 2c, indicating that having CBF constraints up to
the relations of the converging trajectory (red line) and the the relative degree enhances safety. The blue and magenta
converged trajectory (blue line) in Fig. 2a. The optimization dashed lines in Fig. 2c stop at t = 33 and t = 13 with N =
is shown to converge at iteration jt,conv at time step t 16 as infeasibility happens, which shows that a large horizon
for different hyperparameters γ under specific convergence is needed to generate complete closed-loop trajectories for
criteria (20), shown in Fig. 4. We can see that for the first some hyperparameters by NMPC-DHOCBF, while iMPC-
15 time steps the iMPC-DHOCBF triggers more iterations to DHOCBF shows less reliance on selection of horizon since
drive the system to avoid the obstacle than time steps after 20 it can generate complete closed-loop trajectories with both
where the system already passes the obstacle. The maximum N = 16 and N = 24, as shown in Fig. 2b.
converged iteration jt,conv is 1000 at time step t = 2 in Fig. 3) Computation Time: In order to compare computational
4d with γ1 = γ2 = 0.6, which reveals that the peak of the times between our proposed iMPC-DHOCBF and the base-
converged iteration over time increases if we choose larger γ. line NMPC-DHOCBF, 1000 independent randomized safe
For the majority of the time-steps, the iterative optimization states are generated in state constraint X in (18). To make
converges within 100 iterations (jt,conv < 100). a fair comparison, both approaches use the same N and
2) Convergence with Different Hyperparameters: Fig. 2b, mcbf and the computational time and feasibility are evaluated
2c and 2d show the closed-loop trajectories generated by at those randomized sample states. The distributions of the
solving iMPC-DHOCBF (solid lines) and NMPC-DHOCBF computation times and infeasibility rates in Tab. I and Tab.
(dashed lines) at converged iteration jt,conv from t = 0 II correspond to generating one time-step trajectories. The
to t = tsim = 45 with different hyperparameters. Both mean and standard deviation of computation times increase
controllers show good performance on obstacle avoidance. if the horizon N or mcbf become larger for NMPC-DHOCBF

3373
Authorized licensed use limited to: Shanghai Univ of Electric Power. Downloaded on April 01,2024 at 03:17:37 UTC from IEEE Xplore. Restrictions apply.
 (a) γ1 = 0.4, γ2 = 0.4 (b) γ1 = 0.4, γ2 = 0.6 (c) γ1 = 0.6, γ2 = 0.4 (d) γ1 = 0.6, γ2 = 0.6
Fig. 4: Number of iterations jt,conv at each time-step using controller iMPC-DHOCBF with different values of hyperparameters γ1 , γ2 with
N = 24, mcbf = 2. We can observe that, for almost all time-steps, the iterative optimization converges within 100 iteraitons (jt,conv < 102 ),
which is affected very little with respect to hyperparameters.

Approaches N =4 N =8 N = 12 N = 16 N = 20 N = 24
NMPC-DHOCBF mean / std (s) 3.687 ± 6.360 23.882 ± 17.988 27.329 ± 20.115 28.953 ± 22.058 30.970 ± 23.564 29.929 ± 22.105
(mcbf = 2) infeas. rate 5.8% 27.5% 21.1% 16.4% 14.5% 14.4%
NMPC-DHOCBF mean / std (s) 2.933 ± 4.678 19.077 ± 14.024 20.418 ± 15.401 22.749 ± 17.039 24.053 ± 17.811 25.365 ± 18.211
(mcbf = 1) infeas. rate 6.3% 13.9% 13.0% 14.6% 13.8% 15.4%
iMPC-DHOCBF mean / std (s) 0.135 ± 0.294 0.104 ± 0.242 0.102 ± 0.217 0.131 ± 0.301 0.165 ± 0.400 0.135 ± 0.274
(mcbf = 2) infeas. rate 6.3% 8.0% 10.4% 10.9% 10.9% 10.2%
iMPC-DHOCBF mean / std (s) 0.131 ± 0.286 0.114 ± 0.260 0.109 ± 0.237 0.137 ± 0.316 0.173 ± 0.414 0.152 ± 0.317
(mcbf = 1) infeas. rate 6.3% 8.0% 10.4% 10.9% 10.9% 11.1%

TABLE I: Statistical benchmark for computation time and feasibility between NMPC-DHOCBF and iMPC-DHOCBF with randomized
states. The target position is shared among four approaches and the hyperparameters are fixed as γ1 = γ2 = 0.4 for all random scenarios.

Approaches N =4 N =8 N = 12 N = 16 N = 20 N = 24
NMPC-DHOCBF mean / std (s) 3.744 ± 6.445 28.779 ± 20.755 31.319 ± 21.921 33.678 ± 25.328 36.430 ± 26.959 39.543 ± 29.941
(mcbf = 2) infeas. rate 5.6% 28.0% 20.9% 16.8% 17.0% 14.6%
NMPC-DHOCBF mean / std (s) 3.032 ± 4.536 21.414 ± 16.518 23.121 ± 17.544 24.011 ± 17.711 26.599 ± 19.480 29.671 ± 20.026
(mcbf = 1) infeas. rate 6.4% 17.0% 15.2% 15.5% 16.7% 13.2%
iMPC-DHOCBF mean / std (s) 0.158 ± 0.326 0.134 ± 0.279 0.163 ± 0.353 0.163 ± 0.373 0.184 ± 0.398 0.164 ± 0.344
(mcbf = 2) infeas. rate 6.1% 8.0% 10.2% 10.7% 10.8% 10.8%
iMPC-DHOCBF mean / std (s) 0.167 ± 0.340 0.139 ± 0.291 0.170 ± 0.362 0.170 ± 0.379 0.201 ± 0.435 0.176 ± 0.378
(mcbf = 1) infeas. rate 6.1% 8.0% 10.2% 10.7% 10.8% 10.8%

TABLE II: Statistical benchmark between NMPC-DHOCBF and iMPC-DHOCBF with the same randomized states as in Tab. I. The target
position is shared among four approaches and the hyperparameters are fixed as γ1 = γ2 = 0.6 for all scenarios. Based on Tab. I and Tab.
II we conclude that iMPC-DHOCBF outperforms NMPC-DHOCBF in computing time and infeasibility rate.

in Tab. I and Tab. II. Different from NMPC-DHOCBF, the obviously limits its feasibility if the number of horizon is
computing time is not heavily influenced by N and mcbf large. Besides, NMPC-DHOCBF is equipped with relaxed
for iMPC-DHOCBF. Based on the data from the two tables, nonlinear CBF constraints (13), while iMPC-DHOCBF has
we also notice that larger hyperparameter values for γ will relaxed linear CBF constraints (14). The linearization of the
slightly reduce the computation speed for both methods, CBF constraints reduces the feasibility region in the state
which is discussed in Sec. IV-B.1 and can be attributed to space, as illustrated in Fig. 1. This meets the expectation
the rise of converged iteration jt,conv . Compared to NMPC- of slight decreased feasibility rate of iMPC-DHOCBF when
DHOCBF, the computing speed of our proposed method number of horizon is small. However, we can see that
is much faster with the improvement in computation time the decline in feasibility rate due to relaxed technique is
directly proportional to the horizon, e.g., 100 ∼ 300 times noticeably outperformed by flexible convergence criteria with
faster than the baseline given the chosen hyperparameters. larger number of horizon N in Tab. I and II, which validates
our linearization technique in the iterative optimization.
4) Optimization Feasibility: The rate of infeasibility in-
creases if the horizon N increases or mcbf is lower for V. C ONCLUSION & F UTURE W ORK
iMPC-DHOCBF. However, these two hyperparameters are We proposed an iterative convex optimization procedure
shown not to affect the infeasibility rate of the NMPC- for safety-critical model predictive control (iMPC) design.
DHOCBF method proportionally. As the horizon increases, Central to our approach are relaxations for the system
the infeasibility rate of iMPC-DHOCBF outperforms that dynamics and for discrete time high-order control barrier
of NMPC-DHOCBF. The main reasons for this come from functions (DHOCBF) in the form of linearized constraints.
the difference in the convergence criteria and relaxation We validated the proposed iMPC-DHOCBF approach by
techniques for CBF constraints, discussed in Rem. 4. The applying it to a model of unicycle navigating in an environ-
NMPC-DHOCBF under IPOPT should have more strict ment with obstacles. We noticed that the computation times
convergence criteria compared to iMPC-DHOCBF, which for the iMPC-DHOCBF method significantly outperform the

3374
Authorized licensed use limited to: Shanghai Univ of Electric Power. Downloaded on April 01,2024 at 03:17:37 UTC from IEEE Xplore. Restrictions apply.
ones corresponding to the baseline, usually with even higher [17] W. Xiao, C. Belta, and C. G. Cassandras, “Adaptive control barrier
feasibility rate. There are still some limitations of iMPC- functions,” IEEE Transactions on Automatic Control, vol. 67, no. 5,
pp. 2267–2281, 2021.
DHOCBF that could be ameliorated. One limitation of the [18] Q. Nguyen and K. Sreenath, “Optimal robust control for bipedal
proposed method is its linearly relaxed technique will slightly robots through control lyapunov function based quadratic programs,”
increase infeasibility rate with small size of the horizon. in Robotics: Science and Systems (RSS), Rome, Italy, July 2015.
[19] M. Jankovic, “Robust control barrier functions for constrained sta-
Another limitation is that the feasibility of the optimization bilization of nonlinear systems,” Automatica, vol. 96, pp. 359–367,
and system safety are not always guaranteed at the same time 2018.
in the whole state space. We will address these limitations [20] A. Clark, “Control barrier functions for stochastic systems,” Automat-
ica, vol. 130, p. 109688, 2021.
in future work with better linearization, different relaxed [21] C. Dawson, Z. Qin, S. Gao, and C. Fan, “Safe nonlinear control using
techniques as well as adaptive warm-up and convergence robust neural lyapunov-barrier functions,” in Conference on Robot
criterion for the optimization problem. Learning. PMLR, 2022, pp. 1724–1735.
[22] Q. Nguyen and K. Sreenath, “Exponential control barrier functions
R EFERENCES for enforcing high relative-degree safety-critical constraints,” in 2016
American Control Conference (ACC), 2016, pp. 322–328.
[1] A. D. Ames, J. W. Grizzle, and P. Tabuada, “Control barrier function [23] W. Xiao and C. Belta, “High-order control barrier functions,” IEEE
based quadratic programs with application to adaptive cruise control,” Transactions on Automatic Control, vol. 67, no. 7, pp. 3655–3662,
in 53rd IEEE Conference on Decision and Control, 2014, pp. 6271– 2022.
6278. [24] X. Tan, W. S. Cortez, and D. V. Dimarogonas, “High-order barrier
[2] A. D. Ames, X. Xu, J. W. Grizzle, and P. Tabuada, “Control barrier functions: Robustness, safety, and performance-critical control,” IEEE
function based quadratic programs for safety critical systems,” IEEE Transactions on Automatic Control, vol. 67, no. 6, pp. 3021–3028,
Transactions on Automatic Control, vol. 62, no. 8, pp. 3861–3876, 2021.
2016. [25] J. Usevitch and D. Panagou, “Adversarial resilience for sampled-
[3] J. Zeng, B. Zhang, Z. Li, and K. Sreenath, “Safety-critical control data systems under high-relative-degree safety constraints,” IEEE
using optimal-decay control barrier function with guaranteed point- Transactions on Automatic Control, pp. 1–1, 2022.
wise feasibility,” in 2021 American Control Conference (ACC), 2021, [26] A. Agrawal and K. Sreenath, “Discrete control barrier functions
pp. 3856–3863. for safety-critical control of discrete systems with application to
[4] J. Zeng, Z. Li, and K. Sreenath, “Enhancing feasibility and safety of bipedal robot navigation.” in Robotics: Science and Systems, vol. 13.
nonlinear model predictive control with discrete-time control barrier Cambridge, MA, USA, 2017.
functions,” in 2021 60th IEEE Conference on Decision and Control [27] R. Grandia, A. J. Taylor, A. D. Ames, and M. Hutter, “Multi-
(CDC), 2021, pp. 6137–6144. layered safety for legged robots via control barrier functions and
[5] M. Diehl, H. G. Bock, and J. P. Schlöder, “A real-time iteration scheme model predictive control,” in 2021 IEEE International Conference on
for nonlinear optimization in optimal feedback control,” SIAM Journal Robotics and Automation (ICRA), 2021, pp. 8352–8358.
on control and optimization, vol. 43, no. 5, pp. 1714–1736, 2005. [28] H. Ma, X. Zhang, S. E. Li, Z. Lin, Y. Lyu, and S. Zheng, “Fea-
[6] G. Paolo, I. Ferrara, and L. Magni, “Mpc for robot manipulators sibility enhancement of constrained receding horizon control using
with integral sliding mode generation,” IEEE/ASME Transactions on generalized control barrier function,” in 2021 4th IEEE International
Mechatronics, vol. 22, no. 3, pp. 1299–1307, 2017. Conference on Industrial Cyber-Physical Systems (ICPS), 2021, pp.
[7] N. Scianca, D. De Simone, L. Lanari, and G. Oriolo, “Mpc for 551–557.
humanoid gait generation: Stability and feasibility,” IEEE Transactions [29] Y. Xiong, D.-H. Zhai, M. Tavakoli, and Y. Xia, “Discrete-time control
on Robotics, vol. 36, no. 4, pp. 1171–1188, 2020. barrier function: High-order case and adaptive case,” IEEE Transac-
[8] R. Grandia, A. J. Taylor, A. Singletary, M. Hutter, and A. D. Ames, tions on Cybernetics, pp. 1–9, 2022.
“Nonlinear model predictive control of robotic systems with control [30] S. He, J. Zeng, and K. Sreenath, “Autonomous racing with multiple
lyapunov functions,” in Proceedings of Robotics: Science and Systems, vehicles using a parallelized optimization with safety guarantee us-
2022. ing control barrier functions,” in 2022 International Conference on
[9] T. D. Son and Q. Nguyen, “Safety-critical control for non-affine Robotics and Automation (ICRA), 2022, pp. 3444–3451.
nonlinear systems with application on autonomous vehicle,” in 2019 [31] Z. Li, J. Zeng, A. Thirugnanam, and K. Sreenath, “Bridging model-
IEEE 58th Conference on Decision and Control (CDC), 2019, pp. based safety and model-free reinforcement learning through system
7623–7628. identification of low dimensional linear models,” in Proceedings of
[10] J. M. Eklund, J. Sprinkle, and S. S. Sastry, “Switched and symmetric Robotics: Science and Systems, 2022.
pursuit/evasion games using online model predictive control with [32] J. Zeng, B. Zhang, Z. Li, and K. Sreenath, “Safety-critical control
application to autonomous aircraft,” IEEE Transactions on Control using optimal-decay control barrier function with guaranteed point-
Systems Technology, vol. 20, no. 3, pp. 604–620, 2012. wise feasibility,” in 2021 American Control Conference (ACC), 2021,
[11] J. V. Frasch, A. Gray, M. Zanon, H. J. Ferreau, S. Sager, F. Borrelli, pp. 3856–3863.
and M. Diehl, “An auto-generated nonlinear mpc algorithm for real- [33] M. Sun and D. Wang, “Initial shift issues on discrete-time iterative
time obstacle avoidance of ground vehicles,” in 2013 European learning control with system relative degree,” IEEE Transactions on
Control Conference (ECC), 2013, pp. 4136–4141. Automatic Control, vol. 48, no. 1, pp. 144–148, 2003.
[12] A. Liniger, A. Domahidi, and M. Morari, “Optimization-based au- [34] A. Thirugnanam, J. Zeng, and K. Sreenath, “Duality-based convex
tonomous racing of 1: 43 scale rc cars,” Optimal Control Applications optimization for real-time obstacle avoidance between polytopes with
and Methods, vol. 36, no. 5, pp. 628–647, 2015. control barrier functions,” in 2022 American Control Conference
[13] X. Zhang, A. Liniger, and F. Borrelli, “Optimization-based colli- (ACC), 2022, pp. 2239–2246.
sion avoidance,” IEEE Transactions on Control Systems Technology, [35] ——, “Safety-critical control and planning for obstacle avoidance be-
vol. 29, no. 3, pp. 972–983, 2020. tween polytopes with control barrier functions,” in 2022 International
[14] K. Galloway, K. Sreenath, A. D. Ames, and J. W. Grizzle, “Torque sat- Conference on Robotics and Automation (ICRA), 2022, pp. 286–292.
uration in bipedal robotic walking through control lyapunov function- [36] I. V. Skrypnik, Methods for analysis of nonlinear elliptic boundary
based quadratic programs,” IEEE Access, vol. 3, pp. 323–332, 2015. value problems. American Mathematical Soc., 1994, vol. 139.
[15] A. D. Ames, S. Coogan, M. Egerstedt, G. Notomista, K. Sreenath, [37] B. Stellato, G. Banjac, P. Goulart, A. Bemporad, and S. Boyd, “Osqp:
and P. Tabuada, “Control barrier functions: Theory and applications,” An operator splitting solver for quadratic programs,” Mathematical
in 2019 18th European Control Conference (ECC), 2019, pp. 3420– Programming Computation, vol. 12, no. 4, pp. 637–672, 2020.
3431. [38] L. T. Biegler and V. M. Zavala, “Large-scale nonlinear programming
[16] Q. Nguyen, A. Hereid, J. W. Grizzle, A. D. Ames, and K. Sreenath, “3d using ipopt: An integrating framework for enterprise-wide dynamic
dynamic walking on stepping stones with control barrier functions,” optimization,” Computers & Chemical Engineering, vol. 33, no. 3,
in 2016 IEEE 55th Conference on Decision and Control (CDC), 2016, pp. 575–582, 2009.
pp. 827–834. [39] J. Lofberg, “Yalmip: A toolbox for modeling and optimization in
matlab,” in 2004 IEEE international conference on robotics and
automation (IEEE Cat. No. 04CH37508), 2004, pp. 284–289.
3375
Authorized licensed use limited to: Shanghai Univ of Electric Power. Downloaded on April 01,2024 at 03:17:37 UTC from IEEE Xplore. Restrictions apply.