Gaussian process regression (GPR) allows unknown functions, along with their uncertainty, to be estimated based on limited samples (Rasmussen and Williams, 2005). A Gaussian process (GP) is a distribution over functions and is defined fully by a mean function m( p ) and a covariance function k ( p , p ′). It is assumed that the vector input p ∈ R d is related to the scalar output y by the function f (⋅) with the following relationship: y = f( p ) + ϵ, where ϵ ∼ N ( 0 , σ ϵ 2 ) . The training input is defined as P with y being the associated targets. Together, this is denoted as D : { ( p 1 , y 1 ) , ( p 2 , y 2 ) , … , ( p n , y n ) } . Observations y and discrete function values to be predicted f _* then follow a multivariate normal distribution: y f * ∼ N m P m P * , K ϵ K P , P * k P * , P K P * , P * , (2) where K ϵ = K ( P , P ) + σ ϵ 2 I , with σ ϵ 2 being the inherent noise on the measurements—allowing optimization under uncertainty—and K P , P ′ = k p 1 , p 1 ′ k p 1 , p 2 ′ … k p 1 , p k ′ k p 2 , p 1 ′ k p 2 , p 2 ′ … k p 2 , p k ′ ⋮ ⋮ ⋱ ⋮ k p k , p 1 ′ k p k , p 2 ′ … k p k , p k ′ . (3)

The mean vector associated with f_* is then f ¯ * = m P * + K P * , P K ϵ − 1 y − m P (4) and the covariance is given by: cov f * = K P * , P * − K P * , P K ϵ − 1 K P , P * . (5)

The estimate of the standard deviation along the estimated function is then the diagonal of cov ( f _*) and is denoted σ _f ( p ). The mean function is often set to zero unless some underlying information about the process exists. The covariance function is generally defined such that points near each other are more correlated. A common choice is the Matern class of covariance functions. This class of functions leads to different levels of differentiability and, thus, smoothness of f( p ) based on a smoothness parameter ν. For ν = 3 2 , the Matern kernel is k ν = 3 2 p , p ′ = 1 + 3 d p , p ′ exp − 3 d p , p ′ , (6) where d ( p , p ′) is the Euclidean distance between p and p ′ scaled by a diagonal matrix Θ: d = p − p ′ Θ − 1 p − p ′ , (7) Θ = diag l 1 , l 2 , … , l d = diag θ , (8) with θ = l 1 l 2 … l d T as the length scales of the function. The length scales encode how rapidly f (⋅) changes along each dimension. These are considered free parameters that must be determined from the training data. This is done by maximizing the marginal log-likelihood of the training examples given the parameters.

Bayesian optimization provides a framework for maximizing functions that are costly to evaluate, where uncertainty is present, and where following the gradient is infeasible (Shahriari et al., 2016). The method consists of three elements: a statistical surrogate model, an acquisition function, and a stopping criterion. A common choice for the surrogate model is Gaussian processes, as defined above. The acquisition function α(⋅) determines where to sample next in the search space based on the surrogate model by balancing exploration and exploitation: p n + 1 = arg p ∈ R d max α p ; D . (9) A common choice for the acquisition function is expected improvement (EI) (Mockus et al., 1978). EI maximizes the expected gain over the previous best sampled score, f ^*. E I p ≔ E max f p − f * , 0 , (10) EI can be calculated analytically as E I p = f ¯ * p − f * Φ f ¯ * p − f * σ f p + σ f p ϕ f ¯ * p − f * σ f p if   σ f p > 0 0 if   σ f p = 0 , (11) where Φ(⋅), ϕ(⋅) are the cumulative distribution function (CDF) and the probability density function (PDF), respectively.

Control barrier functions provide a framework for ensuring system safety (Ames et al., 2014). Throughout this section, a nonlinear control affine system of the following form is considered: x ̇ = f x + g x u , (12) where f  :  R n → R n and g  :  R n → R n × m are local Lipschitz functions, with x ∈ X ⊂ R n and u ∈ U ⊂ R m being the admissible states and control inputs, respectively. The goal of the CBF is to keep the system in a safe region of the state space. This safe region C is defined based on a continuously differentiable function h  :  X ⊂ R n → R as C = x ∈ X | h x ≥ 0 , ∂ C = x ∈ X | h x = 0 , Int C = x ∈ X | h x > 0 . (13)

The system is thus safe if a control law input u leads to the scalar function h ( x ) ≥ 0 ∀ t ∧ ∀ x 0 ∈ C . Equivalently, this can be described as the set C being rendered forward invariant by the control law. When the relative degree r of the system with respect to h is larger than one, higher-order CBFs can be used to render a set C safe. The relative degree of a continuously differentiable function h( x ) with respect to System (12) is the number of times it can be differentiated along (12) before the control input u explicitly shows. Exponential control barrier functions (ECBFs) are a class of higher-order CBFs (Nguyen and Sreenath, 2016). This formulation is based on calculating higher-order time derivatives of h( x ). The r ^th time derivative is h r x , u = L f r h x + L g L f r − 1 h x u , (14) where L f r h ( x ) is the r ^th Lie derivative of h along f. ECBFs are defined based on a series of integrators that relate h ^(r)( x , u ) to h( x ): η ̇ x = F η x + G μ , h x = C η x . (15) With η x = h x L f h x L f 2 h x ⋮ L f r − 1 h x = h x h ̇ x h ̈ x ⋮ h r − 1 x , μ = L f r h x + L g L f r − 1 h x u = h r x , u (16) and F : r × r, G : r × 1, C : 1 × r F = 0 1 0 … 0 0 0 1 … 0 ⋮ ⋮ ⋮ ⋱ ⋮ 0 0 0 … 1 0 0 0 … 0 , G = 0 0 ⋮ 0 1 , C = 1 0 … 0 . (17)

Choosing the state feedback μ = − K η( x ) leads to h( x ) being an explicit function of time t and the initial state x ₀: h x = C e F − G K t η x 0 , (18) where h( x ) is then said to be an ECBF—rendering C forward invariant—if there exists a row vector K _γ such that sup u ∈ U L f r h x + L g L f r − 1 h x u ≥ − K γ η x , ∀ x ∈ I n t C (19) results in h x t ≥ C e F − G K γ t η x 0 ≥ 0 , (20) where h ( x ₀) ≥ 0. A K _γ can be found that satisfies this using the pole placement method, with all poles being real and negative. Additionally, the eigenvalues λ _i of system F − GK _γ must adhere to a condition based on the initial conditions x ₀: λ i F − G K γ ≥ − ν ̇ i − 1 x 0 ν i − 1 x 0 , (21) where ν 0 x = h x , ν 1 x = ν ̇ 0 x + s 1 ν 0 x , ⋮ ν r x = ν ̇ r − 1 x + s r ν r − 1 x , (22) and s ₀, … , s _r are defined as the roots of the characteristic polynomial of F − GK _γ .

We will now present our proposed solution to Problem 1. In regular Bayesian optimization, sampling from anywhere in the search space is assumed to be associated with the same cost. As the robot must move to acquire images, this assumption is not ideal for robotic ultrasound scanning, as switching between sampling points far apart from each other will incur a large time cost while the robot moves between them. To account for this, we consider a non-standard acquisition function that incorporates the ability to sample ultrasound images throughout the path traveled by the robot. Similar methods have been applied to robotic environmental monitoring using unmanned aerial vehicles (Marchant and Ramos, 2014). We parameterize the path as straight lines to ensure the optimization time of the acquisition function is kept low. The path P is defined as P u , p n + 1 | p n = 1 − u p n + 1 + u p n , (23) with u ∈ [0, 1]. The modified acquisition function is denoted α _IC (⋅) and is said to be informative continuous as information throughout the path is considered. It is defined by integrating the base acquisition function over the path and scaling with the inverse of the path length: α I C P u , p n + 1 | p n , α = 1 ‖ p n + 1 − p n ‖ 2 ∫ 0 1 α P u , p n + 1 | p n d u . (24) The scale is included to account for the time spent moving between p _n and p _n+1. Assuming that the robot moves at a constant velocity, maximizing the modified acquisition function leads to a path that maximizes the integral of the base acquisition function per unit of time spent. The formulation leads to the entire path being considered—and thus less time spent in areas with low uncertainty and/or low expected scores. We employ the EI acquisition function as the base acquisition function and denote the informative continuous version as ICEI. A score y is associated with each position in the search space p using a classifier trained on labeled ultrasound images. The range of the output is [0; 1], with 0 indicating poor quality and 1 good. Defining an appropriate stopping criterion is important to ensure a good area has been found without spending excessive time scanning a patient. A statistical stopping criterion is proposed based on the maximum lower bound over the samples, y . The lower bound for a sample is y 1 − δ = y − Z δ σ y , (25) where Z is the standard normal distribution, Z _δ is the critical value for a confidence level of 1 − δ, and σ _y is the prediction of standard deviation for the corresponding sample y based on the surrogate model. As the range of possible scores is known beforehand, defining an appropriate threshold is possible.

Our proposed solution to Problem 2 combines a hybrid force/position controller—also referred to as the nominal controller—with a CBF defined such that bounded contact force is ensured. The hybrid force/position controller allows direct control over the force to ensure consistent ultrasound image quality while making positioning possible during contact. The robot considered in this work is controlled at the joint position level. The CBF is defined in Cartesian space and at the acceleration level to allow for constraints on the force. End-effector accelerations are related to joint accelerations using the following kinematic relationship: q ̈ = J † q x ̈ − J ̇ q q ̇ , (26) where J( q ) is the manipulator Jacobian, ^† denotes the Moore–Penrose pseudo inverse, q is the joint configuration, and x ̈ is the Cartesian space acceleration. The resulting joint accelerations are then double integrated for the robot’s position controller. Figure 2 illustrates the proposed control architecture.

The ultrasound image quality optimization approach assumes a starting position is given, and predetermined bounds of the search space are defined in relation to this. The search was performed over two DOFs such that the input to the Bayesian optimizer was p = [ x , R x ] T , where R _x is rotation about the x-axis. The inherent noise of the measurements was set to σ _ϵ = 0.1. Bayesian optimization was implemented using BoTorch (Balandat et al., 2020). The informative continuous extension was implemented in PyTorch, estimating the integral using a discrete sum. Our proposed method for optimizing ultrasound image quality has been evaluated in a simulated setting. The simulation is based on data gathered from scanning 32 joints across four test subjects. The search space was sampled in a grid using nearest-neighbor interpolation to construct an approximated ground truth. The stopping criterion was set to a maximum lower bound of 0.9 with a confidence level of 95%. Three methods were tested: random sampling, EI, and ICEI. The search space was normalized such that the range was equal for the two DOFs. A distance budget of 10 times the length of the search space was given. In order to simulate the ability to sample continuously, a sample was taken for every 4% of the search space traveled. Based on this, random sampling, EI, and ICEI resulted in 60.7%, 67.8%, and 75.0% success rates, respectively, with mean distance traveled being 8.65, 8.13 and 7.64 times the length of the normalized search space respectively. Figure 3 shows the search patterns resulting from the different methods. Random sampling is, as expected, inefficient. The tendency of ICEI to cause less overlap of paths compared to EI is clear.

EI and ICEI were also compared in a live test. In these tests the search was extended such that p = [ x , R x , R y , R z ] T with a range of [−7.5mm, 7.5 mm] for x and [−4°, 4°] for the rotational DOFs. The test compared the results of searching using EI and ICEI. For each test, the methods were started at the same position. The methods were run until the stopping criterion was reached. When this occurred, the probe was positioned at the maximum lower bound, and a measurement was taken to ensure the success criterion actually had been reached. This cannot be guaranteed due to uncertainties in positioning. If not, the search was continued. If a solution had not been found within 60 s, it was considered a failed scan. Fourteen joints were scanned. EI lead to a success rate of 79% and ICEI to a rate of 92%. The mean recorded time for optimizing the acquisition function during this test was 0.10 ± 0.06 s for EI and 0.14 ± 0.10 s for ICEI. The training time for the Gaussian process using maximum log-likelihood was 0.16 ± 0.08 s. Even with the ICEI acquisition method, querying a new sample point only took around 0.3 s

A KUKA AG IIWA 7 R800 was used to evaluate the safe controller with the fast research interface (FRI) for communication with the robot through the IIWA ROS package (Chatzilygeroudis et al., 2019). The communication rate with the inner joint position controller was set to 200 Hz. The KYOTO KAGAKU Rheumatism Hand Phantom was used with parameters estimated to k ̂ = 1285 and b ̂ = 15 . For the inner joint position controller, the following parameters were found: ω ̂ n = 31.25 and ζ ̂ = 1 ; it is not possible to change these parameters, as it is an inbuilt controller. The parameters of the joint controller were estimated by minimizing the sum of squared errors between the model and measurements taken during an initial experiment where the robot was moved into contact with the hand phantom.

The parameter K _γ used in the safety filter must be defined such that the poles of System (20) are negative and real and adhere to the constraints on initial conditions (21). Pole placement within these constraints affects how conservative the barrier function is and will always ensure safety. The following poles p were utilized in the experiments: p = − 3 − 6 − 9 . A maximum force of f _max = 5N was specified.

The nominal force control parameters where set to K p f = 0.001 and K d f = 1 . The controller was implemented in Python using CVXOPT (Andersen et al., 2020) to solve the ECBF QP problem. The end-effector wrench was estimated from the robot’s joint torque sensors using the following kinematic relationship: h e = J q T † τ external , (39) where τ _external are the measured external joint torques available through the FRI. Figure 4 illustrates the invariance of the proposed method to the contact surface point in the simulation and when running on the robot and shows that the ECBF is able to bound the contact forces below the specified threshold.