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

: 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.


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 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 dualrobot 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.

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, 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:

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: 1.The world coordinate system, denoted as   W , typically shares the same origin and orientation as the master robot's base coordinate system; 2. The base coordinate system of the master robot is defined as   1 B , while the base coordinate system of the slave robot is defined as   2 B ; 3. The master robot end coordinate system is defined as   1 E , and the slave robot end coordinate system is defined as   2 E ; 4. The coordinate system of the actuator is denoted as   TG , while the working position coordinate system of the workpiece is denoted as   WP .Typically, the origin of the working position coordinate system   WP aligns with the origin of the rotating center coordinate system   CP .Additionally, the transformation coordinate system   FP of the workpiece and the transfer flange coincides with the base coordinate system   BP 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: Once the working position   WP is determined, the conversion relationship between the master arm's base coordinate system and the working position can be calculated using the following formula: The simultaneous equations yield the following expression: In the equations above, the attitude transformation matrix W WP T from the world coordinate system to the working position, the transformation matrix WP WP T from the working position to the rotation center, the transformation matrix CP FP T from the rotation center to the transfer, and the transformation matrix T 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: The simultaneous equations yield the following expression: In the provided formula, 2 B W T 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.j P TG T 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 E
T 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.

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 1 P , 2 P , and 3 P .Subsequently, the slave robot is adjusted so that its end effector aligns with that of the master robot, touching points 1 P , 2 P , and 3 P 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,  represents the six joint angles of the master robot at point  represents the six joint angles of the slave robot at point Through the forward kinematics model of the master robot, the position coordinates of points can also be calculated.The unit vector in the 12 PP direction is designated the x axis of the calibration coordinate system   cal , while the unit vector in the direction of  T can be derived.Similarly, matrix 2

B cal
T can be computed with the base coordinate system   as the reference.
Subsequently, the closed-chain relationship between the kinematics is as follows: This is achieved by right-multiplying the inverse transformation matrix: ( ) Both terms on the right side of the above equation can be solved using forward kinematics.Subsequently, the pose transformation from coordinate system   can be obtained.

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.

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   ij P located at the discrete point j of trajectory i .This includes defining the workpiece rotation center coordinate system   CP , the base coordinate system   BP for workpiece installation and the transfer flange, and the transfer flange coordinate system   FP .The origin of

 
CP 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: where   W denotes the world coordinate system,   B represents the base coordinate system of the workpiece-holding robot, and   W Pij T signifies the homogeneous transformation matrix of the master robot from the world coordinate system to discrete points along the trajectory.

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   CP 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   1 C , as illustrated in Figure 6. ( ) 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

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   ij P aligns with the actuator's axis and maintains a certain height difference from the origin.The forward direction of the actuator coincides with the According to the parametric equation, the arc length L s of the curve is: The differential of the arc length is: The linear velocity of the actuator along the curve arc is: The change rate of the parameter  with respect to time is: When the discrete time period is T , the coordinates of the 1 n + discrete point on the curve trajectory are expressed as ( ) , where ( ) 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.In the   CP 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.

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. T .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 j CP P T in the rotating coordinate system, the final attitude transformation matrix of the master robot at each moment of rotation is as follows: The actuator pose transformation matrix of the slave robot at each moment is:

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.

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.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 ( ) ( ) , , 1.5, 0.2, 0.881 x y z =− . The simulation parameters of the robot are detailed in Table 3.   (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.The simulation parameters for the dual-arm motion in working scenario 2 are presented in Table 4, with the joint angle curves depicted in Figures 17 and 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.

Simulation Parameter Name
Simulation Parameter/(mm, deg) .181, 0, 0, 0  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.

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.

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.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.

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 1 P is represented as 1 M , and that of the slave robot is denoted as 1 S .Similarly, in space, the joint-angle sequence of the master robot at the second point The closed-loop relationship between the kinematics relationship is as follows: 12 21 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: ( )

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.
Import the simulated trajectory into the master-slave robot controller program, as illustrated in Figure 22, for the experimental procedure.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.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 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.

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 dualarm robots, a new approach to enhance the application of dual-arm robots, is proposed.

Figure 1 .
Figure 1.DH coordinate systems of dual-arm robot.

Figure 2 .
Figure 2. The coordinate systems' relationship of dual-arm robots.

1 P , 2 P , and 3 P
in the master robot's base coordinate system   1 B can be calculated.Similarly, the position coordinates in the slave robot's base coordinate system   2 B

z
axis of the calibration coordinate system   cal .The y axis is determined using the vector cross-product zx  .

Figure 3 .
Figure 3. Calibration of the base coordinate system for dual-arm robots.

Figure 4 .
Figure 4.The workpiece and its three-dimensional model: (a) workpiece; (b) three-dimensional model of workpiece.
illustrates the intersection diagram, defining the rotation center coordinate system   CP within the workpiece coordinate system.The origin coordinate system of the branch pipe of the saddle curve is denoted as   1 C .

Figure 7 . 1 C
Figure 7. Schematic diagram of intersecting pipe trajectory.The y axis of the branch coordinate system   1 C aligns with the y axis of the direction of the discrete point coordinate system.The discretization of spatial points involves six variables: three spatial position parameters ( ) the linear velocity at the end of the actuator is denoted as L V .The movement time is represented by 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:where T denotes the interpolation time period; this is simplified here as the independent variables in the parametric equation of the curve trajectory, while n  represents a specific point in the interpolation curve.

Figure 9 .
Figure 9. Illustration of constraints.Constraint 1 in Figure 9 represents the pose constraint between the actuator  TG

Figure 10 .
Figure 10.Flowchart of the dual-arm motion cooperative planning method.

Figure 11 .
Figure 11.Logic block diagram of the dual-arm robot motion cooperative program.

Figure 13 .
Figure 13.Master robot joint angle change curve.

Figure 14 .
Figure 14.Slave robot joint angle change curve. 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 .
Figure 16.Motion process of the actuator and workpiece in a circular trajectory.

Figure 17 .
Figure 17.Joint angle change curves of the master robot.

Figure 18 .
Figure 18.Joint angle change curves of the slave robot.

2 P is indicated as 2 M 2 S 3 P is labeled as 3 M 3 SFigure 21 .
Figure 21.Point-to-point calibration experiment for dual-arm robots: (a) spatial first encounter; (b) spatial second encounter; (c) spatial third encounter; (d) close-up view of first encounter; (e) closeup view of second encounter; (f) close-up view of third encounter.

Figure 22 .Figure 23 .
Figure 22.The key nodes of dual-arm motion coordination trajectory: (a) zero pose; (b) docking point; (c) forward 1/2 trajectory; (d) directional 1/2 trajectory.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.

Figure 24 .Figure 25 .Figure 26 .
Figure 24.Velocity change curves of a dual-arm robot (scenario 1): (a) joint angular velocity curves of the master robot; (b) joint angular velocity curves of the slave robot.(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.

Figure 27 .
Figure 27.Velocity change curves of a dual-arm robot (Scenario 2): (a) joint-angular-velocity of the master robot; (b) joint-angular-velocity of the slave robot.

Table 1 .
D-H parameters of the six-degrees-of-freedom industrial robot.
i a /mm

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

Table 3 .
Parameters for central rotational motion.

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

/rad 3  /rad 4  /rad 5  /rad 6  /rad
Taking the unit vector in the direction of the 12 PP vector as the x axis of the intermediate calibration coordinate system, denoted as  

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