# Trajectory Planning of Robotic Arm Based on Particle Swarm Optimization Algorithm
## 2 Trajectory Planning
### Pose Planning  
Cartesian space trajectory planning includes both pose planning and velocity planning. This is realized through the design of the position of the robot end manipulator and the combination of the acceleration and deceleration control models. The pose planning of the manipulator consists of solving the function of the pose of the end manipulator in the workspace with respect to time. The basic idea is described below. Firstly, the path sequence in Cartesian space is obtained, and the interpolation algorithm is applied to realize it. Consequently, the inverse kinematic operation is performed on the space coordinates for each joint constituting the manipulator. This operation maps the position and the attitude of a given point in Cartesian space to the position and attitude of each joint in joint space. Two commonly employed spatial interpolation methods are line interpolation and arc interpolation. Trajectory planning in Cartesian space allows the determination of the specific path of the manipulator's end effector in 3D space. This path is defined by the specific task assigned to the manipulator. During line interpolation, the starting point \(P_{1}(x_{1},y_{1},z_{1})\) and the ending point \(P_{2}(x_{2},y_{2},z_{2})\) of the terminal manipulator are expressed as follows:  
\[s=\sqrt{(x_{2}-x_{1})^{2}+(y_{2}-y_{1})^{2}+(z_{2}-z_{1})^{2}} \tag{1}\]\[\left\{ \begin{array}{l}x_{k}=x_{1}+\lambda_{k}(x_{2}-x_{1})\\ y_{k}=y_{1}+\lambda_{k}(y_{2}-y_{1})\\ z_{k}=z_{1}+\lambda_{k}(z_{2}-z_{1})\end{array} \right. \tag{2}\]  
where \(\lambda_{k}\) is the normalization factor (\(\lambda_{k}\in[0,1]\)) and \(s\) denotes the straight-line distances between two points. In the arc interpolation, for any not-on-a-line three points \(P_{1}(x_{1},y_{1},z_{1})\), \(P_{2}(x_{2},y_{2},z_{2})\), and \(P_{3}(x_{3},y_{3},z_{3})\) expressed in the Cartesian coordinate system, the steps that apply the arc planning for these three points are defined as follows:  
Step 1: According to the coordinates of the three points in space, the curve equation of the arc can be calculated with reference to the placement of these three. Then, the center coordinate \(P_{0}(x_{0},y_{0},z_{0})\) and radius \(r\) of the arc are generated. Let the equation of the plane where \(P_{1}\), \(P_{2}\), and \(P_{3}\) are located be \(Ax+By+Cz+D=0\). Step 2: Set up the relative coordinate system \(o^{\prime}-x^{\prime}y^{\prime}z^{\prime}\) for the plane of the space arc within the absolute coordinate system \(o-xyz\). The origin \(o^{\prime}\) of this relative coordinate system is the center \(P_{0}\) of the arc. Let the direction of the axis \(x^{\prime}\) be \(P_{0}P_{1}\), as depicted in Figure 1. According to coordinates \(P_{0}\) and \(P_{1}\), the direction cosine of the axis \(x^{\prime}\) in the relative coordinate system can be expressed as follows:  
\[\overrightarrow{n}=\left  
Let the homogeneous coordinate of any point \(P\) in the absolute coordinate system be \( \begin{bmatrix}x&y&z&1\end{bmatrix} ^{T}\); then, the coordinate of point \(P\) in the relative coordinate system \(o^{\prime}-x^{\prime}y^{\prime}z^{\prime}\) is \( \begin{bmatrix}x^{\prime}&y^{\prime}&z^{\prime}&1\end{bmatrix} ^{T}\). As a result, one can write  
\[ \begin{bmatrix}x^{\prime}&y^{\prime}&z^{\prime}&1\end{bmatrix} ^{T}=T^{-1} \begin{bmatrix}x^{\prime}&y^{\prime}&z^{\prime}&1\end{bmatrix} ^{T} \tag{6}\]  
Step 3: Find the angle \(\theta\) between the starting point and the ending point of the arc. Referring to Equation (6), the corresponding coordinates of the three points \(P_{1}\), \(P_{2}\), and \(P_{3}\) in space using the relative coordinate system are denoted as \({P_{1}}^{\prime}(x_{1}{}^{\prime},y_{1}{}^{\prime},0)\), \({P_{1}}^{\prime}(x_{2}{}^{\prime},y_{2}{}^{\prime},0)\), and \({P_{1}}^{\prime}(x_{3}{}^{\prime},y_{3}{}^{\prime},0)\). Moreover, let the position angle at the end of the whole arc trajectory interpolation process be \(\theta\). The following is available:  
\[\theta=\left\{