Revised Control Barrier Function with Sensing of Threats from Relative Velocity Between Humans and Mobile Robots

Abstract

The mobile robot, which comprises a mobile platform and a robotic arm, has been widely adopted in industrial automation. Existing safe control methods with real-time trajectory alternation face difficulties in efficiently identifying threats from fast relative motion between humans and robots, causing hazards in environments of dense human–robot coexistence. This work firstly builds a safe mobile robot control framework in the kinematic sense. Secondly, the proximity between parts of a human and a mobile robot is efficiently solved by convex programming with parametric description of skew line segments. It is also no longer required to perform case-by-case analysis of skew line segments’ relative pose in space. Thirdly, a novel threatening index is proposed to select the most threatened human parts based on mutual projection of human–robot relative velocity and their common normal vector. Eventually, this index is incorporated into the safety constraint, showing the improved safe control performance in the simulated human–mobile robot coexistence scenario.

1. Introduction

A mobile robot incorporates a mobile platform and a robotic arm to form a hig y flexible and efficient production unit [1]. This integration offers innovative solutions for managing complex tasks [2]. While human–robot collaboration scenarios continue to proliferate, significant challenges remain in establishing reliable safe control frameworks for dynamic environments [3].

Many methods have been proposed to address the challenges of path planning for mobile robots in complex environments, such as potential field [4,5] and danger field-based methods [6]. These methods are widely used for real-time obstacle avoidance due to their high computational efficiency. Control barrier functions (CBFs) are defined in control systems by ensuring that a system’s state remains within a predefined safe state set through the design of constrained control inputs [7]. It is particularly efficient in real-time obstacle avoidance if the safe conditions are set up as inequality constraints in quadratic programming. However, CBFs may face issues of high computational complexity and occasional collision with high-dimensional space or low-sampling-rate perception and control, and their definition of obstacle distances may not be comprehensive enough [8]. In recent years,  deep reinforcement learning (DRL) [9], neural belief tracking (NBT) [10], and other methods based on artificial intelligence have also been applied to trajectory planning in the human–machine interaction. By learning and predicting the trajectories of dynamic obstacles, neural networks can significantly enhance the intelligence and adaptability of path planning [11,12]. However, they lack formal safety guarantees, and their performance relies on high-quality training data [13,14]. The CBF-based safe reinforcement learning (CSRL) algorithm has been employed to address target pursuit problems [15]. The combination of CBF and model predictive control (MPC) can address the efficiency issues of online obstacle avoidance in unstructured environments filled with static and dynamic obstacles [16]. The path planning and task allocation in multi-robot systems has recently been optimized by integration of DRL, MPC, and graph neural networks (GNNs) [17]. However, alternating the trajectory in the real time remains a problem when these advanced networks are incorporated.

By sensing and adjustment of the distance from the obstacles, robots can prevent collisions [18]. The trajectory of mobile robots in the environment with static obstacles can be planned through offline programming methods [19]. For a robotic arm with a fixed base, a conservative approach is to define the safe boundary based on robotic arm’s own workspace [18]. However, mobile robots sometimes need to deal with crowded, dynamic human–robot environments, where the uncertainty of human motion greatly challenges the real-time trajectory planning [20]. The difficulty of trajectory planning is further increased with mobile robots as the kinematics of the robotic arms couples with that of the mobile platform, especially when dealing with multiple moving humans or obstacles [21]. The determination of minimum human–robot distance is done by case-by-case analysis depending on the relative locations of the skew line segments and their common normals [22]. The “look-backward-and-forward” algorithm is further developed to predict the human trajectory [23], while the estimation of prediction error is further incorporated into the discrete-time control barrier function (DCBF) constraints. However, for crowded environment with dense and fast-moving humans and mobile robots, considering only the minimum distance between the individual parts of a human and a mobile robot is insufficient to indicate the threat in the future. Notably, the human-robot relative velocity critically indicates the level of danger [24] or injury severity [25] when facing potential impacts. They further affect operators’ mental workload, anxiety, and risk perception [26]. The robot’s velocity is included as a sigmoid-like smooth function term to shape the CBF [27]. A broader range of such constraints are included, such as robot joint velocities and the end-effector’s velocity, as well as wall boundaries and cylindrical capsules for human and robot parts [28]. However, how to incorporate the relative velocity into the safety index is not well studied when designing the robot controller.

This paper aims to incorporate the human–robot relative velocity in the control barrier function (CBF), to achieve safe control with real-time trajectory alternation for mobile robots in human–robot coexisting environments. The main contributions are as follows:

• The coupling kinematics of the mobile robot with a mobile platform and a robotic arm is formed in the kinematic sense rather than solely with the fixed-base robot arm in [22], so that the kinematic control problem in human–robot environments is set up with a discrete-time control barrier function (DCBF) and discrete-time control Lyapunov function (DCLF).
• By setting up the parametric description of skew line segments, the minimum distance between a pair of human skeletons and a link of a robotic arm (or an outline of a mobile platform) is efficiently solved in real time by convex programming. Compared with [22,28], it is no longer necessary to make a case-by-case analysis depending on the relative locations of the skew line segments and their common normal.
• By mutual projections of relative velocity of parts of a human and a mobile robot and their common normal vector, two projection indexes are given. Thereafter, a novel threat index is formed to give a normalized “distance” to select the most threatened human parts. In this way, the relative velocities between parts of humans and mobile robots are successfully incorporated into the DCBF-based constraint rather than just their absolute velocities [27,28].

Simulation of human and mobile-robot environments validates the effectiveness of the proposed safe control method for real-time trajectory alternation.

2. Related Works

Control Barrier Functions (CBFs) have emerged as a formal methodology for guaranteeing system safety in automation and robotics. They construct dynamic safety boundaries by transforming complex safety constraints into computable mathematical conditions, embedding these conditions within controller optimization frameworks to ensure system states remain within safe parameters [29]. Compared to traditional approaches, CBFs uniquely provide rigorous mathematical safety guarantees while meeting the real-time demands of modern control systems. Key research contributions include:

Standard CBFs based on Zeroing Barrier Functions: This class of CBFs define safe sets via differential inequalities governing system trajectories to prevent boundary violations [7]. Applications include collision avoidance for autonomous vehicles, collaborative robots, and UAV formations. Representative implementations encompass event-triggered CBFs for efficient traffic control [30], dual-arm coordination with obstacle avoidance [31], and minimum-intervention collision-free UAV control [32].

High-Order CBFs: This class of CBFs employ recursively defined function chains to convert safety constraints with relative degrees greater than one into feasible conditions explicitly incorporating control inputs [33]. They have been applied to predictive human–robot safety interaction and fall prevention for legged robots. Notable examples include control of articulated robots using dynamic obstacle trajectory prediction [22] and dynamic equilibrium maintenance combined with obstacle-aware trajectory optimization for bipedal robots [34].

Current challenges involve enhancing adaptability and robustness under strong dynamic coupling and unmodeled disturbances [35], alleviating computational bottlenecks (e.g., quadratic programming for multi-agent systems) [36], and mitigating prediction error accumulation in dynamic environments [37]. Potential solutions include cross-disciplinary integration, such as incorporating reinforcement learning into stochastic control frameworks [38]. In view of the above, CBFs are still promoted as a universal design paradigm for safety-critical applications, enabling reliable unmanned systems in industrial automation [39] and medical surgery [40].

3. Problem Formulation

This section firstly models the mobile robot under a discrete-time kinematic control framework. Later, the existing safe control framework is imported with both discrete-time control barrier function (DCBF) and discrete-time control Lyapunov function (DCLF).

3.1. Control of a Mobile Robot

The mobile robot consists of a mobile platform and a robotic arm, as shown in Figure 1.

The mobile platform is equipped with four Mecanum wheels, and its kinematics are given as:

$$\begin{array}{ccc}\hfill p_{k+1}& =& p_k+v_k{\delta }_t\hfill \end{array}$$

(1)

$$\begin{array}{ccc}\hfill v_k& =& J_c{u}_k\hfill \end{array}$$

(2)

where the subscript k denotes the k-th instant for all variables, $p\in {\mathbb{R}}^3$ represents the pose of mobile platform, $v\in {\mathbb{R}}^3$ is its planar velocity, $u\in {\mathbb{R}}^4$ is its control input and $J_c\in {\mathbb{R}}^{3\times 4}$ is its Jacobian matrix.

Assume a robotic arm with n revolute joints is installed on the mobile platform, and its joint displacements are denoted as $q\in Q\subseteq {\mathbb{R}}^n$. Hence, its kinematics are given by

$$\begin{array}{ccc}\hfill q_{k+1}& =& q_k+w_k{\delta }_t\hfill \end{array}$$

(3)

$$\begin{array}{ccc}\hfill a_k& =& g\left({\chi }_k\right)={\widehat{\overline{p}}}_k{T}_R^C{T}_a\left(q_k\right)\in SE\left(3\right)\hfill \end{array}$$

(4)

where $\chi ={\left[\begin{array}{cc}{q}^T& p^T\end{array}\right]}^T$, $\overline{p}={\left[\begin{array}{cccccc}p\left(1\right)& p\left(2\right)& 0& 0& 0& p\left(3\right)\end{array}\right]}^T$, a is the pose in the task space, $T_a\left(q\right)$ is the forward kinematics of the robotic arm, $T_R^C$ is the homogeneous transformation matrix (HTM) from the frame of the mobile platform to the base frame of the robotic arm, and w is the control input of the robotic arm, “∧” is the operator for $(•)\in {\mathbb{R}}^6\to \left(\widehat{•}\right)\in SE\left(3\right)$.

The task is to track a desired pose $a_d$ with the mobile robot’s end effector. We can design a discrete-time feedback controller s with a control gain $K_s$:

$$s_k=K_s{\left[{\left(a_k\right)}^{-1}{a}_{d,k}\right]}^{\vee }$$

(5)

where s is the velocity of the end effector, “∨” is the operator for $(•)\in SE\left(3\right)\to {(•)}^{\vee }\in {\mathbb{R}}^6$, and 

$$s_k=J_{e,k}{\nu }_k\triangleq \left[\begin{array}{cc}{J}_{\mathrm{base},k}& J_{\mathrm{a},k}\end{array}\right]\left[\begin{array}{c}{u}_k\\ w_k\end{array}\right]$$

(6)

In (6), $J_{\mathrm{a}}\in {\mathbb{R}}^{6\times n}$ is the Jacobian matrix of the robotic arm, $J_{\mathrm{base}}=\left[{\mathrm{Ad}}_{T_a^{-1}\left(q\right){\widehat{\overline{p}}}^{-1}}\right]{\overline{J}}_c$, where $\left[{\mathrm{Ad}}_{(•)}\right]$ is adjoint transformation. In addition,

$${\overline{J}}_c=\left[\begin{array}{c}{J}_c(1,•)\\ J_c(2,•)\\ 0_{1\times 4}\\ 0_{1\times 4}\\ 0_{1\times 4}\\ J_c(3,•)\end{array}\right]\in {\mathbb{R}}^{6\times 4}.$$

where $J_c(1,•)$ is the first row of $J_c$, etc. So the velocity (i.e., the control signal) of the entire mobile robot in the joint space ${\nu }_k$ is:

$${\nu }_k=J_{e,k}^{†}{K}_s{\left[{\left(a_k\right)}^{-1}{a}_{d,k}\right]}^{\vee }\triangleq w\left({\chi }_k\right)$$

(7)

where $J_e^{†}$ is the (pseudo-)inverse of $J_e$.

3.2. Safety and Stability Constraints

To achieve safe human–robot interaction, a safe set C is defined through a continuous differentiable function $H:Q\to \mathbb{R}$.

$$C=\{{\chi }_k\in Q:B\left({\chi }_k\right)\ge 0\}$$

(8)

where $B(·)$ is a discrete-time control barrier function (DCBF). $\Delta B\left({\chi }_k\right)=B\left({\chi }_{k+1}\right)-B\left({\chi }_k\right)\ne 0$, $\chi \in \partial C$ and

$$\Delta B\left({\chi }_k\right)\ge -\gamma B\left({\chi }_k\right)$$

(9)

with a constant coefficient $\gamma \in (0,1]$. This ensures the safe operation of the system.

Further, the stability of the system is constrained by the discrete-time Lyapunov function (DCLF) $V\left({\chi }_k\right)$, and 

$$\Delta V\left({\chi }_k\right)\le -\alpha V\left({\chi }_k\right)+\delta$$

(10)

where $\Delta V\left({\chi }_k\right)=V\left({\chi }_{k+1}\right)-V\left({\chi }_k\right)$, and $\alpha \in (0,1]$ is a constant, $\delta \ge 0$ is a slack variable.

Based on the above constraints, the controller under human–robot coexisting environment is synthesized by:

$$\begin{array}{cc}\hfill {\nu }_k^{*}=argmin(\parallel {\nu }_k-{\overline{\nu }}_k{\parallel }^2& +{\beta \delta }^2)\hfill \end{array}$$

(11)

$$\begin{array}{cc}\hfill \mathrm{s}.\mathrm{t}.\Delta B({\chi }_k,\nu \left({\chi }_k\right))& + \gamma B\left({\chi }_k\right)\ge 0\hfill \end{array}$$

(12)

$$\begin{array}{cc}\hfill \Delta V({\chi }_k,\nu \left({\chi }_k\right))& + \alpha V\left({\chi }_k\right)\le \delta \hfill \end{array}$$

(13)

${\overline{\nu }}_k={\left[\begin{array}{cc}{\overline{u}}_k^T& {\overline{w}}_k^T\end{array}\right]}^T$ are the desired velocity when there is no human interruption, $\beta >0$ is the slack variable.

4. Main Results

4.1. Distance Between a Mobile Robot and Human

This subsection aims to effectively seek the minimum distance between a pair of skew line segments by forming a convex optimization problem with parametric description of the points on the line segments.

Obtaining the closest distance between humans and the mobile robots in real time is necessary to establish CBF constraints in (12) for safe human–robot interaction. To calculate their closest distance, one can model the human body as a skeletal structure and simplify the robotic arm as a linkage mechanism, while the moving platform is modeled as a rectangle with four enclosed line-segments. At this point, the problem is transformed to calculation of the distance between spatial line segments. Taking Figure 2 as an example, let points $\mathcal{A}$ and $\mathcal{B}$ be the endpoints of the i-th link of the robotic arm or outline of the moving platform, and points $\mathcal{C}$ and $\mathcal{D}$ be the endpoints of the n-th segment of human skeleton. The parametric equations of the pair of skew line segments $(i,n)$ are:

$$\begin{array}{c}\hfill P\left(s\right)=\mathcal{A}+s(\mathcal{B}-\mathcal{A}),s\in [0,1]\end{array}$$

(14)

$$\begin{array}{c}\hfill Q\left(t\right)=\mathcal{C}+t(\mathcal{D}-\mathcal{C}),t\in [0,1]\end{array}$$

(15)

where s and t are normalized distances from points $\mathcal{A}$ and $\mathcal{C}$, respectively.

The problem is transformed into finding points $P\left(s\right)$ and $Q\left(t\right)$ such that the distance between them is minimized. This is achieved by the following convex optimization problem:

$$min{f}_{i,n}(s,t)\mathrm{s}.\mathrm{t}.s,t\in [0,1]$$

(16)

where

$$\begin{array}{ccc}\hfill f_{i,n}(s,t)& =& {\parallel P\left(s\right)-Q\left(t\right)\parallel }^2\hfill \\ & =& \parallel (\mathcal{A}+s\left(\overrightarrow{\mathcal{A}\mathcal{B}}\right))-(\mathcal{C}+t\left(\overrightarrow{\mathcal{C}\mathcal{D}}\right)){\parallel }^2\hfill \end{array}$$

(17)

To deal with the constraints, we construct the Lagrangian function:

$$L=f_{i,n}(s,t)-{\lambda }_{1}s+{\lambda }_2(s-1)-{\lambda }_{3}t+{\lambda }_4(t-1)$$

(18)

where Lagrange multipliers ${\lambda }_1,\dots ,{\lambda }_4$ are all non-negative. The solution $(s^{*},t^{*})$ to the above optimization problem needs to satisfy the Karush–Kuhn–Tucker (KKT) conditions:

$$\begin{array}{ccc}\hfill {\displaystyle \frac{\partial L}{\partial s}}& =& 2{\left(\overrightarrow{\mathcal{A}\mathcal{B}}\right)}^T(\overrightarrow{\mathcal{C}\mathcal{A}}+s\left(\overrightarrow{\mathcal{A}\mathcal{B}}\right)-t\left(\overrightarrow{\mathcal{C}\mathcal{D}}\right))\hfill \\ & & +{\lambda }_2-{\lambda }_1=0\hfill \end{array}$$

(19)

$$\begin{array}{ccc}\hfill {\displaystyle \frac{\partial L}{\partial t}}& =& -2{\left(\overrightarrow{\mathcal{C}\mathcal{D}}\right)}^T(\overrightarrow{\mathcal{C}\mathcal{A}}+s\left(\overrightarrow{\mathcal{A}\mathcal{B}}\right)-t\left(\overrightarrow{\mathcal{C}\mathcal{D}}\right))\hfill \\ & & +{\lambda }_4-{\lambda }_3=0\hfill \end{array}$$

(20)

$$\begin{array}{ccc}\hfill s,t& \in & [0,1]\hfill \end{array}$$

(21)

$$\begin{array}{ccc}\hfill {\lambda }_i& \ge & 0\hfill \end{array}$$

(22)

$$\begin{array}{ccc}\hfill {\lambda }_{1}s& =& {\lambda }_2(s-1)={\lambda }_{3}t={\lambda }_4(t-1)=0\hfill \end{array}$$

(23)

Thus, the closest distance between all pairs of skew line segments is calculated by

$$d^{min}=\underset{i,n}{inf}\sqrt{f_{i,n}^{*}}$$

(24)

where $f_{i,n}^{*}$ is optimal $f_{i,n}$ obtained by solving (16) with the i-th line segment of the mobile robot and n-th segment of the human skeletons.

4.2. The Threatening Index Based on Relative Velocity

Based on the minimum distance between a human and a mobile robot, we can set up the corresponding safety constraint. However, this is insufficient to ensure the safety in real-time under complex interaction scenarios, especially when the humans and robots are moving fast. Thus, this subsection aims to extend the definition of “minimum distance” by considering the factor of the relative velocity between a human and a robot.

As shown in Figure 3, $r_i$ denotes the i-th link of the robot, $h_m$ and $h_n$ represent two segments of the human skeleton, and $V_{r,(•)}$ represents the velocity of $P_{(•)}$. $V_{h,(•)}$ represents the velocity of $Q_{(•)}$. According to the method proposed in Section 4.1, the closest points between $r_i$ and $h_m$ are $P_{i,m}$ and $Q_{i,m}$, while the closest points between $r_i$ and $h_n$ are $P_{i,n}$ and $Q_{i,n}$. It can be observed that $d_{i,m}<d_{i,n}$. However, point $Q_{i,m}$ is moving away from point $P_{i,m}$, while point $Q_{i,n}$ is moving towards point $P_{i,n}$. Under the proximity-based DCBF constraint (12), $d_{i,m}$ is taken as the minimum distance, at which the robot will take evasive action towards point $Q_{i,m}$. However, this ignores the more threatening point $Q_{i,n}$, which may lead to a collision with $h_n$ in the near future. Therefore, when constructing the DCBF constraint, it is also necessary to consider the units that give greater threats in relative motion in addition to the minimum distance between humans and the robot.

Taking $r_i$ and $h_n$ in Figure 3 as an example, the threat level to the system is quantified by the relative velocities between humans and robots. This is given by the threatening index based on the principle illustrated in Figure 4.

As shown in Figure 4, $v_{i,n}$ is the relative velocity between $P_{i,n}$ and $Q_{i,n}$ and

$$v_{i,n}=V_{r,i,n}-V_{h,i,n}$$

$\theta$ is the angle between the relative velocity vector and the distance vector (sharing a common starting point), and $D^{min}$ is the predefined threshold of safe distance between humans and robots. Hence, the threatening index $k_{i,n}$ for skew line segment pair $(i,n)$ is calculated as

$$\begin{array}{ccc}\hfill k_{\mathrm{I},i,n}& =& {\displaystyle \frac{v_{i,n}{\delta }_t}{d_{i,n}cos{\theta }_{i,n}}}\hfill \end{array}$$

(25)

$$\begin{array}{ccc}\hfill k_{\mathrm{A},i,n}& =& {\displaystyle \frac{D^{min}-d_{i,n}sin{\theta }_{i,n}}{D^{min}}}\hfill \end{array}$$

(26)

$$\begin{array}{ccc}\hfill k_{i,n}& =& \left\{\begin{array}{c}0,\mathrm{if}{k}_{\mathrm{A},i,n}<0\hfill \\ k_{\mathrm{I},i,n}(k_{\mathrm{A},i,n}+1),\mathrm{if}{k}_{\mathrm{A},i,n}\ge 0\hfill \end{array}\right.\hfill \end{array}$$

(27)

where $k_{\mathrm{I},i,n}$ is related to the intensity of the robot’s impact on the human body at the current relative velocity. $k_{\mathrm{A},i,n}$ is related to the angle of impact at the time of collision, and ${\delta }_t$ is the control period.

• When $k_{\mathrm{A},i,n}<0$, it indicates that the robot moving at the current relative velocity will not intrude into the safety range of the obstacle. Thus, the threatening index is set to zero.
• When $k_{\mathrm{A},i,n}\ge 0$, the robot has the risk of intruding into the safety range of the obstacle at the current relative velocity. In this case, $k_{i,n}$ is set to $k_{\mathrm{I},i,n}(k_{\mathrm{A},i,n}+1)$, where $(k_{\mathrm{A},i,n}+1)$ ensures that the threat coefficient is at least $k_{\mathrm{I},i,n}$ when $k_{\mathrm{A},i,n}=0$. Thus, a non-zero threatening index is issued.

We compute the threatening index $k_{i,n}$ between each pair $(i,n)$ and select the maximum $\overline{k}$. Later, we obtain the minimum $\underline{d}$ across distance $f_{i,n}$ for pair $(i,n)$ with the identical maximum threatening index $\overline{k}$:

$$\begin{array}{ccc}\hfill \overline{k}& =& \underset{i,n}{max}{k}_{i,n},\hfill \end{array}$$

(28)

$$\begin{array}{ccc}\hfill \underline{d}& =& min\{f_{i,n}\mid k_{i,n}=\overline{k}\}\hfill \end{array}$$

(29)

We can further refine the selection of the object pairs $(i,n)$, all of which have $f_{i,n}=d^{min}$. This is done by selecting the pair with the highest threatening index:

$$\begin{array}{ccc}\hfill S& =& \{(i,n)\mid f_{i,n}=d^{min}\},\hfill \end{array}$$

(30)

$$\begin{array}{ccc}\hfill (i^{*},n^{*})& =& arg\underset{(i,n)\in S}{max}{k}_{i,n},\hfill \end{array}$$

(31)

4.3. Revised Safety Constraints

After identifying the most threatened human parts with the threatening index based on human–robot relative velocity, a more comprehensive safety constraint is constructed in this section.

Based on the distance and threatening index between humans and robots, we can design multi-objective constraints for the safe set $\mathcal{C}$ defined in (12) with the prescribed safe distance $D^{min}$ as

$$\Delta B\left({\chi }_k\right)={\lambda }_{\mathrm{D}}{B}_{\mathrm{D}}+(1-{\lambda }_{\mathrm{D}})B_{\mathrm{V}}$$

(32)

where $B_{\mathrm{D}}$ corresponds to the constraint of the closest distance, and $B_{\mathrm{V}}$ corresponds to the distance constraint of the object with the maximum threatening index based on the relative velocity. The specific constraints are as follows:

$$\begin{array}{ccc}\hfill B_{\mathrm{D}}& =& d^{min}-D^{min}\ge 0\hfill \end{array}$$

(33)

$$\begin{array}{ccc}\hfill B_{\mathrm{V}}& =& \underline{d}-D^{min}\ge 0\hfill \end{array}$$

(34)

${\lambda }_{\mathrm{D}}$ is the weight distribution coefficients for the two constraints, allocated based on the threatening index:

$${\lambda }_{\mathrm{D}}={\displaystyle \frac{1}{\overline{k}+1}}$$

(35)

To ensure the forward invariance of the set $\mathcal{C}$, the DCBF-based constraints (12) are further given as:

$$\begin{array}{ccc}\hfill d_{k+1}^{min}-d_k^{min}& \ge & -\gamma (d_k^{min}-D^{min})\hfill \end{array}$$

(36)

$$\begin{array}{ccc}\hfill {\underline{d}}_{k+1}-{\underline{d}}_k& \ge & -\gamma ({\underline{d}}_k-D^{min})\hfill \end{array}$$

(37)

Unlike prior studies on fixed-base robotic arms that only consider the distance between humans and the robotic arm [22], this composite constraint considers both human–robot distance and relative velocity.

When the mobile robot aims to reach a desired posture after completing the trajectory, we can construct a constraint about $V\left({\chi }_k\right)$ as

$$\begin{array}{ccc}\hfill d_{g,k}& =& \parallel {\wp }_k-{\wp }_{d,k}{\parallel }_2\hfill \end{array}$$

(38)

$$\begin{array}{ccc}\hfill V\left({\chi }_k\right)& \triangleq & ϵ-d_{g,k}>0\hfill \end{array}$$

(39)

where $d_g$ is the distance between the current end-effector position ℘ and the desired position ${\wp }_d$. ℘ and ${\wp }_d$ are the position vectors extracted from the a in (4) and $a_d$ in (5), respectively. $ϵ$ is a very small constant. So the DCLF-based constraint in (13) is given as:

$$-d_{g,k+1}+d_{g,k}\le -\alpha (ϵ-d_{g,k})+\delta$$

(40)

By constructing the aforementioned constraints, the mobile robot can be equipped with a framework based on the hybrid DCBF and DCLF, ensuring safe and efficient operation in dense human-robot coexistence environments. The proposed safe control method for a mobile robot is summarized in Figure 5.

5. Simulation

5.1. The Setting of Simulation

To verify the effectiveness of the proposed method, two mobile robots are imported for simulation. Each consists of a mobile platform with four Mecanum wheels and a KUKA iiwa robotic arm. The task is to let them approach their corresponding target poses in a human–robot environment, interrupted by three humans. The paths of the humans are manually preset but not known in advance by the mobile robots, and the posture data of human bodies are obtained from a motion capture dataset. The simulation is performed on a workstation equipped with a 12th Gen Intel® Core™ i9-12900H 2.50 GHz processor. The optimization problem is solved by fmincon function in MATLAB(2022a), selecting the interior-point method for its low memory footprint and real-time capability. The initial solution is set to the current state values.

Figure 6 illustrates the initial state of the mobile robot. A red dot in the XY-plane marks the origin of the mobile platform’s frame, and the blue lines outline its edges. A robotic arm is mounted on each mobile platform. The joint positions are indicated by red dots, and the links are depicted as black line segments. A pentagram denotes the target for the robotic arm’s end-effector. The mobile robots aim to reach their targets while maintaining safe distances from all humans. Robot 1’s mobile platform starts at ${[-3,-3,\pi /2]}^T\in SE\left(2\right)$, and its arm is initially rested vertically. The goal is to let the end effector move to ${[0,0.97,0,3.68,3,1]}^T\in SE\left(3\right)$. Robot 2’s platform starts at ${[3,3,-\pi /2]}^T\in SE\left(2\right)$ with its arm being initialized in the same way as Robot 1, targeting ${[0,0.97,0,-2.32,-3,1]}^T\in SE\left(3\right)$. The homogeneous transformation matrix from the mobile platform’s frame to the robotic arm base is ${\left(T_R^C\right)}^{\vee }={[0,0,0,0,0,0.3]}^T$. Human 1 moves back and forth between the robots’ target points, Human 2 orbits the origin, and Human 3 traverses radially across the mobile robots’ paths.

When screening obstacle-avoidance objects, the mobile platform is simplified as a planar quadrilateral bounding box. For the robotic arm, the rotations of joints 1, 3, and 5 in the KUKA iiwa configuration do not affect the positions of subsequent links. Therefore, the robotic arm can be simplified into four segments: from the base to joint 2 (Link 0 in Figure 1, joint 2 to joint 4 (Link 1), joint 4 to joint 6 (Link 2), and joint 6 to joint 7 (Link 3). Link 0 is fixed to the mobile platform and will not extend beyond the planar quadrilateral bounding box of the mobile platform. The mobile robot is thus simplified into four objects: the planar quadrilateral bounding box of the mobile platform and Link 1, 2, and 3 of the robotic arm.

By incorporating the DCBF-based constraints (36), (37) into the optimization formulation (12), we can obtain ${\dot{\chi }}_k$ to drive a mobile robot to maintain a safe distance from humans or other obstacles during movement. The DCBF constraints based on proximity and threatening index are set up for the mobile platform and Link 1, 2, and 3. Consequently, there are eight DCBF-based constraints to ensure safety in such human–robot coexistence environments. When the end-effector approaches the target point, the DCLF-based constraint (13) is applied to all control methods, ensuring that the end-effector moves as close as possible to $a_d$ if no human is interfering while maintaining stability. Consequently, the optimization problem (11) is formulated, and we impose the following joint limits for the mobile platform:

• Limits of wheels’ velocity: $[10,10,10,10]$;
• Limits of joints’ velocity for the robotic arm: $[85,85,100,75,130,135,135]$;
• Limits of joints’ position limits for the robotic arm: $[170,120,170,120,170,120,175]$.

We would like to compare our safe control method with the conventional one containing DCBF with proximity-based DCBF. Further, we also compare our method with the Dynamic Window Approach (DWA) and Artificial Potential Field (APF) method to accomplish navigation and obstacle avoidance for the mobile platform and robotic arm, respectively [41]. For the method with proximity-based DCBF, the simulation setup is identical to that used for both proximity-based and threatening-index-based DCBF. For the DWA, the prediction horizon is set to six steps, and the scoring weights for the target, velocity, obstacles, orientation, and path smoothness are [0.70, 0.15, 0.05, 0.05, 0.05]. For the APF, the obstacle repulsion and the target attraction coefficients are set to be 0.5 and 1.5 accordingly. The codes for implementation of this simulation example are given in the Supplementary Material.

5.2. Results and Discussions

Figure 7 and Figure 8 show the distances between the two mobile robots and human units. The blue lines correspond to the control with DCBF constraints that consider both the closest point and most threatening point, designated as “DCBF (D&V)”. For comparison, the red lines correspond to the safe control with proximity-based DCBF constraints, designated “DCBF (D)”, and the green lines correspond to the control with DWA&APF method. It can be observed that with our method, all parts of the mobile robots maintain safe distances from humans. In contrast, targets to be dodged are incorrectly selected by proximity-based DCBF and undesired closer human–robot distances at subsequent moments occur, as proximity-based DCBF neglects the relative velocity during human–robot interactions and it treats humans as static obstacles at the current moment. Meanwhile, the mobile robots’ moving trajectories by the DWA&APF method sometimes violate the safety distance when encountering overly dense scenarios or aggressive maneuvers. Additionally, in dense environments, our method achieves a slight temporal advantage over the proximity-based DCBF constraints and less oscillation compared with that by DWA&APF method.

Figure 9 demonstrates the target convergence process of dual mobile robots’ end-effectors, where red and blue trajectories represent Robot 1 and 2, respectively. Comparative experiments reveal that under integrated constraints with proximity-based DCBF (36) and highest-threatening-index-based DCBF (37), Robot 1 reaches its target at the 60th time instant, converging $25\%$ faster compared to the distance-constraint-only approach, which requires 80 time instants. Both configurations of constraints enable Robot 2 to synchronously arrive at its target at the 78th instant, limited by narrow passage dynamic constraints. Meanwhile, by means of the DWA&APF method, Robot 2 successfully initiates its navigation task. However, Robot 1 fails to move towards its target point. This is due to the fact that a human unit is near Robot 1 initially, causing the controller to prioritize obstacle avoidance. Subsequently, as Robot 2 approaches, it acts with other obstacles to obstruct Robot 1, leading Robot 1 to become stuck and unable to complete the navigation. Since Robot 1 is stuck near Robot 2’s target point, Robot 2 also fails to reach its destination. This performance enhancement stems from the proactive threat mitigation mechanism embedded in our proposed integrated constraints, which accelerates disengagement from hig y dense human–robot coexistence zones.

6. Conclusions

This paper establishes a safe control framework for environments with multiple coexisting mobile robots and humans. The coupled kinematic model for a mobile platform and a robot arm is firstly built with defined discrete-time control barrier function (DCBF) and discrete-time Lyapunov function (DCLF). Subsequently, the closest distances between parts of humans and the mobile robots are solved by convex programming with parametric description of pairs of skew lines. Specifically, the threatening index is defined and incorporated into DCBF by mutual projections of the relative velocity and common normal vector of skew line segments. Simulation of a scenario with two mobile robots and three humans validated the effectiveness of the proposed method. The proposed method can be applied to the safe control of mobile robots in crowded human–robot environments. The kinematic modeling of the current work is applicable to the mobile robot with Mecanum wheels. In future work, we will extend our proposed method to a mobile robot with power caster wheels and dual robotic arms.