# Multi-Robot Trajectory Planning and Position/Force Coordination Control in Complex Welding Tasks
## 3 Task-Oriented Multi-Robot Trajectory Planning
### Trajectory Planning Strategy
\[\max f(x,y,z)=\frac{k_{dm}\cdot\mathrm{DATBDMM}_{cv}+k_{fm}\cdot\mathrm{FM}}{ k_{gje}\cdot\mathrm{GJE}}, \tag{2}\]  
where \(f\) represents a multi-objective optimal function, \(k_{dm}k_{fm}k_{gje}\) represent the influence factors corresponding to the degree of dual-directional operation of the task, the flexibility, and the amount of joint change, respectively. According to the importance of each parameter and our preliminary experience, the value of \(k_{dm}\) is set to 0.5, \(k_{fm}\) is set to 0.3 and \(k_{gje}\) is set to 0.2. In the second step, the trajectory of the operated object in the reference coordinate system is planned according to the welding task. The workpiece coordinate system which formed by the pipe connection is shown in Figure 3, the weld formed by the pipe is a saddle-shaped curve. The principle of establishing the reference coordinate system of the operated object is, the intersection point of the pipe center-line is set as the origin of the coordinate system, the \(z\)-axis is along the center-line of the upper tube and the \(x\)-axis is along the center-line of the lower tube. In Figure 3, \(C\) is a point on the saddle-shaped curve, and the coordinate in  
Figure 3: The schematic diagram of the object coordinate system and welding coordinate system. system of the operated object is \(C(x,y,z)\). It can use the parametric equation to represent the equation of the saddle-shaped curve in the reference coordinate system as shown in Equation (3). \[\left\{ \begin{array}{l}x=r\cdot\cos\theta\\ y=r\cdot\sin\theta\\ z=\sqrt{R^{2}-r^{2}\cdot\sin^{2}\theta}\end{array} \right., \tag{3}\]  
where \(r\) is the radius of the upper tube \(R_{b}\), \(R\) is the radius of the lower tube \(R_{a}\), and \(\theta\) is the rotation angle parameter. The solder point \(C\) can be adjusted to the ship-type welding posture by two-step, the weld coordinate system rotates \(\alpha\) around the \(z_{object}\), and rotates \(\beta\) around the \(x_{object}\). Where \(\beta\in(0,2\pi)\), \(\alpha\) is the angle between \(z_{weld}\) and the reference coordinate system \(z_{o}\) of the operated object. The formula is shown in Equation (4). \[\alpha=\cos^{-1}\frac{\sqrt{R^{2}-r^{2}\cdot\sin^{2}\theta}}{R\cdot\sqrt{2+2 \cdot\frac{r}{R}\cdot\sin^{2}\theta}}. \tag{4}\]  
Based on the above analysis, the trajectory of the operated object in its reference coordinate system can be obtained as shown in Equation (5). \[\left\{ \begin{array}{l}x=0\\ y=0\\ z=0\\ R=f\left(2\pi\cdot i\cdot t/T\right)\\ P=0\\ Y=2\pi\cdot i\cdot t/T\end{array} \right., \tag{5}\]  
where \(i\) denotes the \(i\)-th communication cycle, \(t\) denotes the total time which is required for the welding task, and \(T\) denotes the interpolation period. \(f(\cdot)\) is a variable attitude function, it can be expressed as Equation (6). \[f(\theta)=\cos^{-1}\frac{\sqrt{R^{2}-r^{2}\sin^{2}\theta}}{R\sqrt{2+2\frac{r}{ R}\sin^{2}\theta}}. \tag{6}\]  
In the third step, the trajectory of each robot is planned according to the constraint relationship between the robot's end-effector and the operated object. The movement of the operated object \(m\) (assumed coincident with the \(\mathbf{T}_{object}\)) in \(\mathbf{T}_{o}\) can be described as \(\mathbf{T}_{m}^{o}(t)\). \(\mathbf{T}_{m}^{o}(t)\) is obtained according to the welding task (Equation (5)), and it can be expressed as Equation (7). \[\mathbf{T}_{m}^{o}(t)=\left[ \begin{array}{cc}\mathbf{R}_{m}^{o}(t)&\mathbf{p}_{m}^{o}(t )\\ 0&1\end{array} \right], \tag{7}\]  
where \(\mathbf{R}_{m}^{o}(t)\) represents the \((3\times 3)\) rotation matrix of the centroid with respect to the reference coordinate system of the operated object, and \(\mathbf{p}_{m}^{o}(t)\) represents the \((3\times 1)\) position matrix of the centroid with respect to the reference coordinate system of the operated object. According to the closed-chain constraint formed by the transfer robot and the operated object, the pose constraint relationship of the following Equation (8) can be obtained. \[\left\{