## 3 The 3D Motion-Planning Algorithm
### Kinematic Constrains and Cost Function  
The tip of a CCS needle is asymmetrical and, therefore, its insertion trajectory consists of several circular segments, as illustrated in Figure 3a. CCS needle robots are a nonholonomic system. Therefore, the needle insertion trajectory should be continuous and smooth, and adjacent segments should have a common tangential direction at their intersection point. In addition, the curvature of each circular segment depends on the control strategy and mechanical properties of the tissue. Given that use of the duty cycle control strategy is prohibited, linear segments cannot be generated. The minimal and maximal curvatures of the resulting circular segments are \(\rho_{min}=k\delta_{min}>0\) and \(\rho_{max}=k\delta_{max}\), respectively. Where \(\delta_{min}\) and \(\delta_{max}\) denote the minimal and maximal stylet translations (steering offset), respectively.  
The objective of the motion planner is to generate a feasible and optimal trajectory from among several options. The goodness of the generated trajectory can be evaluated from the perspectives of safety and trauma reduction, among other factors. Herein, each segment \(S\) generated by the motion planner is evaluated in terms of its cost function, as follows:  
\[c(S)=c_{1}f_{L}+c_{2}f_{R}+c_{3}f_{C}, \tag{9}\]  
where \(f_{L}\), \(f_{R}\), and \(f_{C}\) denotes the relative segment length, needle rotation angle, and clearance of the obstacle, respectively. Moreover, \(c_{i}(i=1,2,3)\) denote the weights of the three aforementioned optimization criteria, and their sum is equal to one.  
A short trajectory length can reduce the trauma caused to patients. Therefore, the first optimization term is the segment length. In Figure 3a, the absolute length of the blue segment is shorter than that of the green segment, and the curvature of the blue segment \(\rho_{b}\) is larger than that of the green segment \(\rho_{a}\). For each trajectory, the insertion depth from the start point to the target point is fixed. The blue segment requires another dashed segment to reach the same insertion depth as the green segment, and their total absolute length is longer than that of the green segment. Therefore, the relative segment length \(f_{L}\) is adopted instead of the absolute segment length, as follows:  
\[f_{L}=\frac{\rho_{i}}{\rho_{min}}, \tag{10}\]  
where \(\rho_{i}\) is the curvature of segment \(i\). The second optimization term is the rotation angle of the needle shaft:  
\[f_{R}=\theta_{i}, \tag{11}\]  
The needle needs to rotate from the end point of the last segment to the start point of the new segment. As illustrated in Figure 3b, the blue segment is better than the green segment because \(\theta_{b}<\theta_{a}\), which means that the damage cause by the needle rotation in the blue segment is less than that in the green segment. The third optimization term is related to segment safety. As illustrated in Figure 3c, the green segment is superior to the blue segment because the blue segment is considerably closer to the obstacle than the green segment, and this proximity is defined as follows:  
\[f_{c}=\begin{cases}2^{\frac{C_{d}}{d_{ij}}}&d_{ij}<10\\ 0&d_{ij}\geq 10\end{cases} \tag{12}\]  
where \(C_{d}\) is a constant, and \(d_{ij}\) is the distance between the \(i_{th}\) point of segment \(j\) to the center of an obstacle.