# Multi-Robot Trajectory Planning and Position/Force Coordination Control in Complex Welding Tasks
## 3 Task-Oriented Multi-Robot Trajectory Planning
### Trajectory Planning Strategy
\begin{array}{l}\mathbf{T}_{bi}^{w}\cdot\mathbf{T}_{i}^{bi}(\mathbf{q}_{i})\cdot \mathbf{T}_{m}^{i}=\mathbf{T}_{m}^{w}\\ \mathbf{T}_{o}^{w}\cdot\mathbf{T}_{m}^{o}(t)=\mathbf{T}_{m}^{w}\end{array} \right., \tag{8}\]where \(\mathbf{T}_{bi}^{w}\) is homogeneous transform representing the robot base frame \(\mathbf{T}_{bi}\) with respect to the world frame \(\mathbf{T}_{w}\). \(\mathbf{T}_{i}^{bi}(\mathbf{q}_{i})\) is homogeneous transform representing the end-effector frame of robot \(\mathbf{T}_{i}\) with respect to its base frame \(\mathbf{T}_{bi}\). \(\mathbf{T}_{m}^{i}\) is homogeneous transform representing the mass frame of object \(\mathbf{T}_{m}\) with respect to the end-effector frame of robot \(\mathbf{T}_{i}\). \(\mathbf{T}_{m}^{w}\) is homogeneous transform representing the mass frame of object \(\mathbf{T}_{m}\) with respect to the world frame \(\mathbf{T}_{w}\). \(\mathbf{T}_{o}^{w}\) is homogeneous transform representing the object frame \(\mathbf{T}_{o}\) with respect to the world frame \(\mathbf{T}_{w}\). From Equation (8), the kinematics of the \(i\)-th manipulator can be obtained as  
\[ \begin{split}\mathbf{T}_{i}^{bi}(q_{i})&=(\mathbf{T}_{bi} ^{w})^{-1}\cdot\mathbf{T}_{o}^{w}\cdot\mathbf{T}_{m}^{o}(t)\cdot(\mathbf{T}_{m}^{i})^{-1} \\ &=\mathbf{T}_{w}^{bi}\cdot\mathbf{T}_{o}^{w}\cdot\mathbf{T}_{m}^{o}(t)\cdot \mathbf{T}_{i}^{m},\end{split}  \tag{9}\]  
where \(\mathbf{T}_{w}^{bi}\), \(\mathbf{T}_{o}^{w}\) and \(\mathbf{T}_{i}^{m}\) are constant matrix. In the entire cooperative welding task, the transfer robots coordinate the workpiece to meet the requirements of the ship-type welding. The welding robot doesn't need to adjust the posture during the whole welding process, only the position of the welding torch in the operated object coordinate system needs to adjust. The position transformation matrix \(\mathbf{p}_{3}^{o}\) of the tip of the welding torch which relative to the reference coordinate system of the operated object can be expressed as Equation (10). \[p_{3}^{o}=\left( \begin{array}{c}r\cdot\cos\alpha\cdot\cos\beta\cdot\cos \theta-r\cdot\cos\alpha\cdot\cos\beta\cdot\sin\theta+\sin\alpha\cdot\sqrt{R^{2 }-r^{2}\cdot\sin^{2}\!\theta}\\ r\cdot\sin\beta\cdot\cos\theta+r\cdot\cos\alpha\cdot\cos\theta\\ -r\cdot\sin\alpha\cdot\cos\beta\cdot\cos\theta+r\cdot\sin\alpha\cdot\sin \beta\cdot\sin\theta+\cos\alpha\cdot\sqrt{R^{2}-r^{2}\cdot\sin^{2}\!\theta} \end{array} \right). \tag{10}\]  
According to the conversion relationship between the welding robot and the operated object, and the coordinate transform between the welding robot and the world coordinate system, Equation (11) can be obtained. \[\left\{ \begin{array}{c}\mathbf{T}_{b3}^{w}\cdot\mathbf{T}_{3}^{b3}(\mathbf{q}_{i})=\bm {T}_{3}^{w}\\ \mathbf{T}_{o}^{w}\cdot\mathbf{T}_{3}^{o}(t)=\mathbf{T}_{3}^{w}\end{array} \right. \tag{11}\]  
According to the Equation (11), the motion of the welding robot relative to its own coordinate system can be obtained as in Equation (12). \[\mathbf{T}_{3}^{b3}(\mathbf{q}_{i})=\mathbf{T}_{w}^{b3}\cdot\mathbf{T}_{o}^{w}\cdot\mathbf{T}_{3}^ {o}(t). \tag{12}\]  
According to Equations (9) and (11), the trajectory of the transfer robot and the welding robot in their own coordinate system can be obtained. Based on the solution formula of inverse kinematics, the trajectory of each robot's joint angles can be obtained.