Crossing-Point Estimation in Human–Robot Navigation—Statistical Linearization versus Sigma-Point Transformation

Abstract

Interactions between mobile robots and human operators in common areas require a high level of safety, especially in terms of trajectory planning, obstacle avoidance and mutual cooperation. In this connection, the crossings of planned trajectories and their uncertainty based on model fluctuations, system noise and sensor noise play an outstanding role. This paper discusses the calculation of the expected areas of interactions during human–robot navigation with respect to fuzzy and noisy information. The expected crossing points of the possible trajectories are nonlinearly associated with the positions and orientations of the robots and humans. The nonlinear transformation of a noisy system input, such as the directions of the motion of humans and robots, to a system output, the expected area of intersection of their trajectories, is performed by two methods: statistical linearization and the sigma-point transformation. For both approaches, fuzzy approximations are presented and the inverse problem is discussed where the input distribution parameters are computed from the given output distribution parameters.

1. Introduction

The planning and performing of mobile robot tasks in the presence of human operators while sharing the same workspace requires a high level of stability and safety. Research activities regarding navigation, obstacle avoidance, adaptation and collaboration between robots and human agents have been widely reported [1,2]. Multiple target tracking for robots using higher control levels in a control hierarchy are discussed in [3,4]. A human-friendly interaction between robots and humans can be obtained by human-like sensor systems [5]. A prominent role in robot navigation is the trajectory-crossing problem of robots and humans [6,7] and corresponding fuzzy solutions [8]. Motivations for a fuzzy solution of the intersection problem are manifold. One point is an uncertain measurement of the position and orientation of the human agent, because of which the use of a fuzzy signal and an adequate fuzzy processing seems natural [9,10]. Another aspect is the need for decreasing the computing effort in the case of complex calculations during a very small time interval. System uncertainties and observation noise lead to uncertainties of the intersection estimations.

The objective of this work is the formulation of the crossing/intersection problem by taking into account the uncertainties in human–robot systems, including sensors and motor characteristics. An important aspect is to define permissible uncertainties in a human–robot system for a given uncertainty at a possible intersection of their trajectories. Taking into account the nonlinearities, this is performed by the differential approach and a following analysis of the regarding Gaussian distributions. This approach is compared with the sigma-point transformation, which represents a simplification of the computation and a qualitative extension of the analysis regarding the statistics of the random variables. For broader areas of possible intersections, both methods are extended to fuzzy regions together with different numbers and shapes of fuzzy sets. The most important contributions are as follows:

• An investigation of uncertainties of possible intersection areas originating from sensor noise or system uncertainties.
• A direct and inverse transformation of the error variables at the intersection areas for two input variables (orientation angles) and two output variables (intersection coordinates).
• An extension of the method from two to six input variables (two orientation angles and four position coordinates).
• An exploration of the formulations of fuzzy versions.
• A formulation of the problem by the sigma-point transformation and corresponding comparison of the two methods.

This paper deals with the one-robot one-human trajectory-crossing problem, where small uncertainties in the position and orientation may lead to high uncertainties at the intersection points. The position and orientation of the human and robot are nonlinearly coupled but can be linearized. In the following, the linear part of the nonlinear system is considered in the analysis reported for small variations in the input [11]. Then, the “direct task” is described, meaning that the parameters of the input distribution are transformed to the output distribution parameters. The “inverse task” is also solved, meaning that for the defined output distribution parameters the input parameters are calculated. In this paper, two methods are outlined:

• The statistical linearization, which linearizes the nonlinearity around the operating area at the intersection. The means and standard deviations on the input parameters positions (orientations) are transformed through the linearized nonlinear system to obtain the means and standard deviations of the output parameters (the position of intersection).
• The sigma-point transformation, which calculates the so-called sigma points of the input distribution, including the mean and covariance of the input. The sigma points are directly propagated through the nonlinear system [12,13,14] to obtain the means and covariance of the output and, with this, the standard deviations of the output (the position of intersection). The advantage of the sigma-point transformation is that it captures the first- and second-order statistics of a random variable, whereas the statistical linearization approximates a random variable only by its first order. However, the computational complexity of the extended Kalman filter (EKF, differential approach) and unscented Kalman filter (UKF, sigma-point approach) is of the same order [13].

This paper is organized as follows. Section 2 describes the related work already conducted on unscented Kalman filters in mobile robot applications. In Section 3, the general intersection problem and its analytical approach is described. Section 4 deals with the transformation/conversion of Gaussian distributions for a two-input–two-output system and for a six-input–two-output system plus the corresponding inverse and fuzzy solutions. In Section 5, the sigma-point approach plus inverse and fuzzy solutions are addressed. Section 6 presents simulations of the statistical linearization and the sigma-point transformation to show the quality of the input–output conversion of the distributions and the impact of different resolutions of fuzzy approximations on the accuracy of the random variable intersection. Finally, Section 7 concludes this paper with a discussion of the two different approaches and a comparison of the methods.

2. Related Work

The crossing problem for mobile robots has been especially dealt with by [6,7]. Both publications deal with the so-called rendezvous problem whereby the key point is the trajectory planning under time constraints, taking into account the dynamics of the contributing robots. Uncertainties of possible intersection areas that come from sensor noise or system uncertainties are not discussed deeply. A fuzzy-adaptive extended Kalman filter (FAEKF) for the real-time attitude estimation of a mobile robot is proposed in [15] where fuzzy IF–THEN rules-based adaption laws modify the noise covariance matrices of the filter. However, the use of unscented Kalman filters or sigma-point transformation has not been discussed. For the estimation of landmarks, a simultaneous localization and mapping (SLAM) method is presented by [16] where an iterated sigma-point FastSLAM (ISP-FastSLAM) algorithm is proposed to minimize statistical linearization errors through the Gaussian–Newton iteration. A further application is presented by [17] where a walking robot uses sigma-point transformation for state estimation to guarantee stability in the system’s hybrid dynamics, which contains continuous and switching parts during movement. In [18], a vision-based SLAM system uses both extended Kalman filters (EKFs) and sigma-point Kalman filter (SPKF) algorithms and showed its superiority over the EKF. The pose estimation of mobile robots is discussed in [19] whereby several filter techniques like the Kalman filter (EKF), the unscented Kalman filter (UKF) and several variants of the particle filter (PF) are compared. It turns out that the UKF (also the sigma-point approach) exhibits almost the same computational cost. In [20], the inter-robot and robot–target correlations are discussed, and unscented transformation-based collaborative self-localization and a target tracking algorithm between robots are proposed. A tutorial on different approaches to exploit the structure of a system’s state and measurement models to reduce the computational demand of the algorithms is presented by [21]. In this publication, the computational complexity of different state estimation algorithms is presented, showing the superiority of the sigma-point transformation algorithms.

In all these publications, the problem of obstacle avoidance and/or the crossing problem in the presence of human actors are not taken into account, because of which the present paper is a further contribution to the robot–human interaction problem.

3. Computation of Intersections

The problem can be stated as follows:

A robot and human agent move in a common area according to their tasks or intentions. To avoid collisions, possible intersections of the paths of the agents should be predicted for both the trajectory planning and on-line interactions. To accomplish this, the positions, orientations and intended movements of the robot and human should be estimated as accurately as needed.

In this connection, uncertainties and noise on the random variables’ position/orientation ${\mathbf{x}}_R$, ${\mathbf{x}}_H$, ${\varphi }_R$ and ${\varphi }_H$ of the robot and human have a great impact on the calculation of the expected intersection position ${\mathbf{x}}_c$. The random variable ${\mathbf{x}}_c$ is calculated as the crossing point of the extension of the orientation or velocity vectors of the robot and human, which may change during motion depending on the task and current interaction. The task is to calculate the intersection and its uncertainty in the presence of the known uncertainties of the acting agent robot and human.

System noise ${\mathbf{w}}_R$ and ${\mathbf{w}}_H$ for the robot and human can be obtained from experiments. The noise ${\mathbf{w}}_c$ of the “virtual” intersection is composed of the nonlinear transformed noise ${\mathbf{w}}_R$ and ${\mathbf{w}}_H$ and some additional noise ${\mathbf{v}}_c$ that may come from uncertainties of the nonlinear computation of the intersection position ${\mathbf{x}}_c$ (see Figure 1). In the following, the geometrical relations are described as well as the fuzzy approximations and nonlinear transformations of the random variables ${\mathbf{x}}_R$, ${\mathbf{x}}_H$, ${\varphi }_R$ and ${\varphi }_H$.

3.1. Geometrical Relations

Let the y-axis of the mobile coordinate frame of the robot and human be aligned with their directions of motion. Furthermore, let the orientation angles ${\varphi }_R$ and ${\varphi }_H$ of the robot and human be measured from the x-axis of the base frame counterclockwise. Let the intersection $(x_c,y_c)$ of the two linear trajectories ${\mathbf{x}}_R\left(t\right)$ and ${\mathbf{x}}_H\left(t\right)$ in a plane be described by the following relations (see Figure 2):

$$\begin{array}{c}\hfill x_H=x_R+d_{RH}cos({\varphi }_R+{\delta }_R)\\ \hfill y_H=y_R+d_{RH}sin({\varphi }_R+{\delta }_R)\\ \hfill x_R=x_H+d_{RH}cos({\varphi }_H+{\delta }_H)\\ \hfill y_R=y_H+d_{RH}sin({\varphi }_H+{\delta }_H)\end{array}$$

(1)

where ${\mathbf{x}}_H=(x_H,y_H)$ and ${\mathbf{x}}_R=(x_R,y_R)$ are the positions of the human and robot and ${\varphi }_H$ and ${\varphi }_R$ are their orientation angles, and ${\delta }_H$ and ${\delta }_R$ are the positive angles measured from the y coordinates counterclockwise. The angle at the intersection is $\tilde{\beta }=\pi -{\delta }_R-{\delta }_H$. The variables ${\mathbf{x}}_H$, ${\mathbf{x}}_R$, ${\varphi }_R$, ${\varphi }_H$${\delta }_H$ and ${\varphi }_H$${\delta }_R$; distance $d_{RH}$; and angle $\gamma$ are assumed to be measurable. Angle $\gamma$ is a bearing angle for the robot-to-human direction measured in base coordinates. If ${\varphi }_H$ is not directly measurable, then it can be computed by

$${\varphi }_H=arcsin((y_H-y_R)/d_{RH})-{\delta }_H+\pi$$

(2)

The coordinates $x_c$ and $y_c$ of the intersection are computed straightforwardly by [8]

$$\begin{array}{ccc}\hfill x_c& =& \frac{A-B}{tan{\varphi }_R-tan{\varphi }_H}\hfill \\ \hfill y_c& =& \frac{Atan{\varphi }_H-Btan{\varphi }_R}{tan{\varphi }_R-tan{\varphi }_H}\hfill \\ \hfill A& =& x_{R}tan{\varphi }_R-y_R\hfill \\ \hfill B& =& x_{H}tan{\varphi }_H-y_H\hfill \end{array}$$

(3)

Rewriting (3) leads to

$$\begin{array}{ccc}\hfill x_c& =& \left(x_R\frac{tan{\varphi }_R}{G}-y_R\frac{1}{G}\right)-\left(x_H\frac{tan{\varphi }_H}{G}-y_H\frac{1}{G}\right)\hfill \\ \hfill y_c& =& \left(x_R\frac{tan{\varphi }_{R}tan{\varphi }_H}{G}-y_R\frac{tan{\varphi }_H}{G}\right)\hfill \\ & -& \left(x_H\frac{tan{\varphi }_{H}tan{\varphi }_R}{G}-y_H\frac{tan{\varphi }_R}{G}\right)\hfill \\ \hfill G& =& tan{\varphi }_R-tan{\varphi }_H\hfill \end{array}$$

(4)

After rearranging (4), we observe that ${\mathbf{x}}_c={(x_c,y_c)}^T$ is linear in ${\mathbf{x}}_{RH}={(x_R,y_R,x_H,y_H)}^T$

$$\begin{array}{c}\hfill {\mathbf{x}}_c=A_{RH}·{\mathbf{x}}_{RH}\end{array}$$

(5)

where

$$\begin{array}{c}\hfill A_{RH}=f({\varphi }_R,{\varphi }_H)=\\ \hfill \frac{1}{G}\left(\begin{array}{cccc}tan{\varphi }_R& -1& -tan{\varphi }_H& 1\\ tan{\varphi }_{R}tan{\varphi }_H& -tan{\varphi }_H& -tan{\varphi }_{R}tan{\varphi }_H& tan{\varphi }_R\end{array}\right)\end{array}$$

This notation is of advantage for further computations, such as the fuzzification of the intersection problem and the transformation of the error distributions.

3.2. Computation of Intersections—Fuzzy Approach

The fuzzy solution presented in the following is a combination of classical analytical (crisp) methods and rule-based methods in the sense of a Takagi–Sugeno fuzzy rule base. An appropriate choice of the number of fuzzy sets and corresponding fuzzy rules depends strongly on the specific application. In the present case, fuzzy sets are used as the approximation of nonlinear functions. In the following, we introduce a fuzzy rule-based approximation of (5) with $n\times n$ fuzzy rules $R_{i,j}$

$$\begin{array}{ccc}\hfill R_{i,j}:IF{\varphi }_R& =& {{\mathrm{\Phi }}_R}_{i}AND{\varphi }_H={{\mathrm{\Phi }}_H}_j\hfill \\ \hfill THEN{\mathbf{x}}_c& =& {A_{RH}}_{i,j}·{\mathbf{x}}_{RH}\hfill \end{array}$$

(6)

n—the number of fuzzy terms, ${\mathrm{\Phi }}_{R_i}$ and ${\mathrm{\Phi }}_{H_j}$ for ${\varphi }_R$ and ${\varphi }_H$, with the result

$$\begin{array}{c}\hfill {\mathbf{x}}_c=\sum _{i,j}{w}_i\left({\varphi }_R\right)w_j\left({\varphi }_H\right)·{A_{RH}}_{i,j}·{\mathbf{x}}_{RH}\end{array}$$

(7)

$i,j=1\dots n$, $w_i\left({\varphi }_R\right),w_j\left({\varphi }_H\right)\in [0,1]$ are normalized membership functions with ${\sum }_i{w}_i\left({\varphi }_R\right)=1$ and ${\sum }_j{w}_j\left({\varphi }_H\right)=1$.

Let the universes of discourse for ${\varphi }_R$ and ${\varphi }_H$ be ${\varphi }_R,{\varphi }_H\in [0,360]$. Furthermore, let these universes of discourse be divided into n partitions (for example, 6) of 60, which leads to $6\times 6$ fuzzy rules. The corresponding membership functions are shown in Figure 3. It turns out that this resolution leads to a poor fuzzy approximation. The approximation quality can be improved by increasing the number of fuzzy sets, which however results in a quadratic increase in the number of fuzzy rules. To avoid an “explosion” of the number of fuzzy rules being computed in one time step, a set of sub-areas covering a small number of rules for each sub-area is defined. Based on the measurements of ${\varphi }_R$ and ${\varphi }_H$, the appropriate sub-area is selected together with a corresponding set of rules (see Figure 4, sub-area $A_R,A_H$). With this, the number of rules to be activated at one time step of calculation is low, although the total number of rules can be high. At the borderlines between the sub-areas, abrupt changes may occur, which can be avoided by overlapping the sub-areas.

3.3. Differential Approach

The positions and orientations of robots and humans are usually corrupted with noise originated from system uncertainties, sensor errors and motor characteristics. These uncertainties become apparent in uncertainties in the crossing/intersection areas of the trajectories of the robot and human. The analysis of uncertainty and noise at ${\mathbf{x}}_c$ generated by the noise at ${\varphi }_R$, ${\varphi }_H$ and ${\mathbf{x}}_{RH}={(x_R,y_R,x_H,y_H)}^T$ requires a linearization of (4) around the operating points and with this a differential strategy. Let, for simplification, only the orientation angles ${\varphi }_R$ and ${\varphi }_H$ be corrupted with noise. In Section 4.3, the positions ${\mathbf{x}}_{RH}={(x_R,y_R,x_H,y_H)}^T$ are taken into account, too.

Differentiating (4) with ${\mathbf{x}}_{RH}=const.$ yields

$$\begin{array}{c}\hfill {\mathbf{dx}}_c=\tilde{J}·\mathbf{d}\mathbf{Œ}\\ \hfill \mathbf{d}\mathbf{Œ}={\left(d{\varphi }_{R}d{\varphi }_H\right)}^T;\tilde{J}=\left(\begin{array}{cc}{\tilde{J}}_{11}& {\tilde{J}}_{12}\\ {\tilde{J}}_{21}& {\tilde{J}}_{22}\end{array}\right)\end{array}$$

(8)

where

$$\begin{array}{ccc}\hfill {\tilde{J}}_{11}& =& \left(\begin{array}{cccc}-tan{\varphi }_H& 1& tan{\varphi }_H& -1\end{array}\right)\frac{{\mathbf{x}}_{RH}}{G^2·{cos}^2{\varphi }_R}\hfill \\ \hfill {\tilde{J}}_{12}& =& \left(\begin{array}{cccc}tan{\varphi }_R& -1& -tan{\varphi }_R& 1\end{array}\right)\frac{{\mathbf{x}}_{RH}}{G^2·{cos}^2{\varphi }_H}\hfill \\ \hfill {\tilde{J}}_{21}& =& {\tilde{J}}_{11}·tan{\varphi }_H\hfill \\ \hfill {\tilde{J}}_{22}& =& {\tilde{J}}_{12}·tan{\varphi }_R\hfill \end{array}$$

The following sections deal with the accuracy of the computed intersection in the case of noisy orientation information (see Figure 5).

4. Transformation of Gaussian Distributions

4.1. General Assumptions

Consider a nonlinear system

$$\mathbf{z}=F\left(\mathbf{x}\right)$$

(9)

where the random variables $\mathbf{x}={(x_1,x_2)}^T$ denote the input, $\mathbf{z}={(z_1,z_2)}^T$ denotes the output and F denotes a nonlinear transformation. The distribution of the uncorrelated Gaussian distributed components $x_1$ and $x_2$ is described by

$$f_{x_1,x_2}=\frac{1}{2\pi {\sigma }_{x_1}{\sigma }_{x_2}}exp(-\frac{1}{2}(\frac{e_{x_1}^2}{{\sigma }_{x_1}^2}+\frac{e_{x_2}^2}{{\sigma }_{x_2}^2}))$$

(10)

where $e_{x_1}=x_1-\overline{x_1}$, with $\overline{x_1}$—the mean $\left(x_1\right)$ and ${\sigma }_{x_1}$—the standard deviation $x_1$, and $e_{x_2}=x_2-\overline{x_2}$, with $\overline{x_2}$—the mean $\left(x_2\right)$ and ${\sigma }_{x_2}$—the standard deviation $x_2$.

The goal is as follows: Given the nonlinear transformation (9) and the distribution (10), compute the output signals $z_1$ and $z_2$ and their distributions together with their standard deviations and the correlation coefficient. Linear systems transform Gaussian distributions linearly such that the output signals are also Gaussian-distributed. This does not apply for nonlinear systems, but if the input standard deviation is small enough, then a local linear transfer function can be built for which the outputs are Gaussian-distributed. Suppose the input standard deviations are small with respect to the nonlinear function, then the output distribution can be written as follows:

$$\begin{array}{c}\hfill f_{z_1,z_2}=\frac{1}{2\pi {\sigma }_{z_1}{\sigma }_{z_2}\sqrt{1-{\rho }_{z_{12}}^2}}·\\ \hfill exp(-\frac{1}{2(1-{\rho }_{z_{12}}^2)}(\frac{e_{z_1}^2}{{\sigma }_{z_1}^2}+\frac{e_{z_2}^2}{{\sigma }_{z_2}^2}-\frac{2{\rho }_{z_{12}}{e}_{z_1}{e}_{z_2}}{{\sigma }_{z_1}{\sigma }_{z_2}}))\end{array}$$

(11)

${\rho }_{z_{12}}$—the correlation coefficient.

4.2. Statistical Linearization, Two Inputs–Two Outputs

Let the nonlinear transformation $\mathbf{F}$ be described by two smooth transfer functions (see block scheme Figure 6)

$$\begin{array}{c}\hfill z_1=f_1(x_1,x_2)\\ \hfill z_2=f_2(x_1,x_2)\end{array}$$

(12)

where $(x_1,x_2)=({\varphi }_R,{\varphi }_H)$ and $(z_1,z_2)=(x_c,y_c)$.

The linearization of (12) yields

$$\begin{array}{c}\hfill \mathbf{dz}=\tilde{J}·\mathbf{dx}or{\mathbf{e}}_{\mathbf{z}}=\tilde{J}·{\mathbf{e}}_{\mathbf{x}}\end{array}$$

(13)

with

$$\begin{array}{c}\hfill {\mathbf{e}}_{\mathbf{z}}={(e_{z_1},e_{z_2})}^{T}and{\mathbf{e}}_{\mathbf{x}}={(e_{x_1},e_{x_2})}^T\\ \hfill \mathbf{dz}={(d{z}_1,d{z}_2)}^{T}and\mathbf{dx}={(d{x}_1,d{x}_2)}^T\end{array}$$

(14)

$$\begin{array}{c}\hfill \tilde{J}=\left(\begin{array}{c}\partial f_1/\partial x_1,\partial f_1/\partial x_2\\ \partial f_2/\partial x_1,\partial f_2/\partial x_2\end{array}\right)\end{array}$$

(15)

4.2.1. Output Distribution

To obtain the density $f_{z_1,z_2}$ (11) of the output signal, we invert (15) and substitute the entries of ${\mathbf{e}}_{\mathbf{x}}$ into (10). $\tilde{J}$ is invertible if it is positive definite with $|\tilde{J}|>0$. Otherwise, there exist singularities due to different constellations of the position vector ${\mathbf{x}}_{RH}$ and/or the orientations ${\varphi }_R$ and ${\varphi }_H$. To find all the singularities requires a further analysis, which is not the content of this paper. However, a simple heuristic leads us to some obvious situations: If ${\varphi }_R={\varphi }_H$ or ${\varphi }_R={\varphi }_H+\pi$, then the human and robot would move in parallel either in the same or the opposite direction. On the other hand, one may also obtain diverging trajectories with no crossing.

$$\begin{array}{c}\hfill {\mathbf{e}}_{\mathbf{x}}=J·{\mathbf{e}}_{\mathbf{z}}\end{array}$$

(16)

with $J={\tilde{J}}^{-1}$ and

$$\begin{array}{c}\hfill J=\left(\begin{array}{cc}{J}_{11}& J_{12}\\ J_{21}& J_{22}\end{array}\right)=\left(\begin{array}{c}{\mathbf{j}}_{\mathbf{xz}}\\ {\mathbf{j}}_{\mathbf{yz}}\end{array}\right)\end{array}$$

(17)

where ${\mathbf{j}}_{\mathbf{xz}}=(J_{11},J_{12})$ and ${\mathbf{j}}_{\mathbf{yz}}=(J_{21},J_{22})$. The entries $J_{ij}$ are the result of the inversion of $\tilde{J}$. From this substitution, we obtain

$$\begin{array}{c}\hfill f_{x_1,x_2}=K_{x_1,x_2}·\\ \hfill exp(-\frac{1}{2}·{{\mathbf{e}}_{\mathbf{z}}}^T·({{\mathbf{j}}_{{\mathbf{x}}_{\mathbf{1}},\mathbf{z}}}^T,{{\mathbf{j}}_{{\mathbf{x}}_{\mathbf{2}},\mathbf{z}}}^T)·S_x^{-1}·\left(\begin{array}{c}{\mathbf{j}}_{{\mathbf{x}}_{\mathbf{1}},\mathbf{z}}\\ {\mathbf{j}}_{{\mathbf{x}}_{\mathbf{2}},\mathbf{z}}\end{array}\right)·{\mathbf{e}}_{\mathbf{z}})\end{array}$$

(18)

where $K_{x_1,x_2}=\frac{1}{2\pi {\sigma }_{x_1}{\sigma }_{x_2}}$ and

$$\begin{array}{c}\hfill S_x^{-1}=\left(\begin{array}{c}\frac{1}{{\sigma }_{x_1}^2},0\\ 0,\frac{1}{{\sigma }_{x_2}^2}\end{array}\right)\end{array}$$

(19)

The exponent of (18) is rewritten into

$$\begin{array}{c}\hfill x_{po}=-\frac{1}{2}·(\frac{1}{{\sigma }_{x_1}^2}{(e_{z_1}{J}_{11}+e_{z_2}{J}_{12})}^2\\ \hfill +\frac{1}{{\sigma }_{x_2}^2}{(e_{z_1}{J}_{21}+e_{z_2}{J}_{22})}^2)\end{array}$$

(20)

and furthermore

$$\begin{array}{c}\hfill x_{po}=-\frac{1}{2}·[e_{z_1}^2(\frac{J_{11}^2}{{\sigma }_{x_1}^2}+\frac{J_{21}^2}{{\sigma }_{x_2}^2})+e_{z_2}^2(\frac{J_{12}^2}{{\sigma }_{x_1}^2}+\frac{J_{22}^2}{{\sigma }_{x_2}^2})+\\ \hfill 2·e_{z_1}{e}_{z_2}(\frac{J_{11}{J}_{12}}{{\sigma }_{x_1}^2}+\frac{J_{21}{J}_{22}}{{\sigma }_{x_2}^2})]\end{array}$$

(21)

Let

$$\begin{array}{c}\hfill A=(\frac{J_{11}^2}{{\sigma }_{x_1}^2}+\frac{J_{21}^2}{{\sigma }_{x_2}^2});B=(\frac{J_{12}^2}{{\sigma }_{x_1}^2}+\frac{J_{22}^2}{{\sigma }_{x_2}^2})\\ \hfill C=(\frac{J_{11}{J}_{12}}{{\sigma }_{x_1}^2}+\frac{J_{21}{J}_{22}}{{\sigma }_{x_2}^2})\end{array}$$

(22)

then a comparison of $x_{po}$ in (21) and the exponent in (11) yields

$$\begin{array}{c}\hfill \frac{1}{(1-{\rho }_{z_{12}}^2)}\frac{1}{{\sigma }_{z_1}^2}=A;\frac{1}{(1-{\rho }_{z_{12}}^2)}\frac{1}{{\sigma }_{z_2}^2}=B\\ \hfill \frac{-2{\rho }_{z_{12}}}{(1-{\rho }_{z_{12}}^2)}\frac{1}{{\sigma }_{z_1}{\sigma }_{z_2}}=2C\end{array}$$

(23)

The standard deviations ${\sigma }_{z_1}$ and ${\sigma }_{z_2}$ and the correlation coefficient ${\rho }_{z_{12}}$ yield

$$\begin{array}{c}\hfill {\rho }_{z_{12}}=-\frac{C}{\sqrt{AB}}\\ \hfill \frac{1}{{\sigma }_{z_1}^2}=A-\frac{C^2}{B};\frac{1}{{\sigma }_{z_2}^2}=B-\frac{C^2}{A}\end{array}$$

(24)

The result is as follows: If the parameter of the input distribution and the transfer function $F(x,y)$ are known, then the output distribution parameters can be computed straightforwardly.

4.2.2. Fuzzy Solution

To save computing costs in real time, we create a TS fuzzy model that is represented by the rules $R_{ij}$.

$$\begin{array}{c}\hfill R_{ij}:\\ \hfill IF{x}_1={X_1}_{i}AND{x}_2={X_2}_i\end{array}$$

(25)

$$\begin{array}{c}\hfill THEN{\rho }_{z_{12}}=-\frac{C_{ij}}{\sqrt{A_{ij}{B}_{ij}}}\\ \hfill AND\\ \hfill \frac{1}{{\sigma }_{z_1}^2}=A_{ij}-\frac{C_{ij}^2}{B_{ij}};\\ \hfill AND\\ \hfill \frac{1}{{\sigma }_{z_2}^2}=B_{ij}-\frac{C_{ij}^2}{A_{ij}}\end{array}$$

where ${X_1}_i,{X_2}_i$ are fuzzy terms for $x_1,x_2$, and $A_{ij},B_{ij},C_{ij}$ are functions of the predefined variables $x_1={x_1}_i$ and $x_2={x_2}_i$.

From (25), we derive

$$\begin{array}{c}\hfill {\rho }_{z_{12}}=-\sum _{ij}{w}_i\left(x_1\right)w_j\left(x_2\right)\frac{C_{ij}}{\sqrt{A_{ij}{B}_{ij}}}\\ \hfill \frac{1}{{\sigma }_{z_1}^2}=\sum _{ij}{w}_i\left(x_1\right)w_j\left(x_2\right)(A_{ij}-\frac{C_{ij}^2}{B_{ij}})\\ \hfill \frac{1}{{\sigma }_{z_2}^2}=\sum _{ij}{w}_i\left(x_1\right)w_j\left(x_2\right)(B_{ij}-\frac{C_{ij}^2}{A_{ij}})\end{array}$$

(26)

$w_i\left(x_1\right)\in [0,1]$ and $w_j\left(x_2\right)\in [0,1]$ are the weighting functions with ${\sum }_i{w}_i\left(x_1\right)=1$, ${\sum }_j{w}_j\left(x_2\right)=1$.

4.2.3. Inverse Solution

The previous paragraph discussed the direct transformation task: Let the distribution parameters of the input variable be defined and find the corresponding output parameters. However, it might also be useful to solve the inverse task: Given the output parameters (standard deviation and correlation coefficient), find the corresponding input parameters. This solution of the inverse task is similar to those discussed in Section 4.2. The starting points are equations (10) and (11), which describe the distributions of the inputs and outputs, respectively. Then, we substitute (13) into (10) and rename the resulting exponent ${x_{po}}_z$ into ${x_{po}}_x$ and discuss the exponent ${x_{po}}_x$

$$\begin{array}{c}\hfill {x_{po}}_x=\frac{-1}{2(1-{\rho }_{z_{12}}^2)}({{\mathbf{e}}_{\mathbf{x}}}^T{\tilde{J}}^T{S}_z^{-1}\tilde{J}{\mathbf{e}}_{\mathbf{x}}-\frac{2{\rho }_{z_{12}}{e}_{z_1}{e}_{z_2}}{{\sigma }_{z_1}{\sigma }_{z_2}})\end{array}$$

(27)

with

$$\begin{array}{c}\hfill S_x^{-1}=\left(\begin{array}{c}\frac{1}{{\sigma }_{z_1}^2},0\\ 0,\frac{1}{{\sigma }_{z_2}^2}\end{array}\right)\end{array}$$

Now, comparing (27) with the exponent of (10) of the input density, we find that the mixed term in (27) must be zero, from which we obtain the correlation coefficient ${\rho }_{z_{12}}$ and with this the standard deviations of the inputs

$$\begin{array}{c}\hfill {\rho }_{z_{12}}=(\frac{{\tilde{J}}_{11}{\tilde{J}}_{12}}{{\sigma }_{z_1}^2}+\frac{{\tilde{J}}_{21}{\tilde{J}}_{22}}{{\sigma }_{z_2}^2})\frac{{\sigma }_{z_1}{\sigma }_{z_2}}{({\tilde{J}}_{11}{\tilde{J}}_{22}+{\tilde{J}}_{12}{\tilde{J}}_{21})}\\ \hfill \frac{1}{{\sigma }_x^2}=(\frac{{\tilde{J}}_{11}^2}{{\sigma }_{z_1}^2}+\frac{{\tilde{J}}_{21}^2}{{\sigma }_{z_2}^2}-\frac{2{\rho }_{z_{12}}}{{\sigma }_{z_1}{\sigma }_{z_2}}{\tilde{J}}_{11}{\tilde{J}}_{21})/(1-{\rho }_{z_{12}}^2)\\ \hfill \frac{1}{{\sigma }_y^2}=(\frac{{\tilde{J}}_{12}^2}{{\sigma }_{z_1}^2}+\frac{{\tilde{J}}_{22}^2}{{\sigma }_{z_2}^2}-\frac{2{\rho }_{z_{12}}}{{\sigma }_{z_1}{\sigma }_{z_2}}{\tilde{J}}_{12}{\tilde{J}}_{22})/(1-{\rho }_{z_{12}}^2)\end{array}$$

(28)

The detailed development can be found in [22].

4.3. Six Inputs–Two Outputs

Consider again the nonlinear system

$${\mathbf{x}}_{\mathbf{c}}=F\left(\mathbf{x}\right)$$

(29)

In the previous subsections, we assumed the positions ${\mathbf{x}}_R$ and ${\mathbf{x}}_H$ not to be corrupted with noise. However, taking into account the positions to be random variables, the number of inputs is 6 so that the input vector yields $\mathbf{x}={(x_1,x_2,x_3,x_4,x_5,x_6)}^T$ or $\mathbf{x}=({\varphi }_R,{\varphi }_H,x_R,y_R,x_H,y_H)$ with the output vector ${\mathbf{x}}_{\mathbf{c}}={(x_c,y_c)}^T$.

Furthermore, let the uncorrelated Gaussian-distributed inputs $x_1\dots x_6$ be described by the 6-dim density

$$f_{x_i}=\frac{1}{{\left(2\pi \right)}^{6/2}{\left|S_x\right|}^{1/2}}exp(-\frac{1}{2}\left({{\mathbf{e}}_{\mathbf{x}}}^T{S_x}^{-1}{\mathbf{e}}_{\mathbf{x}}\right))$$

(30)

where ${\mathbf{e}}_{\mathbf{x}}={({e_x}_1,{e_x}_2,\dots ,{e_x}_6)}^T$; ${\mathbf{e}}_{\mathbf{x}}=\mathbf{x}-\overline{\mathbf{x}}$, $\overline{\mathbf{x}}$—the mean($\mathbf{x}$) and $S_x$—the covariance matrix.

$$S_x=\left(\begin{array}{cccc}{\sigma }_{x_1}^2& 0& \dots & 0\\ 0& {\sigma }_{x_2}^2& \dots & 0\\ \dots & \dots & \dots & \dots \\ 0& \dots & 0& {\sigma }_{x_6}^2\end{array}\right)$$

According to (11), the output density is described by

$$\begin{array}{c}\hfill f_{x_c,y_c}=\frac{1}{2\pi {\sigma }_{x_c}{\sigma }_{y_c}\sqrt{1-{\rho }^2}}·\\ \hfill exp(-\frac{1}{2(1-{\rho }^2)}({\mathbf{e}}_{{\mathbf{x}}_{\mathbf{c}}}^{\mathbf{T}}{S_c}^{-1}{\mathbf{e}}_{{\mathbf{x}}_{\mathbf{c}}}-\frac{2\rho e_{x_c}{e}_{y_c}}{{\sigma }_{x_c}{\sigma }_{y_c}}))\end{array}$$

(31)

$\rho$—the correlation coefficient, ${\mathbf{e}}_{x_c}={(e_{x_c},e_{y_c})}^T$.

After some calculations [23], we find for $\rho$, $\frac{1}{{\sigma }_{x_c}^2}$ and $\frac{1}{{\sigma }_{y_c}^2}$

$$\begin{array}{ccc}\hfill \rho & =& -\frac{C}{\sqrt{AD}}\hfill \\ \hfill \frac{1}{{\sigma }_{x_c}^2}& =& A-\frac{C^2}{D};\frac{1}{{\sigma }_{y_c}^2}=D-\frac{C^2}{A}\hfill \end{array}$$

(32)

with

$$\begin{array}{c}\hfill A=\sum _{i=1}^6\frac{1}{{\sigma }_{x_i}^2}{J}_{i1}^2;B=\sum _{i=1}^6\frac{1}{{\sigma }_{x_i}^2}{J}_{i1}{J}_{i2}\\ \hfill C=\sum _{i=1}^6\frac{1}{{\sigma }_{x_i}^2}{J}_{i1}{J}_{i2};D=\sum _{i=1}^6\frac{1}{{\sigma }_{x_i}^2}{J}_{i2}^2\end{array}$$

(33)

This is the counterpart to the 2-dim input case (24).

4.3.1. Inverse Solution

An inverse solution cannot be uniquely computed due to the undetermined character of the 6-input–2-output system. Therefore, from the required variances at the intersection position (output), the corresponding variances for the positions and orientations of the robot–human or robot–robot (input) cannot be concluded.

4.3.2. Fuzzy Approach

The steps to the fuzzy approach are very similar to those of the 2-input case:

-

Define the operation points ${\mathbf{x}}_i={(x_1,x_2,x_3,x_4,x_5,x_6)}_i^T$;

-

Compute $A_i$, $B_i$ and $C_i$ at ${\mathbf{x}}_i={(x_1,x_2,x_3,x_4,x_5,x_6)}_i^T$ from (33);

-

Formulate the fuzzy rules $R_i$ according to (25) and (26), $i=1\dots n$.

The number n of rules is computed as follows:

With $l=6$—the number of fuzzy terms and $k=6$—the number of inputs, we obtain $n=l^k=6^6$—the number of rules.

This number of rules is unacceptably high. To limit n to an adequate number, one has to limit the number of inputs and/or fuzzy terms to look for the most influential variables either in a heuristic or systematic way [24]. This however is not the issue to be discussed in this paper.

5. Sigma-Point Transformation

In the following, the estimation/identification of the standard deviations of possible intersection coordinates of trajectories for both the robot–robot and human–robot combinations by means of the sigma-point technique is discussed. The following method is based on the unscented Kalman filter technique where the intersections cannot be directly measured but predicted/computed only. Nevertheless, it is possible to compute the variance of the predicted events, such as possible collisions or planned rendezvous situations, by a direct propagation of statistical parameters—the sigma points—through the nonlinear geometrical relation, which is a result of the crossing of two trajectories. Let $\mathbf{x}={(x_1,x_2)}^T$—the input vector and ${\mathbf{x}}_{\mathbf{c}}={(x_{c1},x_{c2})}^T$—the output vector where for the special case ${(x_1,x_2)}^T={({\varphi }_R,{\varphi }_H)}^T$ and ${(x_{c1},x_{c2})}^T={(x_c,y_c)}^T$. The nonlinear relation between $\mathbf{x}$ and ${\mathbf{x}}_{\mathbf{c}}$ is given by (34)

$${\mathbf{x}}_{\mathbf{c}}=\mathbf{F}\left(\mathbf{x}\right)$$

(34)

For the discrete case, we obtain for the state ${\mathbf{x}}_{\mathbf{c}}$

$${\mathbf{x}}_{\mathbf{c}}\left(k\right)=\mathbf{F}(\mathbf{x}(k-1)+\mathbf{w}(k-1))$$

(35)

and for the measured output ${\mathbf{z}}_{\mathbf{c}}\left(k\right)$

$${\mathbf{z}}_{\mathbf{c}}\left(k\right)=\mathbf{h}\left({\mathbf{x}}_{\mathbf{c}}\right)\left(k\right)+\mathbf{v}\left(k\right))$$

(36)

where $\mathbf{w}$ and $\mathbf{v}$ are the system noise and measurement noise, respectively. $\mathbf{h}\left({\mathbf{x}}_{\mathbf{c}}\right)$ is the output nonlinearity. Furthermore, let there be the following:

• $\overline{\mathbf{x}}\left(k\right)$—the mean at time $t_k$;
• $\mathbf{P}\left(k\right)$—the covariance matrix;
• ${\mathbf{x}}_0$—the initial state with the known mean ${\mu }_0=E\left({\mathbf{x}}_0\right)$;
• ${\mathbf{P}}_0\left(k\right)=E\left[({\mathbf{x}}_0-{\mu }_0){({\mathbf{x}}_0-{\mu }_0)}^T\right]$.

5.1. Selection of Sigma Points

Sigma points are the selected parameters of a given error distribution of a random variable. Sigma points lie along the major eigen-axes of the covariance matrix of the random variable. The height of each sigma point (see Figure 7) represents its relative weight $W^j$ used in the following selection procedure.

Let $\mathbf{X}(k-1)$ be a set of $2n+1$ sigma points where n is the dimension of the state space (in our example, $n=2$).

$$\mathbf{X}(k-1)=\left\{({\mathbf{x}}^j(k-1),W^j)\right|j=0\dots 2n\}$$

(37)

Consider the following selection of sigma points

$$\begin{array}{ccc}\hfill {\mathbf{x}}^0(k-1)& =& \overline{\mathbf{x}}(k-1)\hfill \\ & & -1<W^0<1\hfill \\ \hfill W^0& =& \frac{\lambda }{n+\lambda };\lambda ={\alpha }^2(n+\kappa )-n\hfill \end{array}$$

(38)

$$\begin{array}{cccc}& \hfill {\mathbf{x}}^i(k-1)& =& \overline{\mathbf{x}}(k-1)+\sqrt{\left(\frac{n}{1-W^0}\mathbf{P}(k-1)\right)};i=1\dots n\hfill \\ & \hfill {\mathbf{x}}^i(k-1)& =& \overline{\mathbf{x}}(k-1)-\sqrt{\left(\frac{n}{1-W^0}\mathbf{P}(k-1)\right)};i=(n+1)\dots 2n\hfill \end{array}$$

$$\begin{array}{ccc}\hfill W^j& =& \frac{1-W^0}{2n}\hfill \end{array}$$

(39)

under the following condition

$$\sum _{j=0}^{2n}{W}^j=1$$

(40)

$\alpha$ and $\kappa$ are scaling factors. A usual choice is $\alpha ={10}^{-2}$ and $\kappa =0$. $\sqrt{\frac{n}{1-W^0}\mathbf{P}(k-1)}$ is the row/column of the matrix square root of $\frac{n}{1-W^0}\mathbf{P}$. The square root of a matrix $\mathbf{P}$ is the solution $\mathbf{S}$ for $\mathbf{P}=\mathbf{S}·\mathbf{S}$, which is obtained by Cholesky factorization.

5.2. Model Forecast Step

To go on with the UKF, the following step is devoted to the model forecast. In this way, the sigma points ${\mathbf{x}}^j\left(k\right)$ are propagated through the nonlinear process model

$${\mathbf{x}}_c^{f,j}\left(k\right)=\mathbf{F}\left({\mathbf{x}}^j(k-1)\right)$$

(41)

where the superscript f means “forecast”. From these transformed and forecasted sigma points, the mean and covariance for the forecast value of ${\mathbf{x}}_c\left(k\right)$ are

$$\begin{array}{ccc}\hfill & {\mathbf{x}}_c^f\left(k\right)& =\sum _{j=0}^{2n}{W}^j{\mathbf{x}}_c^{f,j}\left(k\right)\hfill \\ & {\mathbf{P}}^f\left(k\right)& =\sum _{j=0}^{2n}{W}^j({\mathbf{x}}_c^{f,j}\left(k\right)-{\mathbf{x}}_c^f\left(k\right)){({\mathbf{x}}_c^{f,j}\left(k\right)-{\mathbf{x}}_c^f\left(k\right))}^T\hfill \end{array}$$

(42)

5.3. Measurement Update Step

In this step, the sigma points are propagated through the nonlinear observation model

$${\mathbf{z}}_c^{f,j}\left(k\right)=\mathbf{h}\left({\mathbf{x}}_c^j(k-1)\right)$$

(43)

from which we obtain the mean and covariance (innovation covariance)

$$\begin{array}{c}\hfill {\mathbf{z}}_c^f(k-1)=\sum _{j=0}^{2n}{W}^j{\mathbf{z}}_c^{f,j}(k-1)\\ \hfill Cov\left({\tilde{\mathbf{z}}}_c^f(k-1)\right)=\\ \hfill \sum _{j=0}^{2n}{W}^j({\mathbf{z}}_c^{f,j}(k-1)-{\mathbf{z}}_c^f(k-1))\times \\ \hfill {({\mathbf{z}}_c^{f,j}(k-1)-{\mathbf{z}}_c^f(k-1))}^T+\mathbf{R}\left(k\right)\end{array}$$

(44)

and the cross-covariance

$$\begin{array}{c}\hfill Cov({\tilde{\mathbf{x}}}_c^f\left(k\right),{\tilde{\mathbf{z}}}_c^f(k-1))=\\ \hfill \sum _{j=0}^{2n}{W}^j({\mathbf{x}}_c^{f,j}\left(k\right)-{\mathbf{x}}_c^f\left(k\right)){({\mathbf{z}}_c^{f,j}(k-1)-{\mathbf{z}}_c^f(k-1))}^T\end{array}$$

(45)

5.4. Data Assimilation Step

In this step, the forecast information is combined with the new information from the output $\mathbf{z}\left(k\right)$ from which we obtain, with the Kalman filter, gain $\mathbf{K}$

$${\widehat{\mathbf{x}}}_c\left(k\right)={\mathbf{x}}_c^f\left(k\right)+\mathbf{K}\left(k\right)({\mathbf{z}}_c\left(k\right)-{\mathbf{z}}_c^f(k-1))$$

(46)

The gain $\mathbf{K}$ is given by

$$\mathbf{K}\left(k\right)=Cov({\tilde{\mathbf{x}}}_c^f\left(k\right),{\tilde{\mathbf{z}}}_c^f(k-1))·Co{v}^{-1}\left({\tilde{\mathbf{z}}}_c^f(k-1)\right)$$

(47)

and the posterior covariance is updated by

$$\mathbf{P}\left(k\right)={\mathbf{P}}^f\left(k\right)-\mathbf{K}\left(k\right)·Cov\left({\tilde{\mathbf{z}}}_c^f(k-1)\right){\mathbf{K}}^T\left(k\right)$$

(48)

Usually, it is sufficient to compute the mean and variance for the output/state ${\mathbf{x}}_c$ of the nonlinear static system $\mathbf{F}\left(\mathbf{x}\right)$. In this case, it is possible to stop further computing at Equation (42), meaning to rather calculate the transformed sigma points ${\mathbf{x}}_c^{f,j}$ and develop the specific output means and variances from (41) and (42). In this connection, it is enough to substitute the covariance matrix $\mathbf{Q}$ into (38) instead of $\mathbf{P}$. One advantage of the sigma-point approach prior to statistical linearization is the easy scalability to multi-dimensional random variables.

For the intersection problem, there are 2 cases:

• The 2 inputs, 2 outputs (2 orientation angles and 2 crossing coordinates);
• The 6 inputs, 2 outputs (2 orientation angles and 4 position coordinates, and 2 crossing coordinates).

For the statistical linearization (method 1), the step from the 2 inputs–2 outputs case to the (6,2)-case is computationally more costly than that for the sigma-point approach (method 2), (see Equations (20)–(24) versus Equations (37) and (40)–(42)).

5.5. Sigma Points—Fuzzy Solutions

In order to lower the computing effort, the application of the TS fuzzy interpolation may be a solution, which will be shown in the following. Having a look at the two-dimensional problem, we can see a nonlinear propagation of the input sigma points through a nonlinear function $\mathbf{F}$. Let ${\mathbf{x}}^j$ be the two-dimensional “input” sigma points

$${\mathbf{x}}^j={(x_1^j,x_2^j)}^T$$

(49)

or for the special case “intersection”

$${\mathbf{x}}^j={({\varphi }_R^j,{\varphi }_H^j)}^T$$

(50)

The propagation through $\mathbf{F}$ leads to the “output” sigma points

$${\mathbf{x}}_c^{f,j}\left(k\right)=\mathbf{F}\left({\mathbf{x}}^j(k-1)\right)$$

(51)

or for the special case

$$\begin{array}{ccc}\hfill {\mathbf{x}}_c^{f,j}\left(k\right)& =& \mathbf{F}(x_1^j(k-1),x_2^j(k-1))=\hfill \\ & & \mathbf{F}({\varphi }_R^j(k-1),{\varphi }_H^j(k-1))\hfill \end{array}$$

(52)

The special nonlinear function $\mathbf{F}$ is described by (see (5))

$$\begin{array}{c}\hfill {\mathbf{x}}_c={\mathbf{A}}_{RH}({\varphi }_R,{\varphi }_H)·{\mathbf{x}}_{RH}\end{array}$$

(53)

where ${\mathbf{A}}_{RH}$ is a nonlinear matrix (6) linearly combined with the position vector ${\mathbf{x}}_{RH}={(x_R,y_R,x_H,y_H)}^T$.

A fuzzification aims at ${\mathbf{A}}_{RH}$:

$$\begin{array}{c}\hfill {\mathbf{F}}^{fuzz}({\varphi }_R,{\varphi }_H)={\mathbf{A}}_{RH}^{fuzz}·{\mathbf{x}}_{RH}=\\ \hfill \sum _{l_1,l_2}^m{w}^{l_1}\left({\varphi }_R\right)w^{l_2}\left({\varphi }_H\right)·{\mathbf{A}}_{RH}({\varphi }_R^{l_1},{\varphi }_H^{l_2})·{\mathbf{x}}_{RH}\end{array}$$

(54)

Applied to the sigma points $({\varphi }_R^j,{\varphi }_H^j)$, we obtain a TS fuzzy model described by the following rules $R_{l_1,l_2}$

$$R_{l_1,l_2}:$$

(55)

$$\begin{array}{c}\hfill IF{\varphi }_R^j={{\mathrm{\Phi }}_R^j}_{l_1}AND{\varphi }_H^j={{\mathrm{\Phi }}_H^j}_{l_2}\\ \hfill THEN{\mathbf{x}}_c^{f,j}={\mathbf{A}}_{RH}({\varphi }_R^{l_1,j},{\varphi }_H^{l_2,j})·{\mathbf{x}}_{RH}\end{array}$$

where ${{\mathrm{\Phi }}_R^j}_{l_1},{{\mathrm{\Phi }}_H^j}_{l_2}$ are fuzzy terms for ${\varphi }_R^j,{\varphi }_H^j$; the matrices ${\mathbf{A}}_{RH}$ are functions of the predefined variables ${\varphi }_R^j$ and ${\varphi }_H^j$. This set of rules leads to the result

$$\begin{array}{c}\hfill {\mathbf{x}}_c^{f,j}={\mathbf{F}}^{fuzz}({\varphi }_R^j,{\varphi }_H^j)=\\ \hfill \sum _{l_1,l_2}^m{w}^{l_1}\left({\varphi }_R^j\right)w^{l_2}\left({\varphi }_H^j\right)·{\mathbf{A}}_{RH}({\varphi }_R^{l_1,j},{\varphi }_H^{l_2,j})·{\mathbf{x}}_{RH}\end{array}$$

(56)

$w^{l_1}\left({\varphi }_R^j\right)\in [0,1]$ and $w^{l_2}\left({\varphi }_H^j\right)\in [0,1]$ are weighting functions with ${\sum }_{l_1}{w}^{l_1}=1$, ${\sum }_{l_2}{w}^{l_2}=1$. The advantage of this approach is that the $l_1\times l_2$ matrices ${\mathbf{A}}_{RH}^{l_1,l_2,j}={\mathbf{A}}_{RH}({\varphi }_R^{l_1,j},{\varphi }_H^{l_2,j})$ can be computed off-line. Then, the calculation of the mean and covariance matrix is obtained by

$$\begin{array}{c}\hfill {\mathbf{x}}_c^f\left(k\right)=\sum _{j=0}^{2n}{\mathbf{W}}^j{\mathbf{x}}_c^{f,j}\left(k\right)\\ \hfill {\mathbf{P}}^f\left(k\right)=\sum _{j=0}^{2n}{\mathbf{W}}^j{\tilde{\mathbf{x}}}_c^{f,j}\left(k\right){\left({\tilde{\mathbf{x}}}_c^{f,j}\left(k\right)\right)}^T\\ \hfill {\tilde{\mathbf{x}}}_c^{f,j}={\mathbf{x}}_c^{f,j}-{\mathbf{x}}_c^f\end{array}$$

(57)

From the covariance ${\mathbf{P}}^f$, the variances ${{\sigma }_c}_{xx},{{\sigma }_c}_{yy},{{\sigma }_c}_{xy}$ can be obtained

$$\begin{array}{c}\hfill {{\sigma }_c}_{xx}=E\left({(x_c^f-{\overline{x}}_c^f)}^2\right))\\ \hfill {{\sigma }_c}_{yy}=E\left({(y_c^f-{\overline{y}}_c^f)}^2\right))\\ \hfill {{\sigma }_c}_{xy}={{\sigma }_c}_{yx}=E((x_c^f-{\overline{x}}_c^f)·(y_c^f-{\overline{y}}_c^f))\end{array}$$

(58)

5.6. Inverse Solution

The inverse solution for the sigma-point approach is much easier to obtain than that for the statistical linearization method. Starting from Equation (34), we build the inverse function

$$\mathbf{x}={\mathbf{F}}^{-1}\left({\mathbf{x}}_c\right)$$

(59)

on the condition that ${\mathbf{F}}^{-1}$ exists. Then, the covariance matrix $\mathbf{P}$ is defined in correspondence to the required variances ${{\sigma }_c}_{xx},{{\sigma }_c}_{yy}$ and ${{\sigma }_c}_{xy}$. The following steps correspond to Equations (34)–(42). The position vector ${\mathbf{x}}_{RH}$ is assumed to be known. The inversion of $\mathbf{F}$ requires a linearization of ${\mathbf{x}}_{RH}$ and a starting point to obtain a stable convergence to the inverse ${\mathbf{F}}^{-1}$. The result is the mean $\mathbf{x}$ and the covariance $\mathbf{Q}$ at the input. A reliable inversion is only possible for the 2-input–2-output case.

5.7. Six-Inputs–Two-Outputs

This case works exactly as the 2-input–2-output case along with Equations (34)–(42) due to the fact that the computation of the sigma points (38)–(40) and the propagation through the nonlinearity $\mathbf{F}$ automatically include the input and output dimensions.

6. Simulation Results

The following simulations show the results of the uncertainties of the predicted intersections based on statistical linearization and sigma-point transformation. For both methods, identical parameters are employed for comparison reasons (see Figure 2). The position/orientation of the robot and human are given by the following:

• ${\mathbf{x}}_R={(x_R,y_R)}^T={(2,0)}^T$m;
• ${\mathbf{x}}_H={(x_H,y_H)}^T={(4,10)}^T$m;
• ${\varphi }_R=1.78$ rad = 102°;
• ${\varphi }_H=3.69$ rad = 212°.
• ${\varphi }_R$ and ${\varphi }_H$ are corrupted by Gaussian noise with standard deviations (std) of ${\sigma }_{{\varphi }_R}={\sigma }_{x_1}=0.02$ rad, ${(=1.1}^{°})$, and ${\sigma }_{{\varphi }_H}={\sigma }_{x_2}=0.02$ rad, ${(=1.1}^{°})$.

6.1. Statistical Linearization

Table 1. Standard deviations and fuzzy and non-fuzzy results.

Input Std	0.02 Gauss, Bell Shaped (GB)				Gauss	0.05 GB
sector size/°	${60}^{°}$	${30}^{°}$	${15}^{°}$	$\mathbf{7}.{\mathbf{5}}^{°}$	$\mathbf{7}.{\mathbf{5}}^{°}$	$\mathbf{7}.{\mathbf{5}}^{°}$
non-fuzz ${{\sigma }_x}_c$	0.143	0.140	0.138	0.125	0.144	0.366
fuzz ${{\sigma }_x}_c$	0.220	0.184	0.140	0.126	0.144	0.367
non-fuzz ${{\sigma }_x}_m$	0.160	0.144	0.138	0.126	0.142	0.368
fuzz ${{\sigma }_x}_m$	0.555	0.224	0.061	0.225	0.164	0.381
non-fuzz ${{\sigma }_y}_c$	0.128	0.132	0.123	0.114	0.124	0.303
fuzz ${{\sigma }_y}_c$	0.092	0.087	0.120	0.112	0.122	0.299
non-fuzz ${{\sigma }_y}_m$	0.134	0.120	0.123	0.113	0.129	0.310
fuzz ${{\sigma }_y}_m$	0.599	0.171	0.034	0.154	0.139	0.325
non-fuzz ${{\rho }_{xy}}_c$	0.576	0.541	0.588	0.561	0.623	0.669
fuzz ${{\rho }_{xy}}_c$	−0.263	0.272	0.478	0.506	0.592	0.592
non-fuzz ${{\rho }_{xy}}_m$	0.572	0.459	0.586	0.549	0.660	0.667
fuzz ${{\rho }_{xy}}_m$	0.380	0.575	0.990	0.711	0.635	0.592

shows a comparison of the non-fuzzy method with the fuzzy approach using sectors of ${60}^{°},{30}^{°},{15}^{°},7.5^{°}$ of the unit circle for the orientations of the robot and human. The notations in

Table 2. Covariances, standard deviations—computed and measured.

Outputs	Covariance, Computed	Covariance, Measured	${{\mathit{\sigma }}_{\mathit{x}}}_{\mathit{c}}$, Comp/Meas	${{\mathit{\sigma }}_{\mathit{y}}}_{\mathit{c}}$, Comp/Meas
2 inputs	$P=\left(\begin{array}{cc}0.0213& 0.0114\\ 0.0114& 0.0159\end{array}\right)$	$covar=\left(\begin{array}{cc}0.0264& 0.0146\\ 0.0146& 0.0166\end{array}\right)$	$0.145/0.144$	$0.126/0.134$
2 inputs, stat. lin.	-	-	$0.144/0.142$	$0.124/0.129$
6 inputs	$P=\left(\begin{array}{cc}0.0235& 0.0127\\ 0.0127& 0.0165\end{array}\right)$	$covar=\left(\begin{array}{cc}0.0213& 0.0131\\ 0.0131& 0.0182\end{array}\right)$	$0.135/0.145$	$0.122/0.134$
Direct solution	$P=\left(\begin{array}{cc}0.0234& 0.0133\\ 0.0133& 0.0151\end{array}\right)$	$covar=\left(\begin{array}{cc}0.0264& 0.0146\\ 0.0146& 0.0166\end{array}\right)$	$0.152/0.162$	$0.128/128$
Inverse solution	$P={10}^{-3}\times \left(\begin{array}{cc}0.4666& 0.0522\\ 0.0522& 0.4744\end{array}\right)$	$covar={10}^{-3}\times \left(\begin{array}{cc}0.4841& -0.0190\\ -0.0190& 0.396\end{array}\right)$	$0.0215/0.0220$	$0.0217/0.0190$

are as follows: ${{\sigma }_x}_c$—std-computed, ${{\sigma }_x}_m$—std-measured, etc. As expected, we see that higher resolutions lead to a better match between the fuzzy and analytical approach. Furthermore, the match between the measured and calculated values depends on the form of membership functions (MFS). For example, low input standard deviations (0.02 rad) show a better match for Gaussian membership functions, and higher input standard deviations (0.05 rad = $2.9^{°}$) require Gaussian bell-shaped membership functions, which comes from different smoothing effects (see columns 4 and 5 in Table 2).

A comparison of the control surfaces and corresponding measurements $x_{cm},y_{cm}$ (black and red dots) is depicted in Figure 8, Figure 9 and Figure 10. Figure 8 shows the control surface of $x_c$ and $y_c$ for the non-fuzzy case (4). The control surfaces of the fuzzy approximations (7) for the ${30}^{°}$ and $7.5^{°}$ sectors are shown in Figure 9 and Figure 10. The resolution ${30}^{°}$ (Figure 9) shows a very high deviation compared to the non-fuzzy approach (Figure 8), which decreases further down to the resolution $7.5^{°}$ (Figure 10). This explains the high differences between the measured and computed standard deviations and correlation coefficients, in particular for sector sizes of ${30}^{°}$ and higher.

6.2. Sigma-Point Method

Two-inputs–two-outputs:

The simulation of the sigma-point method is based on a Matlab implementation of an unscented Kalman filter by [25]. The first example deals with the 2-inputs–2-outputs case in which only the orientations are taken into account, but the disturbances of the positions of the robot and human are not part of the sigma-point calculation. A comparison between the computed and measured covariance shows a very good match. The same holds for the standard deviations ${{\sigma }_x}_c,{{\sigma }_y}_c$. A comparison with the statistical linearization shows a good match as well (see Table 2, rows 1 and 2).

A view at the sigma points presents the following results: Figure 11 shows the two-dimensional distribution of the orientation angles $({\varphi }_R$, ${\varphi }_H)$ and the corresponding sigma points $s_1,\dots ,s_5$ where $s_1$ denotes the mean value. Figure 12 shows the two-dimensional distribution of the intersection coordinates $(x_c,y_c)$ with the sigma points $S_1,\dots ,S_5$. $S_1$ denotes the mean value and $S_1,\dots ,S_5$ are distributed in such a way that the $s_i$ are transformed into $S_i$, $i=1\dots 5$. From both figures, an optimal selection of both $s_1,\dots ,s_5$ and $S_1,\dots ,S_5$ can be observed, which results in a good match of the computed and measured standard deviations ${\sigma }_{{\mathbf{x}}_c}$.

Six-inputs–two-outputs:

The 6-inputs–2-outputs example shows that the additional consideration of 4 input position coordinates with ${\sigma }_{{\mathbf{x}}_R}=0.02$ leads to similar results both for the computed and measured covariances and between the sigma-point method and statistical linearization (see $P(7,7)={{\sigma }_x}_c^2$, $P(8,8)={{\sigma }_y}_c^2$ and $covar(7,7)={{\sigma }_x}_m^2$, $covar(8,8)={{\sigma }_y}_m^2$, and ${{\sigma }_x}_c^2$—computed, and ${{\sigma }_x}_m^2$—the measured variation). Table 2 shows the covariance submatrix considering the output positions only.

Computed covariance:

$$\begin{array}{ccc}\hfill P& =& {10}^{-1}\times \left(\begin{array}{cccccccc}0.004& -0.000& -0.000& 0.000& -0.000& -0.000& -0.030& -0.018\\ -0.000& 0.004& 0.000& -0.000& -0.000& -0.000& 0.003& -0.017\\ -0.000& 0.000& 0.004& 0.000& -0.000& -0.000& 0.004& 0.002\\ 0.000& -0.000& 0.000& 0.004& -0.000& -0.000& 0.001& 0.000\\ -0.000& -0.000& -0.000& -0.000& 0.004& 0.000& 0.000& -0.002\\ -0.000& -0.000& -0.000& -0.000& 0.000& 0.004& -0.001& 0.004\\ -0.030& 0.003& 0.004& 0.001& 0.000& -0.001& \mathbf{0}.\mathbf{235}& \mathbf{0}.\mathbf{127}\\ -0.018& -0.017& 0.002& 0.000& -0.002& 0.004& \mathbf{0}.\mathbf{127}& \mathbf{0}.\mathbf{165}\end{array}\right)\hfill \\ \hfill {{\sigma }_x}_c& =& 0.153,{{\sigma }_y}_c=0.122\hfill \end{array}$$

(60)

Measured covariance:

$$\begin{array}{ccc}\hfill covar& =& {10}^{-1}\times \left(\begin{array}{cccccccc}0.004& 0.000& 0.000& 0.000& 0.000& -0.000& -0.028& -0.020\\ 0.000& 0.004& 0.000& 0.001& 0.000& -0.000& 0.000& -0.020\\ 0.000& 0.000& 0.004& -0.000& 0.001& -0.001& 0.003& 0.001\\ 0.000& 0.001& -0.000& 0.004& -0.000& -0.000& -0.000& -0.003\\ 0.000& 0.000& 0.001& -0.000& 0.005& -0.000& -0.001& -0.006\\ -0.000& -0.000& -0.001& -0.000& -0.000& 0.005& -0.000& 0.005\\ -0.028& 0.000& 0.003& -0.000& -0.001& -0.000& \mathbf{0}.\mathbf{213}& \mathbf{0}.\mathbf{131}\\ -0.020& -0.020& 0.001& -0.003& -0.006& 0.005& \mathbf{0}.\mathbf{131}& \mathbf{0}.\mathbf{182}\end{array}\right)\hfill \\ \hfill {{\sigma }_x}_c& =& 0.145,{{\sigma }_y}_c=0.134\hfill \end{array}$$

(61)

Two-inputs–two-outputs, direct and inverse solution

The next example shows the computation of the direct and inverse cases. In the direct case, we obtain again similar values between the computed and measured covariances and, with this, the standard deviations. The results of the inverse solution lead to similar values of the original inputs (orientations $x_1={\varphi }_R,x_2={\varphi }_H$) (see Table 2). The simulations of the fuzzy versions showed the same similarities and can therefore be left out here.

Two-inputs–two-outputs, moving robot–human

The next example deals with the robot and human in motion. Figure 13 shows the positions and orientations of the robot and human at selected time steps $t_1\dots t_5$ and the development of the corresponding intersections ${\mathbf{x}}_c$.

Figure 14 shows the corresponding time plot. The time steps $t_1\dots t_5$ are taken at $0.58$ s, $\dots ,4.58$ s with a time distance of $1\mathrm{s}$, which is 25 time steps of 0.04 s each. The robot and human start at

• ${\mathbf{x}}_R={(x_R,y_R)}^T={(2,0)}^T$m
• ${\mathbf{x}}_H={(x_H,y_H)}^T={(4,10)}^T$m
• with the velocities
• ${\dot{x}}_R\left(k\right)=-0.21$ m/s;
• ${\dot{y}}_R\left(1\right)=+0.24$ m/s;
• ${\dot{x}}_H\left(k\right)=-0.26$ m/s;
• ${\dot{y}}_H\left(1\right)=-0.24$ m/s.
• k is the time step.

Figure 14. Time plot, robot and human.

Figure 14. Time plot, robot and human.

The x components of the velocities ${\dot{x}}_R\left(k\right)$ and ${\dot{x}}_H\left(k\right)$ stay constant during the whole simulation.

The y components change their velocities with constant factors

$$\begin{array}{ccc}\hfill {\dot{y}}_R(k+1)& =& K_R·{\dot{y}}_R\left(k\right)\hfill \\ \hfill {\dot{y}}_H(k+1)& =& K_H·{\dot{y}}_H\left(k\right)\hfill \end{array}$$

where $K_R=1.2$ and $K_H=0.9$. The orientation angles start with the following:

• ${\varphi }_R=1.78$ rad;
• ${\varphi }_H=3.69$ rad.

They change their values every second according to the direction of motion.

From both plots, one observes an expected decrease in the output standard deviations for a mutual decrease in their distances to the specific intersection and a good match between the computed and measured values ${\mathbf{x}}_c$ (see

Table 3. Covariances, standard deviations—computed and measured, moving robot–human.

Outputs	Covariance, Computed	Covariance, Measured	${{\mathit{\sigma }}_{\mathit{x}}}_{\mathit{c}}$, Comp/Meas	${{\mathit{\sigma }}_{\mathit{y}}}_{\mathit{c}}$, Comp/Meas
$t_1$	$P=\left(\begin{array}{cc}0.0220& 0.0017\\ 0.0017& 0.0163\end{array}\right)$	$covar=\left(\begin{array}{cc}0.0246& -0.0002\\ -0.0002& 0.0202\end{array}\right)$	$0.148/0.156$	$0.127/0.142$
$t_2$	$P=\left(\begin{array}{cc}0.0198& 0.0023\\ 0.0023& 0.0138\end{array}\right)$	$covar=\left(\begin{array}{cc}0.0222& 0.0018\\ 0.0018& 0.0153\end{array}\right)$	$0.140/0.148$	$0.117/0.123$
$t_3$	$P=\left(\begin{array}{cc}0.0168& 0.0030\\ 0.0030& 0.0107\end{array}\right)$	$covar=\left(\begin{array}{cc}0.0140& 0.0040\\ 0.0040& 0.0088\end{array}\right)$	$0.129/0.118$	$0.103/0.093$
$t_4$	$P=\left(\begin{array}{cc}0.0151& 0.0029\\ 0.0029& 0.0083\end{array}\right)$	$covar=\left(\begin{array}{cc}0.0127& 0.0014\\ 0.0014& 0.0073\end{array}\right)$	$0.122/0.112$	$0.091/0.085$
$t_5$	$P=\left(\begin{array}{cc}0.0125& 0.0023\\ 0.0023& 0.0061\end{array}\right)$	$covar=\left(\begin{array}{cc}0.0102& 0.0030\\ 0.0030& 0.0056\end{array}\right)$	$0.112/0.101$	$0.078/0.074$

). With the information about the distance of the robot and the standard deviation from and at the expected intersection, respectively, it becomes possible to plan either an avoidance strategy or mutual cooperation between the robot and human.

7. Summary and Conclusions

The content of this work is the prediction of encounter situations of mobile robots and human agents in shared areas by analyzing planned/intended trajectories in the presence of uncertainties and system and observation noise. In this context, the problem of intersections of trajectories with respect to system uncertainties and Gaussian noise of the position and orientation of the agents involved is discussed. The problem is addressed by two methods: the statistical linearization of distributions and the sigma-point transformation of the distribution parameters. The positions and orientations of the robot and human are corrupted with Gaussian noise represented by the parameters’ mean and standard deviation. The goal is to calculate the mean and standard deviation/variation at the intersection via the nonlinear relation between the positions/orientations of the robot and human, on the one hand, and the position of the intersection of their intended trajectories, on the other hand.

This analysis is realized by the statistical linearization of the nonlinear relation between the statistics of the robot and human (input) and the statistics of the intersection (output). The output results are the mean and standard deviation of the intersection as functions of the input parameters’ mean and standard deviation of the positions and orientations of robot and human. This work is first carried out for two-input–two-output relations (two orientations of the robot–human and two intersection coordinates) and then for six inputs–two outputs (two orientations and four position coordinates of the robot–human and two intersection coordinates). These cases were extended to their fuzzy versions by different Takagi–Sugeno (TS) fuzzy approximations and compared with the non-fuzzy case. Up to a certain resolution, the approximation works as accurately as the original non-fuzzy version. For the two-input–two-output case, an inverse solution is derived, except for the six-input–two-output case because of the undetermined nature of the differential input–output relation.

The sigma-point transformation aims at transforming/propagating distribution parameters—the sigma points—directly through nonlinearities. The transformed sigma points are then converted into the distribution parameters’ mean and covariance matrix. The sigma-point transformation is closely connected to the unscented Kalman filter, which is used in the example of the robot and human in motion. The specialty of the example is a computed virtual system output (“observation”)—the intersection of two intended trajectories—where the corresponding output uncertainty is a sum of the transformed position/orientation noise and the computational uncertainty from the fuzzy approximation. In total, the comparison between the computed and measured covariances shows a very good match and the comparison with the statistical linearization shows good coincidences as well. Both the sigma-point transformation and the differential statistical linearization scales for more than two variables linearly. Their computational complexity is in the same order [13]. However, if the model is nonlinear, then the differential linearization (EKF) serves as the first-order or second-order approximating estimator. If the system is highly nonlinear, the EKF may diverge and the sigma-point approach produces typically better results. In summary, a prediction of the accuracy of human–robot trajectories using the methods presented in this work increases the performance of human–robot collaboration and human safety. In future work, this method can be used for robot–human scenarios in factory workshops and for robots working in complicated environments like rescue operations in cooperation with human operators.