Safety-Critical End-Effector Formation Control for Planar Underactuated Manipulators

Abstract

While networked multi-agent systems have been widely explored, the challenges introduced by underactuation still impede safety-critical cooperative control of multiple underactuated manipulators. This paper introduces a distributed framework for end-effector formation control and obstacle avoidance in planar n-link manipulators with a passive first joint and active remaining joints—termed PAⁿ⁻¹ manipulators. By exploiting the integrability of each PAⁿ⁻¹ manipulator’s second-order nonholonomic constraint, we reformulate the dynamics into a cascaded structure and derive a reduced-order model driven solely by active joint velocities. Building on this reduced-order model, we design safety-critical distributed formation control laws for the reduced-order dynamics, which serve as the manipulators’ desired active joint velocities. Then, we employ the backstepping method to obtain control inputs for the full-order dynamics. To guarantee safety, we treat backstepping tracking errors as matched disturbances and address them within a robust control barrier function framework. Numerical simulations and comparative studies confirm the effectiveness of the proposed approach.

1. Introduction

Formation control for networked multi-agent systems has attracted considerable attention over the past decade [1,2,3,4], driven by applications ranging from unmanned aerial vehicles to autonomous surface vessels. Early works focused primarily on simple agent models—such as single- or double-integrator kinematic points—where each agent’s control input directly influences the state variables that define the collective formation. To bridge the gap between these results and the more complex dynamics, Wu et al. [5] introduced the distributed end-effector formation control for multiple cooperative manipulators, in which the formation is defined by end-effector positions whereas the control inputs act at the joint level.

Despite its promising contributions, the approach proposed in Ref. [5] overlooks two realistic and critical challenges inherent to multi-agent systems. First, actuator faults represent a significant issue, particularly in multi-agent systems where such faults occur frequently and pose substantial risks. These faults can severely compromise system performance; in extreme cases, manipulators may lose control authority over one or more joints, thus becoming underactuated—meaning the manipulator has fewer independent control inputs than degrees of freedom. The presence of the underactuated manipulators in the group will make the method in [5] inapplicable. Second, formation control is often applied to multi-manipulator teams performing complex tasks in cluttered, obstacle-rich environments—such as hazardous-material handling or robotic surgery. Therefore, guaranteeing collision-free coordination at all times is vital for safe operation.

Several studies have addressed actuator faults [6,7,8] and collision avoidance [9,10] within multi-agent systems. However, these works mainly deal with milder actuator faults, typically modeled as multiplicative or additive perturbations of available inputs, where all degrees of freedom remain controllable. By contrast, this study focuses on severe joint faults that disable torque production at some joints, removing input channels and leading to structural underactuation rather than input deviations. In our earlier work [11,12], we established distributed end-effector formation control for multiple underactuated planar manipulators. Nonetheless, that investigation was limited to two-link manipulators and did not account for obstacle avoidance. Recently, control barrier functions (CBFs) have emerged as powerful tools for enforcing safety constraints in real time [13,14,15]. However, constructing suitable CBFs for high-order dynamical systems, particularly for underactuated manipulators, remains challenging due to issues related to the relative degree of the functions. For fully actuated manipulators, a standard approach is to transform the dynamics into a cascaded form and then construct CBFs via the reduced-order model [15,16]. However, underactuated manipulators do not have independent actuation at all joints, which leads to force coupling between active and passive joints. This coupling gives rise to second-order nonholonomic constraints—second-order differential equations in the joint angles—within the system dynamics. Consequently, the dynamics of underactuated manipulators generally cannot be transformed into a cascaded form composed of multiple first-order subsystems, which prevents the direct extension of the aforementioned CBF-based methods.

Motivated by these gaps, this study investigates the safety-critical end-effector formation control of a team of planar n-link underactuated manipulators under explicit safety constraints, simultaneously addressing underactuation and obstacle avoidance. In each manipulator, the first joint is passive and the remaining n − 1 joints are actuated, a configuration referred to as the PAⁿ⁻¹ manipulator. Our main contributions are as follows.

As noted earlier, a key challenge is that the safety constraint on the end-effector position has relative degree greater than one with respect to the torque inputs, resulting in a relative-degree mismatch for CBF-based synthesis. To cope with this, we aim to reformulate the underactuated manipulator dynamics into a cascaded form of first-order subsystems and derive a reduced-order model for each agent. However, the dynamics coupling, manifested as second-order nonholonomic constraints, prevents a direct transformation, unlike in fully actuated systems. In this study, we exploit the integrability of the second-order nonholonomic constraint of the PAⁿ⁻¹ manipulator [17,18]. By integrating the second-order constraint on the joint accelerations into a first-order kinematic constraint, we decouple the second-order and first-order terms present in the dynamics, thereby reformulating the system into a cascaded form.

On the resulting cascaded representation, we can synthesize a CBF-based controller on the reduced-order subsystem to generate ideal active-joint velocity commands that satisfy safety constraints. Backstepping [19,20,21] is then applied to the upper-level dynamics to design a tracking controller that compels the upper-level output to follow the prescribed velocities. However, directly applying a standard backstepping procedure inevitably introduces tracking errors, which could lead to violations of prescribed safety constraints. To mitigate this risk, we model the tracking error as a matched disturbance and employ the robust CBF (RCBF) framework proposed in [15]. Specifically, we conservatively shrink the original safe set by an amount proportional to the worst-case disturbance magnitude, thereby ensuring the system state remains strictly within the true safe region despite the presence of disturbances. Simulation results from two numerical case studies confirm the effectiveness of the proposed control scheme.

The remainder of this paper is organized as follows. In Section 2, we review the necessary background, including the dynamics model of the discussed underactuated manipulators and CBFs for obstacle avoidance. Section 3 formulates the problem statement. In Section 4, we present our main results: the distributed formation control design via cascaded reduction and the robust CBF-based obstacle-avoidance scheme. Section 5 provides numerical simulations and comparative studies. Finally, Section 6 concludes the paper and discusses potential research directions in the future.

2. Preliminaries

In this section, we briefly introduce the notation and review the essential background, including the dynamic model of the discussed underactuated PAⁿ⁻¹ manipulators and the CBF-based method for obstacle avoidance.

Notation: Define $\mathrm{col}(p_1,\dots ,p_N)={\left[p_1^T,\dots ,p_N^T\right]}^T$ as the stacked vector of the column vectors $p_i,i=\{1,\dots ,N\}$; define $blockdiag(M_1,\dots ,M_N)$ as the block-diagonal matrix whose i-th diagonal block is $M_i,\{i=1,\dots ,N\}$. Finally, let ${\left[M\right]}_{\alpha \beta }$ denote the entry of matrix M in its $\alpha$th row and $\beta$th column.

2.1. Dynamics of Underactuated Manipulators

Consider a group of robotic manipulators, each with n links, operating in a gravity-free $X-Y$ plane (see Figure 1). Each manipulator’s first joint is unactuated (e.g., due to actuator fault), while the remaining n − 1 joints are actuated. For the $\alpha$-th link ($\alpha \in \{1,\dots ,n\}$) of manipulator $i\in \{1,\dots ,N\}$, let $m_{i,\alpha }$ denote its mass, $I_{i,\alpha }$ its moment of inertia about its center of mass (CoM), $L_{i,\alpha }$ its length, and $l_{i,\alpha }$ the distance from joint $\alpha$ to its CoM. Denote the global frame by Σ_W and the local frame of manipulator i by Σ_i, with their relative rotation given by ${\varphi }_i$. Let $q_{i,\alpha },\alpha \in \{1,\dots ,n\}$ denote the generalized coordinate (e.g., joint angle) and ${\dot{q}}_{i,\alpha },{\ddot{q}}_{i,\alpha }$ its first and second derivatives; denote the corresponding link angle relative to the vertical axis of Σ_i as ${\theta }_{i,\alpha }={\sum }_{\gamma =1}^{\alpha }{q}_{i,\gamma }$. Then, define the generalized coordinate vector $q_i={[q_{i,1},\dots ,q_{i,n}]}^{\mathrm{T}}\in {\mathbb{R}}^n$ and the active-joint sub-vector $q_i^{\mathrm{a}}={[q_{i,2},\dots ,q_{i,n}]}^{\mathrm{T}}\in {\mathbb{R}}^{n-1}$. Let ${\tau }_{i,\alpha }$ be the torque at joint $\alpha$, and stack these as ${\tau }_i={[{\tau }_{i,1},\dots ,{\tau }_{i,n}]}^{\mathrm{T}}=col[0,{\tau }_i^{\mathrm{a}}]\in {\mathbb{R}}^n$, where ${\tau }_i^{\mathrm{a}}={[{\tau }_{i,2},\dots ,{\tau }_{i,n}]}^{\mathrm{T}}\in {\mathbb{R}}^{n-1}$ contains only the nontrivial ones.

Utilizing the dynamic formulation of the n-link manipulator from [22,23], the manipulator’s motion in a two-dimensional, gravity-free plane is described by

$$\begin{array}{cc}\hfill & M_i\left(q_i\right){\ddot{q}}_i+C_i\left(q_i,{\dot{q}}_i\right){\dot{q}}_i={\tau }_i,\hfill \\ \hfill & M_i\left(q_i\right)\in {\mathbb{R}}^{n\times n}=S^{\mathrm{T}}{\overline{M}}_{i}S,\hfill \\ \hfill & C_i\left(q_i,{\dot{q}}_i\right)\in {\mathbb{R}}^{n\times n}=S^{\mathrm{T}}{\overline{C}}_{i}S,\hfill \end{array}$$

(1)

where $M_i\left(q_i\right)$ is the symmetric positive-definite inertia matrix, and $C_i(q_i,{\dot{q}}_i){\dot{q}}_i$ represents Coriolis and centrifugal forces. Specifically, the matrix $S\in {\mathbb{R}}^{n\times n}$ is defined as

$$S\in {\mathbb{R}}^{n\times n}=\left[\begin{array}{ccccc}1& 0& \cdots & 0& 0\\ 1& 1& \cdots & 0& 0\\ ⋮& ⋮& \ddots & ⋮& ⋮\\ 1& 1& \cdots & 1& 0\\ 1& 1& \cdots & 1& 1\end{array}\right].$$

(2)

The elements of the matrices ${\overline{M}}_i,{\overline{C}}_i\in {\mathbb{R}}^{n\times n}$ at the $(\alpha ,\beta )$-th position are given by

$$\begin{array}{cc}\hfill & {\left[{\overline{M}}_i\right]}_{\alpha \beta }={\lambda }_{i,\alpha \beta }cos\left({\theta }_{i,\beta }-{\theta }_{i,\alpha }\right),\hfill \end{array}$$

(3)

$$\begin{array}{cc}\hfill & {\left[{\overline{C}}_i\right]}_{\alpha \beta }=-{\lambda }_{i,\alpha \beta }{\dot{\theta }}_{i,\beta }sin\left({\theta }_{i,\beta }-{\theta }_{i,\alpha }\right),\hfill \end{array}$$

(4)

with the mechanical parameter ${\lambda }_{i,\alpha \beta }$ defined as

$$\begin{array}{cc}\hfill & {\lambda }_{i,\alpha \alpha }=I_{i,\alpha }+m_{i,\alpha }{l}_{i,\alpha }^2+L_{i,\alpha }^2\sum _{\gamma =\alpha +1}^n{m}_{i,\gamma },1\le \alpha \le n,\hfill \\ \hfill & {\lambda }_{i,\alpha \beta }={\lambda }_{i,\beta \alpha }=m_{i,\beta }{L}_{i,\alpha }{l}_{i,\beta }+L_{i,\alpha }{L}_{i,\beta }\sum _{\gamma =\beta +1}^n{m}_{i,\gamma },1\le \alpha <\beta \le n.\hfill \end{array}$$

(5)

Here, summation terms for the case when $\alpha =n$ or $\beta =n$ are defined to be zero.

As the simplest nontrivial case ($n=2$), the PA manipulator (whose base joint is passive and the second joint is active) exhibits important differences in its dynamic behavior compared to the manipulators with $n\ge 3$, and is therefore discussed separately. Derivable directly from the general n-link formulation, the PA manipulator’s dynamics are given by

$$\begin{array}{cc}\hfill & M_i\left(q_i\right){\ddot{q}}_i+C_i\left(q_i,{\dot{q}}_i\right)q_i={\tau }_i,\hfill \end{array}$$

(6)

where $q_i={[q_{i,1},q_{i,2}]}^{\mathrm{T}}$ and ${\tau }_i={[0,{\tau }_{i,2}]}^{\mathrm{T}}$. The matrices $M_i\left(q_i\right),C_i(q_i,{\dot{q}}_i)$ are, respectively, given by

$$\begin{array}{c}\hfill M_i\left(q_i\right)=\left[\begin{array}{cc}{\lambda }_{i,11}+{\lambda }_{i,22}+2{\lambda }_{i,12}cos{q}_{i,2}& {\lambda }_{i,22}+{\lambda }_{i,12}cos{q}_{i,2}\\ {\lambda }_{i,22}+{\lambda }_{i,12}cos{q}_{i,2}& {\lambda }_{i,22}\end{array}\right],\end{array}$$

(7)

$$C_i(q_i,{\dot{q}}_i)={\lambda }_{i,12}\left[\begin{array}{cc}-{\dot{q}}_{i,2}& -{\dot{q}}_{i,1}-{\dot{q}}_{i,2}\\ {\dot{q}}_{i,1}& 0\end{array}\right]sin{q}_{i,2},$$

(8)

where ${\lambda }_{i,11}=m_{i,1}{l}_{i,1}^2+m_{i,2}{L}_{i,1}^2+I_{i,1}$, ${\lambda }_{i,22}=m_{i,2}{l}_{i,2}^2+I_{i,2}$, and ${\lambda }_{i,12}=m_{i,2}{L}_{i,1}{l}_{i,2}$.

2.2. Control Barrier Functions

Define the state vector for the manipulator i as $x_i=col(q_i,{\dot{q}}_i)\in {\mathbb{R}}^{2n}$. The dynamics, Equations (1) and (6), can be rewritten as

$${\dot{x}}_i=f_i\left(x_i\right)+g_i\left(x_i\right)u_i,$$

(9)

where $u_i\left(t\right)\in {\mathbb{R}}^{n-1}$ denotes the active control input, and $f_i:{\mathbb{R}}^{2n}\to {\mathbb{R}}^{2n}$, $g_i:{\mathbb{R}}^{2n}\to {\mathbb{R}}^{2n\times (n-1)}$ are $C^{\infty }$ differentiable functions.

To enforce safety constraints on the system, Equation (9), we employ a CBF-based safety-critical control framework. Define the safe set $\mathcal{C}$ as

$$\mathcal{C}=\{x_i\in {\mathbb{R}}^{2n}:h_i\left(x_i\right)\ge 0\},$$

(10)

where $h_i\left(x_i\right)$ is a continuously differentiable function. The system is safe with respect to $\mathcal{C}$ if $\mathcal{C}$ is forward invariant. In other words, whenever the state starts in $\mathcal{C}$, it remains in $\mathcal{C}$ for all time (see, e.g., ([15], Definition 1)). The function $h_i\left(x_i\right)$ is called a CBF for the system, Equation (9), if there exists an extended class-$\mathcal{K}$ function $\kappa (·)$ such that

$$\underset{u_i}{sup}\left[L_{f_i}{h}_i\left(x_i\right)+L_{g_i}{h}_i\left(x_i\right)u_i\right]>-\kappa \left(h_i\left(x_i\right)\right)$$

(11)

holds for all $x_i$ (see, e.g., ([15], Definition 5)). Intuitively, this condition implies that at any state $x_i$, there exists some control input $u_i$ that prevents $h_i\left(x_i\right)$ from decreasing too rapidly (or violating $h_i<0$). As shown in ([15], Lemma 2), when the input $u_i$ is unbounded, the condition in Equation (11) reduces to, for all $x_i$,

$$L_{g_i}{h}_i\left(x_i\right)=0⟹L_{f_i}{h}_i\left(x_i\right)>-\kappa \left(h_i\left(x_i\right)\right).$$

(12)

As noted in [15], the strict inequalities in Equations (11) and (12) guarantee the Lipschitz continuity of the CBF-based controller. In practice, one imposes the CBF constraint in a simplified linear inequality form

$$L_{f_i}{h}_i\left(x_i\right)+L_{g_i}{h}_i\left(x_i\right)u_i+\kappa \left(h_i\left(x_i\right)\right)\ge 0$$

(13)

for all times. Any Lipschitz continuous control law satisfying this will render the safe set $\mathcal{C}$ forward invariant ([15], Theorem 4). In other words, if $h_i\left(x_i\right)$ is initially nonnegative, it remains nonnegative for all future time, which guarantees the enforced safety constraint is never violated. Note that the constraint in Equation (13) is affine in the control input $u_i$, which makes it convenient to incorporate into a real-time QP. However, constructing suitable CBFs for high-order dynamical systems, particularly for underactuated manipulators, can be nontrivial, as challenges often arise due to issues related to the relative degree of the functions.

3. Problem Formulation

This paper addresses the safety-critical end-effector formation control for a team of PAⁿ⁻¹ manipulators, ensuring that their end-effectors collectively achieve a prescribed geometric formation while simultaneously avoiding obstacles in the workspace. This section formulates the control problem.

3.1. Formation Shape Specification

The end-effector position $p_i^{\mathrm{end}}$ of the i-th manipulator in the world frame Σ_W is defined as

$$p_i^{\mathrm{end}}\left(q_i\right)=p_{i0}+P_i\left(q_i\right),$$

(14)

where $p_{i0}\in {\mathbb{R}}^2$ is the base position and

$$P_i\left(q_i\right)={\left[P_{i,1}\left(q_i\right),P_{i,2}\left(q_i\right)\right]}^{\mathrm{T}}={\left[-\sum _{\alpha =1}^n{L}_{i,\alpha }sin\left({\theta }_{i,\alpha }+{\varphi }_i\right),\sum _{\alpha =1}^n{L}_{i,\alpha }cos\left({\theta }_{i,\alpha }+{\varphi }_i\right)\right]}^{\mathrm{T}},$$

(15)

Here, ${\theta }_{i,\alpha }={\sum }_{\gamma =1}^{\alpha }{q}_{i,\gamma }$ and ${\varphi }_i$ is the orientation offset.

Now, introduce an undirected graph $\mathcal{G}=(\mathcal{V},\mathcal{E})$ to describe the topological connections among the manipulators’ end-effectors. Each vertex $i\in \mathcal{V}$ corresponds to the end-effector of the i-th manipulator, and an edge $(i,j)\in \mathcal{E}$ implies a neighbor relationship between manipulators i and j. The stacked position vector of all end-effectors is denoted as $p^{\mathrm{end}}\in {\mathbb{R}}^{2N}=col(p_1^{\mathrm{end}},...,p_N^{\mathrm{end}})$. Using the incidence matrix $B\in {\mathbb{R}}^{N\times \left|\mathcal{E}\right|}$ of graph $\mathcal{G}$, define the relative displacement vector as

$$z=col(z_1,...,z_{\left|\mathcal{E}\right|})={\overline{B}}^{\mathrm{T}}{p}^{\mathrm{end}},\mathrm{where}\overline{B}=B\otimes I_2.$$

(16)

Here, ⊗ denotes the Kronecker product. Each element $z_k=p_i^{\mathrm{end}}-p_j^{\mathrm{end}}$ of the stacked vector z corresponds to edge $(i,j)$. Given a desired formation shape specified by the desired distance vector $d\in {\mathbb{R}}^{\left|\mathcal{E}\right|}$, there exists a reference position $p^{\ast }$ of $p^{\mathrm{end}}$ such that for every k, we have $\parallel z_k\left(p^{\ast }\right)\parallel =d_k$. The desired formation shape set is thus defined as [11]

$$\mathcal{S}=\{p^{\mathrm{end}}:p^{\mathrm{end}}=(I_N\otimes \mathcal{R})p^{\ast }+{\mathbf{1}}_N\otimes b,;\mathcal{R}\in \mathbf{SO}\left(2\right),b\in {\mathbb{R}}^2\},$$

(17)

where $I_N$ denotes the $N\times N$ identity matrix, and ${\mathbf{1}}_N$ denotes the N-dimensional vector whose entries are all ones. Each manipulator’s end-effector workspace is denoted by ${\mathcal{W}}_i$, and the collaborative workspace of all manipulators is $\mathcal{W}={\mathcal{W}}_1\times \dots \times {\mathcal{W}}_N$. Consequently, the realizable formation set within the workspace is defined as ${\mathcal{S}}_W=\mathcal{S}\cap \mathcal{W}$.

3.2. Obstacle Avoidance Constraints

For operational safety, we require each manipulator’s end-effector to remain outside every obstacle. Let $\mathcal{O}$ denote the set of obstacles in the manipulators’ plane, where each obstacle $j\in \mathcal{O}$ is modeled as a closed disk of radius $r_j>0$ centered at $o_j\in {\mathbb{R}}^2$. The safety requirement can then be compactly stated as

$$∥p_i^{\mathrm{end}}\left(q_i\right)-o_j∥\ge r_j,\forall i=1,\dots ,N,j\in \mathcal{O}.$$

(18)

In this work, we restrict collision checks to those between end-effectors and obstacles. Although link collisions can be important in many practical settings, they can be accommodated by introducing analogous safety constraints. This simplification streamlines both problem statement and analysis. Moreover, in applications where links are shielded or kept safely away from obstacles—or where only the end-effector operates within tightly controlled hazardous zones—focusing exclusively on the end-effector is both realistic and sufficient.

3.3. Control Objectives

With the problem clearly structured, we specify the following two primary control objectives (see also Figure 2):

• Formation Keeping: Develop a control strategy ensuring that the manipulators’ end-effectors converge into the feasible set ${\mathcal{S}}_W$, achieving and maintaining the desired geometric shape.
• Obstacle Avoidance: Guarantee that the positions of all end-effectors satisfy the safety constraints in Equation (18) at all times, thus preventing collisions.

4. Main Results

In this section, we summarize our primary theoretical and methodological contributions. Section 4.1 leverages the integrability of the second-order nonholonomic constraint inherent to the planar PAⁿ⁻¹ manipulator to reformulate its dynamics into a cascaded form. Based on this reformulation, Section 4.2 proposes a safety-critical formation controller to ensure that both specified control objectives are achieved.

4.1. Modeling Underactuated Manipulators in the Cascaded Form

Note that the motion Equation (1) of the PAⁿ⁻¹ manipulator i is subject to the following second-order nonholonomic constraint

$$\sum _{\alpha =1}^n{\left[M_i\right]}_{1\alpha }{\ddot{q}}_{i,\alpha }+{\left[C_i\right]}_{1\alpha }{\dot{q}}_{i,\alpha }=0,$$

(19)

which, in the special case $n=2$, reduces to

$${\left[M_i\right]}_{11}\left(q_i\right){\ddot{q}}_{i,1}+{\left[M_i\right]}_{12}\left(q_i\right){\ddot{q}}_{i,2}-{\lambda }_{i,12}\left(2{\dot{q}}_{i,1}{\dot{q}}_{i,2}+{\dot{q}}_{i,2}^2\right)sin{q}_{i,2}=0.$$

(20)

We now recall a lemma that will be used later.

Lemma 1 

([17,18]). Consider the PAⁿ⁻¹ manipulator indexed by i starts from rest, i.e., ${\dot{q}}_i\left(0\right)=\mathbf{0}$. Then, the constraint in Equation (19) can be integrated to

$$\sum _{\alpha =1}^n{\left[M_i\right]}_{1\alpha }\left(q_i\right){\dot{q}}_{i,\alpha }=0.$$

(21)

In the special case $n=2$, Equation (21) simplifies to

$${\left[M_i\right]}_{11}\left(q_i\right){\dot{q}}_{i,1}+{\left[M_i\right]}_{12}\left(q_i\right){\dot{q}}_{i,2}=0.$$

(22)

Remark 1. 

In the special case of $n=2$, integrating the joint velocity constraint in Equation (22) directly yields a relationship between the two joint angles, allowing us to express $q_{i,1}$ as a function of $q_{i,2}$, i.e., $q_{i,1}=F\left(q_{i,2}\right)$, where the explicit form of $F\left(q_{i,2}\right)$ can be found in ([11], Lemma 1). As a result, its end-effector position is restricted to a curve parameterized by $q_{i,2}$, namely $p_i^{\mathrm{end}}\left(q_{i,2}\right)=p_{i0}+P_i(F\left(q_{i,2}\right),q_{i,2})$. Note that the curve is uniquely determined by the mechanical parameters and the initial state of the PA manipulator. This restriction reduces the workspace of the manipulator group where PA manipulators are involved. A detailed analysis given in our previous work [11,12] shows that the proposed end-effector formation controller is applicable only when the group contains at most three PA manipulators.

Now, we are ready to model the PAⁿ⁻¹ manipulator i in the cascaded form using the constraint in Equation (21). Since $M_i$ is positive definite, we know ${\left[M_i\right]}_{11}\left(q_i\right)>0$ for all $q_i$. Hence, from Equation (21), the passive joint velocity ${\dot{q}}_{i,1}$ can be solved in terms of the active joint velocities ${\dot{q}}_{i,2},\dots ,{\dot{q}}_{i,n}$.

$${\dot{q}}_{i,1}=-{\displaystyle \frac{1}{{\left[M_i\right]}_{11}}}\sum _{\alpha =2}^n{\left[M_i\right]}_{1\alpha }{\dot{q}}_{i,\alpha }.$$

(23)

Defining $v_i={\dot{q}}_i^{\mathrm{a}}$, we obtain the reduced-order model of the PAⁿ⁻¹ manipulator from the constraint in Equation (23)

$$\begin{array}{cc}\hfill \underset{{\dot{x}}_i^0}{\underbrace{\left(\begin{array}{c}{\dot{q}}_{i,1}\\ {\dot{q}}_i^{\mathrm{a}}\end{array}\right)}}& =\underset{g_i^0\left(x_i^0\right)}{\underbrace{\left(\begin{array}{c}{\displaystyle -\frac{1}{{\left[M_i\right]}_{11}}{col}^{\mathrm{T}}\left(-{\left[M_i\right]}_{12},\dots ,-{\left[M_i\right]}_{1n}\right)}\\ I_{n-1}\end{array}\right)}}\underset{x_i^1}{\underbrace{v_i}}.\hfill \end{array}$$

(24)

We now express ${\dot{v}}_i$ by the active joint torque vector ${\tau }_i^a$. We partition the generalized coordinate $q_i\in {\mathbb{R}}^n$ of the PAⁿ⁻¹ manipulator i into the passive-joint scalar $q_{i,1}\in \mathbb{R}$ and the active-joint vector $q_i^{\mathrm{a}}\in {\mathbb{R}}^{n-1}$. Define $H_i(q_i,{\dot{q}}_i)=C_i(q_i,{\dot{q}}_i){\dot{q}}_i$, and Equation (1) can be rewritten in a block form as

$$\underset{M_i\left(q_i\right)}{\underbrace{\left[\begin{array}{cc}{M}_i^{\mathrm{uu}}\left(q_i\right)& M_i^{\mathrm{ua}}\left(q_i\right)\\ M_i^{\mathrm{au}}\left(q_i\right)& M_i^{\mathrm{aa}}\left(q_i\right)\end{array}\right]}}\underset{{\ddot{q}}_i}{\underbrace{\left[\begin{array}{c}{\ddot{q}}_{i,1}\\ {\ddot{q}}_i^{\mathrm{a}}\end{array}\right]}}+\underset{H_i(q_i,{\dot{q}}_i)}{\underbrace{\left[\begin{array}{c}{H}_i^{\mathrm{u}}(q_i,{\dot{q}}_i)\\ H_i^{\mathrm{a}}(q_i,{\dot{q}}_i)\end{array}\right]}}=\left[\begin{array}{c}0\\ {\tau }_i^{\mathrm{a}}\end{array}\right].$$

(25)

Here, $M_i^{\mathrm{uu}}\left(q_i\right)={\left[M_i\right]}_{11}\left(q_i\right)\in \mathbb{R}$ is positive, and $M_i^{\mathrm{aa}}\left(q_i\right)\in {\mathbb{R}}^{(n-1)\times (n-1)}$ is symmetric and positive-definite; $H_i^{\mathrm{u}}$, $H_i^{\mathrm{a}}$ collect the Coriolis forces on the passive and active coordinates, respectively. Since the scalar $M_i^{\mathrm{uu}}\left(q_i\right)$ is positive, we have

$${\ddot{q}}_{i,1}=-{\displaystyle \frac{1}{M_i^{\mathrm{uu}}\left(q_i\right)}}\left[M_i^{\mathrm{ua}}\left(q_i\right){\ddot{q}}_i^{\mathrm{a}}+H_i^{\mathrm{u}}(q_i,{\dot{q}}_i)\right].$$

(26)

Then, substituting Equation (26) into the second equation in Equation (25) yields

$$M_i^{\mathrm{Comb}}\left(q_i\right){\ddot{q}}_i^{\mathrm{a}}+H_i^{\mathrm{Comb}}(q_i,{\dot{q}}_i)={\tau }_i^{\mathrm{a}},$$

(27)

where

$$M_i^{\mathrm{Comb}}\left(q_i\right)=M_i^{\mathrm{aa}}\left(q_i\right)-{\displaystyle \frac{1}{M_i^{\mathrm{uu}}\left(q_i\right)}}{M}_i^{\mathrm{au}}\left(q_i\right)M_i^{\mathrm{ua}}\left(q_i\right),$$

(28)

$$H_i^{\mathrm{Comb}}(q_i,{\dot{q}}_i)=H_i^{\mathrm{a}}(q_i,{\dot{q}}_i)-{\displaystyle \frac{1}{M_i^{\mathrm{uu}}\left(q_i\right)}}{M}_i^{\mathrm{au}}\left(q_i\right)H_i^{\mathrm{u}}(q_i,{\dot{q}}_i).$$

(29)

As the Schur complement of the symmetric positive-definite matrix $M_i$, the matrix $M_i^{\mathrm{Comb}}$ is also symmetric and positive-definite [24].

Therefore, from Equation (27), we obtain

$$\underset{{\dot{x}}_i^1}{\underbrace{{\dot{v}}_i}}=\underset{f_i^1(x_i^0,x_i^1)}{\underbrace{-{\left(M_i^{\mathrm{Comb}}\left(q_i\right)\right)}^{-1}{H}_i^{\mathrm{Comb}}(q_i,{\dot{q}}_i)}}+\underset{g_i^1\left(x_i^0\right)}{\underbrace{{\left(M_i^{\mathrm{Comb}}\left(q_i\right)\right)}^{-1}}}{\tau }_i^a$$

(30)

Note that $-{\left(M_i^{\mathrm{Comb}}\left(q_i\right)\right)}^{-1}{H}_i^{\mathrm{Comb}}(q_i,{\dot{q}}_i)$ is a function of states $x_i^0$ and $x_i^1$, since, by Equation (24), ${\dot{q}}_i$ can be expressed in terms of $x_i^1$. Accordingly, the PAⁿ⁻¹ manipulator i can be expressed in the cascaded form Equations (24) and (30), which facilitates the controller design via the standard backstepping method.

Remark 2. 

It should be noted that neither our previous studies [11,12] nor most existing research on underactuated manipulator control [23,25] transformed the manipulator into a cascaded form. In this study, however, we introduce such a cascaded-form transformation explicitly to fulfill the requirements of the CBF-based safety-critical controller design. As discussed in the Introduction, this cascaded-form transformation is not universally applicable to all underactuated manipulators. Specifically, the transformation here leverages the integrability of the second-order nonholonomic constraint, which is a distinct property of the PAⁿ⁻¹ manipulator.

4.2. Safety-Critical Controller for End-Effector Formation

In what follows, we extend the distributed end-effector formation controller proposed in [11] for the two-link case to the general n-link manipulator and incorporate obstacle avoidance based on the obtained cascaded-form dynamics.

4.2.1. End-Effector Formation Control for Reduced-Order Model

Let $d\in {\mathbb{R}}^{\left|\mathcal{E}\right|}$ denote the desired distance vector for an infinitesimally rigid formation (see [26] for its definition). The error on each edge ${\mathcal{E}}_k$ is then defined by

$$e_k={\parallel z_k\parallel }^2-d_k^2,$$

(31)

and collect them into $e=\mathrm{col}(e_1,\dots ,e_{\left|\mathcal{E}\right|})$. Then, define the formation potential function

$$V\left(e\right)={\displaystyle \frac{K_P}{2}}\sum _{k=1}^{\left|\mathcal{E}\right|}{e}_k^2.$$

(32)

Since

$${\displaystyle \frac{\partial z_k}{\partial p_i^{\mathrm{end}}}}={\left[B\right]}_{ik},D_k\left(z_k\right)={\displaystyle \frac{\partial e_k}{\partial z_k}}=2z_k,$$

(33)

it follows that the gradient of $V\left(e\right)$ with respect to $p_i^{\mathrm{end}}$ is

$$K_P{\widehat{e}}_i={\displaystyle \frac{\partial V\left(e\right)}{\partial p_i^{\mathrm{end}}}}=K_P\sum _{k=1}^{\left|\mathcal{E}\right|}{\left[B\right]}_{ik}{D}_k\left(z_k\right)e_k.$$

(34)

Collecting all ${\widehat{e}}_i$ into the vector $\widehat{e}=\mathrm{col}({\widehat{e}}_1,\dots ,{\widehat{e}}_N)\in {\mathbb{R}}^{2N}$, and letting $D\left(z\right)\in {\mathbb{R}}^{2\left|\mathcal{E}\right|\times \left|\mathcal{E}\right|}$ be the block-diagonal matrix with blocks $D_k\left(z_k\right)$, we may therefore rewrite Equation (34) in the compact form as

$$K_P\widehat{e}={\displaystyle \frac{\partial V\left(e\right)}{\partial p^{\mathrm{end}}}}=K_P\overline{B}D\left(z\right)e.$$

(35)

Note that for the infinitesimally rigid framework $(\mathcal{G},p^{\ast })$, the matrix $D{\left(z\right)}^{\mathrm{T}}{\overline{B}}^{\mathrm{T}}\overline{B}D\left(z\right)$ is positive definite at $p^{\ast }$ [27].

Next, for the reduced-order model, Equation (24), of each PAⁿ⁻¹ manipulator i, we design the desired active joint velocity $v_i$ to ensure that the prescribed formation shape can be achieved. Before proceeding, we state the following assumption.

Assumption 1. 

There exists a constant $\mu >0$ and a neighborhood

$${\mathcal{S}}_{W_{\mu }}=\left\{p^{\mathrm{end}}\in {\mathbb{R}}^{2N}:\parallel e\parallel <\mu \right\}$$

(36)

around ${\mathcal{S}}_W$ such that, for each PAⁿ⁻¹ manipulator i ($n\ge 3$), its kinematic Jacobian matrix $J_i\left(q_i\right)$ remains full row rank for every $p^{\mathrm{end}}\in {\mathcal{S}}_{W_{\mu }}$.

Note that $J_i\left(q_i\right)={\displaystyle \frac{\partial p_i^{\mathrm{end}}}{\partial q_i}}\in {\mathbb{R}}^{2\times n}$ is the standard Jacobian matrix of an n-link planar manipulator. Denote ${\overline{J}}_i\left(q_i\right)\in {\mathbb{R}}^{2\times (n-1)}$ as the Jacobian of the end-effector position with respect to the n − 1 active joint angles, which is specifically defined for PAⁿ⁻¹ manipulators:

$${\overline{J}}_i\left(q_i\right)=J_i\left(q_i\right)\left(\begin{array}{c}{\displaystyle -\frac{1}{{\left[M_i\right]}_{11}}{col}^{\mathrm{T}}\left(-{\left[M_i\right]}_{12},\dots ,-{\left[M_i\right]}_{1n}\right)}\\ I_{n-1}\end{array}\right).$$

(37)

Clearly, when $n\ge 3$, the assumption that $J_i\left(q_i\right)$ has full row rank implies that the matrix ${\overline{J}}_i\left(q_i\right)$ also has full row rank, owing to the presence of the block $I_{n-1}$ in the multiplied matrix. The following remarks further clarify this assumption.

Remark 3. 

Assumption 1 is introduced to ensure that the manipulators avoid singular points during the control process. This requirement is standard in the end-effector control (see, e.g., ([28], Property 5), ([29], Theorem 1), [24]) and is not restrictive. Although the existence of $S_{W_{\mu }}$ is required, its size is not critical; it suffices that the reference point is non-singular. By continuity, a singularity-free neighborhood $S_{W_{\mu }}$ always exists if the reference point $p^{\ast }\in {\mathcal{S}}_W$ is chosen such that $q_i^{\ast }$ is non-singular for all i. Since singular configurations occupy only a negligible subset of the joint space, it is always possible to select $q_i^{\ast }$ such that the manipulator operates away from singularities.

Remark 4. 

When $n=2$, ${\overline{J}}_i\left(q_i\right)$ reduces to the $2\times 1$ vector ${\overline{J}}_i\left(q_i\right)={\left[{\overline{J}}_{i,1}\left(q_i\right),{\overline{J}}_{i,2}\left(q_i\right)\right]}^{\mathrm{T}}$, and therefore cannot be full row rank. In this case, as shown in our previous work [11], we do not require $J_i\left(q_i\right)$ to remain full row rank; instead, we require that both components of ${\overline{J}}_i\left(q_i\right)$ are non-zero, i.e.,

$${\overline{J}}_{i,1}\left(q_i\right)\ne 0\mathrm{and}{\overline{J}}_{i,2}\left(q_i\right)\ne 0.$$

(38)

For a detailed analysis of this special case, see [11].

Theorem 1. 

Consider the reduced-order model, Equation (24), of the PAⁿ⁻¹ manipulators. Under Assumption 1, the end-effector formation keeping problem can be solved locally by the distributed control law

$$v_i=v_{i0}=-K_P{\left(\begin{array}{c}{\displaystyle -\frac{1}{{\left[M_i\right]}_{11}}{col}^{\mathrm{T}}\left(-{\left[M_i\right]}_{12},\dots ,-{\left[M_i\right]}_{1n}\right)}\\ I_{n-1}\end{array}\right)}^{\mathrm{T}}{J}_i^{\mathrm{T}}\left(q_i\right){\widehat{e}}_i,$$

(39)

where $K_P>0$ is the control gain, ${\widehat{e}}_i$ is defined in Equation (34), and $M_i$ denotes the inertia matrix associated with the dynamics of manipulator i, whose expression is given in Equation (1).

Proof. 

Consider the Lyapunov function as $U_1=V\left(e\right)$ defined in (32). Routine computation to the time derivative yields

$$\begin{array}{c}\hfill \dot{V}\left(e\right)=\sum _{i=1}^N{\left({\displaystyle \frac{\partial V}{\partial p_i^{\mathrm{end}}}}\right)}^T{\dot{p}}_i^{\mathrm{end}}=K_P\sum _{i=1}^N{{\widehat{e}}_i}^T{\dot{p}}_i^{\mathrm{end}}=K_P\sum _{i=1}^N{{\widehat{e}}_i}^T{J}_i\left(q_i\right){\dot{q}}_i\end{array}$$

(40)

Substituting the reduced-order model of Equation (24) into (40) yields

$$\dot{V}\left(e\right)=K_P\sum _{i=1}^N{{\widehat{e}}_i}^T{J}_i\left(q_i\right)\left(\begin{array}{c}-{\displaystyle \frac{1}{{\left[M_i\right]}_{11}}}{\mathrm{col}}^{\mathrm{T}}(-{\left[M_i\right]}_{12},\dots ,-{\left[M_i\right]}_{1n})\\ I_{n-1}\end{array}\right)v_i.$$

(41)

Substituting the distributed formation controller in Equation (39) into Equation (41) yields

$$\begin{array}{cc}\hfill \dot{V}\left(e\right)& =-K_P^2\sum _{i=1}^N{\widehat{e}}_i^{\mathrm{T}}{\overline{J}}_i\left(q_i\right){\overline{J}}_i^{\mathrm{T}}\left(q_i\right){\widehat{e}}_i=-{∥K_P\xi ∥}^2.\hfill \end{array}$$

(42)

where

$$\xi =\mathrm{col}\left({\overline{J}}_1^{\mathsf{T}}\left(q_1\right){\widehat{e}}_1,\dots ,{\overline{J}}_N^{\mathsf{T}}\left(q_N\right){\widehat{e}}_N\right).$$

(43)

Hence, $V\left(e\right(t\left)\right)$ is nonincreasing. Therefore, if $V\left(e\left(0\right)\right)\le \frac{1}{2}{K}_P{\mu }^2$, then for all $t\ge 0$, we have $\parallel e\left(t\right)\parallel <\mu$, i.e., the closed-loop trajectory remains in the set ${\mathcal{S}}_{W_{\mu }}$.

To invoke LaSalle’s invariance principle [30], define the auxiliary state variable $y_i=col\left(C_{q_i},S_{q_i}\right)$ for the manipulator i, where $C_{q_i}$ and $S_{q_i}$ are vectors with elements being the cosine and sine, respectively, of the corresponding elements of $q_i$. Then, the closed-loop manipulator i can be expressed by

$${\dot{y}}_i=F_i\left(y_i\right)=col\left(-S_{q_i}\odot {\dot{q}}_i,C_{q_i}\odot {\dot{q}}_i\right),$$

(44)

where “⊙” denotes the element-wise (Hadamard) product. From Equations (24) and (39), we know that ${\dot{q}}_i$ is a continuous function of $y_i$. Note that the overall state $y=col(y_1,\dots ,y_N)$ evolves in a compact forward-invariant set. Hence, by LaSalle’s invariance principle [30], the closed-loop systems converge to the largest invariant subset of

$$\left\{y:\dot{V}=0\right\}=\left\{y:{\overline{J}}_i^{\mathrm{T}}\left(q_i\right){\widehat{e}}_i=0,i=1,\dots ,N\right\}.$$

(45)

Under Assumption 1, since each ${\overline{J}}_i\left(q_i\right)$ has full row rank, it follows that

$${\overline{J}}_i^{\mathrm{T}}\left(q_i\right){\widehat{e}}_i=0⟹{\widehat{e}}_i=0,i=1,\dots ,N.$$

(46)

Hence, $\widehat{e}=\mathrm{col}({\widehat{e}}_1,\dots ,{\widehat{e}}_N)=0$. Finally, since $D^{\mathrm{T}}\left(z\right){\overline{B}}^{\mathrm{T}}$ is full rank, Equation (35) yields $e=0$. Therefore, we have

$$e\left(t\right)\to 0\mathrm{as}t\to \infty ,$$

(47)

which completes the proof. □

4.2.2. RCBF-Based Safety-Critical Controller

Theorem 1 provides the explicit formation controller $v_{i0}$ for the reduced-order model in Equation (24) of the underactuated PAⁿ⁻¹ manipulator i. However, the control law presented in Equation (39) does not consider safety constraints.

Assume a desired reference $v_i^{\ast }$ for $v_i$, which achieves the control objectives. Then, for the cascaded system described by Equations (24) and (30), we can design the control input ${\tau }_i^a$ to make the output of (30) track the desired reference $v_i^{\ast }$. Define the tracking error as

$${\tilde{e}}_i=v_i-v_i^{\ast }.$$

(48)

We now derive the safety-critical reference $v_i^{\ast }$ based on the nominal control law $v_{i0}$ given in Equation (39). For each manipulator i and obstacle j, according to the safety constraint (18), we define the safe set as

$${\mathcal{C}}_{ij}=\left\{q_i\in {\mathbb{R}}^n\mid h_{ij}\left(q_i\right)\ge 0\right\},$$

(49)

where

$$h_{ij}\left(q_i\right)={\parallel p_i^{\mathrm{end}}\left(q_i\right)-o_j\parallel }^2-r_j^2.$$

(50)

It is straightforward to verify that the function $h_{ij}$ defined in Equation (50) is a CBF for the reduced-order model, Equation (24), since the condition $L_{g_i^0}{h}_{ij}\left(q_i\right)=0$ occurs only when $p_i^{\mathrm{end}}=o_j$. Substituting the tracking error in Equation (48) into the reduced-order model in Equation (24) yields

$${\dot{q}}_i=g_i^0\left(q_i\right)\left(v_i^{\ast }+{\tilde{e}}_i\right),$$

(51)

where ${\tilde{e}}_i$ is treated as a matched disturbance and assumed bounded by

$$\underset{t\ge 0}{sup}\parallel {\tilde{e}}_i\left(t\right)\parallel \le \delta ,\delta \ge 0.$$

(52)

To account for the safety hazards introduced by ${\tilde{e}}_i\left(t\right)$, we adopt the RCBF-based approach proposed in [15] and define the following family of inflated safe sets:

$${\mathcal{C}}_{ij}^{\delta }=\left\{q_i\in {\mathbb{R}}^n\mid h_{ij}^{\delta }\left(q_i\right)\ge 0\right\},$$

(53)

where

$$h_{ij}^{\delta }\left(q_i\right)=h_{ij}\left(q_i\right)+{\Gamma }_{ij}\left(\delta \right),{\Gamma }_{ij}\in {\mathcal{K}}_{\infty },$$

(54)

and ${\Gamma }_{ij}$ will be specified later.

According to ([15], Definition 7), a continuously differentiable function $h_{ij}:{\mathbb{R}}^n\to \mathbb{R}$, which defines the set ${\mathcal{C}}_{ij}^{\delta }$ as in Equation (53), is called an input-to-state safe control barrier function (ISSf-CBF) for system, Equation (51), if there exists a constant $\epsilon >0$ such that, for all $q_i\in {\mathbb{R}}^n$, the following inequality holds:

$$\underset{v_i}{sup}\left\{L_{f_i^0}{h}_{ij}\left(q_i\right)+L_{g_i^0}{h}_{ij}\left(q_i\right)v_i\right\}>-\kappa \left(h_{ij}\left(q_i\right)\right)+{\displaystyle \frac{1}{\epsilon }}{∥L_{g_i^0}{h}_{ij}\left(q_i\right)∥}^2.$$

(55)

Since $f_i^0=0$, it follows from ([15], Theorem 5) that any Lipschitz controller $v_i$ satisfying

$$\nabla h_{ij}^{\mathrm{T}}\left(q_i\right)g_i^0\left(q_i\right)v_i\ge -\kappa \left(h_{ij}\left(q_i\right)\right)+{\displaystyle \frac{1}{\epsilon }}{∥\nabla h_{ij}^{\mathrm{T}}\left(q_i\right)g_i^0\left(q_i\right)∥}^2$$

(56)

ensures that the closed-loop system remains within the inflated safe set ${\mathcal{C}}_{ij}^{\delta }$ with

$${\displaystyle {\Gamma }_{ij}\left(\delta \right)=-{\kappa }^{-1}\left(-\frac{\epsilon {\delta }^2}{4}\right),}$$

(57)

thereby guaranteeing obstacle avoidance. Note that the definition in Equation (55) employs a strict inequality to ensure Lipschitz continuity when $L_{g_i^0}{h}_{ij}\left(q_i\right)=0$ [15]. A non-strict inequality is admissible if $L_{g_i^0}{h}_{ij}\left(q_i\right)\ne 0$ for all $q_i$. In this paper, the condition $L_{g_i^0}{h}_{ij}\left(q_i\right)=0$ occurs only when $p_i^{\mathrm{end}}=o_j$, which is excluded as long as the manipulator end-effectors do not start from the worst-case safety condition. Therefore, the non-strict inequality in Equation (56) is sufficient, and we adopt it as it is more convenient for QP-based synthesis.

Stacking the constraints in Equation (56) for all $j=1,\dots ,\left|\mathcal{O}\right|$ yields

$$A_i\left(q_i\right)v_i\ge b_i\left(q_i\right),$$

(58)

where

$$A_i\left(q_i\right)=\left[\begin{array}{c}\nabla h_{i1}^{\mathrm{T}}\left(q_i\right)g_i^0\left(q_i\right)\\ ⋮\\ \nabla h_{i\left|\mathcal{O}\right|}^{\mathrm{T}}\left(q_i\right)g_i^0\left(q_i\right)\end{array}\right],b_i\left(q_i\right)=\left[\begin{array}{c}-\kappa h_{i1}\left(q_i\right)+{\displaystyle \frac{1}{\epsilon }}{∥\nabla h_{i1}^{\mathrm{T}}\left(q_i\right)g_i^0\left(q_i\right)∥}^2\\ ⋮\\ -\kappa h_{i\left|\mathcal{O}\right|}\left(q_i\right)+{\displaystyle \frac{1}{\epsilon }}{∥\nabla h_{i\left|\mathcal{O}\right|}^{\mathrm{T}}\left(q_i\right)g_i^0\left(q_i\right)∥}^2\end{array}\right].$$

(59)

If the state $q_i$ is measurable, the matrices $A_i\left(q_i\right)$ and vectors $b_i\left(q_i\right)$ are known in real time. Hence, Equation (58) represents an affine constraint on the instantaneous control input $v_i$. To compute the safety-filtered control input $v_i^{\ast }$, we solve the following optimization problem

$$v_i^{\ast }=arg\underset{v_i\in {\mathbb{R}}^{n-1}}{min}{\displaystyle \frac{1}{2}}{\parallel v_i-v_{i0}\left(q_i\right)\parallel }^2\mathrm{s}.\mathrm{t}.A_i\left(q_i\right)v_i\ge b_i\left(q_i\right).$$

(60)

Equivalently, noting that ${\displaystyle \frac{1}{2}}{\parallel v_i-v_{i0}\parallel }^2={\displaystyle \frac{1}{2}}{v}_i^{\mathrm{T}}{v}_i-v_{i0}^{\mathrm{T}}{v}_i+\mathrm{const}.$, we may rewrite Equation (60) as

$$v_i^{\ast }=arg\underset{v_i\in {\mathbb{R}}^{n-1}}{min}\left({\displaystyle \frac{1}{2}}{v}_i^{\mathrm{T}}{v}_i-v_{i0}^{\mathrm{T}}\left(q_i\right)v_i\right)\mathrm{s}.\mathrm{t}.A_i\left(q_i\right)v_i\ge b_i\left(q_i\right),$$

(61)

which is a standard convex QP solvable at each time step. The optimal solution $v_i^{\ast }$ guarantees obstacle avoidance for the manipulators’ end-effectors while deviating only minimally from the nominal formation controller $v_{i0}$.

Based on the objective of ensuring that the output of Equation (30) asymptotically tracks the reference velocity $v_i^{\ast }$, we design the torque input ${\tau }_i^a$ for the full manipulator dynamics. By substituting Equation (30) into the time derivative of Equation (48), we obtain error dynamics.

$${\dot{\tilde{e}}}_i=f_i^1(x_i^0,x_i^1)+g_i^1\left(x_i^0\right){\tau }_i^a-{\dot{v}}_i^{\ast }.$$

(62)

Consider the Lyapunov candidate

$$U_2=\frac{1}{2}{\tilde{e}}_i^T{\tilde{e}}_i,$$

(63)

and choose the control law

$${\tau }_i^a={\left[g_i^1\left(x_i^0\right)\right]}^{-1}\left({\dot{v}}_i^{\ast }-f_i^1(x_i^0,x_i^1)-K_e{\tilde{e}}_i\right),$$

(64)

where $K_e>0$ is the control gain. It is straightforward to verify that the tracking error in Equation (48) asymptotically converges to zero, i.e., $v_i\to v_i^{\ast }$. The detailed proof is standard and hence omitted. See Figure 3 for the overall control framework diagram.

Remark 5. 

In the controller (64), we employ the time derivative ${\dot{v}}_i^{\ast }$. A differentiable closed-form solution to the QP problem in Equation (61) exists when the QP has a single constraint [15], and arbitrarily complex obstacle environments can be represented by a single composite CBF [31]. Therefore, an explicit expression for ${\dot{v}}_i^{\ast }$ always exists. In practical implementations, the time derivative ${\dot{v}}_i^{\ast }$ can be approximated numerically using finite-difference schemes, as employed in the simulations of this study.

5. Simulations

In this section, the proposed safety-critical end-effector formation control strategy is validated by means of two representative cases.

5.1. Manipulator Group Formed by PA³ Manipulators

Consider a network of $N=4$ underactuated manipulators operating in a gravity-free plane. Each manipulator is a four-link mechanism with a passive first joint and three actively driven joints; in accordance with the naming convention adopted in this paper, we refer to it as the PA³ manipulator. The mechanical parameters for the i-th manipulator are

$$\begin{array}{cccc}\hfill m_{i,1}& =m_{i,2}=0.3\mathrm{kg},\hfill & \hfill m_{i,3}& =m_{i,4}=0.4\mathrm{kg},\hfill \\ \hfill L_{i,1}& =L_{i,2}=0.3\mathrm{m},\hfill & \hfill L_{i,3}& =L_{i,4}=0.4\mathrm{m},\hfill \\ \hfill l_{i,1}& =l_{i,2}=0.15\mathrm{m},\hfill & \hfill l_{i,3}& =l_{i,4}=0.2\mathrm{m},\hfill \\ \hfill I_{i,1}& =I_{i,2}=0.0022\mathrm{kg}{\mathrm{m}}^2,\hfill & \hfill I_{i,3}& =I_{i,4}=0.0053\mathrm{kg}{\mathrm{m}}^2.\hfill \end{array}$$

(65)

Their base positions in the global $X-Y$ plane are $p_{10}={[0,0]}^{\mathrm{T}}$, $p_{20}={[2.5,0]}^{\mathrm{T}}$, $p_{30}={[2.5,2]}^{\mathrm{T}}$, and $p_{40}={[0,2]}^{\mathrm{T}}$ and the orientation offset between each local coordinate frame and the global frame is ${\varphi }_i=0$. All manipulators start from rest (${\dot{q}}_i\left(0\right)=0$) with the following joint configurations:

$$\begin{array}{cccc}\hfill {\displaystyle q_1\left(0\right)}& ={\left[-{\displaystyle \frac{\pi }{3}},{\displaystyle \frac{\pi }{6}},-{\displaystyle \frac{\pi }{4}},{\displaystyle \frac{\pi }{8}}\right]}^{\mathrm{T}},\hfill & \hfill q_2\left(0\right)& ={\left[{\displaystyle \frac{\pi }{4}},0,{\displaystyle \frac{\pi }{3}},-{\displaystyle \frac{\pi }{2}}\right]}^{\mathrm{T}},\hfill \\ \hfill q_3\left(0\right)& ={\left[{\displaystyle \frac{3\pi }{4}},-{\displaystyle \frac{\pi }{4}},{\displaystyle \frac{\pi }{3}},-{\displaystyle \frac{\pi }{6}}\right]}^{\mathrm{T}},\hfill & \hfill q_4\left(0\right)& ={\left[-{\displaystyle \frac{3\pi }{4}},-{\displaystyle \frac{\pi }{4}},{\displaystyle \frac{\pi }{2}},-{\displaystyle \frac{\pi }{8}}\right]}^{\mathrm{T}}.\hfill \end{array}$$

(66)

The desired formation is a square of side length 0.7 m, whose interconnection topology is captured by the incidence matrix of the formation graph $\mathcal{G}$.

$$B=\left[\begin{array}{ccccc}1& 0& 0& -1& 1\\ -1& 1& 0& 0& 0\\ 0& -1& 1& 0& -1\\ 0& 0& -1& 1& 0\end{array}\right].$$

(67)

Three circular obstacles—each of radius $0.08\mathrm{m}$—are located at ${\mathrm{ob}}_1=[1.20,0.70]$, ${\mathrm{ob}}_2=[0.90,1.40]$ and ${\mathrm{ob}}_3=[1.04,1.25]$ in the $X-Y$ plane.

Two sets of comparative simulations were conducted on this group of underactuated manipulators: one without safety constraints (results shown in Figure 4) and the other under the proposed safety-critical controller (results shown in Figure 5). Without considering safety constraints, we use the control law in Equation (64) with $v_i^{\ast }=v_{i0}$ and control parameters set as $K_P=2$ and $K_e=100$. The simulation results are shown in Figure 4. Sub-figures (a) and (b) depict the convergence of joint angles and velocities to equilibrium. Sub-figure (c) illustrates that the distances defined on each edge of the graph $\mathcal{G}$ converge to their desired values, confirming the formation of the target square. Sub-figure (d) shows the trajectories of the networked end-effectors and indicates that manipulators 1 and 4 collide with obstacles, creating a potential safety hazard. These results demonstrate that, the nominal formation control strategy can achieve convergence but may lead to collisions in cluttered environments.

To enforce safety, we use the safety-critical control law in Equation (64) with $v_i^{\ast }$ obtained from QP in Equation (61). The control gains remain $K_P=2$ and $K_e=100$, and the RCBF parameters are chosen as $\kappa =0.5$ and $\epsilon =5$. Note that selecting a smaller $\epsilon$ and a larger $\kappa$ reduces ${\Gamma }_{ij}\left(\delta \right)$ and makes the inflated safe set closer to the original one. However, such a choice narrows the feasible range of the controller $v_i$ and requires larger control torques (see Equation (56)). In addition, the upper bound of the backstepping tracking error ${\tilde{e}}_i\left(t\right)$ can be reduced by choosing a relatively large $K_e$, which in turn decreases ${\Gamma }_{ij}\left(\delta \right)$. However, we also note that excessively large control gains may lead to high control torques. In summary, the values of $\kappa$, $\epsilon$, and $K_e$ should be selected by balancing system safety and controller feasibility. As shown in Figure 5, by incorporating the RCBF-based safety filter, the manipulators are able to maintain both formation accuracy and obstacle avoidance simultaneously. This verifies the effectiveness of the safety-critical controller and confirms the theoretical guarantees discussed in Section 4.

5.2. Manipulator Group Including the PA Manipulator

We also consider a network of $N=4$ two-link manipulators, one of which is an underactuated PA manipulator. As noted in Remark 1, the PA manipulator’s joint-angle constraint restricts its motion to a predefined curve, thereby reducing its workspace. Accordingly, in this simulation only manipulator 4 is underactuated, while the others remain fully actuated. Even if an obstacle occupies PA manipulator’s limited workspace, the cooperative system can achieve the formation control. All manipulators share the following mechanical parameters: $m_{i,1}=m_{i,2}=0.2\mathrm{kg}$, $L_{i,1}=L_{i,2}=0.5\mathrm{m}$, $l_{i,1}=l_{i,2}=0.15\mathrm{m}$, and $I_{i,1}=I_{i,2}=0.005\mathrm{kg}{\mathrm{m}}^2$. Their base positions are $p_{10}={[0,2]}^{\mathrm{T}}$, $p_{20}={[2,2]}^{\mathrm{T}}$, $p_{30}={[2.5,3]}^{\mathrm{T}}$, and $p_{40}={[0.5,3]}^{\mathrm{T}}$, with orientation offsets ${\varphi }_i=0$. The initial joint configurations (with initial joint velocities being zero) are $q_1\left(0\right)={[-\pi /3,\pi /6]}^{\mathrm{T}}$, $q_2\left(0\right)={[0,2\pi /3]}^{\mathrm{T}}$, $q_3\left(0\right)={[\pi /3,\pi /3]}^{\mathrm{T}}$, and $q_4\left(0\right)={[0,-\pi /2]}^{\mathrm{T}}$. The desired formation is a square of side length $0.8\mathrm{m}$, with incidence matrix B defined in Equation (67). A circular obstacle of radius $0.1\mathrm{m}$ is placed at ${\mathrm{ob}}_1=[1,3.3]$.

Two sets of comparative simulations were conducted on this group of manipulators: one without safety constraints, implemented using the method in [11] (results shown in Figure 6), and the other under the proposed safety-critical controller (results shown in Figure 7). Without considering safety constraints, we apply the control law in Equation (64) with $v_i^{\ast }=v_{i0}$ and gains $K_P=1$, $K_e=100$. In this case, the control law in Equation (64) is indeed in the same form as that proposed in [11]. The simulation results in Figure 6 illustrate that an obstacle lies along the PA manipulator’s path toward its desired equilibrium. Due to without incorporating the safety-guaranteed strategy, the manipulator’s end-effector will collide with that obstacle. To enforce safety, we apply the safety-critical control law in Equation (64) using $v_i^{\ast }$ obtained from QP in Equation (61), while keeping $K_P=1$ and $K_e=100$. For the RCBF-based controller we set $\kappa =0.2$ and $\epsilon =20$. Since the PA manipulator cannot deviate from its predefined curve, it stops just before collision (as shown in Figure 7d); Accordingly, the other manipulators adjust their respective desired equilibrium, enabling the cooperative system to achieve the desired formation. Figure 8 presents the distance between the PA manipulator’s end-effector and the obstacle center. Unlike the results in [11], the proposed safety-critical controller ensures that the manipulator remains within the safety boundary.

6. Conclusions and Discussion

6.1. Brief Review of Our Work and Potential Applications

Cooperative control of manipulators has emerged as a growing research focus, motivated by applications such as coordinated object transport, cooperative assembly, and precision tasks requiring multiple arms to act in concert [5,32,33,34,35]. We focus on safety-critical, distributed end-effector formation control for multiple planar underactuated PAⁿ⁻¹ manipulators. This setting arises from actuator faults that induce underactuation, a condition under which conventional controllers fail. It is also motivated by safety-sensitive domains (e.g., hazardous material handling, robotic surgery) that demand guaranteed collision avoidance.

We leverage the integrability of the second-order nonholonomic constraints inherent in PAⁿ⁻¹ manipulators, which enables us to derive a cascaded form of their dynamics. Using this cascaded-form transformation, we extract a reduced-order model for each manipulator and then design a distributed formation controller via a backstepping approach. Tracking errors introduced by the backstepping method may compromise safety by driving the system outside the safety set defined by a general CBF. To mitigate this impact, we incorporate an RCBF into the control law.

6.2. Comparison with Previous Work

Although actuator faults [6,36] and collision-avoidance [9,10] have been studied in multi-agent systems, these works focus on simple agents—whose control inputs directly determine the formation’s state—and do not address severe faults that render the system underactuated. For fully actuated manipulators, a commonly used safety-critical control strategy is to transform the dynamics into a cascaded form and then construct CBFs via the reduced-order model [15,16]. However, As mentioned in the Introduction, the second-order nonholonomic constraints inherent in the underactuated systems make their dynamics generally unable to be transformed into a cascaded form composed of multiple first-order subsystems, which prevents the direct extension of the existing CBF-based methods. Neither our previous studies [11,12] nor other works on single underactuated manipulators [23,25] have addressed safety considerations. To address this challenge, we exploit a key property discussed in [17,18]: for the underactuated PAⁿ⁻¹ manipulator, its second-order coupling can be integrated into a first-order coupling. Exploiting this property, this paper decomposes the dynamics of PAⁿ⁻¹ underactuated manipulators into a cascaded form and thereby constructs a valid CBF. It is important to emphasize that this reduction (from second-order coupling to first-order coupling) does not apply to any other underactuated manipulators. In other words, the method proposed is a specific solution tailored for PAⁿ⁻¹ underactuated manipulators.

The formation control for multi-agent systems generally presents greater complexity than the positioning problem for a single agent. However, it is important to clarify that our proposed CBF-based safety-critical control may not directly extend to the single PAⁿ⁻¹ manipulator’s end-effector positioning problem addressed in [37,38]. The primary difficulty in the end-effector positioning control for a single PAⁿ⁻¹ manipulator arises from the necessity of determining feasible trajectories for actuated joints to achieve a specific position. Introducing obstacle avoidance further complicates this trajectory planning, rendering existing solutions inadequate and complicating the integration of safety-critical methods. Conversely, our study focuses on arranging multiple manipulators into a predefined formation rather than specifying precise individual end-effector positions, thereby bypassing the trajectory-planning challenge and enabling the use of the CBF framework.

In addition, we note two mainstream CBF-based approaches to safety-critical control of cascaded systems. The first is the RCBF method adopted in this work, where the safety controller is designed for the reduced-order model and the tracking error introduced by backstepping is treated as a bounded disturbance. The RCBF-based method guarantees that the closed-loop system remains within an inflated safety set. The second is the CBF backstepping (or safe backstepping) approach [15,16], which provides a theoretical framework for guaranteeing safety in cascaded systems with an arbitrary number of layers. Although this method ensures invariance of the original safety set in principle, it requires the reduced-order subsystem controller to be smooth, thereby tolerating only analytic QP solutions and substantially increasing both derivational and implementation complexity. It often leads to overly conservative safety sets. For the two-layer cascaded system studied here, the RCBF approach offers a more practical balance between safety assurance and implementation simplicity.

6.3. Limitations and Future Research Directions

The present study is confined to collision avoidance at the end-effectors and guarantees asymptotic convergence to a static formation. Future research should therefore consider more complex scenarios, including link-obstacle interactions, dynamic obstacles, and inter-manipulator constraints [39]. Extending the framework to support time-varying formations and incorporating fixed-time control for rapid, predefined-time convergence [40] would also be valuable directions.

Moreover, the success of our approach hinges on the integrability of the second-order nonholonomic constraint—an intrinsic feature of PAⁿ⁻¹ manipulators. Addressing obstacle avoidance for other classes of underactuated manipulators remains an interesting research direction. As noted, due to the lack of such integrability, transforming these systems into cascaded form is generally infeasible. Promising avenues include (i) exploiting structural properties of the dynamics to design exponential CBFs [41]; (ii) computing CBFs online [42] or synthesizing them offline via optimization-based methods [43]; and (iii) going beyond CBFs by employing barrier Lyapunov functions (BLFs) to enforce state constraints, as demonstrated for underactuated cranes in [44].