## 3 The 3D Motion-Planning Algorithm
### The 3D Intraoperative Motion Planner
```
1:\(Q_{\textit{bestnodes}}=\textit{BestNode}\big{(}t_{\textit{prebest}}\big{)}\)
2:\(for(i=1;\ i<ValidTreeNum;\ i++)\)
3:\(t_{i}=InitializeTree()\);
4:\(Q_{\textit{environment}}=ObsAndTxtUpdate(q_{ins},q_{tar},Q_{obs})\);
5:\(E=\textit{EnvironmentConstruction}(Q_{\textit{environment}})\);
6:\(if(i==1)\)
7:\(DirectTrajectory(t_{i})\);
8:continue;
9:end
10:\(for(j=1;\ j\leq maxIteration;\ j++)\)
11:\(if\Big{(}\Big{\|}q_{\textit{new}}-q_{\textit{goal}}\Big{\|}<d\Big{)}\)
12:\(T_{\textit{valid}}.AddTree(t_{i})\);
13:\(break\);
14:\(end\)
15:\(if(i<\textit{thresnum}\ \&\&\ \textit{bestexist}==false)\)
16:\(q_{\textit{samp}}=\textit{BestSelection}(Q_{\textit{bestnodes}},t_{i},E)\);
17:\(else\)
18:\(q_{\textit{samp}}=\textit{RandomSample}(P_{\textit{threshold}})\);
19:\(end\)
20:\(q_{\textit{near}}=\textit{NearestNode}(q_{\textit{samp}},t_{i})\);
21:\(if\big{(}ConstrainsFree(q_{\textit{near}},\ q_{\textit{samp}},E)==true\big{)}\)
22:\(u=\textit{InputComputation}(q_{\textit{near}},\ q_{\textit{samp}})\);
23:\(else\)
24:\(continue\);
25:\(end\)
26:\(q_{\textit{new}}=\textit{VarSegLen}(q_{\textit{near}},\ q_{\textit{samp}},u,\ \textit{s},\textit{direct})\);
27:\(Q_{\textit{neighbor}}=\textit{FindNeighbor}(q_{\textit{new}},\ t_{i},\ r)\);
28:\(t_{i}=\textit{OptNewNodePar}\Big{(}q_{\textit{new}},\ Q_{\textit{neighbor}},t_{i}\Big{)}\);
29:\(t_{i}=\textit{OptNeighborPar}\Big{(}q_{\textit{new}},\ Q_{\textit{neighbor}},\ t_{i}\Big{)}\);
30:\(t_{i}.AddSeq(q_{\textit{new}})\);
31:\(t_{i}.AddSegment(q_{\textit{near}},q_{\textit{new}},u)\);
32:\(end\)
33:\(t_{i}.CostComputation()\);
34:\(end\)
35:\(t_{\textit{best}}=\textit{SelectBestTrajectory}(T_{\textit{valid}})\)
36:\(return\ t_{\textit{best}}\)
```

**Algorithm 2**Intraoperative Motion Planning Algorithm

## 4 Feedback Controller Design

During update time, a feedback controller can accurately track the proximal segment of the best trajectory provided by the motion planner and feed the end point of the proximal segment to the motion planner as the insertion point for the next update round. The above procedures are repeated until the needle reaches the target. A feedback controller is required to complete this task. ### Chained Form Presentation

The kinematics in Equation (6) indicates that the steering system is nonholonomic. In [32], a method was introduced to convert a nonholonomic system into a chained form system, for which the design of the feedforward and feedback controllers are straightforward. The kinematics can be transformed into the two chained forms with three new inputs \((u_{1},u_{2},u_{3})\) and seven states \((\xi_{1},\xi_{2},\xi_{3},\xi_{4},\xi_{5},\xi_{6},\xi_{7})\), as follows:

\[ \begin{split}\dot{\xi}_{1}=u_{1}&\dot{\xi}_{2}=u_{2 }\qquad\dot{\xi}_{5}=u_{3}\\ \dot{\xi}_{3}=\xi_{2}u_{1}\qquad\dot{\xi}_{6}=\xi_{5}u_{1}\\ \dot{\xi}_{4}=\xi_{3}u_{1}\qquad\dot{\xi}_{7}=\xi_{6}u_{1}.\end{split}  \tag{13}\]

By choosing \(\xi_{1}=x\), \(\xi_{4}=y\), and \(\xi_{7}=z\), the kinematics can be expressed as follows:

\[\xi= \begin{bmatrix}\xi_{1}\\ \xi_{2}\\ \xi_{3}\\ \xi_{4}\\ \xi_{5}\\ \xi_{6}\\ \xi_{7}\end{bmatrix} =M(q)= \begin{bmatrix}x\\ k\delta_{offset}sec^{3}\gamma sec\beta\\ tant\gamma\\ y\\ k\delta_{offset}sec^{2}\gamma tant\gamma sec\dot{\beta}tant\beta+k\delta_{in}sec^{2} \gamma sec^{3}\beta\\ sec\gamma tant\beta\\ z\end{bmatrix} , \tag{14}\]

where \(M\) is a function that converts the original state \(q\) into the chained form state \(\xi\). Once the desired trajectory is given, the control input \(u\) can be computed using the chained form state. The original input \(v\) can be computed as follows:\[v= \begin{bmatrix}N_{11}&N_{12}&N_{13}\\ N_{21}&N_{22}&N_{23}\\ N_{31}&N_{32}&N_{33}\end{bmatrix}