# Solving the Time-Varying Inverse Kinematics Problem for the Da Vinci Surgical Robot
## 2 Kinematics Analysis
### Mathematical Model of Inverse Kinematics
Each element in matrix \(\mathbf{A}\) is a function containing \(\mathbf{x}_{1}\), \(\mathbf{x}_{2}\), and \(\mathbf{x}_{3}\), while the elements in matrix \(\mathbf{B}\) are functions containing \(\mathbf{x}_{4}\), \(\mathbf{x}_{5}\), and \(\mathbf{x}_{6}\). In addition, corresponding elements in matrix \(\mathbf{A}\) and matrix \(\mathbf{B}\) are equal, i.e., \(Aij=\mathbf{B}ij\). The specific contents of the elements are detailed in Appendix A. Therefore,  
\[\mathbf{x}_{4}=-\frac{i}{I_{IJ}}\mathbf{A}^{L2}-\frac{\sqrt{2}i}{I_{IJ}}\mathbf{A}^{13}=f_{3}( \mathbf{x}_{1},\mathbf{x}_{2},\mathbf{x}_{3}). \tag{18}\]  
Similarly, by combining \(\mathbf{A}_{13}=\mathbf{B}_{13}\) and \(\mathbf{A}_{14}=\mathbf{B}_{14}\) with Equation (16), the following can be obtained:  
\[\left[ \begin{array}{c}\mathbf{x}_{5}\\ \mathbf{x}_{5}^{-1}\end{array} \right]=\left[ \begin{array}{cc}-\frac{\sqrt{2}}{( \mathbf{A}^{L2}i-l_{IJ})}&-\frac{\sqrt{2}ill_{IJ}}{(\mathbf{A}^{L2}i-l_{IJ})}\\ -\frac{\sqrt{2}}{(\mathbf{A}^{L2}-l_{IJ})}&-\frac{\sqrt{2}ll}{(\mathbf{A}^{L2}-l_{IJ} )}\end{array} \right]\left[ \begin{array}{c}\mathbf{A}_{14}\\ \mathbf{A}_{13}\end{array} \right]. \tag{19}\]  
Therefore, \(\mathbf{x}_{5}\) can be represented by \(\mathbf{x}_{1}\), \(\mathbf{x}_{2}\), and \(\mathbf{x}_{3}\):  
\[\mathbf{x}_{5}=-\frac{\sqrt{2}\mathbf{A}_{14}}{\left(\mathbf{A}^{L2}i-l_{IJ}\right)}-\frac {\sqrt{2}il_{IJ}\mathbf{A}_{13}}{\left(\mathbf{A}^{L2}i-l_{IJ}\right)}=f_{4}(\mathbf{x}_{1 },\mathbf{x}_{2},\mathbf{x}_{3}). \tag{20}\]  
Combined with Equations (16) to (20) and \(\mathbf{A}_{11}=\mathbf{B}_{11}\), \(\mathbf{x}_{6}\) can finally be represented by \(\mathbf{x}_{1}\):  
\[\mathbf{x}_{6}=\frac{(b_{1}i+b_{2})\mathbf{x}_{1}\mathbf{x}_{2}^{-1}+(b_{3}i+b_{4})\mathbf{x} _{2}^{-1}+(b_{5}i+b_{6})\mathbf{x}_{1}^{-1}\mathbf{x}_{2}^{-1}}{\frac{1}{8}\mathbf{x}_{5} \mathbf{x}_{4}+\frac{i}{4}\mathbf{x}_{4}-\frac{1}{8}\mathbf{x}_{5}^{-1}\mathbf{x}_{4}+\frac{1 }{4}\mathbf{x}_{5}+\frac{1}{4}\mathbf{x}_{5}^{-1}+\frac{1}{8}\mathbf{x}_{5}\mathbf{x}_{4}^{-1} -\frac{i}{4}\mathbf{x}_{4}^{-1}-\frac{1}{8}\mathbf{x}_{5}^{-1}\mathbf{x}_{4}^{-1}}=f_{5}( \mathbf{x}_{1}), \tag{21}\]  
where \(b_{\rm j}\) is a constant related to the robot's parameters. From Equations (16) to (21), the variables \(\mathbf{x}_{2}\), \(\mathbf{x}_{3}\), \(\mathbf{x}_{4}\), \(\mathbf{x}_{5}\), and \(\mathbf{x}_{6}\) can be represented with the variable \(\mathbf{x}_{1}\). Let \(\mathbf{A}_{24}\) and \(\mathbf{B}_{24}\) be equal, then:  
\[F(\mathbf{x}_{1})=\mathbf{A}_{24}-\mathbf{B}_{24}=0. \tag{22}\]  
By solving Equation (22), \(\mathbf{x}_{1}\) can be solved, then \(\mathbf{x}_{2}\), \(\mathbf{x}_{3}\), \(\mathbf{x}_{4}\), \(\mathbf{x}_{5}\), and \(\mathbf{x}_{6}\) can be subsequently solved by Equation (16) to (21). Specifically, we know from Equation (14) that \(\mathbf{x}_{2}=\pm\sqrt{f_{1}(\mathbf{x}_{1})}\), therefore, \(F(\mathbf{x}_{1})\) will have two expressions:  
\[F(\xi_{1})=\left\{ \begin{array}{c}F_{1}(\mathbf{x}_{1})\mathbf{x}_{2}=\sqrt{f_{1}( \mathbf{x}_{1})}\\ F_{2}(\mathbf{x}_{1})\mathbf{x}_{2}=-\sqrt{f_{1}(\mathbf{x}_{1})}\end{array} \right.. \tag{23}\]  
Equation (23) will be electively solved according to the minimum norm of the distance the joint moved.