PathRL: An End-to-End Path Generation Method for Collision Avoidance via Deep Reinforcement Learning

In our formulation, a state $s_{t}=(m_{t},g_{t})$ at time step $t$ consists of two parts: an egocentric costmap $m_{t}$ and a relative target pose $g_{t}$.

The egocentric costmap¹¹1http://wiki.ros.org/costmap_2d is a $84\times 84$ grid map that is generated by a 3D laser sensor with $180^{\circ}$ field of view and presents information about the environment around the robot, including the shape of the robot and the observable appearances of various obstacles.

The relative target pose $g_{t}=(x^{g}_{t},y^{g}_{t},\alpha^{g}_{t})$ of the robot includes the relative position of the navigation target point $(x^{g}_{t},y^{g}_{t})$ and the relative target orientation $\alpha^{g}_{t}$ of the robot w.r.t. the current global pose $(x_{t},y_{t},\alpha_{t})$ of the robot at time $t$, i.e., this relative target pose $g_{t}$ is the local pose for the local coordinate system when the pose of the robot is identified as the origin.

In pathRL, the input of the DRL network is three consecutive path-based frames. These three frames contain the sensor information and relative target poses of the robot at the poses of $(x_{t}^{-2},y_{t}^{-2},\alpha_{t}^{-2})$, $(x_{t}^{-1},y_{t}^{-1},\alpha_{t}^{-1})$, and $(x_{t},y_{t},\alpha_{t})$, respectively. Notice that, we define the two nearest consecutive paths that the robot must follow to reach its current pose $(x_{t},y_{t},\alpha_{t})$ as $P^{-1}_{t}$ and $P^{-2}_{t}$, respectively. We use $(x_{t}^{-1},y_{t}^{-1},\alpha_{t}^{-1})$ and $(x_{t}^{-2},y_{t}^{-2},\alpha_{t}^{-2})$ to denote the starting poses of the robot for these two paths, respectively.

As shown in Fig. 2, we can specify the input of the DRL policy, i.e., three consecutive path-based frames, as $\vec{\boldsymbol{s}}_{t}=(s_{t-(\tau_{1}+\tau_{2})},s_{t-\tau_{1}},s_{t})$, where $s_{t-(\tau_{1}+\tau_{2})}=(m_{t}^{-2},g_{t}^{-2})$ denotes an egocentric costmap and a relative target pose w.r.t. the pose $(x_{t}^{-2},y_{t}^{-2},\alpha_{t}^{-2})$ of the robot, $s_{t-\tau_{1}}=(m_{t}^{-1},g_{t}^{-1})$ w.r.t. the pose $(x_{t}^{-1},y_{t}^{-1},\alpha_{t}^{-1})$, and $s_{t}=(m_{t},g_{t})$ w.r.t. the pose $(x_{t},y_{t},\alpha_{t})$. We use $\tau_{1}$ to denote the amount of the time steps for following the path $P^{-1}_{t}$ and $\tau_{2}$ to denote the amount of the time steps for following the path $P^{-2}_{t}$.

In this paper, we intend to train a DRL policy that outputs navigation paths directly. In our formulation, we use Bézier curve to fit the navigation path via the current position of the robot and other $n$ control points generated by the DRL policy. As shown in Fig. 1, the action space is the possible control points that can be selected in the rectangular area in front of the robot, which is high-dimensional. Therefore, to reduce the dimension of the action space and the training difficulty, we discretize the selection of longitudinal coordinate values for these $n$ control points.

In specific, the $n$ control points are identified as $(l_{t}^{1},h_{t}^{1})$, $(l_{t}^{2},h_{t}^{2})$, $\ldots$, $(l_{t}^{n},h_{t}^{n})$, where the point $(l,h)$ denotes the local longitudinal coordinate $l$ and the local horizontal coordinate $h$ when the current pose $(x_{t},y_{t},\alpha_{t})$ of the robot is considered as the origin of the local coordinates. We assign a fixed interval between two adjacent longitudinal coordinates, i.e., $l_{t}^{i+1}-l_{t}^{i}=l_{t}^{1}=d$ for $1\leq i<n$. In our experiments, we set $n=3$ and $d=0.4/3m$, then $l_{t}^{1}$, $l_{t}^{2}$, and $l_{t}^{3}$ are assigned to be $0.4/3m$, $0.8/3m$, and $0.4m$, respectively.

Notice that, the previous path $P_{t}^{-1}$ is specified by the starting pose $(x_{t-\tau},y_{t-\tau},\alpha_{t-\tau})$ of the robot and $n$ local control points $(l_{t-\tau}^{1},h_{t-\tau}^{1})$, $\ldots$, $(l_{t-\tau}^{n},h_{t-\tau}^{n})$, where $\tau$ is the amount of the time steps for following the path $P_{t}^{-1}$. Particularly, the current position $(x_{t},y_{t})$ of the robot at time $t$ is the ending position of the robot for the previous path $P_{t}^{-1}$, then $(x_{t},y_{t})$ is equivalent to the global position for the last control point $(l_{t-\tau}^{n},h_{t-\tau}^{n})$ of $P_{t}^{-1}$, i.e.,

The DRL policy aims to maximize the cumulative reward. In robot navigation tasks, the primary goal of the robot is to reach the target without collision within a limited time. Based on this goal, the robot’s trajectory is further required to be smoother. Therefore, the reward function of the algorithm is defined as follows:

where $r_{t}$ is the sum of four parts, $r_{t}^{\textit{goal}}$, $r_{t}^{\textit{safe}}$, $r_{t}^{\textit{curvature}}$ and $r_{t}^{\textit{straight}}$.

In particular, $r_{t}^{\textit{goal}}$ is the reward when the robot reaches the local target:

$r_{t}^{\textit{safe}}$ specifies the penalty when the robot encounters a collision:

We use the $r_{t}^{\textit{curvature}}$ to encourage the DRL algorithm to output paths with less curvature:

where $N$ is the total number of points after the discretization of the path, $\varepsilon_{1}$ is a hyper-parameter and $k(x)$ is the curvature of the path at the $i$th point. The following formula defines $k(x)$:

where $B(x)$ means Quadratic Bézier curve function. The intuition behind the definition of $r_{t}^{\textit{curvature}}$ is that the larger the curvature of points along the path, the more unstable the robot’s motion becomes, and thus the penalty should be greater.

The purpose of $r_{t}^{\textit{straight}}$ is to encourage the DRL algorithm to generate paths that maximize alignment with the direction of the vehicle body. This strategic approach ensures the smooth transition between adjacent paths, thereby enhancing the smoothness of the robot’s movement trajectory.

where $\varepsilon_{2}$ is a hyper-parameter, $L_{min}$ is the length of the shortest path, which is related to the action space dimension and the distance between the path points, and $L$ is the length of the path output by the DRL algorithm. The intuition behind the definition of $r_{t}^{\textit{straight}}$ is to avoid sharp angles between adjacent paths. In our experiments, we set $r_{arr}=500$, $r_{col}=-700$, $N=100$, $\varepsilon_{1}=0.4$ and $\varepsilon_{2}=200$.

Table I shows that PathRL outperforms the other DRL methods by a considerable improvement of “Average curvature (CUR)” without sacrificing “Success rate (SUCC)”. In dynamic environments, we find out that both “Average length (LEN)” and “Average time (TIME)” of PathRL are slightly larger than that of other DRL methods, as PathRL is required to drive the robot to avoid dynamic obstacles stably and smoothly, which is preferred for most real-world applications. Notice that, compared to PathRL, PSD has additional pedestrian map input, resulting in a higher success rate in pedestrian scenarios.

Notice that, additional smoothing requirements are preferred for an Ackermann steering robot. We use the average changes of the steering angle at each step, i.e. $\Delta\theta=\theta_{t+1}-\theta_{t}$, as a metric to assess the smoothness and comfort of the robot’s movement.

Table II summarizes the results of the $\Delta\theta$ metric for three reinforcement learning methods. Since PathRL directly outputs the path and decouples the motion control module from the DRL policy, it achieves a dramatic improvement of the stability of the steering angle for the robot comparing with other DRL methods.

In our setting, the DRL navigation policies are trained using the experiences collected in various simulation environments. Then it is crucial for these policies to be robust in the presence of the differences between the simulation model and the real-world robot platform. Notice that, DRL-based navigation methods that directly commands the robot with low-level controls are sensitive to parameters of the robot platform, which limits the generality of methods. Meanwhile, PathRL directly outputs the path and decouples the motion control module from the DRL policy. Then, sophisticated velocity-planning and path-following controllers, whose parameters are specified according to the real-world robot platform, can be applied for the robot to follow the paths generated by the DRL policy. In the following, we consider the impact for the navigation performance for different DRL-based navigation methods when varying the maximum driving speed of the robot platform.

In our experiment, we respectively apply the four DRL methods to train corresponding navigation policies in the same simulation environments, where the maximum moving speed of the robot is 0.2 m/s. Then we test the performance of these navigation policies in both static and multi-agent scenarios when the maximum moving speed is increased to 0.4 m/s, 0.6 m/s, and 0.8 m/s, respectively. The experimental results are summarized in Figure 5, where “x-static” and “x-magent’ denote the performance of the trained navigation policy using the DRL method “x” tested in static and mullti-agent scenarios, respectively. Comparing with other DRL methods, the performance of PathRL is more robust when the maximum moving speed is increased, which maintains the stable success rate and average curvature of moving trajectories.

Additionally, we compare the performance of PathRL with TEB algorithm in 3D gazebo environments. Notice that, the DRL navigation policy is trained in previous training environments using PathRL, which is only tested in gazebo environments here and We make every effort to fine-tune the parameters of the TEB algorithm to achieve improved performance. We test PathRL and TEB in environments of two scenarios, i.e., static scenario and dynamic scenario. As illustrated in Fig. 6, the testing environments of the static scenario is equipped with 8 possible routes (i.e., global paths) with different origins and destinations surrounding with static obstacles. The testing environments of the dynamic scenario contains 4 different routes surrounding with 25–35 pedestrians which are guided by the social forces model [17, 26, 27]. Table III summarizes the performance of PathRL and TEB in testing gazebo environments, where each method is tested in the same environment for five times. In addition, we use “NCOLL” to denote the average number of collisions during the execution of the policy or algorithm in testing environments.

The input of DRL navigation position consists three consecutive state frames. In PathRL, there are two alternative considerations of “consecutive”, i.e., $i$) sensor-based three consecutive frames, where $\vec{\boldsymbol{s}}_{t}=(s_{t-2},s_{t-1},s_{t})$; $ii$) path-based three consecutive frames, where $\vec{\boldsymbol{s}}_{t}=(s_{t-(\tau_{1}+\tau_{2})},s_{t-\tau_{1}},s_{t})$. Notice that, path-based consecutive frames have greater time intervals. The path-based frames introduces information for a longer span of time, which can benefit robot collision avoidance in dynamic scenarios and ensure the smooth transition between adjacent paths. To evaluate the advantage of path-based consecutive frames, we consider following ablation experiments. Specifically, we compare the performance of our design with sensor-based three consecutive frames in a dynamic scenario. As shown in Table IV, the design of path-based three consecutive frames achieves a higher success rate and smoother driving trajectory than the design of sensor-based three consecutive frames, with slight larger values of “LEN” and “TIME”.

Our experiments show that the $r_{straight}$ term in the reward function plays an important role in improving the experimental results and guaranteeing the smooth transition. On the one hand, without the $r_{straight}$ constraint, the robot would twist during its movement, resulting in an uneven driving trajectory. On the other hand, the inclusion of the $r_{straight}$ term also results a significant improvement of the navigation performance. Table V summarizes the ablation study.