Goal-Reaching Trajectory Design Near Danger with Piecewise Affine Reach-avoid Computation

TL;DR

The near danger case, also known as a narrow gap, where the agent starts near the goal, but must navigate through tight obstacles that block its path is addressed, with a Piecewise Affine Reach-avoid Computation method to tightly approximate the reachable set of the planning model.

Abstract

Autonomous mobile robots must maintain safety, but should not sacrifice performance, leading to the classical reach-avoid problem: find a trajectory that is guaranteed to reach a goal and avoid obstacles. This paper addresses the near danger case, also known as a narrow gap, where the agent starts near the goal, but must navigate through tight obstacles that block its path. The proposed method builds off the common approach of using a simplified planning model to generate plans, which are then tracked using a high-fidelity tracking model and controller. Existing approaches use reachability analysis to overapproximate the error between these models and ensure safety, but doing so introduces numerical approximation error conservativeness that prevents goal-reaching. The present work instead proposes a Piecewise Affine Reach-avoid Computation (PARC) method to tightly approximate the reachable set of the planning model. PARC significantly reduces conservativeness through a careful choice of the planning model and set representation, along with an effective approach to handling time-varying tracking errors. The utility of this method is demonstrated through extensive numerical experiments in which PARC outperforms state-of-the-art reach avoid methods in near-danger goal reaching. Furthermore, in a simulated demonstration, PARC enables the generation of provably-safe extreme vehicle dynamics drift parking maneuvers. A preliminary hardware demo on a TurtleBot3 also validates the method.

Figures (17)

• Figure 1: Our overall goal is to find safe goal-reaching motion plans near danger. In (a), we show the components of our proposed Piecewise Affine Reach-avoid Computation (PARC) method, with the paper section number on the top left, and the corresponding symbol on the bottom. This method enables a new state-of-the-art for safe reach-avoid problems, as evidenced by (b), which shows examples of extreme vehicle maneuvers. These planned drift parking trajectories are sampled from the Backward Reach-Avoid Set (BRAS, green triangle on the top left) using PARC. Our vehicle is shown with a blue timelapse. The two red cars are obstacles. The goal set for the center of mass is the green rectangle. The small size of the BRAS indicates the challenge of finding safe trajectories.
• Figure 2: (Right) Visualization of the reach set computation in Section \ref{['subsec:parc_reach']} for the TurtleBot example (Section \ref{['subsec:turtlebot_example']}), projected onto the planning state space $P$, when $\mathbf{p}_0=[-4,0,\pi/5]^\intercal$, $t_{\mathrm{\textnormal{f}}}=4(\unit{s})$. The goal set is the green polytope extended to all $\theta \in [-\pi,\pi]$. An expert plan, computed by solving a two-point boundary value problem, passes through two different types of modes (blue and magenta line). We compute the corresponding $t_{\mathrm{\textnormal{f}}}$-time BRS $\overline{\Omega}_{0}$ (blue polytope with solid outline). Intermediate BRSs $\overline{\Omega}_{t}$ are the polytopes without outlines, colored by the type of mode. (Left) Illustration of the trajectory parameter space; the grey region represents $K$, the green region represents the projection of $\overline{\Omega}_{0}$ onto $K$. After reach-set computation, the state $\mathbf{x}_{\mathrm{\textnormal{sample}}}=[\mathbf{p}_{\mathrm{\textnormal{sample}}}^\intercal, \mathbf{k}_{\mathrm{\textnormal{sample}}}^\intercal]^\intercal$ is sampled from $\overline{\Omega}_{0}$: black dot (right) denotes the $\mathbf{p}_{\mathrm{\textnormal{sample}}}$ and black star (left) denotes the $\mathbf{k}_{\mathrm{\textnormal{sample}}}$. The resulting trajectory is denoted as the black line which reaches the goal.
• Figure 3: (Left) Visualization of the reach set (green) and the avoid set (red) computation from Section \ref{['subsec:parc_avoid']} for the TurtleBot example (Section \ref{['subsec:turtlebot_example']}), sliced in the first three dimensions with respect to the initial condition $\mathbf{p}_0=[-3.5,-0.5,\pi/5]^\intercal$. The black star denotes the expert plan with $\mathbf{k} = [-0.318, 0.9]^\intercal$ and the blue star denotes a safe plan with $\mathbf{k} = [-0.320, 1.1]^\intercal$. (Right) Illustration of the resulting trajectory in $p_x$ and $p_y$, with the expert plan shown as a dashed black line and the safe plan shown as a dashed blue line. While the expert plan collides with the obstacle (red), PARC is still able to compute a safe plan with the same mode sequence that safely reaches the goal set (green).
• Figure 4: Visualization of the reach set (green), the avoid set (red), and a grid-based over-approximation of the true avoid set (yellow) for the TurtleBot example (Section \ref{['subsec:turtlebot_example']}) for different values of the timestep $\Delta t$, with the same problem setup and slicing as Fig. \ref{['fig:avoid_set_turtlebot']}. The grid-based over-approximation was obtained by a slower method detailed in Appendix \ref{['subsec:grid_avoid_set']} with forward reachable set (FRS) verification on the reach set, divided into a $50\times50$ grid. Both the reach set and the numerical overapproximation error gets smaller as $\Delta t$ decreases.
• Figure 5: Results of PARC on a naïve choice of planning model, without accounting for tracking error. Green indicates the goal region, red indicates the obstacles with the volume of the drone accounted for, and blue shows the reach set set-differenced with the avoid set without tracking error. Safe trajectories were successfully generated from the left of the leftmost obstacle.

Theorems & Definitions (28)

• Lemma 1
• proof
• Remark 3
• Remark 5: Fixed Final Time
• Remark 6: Goal and Obstacles
• Remark 7: Disturbances to the System
• Remark 8
• Proposition 10: Reach Set w/o Tracking Error
• proof
• Lemma 11: PWA-ETI in Workspace