# An Algorithm for Solving Robot Inverse Kinematics Based on FOA Optimized BP Neural Network
## 2 Robot Kinematics  
The robot torso coordinate system is established according to the D-H method, as shown in Figure 2.  
The system has four linkages and five coordinate systems. Here, \(\alpha_{i}\), \(a_{i}\), \(d_{i}\), and \(\theta_{i}\) respectively represent the connecting rod torsion angle, connecting rod length, connecting rod offset, and joint angle. According to the built robot kinematics model, Table 1 shows the link parameters of the robot. After defining the linkage coordinate system and the corresponding linkage parameters, the kinematic equation can be established. The D-H transformation matrix \(\overset{i-1}{i}T\) can be obtained by right multiplying the following four transformation matrices:  
\[ \begin{array}{llll}\overset{i-1}{i}T&=R_{x}(\alpha_{i-1})D_{x}(\alpha_{i-1} )R_{x}(\theta_{i})D_{z}(d_{i})\\ &=\left[\begin{array}{cccc}c\partial_{i}&-s\theta_{i}&0&a_{i-1}\\ s\theta_{i}c\alpha_{i-1}&c\partial_{i}c\alpha_{i-1}&-s\alpha_{i-1}&-s\alpha_{i- 1}d_{i}\\ s\theta_{i}s\alpha_{i-1}&c\theta_{i}s\alpha_{i-1}&c\alpha_{i-1}&c\alpha_{i-1}d_ {i}\\ 0&0&0&1\end{array}\right]\end{array}  \tag{1}\]  
where \(\overset{i-1}{i}T\) is the D-H transformation matrix transformed from coordinate system \(i\) to \(i-1\), \(R_{x}(\alpha_{i-1})\) is the rotation matrix representing a rotation of \(\alpha_{i-1}\) around the X axis, \(D_{x}(\alpha_{i-1})\) is the translation matrix representing a translation of \(a_{i-1}\) along the X axis, \(R_{z}(\theta_{i})\) is the rotation matrix representing a rotation of \(\theta_{i}\) around the Z axis, \(D_{z}(d_{i})\) is the translation matrix representing a translation of \(d_{i}\) around the Z axis, \(c\theta_{i}\) is short for \(\cos\theta_{i}\), \(s\theta_{i}\) is short for \(\sin\theta_{i}\), etc. The transformation matrices of each link determined by the link parameters \begin{table}
\begin{tabular}{c c c c c} \hline \hline \(i\) & \(\alpha_{i-1}\) (rad) & \(a_{i-1}\) (mm) & \(d_{i}\) (mm) & \(\theta_{i}\) (rad) \\ \hline
1 & \(-\pi/2\) & 0 & 0 & \(\theta_{1}\) \\
2 & \(\pi/2\) & 0 & \(l_{1}\) & \(\theta_{2}\) \\
3 & \(-\pi/2\) & 0 & 0 & \(\theta_{3}-\pi\) \\
4 & 0 & \(l_{2}\) & 0 & 0 \\ \hline \hline \end{tabular}
\end{table} 
Table 1: Link parameters of the robot. Figure 2: Robot kinematics model. are multiplied to obtain the position and posture of the center point of the robot's chest in the base coordinate system:  
\[ \begin{array}{c}{}^{0}_{4}T={}^{0}_{1}T{}^{1}_{2}T{}^{2}_{3}T{}^{3}_{4}T=\left[ \begin{array}{ccc}{}^{0}_{4}R{}_{3\times 3}&{}^{0}_{4}P{}_{3\times 1}\\ 0&1\end{array}\right]=\left[\begin{array}{ccc}r_{11}&r_{12}&r_{13}&p_{x}\\ r_{21}&r_{22}&r_{23}&p_{y}\\ r_{31}&r_{32}&r_{33}&p_{z}\\ 0&0&0&1\end{array}\right]\end{array}  \tag{2}\]  
where \({}^{0}_{4}R{}_{3\times 3}\) is the rotation transformation matrix transformed from coordinate system 4 to 0, \({}^{4}_{4}P{}_{3\times 1}\) is the position vector transformed from coordinate system 4 to 0, \(r_{ij}\) is the element of \({}^{0}_{4}R{}_{3\times 3}\), and \(p_{x}\), \(p_{y}\), \(p_{y}\) is the component of the translation vector \({}^{0}_{4}P{}_{3\times 1}\). The rotation matrix \({}^{0}_{4}R{}_{3\times 3}\) can be described by rotating around the axis of a fixed reference coordinate system, as shown in Formula (3):  
\[ \begin{array}{c}{}^{0}_{4}R{}_{XYZ}(\gamma,\beta,\alpha)=\left[\begin{array} []{ccc}cac\beta&cas\beta s\gamma-sac\gamma&cas\beta c\gamma+sas\gamma\\ sac\beta&sas\beta s\gamma+cac\gamma&sas\beta c\gamma-cas\gamma\\ -s\beta&c\beta s\gamma&c\beta c\gamma\end{array}\right]\end{array}  \tag{3}\]  
where \({}^{0}_{4}R{}_{XYZ}(\gamma,\beta,\alpha)\) is the equivalent rotation matrix that rotates \(\gamma\), \(\beta\), and \(\alpha\) around the X, Y, and Z axes of the fixed reference coordinate system. Equivalently derived X-Y-Z fixed angle from the rotation matrix:  
\[ \begin{array}{c}\beta=\mathrm{Atan2}(-r_{31}\sqrt{r_{11}^{2}+r_{21}^{2}}) \\ \alpha=\mathrm{Atan2}(r_{21}/c\beta,r_{11}/c\beta)\\ \gamma=\mathrm{Atan2}(r_{32}/c\beta,r_{33}/c\beta)\end{array}