A discrete number of points of the bronchus centerline are defined as the desired CR tip trajectory for problem simplification, and the last point on this trajectory is set as the final target. Deriving a close form for the inverse kinematics is challenging when considering CCMs, as the constant curvature assumption cannot be applied. For that, we developed a semi-analytical method to calculate the shape of the robot when attempting to reach a particular target within its workspace. The 3-DOF deflection of the robot has been considered for that. Given a starting base position, we aimed to calculate the deflection of the robot, the external rotation and the base translation along the longitudinal direction that would allow the tip of the robot to reach a point.
The required rotation angle is calculated by finding the angle between the normal vector of the current deflection plane and the normal vector of the desired deflection plane, as in Eqs. (
9) and (
10). The normal
\(\mathbf {n_{1}}\) of the current deflection plane is found by computing the cross-product of
\(\mathbf {B}\) and
\(\mathbf {T}\), while the normal of the desired deflection plane,
\(\mathbf {n_{2}}\), is found by computing the cross-product between the vector that connects the target point with the origin and
\(\mathbf {T}\).
$$\begin{aligned} |\alpha |= & {} \arctan {\frac{\left||\mathbf {n_{2}}\times \mathbf {n_{1}}\right||}{\mathbf {n_{2}} \cdot \mathbf {n_{1}}}} \end{aligned}$$
(9)
$$\begin{aligned} {\alpha }= & {} \text {sgn}(\left||\mathbf {n_{2}}\times \mathbf {n_{1}}\right||\cdot \mathbf {T})\cdot |\alpha | \end{aligned}$$
(10)
In order to deflect the robot to align its tip
y-coordinate with the
y-coordinate of the target, the maximum and minimum changes of cable length—
\(\Delta L_{\mathrm{min}}\) and
\(\Delta L_{\mathrm{max}}\)—that the robot can hold given the CCMs constraints are computed. Then, given these cable length constraints, a constrained minimization problem can be formulated to find the deflection and robot shape that minimizes the distance between the tip
y-coordinate
\(\mathbf {p}_{\mathrm{rot}\_\text {defl}\_\text {tip}_{y}}\) and the target
y-coordinate
\(\mathbf {p}_{\mathrm{target}_{y}}\) in the local frame, which can be easily solved iteratively:
$$\begin{aligned} \begin{aligned}&\text {minimize}&\mathbf {p}_{\mathrm{rot}\_\text {defl}\_\text {tip}_{y}}-\mathbf {p}_{\mathrm{target}_{y}}\\&{\text {subject to}}&\Delta L \in [\Delta L_{\mathrm{min}},\Delta L_{\mathrm{max}}] \end{aligned} \end{aligned}$$
(11)
Allowing the base of the robot to move along the centerline makes this method not applicable, as the translation stops being linear. Hence, an iterative method is implemented to quickly find the required external rotation
\(\alpha \), deflection input
\(\Delta L\) and translation along the centerline
\(d_{\mathrm{centerline}}\) for the robot to reach a particular target at this stage of the trajectory. Although previous work had suggested the use of the gradient-based optimization solver
fmincon from MATLAB
® to solve the inverse kinematics [
12], this function was found to get stuck in local minima. Instead, the MATLAB
® global search method
GlobalSearch was found to provide good results. Given a trajectory of
m steps, we also aim to minimize the robot collision cost function at each step,
\(c_{j}(\mathbf {q})\), with
\(j \in [1,m]\). Redundant configurations need to be explored to find the deflection and translation that give the least collision. Thus, the inverse kinematics problem can be formulated as a nonlinear, constrained optimization problem:
$$\begin{aligned} \begin{aligned}&\mathbf {q}^{*} = \mathop {\hbox {argmin}}\limits _{q} c_{j}(\mathbf {q}) \\&\text {subject to: } ||{f(\mathbf {q})-\mathbf {p}_{\mathrm{target}_{j}}} ||= 0 \end{aligned} \end{aligned}$$
(12)
where
\(\mathbf {q}^{*}\) is the optimal set of parameters that defines the best motion plan to perform a step and reach the next step target
\(\mathbf {p}_{\mathrm{target}_{j}}\) and
\(f(\mathbf {q})\) is the function that uses the set of parameters
\(\mathbf {q}\) to find the robot tip position
\(\mathbf {p}_{\mathrm{tip}_{j}}\).
We calculate the robot collision with the anatomy at a particular step
\(c_{j}(\mathbf {q})\) with a delta (
\(\varDelta \)) function that checks if the robot structure collides with the anatomy by checking whether a discrete number of points along the centerline of the robot are inside the eroded lumen of the airways, as in [
9]. The center of the cable-guide disks and tip positions are used for this purpose. It is then defined as:
$$\begin{aligned} c_{j}(\mathbf {q}) = \sum _{i=1}^{n+1}\Delta ({\mathbf {p}}) \end{aligned}$$
(13)
where
\(\mathbf {p}\) represents the center of the disks and tip positions for that particular robot configuration defined by
\(\mathbf {q}\).
Table 1
Summary of the design optimization results
Fixed base |
Joint bending limits | \(20^{\circ }\) for all the joints | \(40^{\circ }\) for the first two joints. \(5^{\circ }\) for the rest |
Objective function | 36.60 | 18.86 |
Moving base up to 5 mm |
Joint bending limits | \(20^{\circ }\) for all the joints | \(40^{\circ }\) for the first two joints. \(5^{\circ }\) for the rest |
Objective function | 35.24 | 17.65 |
Moving base up to 7.5 mm |
Joint bending limits | \(20^{\circ }\) for all the joints | \(35^{\circ }\) for the first two joints. \(5^{\circ }\) for the rest |
Objective function | 25.44 | 6.5 |
Moving base up to 10 mm |
Joint bending limits | \(20^{\circ }\) for all the joints | \(30^{\circ }\) for the first two joints. \(5^{\circ }\) for the rest |
Objective function | 2.66 | 0.11 |