Autonomous Scanning Target Localization for Robotic Lung Ultrasound Imaging

A. Related Works

Attempts have been made to develop tele-operative RUS systems for LUS applications. Tsumura et al. proposed a gantry-based platform for LUS using a novel passive-actuated force control mechanism that maintains constant contact force with the patient while achieving a large workspace [16]. Ye et al. implemented a remote teleoperated RUS system communicating via 5G network which was deployed to Wuhan, China, for COVID-19 patient diagnosis [17].

Though tele-operative systems allow remote and contact-less US imaging, the procedure is still heavily operator-dependent and may require special training on the user. In this sense, RUS systems with higher-level autonomy could be more clinically valuable. Huang et al. demonstrated a framework using an RGB-D camera to segment the scanning target via color thresholding, meanwhile computing the desired probe position and orientation from the point cloud data [18]. Virga et al. and Hennersperger et al. adopted the patient-to-MRI registration to solve for the probe pose that covers the organ to be examined [19], [20], Yorozu et al. and Jiang et al. implemented US confidence map [21] to find the optimal probe in-plane orientation for maximizing the US signal quality [22], [23]. Similarly, Visual-servoing is applied to adjust the probe’s rotation so that the artifact of interest is centered in the field of view (FOV) in the US image [24], [26], [27].

The workflow of a RUS system can be decomposed into two phases. In the first phase, the scanning target is identified in 3D space. A rough probe landing pose is estimated and converted into robot motion. The second phase further optimizes the probe placement, aiming to obtain a high-quality US image while assuring patient’s safety. While several previous studies have investigated the US probe placement optimization [18] – [27], few works explicitly tackle the first phase. Many of them (e.g. [19], [20]) require pre-operative inputs such as CT/MRI scan for each patient, compromising cost-effectiveness and accessibility. Some adopt patient recognition methods such as color thresholding (e.g. [18]), which involves environmental constraints, requiring further generalization. Without first automating the first phase, the second phase can hardly eliminate manual assistance for initial positioning, therefore downgrading the autonomous RUS approach.

B. Contribution

This work focuses on the scanning target localization, highlighted as the first phase in the previous section, to bring the autonomous RUS a step forward. The scanning targets are designed based on the BLUE protocol that has been used for LUS. For the purpose of recognizing scanning targets automatically and dynamically and being less subject to an individual patient, we implement a learning-based human pose estimation algorithm using an RGB-D camera in real-time. RGB images are used to localize targets first in 2D, and then the detected 2D targets are extended to 3D by combining with depth sensing. We also develop the pipeline of implementing the target localization with the RUS execution. A serial manipulator places the US probe vertical to each target based on the previous-step localization with the desired contact force. Once the probe is placed automatically, the operator can perform the further pose refinement manually by observing real-time US images displayed, as semi-autonomous scanning. The contributions of this work fold under two aspects. i) We introduce the novel framework for autonomous robotic US probe placement based on scanning target localization without requiring manual input. The pipeline combines human segmentation in 2D and vertical landing pose calculation in 3D for robotic US probe placement. ii) We implement and evaluate the safety and efficacy of the semi-autonomous RUS system for LUS diagnosis by imaging an upper torso mannequin and a lung ultrasound phantom.

A. Scanning Target Recognition with DensePose

DensePose uses the Mask R-CNN for instance segmentation and regresses the 2D coordinates representing anatomical locations as vertices on the SMPL model. It outputs a 3-channel matrix with the same dimension as the input image (see Fig. 1a). The first channel is the classification for each pixel (the class ID ranges from 0 to 24, where 0 refers to the background, 1 to 24 each corresponds to a part of the body, e.g., head, arm, leg, etc.). The second and third dimensions, denoted as set U and V, are the mapped 2D human mesh coordinates consisting of u (vertical) and v (horizontal) coordinates for all body parts. A 2D pixel [r, c]^⊤ can be one-to-one mapped into human mesh coordinates as [u = U(r, c), v = V (r, c)]^⊤. To map from mesh coordinates [u, v]^⊤ to a pixel [r, c]^⊤, which is not guaranteed to be one-to-one, (1) is adopted, where n is the number of possible [r_i, c_i]^⊤ pairs.

A graphical interpretation of the above workflow can be seen in Fig. 1b–d. The desired targets to be automatically detected must first be specified in [u, v]^⊤. Then, these targets in the pixel [r, c]^⊤ can be solved from arbitrary camera pose relative to the patient using (1) but referring to the same position on the chest ideally. The detailed implementation to define the reference target locations [u, v]^⊤ will be covered in Section III.

B. 3D Probe Pose Computation

Based on the recognized 2D scanning targets in the image space, we need to compute the end-effector pose of the robot under the camera frame normal to the scanning surface, expressed as a transformation matrix $T_{tar}^{cam}\in SE$ (3). Depth information is used to bridge 2D to 3D. We first align the depth frame to the color frame to ensure the 2D targets in the color image remain unchanged in the depth image before applying spatial filtering and hole filling techniques to smoothen the depth data. The deprojection operation $\mathcal{D}:{ℝ}^2\to {ℝ}^3$, which maps a pixel in the depth image to a 3D point in the point cloud, is provided as an API in RealSense SDK. Therefore, a general 2D target P₀ can correspond to the 3D point $P_0^{\prime }$ in the point cloud through $\mathcal{D}\left(P_0\right)$.

The calculation of the surface normal originated at $P_0^{\prime }$ is illustrated in Fig. 2. 3 unique points on a surface are required to determine the normal vector of a given surface. However, the depth measure introduces a significant amount of noise that leads to fluctuating 3D coordinates reading. Our solution is to find supplementary points neighboring to the target, providing additional normal vectors that can be averaged into a steady one. After specifying P₀, 8 more points (P₁ to P₈ in Fig. 2b) are indexed from the depth image such that they form a square patch with edge length L. Inside the patch, 8 sub-planes, each consisting of 3 points, can be found (S₁ to S₈ in Fig. 2b). Deprojection of P₀ to P₈ results in a surface where each sub-plane contributes a normal vector. Averaging and normalizing all 8 normal gives the estimated surface normal V_avg (see Figs. 3c–d).

The desired end-effector pose under the camera frame can be decomposed into the rotational part R_tar ∈ SO(3) and the translational part $P_{tar}=P_0^{\prime }$. The approach vector V_z in the rotation matrix should be −V_avg so that it points out-wards from the end-effector tip, leaving only the orientation vector V_x of the probe’s long axis undetermined. In lung examination, the long axis is usually parallel to the body’s midsagittal plane to visualize the ribs. In our case, V_x should be parallel to the x-axis of the robot base frame, then V_y can be calculated according to the right-hand rule. The final target pose is given in (2).

C. Position and Force Control of the Robot

The Franka Emika 7-DoF manipulator provides a direct Cartesian space control interface and is equipped with joint torque sensors in all 7 joints, making task space wrench estimation possible. The hardware setup of the robot can be seen in Fig. 3: we designed an end-effector in [10] that holds a linear ultrasound probe and the RealSense camera, mounted to the flange of the manipulator. Based on the mechanical data of the end-effector, the transformation $T_{cam}^{eef}$ from the end-effector frame F_eef to the camera frame F_cam is obtained. Further, the transformation $T_{tar}^{\mathit{\text{base}}}$ from the target frame F_tar to the robot base frame F_base can be solved from (3).

The robot operates under two different modes: non-contact mode for executing non-collision motion and contact mode for maintaining constant contact force with the tissue surface. For safety considerations, an entry pose F_ent above the target pose F_tar along V_avg with displacement C_d is set as the goal under non-contact mode instead of F_tar. Once the entry pose F_ent is reached, the robot enters contact mode where force is exerted by reducing C_d (see Fig. 3). The equation to set C_d is given below:

where k_p is the proportional gain, ${\widehat{F}}_z$ is the desired contact force along the end-effector’s z-axis, which in our case is set to 5 N, F_z is the estimated force along the end-effector’s z-axis read directly from the robot. With the orientation unchanged, the new translational component $P_{tar}^{\prime }$ after applying C_d is:

The control to the robot is a task space twist defined as [v_x, v_y, v_z, w_x, w_y, w_z], representing linear velocities along x, y, z axes and angular velocities along the three axes. The rotation part of $T_{tar}^{\mathit{\text{base}}}$ is converted to roll pitch and yaw angles to ease angular velocity calculation. A PD controller is implemented to command end-effector velocity using errors of translational components and roll pitch yaw angles, as well as derivative of the errors. The elbow joint angle is limited to 180 degrees to avoid singularities.

D. System Integration

The overall system diagram and a flowchart of the LUS procedure using the developed system are shown in Fig. 4. The software architecture in Fig. 4a is built upon ROS (Robot Operating System). DensePose runs at 2fps on 480 by 480 image input to infer the scanning targets, which are later converted to end-effector poses in terms of translational goal and rotational goal relative to the robot base by incorporating with depth sensing and robot pose measurement. Motion errors are then calculated to be fed into the PD controller, which gives the velocity twist command to the robot. Mean-while, a state machine is designed to determine a Boolean variable isContact that controls the switch between contact mode and non-contact mode. isContact is assigned to 1 if the robot’s motion error is within the sufficiently small threshold (2 mm in translation, 0.03 rad in rotation) and 0 elsewhere.

Fig. 4b presents the implemented workflow of the LUS procedure: 4 targets are computed and queued at the beginning of the procedure. The robot reaches the targets sequentially and stays on each target for 20 seconds for US image acquisition.

A. Validation of 2D Scanning Targets Localization

We aim to localize four scanning targets on the anterior chest using the DensePose based framework and quantitatively evaluate the influence of subject positioning on the localization accuracy. A mannequin (CPR-AED Training Manikin, Prestan, USA) with human-mimicking anterior anatomy is used as the experiment subject. As stated in section II, the targets need to be specified in [u, v]^⊤. We place four ArUco markers empirically on the mannequin and read their centroid positions in the image while simultaneously running DensePose to acquire U and V data. The robot is configured at the home position during this process because DensePose achieves the best accuracy and stability when the input image captures the front view of the mannequin. The scanning targets in [u, v]^⊤ can be obtained by plugging the markers’ centroid positions into U and V, The targets in image coordinates are then localized using (1). To quantify the localization accuracy, we keep the markers on the mannequin and use their centroid position as the ground truth, then place the mannequin at different locations. The mannequin is translated horizontally and vertically on the test table plane, altogether covering 15 locations starting from the default location. The 2D targets are calculated using DensePose at each location and sampled 100 times before being compared with the ground truth.

B. Validation of 3D Probe Pose Calculation

The 3D probe pose calculation is evaluated through the preciseness of 3D target point localization and normal vector calculation. Without loss of generality, a 3 by 4 virtual grid (see Fig. 6a) covering a whole anterior chest area, including the four anterior targets, is overlaid on the same mannequin. We place the ArUco marker at the center of each cell and estimate its 3D pose with OpenCV, knowing the camera intrinsic. The translational component and the approach vector of the marker’s pose are used as the 3D target ground truth and normal vector ground truth, respectively. We then deproject the 2D target at the center of the marker to 3D using the depth camera and calculate the normal vector using our patch-based method to get an estimation of the same variables and calculate the error accordingly. Similar to the validation in A., the error contains 100 samples to exhibit the stability of the calculation.

C. Validation of Robot Motion

Based on the defined target, we examine the robot’s motion when moving to the designated pose using the mannequin above. The system runs in a complete loop as depicted in Fig. 4b, covering all four anterior targets. The robot motion data is logged at a frequency of 10 Hz.

D. Validation of US Image Acquisition and Force Control

To evaluate the imaging capability of the proposed semi-autonomous RUS system, a lung phantom (COVID-19 Live Lung Ultrasound Simulator, CAE Blue Phantom^™, USA) simulating healthy lung and pathological landmarks (thickened pleural lining, diffused bilateral B-lines, sub-pleural consolidation) is used as the testing subject. The phantom simulates the respiratory motion of the lung during breathing as well. Since the phantom geometry is limited to the left anterior chest and does not have any external body feature for DensePose to recognize, the 2D scanning targets are assigned manually. Diagnostic landmarks are supposed to be observable in the US image if the system is effective. Comparing with the mannequin, the rigidity of the phantom is closer to the actual human chest; hence we recorded the end-effector force at 10 Hz during scanning to validate the force control.

A. Evaluation of 2D Scanning Targets

Fig. 2a shows the inferred 2D scanning targets on the mannequin when placed at the default location with 0 horizontal and vertical offset. Fig. 5 shows the error, defined as the Euclidean distance between the averaged pixel position of the inferred targets and the ground truth with respect to varied mannequin locations. The averaged errors for the four targets in pixels are 20.97 ± 5.40, 26.59 ± 6.36, 21.95 ± 6.28 and 25.31 ± 5.67, approximately corresponding to 17.41 ± 4.48 mm, 22.07 ± 5.28 mm, 18.22 ± 5.21 mm, and 21.00 ± 4.71 mm, respectively, showing good reproducible 2D localization precision. The bounding region (see Fig. 2a) of each scanning target is roughly 70 mm by 70 mm in size; hence the errors account for 21% to 34% of the region’s diagonal length, which is considered satisfactory for a rough landing. Such rough landing can be corrected through the following manual refinement of the probe placement.

B. Evaluation of 3D Probe Pose

The distribution of the errors on the virtual grid is shown in Fig. 6b, where the rotational error is defined as the absolute angle difference between the estimated and ground truth normal vectors, the translational error is defined as the Euclidean distance between the 3D target point measured using the depth camera and the ground truth. In terms of translation, the mean error for all the cells is 7.76 ± 2.71 mm, among which around 59% are along the z direction of the camera frame, whereas errors along x and y direction are mostly ignorable. The rotation error for all the cells is 12.61 ± 3.32 deg. In cells containing the anterior scanning targets, the mean translational error is 6.92 ± 2.75 mm, and the mean rotational error is 10.35 ± 2.97 deg, both of which are lower than the all-inclusive mean and meet our rough landing requirement. Nonetheless, the unbalanced scattering of errors suggests the necessity to improve consistency.

C. Evaluation of Robot Motion

The recorded robot motion is shown in Fig. 7. The errors when moving to entry poses are calculated by subtracting the current end-effector pose from the entry pose (defined as F_ent in Fig. 3) of each target; the errors when moving to target poses are calculated by subtracting the current end-effector pose from the target pose (defined as F_tar in Fig. 3). From the plot, both translational and rotational errors converge when moving to the entry poses, proving the effectiveness of the PD controller. While rotational error continues to stay low when reaching the target poses, the steady-state error is noticed in the z direction in translation.

D. Evaluation of US Imaging and Force Control

Representative US images when setting the desired constant contact force at 5 N are shown in Fig. 8c. The healthy part of the phantom (target A) presented no respiratory abnormality where the rib and pleural lines are clearly visible. The thickened plural lines were imaged in the other part of the phantom (target B), simulating typical pathological abnormalities that appear in COVID-19 patients. Capturing these features supports the effectiveness of the proposed probe placement approach, including the probe orientation control and desired contact force selection. The appearance of air gaps on the top-left corner suggests further image quality improvement through manual fine adjustment. The recorded force while turning on the breathing motion is shown in Fig. 8b. The error was stabilized at around 70% of the desired force with limited oscillation even during breathing. However, zero-drifting can be observed before landing, along with an overshoot of 1 N when scanning target B, indicating room for further optimization.