Research on Collaboration Motion Planning Method for a Dual-Arm Robot Based on Closed-Chain Kinematics

Abstract

Aiming to address challenges in the motion coordination of dual-arm robot engineering applications, a comprehensive set of planning methods is devised. This paper takes a dual-arm system composed of two six-degrees-of-freedom industrial robots as the research object. Initially, a transformation model is established for the characteristic trajectories between the workpiece coordinate system and various other coordinate systems. Subsequently, the position and orientation curves of the working trajectory are discretized to facilitate the controller’s execution. Furthermore, an analysis is conducted of the closed-chain kinematics relationship between two arms of the robot and a pose-calibration method based on a reference coordinate system is introduced. Finally, constraints to the collaborative motion of the dual-arm robot are analyzed, leading to the establishment of a motion collaboration planning methodology. Simulations and experiments demonstrate that the proposed approach enables effective and collaborative task planning for dual-arm robots. Moreover, joint angle and angular velocity curves corresponding to the motion trajectory exhibit smoothness, reducing joint impacts.

1. Introduction

With the development of science and technology and the advancement of engineering practices, application scenarios for dual-arm robots are expanding [1,2,3,4,5]. There is increasing focus on the intricate spatial maneuvers achievable by dual-arm robots. Research on these dual-arm systems holds significant importance for enhancing the performance of future robotic platforms.

Research on the coordinated motion of dual-arm robots remains relatively scarce, but recent studies have shed light on the subject. Here is a summary of the current research: Shaoying He [3] introduced timing and prediction controllers to the trajectory planning of a gantry-type dual-arm robot. This approach significantly reduced the programming workload during operations, demonstrating effective motion planning for gantry welding robots. David C [6] categorized dual-arm robots into master–slave motion modes, establishing closed-chain kinematics constraints between the two arms. Offline simulations validated the feasibility and convenience of this method. Xuemei Liu [4] investigated collaborative welding involving three robots in a gantry frame scenario. Kinematics relationships were established among the robots during collaborative movements, assigning picking tasks to the master arm and welding tasks to the slave arm. Feasibility was confirmed through Adams simulations. Avishai S [7] investigated the sampling-based motion planning method for dual-arm robots under closed-chain kinematics, validating its effectiveness through experiments. Riboli M [8] tackled task assignment and motion planning for dual-arm robots in space cooperation settings, employing photoelectric sensors to prevent arm collisions. Jiahao Xiong [9] addressed buffeting issues in dual-arm collaboration welding by introducing a smoothness function. Several welding tracks were evaluated, leading to the selection of the most stable option. Meng Shi [10] explored coupling and superposition motions between arms in dual-arm robot collaboration. A multi-robot trajectory coordination teaching method was proposed and validated through experiments. Yangqiang Hou [11] optimized the collaborative trajectories of a dual-arm robot using a genetic algorithm, selecting the smoothest welding trajectory among various positions and paths. Qichao Tang [12] introduced a novel, discrete, multi-objective, cross-entropy optimization algorithm (CrMOCEO) for dual-robot collaborative arc welding-path-planning, demonstrating its feasibility and effectiveness via simulations. Lei Wang [13] investigated centralized, decentralized, and distributed control approaches for dual-arm robots. Centralized control eliminated communication delays between robots, ensuring effective cooperation. Jens F [5] integrated a dual-arm collaborative robot system in a smart factory and an Industry 4.0 context, with a particular focus on solving complex industrial disassembly tasks. However, this approach depends heavily on the assistance of various high-precision vision sensors and may not be conducive to low-cost or large-scale industrial applications. These studies collectively contribute to advancing the understanding and application of dual-arm robot motion coordination, offering insights into various control strategies and optimization techniques.

Currently, there is limited research on the collaboration motion planning of dual-arm robots, and few articles provide a comprehensive framework for such coordination. The primary contributions of this paper are as follows: establishing a closed-chain kinematics model for dual-arm robots (composed of two six-degrees-of-freedom industrial robots), investigating trajectory planning for the robot, and exploring methods for planning coordinated motion tasks for the two arms. The experimental results demonstrate that the motion planning scheme proposed in this paper effectively coordinates the motion of the dual-arm robot. Specifically, this paper achieves the collaborative motion of the dual arms by integrating a feature trajectory extraction of the workpiece with closed-chain kinematics. This approach simplifies the task-planning processes for dual-arm robots and lays a theoretical foundation for further research on the collaborative control of such robots.

2. Construction of a Dual-Arm Robot Model

2.1. Kinematics Model

The standard Denavit–Hartenberg (D–H) method was utilized to establish the coordinate system for the robot’s links. Two six-degrees-of-freedom industrial robots served as the research subject. Following the defined rules, the D–H parameters for the robot are presented in

Table 1. D–H parameters of the six-degrees-of-freedom industrial robot.

Link Number $\mathit{i}$	${\mathit{a}}_{\mathit{i}}$/mm	${\mathit{\alpha }}_{\mathit{i}}$/rad	${\mathit{d}}_{\mathit{i}}$/mm	${\mathit{\theta }}_{\mathit{i}}$/rad
1	350	$-\pi /2$	684	${\theta }_1\left(\pi /2\right)$
2	1200	0	0	${\theta }_2\left(-\pi /2\right)$
3	0	$-\pi /2$	0	${\theta }_3\left(0\right)$
4	0	$\pi /2$	1050	${\theta }_4\left(0\right)$
5	0	$\pi /2$	0	${\theta }_5\left(\pi /2\right)$
6	0	0	0	${\theta }_6\left(-\pi /2\right)$

, while Figure 1 illustrates the joint coordinate systems of the robot.

The homogeneous transformation matrix between the adjacent coordinate systems of the robots is as follows:

$${}_i{}^{i-1}T=\left[\begin{array}{cccc}\cos \left({\theta }_i\right)& -\cos \left({\alpha }_i\right)\sin \left({\theta }_i\right)& \sin \left({\alpha }_i\right)\sin \left({\theta }_i\right)& a_i\cos \left({\theta }_i\right)\\ \sin \left({\theta }_i\right)& \cos \left({\alpha }_i\right)& -\sin \left({\alpha }_i\right)\cos \left({\theta }_i\right)& a_i\sin \left({\theta }_i\right)\\ 0& \sin \left({\alpha }_i\right)& \cos \left({\alpha }_i\right)& d_i\\ 0& 0& 0& 1\end{array}\right]$$

(1)

2.2. Closed-Chain Kinematics of Dual-Arm Robots

To solve the inverse kinematics problem of dual-arm robots, understanding the pose relationship of each intermediate node between the two robots is crucial. The robot holding the workpiece is designated as the master robot (number 1), while the robot holding the actuator is designated as the slave robot (number 2). The coordinate system is defined as follows:

• The world coordinate system, denoted as $\left\{W\right\}$, typically shares the same origin and orientation as the master robot’s base coordinate system;
• The base coordinate system of the master robot is defined as $\left\{B1\right\}$, while the base coordinate system of the slave robot is defined as $\left\{B2\right\}$;
• The master robot end coordinate system is defined as $\left\{E1\right\}$, and the slave robot end coordinate system is defined as $\left\{E2\right\}$;
• The coordinate system of the actuator is denoted as $\left\{TG\right\}$, while the working position coordinate system of the workpiece is denoted as $\left\{WP\right\}$. Typically, the origin of the working position coordinate system $\left\{WP\right\}$ aligns with the origin of the rotating center coordinate system $\left\{CP\right\}$. Additionally, the transformation coordinate system $\left\{FP\right\}$ of the workpiece and the transfer flange coincides with the base coordinate system $\left\{BP\right\}$ of the workpiece, as illustrated in Figure 2.

The kinematics chain relationship between the master arm and slave arm of dual-arm robot can be analyzed. The kinematics relationship of the master arm is as follows:

$${}^{W}T_{Pj}={}^{W}T_{E1}{}^{E1}T_{FP}{}^{FP}T_{CP}{}^{CP}T_{P_j}$$

(2)

Once the working position $\left\{WP\right\}$ is determined, the conversion relationship between the master arm’s base coordinate system and the working position can be calculated using the following formula:

$${}^{W}T_{Pj}={}^{W}T_{WP}{}^{WP}T_{CP}{}^{CP}T_{Pj}$$

(3)

The simultaneous equations yield the following expression:

$${}^{W}T_{E1}={}^{W}T_{WP}{}^{WP}T_{CP}{}^{CP}T_{FP}{}^{FP}T_{E1}$$

(4)

In the equations above, the attitude transformation matrix ${}^{W}T_{WP}$ from the world coordinate system to the working position, the transformation matrix ${}^{WP}T_{WP}$ from the working position to the rotation center, the transformation matrix ${}^{CP}T_{FP}$ from the rotation center to the transfer, and the transformation matrix ${}^{FP}T_{E1}$ from the transfer flange to the end of the master robot are provided. From these matrices, we can obtain the end rotation transformation matrix and the inverse solution for each joint of the master robot. The kinematics relationship of the slave arm is as follows:

$${}^{W}T_{Pj}={}^{W}T_{B2}{}^{B2}T_{E2}{}^{E2}T_{TG}{}^{TG}T_{P_j}$$

(5)

The simultaneous equations yield the following expression:

$${}^{B2}T_{E2}={}^{B2}T_W{}^{W}T_{WP}{}^{WP}T_{CP}{}^{CP}T_{P_j}{}^{P_j}T_{TG}{}^{TG}T_{E2}$$

(6)

In the provided formula, ${}^{B2}T_W$ denotes the pose transformation matrix of the world coordinate system under the base coordinate system of the slave robot. This matrix is linked to the positional relationship between the master and slave robot and can be established through calibration. ${}^{P_j}T_{TG}$ represents the transformation of the coordinate system from a discrete point to the end of the actuator. The transformation relationship depends on the specific working attitude, and different working modes may lead to different transformation matrices. ${}^{TG}T_{E2}$ signifies the installation relationship between the actuator and the end flange of the robot. This relationship is associated with the selection and installation method of the actuator and is a known quantity. From these parameters, the inverse kinematics of the robot can be calculated.

2.3. Calibration of the Base Coordinate Systems for Dual-Arm Robots

In the preceding section, the kinematics relationship of the dual-arm robots is derived. Within this context, calibration of the positional relationship between the master robot and the slave robot becomes necessary. This calibration involves establishing the relative pose relationship between the master robot and the base coordinates of the slave robot. This is achieved through the spatial three-point method [14,15].

The teaching mode is utilized to adjust the movement of the master robot within the working space with two arms, aiming to reach the three non-collinear points $P_1$, $P_2$, and $P_3$. Subsequently, the slave robot is adjusted so that its end effector aligns with that of the master robot, touching points $P_1$, $P_2$, and $P_3$ sequentially, as depicted in Figure 3. The joint angles of both the master and slave robot at these three points are recorded, as shown in Equation (7). Here, ${}^M\theta _{P1}$ represents the six joint angles of the master robot at point $P_1$, while ${}^S\theta _{P1}$ represents the six joint angles of the slave robot at point $P_1$.

$$\begin{array}{l}{}^M\theta _{P1}=\left\{\begin{array}{llllll}{}^M\theta _{P11}\hfill & {}^M\theta _{P12}\hfill & {}^M\theta _{P13}\hfill & {}^M\theta _{P14}\hfill & {}^M\theta _{P15}\hfill & {}^M\theta _{P16}\hfill \end{array}\right\}\\ {}^M\theta _{P2}=\left\{\begin{array}{llllll}{}^M\theta _{P21}\hfill & {}^M\theta _{P22}\hfill & {}^M\theta _{P23}\hfill & {}^M\theta _{P24}\hfill & {}^M\theta _{P25}\hfill & {}^M\theta _{P26}\hfill \end{array}\right\}\\ {}^M\theta _{P3}=\left\{\begin{array}{llllll}{}^M\theta _{P31}\hfill & {}^M\theta _{P32}\hfill & {}^M\theta _{P33}\hfill & {}^M\theta _{P34}\hfill & {}^M\theta _{P35}\hfill & {}^M\theta _{P36}\hfill \end{array}\right\}\\ {}^S\theta _{P1}=\left\{\begin{array}{llllll}{}^S\theta _{P11}\hfill & {}^S\theta _{P12}\hfill & {}^S\theta _{P13}\hfill & {}^S\theta _{P14}\hfill & {}^S\theta _{P15}\hfill & {}^S\theta _{P16}\hfill \end{array}\right\}\\ {}^S\theta _{P2}=\left\{\begin{array}{llllll}{}^S\theta _{P21}\hfill & {}^S\theta _{P22}\hfill & {}^S\theta _{P23}\hfill & {}^S\theta _{P24}\hfill & {}^S\theta _{P25}\hfill & {}^S\theta _{P26}\hfill \end{array}\right\}\\ {}^S\theta _{P3}=\left\{\begin{array}{llllll}{}^S\theta _{P31}\hfill & {}^S\theta _{P32}\hfill & {}^S\theta _{P33}\hfill & {}^S\theta _{P34}\hfill & {}^S\theta _{P35}\hfill & {}^S\theta _{P36}\hfill \end{array}\right\}\end{array}$$

(7)

Through the forward kinematics model of the master robot, the position coordinates of points $P_1$, $P_2$, and $P_3$ in the master robot’s base coordinate system $\left\{B1\right\}$ can be calculated. Similarly, the position coordinates in the slave robot’s base coordinate system $\left\{B2\right\}$ can also be calculated. The unit vector in the $\overrightarrow{P_1{P}_2}$ direction is designated the $x$ axis of the calibration coordinate system $\left\{cal\right\}$, while the unit vector in the direction of $\overrightarrow{P_1{P}_2}\times \overrightarrow{P_1{P}_3}$ is designated the $z$ axis of the calibration coordinate system $\left\{cal\right\}$. The $y$ axis is determined using the vector cross-product $z\times x$.

Through the aforementioned calculations, matrix ${}^{B1}T_{cal}$ can be derived. Similarly, matrix ${}^{B2}T_{cal}$ can be computed with the base coordinate system $\left\{B2\right\}$ as the reference. Subsequently, the closed-chain relationship between the kinematics is as follows:

$${}^{B1}T_{cal}{}^{cal}T_{B2}{}^{B2}T_{B1}=I$$

(8)

This is achieved by right-multiplying the inverse transformation matrix:

$${}^{B1}T_{B2}={}^{B1}T_{cal}{\left({}^{B2}T_{cal}\right)}^{-1}$$

(9)

Both terms on the right side of the above equation can be solved using forward kinematics. Subsequently, the pose transformation from coordinate system $\left\{B1\right\}$ to coordinate system $\left\{B2\right\}$ can be obtained.

3. Construction of the Dual-Arm Robot Model

There are two challenges involved in analyzing the collaborative motion of dual-arm robots: (1) selecting the midpoint for collaborative motion between the two arms and dynamically planning the entire task trajectory; (2) effectively combining the task constraints and work-point selection. This chapter delves into a detailed analysis of these two issues and ultimately proposes a general method for dual-arm collaborative planning.

3.1. Workpiece Model

Using the welding of a workpiece in engineering as an example, a method for collaboration between dual-arm robots is proposed. Typically, in industrial settings, the dual-arm robot holds the workpiece with its master arm while gripping the actuator in the slave arm. Motion cooperation is achieved by altering the position and posture of both the workpiece and the actuator. This enables effective welding operations.

The geometric dimensions of the workpiece are measured using precision measuring instruments [16,17]. The physical size of the workpiece is illustrated in Figure 4a, while its three-dimensional representation is depicted in Figure 4b.

The workpiece and the robot’s end effector are connected via a transfer flange. When solving the inverse kinematics for the trajectory, the process involves first determining the pose of the trajectory in the rotation center coordinate system. Then, the relationship between the rotation center of the workpiece and the base coordinate system is established to solve the inverse joint solution for each discrete point. A coordinate system, as depicted in Figure 5, is established, with the discrete point coordinate system $\left\{P_{ij}\right\}$ located at the discrete point $j$ of trajectory $i$. This includes defining the workpiece rotation center coordinate system $\left\{CP\right\}$, the base coordinate system $\left\{BP\right\}$ for workpiece installation and the transfer flange, and the transfer flange coordinate system $\left\{FP\right\}$. The origin of $\left\{CP\right\}$ is positioned at the center of the ball, allowing the robot to grip the workpiece and rotate it around the center of the ball to adjust its working orientation.

The homogeneous pose conversion of discrete points along the workpiece track is outlined as follows:

$${}^{W}T_{Pij}={}^{W}T_B{}^{B}T_{FP}{}^{FP}T_{BP}{}^{BP}T_{CP}{}^{CP}T_{Pij}$$

(10)

where $\left\{W\right\}$ denotes the world coordinate system, $\left\{B\right\}$ represents the base coordinate system of the workpiece-holding robot, and $\left\{{}^{W}T_{Pij}\right\}$ signifies the homogeneous transformation matrix of the master robot from the world coordinate system to discrete points along the trajectory.

3.2. Feature Extraction of Workpiece Model

In the workpiece coordinate system established above, the characteristic trajectory equations are analyzed to facilitate trajectory discretization in subsequent steps. Workpiece trajectory forms are generally categorized as saddle curves, arc curves, and elliptic curves. In this paper, the focus is placed on saddle curves and arc curves. The rotation center coordinate system $\left\{CP\right\}$ is defined within the workpiece coordinate system, and is specifically denoted as the spherical coordinate system. The origin coordinate system of the circular curve auxiliary tube cylinder is denoted as $\left\{C_1\right\}$, as illustrated in Figure 6.

In Figure 6, the $x$ axis of the tubular coordinate system $\left\{C_1\right\}$ is aligned with the $x$ axis of the spherical coordinate system $\left\{CP\right\}$. The origin of the tubular coordinate system $\left\{C_1\right\}$ is determined by translating the eccentricity $e$ along the $x$ axis and rotating angle $\alpha$ counterclockwise around the $x$ axis. Consequently, the parametric equation of the arc trajectory is obtained [18,19]. ${}^{CP}P_{ij}$ and ${}^{C_1}P_{ij}$ represent the coordinate representation of discrete point $j$ of trajectory $i$ in the master coordinate system $\left\{CP\right\}$ and the branch coordinate system $\left\{C_1\right\}$, respectively. Through the transformation relationship between the coordinate systems and the model parameters, the parametric equation under the coordinate system $\left\{CP\right\}$ is obtained:

$$\left\{\begin{array}{l}{}^{CP}P_{ijx}=r\cos \beta +e\hfill \\ {}^{CP}P_{ijy}=r\sin \beta r\cos \alpha -{}^{C_1}P_{ijz}\sin \alpha \hfill \\ {}^{CP}P_{ijz}=r\sin \beta r\sin \alpha +{}^{C_1}P_{ijz}\cos \alpha \hfill \\ \beta \in \left(0,2\pi \right)\hfill \end{array}\right.$$

(11)

The saddle curve is created at the junction of the main pipe and the branch pipe, with the main pipe having the largest diameter and the branch pipe having the smallest diameter. Figure 7 illustrates the intersection diagram, defining the rotation center coordinate system $\left\{CP\right\}$ within the workpiece coordinate system. The origin coordinate system of the branch pipe of the saddle curve is denoted as $\left\{C_1\right\}$.

The $y$ axis of the branch coordinate system $\left\{C_1\right\}$ aligns with the $y$ axis of the master coordinate system $\left\{CP\right\}$. This alignment is achieved by shifting eccentricity $e$ of the master coordinate system $\left\{CP\right\}$ along the $y$ axis and then rotating angle $\alpha$ counterclockwise around the $y$ axis. Through this transformation, the parametric equation in the principal coordinate system $\left\{CP\right\}$ is derived:

$$\left\{\begin{array}{l}{}^{CP}P_{ijx}=r\cos \beta \cos \alpha +\sqrt{R^2-{\left(r\sin \beta +e\right)}^2}-r\cos \beta \cos \alpha \hfill \\ {}^{CP}P_{ijy}=r\sin \beta +e\hfill \\ {}^{CP}P_{ijz}=-r\cos \beta \sin \alpha +\frac{\left(\sqrt{R^2-{\left(r\sin \beta +e\right)}^2}-r\cos \beta \cos \alpha \right)\cos \alpha }{\sin \alpha }\hfill \\ \beta \in \left(0,2\pi \right)\hfill \end{array}\right.$$

(12)

3.3. Motion-Synchronized Trajectory Discretization

The spatial trajectory represents a continuous curve, while the instructions transmitted to the motor via the controller consist of scattered-point instructions. These instructions cannot be directly applied to the continuous curve. Therefore, the continuous curve needs to be discretized into a finite number of discrete points [20,21,22]. Subsequently, the inverse solution of the joint angles corresponding to each discrete point is determined individually. This process allows for the handover of instructions to the robot for execution. Additionally, besides controlling the motion trajectory of the actuator, it is also necessary to regulate the attitude of the actuator. Typically, the $z$ axis of the discrete point coordinate system $\left\{P_{ij}\right\}$ aligns with the actuator’s axis and maintains a certain height difference from the origin. The forward direction of the actuator coincides with the $x$ direction of the discrete point coordinate system. The discretization of spatial points involves six variables: three spatial position parameters $\left(x,y,z\right)$ and three spatial attitude parameters $\left(A,B,C\right)$.

The starting point coordinate of the linear trajectory is denoted as $P_s\left(x_s,y_s,z_s\right)$, the endpoint coordinate is denoted as $P_f\left(x_f,y_f,z_f\right)$, and the linear velocity at the end of the actuator is denoted as $V_L$. The movement time is represented by $t\in \left(0,t_f\right)$ and $t_f$ signifies the total travel time of the actuator along the linear trajectory. Therefore, the coordinate of discrete travel point $n$ in the middle of the linear trajectory is denoted as follows:

$$P_n=P_s+\frac{P_s{P}_f}{‖P_s{P}_f‖}{V}_{L}nT$$

(13)

where $T$ denotes the interpolation time period; this is simplified here as $x_n=P_x\left({\theta }_n\right),y_n=P_y\left({\theta }_n\right),z_n=P_z\left({\theta }_n\right)$. In this representation, ${\theta }_n\in \left({\theta }_s,{\theta }_f\right)$ and $\theta$ denote the independent variables in the parametric equation of the curve trajectory, while ${\theta }_n$ represents a specific point in the interpolation curve.

According to the parametric equation, the arc length $s_L$ of the curve is:

$$s_L={\displaystyle {\int }_{{\theta }_s}^{{\theta }_f}\sqrt{P_x^{\prime }{\left({\theta }_n\right)}^2+P_y^{\prime }{\left({\theta }_n\right)}^2+P_z^{\prime }{\left({\theta }_n\right)}^2}}d\theta$$

(14)

The differential of the arc length is:

$$d{s}_L=\sqrt{P_x^{\prime }{\left({\theta }_n\right)}^2+P_y^{\prime }{\left({\theta }_n\right)}^2+P_z^{\prime }{\left({\theta }_n\right)}^2}d\theta$$

(15)

The linear velocity of the actuator along the curve arc is:

$$V_s=\frac{d{s}_L}{dt}=\frac{d{s}_L}{d\theta }\frac{d\theta }{dt}=\sqrt{P_x^{\prime }{\left({\theta }_n\right)}^2+P_y^{\prime }{\left({\theta }_n\right)}^2+P_z^{\prime }{\left({\theta }_n\right)}^2}\dot{\theta }$$

(16)

The change rate of the parameter $\theta$ with respect to time is:

$$\dot{\theta }=\frac{V_s}{\sqrt{P_x^{\prime }{\left({\theta }_n\right)}^2+P_y^{\prime }{\left({\theta }_n\right)}^2+P_z^{\prime }{\left({\theta }_n\right)}^2}}$$

(17)

When the discrete time period is $T$, the coordinates of the $n+1$ discrete point on the curve trajectory are expressed as $x_{n+1}=P_x\left({\theta }_n\right)+\dot{\theta }T$, $y_{n+1}=P_y\left({\theta }_n\right)+\dot{\theta }T$, $z_{n+1}=P_z\left({\theta }_n\right)+\dot{\theta }T$, where ${\theta }_n\in \left({\theta }_s,{\theta }_f\right)$.

The saddle curve and arc curve are discretized, and simulations are conducted in MATLAB R2019b to validate the accuracy of the formulas and coordinate systems. The simulation parameters for the saddle curve and ball tube penetration are detailed in

Table 2. Parameters for the discrete simulation of curve trajectory poses.

Name	Main Pipe Radius $\mathit{R}$	Branch Pipe Radius $\mathit{r}$	Rotation Angle $\mathit{\alpha }$	Eccentricity $\mathit{e}$
Saddle curve	50 mm	45 mm	$\pi /2$ rad	0 mm
Arc curve	50 mm	20 mm	0 rad	0 mm

. In the $\left\{CP\right\}$ coordinate system, the position and orientation of the discrete curves are depicted in Figure 8a,b, respectively. In these figures, the $x$ axis of each discrete point aligns with the tangent direction of the curve, the $z$ axis direction points outward, and the $y$ axis direction points inward.

3.4. Collaborative Motion Constraint Analysis

By analyzing the cooperative motion of a dual-arm robot, the task constraints can be divided into two parts: the first part involves the pose constraint between the trajectory’s discrete points and the actuator, while the second part involves the position and pose constraint of the discrete points in space, as illustrated in Figure 9.

Constraint 1 in Figure 9 represents the pose constraint between the actuator $\left\{TG\right\}$ and the discrete point $\left\{P_j\right\}$ along the workpiece’s trajectory, expressed by matrix ${}^{P_j}T_{TG}$. This constraint varies depending on the specific task and is determined based on real-world conditions. Constraint 2, on the other hand, signifies the pose constraint between the trajectory discrete point $\left\{P_j\right\}$ and the world coordinate system $\left\{W\right\}$, denoted by matrix ${}^{W}T_{P_j}$. This constraint is associated with the planned working position of the workpiece, ensuring that the working position falls within the common working space of both arms.

Given the discrete attitude transformation matrix ${}^{CP}T_{P_j}$ in the rotating coordinate system, the final attitude transformation matrix of the master robot at each moment of rotation is as follows:

$${}^{W}T_{E1}={}^{W}T_{WP}{}^{WP}T_{CP}{}^{CP}T_{FP}{}^{FP}T_{E1}$$

(18)

The actuator pose transformation matrix of the slave robot at each moment is:

$${}^{W}T_{E2}={}^{W}T_{WP}{}^{WP}T_{CP}{}^{CP}T_{P_j}{}^{P_j}T_{TG}{}^{TG}T_{E2}$$

(19)

3.5. Procedure for Cooperative Planning for Dual-Arm Motions

The general methods of collaborative planning for dual-arm robots can be summarized as follows:

• Establish a 3D model of the workpiece based on the requirements of the task and determine the conversion relationship between the connecting rod coordinate system and the workpiece coordinate system;
• Derive the characteristic equation of the task curve in the base coordinate system based on the workpiece model;
• Discretize the obtained curve according to the characteristic equation from step 2 and Formulas (13)–(17);
• Analyze the collaborative motion of dual-arm robots, and derive the kinematics constraint relationship of the robot during motion using Formulas (18) and (19).
• The above processes are summarized in Figure 10.

4. Simulation Verification and Analysis

MATLAB R2019b Simscape was utilized to perform simulation for the motion collaboration planning method of dual-arm robots, with the motion planning program scripted following the procedure outlined in Figure 10. The logic block diagram is depicted in Figure 11. The workpiece is mounted at the end of the master arm, with the installed actuator extending from the end of the slave’s arm.

(1)

Simulation Scenario 1 (central rotation)

In working scenario 1, the master robot picks up the workpiece and rotates around the center of the workpiece, while the actuator moves vertically along the working track. The four key positions of the dual-arm robot during the movement process are illustrated in Figure 12. The robot begins from the zero position, first reaching the contact point of the working task, then moving counterclockwise along the track by half a track, and finally returning to the contact point clockwise by half a track.

The trajectory simulation values of the dual-arm robot’s joints were input into the robot to observe if each joint’s trajectory curve was smooth without abrupt changes, if the trajectory stayed within the joint limits, and if the joint angles were correct. The incremental change curve of each joint angle of the master robot, from the initial moment to the contact point moment, is depicted in Figure 13 and the incremental change curve of each joint angle of the slave robot is depicted in Figure 14. The movement duration was 16 s. Using the base coordinate system of the master robot as a reference, the rotation center coordinate of the workpiece is $\left(x,y,z\right)=\left(-1.5,0.2,0.881\right)$. The simulation parameters of the robot are detailed in

Table 3. Parameters for central rotational motion.

Simulation Parameter Name	Simulation Parameter/(mm, deg)
Initial coordinates of the master robot	$\left(x,y,z,A,B,C\right)=\left(0,1.4,1.181,0,0,0\right)$
Rotation center coordinates of the master robot	$\left(x,y,z,A,B,C\right)=\left(-1.5,0.2,1.081,0,0,0\right)$
Initial coordinates of the slave robot	$\left(x,y,z,A,B,C\right)=\left(0,1.4,1.181,0,0,0\right)$
Time from start point to rotation center	0~5 s
Clockwise 1/2 trajectory time	5~13 s
Counterclockwise 1/2 trajectory time	13~16 s

.

(2)

Simulation Scenario 2 (Rigid Body Translation)

In scenario 2, the master robot executes a circular trajectory in the spatial plane, while the slave robot coordinates its circular trajectory based on the workpiece dimensions while tracking the master robot’s circular movement. Key positions during the dual-arm cooperative circular motion are illustrated in Figure 15, and the circular trajectory of the actuator on the workpiece surface is depicted in Figure 16. Stages (a) to (b) in Figure 15 respectively represent the movement from the robot’s zero point to the docking point, stages (b) to (c) indicate that the motion trajectory has traversed by 1/4 of a circle, and stages (c) to (d) indicate that the motion trajectory has traversed 3/4 of a circle. Figure 16 displays the trajectory of the actuator end on the workpiece.

The simulation parameters for the dual-arm motion in working scenario 2 are presented in

Table 4. Rigid body translation circular motion parameters.

Simulation Parameter Name	Simulation Parameter/(mm, deg)
Initial coordinates of the master robot	$\left(x,y,z,A,B,C\right)=\left(0,1.4,1.181,0,0,0\right)$
Docking point coordinate of the master robot	$\left(x,y,z,A,B,C\right)=\left(-1.5,0.5,0.081,0,0,0\right)$
Initial coordinates of the slave robot	$\left(x,y,z,A,B,C\right)=\left(0,1.375,0.79,0,0,0\right)$
Time from start point to docking point	0~5 s
Time from docking point to 1/4 circle	5~13 s
Time for 3/4 circle to return to docking point	13~16 s

, with the joint angle curves depicted in Figure 17 and Figure 18. Figure 17 illustrates the angle change curve of the master robot’s joints, while Figure 18 showcases the angle change curve of the slave robot’s joints. To account for the impact force of the robot’s joints at the turning point, a trapezoidal velocity planning method is employed for the overall trajectory.

From the collaboration motion simulation of the two operating scenarios, it is evident that the actuator can track the working trajectory during motion, and the joint angle change curves of the dual-arm robot remain within the robot’s joint limit range.

5. Experimental Verification and Analysis

After completing the simulation, it is essential to construct a dual-arm robot system for experimental validation. The cooperation of the dual-arm robot will be observed during the experiment. Subsequently, simulation data will be imported into the experimental system for cooperative movement experiments following the development of upper computer and controller programs.

5.1. Experimental Platform Setup

The robots utilized in this experimental setup were two standalone industrial robots from a particular company. By employing an industrial switch and Ethernet communication, these two independent robots were interconnected to create a dual-arm robot system. The experimental equipment is detailed in

Table 5. Experimental hardware equipment.

Experimental Equipment	Model	Quantity
Host computer	MECHREVO Code01	1
Industrial switch	ICP DAS NS-205	1
Controller	NI-cRIO 9030	2
Industrial robot	150 kg Industrial Robot	2
Actuator	Experimental Equipment	1
Workpiece	Experimental Equipment	1

.

During the dual-arm cooperative movement, the master robot installs the workpiece, while the slave robot installs the actuator. The initial position of the dual-arm robot is its zero position, as depicted in Figure 19. Figure 19a illustrates the installation of the workpiece, while Figure 19b illustrates the installation of the actuator.

5.2. Calibration of the Base Coordinate System for Dual-Arm Robots

Prior to conducting the experiment, it is essential to calibrate the base coordinate system of the dual-arm robot. The calibration site and the tools used for robot installation calibration are illustrated in Figure 20.

The master robot is denoted as $M$, while the slave robot is labeled as $S$. The joint-angle sequence of the master robot at the initial spatial point $P_1$ is represented as $M_1$, and that of the slave robot is denoted as $S_1$. Similarly, in space, the joint-angle sequence of the master robot at the second point $P_2$ is indicated as $M_2$, and the secondary robot’s joint-angle sequence is denoted as $S_2$. Likewise, in space, the joint-angle sequence of the master robot at the third point $P_3$ is labeled as $M_3$, and the slave robot’s joint-angle sequence is denoted as $S_3$. The experiments involving the spatial docking points are illustrated in Figure 21, with the recorded data from the experiments presented in

Table 6. Joint angle data calibration for dual-arm robot.

Number	${\mathit{\theta }}_1$/rad	${\mathit{\theta }}_2$/rad	${\mathit{\theta }}_3$/rad	${\mathit{\theta }}_4$/rad	${\mathit{\theta }}_5$/rad	${\mathit{\theta }}_6$/rad
$M_1$	3.3501	−1.9332	0.3150	−1.1992	0.8205	−1.5709
$M_2$	2.9103	−1.7692	0.5913	−2.4699	1.0582	−1.5709
$M_3$	2.4076	−1.3606	−0.3061	0.0000	1.5709	−1.5709
$S_1$	0.0525	−1.6430	0.1389	−1.3936	0.4213	−0.4161
$S_2$	0.4697	−1.5182	0.0881	−1.1845	1.1627	−0.4936
$S_3$	0.6904	−1.2629	0.1824	−2.8633	1.5296	−1.1215

.

Utilizing forward kinematics, the end pose of the $M_1$ group sequence is computed. The position of point $P_1$ in the master robot’s base coordinate system is derived by extracting the first three rows of the final column. Subsequently, the positions of points $P_2$ and $P_3$ are obtained by sequential substitution:

$$\left\{\begin{array}{l}{}^{M}P_1=\left(-1.2367,-0.0200,1.4260\right)\hfill \\ {}^{M}P_2=\left(-1.2527,0.4883,1.2630\right)\hfill \\ {}^{M}P_3=\left(-1.2466,1.1244,1.2638\right)\hfill \end{array}\right.$$

(20)

Taking the unit vector in the direction of the $\overrightarrow{P_1{P}_2}$ vector as the $x$ axis of the intermediate calibration coordinate system, denoted as $\left\{cal\right\}$, and the unit vector in the direction of $\overrightarrow{P_1{P}_2}\times \overrightarrow{P_1{P}_3}$ as the $z$ axis, the $y$ axis is obtained through the cross-product $z\times x$. This process yields a $3\times 3$ attitude matrix. By selecting the $P_1$ point as the reference position, a terminal attitude position transformation matrix can be obtained based on the master robot’s base coordinate system. The translation is as follows:

$${}^{B1}T_{cal}=\left[\begin{array}{cccc}0.0301& -0.1238& 0.9918& -1.2527\\ -0.9518& -0.3066& -0.0094& 0.4883\\ 0.3053& -0.9438& -0.1271& 1.2630\\ 0& 0& 0& 1\end{array}\right]$$

(21)

Similarly, using $S_1$, $S_2$, $S_3$, and the previous method, a set of position coordinates can be obtained with the robot as the reference frame:

$$\left\{\begin{array}{l}{}^{S}P_1=\left(1.6304,-0.0542,1.4159\right)\hfill \\ {}^{S}P_2=\left(1.5355,0.4484,1.2480\right)\hfill \\ {}^{S}P_3=\left(1.4557,1.0791,1.2716\right)\hfill \end{array}\right.$$

(22)

The pose transformation matrix in the robot coordinate system is as follows:

$${}^{B2}T_{cal}=\left[\begin{array}{cccc}0.1764& -0.1132& 0.9778& 1.5355\\ -0.9336& -0.3339& 0.1298& 0.4484\\ 0.3118& -0.9358& -0.1646& 1.2480\\ 0& 0& 0& 1\end{array}\right]$$

(23)

The closed-loop relationship between the kinematics relationship is as follows:

$${}^{B1}T_{cal}{}^{cal}T_{B2}{}^{B2}T_{B1}=I$$

(24)

By right-multiplying the inverse transformation matrix, the transformation matrix from the robot’s base coordinate system to the master robot’s base coordinate system can be obtained, as follows:

$${}^{B1}T_{B2}={}^{B1}T_{cal}{\left({}^{B2}T_{cal}\right)}^{-1}=\left[\begin{array}{cccc}0.9891& 0.1420& -0.0380& -2.7878\\ -0.1424& 0.9898& -0.0083& 0.2734\\ 0.0364& 0.0136& 0.9992& -0.0461\\ 0& 0& 0& 1\end{array}\right]$$

(25)

5.3. The Coordinated Motion of Dual-Arm Robots

After completing the calibration of the base coordinate system, motion coordination experiments are conducted, which are divided into scenario 1 and scenario 2.

(1)

Coordinated motion scenario one for dual-arm robot (central rotation).

Import the simulated trajectory into the master–slave robot controller program, as illustrated in Figure 22, for the experimental procedure.

The corresponding joint curves are shown in Figure 23. From the experimental curves, it can be observed that the trajectory executed by the dual-arm robot during the experiment is identical to the simulated trajectory.

To assess the smoothness of the robot’s movement, Figure 24 displays the change curves of the robot’s joint angular velocity. Figure 24a depicts the joint angular velocity curve of the master robot, while Figure 24b illustrates the joint angular velocity curve of the slave robot. During the timeframe of 0 s to 5 s, a trapezoidal velocity curve is executed at the end. The figure shows that the joint angular velocity remains zero for 5 s. Between 5 s and 16 s, the joint speed of the master robot’s sixth joint undergoes the most significant change, followed by the sixth joint of the slave robot, while the other joints experience minimal changes.

(2)

Dual-arm cooperative scenario 2 (Rigid Body Translation)

The motion trajectories can be imported into the control program for conducting cooperative experiments, as depicted in Figure 25.

The joint-angle change curve of the dual-arm cooperative circular trajectory is depicted in Figure 26. In Figure 26a, the joint-angle change curve of the master robot is illustrated, while Figure 26b displays the joint-angle change curve of the slave robot. Both curves closely align with the simulation curves presented in Section 3, affirming their ability to fulfill collaboration motion tasks.

The joint-angular velocity curves of the dual-arm cooperative circular trajectory are depicted in Figure 27. In Figure 27a, the curves illustrate the joint velocity changes in the master robot, while Figure 27b showcases the joint velocity changes in the slave robot. Both sets of curves demonstrate that the master and slave robots undergo a uniform acceleration and deceleration from 0 at 0 s to 5 s and from 5 s to 15 s, mitigating the impact of the trajectory on the joints at the turning point.

During the experiment, the midpoint of the cooperative motion is measured under working scenarios 1 and 2, focusing primarily on the height error between the track points of the master robot and the actuator of the slave robot. The measurement data are presented in

Table 7. The measurement values of the intermediate point errors in the experiment.

	Measurement Error for Scenario 1	Measurement Error for Scenario 2
Docking Point	1.5 mm	1.8 mm
Intermediate Point 1	2.4 mm	2.1 mm
Intermediate Point 2	1.7 mm	2.6 mm
End Point	1.4 mm	1.7 mm

.

The errors listed in Table 7 are plotted as curves, as depicted in Figure 28.

The effectiveness of the proposed method is evident from the results of experiments involving two types of motion coordination: rigid-body rotation and rigid-body translation. An analysis of the trajectory curves for each joint reveals that the joints remain within the robot’s limits, with reasonable angular acceleration observed. Deceleration and acceleration processes occur at turning points, effectively mitigating the joint impact forces. Throughout the experiment, the distance between the actuator and discrete points on the workpiece was measured, yielding a maximum distance error of 2.6 mm. This error is attributed to inaccuracies in calibrating the base coordinate system of the dual-arm robot and the end calibration results of the actuator. While the calibration method employed for the robot’s base coordinate system is convenient and straightforward, its accuracy is insufficient, contributing to errors.

6. Discussion

In this paper, a cooperative motion trajectory planning method for a dual-arm robot is investigated. The main conclusions are as follows:

(1)

A mathematical model of a two-arm robot is established, and the closed-chain kinematics relationship of the dual-arm robot during a motion coordination process is derived. This facilitates the interpolation of the working trajectory, enhancing operational efficiency;

(2)

A three-point spatial method is employed to achieve self-calibration of the relative pose relationship between the master robot’s base coordinates and the slave robot’s base coordinates, streamlining the calibration process for dual-arm robots;

(3)

By extracting the feature trajectory of the workpiece model and integrating it with closed-chain kinematics, the trajectory for dual-arm coordinated motion is planned, simplifying the task planning method of the dual-arm robot. The effectiveness of the method is verified through simulation and experimentation.

Through simulation and experimentation, the motion coordination method for dual-arm robots, a new approach to enhance the application of dual-arm robots, is proposed.