Humanoid Robot Teleoperation for Nonprehensile Transportation: A Multiple-Constraint Safety-Critical Control Framework

Abstract

This paper investigates the conflicting multiple constraints and safety challenges in humanoid robot teleoperation for nonprehensile transportation tasks. The robot’s complex workspace and high degrees of freedom frequently conflict with highly dynamic task requirements, imposing stringent demands on coordinated motion. To address these issues, this paper proposes a Multiple-Constraint Safety-Critical Control Framework (MC-SCCF) featuring a hierarchical three-layer architecture. The top layer guarantees intrinsic safety against workspace boundaries using a continuously differentiable reachability surrogate model and an improved control barrier function (CBF)-based safe velocity filter for smooth deceleration. The middle layer maps user commands into pose-coupled reference trajectories to ensure task-level object safety, satisfying strict non-slip and non-toppling constraints. The bottom layer utilizes a quadratic programming (QP)-based inverse kinematics solver to achieve self-collision avoidance, coordinated motion, and optimal configuration while strictly enforcing joint and manipulability limits. Simulations and hardware experiments demonstrate that the MC-SCCF achieves real-time, high-precision reachability evaluation and successfully coordinates task dynamics with physical constraints, enhancing operational safety and the human–robot interaction experience.

1. Introduction

Humanoid robots, characterized by their anthropomorphic morphology and versatile motion capabilities, exhibit significant application potential in unstructured environments such as disaster rescue, domestic service, and industrial collaboration. Within these complex scenarios, object transportation is a fundamental and critical task. As an essential mode of operation, nonprehensile transportation demonstrates unique advantages in practical applications [1], making it particularly suitable for objects that are difficult to grasp directly. For instance, during the transfer of hazardous, fragile, or contaminated items, or when carrying food on a tray, nonprehensile transportation achieves object manipulation by strategically leveraging friction, gravity, and inertial forces. This approach not only reduces the internal forces exerted on the objects, thereby lowering the risk of damage, but also enables the simultaneous transportation of multiple objects, significantly improving operational efficiency.

However, such tasks impose stringent requirements on the dynamic coordination capabilities of the system. Given the current limitations in the decision-making capabilities of fully autonomous robots in complex dynamic environments, teleoperation, as a human-in-the-loop solution, integrates the cognitive adaptability of human operators with the physical manipulation capabilities of robots. Consequently, it has emerged as the prominent approach for executing such complex transportation tasks.

Although teleoperation enables robots to exploit human cognitive capabilities in complex environments, transferring nonprehensile transportation tasks to humanoid robot platforms still poses major challenges to control system design. On the one hand, nonprehensile transportation relies on friction, gravity, and inertial forces to maintain object stability. The object must be prevented from slipping or toppling during motion, which imposes strict coupling among the position, orientation, velocity, and acceleration of the end-effector trajectory. In particular, during highly dynamic teleoperation, the object orientation is highly sensitive to variations in linear and angular accelerations. Any abrupt change or relaxation of constraints may directly lead to task failure. On the other hand, the upper body of a humanoid robot has a highly redundant dual-arm structure. Intrinsic constraints, including dual-arm coordination, joint limits, singularity avoidance, self-collision avoidance, and workspace boundaries, coexist and become dynamically coupled as the robot configuration changes.

Traditional robot safety control methods usually assign high priority to constraints related to the robot’s intrinsic safety, while treating task constraints as low-priority or soft constraints [2,3,4,5]. This design can prioritize the prevention of dangerous robot motions in general manipulation tasks. For nonprehensile transportation, however, object non-slipping, non-toppling, trajectory smoothness, and orientation stability are not secondary objectives that can be arbitrarily relaxed. Instead, they are hard conditions that determine whether the task can be completed. Thus, if the conventional priority allocation is directly adopted, and constraints such as workspace boundaries, joint limits, and singularity avoidance are combined with object dynamic constraints in a single optimization problem, or if their conflicts are handled only by weight tuning, two problems often arise. First, the abrupt activation of robot intrinsic constraints may induce acceleration discontinuities at the end-effectors, thereby disrupting object balance. Second, excessively conservative restrictions on robot motion may be imposed to maintain object stability, which greatly compresses the effective operational workspace.

When a humanoid robot executes the task, workspace boundary constraints are particularly prone to conflicts with task dynamic constraints. Workspace boundary evaluation for dual-arm coordinated tasks is fundamentally different from that in single-arm scenarios. Because the dual end-effectors must maintain a relative pose relationship and the two kinematic chains share a movable waist joint, even if a target pose lies within the single-arm reachable regions of the left and right arms separately, there is no guarantee that a common waist configuration exists such that both arms can reach their target poses simultaneously. Therefore, the workspace boundary in nonprehensile transportation is not merely a geometric envelope approached by a single end-effector. Instead, it results from the combined effects of dual-arm cooperative reachability, waist configuration, object orientation, and task constraints.

To address these issues, the core idea of this paper is to hierarchically decouple multiple constraints according to their physical roles and conflict relationships in nonprehensile transportation. Specifically, the constraints are divided into three levels. The top layer handles the cooperative workspace boundary constraints of the robot. The middle layer addresses task dynamic constraints, such as object non-slipping and non-toppling. The bottom layer processes intrinsic kinematic constraints of the robot, including dual-arm coordination, trajectory tracking, singularity avoidance, self-collision avoidance, and joint limits. This hierarchy is not a simple priority setting that places robot safety above task constraints. Instead, it reorganizes the order in which constraints take effect for nonprehensile transportation. The top layer first constrains user inputs through a safe velocity filter, allowing the system to decelerate smoothly with controlled acceleration before approaching the cooperative workspace boundary. This prevents direct conflicts between workspace boundary constraints and the object dynamic constraints handled in the middle layer. Based on the safe velocity output from the top layer, the middle layer generates pose-coupled trajectories that satisfy multi-object non-slipping and non-toppling constraints. The bottom layer then handles the remaining kinematic constraints, including dual-arm coordinated motion, self-collision avoidance, singularity avoidance, and configuration optimization within the null space of the task trajectory.

In this framework, the top-level safe velocity filter plays a key role in decoupling constraints. Unlike conventional hard boundary truncation methods, the proposed safe velocity filter based on an improved CBF explicitly introduces a maximum acceleration constraint during deceleration. This allows the robot to achieve smooth and controlled deceleration when approaching workspace boundaries, thereby reducing the risk of abrupt orientation changes and object toppling. Compared with conventional linear CBFs, this method avoids the tradeoff between overly conservative deceleration and excessive acceleration peaks at the activation instant. It maintains a larger effective operational workspace while keeping the object forces smooth during nonprehensile transportation. In addition, to address the difficulty of analytically describing the cooperative workspace boundaries of the robot, this paper designs a reachability determination method based on a differentiable reachability surrogate model. This method estimates the cooperative workspace boundary distance along the current motion direction in real time, providing boundary information for the top-level velocity filter.

Based on the above hierarchical decoupling mechanism, the bottom-level controller no longer needs to simultaneously handle the direct conflict between workspace boundary constraints and object dynamic constraints. Instead, it can track the object trajectory and achieve dual-arm coordinated motion through quadratic programming while satisfying the task trajectory generated by the middle layer. Self-collision avoidance, singularity avoidance, joint limits, and configuration optimization are then realized in the task null space. Accordingly, this paper constructs a Multiple-Constraint Safety-Critical Control Framework (MC-SCCF) for humanoid robot teleoperation in nonprehensile transportation, as shown in Figure 1. The proposed framework enables coordinated processing of workspace boundary constraints, task dynamic constraints, and intrinsic kinematic constraints of the robot.

The primary contributions of this paper are summarized as follows:

• A MC-SCCF is proposed for humanoid robot nonprehensile transportation. Different from conventional priority-based designs that simply place robot intrinsic safety constraints above task constraints, this paper decouples multiple constraints into three hierarchical levels according to the conflict relationships among workspace boundary constraints, object dynamic constraints, and intrinsic kinematic constraints of the robot. The top layer is responsible for the intrinsic safety of the robot under workspace constraints. It handles cooperative workspace boundaries through a safe velocity filter and isolates the direct impact of boundary constraints on task dynamic constraints. The middle layer generates pose-coupled reference trajectories that satisfy non-slipping and non-toppling requirements, thereby enforcing object dynamic constraints and ensuring task safety. The bottom layer handles the remaining kinematic constraints and task execution. While satisfying joint and manipulability constraints, it achieves self-collision avoidance, coordinated motion, and configuration optimization. This framework provides a systematic method for coordinating multiple hard constraints in humanoid robot nonprehensile transportation.
• A top-level safe velocity modulation strategy is proposed to handle the workspace boundaries of humanoid robots. To address the acceleration discontinuities caused by conventional hard boundary truncation and the tradeoff between conservative deceleration and orientation jitter in conventional linear CBFs, this paper designs an improved CBF based on acceleration constraints. This method enables the robot to decelerate smoothly with controlled acceleration when approaching workspace boundaries, thereby keeping the object forces and orientation variations more stable. In addition, to address the difficulty of analytically describing the cooperative workspace boundaries of humanoid dual arms, this paper proposes a boundary determination method based on a differentiable reachability surrogate model, which enables real-time estimation of the cooperative workspace boundary distance.
• The effectiveness of the proposed MC-SCCF is validated through experiments. Teleoperation experiments of multi-object nonprehensile transportation are designed and conducted on a physical humanoid robot platform. The experimental results demonstrate that this multilayered control framework can simultaneously guarantee the task dynamic constraints and the robot’s intrinsic kinematic constraints, thereby enhancing operational safety and smoothness while improving the human–robot interaction experience.

2. Related Work

2.1. Determination of Workspace Boundaries and Target Reachability Analysis

When humanoid robots execute nonprehensile transportation tasks, accurately evaluating the reachability of target poses and determining workspace boundaries are prerequisites for ensuring system safety and task success [6,7]. Traditional methods for determining workspace boundaries primarily include the Monte Carlo method [8], geometric envelope scanning [9], and numerical tracking methods based on Jacobian matrix singularity analysis [10,11]. These methods can macroscopically delineate the furthest theoretical physical boundaries reachable by the robot’s end-effectors [12]. For dual-arm nonprehensile transportation tasks, merely obtaining a rough outer envelope surface is insufficient [13,14]. During practical teleoperation, the robot frequently needs to perform precise pose adjustments near the workspace boundaries. In such cases, accurately evaluating the reachability of specific target poses in real time becomes critical.

Reachability evaluation for dual-arm coordinated tasks is subject to relative pose constraints, which restrict the effective workspace. Furthermore, because the dual arms of a humanoid robot share a movable waist joint, the two kinematic chains exhibit highly coupled characteristics. Every movement of the waist simultaneously affects the workspaces of both arms [15,16]. Currently, there are three primary strategies for evaluating target pose reachability. The first category is optimization-based numerical inverse kinematics, such as whole-body control (WBC) methods [17,18,19]. Although this approach can handle complex dual-arm coordination and waist coupling constraints through nonlinear optimization, its solution process depends on the selection of initial values [20]. If used solely for preliminary reachability queries, this method is not only time-consuming but also prone to falling into local minima, leading to misjudgments [21,22,23,24]. The second category comprises algebraic or geometric analytical methods [25]. These methods exhibit fast computational speeds but typically require the robot to possess specific structures, such as a spherical wrist, to achieve kinematic decoupling. Therefore, they are difficult to generalize directly to the highly redundant upper-body systems of humanoid robots [26]. The third category is the currently widely applied data-driven reachability map method [27,28,29,30,31]. While this method achieves fast online querying by calculating and recording the reachability probability within Cartesian space voxels offline, extending it to dual-arm coupled configurations with a waist joint requires traversing every possible waist configuration for querying, which is computationally inefficient in practical applications [32,33].

In summary, when dealing with the highly coupled dual-arm systems of humanoid robots with a waist, existing reachability analysis methods frequently fall into a conflict between computational efficiency and evaluation accuracy, making it difficult to meet the demands of teleoperation tasks. Designing a real-time and accurate method for evaluating the reachability of cooperative dual-arm workspace boundaries remains a challenge in current teleoperation control frameworks.

2.2. Robot Intrinsic Constraint Processing Methods in Teleoperation

In humanoid robot teleoperation, ensuring the intrinsic safety of the robot (e.g., joint limits, self-collision avoidance, and singularity avoidance) is the foundation for stable system operation. Current solutions are primarily divided into two categories: feedback-based assistive guidance and optimization-based local autonomous control.

Feedback-based assistive guidance aims to enhance the operator’s perception of the environment and constraints through multimodal information, thereby guiding the operator to make correct decisions. Regarding force feedback, virtual fixtures are widely applied to prevent the manipulator from colliding with obstacles and to avoid self-collisions by establishing repulsive force fields around obstacles and between robot links [34,35]. Metrics related to robot reachability maps and manipulability are utilized to establish attractive, repulsive, or damping forces. These forces guide the operator to actively avoid robot singular configurations and stay clear of dangerous areas such as workspace boundaries [36,37,38,39]. In addition, tactile feedback conveys the contact state with the interacting object through vibration or electrical stimulation, or it alerts the operator to dangerous situations using stimuli of varying intensities and frequencies as an alternative to force feedback [40,41]. Visual feedback provides the operator with a global perspective through the integration of digital model simulations, predicted trajectories, AR/VR systems, and multimodal information, intuitively presenting potential collision risks to the operator in a feedforward manner [42,43].

Force and tactile feedback impose stringent requirements on hardware equipment, communication latency, and force algorithms. Inappropriate force guidance may interfere with the operator’s normal tasks. More importantly, feedback-based assistance merely serves as a cue. Ultimate safety still depends on the operator’s experience and reaction speed, making it difficult to provide deterministic safety guarantees during highly dynamic interactions.

In contrast, optimization-based local autonomous control has become the current mainstream trend. In this approach, algorithms directly manage constraint processing. Its core idea is to model the control problem as a constrained optimization problem, typically QP. Within this framework, joint limits, velocity boundaries, and obstacle avoidance requirements are transformed into mathematical inequality constraints, ensuring that the generated control commands lie strictly within the feasible region [4,5,44]. Recently, control barrier functions (CBFs) and control Lyapunov functions (CLFs), serving as effective tools for constructing safe sets and ensuring stability [45,46,47], are frequently integrated into optimization frameworks to enhance performance.

In teleoperation research focusing on nonprehensile transportation, some researchers have attempted to combine these two approaches. Selvaggio et al. [48] proposed a shared control architecture that introduces friction cone constraints into the QP to handle object dynamics while simultaneously generating virtual forces based on position deviations to guide the user. Muchacho et al. [49,50] processed task constraints via model predictive control (MPC) and indicated the robot’s intrinsic motion limitations through force feedback. In our previous work [51,52], we proposed a shared teleoperation framework based on virtual object model predictive control (VO-MPC). This method resolves the algorithm’s dependence on precise object parameters and enhances the system’s robustness to unknown objects by transforming complex multi-object constraints into the boundary constraints of a single virtual offset object.

Nevertheless, the aforementioned studies are primarily based on single-manipulator systems, whose kinematic constraints are relatively simple. Directly migrating these methods to humanoid robot nonprehensile transportation tasks presents difficulties. The upper-body system of a humanoid robot possesses high kinematic redundancy, resulting in complex intrinsic kinematic constraints. Concurrently, nonprehensile transportation tasks involve strict dynamic constraints. A coupling relationship exists between the object’s position and orientation trajectories, and the object’s balance is highly sensitive to acceleration and pose. These two types of constraints frequently conflict during complex motions. Achieving compatibility among multiple hard constraints through weight allocation or soft constraint processing is difficult. Therefore, existing research has yet to resolve this challenge: how to construct a hierarchically decoupled control framework that achieves the unified integration of task execution and system safety while satisfying multiple physical constraints.

3. Safe Velocity Modulation Mechanism

Within the proposed MC-SCCF, the primary objective of the top layer is to transform the desired velocities input by the operator into safe velocity commands. To address workspace boundary constraints, this section formulates a safe velocity filter that is minimally invasive to the operator’s intent. This filter utilizes an improved CBF as the optimization constraint. The improved CBF employs a piecewise function, enabling smooth, acceleration-controlled deceleration as the manipulator’s end-effector approaches the workspace boundaries. Furthermore, to resolve the challenges of target reachability evaluation and workspace boundary distance calculation, a boundary evaluation method based on a differentiable reachability surrogate model is developed.

3.1. Safe Velocity Filter Design

Let ${\dot{x}}_{usr}$ denote the desired velocity command input by the operator. $x_{fil}$ and ${\dot{x}}_{fil}$ represent the position and velocity of the safe velocity filter output of the transported object in the task space, respectively, and $x$ denotes the actual position of the object. We formulate the following real-time QP problem to achieve safety filtering by solving for the optimal control input $u^{*}$, which represents the desired acceleration ${\ddot{x}}_{fil}$:

$$\begin{array}{c}{u}^{*}=\arg \underset{u}{\min }\Vert {\dot{x}}_{fil}-{\dot{x}}_{usr}{\Vert }^2\\ \mathrm{s}.\mathrm{t}. \dot{h}\left(x\right)\ge -\alpha \left(h\left(x\right)\right)\\ {\dot{x}}_{fil}=f\left(x_{fil},u\right)\end{array}$$

In this formulation, $h\left(x\right)$ represents the signed distance from the object to the workspace boundary along the direction of the current desired velocity. Within the proposed multilayer optimization framework, the middle and bottom layers ensure that $x$ tracks $x_{fil}$ in real time. The term $f\left(x_{fil},u\right)$ denotes the model relating the end-effector velocity to the input. The core of this framework is the inequality constraint $\dot{h}\left(x\right)\ge -\alpha \left(h\left(x\right)\right)$, which dictates the braking characteristics of the system as it approaches the boundaries.

The definition of $h\left(x\right)$ is further clarified as follows. Since the workspace boundary is generally a complex surface and the interior of the workspace is connected, $h\left(x\right)$ is defined as the boundary distance along the current motion direction. Specifically, a ray is constructed from the current position of the object along its current velocity direction. This ray may intersect the workspace boundary at one or more points, and $h\left(x\right)$ is defined as the distance from the current object position to the nearest intersection. Therefore, when the velocity direction of the object remains unchanged, the intersection between the ray and the workspace boundary is fixed, and $\dot{h}\left(x\right)=-\Vert \dot{x}\Vert$, where $\dot{x}$ denotes the actual velocity of the object. In this case, $h\left(x\right)$ continuously decreases as the object moves. Since the object always remains inside the workspace, $h\left(x\right)\ge 0$ is always satisfied. When the object position or velocity direction changes, the ray determined by the current position and velocity direction also changes, and its intersection with the workspace boundary changes accordingly. Thus, $h\left(x\right)$ is redefined with respect to the current motion direction. Under this definition, $h\left(x\right)$ does not describe whether the object moves away from a fixed workspace boundary. When the velocity direction changes, what changes is the workspace boundary approached by the current motion.

3.2. Improved CBF Based on Acceleration Constraints

In handling workspace boundary constraints, CBFs provide an effective mechanism for ensuring forward invariance by modulating the system state derivative. For a given safe set $\mathcal{C}=\left\{x\in {ℝ}^n\mid h\left(x\right)\ge 0\right\}$, the function $\alpha \left(h\left(x\right)\right)$ in conventional CBF design is typically chosen as a linear class $\mathcal{K}$ function, $\gamma h\left(x\right)$, where $\gamma >0$ is a constant. When the barrier constraint is activated, the system velocity toward the boundary decays exponentially and asymptotically reaches zero at the boundary.

However, this linear class $\mathcal{K}$ function possesses limitations in nonprehensile transportation tasks. In such tasks, the object’s orientation is coupled with its acceleration. When an object approaches the boundary at a certain velocity, the instantaneous activation of the CBF constraint can generate a large acceleration peak, with an acceleration norm of $\gamma \left|\dot{h}\right|$, leading to the object tilting and orientation jitter. Reducing $\gamma$ to mitigate orientation jitter would cause the robot to restrict its velocity at a significant distance from the boundary, thereby compressing the effective workspace and reducing operational efficiency.

To address this, we design an improved CBF based on acceleration constraints, aimed at ensuring the controllability and boundedness of the object’s orientation during deceleration. The improved class $\mathcal{K}$ function is defined as follows:

$$\dot{h}(x)\ge \left\{\begin{array}{l}-\sqrt{2a_{\max }\left(h\left(x\right)-0.5h_t\right)},\hfill \\ -\gamma h\left(x\right),\hfill \end{array}\right.\begin{array}{c}h\left(x\right)\ge h_t,\\ 0\le h\left(x\right)<h_t,\end{array}$$

where $a_{\max }$ denotes the maximum allowable acceleration during the deceleration phase, and $h_t$ is the switching threshold used to smoothly connect the piecewise function. To ensure constraint continuity and avoid step signals at the switching point $h_t$, the gain must satisfy the matching condition $\gamma =\sqrt{a_{\max }/h_t}$.

Proposition 1.

Consider the system state in the safe set $\mathcal{C}=\left\{x\in {ℝ}^n\mid h\left(x\right)\ge 0\right\}$. If the control input always satisfies the improved control barrier function constraint $\dot{h}\left(x\right)\ge -\alpha \left(h\left(x\right)\right)$, where

$$\alpha \left(h\right)=\left\{\begin{array}{l}\sqrt{2a_{\max }\left(h-0.5h_t\right)},\hfill \\ \gamma h,\hfill \end{array}\right.\begin{array}{c}h\ge h_t,\\ 0\le h<h_t,\end{array}$$

 and $\gamma =\sqrt{a_{\max }/h_t}$, then the safe set $\mathcal{C}$ is forward invariant.

Proof. 

When $0\le h<h_t$, we have $\alpha \left(h\right)=\gamma h$. Since $\gamma =\sqrt{a_{\max }/h_t}>0$, this segment is strictly increasing and satisfies $\alpha \left(0\right)=0$. □

When $h\ge h_t$, we have $\alpha \left(h\right)=\sqrt{2a_{\max }\left(h-0.5h_t\right)}$. Since $a_{\max }>0$, its derivative is ${\alpha }^{\prime }\left(h\right)=a_{\max }/\sqrt{2a_{\max }\left(h-0.5h_t\right)}>0$. Thus, this segment is also strictly increasing.

At the switching point $h=h_t$, the left and right limits of the two segments and their derivatives satisfy

$$\begin{array}{c}\alpha \left(h_t^{-}\right)=\alpha \left(h_t^{+}\right)=\sqrt{a_{\max }{h}_t}\\ {\alpha }^{\prime }\left(h_t^{-}\right)={\alpha }^{\prime }\left(h_t^{+}\right)=\sqrt{a_{\max }/h_t}\end{array}$$

Therefore, $\alpha \left(h\right)$ is continuously differentiable for $h\ge 0$. It also satisfies $\alpha \left(0\right)=0$, and $\alpha \left(h\right)>0$ holds for any $h>0$.

Consider the comparison system: $\dot{z}\left(t\right)=-\alpha \left(z\left(t\right)\right),z\left(0\right)=h\left(x\left(0\right)\right).$

Since $\alpha \left(0\right)=0$ and $\alpha \left(z\right)>0$ for any $z>0$, $z\left(t\right)$ cannot cross from the nonnegative half-axis to the negative half-axis. According to the comparison lemma, we have

$$h\left(x\left(t\right)\right)\ge z\left(t\right)\ge 0$$

Thus, as long as the initial state lies in the safe set, the system trajectory remains in the safe set, and the workspace boundary is not violated. This completes the proof.

Furthermore, when the object approaches the workspace boundary and the barrier constraint is active, we have

$$\dot{h}=-\alpha \left(h\right),\Vert \dot{x}\Vert =\alpha \left(h\right)$$

Therefore, the acceleration magnitude during deceleration can be written as

$$\Vert \ddot{x}\Vert =\left|{\alpha }^{\prime }\left(h\right)\alpha \left(h\right)\right|$$

When $h\ge h_t$, we obtain $\Vert \ddot{x}\Vert =a_{\max }$.

When $0\le h<h_t$, we obtain $\Vert \ddot{x}\Vert =a_{\max }h/h_t\le a_{\max }$.

Thus, throughout the boundary-approaching process, the acceleration during deceleration always satisfies $\Vert \ddot{x}\Vert \le a_{\max }$.

The improved class $\mathcal{K}$ function partitions the boundary approach process into a constant deceleration phase and an exponential convergence phase:

Constant Deceleration Phase ($h\left(x\right)\ge h_t$): When $\Vert \dot{x}\Vert >\sqrt{2a_{\max }\left(h\left(x\right)-0.5h_t\right)}$, the system undergoes constant deceleration subject to the constraint of $a_{\max }$.

Exponential Convergence Phase ($0\le h\left(x\right)<h_t$): When $\Vert \dot{x}\Vert >h\left(x\right)\sqrt{a_{\max }/h_t}$, the constraint switches to the linear CBF form, and the system velocity decays exponentially.

Assuming a constant velocity direction for the object, when $\Vert \dot{x}\Vert \ge \sqrt{a_{\max }{h}_t}$, the object undergoes two stages of deceleration, and the total deceleration distance is:

$$D_{dec}=\frac{{\Vert \dot{x}\Vert }^2}{2a_{\max }}+\frac{1}{2}{h}_t.$$

(1)

When $\Vert \dot{x}\Vert <\sqrt{a_{\max }{h}_t}$, the deceleration process remains in the exponential convergence phase, and the deceleration distance is:

$$D_{dec}=\Vert \dot{x}\Vert \sqrt{\frac{h_t}{a_{\max }}}.$$

(2)

By introducing a maximum acceleration constraint, the improved CBF ensures stable forces on the object, with controlled acceleration and pose during deceleration. As the robot nears the boundary, the switch to the linear CBF ensures smoother deceleration. This leverages the asymptotic convergence of the exponential function to eliminate chattering phenomena as the velocity approaches zero, ensuring the robot stops smoothly at the boundary.

3.3. Workspace Boundary Determination

To achieve velocity modulation, the system must calculate the distance $h\left(x\right)$ from the current position to the workspace boundary in real time. Under the constraint of the CBF, the object will stop at the workspace boundary. Therefore, $h\left(x\right)$ is defined as the Euclidean distance between the current position and the furthest reachable position in the current velocity direction with a static orientation.

During dual-arm cooperative tasks, workspace reachability can be defined as whether there exists a waist configuration $q_{waist}^{*}$ that makes the target poses of both end-effectors simultaneously reachable. Querying by traversing all waist configurations cannot satisfy real-time requirements. Hence, we design a target reachability evaluation method utilizing a differentiable reachability surrogate model. The reachability of target points is iteratively determined using the bisection method to achieve a fast and accurate approximation of the workspace boundary.

3.3.1. Reachability Determination

In dual-arm cooperative tasks, the reachability of target poses is restricted not only by kinematic coupling but also by the robot’s self-collision constraints. Therefore, prior to conducting the kinematic reachability search, the shortest distance between the bounding volumes of the dual-arm end-effectors and the bounding volume of the robot base at the target pose is calculated. If a collision occurs, the target is directly determined as unreachable. When no collision exists, reachability determination at the kinematic level is performed.

Considering the kinematic independence of the two arms relative to their respective shoulder bases, we reduce the dimensionality of the problem. First, the true discrete reachability maps of both arms relative to the shoulder coordinate systems are established, and a continuously differentiable reachability surrogate model is trained accordingly. Subsequently, utilizing the gradient information of the surrogate model, the optimal waist configuration is determined to maximize the reachability of the dual-arm target positions. Finally, according to the optimal configuration, the reachability of the target pose is determined within the true reachability map. The reachability determination process is summarized in Algorithm 1.

The discrete reachability map divides the space into three-dimensional voxels with an edge length of $d_r$, and the centroid of each voxel represents its position. The selection of the grid resolution $d_r$ requires a tradeoff between storage cost and determination accuracy. An excessively large $d_r$ introduces notable discretization errors, leading to inaccurate boundary determination. In contrast, an excessively small $d_r$ causes the number of voxels to increase cubically, which increases both the storage burden and the size of the training dataset. Considering the robot scale, task accuracy requirements, and storage cost, $d_r$ is set to 0.05 m.

The axis-angle representation is used to specify orientations. Specifically, $n_a$ unit rotational axes uniformly distributed in space are defined, and $n_c$ rotational angles uniformly distributed within $\left[0,\pi \right]$ are specified. These axes and angles jointly constitute $n_a{n}_c$ orientations. At the position of each voxel, the inverse kinematics for all orientations are calculated sequentially. All poses are encoded, and the existence of their inverse kinematic solutions is recorded. Compared with uniform longitude-latitude sampling, spherical sampling ensures an approximately uniform distribution of directions on the sphere and avoids statistical bias caused by excessive sampling density near the polar regions. In this section, $n_a=40$ and $n_c=18$ are used, meaning that each voxel position covers 720 discrete orientations.

The reachability of position $P$ is defined as:

$$R\left(P\right)={\displaystyle \frac{r\left(P\right)}{n_a{n}_c}}\times 100\%\in \left[0,1\right]$$

where $r\left(P\right)$ denotes the number of existing inverse kinematic solutions at this position. Let $P_l$ and $P_r$ denote the positions of the left arm end-effector relative to the left shoulder base and the right arm end-effector relative to the right shoulder base, respectively. $R_l\left(P_l\right)$ and $R_r\left(P_r\right)$ represent the reachability maps of the left and right arms, respectively. The reachability maps of the dual arms are illustrated in Figure 2.

We employ a fully connected neural network (FCNN) to train the reachability surrogate model, which learns the nonlinear mapping ${\widehat{R}}_i\left(P_i\right)$ from the end-effector position $P_i$ to the reachability index $R_i\left(P_i\right)$, where $i\in \left\{l,r\right\}$. The input is the three-dimensional end-effector position expressed in the corresponding shoulder base frame, and the output is the continuous reachability prediction at that position. In each shoulder coordinate frame, the original sampling space is discretized into ${\left(2N+1\right)}^3$ three-dimensional grid points, where $N=1 \mathrm{m}/d_r=20$, given that the manipulator length is 1 m. Therefore, each single-arm reachability dataset contains 68,921 sampled points.

Before training, the robot joints are set to the zero configuration. The end-effector positions in the left- and right-arm reachability maps are first transformed into their corresponding shoulder base frames. Invalid samples with a reachability index of zero are then removed, and only spatial points with valid inverse kinematic solutions are retained for training. Each sample consists of a three-dimensional end-effector position and the corresponding reachability index. The valid datasets contain 30,757 samples for the left arm and 30,753 samples for the right arm, which are randomly divided into training and validation sets at a ratio of 70% and 30%, respectively.

The FCNN contains three hidden fully connected layers with 256, 128, and 64 neurons, respectively. The Swish activation function is used in all hidden layers, and batch normalization layers are added after the first two hidden layers to improve training stability. The network is trained using the Adam optimizer with a maximum of 150 epochs, a mini-batch size of 512, and an initial learning rate of 0.001. A piecewise learning-rate decay strategy is adopted, in which the learning rate is reduced by a factor of 0.5 every 30 epochs. During training, $1\times {10}^{-4}$ L2 regularization, gradient clipping, and validation-based early stopping are applied to reduce the risk of overfitting and improve the generalization capability of the model. After training, the surrogate model provides a continuous fit to the discrete reachability data, with $R^2=97.1\%$ on the test set. This continuously differentiable model supports the subsequent reachability gradient calculation and waist configuration optimization. The fitting results are shown in Figure 3. In the illustrated cross-sections, the fitting error is less than 0.15.

Utilizing this continuously differentiable reachability surrogate model, the gradient of reachability with respect to position, denoted as $\nabla {\widehat{R}}_i\left(P_i\right)$, can be calculated. This gradient indicates the direction of the steepest increase in reachability. Consequently, the search for the optimal waist configuration $q_{waist}^{*}$ is modeled as a gradient-driven iterative optimization process.

For a given waist configuration, the reachability gradient $\nabla {\widehat{R}}_i\left(P_i\right)$ at the target position $P_i$ can be regarded as a virtual attractive force pulling the position toward the direction of increasing reachability. The reaction force of this virtual attractive force $-\nabla {\widehat{R}}_i\left(P_i\right)$, acts on the shoulder base. If the waist joint is set to damping mode, it will move under the influence of $-\nabla {\widehat{R}}_i\left(P_i\right)$ toward a configuration that increases the reachability of the target position. The iterative formula for the waist joint angle is defined as:

$$\Delta q_{waist}=-{\displaystyle \frac{1}{\beta }}\left(J_{waist,l}^{\mathrm{T}}\nabla {\widehat{R}}_l\left(P_l\right)+J_{waist,r}^{\mathrm{T}}\nabla {\widehat{R}}_r\left(P_r\right)\right).$$

where $\beta$ is the damping coefficient, and $\Delta q_{waist}$ represents the update step size. $J_{waist,i}$ denotes the Jacobian matrix of the waist joint, with $i\in \left\{l,r\right\}$.

The iteration terminates when $\Vert \Delta q_{waist}\Vert <\epsilon$ or the maximum number of iterations, $\max \_\mathrm{iters}$, is reached. Utilizing $q_{waist}^{*}$, the system calculates the exact poses of the dual-arm target poses within their respective shoulder coordinate systems and performs a nearest neighbor query within the true discrete reachability maps. If inverse kinematic solutions exist for the nearest neighbors of both target poses, the target is determined as reachable; otherwise, it is determined as unreachable.

Algorithm 1: Reachability determination of target pose

Input:  ${}^{S}P_l,{}^{S}P_r$: Target positions of left and right end-effectors in world frame; ${}^S\Phi _l,{}^S\Phi _r$: Target orientations of left and right end-effectors in world frame; $\epsilon$: Convergence threshold. Output:  Reachability of target poses. 1: If $\mathrm{ComputeMinDistance}({}^{S}P_l,{}^{S}P_r,{\mathcal{V}}_{base})<0$ Then 2:          Return false 3: End If 4: $q_{waist}\leftarrow 0$ 5: for $i=1:\max \_\mathrm{iters}$ do 6: // Step 1: Compute EE positions relative to shoulder frames 7: $P_l\leftarrow \mathrm{Kinematics}(q_{waist},{}^{S}P_l)$, $P_r\leftarrow \mathrm{Kinematics}(q_{waist},{}^{S}P_r)$ 8: // Step 2: Compute Virtual Forces and Map to Waist Joints 9: ${\tau }_{waist}\leftarrow -J_{waist,l}^{\mathrm{T}}\nabla {\widehat{R}}_l\left(P_l\right)-J_{waist,r}^{\mathrm{T}}\nabla {\widehat{R}}_r\left(P_r\right)$ 10: // Step 3: Enforce joint limits and Update Waist Angle 11: $\Delta q_{waist}\leftarrow \max \left(\min \left({\tau }_{waist}/\beta ,{\overline{q}}_{waist}-q_{waist}\right),{\underset{¯}{q}}_{waist}-q_{waist}\right)$ 12: $q_{waist}\leftarrow q_{waist}+\Delta q_{waist}$ 13: // Step 4: Check Convergence 14:        If $\Vert \Delta q_{waist}\Vert <\epsilon$ OR $i==\max \_\mathrm{iters}$ Then 15:             $q_{waist}^{*}\leftarrow q_{waist}$ 16:        Break for 17:        End If 18: End for 19: // Step 5: Nearest Neighbor Query 20: $\left(P_l,{\Phi }_l\right)\leftarrow \mathrm{Kinematics}(q_{waist}^{*},{}^{S}P_l,{}^S\Phi _l)$, $\left(P_r,{\Phi }_r\right)\leftarrow \mathrm{Kinematics}(q_{waist}^{*},{}^{S}P_r,{}^S\Phi _r)$ 21: $({\widehat{P}}_l,{\widehat{\Phi }}_l)\leftarrow {\mathrm{NearestNeighbor}(\mathrm{Map}}_l,P_l,{\Phi }_l)$, $({\widehat{P}}_r,{\widehat{\Phi }}_r)\leftarrow {\mathrm{NearestNeighbor}(\mathrm{Map}}_r,P_r,{\Phi }_r)$ 22: ${\mathrm{isReachable}}_l\leftarrow {\mathrm{CheckIK}(\mathrm{Map}}_l,{\widehat{P}}_l,{\widehat{\Phi }}_l)$, ${\mathrm{isReachable}}_r\leftarrow {\mathrm{CheckIK}(\mathrm{Map}}_r,{\widehat{P}}_r,{\widehat{\Phi }}_r)$ 23: // Step 6: Reachability Decision 24: If ${\mathrm{isReachable}}_l$ AND ${\mathrm{isReachable}}_r$ Then 25:        Return true 26: Else 27:        Return false 28: End If

3.3.2. Boundary Distance Calculation

The purpose of calculating the distance $h\left(x\right)$ from the current position to the workspace boundary is to determine whether the CBF constraint needs to be applied to the current motion. Therefore, precise boundary searching is not required at all times. We improve computational efficiency through predictive pruning.

• Predictive Stopping Pose Detection (Pruning Step): First, the system calculates the Predicted Stopping Pose (PSP) based on the current motion state. The PSP is defined as the pose where the object would come to a complete stop along the current velocity direction if the CBF constraint were triggered at the current moment. According to (1) and (2), the position of the PSP is calculated as $x_{\mathrm{PSP}}=x+D_{dec}\frac{\dot{x}}{\Vert \dot{x}\Vert }$. The orientation of the PSP is defined as the static orientation of the object. Then, the reachability of the PSP is verified using the method described in Section 3.3.1. If the PSP is determined to be reachable, it indicates that the robot can safely stop within the workspace given its current velocity and braking capability. In this case, the CBF constraint will not be activated, and the calculation of the precise boundary distance $h\left(x\right)$ is unnecessary.
• Bisection-Based Boundary Approximation (Bisection Search): If the PSP is determined to be unreachable, it implies a risk of boundary violation, and the system initiates a precise search. A bisection search is performed along the ray connecting the current position and the PSP to iteratively sample and determine the reachability of each point. The iteration terminates when the length of the sampling interval is smaller than the spatial resolution $d_r$ of the offline reachability map.

4. Dynamic Constraint Processing Method Based on a Virtual Object

Within the middle layer of the MC-SCCF, the primary objective is to ensure that the transported object (or multiple objects on a tray) does not slip or topple when the robot executes nonprehensile transportation tasks. Nonprehensile transportation relies on contact forces and inertial forces to maintain stability. We adopt the dynamic constraint processing algorithm based on a virtual object proposed in our previous work [51,52]. The core idea of this algorithm is to transform complex multi-object constraints into the constraints of a single virtual offset object, combined with MPC to achieve online trajectory smoothing.

4.1. Construction of Virtual Objects

In nonprehensile transportation systems, the risk of object instability (slipping or toppling) is highly correlated with its relative position on the tray, geometric dimensions, and the friction coefficient of the contact surface. To achieve constraint dimensionality reduction, we extract the limit parameters from multiple real objects on the tray: the minimum base edge length $d=\min \left\{d_i\right\}$, the maximum height $h=\max \left\{h_i\right\}$, and the minimum friction coefficient $\mu =\min \left\{{\mu }_i\right\}$. Two special virtual objects are constructed:

• Virtual reference object O (for trajectory planning): This object is placed at the geometric center of the tray. The controller refers to object O for trajectory planning and ensures $C^4$ continuity of the position. Simultaneously, the center of mass (CoM) coordinate system of O is utilized as the center of rotation of the tray to calculate the orientation. Reference object O and offset object A share the same shape and mass.
• Virtual offset object A (for dynamic constraint analysis): The shape of offset object A is a rectangular prism with a square base, featuring a base edge length of $d$, a height of $h$, and a friction coefficient of $\mu$. Its mass is concentrated at its eight vertices, with a total mass of 1 kg. The maximum moment of inertia of the offset object is defined as $J_{\max }=\frac{1}{4}\left(d^2+h^2\right)$. The specific position of offset object A is arbitrary, provided that the distance between A and O is maximized. The distance between the offset object and the reference object is expressed as $\Vert P_A\Vert =\sqrt{R_{tray}^2+{\left(0.5h\right)}^2}$, where $R_{tray}$ denotes the maximum radius of the actual tray. Although the offset object is not necessarily located on the physical tray, it can be assumed that the tray is sufficiently large, allowing the offset object to be utilized for constraint calculation.

This configuration renders the offset object more susceptible to slipping and toppling than any real object on the tray. Under any motion state, as long as the offset object remains stable, all real objects on the tray are guaranteed to maintain stability. Therefore, the dynamic constraints of multiple objects are transformed into the constraints of a single offset object.

4.2. Dynamic Constraint Analysis Based on a Virtual Object

In nonprehensile transportation, the system counteracts tangential contact forces by adjusting the tray’s orientation. Thus, the object’s orientation and linear acceleration are highly coupled. During motion, the offset object and the reference object share the identical orientation, angular velocity, and angular acceleration; nevertheless, their linear accelerations differ. To ensure safety, offset object A must simultaneously satisfy the friction cone constraint and the zero moment point (ZMP) constraint. By neglecting high-order minor coupling terms, these two physical constraints can be directly transformed into a restriction on the angular acceleration $\Vert \dot{\mathsf{\Omega }}\Vert$ of reference object O:

$$\Vert \dot{\mathsf{\Omega }}\Vert \le \min \{\frac{\Vert \ddot{x}-g\Vert }{\Vert P_A\Vert }\sin {\theta }_0,\frac{\Vert \ddot{x}-g\Vert d}{2J_{\max }+\Vert P_A\Vert \sqrt{d^2+h^2}}\},$$

(3)

where $\ddot{x}$ indicates the linear acceleration of the reference object, $g$ represents the gravitational acceleration, and ${\theta }_0=\arctan \mu$.

4.3. Trajectory Smoothing Based on MPC

Input trajectories generated by human operators during teleoperation typically lack high-order continuity. To generate smooth trajectories that strictly satisfy the aforementioned angular acceleration boundaries, we formulated an MPC position controller structured with a cascade of five integrators. The state vector is defined as ${\mathit{X}}_{des}={\left[x_{des}^{\mathrm{T}},{\dot{x}}_{des}^{\mathrm{T}},{\ddot{x}}_{des}^{\mathrm{T}},{\stackrel{⃛}{x}}_{des}^{\mathrm{T}},{\stackrel{⃜}{x}}_{des}^{\mathrm{T}}\right]}^{\mathrm{T}}$. Here, $x_{des}\in {ℝ}^3$ denotes the desired position of the reference object, and $\mathit{u}\in {ℝ}^3$ represents the fifth derivative of $x_{des}$. The control objective is to calculate the optimal system input $\mathit{u}$ such that $x_{des}$ tracks $x_{fil}$, which is output by the top-layer controller detailed in Section 3.

To eliminate the tangential forces exerted on the object, the desired orientation of the tray, denoted by $\Phi =\alpha \widehat{n}$, is entirely determined by the acceleration ${\ddot{x}}_{des}$ of the reference object. Its rotational axis $\widehat{n}$ and rotational angle $\alpha$ are defined as follows:

$$\widehat{n}=\frac{{\ddot{x}}_{des}\times g}{\Vert {\ddot{x}}_{des}\times g\Vert },\alpha =\arccos \left({\displaystyle \frac{g^{\mathrm{T}}\left(g-{\ddot{x}}_{des}\right)}{\Vert g\Vert \Vert g-{\ddot{x}}_{des}\Vert }}\right).$$

Successive differentiation of the orientation reveals that the angular acceleration ${\dot{\mathsf{\Omega }}}_{des}$ of the reference object is dictated by ${\stackrel{⃜}{x}}_{des}$. The nonlinear angular acceleration limit $\Vert {\dot{\mathsf{\Omega }}}_{des}{\Vert }_{\max }$ in (3) can be mapped into real-time upper and lower bounds for the fourth derivative of the position within the MPC state vector:

$$\Vert {\stackrel{⃜}{x}}_{des}\Vert \le \left\{\begin{array}{l}{\Vert {\dot{\mathsf{\Omega }}}_{des}\Vert }_{\max }{\displaystyle \frac{\sqrt{\begin{array}{l}{\ddot{x}}_{des,1}^2+{\ddot{x}}_{des,2}^2\hfill \end{array}}}{\alpha }},\alpha \ne 0,\hfill \\ \Vert {\dot{\mathsf{\Omega }}}_{des}{\Vert }_{\max }\left|g_3-{\ddot{x}}_{des,3}\right|,\alpha =0,\hfill \end{array}\right.$$

(4)

where ${\ddot{x}}_{des}={\left[\begin{array}{ccc}{\ddot{x}}_{des,1}& {\ddot{x}}_{des,2}& {\ddot{x}}_{des,3}\end{array}\right]}^{\mathrm{T}}$ and $g={\left[\begin{array}{ccc}0& 0& g_3\end{array}\right]}^{\mathrm{T}}$.

The MPC optimization problem is formulated as follows:

$$\begin{array}{c}{\min }_{\mathit{u}}{\displaystyle {\sum }_{k=0}^{N-1}\left(\Vert {\mathit{X}}_{fil}\left(k+1\right)-{\mathit{X}}_{des}\left(k+1\right){\Vert }_{W_x}^2+\Vert \mathit{u}\left(k\right){\Vert }_{W_u}^2\right)},\\ \mathrm{s}.\mathrm{t}.:{\mathit{X}}_{\mathit{des}}\left(k+1\right)={\mathit{A}}_{\mathit{d}}{\mathit{X}}_{\mathit{des}}\left(\mathrm{k}\right)+{\mathit{B}}_{\mathit{d}}\mathit{u}\left(\mathrm{k}\right),\\ \left(4\right),\overline{u}\le \mathit{u}\left(k\right)\le \underset{¯}{u}.\end{array}$$

In this formulation, $N$ denotes the prediction horizon of the controller; ${\mathit{X}}_{fil}$ represents the desired state output by the top-layer controller, defined as ${\mathit{X}}_{fil}={\left[x_{fil}^{\mathrm{T}},{\dot{x}}_{fil}^{\mathrm{T}},{\ddot{x}}_{fil}^{\mathrm{T}}\right]}^{\mathrm{T}}$. The equation ${\mathit{X}}_{des}\left(k+1\right)={\mathit{A}}_{\mathit{d}}{\mathit{X}}_{des}\left(k\right)+{\mathit{B}}_{\mathit{d}}\mathit{u}\left(k\right)$ represents the discretized state-space model. The notation $\Vert \cdot {\Vert }_W^2={\left(\cdot \right)}^{\mathrm{T}}W\left(\cdot \right)$ is employed, where $W_x$ and $W_u$ indicate the weight coefficient matrices for the output and input, respectively.

5. Constraints and Optimization Objectives of Inverse Kinematics

This section maps task-space trajectories to the robot’s joint space utilizing a QP-based low-level controller. This approach ensures that the robot strictly satisfies joint limits and manipulability constraints while achieving self-collision avoidance, configuration optimization, and coordinated motion.

Let $q\in {ℝ}^n$ represent the generalized coordinates of the humanoid robot’s upper-body system, where $n$ indicates the degree of freedom (DoF) of the robot, encompassing the waist joint and the joints of the left and right arms. For each end-effector $i\in \{l,r\}$, the relationship between its task-space velocity ${\dot{\chi }}_i$ and the joint velocity $\dot{q}$ is expressed as follows:

$${\dot{\chi }}_i=J_i\left(q\right)\dot{q}.$$

where ${\dot{\chi }}_i\in {ℝ}^6$, and $J_i\left(q\right)\in {ℝ}^{6\times n}$ denotes the Jacobian matrix.

5.1. Task Constraints

5.1.1. Trajectory Tracking

In nonprehensile transportation tasks, stability is independent of the virtual reference object’s orientation about its Z-axis, which indicates the normal direction of the contact surface. Therefore, we relax the strict tracking requirement for the Z-axis orientation, constraining only the remaining five DoFs. The objective of orientation tracking shifts to aligning the actual Z-axis ($z_O$) of the reference object’s coordinate frame with the desired Z-axis ($z_{O_d}$).

As demonstrated in Figure 4, $\left\{S\right\}$ denotes the world frame, $\left\{O\right\}$ represents the coordinate frame of the virtual reference object, and $z_O$ indicates the Z-axis of the object’s coordinate frame. Similarly, $z_{O_d}$ denotes the Z-axis of the desired coordinate frame. A coordinate frame $\left\{O^{\prime }\right\}$ is introduced, which is rigidly attached to $\left\{O\right\}$, such that $z_{O^{\prime }}=z_O$, $y_{O^{\prime }}={\displaystyle \frac{z_O\times z_{O_d}}{\parallel z_O\times z_{O_d}\parallel }}$, and $x_{O^{\prime }}=y_{O^{\prime }}\times z_{O^{\prime }}$.

The orientation of $\left\{O^{\prime }\right\}$ is represented by the rotation matrix $R_{O^{\prime }}=[x_{O^{\prime }},y_{O^{\prime }},z_{O^{\prime }}]$. Using XYZ Euler angles, it is expressed as ${\Phi }_{O^{\prime },XYZ}={[{\Phi }_x,{\Phi }_y,{\Phi }_z]}^{\mathrm{T}}$. The orientation deviation Euler angles within $\left\{O^{\prime }\right\}$ are calculated as $\mathit{\Delta }{\Phi }_{XYZ}={[0,\alpha ,\mathrm{C}]}^{\mathrm{T}}$, where $\alpha$ indicates the angle between $z_O$ and $z_{O_d}$, and $C$ represents an arbitrary value. The tracking constraint transformed into joint velocities is formulated as:

$$J_{i,O}\left(q\right)\dot{q}={\lambda }_O{R}_{O^{\prime }}{E}_{XYZ}\left({\Phi }_{O^{\prime },XYZ}\right)\mathit{\Delta }{\Phi }_{XYZ},$$

(5)

where $J_{i,O}\left(q\right)\in {ℝ}^{3\times n}$ indicates the orientation component of $J_i\left(q\right)$. $E_{XYZ}\in {ℝ}^{3\times 3}$ represents the mapping between the XYZ Euler angle rates and the angular velocity vector, defined as:

$$E_{XYZ}=\left[\begin{array}{ccc}1& 0& \sin {\Phi }_y\\ 0& \cos {\Phi }_x& -\sin {\Phi }_x\cos {\Phi }_y\\ 0& \sin {\Phi }_x& \cos {\Phi }_x\cos {\Phi }_y\end{array}\right].$$

The tracking constraint for the object position is defined as:

$$J_{i,P}\left(q\right)\dot{q}={\lambda }_P\mathit{\Delta }P,$$

(6)

where $J_{i,P}\left(q\right)\in {ℝ}^{3\times n}$ denotes the position component of $J_i\left(q\right)$, and $\mathit{\Delta }P$ indicates the position deviation of the object.

5.1.2. Constant Relative Pose of Dual-Arm End-Effectors

Nonprehensile transportation tasks require that the dual-arm end-effectors maintain no relative motion; otherwise, the tray might fall, or the object might slip and lose contact. As a result, the relative pose between the end-effectors must remain constant. Transforming this relative pose constraint into joint velocity constraints yields:

$${\displaystyle \frac{\partial {\chi }_{rel}\left(q\right)}{\partial q}}\dot{q}=J_{rel}\left(q\right)\dot{q}=-{\lambda }_{rel}\left({\chi }_{rel}\left(q\right)-{\chi }_{rel}\left(q_0\right)\right).$$

(7)

In this formulation, ${\chi }_{rel}\left(q\right)$ denotes the relative pose of the end-effectors at joint angle $q$, while $q_0$ indicates the initial joint configuration. $J_{rel}\left(q\right)$ represents the relative Jacobian matrix, defined as $J_{rel}\left(q\right)=\mathsf{\Psi }\left(q\right)J\left(q\right)$, where $J\left(q\right)$ indicates the composite Jacobian matrix $J(q)={[J_l^{\mathrm{T}}(q)\hspace{1em}{J}_r^{\mathrm{T}}(q)]}^{\mathrm{T}}$. Following the methodology in [53], $\mathsf{\Psi }\left(q\right)$ is expressed as:

$$\Psi \left(q\right)=\{\begin{array}{c}\left[\begin{array}{cccc}\begin{array}{c}-R_l^{\mathrm{T}}\\ \mathbf{0}\end{array}& \begin{array}{c}{R}_l^{\mathrm{T}}S\left({}^{l}P_r\right)\\ -R_l^{\mathrm{T}}\end{array}& \begin{array}{c}{R}_l^{\mathrm{T}}\\ \mathbf{0}\end{array}& \begin{array}{c}\mathbf{0}\\ R_l^{\mathrm{T}}\end{array}\end{array}\right],l\mathrm{relative}\mathrm{to}r,\\ \left[\begin{array}{cccc}\begin{array}{c}{R}_r^{\mathrm{T}}\\ \mathbf{0}\end{array}& \begin{array}{c}\mathbf{0}\\ R_r^{\mathrm{T}}\end{array}& \begin{array}{c}-R_r^{\mathrm{T}}\\ \mathbf{0}\end{array}& \begin{array}{c}{R}_r^{\mathrm{T}}S\left({}^{r}P_l\right)\\ -R_r^{\mathrm{T}}\end{array}\end{array}\right],r\mathrm{relative}\mathrm{to}l,\end{array}$$

where $R_i$($i\in \{l,r\}$) indicates the absolute orientation of the end-effectors, ${}^{l}P_r$ represents the position of the right end-effector relative to the left, and $S{(}^l{P}_r)$ denotes the skew-symmetric matrix of ${}^{l}P_r$.

5.1.3. Singularity Avoidance

To maintain adequate kinematic dexterity, the robot must continuously avoid singular configurations. Considering that formulating manipulability directly as an optimization objective might conflict with other objectives, we opt to constrain its lower bound. This approach guarantees system flexibility while releasing substantial optimization space. Utilizing a CBF, this requirement is modeled as an inequality constraint to ensure that the manipulability measure $M\left(q\right)$ consistently remains above a safety threshold $M_{\min }$:

$$\dot{M}\left(q\right)\ge -{\lambda }_M\left(M\left(q\right)-M_{\min }\right).$$

(8)

where $\dot{M}\left(q\right)=M\left(q\right){\displaystyle \sum _{i=1}^n\mathrm{tr}\left(\frac{\partial J\left(q\right)}{\partial q_i}{J}^{†}\left(q\right)\right){\dot{q}}_i}$, $M\left(q\right)=\sqrt{\det \left(J\left(q\right)J^{\mathrm{T}}\left(q\right)\right)}$.

5.1.4. Joint Physical Constraints

Let the joint position limits of the robot be denoted as $[\underset{¯}{q},\overline{q}]$. Considering the control period $T_s$, the joint position constraints for the subsequent time step can be transformed into velocity constraints at the current moment:

$$\underset{¯}{q}-q\le \dot{q}{T}_s\le \overline{q}-q$$

(9)

5.2. Optimization Objectives

5.2.1. Self-Collision Avoidance

Employing an artificial potential field (APF) strategy for self-collision avoidance ensures safety during the dual-arm collaboration process [54,55]. Spherical bounding volumes are utilized to approximate the geometric shape of the robot [56,57]. When the minimum Euclidean distance between bounding volumes is less than a safety threshold, a repulsive force $F_{ij}$ is generated along the line connecting their centers:

$$F_{ij}=\left\{\begin{array}{cc}\eta \left({\displaystyle \frac{1}{d_{ij}}}-{\displaystyle \frac{1}{d_{safe}}}\right){\displaystyle \frac{1}{d_{ij}^2}}{n}_{ij},& \mathrm{if}{d}_{\mathit{ij}}<d_{\mathit{safe}},\\ 0,& \mathrm{if}{d}_{\mathit{ij}}\ge d_{\mathit{safe}},\end{array}\right.$$

where $d_{ij}$ indicates the minimum distance between bounding volumes $i$ and $j$, $d_{safe}$ represents the safety threshold, $\eta$ is the repulsion gain coefficient, and $n_{ij}$ denotes the unit vector indicating the direction of repulsion.

Assuming the joints possess viscous damping characteristics with a damping coefficient $B$, the desired avoidance velocity induced by the repulsive force is formulated as:

$${\dot{q}}_{avoid}=B^{-1}{\displaystyle \sum J_{ij}^{\mathrm{T}}{F}_{ij}}.$$

Subsequently, ${\dot{q}}_{avoid}$ is incorporated into the cost function as the desired velocity of a secondary task, guiding the robot configuration away from self-collision regions.

5.2.2. Waist Joint Regulation

To ensure more anthropomorphic waist motion and prevent the waist from remaining in extreme pitch states for extended periods, an optimization objective is introduced to return the waist pitch joint to its nominal upright position $q_{nom}$. A negative feedback control law featuring saturation characteristics is designed to calculate the desired velocity ${\dot{q}}_{waist}$ of the waist joint:

$${\dot{q}}_{waist}=\left\{\begin{array}{cc}\begin{array}{c}{\overline{\dot{q}}}_{waist},\\ {\dot{\underset{¯}{q}}}_{waist},\\ -{\lambda }_{waist}\left(q_{waist}-q_{nom}\right),\end{array}& \begin{array}{l}\mathrm{if}-{\lambda }_{\mathrm{waist}}\left(q_{\mathit{waist}}-q_{\mathit{nom}}\right)\ge {\overline{\dot{q}}}_{\mathit{waist}},\hfill \\ \mathrm{if}-{\lambda }_{\mathrm{waist}}\left(q_{\mathit{waist}}-q_{\mathit{nom}}\right)\le {\underset{¯}{\dot{q}}}_{\mathit{waist}},\hfill \\ \mathrm{otherwise}.\hfill \end{array}\end{array}\right.$$

In this formulation, ${\lambda }_{waist}$ denotes the regulation gain, while ${\overline{\dot{q}}}_{waist}$ and ${\underset{\_}{\dot{q}}}_{waist}$ represent the upper and lower bounds of the saturation velocity, respectively, with typically ${\overline{\dot{q}}}_{waist}=-{\underset{\_}{\dot{q}}}_{waist}$. The design of this saturation block aims to prevent excessively large regulation velocities from causing disturbances to the primary tasks.

5.3. Inverse Kinematics Solution

Based on the aforementioned formulations, the optimal joint velocity vector $\dot{q}$ is obtained by solving the following problem:

$$\underset{\dot{q}}{\min } J\left(\dot{q}\right)={\Vert \dot{q}-{\dot{q}}_{avoid}\Vert }_{W_1}^2+{\Vert \dot{q}-{\dot{q}}_{waist}\Vert }_{W_2}^2+{\Vert \dot{q}\Vert }^2$$

s.t. (5), (6), (7), (8), (9).

where $W_1$ and $W_2$ indicate the weight matrices for collision avoidance and waist regulation, respectively. The penalty term $\parallel \dot{q}{\parallel }^2$ at the end of the cost function is employed to ensure numerical stability and minimize energy consumption. By solving the aforementioned QP problem, the controller can output safe and reliable joint velocities with millisecond-level real-time performance.

6. Simulations

The simulations primarily tested the methods proposed in Section 3. To verify the effectiveness and real-time performance of the proposed MC-SCCF, we conducted systematic evaluation tests in a simulation environment. The humanoid upper-body robot model utilized features 16 DoFs, comprising two 7-DoF spherical–revolute–spherical (SRS) manipulators and a 2-DoF waist. The robot is illustrated in Figure 5. The yellow translucent objects in the figure denote the bounding volumes attached to the robot links for self-collision avoidance. The relevant parameters of the algorithms are listed in

Table 1. Algorithm Parameters.

Parameter	Value	Parameter	Value
$\beta$	0.015 rad/Nm	$d$	5 cm
$\epsilon$	0.001 rad	$h$	25 cm
$\max \_\mathrm{iters}$	50	$\mu$	0.15
$W_u$	${\mathrm{I}}_3$	$R_{tray}$	38 cm
$W_x$	$\mathrm{diag}\left(100{\mathrm{I}}_3,5{\mathrm{I}}_3\right)$

.

The parameters $d,h,\mu ,R_{tray},W_x$ and $W_u$ in the table are used in the middle-layer task constraint processing module. Here, $d$ and $h$ denote the base edge length and height of the virtual reference object and the virtual offset object, respectively, and $R_{tray}$ denotes the maximum radius of the tray. In this section, the virtual object is set as a rectangular prism with a base edge length of 5 cm and a height of 25 cm, corresponding to an aspect ratio of 5:1. This setting results in a high center of mass and a small support area, making the object prone to toppling. The friction coefficient $\mu$ is set to 0.15 to simulate low-friction contact conditions. These settings impose stricter task dynamic constraints and are more likely to induce conflicts between the dynamic constraints and other constraints. $W_x$ and $W_u$ are the state error weight and input weight in the trajectory smoother, respectively, which balance trajectory tracking accuracy and trajectory smoothness. The parameters $\beta ,\epsilon$ and $\max \_\mathrm{iters}$ are used in the top-level safe velocity filter, affecting the convergence rate of the boundary constraint, numerical tolerance, and upper limit on online search iterations, respectively. The relevant parameters are selected through preliminary simulation tests and are validated in hardware experiments, showing that they meet the combined requirements of real-time performance, constraint safety, and smooth velocity modulation.

6.1. Verification of the Reachability Determination Method

To evaluate the reachability determination algorithm based on the differentiable surrogate model, we conduct comparative tests using both the benchmark method and the proposed method.

Benchmark method: For a given dual-arm target pose, after confirming that no interference occurs with the mobile base, all possible configurations within the waist joint limits are traversed at a resolution of 0.5°. For each waist configuration, the analytical inverse kinematic solutions of both arms are solved. Since the employed manipulators have an SRS configuration, analytical inverse kinematic solutions are available. If analytical solutions exist for both arms under any waist configuration, the target pose is determined to be truly reachable. If no solution exists after all waist configurations are traversed, the target pose is determined to be truly unreachable. The results obtained by the benchmark method are used as the ground truth for the reachability of dual-arm target poses. The selection of bounding volumes is shown in Figure 5.

In nonprehensile transportation tasks, a deterministic relative pose exists between the robot’s dual-arm end-effectors and the tray. Instead of directly evaluating the reachability of arbitrary dual-arm target poses, our evaluation strategy involves randomly generating tray poses in space and thereafter deriving the target poses of the dual-arm end-effectors. Accordingly, this approach aligns more closely with practical task requirements.

Specifically, within a spherical space with a radius of 1.5 m centered at the robot’s waist base joint, ${10}^6$ target poses of trays with varying widths (25 cm to 80 cm) and random orientations are generated. The accuracy of the algorithm is evaluated by comparing its determination results against the aforementioned ground truth. The experimental results are presented in

Table 2. Confusion Matrix for Reachability Determination Results.

Ground Truth	Detected as Reachable	Detected as Unreachable
Reachable	348,023 (TP, True Positive)	12,869 (FN, False Negative)
Unreachable	8558 (FP, False Positive)	630,550 (TN, True Negative)

.

The precision of the algorithm is ${\displaystyle \frac{\mathrm{TP}}{\mathrm{TP}+\mathrm{FP}}}\times 100\%\approx 97.60\%$. This indicates that when the algorithm determines a pose as reachable, the probability of the robot reaching it is 97.60%. The overall accuracy of the algorithm is ${\displaystyle \frac{\mathrm{TP}+\mathrm{TN}}{\mathrm{Total}}}\times 100\%\approx 97.86\%$.

The computational efficiency results for the two methods in determining target pose reachability are listed in

Table 3. Computational Efficiency Results for the Two Methods.

Metric	Benchmark Method	Our Proposed
Waist configuration iterations	——	8.5 (±2.4)
Average query time	10.4 ms (±6.8 ms)	60 μs (±34 μs)

. The benchmark method requires traversing the waist configurations and solving the inverse kinematics of both manipulators under each waist configuration. Moreover, its determination depends on the sampled waist configurations. For unreachable targets and some poses close to joint limits, the benchmark method must traverse all waist configurations before producing a result. Therefore, its query time is much higher than that of the proposed method.

For the proposed method, the average number of iterations required to compute the optimal waist configuration for each reachability query is 8.5, with a standard deviation of ±2.4. The average computation time for a single reachability query is 60 μs, with a standard deviation of ±34 μs. Although the test results cannot guarantee absolute accuracy, an accuracy above 97% is sufficient for practical use. Minor prediction errors can be eliminated in engineering deployment by introducing a static safety margin. The computational efficiency of the algorithm also meets the expected objective, and this computational overhead is sufficiently low to support the real-time control requirements of teleoperation.

6.2. Evaluation of the Workspace Boundary Determination Algorithm

To objectively evaluate the accuracy and computational efficiency of the workspace boundary determination algorithm, the experiment directly employs the bisection search for the workspace boundary (without applying the predictive stopping pose pruning function). The experiment randomly generates ${10}^4$ initial tray positions and corresponding velocity directions within the workspace. A high-precision search for the true workspace boundary distance is conducted combining the true reachability determination method from Section 5.1 with the bisection method, setting the iteration termination condition to a sampling interval length of less than 1 cm. The true distance is denoted as $D_0$, while the boundary distance detected by the proposed method is defined as $\widehat{D}$. The detection deviation of the distance determination is defined as $E=\widehat{D}-D_0$. The experimental results are revealed as follows.

The experimental results demonstrate that the proposed method exhibits high precision in estimating the boundary distance. As illustrated in Figure 6a, the determination deviation distribution is relatively even, with a mean of only 2.26 cm and a standard deviation of 1.29 cm. Among the 10,000 random samples, the maximum detection deviation remains within 4.5 cm. This detection deviation is primarily restricted by the grid resolution (5 cm) of the discrete reachability map; thus, improving this resolution can further enhance the determination accuracy. During actual deployment, to completely eliminate the risk of boundary violation, the safety can be guaranteed at the algorithmic level simply by setting the practical safe boundary distance in the control system to the detected distance minus 4.5 cm.

The time consumption distribution histogram in Figure 6b exhibits the high efficiency of this search strategy. The average time for a single workspace boundary detection is 1.18 ms, with a standard deviation of 0.34 ms, and the maximum detection time consistently remains below 2.5 ms. The computational efficiency of this algorithm can fulfill the requirements of real-time control frameworks for most humanoid robots.

6.3. Evaluation of the Safe Velocity Filter

To evaluate the advantages of the proposed acceleration-constrained piecewise CBF, we compare it with two standard linear CBF baseline methods. Both baselines adopt the conventional class $\mathcal{K}$ function $\alpha \left(h\right)=\gamma h$, but with different gains. All other components of the control framework, including the middle-layer trajectory smoothing module and the low-level inverse kinematics controller, remain unchanged. Therefore, this comparison allows the effect of replacing the standard CBF with the proposed improved CBF to be evaluated independently. The parameter settings of the control schemes are listed in

Table 4. Parameters for Different Safe Velocity Filter Schemes.

Mode	$\mathit{\gamma }$	${\mathit{a}}_{\mathbf{max}}$	${\mathit{h}}_{\mathit{t}}$
CBF 1	1.20	——	——
CBF 2	1.85	——	——
Improved CBF	——	0.7 m/s2	0.15 m

.

A dynamic deceleration simulation scenario is designed: the robot’s dual arms support a tray carrying an object and execute linear motion toward the workspace boundary at various constant initial velocities ranging from 0.5 m/s to 0.9 m/s, applying the three aforementioned control schemes respectively. The velocity and orientation of the object are recorded during the experiment, focusing on statistically analyzing the maximum tilt angle and the deceleration distance of the object during motion. The comparative simulation results are exhibited in Figure 7.

Under conventional linear CBF control (CBF 1 and CBF 2), both the maximum tilt angle and the deceleration distance of the object are positively correlated with the object’s velocity. A horizontal comparison of the two conventional schemes reveals that at the same velocity, a smaller $\gamma$ value reduces the maximum tilt angle but increases the deceleration distance. This indicates that mitigating orientation jitter during the deceleration phase by decreasing $\gamma$ comes at the cost of reducing the robot’s flexibility and operational workspace. This experimental phenomenon is consistent with the analysis presented in Section 3.

When employing the improved CBF scheme, the maximum tilt angle during the deceleration phase no longer exhibits a notable correlation with the velocity. Theoretically, the maximum tilt angle should not vary with the object’s velocity. In practice, due to the trajectory smoothing effect of the middle-layer controller, the maximum tilt angle is influenced by the velocity to a certain extent, which is expected. Nonetheless, compared to the conventional schemes, the improved CBF achieves an effective limitation on the maximum tilt angle of the deceleration orientation, fulfilling the anticipated objective of maintaining a controlled maximum tilt angle during deceleration.

Comparing the results of the improved CBF and CBF 1, at an object velocity of 0.9 m/s, the maximum tilt angles during the deceleration phase are approximately identical. However, regarding the deceleration distance, the improved CBF yields a shorter distance than CBF 1. Comparing the improved CBF with CBF 2, the deceleration distances are roughly equivalent at velocities of 0.5 m/s and 0.6 m/s; in contrast, as the velocity increases, the maximum tilt angle under CBF 2 becomes markedly larger than that under the improved CBF.

As a result, compared to conventional CBFs, the improved method introduces a maximum acceleration constraint and utilizes a piecewise transition, achieving a tradeoff between constrained space and orientation controllability. It is less invasive to user commands when far from the boundary, thereby preserving a larger operational space; meanwhile, when approaching the boundary, the deceleration is smoother with controlled acceleration, effectively mitigating the risk of object toppling.

7. Experiments

7.1. Experimental Platform and Task Setup

To verify the effectiveness of the proposed MC-SCCF in a physical system, this section designs an experiment for a humanoid robot executing a nonprehensile transportation task. The hardware system for teleoperation is illustrated in Figure 8. The experiment utilizes our custom-built dual-arm humanoid robot [58]. Its upper body comprises two 7-DoF SRS anthropomorphic manipulators and 2-DoF (yaw and pitch) head and waist joints, mounted on a tracked mobile base. The experiments in this study solely control the upper body manipulation of the robot, without involving the locomotion of the mobile base.

The master device employs a CyberSystem hand controller to track the 3D position data of the human wrist, which is mapped to the humanoid robot’s workspace via an incremental Cartesian space mapping. Because the robot features a large workspace, the displacement of the master user is scaled up by a factor of 4 before being mapped to the robot’s workspace. Additionally, the master device is equipped with a foot pedal functioning as a clutch switch. Thus, operators can adjust their arm positions at any time during the task to achieve more comfortable operation.

A computer running Ubuntu 20.04, equipped with an Intel Core i9-9900K CPU and 64 GB of RAM, is utilized to execute the teleoperation controller. The controller is implemented in C++, applying multithreading technology to separate hand controller data acquisition, teleoperation control, and state feedback reception. The data acquisition thread collects user input data from the master device at a frequency of 40 Hz, with a communication latency between the master device and the teleoperation controller strictly below 1 ms. The teleoperation control operates within the planning thread at a control frequency of 20 Hz, and the communication latency between the teleoperation controller and the robot is less than 20 ms. The optimization problem is solved using the open-source qpOASES package. The thread dedicated to receiving robot state feedback processes a 24 Hz video stream and 100 Hz robot joint states.

The experiment requires operators to control the robot via the master device to execute an obstacle-avoidance transportation task. Operators must teleoperate the robot’s dual arms to lift a tray carrying multiple objects from a starting point at the rear left, cross an obstacle directly in front, and ultimately place it smoothly in a target area on the right. The experimental process is demonstrated in Figure 9.

The dimensions of the tray are 560 mm $\times$ 375 mm $\times$ 80 mm. 14 objects to be transported are randomly positioned on the tray. These objects comprise common items, including vases, water bottles, wooden blocks, and glassware, with heights ranging from 100 mm to 235 mm, as exhibited in Figure 8. According to the dimensions of the tray and the objects placed on it, the relevant parameters of the middle-layer controller are listed in

Table 5. Parameters of the Middle-Layer Controller.

Parameter	Value	Parameter	Value
$d$	5 cm	$h$	24 cm
$\mu$	0.25	$R_{tray}$	28 cm
$W_u$	${\mathrm{I}}_3$	$W_x$	$\mathrm{diag}\left(100{\mathrm{I}}_3,5{\mathrm{I}}_3\right)$

. Because objects inevitably slide during motion, fiducial markers are attached to their tops, and a camera is employed to detect their sliding distances. The camera is fixed above the robot platform and is exclusively utilized to detect sliding distances for result analysis; the positions of the objects are practically under open-loop control.

Visual feedback for the operator is provided by the stereo camera on the robot’s head. No global field of view (FoV) is available during the experiment, which is highly typical in practical teleoperation task execution scenarios. The controller provides kinematic simulations, where both the actual and desired configurations of the robot are simultaneously displayed on the simulation interface. Operators can switch perspectives within the simulation interface to obtain an intuitive understanding of the teleoperated robot’s current state.

7.2. Experimental Conditions and Evaluation Metrics

This section defines the following three control modes:

• No-SVF: No Safe Velocity Filter Mode (Hard Boundary Mode). The middle-layer MPC (ensuring multi-object dynamic constraints) and low-level kinematic hard limits are enabled, whereas the top-layer safe velocity filter is deactivated.
• CBF: Linear CBF Mode (Standard CBF Baseline Mode). The middle-layer MPC is enabled, and a conventional linear CBF ($\gamma =1.5$) is employed at the top layer as the safe velocity filter.
• MC-SCCF: Proposed Multiple-Constraint Safety-Critical Control Framework. The middle-layer MPC is enabled, and the improved CBF proposed in this study is utilized at the top layer (with algorithm parameters identical to those in the Section 6 simulations).

To reduce experimental randomness, a total of 10 subjects were recruited. All participants signed an informed consent form prior to participating in the experiment. During the procedures, exclusively the input trajectories from the subjects operating the hand controller were recorded, strictly excluding any sensitive or privacy-related data. The experimental protocol was approved by the institutional ethics committee (Approval No. HIT-2025013) and conducted in accordance with the Declaration of Helsinki. Among the subjects, one was female and nine were male, with four possessing prior teleoperation experience. All subjects exhibited normal cognitive levels and equipment operation capabilities. Before the experiment commenced, the designers spent 40 min explaining the task to each subject and assisting them in familiarizing themselves with the equipment. For each trial, the initial robot configuration, the placement of the tray, and the arrangement of objects on the tray remained strictly identical. Each subject performed the experiment using the three modes separately, conducting four trials per mode. This yielded a total of 120 experimental trials.

The evaluation metrics encompass three dimensions: task metrics, subjective evaluation metrics, and control performance metrics. The task metrics include the following:

• Task Success Rate: A success is defined as the tray reaching the target position without colliding with obstacles, and without any objects toppling or falling.
• Completion Time ($\mathrm{T}$): Timing initiates when the operator lifts the tray and concludes when the tray is placed at the target position.
• Total Object Sliding Distance ($\mathrm{S}$): The sum of the sliding distances of all objects on the tray. A shorter sliding distance indicates smoother motion.
• Activation Time Ratio of the Safe Velocity Filter: Calculated as ${\mathrm{T}}_{\mathrm{Filter}}/\mathrm{T}\times 100\%$, where ${\mathrm{T}}_{\mathrm{Filter}}$ denotes the duration for which the filter remains active.
• Number of Pauses During the Task: Operators can adjust the master device to the most optimal operational area during pauses.

Subjective metrics were collected via a 7-point Likert scale, gathering the subjects’ scores on operational intuitiveness, task difficulty, cognitive load (concentration), and operational confidence.

Control performance metrics comprise the robot’s trajectory tracking accuracy, relative pose error between dual-arm end-effectors, manipulability, minimum distance, and computation time. These metrics are primarily utilized to analyze the effectiveness of the low-level controller.

7.3. Experimental Results

7.3.1. Task Metrics

The statistical experimental results under the three modes are demonstrated in

Table 6. Success Rates of the Three Control Modes.

Control Mode	Success Rate
No-SVF	82.5%
CBF	100%
MC-SCCF	100%

and Figure 10. Regarding the task success rate, Mode No-SVF achieved 82.5%. The failures were caused by the lack of inverse kinematic solutions leading to emergency stops, as well as the toppling and slipping of objects. This indicates that when facing complex workspace boundaries and multiple dynamic constraints, relying solely on the operator’s intuition is insufficient to maintain the system within the safe region. In contrast, Modes CBF and MC-SCCF, which incorporate the top-layer safe velocity filter, both achieved a 100% success rate. This result intuitively confirms the necessity and effectiveness of the velocity filtering mechanism in evading kinematic boundary violations and preventing task failures.

The completion time $\mathrm{T}$ and the number of pauses intuitively reflect the system’s efficiency and the fluency of human–robot collaboration. Mode MS-SCCF exhibited the optimal comprehensive performance, requiring the shortest task duration (76.8 s) while maintaining the number of pauses at a reasonable level (4.2). In contrast, although Mode CBF simultaneously guaranteed safety, it required the longest task duration (103 s) and the highest number of pauses (6.72). The underlying reason is that the conventional linear CBF is highly conservative and frequently modifies user input commands, causing severe positional mapping deviations between the master and slave devices. The operator’s subjective displacement fails to elicit a response at the slave end, forcing the operator to frequently depress the clutch switch to readjust the master device’s position. This not only increases the operation time but also disrupts the continuity of teleoperation.

The total object sliding distance $\mathrm{S}$ measures the smoothness of the transportation process. Because touching hard boundaries in Mode No-SVF easily triggers emergency stops and abrupt acceleration changes, the object sliding distance reached 121 mm. In Modes CBF and MC-SCCF, where the top-layer safe velocity filter is enabled, the sliding distance was substantially suppressed (57.8 mm and 60.9 mm, respectively). The sliding distance in Mode CBF was slightly smaller than that in Mode MC-SCCF, as its conservative filtering strategy resulted in a slower overall motion velocity. Nonetheless, while maintaining high efficiency, Mode MS-SCCF still controlled the sliding distance to an optimal level nearly equivalent to that of Mode CBF. This supports the efficacy of the improved CBF in controlling acceleration and smoothness during the deceleration phase.

Regarding the activation time ratio of the safe velocity filter (${\mathrm{T}}_{\mathrm{Filter}}/\mathrm{T}$), Mode MS-SCCF still triggered the safe velocity regulation mechanism for 29.5% of the time. The experimental task is not inherently complex, and the robot possesses a large workspace; nevertheless, operators easily drive the robot near the workspace boundaries. This is primarily attributed to two reasons. First is the configuration mismatch. Human operators have a different kinematic configuration from the robot. Particularly when holding a tray of fixed width with both arms, a closed-loop kinematic chain is formed. Operators cannot maintain a clear perception of the robot’s configuration at all times. Thus, trajectories input by the operator at the master device frequently cause the robot to touch the workspace boundaries at the slave end.

The second vital reason is that teleoperation typically relies on screens or cameras with specific viewing angles, leading to inaccurate depth perception and limited telepresence. When facing unstructured environments, highly conservative operations are often required. For instance, during the experiment, operators could not accurately evaluate the safe clearance between the tray bottom and the obstacle. To evade collision risks, subjects generally preferred conservative obstacle avoidance strategies, such as excessive elevation or excessive retraction, which accordingly made it easier to drive the robot to its extension limits. However, precisely because operators inevitably drive the robot close to the boundaries, the top-layer velocity modulation mechanism proves to be particularly crucial.

7.3.2. Subjective Evaluation Metrics

The statistical results of the Likert scale align closely with the objective data, as demonstrated in Figure 11. The figure illustrates evaluation results across four dimensions: the intuitiveness of the control mode (7 indicates extremely intuitive), the difficulty of the task (7 indicates extremely easy), the cognitive load and concentration (7 indicates extremely effortless), and the confidence in completing similar future tasks (7 indicates highly confident). The means and standard deviations of the subjective evaluation metrics are listed in

Table 7. Means and Standard Deviations of Subjective Evaluation Metrics under the Three Control Modes.

Subjective Metric	No-SVF	CBF	MS-SCCF
Intuitiveness	6 ± 0.9	2.4 ± 1.3	5.3 ± 1.3
Task Difficulty	2.6 ± 1.3	3.5 ± 2.1	5.9 ± 1.4
Concentration	3 ± 1.3	3.6 ± 1.4	5 ± 1.2
Confidence	3.9 ± 2.3	5.1 ± 1.4	6.3 ± 0.8

.

Mode No-SVF: Although the task success rate under this mode was relatively low (only 82.5%) and objective metrics indicated a larger object sliding distance, operators generally awarded it the highest score for operational intuitiveness. This is primarily because Mode No-SVF did not introduce the safe velocity filter; accordingly, user input commands could be directly and entirely transformed into the tray’s motion, realizing a what-you-see-is-what-you-get experience. Nonetheless, this mechanism lacking safety protection also resulted in Mode No-SVF receiving the lowest task difficulty score. In assessing confidence for future tasks, the operators’ scores exhibited considerable variance. Operators who assigned low scores believed that without the protection of the top-layer safe velocity filter, the robot was highly prone to touching workspace boundaries during operation, rendering the task more difficult. This group also assigned relatively lower scores for cognitive load (concentration) in Mode No-SVF. In contrast, operators who assigned high scores were more inclined to believe that the superior operational intuitiveness of Mode No-SVF was more conducive to task completion; these high-scoring individuals simultaneously assigned higher scores for concentration.

Mode CBF: Benefiting from the introduction of the conventional linear CBF, the overall evaluation of task difficulty for this mode was easier than that of Mode No-SVF, though the statistical results similarly presented considerable variance. The core reason subjects found it easy was that the algorithm guaranteed safety; the tray would not cross workspace boundaries, thereby avoiding situations without inverse kinematic solutions. However, the reason this mode was perceived as difficult was its poor operational intuitiveness. Due to the conservative filtering mechanism of the conventional CBF, master inputs were frequently suppressed by the system, triggering a notable sluggishness in operation. This unstable mapping of master-slave positions, coupled with the robot’s frequent slow motions, substantially increased the operators’ cognitive load.

Mode MC-SCCF: Integrating all subjective metrics, Mode MS-SCCF provided a superior interaction experience among the tested modes. Subjects required the lowest concentration when executing tasks, indicating that this mode possesses a minimal cognitive load, allowing users to complete the nonprehensile transportation experiment with ease. Furthermore, this mode not only overcame the sluggishness noted in Mode CBF and preserved adequate operational intuitiveness but also provided operators with the highest confidence for completing future tasks. Overall, driven by the MC-SCCF integrated with the improved CBF, Mode MS-SCCF balances system safety and operational intuitiveness, making it the most preferred among the three modes.

7.3.3. Control Performance Metrics

To verify the effectiveness of the low-level controller and more intuitively demonstrate the experimental process, a set of trials using Mode MS-SCCF was selected. Figure 12 illustrates the variations in relevant variables during the experiment, where the red regions indicate periods of operational pauses.

During the experiment, the maximum motion velocity of the object exceeded 0.25 m/s, and its maximum tilt angle exceeded 6°. Throughout the entire motion, the trajectory position error remained below $1\times {10}^{-4}$ m, the relative position error between the dual-arm end-effectors was below $6\times {10}^{-5}$ m, and the relative orientation error was below $8\times {10}^{-5}$ rad. This achieved the objectives of trajectory tracking and maintaining a constant relative pose between the dual-arm end-effectors.

Although the robot’s configuration underwent considerable changes during transportation, safety was consistently guaranteed. The robot’s manipulability remained above 0.04, and the minimum distance between its bounding volumes was greater than 0.29 m. The goals of evading singular configurations and avoiding self-collisions were realized. During the lifting of the tray and obstacle crossing, driven by the waist regulation objective, the waist pitch axis automatically returned to its upright state after providing the necessary forward-reaching compensation. This not only prevented the waist joint from remaining in a high-load configuration for extended periods but also rendered the robot’s overall operational posture more natural and anthropomorphic.

Within each control period during the task, the inverse kinematics solving time was approximately 3 ms, with a peak below 6 ms. This computational overhead is lower than the control period of the teleoperation system, meets the real-time requirements of teleoperation control.

The data indicate that the low-level controller can effectively achieve the design objectives of multiple-constraint coordinated processing and configuration optimization while fulfilling real-time performance requirements.

8. Discussion

The proposed Multiple-Constraint Safety-Critical Control Framework decouples offline computation from online control in its design, thereby reducing the computational burden during online control. For workspace boundary determination, both the discrete reachability map and the differentiable reachability surrogate model are constructed offline. During online operation, only the surrogate model is used for gradient-guided waist configuration search, followed by nearest-neighbor queries in the true discrete reachability map. Therefore, the computational overhead of online reachability determination is low. The simulation results show that the average computation time for a single target pose reachability query is approximately 60 μs, while the average computation time for workspace boundary detection is approximately 1.18 ms, with the maximum detection time remaining below 2.5 ms. In the hardware experiments, the teleoperation controller runs at 20 Hz, the master device data acquisition frequency is 40 Hz, the robot joint state feedback frequency is 100 Hz, and the video feedback frequency is 24 Hz. These results indicate that the proposed method satisfies the real-time control requirements of the current experimental platform.

From the perspective of scalability, the proposed framework adopts a modular structure consisting of top-level safe velocity filtering, middle-layer task constraint processing, and bottom-level inverse kinematics optimization. Information is transmitted among different layers through desired velocities, reference trajectories, and task-space constraints. Thus, each module can be replaced or extended according to the specific robot platform and task requirements. For example, when the robot structure changes, the corresponding reachability map and reachability surrogate model can be reconstructed. When the task objects or contact conditions change, the middle-layer virtual object model and dynamic constraints can be adjusted. When the robot degrees of freedom or the number of collision objects increases, the corresponding kinematic constraints or optimization objectives can be added to the bottom-level QP. It should be noted that the scalability of the proposed method is still affected by the robot degrees of freedom, the number of collision bounding volumes, the number of constraints, and the scale of the QP problem. For whole-body humanoid robots with higher degrees of freedom, hierarchical solving or parallel implementation strategies require further investigation.

From the perspective of noise robustness, the proposed framework provides a buffering capability against a certain level of input noise. First, the middle-layer MPC smooths the operator input and generates high-order continuous reference trajectories, thereby suppressing high-frequency jitter in the master input. Second, the top-level safe velocity filter processes user commands before they enter the task trajectory generation module. This prevents noisy velocity commands from causing sudden boundary violations or emergency stops when the robot approaches the workspace boundary.

Although the proposed MC-SCCF has been validated through simulations and hardware experiments, several limitations remain and require further study. First, the current experiments mainly focus on dual-arm upper-body manipulation of a humanoid robot with a fixed base. The framework has not yet been jointly validated with mobile base motion or whole-body motion control. Future work will extend this framework to mobile manipulation scenarios while considering base motion, whole-body balance maintenance, and dual-arm coordinated control. Second, the accuracy of the reachability surrogate model depends on the resolution and coverage of the offline reachability map. When the robot platform or kinematic configuration changes, the reachability map must be regenerated and the surrogate model must be retrained. Future work will explore reachability representations with better generalization capability, as well as efficient model transfer and adaptation strategies for different humanoid robot platforms. Third, the current task modeling does not explicitly consider communication or control delays during teleoperation. Since delays may affect operator intent mapping, safe velocity filtering, and task stability, future work will introduce delay-aware control strategies and further evaluate the robustness of the framework under different delay conditions.

9. Conclusions

To address the challenges of multiple constraint conflicts and safety control in humanoid robot teleoperation for nonprehensile transportation tasks, this article proposes a hierarchically decoupled Multiple-Constraint Safety-Critical Control Framework (MC-SCCF). This framework divides the system into a top layer responsible for intrinsic safety regarding workspace boundaries, a middle layer managing object dynamic equilibrium, and a low level handling various kinematic hard constraints and configuration optimization.

System simulation tests demonstrate that the target pose determination method based on the differentiable reachability surrogate model exhibits high accuracy and real-time performance. The average computation time per query is only 60 µs, with a prediction accuracy reaching 97.86%. The bisection search algorithm for the workspace boundary distance can control the average detection error to 2.26 cm (with a maximum detection deviation below 4.5 cm) at a marginal computational overhead of 1.18 ms on average. In dynamic deceleration scenarios, the proposed improved CBF based on the maximum acceleration constraint overcomes the overly conservative limitations of the conventional linear CBF. It limits the maximum tilt angle of the object as the robot approaches the boundary, achieving smooth deceleration and orientation controllability. The simulation data adequately prove the theoretical effectiveness of the proposed safe velocity filter and reachability determination methods.

We recruited 10 subjects to conduct multi-object obstacle-crossing transportation teleoperation experiments on a physical dual-arm humanoid robot platform. Task metrics reveal that the control mode employing the proposed framework (Mode MC-SCCF) achieved a 100% task success rate, whereas Mode No-SVF, lacking top-layer protection, had a success rate of only 82.5%. Regarding efficiency and smoothness, Mode MS-SCCF shortened the task completion time to 76.8 s, which is superior to the 103 s of the conventional conservative CBF strategy (Mode CBF). Furthermore, this framework reduced the operators’ average number of pauses to 4.2 and markedly suppressed the total object sliding displacement to 60.9 mm (a reduction of nearly 50% compared to the 121 mm in Mode No-SVF). Subjective Likert scale feedback indicates that this control scheme considerably reduces the operators’ cognitive load and provides users with high operational intuitiveness and task confidence. These statistical results confirm the operators’ recognition of this scheme. Additionally, the completion of extensive experiments validates the stability of the control architecture during deployment on a physical robot.

Control performance data throughout the experiments further demonstrate that the low-level system, when handling highly dynamic teleoperation commands, effectively maintains a constant relative pose between the dual-arm end-effectors and high-precision trajectory tracking. The trajectory position error consistently remained below ${10}^{-4}$ m, while the relative position and orientation errors between the dual arms were below $6\times {10}^{-5}$ m and $8\times {10}^{-5}$ rad, respectively. Moreover, the robot’s manipulability consistently exceeded 0.04, the minimum distance between bounding volumes was maintained above 0.29 m, and configuration optimization objectives such as automatic waist regulation were realized. The average solving time of the inverse kinematics algorithm was approximately 3 ms (with a peak below 6 ms), fully satisfying the real-time requirements of teleoperation and directly verifying the effectiveness of the low-level controller.

In summary, the control framework proposed in this article successfully resolves the underlying conflicts between stringent task dynamic constraints and complex intrinsic constraints of humanoid robots. Under the premise of ensuring system safety, this research substantially improves the efficiency and user experience of nonprehensile transportation tasks, providing a practical and feasible control-theoretic solution for humanoid robots to execute complex collaborative tasks in unstructured environments.