# Optimization Control of Cooperative Trajectory towards Dual Arms Based on Time-Varying Constrained Output State

Authors: Hui Zhang; Wenbin Zha; Xiangrong Xu; Yongfei Zhu
Journal: Complexity
 (2021)

---

## Abstract

Aiming at the impact and disturbance of dual-arm robots in the process of coordinated transportation, a dual-arm cooperative trajectory optimization control based on time-varying constrained output state is proposed. According to the constraint relationship of the end-effector trajectory of the dual-arm coordinated transportation, the joint space trajectory mathematical model of the dual-arm coordinated transportation was established by using the master-slave construction method. Based on the time impact optimization index of joint trajectory, a multiobjective nonlinear equation is established. Using random probability distribution to extract the interpolation features of nonuniform quintic B-spline trajectory, the feature optimization target is selected, and the Newton numerical algorithm is used for iterative optimization. At the same time, it is combined with an elite retention genetic algorithm to further optimize the target. Based on the disturbance and tracking problem, a PD control method based on time-varying constrained output state is proposed, and the control law is designed. Its convergence is verified by establishing the Lyapunov function equation and asymmetric term. The trajectory optimization results show that the proposed trajectory optimization method can increase the individual diversity and enhance the individual local optimization, thus avoiding the premature impact of the elite retention genetic algorithm. Finally, the proposed control method is simulated on the platform of Gazebo; compared with the traditional PD control method, the results show that the proposed control algorithm has high robustness, and the rationality of the coordinated trajectory control method is verified by the double-arm handling experiment.


---

## Body

## 1. Introduction

With the diversification of production tasks, the drawback of single operation for the traditionally mechanical arm has gradually emerged in recent years. In comparison, dual-arm robot possesses better flexibility and load capacity and is more competent for cooperative tasks. Particularly, in the face of some complex operation tasks, the advantages of dual-arm robots are more obvious, such as carrying heavy objects and flexible assembly. Thus, it has attracted more attention of researchers [1–3]. The dual-arm robot is not just a simple superposition of two single-arm robots but cooperates with each other in the same system. Compared with the single-arm operation, the dual-arm operation expands the cooperation between the dual arm. Thus, the coordinated control between two manipulators and adaptability of the environment is very important. The trajectory optimization and coordinated control between the dual-arm have been focused widely [4–6]. The coordinated operation consists of two parts: one is the coordinated trajectory planning, and the other is the coordinated motion control method.Motion planning is the basis of dual-arm control. The dual-arm cooperative motion planning methods mainly include master-slave motion planning, obstacle avoidance based on motion planning, multipriority motion planning, and other planning methods. Trajectory optimization is a key research field of manipulator trajectory planning. To realize it, lots of methods have been developed so far. The optimal trajectory proposed by researchers in the last century is time-optimal trajectory. The joint motion angular velocity and angular acceleration continuous cubic spline interpolation curve were used. Under the speed constraint, the speed was adjusted to minimize the motion time. Because of the quick development of robotic techniques, the multiobjective optimization algorithm is more and more popular in the field of manipulator trajectory. Among them, the genetic algorithm is widely used. Particularly, to avoid the defects, it is usually employed by mixing with other optimization algorithms. In the previous research [7, 8], for the optimization of the quintic B-spline trajectory with the goal of motion time, energy consumption, and jerky, an improved immune clonal selection algorithm was proposed. In order to overcome the deficiency of local optimization ability of the algorithm, He et al. [9] put forward a difference algorithm combined with the nonlinear program to optimize the objective function. To prevent the evolution of the elite population from being destroyed and affecting the global planning later, Gu’s group utilized the elite retention strategy in the hybrid algorithm to avoid premature population [10]. All the above researches belong to the population competition strategy, and the search time is too long.Owing to the friction, model error, and unknown disturbance, the end-effector trajectory of the manipulator will produce obvious deviation. Therefore, the control of trajectory motion is particularly important. At present, the main control methods used in the manipulator are master-slave control, position/force hybrid control, impedance control, adaptive control [11–13], neural network control [14–16], fuzzy control [17], and so on. In [18], the authors adopted a PD model compensation synovial control law to achieve dual joint trajectory tracking, but it was not suitable for a high degree of freedom obviously. Wei et al. [19] proposed that the chattering of the system could be easily suppressed by using traditionally active compliance control, but in that case, an accurate dynamic model was needed. By virtue of constrained neural network position control [20, 21], the anti-interference ability could be also enhanced and the chattering of the system could be suppressed effectively. However, for the high degree of freedom robot, the function of neural network fitting unknown term was not obvious. Besides, a position/force hybrid control method was also reported [22], in which the desired position and contact force at the end of the dual arm were taken as the input control variables, and the position control and force control modes were switched by selecting the matrix. However, this switching control mode was prone to make the system unstable and had poor suppression to external interference. Position/force hybrid control is to put force control and position control in orthogonal space to satisfy force control or position control at the expense of position in a certain direction.In this paper, to solve the impact and disturbance problem of dual-arm coordinated transportation, a dual-arm cooperative trajectory optimization control method based on time-varying constraint output state is proposed, and three aspects of coordination planning, trajectory optimization, and joint control are discussed in detail. Firstly, the master-slave kinematics modeling method is used to plan the dual-arm trajectory and establish the time impact trajectory optimization equation. According to the coordinated transportation, the constraint equations are established to obtain the pose, velocity, and acceleration of the master-slave arm. Then, under the trajectory of the dual-arm robot that is well planned, the angle, angular velocity, and angular acceleration are optimized. To avoid the premature problem caused by the lack of local optimization ability, the genetic algorithm with elite mechanism and the random feature selection combined with Newton numerical iteration method [23, 24] are employed as the global optimization and the local optimization, respectively. At the same time, a PD joint position control method with time-varying output constraints is proposed to avoid the problem of complicated modeling on the premise of overcoming the interference. A dual-arm control system is built by the combination with the master-slave framework. Also in our case, the rationality of the optimization algorithm is further verified by trajectory optimization experiments, and the effectiveness of the trajectory optimization control method of dual-arm coordinated transportation is tested by using Gazebo simulation.


## 2. Motion Planning of Coordination

Figure1 shows a way of coordinated handling. When the arms start to handle objects, two end-effectors are relatively stationary. Thus, the two end-effectors and the object can be regarded as a whole. In working, the master-slave method can be used to plan the coordinated transportation of the dual arm with the left arm as the main arm and the right arm as the slave arm. The movement tracking of the main arm is preset, and the movement tracking of the slave arm is obtained from the movement track of the main arm according to the constraint conditions of the dual arm.Figure 1
Sketch diagram of dual-arm cooperation transportation.


### 2.1. Position and Orientation Constraints

In Figure1, the pose constraint relationship between the object and the end-effector of the main arm is given by(1)TBase0LfTLfM0=TBase0M0,where TBase0Lf and TBase0M0 are the position and orientation matrixes of the end of the manipulator and the object in the global coordinate system Base0; TLfM0 is the position and orientation matrixes of the object in the coordinate system of the end of the main arm. Because the robotic end-effector and the object are relatively stationary in the process of the object being transported, TLfM0 can be determined according to the initial pose of the main arm and the object in the global coordinate system.Similarly, the pose relationship between master arm and slave arm can be expressed as follows:(2)TBase0LfTLfM0TM0Rf=TBase0Rf,where TBase0Lf and TBase0Rf are the position and orientation matrixes of {Base0} at the end of the master and slave arms; TM0Rf is the pose matrix of the end of the slave arm under the centroid M0 and remains unchanged.AsTLfM0TM0Rf=TLfRf, TLfRf can be obtained according to the initial state of cooperation between dual arm. Thus, equation (2) can be changed as follows:(3)TBase0LfTLfRf=TBase0Rf.In order to facilitate the experimental results, it only needs to plan the position and pose of the main arm end-effector. The position and pose matrix of the end-effector of the main arm can be obtained by equation (3) and the corresponding joint angle can be calculated by the inverse solution.


### 2.2. Speed Constraint

The relationship between the position and posture and the differential motion of the main arm end-effector is as follows:(4)TLnext=TLcur+dTL,where TLnext is the next moment pose of the main arm end-effector, TLcur is the current moment pose of the main arm end-effector, and dTL is transformed from the differential motion (dqL) of the main arm joint. Here, dqL can be calculated from equation (5), where q˙L is the joint velocity planned by the main arm, Δt is the instantaneous time difference, and acceleration has little effect on the joint control feedback. In order to improve the reliability of TLnext and ignore the influence of instantaneous joint angular acceleration, an appropriate Δt can be set to reduce the influence of acceleration.(5)dqL=q˙LΔt.Then, the next moment position and posture from the end-effector of the slave arm can be obtained from equation (3):(6)TRnext=TLnextTLfRf,where TRnext is the position and posture of the slave manipulator end-effector at the next moment. The joint angle and joint differential dqR at the next moment can be obtained by inverse solution, and thus, the joint angular velocity (q˙R) could be obtained.


### 2.3. Acceleration Constraint

In order to analyze the acceleration constraint relationship of the dual-arm robots in coordinated transportation, equation (5) can be changed into the relationship among the displacement, velocity, and acceleration of the main arm, as shown in the following equation:(7)dqL=q˙LΔta+12q¨LΔta2,where q¨L is the angular acceleration of the main arm joint and Δta is the instantaneous time difference, which is a small value. To ignore the instantaneous acceleration, here, Δta should be many times larger than the value of Δt in equation (5).The corresponding joint angular acceleration (q¨R) of the slave arm can be calculated by repeating equation (6).To sum up, through the planning of the main arm position and pose, the corresponding main arm joint angle can be obtained. Then, the joint angle and angular velocity as well as angular acceleration of the main and slave arm in the joint space can be further planned.


## 2.1. Position and Orientation Constraints

In Figure1, the pose constraint relationship between the object and the end-effector of the main arm is given by(1)TBase0LfTLfM0=TBase0M0,where TBase0Lf and TBase0M0 are the position and orientation matrixes of the end of the manipulator and the object in the global coordinate system Base0; TLfM0 is the position and orientation matrixes of the object in the coordinate system of the end of the main arm. Because the robotic end-effector and the object are relatively stationary in the process of the object being transported, TLfM0 can be determined according to the initial pose of the main arm and the object in the global coordinate system.Similarly, the pose relationship between master arm and slave arm can be expressed as follows:(2)TBase0LfTLfM0TM0Rf=TBase0Rf,where TBase0Lf and TBase0Rf are the position and orientation matrixes of {Base0} at the end of the master and slave arms; TM0Rf is the pose matrix of the end of the slave arm under the centroid M0 and remains unchanged.AsTLfM0TM0Rf=TLfRf, TLfRf can be obtained according to the initial state of cooperation between dual arm. Thus, equation (2) can be changed as follows:(3)TBase0LfTLfRf=TBase0Rf.In order to facilitate the experimental results, it only needs to plan the position and pose of the main arm end-effector. The position and pose matrix of the end-effector of the main arm can be obtained by equation (3) and the corresponding joint angle can be calculated by the inverse solution.


## 2.2. Speed Constraint

The relationship between the position and posture and the differential motion of the main arm end-effector is as follows:(4)TLnext=TLcur+dTL,where TLnext is the next moment pose of the main arm end-effector, TLcur is the current moment pose of the main arm end-effector, and dTL is transformed from the differential motion (dqL) of the main arm joint. Here, dqL can be calculated from equation (5), where q˙L is the joint velocity planned by the main arm, Δt is the instantaneous time difference, and acceleration has little effect on the joint control feedback. In order to improve the reliability of TLnext and ignore the influence of instantaneous joint angular acceleration, an appropriate Δt can be set to reduce the influence of acceleration.(5)dqL=q˙LΔt.Then, the next moment position and posture from the end-effector of the slave arm can be obtained from equation (3):(6)TRnext=TLnextTLfRf,where TRnext is the position and posture of the slave manipulator end-effector at the next moment. The joint angle and joint differential dqR at the next moment can be obtained by inverse solution, and thus, the joint angular velocity (q˙R) could be obtained.


## 2.3. Acceleration Constraint

In order to analyze the acceleration constraint relationship of the dual-arm robots in coordinated transportation, equation (5) can be changed into the relationship among the displacement, velocity, and acceleration of the main arm, as shown in the following equation:(7)dqL=q˙LΔta+12q¨LΔta2,where q¨L is the angular acceleration of the main arm joint and Δta is the instantaneous time difference, which is a small value. To ignore the instantaneous acceleration, here, Δta should be many times larger than the value of Δt in equation (5).The corresponding joint angular acceleration (q¨R) of the slave arm can be calculated by repeating equation (6).To sum up, through the planning of the main arm position and pose, the corresponding main arm joint angle can be obtained. Then, the joint angle and angular velocity as well as angular acceleration of the main and slave arm in the joint space can be further planned.


## 3. Joint Space Trajectory Optimization

### 3.1. Establishing the Optimization Equation

The master-slave motion planning method used in this paper focuses on the main arm. To ensure the great accuracy of angular displacement, the instantaneous variables of the slave arm are calculated at the expense of the accuracy of the next order variables. By analogy, the impact of the slave arm joint is not accurate. Therefore, the coordinated motion trajectory of the dual arm is adjusted indirectly only by optimizing the time impact equation of the main arm.The B-spline curve has the advantage of local adjustment, which can improve the resolution on the premise of ensuring the accuracy of the fitting curve [25, 26]. Therefore, in our case, to establish a multiobjective nonlinear optimization equation about time impact, a nonuniform quintic B-spline curve to interpolate the joint trajectory of the main arm is adopted and shown in the following equation:(8)minS1=∑i=0n−1ti+1−ti=tfS2=∫0tfJx2dxs⋅t.vt≤Vmax,at≤Amax,where ti+1−ti is time interval; tf is the execution time of the arm; v and a are the angular velocity and angular acceleration variables respectively; Vmax and Amax are the constraint angular velocity and angular acceleration variable limits; Jx is the joint acceleration (i.e., impact); S2 is the square sum integral of impact.In order to obtain the minimum value and increase the constraint-specific gravity, equation (8) is simplified to equation (9), in which w1,w2,m,n are the weights. Owing to the limit of angular velocity and angular acceleration of the slave arm, Vmax and Amax can be reduced appropriately.(9)minf=w1S12+w2S2+n∗sumv2+m∗suma2s⋅t.vt≤Vmax,at≤Amax.


### 3.2. Elite Genetic Algorithm Based on Individual Optimization

#### 3.2.1. Genetic Algorithm Based on Elite Mechanism

The genetic algorithm based on the elite mechanism extends the binary tournament mechanism and elite population retention mechanism on the basis of the genetic algorithm, which greatly improves the convergence ability. However, the binary tournament mechanism and elite population retention mechanism will lead to the decline of population diversity, thus ignoring the potential of some individuals and resulting in a premature algorithm, as shown in Figure2.Figure 2
The demonstration for genetic optimization algorithm of the elite selection.The crossover operator can be described as equation (10), in which r is a random number of 0∼1 and x1,x2 are individuals in a cross position.(10)v=r∗x2+1−r∗x1.Considering that the time interval variables are all positive, the mutation operator can be changed as follows:(11)v=absx3+x3−xmaxfp,r1>0.5,x3+xmin−x3fp,r1<0.5,,where r1 is a random number of 0∼1 and x3 is the mutation position individual, fp=1−p1/p22, p1 is the current iteration number, p2 is the maximum iteration number, and abs is the absolute value.


#### 3.2.2. Newton Numerical Iteration Method

Newton numerical iteration is a method to get the root of an equation in nonlinear programming. For the equation with a positive objective function, the root obtained is minimum.The expression of Newton’s solution is equation (12), where fdx is the derivative of fx in equation (9), xk+1 is the variable solution of iterative update time interval, and k is the number of iterations.(12)xk+1=xk−fd−1xkfxk.


#### 3.2.3. Algorithm Principle

The improved algorithm includes two aspects. On the one hand, an elite selection genetic algorithm is used to optimize the population. On the other hand, the Newton numerical iteration method is used to optimize individuals. As shown in Figure3, our algorithm first optimizes individual operations to tap individual potential before retaining the excellent population. Then, the local search ability is enhanced and the impact of precocity could be reduced.Figure 3
The diagram of the improved algorithm.As there are many time interval variables in the nonuniform quintic B-spline curve equation, the random sampling method can be used to jump out of the local extremum of local optimization [27]. Before Newton numerical iteration method, the random probability distribution is used to extract a single characteristic variable. The expression is as follows:(13)x=maxx1,…,xn,0≤r<p1,minx1,…,xn,p1≤r<p2,randx1,…,xn,p2≤r≤1,where xn is the nth time interval variable; maxx1,…,xn is the maximum value of time interval variable; minx1,…,xn is the minimum value of time interval variable; randx1,…,xn is the random value of time interval variable; r is a random number of 0∼1; p1 and p2 are the distribution probability boundary.Through the combination of equations (12) and (13), we can optimize the objective function by taking the time discontinuous individual as the variable, limit the number of iterations, find the optimal variable from the Newton numerical iteration process, and optimize the individual. So as to overcome the potential neglect problem caused by the decline of population diversity.


### 3.3. Realization of Optimization Steps

Step 1.
Population initialization. Set population size PS groups, the time interval variable range, and maximum number of evolution (genmax). According to the left arm joint position (qL), randomly select PS groups of time interval variables x1,…,xn. Set fitness as optimization objective equation.Step 2.
Individual single variable optimization. The number of iterations is limited tok. The Newton numerical iteration method and random probability distribution are used to select a single variable in the individual for iterative optimization. The fitness is used as the measurement standard, and the optimal solution is selected to replace the current individual to form a new population.Step 3.
Select the operation. With fitness as a measure, a binary tournament mechanism is adopted and the number of repeated selection candidates is limited to select the better candidate to enter the new population.Step 4.
Use Step2 to optimize individuals.Step 5.
Cross operation. Two individuals are randomly selected and the crossover position is randomly selected to form a new individual by using the crossover operator.Step 6.
Use Step2 to optimize individuals.Step 7.
Mutation operation. A new individual is formed by randomly selecting an individual and a mutation position.Step 8.
Use Step2 to optimize individuals.Step 9.
Elite retention. The offspring and parents were combined, and the best population was selected according to fitness.Step 10.
Evolutionary loop operation. While the current evolution numbergencur is less than the maximum evolution number genmax, return to Step 1.


## 3.1. Establishing the Optimization Equation

The master-slave motion planning method used in this paper focuses on the main arm. To ensure the great accuracy of angular displacement, the instantaneous variables of the slave arm are calculated at the expense of the accuracy of the next order variables. By analogy, the impact of the slave arm joint is not accurate. Therefore, the coordinated motion trajectory of the dual arm is adjusted indirectly only by optimizing the time impact equation of the main arm.The B-spline curve has the advantage of local adjustment, which can improve the resolution on the premise of ensuring the accuracy of the fitting curve [25, 26]. Therefore, in our case, to establish a multiobjective nonlinear optimization equation about time impact, a nonuniform quintic B-spline curve to interpolate the joint trajectory of the main arm is adopted and shown in the following equation:(8)minS1=∑i=0n−1ti+1−ti=tfS2=∫0tfJx2dxs⋅t.vt≤Vmax,at≤Amax,where ti+1−ti is time interval; tf is the execution time of the arm; v and a are the angular velocity and angular acceleration variables respectively; Vmax and Amax are the constraint angular velocity and angular acceleration variable limits; Jx is the joint acceleration (i.e., impact); S2 is the square sum integral of impact.In order to obtain the minimum value and increase the constraint-specific gravity, equation (8) is simplified to equation (9), in which w1,w2,m,n are the weights. Owing to the limit of angular velocity and angular acceleration of the slave arm, Vmax and Amax can be reduced appropriately.(9)minf=w1S12+w2S2+n∗sumv2+m∗suma2s⋅t.vt≤Vmax,at≤Amax.


## 3.2. Elite Genetic Algorithm Based on Individual Optimization

### 3.2.1. Genetic Algorithm Based on Elite Mechanism

The genetic algorithm based on the elite mechanism extends the binary tournament mechanism and elite population retention mechanism on the basis of the genetic algorithm, which greatly improves the convergence ability. However, the binary tournament mechanism and elite population retention mechanism will lead to the decline of population diversity, thus ignoring the potential of some individuals and resulting in a premature algorithm, as shown in Figure2.Figure 2
The demonstration for genetic optimization algorithm of the elite selection.The crossover operator can be described as equation (10), in which r is a random number of 0∼1 and x1,x2 are individuals in a cross position.(10)v=r∗x2+1−r∗x1.Considering that the time interval variables are all positive, the mutation operator can be changed as follows:(11)v=absx3+x3−xmaxfp,r1>0.5,x3+xmin−x3fp,r1<0.5,,where r1 is a random number of 0∼1 and x3 is the mutation position individual, fp=1−p1/p22, p1 is the current iteration number, p2 is the maximum iteration number, and abs is the absolute value.


### 3.2.2. Newton Numerical Iteration Method

Newton numerical iteration is a method to get the root of an equation in nonlinear programming. For the equation with a positive objective function, the root obtained is minimum.The expression of Newton’s solution is equation (12), where fdx is the derivative of fx in equation (9), xk+1 is the variable solution of iterative update time interval, and k is the number of iterations.(12)xk+1=xk−fd−1xkfxk.


### 3.2.3. Algorithm Principle

The improved algorithm includes two aspects. On the one hand, an elite selection genetic algorithm is used to optimize the population. On the other hand, the Newton numerical iteration method is used to optimize individuals. As shown in Figure3, our algorithm first optimizes individual operations to tap individual potential before retaining the excellent population. Then, the local search ability is enhanced and the impact of precocity could be reduced.Figure 3
The diagram of the improved algorithm.As there are many time interval variables in the nonuniform quintic B-spline curve equation, the random sampling method can be used to jump out of the local extremum of local optimization [27]. Before Newton numerical iteration method, the random probability distribution is used to extract a single characteristic variable. The expression is as follows:(13)x=maxx1,…,xn,0≤r<p1,minx1,…,xn,p1≤r<p2,randx1,…,xn,p2≤r≤1,where xn is the nth time interval variable; maxx1,…,xn is the maximum value of time interval variable; minx1,…,xn is the minimum value of time interval variable; randx1,…,xn is the random value of time interval variable; r is a random number of 0∼1; p1 and p2 are the distribution probability boundary.Through the combination of equations (12) and (13), we can optimize the objective function by taking the time discontinuous individual as the variable, limit the number of iterations, find the optimal variable from the Newton numerical iteration process, and optimize the individual. So as to overcome the potential neglect problem caused by the decline of population diversity.


## 3.2.1. Genetic Algorithm Based on Elite Mechanism

The genetic algorithm based on the elite mechanism extends the binary tournament mechanism and elite population retention mechanism on the basis of the genetic algorithm, which greatly improves the convergence ability. However, the binary tournament mechanism and elite population retention mechanism will lead to the decline of population diversity, thus ignoring the potential of some individuals and resulting in a premature algorithm, as shown in Figure2.Figure 2
The demonstration for genetic optimization algorithm of the elite selection.The crossover operator can be described as equation (10), in which r is a random number of 0∼1 and x1,x2 are individuals in a cross position.(10)v=r∗x2+1−r∗x1.Considering that the time interval variables are all positive, the mutation operator can be changed as follows:(11)v=absx3+x3−xmaxfp,r1>0.5,x3+xmin−x3fp,r1<0.5,,where r1 is a random number of 0∼1 and x3 is the mutation position individual, fp=1−p1/p22, p1 is the current iteration number, p2 is the maximum iteration number, and abs is the absolute value.


## 3.2.2. Newton Numerical Iteration Method

Newton numerical iteration is a method to get the root of an equation in nonlinear programming. For the equation with a positive objective function, the root obtained is minimum.The expression of Newton’s solution is equation (12), where fdx is the derivative of fx in equation (9), xk+1 is the variable solution of iterative update time interval, and k is the number of iterations.(12)xk+1=xk−fd−1xkfxk.


## 3.2.3. Algorithm Principle

The improved algorithm includes two aspects. On the one hand, an elite selection genetic algorithm is used to optimize the population. On the other hand, the Newton numerical iteration method is used to optimize individuals. As shown in Figure3, our algorithm first optimizes individual operations to tap individual potential before retaining the excellent population. Then, the local search ability is enhanced and the impact of precocity could be reduced.Figure 3
The diagram of the improved algorithm.As there are many time interval variables in the nonuniform quintic B-spline curve equation, the random sampling method can be used to jump out of the local extremum of local optimization [27]. Before Newton numerical iteration method, the random probability distribution is used to extract a single characteristic variable. The expression is as follows:(13)x=maxx1,…,xn,0≤r<p1,minx1,…,xn,p1≤r<p2,randx1,…,xn,p2≤r≤1,where xn is the nth time interval variable; maxx1,…,xn is the maximum value of time interval variable; minx1,…,xn is the minimum value of time interval variable; randx1,…,xn is the random value of time interval variable; r is a random number of 0∼1; p1 and p2 are the distribution probability boundary.Through the combination of equations (12) and (13), we can optimize the objective function by taking the time discontinuous individual as the variable, limit the number of iterations, find the optimal variable from the Newton numerical iteration process, and optimize the individual. So as to overcome the potential neglect problem caused by the decline of population diversity.


## 3.3. Realization of Optimization Steps

Step 1.
Population initialization. Set population size PS groups, the time interval variable range, and maximum number of evolution (genmax). According to the left arm joint position (qL), randomly select PS groups of time interval variables x1,…,xn. Set fitness as optimization objective equation.Step 2.
Individual single variable optimization. The number of iterations is limited tok. The Newton numerical iteration method and random probability distribution are used to select a single variable in the individual for iterative optimization. The fitness is used as the measurement standard, and the optimal solution is selected to replace the current individual to form a new population.Step 3.
Select the operation. With fitness as a measure, a binary tournament mechanism is adopted and the number of repeated selection candidates is limited to select the better candidate to enter the new population.Step 4.
Use Step2 to optimize individuals.Step 5.
Cross operation. Two individuals are randomly selected and the crossover position is randomly selected to form a new individual by using the crossover operator.Step 6.
Use Step2 to optimize individuals.Step 7.
Mutation operation. A new individual is formed by randomly selecting an individual and a mutation position.Step 8.
Use Step2 to optimize individuals.Step 9.
Elite retention. The offspring and parents were combined, and the best population was selected according to fitness.Step 10.
Evolutionary loop operation. While the current evolution numbergencur is less than the maximum evolution number genmax, return to Step 1.


## 4. PD Control Method of Time-Varying Output Constraint State

### 4.1. Problem Description

To overcome the disturbance of PD control and reduce the modeling difficulty of active compliance control, a constraint output state is proposed.It is assumed that the constraint condition of the manipulator output angular displacement isk¯mt≤θt≤k¯mt. In order to ensure the convergence of the closed-loop system, the following error constraints are given:(14)kat=θdt−k¯mt,kbt=k¯mt−θdt.Let angular displacement errore1=e11,e12,…,e1n=θ−θd and angular velocity error e2=e21,e22,…,e2n=θ˙−α, in which α is the virtual controller.Defineα:(15)α=−k1+se1+θ˙d,where k1=diagk11t,…,k1nt;s=diags11t,…,s1nt.Lets1it=k˙ai/kai2+k˙bi/kbi2+βi. βi>0 and e1 still affects the convergence of e2.Set a piecewise functionhie1i=1,e1i>0,0,e1i≤0. Then, the error variable can be changed to(16)ξai=e1ikai,ξbi=e1ikbi,ξi=hie1iξbi+1−hie1iξai,i=1,…,n.In order to realize the convergence of the closed-loop system, the following two proved lemmas are given [28, 29].Lemma 1.
∀ζ<1, integer p>0.Existential inequalitylog1/1−ζ2p<ζ2p/1−ζ2p.Lemma 2.
If and only if−ka<e1<kb, ζ<1.By Lemma2, the output state is included in the time-varying constraints (−ka<e1<kb). Obviously, the constraints include the constant case mentioned in literature [29, 30]. Therefore, the time-varying constraints are more flexible.


### 4.2. Controller Design and Stability Analysis

Lyapunov matrix equation is as follows:(17)V=12p∑i=1nlog11−ξi+12e2Te2.Then,(18)V˙≤−∑i=1nk1iξi2p1−ξi2p+∑i=1nuie1i2p−1e2i+ee2Te˙2,where e˙2=θ¨−α˙=W−1τ−Cθ˙−G−Wα˙−τd−α˙ and ui=h1i/ebi2p−1−e2i2p−1+1−h1i/eai2p−1−e2i2p.Then, equation (18) can be changed to(19)V˙≤−∑i=1nk1iξi2p1−ξi2p+∑i=1nuie1i2p−1e2i+e2TW−1τ−Cθ˙−G−Wα˙−τd,where W, C, and G are inertia matrix, centrifugal and Coriolis force term matrix, and gravity moment matrix in the dynamic model. They are all positive definite matrixes.By using PD control strategy, the controller can be designed as follows:(20)τ=−WK2e2+K3e1+K4ve˙1+A,where A=u1e112p−1,…,une1n2p−1 and K2i>0.5, as control gain matrix.In this paper, considering the complexity of dynamic modeling, the inertia estimation matrixKf is selected, and the controller is changed to(21)τ=−KfK2e2+K3e1+K4ve˙1+A.Combining equation (19) with equation (21), (22) can be obtained:(22)V˙≤−∑i=1nk1iξi2p1−ξi2p−e2TK2e2+e2Tε,where ε is the approximation error.From Young’s inequality, equation (23) is obtained:(23)∑i=1ne2iε≤12e2Te2+12ε2.According to Lemma1, we can get the following result:(24)V˙≤−∑i=1nk1ilog11−ξi2p−∑i=1nk2i∗e2i2+12ε2,where k2i∗−0.5>0.Obviously,(25)V˙≤−pV+C,where p=2pk1i,2k2i and C=1/2ε2.Thus, equation (26) is easy to be obtained:(26)0≤V≤V0e−pt+Cp.Based on the constructed Lyapunov function, the following result can be obtained:(27)12plog11−ξi2p≤Vt≤V¯0.Obviously,(28)−1<ξ<1.So when the initialξ0<1, based on Lemma 2, we can determine ξt<1, −kai<e1i<kbi, that is, the angular displacement error does not exceed the error constraint limit, so e1i is convergence. It is easy to conclude that α is convergence, too.From equation (27), the following equation is established:(29)e2≤2V¯0.Therefore, the output angular velocity error converges. To sum up, all output convergence can be gained by constraining the output state.


### 4.3. Dual-Arm Coordinate Control

In this paper, we choose the dual-arm robots as the experiment object. Using the master-slave programming method with the left arm as the main arm and the right arm as the slave arm, the expected angular displacement, angular velocity, and angular acceleration of the dual arm are planned as the input value. Under the constraint of the constraint equation, the dual arm is controlled, respectively, according to the output value feedback. The coordinated control framework is shown in Figure4.Figure 4
The control framework of dual-arm coordination.As shown in Figure4, the trajectory of the main arm is optimized and then input into the joint controller to complete the coordinated transportation operation.


## 4.1. Problem Description

To overcome the disturbance of PD control and reduce the modeling difficulty of active compliance control, a constraint output state is proposed.It is assumed that the constraint condition of the manipulator output angular displacement isk¯mt≤θt≤k¯mt. In order to ensure the convergence of the closed-loop system, the following error constraints are given:(14)kat=θdt−k¯mt,kbt=k¯mt−θdt.Let angular displacement errore1=e11,e12,…,e1n=θ−θd and angular velocity error e2=e21,e22,…,e2n=θ˙−α, in which α is the virtual controller.Defineα:(15)α=−k1+se1+θ˙d,where k1=diagk11t,…,k1nt;s=diags11t,…,s1nt.Lets1it=k˙ai/kai2+k˙bi/kbi2+βi. βi>0 and e1 still affects the convergence of e2.Set a piecewise functionhie1i=1,e1i>0,0,e1i≤0. Then, the error variable can be changed to(16)ξai=e1ikai,ξbi=e1ikbi,ξi=hie1iξbi+1−hie1iξai,i=1,…,n.In order to realize the convergence of the closed-loop system, the following two proved lemmas are given [28, 29].Lemma 1.
∀ζ<1, integer p>0.Existential inequalitylog1/1−ζ2p<ζ2p/1−ζ2p.Lemma 2.
If and only if−ka<e1<kb, ζ<1.By Lemma2, the output state is included in the time-varying constraints (−ka<e1<kb). Obviously, the constraints include the constant case mentioned in literature [29, 30]. Therefore, the time-varying constraints are more flexible.


## 4.2. Controller Design and Stability Analysis

Lyapunov matrix equation is as follows:(17)V=12p∑i=1nlog11−ξi+12e2Te2.Then,(18)V˙≤−∑i=1nk1iξi2p1−ξi2p+∑i=1nuie1i2p−1e2i+ee2Te˙2,where e˙2=θ¨−α˙=W−1τ−Cθ˙−G−Wα˙−τd−α˙ and ui=h1i/ebi2p−1−e2i2p−1+1−h1i/eai2p−1−e2i2p.Then, equation (18) can be changed to(19)V˙≤−∑i=1nk1iξi2p1−ξi2p+∑i=1nuie1i2p−1e2i+e2TW−1τ−Cθ˙−G−Wα˙−τd,where W, C, and G are inertia matrix, centrifugal and Coriolis force term matrix, and gravity moment matrix in the dynamic model. They are all positive definite matrixes.By using PD control strategy, the controller can be designed as follows:(20)τ=−WK2e2+K3e1+K4ve˙1+A,where A=u1e112p−1,…,une1n2p−1 and K2i>0.5, as control gain matrix.In this paper, considering the complexity of dynamic modeling, the inertia estimation matrixKf is selected, and the controller is changed to(21)τ=−KfK2e2+K3e1+K4ve˙1+A.Combining equation (19) with equation (21), (22) can be obtained:(22)V˙≤−∑i=1nk1iξi2p1−ξi2p−e2TK2e2+e2Tε,where ε is the approximation error.From Young’s inequality, equation (23) is obtained:(23)∑i=1ne2iε≤12e2Te2+12ε2.According to Lemma1, we can get the following result:(24)V˙≤−∑i=1nk1ilog11−ξi2p−∑i=1nk2i∗e2i2+12ε2,where k2i∗−0.5>0.Obviously,(25)V˙≤−pV+C,where p=2pk1i,2k2i and C=1/2ε2.Thus, equation (26) is easy to be obtained:(26)0≤V≤V0e−pt+Cp.Based on the constructed Lyapunov function, the following result can be obtained:(27)12plog11−ξi2p≤Vt≤V¯0.Obviously,(28)−1<ξ<1.So when the initialξ0<1, based on Lemma 2, we can determine ξt<1, −kai<e1i<kbi, that is, the angular displacement error does not exceed the error constraint limit, so e1i is convergence. It is easy to conclude that α is convergence, too.From equation (27), the following equation is established:(29)e2≤2V¯0.Therefore, the output angular velocity error converges. To sum up, all output convergence can be gained by constraining the output state.


## 4.3. Dual-Arm Coordinate Control

In this paper, we choose the dual-arm robots as the experiment object. Using the master-slave programming method with the left arm as the main arm and the right arm as the slave arm, the expected angular displacement, angular velocity, and angular acceleration of the dual arm are planned as the input value. Under the constraint of the constraint equation, the dual arm is controlled, respectively, according to the output value feedback. The coordinated control framework is shown in Figure4.Figure 4
The control framework of dual-arm coordination.As shown in Figure4, the trajectory of the main arm is optimized and then input into the joint controller to complete the coordinated transportation operation.


## 5. Trajectory Optimization Simulation

### 5.1. Initial State and Trajectory Setting of Dual Arm

In this paper, dual-arm robots are composed of two iiwa14 manipulators, and its D-H model is built by using Robotics Toolbox.The base position of the left and right arms in the global coordinate system is as follows:TBase0L=1000010000100001,TBase0R=1000.5010000100001.Let the starting angle of the left arm beqL = [−1.768, −0.321, 0, 1.588, 0, −1.233, −1.768] (in radian system), qR=−qL, and the Cartesian space trajectory of the end-effector position of the left arm (stationary during the period) is as follows:(30)xi=x0+xf−x0N,yi=y0+yf−y0N,zi=z0+zf−z0N,where xf=x0,yf=y0,zf−z0=0.3,N=50.


### 5.2. Optimization Calculation

The weightsw1,w2,m,n in the objective equation are set to be 1, and the constraint conditions Vmax and Amax are set to be 0.4 (rad/s) and 1 (rad/s2).The initial velocity, initial acceleration, terminal velocity, and terminal acceleration of each joint are set to be 0 by using a 5-degree nonuniform B-spline curve interpolation trajectory. The optimal retention coefficient of the optimization algorithm, the upper limit of replication times of the binary tournament, the population size, the maximum evolution algebragenmax, and the cross probability are 1/2, 2, 30, 100, and 0.6, respectively. The mutation probability is given by pmutation=0.2+0.001−0.2∗gen/genmax. In Newton’s iterative method, the upper limit of iteration times is 3, and the maximum as well as minimum features of trajectories are extracted by random probability distribution. The distribution probability boundaries p1 and p2 are 0.5 and 1, respectively. After the interval variable area is set to (0.05, 1), the results of 5 random trials are shown in Table 1, and the fitness changes of test 1 in three algorithms (the improved optimization algorithm in this paper, the elite genetic algorithm (ESGA), and the standard genetic algorithm (SGA)) are shown in Figure 5.Table 1
Three methods of randomized trials.
Test 1Test 2Test 3Test 4Test 5Modified optimization algorithmBest fitness107.7133.7129.2130.1112.7Avg fitness107.7133.7129.2130.1112.3Average time/s103.6115.4119.4126.0112.3ESGABest fitness346.1347.8388.6256.9266.2Avg fitness346.5342.5388.6256.9266.2Average time/s29.228.829.726.026.4SGABest fitness446.0477.3538.2540.1544.9Avg fitness689.6672.7699.4676.7696.4Average time/s28.329.226.628.633.7Figure 5
The changes of fitness for three algorithms. (a) Modified optimization algorithm. (b) ESGA. (c) SGA.
(a)(b)(c)It can be seen from Table1 and Figure 5 that both the optimal fitness and average fitness of the improved optimization algorithm as well as ESGA are almost the same in the end. They converge at about 50 generations, indicating that the algorithm with elite mechanism will reduce the population diversity. Table 1 and Figure 5 show that the population diversity of SGA is better than that of ESGA and our optimization algorithm and the evolution direction is limited due to the uneven population. As also shown in Table 1 and Figures 5(a) and 5(b), ESGA adds a competition mechanism and retains the elite part; compared with the improved optimization algorithm, the local optimization ability is poor; thus, it is unable to tap individual potential and avoid premature ESGA. In a word, the improved optimization algorithm can enhance the local search ability to optimize individuals and the influence of premature ESGA can be avoided, but the disadvantage is also clear; namely, the optimization time is longer.To further optimize the algorithm, we need to increase the diversity of individual variables. There are three schemes, scheme 1 is to randomly extract the maximum and minimum features of the trajectory Scheme 2 takes random extraction of maximum and minimum features as the main method and random extraction of features as the secondary method. Scheme 3 is to randomly extract trajectory features. (p1, p2) of the three schemes are (0.5, 1), (0.45, 0.9), and (0, 0), respectively. The results of three randomized trials are displayed in Table 2, and the fitness changes of test 1 in three schemes are shown in Figure 6.Table 2
Random test of three schemes.
Test 1Test 2Test 3Scheme 1Best fitness107.7133.7129.2Avg fitness107.7133.7129.2Average time/s103.6115.4119.4Scheme 2Best fitness85.080.291.9Avg fitness85.180.691.8Average time/s122.6122.5132.8Scheme 3Best fitness103.470.592.6Avg fitness103.470.793.1Average time/s143.5130.8132.4Figure 6
The fitness change of three different schemes. (a) Scheme 1 is to randomly extract the maximum and minimum features of the trajectory. (b) Scheme 2 takes random extraction of maximum and minimum features as the main method and random extraction of features as the secondary method. (c) Scheme 3 is to randomly extract trajectory features.
(a)(b)(c)From Table2 and Figure 6, it can be seen that the evolution of scheme 1 is stagnant in about 50 generations, while schemes 2 and 3 are still evolving slowly and have better optimization ability. Therefore, random feature extraction can increase the diversity of individual variables, so that individual variables can continue to be optimized. From Table 2 and Figures 6(b) and 6(c), in comparison, the stability of optimization variables of scheme 3 is not as good as scheme 2. Also, the average optimization ability and average time of scheme 2 are better than those of scheme 3 in the three tests. Therefore, scheme 2 is the best.The optimal test (test 2 of scheme 3) is selected from the tested tests to plan the trajectory of dual-arm robots. The planned trajectory is shown in Figure7.Figure 7
Coordinated trajectory of the dual arm. (a) Dual-arm joint angles. (b) Angular velocity of the dual-arm joints. (c) Angular acceleration of the dual-arm joints. (d) End-effector trajectory of the dual arm.
(a)(b)(c)(d)Figures7(a)–7(c) show that the improved optimization algorithm does not exceed the motion constraints. Moreover, the motion trajectory is smooth and does not produce mutation, which verifies the rationality of the improved optimization algorithm again. Figures 7(a) and 7(d) suggest that the coordinated movement can be completed by coordinating the pose constraint relationship. Figures 7(a)–(7c) indicate that the coordinated motion trajectory can be obtained by combining the differential motion with the constraint relationship of coordinated motion, which is the basis of dynamic simulation and can be determined by the results of Gazebo simulation.


## 5.1. Initial State and Trajectory Setting of Dual Arm

In this paper, dual-arm robots are composed of two iiwa14 manipulators, and its D-H model is built by using Robotics Toolbox.The base position of the left and right arms in the global coordinate system is as follows:TBase0L=1000010000100001,TBase0R=1000.5010000100001.Let the starting angle of the left arm beqL = [−1.768, −0.321, 0, 1.588, 0, −1.233, −1.768] (in radian system), qR=−qL, and the Cartesian space trajectory of the end-effector position of the left arm (stationary during the period) is as follows:(30)xi=x0+xf−x0N,yi=y0+yf−y0N,zi=z0+zf−z0N,where xf=x0,yf=y0,zf−z0=0.3,N=50.


## 5.2. Optimization Calculation

The weightsw1,w2,m,n in the objective equation are set to be 1, and the constraint conditions Vmax and Amax are set to be 0.4 (rad/s) and 1 (rad/s2).The initial velocity, initial acceleration, terminal velocity, and terminal acceleration of each joint are set to be 0 by using a 5-degree nonuniform B-spline curve interpolation trajectory. The optimal retention coefficient of the optimization algorithm, the upper limit of replication times of the binary tournament, the population size, the maximum evolution algebragenmax, and the cross probability are 1/2, 2, 30, 100, and 0.6, respectively. The mutation probability is given by pmutation=0.2+0.001−0.2∗gen/genmax. In Newton’s iterative method, the upper limit of iteration times is 3, and the maximum as well as minimum features of trajectories are extracted by random probability distribution. The distribution probability boundaries p1 and p2 are 0.5 and 1, respectively. After the interval variable area is set to (0.05, 1), the results of 5 random trials are shown in Table 1, and the fitness changes of test 1 in three algorithms (the improved optimization algorithm in this paper, the elite genetic algorithm (ESGA), and the standard genetic algorithm (SGA)) are shown in Figure 5.Table 1
Three methods of randomized trials.
Test 1Test 2Test 3Test 4Test 5Modified optimization algorithmBest fitness107.7133.7129.2130.1112.7Avg fitness107.7133.7129.2130.1112.3Average time/s103.6115.4119.4126.0112.3ESGABest fitness346.1347.8388.6256.9266.2Avg fitness346.5342.5388.6256.9266.2Average time/s29.228.829.726.026.4SGABest fitness446.0477.3538.2540.1544.9Avg fitness689.6672.7699.4676.7696.4Average time/s28.329.226.628.633.7Figure 5
The changes of fitness for three algorithms. (a) Modified optimization algorithm. (b) ESGA. (c) SGA.
(a)(b)(c)It can be seen from Table1 and Figure 5 that both the optimal fitness and average fitness of the improved optimization algorithm as well as ESGA are almost the same in the end. They converge at about 50 generations, indicating that the algorithm with elite mechanism will reduce the population diversity. Table 1 and Figure 5 show that the population diversity of SGA is better than that of ESGA and our optimization algorithm and the evolution direction is limited due to the uneven population. As also shown in Table 1 and Figures 5(a) and 5(b), ESGA adds a competition mechanism and retains the elite part; compared with the improved optimization algorithm, the local optimization ability is poor; thus, it is unable to tap individual potential and avoid premature ESGA. In a word, the improved optimization algorithm can enhance the local search ability to optimize individuals and the influence of premature ESGA can be avoided, but the disadvantage is also clear; namely, the optimization time is longer.To further optimize the algorithm, we need to increase the diversity of individual variables. There are three schemes, scheme 1 is to randomly extract the maximum and minimum features of the trajectory Scheme 2 takes random extraction of maximum and minimum features as the main method and random extraction of features as the secondary method. Scheme 3 is to randomly extract trajectory features. (p1, p2) of the three schemes are (0.5, 1), (0.45, 0.9), and (0, 0), respectively. The results of three randomized trials are displayed in Table 2, and the fitness changes of test 1 in three schemes are shown in Figure 6.Table 2
Random test of three schemes.
Test 1Test 2Test 3Scheme 1Best fitness107.7133.7129.2Avg fitness107.7133.7129.2Average time/s103.6115.4119.4Scheme 2Best fitness85.080.291.9Avg fitness85.180.691.8Average time/s122.6122.5132.8Scheme 3Best fitness103.470.592.6Avg fitness103.470.793.1Average time/s143.5130.8132.4Figure 6
The fitness change of three different schemes. (a) Scheme 1 is to randomly extract the maximum and minimum features of the trajectory. (b) Scheme 2 takes random extraction of maximum and minimum features as the main method and random extraction of features as the secondary method. (c) Scheme 3 is to randomly extract trajectory features.
(a)(b)(c)From Table2 and Figure 6, it can be seen that the evolution of scheme 1 is stagnant in about 50 generations, while schemes 2 and 3 are still evolving slowly and have better optimization ability. Therefore, random feature extraction can increase the diversity of individual variables, so that individual variables can continue to be optimized. From Table 2 and Figures 6(b) and 6(c), in comparison, the stability of optimization variables of scheme 3 is not as good as scheme 2. Also, the average optimization ability and average time of scheme 2 are better than those of scheme 3 in the three tests. Therefore, scheme 2 is the best.The optimal test (test 2 of scheme 3) is selected from the tested tests to plan the trajectory of dual-arm robots. The planned trajectory is shown in Figure7.Figure 7
Coordinated trajectory of the dual arm. (a) Dual-arm joint angles. (b) Angular velocity of the dual-arm joints. (c) Angular acceleration of the dual-arm joints. (d) End-effector trajectory of the dual arm.
(a)(b)(c)(d)Figures7(a)–7(c) show that the improved optimization algorithm does not exceed the motion constraints. Moreover, the motion trajectory is smooth and does not produce mutation, which verifies the rationality of the improved optimization algorithm again. Figures 7(a) and 7(d) suggest that the coordinated movement can be completed by coordinating the pose constraint relationship. Figures 7(a)–(7c) indicate that the coordinated motion trajectory can be obtained by combining the differential motion with the constraint relationship of coordinated motion, which is the basis of dynamic simulation and can be determined by the results of Gazebo simulation.


## 6. Simulation of Coordinated Operation

To verify the feasibility of the dual-arm coordinated motion trajectory and control method, the information of dual-arm coordinated motion trajectory is connected with the plan interface of Moveit, and the coordinated handling control simulation is realized on the Gazebo platform.In order to verify the tracking performance of the proposed control method, a comparative test is carried out with a single arm as the research object. The expected angular displacement, angular velocity, and angular acceleration of the joint are sin(t) (rad), cos(t) (rad/s), and −sin(t) (rad/s2), respectively. The constrained boundary km(t) is (0.2θd − 3, 1.2θd + 0.8). In the virtual controller, k1 and β are diag(0.1,…, 0.1) and 0.2, respectively. The factors k3 and k4 in the adaptive law are 5 and 1, respectively. In the controller, k2 is 30 and kf is 0.4∗diag(3, 4, 4, 4, 4, 3, 2). The real-time ratio when debugging Gazebo ranges from 0.92 to 0.97, and the comparison effect with the PD control method of selecting appropriate PID parameters is shown in Figure 7.In Figure8, the initial and end time of planning trajectory are about 4 and 14 s, respectively. Because the initial and end speed of the planned trajectory are inconsistent with the set speed in Gazebo, there will be sudden changes at the beginning and end of the trajectory. The comparison between Figures 8(a)–8(d) shows that the tracking error of PD control in joint position and speed is far more than that of the proposed control method, testing the effectiveness of the proposed control method indirectly.Figure 8
Comparison of control methods. (a) PD control error of joint angle. (b) PD control error of joint angular velocity. (c) Improved control joint angular error. (d) Improved control joint error angular velocity.
(a)(b)(c)(d)For the dual-arm coordinated transportation experiment, the mass of the object is set as 10 kg, the expected trajectory of the dual arm is the planned dual-arm coordinated trajectory, and the other control parameters remain unchanged. The transportation process and trajectory are shown in Figures9 and 10.Figure 9
Gazebo transport process.Figure 10
The joint information of dual arm and the end-effector trajectory in Gazebo. (a) Joint angular error of the left arm. (b) Joint angular velocity error of left arm. (c) Joint torque of left arm. (d) Joint angular error of right arm. (e) Joint angular velocity error of right arm. (f) Joint torque of right arm. (g) End trajectory of dual arm.
(a)(b)(c)(d)(e)(f)(g)Figure8 shows that the task of dual-arm coordinated transportation can be completed by the dual-arm coordinated trajectory planning and the dual-arm coordinated transportation method proposed in this paper. The mass of the block in Figure 8 is 10 kg, which indirectly verifies the strong anti-interference ability of the proposed control algorithm in the process of coordinated transportation.To further verify the transportation effect of the dual arm, the joint information is derived from the Gazebo, and the tracking error of the dual arm, the joint torque, and the trajectory of the transportation end are drawn in Figure9.It can be seen from Figures10(a), 10(b), 10(d), and 10(e) that the tracking position error curve of the dual arm in the process of transportation (affected by the interference force when the end-effector contacts with the object) is in a smooth state as a whole, and the maximum error difference is less than 0.004 rad. In addition, the tracking speed error curve is in a small vibration curve, the maximum error is not more than 0.03 rad/s, and the average error is less than 0.01 rad/s. In Figure 10(g), the end-effector trajectory is almost the same as the expected trajectory. In conclusion, the effectiveness of the coordinated planning method, the high tracking accuracy, and the robustness of the control method are all verified. Figures 10(c) and 10(f) show that the output torque of the manipulator is smooth without mutation, confirming the rationality of joint trajectory optimization control.


## 7. Conclusion

Aiming at the problem of impact and disturbance of dual-arm coordinated transportation, a dual-arm cooperative trajectory optimization control method based on time-varying constraint output state was proposed, and three aspects of coordination planning, trajectory optimization, and joint control were mainly discussed.(1)
Considering the joint impact problem in the next step, this paper adopts the master-slave path planning method, plans the left arm joint trajectory with nonuniform fifth B-spline, and takes the coordinated motion as the constraint condition. And the joint displacement, joint velocity, and joint acceleration of the right arm by combining the instantaneous differential motion relationship were calculated. Thus, the calculation was simplified and the accuracy of the Jacobian inverse matrix could be avoided.(2)
To solve the joint impact problem, the main arm was taken as the joint optimization objective and the joint optimization objective equation was established. At the same time, to overcome the local extremum defect of traditional genetic algorithm with elite strategy, a genetic algorithm based on elite mechanism and individual random variable optimization was also proposed.(3)
For the disturbance and trajectory tracking problems, a PD joint position control method with time-varying output constraints was proposed on the premise of overcoming the complex modeling problems of various controllers. By the combination with the master-slave framework, a dual-arm robot control system was built.(4)
The rationality of the proposed optimization algorithm was verified by trajectory optimization experiments. Based on the optimization algorithm, three schemes were compared and analyzed, and the second scheme was the best one.(5)
The effectiveness of this method was tested by the simulation of Gazebo.



---

*Source: 5338134-2021-08-16.xml*