# Trajectory Planning and Optimization for a Par4 Parallel Robot Based on Energy Consumption
## 3 Trajectory Planning of the Par4 Parallel Robot
### Space-Path Coordinate Transform
#### 3.3.2 Piecewise Motion Planning Based on the Lame Curve  
Based on Figures 2 and 3, there are five segments in the gate-shaped pick-and-place movement: The AB segment rising vertically, the BC segment rising along the Lame curve, the CF segment moving horizontally, the FG segment falling along the Lame curve and the GH segment falling vertically. Due to the symmetry of the motion trajectory, the following section will mainly study the motion planning of the AB segment, BC segment, and CF segment in the **U**O**'**W** plane local coordinate system, and finally convert the planning to the space coordinate OXYZ. 1. AB Segment Motion Planning  
The displacement range is \(0\leq s(t)<(H_{a}-f)\), with vertical motion only. The spatial coordinates of point A are set as \((x_{A},y_{A},z_{A})\), and the current time is denoted as \(t_{i}\), so, the displacement, velocity and acceleration laws in the space coordinate OXYZ can be shown in Equations (11)-(13):  
\[\left[ \begin{array}{cccc}x_{i}&y_{i}&z_{i}\end{array} \right]=\left[ \begin{array} []{cccc}x_{A}&y_{A}&z_{A}+s(t_{i})\end{array} \right] \tag{11}\]  
\[\left[ \begin{array}{cccc}v_{xi}&v_{yi}&v_{zi}\end{array} \right]=\left[ \begin{array}{cccc}0&0&s^{\prime}(t_{i})\end{array} \right] \tag{12}\]  
\[\left[ \begin{array}{cccc}a_{xi}&a_{yi}&a_{zi}\end{array} \right]=\left[ \begin{array}{cccc}0&0&s^{\prime\prime}(t_{i})\end{array} \right]. \tag{13}\]  
2. BC Segment Motion Planning
* Displacement Planning  
When the displacement is taken as \((H_{a}-f)\leq s(t)<(H_{a}-f)+I_{BC}\), it moves along the Lame curve. Taking the previous running point to be \(k_{i-1}\), the current running point is \(k_{i}\), as indicated in Figure 3. The key is to calculate the interpolation values \(\Delta u_{i}\) and \(\Delta w_{i}\). In Figure 3, there is the increment of arc length \(\Delta l_{i}=s(t_{i})-s(t_{i-1})\), in which \(t_{i}\) represents the present time, and \(t_{i-1}\) the previous time. According to Equation (4), Equation (14) is deduced:  
\[u_{i}^{\prime}(w)=-(w_{i}^{2}e^{3})/(u_{i}^{2}f^{3}) \tag{14}\]  
When the interpolation time is \(\Delta t_{i}\to 0\), then \(\Delta l_{i}\to 0\). The approximate arc differential calculation formula can be obtained as in Equation (15):  
\[\Delta w_{i}\approx\Delta l_{i}/\sqrt{1+[u_{i}^{\prime}(w)]^{2}}. \tag{15}\]Thus, \[w_{i}=w_{i-1}+\Delta w_{i}.\] (16) The following can be obtained from Equations (4) and (16): \[u_{i}=e[1-(w_{i}/f)^{3}]^{1/3}\] (17) \[\Delta u_{i}=u_{i}-u_{i-1}.\] (18) According to the analysis above, to avoid \(u_{i}^{\prime}(w)\rightarrow\infty\) when \(k_{i}\) is approaching the W axis, \(w_{i}^{\prime}(u)=-(u_{i}^{2}f^{3})/(w_{i}^{2}e^{3})\) can be calculated first with Equation (4), and in the same way, \(\Delta u_{i}\) can be calculated first, before calculating \(u_{i}\), \(w_{i}\), and \(\Delta w_{i}\) in turn. There is a strong duality between the two calculated methods. In order to reduce calculation errors, when \((H_{a}-f)\leq s(t)<(H_{a}-f)+l_{BC}/2\), the former method is adopted; when \((H_{a}-f)+l_{BC}/2\leq s(t)<(H_{a}-f)+l_{BC}\), the latter method is adopted. The coordinate transformation can be attained as  
\[\left[ \begin{array}{c}x_{i}\\ y_{i}\\ z_{i}\end{array} \right]=\left[ \begin{array}{cc}\lambda_{11}&\lambda_{13}\\ \lambda_{21}&\lambda_{23}\\ \lambda_{31}&\lambda_{33}\end{array} \right]\left[ \begin{array}{c}u_{i}\\ w_{i}\end{array} \right]+\left[ \begin{array}{c}\lambda_{14}\\ \lambda_{24}\\ \lambda_{34}\end{array} \right] \tag{19}\]  
\[\text{If }\mathbf{X}_{i}=\left[ \begin{array}{c}x_{i}\\ y_{i}\\ z_{i}\end{array} \right],\mathbf{Q}_{1}=\left[ \begin{array}{cc}\lambda_{11}& \lambda_{13}\\ \lambda_{21}&\lambda_{23}\\ \lambda_{31}&\lambda_{33}\end{array} \right],\mathbf{Q}_{2}=\left[ \begin{array}{ cc}\lambda_{14}&\lambda_{24}&\lambda_{34}\end{array} \right]^{\!\!T},\mathbf{U}_{i}= \left[ \begin{array}{cc}u_{i}&w_{i}\end{array}