## 3 The 3D Motion-Planning Algorithm
### The 3D Intraoperative Motion Planner
Once the reference trajectory and the measured states are available, the desired input is computed using Equation (17), and the compensated input is computed using Equation (20). Equation (15) is used to compute the original input to the kinematics. The _Joint Velocity Conversion_ function converts the kinematic input into joint input by using Equation (7), and a proportional-derivative (PD) controller is used to guarantee accuracy in the joint space. ## 5 Simulation Results

The simulation system used to evaluate the optimal control system was built in the MATLAB software environment, on a PC equipped with a 2.3 GHz 8-core Intel i7 CPU. The working environment was modeled in a workspace measuring \(200\times 200\times 200\) mm\({}^{3}\). Five spherical obstacles, with diameters of \(10\sim 15\) mm, were set in this environment. The initial target was placed at a depth of 185 mm and the initial insertion point was placed at the origin of the coordinate system. The weight coefficients of the cost function were set to \(c_{1}=0.1\), \(c_{2}=0.5\), and \(c_{3}=0.4\). ### Motion Planner Comparison

To evaluate the performance of the static motion planner proposed herein, we compared the underlying static algorithm to the conventional goal-biased RRT* algorithm, and the difference between these two algorithms was the _VarSegLen_ function. In the conventional goal-biased RRT* algorithm, the new point is the end point of a truncated segment, with a predefined arc length. This is identical to the case in which the Boolean value _direct_ is false in the proposed static motion-planning algorithm; otherwise, the new point is the target point. For a fair comparison, the parameters of these two algorithms were identical, and they will be optimized later. The simulation was carried out with the task of generating 100 trajectories and the test was repeated five times to increase the reliability of the comparison results. The results are depicted in Figure 6 and the performance was evaluated in terms of the average node number, the average trajectory costs, and the total time required to complete the task. The green bars denote the results of the conventional goal-biased RRT* algorithm and the yellow bars denote the results of the proposed static motion-planning algorithm. According to these results, the trajectory generated by the static motion-planning algorithm consists of 3.73 nodes, which is almost half the number of nodes generated by the conventional algorithm. The smaller the number of nodes in a trajectory, the smaller the angle the needle requires to rotate. In terms of cost, the comparison results indicate that the algorithm proposed herein can generate a much more optimized trajectory than that generated by the conventional algorithm. The results of the total time required indicate the algorithm proposed herein is computationally efficient. In addition, the static motion-planning algorithm is superior as a sampling-based planning algorithm, and the performance of the intraoperative algorithm obtained by modifying the static algorithm is evaluated later in this work. Figure 5: A block diagram of the optimal CCS needle-steering system. ### Parameter Optimization

To enhance the performance of the needle-steering system, the parameters of the optimal controller have to be optimized. The optimal controller consists of a motion planner and a feedback controller, both of which should be tuned; _maxIteration_ is a parameter of the motion planner. An optimal trajectory may not be found with a small _maxIteration_ value, it may also waste time if the feasible trajectory does not exist when this value is too big.