2.1. Basic description

This study proposes a dual-arm motion planning framework that integrates joint discomfort minimization with joint collision avoidance and trajectory tracking constraints at the kinematic optimization level. The planning objectives for dual manipulators are primarily governed by three coupled requirements. First, the end-effectors must rigorously track predefined spatial trajectories; second, real-time inter-joint collision avoidance must be enforced during cooperative task execution in overlapping workspaces to prevent physical interference; finally, the integration of joint discomfort minimization enables dynamic joint motion adaptation, enhancing ergonomic motion naturalness while preventing kinematically anomalous configurations. This tri-objective optimization paradigm systematically coordinates joint space utilization, thereby reducing redundant collision avoidance maneuvers and improving operational fluency in constrained workspaces.

Let $ x \in \mathbb{R}^m$ , where $m$ is the task-space dimension of the robotic arm’s end-effector position. Then the relationship between the end-effector position and the joint angle $ \theta \in \mathbb{R}^n$ (with $n$ being the number of DOF) is defined by the forward kinematics:

(1) \begin{equation} x = f(\theta ) \end{equation}

where $ \theta \in \mathbb{R}^n$ , n is the DOF of the robot arm and $ f(\!\cdot\! )$ is the positive kinematics of the robot. The mapping relationship between $ \dot x \in \mathbb{R}^m$ and $ \dot \theta \in \mathbb{R}^n$ at the velocity level is expressed as:

(2) \begin{equation} \dot x = J(\theta )\dot \theta \end{equation}

where $J(\theta ) \in \mathbb{R}^{m \times n}$ is the Jacobian matrix. For redundant manipulators, the condition $m\lt n$ holds, enabling infinite joint space solutions for a given task-space trajectory.

During the execution of collaborative dual-arm tasks, humanoid robots face the risk of arm collisions. Collision detection methods based on bounding volume-based are commonly employed to determine whether a robot collides with its external environment. We specifically adopt spherical bounding volumes (SBVs) to characterize the robot’s kinematic configuration, enabling efficient real-time collision avoidance through analytical distance computation. To distinguish between the SBVs of the left and right arms, we denote ${}^rA_i$ and ${}^rJ_i$ as the i-th SBV center and corresponding Jacobian matrix for the left arm, respectively, and ${}^lA_j$ and ${}^lJ_j$ as the j-th SBV center and corresponding Jacobian matrix for the right arm, respectively.

Figure 2.

Spherical bounding volume-based dual-arm manipulators.

As illustrated in Figure 2, let the SBV centers for the right manipulator be represented by the matrix ${}^rA = [{}^rA_1,\ldots ,{}^rA_a,{}^rA_{(a+1)},\ldots ,{}^rA_{(a+b)}] \in \mathbb{R}^{m \times (a+b)}$ where $m$ denotes the spatial dimension, $a$ and $b$ respectively represent the number of SBVs allocated to the forearm and end-effector segments. The corresponding SBV radii are defined as $r_1$ (forearm) and $r_2$ (end-effector). Similarly, the left manipulator’s SBV centers are represented by the matrix ${}^lA = [{}^lA_1,\ldots ,{}^lA_a,{}^lA_{(a+1)},\ldots ,{}^lA_{(a+b)}] \in \mathbb{R}^{m \times (a+b)}$ . The sufficient condition for collision-free dual-arm operation requires that for any pair of SBV centers ${}^rA_i \in {}^rA$ and $ {}^lA_j \in {}^lA$ , the following inequality holds $|| {}^rA_i - {}^lA_j||_2^2 \ge D$ , where the safety threshold $D$ is determined by $D = R_r + R_l + \Delta d$ , with $ R_r \in \{r_1,r_2\}$ and $ R_l \in \{r_1,r_2\}$ denoting the SBV radii of the right and left manipulators, respectively, and $\Delta d \gt 0$ specifying the redundant safety margin. According to the velocity escape method [Reference Zhang and Wang11], $||{}^rA_i - {}^lA_j||_2^2 \ge D$ is described as a kinematic function as follows:

(3) \begin{equation} \begin{split} \frac{\text{d}}{\text{d}t} ||{}^rA_i - {}^lA_j||_2^2 & \ge K(D - ||{}^rA_i - {}^lA_j||_2^2) \\[3pt] i, j &= 1, 2, \ldots , (a + b) \end{split} \end{equation}

The kinematic mapping between SBV center coordinates and joint angles satisfies ${}^rA_i={}^rf_i(\theta _r)$ , ${}^lA_j={}^lf_j(\theta _l)$ , where ${}^rf_i(\!\cdot\! )$ and ${}^lf_j(\!\cdot\! )$ denote forward kinematics functions for respective SBV centers. The velocity propagation is governed by Jacobian matrices ${}^r{\dot A} = {}^rJ_i {\dot \theta }_r$ and ${}^l{\dot A} = {}^lJ_j {\dot \theta _l}$ . The temporal derivative of SBV center distance is derived as:

(4) \begin{align} \begin{split} \frac{\text{d}}{\text{d}t} ||{}^rA_i - {}^lA_j||_2^2 &= \frac{\text{d}}{\text{d}t} \left ( \sqrt {({}^rA_i - {}^lA_j)^T ({}^rA_i - {}^lA_j)} \right ) \\[2pt] &= \frac {1}{\|{}^rA_i - {}^lA_j}\|_2^2 ({}^rA_i - {}^lA_j)^T ({}^r{\dot A_i} - {}^r{\dot A_j}) \\[2pt] &= \frac {1}{\|{{}^rA_i - {}^lA_j}\|_2^2} \left ( {}^rA_i - {}^lA_j \right )^T \left ( {}^rJ_i {\dot {\theta }}_r - {}^lJ_j {\dot {\theta }_l} \right ) \end{split} && \end{align}

Equations (3) and (4) give

(5) \begin{equation} \begin{split} {\overrightarrow {{}^rA_i-{}^lA_j}}^T [{}^rJ_i, -{}^lJ_j]\big[{\dot \theta }^T_r,{\dot \theta ^T_l}\big]^T \geq K \left (D - \|{}^rA_i - {}^lA_j\|_2^2 \right ) \end{split} \end{equation}

where ${\overrightarrow {{}^rA_i-{}^lA_j}}^T = \frac {({}^rA_i - {}^lA_j)^T}{\|{}^rA_i - {}^lA_j\|_2^2}$ is the unit vector from ${}^lA_j$ to ${}^rA_i$ and $K \gt 0$ . To ensure comprehensive collision avoidance, all SBV centers must satisfy (5). This leads to the aggregated constraint:

(6) \begin{equation} \begin{split} J_{\text{ob}}[{\dot \theta ^T_r},{\dot \theta }^T_l]^T \leq K D_{\text{dis}} \end{split} \end{equation}

where

\begin{align*} J_{\text{ob}} = \begin{bmatrix} -\:{\overrightarrow {{}^rA_1-{}^lA_1}}^T {}^rJ_1 & \:{\overrightarrow {{}^rA_1-{}^lA_1}}^T {}^lJ_1 \\ \vdots & \vdots \\ -\:{\overrightarrow {{}^rA_1-{}^lA_b}}^T {}^rJ_{1} & \:{\overrightarrow {{}^rA_1-{}^lA_b}}^T {}^lJ_b \\ \vdots & \vdots \\ -\:{\overrightarrow {{}^rA_a-{}^lA_1}}^T {}^rJ_{a} & \:{\overrightarrow {{}^rA_a-{}^lA_1}}^T {}^lJ_{1} \\ \vdots & \vdots \\ -\:{\overrightarrow {{}^rA_a-{}^lA_b}}^T {}^rJ_{a} & \:{\overrightarrow {{}^rA_a-{}^lA_b}}^T {}^lJ_{b} \end{bmatrix} \in \mathbb{R}^{(a+b)^2 \times 2m} , D_{\text{dis}} = \begin{bmatrix} \| {}^rA_1 - {}^lA_1 \|_2^2 - D \\ \vdots \\ \| {}^rA_1 - {}^lA_b \|_2^2 - D \\ \vdots \\ \| {}^rA_a - {}^lA_1 \|_2^2 - D \\ \vdots \\ \| {}^rA_a - {}^lA_b \|_2^2 - D \end{bmatrix} \in \mathbb{R}^{(a+b)^2}. \end{align*}

The structure of $J_{\text{ob}}$ explicitly accounts for counteracting velocity components between manipulators, fully taking into account the relative motion of the dual arms.

Conventional dual-arm motion planning algorithms, predominantly based on velocity norm minimization, often neglect critical humanoid-specific constraints such as shoulder/waist joint limits and workspace boundaries. This oversight leads to two detrimental effects in overlapping workspace operations: (1) tendency to approach joint limit thresholds, compromising end-effector precision and (2) generation of kinematically unnatural arm configurations. As demonstrated in Figure 1(a), minimizing velocity norm solutions frequently drive shoulder joints toward mechanical limits, whereas it can be seen from Figure 1(b) that (b) is superior to (a) in terms of the naturalness of the joint and is not approaching the joint limit position. Therefore, this paper improves upon the traditional optimization objective of the velocity norm minimization by proposing a new optimization target – joint discomfort minimization – that is better suited to human joint motion. In ergonomic studies, the motion of both arms is influenced by the elastic potential energy of muscles and gravitational potential energy [Reference Zhao, Xie and Song32]. The greater the elastic and gravitational potential energies experienced during arm movement, the lower the comfort level of the arms.

Joint discomfort is derived from VSR [Reference Yang, Marler and Kim33], which defines a neutral position during motion, termed the joint comfort angle, where the comfort of each joint angle is maximized. For biological organisms in natural states – whether in motion or at rest – the minimization of total potential energy (formulated as the aggregation of elastic and gravitational potential energies) corresponds to the state of maximum joint comfort. This implies that biological systems inherently seek an optimal configuration characterized by minimum joint discomfort. This principle constitutes a pivotal direction in current research on natural humanoid motion: the minimization of motion-related total potential energy correlates directly with enhanced naturalness and anthropomorphism in the motion process.

The degree of joint discomfort is evaluated based on the joint’s rotational position relative to its upper and lower limits, with discomfort increasing significantly as the joint approaches these limits. In Ref. [Reference Ma, Zhang, Chablat, Bennis and Guillaume34], joint discomfort was proposed as an objective function for predicting human postures. To ensure the objective function remains convex, the target function is described as follows:

(7) \begin{equation} \begin{split} H_{\text{discomfort}}(\theta ) = \sum _{i=1}^{n}\beta (i) \left ( \frac {\theta (i) - \theta _{\text{comfort}}(i)}{\theta _{\text{max}}(i) - \theta _{\text{min}}(i)} \right )^2 \end{split} \end{equation}

where $n$ is the DOFs of the robotic arm; $\beta (i),\theta (i),\theta _{\text{max}}(i),\theta _{\text{min}}(i)$ represent the comfort weight coefficient, joint angle, upper joint angle limit, and lower joint angle limit of the $i$ -th joint, respectively. An example of joint discomfort calculation is illustrated in Figure 3. The overall joint discomfort reaches its minimum at the joint comfort angle position, while discomfort increases as the joint approaches its upper or lower limits. Joint discomfort is typically used in ergonomics to assess the comfort of individual movements, without considering other constraining factors. This paper adapts this metric for velocity-level optimization to enhance the comfort and naturalness of robot joint motions, while accounting for additional constraints.

Figure 3.

An example of the joint discomfort, $\theta _{\text{comfort}}=80 , \theta _{\text{max}}=180,\theta _{\text{min}}=20$ (°).

2.2. Control objective

During the motion of a humanoid robot’s dual-arm system, the system is constrained by its mechanical structure and motor performance. Consequently, in practical scenarios, both the joint angles $\theta$ and joint angular velocity $\dot \theta$ are subject to upper and lower limits. In the actual planning process for dual arms, it is necessary to separately consider the joint angles $\theta _r$ and $\theta _l$ and the joint angular velocities $\dot \theta _r$ and $\dot \theta _l$ of the right and left arms, respectively, ensuring that neither arm exceeds its limits. Thus, the constraints are defined as $\theta ^-_r\lt \theta _r\lt \theta ^+_r, \theta ^-_l\lt \theta _l\lt \theta ^+_l, \dot \theta ^-_r\lt \dot \theta _r\lt \dot \theta ^+_r, \dot \theta ^-_l\lt \dot \theta _l\lt \dot \theta _l$ , where $\theta ^-_r,\theta ^-_l,\theta ^+_r,\theta ^+_l$ represent the lower and upper bounds of the joint angles for the right and left arms, respectively, and $\dot \theta ^-_r,\dot \theta ^-_l,\dot \theta ^+_r,\dot \theta ^+_l$ denote the lower and upper bounds of the joint angular velocities for the right and left arms, respectively. For a redundant dual-arm manipulator, the control task of joint discomfort minimization is selected.

For a redundant manipulator $x = f(\theta )$ , given the desired positions of the dual-arm end-effectors $x_{rd}$ and $x_{ld}$ , the joint velocity vectors $\dot {\theta }_r$ and $\dot {\theta }_l$ are planned to approach ${}^rx_{d}$ and ${}^lx_{d}$ , while ensuring that the dual-arm manipulator do not collide during the planned motion. The optimization problem is formulated as follows:

(8a) \begin{align} \text{min} &\quad \frac {1}{2} \dot {\theta }_r^T \dot {\theta }_r + H_r + \frac {1}{2} \dot {\theta }_l^T \dot {\theta }_l + H_l \\[-10pt] \nonumber\end{align}

(8b) \begin{align} \text{s.t.} &\quad {}^rx_{d} = f(\theta _r) \\[-10pt] \nonumber\end{align}

(8c) \begin{align} &\quad {}^lx_{d} = f(\theta _l) \\[-10pt] \nonumber\end{align}

(8d) \begin{align} &\quad \theta _r^- \lt \theta _r \lt \theta _r^+ \\[-10pt] \nonumber \end{align}

(8e) \begin{align} &\quad \theta _l^- \lt \theta _l \lt \theta _l^+ \\[-10pt] \nonumber \end{align}

(8f) \begin{align} &\quad \dot {\theta }_r^- \lt \dot {\theta }_r \lt \dot {\theta }_r^+ \\[-10pt] \nonumber \end{align}

(8g) \begin{align} &\quad \dot {\theta }_l^- \lt \dot {\theta }_l \lt \dot {\theta }_l^+ \\[-10pt] \nonumber \end{align}

(8h) \begin{align} &\quad J_{\text{ob}} \big[\dot {\theta }_r^T, \dot {\theta }_l^T\big]^T \leq K D_{\text{dis}} \end{align}

let $\theta = [\theta _r^T,\theta _l^T]^T$ , $H = H_r+H_l$ , $\theta ^- = [(\theta _r^-)^T,(\theta _l^-)^T]^T$ , $\theta ^+ = [(\theta _r^+)^T,(\theta _l^+)^T]^T$ , $\dot \theta ^- = [(\dot \theta _r^-)^T,(\dot \theta _l^-)^T]^T$ , $\dot \theta ^+ = [(\dot \theta _r^+)^T,(\dot \theta _l^+)^T]^T$ , and $x_d = [{}^rx_{d}^T,{}^lx_{d}^T]^T$ . Equation (8) can be reformulated as follows:

(9a) \begin{align} \text{min} &\quad \frac {1}{2} \dot {\theta }^T \dot {\theta } + H \\[-10pt] \nonumber \end{align}

(9b) \begin{align} \text{s.t.} &\quad x_d = f(\theta ) \\[-10pt] \nonumber \end{align}

(9c) \begin{align} &\quad \theta ^- \lt \theta \lt \theta ^+ \\[-10pt] \nonumber\end{align}

(9d) \begin{align} &\quad \dot {\theta }^- \lt \dot {\theta } \lt \dot {\theta }^+ \\[-10pt] \nonumber\end{align}

(9e) \begin{align} &\quad J_{\text{ob}} \dot {\theta } \leq K D_{\text{dis}} \\[4pt] \nonumber \end{align}

2.3. QP-type problem reformulation

From Eq. (9), it can be observed that (9d) and (9e) describe the velocity level, (9b) and (9c) describe the position level, while (9a) involves descriptions at both the position and velocity levels. This makes the equation difficult to solve in real time. Therefore, in this section, Eq. (9) is transformed into a QP problem description at the velocity level.

1. (1) Optimization target reconstruction ( 9a )

The optimization objective $H$ is reformulated at the velocity level by taking the partial derivative with respect to time. Consequently, the time derivative is expressed as $\frac {\partial H}{\partial t} = \sum _{i=1}^{n} \frac {\partial }{\partial t} \left ( \beta _i \left ( \frac {\theta _i - \theta _{\text{comfort}, i}}{\theta _{\text{max}, i} - \theta _{\text{min}, i}} \right )^2 \right )$ . The resulting equation is detailed as follows:

(10) \begin{equation} \begin{split} \dot H = \sum _{i=1}^{n} \left (\frac {2\beta (i)\left (\theta (i) - \theta _{\text{comfort}}(i)\right )}{\left ( {\theta (i)}^+-{\theta (i)}^-\right )^2} \dot \theta (i)\right ) \end{split} \end{equation}

By performing a matrix transformation on this equation, the following expression is obtained:

(11) \begin{equation} \dot {H} = \left ( \tilde {\beta } \cdot \Delta \theta \right )^T \dot {\theta } \end{equation}

where $ \tilde {\beta } \in \mathbb{R}^{14 \times 1}$ , $ \tilde {\beta }(i) = \frac {2 \beta (i)}{({\theta (i)}^+ - {\theta (i)}^-)^2}$ , $ \Delta \theta \in \mathbb{R}^{14 \times 1}$ , and $ \Delta \theta (i) = \theta (i) - \theta _{\text{comfort}}(i)$ , and $ (\!\cdot\! )$ denotes the element-wise multiplication of matrices.

The physical description of the optimization objective in this reformulation process is as follows: based on the deviation between the current joint angle and the joint comfort angle, the current joint angular velocity is further optimized to approach the joint comfort angle while satisfying other constraints, thereby minimizing joint discomfort under the given constraints. Therefore, based on Eq. (9), the optimization objective can be reconstructed at the velocity level as:

(12) \begin{equation} \text{min} \quad \frac {1}{2} \dot {\theta }^T \dot {\theta } + \left ( \tilde {\beta } \cdot \Delta \theta \right )^T \dot {\theta } \end{equation}

1. (2) Reconstruction of Equality Constraint ( 9b )

To transform constraints (9b) into velocity-level equality constraints, the planning error $ e$ is defined as $ e = x - x_d$ . The evolution equation is given by:

(13) \begin{equation} \dot {e} = -\gamma \Phi (e) \end{equation}

where $ \gamma$ and $ \Phi (\!\cdot\! )$ represent a positive constant and a monotonically increasing odd function, respectively. Combining Eqs. (1), (2), and (13), we obtain

(14) \begin{equation} J \dot {\theta } = \dot {x}_d - \gamma \Phi (e) \end{equation}

where $ J = \begin{bmatrix} J_r &\quad 0 \\[1pt] 0 &\quad J_l \end{bmatrix}$ . It can be noted that by rewriting (9b), $ \dot {\theta }$ is subject to equality constraints. Although $ \dot {\theta }$ can be obtained from the pseudoinverse of $ J$ , the redundancy of the Jacobian matrix implies that the solution for $ \dot {\theta }$ is not unique. Through additional inequality constraints and the objective function (9c–9e), this implicit formulation can fully use the redundant DOF of the robot.

1. (3) Inequality constraint reconstruction:

The joint inequality constraint (9c) is reconstructed into a velocity-level description. For a joint angle $ \theta (t)$ at a specific time instant, let $ \theta ^+ - \theta (t)$ represent the difference between $ \theta (t)$ and its upper bound $ \theta ^+$ . According to the velocity escape method [Reference Zhang and Wang11], it can be proven that when $ \dot {\theta }$ satisfies the following inequality, $ \theta (t) \leq \theta ^+$ always holds:

(15) \begin{equation} \dot {\theta } \leq \alpha (\theta ^+ - \theta (t)) \end{equation}

where $ \alpha \gt 0$ is a positive constant. Similarly, the lower bound constraint on the joint angle, $ \theta ^-\leq \theta (t)$ , can be described as:

(16) \begin{equation} \dot {\theta } \geq \alpha (\theta ^- - \theta (t)) \end{equation}

By combining Eqs. (9c), (9d), (15), and (16), the reformulated inequality constraint can be expressed as:

(17) \begin{equation} \text{max}(\alpha (\theta ^- - \theta (t)), \dot {\theta }^-) \leq \dot {\theta } \leq \text{min}(\dot {\theta }^+, \alpha (\theta ^+ - \theta (t))) \end{equation}

Thus far, through the reconstruction of equality and inequality constraints, the resulting QP problem is formulated as follows:

(18a) \begin{align} \text{min} &\quad \frac {1}{2} \dot {\theta }^T \dot {\theta } + \left ( \tilde {\beta } \cdot \Delta \theta \right )^T \dot {\theta } \\[-10pt] \nonumber \end{align}

(18b) \begin{align} \text{s.t.} &\quad J \dot {\theta } = \dot {x}_d - \gamma \Phi (e) \\[-10pt] \nonumber \end{align}

(18c) \begin{align} \quad \max (\alpha &(\theta ^- - \theta (t)), \dot {\theta }^-) \leq \dot {\theta } \leq \min (\dot {\theta }^+, \alpha (\theta ^+ - \theta (t))) \\[-10pt] \nonumber \end{align}

(18d) \begin{align} &\quad J_{ob} \dot {\theta } \leq K D_{dis} \\[6pt] \nonumber \end{align}

As described in Eq. (18), the solution $ \dot {\theta }$ of the QP problem simultaneously satisfies the equality constraint (18b) and the inequality constraints (18c) and (18d), which correspond to the convergence of the motion planning error, joint constraints, and self-collision avoidance constraints of the dual arms, respectively. Furthermore, to ensure that the joint motion of the humanoid dual arms tends to minimize joint discomfort during the execution of the planned motion, a linear term $ (\tilde {\beta } \cdot \Delta \theta )^T \dot {\theta }$ is added to the joint velocity term $ \dot {\theta }^T \dot {\theta }$ . This aims to achieve an overall minimum of the Euclidean norm and joint discomfort, optimizing the joint velocities in real time to make the dual-arm motion more natural and humanoid. Therefore, the motion planning problem of minimizing joint discomfort and achieving dynamic obstacle avoidance for the redundant dual-arm manipulator is transformed into solving the QP problem in real time.

3.1. RNN design

To address the constrained optimization problem (18), let $ L$ be the Lagrange function defined as:

(19) \begin{equation} \begin{split} L = & \frac {1}{2} \dot {\theta }^T \dot {\theta } + \left ( \tilde {\beta } \cdot \Delta \theta \right )^T \dot {\theta } \\[3pt] & + \lambda _1^T (\dot {x}_d - \gamma \Phi (e) - J \dot {\theta }) \\[3pt] & + \lambda _2^T (J_{\text{ob}} \dot {\theta } - K D_{\text{dis}}) \end{split} \end{equation}

where $ \lambda _1 \in \mathbb{R}^{2m}$ and $ \lambda _2 \in \mathbb{R}^{(a+b)^2}$ are the dual-state variables of the dual arms, corresponding to the constraints in (18b) and (18d), respectively. Using the Karush–Kuhn–Tucker (KKT) conditions, the solution to Eq. (18) satisfies:

(20a) \begin{align} &\dot {\theta } = P_\Omega \left ( \dot {\theta } - \frac {\partial L}{\partial \dot {\theta }} \right ) \\[-10pt] \nonumber \end{align}

(20b) \begin{align} &J(\theta ) \dot {\theta } = \dot {x}_d - \gamma \Phi (e) \\[-10pt] \nonumber \end{align}

(20c) \begin{align} &\begin{cases} \lambda _2 \gt 0 & \text{if } J_{\text{ob}} \dot {\theta } \gt K D_{\text{dis}} \\ \lambda _2 = 0 & \text{if } J_{\text{ob}} \dot {\theta } \leq K D_{\text{dis}} \end{cases} \\[12pt] \nonumber\end{align}

in which $ P_\Omega = \text{argmin}_{y \in \Omega } \| y - x \|$ is the saturation operator corresponding to the joint velocity constraint (18c), where $ \Omega$ is defined as: $ \Omega = \{ \dot {\theta } \in \mathbb{R}^{2n} \mid \max\! (\alpha (\theta ^- - \theta ), \dot {\theta }^-) \leq \dot {\theta } \leq \min\! (\dot {\theta }^+, \alpha (\theta ^+ - \theta )) \}$ . Furthermore, due to the non-negative operation in (20c), Eq. (20c) can be rewritten as:

(21) \begin{equation} \lambda _2 = (\lambda _2 + J_{\text{ob}} \dot {\theta } - K D_{\text{dis}})^+ \end{equation}

where $ (\! \bullet \!)^+$ is a non-negative operator defined as: $ y^+ = [y_1; \dots ; y_b]^+ = [\!\max (y_1, 0); \dots ; \max\! (y_b, 0)]$

Using the nonlinear equations from (20) and (21) and leveraging the characteristics of parallel computing, the following RNN is established:

(22a) \begin{align} \epsilon \ddot {\theta } &= -\dot {\theta } + P_\Omega \left ( \tilde {\beta } \cdot \Delta \theta + J^T \lambda _1 - J_{\text{ob}}^T \lambda _2 \right ) \\[-10pt] \nonumber \end{align}

(22b) \begin{align} \epsilon \dot {\lambda }_1 &= \dot {x}_d - \gamma \Phi (e) - J \dot {\theta } \\[-10pt] \nonumber \end{align}

(22c) \begin{align} \epsilon \dot {\lambda }_2 &= -\lambda _2 + \left ( \lambda _2 + J_{\text{ob}} \dot {\theta } - K D_{\text{dis}} \right )^+ \\[12pt] \nonumber \end{align}

where $ \epsilon \gt 0$ is an adjustable parameter, the magnitude of which significantly affects the convergence speed of the equations. The term $ \tilde {\beta } \cdot \Delta \theta$ influences the adjustment of joint discomfort during motion, regulating the joint acceleration of each moving joint under the given constraints to minimize overall joint discomfort. Additionally, the state variables $ \lambda _1$ and $ \lambda _2$ play critical roles in target tracking and dynamic obstacle avoidance of the dual arms, respectively. As shown in Eq. (22a), the joint velocity command is composed of three factors: $ \tilde {\beta } \cdot \Delta \theta$ , $ J^T \lambda _1$ , and $ J_{\text{ob}}^T \lambda _2$ , representing the comfort angle offset, target tracking, and dynamic obstacle avoidance velocity components, respectively. From Eqs. (22b) and (22c), it is evident that target tracking and dynamic obstacle avoidance are achieved by controlling and adjusting $ \lambda _1$ and $ \lambda _2$ during the iteration process to regulate the joint velocities. Notably, this approach calculates the rate of change of $ \dot {\theta }$ in each iteration cycle, ensuring the continuity of the joint velocity commands.

3.2. Stability analysis

Before proving the convergence of the RNN-based motion planning method, the following relevant definitions and lemmas are first provided.

Definition 1. A continuously differentiable function $ F(\! \bullet \!)$ is a monotone function if its gradient satisfies the following positive semi-definite property:

(23) \begin{equation} y^T (\nabla F + \nabla F^T) y \geq 0, \quad \forall y \in \mathbb{R}^k \end{equation}

where $ \nabla F$ is the gradient function of $ F(\! \bullet \!)$ .

Lemma 1. A dynamic neural network is convergent if it satisfies the following dynamic equation:

(24) \begin{equation} \kappa \dot {x} = -x + P_S \left ( x - F(x) \right ) \end{equation}

where $ \kappa \gt 0$ is a positive constant describing the network’s convergence speed, $ F(x)$ is a monotone function, and $ P_S = \text{argmin}_{y \in S} \| y - x \|$ is the projection operator onto the convex set $ S$ .

Algorithm 1:

Minimizing Joint Discomfort Scheme in Dynamic Obstacle Avoidance for Dual-Arm Manipulators

This paper analyzes and proves its stability in three parts. First, it demonstrates that the neural network can converge to its equilibrium point. Then, it proves that this equilibrium point is equivalent to the optimal solution of the QP formulation of the problem. Finally, it analyzes the convergence of the planning error.

Proof 1. Equation (22) can be reformulated as:

(25a) \begin{align} \epsilon \ddot {\theta } &= -\dot {\theta } + P_\Omega \left ( \dot {\theta } - (\dot {\theta } - \tilde {\beta } \cdot \Delta \theta - J^T \lambda _1 + J_{\text{ob}}^T \lambda _2) \right ) \\[-10pt] \nonumber \end{align}

(25b) \begin{align} \epsilon \dot {\lambda }_1 &= -\lambda _1 + \left ( \lambda _1 - (\!-\dot {x}_d + \gamma \Phi (e) + J \dot {\theta }) \right ) \\[-10pt] \nonumber \end{align}

(25c) \begin{align} \epsilon \dot {\lambda }_2 &= -\lambda _2 + \left ( \lambda _2 - (\!-J_{\text{ob}} \dot {\theta } + K D_{\text{dis}}) \right )^+ \\[10pt] \nonumber \end{align}

It is worth noting that in Eq. (25b), $ \lambda _1 - (-\dot {x}_d + \gamma \Phi (e) + J \dot {\theta })$ can be regarded as a special saturation operator $ P_R$ , with an upper bound of $ +\infty$ and a lower bound of $ -\infty$ . Similarly, in Eq. (25c), $ \left ( \lambda _2 - (-J_{ob} \dot {\theta } + K D_{dis}) \right )^+$ can be described as the saturation operator $ P_{R+} = (\! \bullet \!)^+$ , with an upper bound of $ +\infty$ and a lower bound of 0. Therefore, by defining an auxiliary vector $ \xi = [\dot {\theta }^T, \lambda _1^T, \lambda _2^T]^T$ , the constructed dynamic neural network can be reformulated as:

(26) \begin{equation} \epsilon \dot {\xi } = -\xi + P_S \left [ \xi - F(\xi ) \right ] \end{equation}

where $ P_S(\! \bullet \!) = [P_\Omega ; P_R; P_{R+}]$ is the projection operator, and $ F(\xi )$ is a differentiable function defined as:

(27) \begin{equation} F(\xi ) = \begin{bmatrix} F_1 \\[2pt] F_2 \\[2pt] F_3 \end{bmatrix} = \begin{bmatrix} \dot {\theta } - \tilde {\beta } \cdot \Delta \theta - J^T \lambda _1 + J_{\text{ob}}^T \lambda _2 \\[2pt] -\dot {x}_d + \gamma \Phi (e) + J \dot {\theta } \\[2pt] -J_{\text{ob}} \dot {\theta } + K D_{\text{dis}} \end{bmatrix} \end{equation}

The gradient of the function $ F(\xi )$ is calculated as:

(28) \begin{equation} \nabla F(\xi ) = \frac {\partial F}{\partial \xi } = \begin{bmatrix} I & -J^T & J_{\text{ob}} \\[2pt] J &\quad 0 &\quad 0 \\[2pt] -J_{\text{ob}}^T &\quad 0 &\quad 0 \end{bmatrix} \end{equation}

Thus, $ \nabla F(\xi ) + \nabla F^T(\xi ) = \begin{bmatrix} 2I &\quad 0 &\quad 0 \\ 0 &\quad 0 &\quad 0 \\ 0 &\quad 0 &\quad 0 \end{bmatrix}$ , and according to Theorem 1, $ F(\xi )$ is monotone. Therefore, for any vector $ y = [y_1;\, y_2;\, y_3]$ , where $ y_1 \in \mathbb{R}^n$ , $ y_2 \in \mathbb{R}^m$ , and $ y_3 \in \mathbb{R}^{ab}$ , the following holds:

(29) \begin{equation} y^T (\nabla F + \nabla F^T) y = y_1^T y_1 \geq 0 \end{equation}

By comparing Eq. (26) with Eq. (24), it is evident that their forms are identical. Moreover, since $ F(\xi )$ is monotone, according to Theorem 1, the established RNN is globally stable and converges to its optimal solution.

In the derivation process of the above equations, the convergence of the constructed RNN has been proven. Meanwhile, from Eq. (20a), it can be observed that the joint discomfort parameter serves as a regulatory parameter within this RNN. It adjusts the optimal solution toward minimizing the overall joint discomfort without affecting the convergence of the entire global network.

Proof 2. Let $ \dot {\xi } = 0$ . Then, the optimal solution of the network is given by:

(30a) \begin{align} \dot {\theta } &= P_\Omega (\tilde {\beta } \cdot \Delta \theta + J^T \lambda _1 - J_{\text{ob}}^T \lambda _2) \\[-10pt] \nonumber \end{align}

(30b) \begin{align} J \dot {\theta } &= \dot {x}_d - \gamma \Phi (e) \\[-10pt] \nonumber\end{align}

(30c) \begin{align} \lambda _2 &= (\lambda _2 + J_{\text{ob}} \dot {\theta } - K D_{\text{dis}})^+ \\[10pt] \nonumber \end{align}

By comparing Eqs. (22) and (30), it can be concluded that the solutions of the two sets of equations are identical. That is to say, the solution to the QP problem can also be obtained using the RNN.

Proof 3. Define the Lyapunov function for the planning error $ e$ as $ V = \frac {1}{2} e^T e$ . Then, its time derivative is

(31) \begin{equation} \dot {V} = e^T \dot {e} = -\gamma e^T \Phi (e) \end{equation}

According to the definition of $ \Phi (e)$ , $ \Phi (e)$ and $ e$ lie in the same quadrant. The equation $ \dot {V} \leq 0$ holds if and only if $ e = 0$ . Based on Barbalat’s lemma, when $ e \to 0$ , $ t \to \infty$ .

4.1. Numerical simulation of dual-arm fixed point motion

To demonstrate the differences between the proposed joint discomfort minimization objective and the conventional minimum velocity norm optimization, a dual-arm fixed point tracking simulation was conducted. The target positions for the right and left arms were set to $[0.4, -0.2007, 0.9]$ and $[0.417, 0.3594, 0.9843]$ , respectively, where the left arm’s target corresponds to the end-effector position at the joint comfort angles. Collision avoidance was not considered in this experiment, as it focused solely on fixed point tracking. From the joint motion comparisons in Figures 5(a) and 5(b), it can be observed that the robot’s joint motion in Figure 5(a) is more inclined toward humanoid grasping behavior. The joint angle time-series plots in Figures 5(a) and 5(b) reveal that, after incorporating the minimum joint discomfort function, the joint angles tend to move toward the joint comfort angles. In this motion, the changes in $\theta (2)$ and $\theta (5)$ are particularly pronounced, gradually approaching the joint comfort angles. For instance, $\theta (5)$ converges from 1.57 rad to 0 rad. However, since the right arm’s target point does not correspond to the end-effector position at the joint comfort angle, its joint motion does not fully converge to the comfort angles but instead achieves the joint discomfort minimization under the constraint of end-effector tracking. In contrast, the left arm’s target point aligns with the end-effector position at the joint comfort angle, resulting in all joints converging to the comfort angles with an error controlled within $10^{-4}$ rad, reducing the left arm’s joint discomfort to 0. The overall joint discomfort is shown in Figure 4(f), with results consistent with the above observations. Under the minimum joint discomfort optimization, the motion of each joint converges to a configuration with the least joint discomfort while satisfying the remaining constraints. In contrast, as shown in Figure 5(b), motion based on the minimum velocity norm does not alter the joint configuration after satisfying end-effector tracking, with $\theta (2)$ and $\theta (5)$ showing the most noticeable differences. Additionally, Figure 6 (b) indicates that the overall joint discomfort during motion under the minimum velocity norm increases progressively and stabilizes without further change after reaching the target point. Figure 7 depicts the dual-arm motion at the same target point but with different initial robot positions. Figure 7(b) illustrates the motion process under the minimum velocity norm, where the joint configuration changes and differs significantly from the previous configuration under the same norm. In contrast, Figure 7(a) shows the motion process under the minimum joint discomfort optimization, where the final joint configuration remains consistent with the previous configuration. Thus, joint discomfort minimization can control the joint configurations throughout the motion process, resulting in more natural movements.

Figure 4.

Numerical data for fixed point motion under conditions of joint discomfort minimization: (a) profile of joint speed, (b) profile of $\lambda _1$ , (c) profile of $\lambda _2$ , (d) planning errors along x, y, and z axes, (e) velocity errors along x, y, and z axes, and (f) profile of joint discomfort value.

Figure 5.

Fixed point motion based on (a) joint discomfort minimization and the profile of joint angles and (b) velocity norm operation and the profile of joint angles.

Figure 6.

Numerical data for fixed point motion under conditions of velocity norm minimization: (a) planning errors along x, y, and z axes and (b) profile of joint discomfort values.

Figure 7.

Two motions of the same reference point after changing the starting point: (a) the motion based on joint discomfort minimization and (b) the motion based on velocity norm minimization.

Finally, a comparison of motion accuracy was conducted. As depicted in Figures 4(c) and 4(d), the end-effector achieves a position accuracy of $10^{-9}$ m for fixed-point tracking and a velocity tracking accuracy of $10^{-10}$ m/s. This position tracking accuracy is slightly higher than that observed under the optimized velocity norm, as substantiated by the comparison in Figure 6(a). Additionally, the motion process satisfies velocity constraints, as evidenced by Figure 4(a).

Figure 8.

Numerical data for linear motion under conditions of joint discomfort minimization: (a) profile of joint speed, (b) profile of $\lambda _1$ , (c) profile of $\lambda _2$ , (d) planning errors along x, y, and z axes, (e) velocity errors along x, y, and z axes, (f) profile of joint discomfort values, and (g) minimum distance between the SBV centers of the left and right arms.

Figure 9.

Fixed point motion based on (a) joint discomfort minimization and the profile of joint angles and (b) velocity norm minimization and the profile of joint angles.

4.2. Simulation of linear crossover motion of dual-arm manipulators

This experiment involves a simulation of dual-arm dynamic cross-collision avoidance motion. The desired motion trajectory for the right arm is given by $[0.4, -0.2 + \frac {0.35}{15} t, 1]$ in meters, and for the left arm by $[0.35, 0.2 - \frac {0.35}{15} t, 0.85]$ in meters. The dual-arm collision avoidance trajectory in this experiment involves linear motion, during which the arms cross each other. By comparing the cross-collision avoidance effectiveness and the overall joint discomfort coefficients of the left and right arms, the effects of minimum velocity norm and minimum joint discomfort optimization objectives are evaluated. First, comparing the robot’s trajectory tracking motion in Figures 9(a) and 9(b), the robot in Figure 9(a) exhibits an elbow-lifting action to approach the joint comfort angle. After the trajectory diverges, it deviates from the joint comfort angle due to end-effector tracking constraints. This can also be observed in the joint angle time-series plot, where $\theta (2)$ in Figure 9(a) initially approaches the joint comfort angle and then moves away over time. In Figure 9(b), the robot’s motion adheres to the minimum velocity norm optimization objective, with joint angle changes occurring linearly without the aforementioned approach behavior. Throughout the course of linear tracking, comparing Figures 8(f) and 10(c), the joint discomfort values for the joint discomfort optimization were reduced by at least $27\%$ with the velocity paradigm optimization. The comparison between Figures 8(f) and 10(c) yields the same conclusion: minimizing joint discomfort optimization allows the robot to approach the joint comfort angle during motion, adjusting joint configurations and reducing overall joint discomfort while satisfying other constraints.

Comparing Figures 8(g) and 10(d), under the same trajectory segment, the robot’s linear motion based on velocity norm minimization begins collision avoidance calculations around 11 s, with the right arm’s second joint quickly approaching its limit. This process not only increases the robot’s computational burden but also severely restricts end-effector tracking after the second joint reaches its limit. In contrast, during the same linear motion based on joint discomfort minimization, no joint collision avoidance calculations are required even at 15 s. Moreover, all joints, particularly the right arm’s second joint, do not approach their limits, leaving additional motion space for end-effector tracking and collision avoidance. The primary reason for this phenomenon is that the joint comfort angle is typically located at an intermediate position in the robot’s motion range, and during motion, the joint angles are adjusted in real time to approach this comfort angle, thereby providing more spatial freedom and better utilizing the humanoid dual-arm spatial structure.

Figure 10.

Numerical data for fixed point motion under conditions of velocity norm minimization: (a) profile of $\lambda _2$ , (b) planning errors along x, y, and z axes, (c) profile of joint discomfort values, and (d) minimum distance between the SBV centers of the left and right arms.

Figure 11.

Top view of the robot in linear motion: (a) the motion based on joint discomfort minimization and (b) the motion based on velocity norm minimization.

Finally, regarding the naturalness of the robotic arms, Figure 11 provides a top-down view of the dual-arm linear motion. As seen in Figure 11(b), the wrist joint of the robot under minimizing velocity norm exhibits unnaturalness. In contrast, the motion in Figure 11(a) appears more natural. The main reason for this difference is that the joint comfort angle, located at an intermediate position with optimal comfort, drives the robot under the minimum joint discomfort optimization to move toward the direction of maximum comfort, resulting in greater naturalness in Figure 11(a). The comparison between Figures 8(d), 8(e), and 8(b) demonstrates that the end-effector tracking accuracy of both approaches remains at the same order of magnitude (<0.15 mm).

4.3. Humanoid obstacle avoidance gripping placement experiment on real machine

The robot grasping motion based on joint discomfort minimization is illustrated in Figure 13(b1)–(b4), while the motion based on velocity norm minimization is shown in Figure 13(c1)–(c4). Both methods successfully executed the tasks of grasping, dual-arm collision avoidance, and object placement. However, there is a significant disparity in the joint motion postures between the two approaches.

Figure 12.

Data analysis of fixed point grasping: (a–d) joint discomfort minimization data with H, min dual-arm distance, distance error, and total position error compare with human data and (e–k) velocity norm minimization data with H, min dual-arm distance, distance error, and total position error compare with human data.

Therefore, to better benchmark the anthropomorphism and naturalness, we recruited five human subjects to perform the same end-effector motion tasks. Figure 13(a) depicts the operation process of one subject. We utilized a depth camera to record spatial positional data for both the human and robot motions. The average spatial positions of the subjects’ wrists and elbows were calculated to serve as a baseline, and the deviation of the robot’s wrists and elbows from this baseline was recorded as the criterion for evaluating anthropomorphism. The compared results are presented in Figure 12(c–d) and (g–k).

In terms of naturalness and anthropomorphism, quantitative metrics – specifically the joint discomfort comparisons in Figures 12 (a) and 12(e) and the deviations from the human elbow/wrist trajectory baselines in Figure 12 (c), (d), (g) and (k) – clearly demonstrate that the RNN solver based on joint discomfort minimization produces motion that is more natural and human-like. Furthermore, the visual comparison in Figure 13 (a–c) between the two motion generation methods and the human motion confirms that the approach incorporating joint discomfort minimization more closely resembles human movement patterns.

Comparing Figures 12 (a) and 12(e), the overall joint discomfort of the robot based on joint discomfort minimization remains consistently more than 23% lower than that of the robot based on velocity norm minimization. Moreover, during the motion process, the proposed method initially converges towards the direction of minimum overall joint discomfort before diverging, thereby effectively regulating the joint configuration and utilizing the joint motion space.

Figure 13.

Fixed point grasping based on human (a), joint discomfort minimization (b), and velocity norm minimization (c). (1)–(4) is physical experiment with time.

In summary, through simulation, experimentation, and comparative analysis, the proposed joint discomfort metric parameter effectively regulates joint configurations and motion space while maintaining precision in target tracking and dual-arm constrained collision avoidance. This approach better leverages the spatial constraints and range of the humanoid structure, enhancing the naturalness of motion during high-precision tasks executed by dual arms.