Realtime Collision Avoidance for Mobile Robots in Dense Crowds using Implicit Multi-sensor Fusion and Deep Reinforcement Learning

There is extensive work on collision avoidance in dynamic scenes for robots. These include techniques based on potential-field methods Tilove (1990), social-forces Helbing and Molnar (1995), velocity obstacles van den Berg et al. (2009); Alonso-Mora et al. (2010), etc. These methods have been used in simulated environments and can scale to a large number of agents. In terms of real-world scenarios, these methods require accurate sensing of obstacles’ positions and velocities and parameter tuning that is scenario-dependent. These requirements make it difficult to directly apply them for navigation in dense crowds. At the same time, these methods have been used to generate training trajectories for initializing some learning-based methods. The Dynamic Window Approach (DWA) Fox et al. (1997) is another widely used method which calculates reachable dynamically-constrained velocities for collision avoidance within a short time interval. However, it does not scale well to large numbers of dynamic obstacles.

Sensor-based navigation algorithms are widely used to navigate a robot among pedestrians Fox et al. (1998). In Amditis et al. (2002), data from a radar and far infrared (IR) camera were synchronized and fused to track obstacles for collision avoidance. A significant problem in navigating among pedestrians is modeling their unknown intentions. Thompson et al. (2009) used the observed motions of humans to generate a motion probability grid to model pedestrian intentions. Some methods Large et al. (2004) learned obstacle motions from trajectories in a captured video or used laser scan data and Hidden Markov Models to estimate human trajectories Bennewitz et al. (2005).

Other techniques use a Partially Observable Markov Decision Process (POMDP) to model the uncertainties in the intentions of pedestrians. A POMDP-based planner was presented in Bai et al. (2015) to estimate the pedestrians goals for autonomous driving which was later augmented with an ORCA-based pedestrian motion model Luo et al. (2018). The resulting POMDP planner runs in near real-time and is able to choose actions for the robot. Our approach is complimentary to these methods in that we model the navigation problem as a POMDP.

There is considerable work on using different learning methods for collision avoidance and navigation in dense environments.

We assume that the dynamics of the robot we train is bounded by non-holonomic constraints Alonso-Mora et al. (2010). The robot at any point in time knows its environment only to the extent of its sensor observations, and no global knowledge of the state of the environment may be available. In addition, in accordance with real-world scenarios, the robot may not be able to access other hidden parameters of pedestrians and other dynamic agents, such as goals and states. At each time step $t$, the robot has access to an observation vector $o^{t}$ which it uses to compute a collision-free action that drives it towards its goal from the current position by avoiding collisions with the static and dynamic obstacles. We assume that it is the sole responsibility of the robot to avoid collisions with pedestrians.

Our approach is designed to exploit multiple sensors simultaneously. These include a lidar to measure the proximity to different obstacles and pedestrians. However, the lidar’s raw data does not provide sufficient information to differentiate between animate and inanimate obstacles or sudden changes in the orientation of obstacles. As a result, it is difficult to infer whether the obstacles are moving towards or away from the robot. Therefore, we also use RGB or depth cameras for such observations. Furthermore, these cameras are able to capture the interactions between the obstacles and pedestrians in the scene.

We formulate the navigation among pedestrians as a POMDP, which is solved using deep reinforcement learning. Formally, a POMDP is modeled as a 6-tuple (S, A, P, R, $\Omega$, O) Fan et al. (2018b), where the symbols represent the state space, action space, state-transition model, reward function, observation space and observation probability distribution given the system state, respectively. As stated earlier, our robot has access only to the observations, which can be sampled from the system’s state space. Next, we describe our observation and action spaces.

Using data from the 2-D lidar along with a camera makes our observation space high-dimensional. In addition, since the individual sensor streams have different dimensions (1-D for lidar and 2-D for images), they cannot be processed together. Therefore, we split the robot’s observation vector into four components, $\textbf{o}^{t}=[\textbf{o}^{t}_{lid},\textbf{o}^{t}_{cam},\textbf{o}^{t}_{g},\textbf{o}^{t}_{v}]$, where $\textbf{o}^{t}_{lid}$ denotes raw noisy 2-D lidar measurements, $\textbf{o}^{t}_{cam}$ denotes the raw image data from either a depth or an RGB camera, $\textbf{o}^{t}_{g}$ refers to the relative goal location with respect to the robot, and $\textbf{o}^{t}_{v}$ denotes the current velocity of the robot. $\textbf{o}^{t}_{lid}$ is mathematically represented as:

Where l represents a list of proximity values and $l_{i}$ denotes the $i^{th}$ element of the list. $\textbf{o}^{t}_{cam}$ can be mathematically denoted as:

The action space of the robot is composed of its linear and angular velocities $\textbf{a}^{t}=[v^{t},\omega^{t}]$. The objective of the navigation algorithm is to select an action $\textbf{a}^{t}$ at each time instance, sampled from a trained policy $\pi_{\theta}$ as:

We use a policy gradient based Sutton et al. (1999) reinforcement learning method called Proximal Policy Optimization (PPO) Schulman et al. (2017) to solve the optimization problem in Equation 4. We adapt a policy gradient method since it directly models the strategy that generates actions, given the observations from the agents, and is more suitable for continuous action spaces such as ours. Compared with other policy gradient methods, PPO provides better stability during training by bounding the parameter ($\theta$) updates to a trust region, i.e., it ensures that the updated policy does not diverge from the previous policy. At each training episode, a robot in simulation collects a batch of observations until a time $T_{max}$ and the policy is then updated based on a loss function. PPO uses a surrogate loss function which is optimized using the Adam optimizer under the Kullback-Lieber(KL) divergence constraint which is given as:

Where $\hat{A^{t}_{i}}$ is the advantage function, $\beta$ and $\xi$ are hyperparameters. The key issue is to design methods that take into account multiple sensor inputs and make sure that the training module converges fast and is able to handle all kind of observations.

Since the data from the lidar and camera have different dimensions (see Eqns 1, 2), they cannot be processed together. Therefore, our network (Fig.3) consists of four branches, each processing a single component of the observation $o^{t}$.

Branches 1 and 2 consist of multiple 1-D and 2-D convolutional layers to process the lidar and image observations, respectively, as the convolutional layers exhibit good performance in terms of extracting features from the input data Krizhevsky et al. (2017). Three consecutive lidar frames, each containing a list of proximity values are fed into two 1-D convolutional layers and a fully connected layer for processing. The first hidden layer has $32$ filters and stride length = 2. The second hidden layer has 16 filters, and stride length = 2. Kernels, which capture the relationship among nearby elements in an array, of size 5 and 3 are used in the convolutional layers respectively.

Branch 2 is used to learn to detect motion from the three image frames, characterize the interaction based on obstacle motion and to move towards the free space detected in the frames. Three consecutive image frames are fed into a 2-D convolutional layer with 64 filters, kernel size = 5, and stride length = 2. The next layer uses 64 2-D convolutional filters, kernel size = 5 and stride length = 1. The output of this layer is passed on to 32 2-D convolutional layers of kernel size = 3, and stride length = 2. This is followed by two fully connected layers of sizes 512 and 256. Layer FC4 ensures that the output of branch 2 has the same dimensions as the output of branch 1. Layers FC1 and FC4 at the end of branches 1 and 2 also ensure that the data from the perception sensors have the most effect on the actions generated by the network. ReLU activation is applied to the outputs of all hidden layers in branches 1 and 2.

The output of branches 1 and 2 and the goal and current velocity observations are then correlated together by the fully connected layer FC2 with 128 rectifier units. In the output layer, a sigmoid activation is used to restrict the robot’s linear velocity between (0.0, 1.0) m/s and a tanh function restricts the angular velocity between (-0.4, 0.4) rad/s. The output velocity is sampled from a Gaussian distribution which uses the mean and log standard deviation which were updated during training. The training of the network is end-to-end and all parts of the network are trained simultaneously.

We train the robot to reach its goal in the least possible time, while avoiding the obstacles. Therefore, the robot is rewarded for heading towards and reaching its goal, and penalized for moving too close or colliding with an obstacle. In addition, the robot is expected to avoid oscillatory velocities, follow smooth trajectories and reach intermediate waypoints before reaching the goal. Although the penalty for collision teaches the robot not to collide, it does not specifically result in the robot maintaining a safe distance from obstacles. This needs to be considered when the dynamic obstacles are pedestrians. Some previous algorithms Long et al. (2017); Chen et al. (2017) train their model for multi-agent collision avoidance, which results in the robot not maintaining a safe distance from pedestrians. The intermediate waypoints provide the robot with a sense of direction and guide it towards its goal. Formally, the total reward collected by a robot i at time instant t can be given as:

where the reward for reaching the goal $(r_{g})^{t}_{i}$ or an intermediate waypoint is given as:

The collision penalty $(r_{c})^{t}_{i}$ is given as:

The oscillatory behaviors (choosing sudden large angular velocities) are penalized as:

The penalty for moving too close to an obstacle is given by:

We set $r_{wp}$ = 10, $r_{goal}$ = 20, and $r_{collision}$ = -20 in our formulation. Critical behaviors such as collision avoidance and goal reaching have a higher priority in terms of the overall reward collected by the robot, while choosing smoother velocities and maintaining a safe distance from obstacles contribute to the reward with a slightly lower priority.

A major challenge in learning based methods is to close the sim-to-reality gap that arises when using a policy trained using simulated environments and sensor observations is evaluated in the real world. Additionally, when using multiple sensors, especially cameras, the simulation should contain as many real world features or characteristics as possible for the training to generalize well. One of our goals is to take advantage of the camera’s ability to observe such complex features (Fig. 2). To address these issues simultaneously, we use high-fidelity 3-D environments that replicate real-world open spaces, and indoor scenarios with realistic moving pedestrians, and occluded environments.

The policy training is carried out in multiple stages, starting with a low-complexity static scenario, to dynamic scenarios with pedestrians. The simple scenarios initialize the policy $\pi_{\theta}$ with capabilities such as static collision avoidance and goal-reaching, while dynamic collision avoidance capability is learned in complex scenarios with moving pedestrians. The different type of environments used in training are shown in (Fig.4). These include:

Static Scenario: We use two independent robots that must start from fixed initial locations and head towards fixed goals straight in front of them while avoiding a wall. A single policy is shared between the two robots so that both left and right turning maneuvers are learned simultaneously. Using this initialized policy, we then provide random initial locations and random goals to the two robots for the goal-reaching and static collision avoidance capabilities to generalize well.

Random Static and Dynamic Obstacles: In this scenario, a single robot has to move towards a random goal in the presence of static human models and randomly moving pedestrians. We ensure that there is more than enough space through which the robot can navigate. To generate pedestrian trajectories, we assign several waypoints or goals in the environment for the pedestrian models to move to at different time instants. The simulated pedestrians move at near human walking speeds and tend to mimic natural human walking motions.

Scenario with occluded obstacles: The saved model from the previous scenario is next trained on a complex corridor scenario, which requires the robot to perform several sharp turns and avoid pedestrians who can only be observed in close proximity. This trains the robot to perform quick maneuvers in real-world scenarios where occluded pedestrians walk towards the robot suddenlyr.

Sim-to-real transfer: Both the scenarios with dynamic obstacles make the sim-to-real transfer and generalization easier, as the depth camera observes noisy 3-D real-life like data such as pedestrian contours and walls, which is fused with more accurate lidar data. Such fusion was not possible in previous methods Long et al. (2017); Fan et al. (2018b), as they were trained in a 2.5D simulator. However, in the corridor scenario, there is a danger of the policy overfitting to follow walls and moving to goals which are straight ahead of the robot instead of generalizing to all scenarios. To prevent this, we also parallelly run robots in the simple static scenarios with random goals alongside the corridor scenario.

We train our model in simulations that were created using ROS Kinetic and Gazebo 8.6 on a workstation with an Intel Xeon 3.6GHz processor and an Nvidia GeForce RTX 2080Ti GPU. We use Tensorflow, Keras and Tensorlayer for implementing our network. We use models of the Hokuyo 2-D lidar and the Orbbec Astra depth camera in Gazebo to simulate sensor data during training and evaluation. The Hokuyo lidar has a range of $4$ meters, FOV of 240^∘ and provides $512$ range values per scan. The Astra camera has a minimum and maximum sensing range of $1.4$ meters and $5$ meters respectively. We use images of size $150\times 120$ with added Gaussian noise $\mathcal{N}(0,0.2)$ as inputs to our network training. We mount these sensors on a Turtlebot 2 and a Clearpath Jackal robot to test our model in real-world scenarios such as crowded corridors and occluded scenes.

The convergence of our reward function versus the number of iterations for different training scenarios is shown in Fig.5. The training in all scenarios starts to converge around 100 iterations, and stabilizes around 200 iterations and the total process completes in six days. This increase in training time when compared with previous methods is due to the high dimensionality of the 3-D depth data used during training. Previous methods do not perform any kind of implicit sensor fusion to detect and avoid obstacles. However, the training time does not affect our run-time performance, which can be observed from our real-world tests.

We consider five different test scenarios that have narrower or different sections, as compared to the synthetic datasets used during our training phase. This demands tight maneuvers from the robot to reach the goal. These testing scenarios are more challenging than existing simulation benchmarks and help to better test CrowdSteer’s sim-to-real, and generalization capabilities. We define Least Passage Space (LPS) as the minimum space available to the robot in-between obstacles when moving towards the goal. The scenarios we consider are:

1. Narrow-Static: Scenario with only static obstacles where the LPS is $<$ 0.7 meters.

2. Narrow-ped: Scenario with 8 pedestrians walking in the opposite direction to the robot’s motion in a narrow corridor, with an LPS of $<$ 1.5 meters.

3. Occluded-ped: Scenario with sharp turns with static obstacles and pedestrians that are occluded by walls. The LPS is $<$ 1 meter.

4. Dense-Ped: Scenario with 18 pedestrians in a corridor of width 6 meters. Pedestrians may walk together as pairs, which require a robot to make sharp turns in the presence of multiple dynamic obstacles. The LPS is $<$ 1 meter.

5. Circle: To test the generalization of our method for multi-robot collision avoidance, we make 4 robots move towards antipodal positions on a circle. The trajectories of the 4 robots is shown in Fig. 6.

We compare the benefits of our hybrid sensing method with three prior algorithms: (i) DWA Fox et al. (1997) which uses a lidar / proximity sensors to detect obstacles along with a global planner which requires a map of the environment.(ii) An implementation that uses a single depth camera that was trained using PPO and our reward functions; (iii) Fan et al. Long et al. (2017), the current state-of-the-art learning-based collision avoidance method for dense crowd navigation, which uses a single 2-D lidar for sensing. Its real-world implementation is shown in Fan et al. (2018a). Fan et al. Long et al. (2017) assumes that the pedestrians in dense scenarios would cooperate with the robot to avoid collisions. On the other hand, we assume that the robot takes the full responsibility to avoid collisions, though the pedestrians may or may not be cooperative. We use the following metrics to evaluate the performance of different navigation algorithms:

• •

  Success Rate - The number of times that the robot reached its goal without colliding with an obstacle over the total number of attempts.

• •

  Average Trajectory Length - The trajectory length traversed by the robot until the goal is reached, calculated as the sum of linear movement segments over small time intervals over the total number of attempts. In cases where the robot never reached the goal, we report the trajectory length until it collided or started oscillating indefinitely.

• •

  Mean Time - Average time taken to reach the goal over all attempts. If the goal is never reached in all attempts, we report the mean time until a collision or oscillation.

• •

  Average velocity - The average velocity of the robot until a collision occurs or the goal is reached over all attempts.

The results of our experiments and our ablation study to check the benefits of using multiple sensors (both lidar and depth camera) versus using one sensor (only depth camera) are shown in Table 1.
Comparison with DWA: DWA Fox et al. (1998) and CrowdSteer succeed in reaching the goal 100% of the times in static scenarios. However, as the number and density of dynamic agents in the environment increase, DWA’s success rate drops considerably when compared to CrowdSteer. This is due to the re-planning time it takes in the presence of dynamic obstacles or pedestrian. DWA performs well in the occluded-ped scenario due to its use of global map, which eliminates many occlusions. In cases where DWA reached the goal, it has more optimal trajectories and mean time due to the optimal global planner. CrowdSteer manages to have comparable performances without any global knowledge or planner.

Comparison with Fan et al. Long et al. (2017): The robot that uses Fan et al.’s method works well in the static scenario. However, while it manages to avoid collisions, it either gets stuck or starts oscillating indefinitely in scenarios with non-cooperative dynamic obstacles and occluded spaces. The values for the trajectory length, mean time and velocity reported when Fan et al’s method always failed, are until a collision or oscillation to give a sense of how much the robot traversed towards the goal before failing. These results are due to Fan et al.’s training, which is based on multi-agent collision avoidance where all the agents share the responsibility of avoiding collisions. This assumption may not always hold in dense crowds. CrowdSteer therefore manages to outperform Fan et al’s method in all scenarios.

Apart from having a much better success rate, CrowdSteer has similar performance as prior methods in terms of average velocities, better trajectory lengths and mean time to Fan et al. which in turn had better success rates, 41.6% lower time taken to reach the goal, and 14% higher average speeds than works like NH-ORCA Alonso-Mora et al. (2010). Therefore, our method outperforms the current state-of-the-art in dense and occluded scenarios.

Comparing Smoothness: We also compare the smoothness of the trajectories computed by CrowdSteer and Fan et al. Long et al. (2017) method (Figure 7) in the Occluded-Ped, and Narrow-static scenarios. CrowdSteer is trained to maintain a safe distance from obstacles and avoids oscillations. This results in significant improvement in the smoothness of the trajectories.

Ablation study for multi-sensor fusion: We compare the effects of using fused data from two sensors (CrowdSteer) versus a model which uses one sensor (Depth Camera) trained with our training scenarios and reward function in different scenarios. Using only a depth camera limits the robot’s sensing range and field of view. Due to this, the depth camera model does not have a 100% success rate in any scenario. This also reflects in the success rate drop in the Occluded-Ped and Dense-Ped benchmarks. However, it still manages to succeed in the dynamic testing scenarios due to our training scenarios and network architecture. Our CrowdSteer algorithm takes advantage of both the high accuracy of the lidar and the complex features extracted from the depth images and demonstrates significantly better success rates, lower mean time, and higher average velocities.

Ablation study of smoothness: We study the effect of our reward function on the robot’s trajectory smoothness. We trained two policies, one including the penalty for oscillations in the reward function (Eqn. 9), and the other without it. The average number of oscillations in the robot’s trajectory are summarized in Table 2. The models are evaluated in two scenarios: (i) Scenario without any obstacles, and the robot moves in a straight line for 10 meters towards the goal, (ii) Scenario where the robot must maneuver to avoid a static obstacle before reaching the goal. Empty/sparse scenarios are used so that turns during dynamic obstacle avoidance does not affect the number of oscillations. We observe that there is a significant reduction in the number of oscillations when the penalty is included.

Real-world scenarios: We use CrowdSteer to navigate a Turtlebot 2 and a Clearpath Jackal robot in crowds with varying densities ( 1-3 person/$m^{2}$), as shown in the video. The robots face high randomness in terms of the direction and velocities of pedestrians, which was not encountered during training. We compare the motion of CrowdSteer with Fan et al.Long et al. (2017) method in similar scenarios. Compared to Fan et al., we observe that CrowdSteer has smoother trajectories in both robots and avoids all collisions with the obstacles. On the other hand, Fan et al.Long et al. (2017) method to compute collision-free velocities are highly oscillatory. In occluded spaces such as corridors, our CrowdSteer algorithm was able to avoid sudden obstacles which appear in places such as T and L junctions. These tests show the advantages of implicit sensor fusion and our training scenarios with occlusions. Our real-world tests also demonstrate our method’s strong sim-to-real and generalization capabilities.

Failure Cases: CrowdSteer may not work well certain cases. In highly spacious regions, CrowdSteer could exhibit oscillatory behaviors. It might also fail for acute angled turns and in environments with reflective or transparent surfaces, and high interference from infrared light in the surroundings. In crowds with density ¿ 4 people/$m^{2}$ or scenarios with very minimal or narrow space for navigation, the robot may not find a collision-free path.