# Solving the Time-Varying Inverse Kinematics Problem for the Da Vinci Surgical Robot
## 2 Kinematics Analysis
### Mathematical Model of Inverse Kinematics  
In the case of inverse kinematics, the known pose matrix is \({}^{0}{}_{6}\mathbf{T}\) in Equation (3), and the unknowns are \(q_{1}\), \(q_{2}\), \(q_{3}\), \(q_{4}\), \(q_{5}\) and \(q_{6}\). Left-multiplying the matrices \(\mathbf{T}=\{\mathbf{q}_{3}\}^{y^{-1}}\{\mathbf{q}_{2}\}_{z}^{-1}\{\frac{\pi}{2}\}_{y}^{-1} \{-\left(\beta-\frac{\pi}{2}\right)\}_{x}^{-1}\{l_{QH}\}^{z^{-1}}\{\mathbf{q}_{1} \}_{z}^{-1}\{-(\pi-\alpha)\}_{x}^{-1}\) on both sides of Equation (3) yields Equation (4):  
\[\mathbf{T}_{6}^{0}\mathbf{T}=\left\{-\frac{\pi}{2}\right\}_{x}\{\mathbf{q}_{4}\}_{z}\Big{\{} \frac{\pi}{2}\Big{\}}_{x}\{\mathbf{q}_{5}\}_{z}\{l_{IJ}\}^{y}\Big{\{}\frac{\pi}{2} \Big{\}}_{y}\{\mathbf{q}_{6}\}_{z}. \tag{4}\]  
To simplify the complexity of the formula, we construct a unitary matrix [26]\(\mathbf{U}\) as follows. After introducing the matrix \(\mathbf{U}\), the variables in the \({}^{0}{}_{6}\mathbf{T}\) can be replaced by the Euler formula. \[\mathbf{U}=\left[ \begin{array}{cccc}\frac{1}{\sqrt{2}}i&-\frac{1}{\sqrt{2}}i&0&0 \\ \frac{1}{\sqrt{2}}&\frac{1}{\sqrt{2}}&0&0\\ 0&0&1&0\\ 0&0&0&1\end{array} \right] \tag{5}\]  
In addition, we know that \(\mathbf{E}=\mathbf{U}^{-1}\mathbf{U}\). Left-multiply the inverse of unitary matrix \(\mathbf{U}^{-1}\) and right-multiply the matrix \(\mathbf{U}\) on both sides of Equation (4). Then, multiply the \(\mathbf{E}\) matrix between these two matrices to transform Equation (4) into the following:  
\[\mathbf{Y}_{3}\mathbf{X}_{2}\mathbf{C}_{1}\mathbf{X}_{1}\mathbf{C}_{2}\mathbf{C}_{3}=\mathbf{C}_{4}\mathbf{X} _{4}\mathbf{C}_{5}\mathbf{X}_{5}\mathbf{C}_{6}\mathbf{X}_{6}. \tag{6}\]  
Here, \(\mathbf{C}_{1}=\mathbf{U}^{-1}[|l_{\text{QH}}]^{2}\{\pi/2-\beta|_{2}\}^{-1}\mathbf{U}\), \(\mathbf{C}_{2}=\mathbf{U}^{-1}[|\alpha-\pi|_{x}]^{-1}\mathbf{U}\), \(\mathbf{C}_{3}=\mathbf{U}^{-10}{}_{6}\mathbf{T}\mathbf{U}\), \(\mathbf{C}_{4}=\mathbf{U}^{-1}\{-\pi/2|_{x}\mathbf{U}\), \(\mathbf{C}_{5}=\mathbf{U}^{-1}[\pi/2]_{x}\mathbf{U}\), and \(\mathbf{C}_{6}=\mathbf{U}^{-1}[\{\mu\}]^{y}[\pi/2]_{y}\mathbf{U}\). Clearly, \(\mathbf{C}_{1}\), \(\mathbf{C}_{2}\), \(\mathbf{C}_{3}\), \(\mathbf{C}_{4}\), \(\mathbf{C}_{5}\) and \(\mathbf{C}_{6}\) are constant matrices or parameter matrices related to the structure of the robot. In addition, \(\mathbf{X}_{j}\) (\(j=1\), 2, 4, 5, 6) is as follows:  
\[\mathbf{X}_{j}=\left\{ \begin{array}{ll}T_{Xj}^{-1}&j=1,2\\ T_{Xj}&j=4,5,6\end{array} \right., \tag{7}\]  
where \(\mathbf{T}_{Xj}\) (j = 1, 2, 4, 5, 6) represents the results of the transform matrix of \(q_{j}\) rotating around the z-axis, which undergoes the left-multiplication of \(\mathbf{U}^{-1}\) and right-multiplication of \(\mathbf{U}\). Let \(\mathbf{x}_{j}=\cos(q_{j})+i\sin(q_{j})\), \(\mathbf{x}_{j}^{-1}=\cos(q_{j})-i\sin(q_{j})\); thus, \(\mathbf{T}_{Xj}\) can be expressed as follows:  
\[\mathbf{T}_{Xj}=\mathbf{U}^{-1}\{\mathbf{q}_{i}\}_{z}\mathbf{U}=\left[ \begin{array}{cccc}\bm {x}_{j}&0&0&0\\ 0&\mathbf{x}_{j}^{-1}&0&0\\ 0&0&1&0\\ 0&0&0&1\end{array} \right]. \tag{8}\]  
Let \(\mathbf{Y}_{j}\) represents the result of the transform matrix \(\mathbf{q}_{j}\) along the y-axis, which undergoes the left-multiplication of \(\mathbf{U}^{-1}\) and right-multiplication of \(\mathbf{U}\), and \(\mathbf{x}_{j}=\mathbf{q}_{j}\); then, \(\mathbf{Y}_{j}\) can be expressed as follows:  
\[Y_{j}=\mathbf{U}^{-1}\{\mathbf{q}_{i}\}^{y}\mathbf{U}=\left[ \begin{array}{cccc}1&0&0&- \frac{\sqrt{2}}{2}\mathbf{x}_{j}\\ 0&1&0&-\frac{\sqrt{2}}{2}\mathbf{x}_{j}\\ 0&0&1&0\\ 0&0&0&1\end{array} \right]. \tag{9}\]  
Take \(\mathbf{A}=\mathbf{Y}_{3}\mathbf{X}_{2}\mathbf{C}_{1}\mathbf{X}_{1}\mathbf{C}_{2}\mathbf{C}_{3}\), \(\mathbf{B}=\mathbf{C}_{4}\mathbf{X}_{4}\mathbf{C}_{5}\mathbf{X}_{5}\mathbf{C}_{6}\mathbf{X}_{6}\). Then the dimensions of matrix \(\mathbf{A}\) and matrix \(\mathbf{B}\) are both 4 \(\times\) 4.