Agile Robot Navigation through Hallucinated Learning and Sober Deployment

In terms of classical motion planning, mobile robot navigation is the problem of moving a robot from one point to another within an obstacle-occupied space in a collision-free manner. Such planning usually happens in the robot Configuration Space (C-Space) [12] and produces (asymptotically) optimal motion plans based on a predefined metric, such as maximum clearance, shortest path, or a combination thereof [13]. These metrics are optimized in terms of C-space representation (e.g. cellular decomposition [13]), global planning (e.g. Dijkstra’s), and local planning. For example, the optimization-based Elastic Bands (E-Band) planner [8] uses repulsive forces generated from obstacles to deform and optimize an initial trajectory, primarily to achieve maximum clearance. The Dynamic Window Approach (DWA) [9] samples feasible actions and scores them based on a weighted score of distance to obstacles, closeness to a global path, and progress toward the goal. The new hallucination strategy proposed in this paper assumes the planner learned from hallucinated training data primarily seeks the shortest path. We leave hallucination strategies that optimize for maximum clearance for future work.

Despite decades of effort to develop classical autonomous navigation systems [8, 9], machine learning has recently shown promise for creating competitive end-to-end planners with obstacle avoidance [1], enabling terrain-based navigation [4, 14], allowing robots to move around humans [2], and tuning parameters for classical navigation systems [5, 15, 16]. All these learning methods require either extensive or high-quality training data, such as that derived from trial-and-error exploration or from human demonstrations [17].

To address the difficulty in obtaining extensive or high-quality training data, self-supervised LfH [6] collects training data in an obstacle-free environment with complete safety, and then learns a local navigation policy through synthetically projecting the most constrained C-space ($C_{obst}^{*}$) onto the robot perception. The most constrained C-space corresponds to the obstacle configuration with as many obstacles as possible such that the executed motion is the only feasible and therefore optimal motion plan: if any more obstacles were to be added, then the motion plan would no longer be feasible. However, LfH also requires hallucination of $C_{obst}^{*}$ during deployment. This hallucination “on-the-fly” requirement assumes prior knowledge such as a relatively high-resolution global path is available. Furthermore, this previous work [6] only works well when the linear velocity in the training set is mostly constant, due to the fact that the robot always hallucinates navigating in the most constrained scenarios and the trained local planner is therefore relatively conservative. Varying speeds in the same most constrained space will lead to ambiguity, as shown in our experiments (Section IV). In this work, we eliminate the necessity of hallucination “on-the-fly”, and therefore the requirement of an available high-resolution global path. During sober deployment, the local planner only needs a single local goal point, along with the real, rather than hallucinated, perception. In addition, the novel hallucination technique also teaches the robot to vary its speed in response to the real environment, instead of the hallucinated most constrained spaces.

We first present a simplified example, which assumes a point mass robot going through three non-colinear configurations. In this case, the robot’s workspace is exactly the C-space. We use the same notation used by Xiao et al. to formalize LfH [6]: given a robot’s C-space partitioned by unreachable (obstacle) and reachable (free) configurations, $C=C_{obst}\cup C_{free}$, the classical motion planning problem is to find a function $f(\cdot)$ that can be used to produce optimal plans $p=f(C_{obst}\leavevmode\nobreak\ |\leavevmode\nobreak\ c_{c},c_{g})$ that result in the robot moving from the robot’s current configuration $c_{c}$ to a specified goal configuration $c_{g}$ without intersecting (the interior of) $C_{obst}$. Here, a plan $p\in\mathcal{P}$ is a sequence of low-level actions $\{u_{i}\}_{i=1}^{t}$ ($u_{i}\in\mathcal{U}$, $\mathcal{P}$ and $\mathcal{U}$ are the robot’s plan and action space, respectively). Considering the “dual” problem of finding $f(\cdot)$, LfH [6] includes a method to find the (unique) most constrained unreachable set corresponding to $p$, $C_{obst}^{*}$. In this work, however, we introduce the definition of a (not unique) minimal unreachable set, $C_{obst}^{min}$, corresponding to $p$:

In other words, every member, $C_{obst}^{min}\in\mathcal{C}_{obst}^{min}$, is a minimal set of obstacles that lead to $p$ being an optimal plan.¹¹1$C_{obst}^{min}$ is minimal in the sense that no subset of it also leads to the same optimal plan, i.e. nothing can be removed such that $p$ remains optimal. Any path that arises from an optimal plan can be approximated by connected line segments. The simplest case of a robot path following an optimal plan $p$ to move from $c_{c}$ to $c_{g}$ (except a straight line) is composed of configurations on two straight line segments, $c_{c}-c_{m}$ and $c_{m}-c_{g}$. The intermediate turning point is defined as $c_{m}$. Since $c_{m}$ is one configuration on the robot’s path, we say $p$ goes through $c_{m}$ (Fig. 1). In the following, given a point-mass robot moving from $c_{c}$ to $c_{g}$ according to some optimal plan $p$ computed by $f(\cdot)$, we show two necessary conditions ($\Longrightarrow$) for an unreachable set ($C_{obst}$) to be a minimal unreachable set ($C_{obst}^{min}$), to aid in identifying the representative unreachable set ($C_{obst}^{\overline{min}}$) used in Sec. III-B.

We name the union of all robot configurations in the triangle defined by $c_{c}$, $c_{m}$, and $c_{g}$ as $G_{1}$. On the other side of line segment $c_{c}-c_{g}$, we name the union of all configurations in the half ellipse, whose focal points locate at $c_{c}$ and $c_{g}$, and whose major axis length is $|c_{c}c_{m}|+|c_{m}c_{g}|$, as $G_{2}$. We define $G=G_{1}\cup G_{2}$ (the grey area in Fig. 1).

Based on the two necessary conditions of $C_{obst}\in\mathcal{C}_{obst}^{min}$ (Proposition 2 and III.3), one class of minimal unreachable sets could be simply constructed by connecting $c_{m}$ with some point $c_{e}$ on the left boundary of the ellipse with a straight line $c_{m}-c_{e}$ ($C_{obst}^{min_{1}}$ in Fig. 1 a), or two line segments $c_{m}-c_{g}(c_{c})$ and $c_{g}(c_{c})-c_{e}$ ($C_{obst}^{min_{2}}$ in Fig. 1 b), if not all configurations on $c_{m}-c_{e}$ are in $G$. In particular, for efficiency, we simply represent all minimal unreachable sets with a special case, $C_{obst}^{\overline{min}}$ (Fig. 1 d), which is all configurations along the straight line $c_{m}$ and $c_{m}^{\prime}$ (the reflective symmetry point of $c_{m}$ with respect to $c_{c}-c_{g}$). Empirical evidence of this approximation’s sufficiency for the purpose of learning will be provided in Sec. IV. Here, we further provide one more observation to help develop intuition regarding how to identify a $C_{obst}^{min}$.

Based on the propositions discussed in Sec. III-A, we present the hallucination technique for a realistic robot. In realistic scenarios, we approximate all $C_{obst}^{min}$ in $\mathcal{C}_{obst}^{min}$ with $C_{obst}^{\overline{min}}$. We hypothesize that all $C_{obst}^{min}$ are sufficiently similar that using just one leads to learning that is just as good as if we used them all, especially when (1) $c_{c}$, $c_{m}$, and $c_{g}$ extracted from realistic trajectories are close to each other, and (2) $C_{obst}$ is instantiated in terms of discrete LiDAR beams. Empirical evidence of this sufficiency will be provided in Sec. IV. However, a realistic robot cannot be modeled as a simple point mass because: (1) the size is not negligible and we need a path for the center of the robot that causes no part of the robot to collide with an obstacle; (2) nonholonomic robots cannot change motion direction instantly, so we need to generalize to a continuously turning path from the piece-wise point mass example. In Sec. III-B, we aim to adapt the point mass example (Sec. III-A) to real-world robots, and therefore need to address these differences.

To address (1), we define one point to the left and another to the right of the centroid of the robot, offset by the robot width, as footprint points, as shown in Fig. 2. The instantaneous linear velocity along the line between these two footprint points is zero for nonholonomic vehicles. The polygon defined by a sequence of footprint points must belong to free space. The actual path executed by the optimal plan $p$ follows the middle of this area. To address (2), we define $c_{\omega}$ as configurations between the current and goal configurations with a non-zero angular velocity ($\omega\neq 0$). Given the current configuration, each $c_{\omega}$, and each $c_{\omega}$’s next configuration, their left or right footprint points are treated as $c_{c}$, $c_{m}$, and $c_{g}$ in the point robot case: based on the sign of $\omega$, the robot turns left or right, and the left or right footprint points are chosen. For each triple of point robot $c_{c}$, $c_{m}$, and $c_{g}$, for efficiency, we approximate all different $C_{obst}^{min}$ with $C_{obst}^{\overline{min}}$ (Fig. 1 d). For a realistic optimal plan $p$ with actual $c_{c}$ (current) and $c_{g}$ (goal) and multiple point robot $c_{c}-c_{m}-c_{g}$ triples in between, we define the union of all $C_{obst}^{\overline{min}}$ as $\mathcal{C}_{obst}^{\overline{min}}$.

In particular, we define an “opposite” function of $f(\cdot)$: $\mathcal{C}_{obst}^{\overline{min}}=o(p\leavevmode\nobreak\ |\leavevmode\nobreak\ c_{c},c_{g})$ as the minimal hallucination function.³³3Note the inverse function $f^{-1}(\cdot)$ does not exist. Technically, $c_{g}$ can be uniquely determined by $p$ and $c_{c}$, but we include it as an input to $o(\cdot)$ for notational symmetry with $f(\cdot)$. This function finds $\mathcal{C}_{obst}^{\overline{min}}$ based on the fact that $p$ is an optimal plan. As visualized in Fig. 2, $C_{obst}$ is instantiated in terms of discrete LiDAR range readings. For each LiDAR beam, we define a minimum and a maximum range, which limit the possible range readings of this particular beam based on the optimal plan $p$. The minimum range of all beams is determined by the left and right boundary, as configurations within the boundary must be in $C_{free}$. We project all $C_{obst}^{\overline{min}}$ onto the corresponding LiDAR beams and for the beams directly intersect any $C_{obst}^{\overline{min}}$, the maximum range is set as the distance from the robot to the intersection point. For other beams, the maximum range is simply the LiDAR’s physical limit, or manually pre-processed to a certain threshold.

Given the representative minimal unreachable set, we want to find all possible unreachable sets, or their sensor readings, that could lead to $p$ being the optimal plan. Based on the LiDAR scan with a minimum and maximum range for each beam (end of Sec. III-B), a sampling strategy is devised to create many obstacle sets, $C_{obst}$, in which $p$ is optimal.

Our sampling strategy aims at creating different range readings that (1) resemble real-world obstacles, and (2) respect uncertainty/safety. For (1), most real-world obstacles have a certain footprint, and their surface contribute to continuity among neighboring beams. Starting from the first beam, a random range is sampled between min and max with a uniform distribution. Moving on to the neighboring beam, with a probability $\alpha$, we increase, or decrease, the previous range by a small random amount, and assign the value to the current beam. This practice is to simulate the continuity in neighboring beams. We make sure this value is within the min and max range of that beam. With probability $1-2\alpha$, we start from scratch and randomly sample between this beam’s min and max values. This practice is to simulate the scenarios where the next beam misses the current obstacle, and reaches another one or does not reach any obstacle at all. For (2), we add an offset value as a function of the optimal plan $p$ to the ranges. For example, given a faster speed of $p$, a larger positive offset is added to the range reading (obstacles are farther away), because faster motion is correlated with more uncertainty or less safety (details can be found in Sec. IV-A). One example scan sample is shown in Fig. 2 as blue $\times$’s.

The entire HLSD pipeline is described in Alg. 1. The inputs to the algorithm are a random exploration policy $\pi_{rand}$ in open space; the minimal hallucination function $o(\cdot)$; a sampling count of hallucinated $C_{obst}$ to be generated per data point; the offset$(\cdot)$ function; the probability $\alpha$; and a parameterized planner $f_{\theta}(\cdot)$. For every data point $(p,c_{c},c_{g})$ in $\mathcal{D}_{raw}$ collected using $\pi_{rand}$ in open space (line 2), we hallucinate $\mathcal{C}_{obst}^{\overline{min}}$ (line 5) and generate the min and max values for every LiDAR beam (line 6). Lines 8–15 correspond to the sampling technique to generate random laser scans. We instantiate $C_{obst}$ as LiDAR readings $L$ and add it to $\mathcal{D}_{train}$ (line 16). This process is repeated sampling count times for every data in $\mathcal{D}_{raw}$. Finally, we train $f_{\theta}(\cdot)$ with supervised learning (line 19). This hallucinated learning enables sober deployment with perception of the real configuration $C_{obst}^{real}$ without runtime hallucination (Lines 21–22).

In order to instantiate Alg. 1, with $o(\cdot)$ described in detail in Sec. III-B, one still needs to define $f_{\theta}(\cdot)$, $\pi_{rand}$, sampling count, $\alpha$, and offset$(\cdot)$:

$f_{\theta}(\cdot)$: For $p=\{u_{i}\}_{i=1}^{t}=f_{\theta}(C_{obst}^{real}\leavevmode\nobreak\ |\leavevmode\nobreak\ c_{c},c_{g})$, instead of requiring a high-resolution global path from the global planner (Dijkstra’s) to construct $C_{obst}^{*}$ in LfH [6], we only query a single local goal $c_{g}$ on the global path 1m away from the robot at each time step, and $c_{c}$ is the origin in the robot frame (orientation is ignored for simplicity). The UGV is equipped with a 720-dimensional 2D LiDAR with a 270^∘ field of view, and we clip the maximum range to 1m to reduce the input space ($C_{obst}^{real}$). The planning horizon $t$ of $p$ is set to 1, i.e. only a single action $p=\{u_{1}\}=\{\left(v_{1},\omega_{1}\right)\}$ (linear and angular velocity) is produced. We use the same three-layer neural network, with 256 hidden neurons and ReLU activation for each layer as in LfH [6].

$\pi_{rand}$: $\pi_{rand}$ randomly picks a target $\left(\hat{v},\hat{\omega}\right)$ pair and commands the robot to reach that speed with constant increments/decrements considering acceleration limit. After reaching $\left(\hat{v},\hat{\omega}\right)$, $\pi_{rand}$ keeps that command with some probability (0.9) or otherwise generates a new target command. We limit the output $v\in[0,1.0]$m/s and $\omega\in[-1.57,1.57]$rad/s. During training in a simulated open space, control inputs ($v$ and $\omega$) and robot configurations ($x$, $y$, and $\psi$) are recorded. Unlike the dataset collected by LfH [6], which mostly contains $v=0.4$m/s, our dataset contains a variety of $v$ values. 12585 data points are collected in a 505s real-time simulation, including a variety of motions in an open space.

sampling count, $\alpha$, and offset$(\cdot)$: We set sampling count to 10 and $\alpha$ to 0.48. The offset($\cdot$) function linearly maps current $v$ in the range [0.3, 1.0]m/s to an offset value between [0, 1]m. Considering the fact that a real robot also needs to turn even in open space due to nonholonomic constraints, as opposed to an ideal point mass robot which does not, we also hallucinate $C_{obst}=\emptyset$ for configurations where $v>0.8$m/s. Considering highly constrained spaces, we additionally hallucinate the most constrained $C_{obst}^{*}$ for configurations where $v<0.3$m/s. So $|\mathcal{D}_{train}|=12*|\mathcal{D}_{raw}|$. Training takes less than three minutes on a NVIDIA GeForce GTX 1650 laptop GPU.

After computing $p$ with Alg. 1, we use the same Model Predictive Control model as in LfH [6] to check for collisions. When a collision is detected, the robot enters a two-phase recovery behavior: it first queries the global path immediately in front of the robot, and rotates to align the robot heading with the tangential direction of the global path. If this recovery behavior is still not safe as determined by the collision checking, the robot backs up at $v=-0.2$m/s. Since LfH learns only from the most constrained C-space, it requires runtime hallucination to match with training data, and a Turn in Place module to drive the robot out of out-of-distribution scenarios. Neither of those components are required by HLSD. Because our dataset contains varying $v\in[0,1.0]$m/s, the LfH speed modulation module to adapt mostly constant $v$ to real environments is not necessary either. The robot is able to react to the real obstacle configuration by using $f_{\theta}(\cdot)$ alone because $\mathcal{D}_{train}$ covers a wide range of distributions.

We first use the Benchmark Autonomous Robot Navigation (BARN) dataset with 300 navigation environments and quantified difficulty levels [11] (example environments shown in Fig. 3) to compare (1) DWA, (2) original LfH trained on the mostly constant 0.4m/s dataset with hallucinated $C_{obst}^{*}$ (LfH 0.4), (3) LfH trained on our varying speed 1.0m/s dataset with $C_{obst}^{*}$ (LfH 1.0), (4) HLSD trained on the 0.4m/s dataset with augmentations to $C_{obst}^{\overline{min}}$ (HLSD 0.4), and (5) HLSD trained on the 1.0m/s dataset with $C_{obst}^{\overline{min}}$ (HLSD 1.0). The baselines are chosen for the following reasons: DWA is a widely used local planner and its implementation is available through the ROS move_base navigation stack. We only consider similar self-supervised approaches and exclude methods that require expert supervision [1, 18], because HLSD does not require any expert motion trajectories. Although DWA’s default max linear velocity is 0.5m/s, for a fair comparison, we set DWA’s max linear velocity to the same as HLSD’s (1.0m/s). We find that by also doubling DWA’s default sampling rate for linear and angular velocity (12 and 40), the robot’s performance is roughly the same as when using the default parameters (but at double the speed). Simulated results are shown in Fig. 4.

In each of the 300 navigation environments generated by Cellular Automaton, the robot navigates between a specified start and goal location without a map. We record the traversal time for each trial, with a maximum of 50s. Three trials are conducted for each planner in each environment, resulting in a total of 4500 trials. The difficulty of the 300 environments are ordered from left to right based on the DWA performance (blue line). The performances of other planners are plotted as dots, for which a line is fit using linear regression. LfH 1.0 (red) fails many trials. The reason is that learning from a dataset with varying speed and always hallucinating the most constrained spaces causes ambiguity: the same most constrained C-space can map to different plans, which confuses the learner. LfH 0.4 (magenta) and HLSD 0.4 (cyan), both trained on the mostly constant 0.4m/s dataset without ambiguity, achieve similar performance and are less sensitive to navigational difficulty, especially LfH 0.4, but LfH 0.4 requires runtime hallucination and other components. Note that DWA has max speed of 1.0m/s, while LfH 0.4 and HLSD 0.4 have up to 0.6m/s max speed, modulated from mostly 0.4m/s learned from the dataset. DWA has an advantage in easy environments (left) due to its fast speed, but in difficult ones (right), LfH 0.4 and HLSD 0.4 are more stable. Also having 1.0m/s max speed, HLSD 1.0 considers the varying linear velocity in the 1.0m/s dataset with the offset($\cdot$) function (line 9 in Alg. 1), and achieves the best result, outperforming all alternatives across the entire range of difficulties. The means and standard deviations of all five planners calculated from all trials are shown in Tab. I. LfH 1.0 has the largest average time and variance, because the learner is confused by the varying training labels in the same most constrained spaces. DWA has large time and also high variance due to its sampling nature. Again, HLSD 0.4 performs similarly as LfH 0.4 does. HLSD 1.0 still outperforms all other planners.

We also deploy the same set of local planners in a physical test course, five trials each (Fig. 5 top). The results are shown in Tab. I. The complicated obstacle course causes DWA to execute many recovery behaviors, and the robot takes a long average time with large variance to finish the traversal. LfH 0.4 and HLSD 0.4 are both able to successfully navigate the robot through, with similar traversal times. But note that LfH 0.4 requires a fine-resolution global path for hallucination during deployment and the Turn in Place module, while HLSD does not and only reacts to the real obstacles without any other extra components. In this physical obstacle course, LfH 1.0 fails every trial due to multiple collisions. Our HLSD 1.0 achieves the best performance both in terms of average time and standard deviation. HLSD deployment in a natural cluttered environment is shown in Fig. 5 bottom.