## 3 The 3D Motion-Planning Algorithm
### The 3D Intraoperative Motion Planner
```
1:\(Q_{cniu}=ObsAndTarInit(q_{ins},q_{tar},Q_{obs})\);
2:\(for(i=1;\ i<ValidTreeNum;\ i++)\)
3:\(t_{i}=InitializeTree()\);
4:\(E=EnvionmentConstruction(Q_{cniv})\);
5:\(for(j=1;\ j\leq maxIteration;\ j++)\)
6:\(if\left(\left\|q_{new}-q_{goal}\right\|<d\right)\)
7:\(T_{valid}.AddTree(t_{i})\);
8:\(break\);
9:\(end\)
10:\(q_{samp}=RandomSample(P_{threshold})\);
11:\(q_{narr}=NearestNode(q_{samp},t_{i})\);
12:\(if\left(ConstrainsFree(q_{narr},\ q_{samp},E)==true\right)\)
13:\(u=InputComputation(q_{near},\ q_{samp})\);
14:\(else\)
15:continue;
16:\(end\)
17:\(q_{new}=VarSegLen(q_{narr},\ q_{samp},u,\ s,direct)\);
18:\(Q_{neighbor}=FindNeighbor(q_{new},\ t_{i},\ r)\);
19:\(t_{i}=OptNewNodePar(q_{new},\ Q_{neighbor},t_{i})\);
20:\(t_{i}=OptNeighborPar(q_{new},\ Q_{neighbor},\ t_{i})\);
21:\(t_{i}.AddNode(q_{new})\);
22:\(t_{i}.AddSegment(q_{narr},q_{new},u)\);
23:\(end\)
24:\(t_{i}.CostComputation()\);
25:\(end\)
26:\(t_{best}=SelectBestTrajectory(T_{valid})\);
27:\(return\ t_{best}\)
```  
**Function 1:**\(RandomSample()\)  
\(Input:\{P_{threshold}\}\)\(Output:\{q_{samp}\}\)  
```
1:\(p=rand(0\sim 1)\);
2:\(if(p<P_{threshold})\)
3:\(q_{samp}=q_{goal}\);
4:\(direct=true\);
5:else
6:\(q_{samp}=RandomNode()\);
7:\(direct=false\);
8:end
9:return\(q_{samp}\)
```  
**Algorithm 1**Static Motion Planning Algorithm  
```
1:\(Q_{cniu}=ObsAndTarInit(q_{ins},q_{tar},Q_{obs})\);
2:\(for(i=1;\ i<ValidTreeNum;\ i++)\)
3:\(t_{i}=InitializeTree()\);
4:\(E=EnvionmentConstruction(Q_{cniv})\);
5:\(for(j=1;\ j\leq maxIteration;\ j++)\)
6:\(if\left(\left\|q_{new}-q_{goal}\right\|<d\right)\)
7:\(T_{valid}.AddTree(t_{i})\);
8:\(break\);
9:\(end\)
10:\(q_{samp}=RandomSample(P_{threshold})\);
11:\(q_{narr}=NearestNode(q_{samp},t_{i})\);
12:\(if\left(ConstrainsFree(q_{narr},\ q_{samp},E)==true\right)\)
13:\(u=InputComputation(q_{narr},\ q_{samp})\);
14:\(else\)
15:\(continue\);
16:\(end\)
17:\(q_{new}=VarSegLen(q_{narr},\ q_{samp},u,\ s,direct)\);
18:\(Q_{neighbor}=FindNeighbor(q_{new},\ t_{i},\ r)\);
19:\(t_{i}=OptNewNodePar(q_{new},\ Q_{neighbor},t_{i})\);
20:\(t_{i}=OptNeighborPar(q_{new},\ Q_{neighbor},\ t_{i})\);
21:\(t_{i}.AddNode(q_{new})\);
22:\(t_{i}.AddSegment(q_{narr},q_{new},u)\);
23:\(end\)
24:\(t_{i}.CostComputation()\);
25:\(end\)
26:\(t_{best}=SelectBestTrajectory(T_{valid})\);
27:\(return\ t_{best}\)
```  
**Function 2:**\(RandomSample()\)  
```
1:\(p=rand(0\sim 1)\);
2:\(if(p<P_{threshold})\)
3:\(q_{samp}=q_{goal}\);
4:\(direct=true\);
5:else
6:\(q_{samp}=RandomNode()\);
7:\(direct=false\);
8:end
9:return\(q_{samp}\)
```  
**Algorithm 2**Static Motion Planning Algorithm  
```
1:\(P_{threshold}=P_{threshold}\)
2:\(P_{threshold}=P_{threshold}\)
3:\(P_{threshold}=P_{threshold}\)
4:\(P_{threshold}=P_{threshold}\)
5:\(P_{threshold}=P_{threshold}\)
6:\(P_{threshold}=P_{threshold}\)
7:\(P_{threshold}=P_{threshold}\)
8:\(P_{threshold}=P_{threshold}\)
9:\(P_{threshold}=P_{threshold}\)
10:\(P_{threshold}=P_{threshold}\)
11:\(P_{threshold}=P_{threshold}

Unlike the conventional RRT algorithm that truncates the feasible segment with a constant arc length \(s\), the proposed algorithm uses the _VarSegLen_ function to identify the new point \(q_{new}\) that will be added to the tree. As expressed in the _VarSegLen_ function, if the Boolean value _direct_ is false, the new point is the end point of the segment that is truncated by the _NewNode_ function with a predefined arc length \(s\), as illustrated in Figure 4a. Otherwise, the new point is the sampling point. In this algorithm, the Boolean value is true only when the sampling point is the target point. Therefore, the _VarSegLen_ function can directly generate the segment from the nearest point to the target point. Given that the tree requires fewer sampling points (nodes) and segments, this function reduces the needle shaft rotation, thereby improving the steering safety. The superiority of this function is demonstrated in the experimental section.