Force-Closure-Based Weighted Hybrid Force/Position Fuzzy Coordination Control for Dual-Arm Robots

Abstract

There is a strong coupling between two arms in cooperative operations of dual-arm robots. To enhance the coordination and cooperation ability of dual-arm robots, a force-closure-based weighted hybrid force/position fuzzy coordination control method is proposed. Firstly, to improve the grasping stability of dual-arm robots, the force-closure dynamic constraints are established by combining the friction cone constraints with the force and torque balance constraints. Then the optimal distribution of contact force is performed according to the minimum energy consumption principle. Secondly, to enhance the coordination of dual-arm robots, the weighted hybrid force/position control method is modified by adding the synchronization error between two arms. Then the Lyapunov method is adopted to prove the stability of the proposed coordination control method. Thirdly, the fuzzy self-tuning technique is adopted to adjust the control gains automatically. Lastly, a simulation and experiment are performed for collaborative transport. The results show that, compared with the position coordination control and the traditional hybrid force/position control, the weighted hybrid force/position fuzzy coordination control can improve control accuracy and has good cooperation ability and strong robustness. Therefore, the proposed method can effectively realize the coordination control of dual-arm robots.

1. Introduction

In contrast to a single robot manipulator [1,2,3], multiple robot manipulators [4,5,6] can handle more complex tasks efficiently. Among the multiple robot manipulators, dual-arm robots [7,8,9] can be applied to various complex operations due to their coordinated performance and human-like characteristics. Therefore, dual-arm robots have wide application prospects.

For cooperative operations of dual-arm robots, there are kinematic constraints and dynamic constraints between two arms [10]. However, traditional dynamic constraints are just to maintain the force and torque balance of the workpiece. They cannot guarantee that the workpiece will not slip or even fall off during operations. Therefore, it is necessary to add the friction cone constraints to realize the force-closure grasps [11,12,13,14] for more stable operations. Carabis et al. [15] adjusted the contact force dynamically to maintain the stability of grasp according to the friction cone constraints when the contact force would exceed the slippage threshold. Peng et al. [16] calculated the maximum normal contact force based on the maximum load and the friction cone constraints. The desired normal contact force was selected without exceeding the maximum normal force. Zhou et al. [17] converted the friction cone constraints into a positive definite Hermitian matrix and then designed a cost function accordingly. The minimum normal contact force was obtained by minimizing the cost function. Nevertheless, the above methods can only guarantee that the contact force satisfies the force-closure dynamic constraints or is minimal under this condition. They cannot guarantee the optimal distribution of joint torques. Hence, for the continuous operation requirements of dual-arm robots, such as long-term transportation tasks, the issue of energy consumption needs to be given priority consideration. The minimum energy consumption of dual arm robots can be guaranteed through the optimal distribution of joint torques. Therefore, this paper adopts the minimum energy consumption principle to distribute the contact force and joint torques for the dual-arm robot under the condition of satisfying the force-closure dynamic constraints.

To guarantee the accuracy of cooperative operations, it is necessary to achieve the coordination control of dual-arm robots. Common coordination control methods are to integrate kinematic constraints and dynamic constraints into the hybrid force/position control or the impedance control [18,19]. However, impedance control requires the establishment of an accurate impedance model [20,21]. In the case that the operation task is complex, the establishment process is cumbersome. An inaccurate impedance model will affect the real-time control performance. In contrast, hybrid force/position control requires switching between the position and force control in real time according to the actual environment. Chattering easily occurs in the switching process [22,23]. Moreover, the position accuracy can only be guaranteed in the position constraint space, and the force accuracy can only be guaranteed in the force constraint space. Therefore, for the limitations of traditional force control methods, we proposed a weighted hybrid force/position control (WHFPC) method in our previous research [24]. It summed the force and position control laws in a weighted way. The precise control of force and position in any constraint direction is realized. However, WHFPC is only applicable to a single robot manipulator. Therefore, for improving the operational ability and applicability, the constraints of dual-arm robots are introduced into WHFPC to realize the coordination control.

To further improve the coordination performance of dual-arm robots, it is essential to guarantee the synchronization between the two arms. Zhang et al. [25] added the synchronization error to the finite-time sliding mode controller to ensure the convergence of the synchronization and tracking errors. Mohammed et al. [26] designed an observer-based synchronous output feedback tracking control strategy for multi-manipulator synchronization. Duong et al. [27] designed a synchronous control strategy with the time delay estimation to handle the uncertainty of the closed-loop kinematic chain for reducing the synchronization error. Zhang et al. [28] proposed a supertwisting sliding mode synchronization control method to enhance the parallel robot coordination. However, the above synchronization control methods are only suitable for the position control but not the traditional force control. In contrast, WHFPC sums the force and position control laws in a weighted way. Therefore, the synchronization error can be introduced into its position control law to realize position control, force control, and synchronization control simultaneously. Accordingly, a weighted hybrid force/position coordination control (WHFPCC) method is designed for improving the coordination and overall performance of dual-arm robots.

To enhance the adaptability of WHFPCC, it is necessary to dynamically tune the gains of the controller in accordance with the actual operation situations. The fuzzy self-tuning technology [29,30] can tune the control gains adaptively according to the variation in errors; thus, the system has strong adaptability and better control effect. The traditional fuzzy self-tuning technology can only be applied individually to position control or force control. However, the gains of position control, force control, and synchronization control of WHFPCC affect each other. Hence, all the gains must be tuned simultaneously. On the basis of the characteristics of WHFPCC, the fuzzy self-tuning technology is improved and applied to WHFPCC. Therefore, a weighted hybrid force/position fuzzy coordination control (WHFPFCC) method is proposed to simultaneously tune the position, force, and synchronization control gains to improve the control precision of dual-arm robots.

The main contributions are listed below:

(1)

To guarantee the stable grasp of the workpiece, the force-closure dynamic constraints of dual-arm robots are established by combining the force and torque balance constraints of the workpiece with the friction cone constraints. Moreover, to reduce the energy consumption, the minimum energy consumption principle is adopted to optimize the distribution of the contact force and joint torques of dual-arm robots under the force-closure dynamic constraints.

(2)

WHFPCC is proposed to realize position control, force control, and synchronization control simultaneously. The synchronization error between two arms is introduced into WHFPC in the form of a weighted sum. The Lyapunov method is adopted to prove the stability of WHFPCC. It can effectively enhance the cooperation performance of dual-arm robots.

(3)

WHFPFCC is proposed to tune the position, force, and synchronization control gains simultaneously. According to the characteristics of WHFPCC, a fuzzy controller is developed by adopting the principles of dominant variable and input unsaturation. It can adaptively tune the control gains according to different operating conditions and effectively improve the control accuracy.

The following organization of this paper is arranged below. In Section 2, force-closure dynamic constraints are analyzed for dual-arm robot coordination. In Section 3, WHFPCC is proposed for dual-arm robots, and the proof of its stability is described. In Section 4, WHFPFCC is proposed for dual-arm robots. In Section 5, numerical simulations are carried out. In Section 6, physical experiments are carried out. In Section 7, conclusions are summarized.

2. Force-Closure Dynamic Constraints of Dual-Arm Robot Coordination

Compared with the single robot manipulator, dual-arm robot coordination is more complex. Two arms interact with each other in actual operations. There are certain constraints between each other. Hence, dual-arm robot coordination is coupled. Dual-arm robot coordination includes loose coordination and tight coordination, among which the tight coordination has stronger coupling. For tight coordination of dual-arm robots, not only the kinematic constraint [31,32] but also the dynamic constraint [19,32] should be satisfied. As shown in Figure 1, the dynamic constraint can be derived from force analysis of the workpiece.

The force and the torque balance constraints can be derived from the force and torque analysis of the workpiece, as shown in Equations (1) and (2), respectively.

The force balance constraint [19,32] in the world frame can be described below:

$${\mathit{f}}_{\mathit{l}}+{\mathit{f}}_r+{\mathit{G}}_{\mathit{t}}=m_t{\dot{\mathit{v}}}_{\mathit{t}},$$

(1)

where ${\mathit{G}}_{\mathit{t}}={\left[\begin{array}{ccc}0& 0& G_{\mathrm{z}}\end{array}\right]}^T$. ${\mathit{f}}_{\mathit{l}}$ and ${\mathit{f}}_{\mathit{r}}$ are the contact force vectors of the left-arm robot and the right-arm robot, respectively, and their dimensions are both 3. ${\mathit{G}}_{\mathit{t}}$ is the gravity vector of the workpiece, $G_{\mathrm{z}}$ is the component of ${\mathit{G}}_{\mathit{t}}$ in the z axis, and $m_t$ is the mass of the workpiece. ${\dot{\mathit{v}}}_{\mathit{t}}$ is the linear acceleration vector of the workpiece centroid in the world frame, and its dimension is 3.

The torque balance constraint [19,32] in the world frame can be described below:

$${\mathit{\tau }}_{\mathit{l}}+{\mathit{d}}_l\times {\mathit{f}}_l+{\mathit{\tau }}_{\mathit{r}}+{\mathit{d}}_{\mathit{r}}\times {\mathit{f}}_{\mathit{r}}={\mathit{I}}_{\mathit{t}}{\dot{\mathit{\omega }}}_{\mathit{t}}+{\mathit{\omega }}_{\mathit{t}}\times \left({\mathit{I}}_{\mathit{t}}{\mathit{\omega }}_{\mathit{t}}\right),$$

(2)

where ${\mathit{\tau }}_{\mathit{l}}$ is the contact torque vector between the workpiece and the left-arm robot, and its dimension is 3; ${\mathit{\tau }}_{\mathit{r}}$ is the contact torque vector between the workpiece and the right-arm robot, and its dimension is 3; ${\mathit{d}}_{\mathit{l}}$ is the distance vector between the workpiece centroid and the force action point of the left-arm robot, and its dimension is 3; ${\mathit{d}}_{\mathit{r}}$ is the distance vector between the workpiece centroid and the force action point of the right-arm robot, and its dimension is 3; ${\mathit{I}}_{\mathit{t}}$ is the inertia tensor matrix, and its dimension is 3 × 3; ${\mathit{\omega }}_{\mathit{t}}$ is the angular velocity vector of the workpiece centroid in the world frame, and its dimension is 3; and ${\dot{\mathit{\omega }}}_{\mathit{t}}$ is the angular acceleration vector of the workpiece centroid in the world frame, and its dimension is 3.

The vector cross product can be converted into the dot product between a matrix and a vector by Equation (3). Therefore, the force balance constraint and the torque balance constraint can be combined into the generalized force balance constraint by Equation (3), as described in Equation (4).

$$\left\{\begin{array}{c}{\mathit{d}}_l\times {\mathit{f}}_l={\mathit{S}}_l·{\mathit{f}}_{\mathit{l}}\\ {\mathit{d}}_{\mathit{r}}\times {\mathit{f}}_r={\mathit{S}}_{\mathit{r}}·{\mathit{f}}_{\mathit{r}}\end{array}\right.,$$

(3)

where ${\mathit{S}}_{\mathit{l}}=\left[\begin{array}{ccc}0& -d_{zl}& d_{yl}\\ d_{zl}& 0& -d_{xl}\\ -d_{yl}& d_{xl}& 0\end{array}\right]$; ${\mathit{S}}_r=\left[\begin{array}{ccc}0& -d_{zr}& d_{yr}\\ d_{zr}& 0& -d_{xr}\\ -d_{yr}& d_{xr}& 0\end{array}\right]$; ${\mathit{S}}_l$ and ${\mathit{S}}_r$ are the geometric parametric matrices related to ${\mathit{d}}_l$ and ${\mathit{d}}_{\mathit{r}}$, respectively; $d_{xl},d_{yl},d_{zl}$ are the components of ${\mathit{d}}_l$ in the x, y, and z directions, respectively; and $d_{xr},d_{yr},d_{zr}$ are the components of ${\mathit{d}}_r$ in the x, y, and z directions, respectively.

$${\mathit{N}}_{\mathit{t}}=\mathit{W}{\mathit{F}}_{\mathit{s}\mathit{w}}+{\mathit{G}}_{\mathit{o}},$$

(4)

where ${\mathit{N}}_{\mathit{t}}=\left[\begin{array}{c}{m}_t{\dot{\mathit{v}}}_{\mathit{t}}\\ {\mathit{I}}_{\mathit{t}}{\dot{\mathit{\omega }}}_{\mathit{t}}+{\mathit{\omega }}_{\mathit{t}}\times \left({\mathit{I}}_{\mathit{t}}{\mathit{\omega }}_{\mathit{t}}\right)\end{array}\right]$, $\mathit{W}=\left[\begin{array}{cc}\begin{array}{cc}{\mathit{I}}_{\mathbf{3}\times \mathbf{3}}& {\mathit{O}}_{\mathbf{3}\times \mathbf{3}}\end{array}& \begin{array}{cc}{\mathit{I}}_{\mathbf{3}\times \mathbf{3}}& {\mathit{O}}_{\mathbf{3}\times \mathbf{3}}\end{array}\\ \begin{array}{cc}{\mathit{S}}_{\mathit{l}}& {\mathit{I}}_{\mathbf{3}\times \mathbf{3}}\end{array}& \begin{array}{cc}{\mathit{S}}_{\mathit{r}}& {\mathit{I}}_{\mathbf{3}\times \mathbf{3}}\end{array}\end{array}\right]$, ${\mathit{F}}_{\mathit{s}\mathit{w}}=\left[\begin{array}{c}{\mathit{F}}_{\mathit{l}\mathit{w}}\\ {\mathit{F}}_{\mathit{r}\mathit{w}}\end{array}\right]$, ${\mathit{F}}_{\mathit{l}\mathit{w}}=\left[\begin{array}{c}{\mathit{f}}_{\mathit{l}}\\ {\mathit{\tau }}_{\mathit{l}}\end{array}\right]$, ${\mathit{F}}_{\mathit{r}\mathit{w}}=\left[\begin{array}{c}{\mathit{f}}_{\mathit{r}}\\ {\mathit{\tau }}_{\mathit{r}}\end{array}\right]$, and ${\mathit{G}}_{\mathit{o}}=\left[\begin{array}{c}{\mathit{G}}_{\mathit{t}}\\ {\mathit{O}}_{3\times 1}\end{array}\right]$.

To guarantee that the workpiece would not slip or even fall off during operations, the generalized contact force between the workpiece and the dual-arm robot should meet not only the above constraints but also the friction cone constraint for force-closure grasps. In this paper, the end effector of the dual-arm robot is a two-finger gripper, and the contact mode between the workpiece and this end effector is hard-finger contact (namely, point contact with friction) [13]. Therefore, the point contact with friction is taken as the contact mode to analyze the friction cone constraint.

The generalized contact force vector of the dual-arm robot is described in Equation (5). ${\mathit{F}}_{\mathit{l}\mathit{n}}$, ${\mathit{F}}_{\mathit{r}\mathit{n}}$ can be decomposed into the tangential force $f_{lnx}$, $f_{lny}$ and $f_{rnx}$, $f_{rny}$ along the workpiece surface direction and into the normal force $f_{lnz}$, $f_{lnz}$, whose directions are perpendicular to the workpiece surface. Then the friction cone constraint of force-closure is described in Equation (6).

$$\left\{\begin{array}{c}{\mathit{F}}_{\mathit{l}\mathit{n}}={\left[\begin{array}{cc}\begin{array}{ccc}{f}_{lnx}& f_{lny}& f_{lnz}\end{array}& \begin{array}{ccc}0& 0& 0\end{array}\end{array}\right]}^T\\ {\mathit{F}}_{\mathit{r}\mathit{n}}={\left[\begin{array}{cc}\begin{array}{ccc}{f}_{rnx}& f_{rny}& f_{rnz}\end{array}& \begin{array}{ccc}0& 0& 0\end{array}\end{array}\right]}^T\end{array}\right.,$$

(5)

where ${\mathit{F}}_{\mathit{l}\mathit{n}}$ is the generalized contact force vector of the left-arm robot in its contact frame; ${\mathit{F}}_{\mathit{r}\mathit{n}}$ is the generalized contact force vector of the right-arm robot in its contact frame; $f_{lnx}$, $f_{lny}$, and $f_{lnz}$ are the components of ${\mathit{F}}_{\mathit{l}\mathit{n}}$ in the x, y, and z directions of the contact frame of the left-arm robot, respectively; and $f_{rnx}$, $f_{rny}$, and $f_{lnz}$ are the components of ${\mathit{F}}_{\mathit{r}\mathit{n}}$ in the x, y, and z directions of the contact frame of the right-arm robot, respectively.

$$\left\{\begin{array}{c}\sqrt{f_{lnx}^2+f_{lny}^2}\le \mu f_{lnz}\\ \sqrt{f_{rnx}^2+f_{rny}^2}\le \mu f_{rnz}\end{array}\right., \left\{\begin{array}{c}{f}_{lnz}\ge 0\\ f_{rnz}\ge 0\end{array}\right.,$$

(6)

where $\mu$ is the frictional coefficient of the workpiece surface.

The generalized force balance constraint (4) is established in the world frame, but the friction cone constraint (6) is established in the contact frame. They can be translated to each other by Equation (7).

$$\left\{\begin{array}{c}{\mathit{F}}_{\mathit{l}\mathit{w}}={\mathit{R}}_{\mathit{o}\mathit{l}}{\mathit{F}}_{\mathit{l}\mathit{n}}\\ {\mathit{F}}_{\mathit{r}\mathit{w}}={\mathit{R}}_{\mathit{o}\mathit{r}}{\mathit{F}}_{\mathit{r}\mathit{n}}\end{array}\right.,$$

(7)

where ${\mathit{F}}_{\mathit{l}\mathit{w}}=\left[\begin{array}{c}{\mathit{f}}_l\\ {\mathit{O}}_{3\times 1}\end{array}\right]={\left[\begin{array}{cccccc}{f}_{lwx}& f_{lwy}& f_{lwz}& 0& 0& 0\end{array}\right]}^T$, ${\mathit{R}}_{\mathit{o}\mathit{l}}=\left[\begin{array}{cc}{\mathit{R}}_{\mathit{o}\mathit{l}\mathit{w}}^{\mathit{o}\mathit{l}\mathit{n}}& {\mathit{O}}_{\mathbf{3}\times \mathbf{3}}\\ {\mathit{O}}_{\mathbf{3}\times \mathbf{3}}& {\mathit{R}}_{\mathit{o}\mathit{l}\mathit{w}}^{\mathit{o}\mathit{l}\mathit{n}}\end{array}\right]$, ${\mathit{F}}_{\mathit{r}\mathit{w}}=\left[\begin{array}{c}{\mathit{f}}_r\\ {\mathit{O}}_{3\times 1}\end{array}\right]={\left[\begin{array}{cccccc}{f}_{rwx}& f_{rwy}& f_{rwz}& 0& 0& 0\end{array}\right]}^T$, and ${\mathit{R}}_{\mathit{o}\mathit{r}}=\left[\begin{array}{cc}{\mathit{R}}_{\mathit{o}\mathit{r}\mathit{w}}^{\mathit{o}\mathit{r}\mathit{n}}& {\mathit{O}}_{\mathbf{3}\times \mathbf{3}}\\ {\mathit{O}}_{\mathbf{3}\times \mathbf{3}}& {\mathit{R}}_{\mathit{o}\mathit{r}\mathit{w}}^{\mathit{o}\mathit{r}\mathit{n}}\end{array}\right]$. ${\mathit{F}}_{\mathit{l}\mathit{w}}$ is the generalized contact force vector of the left-arm robot in the world frame; ${\mathit{F}}_{\mathit{r}\mathit{w}}$ is the generalized contact force vector of the right-arm robot in the world frame; $f_{lwx}$, $f_{lwy}$, and $f_{lwz}$ are the components of ${\mathit{F}}_{\mathit{r}\mathit{w}}$ in the x, y, and z directions of the world frame, respectively; $f_{rwx}$, $f_{rwy}$, and $f_{lwz}$ are the components of ${\mathit{F}}_{\mathit{r}\mathit{w}}$ in the x, y, and z directions of the world frame, respectively; ${\mathit{R}}_{\mathit{o}\mathit{l}}$ is the coordinate transformation matrix between ${\mathit{F}}_{\mathit{l}\mathit{w}}$ and ${\mathit{F}}_{\mathit{l}\mathit{n}}$; ${\mathit{R}}_{\mathit{o}\mathit{r}}$ is the coordinate transformation matrix between ${\mathit{F}}_{\mathit{r}\mathit{w}}$ and ${\mathit{F}}_{\mathit{r}\mathit{n}}$; ${\mathit{R}}_{\mathit{o}\mathit{l}\mathit{w}}^{\mathit{o}\mathit{l}\mathit{n}}$ is the transformation matrix between the world frame and the contact frame of the left-arm robot, and its dimension is 3 × 3; and ${\mathit{R}}_{\mathit{o}\mathit{r}\mathit{w}}^{\mathit{o}\mathit{r}\mathit{n}}$ is the transformation matrix between the world frame and the contact frame of the right-arm robot, and its dimension is 3 × 3.

The contact mode of the dual-arm robot is the point contact with friction. Hence, there is no contact torque. Equation (4) can be rewritten as Equation (8). Then, Equations (5)–(8) constitute the force-closure dynamic constraint of dual-arm robot coordination.

$${\mathit{N}}_{\mathit{t}}={\mathit{W}}^{\prime }{\mathit{F}}_{\mathit{s}\mathit{w}}+{\mathit{G}}_{\mathit{o}},$$

(8)

where ${\mathit{W}}^{\prime }=\left[\begin{array}{cc}\begin{array}{cc}{\mathit{I}}_{\mathbf{3}\times \mathbf{3}}& {\mathit{O}}_{\mathbf{3}\times \mathbf{3}}\end{array}& \begin{array}{cc}{\mathit{I}}_{\mathbf{3}\times \mathbf{3}}& {\mathit{O}}_{\mathbf{3}\times \mathbf{3}}\end{array}\\ \begin{array}{cc}{\mathit{S}}_{\mathit{l}}& {\mathit{O}}_{\mathbf{3}\times \mathbf{3}}\end{array}& \begin{array}{cc}{\mathit{S}}_{\mathit{r}}& {\mathit{O}}_{\mathbf{3}\times \mathbf{3}}\end{array}\end{array}\right]$.

For the convenience of the subsequent solution, Equations (5)–(7) are uniformly rewritten in the form whose independent variable is ${\mathit{F}}_{\mathit{s}\mathit{w}}$, as described in Equations (9)–(12). Equation (9) corresponds to Equation (5). This means that the generalized contact force ${\mathit{F}}_{\mathit{s}\mathit{w}}$ does not include the contact torque in the case that the contact mode of the dual-arm robot is the point contact with friction. Equations (10)–(12) corresponds to Equations (6) and (7). For the convenience of subsequent calculation, both sides of Equation (6) are squared simultaneously. Due to the normal contact force $f_{inz}\ge 0$, the squared operation would not change the constraint.

$${\mathit{A}}_{\mathit{\tau }}{\mathit{R}}_{\mathit{o}\mathit{s}}^{-\mathbf{1}}{\mathit{F}}_{\mathit{s}\mathit{w}}={\mathit{O}}_{\mathbf{12}\times \mathbf{1}},$$

(9)

where ${\mathit{R}}_{\mathit{o}\mathit{s}}=\left[\begin{array}{cc}{\mathit{R}}_{\mathit{o}\mathit{l}}& {\mathit{O}}_{\mathbf{6}\times \mathbf{6}}\\ {\mathit{O}}_{\mathbf{6}\times \mathbf{6}}& {\mathit{R}}_{\mathit{o}\mathit{r}}\end{array}\right]$, and ${\mathit{A}}_{\mathit{\tau }}=diag\left(0,0,0,1,1,1,0,0,0,1,1,1\right)$, that is, ${\mathit{A}}_{\mathit{\tau }}$ is a diagonal matrix with $\left(0,0,0,1,1,1,0,0,0,1,1,1\right)$ as the main diagonal elements.

$${\left({\mathit{A}}_{\mathit{l}\mathbf{1}}{\mathit{R}}_{\mathit{o}\mathit{s}}^{-\mathbf{1}}{\mathit{F}}_{\mathit{s}\mathit{w}}\right)}^T\left({\mathit{A}}_{\mathit{l}\mathbf{1}}{\mathit{R}}_{\mathit{o}\mathit{s}}^{-\mathbf{1}}{\mathit{F}}_{\mathit{s}\mathit{w}}\right)+{\left({\mathit{A}}_{\mathit{l}\mathbf{2}}{\mathit{R}}_{\mathit{o}\mathit{s}}^{-\mathbf{1}}{\mathit{F}}_{\mathit{s}\mathit{w}}\right)}^T\left({\mathit{A}}_{\mathit{l}\mathbf{2}}{\mathit{R}}_{\mathit{o}\mathit{s}}^{-\mathbf{1}}{\mathit{F}}_{\mathit{s}\mathit{w}}\right)\le {\mu }^2{\left({\mathit{A}}_{\mathit{l}\mathbf{3}}{\mathit{R}}_{\mathit{o}\mathit{s}}^{-\mathbf{1}}{\mathit{F}}_{\mathit{s}\mathit{w}}\right)}^T\left({\mathit{A}}_{\mathit{l}\mathbf{3}}{\mathit{R}}_{\mathit{o}\mathit{s}}^{-\mathbf{1}}{\mathit{F}}_{\mathit{s}\mathit{w}}\right),$$

(10)

$${\left({\mathit{A}}_{\mathit{r}\mathbf{1}}{\mathit{R}}_{\mathit{o}\mathit{s}}^{-\mathbf{1}}{\mathit{F}}_{\mathit{s}\mathit{w}}\right)}^T\left({\mathit{A}}_{\mathit{r}\mathbf{1}}{\mathit{R}}_{\mathit{o}\mathit{s}}^{-\mathbf{1}}{\mathit{F}}_{\mathit{s}\mathit{w}}\right)+{\left({\mathit{A}}_{\mathit{r}\mathbf{2}}{\mathit{R}}_{\mathit{o}\mathit{s}}^{-\mathbf{1}}{\mathit{F}}_{\mathit{s}\mathit{w}}\right)}^T\left({\mathit{A}}_{\mathit{r}\mathbf{2}}{\mathit{R}}_{\mathit{o}\mathit{s}}^{-\mathbf{1}}{\mathit{F}}_{\mathit{s}\mathit{w}}\right)\le {\mu }^2{\left({\mathit{A}}_{\mathit{r}\mathbf{3}}{\mathit{R}}_{\mathit{o}\mathit{s}}^{-\mathbf{1}}{\mathit{F}}_{\mathit{s}\mathit{w}}\right)}^T\left({\mathit{A}}_{\mathit{r}\mathbf{3}}{\mathit{R}}_{\mathit{o}\mathit{s}}^{-\mathbf{1}}{\mathit{F}}_{\mathit{s}\mathit{w}}\right),$$

(11)

$${\mathit{A}}_{\mathit{l}\mathbf{3}}{\mathit{R}}_{\mathit{o}\mathit{s}}^{-\mathbf{1}}{\mathit{F}}_{\mathit{s}\mathit{w}}\ge {\mathit{O}}_{\mathbf{1}\times \mathbf{1}}, {\mathit{A}}_{\mathit{r}\mathbf{3}}{\mathit{R}}_{\mathit{o}\mathit{s}}^{-\mathbf{1}}{\mathit{F}}_{\mathit{s}\mathit{w}}\ge {\mathit{O}}_{\mathbf{1}\times \mathbf{1}},$$

(12)

where ${\mathit{A}}_{\mathit{l}\mathit{i}}(i=1\sim 3)$ is a 1 × 12 matrix. Its ith element is 1, and its other elements are 0. ${\mathit{A}}_{\mathit{r}\mathit{i}}(i=1\sim 3)$ is a 1 × 12 matrix. Its (i + 6)th element is 1, and its other elements are 0.

To minimize the energy consumption of coordination operations, the minimization of joint driving torque is regarded as the optimization objective, and then the generalized contact force of the dual-arm robot is distributed according to this optimization objective. The objective function is designed to get the minimum value of the square sum of the driving torque for each joint, and it can be converted into a function whose independent variable is the generalized contact force ${\mathit{F}}_{\mathit{s}\mathit{w}}$ according to the dynamic model of the dual-arm robot. The dynamic model is described in Equation (13). Therefore, the generalized contact force distribution can be described as a constrained optimization problem with the objective function and the constraints (8)–(12), as described in Equation (14).

$${\mathit{T}}_{\mathit{s}}={\mathit{M}}_{\mathit{s}}\left({\mathit{q}}_{\mathit{s}}\right){\ddot{\mathit{q}}}_{\mathit{s}}+{\mathit{C}}_{\mathit{s}}\left({\mathit{q}}_{\mathit{s}},{\dot{\mathit{q}}}_{\mathit{s}}\right){\dot{\mathit{q}}}_{\mathit{s}}+{\mathit{G}}_{\mathit{s}}\left({\mathit{q}}_{\mathit{s}}\right)+{\mathit{N}}_{\mathit{s}}\left({\dot{\mathit{q}}}_{\mathit{s}}\right)+{\mathit{D}}_{\mathit{s}}+{\mathit{J}}_{\mathit{s}}^{\mathit{T}}{\mathit{F}}_{\mathit{s}\mathit{w}},$$

(13)

where ${\mathit{N}}_{\mathit{s}}\left({\dot{\mathit{q}}}_{\mathit{s}}\right)=diag\left(sgn\left({\dot{\mathit{q}}}_{\mathit{s}}\right)\right){\mathit{F}}_{\mathit{c}\mathit{s}}+{\mathit{\beta }}_{\mathit{s}}{\dot{\mathit{q}}}_{\mathit{s}}$, ${\mathit{T}}_{\mathit{s}}=\left[\begin{array}{c}{\mathit{T}}_{\mathit{l}}\\ {\mathit{T}}_{\mathit{r}}\end{array}\right]$, ${\mathit{M}}_{\mathit{s}}\left({\mathit{q}}_{\mathit{s}}\right)=\left[\begin{array}{cc}{\mathit{M}}_{\mathit{l}}\left({\mathit{q}}_{\mathit{l}}\right)& {\mathit{O}}_{\mathit{n}\times \mathit{n}}\\ {\mathit{O}}_{\mathit{n}\times \mathit{n}}& {\mathit{M}}_{\mathit{r}}\left({\mathit{q}}_{\mathit{r}}\right)\end{array}\right]$, ${\ddot{\mathit{q}}}_{\mathit{s}}=\left[\begin{array}{c}{\ddot{\mathit{q}}}_{\mathit{l}}\\ {\ddot{\mathit{q}}}_{\mathit{r}}\end{array}\right]$, ${\mathit{C}}_{\mathit{s}}\left({\mathit{q}}_{\mathit{s}},{\dot{\mathit{q}}}_{\mathit{s}}\right)=\left[\begin{array}{cc}{\mathit{C}}_{\mathit{l}}\left({\mathit{q}}_{\mathit{l}},{\dot{\mathit{q}}}_{\mathit{l}}\right)& {\mathit{O}}_{\mathit{n}\times \mathit{n}}\\ {\mathit{O}}_{\mathit{n}\times \mathit{n}}& {\mathit{C}}_{\mathit{r}}\left({\mathit{q}}_{\mathit{r}},{\dot{\mathit{q}}}_{\mathit{r}}\right)\end{array}\right]$, ${\dot{\mathit{q}}}_{\mathit{s}}=\left[\begin{array}{c}{\dot{\mathit{q}}}_{\mathit{l}}\\ {\dot{\mathit{q}}}_{\mathit{r}}\end{array}\right]$, ${\mathit{G}}_{\mathit{s}}\left({\mathit{q}}_{\mathit{s}}\right)=\left[\begin{array}{c}{\mathit{G}}_{\mathit{l}}\left({\mathit{q}}_{\mathit{l}}\right)\\ {\mathit{G}}_{\mathit{r}}\left({\mathit{q}}_{\mathit{r}}\right)\end{array}\right]$, ${\mathit{q}}_{\mathit{s}}=\left[\begin{array}{c}{\mathit{q}}_{\mathit{l}}\\ {\mathit{q}}_{\mathit{r}}\end{array}\right]$, ${\mathit{N}}_{\mathit{s}}\left({\dot{\mathit{q}}}_{\mathit{s}}\right)=\left[\begin{array}{c}{\mathit{N}}_{\mathit{l}}\left({\dot{\mathit{q}}}_{\mathit{l}}\right)\\ {\mathit{N}}_{\mathit{r}}\left({\dot{\mathit{q}}}_{\mathit{r}}\right)\end{array}\right]$, ${\mathit{F}}_{\mathit{c}\mathit{s}}=\left[\begin{array}{c}{\mathit{F}}_{\mathit{c}\mathit{l}}\\ {\mathit{F}}_{\mathit{c}\mathit{r}}\end{array}\right]$, ${\mathit{\beta }}_{\mathit{s}}=\left[\begin{array}{cc}{\mathit{\beta }}_{\mathit{l}}& {\mathit{O}}_{\mathit{n}\times \mathit{n}}\\ {\mathit{O}}_{\mathit{n}\times \mathit{n}}& {\mathit{\beta }}_{\mathit{r}}\end{array}\right]$, ${\mathit{D}}_{\mathit{s}}=\left[\begin{array}{c}{\mathit{D}}_{\mathit{l}}\\ {\mathit{D}}_{\mathit{r}}\end{array}\right]$, and ${\mathit{J}}_{\mathit{s}}=\left[\begin{array}{cc}{\mathit{J}}_{\mathit{l}}& {\mathit{O}}_{\mathbf{6}\times \mathit{n}}\\ {\mathit{O}}_{\mathbf{6}\times \mathit{n}}& {\mathit{J}}_{\mathit{r}}\end{array}\right]$. The three elements in each vector/matrix set below correspond to left-arm robot, right-arm robot, and dual-arm robot, respectively. ${\mathit{T}}_l$, ${\mathit{T}}_r$, and ${\mathit{T}}_s$ are the joint torque vectors, and their dimensions are n, n, and 2n, respectively; ${\ddot{\mathit{q}}}_l$, ${\ddot{\mathit{q}}}_r$, and ${\ddot{\mathit{q}}}_s$ are the joint angular acceleration vectors, and their dimensions are n, n, and 2n, respectively; ${\dot{\mathit{q}}}_l$, ${\dot{\mathit{q}}}_r$, and ${\dot{\mathit{q}}}_s$ are the joint angular velocity vectors, and their dimensions are n, n, and 2n, respectively; ${\mathit{M}}_l\left({\mathit{q}}_l\right)$, ${\mathit{M}}_r\left({\mathit{q}}_r\right)$, and ${\mathit{M}}_s\left({\mathit{q}}_s\right)$ are the inertial matrixes, and their dimensions are n × n, n × n, and 2n × 2n, respectively; ${\mathit{C}}_l\left({\mathit{q}}_l,{\dot{\mathit{q}}}_l\right)$, ${\mathit{C}}_r\left({\mathit{q}}_r,{\dot{\mathit{q}}}_r\right)$, and ${\mathit{C}}_s\left({\mathit{q}}_s,{\dot{\mathit{q}}}_s\right)$ are the centrifugal force and Coriolis force matrixes, and their dimensions are n × n, n × n, and 2n × 2n, respectively; ${\mathit{G}}_l\left({\mathit{q}}_l\right)$, ${\mathit{G}}_r\left({\mathit{q}}_r\right)$, and ${\mathit{G}}_s\left({\mathit{q}}_s\right)$ are the gravity matrixes, and their dimensions are n × 1, n × 1, and 2n × 1, respectively; ${\mathit{N}}_l\left({\dot{\mathit{q}}}_l\right)$, ${\mathit{N}}_r\left({\dot{\mathit{q}}}_r\right)$, and ${\mathit{N}}_s\left({\dot{\mathit{q}}}_s\right)$ are the joint friction vectors, and their dimensions are n, n, and 2n, respectively; ${\mathit{F}}_{\mathit{c}\mathit{l}}$, ${\mathit{F}}_{\mathit{c}\mathit{r}}$, and ${\mathit{F}}_{\mathit{c}\mathit{s}}$ are the Coulomb friction vectors, and their dimensions are n, n, and 2n, respectively; $diag\left(sgn\left({\dot{\mathit{q}}}_{\mathit{s}}\right)\right)$ is the diagonal matrix with $sgn\left({\dot{\mathit{q}}}_{\mathit{s}}\right)$ as the main diagonal elements, and its dimension is 2n × 2n; ${\mathit{\beta }}_{\mathit{l}}$, ${\mathit{\beta }}_r$, and ${\mathit{\beta }}_s$ are the viscous friction coefficient matrixes, and their dimensions are n × n, n × n, and 2n × 2n, respectively; ${\mathit{D}}_l$, ${\mathit{D}}_r$, and ${\mathit{D}}_s$ are the external disturbance vectors, and their dimensions are n, n, and 2n, respectively; ${\mathit{J}}_l$, ${\mathit{J}}_r$, and ${\mathit{J}}_s$ are the Jacobian matrixes, and their dimensions are 6 × n, 6 × n, and 12 × 2n, respectively; and $n$ is the number of degrees of freedom of the left-arm robot or the right-arm robot.

$$\begin{array}{c}\min f\left({\mathit{F}}_{\mathit{s}\mathit{w}}\right)={\mathit{T}}_{\mathit{s}}^{\mathit{T}}{\mathit{B}}_{\mathit{w}}{\mathit{T}}_{\mathit{s}}={\left({\mathit{H}}_{\mathit{s}}+{\mathit{J}}_{\mathit{s}}^{\mathit{T}}{\mathit{F}}_{\mathit{s}\mathit{w}}\right)}^T{\mathit{B}}_{\mathit{w}}\left({\mathit{H}}_{\mathit{s}}+{\mathit{J}}_{\mathit{s}}^{\mathit{T}}{\mathit{F}}_{\mathit{s}\mathit{w}}\right)\\ \mathrm{subject} \mathrm{to} \left\{\begin{array}{c}{h}_i\left({\mathit{F}}_{\mathit{s}\mathit{w}}\right) i=1~2\\ g_j\left({\mathit{F}}_{\mathit{s}\mathit{w}}\right) j=1~10\end{array}\right.\end{array},$$

(14)

where ${\mathit{H}}_{\mathit{s}}={\mathit{M}}_{\mathit{s}}\left({\mathit{q}}_{\mathit{s}}\right){\ddot{\mathit{q}}}_{\mathit{s}}+{\mathit{C}}_{\mathit{s}}\left({\mathit{q}}_{\mathit{s}},{\dot{\mathit{q}}}_{\mathit{s}}\right){\dot{\mathit{q}}}_{\mathit{s}}+{\mathit{G}}_{\mathit{s}}\left({\mathit{q}}_{\mathit{s}}\right)+{\mathit{N}}_{\mathit{s}}\left({\dot{\mathit{q}}}_{\mathit{s}}\right)+{\mathit{D}}_{\mathit{s}}$; ${\mathit{B}}_{\mathit{w}}$ is the weighting coefficient matrix, and its dimension is n × n; $h_i\left({\mathit{F}}_{\mathit{s}\mathit{w}}\right)$ is the equality constraint, corresponding to Equations (8) and (9); and $g_j\left({\mathit{F}}_{\mathit{s}\mathit{w}}\right)$ is the inequality constraint, corresponding to Equations (10)–(12).

There are many mature numerical solutions for nonlinear constrained optimization problems in mathematics. The interior-exterior point method [33,34,35] can convert a constrained optimization problem into an unconstrained one, and it can effectively reduce the difficulty of the solution. Therefore, this method is adopted to address the aforementioned problem to obtain the optimal generalized contact force.

3. Weighted Hybrid Force/Position Coordination Controller for Dual-Arm Robots

3.1. WHFPCC

The goal of dual-arm robot coordination is to realize the stable and precise control of the workpiece. Firstly, to meet the requirements of operations, the desired trajectory of the workpiece should be planned. Secondly, the desired state of the dual-arm robot should be calculated by the kinematics and dynamic constraints. Finally, WHFPCC should be adopted to realize position control, force control, and synchronization control simultaneously. The corresponding process is illustrated in Figure 2.

WHFPC is applied to the position and force control of the dual-arm robot. Its controller is described below:

$$\begin{array}{ll}{\mathit{u}}_{\mathit{s}}& ={\mathit{M}}_{\mathit{s}}\left({\mathit{X}}_{\mathit{s}}\right)\left[{\ddot{\mathit{X}}}_{\mathit{d}\mathit{s}}+{\mathit{k}}_{\mathit{d}\mathit{s}}\left({\dot{\mathit{X}}}_{\mathit{d}\mathit{s}}-{\dot{\mathit{X}}}_{\mathit{s}}\right)+{\mathit{k}}_{\mathit{p}\mathit{s}}\left({\mathit{X}}_{\mathit{d}\mathit{s}}-{\mathit{X}}_{\mathit{s}}\right)+{\mathit{k}}_{\mathit{f}\mathit{s}}\left({\mathit{F}}_{\mathit{d}\mathit{s}\mathit{w}}-{\mathit{F}}_{\mathit{s}\mathit{w}}\right)\right]\\ & +{\mathit{C}}_{\mathit{s}}\left({\mathit{X}}_{\mathit{s}},{\dot{\mathit{X}}}_{\mathit{s}}\right){\dot{\mathit{X}}}_{\mathit{s}}+{\mathit{G}}_{\mathit{s}}\left({\mathit{X}}_{\mathit{s}}\right)+{\mathit{N}}_{\mathit{s}}\left({\dot{\mathit{X}}}_{\mathit{s}}\right)+{\mathit{D}}_{\mathit{s}}+{\mathit{J}}_{\mathit{s}}^{\mathit{T}}{\mathit{F}}_{\mathit{s}\mathit{w}}\end{array},$$

(15)

where ${\mathit{u}}_{\mathit{s}}=\left[\begin{array}{c}{\mathit{u}}_{\mathit{l}}\\ {\mathit{u}}_{\mathit{r}}\end{array}\right]=\left[\begin{array}{c}{\left(u_1\cdots u_j\cdots u_n\right)}^T\\ {\left(u_{n+1}\cdots u_{n+j}\cdots u_{\mathbf{2}n}\right)}^T\end{array}\right]$$\left(j=1\sim n\right)$, ${\ddot{\mathit{X}}}_{\mathit{d}\mathit{s}}=\left[\begin{array}{c}{\ddot{\mathit{X}}}_{\mathit{d}\mathit{l}}\\ {\ddot{\mathit{X}}}_{\mathit{d}\mathit{r}}\end{array}\right]$; ${\dot{\mathit{X}}}_{\mathit{d}\mathit{s}}=\left[\begin{array}{c}{\dot{\mathit{X}}}_{\mathit{d}\mathit{l}}\\ {\dot{\mathit{X}}}_{\mathit{d}\mathit{r}}\end{array}\right]$, ${\dot{\mathit{X}}}_{\mathit{s}}=\left[\begin{array}{c}{\dot{\mathit{X}}}_{\mathit{l}}\\ {\dot{\mathit{X}}}_{\mathit{r}}\end{array}\right]$, ${\mathit{X}}_{\mathit{d}\mathit{s}}=\left[\begin{array}{c}{\mathit{X}}_{\mathit{d}\mathit{l}}\\ {\mathit{X}}_{\mathit{d}\mathit{r}}\end{array}\right]$, ${\mathit{X}}_{\mathit{s}}=\left[\begin{array}{c}{\mathit{X}}_{\mathit{l}}\\ {\mathit{X}}_{\mathit{r}}\end{array}\right]$, ${\mathit{F}}_{\mathit{d}\mathit{s}\mathit{w}}=\left[\begin{array}{c}{\mathit{F}}_{\mathit{d}\mathit{l}\mathit{w}}\\ {\mathit{F}}_{\mathit{d}\mathit{r}\mathit{w}}\end{array}\right]$, ${\mathit{k}}_{\mathit{d}\mathit{s}}=\left[\begin{array}{cc}{\mathit{k}}_{\mathit{d}\mathit{l}}& {\mathit{O}}_{\mathbf{6}\times \mathbf{6}}\\ {\mathit{O}}_{\mathbf{6}\times \mathbf{6}}& {\mathit{k}}_{\mathit{d}\mathit{r}}\end{array}\right]$, ${\mathit{k}}_{\mathit{p}\mathit{s}}=\left[\begin{array}{cc}{\mathit{k}}_{\mathit{p}\mathit{l}}& {\mathit{O}}_{\mathbf{6}\times \mathbf{6}}\\ {\mathit{O}}_{\mathbf{6}\times \mathbf{6}}& {\mathit{k}}_{\mathit{p}\mathit{r}}\end{array}\right]$, ${\mathit{k}}_{\mathit{f}\mathit{s}}=\left[\begin{array}{cc}{\mathit{k}}_{\mathit{f}\mathit{l}}& {\mathit{O}}_{\mathbf{6}\times \mathbf{6}}\\ {\mathit{O}}_{\mathbf{6}\times \mathbf{6}}& {\mathit{k}}_{\mathit{f}\mathit{r}}\end{array}\right]$, ${\mathit{M}}_{\mathit{s}}\left({\mathit{X}}_{\mathit{s}}\right)=\left[\begin{array}{cc}{\mathit{M}}_{\mathit{l}}\left({\mathit{X}}_{\mathit{l}}\right)& {\mathit{O}}_{n\times \mathbf{6}}\\ {\mathit{O}}_{n\times \mathbf{6}}& {\mathit{M}}_{\mathit{r}}\left({\mathit{X}}_{\mathit{r}}\right)\end{array}\right]$, ${\mathit{M}}_{\mathit{l}}\left({\mathit{X}}_{\mathit{l}}\right)={\mathit{M}}_{\mathit{l}}\left({\mathit{q}}_{\mathit{l}}\right){\mathit{J}}_{\mathit{l}}^{+}$, ${\mathit{M}}_{\mathit{r}}\left({\mathit{X}}_{\mathit{r}}\right)={\mathit{M}}_{\mathit{r}}\left({\mathit{q}}_{\mathit{r}}\right){\mathit{J}}_{\mathit{r}}^{+}$, ${\mathit{C}}_{\mathit{s}}\left({\mathit{X}}_{\mathit{s}},{\dot{\mathit{X}}}_{\mathit{s}}\right)=\left[\begin{array}{cc}{\mathit{C}}_{\mathit{l}}\left({\mathit{X}}_{\mathit{l}},{\dot{\mathit{X}}}_{\mathit{l}}\right)& {\mathit{O}}_{n\times \mathbf{6}}\\ {\mathit{O}}_{n\times \mathbf{6}}& {\mathit{C}}_{\mathit{r}}\left({\mathit{X}}_{\mathit{r}},{\dot{\mathit{X}}}_{\mathit{r}}\right)\end{array}\right]$, ${\mathit{C}}_{\mathit{l}}\left({\mathit{X}}_{\mathit{l}},{\dot{\mathit{X}}}_{\mathit{l}}\right)={\mathit{C}}_{\mathit{l}}\left({\mathit{q}}_{\mathit{l}},{\dot{\mathit{q}}}_{\mathit{l}}\right){\mathit{J}}_{\mathit{l}}^{+}-{\mathit{M}}_{\mathit{l}}\left({\mathit{q}}_{\mathit{l}}\right){\mathit{J}}_{\mathit{l}}^{+}{\dot{\mathit{J}}}_{\mathit{l}}{\mathit{J}}_{\mathit{l}}^{+}$, ${\mathit{C}}_{\mathit{r}}\left({\mathit{X}}_{\mathit{r}},{\dot{\mathit{X}}}_{\mathit{r}}\right)={\mathit{C}}_{\mathit{r}}\left({\mathit{q}}_{\mathit{r}},{\dot{\mathit{q}}}_{\mathit{r}}\right){\mathit{J}}_{\mathit{r}}^{+}-{\mathit{M}}_{\mathit{r}}\left({\mathit{q}}_{\mathit{r}}\right){\mathit{J}}_{\mathit{r}}^{+}{\dot{\mathit{J}}}_{\mathit{r}}{\mathit{J}}_{\mathit{r}}^{+}$, ${\mathit{G}}_{\mathit{s}}\left({\mathit{X}}_{\mathit{s}}\right)={\mathit{G}}_{\mathit{s}}\left({\mathit{q}}_{\mathit{s}}\right)$, and ${\mathit{N}}_{\mathit{s}}\left({\dot{\mathit{X}}}_{\mathit{s}}\right)={\mathit{N}}_{\mathit{s}}\left({\dot{\mathit{q}}}_{\mathit{s}}\right)$. The three elements in each vector/matrix set below correspond to left-arm robot, right-arm robot, and dual-arm robot, respectively. ${\mathit{u}}_{\mathit{l}}$, ${\mathit{u}}_{\mathit{r}}$, and ${\mathit{u}}_{\mathit{s}}$ are the control input vectors, and their dimensions are n, n, and 2n, respectively; ${\ddot{\mathit{X}}}_{\mathit{d}\mathit{l}}$, ${\ddot{\mathit{X}}}_{\mathit{d}\mathit{r}}$, and ${\ddot{\mathit{X}}}_{\mathit{d}\mathit{s}}$ are the desired terminal acceleration vectors, and their dimensions are 6, 6, and 12, respectively; ${\dot{\mathit{X}}}_{\mathit{d}\mathit{l}}$, ${\dot{\mathit{X}}}_{\mathit{d}\mathit{r}}$, and ${\dot{\mathit{X}}}_{\mathit{d}\mathit{s}}$ are the desired terminal velocity vectors, and their dimensions are 6, 6, and 12, respectively; ${\mathit{X}}_{\mathit{d}\mathit{l}}$, ${\mathit{X}}_{\mathit{d}\mathit{r}}$, and ${\mathit{X}}_{\mathit{d}\mathit{s}}$ are the desired terminal position and posture vectors, and their dimensions are 6, 6, and 12, respectively; ${\dot{\mathit{X}}}_{\mathit{l}}$, ${\dot{\mathit{X}}}_{\mathit{r}}$, and ${\dot{\mathit{X}}}_{\mathit{s}}$ are the actual terminal velocity vectors, and their dimensions are 6, 6, and 12, respectively; ${\mathit{X}}_{\mathit{l}}$, ${\mathit{X}}_{\mathit{r}}$, and ${\mathit{X}}_{\mathit{s}}$ are the actual terminal position and posture vectors, and their dimensions are 6, 6, and 12, respectively; ${\mathit{F}}_{\mathit{d}\mathit{l}\mathit{w}}$, ${\mathit{F}}_{\mathit{d}\mathit{r}\mathit{w}}$, and ${\mathit{F}}_{\mathit{d}\mathit{s}\mathit{w}}$ are the desired generalized contact force vectors, and their dimensions are 6, 6, and 12, respectively; ${\mathit{k}}_{\mathit{d}\mathit{l}}$, ${\mathit{k}}_{\mathit{d}\mathit{r}}$, and ${\mathit{k}}_{\mathit{d}\mathit{s}}$ are the differential parameter matrixes, and their dimensions are 6 × 6, 6 × 6, and 12 × 12, respectively; ${\mathit{k}}_{\mathit{p}\mathit{l}}$, ${\mathit{k}}_{\mathit{p}\mathit{r}}$, and ${\mathit{k}}_{\mathit{p}\mathit{s}}$ are the proportional parameter matrixes of position control, and their dimensions are 6 × 6, 6 × 6, and 12 × 12, respectively; ${\mathit{k}}_{\mathit{f}\mathit{l}}$, ${\mathit{k}}_{\mathit{f}\mathit{r}}$, and ${\mathit{k}}_{\mathit{f}\mathit{s}}$ are the proportional parameter matrixes, and their dimensions are 6 × 6, 6 × 6, and 12 × 12, respectively; and ${\mathit{J}}_{\mathit{l}}^{+}$, ${\mathit{J}}_{\mathit{r}}^{+}$, and ${\mathit{J}}_{\mathit{s}}^{+}$ are the pseudo-inverse matrixes of ${\mathit{J}}_{\mathit{l}}$, ${\mathit{J}}_{\mathit{r}}$, and ${\mathit{J}}_{\mathit{s}}$, respectively. For the non-redundant robot, ${\mathit{J}}_{\mathit{l}}^{+}={\mathit{J}}_{\mathit{l}}^{-\mathbf{1}}$, ${\mathit{J}}_{\mathit{r}}^{+}={\mathit{J}}_{\mathit{r}}^{-\mathbf{1}}$, and ${\mathit{J}}_{\mathit{s}}^{+}={\mathit{J}}_{\mathit{s}}^{-\mathbf{1}}$.

To realize the dual-arm robot coordination, it is not enough to consider only its own state for the single arm. The state of the other arm should also be considered. Therefore, in the position controller of the single arm, the synchronization error between two arms is considered to ensure the dual-arm robot coordination and the position tracking accuracy of the workpiece. WHFPCC can be obtained by adding the synchronization error to the controller (15), as described in Equation (16).

$$\begin{array}{ll}{\mathit{u}}_{\mathit{s}}& ={\mathit{M}}_{\mathit{s}}\left({\mathit{X}}_{\mathit{s}}\right)\left[{\ddot{\mathit{X}}}_{\mathit{d}\mathit{s}}+{\mathit{k}}_{\mathit{d}\mathit{s}}\mathbf{\Delta }{\dot{\mathit{X}}}_{\mathit{s}}+{\mathit{k}}_{\mathit{p}\mathit{s}}\mathbf{\Delta }{\mathit{X}}_{\mathit{s}}+{\mathit{k}}_{\mathit{c}\mathit{s}}{\mathit{A}}_{\mathit{s}}\mathbf{\Delta }{\mathit{X}}_{\mathit{s}}+{\mathit{k}}_{\mathit{f}\mathit{s}}\left({\mathit{F}}_{\mathit{d}\mathit{s}\mathit{w}}-{\mathit{F}}_{\mathit{s}\mathit{w}}\right)\right]\\ & +{\mathit{C}}_{\mathit{s}}\left({\mathit{X}}_{\mathit{s}},{\dot{\mathit{X}}}_{\mathit{s}}\right){\dot{\mathit{X}}}_{\mathit{s}}+{\mathit{G}}_{\mathit{s}}\left({\mathit{X}}_{\mathit{s}}\right)+{\mathit{N}}_{\mathit{s}}\left({\dot{\mathit{X}}}_{\mathit{s}}\right)+{\mathit{D}}_{\mathit{s}}+{\mathit{J}}_{\mathit{s}}^{\mathit{T}}{\mathit{F}}_{\mathit{s}\mathit{w}}\end{array},$$

(16)

where $\mathbf{\Delta }{\mathit{X}}_{\mathit{s}}={\mathit{X}}_{\mathit{d}\mathit{s}}-{\mathit{X}}_{\mathit{s}}$, $\mathbf{\Delta }{\dot{\mathit{X}}}_{\mathit{s}}={\dot{\mathit{X}}}_{\mathit{d}\mathit{s}}-{\dot{\mathit{X}}}_{\mathit{s}}$, ${\mathit{k}}_{\mathit{c}\mathit{s}}=\left[\begin{array}{cc}{\mathit{k}}_{\mathit{c}\mathit{l}}& {\mathit{O}}_{\mathbf{6}\times \mathbf{6}}\\ {\mathit{O}}_{\mathbf{6}\times \mathbf{6}}& {\mathit{k}}_{\mathit{c}\mathit{r}}\end{array}\right]$, and ${\mathit{A}}_{\mathit{s}}=\left[\begin{array}{cc}{\mathit{I}}_{\mathbf{6}\times \mathbf{6}}& -{\mathit{I}}_{\mathbf{6}\times \mathbf{6}}\\ -{\mathit{I}}_{\mathbf{6}\times \mathbf{6}}& {\mathit{I}}_{\mathbf{6}\times \mathbf{6}}\end{array}\right]$. ${\mathit{k}}_{\mathit{c}\mathit{l}}$, ${\mathit{k}}_{\mathit{c}\mathit{r}}$, and ${\mathit{k}}_{\mathit{c}\mathit{s}}$ are the synchronization error coefficient matrixes of left-arm robot, right-arm robot, and dual-arm robot, respectively, and their dimensions are 6 × 6, 6 × 6, and 12 × 12, respectively.

3.2. Stability Analysis

Kinematic constraints [36] of the dual-arm robot coordination are described below:

$${\dot{\mathit{X}}}_{\mathit{l}}={\mathit{Q}}_{\mathit{l}}{\dot{\mathit{X}}}_{\mathit{t}}, {\dot{\mathit{X}}}_{\mathit{r}}={\mathit{Q}}_{\mathit{r}}{\dot{\mathit{X}}}_{\mathit{t}},$$

(17)

$${\ddot{\mathit{X}}}_{\mathit{l}}={\mathit{Q}}_{\mathit{l}}{\ddot{\mathit{X}}}_{\mathit{t}}+{\dot{\mathit{Q}}}_{\mathit{l}}{\dot{\mathit{X}}}_{\mathit{t}}, {\ddot{\mathit{X}}}_{\mathit{r}}={\mathit{Q}}_{\mathit{r}}{\ddot{\mathit{X}}}_{\mathit{t}}+{\dot{\mathit{Q}}}_{\mathit{r}}{\dot{\mathit{X}}}_{\mathit{t}},$$

(18)

where ${\mathit{Q}}_{\mathit{l}}=\left[\begin{array}{cc}{\mathit{I}}_{\mathbf{3}\times \mathbf{3}}& -{\mathit{S}}_{\mathit{l}}\\ {\mathit{O}}_{\mathbf{3}\times \mathbf{3}}& {\mathit{I}}_{\mathbf{3}\times \mathbf{3}}\end{array}\right]$; ${\mathit{Q}}_{\mathit{r}}=\left[\begin{array}{cc}{\mathit{I}}_{\mathbf{3}\times \mathbf{3}}& -{\mathit{S}}_{\mathit{r}}\\ {\mathit{O}}_{\mathbf{3}\times \mathbf{3}}& {\mathit{I}}_{\mathbf{3}\times \mathbf{3}}\end{array}\right]$; and ${\ddot{\mathit{X}}}_{\mathit{l}}$ and ${\ddot{\mathit{X}}}_{\mathit{r}}$ are the terminal acceleration vectors of the left-arm robot and the right-arm robot, respectively, and their dimensions are both 6.

For the convenience of proving stability, the generalized contact force model (8) can be rewritten according to the above kinematics constraints, as described in Equations (19) and (20). Their independent variables are the terminal velocity and acceleration of the left-arm robot and the right-arm robot, and their dependent variable is the generalized contact force.

$${\mathit{F}}_{\mathit{s}\mathit{w}}={\left({\mathit{W}}^{\prime }\right)}^{+}\left[{\mathit{M}}_{\mathit{t}}{\mathit{Q}}_{\mathit{l}}^{-\mathbf{1}}{\ddot{\mathit{X}}}_{\mathit{l}}+\left({\mathit{B}}_{\mathit{t}}{\mathit{Q}}_{\mathit{l}}^{-\mathbf{1}}-{\mathit{M}}_{\mathit{t}}{\mathit{Q}}_{\mathit{l}}^{-\mathbf{1}}{\dot{\mathit{Q}}}_{\mathit{l}}{\mathit{Q}}_{\mathit{l}}^{-\mathbf{1}}\right){\dot{\mathit{X}}}_{\mathit{l}}-{\mathit{G}}_{\mathit{o}}\right]+\left[{\mathit{I}}_{\mathbf{6}\times \mathbf{6}}-{\left({\mathit{W}}^{\prime }\right)}^{+}{\mathit{W}}^{\prime }\right]\mathit{\sigma },$$

(19)

$${\mathit{F}}_{\mathit{s}\mathit{w}}={\left({\mathit{W}}^{\prime }\right)}^{+}\left[{\mathit{M}}_{\mathit{t}}{\mathit{Q}}_{\mathit{r}}^{-\mathbf{1}}{\ddot{\mathit{X}}}_{\mathit{r}}+\left({\mathit{B}}_{\mathit{t}}{\mathit{Q}}_{\mathit{r}}^{-\mathbf{1}}-{\mathit{M}}_{\mathit{t}}{\mathit{Q}}_{\mathit{r}}^{-\mathbf{1}}{\dot{\mathit{Q}}}_{\mathit{r}}{\mathit{Q}}_{\mathit{r}}^{-\mathbf{1}}\right){\dot{\mathit{X}}}_{\mathit{r}}-{\mathit{G}}_{\mathit{o}}\right]+\left[{\mathit{I}}_{\mathbf{6}\times \mathbf{6}}-{\left({\mathit{W}}^{\prime }\right)}^{+}{\mathit{W}}^{\prime }\right]\mathit{\sigma },$$

(20)

where ${\mathit{M}}_{\mathit{t}}=\left[\begin{array}{cc}diag\left(m_t,m_t,m_t\right)& {\mathit{O}}_{\mathbf{3}\times \mathbf{3}}\\ {\mathit{O}}_{\mathbf{3}\times \mathbf{3}}& {\mathit{I}}_{\mathit{t}}\end{array}\right]$, ${\mathit{B}}_{\mathit{t}}=\left[\begin{array}{cc}{\mathit{O}}_{\mathbf{3}\times \mathbf{3}}& {\mathit{O}}_{\mathbf{3}\times \mathbf{3}}\\ {\mathit{O}}_{\mathbf{3}\times \mathbf{3}}& -{\mathit{S}}_{\mathit{t}}\end{array}\right]$, ${\mathit{S}}_{\mathit{t}}=\left[\begin{array}{ccc}0& -{\left({\mathit{I}}_{\mathit{t}}{\mathit{\omega }}_{\mathit{t}}\right)}_z& {\left({\mathit{I}}_{\mathit{t}}{\mathit{\omega }}_{\mathit{t}}\right)}_y\\ {\left({\mathit{I}}_{\mathit{t}}{\mathit{\omega }}_{\mathit{t}}\right)}_z& \mathbf{0}& -{\left({\mathit{I}}_{\mathit{t}}{\mathit{\omega }}_{\mathit{t}}\right)}_x\\ -{\left({\mathit{I}}_{\mathit{t}}{\mathit{\omega }}_{\mathit{t}}\right)}_y& {\left({\mathit{I}}_{\mathit{t}}{\mathit{\omega }}_{\mathit{t}}\right)}_x& 0\end{array}\right]$, and $diag\left(m_t,m_t,m_t\right)$ is a diagonal matrix with $\left(m_t,m_t,m_t\right)$ as the main diagonal elements.

Adding Equation (19) to Equation (20) yields

$${\mathit{F}}_{\mathit{s}\mathit{w}}={\mathit{M}}_{\mathit{t}\mathit{s}}{\ddot{\mathit{X}}}_{\mathit{s}}+{\mathit{B}}_{\mathit{t}\mathit{s}}{\dot{\mathit{X}}}_{\mathit{s}}-{\left({\mathit{W}}^{\prime }\right)}^{+}{\mathit{G}}_{\mathit{o}}+\left[{\mathit{I}}_{\mathbf{6}\times \mathbf{6}}-{\left({\mathit{W}}^{\prime }\right)}^{+}{\mathit{W}}^{\prime }\right]\mathit{\sigma },$$

(21)

where ${\mathit{M}}_{\mathit{t}\mathit{s}}={\left[\begin{array}{c}{\displaystyle \frac{1}{2}}{\left({\mathit{W}}^{\prime }\right)}^{+}{\mathit{M}}_{\mathit{t}}{\mathit{Q}}_{\mathit{l}}^{-\mathbf{1}}\\ {\displaystyle \frac{1}{2}}{\left({\mathit{W}}^{\prime }\right)}^{+}{\mathit{M}}_{\mathit{t}}{\mathit{Q}}_{\mathit{r}}^{-\mathbf{1}}\end{array}\right]}^T$, ${\mathit{B}}_{\mathit{t}\mathit{s}}={\left[\begin{array}{c}{\displaystyle \frac{1}{2}}{\left({\mathit{W}}^{\prime }\right)}^{+}\left({\mathit{B}}_{\mathit{t}}{\mathit{Q}}_{\mathit{l}}^{-\mathbf{1}}-{\mathit{M}}_{\mathit{t}}{\mathit{Q}}_{\mathit{l}}^{-\mathbf{1}}{\dot{\mathit{Q}}}_{\mathit{l}}{\mathit{Q}}_{\mathit{l}}^{-\mathbf{1}}\right)\\ {\displaystyle \frac{1}{2}}{\left({\mathit{W}}^{\prime }\right)}^{+}\left({\mathit{B}}_{\mathit{t}}{\mathit{Q}}_{\mathit{r}}^{-\mathbf{1}}-{\mathit{M}}_{\mathit{t}}{\mathit{Q}}_{\mathit{r}}^{-\mathbf{1}}{\dot{\mathit{Q}}}_{\mathit{r}}{\mathit{Q}}_{\mathit{r}}^{-\mathbf{1}}\right)\end{array}\right]}^T$, and ${\ddot{\mathit{X}}}_{\mathit{s}}=\left[\begin{array}{c}{\ddot{\mathit{X}}}_{\mathit{l}}\\ {\ddot{\mathit{X}}}_{\mathit{r}}\end{array}\right]$.

The dynamic model of dual-arm robots can also be described below:

$${\mathit{u}}_{\mathit{s}}={\mathit{M}}_{\mathit{s}}\left({\mathit{X}}_{\mathit{s}}\right){\ddot{\mathit{X}}}_{\mathit{s}}+{\mathit{C}}_{\mathit{s}}\left({\mathit{X}}_{\mathit{s}},{\dot{\mathit{X}}}_{\mathit{s}}\right){\dot{\mathit{X}}}_{\mathit{s}}+{\mathit{G}}_{\mathit{s}}\left({\mathit{X}}_{\mathit{s}}\right)+{\mathit{N}}_{\mathit{s}}\left({\dot{\mathit{X}}}_{\mathit{s}}\right)+{\mathit{D}}_{\mathit{s}}+{\mathit{J}}_{\mathit{s}}^{\mathit{T}}{\mathit{F}}_{\mathit{s}\mathit{w}},$$

(22)

The input of the controller (16) is the joint torque vector. That is, ${\mathit{u}}_{\mathit{s}}={\mathit{T}}_{\mathit{s}}$. The closed-loop system model is derived by substituting the control law (16) into the dynamic model (22), as described in Equation (23).

$$\Delta {\ddot{\mathit{X}}}_{\mathit{s}}+{\mathit{k}}_{\mathit{d}\mathit{s}}\Delta {\dot{\mathit{X}}}_{\mathit{s}}+\left({\mathit{k}}_{\mathit{p}\mathit{s}}+{\mathit{k}}_{\mathit{c}\mathit{s}}{\mathit{A}}_{\mathit{s}}\right)\Delta {\mathit{X}}_{\mathit{s}}+{\mathit{k}}_{\mathit{f}\mathit{s}}\left({\mathit{F}}_{\mathit{d}\mathit{s}\mathit{w}}-{\mathit{F}}_{\mathit{s}\mathit{w}}\right)={\mathit{O}}_{\mathbf{6}\times \mathbf{1}},$$

(23)

where $\Delta {\ddot{\mathit{X}}}_{\mathit{s}}={\ddot{\mathit{X}}}_{\mathit{d}\mathit{s}}-{\ddot{\mathit{X}}}_{\mathit{s}}$.

The closed-loop system model is rewritten by substituting the generalized contact force model (21) into Equation (23), as described in Equation (24). Its independent variables are the terminal position and posture error of the dual-arm robot and its derivatives.

$$\ddot{\mathit{e}}+{\mathit{k}}_1\dot{\mathit{e}}+{\mathit{k}}_2\mathit{e}={\mathit{O}}_{\mathbf{6}\times \mathbf{1}},$$

(24)

where $\mathit{e}=\Delta {\mathit{X}}_{\mathit{s}}$, ${\mathit{k}}_1={\left({\mathit{I}}_{\mathbf{6}\times \mathbf{6}}+{\mathit{k}}_{\mathit{f}\mathit{s}}{\mathit{M}}_{\mathit{t}\mathit{s}}\right)}^{-1}\left({\mathit{k}}_{\mathit{d}\mathit{s}}+{\mathit{k}}_{\mathit{f}\mathit{s}}{\mathit{B}}_{\mathit{t}\mathit{s}}\right)$, and ${\mathit{k}}_2={\left({\mathit{I}}_{\mathbf{6}\times \mathbf{6}}+{\mathit{k}}_{\mathit{f}\mathit{s}}{\mathit{M}}_{\mathit{t}\mathit{s}}\right)}^{-1}\left({\mathit{k}}_{\mathit{p}\mathit{s}}+{\mathit{k}}_{\mathit{c}\mathit{s}}{\mathit{A}}_{\mathit{s}}\right)$.

The Lyapunov function is chosen to prove the stability of WHFPCC, as described below:

$$\mathit{V}={\displaystyle \frac{1}{2}}{\mathit{e}}^{\mathit{T}}{\mathit{k}}_2\mathit{e}+{\displaystyle \frac{1}{2}}{\dot{\mathit{e}}}^{\mathit{T}}\dot{\mathit{e}},$$

(25)

Substituting Equation (24) into the derivative of Equation (25) yields

$$\dot{\mathit{V}}={\mathit{e}}^{\mathit{T}}{\mathit{k}}_2\dot{\mathit{e}}+{\dot{\mathit{e}}}^{\mathit{T}}\ddot{\mathit{e}}={\mathit{e}}^{\mathit{T}}{\mathit{k}}_2\dot{\mathit{e}}+{\dot{\mathit{e}}}^{\mathit{T}}\left(-{\mathit{k}}_1\dot{\mathit{e}}-{\mathit{k}}_2\mathit{e}\right)=-{\dot{\mathit{e}}}^{\mathit{T}}{\mathit{k}}_1\dot{\mathit{e}},$$

(26)

To realize the Lyapunov stability, coefficients ${\mathit{k}}_1$ and ${\mathit{k}}_2$ should be positive definite matrices by choosing the suitable values of ${\mathit{k}}_{\mathit{p}\mathit{s}}$, ${\mathit{k}}_{\mathit{d}\mathit{s}}$, ${\mathit{k}}_{\mathit{c}\mathit{s}}$, and ${\mathit{k}}_{\mathit{f}\mathit{s}}$. Then the proposed control method would be stable since $V\ge 0$ ands $\dot{V}\le 0$.

4. Weighted Hybrid Force/Position Fuzzy Coordination Controller for Dual-Arm Robots

To automatically select the optimal control gains, WHFPFCC is proposed by adding a fuzzy controller to WHFPCC. It can automatically tune the control gains to achieve optimal control performance.

According to the structural characteristics of WHFPCC, input variables of the fuzzy controller are the absolute values of the generalized contact force errors $\left|\Delta {\mathit{F}}_{\mathit{s}\mathit{w}}\right|$, the absolute values of the position and posture errors $\left|\Delta {\mathit{X}}_{\mathit{s}}\right|$, and the absolute values of the synchronization errors $\left|{\mathit{E}}_{\mathit{s}}\right|$. Here, ${\mathit{E}}_{\mathit{s}}=\left[\begin{array}{c}\Delta {\mathit{X}}_{\mathit{l}}-\Delta {\mathit{X}}_{\mathit{r}}\\ \Delta {\mathit{X}}_{\mathit{r}}-\Delta {\mathit{X}}_{\mathit{l}}\end{array}\right]$. Output variables are the variations $\Delta {\mathit{k}}_{\mathit{p}\mathit{s}}$, $\Delta {\mathit{k}}_{\mathit{d}\mathit{s}}$, $\Delta {\mathit{k}}_{\mathit{f}\mathit{s}}$, and $\Delta {\mathit{k}}_{\mathit{c}\mathit{s}}$ of the control gains ${\mathit{k}}_{\mathit{p}\mathit{s}}$, ${\mathit{k}}_{\mathit{d}\mathit{s}}$, ${\mathit{k}}_{\mathit{f}\mathit{s}}$, and ${\mathit{k}}_{\mathit{c}\mathit{s}}$.

4.1. Fuzzification

The fuzzy sets of input variables are specified as {Very Small (VS), Small (S), Medium (M), Big (B), Very Big (VB)}. Their fuzzy domains are designed as {0,4} accordingly. That is, $\left|\Delta {\mathit{F}}_{\mathit{s}\mathit{w}}\right|,\left|\Delta {\mathit{X}}_{\mathit{s}}\right|,\left|{\mathit{E}}_{\mathit{s}}\right|\in \left\{0,1,2,3,4\right\}$.

The continuous triangular membership function is adopted since the input variables change continuously, as shown in Figure 3. On the basis of the designed fuzzy domain, VS, S, M, B, and VB can be represented as Equations (27)–(31).

$$f\left(x\right)=\left\{\begin{array}{cc}{\displaystyle \frac{x-1}{0-1}}& 0\le x<1\\ 0& x<0,x\ge 1\end{array}\right.=\left\{\begin{array}{cc}1-x& 0\le x<1\\ 0& x<0,x\ge 1\end{array}\right.,$$

(27)

$$f\left(x\right)=\left\{\begin{array}{cc}{\displaystyle \frac{x-0}{1-0}}& 0<x<1\\ {\displaystyle \frac{x-2}{1-2}}& 1\le x<2\\ 0& x\le 0,x\ge 2\end{array}\right.=\left\{\begin{array}{cc}x& 0<x<1\\ 2-x& 1\le x<2\\ 0& x\le 0,x\ge 2\end{array}\right.$$

(28)

$$f\left(x\right)=\left\{\begin{array}{cc}{\displaystyle \frac{x-1}{2-1}}& 1<x<2\\ {\displaystyle \frac{x-3}{2-3}}& 2\le x<3\\ 0& x\le 1,x\ge 3\end{array}\right.=\left\{\begin{array}{cc}x-1& 1<x<2\\ 3-x& 2\le x<3\\ 0& x\le 1,x\ge 3\end{array}\right.,$$

(29)

$$f\left(x\right)=\left\{\begin{array}{cc}{\displaystyle \frac{x-2}{3-2}}& 2<x<3\\ {\displaystyle \frac{x-4}{3-4}}& 3\le x<4\\ 0& x\le 2,x\ge 4\end{array}\right.=\left\{\begin{array}{cc}x-2& 2<x<3\\ 4-x& 3\le x<4\\ 0& x\le 2,x\ge 4\end{array}\right.,$$

(30)

$$f\left(x\right)=\left\{\begin{array}{cc}{\displaystyle \frac{x-3}{4-3}}& 3<x\le 4\\ 0& x\le 3,x>4\end{array}\right.=\left\{\begin{array}{cc}x-3& 3<x\le 4\\ 0& x\le 3,x>4\end{array}\right.,$$

(31)

where $x$ is the input variable after fuzzification, and $f\left(x\right)$ is its membership function.

The fuzzy sets of output variables are specified as {Negative Big (NB), Negative Medium (NM), Negative Small (NS), Zero (ZO), Positive Small (PS), Positive Medium (PM), Positive Big (PB)}. Their fuzzy domains are designed as {−3,3} accordingly. That is, $\Delta {\mathit{k}}_{\mathit{p}\mathit{s}},\Delta {\mathit{k}}_{\mathit{d}\mathit{s}},\Delta {\mathit{k}}_{\mathit{f}\mathit{s}},\Delta {\mathit{k}}_{\mathit{c}\mathit{s}}\in \left\{-3,-2,-1,0,1,2,3\right\}$.

The continuous triangular membership function is adopted since the output variables change continuously, as shown in Figure 4. On the basis of the designed fuzzy domain, NB, NM, NS, ZO, PS, PM, and PB can be represented as Equations (32)–(38).

$$g\left(y\right)=\left\{\begin{array}{cc}{\displaystyle \frac{y-\left(-2\right)}{-3-\left(-2\right)}}& -3\le y<-2\\ 0& y<-3,y\ge -2\end{array}=\left\{\begin{array}{cc}-y-2& -3\le y<-2\\ 0& y<-3,y\ge -2\end{array}\right.\right.,$$

(32)

$$g\left(y\right)=\left\{\begin{array}{cc}{\displaystyle \frac{y-\left(-3\right)}{-2-\left(-3\right)}}& -3<y<-2\\ {\displaystyle \frac{y-\left(-1\right)}{-2-\left(-1\right)}}& -2\le y<-1\\ 0& y\le -3,y\ge -1\end{array}\right.=\left\{\begin{array}{cc}y+3& -3<y<-2\\ -y-1& -2\le y<-1\\ 0& y\le -3,y\ge -1\end{array}\right.,$$

(33)

$$g\left(y\right)=\left\{\begin{array}{cc}{\displaystyle \frac{y-\left(-2\right)}{-1-\left(-2\right)}}& -2<y<-1\\ {\displaystyle \frac{y-0}{-1-0}}& -1\le y<0\\ 0& y\le -2,y\ge 0\end{array}\right.=\left\{\begin{array}{cc}y+2& -2<y<-1\\ -y& -1\le y<0\\ 0& y\le -2,y\ge 0\end{array}\right.,$$

(34)

$$g\left(y\right)=\left\{\begin{array}{cc}{\displaystyle \frac{y-\left(-1\right)}{0-\left(-1\right)}}& -1<y<0\\ {\displaystyle \frac{y-1}{0-1}}& 0\le y<1\\ 0& y\le -1,y\ge 1\end{array}\right.=\left\{\begin{array}{cc}y+1& -1<y<0\\ -y+1& 0\le y<1\\ 0& y\le -1,y\ge 1\end{array}\right.,$$

(35)

$$g\left(y\right)=\left\{\begin{array}{cc}{\displaystyle \frac{y-0}{1-0}}& 0<y<1\\ {\displaystyle \frac{y-2}{1-2}}& 1\le y<2\\ 0& y\le 0,y\ge 2\end{array}=\left\{\begin{array}{cc}y& 0<y<1\\ -y+2& 1\le y<2\\ 0& y\le 0,y\ge 2\end{array}\right.\right.,$$

(36)

$$g\left(y\right)=\left\{\begin{array}{cc}{\displaystyle \frac{y-1}{2-1}}& 1<y<2\\ {\displaystyle \frac{y-3}{2-3}}& 2\le y<3\\ 0& y\le 1,y\ge 3\end{array}\right.=\left\{\begin{array}{cc}y-1& 1<y<2\\ -y+3& 2\le y<3\\ 0& y\le 1,y\ge 3\end{array}\right.,$$

(37)

$$g\left(y\right)=\left\{\begin{array}{cc}{\displaystyle \frac{y-2}{3-2}}& 2<y\le 3\\ 0& y\le 2,y>3\end{array}\right.=\left\{\begin{array}{cc}y-2& 2<y\le 3\\ 0& y\le 2,y>3\end{array}\right.,$$

(38)

where $y$ is the output variable after fuzzification, and $g\left(y\right)$ is its membership function.

4.2. Fuzzy Inference

According to the influence of control gains on the generalized contact force errors, the position and posture errors, and the synchronization errors, the following fuzzy rule is designed:

If $\left|\Delta {\mathit{X}}_{\mathit{s}}\right|$ is A_h, $\left|\Delta {\mathit{F}}_{\mathit{s}\mathit{w}}\right|$ is B_i and $\left|{\mathit{E}}_s\right|$ is C_j, then $\Delta {\mathit{k}}_{\mathit{p}\mathit{s}}$ is D_k, $\Delta {\mathit{k}}_{\mathit{d}\mathit{s}}$ is E_l, $\Delta {\mathit{k}}_{\mathit{f}\mathit{s}}$ is F_m, and $\Delta {\mathit{k}}_{\mathit{c}\mathit{s}}$ is G_n, where A_h, B_i, C_j, D_k, E_l, F_m, and G_n are the fuzzy sets of $\left|\Delta {\mathit{X}}_{\mathit{s}}\right|$, $\left|\Delta {\mathit{F}}_{\mathit{s}\mathit{w}}\right|$, $\left|{\mathit{E}}_s\right|$, $\Delta {\mathit{k}}_{\mathit{p}\mathit{s}}$, $\Delta {\mathit{k}}_{\mathit{d}\mathit{s}}$, $\Delta {\mathit{k}}_{\mathit{f}\mathit{s}}$, and $\Delta {\mathit{k}}_{\mathit{c}\mathit{s}}$, respectively.

The design basis of the fuzzy rules is the dominant variable principle. Among the three input variables of the fuzzy controller, the largest one will be dominant. The control gain corresponding to the dominant input variable will be increased first. In addition, considering that the input of the designed controller is the weighted sum of the inputs of the position, force, and synchronization control, the control gain corresponding to the non-dominant input variable will be appropriately reduced to prevent input saturation. The fuzzy rule base is designed by combining the above principles with practical experience, as shown in

Table 1. Fuzzy rules when the fuzzy set of $\left|\Delta {\mathit{X}}_{\mathit{s}}\right|$ is VS.

$\left|\mathbf{\Delta }{\mathit{X}}_{\mathit{s}}\right|$ Is VS		$\left|{\mathit{E}}_{\mathit{s}}\right|$
		VS	S	M	B	VB
$\left|\Delta {\mathit{F}}_{\mathit{s}\mathit{w}}\right|$	VS	ZO, PS, ZO, ZO	ZO, PS, ZO, ZO	NS, ZO, ZO, PS	NM, ZO, NS, PM	NB, ZO, NM, PB
	S	ZO, PS, ZO, ZO	ZO, PS, ZO, ZO	NS, PS, ZO, PS	NM, ZO, NS, PM	NB, ZO, NS, PB
	M	NS, PM, PS, NS	NS, PM, PS, NS	NM, PS, ZO, ZO	NM, PS, ZO, PS	NB, ZO, NS, PM
	B	NS, PM, PM, NS	NM, PM, PM, ZO	NM, PM, PS, PS	NB, PS, ZO, PS	NB, PS, ZO, PM
	VB	NM, PB, PB, NS	NM, PM, PM, ZO	NB, PM, PS, PS	NB, PM, PS, PM	NB, PS, ZO, PB

Where each element in the table denotes the fuzzy sets of $\Delta {\mathit{k}}_{\mathit{p}\mathit{s}}$, $\Delta {\mathit{k}}_{\mathit{d}\mathit{s}}$, $\Delta {\mathit{k}}_{\mathit{f}\mathit{s}}$, and $\Delta {\mathit{k}}_{\mathit{c}\mathit{s}}$.

,

Table 2. Fuzzy rules when the fuzzy set of $\left|\Delta {\mathit{X}}_{\mathit{s}}\right|$ is S.

$\left|\mathbf{\Delta }{\mathit{X}}_{\mathit{s}}\right|$ Is S		$\left|{\mathit{E}}_{\mathit{s}}\right|$
		VS	S	M	B	VB
$\left|\Delta {\mathit{F}}_{\mathit{s}\mathit{w}}\right|$	VS	ZO, ZO, ZO, ZO	ZO, ZO, ZO, ZO	NS, NS, ZO, PS	NM, NS, NS, PM	NM, NM, NM, PB
	S	ZO, PS, ZO, ZO	ZO, ZO, ZO, ZO	NS, ZO, NS, PS	NM, NS, NS, PM	NM, NS, NM, PM
	M	ZO, PS, PS, NS	NS, PS, PS, NS	NS, ZO, PS, ZO	NM, ZO, ZO, PS	NM, NS, ZO, PS
	B	NS, PM, PM, NS	NS, PS, PM, ZO	NM, PS, PS, PS	NM, ZO, PS, PS	NM, ZO, ZO, PM
	VB	NS, PM, PB, NS	NS, PM, PM, ZO	NM, PS, PM, PS	NM, PS, PS, PM	NM, ZO, ZO, PM

Where each element in the table denotes the fuzzy sets of $\Delta {\mathit{k}}_{\mathit{p}\mathit{s}}$, $\Delta {\mathit{k}}_{\mathit{d}\mathit{s}}$, $\Delta {\mathit{k}}_{\mathit{f}\mathit{s}}$, and $\Delta {\mathit{k}}_{\mathit{c}\mathit{s}}$.

,

Table 3. Fuzzy rules when the fuzzy set of $\left|\Delta {\mathit{X}}_{\mathit{s}}\right|$ is M.

$\left|\mathbf{\Delta }{\mathit{X}}_{\mathit{s}}\right|$ Is M		$\left|{\mathit{E}}_{\mathit{s}}\right|$
		VS	S	M	B	VB
$\left|\Delta {\mathit{F}}_{\mathit{s}\mathit{w}}\right|$	VS	PM, NS, NS, NM	PS, NS, NS, NS	ZO, NM, NS, ZO	NS, NM, NM, PS	NS, NM, NM, PM
	S	PM, ZO, ZO, NM	PS, NS, NS, NS	ZO, NS, NS, ZO	NS, NM, NM, PS	NS, NM, NM, PM
	M	PS, PS, PS, NM	ZO, ZO, PS, NS	ZO, NS, ZO, NS	NS, NS, NS, ZO	NM, NM, NS, PS
	B	PS, PS, PM, NS	ZO, PS, PS, NS	NS, ZO, PS, ZO	NS, NS, ZO, PS	NM, NS, NS, PS
	VB	ZO, PM, PM, NS	NS, PS, PM, NS	NS, ZO, PS, ZO	NM, ZO, PS, PS	NM, NS, ZO, PM

Where each element in the table denotes the fuzzy sets of $\Delta {\mathit{k}}_{\mathit{p}\mathit{s}}$, $\Delta {\mathit{k}}_{\mathit{d}\mathit{s}}$, $\Delta {\mathit{k}}_{\mathit{f}\mathit{s}}$, and $\Delta {\mathit{k}}_{\mathit{c}\mathit{s}}$.

,

Table 4. Fuzzy rules when the fuzzy set of $\left|\Delta {\mathit{X}}_{\mathit{s}}\right|$ is B.

$\left|\mathbf{\Delta }{\mathit{X}}_{\mathit{s}}\right|$ Is B		$\left|{\mathit{E}}_{\mathit{s}}\right|$
		VS	S	M	B	VB
$\left|\Delta {\mathit{F}}_{\mathit{s}\mathit{w}}\right|$	VS	PB, NM, NM, NM	PM, NM, NM, NM	PS, NM, NM, NS	ZO, NM, NB, ZO	NS, NB, NB, PS
	S	PB, NS, NS, NM	PM, NM, NM, NM	PS, NM, NM, NS	ZO, NM, NB, ZO	NS, NM, NB, PS
	M	PM, ZO, ZO, NM	PS, NS, ZO, NS	PS, NS, NS, ZO	ZO, NS, NM, PS	NS, NM, NM, PS
	B	PS, ZO, PS, NS	PS, ZO, ZO, NS	ZO, NS, ZO, ZO	ZO, NS, NS, ZO	NS, NS, NM, PS
	VB	PS, PS, PS, NS	ZO, ZO, PS, ZO	NS, ZO, ZO, ZO	NS, NS, ZO, ZO	NS, NS, NS, PS

Where each element in the table denotes the fuzzy sets of $\Delta {\mathit{k}}_{\mathit{p}\mathit{s}}$, $\Delta {\mathit{k}}_{\mathit{d}\mathit{s}}$, $\Delta {\mathit{k}}_{\mathit{f}\mathit{s}}$, and $\Delta {\mathit{k}}_{\mathit{c}\mathit{s}}$.

and

Table 5. Fuzzy rules when the fuzzy set of $\left|\Delta {\mathit{X}}_{\mathit{s}}\right|$ is VB.

$\left|\mathbf{\Delta }{\mathit{X}}_{\mathit{s}}\right|$ Is VB		$\left|{\mathit{E}}_{\mathit{s}}\right|$
		VS	S	M	B	VB
$\left|\Delta {\mathit{F}}_{\mathit{s}\mathit{w}}\right|$	VS	PB, NM, NB, NB	PB, NM, NB, NM	PM, NM, NM, NS	PM, NB, NM, ZO	PS, NB, NM, PS
	S	PB, NM, NM, NB	PB, NM, NM, NM	PM, NM, NM, NS	PM, NM, NM, ZO	PS, NB, NM, PS
	M	PB, NS, NS, NM	PM, NS, NS, NM	PS, NM, NS, NS	PS, NM, NS, ZO	ZO, NM, NS, PS
	B	PM, NS, ZO, NM	PM, NS, ZO, NS	PS, NS, ZO, NS	PS, NM, NS, ZO	ZO, NM, NS, PS
	VB	PM, ZO, PS, NS	PS, NS, PS, NS	PS, NS, PS, ZO	PS, NS, ZO, ZO	ZO, NM, NS, PS

Where each element in the table denotes the fuzzy sets of $\Delta {\mathit{k}}_{\mathit{p}\mathit{s}}$, $\Delta {\mathit{k}}_{\mathit{d}\mathit{s}}$, $\Delta {\mathit{k}}_{\mathit{f}\mathit{s}}$, and $\Delta {\mathit{k}}_{\mathit{c}\mathit{s}}$.

. According to these fuzzy rules, the fuzzy values of output variables can be obtained by a standard Mamdani inference engine [37].

4.3. Defuzzification

To transform the fuzzy values of output variables into clear control signals, the center of gravity defuzzifier is utilized to defuzzify them, as shown in Equation (39).

$$y_0={\displaystyle \frac{{\displaystyle \int yg\left(y\right)dy}}{{\displaystyle \int g\left(y\right)dy}}},$$

(39)

where $y_0$ is the output variable after defuzzification.

The control gains ${\mathit{k}}_{\mathit{p}\mathit{s}}$, ${\mathit{k}}_{\mathit{d}\mathit{s}}$, ${\mathit{k}}_{\mathit{f}\mathit{s}}$, and ${\mathit{k}}_{\mathit{c}\mathit{s}}$ can be tuned by adding the output variables $\Delta {\mathit{k}}_{\mathit{p}\mathit{s}}$, $\Delta {\mathit{k}}_{\mathit{d}\mathit{s}}$, $\Delta {\mathit{k}}_{\mathit{f}\mathit{s}}$, and $\Delta {\mathit{k}}_{\mathit{c}\mathit{s}}$, as show in Equation (40). Therefore, the control gains can be adaptively tuned for the best effect.

$$\left\{\begin{array}{c}{\mathit{k}}_{\mathit{p}\mathit{s}}^{\mathit{j}+\mathbf{1}}={\mathit{k}}_{\mathit{p}\mathit{s}}^{\mathit{j}}+\Delta {\mathit{k}}_{\mathit{p}\mathit{s}}^{\mathit{j}}\\ {\mathit{k}}_{\mathit{d}\mathit{s}}^{\mathit{j}+\mathbf{1}}={\mathit{k}}_{\mathit{d}\mathit{s}}^{\mathit{j}}+\Delta {\mathit{k}}_{\mathit{d}\mathit{s}}^{\mathit{j}}\\ {\mathit{k}}_{\mathit{f}\mathit{s}}^{\mathit{j}+\mathbf{1}}={\mathit{k}}_{\mathit{f}\mathit{s}}^{\mathit{j}}+\Delta {\mathit{k}}_{\mathit{f}\mathit{s}}^{\mathit{j}}\\ {\mathit{k}}_{\mathit{c}\mathit{s}}^{\mathit{j}+\mathbf{1}}={\mathit{k}}_{\mathit{c}\mathit{s}}^{\mathit{j}}+\Delta {\mathit{k}}_{\mathit{c}\mathit{s}}^{\mathit{j}}\end{array}\right.,$$

(40)

where ${\mathit{k}}_{\mathit{p}\mathit{s}}^{\mathit{j}}$, ${\mathit{k}}_{\mathit{d}\mathit{s}}^{\mathit{j}}$, ${\mathit{k}}_{\mathit{f}\mathit{s}}^{\mathit{j}}$, and ${\mathit{k}}_{\mathit{c}\mathit{s}}^{\mathit{j}}$ are the control gains before adjustment; ${\mathit{k}}_{\mathit{p}\mathit{s}}^{\mathit{j}+\mathbf{1}}$, ${\mathit{k}}_{\mathit{d}\mathit{s}}^{\mathit{j}+\mathbf{1}}$, ${\mathit{k}}_{\mathit{f}\mathit{s}}^{\mathit{j}+\mathbf{1}}$, and ${\mathit{k}}_{\mathit{c}\mathit{s}}^{\mathit{j}+\mathbf{1}}$ are the control gains after adjustment; and $\Delta {\mathit{k}}_{\mathit{p}\mathit{s}}^{\mathit{j}}$, $\Delta {\mathit{k}}_{\mathit{d}\mathit{s}}^{\mathit{j}}$, $\Delta {\mathit{k}}_{\mathit{f}\mathit{s}}^{\mathit{j}}$, and $\Delta {\mathit{k}}_{\mathit{c}\mathit{s}}^{\mathit{j}}$ are the variations of control gains.

In addition, to guarantee the stability of the designed controller, the tuned control gains must meet the stability conditions described in Section 3.2. If the tuned control gains do not meet the stability conditions, the controller should still use the original control gains.

5. Numerical Simulations

To validate the effectiveness and superiority of the proposed method, two comparative simulations are performed on a Matlab platform. The Baxter dual-arm robot built by Rethink Robotics is chosen as the simulation subject. The transport operation of the dual-arm robot is taken as an example for the tight coordination operation. The workpiece moves along its centroid trajectory during the operation.

5.1. Comparative Simulation Case: Straight Line + Circular Arc Trajectory

It is assumed that the overall dimensions of the workpiece are 0.235 m, 0.04 m, and 0.04 m. Its material is aluminum, and its mass is 0.428 kg. The end effector of the dual-arm robot is a two-finger gripper. Its material is plastic. The friction coefficient $\mu$ between the end effector and the workpiece is 0.26 [38]. The desired trajectory of the workpiece centroid is designed as described in Equations (41) and (42). The workpiece moves in a straight line along the Z-axis direction firstly and then moves in a circular arc on the YZ plane.

$$\begin{array}{cc}{P}_{tyd}=0& 0<t\le 8\\ P_{tyd}=-0.08\cos \left({\displaystyle \frac{\mathit{\pi }}{16}}t\right)& 8<t\le 16\end{array},$$

(41)

$$\begin{array}{cc}{P}_{tzd}=-0.01t& 0<t\le 8\\ P_{tzd}=-0.08\sin \left({\displaystyle \frac{\mathit{\pi }}{16}}t\right)& 8<t\le 16\end{array},$$

(42)

where $P_{tyd}$ and $P_{tzd}$ are the desired positions of the workpiece centroid in the Y and Z direction, and $t$ is the execution time.

The position coordination control (PCC) method (43), the traditional hybrid force/position control method (THFPC) [39], and WHFPFCC are applied to the transport operation of the dual-arm robot. Their properties and discrepancies are analyzed according to the simulation results.

$${\mathit{u}}_{\mathit{s}}={\mathit{M}}_{\mathit{s}}\left({\mathit{X}}_{\mathit{s}}\right)\left[{\ddot{\mathit{X}}}_{\mathit{d}\mathit{s}}+{\mathit{k}}_{\mathit{d}\mathit{s}}\Delta {\dot{\mathit{X}}}_{\mathit{s}}+{\mathit{k}}_{\mathit{p}\mathit{s}}\Delta {\mathit{X}}_{\mathit{s}}+{\mathit{k}}_{\mathit{c}\mathit{s}}{\mathit{A}}_{\mathit{s}}\Delta {\mathit{X}}_{\mathit{s}}\right]+{\mathit{C}}_{\mathit{s}}\left({\mathit{X}}_{\mathit{s}},{\dot{\mathit{X}}}_{\mathit{s}}\right){\dot{\mathit{X}}}_{\mathit{s}}+{\mathit{G}}_{\mathit{s}}\left({\mathit{X}}_{\mathit{s}}\right)+{\mathit{N}}_{\mathit{s}}\left({\dot{\mathit{X}}}_{\mathit{s}}\right)+{\mathit{D}}_{\mathit{s}}+{\mathit{J}}_{\mathit{s}}^{\mathit{T}}{\mathit{F}}_{\mathit{s}\mathit{w}},$$

(43)

5.1.1. Force Distribution Analysis of Dual-Arm Robot with Straight Line + Circular Arc Trajectory on the Basis of Dynamic Force Closure

The desired generalized force applied by the dual-arm robot to the workpiece can be determined by the Newton–Euler equation. The desired generalized force applied by the workpiece to the dual-arm robot can be obtained by force closure dynamic constraints and Newton’s third law, as shown in Figure 5.

According to Equation (6), the tangential forces $F_{lt}$ and $F_{rt}$ applied by the left-arm robot and the right-arm robot to the workpiece are $\sqrt{f_{lnx}^2+f_{lny}^2}$ and $\sqrt{f_{rnx}^2+f_{rny}^2}$, respectively. The maximum static friction force $F_{lf}$ between the workpiece and the left-arm robot is $\mu f_{lnz}$. The maximum static friction force $F_{rf}$ between the workpiece and the right-arm robot is $\mu f_{rnz}$. It can be obtained from Figure 4 that the maximum values $F_{lt}^{max}$ and $F_{rt}^{max}$ of tangential force are 8.712 × 10⁻⁴ N and 4.488 × 10⁻⁴ N, respectively. The minimum values $F_{lf}^{min}$ and $F_{rf}^{min}$ of maximum static friction force are 4.999 × 10⁻¹ N and 7.308 × 10⁻¹ N, respectively. Therefore, $F_{lt}^{max}<F_{lf}^{min}$ and $F_{rt}^{max}<F_{rf}^{min}$. That is, the friction cone constraint (6) is always satisfied during the transport operation. The dual-arm robot can always grasp the workpiece stably to ensure that it will not slip or fall off.

5.1.2. Comparative Simulation Results of Dual-Arm Robot Coordination Control with Straight Line + Circular Arc Trajectory

The desired terminal trajectory of the dual-arm robot can be determined by kinematic constraints [31,32] and the desired trajectory of the workpiece centroid. In light of the force distribution results and the desired terminal trajectory of the dual-arm robot, the simulations can be carried out by adopting the above three control methods. Then their terminal position accuracy and terminal force accuracy are compared.

The absolute values of their terminal position, force, and synchronization errors are shown in Figure 6, Figure 7, Figure 8, Figure 9, Figure 10, Figure 11 and Figure 12. The maximum and average values of the position error $\left|{\mathit{e}}_{\mathit{P}}\right|$, force error $\left|{\mathit{e}}_{\mathit{F}}\right|$, and synchronization error $\left|{\mathit{e}}_{\mathit{C}}\right|$ are shown in

Table 6. Maximum and average absolute values of the position and force errors of PCC in the numerical simulation.

		$\left|{\mathit{e}}_{\mathit{P}}\right|$ (m)			$\left|{\mathit{e}}_{\mathit{F}}\right|$ (N)
		X-Axis	Y-Axis	Z-Axis	X-Axis	Y-Axis	Z-Axis
Left-arm robot	Maximum	3.936 × 10−4	6.430 × 10−4	9.193 × 10−4	1.150 × 10−31	4.392 × 10−1	1.252
	Average	2.948 × 10−4	4.548 × 10−4	8.130 × 10−4	7.359 × 10−32	1.212 × 10−1	8.368 × 10−1
Right-arm robot	Maximum	4.191 × 10−4	6.559 × 10−4	9.138 × 10−4	1.150 × 10−31	4.804 × 10−1	1.229
	Average	3.009 × 10−4	4.686 × 10−4	8.104 × 10−4	7.359 × 10−32	6.038 × 10−2	8.630 × 10−1

,

Table 7. Maximum and average absolute values of the position and force errors of THFPC in the numerical simulation.

		$\left|{\mathit{e}}_{\mathit{P}}\right|$ (m)			$\left|{\mathit{e}}_{\mathit{F}}\right|$ (N)
		X-Axis	Y-Axis	Z-Axis	X-Axis	Y-Axis	Z-Axis
Left-arm robot	Maximum	4.492 × 10−4	2.795 × 10−2	3.075 × 10−2	1.150 × 10−31	3.116 × 10−3	3.557 × 10−3
	Average	3.039 × 10−4	8.724 × 10−3	1.744 × 10−2	7.359 × 10−32	8.454 × 10−5	1.331 × 10−3
Right-arm robot	Maximum	4.778 × 10−4	2.558 × 10−2	3.166 × 10−2	1.150 × 10−31	6.074 × 10−3	6.859 × 10−2
	Average	3.407 × 10−4	6.631 × 10−3	1.807 × 10−2	7.359 × 10−32	4.310 × 10−5	2.531 × 10−2

,

Table 8. Maximum and average absolute values of the position and force errors of WHFPFCC in the numerical simulation.

		$\left|{\mathit{e}}_{\mathit{P}}\right|$ (m)			$\left|{\mathit{e}}_{\mathit{F}}\right|$ (N)
		X-Axis	Y-Axis	Z-Axis	X-Axis	Y-Axis	Z-Axis
Left-arm robot	Maximum	1.899 × 10−4	4.288 × 10−4	7.919 × 10−4	1.150 × 10−31	2.684 × 10−3	2.031 × 10−3
	Average	1.583 × 10−4	3.428 × 10−4	5.080 × 10−4	7.359 × 10−32	4.109 × 10−5	7.322 × 10−4
Right-arm robot	Maximum	1.814 × 10−4	4.263 × 10−4	7.879 × 10−4	1.150 × 10−31	4.572 × 10−3	4.271 × 10−3
	Average	1.435 × 10−4	3.449 × 10−4	5.021 × 10−4	7.359 × 10−32	6.971 × 10−5	1.405 × 10−3

and

Table 9. Maximum and average absolute values of the synchronization errors of the three control methods in the numerical simulation.

		$\left|{\mathit{e}}_{\mathit{C}}\right|$ (m)
		X-Axis	Y-Axis	Z-Axis
PCC	Maximum	3.898 × 10−5	5.952 × 10−5	4.176 × 10−5
	Average	1.192 × 10−5	1.362 × 10−5	6.039 × 10−6
THFPC	Maximum	1.168 × 10−4	2.397 × 10−3	9.444 × 10−4
	Average	4.937 × 10−5	2.093 × 10−3	6.346 × 10−4
WHFPFCC	Maximum	5.796 × 10−5	5.690 × 10−5	5.118 × 10−5
	Average	1.668 × 10−5	7.447 × 10−6	1.274 × 10−5

.

For PCC, Table 6 indicates that its maximum and average position errors are both less than 10⁻³ m in the Y and Z directions. Therefore, PCC can maintain acceptable positional accuracy throughout the entire control process.

Its maximum and average force errors are less than 1 N in the Y direction. In the Z direction, its average force error is less than 1 N. However, its maximum force error is greater than 1 N. Therefore, PCC cannot always maintain acceptable force accuracy throughout the entire control process.

Table 9 and Figure 12a indicate that its maximum and average synchronization errors are less than 10⁻⁴ m. Therefore, PCC can ensure the synchronization performance throughout the entire control process.

For THFPC, Table 7 indicates that its maximum and average position errors are both greater than 10⁻³ m in the Y and Z directions. Therefore, THFPC cannot maintain acceptable positional accuracy throughout the entire control process.

Its maximum and average force errors are both less than 1 N in the Y and Z directions. Therefore, THFPC can maintain acceptable force accuracy throughout the entire control process.

Table 9 and Figure 12b indicate that its maximum and average synchronization errors are greater than 10⁻³ m in the Y direction. Therefore, THFPC cannot ensure the synchronization performance throughout the entire control process.

For WHFPFCC, Table 8 indicates that its maximum and average position errors are both less than 10⁻³ m in the Y and Z directions. Therefore, WHFPFCC can maintain acceptable positional accuracy throughout the entire control process.

Its maximum and average force errors are both less than 1 N in the Y and Z directions. Therefore, WHFPFCC can maintain acceptable force accuracy throughout the entire control process.

Table 9 and Figure 12c indicate that its maximum and average synchronization errors are less than 10⁻⁴ m. Therefore, WHFPFCC can ensure the synchronization performance throughout the entire control process.

Figure 6a and Figure 7a indicate that the position errors of PCC reach a stable state at 1.2 s and will fluctuate or mutate due to trajectory switching at 8 s. After 8 s, its position errors show a slight increasing trend. In contrast, Figure 10a and Figure 11a indicate that the position errors of WHFPFCC show a decreasing trend from 1.2 to 8 s and will fluctuate or mutate due to trajectory switching at 8 s. After 8 s, its position errors also show a decreasing trend. Therefore, WHFPFCC can effectively improve the position accuracy.

Figure 8b and Figure 9b indicate that the force errors of THFPC show an increasing trend before 8 s. After 8 s, its force errors show a decreasing trend. In contrast, Figure 10b and Figure 11b indicate that the force errors of WHFPFCC show a decreasing trend from 1.2 to 8 s and after 8 s. Moreover, Table 7 and Table 8 indicate that the maximum and average force errors of WHFPFCC are both less than those of THFPC. Therefore, WHFPFCC can effectively improve the force accuracy.

In addition, the synchronization errors of WHFPFCC are slightly larger than those of PCC. The cause of the above phenomena is described as follows. Compared with the position errors, the synchronization errors are smaller. According to the designed fuzzy rules, the synchronization error gains will be reduced. This results in a small increase in the synchronization errors. However, the increased synchronization errors are still acceptable and do not affect the coordination performance.

To sum up, PCC can only ensure the position accuracy and synchronization performance but cannot ensure the force accuracy. THFPC can only ensure the force accuracy but cannot ensure the position accuracy and synchronization performance. In contrast, WHFPFCC can ensure the position accuracy, the force accuracy, and synchronization performance simultaneously.

5.2. Comparative Simulation Case: Complex Trajectories

It is assumed that the overall dimensions of the workpiece are the same as the ones in Section 5.1. Its material is bronze, and its mass is 1.384 kg. The end effector of the dual-arm robot is a two-finger gripper. Its material is plastic. The friction coefficient $\mu$ between the workpiece and the end effector is 0.23 [38]. The desired trajectories of the workpiece centroid in the X, Y, and Z directions are designed as quintic polynomial trajectories, as described in Equation (44). Initial and final conditions of the transport operation are shown in

Table 10. Initial and final conditions of the transport operation.

	Initial			Final
	Position (m)	Velocity (m/s)	Acceleration (m/s2)	Position (m)	Velocity (m/s)	Acceleration (m/s2)
X-axis	0	0	0	0.04	0	0
Y-axis	0	0	0	−0.06	0	0
Z-axis	0	0	0	−0.08	0	0

.

$$\begin{array}{cc}\begin{array}{c}{P}_{txd}=a_0^x+a_1^{x}t+a_2^x{t}^2+a_3^x{t}^3+a_4^x{t}^4+a_5^x{t}^5\\ P_{tyd}=a_0^y+a_1^{y}t+a_2^y{t}^2+a_3^y{t}^3+a_4^y{t}^4+a_5^y{t}^5\\ P_{tzd}=a_0^z+a_1^{z}t+a_2^z{t}^2+a_3^z{t}^3+a_4^z{t}^4+a_5^z{t}^5\end{array}& 0<t\le 4\end{array},$$

(44)

where $a_0^x,a_1^x,a_2^x,a_3^x,a_4^x,a_5^x,a_0^y,a_1^y,a_2^y,a_3^y,a_4^y,a_5^y,a_0^z,a_1^z,a_2^z,a_3^z,a_4^z,a_5^z$ are polynomial coefficients and can be calculated by the initial and final conditions, and $P_{txd}$ is the desired position of the workpiece centroid in the X direction.

WHFPFCC is adopted for the transport operation of the dual-arm robot under two scenarios: without external disturbances and with external disturbances. It is assumed that the external disturbance acting on the dual-arm robot from 2 to 2.5 s can be equivalently decomposed into the force ${\mathit{F}}_{\mathit{l}}^{\mathit{D}}={\left[\begin{array}{ccc}0.01G_z& 0.03G_z& 0.06G_z\end{array}\right]}^T$ acting on the left-arm robot and the force ${\mathbf{F}}_{\mathbf{r}}^{\mathbf{D}}={\left[\begin{array}{ccc}0.02G_z& 0.04G_z& 0.08G_z\end{array}\right]}^T$ acting on the right-arm robot. The properties and discrepancies of the two scenarios are analyzed according to the simulation results.

5.2.1. Force Distribution Analysis of Dual-Arm Robots with Complex Trajectories on the Basis of Dynamic Force Closure

The desired generalized force applied by the dual-arm robot to the workpiece can be determined by the Newton–Euler equation. The desired generalized force applied by the workpiece to the dual-arm robot can be obtained by force closure dynamic constraints and Newton’s third law, as shown in Figure 13.

According to Equation (6), it can be calculated from Figure 13 that the maximum value $F_{lt}^{max}$ and $F_{rt}^{max}$ of tangential force are 2.37 × 10⁻² N and 1.68 × 10⁻² N, respectively. The minimum values $F_{lf}^{min}$ and $F_{rf}^{min}$ of maximum static friction force are 1.3199 N and 1.8485 N, respectively. Therefore, $F_{lt}^{max}<F_{lf}^{min}$ and $F_{rt}^{max}<F_{rf}^{min}$. That is, the friction cone constraint (6) is always satisfied during the transport operation. The dual-arm robot can always grasp the workpiece stably to ensure that it will not slip or fall off.

5.2.2. Comparative Simulation Results of Dual-Arm Robot Coordination Control with Complex Trajectories

The desired terminal trajectory of the dual-arm robot can be determined by kinematic constraints [31,32] and the desired trajectory of the workpiece centroid. In light of the force distribution results and the desired terminal trajectory of the dual-arm robot, the simulations can be carried out under the above two scenarios. Then their terminal position accuracy and terminal force accuracy can be compared.

The absolute values of their terminal position, force, and synchronization errors are shown in Figure 14, Figure 15, Figure 16, Figure 17 and Figure 18. The maximum and average values of the position error $\left|{\mathit{e}}_{\mathit{P}}\right|$, force error $\left|{\mathit{e}}_{\mathit{F}}\right|$, and synchronization error $\left|{\mathit{e}}_{\mathit{C}}\right|$ are shown in

Table 11. Maximum and average absolute values of the position and force errors without external disturbances.

		$\left|{\mathit{e}}_{\mathit{P}}\right|$ (m)			$\left|{\mathit{e}}_{\mathit{F}}\right|$ (N)
		X-Axis	Y-Axis	Z-Axis	X-Axis	Y-Axis	Z-Axis
Left-arm robot	Maximum	1.709 × 10−4	8.026 × 10−4	5.745 × 10−4	4.321 × 10−3	2.254 × 10−3	4.831 × 10−3
	Average	9.956 × 10−5	3.507 × 10−4	2.899 × 10−4	5.521 × 10−4	6.707 × 10−4	2.075 × 10−3
Right-arm robot	Maximum	1.663 × 10−4	7.232 × 10−4	5.501 × 10−4	1.765 × 10−3	2.821 × 10−3	4.661 × 10−3
	Average	9.812 × 10−5	3.458 × 10−4	3.150 × 10−4	7.274 × 10−4	5.201 × 10−4	1.184 × 10−3

,

Table 12. Maximum and average absolute values of the position and force errors with external disturbances.

		$\left|{\mathit{e}}_{\mathit{P}}\right|$ (m)			$\left|{\mathit{e}}_{\mathit{F}}\right|$ (N)
		X-Axis	Y-Axis	Z-Axis	X-Axis	Y-Axis	Z-Axis
Left-arm robot	Maximum	3.274 × 10−4	8.026 × 10−4	5.745 × 10−4	4.321 × 10−3	2.254 × 10−3	4.831 × 10−3
	Average	1.173 × 10−4	3.676 × 10−4	2.987 × 10−4	8.237 × 10−4	7.808 × 10−4	2.110 × 10−3
Right-arm robot	Maximum	3.773 × 10−4	7.232 × 10−4	5.501 × 10−4	2.584 × 10−3	2.821 × 10−3	4.661 × 10−3
	Average	1.226 × 10−4	3.629 × 10−4	3.234 × 10−4	7.760 × 10−4	5.626 × 10−4	1.196 × 10−3

and

Table 13. Maximum and average absolute values of the synchronization errors under the two scenarios.

		$\left|{\mathit{e}}_{\mathit{C}}\right|$ (m)
		X-Axis	Y-Axis	Z-Axis
Without external disturbances	Maximum	1.323 × 10−4	3.865 × 10−4	2.225 × 10−4
	Average	5.746 × 10−5	5.801 × 10−5	4.385 × 10−5
With external disturbances	Maximum	2.169 × 10−4	3.865 × 10−4	2.225 × 10−4
	Average	6.764 × 10−5	6.285 × 10−5	4.395 × 10−5

.

Table 11, Table 12 and Table 13 indicate that maximum and average position errors under the two scenarios are both less than 10⁻³ m. Maximum and average force errors under the two scenarios are both less than 1 N. Maximum and average synchronization errors under the two scenarios are both less than 10⁻³ m. Therefore, WHFPFCC under the two scenarios can maintain acceptable positional accuracy, force accuracy, and synchronization performance throughout the entire control process.

Figure 14, Figure 15, Figure 16, Figure 17 and Figure 18 indicate that, compared with WHFPFCC without external disturbances, position errors, force errors, and synchronization errors of WHFPFCC with external disturbances will fluctuate or mutate from 2 to 2.5 s due to external disturbances. However, the increased errors are still acceptable and do not affect the control performance.

To sum up, different trajectories, different loads, and external disturbances would not affect the control accuracy and coordination performance of WHFPFCC. WHFPFCC has strong robustness.

6. Physical Experiments

To verify the practical performance of WHFPFCC, the Baxter dual-arm robot built by Rethink Robotics (Hebron, KY, USA) is chosen as the experimental subject. The actual experimental environment is constructed according to the simulation environment in Section 5.1, as show in Figure 19. The desired terminal trajectory and the desired generalized force of the dual-arm robot are the same as the ones in Section 5.1.

The workpiece is a slender rod. Its weight is within the bearing range of a single-arm robot. However, only in the case that the grasping position of the gripper is at the workpiece centroid, the workpiece centroid will remain horizontal in the contact frame (that is, the axis of the workpiece is parallel to the fingers of the gripper). And if the workpiece is affected by external disturbance, the horizontal state is difficult to maintain. In the case that the grasping position of the gripper deviates from the workpiece centroid, the workpiece will have a certain inclination. And the farther the grasping position of the gripper deviates from the workpiece centroid, the greater the inclination angle of the workpiece. If subsequent assembly operations are required, such as placing the workpiece in the corresponding groove, the workpiece may not fit with the groove when the workpiece is in the inclined state. The above problems can be effectively solved by the cooperative gripping of the dual-arm robot. In this situation, the workpiece always maintains a horizontal state in the contact frame, and it is not easily affected by external disturbances.

Table 7 indicates that maximum and average position errors of THFPC are both greater than 10⁻³ m in the Y and Z direction. It means that if THFPC is adopted for coordination control of the dual-arm robot, the closed-chain structure of tight coordination operation would be destroyed. Therefore, THFPC should not be adopted for the physical experiment. Only PCC and WHFPFCC are adopted for the comparative experiment. The absolute values of their terminal position, force and synchronization errors are shown in Figure 20, Figure 21, Figure 22, Figure 23 and Figure 24. The maximum and average values of the position error $\left|{\mathit{e}}_{\mathit{P}}\right|$, force error $\left|{\mathit{e}}_{\mathit{F}}\right|$ and synchronization error $\left|{\mathit{e}}_{\mathit{C}}\right|$ are shown in

Table 14. Maximum and average absolute values of the position and force errors of PCC in the physical experiment.

		$\left|{\mathit{e}}_{\mathit{P}}\right|$ (m)			$\left|{\mathit{e}}_{\mathit{F}}\right|$ (N)
		X-Axis	Y-Axis	Z-Axis	X-Axis	Y-Axis	Z-Axis
Left-arm robot	Maximum	4.250 × 10−4	7.786 × 10−4	9.722 × 10−4	3.177 × 10−1	4.455 × 10−1	1.245
	Average	3.113 × 10−4	5.059 × 10−4	7.880 × 10−4	1.038 × 10−1	2.153 × 10−1	3.727 × 10−1
Right-arm robot	Maximum	4.484 × 10−4	6.943 × 10−4	9.714 × 10−4	5.905 × 10−1	5.024 × 10−1	1.257
	Average	3.189 × 10−4	5.253 × 10−4	7.547 × 10−4	1.234 × 10−1	2.193 × 10−1	3.616 × 10−1

,

Table 15. Maximum and average absolute values of the position and force errors of WHFPFCC in the physical experiment.

		$\left|{\mathit{e}}_{\mathit{P}}\right|$ (m)			$\left|{\mathit{e}}_{\mathit{F}}\right|$ (N)
		X-Axis	Y-Axis	Z-Axis	X-Axis	Y-Axis	Z-Axis
Left-arm robot	Maximum	2.933 × 10−4	4.415 × 10−4	8.761 × 10−4	9.655 × 10−2	2.458 × 10−1	2.705 × 10−1
	Average	1.843 × 10−4	3.333 × 10−4	4.891 × 10−4	2.560 × 10−2	9.808 × 10−2	6.756 × 10−2
Right-arm robot	Maximum	2.778 × 10−4	4.418 × 10−4	8.748 × 10−4	1.176 × 10−1	2.222 × 10−1	2.490 × 10−1
	Average	1.767 × 10−4	3.360 × 10−4	5.225 × 10−4	4.428 × 10−2	1.040 × 10−1	6.999 × 10−2

and

Table 16. Maximum and average absolute values of the synchronization errors of the two control methods in the physical experiment.

		$\left|{\mathit{e}}_{\mathit{C}}\right|$
		X-Axis	Y-Axis	Z-Axis
PCC	Maximum	1.569 × 10−4	2.023 × 10−4	1.833 × 10−4
	Average	3.754 × 10−5	5.063 × 10−5	4.921 × 10−5
WHFPFCC	Maximum	2.145 × 10−4	1.858 × 10−4	1.951 × 10−4
	Average	4.433 × 10−5	3.745 × 10−5	6.019 × 10−5

.

Table 14 and Table 15 indicate that maximum and average position errors of the two control methods are both less than 10⁻³ m. Maximum and average force errors of WHFPFCC are less than 1 N. However, the maximum force error of PCC is greater than 1 N in the Z direction. Table 16 and Figure 24 indicate that the maximum and average synchronization errors of the two control methods are both less than 10⁻³ m and not much different. Hence, PCC can only ensure the position accuracy but cannot ensure the force accuracy. In contrast, WHFPFCC can ensure both the position accuracy and the force accuracy and has good coordination ability.

Figure 20, Figure 21, Figure 22 and Figure 23 indicate that, for the two control methods, the variation trend of the actual control errors is basically the same as the variation trend of the simulation control errors, and the actual steady-state errors are close to the simulation steady-state errors. Hence, WHFPFCC can effectively realize the coordination control of the dual-arm robot and enhance control precision.

7. Conclusions

To enhance the stability and coordination property of dual-arm robot cooperative operations, a force-closure-based weighted hybrid force/position fuzzy coordination control method is proposed for dual-arm robots. Force-closure dynamic constraints are established to improve the stability of grasping for preventing the workpiece from slipping. To reduce the energy consumption of cooperative operation, the minimum energy consumption principle is adopted to distribute the contact force and joint torques of the dual-arm robot. To simultaneously ensure positional accuracy, force accuracy, and coordination performance, the weighted hybrid force/position control is combined with synchronous control. To enhance the adaptability, a fuzzy controller is designed to simultaneously tune the gains of position control, force control, and synchronization control. Simulation and experimental results demonstrate that, compared with the position coordination control method and the traditional hybrid force/position control, the weighted hybrid force/position fuzzy coordination control method can effectively ensure the position accuracy, force accuracy, and synchronization performance. Moreover, it can reduce the position and force control errors. For different trajectories, different loads, and external disturbances, it has strong robustness. Hence, the proposed method can effectively enhance the overall performance and operational ability of dual-arm robots.

To ensure real-time performance, the optimal force distribution is preprocessed offline. It generates the desired contact force before the control begins. In the future, to further improve the robustness, the optimal force distribution will be processed online. Additionally, it will be combined with model linearization, parallel computing, or other methods to enhance the real-time performance of online processing.