# A New Foot Trajectory Planning Method for Legged Robots and Its Application in Hexopod Robots
## 4 The Three-Element Trajectory Determination Method
### Trajectory Planning in Support Phase
#### 4.2.2 Joint Angle Changes Interpolation Function  
The joint angle changes interpolation function is defined as \(\theta_{\rm P}(t)=[\theta_{\rm P\_1}(t),\theta_{\rm P\_2}(t),\theta_{\rm P\_3} (t)]\), where \(\theta_{\rm P\_j}(t),(j=1,2,3)\) is the angle changes interpolation function for the \(jth\) joint. Considering the continuity of the velocity as well as the acceleration, for each joint, \(\theta_{\rm P\_j}(t)\) is a quartic polynomial, as described in Equation (17):  
\[\theta_{\rm P\_j}(t)=a_{4}t^{4}+a_{3}t^{3}+a_{2}t^{2}+a_{1}t+a_{0}. \tag{17}\]  
where \(a_{0}\) to \(a_{4}\) are the parameters of \(\theta_{\rm P\_j}(t)\) and will be different according to different joints.  
To ensure the continuity of the joint angle, the joint angle changes at the start and end time should be zero. To ensure the stability of the joint rotation, the velocity of the joint angle changes at the start, and the end time should also be zero.  
\[\left\{\begin{array}{l}\theta_{\rm P\_j}(t)(T_{f})=0,\\ \theta_{\rm P\_j}(t)(2T_{f})=0,\\ \theta_{\rm P\_j}(t)^{\prime}(T_{f})=0,\\ \theta_{\rm P\_j}(t)^{\prime}(2T_{f})=0.\end{array}\right. \tag{18}\]  
To realize the symmetry of the angle changes, we hope that \(\theta_{\rm P\_j}(t)\) arrives at the set joint angle change \(\Delta\theta_{j}\) at the middle time of this phase; thus, we have the following.  
\[\theta_{\rm P\_j}(\frac{3}{2}T_{f})=\Delta\theta_{j}. \tag{19}\]  
Then, the relationship matrices (20) and (21) can be obtained as follows.  
\[\left[\begin{array}{c}0\\ \Delta\theta_{j}\\ 0\\ 0\\ 0\\ \end{array}\right]=\left[\begin{array}{cccc}{T_{f}}^{4}&{T_{f}}^{3}&{T_{f}} ^{2}&{T_{f}}&1\\ (\frac{3}{2}{T_{f}})^{4}&(\frac{3}{2}{T_{f}})^{3}&(\frac{3}{2}{T_{f}})^{2}& \frac{3}{2}{T_{f}}&1\\ (2T_{f})^{4}&(2T_{f})^{3}&(2T_{f})^{2}&2T_{f}&1\\ 4{T_{f}}^{3}&3{T_{f}}^{2}&2T_{f}&1&0\\ 4(2T_{f})^{3}&3(2T_{f})^{2}&2(2T_{f})&1&0\\ \end{array}\right]\left[\begin{array}{c}a_{4}\\ a_{3}\\ a_{2}\\ a_{1}\\ a_{0}\\ \end{array}\right]. \tag{20}\]  
\(\Delta\theta_{j}\) is set according to the real terrain situation. Based on Formula (21), the parameters \(a_{0}\) to \(a_{4}\) can be solved, and \(\theta_{\rm P\_j}(t)\) is obtained.  
By superimposing \(\theta_{\rm B}(t)\) and \(\theta_{\rm P}(t)\), the trajectory function of the transfer phase \(\theta_{\rm T}(t)\) in joint-space is described as follows (22).  
\[\theta_{\rm T}(t)=\theta_{\rm B}(t)+\theta_{\rm P}(t)=\{\theta_{1}(2T_{f}-t)+ \theta_{\rm P\_1}(t),\theta_{2}(2T_{f}-t)+\theta_{\rm P\_2}(t),\theta_{3}(2T_{ f}-t)+\theta_{\rm P\_3}(t)\}\enspace. \tag{22}\]By combining \(\theta_{\text{S}}\)(t) in support phase and \(\theta_{\text{T}}\)(t) in transfer phase together, the total trajectory of one joint during a movement period in joint-space is defined as the dotted line in Figure 7. The red dotted line is \(\theta_{\text{S}}\)(t). The blue dotted line is \(\theta_{\text{T}}\)(t), and it is the superposition of the green line \(\theta_{\text{B}}\)(t) and the pink line \(\theta_{\text{P}}\)(t).