## 3 The 3D Motion-Planning Algorithm
### The 3D Intraoperative Motion Planner
A test was performed to identify 100 trajectories and optimize the maxIteration value based on the results, which are presented in Figure 7. The success rate refers to the probability of finding an optimal trajectory, the cost time refers to the time required to find 100 trajectories (including available and unavailable trajectories), and the mean iterations refers to the average number of iterations required to find an optimal trajectory. Herein, the _maxIteration_ was set to 40 because the success rate was almost saturated, while the cost time and mean iteration were at acceptable levels. The \(P_{threshold}\) is another parameter that has to be optimized, and to maintain the expansion ability of random numbers in unknown spaces, this value should not be large. A similar test was conducted to optimize this value, and based on the performance results, the \(P_{threshold}\) was set to 15%. The remaining parameters, namely the initial insertion position \( \begin{bmatrix}x_{0}&y_{0}&z_{0}\end{bmatrix} \) and initial insertion angle \( \begin{bmatrix}\beta_{0}&\gamma_{0}\end{bmatrix} \), are listed in Table 1.

Figure 6: The results of the comparison between the static planning algorithms. Figure 7: Optimization of the \(maxIteration\) parameter. A feedback controller is needed to track the path generated by the motion planner. A simulation system for the feedback controller was built for tuning the control gains. The simulation sequence was identical to that of the block diagram depicted in Figure 5, except that it excluded the motion planning part. The control gains \(k_{i}\) decide the root placement of the feedback system and the gains should be set properly, such that the root of the control system is on the right side of plane \(s\) and the tracking error is convergent. The gains were tuned by trial and error, and the optimal values \([k_{1},k_{2},k_{3},k_{4},k_{5},k_{6},k_{7}]\) were tuned to \([0.1,0.8,0.2,0.02,0.8,0.2,0.02]\). The remaining parameters for the control system, such as the steering coefficient \(k\) and the initial steering offset \(\left[\delta_{in0}\quad\delta_{off0}\right]\), are listed in Table 1.

### Evaluation of the Optimal Control Algorithm

In the preoperative stage, when the positions of the initial insertion point, initial target point, and obstacle points are known, the static motion-planning algorithm generates 200 trajectories, as depicted in Figure 8, and selects the best candidate among them. In dynamic environments, the relative motion (with respect to the initial position) of the obstacles \(\Delta Pobs\) and target \(\Delta Ptar\) are defined using sinusoidal functions, as follows:

\[ \begin{array}{l}\overset{\rightarrow}{P}(\theta_{1},\,\theta_{2})=\left[cos \theta_{1}cos\theta_{2}\quad sin\theta_{1}cos\theta_{2}\quad-sin\theta_{2} \right]^{T}\\ \Delta Pobs=8\sin\left(\frac{\pi}{25}t\right)\overset{\rightarrow}{P},\\ \Delta Ptar=5\sin\left(\frac{\pi}{300}t\right)\overset{\rightarrow}{P}\end{array}  \tag{23}\] \begin{table}
\begin{tabular}{c c c} \hline \hline
**Parameters** & & **Values** \\ \hline Max. iterations & \(maxIteration\) & 40 \\ Goal bias & \(P_{threshold}\) & 15\% \\ Insertion position & \([x_{0},\,y_{0}\,\,z_{0}]\) & \([0\,\,\text{mm}\,\,\,0\,\text{mm}\,\,\,0\,\text{mm}]\) \\ Insertion angle & \([\beta_{0}\,\,\,\gamma_{0}]\) & \([0^{\circ}\,\,\,0^{\circ}]\) \\ Desired insertion velocity & \(v_{d1}\) & 1 mm/s \\ Steering coefficient & \(k\) & \(3\times 10^{-3}\,\,\text{mm}^{-2}\) \\ Initial steering offset & \([\delta_{in0}\,\,\delta_{off0}]\) & \([0\,\,\text{mm}\,\,\,0\,\text{mm}]\) \\ Control gains & \(\begin{bmatrix}&k_{1}\\ k_{2}&k_{3}&k_{4}\\ k_{5}&k_{6}&k_{7}\end{bmatrix}\) & \(\begin{bmatrix}&0.1\\ 0.8&0.2&0.021\\ 0.8&0.2&0.021\\ 0.8&0.2&0.021\end{bmatrix}\) \\ \hline \hline \end{tabular}
\end{table} 
Table 1: Parameters used for the simulation. Figure 8: The feasible trajectories generated by the preoperative motion-planning algorithm.