Performance quantification of observer-based robot impact detection under modeling uncertainties

Abstract

This work proposes a design and performance quantification methodology for observer-based impact detection in serial robot manipulators in presence of modelling errors and without force/torque sensor. After expressing the modelling errors between the physical robot and its inverse dynamic model as the sum of contributions due to dynamic parameters uncertainties and numerical differentiation errors for a given trajectory, an observer of the external disturbance torque is designed based on the inverse dynamic model and using a Kalman filter. The influence of each design parameter of the observer on the quality of the external torque estimation is studied first based on simulation results. Then a frequency analysis is conducted to distinguish between the influence of the exact external torque, the modelling uncertainties and the measurement noise on the estimated external torque. Finally a methodology is proposed to determine the optimal design corresponding to the shortest detection time depending on the expected sensitivity.

Appendices

Appendix 1: Calculation of $\varvec{W}_{\delta \tau ^D(t)}$

As detailed in Briquet-Kerestedjian et al. (2016), for a given position trajectory characterized by velocities and accelerations respectively bounded by $\varvec{v_{max}}$ and $\varvec{a_{max}}$ and denoting D(s) the equivalent continuous filter of a numerical differentiation scheme (see Fig. 10 that compares the frequency response of two numerical differentiation schemes), for axis i, the first term of (11) and (12) can respectively be bounded by

$$\begin{aligned} A_{\dot{q}}^i&= |D(j\omega _{eq}^i) - j\omega _{eq}^i|E_{eq}^i \; , \end{aligned}$$

(34)

$$\begin{aligned} A_{\ddot{q}}^i&= |D(j\omega _{eq}^i)^2 - (j\omega _{eq}^i)^2|E_{eq}^i \; , \end{aligned}$$

(35)

where j stands for the imaginary unit and

$$\begin{aligned} E_{eq}^i = \frac{(v_{max}^i)^2}{a_{max}^i} \quad ; \quad \omega _{eq}^i = \frac{a_{max}^i}{v_{max}^i} \; . \end{aligned}$$

(36)

Fig. 10

Frequency responses of continuous exact derivative, first-order backward finite difference and first-order filtered derivative methods with $\omega _{c} = 2\pi 110$ rad/s and $T_{s} = 1$ ms

For the filtered noise term, if the input noise $\xi _{i}$ is white of variance $\sigma _{\xi _i}^2$ (e.g. quantization noise), the output noise filtered by the equivalent discrete filter $\bar{D}(z)$ (of impulse response d[k]) is Gaussian of variance $\displaystyle \varvec{\sigma }_{\dot{q}_i}^2 = \sigma _{\xi _i}^2 \sum\nolimits_{{k = - \infty }}^{\infty } {d^{2} [k]}$, and similarly for $\varvec{\sigma }_{\ddot{q}_i}^2$.

Finally, $\varvec{\delta \dot{q}}$ and $\varvec{\delta \ddot{q}}$ can be bounded at 99,7$\%$ respectively by $\pm (\varvec{A}_{\dot{q}}+3\varvec{\sigma }_{\dot{q}})$ and $\pm (\varvec{A}_{\ddot{q}}+3\varvec{\sigma }_{\ddot{q}})$. Then $\varvec{\delta \dot{q}}$ and $\varvec{\delta \ddot{q}}$ are approximated by equivalent noises of PSD

$$\begin{aligned} \varvec{\sigma }_{\delta \dot{q}}^2 = \left( \frac{1}{3}\varvec{A_{\dot{q}}} + \varvec{\sigma _{\dot{q}}} \right) ^2 \quad ; \quad \varvec{\sigma }_{\delta \ddot{q}}^2 = \left( \frac{1}{3}\varvec{A_{\ddot{q}}} + \varvec{\sigma _{\ddot{q}}} \right) ^2 \; , \end{aligned}$$

(37)

where the two contributions for each term are assumed to be independent. Therefore, the PSD of $\varvec{\delta \tau }^{D}$ is deduced with the matrices $\varvec{M_{\star }}$ and $\varvec{C_{\star }}$ evaluated along the reference trajectory such as:

$$\begin{aligned} \varvec{W}_{\delta \tau ^D}(t) = \varvec{M_{\star }}(\varvec{q_{ref}})&\left( \frac{1}{3}\varvec{A_{\ddot{q}}} + \varvec{\sigma _{\ddot{q}}} \right) ^2\left[ \varvec{M_{\star }}(\varvec{q_{ref}})\right] ^T \nonumber \\ \quad + \varvec{C_{\star }}(\varvec{q_{ref}},\varvec{\dot{q}_{ref}})&\left( \frac{1}{3}\varvec{A_{\dot{q}}} + \varvec{\sigma _{\dot{q}}} \right) ^2\left[ \varvec{C_{\star }}(\varvec{q_{ref}},\varvec{\dot{q}_{ref}})\right] ^T. \end{aligned}$$

(38)

From a practical perspective, in order to avoid too much conservatism and overestimated uncertainties when during the major part of the motion, the bounds on the velocity and acceleration are not reached, the trajectory can be divided into several smaller segments or evaluated on a sliding time horizon in order to update the maximum velocity and acceleration for each time window. The only potential limitation for this approach is the computation power that can disrupt the update of the algorithm in real time.

Appendix 2: Calculation of transfer matrices and covariance matrix $\varvec{P}$

The resulting discrete state-space representation of (19) is

$$\begin{aligned} \left\{ \begin{array}{l} \varvec{X}^a(k+1) = \varvec{A_\star }^a\varvec{X}^a(k) + \varvec{B_\star }^a\varvec{\widetilde{\tau }}(k) + \; \varvec{G}^a_{ext}\varvec{\tau }_{ext}(k) + \varvec{G}^a_{\delta \tau }(k)\varvec{\underline{\delta \tau }}(k) \\ \varvec{Y_\star }^a(k) = \varvec{C_\star }^a\varvec{X}^a(k) + \varvec{v}(k) \end{array} \right. \; , \end{aligned}$$

(39)

with $\varvec{\underline{\delta \tau }}(k)$ of variance $\varvec{\underline{W}}_{\delta \tau }^a = \varvec{I}_{n}$ and

$$\begin{aligned} \varvec{A_\star }^a = e^{\varvec{A_\star }T_{s}} \quad&; \quad \varvec{B_\star }^a = \int _{0}^{T_{s}} e^{\varvec{A_\star }\nu }\varvec{B_\star } \, \mathrm {d}\nu \; ; \nonumber \\ \varvec{C_\star }^a = \varvec{C_\star } \quad&; \quad \varvec{G}^a_{ext} = T_s\varvec{G_\star } \; ; \end{aligned}$$

(40)

$$\begin{aligned} \varvec{G}^a_{\delta \tau }(k)[\varvec{G}^a_{\delta \tau }(k)]^T&\approx T_{s} \, \varvec{G_\star } \, \varvec{W}_{\delta \tau }(t) \, \varvec{G_\star }^T \; ; \end{aligned}$$

(41)

$$\begin{aligned} \varvec{V}^a&= \varvec{V} \, / \, T_{s} \; . \end{aligned}$$

(42)

where $\varvec{G}^a_{\delta \tau }$ can be obtained by Cholesky-like covariance decomposition. For the analysis, the disturbance model (20) is discretised as:

$$\begin{aligned} \varvec{\tau }_{ext}(k+1) = \varvec{A}^a_{ext} \varvec{\tau }_{ext}(k) + \varvec{G}^a_w \varvec{w}_{ext}(k) \; , \end{aligned}$$

(43)

with:

$$\begin{aligned} \varvec{A}^a_{ext} = e^{\varvec{A}_{ext}T_{s}}; \quad \varvec{G}^a_w = \int _{0}^{T_{s}} e^{\varvec{A}_{ext}\nu }\varvec{B}_{ext} \, \mathrm {d}\nu. \end{aligned}$$

(44)

Thus the augmented discrete state-space representation becomes

$$\begin{aligned} \left\{ \begin{array}{l} \varvec{X}_{aug}^a(k+1) = \varvec{A}^a_{aug}\varvec{X}_{aug}^a(k) + \varvec{B}^a_{aug}\varvec{\widetilde{\tau }}(k) + \; \varvec{G}^a_{aug}(k) \varvec{\underline{w}}^a(k) \\ \varvec{Y_\star }^a(k) = \varvec{C}^a_{aug}\varvec{X}_{aug}^a(k) + \varvec{v}(k) \end{array} \right. \; , \end{aligned}$$

(45)

where $\varvec{\underline{w}}^a(k) = \left[ \begin{array}{c} \varvec{\underline{\delta \tau }}(k)\,^T \quad \varvec{w}_{ext}(k)\,^T \end{array}\right] ^T$ and

$$\begin{aligned} \varvec{A}^a_{aug} = \left[ \begin{array}{cc} \varvec{A}^a_\star &{} \varvec{G}^a_{ext} \\ \varvec{0}_{n,2n} &{} \varvec{A}^a_{ext} \end{array} \right] \,&; \, \varvec{B}^a_{aug} = \left[ \begin{array}{c} \varvec{B_\star }^a \\ \varvec{0}_{n,n} \end{array} \right] \, ; \\ \varvec{G}^a_{aug}(k) = \left[ \begin{array}{cc} \varvec{G}^a_{\delta \tau }(k) &{} \varvec{0}_{2n,n} \\ \varvec{0}_{n,n} &{} \varvec{G}^a_w \end{array} \right] \,&; \, \varvec{C}^a_{aug} = \left[ \begin{array}{cc} \varvec{C_\star }^a&\varvec{0}_{n,n} \end{array} \right] \; . \end{aligned}$$

Given the state-space representation (45), the estimation observer is given by

$$\begin{aligned} \nonumber \varvec{\hat{X}}_{aug}^a(k+1) &=\varvec{A}^a_{aug}\varvec{X}_{aug}^a(k) + \varvec{B}^a_{aug}\varvec{\widetilde{\tau }}(k) \nonumber + \varvec{K}_f(k)[\varvec{C_\star }^a \varvec{X}^a(k+1) + \varvec{v}(k+1) \nonumber \\&\quad- \varvec{C_\star }^a \varvec{\hat{X}}^a(k+1) ] \; . \end{aligned}$$

(46)

Denoting $\varvec{\varepsilon }_X(k) = \varvec{X}^a(k) - \varvec{\hat{X}}^a(k)$, (46) is rewritten:

$$\begin{aligned} \varvec{\hat{X}}_{aug}^a(k+1)&= \varvec{A}^a_{aug}\varvec{X}_{aug}^a(k) + \varvec{B}^a_{aug}\varvec{\widetilde{\tau }}(k) \nonumber \\&\quad+ \varvec{K}_f(k)[\varvec{C_\star }^a \varvec{A}^a_\star \varvec{\varepsilon }_X(k) + \varvec{C_\star }^a \varvec{G}^a_{\delta \tau }(k) \varvec{\underline{\delta \tau }}(k) \nonumber \\&\quad+ \varvec{v}(k+1) + \varvec{C_\star }^a \varvec{G}^a_{ext} (\varvec{\tau }_{ext}(k) - \varvec{\hat{\tau }}_{ext}(k)) ] \; . \end{aligned}$$

(47)

With $\varvec{K}_f(k) = \left[ \varvec{K}_1(k)\,^T \; \varvec{K}_2(k)\,^T\right]$ such as $\varvec{K}_1 \in \mathbb {R}^{2n \times n}$ and $\varvec{K}_2 \in ~\mathbb {R}^{n \times n}$, the components of $\varvec{\hat{\tau }}_{ext}(k+1)$ are extracted from (47)

$$\begin{aligned} \nonumber \varvec{\hat{\tau }}_{ext}(k+1)&= [\varvec{A}^a_{ext} - \varvec{K}_2(k)\varvec{C_\star }^a \varvec{G}^a_{ext}]\varvec{\hat{\tau }}_{ext}(k) \nonumber \\&\quad+ \varvec{K}_2(k) [\varvec{C_\star }^a \varvec{A}^a_\star \varvec{\varepsilon }_X(k) + \varvec{C_\star }^a \varvec{G}^a_{\delta \tau }(k) \varvec{\underline{\delta \tau }}(k) \nonumber \\&\quad+ \varvec{v}(k+1) + \varvec{C_\star }^a \varvec{G}^a_{ext} \varvec{\tau }_{ext}(k)] \; . \end{aligned}$$

(48)

Similarly with $\varvec{\hat{X}}^a(k+1)$ extracted from (47), $\varvec{\varepsilon }_X(k+1) = \varvec{X}^a(k+1) - \varvec{\hat{X}}^a(k+1)$ is expressed as:

$$\begin{aligned} \varvec{\varepsilon }_X(k+1)&= \left[ \varvec{A}^a_\star - \varvec{K}_1(k)\varvec{C_\star }^a\varvec{A}^a_\star \right] \varvec{\varepsilon }_X(k) \nonumber \\&\quad+ [\varvec{G}^a_{\delta \tau }(k) - \varvec{K}_1(k)\varvec{C_\star }^a\varvec{G}^a_{\delta \tau }(k)]\varvec{\underline{\delta \tau }}(k) \nonumber \\&\quad- \varvec{K}_1(k)\varvec{v}(k+1) \nonumber \\&\quad+ \left[ \varvec{G}^a_{ext} - \varvec{K}_1(k)\varvec{C_\star }^a\varvec{G}^a_{ext}\right] \left[ \varvec{\tau }_{ext}(k) - \varvec{\hat{\tau }}_{ext}(k)\right]. \end{aligned}$$

(49)

Using (48) and (49), the following state-space representation is derived at each time step k

$$\begin{aligned} \left\{ \begin{array}{l} \varvec{X}_e(k+1) = \varvec{A}_e^k \varvec{X}_e(k) + \varvec{B}_e^k \varvec{U}_e(k) \\ \varvec{Y}_e(k) = \varvec{C}_e \varvec{X}_e(k) \end{array} \right. \; , \end{aligned}$$

(50)

with:

$$\begin{aligned} \varvec{X}_e(k) = \left[ \begin{array}{c} \varvec{\varepsilon }_X(k) \\ \varvec{\hat{\tau }}_{ext}(k) \end{array} \right] \; ; \; \varvec{Y}_e(k) = \varvec{\hat{\tau }}_{ext}(k) \; ; \; \varvec{U}_e(k) = \left[ \begin{array}{c} \varvec{\tau }_{ext}(k) \\ \varvec{\underline{\delta \tau }}(k) \\ \varvec{v}(k+1) \end{array} \right] \end{aligned}$$

(51)

and

$$\begin{aligned} \varvec{A}_e^k&= \left[ \begin{array}{cc} \varvec{A}^a_\star - \varvec{K}_1(k)\varvec{C_\star }^a\varvec{A}^a_\star &{} \quad -(\varvec{G}^a_{ext} - \varvec{K}_1(k)\varvec{C_\star }^a\varvec{G}^a_{ext}) \\ \varvec{K}_2(k) \varvec{C_\star }^a \varvec{A}^a_\star &{} \quad \varvec{A}^a_{ext} - \varvec{K}_2(k)\varvec{C_\star }^a \varvec{G}^a_{ext} \end{array} \right] \\ \varvec{B}_e^k&= \left[ \begin{array}{ccc} \varvec{B}_{11} &{} \; \varvec{B}_{12} &{} \varvec{B}_{13} \\ \varvec{B}_{21} &{} \; \varvec{B}_{22} &{} \varvec{B}_{23} \end{array} \right] \\ \varvec{B}_{11}&= \varvec{G}^a_{ext} - \varvec{K}_1(k)\varvec{C_\star }^a\varvec{G}^a_{ext} \\ \varvec{B}_{12}&= \varvec{G}^a_{\delta \tau }(k) - \varvec{K}_1(k)\varvec{C_\star }^a\varvec{G}^a_{\delta \tau }(k) \\ \varvec{B}_{13}&= - \varvec{K}_1(k) \\ \varvec{B}_{21}&= \varvec{K}_2(k) \varvec{C_\star }^a \varvec{G}^a_{ext} \\ \varvec{B}_{22}&= \varvec{K}_2(k) \varvec{C_\star }^a \varvec{G}^a_{\delta \tau } \\ \varvec{B}_{23}&= \varvec{K}_2(k) \\ \varvec{C}_e&= \left[ \begin{array}{cc} \varvec{0}_{n,2n}&\quad I_{n} \end{array} \right] \; . \end{aligned}$$

Transforming (50) in the complex z-domain, it gives

$$\begin{aligned} \varvec{\hat{\tau }}_{ext}(z) = \underbrace{\varvec{C}_e\left( z\varvec{I}_{3n}-\varvec{A}_e^k\right) ^{-1}\varvec{B}_e^k}_{\varvec{H}^k(z)}\varvec{U}_e(z) \; . \end{aligned}$$

(52)

We define the following transfer matrices calculated at each time step k

$$\begin{aligned} \varvec{H}_1^k(z) \;&\widehat{=} \; \frac{\varvec{\hat{\tau }}_{ext}(z)}{\varvec{\tau }_{ext}(z)} = \varvec{H}^k(z) \left[ \begin{array}{ccc} \varvec{I}_{n}&\varvec{0}_{n,n}&\varvec{0}_{n,n} \end{array} \right] ^T \quad \in \mathbb {R}^{n,n} \; , \nonumber \\ \varvec{H}_2^k(z) \;&\widehat{=} \; \frac{\varvec{\hat{\tau }}_{ext}(z)}{\varvec{\underline{\delta \tau }}(z)} = \varvec{H}^k(z) \left[ \begin{array}{ccc} \varvec{0}_{n,n}&\varvec{I}_{n}&\varvec{0}_{n,n} \end{array} \right] ^T \quad \in \mathbb {R}^{n,n} \; ,\nonumber \\ \varvec{H}_3^k(z) \;&\widehat{=} \; \frac{\varvec{\hat{\tau }}_{ext}(z)}{\varvec{v}(z)} = z \, \varvec{H}^k(z) \left[ \begin{array}{ccc} \varvec{0}_{n,n}&\varvec{0}_{n,n}&\varvec{I}_{n} \end{array} \right] ^T \quad \in \mathbb {R}^{n,n} \; . \end{aligned}$$

(53)

The covariance matrix $\varvec{P}_{\varvec{X}_e}(k) \in \mathbb {R}^{n,n}$ of X_e(k) is defined as:

$$\begin{aligned} \varvec{P}_{\varvec{X}_e}(k) = E\left[ \varvec{X}_e(k)\varvec{X}_e^T(k)\right] \; . \end{aligned}$$

(54)

Assuming X_e(k) and U_e(k) on one hand, and $\varvec{\tau }_{ext}(k)$, $\varvec{\underline{\delta \tau }}(k)$ and $\varvec{v}(k+1)$ on the other hand, are respectively not correlated, thus in steady state and at each time step, $\varvec{P}_{\varvec{X}_e}(k)$ is the positive solution of the algebraic equation of Riccati:

$$\begin{aligned} \varvec{A}_e^k\varvec{P}_{\varvec{X}_e}(k)(\varvec{A}_e^k)^T + \varvec{B}_e^k\left[ \begin{array}{ccc} \varvec{W}_{ext} &{} \varvec{0}_{n,n} &{} \varvec{0}_{n,n} \\ \varvec{0}_{n,n} &{} \varvec{I}_{n} &{} \varvec{0}_{n,n} \\ \varvec{0}_{n,n} &{} \varvec{0}_{n,n} &{} \varvec{V}^a \end{array} \right] (\varvec{B}_e^k)^T = 0 \; . \end{aligned}$$

(55)

Thus the covariance matrix $\varvec{P}_{ext}(k)$ of $\varvec{\hat{\tau }}_{ext}$ is defined by:

$$\begin{aligned} \varvec{P}_{ext}(k) = \varvec{C}_e \varvec{P}_{\varvec{X}_e}(k) \varvec{C}_e^T \; . \end{aligned}$$

(56)

Finally the covariance matrix P(k) of $\varvec{\hat{\tau }}_{ext}$ when $\varvec{\hat{\tau }}_{ext}$ is only due to uncertainties and measurement noise and in absence of collision is obtained by resolving (55) when $\varvec{W}_{ext} = \varvec{0}_{n,n}$.