## 3 The 3D Motion-Planning Algorithm
### The 3D Intraoperative Motion Planner
u \tag{15}\] \[N_{11}=sec\gamma sec\beta\] \[N_{12}=0\] \[N_{13}=0\] \[N_{21}=-3k\delta_{off}{}^{2}sec\gamma tan\gamma sec\beta-k\delta_{ off}\delta_{in}sec\gamma sec\beta tan\beta\] \[N_{22}=\frac{1}{k}cos^{3}\gamma cos\beta\] \[N_{23}=0\] \[N_{31}=k\delta_{off}{}^{2}sec\gamma tan^{2}\gamma sin\beta-k \delta_{off}{}^{2}sec^{3}\gamma sin\beta\] \[\qquad-3k\delta_{off}\delta_{in}sec\gamma tan\gamma sec\beta-3k \delta_{in}{}^{2}sec\gamma sec\beta tan\beta\] \[N_{32}=-\frac{1}{k}cos^{2}\gamma sin\gamma cos^{2}\beta sin\beta\] \[N_{33}=\frac{1}{k}cos^{2}\gamma cos^{3}\beta.\]

### Controller Design

The position, velocity, and acceleration information of the desired trajectory, generated by the intraoperative motion planner, are available. The states of the CCS needle can be computed using \(L(x_{d},y_{d},z_{d})\), as follows:

\[ \begin{array}{l}\xi_{d1}=x_{d}\\ \xi_{d2}=\left(\dot{x}_{d}\ddot{y}_{d}-\ddot{x}_{d}\dot{y}_{d}\right)/\ddot{x }_{d}^{3}\\ \xi_{d3}=\dot{y}_{d}/\dot{x}_{d}\\ \xi_{d4}=y_{d}\\ \xi_{d5}=\left(\dot{x}_{d}\ddot{z}_{d}-\ddot{x}_{d}\dot{z}_{d}\right)/\ddot{x }_{d}^{3}\\ \xi_{d6}=\dot{z}_{d}/\dot{x}_{d}.\end{array}  \tag{16}\]

The desired input \(u_{d}\) into the system can be derived as:

\[ \begin{array}{l}u_{d1}=\dot{\xi}_{d1}=\dot{x}_{d}\\ u_{d2}=\dot{\xi}_{d2}=\frac{\dot{z}_{d}^{2}\ddot{y}_{d}-\dot{x}_{d}\dot{y}_{d}^{ 3}-3\ddot{x}_{d}\ddot{x}_{d}\ddot{y}_{d}+3\dot{x}_{d}^{2}\dot{y}_{d}}{\dot{x}_ {d}^{4}}\\ u_{d3}=\dot{\xi}_{d3}=\frac{\dot{x}_{d}^{2}\ddot{z}_{d}-\dot{x}_{d}\dot{x}_{d}^{ 2}-3\ddot{x}_{d}\ddot{x}_{d}^{2}\ddot{z}_{d}+3\dot{x}_{d}^{2}\dot{z}_{d}}{\dot{x }_{d}^{4}}.\end{array}  \tag{17}\]

However, the needle will deviate from the desired trajectory owing to several factors, such as the external disturbance and tissue inhomogeneity. The system errors can be defined as follows:

\[\begin{array}{l}\dot{e}_{1}=\dot{\xi}_{d1}-\dot{\xi}_{1}=u_{d1}-u_{1}=e_{u1 }\\ \dot{e}_{2}=\dot{\xi}_{d2}-\dot{\xi}_{2}=u_{d2}-u_{2}=e_{u2}\\ \dot{e}_{3}=\dot{\xi}_{d2}u_{d1}-\xi_{2}u_{1}=e_{2}u_



\(\xi_{d6}=\xi_{6}\). The abovementioned equation can be presented as the following state space equation:

\[\dot{e}=A\dot{e}+B\dot{e}_{u}\] \[A= \begin{bmatrix}0&0&0&0&0&0&0\\ 0&0&0&0&0&0&0\\ 0&u_{d1}&0&0&0&0&0\\ 0&0&u_{d1}&0&0&0&0\\ 0&0&0&0&0&0&0\\ 0&0&0&0&u_{d1}&0&0\\ 0&0&0&0&0&u_{d1}&0\end{bmatrix}  \tag{19}\] \[e= \begin{bmatrix}e_{1}\\ e_{2}\\ e_{3}\\ e_{4}\\ e_{5}\\ e_{6}\\ e_{7}\end{bmatrix} ,\;B= \begin{bmatrix}1&0&0\\ 0&1&0\\ \xi_{d2}&0&0\\ \xi_{d3}&0&0\\ 0&0&1\\ \xi_{d5}&0&0\\ \xi_{d6}&0&0\end{bmatrix} ,\;e= \begin{bmatrix}e_{u1}\\ e_{u2}\\ e_{u3}\end{bmatrix} .\]

To compensate for the system error, the full-state feedback control algorithm is employed, and its input is set as follows:

\[ \begin{split} e_{u1}&=k_{1}e_{1}\\ e_{u2}&=k_{2}e_{2}+\frac{k_{3}}{u_{2}}e_{3}+\frac{k_{4}}{u_{2}^{2}}e_{4}\\ e_{u3}&=k_{5}e_{5}+\frac{k_{6}}{u_{d1}}e_{6}+\frac{k_{7}}{u_{d1}^{2}}e_{7}. \end{split}  \tag{20}\]

Upon substituting Equation (20) into (19), the system matrix changes from \(A\) to \(A_{fs}\), and the eigenvalue of the feedback system is:

\[\det\Bigl{(}\lambda I-A_{fs}\Bigr{)}=(\lambda+k_{1})\Bigl{(}\lambda^{3}+k_{2} \lambda^{2}+k_{3}\lambda+k_{7}\Bigr{)}\Bigl{(}\lambda^{3}+k_{2}\lambda^{2}+k_ {3}\lambda+k_{7}\Bigr{)}. \tag{21}\]

These eigenvalues can be manipulated easily by tuning the control gains \([k_{1},k_{2},k_{3},k_{4},k_{5},k_{6},k_{7}]\). The input into the chained system is:

\[u=u_{d}+e_{u}. \tag{22}\]

A block diagram of the optimal control system for steering a CCS needle is depicted in Figure 5. The overall system consists of a motion planning algorithm, a high-level controller, and a low-level controller. In the preoperative stage, the static motion planner selects the best trajectory as the input to the intraoperative motion planner. In the intraoperative stage, the motion planner optimizes the best trajectory based on the updated environmental information, and the optimized best trajectory is fed into the high-level controller and the intraoperative motion planner.