A Closed-Loop Perception, Decision-Making and Reasoning Mechanism for Human-Like Navigation

1 Introduction

Safe, reliable, and flexible navigation and obstacle avoidance strategies are essential for large-scale robotic and autonomous driving applications. In a complex scenario of human-robot coexistence, the agent needs to avoid highly dynamic obstacles and drive quickly to the target, which is fairly challenging. Most conventional algorithms[Fox et al., 1997] implement navigation through real-time path planning, which requires a high computational overhead.

Recent advances in deep learning have greatly improved perception capabilities and even surpassed human levels in many areas[He et al., 2016b; Vaswani et al., 2017]. But supervised learning paradigm, which relies on large amounts of training data, does not easily tackle complex navigation problems in highly dynamic scenarios. Navigation is inherently a complex sequential decision-making task, including global path planning and local obstacle avoidance. It is impractical to collect sufficient data for training. Obviously, Complex decision-making tasks in navigation and obstacle avoidance cannot be well solved by perception model alone.

* Contributed equally to this work. †Corresponding author.

Deep reinforcement learning(DRL)[Mnih et al., 2013; Sutton and Barto, 2018] has been widely studied and applied in many fields, including robot control[Yang et al., 2020; Fu et al., 2021], autonomous driving[Long et al., 2018], etc. The DRL-based approach shows great potential to build a reliable decision-making system. Currently, many researchers have applied DRL to navigation and obstacle avoidance[Faust et al., 2018; Zhang et al., 2021], achieving promising performance. Most of these works attempt to learn a precise policy from observations to actions during environment interactions. However, such one-way learning process is open-loop and lacks a deep understanding of the action. These open-loop methods perform well in trained scenarios, but may not be robust in general scenarios without understanding the intrinsic relations between internal states and output action. Besides, DRL-based methods are fragile and unstable when deployed to real scenarios, since the real world is dynamic, complex, and always changing.

However, for the person with visual impairment, we observe a reasoning process in their mind. They perceive the outside obstacles through their crutch and imagine the surroundings based on their motion when exploring an unfamiliar scene. The imaginary scenes constructed in their minds match the real scenes in reality, forming a closed loop. Inspired by this, we design a two-stage closed-loop mechanism with perception, decision-making, and reasoning components. As shown in Figure 1, the perception model is to compress observation into latent state distribution and the decision-making model converts the latent state into current action, just as a blind person uses a crutch to explore a new scene. Simultaneously the reasoning model deduces the most likely latent states based on the action sequences, like the person imagining the surrounding scene in their mind. This reasoning process constructs a closed-loop learning process by parsing the intrinsic relation between actions and the latent states, generating more reasonable actions than the open-loop system. Unlike conventional feedback control algorithms, our algorithm focuses on the closed loop of the learning process, eliminating the need to accurately model the kinematics and dynamics of the robot and the environment.

We assess our algorithm on two benchmarks that we design for evaluating navigation ability in few-shot and zero-shot scenes. Experiments demonstrate that our approach achieves significant improvement over various baselines, and is more reliable in novel scenarios. In addition, we deploy the algorithm to a real robot in a crowded building. Despite the significant gap between the simulator and the real world, the agent achieves autonomous obstacle avoidance without any human assistance. The main contributions of this work can be summarized as follows:

• We propose a perception, reasoning, and decisionmaking mechanism, which can learn more general rules and robust strategies through the reasoning model and latent state distribution.
• We introduce VAE-Enhanced Demonstration Learning to acquire basic navigation rules from data and design two dual-learning processes in RL-Enhanced Interaction Learning to promote collision avoidance ability by generating reward feedback for each other.
• Extensive experiments show our method achieves the most reliable results, surpassing the previous approaches in terms of safety and task accomplishment.

2 RelatedWork

2.1 Navigation and Planning

In recent years, autonomous navigation and flexible obstacle avoidance have attracted extensive attention. Conventional navigation approaches[Khatib, 1986] are usually implemented by global and local path planning[Fox et al.,

1997]. These approaches have trouble addressing navigation challenge in an unknown environment as they must be provided with the map in advance. In addition, many dynamic path planning and re-planning algorithms[Stentz and others, 1995; Koenig and Likhachev, 2002; Qi et al., 2021] can also achieve path planning in unknown dynamic environments, but the high computational overhead of these algorithms constrains real-time performance in large-scale dynamic environments. Besides, most of them require fine-tuning of parameter in real applications.

With the breakthrough of deep learning, many researchers have deployed supervised learning and reinforcement learning to achieve automatic obstacle avoidance[Chen et al., 2017]. But collecting abundant samples for training is another annoying issue, as manual annotation requires tremendous tedious labor. To overcome this limitation, some researchers attempt to adopt imitation learning[Ho and Ermon, 2016; Pfeiffer et al., 2018]. Imitation algorithm designs a teacher-student learning paradigm to mimic the teacher's action output and the intention behind it. Furthermore, some researches focus on modeling the environment. [Ha and Schmidhuber, 2018] adopted a variational autoencoder(VAE) to learn a compressed spatial and temporal representation of the environment. Although the above-mentioned studies have strikingly improved the performance of the navigation, most of them are based on learning an explicit mapping from perfect observations to deterministic actions. Such an assumption is impractical in a real scenario. These methods may have weak robustness and generalization, especially when confronted with novel scenarios.

2.2 Deep Reinforcement Learning

Deep Reinforcement Learning(DRL) is an optimization algorithm based on Markov Decision Process(MDP). Its optimization goal is to maximize the expected accumulated discount rewards[Sutton and Barto, 2018; Mnih et al., 2013]. Many researchers have applied DRL to traditional decisionmaking tasks and achieved remarkable results. [Tsounis et al., 2020] applied DRL to robot control and realized autonomous walking of a quadruped robot. [Long et al., 2018; Zhang et al., 2021] introduced DRL to solve path planning and obstacle avoidance in a dynamic environment. Some low-resource tasks[He et al., 2016a; Artetxe et al., 2017; Cao et al., 2020] can also be solved using dual DRL. These dual-DRL frameworks utilize the dual property of two tasks to enhance the model performance. However, DRL also has some inherent drawbacks that hinder its performance. One of the troubling challenges is commonly referred to as the "curse of dimensionality", which hinders the collision avoidance performance in large-scale real scenarios. The other is the poor generalizability and robustness when encountering complex and dynamic scene.

3 Approach

In this section, we first introduce the perception, reasoning, and decision-making components, and then describe in detail the design of VAE-Enhanced Demonstration Learning and RL-Enhanced Interaction Learning.

3.1 Model Definition

In previous approaches, the model directly maps external observations into actions [Long et al., 2018; Zhang et al., 2021]. In fact, these observations also contain lots of irrelevant information and measurement noise. We use ot to denote the current observation, at for current action, and st for current latent state based on observation. Assuming that st obeys a normal distribution Nt(s) and it represents the valid features extracted from ot. The perception, decision-making and reasoning models achieves the transition between ot, at, st. The perception and reasoning are two sequence encoders based on observations and actions, while decision-making is a simple decoder depending on the current latent state. This process is similar to human navigation that the perception and reasoning are based on long-term memory, while decision making is an instant subconscious response.

Perception component. The perception model is a sequence encoder P that maps observations sequences ot-n:t into the latent state distribution N P t , where ot-n:t = ot-n · · · ot denotes the sequences of observation, and latent state is sampled from latent state distribution, i.e. sPt ∼ N P t . The perception model extracts useful features from the observed sequences, and compresses them into the latent state, filtering out the irrelevant information.

Decision-Making component. The decision model is a direct mapping from latent states to actions, i.e. at = D (st).

Reasoning component. The reasoning model R is to deduce the latent state distribution N R t based on the action sequence at-n:t, where at-n:t = at-n · · · at represent the sequences of actions. Another latent state is sampled from this distribution, i.e. sRt ∼ N R t . This reasoning process is similar to a person imagining the surrounding scenes based on movements.

3.2 VAE-Enhanced Demonstration Learning

Using DRL to directly train navigation policy from scratch is inefficient since DRL usually encounters a cold start problem. We design a VAE-Enhanced phase to learn from data. This process is similar to imitation learning, where three models learn the general rules from the data. The overview of this process is visualized in Figure 2. It contains two channels:

• Prediction-Denoise-channel: decision-making (D) and perception model(P)
• Reconstruction-VAE-channel: decision-making(D) and reasoning model(R)

Prediction-Denoise-channel. Pair data{ot, at}t=1:N sampled from collected dataset (See section 4.1 for more detailed descriptions about dataset). It means that when ot is observed, the agent should take at. p(st|ot-n:t) means perception model and p(at|st) represents decision-making model. Latent state cannot be observed. The likelihood as follows:

The algorithm maximizes the log-likelihood of the data {ot, at}. So, we adopt a Prediction-Denoise-channel to achieve this objective. The perception model converts observation sequences ot-n:t into latent distribution N P t (s), and then the decision-making model converts latent state with sampling noise sPt to action at. The perception and decisionmaking models are more robust for predicting actions due to the presence of sampling noise in latent state. The predictionchannel can be formalized as

Reconstruction-VAE-channel. Inspired by [Kingma and Welling, 2014], we adopt a Reconstruction-VAE-channel to extract a continuous mapping for reasoning model. Assume that true distribution from action to latent state is p∗ (s|a), and the reasoning model R(st|at-n:t) achieve an estimate of this distribution. To minimize the KL divergence of the two distributions:

Where A means action sequence. The reasoning and decision-making models constitute a standard VAE process, which achieves the reconstruction of the action. Given an action sequence, reasoning model encodes at-n:t into latent state distribution N R t , i.e. µRt , σR t = MLPR(LSTMR(at-n:t)). Decision-making model samples another latent state sRt , and then reconstructs action at, denoted as areconst t =MLPD (sRt ) =MLPD (s ∼ N R t ).

Two channels share one decision-making model, achieving action prediction and reconstruction, respectively. The training objective can be formulated as

The total loss is L = ∑t (L1,t + L2,t + L3,t), where t = 1, 2 · · · K, K is the total number of samples and JS stands for Jensen-Shannon divergence. L1 represents the prediction loss and reconstruction loss for two channels. L2 constrains that the latent state distribution calculated by the perception and reasoning models should be similar. L3 is the regularization constraints two latent distributions to the standard normal distribution. The introduction of the VAE and two channels enhance the generalization and robustness of the three models, avoiding overfitting sparse data.

3.3 RL-Enhanced Interaction Learning

After VAE-Enhanced Demonstration Learning, the model is able to learn general collision avoidance rules. However, the limited training data still confines the performance of the algorithm in highly dynamic scenarios. We introduce the RLEnhanced Interaction Learning to enhance dynamic obstacle avoidance capability through two dual learning processes.

The perception and decision-making models explore the reasonable actions by interacting with the environment, while the reasoning model deduces the latent state distribution based on the action sequence. These two dual processes generate reward feedback for each other and collectively enhance their performance. The whole process is similar to a blind person carefully exploring the outdoor scene and always imagining the surrounding according to the movements.

As shown in Figure 3, the trained parameters from the first phase are used to initialize three models. A value network with random initialization and decision-making model form an Actor-Critic framework for proximal policy optimization(PPO)[Schulman et al., 2017] update.

First, the current ot and historical observation sequences ot-n:t-1 are concatenated together and fed into the perception model to calculate the mean µPt of the latent state distribution. Since policy module in Actor-Critic involves a sampling process, we directly treat µPt as the latent state st without sampling. Then decision-making model(policy module) predicts the action apred t based on st. After that, the environment executes the action apred t and returns navigation-reward Rnav t . Simultaneously, the reasoning model infers the most likely distribution of latent state N R t based on the current action apred t and the historical real actions areal t-n:t-1 . The probability of µPt inN R t is treated as a similarity-rewardRsim t for evaluating the quality of the at. This process can be formulated as

Optimization for perception and decision-making. The PPO is employed to optimize the perception and decisionmaking model via maximizing the expected cumulative reward(Rnav t + Rsim t ). The loss functions of perception and decision-making model are calculated as: LP,D = α ∗ Lpolicy + β ∗ Lvalue - η ∗ H, where H, Lpolicy , Lvalue represent the policy entropy, policy loss, and value loss in PPO update, respectively, and α, β, η are hyper-parameters.

Optimization for reasoning. In order to maintain the stability of the reasoning model, we design a CumulativeDiscount-Reward Weighed loss(DRW-Loss for short) mechanism to optimize the reasoning model asynchronously. The reasoning model is updated at a lower frequency than the PPO. Firstly, we collect the {µPt , apred t , Rnav t } generated by perception and decision-making model during the interaction. Then, we attempt to adopt the µPt and apred t as training samples to optimize the reasoning model. However, at the beginning of training, µPt and apred t are sub-optimal or even incorrect since the policy is not good enough. Such data should be discarded instead of being used to train the reasoning model, as it may cause the model to collapse. Therefore, we calculate the DRW-Loss to stabilize the training. Specifically, DRW-Loss use the navigation-reward Rnav t to calculate the discounted reward Rdiscount t . A large discount reward means corresponding action is more valuable. So we use the Rdiscount t to weight the mean squared error corresponding to the sample {µPt , apred t }:

where γ, λ are hyper-parameters. The reasoning model assesses the action generated by the perception and decisionmaking models through the reward Rsim t . Meanwhile, the perception and decision-making models produce more pair data {µPt , aPt } with high navigation-reward to promote the evolution of the reasoning model. Two dual learning processes are mutually reinforced by corresponding reward feedback, collectively improving performance.

4 Experiment

4.1 Experiment Setup

Scene benchmark. We adopt four simple scenarios to train the algorithm, including open scene, sparse scene, dense scene, and dynamic scene. To investigate obstacle avoidance and path-finding capabilities, we design two testing benchmarks. As shown in Figure 4, the first one is few-shot scene, which includes many scenarios similar to training phase. The second is the zero-shot scene, where the scenarios are quite different. We add noise to the observations and adjust the distribution of the goal-position, obstacle-shape, obstacle-speed, and obstacle-density to design zero-shot scene. Zero-shot scene benchmark is used to evaluate the robustness and generalization in new scenarios. We adopt a lightweight robot simulator ROS-Stage 1 to implement evaluation, which is more compatible with real robots than OpenAI Gym 2 .

Navigation setting. The goal point is randomly selected before navigation. The observation is defined as ot = [oLt ; oPt ; oVt ], which means the concatenation of the Lidar measurement oLt , target relative position oPt and robot velocity oVt . oPt is relative vectors between the robot's real-time position and the target. The action is defined as at = [vt; wt], representing the Forward and Angular velocity respectively. The observation, latent state and action are all continuous high-dimensional vector. Our reward function has two components, including navigation-reward (Rnav ) and similarityreward (Rsim). The Rnav is feedback from the simulator, and Rsim is calculated by the reasoning model. We design the reward function as follows:

1 http://wiki.ros.org/stage

2 http://gym.openai.com/

where P a and P g mean the position of the agent and goal, and ρ is a normalized hyperparameter. We set the hyperparameter ρ equal to the probability of a zero vector in a 90dimensional standard normal distribution.

Dateset collection. Since there is no publicly available navigation dataset, we adopt A∗ algorithm to plan a global path in a static scene for the first stage of pre-training. Actually, many reasonable strategies can be used to generate the dataset. We have tried alternative methods such as manual annotation, RRT* and finally chose A* as the optimized method for dataset generation. Then we use the Timed Elastic Band (TEB) algorithm to transform trajectory generated by A* into action sequences. The TEB considers the kinematics constraints and optimizes the local action velocity based on the global trajectory. Then, we record observation-action data and manually clean up some exception data. Finally, we collect 200 trajectories, each with about 300 pair data {ot, at}.

Baselines. We choose three types of algorithms for comparison. (1) Widely used robot open-source library Move-base 3 . (2) Learning-based navigation algorithms DM-RCA[Long et al., 2018] and MOE-VUCA[Zhang et al., 2021]. (3) DRL baseline algorithms PPO[Schulman et al., 2017] with the same configuration as ours. All comparison algorithms, except the Move-base algorithm are trained in the same scene and each benchmark is evaluated 400 times.

Metrics. The optimization target of the navigation is to avoid collision as much as possible and successfully reach the goal point oPt in a short time. To achieve this, we adopt two metrics (Success rate(%), Arriving step) to evaluate the performance as previous learning-based works (DMRCA,MOE-VUCA) did. Success rate is a comprehensive indicator of the capability to path-finding and collision avoidance and Arriving step assesses the navigation efficiency.

Implementation details. We adopt Microsoft's NNI for tuning parameters in the first stage. In the second stage, we have tested several sets of parameters in a small range and chose the best parameters, and the comparison algorithms also chose the best parameters in the same scene. We set the latent state to 90 dimensions, and the sequence length n is 20, and α, β, η, λ, γ are 1.0, 20.0, 5e-4, 0.01, 0.99. We discover that updating the reasoning model once every 10 PPO updates is more appropriate. Too frequent updates lead to unstable training, yet too slow updates bring performance degradation as the reasoning model evaluates the action and generates the reward. All models are implemented using PyTorch. We use optimizer Adam with a learning rate of 1e-3 in VAE-Enhanced Demonstration learning and 3e5 in RL-Enhanced Interaction learning. Video of real-robot experiments are shown at https://youtu.be/jD 7sCdMMWk. Datasets and part of the code will be released at https://github. com/zwq2018/CL PDR NAV.

4.2 Results and Analysis

As Figure 5, compared to other methods in few-shot scenes, our algorithm achieves comparable performance as others in most scenarios, except for Move-base. However, Move-base requires an environmental map in advance.

As shown in Table 1, the performance of all the methods in zero-shot scene is significantly degraded. Obviously, environmental distribution shift does have a dramatic impact, and our method is more robust and generalizable than others. In highly dynamic scenes like S6-Obstacle change(SC6 for short), Move-base is almost impossible to avoid the obstacle (10% in SC4 and 15% in SC6), but our algorithm achieves the best results(56% and 83%). In complicated maze scenes, our algorithm lead over the runner-up (DM-RCA) by a large margin (9.6% in SC1 and 2.3% in SC2). Additionally, SC8 simulates a complex traffic roundabout, and our algorithm is only inferior to PPO-Pretrained method by a small margin(74.5% compared to 79.75%).

3 http://wiki.ros.org/move base

MOE-VUCA performs the worst among all methods in zero-shot scenes, although it has the shortest Arriving step. We guess it suffers from poor generalization due to the parameter fusion trick it used. DM-RCA is a pretty simple but effective model that greatly outperforms other sophisticated models except ours. It reveals that more sophisticated networks may suffer from the challenge of poor generalization. In addition, we investigate the effect of pre-training using our collected data. For the comparison algorithms, the performance after pre-training with our collected data (denoted by *) does not show a significant improvement over training from scratch. We also observe that the Arriving step of ours is much longer than other algorithms. We suspect that the reasoning model encourages the agent to generate more conservative actions for higher similarity-reward Rsim t , resulting in a longer navigation step. A detailed analysis is provided in Section 4.4.

4.3 Ablation Study

We analyze the contribution of different components in our algorithm. We design three variants of the algorithm: (1)We remove the reasoning model and then train the algorithm using two-stage phases. (2)We replace the DRW-Loss with the mean square error when updating the reasoning model. (3)Without VAE-Enhanced Demonstration Learning phase. Table 2 illustrates that the reasoning model is essential to our algorithm. The introduction of DRW-Loss and VAEEnhanced Demonstration Learning can facilitate the learning of the reasoning model.

4.4 Analysis of the ReasoningModel

We investigate the impact of the reasoning model on actions. The outputs of the decision-making model include Forward Velocity v and Angular Velocity w. Firstly, we collect the w and v of several methods in the same scenario and analyze the corresponding behavior patterns. In Figure 6(a), the action(v) distribution output by DM-RCA are clustered at a larger velocity value(1m/s), forming a uni-modal distribution. In contrast, our actions form a bi-modal distribution(0m/s and 1m/s). Thus our strategy is more conservative, preferring to brake rather than bypass when encountering an obstacle. Obviously, comparison policies are more dangerous than ours. It shows that the reasoning model makes the system safer by constraining the action output, albeit sacrificing navigation efficiency. Secondly, we analyze the relationship between the high-dimensional latent vectors sR and the action aw in Figure 6(b). It visualizes the distribution of latent states after dimension reduction. The 90-dim continuous latent states sR are calculated by reasoning model from continuous action sequences at-n:t. These latent vectors are clustered in different regions depending on the action before. It reveals that reasoning model maps similar actions to neighboring regions in latent space, and different actions are projected to distinct. It exhibits a clustering-like effect.

4.5 Real-World Experiment

We deploy the algorithm on a wheel robot. As shown in Figure 7, the robot navigates in a crowded and complex building with moving staff. We randomly select goals and the robot navigates to the targets one by one. The robot is equipped with a 16-line 3D-LIDAR (Velodyne-16), a depth camera (Real-Sense D435) and an edge computing device (NVIDIA Jetson AGX Xavier with Ubuntu and ROS Melodic). All computational processes are performed onboard. Despite the considerable discrepancy between the training scenes and the real scenes, the robot successfully achieve navigation without human help. It means our closed-loop reasoning mechanism can successfully transfer from a simple simulator to a real robot, due to the good adaptability of our approach.

5 Conclusion

In this work, we introduce a reasoning process to create an inverse mapping from the output action to the latent state, forming a closed loop with the perception and decision-making process. The reasoning model closes the learning processes of the perception and decision-making and implicitly facilitates the whole system to develop safer and more reasonable collision avoidance behaviors. Experiments have shown that our algorithm is more generalizable and robust in the novel scenario, surpassing the previous approaches in terms of safety and task accomplishment. Real-world robot navigation testing also confirms that its capability being deployed to the real scenes with minimal cost.

Acknowledgments

This work is supported by the Key Research and Development Program of Zhejiang Province, China (No. 2021C01013), the National Key Research and Development Project of China (No. 2018AAA0101900), CKCEST, and MOE Engineering Research Center of Digital Library.