STLC: Controllability that handles obstacles

The controllability concept discussed so far has no concern for how far the trajectory travels in $X$ before ${x_{G}}$ is reached. This issue becomes particularly important for underactuated systems and planning among obstacles. These concerns motivate a natural question: Is there a form of controllability that is naturally suited for obstacles? It should declare that if a state is reachable from another in the absence of differential constraints, then it is also reachable with the given system ${\dot x}= f(x,u)$. This can be expressed using time-limited reachable sets. Let $R(x,{\cal U},t)$ denote the set of all states reachable in time less than or equal to $t$, starting from $x$. A system ${\dot x}= f(x,u)$ is called small-time locally controllable (STLC) from ${x_{I}}$ if there exists some $t > 0$ such that ${x_{I}}\in \operatorname{int}(R({x_{I}},{\cal U},t'))$ for all $t' \in (0,t]$ (here, $\operatorname{int}$ denotes the interior of a set, as defined in Section 4.1.1). If the system ${\dot x}= f(x,u)$ is STLC from every ${x_{I}}\in X$, then the whole system is said to be STLC.

Figure 15.2: If the system is STLC, then motions can be made in any direction, in an arbitrarily small amount of time.

Consider using this definition to answer the question above. Since $\operatorname{int}(R({x_{I}},{\cal U},t'))$ is an open set, there must exist some small $\epsilon > 0$ for which the open ball $B({x_{I}},\epsilon)$ is a strict subset of $\operatorname{int}(R({x_{I}},{\cal U},t'))$. See Figure 15.2. Any point on the boundary of $B({x_{I}},\epsilon)$ can be reached, which means that a step of size $\epsilon$ can be taken in any direction, even though differential constraints exist. With obstacles, however, we have to be careful that the trajectory from ${x_{I}}$ to the surface of $B({x_{I}},\epsilon)$ does not wander too far away.

Suppose that there is an obstacle region ${X_{obs}}$, and a violation-free state trajectory ${\tilde{x}}$ is given that terminates in ${x_{G}}$ at time $t_F$ and does not necessarily satisfy a given system. If the system is STLC, then it is always possible to find another trajectory, based on ${\tilde{x}}$, that satisfies the differential constraints. Apply the plan-and-transform method of Section 14.6.2. Suppose that intervals for potential replacement are chosen using binary recursive subdivision. Also suppose that an LPM exists that computes that shortest trajectory between any pair of states; this trajectory ignores obstacles but respects the differential constraints. Initially, $[0,t_F]$ is replaced by a trajectory from the LPM, and if it is not violation-free, then $[0,t_F]$ is subdivided into $[0,t_F/2]$ and $[t_F/2,t_F]$, and replacement is attempted on the smaller intervals. This idea can be applied recursively until eventually the segments are small enough that they must be violation-free.

This final claim is implied by the STLC property. No matter how small the intervals become, there must exist a replacement trajectory. If an interval is large, then there may be sufficient time to wander far from the original trajectory. However, as the time interval decreases, there is not enough time to deviate far from the original trajectory. (This discussion assumes mild conditions on $f$, such as being Lipschitz.) Suppose that the trajectory is protected by a collision-free tube of radius $\epsilon$. Thus, all points along the trajectory are at least $\epsilon$ from the boundary of ${X_{free}}$. The time intervals can be chosen small enough to ensure that the trajectory deviations are less than $\epsilon$ from the original trajectory. Therefore, STLC is a very important property for a system to possess for planning in the presence of obstacles. Section 15.4 covers some mathematical tools for determining whether a nonlinear system is STLC.

A concept closely related to controllability is accessibility, which is only concerned with the dimension of the reachable set. Let $n$ be the dimension of $X$. If there exists some $t > 0$ for which the dimension of $R({x_{I}},{\cal U},t)$ is $n$, then the system is called accessible from ${x_{I}}$. Alternatively, this may be expressed as requiring that $\operatorname{int}(R({x_{I}},{\cal U},t)) \not = \emptyset$.

Example 15..3 (Accessibility)   Recall the system from Section 13.1.3 in which the state is trapped on a circle. In this case $X = {\mathbb{R}}^2$, and the state transition equation was specified by ${\dot x}= y u$ and ${\dot y}= -x u$. This system is not accessible because the reachable sets have dimension one. $\blacksquare$

A small-time version of accessibility can also be defined by requiring that there exists some $t$ such that $\operatorname{int}(R({x_{I}},{\cal U},t')) \not = \emptyset$ for all $t' \in (0,t]$. Accessibility is particularly important for systems with drift.