A whole-body rescue motion control with task-priority strategy for a rescue robot

Abstract

This paper introduces a new rescue robot consisting of dual-manipulator and variable configuration mobile platform for multi-purpose such as casualty extraction and hazardous goods transport. A specific rescue motion strategy using a whole-body is suggested to tackle characteristics of the robot configuration and balancing issue. In order to take into account safety and stability of the robot during the rescue motions, some restrictions are reflected into redundant domain of the robot with different priority. For stable motion control in various scenarios, a singularity-robust inverse kinematics is adopted and modified to induce smoother robot movement. The robustness of the control approach is checked numerically by comparing other method and experiments for the rescue motion strategy are carried out by using a small-scaled simulator in place of the rescue robot under development.

Appendix: Analysis on algorithmic singularity

For easy analysis on the algorithmic singularity issue, 1st-order equation of the TP-CLIK consisting of only two tasks is handled instead of the Eq. (4) and the equation is rewritten as follows:

$$\begin{aligned} \dot{\varvec{q}}= & {} \dot{\varvec{q}}_h + \tilde{\mathbf{J }}_l^\dag \left( \dot{\varvec{x}}_l - \mathbf J _l \dot{\varvec{q}}_h \right) \nonumber \\ \dot{\varvec{q}}_i= & {} \mathbf J _i^\dag \dot{\varvec{x}}_i, ~~~i \in h,l \nonumber \\ \mathbf{N }= & {} (\mathbf I _n - \mathbf J _h^\dag \mathbf J _h ) \end{aligned}$$

(24)

To compare both algorithms with regard to the algorithmic singularity, some terms associated with the singularity are available to be reexpressed based on the singular value decomposition (SVD). For the Eq. (24), let us define the decomposition of the $\mathbf J _h$ and $\mathbf J _l$ in the form

$$\begin{aligned} \mathbf J _h= & {} \sum _{i=1}^{r} \sigma _{i} \varvec{u}_{i} \varvec{v}_{i}^T \end{aligned}$$

(25)

$$\begin{aligned} \mathbf J _l= & {} \sum _{i=1}^{n-r} \sigma _{l,i} \varvec{u}_{l,i} \varvec{v}_{l,i}^T \end{aligned}$$

(26)

$\mathbf J _h$ is assumed to be full-rank for simplification. Then, decomposition of $\tilde{\mathbf{J }}_l$ is induced as follows: (Chiaverini 1997)

$$\begin{aligned} \tilde{\mathbf{J }}_l= & {} \mathbf U \varvec{\varXi } \mathbf V \nonumber \\= & {} \left( \begin{array}{ccc} \varvec{u}_{l,1}&\ldots&\varvec{u}_{l,n-r} \end{array} \right) \nonumber \\&\times \left( \begin{array}{ccc} \sigma _{l,1}\varvec{v}_{l,1}^T \varvec{v}_{r+1} &{} \ldots &{} \sigma _{l,1}\varvec{v}_{l,1}^T \varvec{v}_{n} \\ \vdots &{} \ddots &{} \vdots \\ \sigma _{l,n-r}\varvec{v}_{l,n-r}^T \varvec{v}_{r+1} &{} \ldots &{} \sigma _{l,n-r}\varvec{v}_{l,n-r}^T \varvec{v}_{n} \end{array} \right) \nonumber \\&\times \left( \begin{array}{c} \varvec{v}_{r+1}^T \\ \vdots \\ \varvec{v}_{n}^T \end{array} \right) \end{aligned}$$

(27)

It is noted that a loss of rank of $\tilde{\mathbf{J }}_l$ is related with $\varvec{\varXi }$, which is caused by the kinematic singularity of subtask ($\sigma _{l,n-r} = 0$) or the algorithmic singularity ($\varvec{v}_{l,i}^T \varvec{v}_j = 0,~ i \in [1,n-r],j \in [r+1,n]$). Also, as the inner products get closer to zero, inversion of the ill-conditioned $\tilde{\mathbf{J }}_l$ makes null space velocity excessively to maintain tracking accuracy of subtask.

On the other hand, inversion term of the Eq. (11) as the RTP-CLIK can be written as follows: (Chiaverini 1997)

$$\begin{aligned} (\mathbf I _n - \mathbf J _h^\dag \mathbf J _h )\mathbf{J _l}^\dag = \sum _{i=1}^{n-r}\sum _{j=r+1}^{n} \frac{\varvec{v}_{l,i}^T \varvec{v}_j}{\sigma _{l,i}} \varvec{v}_j\varvec{u}_{l,i}^T \end{aligned}$$

(28)

While the kinematic singularity ($\sigma _{l,i}$) of the subtask can lead joint divergence, there is no effect by the algorithmic singularity. This is because $(\mathbf I _n - \mathbf J _h^\dag \mathbf J _h )\mathbf{J _l}^\dag$ makes the null space velocity of subtask decrease when the associated term, $\varvec{v}_{l,i}^T \varvec{v}_j$, gets closer to zero.