Long-Range Indoor Navigation with PRM-RL

TL;DR

PRM-RL combines reinforcement learning and probabilistic roadmaps to enable long-range, noise-robust indoor robot navigation across complex environments, demonstrated both in simulation and real-world settings.

Contribution

This work introduces PRM-RL, a hierarchical navigation approach that integrates RL-based local control with sampling-based planning for reliable long-range indoor navigation.

Findings

PRM-RL outperforms baseline methods in success rate.

The approach is robust to sensor and control noise.

Robots successfully navigated over 5.8 km in real environments.

Abstract

Long-range indoor navigation requires guiding robots with noisy sensors and controls through cluttered environments along paths that span a variety of buildings. We achieve this with PRM-RL, a hierarchical robot navigation method in which reinforcement learning agents that map noisy sensors to robot controls learn to solve short-range obstacle avoidance tasks, and then sampling-based planners map where these agents can reliably navigate in simulation; these roadmaps and agents are then deployed on robots, guiding them along the shortest path where the agents are likely to succeed. Here we use Probabilistic Roadmaps (PRMs) as the sampling-based planner, and AutoRL as the reinforcement learning method in the indoor navigation context. We evaluate the method in simulation for kinematic differential drive and kinodynamic car-like robots in several environments, and on differential-drive robots at three physical sites. Our results show PRM-RL with AutoRL is more successful than several baselines, is robust to noise, and can guide robots over hundreds of meters in the face of noise and obstacles in both simulation and on robots, including over 5.8 kilometers of physical robot navigation. Video: https://youtu.be/xN-OWX5gKvQ

Tables

Table 1. TABLE I: P2P Reward Components and Their AutoRL-Tuned Values

Reward	Description	Min	Max	Diff	Car
				Drive	Model
$r_{\text{goal}}$	1 when goal reached	0.0	100.0	62.0	0.82
	and 0 otherwise
$r_{\text{goalDist}}$	Negative Euclidean	0.0	1.0	0.38	N/A
	distance to goal
$r_{\text{goalProg}}$	Delta of Euclidean	0.0	5.0	N/A	2.03
	distance to goal
$r_{\text{collision}}$	1 on collision	-100.0	0.0	-57.90	-1.80
	and 0 otherwise
$r_{\text{clearance}}$	Distance to	0.0	1.0	0.67	N/A
	closest obstacle
$r_{\text{step}}$	Constant per-step	-1.0	0.0	-0.43	-0.10
	penalty
$r_{\text{turning}}$	Negative angular	0.0	1.0	0.415	N/A
	speed
$r_{\text{backward}}$	Negative backward	-1.0	0.0	N/A	-0.64
	speed

Table 2. TABLE II: Environments

Environment	Type	Dimensions	Visual
Training	Floor map	$23m$ by $18m$	Fig. 3a
Building 1	Floor map	$183m$ by $66m$	Fig. 3d
Building 2	Floor map	$60m$ by $47m$	Fig. 3c
Building 3	Floor map	$134m$ by $93m$	Fig. 3b
Building Complex	Floor map	$288m$ by $163m$	Fig. 2
Physical Testbed 1	SLAM	$50m$ by $68m$	Fig. 6a
Physical Testbed 2	SLAM	$203m$ by $135m$	N/A (private)
Physical Testbed 3	SLAM	$22m$ by $33m$	Fig. 11c

Table 3. TABLE III: Baselines

Label	Local	Execution	Obstacle Avoid		Monte Carlo	Description
	Planner (LP)	Policy	LP	Execution	rollouts
AutoRL	N/A	AutoRL	N/A	Yes	No	Local AutoRL policy like [12] w/o guidance by a PRM.
PRM-SL	Straight Line	Straight Line	No	No	No	Straight-line PRMs [39] w/ straight-line execution policy.
PRM-GAPF	Straight Line	Guided APF	No	Yes	No	Straight-line PRMs [39] executed by guided APF like [11].
PRM-DWA	Straight Line	Guided DWA	No	Yes	No	Straight-line PRMs [39] executed by guided DWA [24].
PRM-RL	AutoRL	AutoRL	Yes	Yes	Yes	PRM-RL w/ AutoRL for roadmap & execution (ours).
PRM-HTRL	DDPG	DDPG	Yes	Yes	Yes	PRM-RL w/ DDPG for roadmap & execution ([23]).
SF	N/A	SF	N/A	Yes	No	Successor Features (SF) visual local planner [70].

Table 4. TABLE IV: PRM-RL Performance vs Baselines.

Baseline	PRM	Succ.	99%	PRM-RL	Path Dist. (m)		Path / Shortest.		Clearance (m)		Time (s)		Roadmap		Coll Checks
	Density	%	Conf.	Nav$\Delta$%	Mean	Std.	Mean	Std.	Mean	Std.	Plan.	Exec.	Nodes	Edges	(x${10}^6$)
AutoRL	N/A	8.53	1.52	83.20	14.2	15.9	1.98	5.98	0.43	0.57	0.03	42.4	N/A	N/A	N/A
PRM-SL	Sparse	12.40	1.79	79.33	28.6	26.2	1.16	0.63	0.28	0.14	0.07	16.7	1321	28984	1.4
PRM-SL	Dense	9.47	1.59	82.27	20.3	17.3	1.10	0.48	0.28	0.13	0.43	12.8	3302	186491	9.0
PRM-GAPF	Sparse	29.20	2.47	62.53	57.2	42.5	1.35	0.70	0.30	0.13	0.06	44.6	1321	28984	1.4
PRM-GAPF	Dense	28.13	2.45	63.60	47.0	35.9	1.36	0.65	0.30	0.13	0.39	38.1	3302	186491	9.0
PRM-DWA	Sparse	40.53	2.67	51.20	277.6	192.6	7.23	17.09	0.48	0.11	0.06	167.1	1321	28984	1.4
PRM-DWA	Dense	40.13	2.67	51.60	266.2	215.7	6.44	8.20	0.47	0.10	0.40	178.8	3302	186491	9.0
PRM-RL	Sparse	86.53	1.86	5.20	61.5	36.7	1.16	1.90	0.43	0.18	0.14	70.4	1321	55270	74.0
PRM-RL	Dense	91.73	1.50	Best	60.0	36.5	1.10	1.82	0.42	0.14	0.75	62.7	3302	332399	360.0

Symbol	Units or Domain	Meaning
$𝒞$	${ℝ}^{D_c}$	Configuration space of dimension $D_c$
${𝒞}_{\mathrm{free}}$	${ℝ}^{D_c}$	Free portion of configuration space
$S$	${ℝ}^{D_c+D_t}$	State space of robot plus task state
$T$	${ℝ}^2\times {𝕊}^1$	Task space for navigation
$T_{\mathrm{free}}$	${ℝ}^{D_{𝒲}}$	Free portion of the task space
$𝒲$	${ℝ}^{D_{𝒲}}$	Physical workspace of dimension $D_{𝒲}$
$O$	${ℝ}^{D_o}$	Observation space of dimension $D_o$
$A$	${ℝ}^{D_a}$	Action space of dimension $D_a$
$D$	$S\times A$	Task dynamics $\to 𝒞$
$N$	$𝒞\times A$	Noise model $\to O$ or $A$
$R$	$O$	Reward model $(G,r)$
$G$	$𝒞$	True objective $\to ℝ$
$r$	$O$ parametrized with $𝜽$	Dense reward $\to ℝ$
$r_{\text{name}}$	$O$ parametrized with ${𝜽}_{\text{name}}$	Named reward component $\to ℝ$
$\gamma$	$\left[\mathrm{0..1}\right]$	Discount
$𝕀$	$𝔹$	Indicator function $\to \{1,0\}$
$L\left(𝒙\right)$	$𝒞$	Task predicate
$F_s\left(𝒙\right)$	$𝒞$	Sensor w/ dyn. $D_s\left(𝒙\right)$ & noise ${𝒩}_s$
$F_a(𝒙,𝒂)$	$𝒞\times A$	Action w/ dyn. $D_a\left(𝒙\right)$ & noise ${𝒩}_a$
${𝒑}_i$	${𝒞}_{\mathrm{free}}$	Waypoint $i$ on path $𝒫$
${𝒙}_{𝒊}$	$𝒞$	Point $i$ along trajectory $𝒯$
${𝒙}_{𝑺}$	${𝒞}_{\mathrm{free}}$	Start state
${𝒙}_{𝑮}$	${𝒞}_{\mathrm{free}}$	Goal state
$d_G$	meters	Goal success distance
${𝒦}_{\omega }$	$\mathbb{Z}+$	Max trajectory execution steps
$𝒂$	${ℝ}^2\times {𝕊}^1$	Diff drive action $(v,\varphi )$ lin, ang vel.
$\overline{V_i{V}_j}$	$𝒞$	Line from $V_i$ to $V_j$ in graph $(V,E)$
$n_{\omega }$	$\mathbb{Z}+$	Num. edge connection attempts
$n_s$	$\mathbb{Z}\ast$	Num. observed connection successes
$p_s$	$\left[\mathrm{0..1}\right]$	Edge connection success threshold
$d_{\omega }$	meters	Max attempted edge connection dist.
$d_{\pi }$	meters	Policy $\pi$ effective nav. distance
$f_{\pi }$	$\left[\mathrm{0..1}\right]$	Policy $\pi$ effective obst. shrinkage
${\rho }_{\omega }$	$\text{points}/{\text{meters}}^2$	Sampling density per meter
$n$	points	Number of points to sample
$p_n$	$\left[\mathrm{0..1}\right]$	Probability of successful navigation
$n_w$	$\mathbb{Z}+$	Number of waypoints on a path
$V_{𝒲}$	${\text{meters}}^2$	Volume of the workspace
$D_{𝒲}$	$\mathbb{Z}+$	Workspace dimensionality
$n_{{}_{𝒲}}$	$\mathbb{Z}+$	Nodes in the workspace
$m_{\pi }$	$\mathbb{Z}+$	Neighbors of a node
$n_p$	$\mathbb{Z}+$	Number of processors
${\sigma }_x$	$ℝ+$	$𝒩(0,{\sigma }_x)$ noise for $x=l,g,v,a$
${\theta }_n$	$\mathbb{Z}+$	Observation trace length

Equations

$$G_{\text{P2P}}({\mathbf{x}}_{\mathbf{i}},{\mathbf{x}}_{\mathbf{G}})=\mathbb{I}(\Vert {\mathbf{x}}_{\mathbf{i}}-{\mathbf{x}}_{\mathbf{G}}\Vert <d_G),$$

$$R_{{\mathbf{\theta }}_{r_{\text{DD}}}}={\mathbf{\theta }}_{r_{\text{DD}}}^{{}^{⊺}}[r_{\text{goal}}{r}_{\text{goalDist}}{r}_{\text{collision}}{r}_{\text{clearance}}{r}_{\text{step}}{r}_{\text{turning}}{]}^{⊺},$$

$$R_{{\mathbf{\theta }}_{r_{\text{CM}}}}={\mathbf{\theta }}_{r_{\text{CM}}}^{{}^{⊺}}[r_{\text{goal}}{r}_{\text{goalProg}}{r}_{\text{collision}}{r}_{\text{step}}{r}_{\text{backward}}]{}^{⊺},$$

$$\frac{con{n}_{PRM-RL}}{con{n}_{PRM-SL}}=\frac{1-{\rho }_o{f_{\pi }}^2}{1-{\rho }_o}$$

$$P_{survival}={\left(1-\frac{1}{2}{\text{n}}_{\text{walls}}\text{erfc}\left(\frac{d_{\text{safety}}}{\sqrt{2}{\sigma }_{\text{pos}}}\right)\right)}^{\frac{d_{\text{corr}}}{d_{\text{step}}}}$$

$$n_{{}_{\mathcal{W}}}=V_{\mathcal{W}}{\rho }_{\omega }=O(d^{D_{\mathcal{W}}}{\rho }_{\omega }),$$

$$m_{\mathbf{\pi }}=V_{\mathbf{\pi }}{\rho }_{\omega }=O(d_{\mathbf{\pi }}^{D_{\mathcal{W}}}{\rho }_{\omega })$$

$$\frac{m_{\mathbf{\pi }}}{n_{{}_{\mathcal{W}}}}\propto \frac{d_{\mathbf{\pi }}^{D_{\mathcal{W}}}{\rho }_{\omega }}{d^{D_{\mathcal{W}}}{\rho }_{\omega }}=c^{D_{\mathcal{W}}}\le 1.$$

$$O(n_{{}_{\mathcal{W}}}\log n_{{}_{\mathcal{W}}})$$

$$=O(d^{D_{\mathcal{W}}}{\rho }_{\omega }\log d{\rho }_{\omega }),D_{{}_{\mathcal{W}}}\text{is constant.}$$

$$O(n_{{}_{\mathcal{W}}}{m}_{\mathbf{\pi }}{n}_{\omega }{d}_{\mathbf{\pi }})=O(d^{D_{\mathcal{W}}}{\rho }_{\omega }^2{d}_{\mathbf{\pi }}^{D_{\mathcal{W}}+1}{n}_{\omega }),$$

$$O(d^{D_{\mathcal{W}}}{\rho }_{\omega }\log d{\rho }_{\omega }+d^{D_{\mathcal{W}}}{\rho }_{\omega }^2{d}_{\mathbf{\pi }}^{D_{\mathcal{W}}+1}{n}_{\omega })=$$

$$O(d^{D_{\mathcal{W}}}{d}_{\mathbf{\pi }}^{D_{\mathcal{W}}+1}{\rho }_{\omega }^2{n}_{\omega })$$

$$\mathbb{E}[p_s]=\frac{n_s+1}{n_{\omega }+2}$$

Full text

I Introduction

Long-range indoor robot navigation requires human-scale robots (Fig. 1a) to move safely over building-scale distances (Figs. 1b). To robustly navigate long distances in novel environments, we factor the problem into long-range path planning and end-to-end local control, while assuming the robot has mapping and localization. Long-range path planning finds collision-free paths to distant goals not reachable by local control [43]. End-to-end local control produces feasible controls to follow ideal paths while avoiding obstacles (e.g., [40], [24]) and compensating for noisy sensors and localization [12]. We enable end-to-end local control to inform long-range path planning through sampling-based planning.

Sampling-based planners, such as Probabilistic Roadmaps (PRMs) [39] and Rapidly Exploring Random Trees (RRTs) [42, 44], plan efficiently by approximating the topology of the configuration space ($\mathcal{C}$), the set of all possible robot poses, with a graph or tree constructed by sampling points in the collision-free subset of configuration space ($\mathcal{C}_{\mathrm{free}}$), and connecting these points if there is a collision-free local path between them. Typically these local paths are created by line-of-sight tests or an inexpensive local planner, and are then connected in a sequence to form the full collision-free path.

Regardless of how a planner generates a path, executing a path requires handling sensor noise, unmodeled dynamics, and environment changes. Recently, reinforcement learning (RL) agents [41] have solved complex robot control problems [68], generated trajectories under task constraints [22], demonstrated robustness to noise [21], and learned complex skills [57] [55], making them good choices to deal with task constraints. Many simple navigation tasks only require low-dimensional sensors and controls, like lidar and differential drive, and can be solved with easily trainable networks [71, 29, 7]. However, as we increase the problem’s complexity by requiring longer episodes or providing only sparse rewards [20], RL agents become harder to train and do not consistently transfer well to new environments [35] [34].

Long-range navigation presents all these challenges. Sparse rewards and long episodes make long-range agents hard to train. On complex maps, short-range agents are vulnerable to local minima like wide barriers and narrow passages. Even within deployment categories, environments have vast variety: the SunCG dataset had 45,000 houselike environments [64], and the US alone has over 5.6 million office buildings [9].

We present PRM-RL, an approach to long-range navigation which combines PRMs and RL to overcome each other’s shortfalls. In PRM-RL, an RL agent learns a local point-to-point task, incorporating system dynamics and sensor noise independent of long-range environment structure. The agent’s learned behavior then influences roadmap construction; PRM-RL builds a roadmap by connecting two workspace points only if the agent consistently navigates between them in configuration space without collision, thereby learning the long-range environment structure. PRM-RL roadmaps perform better than roadmaps based on pure $\mathcal{C}_{\mathrm{free}}$ connectivity because they respect robot dynamics. RL agents perform better with roadmap guidance, avoiding local minima. PRM-RL thus combines PRM efficiency with RL resilience, creating a long-range navigation planner that not only avoids local-minima traps, but transfers well to new environments, as shown in Fig. 2, where a policy trained on a small training environment scales to a quad-building complex almost two hundred times larger in map area.

This paper contributes PRM-RL as a hierarchical kinodynamic planner for navigation in large environments for robots with noisy sensors. This paper is a journal extension of our conference paper [23], which contributes the original PRM-RL method. Here, we investigate PRM-RL in the navigation context and make the following contributions beyond the original paper:

1. Algorithm 2 for PRM-RL roadmap building;
2. Algorithm 3 for robot deployment;
3. PRM-RL application to kinodynamic planning on a car model with inertia;
4. in-depth analysis of PRM-RL, including: 4.1) correlation between the quality of the local planner and the overall hierarchical planner; 4.2) impact of improving planners and changing parameters on PRM-RL computational time complexity; 4.3) impact of a robust local planner on the effective connectivity of samples in the graph; 4.4) resilience of PRM-RL to noise and dynamic obstacles. All the evaluations and experiments are new and original to this paper. We evaluate PRM-RL with a more effective local planner [12], compare it in simulation against six baselines in eight different buildings, and deploy it to three physical robot testbed environments.

Overall, we show improved performance over both baselines and our prior work, more successful roadmaps, and easier transfer from simulation to robots, including a $37.5\%$ increase in navigation success over [23], while maintaining good performance despite increasing noise. We also show that only adding edges when agents can always navigate them makes roadmaps cheaper to build and improves navigation success; denser roadmaps also have higher simulated success rates, but at substantial roadmap construction cost. Floorplans are not always available or up to date, but we show roadmaps built from SLAM maps close the simulation to reality gap by producing planners which perform almost as well on robot as they do in simulation. SLAM-based PRM-RL enables real robot deployments with up to +$200\text{\,}\mathrm{m}$ collision-free trajectories at three different sites on two different robots with success rates as high as $92.0\%$. We also show that PRM-RL functions well on robots with dynamic constraints, with an $83.4\%$ success rate in simulation.

While this paper focuses on navigation, the analysis and empirical findings will be of interest to the wider motion planning community for two reasons. First, PRM-RL presented here is an example of a hierarchical motion planner that factors models of sensor, localization, and control uncertainties into roadmap construction, resulting in planners that perform as well in simulation as they do on robots. Second, we present a comprehensive analysis of the trade-offs between performance and computational complexity and the interplay between local and global planners that is not specific to navigation.

II Related Work

II-1 Probabilistic roadmaps

PRMs [39] have been used in a wide variety of planning problems from robotics [30, 51] to molecular folding [3, 59, 66]. They have also been integrated with reinforcement learning for state space dimensionality reduction [49, 59] by using PRM nodes as the state space for the reinforcement learning agent. In contrast, our work applies reinforcement learning to the full state space as a local planner for PRMs. In prior work for an aerial cargo delivery task, we trained RL agents to track paths generated from PRMs constructed using a straight-line local planner [22]. Researchers have modified PRMs to handle moving obstacles [32, 61], noisy sensors [50], and localization errors [2, 4]. Safety PRM [50] uses probabilistic collision checking with a straight-line planner, associating a measure of potential collision with all nodes and edges. All those methods address one source of errors at a time. In contrast, PRM-RL uses an RL-based local planner capable of avoiding obstacles and handling noisy sensors and dynamics, and at the node connection phase the RL local planner does Monte Carlo path rollouts with deterministic collision checking. We only add edges if the path can be consistently navigated within a tunable success threshold. Additionally, PRMs built with straight-line geometric planners require a tracking method (often model-based, such as [67]) to compensate for sensor and dynamics uncertainties not known at the planning time. PRM-RL eliminates the need for path tracking and mathematical models for sensor and dynamics uncertainties, because sensor-to-control RL local planners are model-free, and the Monte Carlo rollouts ensure that the feasibility is accounted when connecting the nodes.

PRMs are easy to parallelize, either through parallel edge connections [5], sampling [8], or building sub-regional roadmaps [17] in parallel. To speed up building large scale roadmaps, we use an approach similar to [5] across different computers in a cluster. Individual Monte Carlo rollouts to connect edges can be parallelized across multiple processors or run sequentially to allow for early termination.

II-2 Reinforcement learning in motion planning

Reinforcement learning has recently gained popularity in solving motion planning problems for systems with unknown dynamics [41], and has enabled robots to learn tasks that have been previously difficult or impossible [1, 10, 45]. For example, Deep Deterministic Policy Gradient (DDPG) [46] works with high dimensional continuous state/action spaces and can learn to control robots using unprocessed sensor observations [45].

Researchers have successfully applied deep RL to navigation for robots, including visual navigation with simplified navigation controllers [7, 16, 29, 56, 62, 72], more realistic controllers in game-like environments [6, 15, 54], and extracting navigation features from realistic environments [10, 26]. In local planner settings similar to ours, differential drive robots with 2D lidar sensing, several approaches emerged recently using asynchronous DDPG [65], expert demonstrations [60], DDPG [47], curriculum learning [70], and AutoRL [12]. While any sensor-to-controls obstacle-avoidance agent could be used as both a local planner in PRM-RL and a controller for reactive obstacle avoidance, we choose AutoRL for its simplicity of training, as it automates the search for reward and network architecture.

Recent works [48, 70] learn planning in 2D navigation mazes and measure transferability of learning to new environments. Using the terminology of this paper, this work falls between long-range planning and local control. Their navigation environments vary between task instances, as in both our AutoRL and PRM-RL setting. The mazes appear to be similar in size and complexity to the maps used in our local planning [12, 23]. However, those methods, based on Q-learning, use discretized actions to produce their paths, leading to three consequences. First, the robot’s action discretization inherently limits how close to the optimal path the solution can reach. Second, the method must have a lower-level controller or steering function that tracks the resulting path. Third, while these planners avoid obstacles, they do not take robot dynamics or path feasibility into account. In our work, we rely on continuous action RL algorithms, which can better approximate optimal paths than discrete actions, eliminate the tracking controller, and learn dynamically feasible steering functions that produce linear and angular velocities [13].

II-3 Hierarchical planners with reinforcement learning

Several recent works feature hierarchical planners combined with reinforcement learning, either over a grid [19] or manually selected waypoints [37]. These works connect roadmap points with a straight-line planner and use the reinforcement learning agent as an execution policy at run-time. We use the obstacle-avoidance RL policy as both an execution policy and a local planner for connecting the edges in the roadmap. This approach results in roadmap connectivity that is tuned to the abilities of the particular robot.

III Problem Statement

This section defines key terms for path planning, for trajectory planning, for the navigation problem for robots with noisy depth sensors and actuators, and for the differential drive and car-like robots which are our primary focus in this paper.

The configuration space, $\mathcal{C},$ is the set of possible robot poses. The configuration spaces for differential drive and car-like robots are $\mathcal{C}_{dd}=\mathbb{R}^{2}\times S^{1}$ and $\mathcal{C}_{car}=\mathbb{R}^{2}\times S^{1}\times S^{1},$ respectively. The workspace, $\mathcal{W}$, is the physical space that a robot operates in with dimensionality $D_{\mathcal{W}}$ of 2 or 3. The workspace and the robot’s kinematics divide the configuration space into valid ($\mathcal{C}_{\mathrm{free}}$) and invalid partitions. $\mathcal{C}_{\mathrm{free}}$ is a set of all robot poses that are free of self-collision, collisions in the workspace, and satisfy relevant kinodynamic constraints. To that end, we consider the workspace to be a closed segment on a 2-dimensional manifold, and model the robots’ kinematics with a unicycle or Type (2,0) model [63] for differential drive robots, and single-track model [58] for car-like robots.

The robot state space is the full internal state of the robot including pose, velocity, observations, etc. We assume this state to be hidden and non-observable. The observable state space, $O\subset\mathbb{R}^{N_{s}\theta_{n}}\times\mathcal{C}_{\mathrm{free}}^{2},$ is the same for both robot types and consists of sensor observations ($N_{s}$ lidar rays observed over the last $\theta_{n}$ discrete time steps) as well as the current and goal robot poses, assumed to be in $\mathcal{C}_{\mathrm{free}}.$ The robot action space, $A\subset\mathbb{R}^{2}$, consists of linear and angular velocities $\bm{a}=(v,\phi)\in A$ for both types of robots. We assume sensor observation and actuators to be noisy. The sensor observations are produced by a sensor process $F_{s}:\mathcal{C}_{\mathrm{free}}\rightarrow O$ that can be modeled as a combination of inherent sensor dynamics and a source of noise: $F_{s}(\bm{x})\sim D_{s}(\bm{x})+\mathcal{N}_{s}$. Similarly, actions in the robot’s action space $A$ have a state-dependent effect $F_{a}:\mathcal{C}_{\mathrm{free}}\times A\rightarrow\mathcal{C}$, which also can be modeled as a combination of inherent robot dynamics and a source of noise: $F_{a}(\bm{x},\bm{a})\sim D_{a}(\bm{x},\bm{a})+\mathcal{N}_{a}$.

A path, $\mathcal{P},$ is a sequence of workspace points $\bm{p_{i}}\in\mathcal{W},i\in[0,N]$ from the beginning $p_{0}$ to the end $p_{N}$ of the trajectory. A valid path consists of only valid waypoints: $\forall\bm{p_{i}}\in\mathcal{P}:\bm{p_{i}}\in\mathcal{W}\cap\mathcal{C}_{\mathrm{free}},i=1,\cdots,N_{p}$. We consider a trajectory, $\mathcal{T}$, to be a sequence of robot valid poses $\bm{x}_{j}\in\mathcal{C}_{\mathrm{free}},\,j=1,\cdots,N_{t}$ such that $\bm{x}_{j+1}$ is reachable from $\bm{x}_{j}$ within $\mathcal{C}_{\mathrm{free}}$ under the robot’s kinematic model within a fixed discrete time step $\Delta T,$ for any two consecutive points $\bm{x}_{j+1},\,\bm{x}_{j},\,j=1,\cdots,N_{t}-1$. We assume that the robot is at rest at the beginning and end of the trajectory, i.e., $\dot{\bm{x}}_{1}=\bm{0},\,\dot{\bm{x}}_{N}=\bm{0}.$

In this paper, a point-to-point (P2P) policy $\bm{\pi}:O\rightarrow A,$ maps robot observations to linear and angular velocities in order to generate trajectories. Given a valid start configuration $\bm{x}_{S}$ and a policy $\bm{\pi}$, an executed trajectory $\mathcal{T}$ is a sequence of configuration states that result from drawing actions from the policy and its noise processes: $\mathcal{T}:\bm{x_{0}}=\bm{x_{S}}\land\bm{x_{i}}\sim F_{a}(\bm{x_{i-1}},\bm{\pi}(F_{s}(\bm{x_{i-1}})))$. An executed trajectory is a failure if it produces a point that exits $\mathcal{C}_{\mathrm{free}}$, or if it exceeds a task-specific time-step limit $\mathcal{K}_{\omega}$ without reaching a goal. Given a valid observable goal pose, $\bm{x_{G}},$ a non-failed trajectory is a success if it reaches a point $\bm{x_{i}}$ sufficiently close to the goal with respect to a task-dependent threshold $\|\bm{x_{i}}-\bm{x_{G}}\|\leq d_{G}$, at which point the task has completed.

Graph search over PRMs creates a path, $\mathcal{P}$. Waypoints in the path serve as intermediate goals for the trajectory generating policy $\bm{\pi}.$ A path following policy with respect to a path $\mathcal{P}$ and P2P policy $\bm{\pi},$ $\bm{\pi}_{\textit{pf}}(\bm{x}|{\mathcal{P}},\bm{\pi}),$ produces a trajectory that traverses the waypoints in path $\mathcal{P}$ using the P2P policy $\bm{\pi}.$ A valid executable path with respect to a P2P policy is a path which the P2P policy can reliably execute to achieve task completion—guiding the agent from the start state $\bm{x_{S}}$ of $\mathcal{P}$ to within $d_{G}$ of the goal state $\bm{x_{G}}$ within $\mathcal{K}_{\omega}$ time steps. Because noise makes execution stochastic, we define a path to be reliable if the policy’s probability of task completion using the path exceeds a task-dependent success threshold $p_{s}$.

The key problem that PRM-RL addresses is how to construct a reliable path and path following policy for a given P2P policy in the context of indoor navigation. To that end, we define an agent that performs its task without knowledge of the workspace topology as one in which the transfer function $\dot{\bm{x}}=f(\bm{x},\bm{a})$ that leads the system to task completion is only conditioned on what the robot can observe and what it has been commanded to do. Formally, we learn policies to control an agent that we model as a partially observable Markov decision process (POMDP) represented as a tuple $(O,A,D,N,R,\gamma)$ of observations, actions, dynamics, noise, reward, and discount. The characteristics of the robot determine observations, actions, dynamics, and noise; these are continuous and multi-dimensional. Reward and discount are determined by the requirements of the task: $\gamma\in(0,1)$ is a scalar discount factor, whereas the reward $R$ has a more complicated structure $(G,r)$, including a true scalar objective $G$ representing the task and a weighted dense scalar reward $r:O\times\mathbb{R}^{N_{\theta}}\rightarrow\mathbb{R},$ based on observable features $O$ and a reward parameterization $\theta\in\mathbb{R}^{N_{\theta}}$. We assume a presence of a simplified black-box simulator without knowing the full non-linear system dynamics. The dynamics $D$ and noise $N$ are implicit in the real world but are encoded separately in simulation in the system dynamics and an added noise model.

IV Methods

The PRM-RL method has three stages: training an environment-independent local planner policy with RL, creating a roadmap specific to that local planner and an environment, and deploying that roadmap and policy to the environment for querying, trajectory generation, and execution. Fig. 4 illustrates the method.

First, in the training stage (Fig. 4a), to enable a robot to perform a specific task, we train an agent with RL. For indoor navigation, that task is short-range point-to-point navigation end-to-end from sensors to actions. This task is independent of the environment in which the robot will eventually operate. The RL agent learns to perform a task on an environment comparable to the deployment environment, but smaller in size to make simulation faster and training more tractable. This is a Monte Carlo simulation process: we train multiple policies and select the fittest one for the next stage of PRM-RL, regardless of the learning algorithm used.

Next, in the creation phase (Fig. 4b upper), to prepare a robot to work in a specific environment, we use this best policy as a local planner to build a PRM with respect to the target site. Obstacle maps, such as floor plans or SLAM maps, can be used for any robot we wish to deploy as long as the policy transfers well to the real world. This is a one-time setup per robot and environment. Unlike PRM-SL, in which points are sampled from $\mathcal{C}_{\mathrm{free}}$, PRM-RL samples points from the workspace $\mathcal{W}$ but then rolls out trajectories in $\mathcal{C}$, adding an edge to the roadmap only when the agent can navigate it in simulation with greater probability than $p_{s}$ over $n_{\omega}$ trials. Rather than being determined by the geometry of free space, the resulting roadmap is tuned to capabilities of the particular robot, so different robots over the same floor plan may generate different roadmaps with different connectivity.

Finally, in the deployment phase, (Fig. 4b lower), to perform the task in the environment, the constructed roadmap is queried to generate trajectories, which are executed by the same RL agent used to generate the roadmap. In the geometric PRM-SL framework, querying a roadmap involves connecting the start and goal to the roadmap graph and finding the shortest path between them. In the simulation-based PRM-RL framework we can optionally record additional data about executed trajectories to enable other trajectory metrics (such as minimal energy consumption, shortest time, and so on) that are not generally available with geometry-only approaches. At execution time, the RL agent navigates to the first waypoint. Once the agent is within $d_{G}$ distance from the waypoint, the next waypoint becomes its new goal; the process repeats until the agent has traversed the whole trajectory.

IV-A RL Agent Training

PRM-RL’s global planner is strongly decoupled from the details of the local planner’s construction and training. We explore this with two different agent models: differential drive and carlike robots.

IV-A1 P2P for differential drive robots

The true objective of the P2P agent is to maximize the probability of reaching the goal without collisions,

[TABLE]

where $\mathbb{I}$ is an indicator function, $\bm{x_{G}}$ is the goal position, and $d_{G}$ is the goal radius. The zero-collision property is enforced by terminating episodes on collisions. The goal observation $\bm{o}_{g}$ is the relative goal position in polar coordinates, which is readily available from localization. The reward for P2P for differential drive robots is the dot product of the parameters and reward components:

[TABLE]

where $r_{\text{goal}}$ is 1 when the agent reaches the goal and 0 otherwise, $r_{\text{goalDist}}$ is the negative Euclidean distance to the goal, $r_{\text{collision}}$ is 1 when the agent collides with obstacles and 0 otherwise, $r_{\text{clearance}}$ is the distance to the closest obstacle, $r_{\text{step}}$ is a constant penalty step with value 1, and $r_{\text{turning}}$ is the negative angular speed. We train this model with AutoRL [12] over the DDPG [46] algorithm, which simultaneously finds the reward weights $\bm{\theta_{r_{\text{DD}}}}$ and trains the agent. AutoRL automates hyperparameter search in reinforcement learning using an evolutionary approach. AutoRL takes as inputs a true objective used to evaluate the agent, a parameterized dense reward that the agent uses to train itself, and optionally neural network architecture and algorithm hyperparameters. To train the agent, AutoRL typically optimizes these hyperparameters in phases. First, given an arbitrary or hand-tuned architecture, it trains several populations of RL agents with different reward parameters and optimizes over the true objective. Optionally, a second phase repeats the process with the dense reward fixed while searching over neural network architectures instead.

IV-A2 P2P for carlike robots

The true objective of P2P does not change for car drive, but because the turn radius of the car is limited and the car must perform more complex maneuvers, we choose a slightly different reward model:

[TABLE]

where all values are the same as for diff drive except $r_{\text{goalProg}}$ rewards the delta change of Euclidean distance to goal, and $r_{\text{backwards}}$ is the negative of backwards speed and 0 for forward speed. We dropped $r_{\text{goalDist}}$, $r_{\text{clearance}}$, and $r_{\text{turning}}$ to reduce the space of hyperparameter optimization based on analysis of which parameters seemed to have the most positive impact upon learning differential drive models. We train this model with hyperparameter tuning with Vizier [28] over the DDPG [46] algorithm in a different training regime in which the car model is allowed to collide up to 10 times in training, but is still evaluated on the true objective of zero collisions.

IV-B PRM Construction

The basic PRM method works by sampling robot configurations in the the robot’s configuration space, retaining only collision-free samples as nodes in the roadmap. PRMs then attempt to connect the samples to their nearest neighbors using a local planner. If an obstacle-free path between nodes exists, PRMs add an edge to the roadmap.

We modify the basic PRM by changing the way nodes are connected. Formally, we represent PRMs with graphs modeled as a tuple $(V,E)$ of nodes and edges. Nodes are always in free space, $V_{i}\in\mathcal{C}_{\mathrm{free}}$, and edges always connect valid nodes $(V_{i},V_{j})$, but we do not require that the line of sight $\overline{V_{i}V_{j}}$ between those nodes is in $\mathcal{C}_{\mathrm{free}}$, allowing edges that “go around” corners. Since we are primarily interested in robustness to noise and adherence to the task, we only connect configurations if the RL agent can consistently perform the point-to-point task between two points.

Algorithm 1 describes how PRM-RL adds edges to the PRMs. We sample multiple points from the configuration space, around the start and goal in the workspace, and attempt to connect the two points over $n_{\omega}$ trials. An attempt is successful only if the agent reaches sufficiently close to the goal point. Even with a high success threshold, PRM-RL trajectories are not guaranteed to be collision-free because of sensor and action noise. To compute the total length of a trajectory, we sum the distances for all steps plus the remaining distance to the goal. The length we associate with the edge is the average of the distance of successful edges. The algorithm adds the edge to the roadmap if the success probability $success_{rate}$ is above the threshold $p_{s}$.

The worst-case number of collision checks in Algorithm 1 is $O(\mathcal{K}_{\omega}*n_{\omega})$, because multiple attempts are required for each edge to determine $success_{rate}$. Each trial of checking the trajectory can be parallelized with $n_{\omega}$ processors; alternately, trajectory checking within Algorithm 1 can be serialized, terminating early if the tests fail too many times. Mathematically, for a given success threshold and desired number of attempts, at least $n_{s}=\lceil{p_{s}*n_{\omega}}\rceil$ trials must succeed; therefore we can terminate when $n_{s}>p_{s}*n_{\omega}$, or when the failures exceed the complementary probability $n_{f}>(1-p_{s})*n_{\omega}$. This can provide substantial savings if $p_{s}$ is high, as shown in Section V-D. Much of PRM-RL construction can be parallelized; parallel calls to Algorithm 1 can speed roadmap construction, and roadmaps can be constructed in parallel.

We use a custom kinematic 3D simulator which provides drive models for agents and supports visual sensors such as cameras and lidars. The simulator also provides parameterized noise models for actions, observations, and robot localization, which improves model training and robustness. Stepping is fast compared to full-physics simulations because our simulator is kinematic. This speeds up RL training and roadmap building.

Algorithm 2 describes the roadmap building procedure, where an RL agent is trained once, and used on several environments. While building a roadmap for each environment, we first sample the obstacle map to the given density and store all candidate edges. Two nodes that are within the RL policy range, $d_{\pi},$ are considered candidates. Next, we partition all the candidate edges into subsets for distributed processing. The PRM builder considers each candidate edge and adds it, along with its nodes, to the roadmap if the AddEdge Monte Carlo rollouts returns success above threshold.

IV-C Navigation

Finally, Algorithm 3 describes the navigation procedure, which takes a start and a goal position. These are added to the roadmap if not present. Then, the roadmap is queried for a list of waypoints. If no waypoints are returned, the algorithm returns the start and goal as the path to give the RL agent the opportunity to attempt to navigate on its own. In execution, a higher-level PRM agent gives the RL agent one waypoint at the time as a sub-goal, clearing these goals sequentially as the RL agent gets within goal distance $d_{G}$, until the final destination is reached or $\mathcal{K}_{\omega}$ is exceeded.

V Results

We evaluate PRM-RL’s performance on both floorplan maps and SLAM maps with respect to comparable baselines, construction parameters, simulated noise, and dynamic obstacles, as well as with experiments on robots. Section V-A describes the robot and training setup, evaluation environments, and baselines. Section V-B demonstrates PRM-RL’s superior performance on floormaps with respect to baselines, while the following sections examine PRM-RL’s characteristics in more depth. Section V-C demonstrates PRM-RL’s robustness to noise, and Section V-D explores quality and cost tradeoffs with success threshold and sampling density. Since one of our goals is to assess PRM-RL for real-world navigation, Sections V-E and V-F show PRM-RL’s applicability to SLAM and large-scale maps, and Section V-G analyzes sim2real experiments on real robots. Finally, to demonstrate the extensibility of PRM-RL to new situations, Section V-H shows PRM-RL works well on simulated robots with dynamic constraints, Section V-I explores PRM-RL’s robustness in the face of dynamic obstacles, and Section V-J compares PRM-RL’s performance on a suite of maps used by a visual policy baseline.

V-A Methodology

V-A1 Robot setup

We use two different robot kinematic models, differential drive [43] and simple car model [43, 58]. We control both models with linear and angular velocities commanded at $5$ Hz, receive goal observations from off-the-shelf localization, and represent both as circles with $0.3\text{\,}\mathrm{m}$ radius. The obstacle observations are from 2D lidar data (Fig. 5), with a $220\degree$ FOV resampled to $64$ rays. Following [55], we use an observation trace of the last $\theta_{n}$ frames to provide a simple notion of time to the agent, with $\theta_{n_{CM}}=3$ and $\theta_{n_{DD}}]=1$ for the car model and diff-drive respectively. We use Fetch robots [53] for physical experiments.

V-A2 Obstacle-avoidance local planner training

We train P2P agents with AutoRL over DDPG [12] with reward and network tuning for the differential drive robot, and reward tuning for the car robot. In both cases, the true objective for training the local planner is to navigate within $0.25\text{\,}\mathrm{m}$ of the goal, allowing the RL agent to cope with noisy sensors and dynamics. Table I depicts learned reward hyperparameters. DDPG actor and critic are feed-forward fully-connected networks. Actor networks are three layers deep, while the critics consists of a one or two-layer observation networks joined with the action networks by two fully connected layers. Actor, critic joint, and critic observation layer widths are $(241,12,20)\times(607,242)\times(84)$ for the reward-and-network trained differential drive model and $(50,20,10)\times(10,10)\times(50,20)$ for the reward-trained car model. Appendix Acknowledgments contains the training hyperparameter details. The training environment is $14\text{\,}\mathrm{m}$ by $17\text{\,}\mathrm{m}$ (Fig. 3a). To simulate imperfect real-world sensing, the simulator adds Gaussian noise, $\mathcal{N}(0,0.1)$, to its observations.

V-A3 Evaluation environments

Table II lists our simulation environments, all derived from real-world buildings. Training, Building 1-3, depicted in Fig. 3, and Building Complex (Fig. 2) are metric maps derived from real building floor plans. They are between 12 to 200 times larger than the training environment by area. Physical Testbed 1 (Fig. 6), Physical Testbed 2 and Physical Testbed 3 (Fig. 11c) are derived from SLAM maps used for robot deployment environments.

V-A4 Roadmap building

For simplicity we use uniform random sampling to build roadmaps. We connect PRMs with a $p_{s}$ effective threshold of $\geq 90\%$ over 20 attempts, with a max connection distance $d_{\omega}$ of $10\text{\,}\mathrm{m}$ based on the effective navigation radius $d_{\pi}$ for our P2P agents per [12], except where otherwise stated.

V-A5 Baselines

Table III shows four selected baselines. The baselines differ in the local planner, used for building the roadmap, and the execution policy, which guides the robot. We select baselines given their ability to avoid obstacles and deal with stochasticity. Recall that PRM-RL relies on a stochastic policy capable of obstacle-avoidance to connect nodes in the roadmap using Monte Carlo rollouts of the policy.

The baselines for experimental comparison include a local planner based on AutoRL [12], PRM-SL [39], PRM-GAPF (a modification of [11]), and PRM-DWA. PRM-SL [39] uses roadmaps built with a straight-line planner and a straight-line execution policy. PRM-GAPF uses PRMs built with a straight-line planner, and an execution policy of APF, an artificial potential field planner [40], guided by the PRM-SL path, similar to [11]. PRM-DWA is similar to PRM-GAPF using DWA, a dynamic window avoidance local planner [24].

In addition, we compare against two other baselines numerically: PRM-HTRL [23] is our original PRM-RL with hand-tuned DDPG as the planner; where not otherwise specified, PRM-RL refers to our current approach. Successor Features [70] is a visual-based navigation approach using discretized actions. We do not compare PRM-RL with RRTs here because this work focuses on solving the multi-query problem, while RRTs solve single-query problems, making them comparatively expensive for building long-range trajectories on the fly, especially when incorporating end-to-end controls. While large roadmaps can be expensive to construct, they can be reused for many queries across many robots, and queries are fast. For example, RRT solves a single query in the same environment in about 100 seconds [13], while PRM-RL produces a path in less than a second for a pre-built roadmap (see Table IV), although the roadmap takes hours to build.

For comparisons to baselines, unless otherwise stated, each roadmap is evaluated on 250 queries selected from the $\mathcal{C}_{\mathrm{free}}$ between start and goal positions from $1.5\text{\,}\mathrm{m}$ to $100\text{\,}\mathrm{m}$. We measure start to goal distance by the shortest feasible path as estimated by our simulation framework using a queen’s-move discretized A* search with adequate clearance for the robot to travel without collision.

V-B PRM-RL Performance on Floorplan Maps

Table IV shows PRM-RL’s performance compared to baselines on Buildings 1-3 of our floorplan maps, using both sparse and dense PRMs; Fig. 7a also shows for reference our prior work PRM-HTRL. PRM-RL’s average success rate in the dense condition is $91.7\%$ over all three maps, which outperforms the baselines by $83.2\%$ for pure AutoRL, $82.2\%$ for dense PRM-SL, and $51.6\%$ for dense PRM-DWA. Outperforming AutoRL’s non-guided local policies is not surprising as they do not have the knowledge of the obstacle map, but we include it for completeness. Successful paths executed by AutoRL are shortest, but dense PRM-SL and dense PRM-RL are within $10\%$ of the shortest feasible path as estimated by our simulation framework. DWA path lengths are much longer because DWA exhibits safe looping behavior in box canyons given the action space and noisy observations, but DWA maintains the best clearance to obstacles.

Analyzing collision checks (the complexity of building a roadmap), planning time, and execution time reveals interesting tradeoffs. First, PRM-RL requires 1-2 orders of magnitude more collision checks than PRM-SL; increasing map density only consistently helpes PRM-RL. This is because of the Monte Carlo rollouts. At runtime, path finidng requires no collision checking, as it performs a graph search on a pre-built roadmap. At runtime, planning with a roadmap requires no collision checks, though local control policies may carry out collision checks in execution (e.g., DWA collision checks trajectories in velocity space). Second, PRM-RL planning time is up to twice as long as PRM-SL, because the PRM-RL roadmaps contain more edges due to the RL local planner connecting to nodes that are not in the clear line of sight. Still, planning takes less than a second even for the densest maps, an insignificant part of execution time in both simulation and robot deployment. Last, the execution time for PRM-RL is almost five times longer than PRM-SL because PRM-RL succeeds at longer trajectories thanks to RL control policy that adapts on-the-go to uncertainties not present in planning time (i.e. moving obstacles or sensor noise).

We can draw three observations from these results. First, guiding a local planner with PRM-RL can vastly improve the planner’s success rate; this is not surprising as our local planners are not designed to travel long distances. Second, PRM-RL successfully enables local planners to transfer to new environments: the AutoRL policy only saw the Training Map in training, yet performs at a $91\%$ success rate on our evaluation maps. Third, the PRM success rate is correlated with the success of the local planner. PRM-RL with an AutoRL policy and a sparse PRM build achieves $86.5\%$ success, a $37.5\%$ increase over the sparse $49\%$ success rate reported in PRM-HTRL. This is evidence that investing in a high-fidelity local planner increases PRM-RL’s performance of the overall navigation task. In contrast, moving to a denser PRM map (which we discuss in more detail in Section V-D) provides a lesser increase of $5.2\%$; this is still significant for deployment, however, as it also represents a $38.6\%$ decrease in errors.

V-C PRM-RL Robustness to Noise

Sensors and actuators are not perfect, so navigation methods should be resilient to noise. Fig. 8 shows the evaluation of PRM-RL, PRM-GAPF, and PRM-DWA on the Training map, Buildings 1-3 and Physical Testbed 1 over simulated Gaussian noise sources with mean $0.0$ and $\sigma$ in the following ranges:

• •

Lidar sensor noise $\sigma_{l}$ from $0\text{\,}\mathrm{m}$-$0.9\text{\,}\mathrm{m}$, over three times the radius of the robot.

• •

Goal position noise $\sigma_{g}$ from $0\text{\,}\mathrm{m}$-$0.9\text{\,}\mathrm{m}$, over three times the radius of the goal target.

• •

Action noise of velocity $\sigma_{v}$ $0\,{\text{m}/\text{s}}$-$0.9\,{\text{m}/\text{s}}$ and angular velocity $\sigma_{a}$ $0.9\,{\text{rad}/\text{s}}$.

As lidar and action position noise increase, PRM-RL shows only a slight degradation of $28\%$ on lidar noise and $37\%$ on action noise, even at $0.9\text{\,}\mathrm{m}$. In contrast, PRM-GAPF and PRM-DWA degrade steeply with respect to lidar noise, with success rates dropping to less than $1\%$. These methods seem to be more resistant to increased action noise, but still drop to $39\%$ and $54\%$ of their peak scores for PRM-GAPF and PRM-DWA respectively. All methods were relatively resistant to goal noise, with less than $10\%$ falloff. PRM-RL outperformed PRM-GAPF and PRM-DWA in all conditions.

PRM-RL is resilient to lidar, localization, and actuator noise on the order of tens of centimeters, which is larger than the typical lidar, localization, and actuator errors we observe on our robot platforms, indicating that PRM-RL is a feasible approach to deal with the kinds of errors our robots actually encounter. This is similar to the trend to noise sensitivity reported in [12], and suggests that overall navigation resilience to noise is correlated to that of the local planner.

V-D The Impact of Sampling Density and Success Threshholds

To deploy PRM-RL on real robots, we need to choose sampling density and success threshold parameters that provide the best performance. Fig. 9 shows that PRM-RL success rate increases steadily up to a sampling density of $1.0$ samples per meter, which is roughly twice the size of our robot, and then levels off. At the same time, collision checks increase rapidly with sampling density; we have observed that over $1.0$ samples per meter, the roadmap for the training environment requires more collision checks than some of the larger evaluation roadmaps collected at $0.4$ samples per meter. While PRM-SL theory predicts that performance would continue to improve with increased sampling [38], [33], this suggests that beyond a critical density PRM-RL performance is robot-noise-bound, and that sampling beyond this density provides little benefit at rapidly increasing cost.

These experiments evaluate global planning; however, note the average distance of points at 2.0 samples per square meter is roughly $0.5\text{\,}\mathrm{m}$, the true objective distance used to train our AutoRL local planners. This suggests the asymptotic behavior in these experiments could be explained by the PRM sampling density approaching a distance where the local planner can almost always find a path to a nearby PRM node. Also note the car model’s success rate differs from the diff-drive model, suggesting that success rate is model bound rather than map bound.

Fig. 10 shows that PRM-RL’s success rate increases with the required connection success rate. Because our connection algorithm terminates earlier for higher thresholds when it detects failures, collision checks drop as the success threshold rises. At the end, for larger roadmaps, the success threshold of 100% not only produces the most reliable roadmaps, but requires fewer collision checks to build them.

These results suggest choosing map densities up to $1.0$ samples per meter with as high a success connection threshold as possible. In this paper, we compare sparse and dense parameters: a sampling density of $0.4$ samples per meter and an effective success connection threshold of $\geq 90\%$ which enables comparison with [23], and a density $1.0$ samples per meter with a threshold of $100\%$; in almost all evaluations, we observe better performance with the dense parameters.

V-E PRM-RL Performance on SLAM Maps

Floorplans are not always available or up-to-date, but many robots can readily generate SLAM maps and update them as buildings change. To assess the feasibility of using SLAM maps, we evaluate PRM-RL on SLAM maps generated from some of the same buildings as our floorplan roadmaps. Fig. 6 illustrates a sample roadmap built from a SLAM map generated with the ROS distribution of the GMapping algorithm [27] with the default resolution of 5cm per pixel. This map corresponds to Physical Testbed 1, part of floor two of Building 1 in Fig. 3d and the upper center section of the large-scale roadmap in Fig. 2. This SLAM-derived roadmap has 195 nodes and 1488 edges with 2.1 million collision checks.

We compare PRM-RL with our baselines on the three Physical Testbed SLAM maps. PRM-RL’s success rate is $89\%$ on the dense PRM, a $97\%$ relative increase over PRM-DWA and a $157\%$ increase over PRM-GAPF.

These results show that the performance of PRM-RL with an AutoRL policy is lesser but comparable to its performance on floorplan maps, and exceeds all other baselines; it is even superior to PRM-HTRL on floorplan maps. These results indicate PRM-RL performs well enough to merit tests on roadmaps intended for real robots at physical sites, which we discuss in the following two sections.

V-F Scaling PRM-RL to Large-Scale Maps

Our robot deployment sites are substantially larger than our simulated test maps, raising the question of how PRM-RL would scale up. For example, the SLAM map discussed in the previous section is only part of one building within a quad-building complex. Where the SLAM map is $78\text{\,}\mathrm{m}$ by $44\text{\,}\mathrm{m}$, a map of the quad-building complex is $288\text{\,}\mathrm{m}$ by $163\text{\,}\mathrm{m}$. To assess PRM-RL’s performance on large-scale maps, we build and test roadmaps for maps covering all deployment sites.

Fig. 2 depicts a large floorplan roadmap from the quad-building complex. This roadmap has 15,900 samples and 1.4 million candidate edges prior to connection attempts, of which 689,000 edges were confirmed at a $90\%$ success threshold. This roadmap took 4 days to build using 300 workers in a cluster, and required 1.1 billion collision checks. PRM-RL successfully navigates this roadmap $57.3\%$ of the time, evaluated over 1,000 random navigation attempts with a maximum path distance of $1000\text{\,}\mathrm{m}$. Note our other experiments use a maximum path distance of $100\text{\,}\mathrm{m}$, which generally will not cross the skybridge in this larger map. For reference, using our default evaluation settings, PRM-RL navigates this roadmap $82.3\%$ of the time.

For our other candidate robot deployment site, we use a large SLAM map, $203\text{\,}\mathrm{m}$ by $135\text{\,}\mathrm{m}$. We constructed a roadmap with 2069 nodes and 53,800 edges, collected with 42 million collision checks at the higher success threshold of $100\%$. PRM-RL successfully navigated this $58.8\%$ of the time, evaluated over 1,000 random navigation attempts. As on our smaller SLAM map, the failure cases indicate that the more complex geometry recorded by SLAM proves problematic for our current policies.

These results indicate that PRM-RL’s simulated performance on large-scale roadmaps surpasses the average success threshold we observed previously in [23], making it worthwhile to test on real robots at the physical sites.

V-G Transfer of PRM-RL to Physical Robots

We empirically evaluate PRM-RL on three physical environments on two differential-drive robots. First, we evaluate PRM-RL with the AutoRL policy for a differential-drive robot in Physical Testbed 1 (Fig. 6a). We collected 27 trajectories over $831\text{\,}\mathrm{m}$ of travel with an overall success rate of $85.2\%$; the longest successful trajectory was $83.1\text{\,}\mathrm{m}$ meters. Fig. 6c shows the trajectories of 12 runs.

Second, we evaluate PRM-RL in Physical Testbed 2. For a variant of the roadmap generated at $90\%$ success rate with a density of $0.4$ samples per meter, we collected 8 trajectories over $487.2\text{\,}\mathrm{m}$ of travel; the longest successful trajectory was $96.9\text{\,}\mathrm{m}$. We cannot directly compare this evaluation to our simulated runs because the e-stop policies designed to protect the robot do not match our simulation termination conditions. Nevertheless, we recorded 3 successful runs out of 8, a $37.5\%$ success rate. We then tested a variant of the roadmap generated at $100\%$ success rate and a density of $1.0$ samples per meter over 13 runs on $2542.1\text{\,}\mathrm{m}$ of travel for an improved $84.6\%$ success rate, shown in Fig. 11a; the longest successful trajectory was $221.3\text{\,}\mathrm{m}$.

Third, we apply the same evaluation parameters from Physical Testbed 2 onto a novel environment, Physical Testbed 3. We collected 128 runs, 64 for each variant of the roadmap, testing both with the same sets of start/goal points. The denser map achieved a $92.2\%$ success rate compared to $85.9\%$ for the sparse map. Since Physical Testbed 3 contains large sections of free space, using a denser map resulted in more optimized node connections and reduced robot traversal by an average of $2.5\text{\,}\mathrm{m}$ per run.

Fig. 12 summarizes these results. Despite more aggressive episode termination policies on robots (near-collisions are treated as failures), we nonetheless observe similar results: over several different roadmaps constructed at different densities and success criteria, PRM-RL achieves $85.8\%$ success on robots with an average sim2real gap of $7.43\%$. These results show that the performance of PRM-RL on robots is correlated with the performance of PRM-RL in simulation. This makes PRM-RL a useful method for closing the sim-to-real gap, as improvements or regressions in simulation are likely to be reflected in performance on robots.

V-H PRM-RL with Kinodynamic Constraints

To demonstrate that PRM-RL could guide more complicated robots, we develop a drive model for a simulated F1/10 car [18] with dynamics following [58] with velocity and acceleration constraints enforced by our simulator. In this setup, AutoRL learns a steering function for a robot with kinodynamical constrains, effectively learning to respect velocity and acceleration constraints. Algorithm 1 encodes those constrains globally by connecting only reachable nodes, making PRM-RL effectively a kinodynamic planner [14]. Average success over the four maps in Fig. 3 is $85.8\%$ with a standard deviation of $1.0\%$; average success in simulation on Physical Testbed 1 with a goal distance of $0.25\text{\,}\mathrm{m}$ is $85.8\%$. Fig. 13a illustrates a roadmap built with this model over our training map with $0.4$ samples per meter connected with a $90\%$ success rate; this roadmap has 32 nodes and 313 edges connected after 403,000 edge checks. On this roadmap, PRM-RL exhibits an $83.4\%$ success rate, including cases where the car needs to make a complex maneuver to turn around (Fig. 13b). These results are comparable to results on the robot, indicating that PRM-RL is a viable candidate for further testing on more complicated robot models.

V-I PRM-RL with Dynamic Obstacles

PRM-RL is also resilient in the face of dynamic obstacles, relying on the local planner to avoid them without explicit replanning. We simulated pedestrians with the social force model [31] and tested PRM-RL, PRM-GAPF, and PRM-DWA on Buildings 1-3 with 0 to 40 added agents. While all methods showed a similar degradation in the presence of obstacles, on average $39.0\%$, PRM-RL’s performance only dropped to $53.1\%$ with 40 obstacles, superior to PRM-GAPF ($28.7\%)$ and PRM-DWA ($40.3\%$) even in the zero-obstacle condition. Thus, a resilient local planner can enable PRM-RL to handle dynamic obstacles even though the framework has no explicit support for dynamic replanning.

V-J PRM-RL in Synthetic Environments

The Successor Features visual navigation approach achieves $100\%$ success on training maps and $98\%$ success on transfer to new maps [70]. Successor Features navigates to single targets using vision and a discretized action space. In contrast, PRM-RL navigates to arbitrary targets using lidar and a continuous action space. Nevertheless, both approaches navigate in spaces qualitatively similar to each other in simulation and in testing on robots.

Therefore, we evaluate PRM-RL and our baselines in simulation on close approximations of the maps used in [70], which include four maze-like simulated maps and two physical testbed environments. Fig. 15 shows the results: Both AutoRL and PRM-RL achieved over $90\%$ success on the synthetic maps, with PRM-RL on a dense map achieving $94.6\%$ success. While Successor Features’s $98\%$ success is $3.4\%$ higher than PRM-RL, it has access to a visual sensor, executes discretized actions, and navigates to a single target.

We can draw two conclusions from these findings. First, PRM-RL generalizes well to environments which are different from both our training environment and our previous testing environments. Second, PRM-RL is competitive with methods specifically designed for other environments.

VI Analysis

In the previous section, we empirically established correlations between the local planner’s competence and PRM-RL’s resilience to noise. We also explored the contributions of sampling densities, success thresholds and obstacle map sources to the success of overall navigation. We concluded that 1) using an execution policy that is resilient to noise and avoids obstacles as a local planner improves the overall success of the hierarchical planner; 2) a success threshold of 100% improves overall navigation success, 3) the upper bound to navigation success is not dependent on density but policy performance and robot type, and 4) using realistic obstacle maps, such as SLAM maps, as a basis for building roadmaps provides simulation results closer to reality.

This section provides a deeper analysis of those empirical findings. Section VI-A analyzes the impact of local planner’s obstacle avoidance and noise resilience on roadmap construction. Section VI-B examines the computational complexity of PRM-RL, Section VI-C discusses causes of failure for trajectories over multiple waypoints, and Section VI-D discusses limitations of the method and future work.

VI-A PRM-RL Roadmap Connectivity

Unlike straight-line planners, RL agents can often go around corners and smaller obstacles; Fig. 16 shows how this effectively transforms the configuration space to make obstacles smaller. While the agent never traverses these corner points, as they are not in $\mathcal{C}_{\mathrm{free}}$, they nevertheless do not serve to block the agent’s path, unlike central portions of a larger body which might block or trap the control agent in a local minimum. If we model this as an effective reduction in radius of a circular obstacle $f_{\pi}$ with respect to a policy $\pi$, and model the connection region as a disc filled with randomly sampled obstacles from $0\%$ to $100\%$ in total area density $\rho_{o}$, we can estimate an upper bound on connection success in the idealized case in which obstacles do not occlude and the chance of connection is just the complementary probability of encountering an obstacle over a region of space, $1-\rho_{o}$. This corresponds to the looser bound $1-\rho_{o}{f_{\pi}}^{2}$ in the RL case. Therefore, a conservative estimate of the ratio of samples connected by PRM-RL to those connected by PRM-SL is:

[TABLE]

This simplified model indicates that it becomes harder to connect points as obstacle density increases, but PRM-RL has an increasing advantage over PRM-SL in connecting these difficult roadmaps as RL’s ability to cut corners increases. Hence, in environments with dense obstacles, it makes sense to invest in execution policies that avoid obstacles really well, and use them as local planners. Alternately, it suggests that policies that can learn strategies for dealing with frequently occurring obstacle topologies, like box canyons and corners, are a fruitful area for future work. Because the branching factor of roadmap planning is proportional to the points in the connectivity neighborhood, planning cost can increase exponentially with connection radius, and following [25], we set neighborhood size empirically to balance the benefits of finding points within the effective navigation radius of the planner against the drawbacks of a connectivity neighborhood which contains so many points that planning becomes infeasible.

Conversely, PRM-RL does not connect nodes in a roadmap where a policy cannot navigate reliably. This is the key difference from PRM-SL, and is the cause for the upper limit on performance improvements as the roadmaps increase in density—the roadmaps are policy-bound, rather than sampling bound. One question to ask is why local control policies cannot learn to “drive safe” by repeatedly re-planning the path (e.g., via A* searches or variants). However, an analysis of how noise impacts the behavior of policies indicates that policies which do not memorize the environment may overestimate their ability to navigate because the hazards that they can observe locally may not represent an accurate picture of the hazards of the global environment.

To see why, suppose a policy has learned to navigate safely in the presence of noise by maintaining a distance $d_{safety}$ from walls. Modeling time as discrete and assuming the robot is travelling at a constant speed, so that on each time slice the robot moves a constant distance $d_{step}$ units forward, let us further model sources of variability as transient Gaussian noise orthogonal to the robot’s travel $\mathcal{N}_{pos}(0,\sigma_{pos})$ with zero mean and standard deviation $\sigma_{pos}$. This results in a probability of collision per step of $\frac{1}{2}\text{erfc}\left(\frac{d_{\text{safety}}}{\sqrt{2}\sigma_{\text{pos}}}\right)$ (the cumulative distribution function of the Gaussian noise model $\mathcal{N}_{pos}$ evaluated at $-d_{safety}$, expressed in terms of the complementary Gauss error function $\text{erfc}(x)$). Fig. 17a shows that when the robot is traveling in a narrow corridor it is twice as likely to collide as it does when hugging a wall, even though it may be maintaining $d_{safety}$ from any given wall at all times. Over a path of length $d_{corr}$, a conservative lower bound on the chance of collision rises exponentially with the number of steps it takes to traverse the path,

[TABLE]

causing the narrow corridor case to become unsafe faster than the wall-hugging case as shown in the blue region of Fig. 17b. This means that an RL policy that judges its safety based on locally observable features can overestimate the safety of a path in the region between where both PRM-RL and RL would succeed and where both PRM-RL and RL would fail (Fig. 17c). The same would be true of RL guided by PRM-SL based on clearance, such as in grid-based sampling [19] and Safety PRM [50]. In this case, RL or RL guided by PRM-SL can make erroneous navigation choices, whereas PRM-RL simply does not add that link to the roadmap. While in theory an agent could be designed to cope with this specific issue, other scenarios can present similar problems: no local prediction of collision probability can give a true guarantee of trajectory safety. While this is an idealized analysis, our experience is that real-world environments can be more pathological—for example, at one site curtains behind a glass wall induced a persistent 0.5 meter error in the robot’s perceived location, causing it to drive dangerously close to the wall for long distances despite the presence of a safety layer. PRM-RL provides an automated method to avoid adding those unreliable connections in the roadmap, resulting in the roadmaps that more sparsely connect but transfer more reliably to the robot.

VI-B PRM-RL Computational Complexity

In this section we assess the computational complexity of building a PRM-RL roadmap with known RL policy range $d_{\pi}$, sampling density $\rho_{\omega}$, workspace volume $V_{\mathcal{W}}$, and connection attempts $n_{\omega}$. The cost of building a PRM with $n$ nodes is dominated by the cost of two distinct steps. The first step identifies potential edges by finding $m$ potential nearest neighbors for each of the $n$ nodes. While approaches such as PRM* can construct PRMs with $O(n\log n)$ edge tests, achieving this performance requires limits on $m$, such as connecting only the $m$ nearest neighbors or shrinking the radius of connection as $n$ increases [36]. Approaches using a fixed connection radius, such as Simplified PRM [38], require $O(n^{2})$ edge tests [36]. The second step evaluates the validity of each of the $n\,m$ candidate edges by performing collision checks along each edge, and adds valid edges to the graph.

PRM-RL samples nodes in the workspace $\mathcal{W}$ and attempts to connect all neighbors within range of the policy $d_{\bm{\pi}}$, which can be determined empirically (see [12]) and is independent of the workspace volume $V_{\mathcal{W}}$. Similarly, Section V-D shows PRM-RL’s performance shows diminishing returns beyond a sampling density $\rho_{\omega}$, also independent of workspace volume. Capping sampling at density $\rho_{\omega}$ makes the number of nodes a function of the workspace volume,

[TABLE]

where $D_{\mathcal{W}}\leq 3$ is the workspace dimensionality, and $d$ is the radius of the smallest $D_{\mathcal{W}}$-dimensional sphere that contains the workspace $\mathcal{W}.$ However, because the neighbor volume is a function of policy range and workspace dimensionality, $V_{\bm{\pi}}\propto d_{\bm{\pi}}^{D_{\mathcal{W}}},$ the number of neighbors

[TABLE]

is a variable independent from $n_{{}_{\mathcal{W}}},$ though their ratio is fixed for any given workspace. Let $d_{\bm{\pi}}=cd$ for some constant $c\in\mathbb{R}$. More capable policies have $c$ closer to 1, while less capable policies have $c$ closer to 0. For simplicity, we assume that $0<c\leq 1,$ when limiting the search radius, even if policy’s radius might exceed the workspace’s radius. Thus,

[TABLE]

Therefore we analyze PRM-RL with respect to nodes $n_{{}_{\mathcal{W}}}$ and neighbors $m_{\bm{\pi}}$ in terms of the source variables that determine them: workspace volume $V_{\mathcal{W}}$, sampling density $\rho_{\omega}$, and effective range $d_{\bm{\pi}}$. First, the edge identification step is $O(n_{{}_{\mathcal{W}}}\log n_{{}_{\mathcal{W}}})$ because the number of nearest neighbors given in Eq. (7) is independent of the total number of nodes $n_{{}_{\mathcal{W}}}$ and can be found in $O(\log n_{{}_{\mathcal{W}}})$ with efficient approximate nearest neighbor searches, as described in [36]. From there,

[TABLE]

Second, for the edge validation phase, PRM-RL validates $m_{\bm{\pi}}*n_{{}_{\mathcal{W}}}$ candidate edges with $n_{\omega}$ rollouts. To validate one edge rollout, PRM-RL performs on the order of $O(d_{\bm{\pi}})$ collision checks. Therefore, the cost of adding neighbors to the roadmap is

[TABLE]

by substituting (6) and (7) and rearranging. Combining Eq. (10) and (11), we arrive at the total cost

[TABLE]

because for a typical map $\rho_{\omega}d_{\bm{\pi}}^{D_{\mathcal{W}}}$ dominates $\log d\rho_{\omega}.$

Eq. (13) exposes the following power sources:

• •

Complexity is $O({d_{\pi}}^{2D_{\mathcal{W}}+1})$ in the policy range, so local planners that can reliably cover longer distances increase the computational cost of the roadmap. We recommended choosing a shorter connection distance $d_{\omega}<d_{\pi}$ even if the policy is capable of longer connections.

• •

When the workspace is much larger than the reach of the policy ($0<c\ll 1$), the complexity is almost linear in workspace volume.

• •

Complexity is linear in connection attempts $n_{\omega}$.

• •

Complexity is quadratic in sampling density $\rho_{\omega}$, making it worthwhile to assess the limiting sampling density before building large numbers of roadmaps.

Each edge connection attempt is independent, so roadmap building can be parallelized up to the expected number of samples $V_{\mathcal{W}}\rho_{\omega}$. If parallel rollouts are performed instead of early termination, this can be parallelized further by an additional factor of $n_{\omega}$. So, given $n_{p}\leq V_{\mathcal{W}}\rho_{\omega}n_{\omega}$ processors, the effective time complexity can be reduced up to $O(\frac{V_{\mathcal{W}}\rho_{\omega}n_{\omega}}{n_{p}}\rho_{\omega}{d_{\pi}}^{D_{\mathcal{W}}+1}),$ possibly alleviating some of the time cost of increased sampling.

Last, note that when using early termination, increasing the success threshold $p_{s}$ often (but not always) reduces the required number of connection attempts $n_{s}$. In the worst case scenario where we require $p_{s}=0.5$, early termination can at best cut $n_{s}$ to $\frac{n_{\omega}}{2}$, but as $p_{s}$ increases the number of failures needed to exclude an edge, $\frac{n_{\omega}}{1-p_{s}}$, drops toward 1. Conversely, if navigation is successful then the full $n_{\omega}$ samples need to be collected for an edge; the distribution of successes and failures thus has a large effect on the cost. One way to control this cost to reduce the max connection distance $d_{\omega}$ to less than the effective policy navigation distance $d_{\pi}$; in this case the agent is more often expected to succeed, and $n_{\omega}$ can potentially be reduced. We have observed that these tradeoffs can significantly affect the cost of a run, but must be studied empirically for the environments of interest.

VI-C PRM-RL Trajectory Execution

Because PRM-RL construction calculates the probability of success before adding an edge, we can estimate the expected probability of success of a long-range path that passes through several waypoints. Recall that to add an edge to the roadmap we collect $n_{\omega}=20$ Monte Carlo rollouts and require an observed proportion of successes $p_{s}$ typically of $90\%$ and $100\%$. Given that expected probability of success of a Bernoulli trial observing $n_{s}$ successes out of $n_{\omega}$ samples is

[TABLE]

[52], the actual probability of successful navigation $p_{n}$ over an edge with $p_{s}=100\%$ successful samples is $95.5\%$, and similarly $p_{n}=86.3\%$ for thresholds of $p_{s}=90\%$. Extrapolating over the sequence of edges in a PRM-RL path, the expected success rate is ${p_{n}}^{n_{w}}$ where $n_{w}$ is number of waypoints. In [23] we observe PRM-RL paths with $10.25$ waypoints averaged over our three deployment maps, yielding an estimated probability of success of $22.0\%$ for the $90\%$ threshold and $62.3\%$ for the $100\%$ threshold. Therefore, for the lengths of paths we observe in our typical deployment environments, the $100\%$ threshold improves PRM-RL’s theoretical performance to the point that it is more likely to succeed than not, which is what we observe empirically.

VI-D Limitations

While AutoRL can handle moving obstacles [12], PRM roadmaps remain static after construction, causing two failure modes. First, PRM-RL does not re-plan. If dynamic obstacle avoidance steers a local planner closer to a subsequent waypoint, PRM-RL could re-plan and provide that waypoint as the next waypoint. Second, large changes in the environment can invalidate edges or create new paths (e.g., adding/removing a wall). Re-planning with a roadmap update could handle this scenario (for example, with iterative reshaping [69]).

Another limitation of PRM-RL is that it requires a map. With a sufficiently good local policy, a SLAM algorithm, and an incrementally updatable PRM, it would be possible to build a PRM online by progressively exploring an environment and building the roadmap and SLAM map together. However, while adding online features to a roadmap are certainly feasible, developing an exploration policy is challenging in its own right, and goes hand in hand with improving the quality of the local planner so it can be trusted to execute reliably.

This work focuses on evaluating roadmap construction and performance, so we leave replanning, exploration policies, and online map building for future work.

VII Conclusion

We presented PRM-RL, a hierarchical planning method for long-range navigation that combines sampling-based path planning with RL agent as a local planner in very large environments. The core of the method is that roadmap nodes are connected only when the RL agent can connect them consistently in the face of noise and obstacles. This extension of [23] contributed roadmap construction and robot deployment algorithms, along with roadmap connectivity, computational complexity, and navigation performance analysis. We evaluated the method on a differential drive and a car model with inertia used on floormaps from five building, two noisy obstacle maps, and on three physical testbed environments.

We showed that 1) the navigation quality and resilience to noise of the execution policy directly transfers to the hierarchical planner; 2) a 100% success threshold in roadmap construction yields both the highest quality and most computationally efficient roadmaps; 3) building roadmaps from the noisy SLAM maps that the robot uses at execution time virtually closes the sim2real gap, yielding simulation success rates very similar to those observed on robots. PRM-RL with SLAM maps embed information into the roadmap that the robot uses at execution time, providing a better estimate of performance on the robot. Failure modes include pathologies of the local policy, poorly positioned samples, and sparsely connected roadmaps. In future work, we will examine improved policies with more resistance to noise, better sampling techniques to position the samples strategically, and techniques for improving map coverage with better localization and obstacle maps.

Acknowledgments

The authors thank Jeffrey Bingham, James Davidson, Brian Ichter, Ken Oslund, Peter Pastor, Oscar Ramirez, Chad Richards, Lydia Tapia, Alex Toshev, and Vincent Vanhoucke for helpful discussions and contributions to this project. We also thank the editors and reviewers for their detailed reviews and thoughtful comments.

[Table of Symbols]

[Training Hyperparameters]

Both actor and critic use the AdamOptimizer with $\beta_{1}=0.9$, $\beta_{2}=0.999$, $\epsilon=1e-08$; the actor’s learning rate is $1e-05$ and the critic’s is $0.0005$. The actor uses DQDA gradient clipping and the critic uses $\gamma=0.995$ with the Huber loss for temporal difference errors. $10,000$ initial stabilization steps are followed by a soft target network update of $0.0001$ on every step. Our training batch size is $512$ and our replay buffer has a capacity of $0.5$ million. We train for $5$ million steps, but save policies every $25,000$ steps and select the best policy over the run.