Learning autonomous behaviours for the body of a flexible surgical robot

Abstract

This paper presents a novel strategy to learn a positional controller for the body of a flexible surgical manipulator used for minimally invasive surgery. The manipulator is developed within the STIFF-FLOP European project and is targeted for a laparoscopic use in remote areas of the abdominal region that are not easily accessible by means of currently available rigid tools. While the surgeon controls the end-effector during the task, the flexible body of the manipulator needs to be displaced to enter inside constrained spaces by efficiently exploiting its flexibility, without touching vital organs and structures. The proposed algorithm exploits the instruments of machine learning within the programming by demonstrations paradigm to produce a statistical model of the natural movements of the surgeon during the task. The gathered information is then reused to determine a controller in the null space of the robot that does not interfere with the surgeon task and displaces the robot body within the available space in a fully automated manner.

Appendix 1: Gaussian mixture models

The observations $\{{\varvec{\xi }}_n\}_{n=1}^N$ representing the points of the demonstrations are assumed to be independent realizations of a random vector, that is assumed to be distributed as a linear combination of Normal distributions as

$$\begin{aligned} {\mathcal {P}}\left( {\varvec{\xi _n}}\right) = \sum \limits _{k=1}^K \pi _k\; {\mathcal {N}}\left( {\varvec{\xi _n}}|{\varvec{\mu }}_k,{\varvec{\varSigma }}_k\right) , \end{aligned}$$

with

$$\begin{aligned} {\mathcal {N}}\left( {\varvec{\xi _n}}|{\varvec{\mu }}_k,{\varvec{\varSigma }}_k\right)= & {} \frac{1}{(2\pi )^{\frac{D}{2}} |{\varvec{\varSigma }}_k|^{\frac{1}{2}}}\exp \\&\times \left[ -\frac{1}{2}\left( {\varvec{\xi _n}}-{\varvec{\mu }}_k\right) ^{{\scriptscriptstyle \top }} {\varvec{\varSigma }}_k^{-1} \left( {\varvec{\xi _n}}-{\varvec{\mu }}_k\right) \right] . \end{aligned}$$

The parameters of a Gaussian mixture model (GMM) with K components are thus defined by $\{\pi _k,{\varvec{\mu }}_k,{\varvec{\varSigma }}_k\}_{k=1}^K$, where $\pi _k$ is the prior (mixing coefficient), ${\varvec{\mu }}_k$ is the center, and ${\varvec{\varSigma }}_k$ is the covariance matrix of the k-th mixture component.

The estimation of mixture parameters can be performed by maximizing the log-likelihood of the above distribution of the given dataset. For the set of observations $\{{\varvec{\xi }}_n\}_{n=1}^N$, the log-likelihood of the GMM is

$$\begin{aligned} {\mathcal {L}}\left( {\varvec{\theta }}|{\varvec{\xi }}\right) = \sum _{n=1}^N \log \left( \sum _{k=1}^K \pi _k\; {\mathcal {N}}\left( {\varvec{\xi }}|{\varvec{\mu }}_k,{\varvec{\varSigma }}_k\right) \right) . \end{aligned}$$

(12)

The maximization of the likelihood leads to an expectation-maximization (EM) process iteratively refining the model parameters to converge to a local optimum of the likelihood. These two steps are iteratively applied until a stopping criterion is satisfied. The two steps are described below.

E-step:

$$\begin{aligned}&h_{n,i} = \frac{\pi _i \; {\mathcal {N}}\left( {\varvec{\xi }}_n|{\varvec{\mu }}_i,{\varvec{\varSigma }}_i\right) }{\sum _{k=1}^{K}\pi _k \; {\mathcal {N}}\left( {\varvec{\xi }}_n|{\varvec{\mu }}_k,{\varvec{\varSigma }}_k\right) } .\\ \end{aligned}$$

M-step:

$$\begin{aligned}&\pi _i = \frac{\sum _{n=1}^N h_{n,i} }{N} ,\\&{\varvec{\mu }}_i = \frac{\sum _{n=1}^N h_{n,i}{\varvec{\xi }}_n}{\sum _{n=1}^N h_{n,i} } ,\\&{\varvec{\varSigma }}_i = \frac{\sum _{n=1}^N h_{n,i}\left( {\varvec{\xi }}_n-{\varvec{\mu }}_i\right) \left( {\varvec{\xi }}_n-{\varvec{\mu }}_i\right) ^{\scriptscriptstyle \top }}{\sum _{n=1}^N h_{n,i} } . \end{aligned}$$

The reproduction of an average movement or skill behavior can be formalized as a statistical regression problem. We demonstrated in previous work that Gaussian mixture regression (GMR) offers a simple and elegant solution to handle encoding, recognition, prediction and reproduction in robot learning (Calinon et al. 2010). It provides a probabilistic representation of the movement, where the model can retrieve actions in real-time, within a computation time that is independent of the number of datapoints in the training set.

By defining which variables span for input and output parts (noted respectively by ${\mathcal {I}}$ and ${\mathcal {O}}$ superscripts), a block decomposition of the datapoint ${\varvec{\xi }}_n$, vectors ${\varvec{\mu }}_i$ and matrices ${\varvec{\varSigma }}_i$ can be written as

$$\begin{aligned} {\varvec{\xi }}_n=\begin{bmatrix}{\varvec{\xi }}^{\scriptscriptstyle I}_n\\{\varvec{\xi }}^{\scriptscriptstyle O}_n\end{bmatrix},\quad {\varvec{\mu }}_i=\begin{bmatrix}{\varvec{\mu }}^{\scriptscriptstyle I}_i\\{\varvec{\mu }}^{\scriptscriptstyle O}_i\end{bmatrix},\quad {\varvec{\varSigma }}_i=\begin{bmatrix}{\varvec{\varSigma }}^{\scriptscriptstyle I}_i \;{\varvec{\varSigma }}^{\scriptscriptstyle IO}_i\\{\varvec{\varSigma }}^{\scriptscriptstyle OI}_i {\varvec{\varSigma }}^{\scriptscriptstyle O}_i\end{bmatrix}. \end{aligned}$$

The GMM thus encodes the joint distribution ${\mathcal {P}}({\varvec{\xi }}^{\scriptscriptstyle I},{\varvec{\xi }}^{\scriptscriptstyle O})\sim \sum _{i=1}^K\pi _i{\mathcal {N}}({\varvec{\mu }}_i,{\varvec{\varSigma }}_i)$ of the data ${\varvec{\xi }}$. At each reproduction step n, ${\mathcal {P}}({\varvec{\xi }}^{\scriptscriptstyle O}_n|{\varvec{\xi }}^{\scriptscriptstyle I}_n)$ is computed as the conditional distribution

$$\begin{aligned} {\mathcal {P}}\left( {\varvec{\xi }}^{\scriptscriptstyle O}_n|{\varvec{\xi }}^{\scriptscriptstyle I}_n\right)\sim & {} \sum _{i=1}^K h_i\left( {\varvec{\xi }}^{\scriptscriptstyle I}_n\right) \; {\mathcal {N}} \left( {\varvec{\hat{\mu }}}^{\scriptscriptstyle O}_i\left( {\varvec{\xi }}^{\scriptscriptstyle I}_n \right) ,{\varvec{\hat{\varSigma }}}^{\scriptscriptstyle O}_i\right) , \end{aligned}$$

(13)

$$\begin{aligned} {\mathrm {with}}\quad \hat{{\varvec{\mu }}}^{\scriptscriptstyle O}_i\left( {\varvec{\xi }}^{\scriptscriptstyle I}_n\right)= & {} {\varvec{\mu }}^{\scriptscriptstyle O}_i + {\varvec{\varSigma }}^{\scriptscriptstyle OI}_i{{\varvec{\varSigma }}^{\scriptscriptstyle I}_i}^{-1} \left( {\varvec{\xi }}^{\scriptscriptstyle I}_n-{\varvec{\mu }}^{\scriptscriptstyle I}_i\right) , \end{aligned}$$

(14)

$$\begin{aligned} \hat{{\varvec{\varSigma }}}^{\scriptscriptstyle O}_i= & {} {\varvec{\varSigma }}^{\scriptscriptstyle O}_i - {\varvec{\varSigma }}^{\scriptscriptstyle OI}_i{{\varvec{\varSigma }}^{\scriptscriptstyle I}_i}^{-1} {\varvec{\varSigma }}^{\scriptscriptstyle IO}_i ,\nonumber \\&{\mathrm {and}}\quad h_i\left( {\varvec{\xi }}^{\scriptscriptstyle I}_n\right) = \frac{\pi _i {\mathcal {N}}\left( {\varvec{\xi }}^{\scriptscriptstyle I}_n |\; {\varvec{\mu }}^{\scriptscriptstyle I}_i,{\varvec{\varSigma }}^{\scriptscriptstyle I}_i\right) }{\sum _k^K \pi _k {\mathcal {N}}\left( {\varvec{\xi }}^{\scriptscriptstyle I}_n |\; {\varvec{\mu }}^{\scriptscriptstyle I}_k,{\varvec{\varSigma }}^{\scriptscriptstyle I}_k\right) } . \end{aligned}$$

(15)

In the general case, Eq. (13) represents a multimodal distribution. In problems where a single output is expected (single peaked distribution), Eq. (13) can be approximated by a single normal distribution ${\mathcal {N}}(\hat{{\varvec{\mu }}}^{\scriptscriptstyle O}_n, \hat{{\varvec{\varSigma }}}^{\scriptscriptstyle O}_n)$ with parameters

$$\begin{aligned} \hat{{\varvec{\mu }}}^{\scriptscriptstyle O}_n= & {} \sum _i h_i\left( {\varvec{\xi }}^{\scriptscriptstyle I}_n\right) \left[ {\varvec{\mu }}^{\scriptscriptstyle O}_i + {\varvec{\varSigma }}^{\scriptscriptstyle OI}_i{{\varvec{\varSigma }}^{\scriptscriptstyle I}_i}^{-1} \left( {\varvec{\xi }}^{\scriptscriptstyle I}_n-{\varvec{\mu }}^{\scriptscriptstyle I}_i\right) \right] , \nonumber \\ {\varvec{\hat{\varSigma }}}{}^{\scriptscriptstyle O}_n= & {} \sum _{i=1}^K h_i({\varvec{\xi }}^{\scriptscriptstyle I}_n) {\varvec{{\varSigma }}}_i^{\scriptscriptstyle O} + \sum _{i=1}^K h_i\left( {\varvec{\xi }}^{\scriptscriptstyle I}_n\right) {\varvec{{\mu }}}_i^{\scriptscriptstyle O} \left( {\varvec{{\mu }}}_i^{\scriptscriptstyle O}\right) ^{\scriptscriptstyle \top }- {\varvec{\hat{\mu }}}^{\scriptscriptstyle O}_n {{\varvec{\hat{\mu }}}^{\scriptscriptstyle O}_n}^{\scriptscriptstyle \top }. \nonumber \\ \end{aligned}$$

(16)

Equation (16) is computed in real-time from the model parameters. The retrieved signal encapsulates variation and correlation information in the form of a probabilistic flow tube, see e.g., Lee and Ott (2011).