Detection of robustly collision-free trajectories in unpredictable environments in real-time

Abstract

One of the ultimate goals in robotics is to make robots of high degrees of freedom (DOF) work autonomously in real world environments. However, real world environments are unpredictable, i.e., how the objects move are usually not known beforehand. Thus, whether a robot trajectory is collision-free or not has to be checked on-line based on sensing as the robot moves. Moreover, in order to guarantee safe motion, the motion uncertainty of the robot has to be taken into account. This paper introduces a general approach to detect if a high-DOF robot trajectory is continuously collision-free even in the presence of robot motion uncertainty in an unpredictable environment in real time. Our method is based on the novel concept of dynamic envelope, which takes advantage of progressive sensing over time without predicting motions of objects in an environment or assuming specific object motion patterns. The introduced approach can be used by general real-time motion planners to check if a candidate robot trajectory is continuously and robustly collision-free (i.e., in spite of uncertainty in the robot motion).

Appendix: Robustness of approach over exaggerated \(v_{max}\)

If $v_{max}$ is over-estimated as $v'_{max} > v_{max}$. The effect of such over-estimation can be stated in the following theorem.

Theorem

Let $v'_{max}=cv_{max}, c > 1$, and let $\chi = (\mathbf{q}, t)$ be a collision-free CT-point. Let $E(\chi ,\tau )$ and $E'(\chi ,\tau )$ be the dynamic envelopes defined by $v_{max}$ and $v'_{max}$ respectively. If $(\mathbf{q}, t)$ is detected collision-free at $\tau _l$ by $E(\chi ,\tau _l)$, then, $(\mathbf{q}, t)$ will also be detected collision free by $E'(\chi ,\tau '_l)$, such that $\tau _l < \tau '_l$ and $\tau '_l = \tau _l + (\frac{c-1}{c+p})(t-\tau _l) \le t$, where $-1 \le p \le 1$.

Proof

Suppose at time $\tau _0$, we start observing the dynamic envelopes $E(\chi ,\tau _0)$ and $E'(\chi ,\tau _0)$ with respect to $v_{max}$ and $v'_{max}$, where, based on Eq. (1),

$$\begin{aligned} d(t, \tau _0)&= v_{max}(t-\tau _0) \hbox { and} \\ d'(t, \tau _0)&= v'_{max}(t-\tau _0) = cv_{max}(t-\tau _0) \end{aligned}$$

Clearly for any time $\tau _0 \le \tau < t$, $E'(\chi ,\tau )$ is larger than $E(\chi ,\tau )$. Suppose further that at least one obstacle was on or inside $E(\chi ,\tau _0)$, then it was also on or inside $E'(\chi ,\tau _0)$.

Suppose at time $\tau _l$, where $\tau _0 \le \tau _l \le t$, the dynamic envelope $E(\chi ,\tau _l)$ has shrunk enough to just “squeeze out” obstacles and detected that the CT-point $(\mathbf{q}, t)$ is collision-free. Let $d_{min}(\mathbf{q}, \tau _l)$ denote the minimum distance between $R(\mathbf{q})$ and the obstacles. Thus,

$$\begin{aligned} d(t, \tau _l)= v_{max}(t-\tau _l)= d_{min}(\mathbf{q}, \tau _l) - \epsilon \end{aligned}$$

(19)

where $\epsilon > 0$ is infinitesimally small.

Clearly at $\tau _l$, $E'(\chi ,\tau _l)$ still has an obstacle because it is larger than $E(\chi ,\tau _l)$.

However, according to Definition 1 and Eq. (1), $d(t, t) = cv_{max}(t-t)=0$. Since $(\mathbf{q}, t)$ is a collision-free CT-point, it means that $E'(\chi , t)$ at sensing time $t$ is free of obstacle. Since $E'(\chi ,\tau )$ shrinks continuously as $\tau$ progresses towards $t$, there exists a moment $\tau '_l$, $\tau _l < \tau '_l \le t$, when $E'(\chi ,\tau ' _l)$ is free of obstacle and $(\mathbf{q}, t)$ is detected collision-free.

We now see how $\tau '_l$ is related to $\tau$. Based on Eq. (1),

$$\begin{aligned} d'(t, \tau '_l) = cv_{max}(t-\tau '_l)=d_{min}(\mathbf{q}, \tau '_l) - \epsilon . \end{aligned}$$

(20)

Since obstacles never move with speed greater than $v_{max}$, from $\tau _l$ to $\tau '_l$, the change in minimum distance between $R(\mathbf{q})$ and obstacles can be expressed as:

$$\begin{aligned} d_{min}(\mathbf{q},\tau '_l) - d_{min}(\mathbf{q},\tau _l) \!=\! pv_{max}(\tau '_l \!-\! \tau _l), \ -1 \le p \le 1. \nonumber \\ \end{aligned}$$

(21)

From the Eqs. (19),  (20), and (21), we have

$$\begin{aligned} d'(t, \tau '_l) - d(t, \tau ) = pv_{max}(\tau '_l - \tau _l),\ -1 \le p \le 1 \end{aligned}$$

(22)

From (19),  (20), and (22), we can further obtain

$$\begin{aligned} \tau '_l -\tau _l = (\frac{c-1}{c+p})(t-\tau _l) \le t-\tau _l \end{aligned}$$

(23)

$\square$

Based on Theorem 2, $\tau '_l = t$ corresponds to the worst case scenario where $p=-1$, meaning that the closest obstacle to $R(\mathbf{q})$ at $\tau$ originally inside $E(\chi ,\tau _0)$ moves towards $R(\mathbf{q})$ with $v_{max}$ and stay at $R(\mathbf{q})$ from $\tau$ to $t$, and in all other cases, $\tau '_l < t$.

The significance of the theorem is that, if a CT-point $(\mathbf{q}, t)$ is collision-free, then it will be detected as collision-free no later than time $t$ no matter how badly the actual $v_{max}$ is overestimated as $v'_{max}$. This shows the robustness of our approach.