Learning Orientation Field for OSM-Guided Autonomous Navigation

In contrast to the trajectory planning works relying on High Definition (HD) maps for autonomous driving [15, 16, 17], recent works [7, 8, 9, 18, 10] attempted to accomplish the navigation task by only relying on topometric maps. For instance, [7] utilized semantic terrain information from 3D-LiDAR data and the Markov-Chain Monte Carlo technique to align the OSM road route with the actual road center lines to obtain local navigation waypoints. However, this approach is limited by the accuracy of terrain semantic segmentation and struggles in complex road networks. [8] located the center of the road by extracting the road edges and employs RANSAC [19] to fit the local trajectory. Similarly, [18] optimized the local waypoint by extracting the center line of the road as a reference. A common limitation of these methods is their inability to effectively integrate OSM data for generating local trajectories in scenarios with ambiguous road boundaries or complex intersections. [9] improved this situation by proposing a topometric map registration algorithm that approximates the maximum posterior estimate of the registration between the vehicle and the prior map, demonstrating its feasibility on CARLA road scenes [20] with complex topologies. To further enhance localization in the road network, ALT-Pilot [10] combined LiDAR and camera cues with semantic information from OSM maps, generating local trajectories using the Frenet planner [21].  [22] focused on segmenting precise free spaces with LiDAR and camera to establish reliable local trajectory targets. Unlike these methods, which rely on complex algorithmic rules, we propose a novel data-driven representation that directly encodes both LiDAR scans and OSM data without the need for explicit registration and segmentation, significantly enhancing performance across various scenarios.

End-to-end learning approaches have emerged as a powerful paradigm for trajectory planning in navigation, enabling models to learn directly from raw sensor inputs and generate trajectories without the need for explicit registration and segmentation. Early works utilized CNNs to model spatial and temporal dynamics [2], but these methods often failed with complex real-world scenarios such as intersections and occlusions. To address these challenges, subsequent methods incorporated attention mechanisms [23] and graph-based models [24, 25, 26] to better capture spatial-temporal dependencies. While these methods relied heavily on HDMaps for feature extraction [1, 3], Transfuser [27] and iPlanner [28] generated the path given the local target without relying on a predefined HDMap. Recently, there has been growing interest in utilizing simple map priors. Methods like multi-modal prediction [16, 15, 17, 29, 14] can be employed for this purpose as the task of trajectory prediction for the ego vehicle. [11, 12] adopted bird’s-eye-view (BEV) representations to encode navigation routes using rasterized maps. Tridentnetv2 [13] advanced these representations by integrating graphical information for route encoding. Learning from expert data with limited size and optimality, these methods converge toward sub-optimal solutions. [30] uses 60 hours of video data including trajectory information from multiple robots and environments to improve generalization. However, challenges such as generalization, explainability, and scalability persist, especially in rare or unseen conditions that have yet to be solved, which motivates us to propose a more explainable and robust framework to ensure accurate and reliable trajectory planning in complex environments.

Our OrField can provide an orientation vector at each grid of the 2D space, serving as guidance for robot navigation, shown by the orientation color on the ground in Fig. 2. The estimation starts with an initialization of the OrField. We begin by rasterizing the environment into bird’s eye view (BEV) grids. Each grid is assigned an orientation vector $\mathbf{n}=(n_{x},n_{y})$, where $n_{x}$ and $n_{y}$ denote the orientation components along the x- and y-axes, respectively, as illustrated in Fig. 3 (b1). The vector $\mathbf{n}$ defines the direction in which a robot is encouraged to move within the grid, and the length $\|\mathbf{n}\|_{2}\in[0,1]$ quantifies the strength of the movement. A higher value of $\|\mathbf{n}\|_{2}$ indicates a stronger preference for traversing that grid cell. In a local vicinity, the OrField can be utilized to plan a trajectory for the robot by iteratively following the suggested orientations. The OrField is thus an essential tool for guiding robot navigation over short distances, where the robot can optimize its trajectory by adhering to the directions encoded in the field.

To generate the initial OrField, we utilize the waypoints derived from the OSM’s GPS coordinates, as illustrated in Fig. 3 (i) and (ii). These waypoints are extracted from the topometric map of OSM in the sensor’s swath, and a local route can be formed by connecting these waypoints. However, these paths are generally coarse and non-smooth making the network training difficult. To facilitate the network training, we turn these routes into Bezier curves, shown as the dark dashed curves in Fig. 3 (ii), to produce a continuous and differentiable path. The endpoints of the Bezier curve coincide with the waypoints on the route and the control points are selected at the locations with the maximum offset from the auxiliary line connecting the two endpoints.

Once the Bezier curve is generated, the orientation of each grid cell in the OrField is initialized by the tangent at the closest point on the curve, as illustrated in Fig. 3 (b1). Since this OrField is derived solely from the noisy OSM coordinates and does not consider the actual free space and obstacles, it is referred to as the initial OrField. As a result, the initial OrField is inherently coarse and may lead to inaccuracies, particularly at intersections or sharp turns. Consequently, the orientation vectors in these areas are not always ideal for robot navigation. To mitigate these issues, an optimization scheme is essential for the initial OrField.

To optimize the initial OrField, we propose leveraging LiDAR to help with road navigation and the avoidance of obstacles. A straightforward approach using LiDAR is to estimate the free space from current LiDAR observation, followed by the registration with the OSM routes to address the limitations of relying solely on OSM data. However, this approach still faces several challenges. Discontinuities in free space estimation, caused by occlusions or wrong segmentation, can render difficult path generation because of incorrect understanding of road structure, as shown in Fig. 1 (b). Such issues often arise when large occlusions lead to incomplete sensor observations. Additionally, the registration process is inherently complex, adding additional difficulty to the optimization pipeline. To overcome these challenges, we optimize the initial OrField using a deep neural network that can accurately model the mappings between the initial OrField and the optimized OrField.

Based on the optimized OrField, we propose two field planners, Field-RRT* and Field-Bezier, respectively, to estimate the navigation trajectory.

Field-RRT*. Our approach is based on RRT*, an extension of the RRT algorithm, which finds a path to a target point by iteratively sampling new points, expanding the tree from these points, and eventually reaching the target through the tree’s leaves. Each time sampling a point, the algorithm reconnects the edges to the neighboring nodes, ensuring that the path is progressively shortened with each iteration. However, since the neighbor points are typically identified based on Euclidean distance, the resulting path may not align well with the OrField.

To address this issue, we propose Field RRT*, where the tree is grown under the constraint of the OrField. The demostration is shown in Fig. 6 (a). Specifically, when a new point is sampled, the algorithm selects the point with the lowest energy $E$ from the root to the sampled point (linked via the red dotted line). The total energy $E$ is the sum of the energy values $e$ of all grids along the path from the tree root to the sampled point. For each grid, $e$ is the area of the red region in the unit circle (as shown in the zoom-in of the grid in Fig. 6 (a)), determined by the difference between the orientation vector $\mathbf{n}$ (Section III-A) on that grid and the normalized orientation vector $\mathbf{v}\in\mathbb{R}^{2}$, $\|\mathbf{v}\|_{2}=1$, representing the direction of the edge crossing the grid. For the orientations optimized by our network, $\|\mathbf{n}\|_{2}$ is equal to $1$. We merge the orientation of each grid with its neighbors using a 2-dimensional uniform filter, producing a shortened $\mathbf{n}$ satisfying $\|\mathbf{n}\|_{2}\leq 1$ from which the inequality holds when the orientations of neighboring grids are inconsistent. After selecting the parent, the algorithm searches for neighboring nodes and attempts to reconnect them as children, ensuring that the total energy $E$ is minimized. As shown in Fig. 6 (a), the path generated by Field RRT* better aligns with the actual orientation, resulting in a path that adheres more closely to the desired trajectory.

To mimic the RRT-like algorithms, let us assume there exists a target point, which can be virtually a pseudo target floating in the sky. Given the definition of distance from the RRT leaves to the pseudo target (as illustrated in Fig. 6 (b)), the tree grows similarly to the RRT-like algorithms which find the path with the smallest distance from the root to the target point. Different from the existing RRT-like approaches, we find a path with the smallest deviation from the predefined OrField. In this way, the path is optimized over iterations. The algorithm terminates after a predefined number of iterations.

Field-Bezier. We propose Field-Bezier by introducing driving constraints to ease trajectory planning. Specifically, a batch of Bezier curves are generated from the vehicle to the points on the circle center at the vehicle with a radius equal to a given planning distance. The orientation of the start and end points of the Bezier curve are the same as in the given field. The first and second control points of the Bezier curve are generated by moving the start point forward along its direction and moving the endpoint backward against its direction for a fixed distance, respectively. The Bezier curve is generated by the start and end points, and the two control points. The points and tangents of the generated Bezier curves are sampled at the grids that are occupied by the curve. The energy of each generated Bezier curve is calculated in a similar way to the Field-RRT* algorithm. Finally, we choose the Bezier curve with the lowest energy as the planned trajectory. The comparison between Field-RRT* and Field-Bezier is shown in Fig. 6 (c).

At the frame level, we use the widely used metrics, ADE, FDE, and HitRate, for evaluating trajectory planning performance. The Average Displacement Error (ADE) measures the average difference between the planned trajectory and the ground truth trajectory generated from ground truth poses in datasets. It is defined as $\frac{1}{N}\sum_{i=1}^{N}\|p_{i}-\hat{p_{i}}\|_{2}$, where $p$ and $\hat{p}$ are the sampled points in the ground truth trajectory and planned trajectory. The Final Displacement Error (FDE) depicts the position difference between the last sampled points in the ground truth and the planned trajectory, respectively. It is defined as $\|p_{N}-\hat{p}_{N}\|_{2}$. The last sampled point can be used as the goal for local planning. The HitRate [17] $\in\{0,1\}$ is defined as whether the planned trajectory is hit. A hit happens if $\max_{i=1}^{N}\|p_{i}-\hat{p_{i}}\|_{2}<d$ where $d$ is a distance threshold. In addition, we measure the Coverage of the planned trajectory, defined as $\frac{1}{N}\sum_{i=1}^{N}(\|p_{i}-\hat{p_{i}}\|_{2}<d)$, which is a ”soft” HitRate describing the average hit over all sampled points along the trajectory. The sampling method is shown in Fig. 7 (a). Different sampling radii $R$ are used for comprehensive evaluations on trajectory performance. Finally, we compute the average of all these metrics over the entire trajectory.

At the trajectory level, the planned trajectory is temporally fused, as shown in Fig. 7 (b). $x_{t-2}$, $x_{t-1}$, and $x_{t}$ are the positions of the past two time steps and the current time step, respectively. Each time step provides a planned trajectory with a waypoint, i.e., $p_{t-2}$, $p_{t-1}$, $p_{t}$, to the same distance head $x_{t}$. We fuse these predictions according to a prior prediction variance (empirical setting) and obtain the fused waypoint $\hat{p}_{t}$ for $x_{t}$. We later plot the fused waypoints of all the time steps $t$ and generate the entire planned trajectory over the scene for visualization.

SemanticKITTI [33] and KITTI [35] are popular datasets for validating robotics and autonomous driving systems. Both datasets include LiDAR scans collected using the Velodyne-HDLE64 LiDAR. SemanticKITTI provides 22 sequences with semantic labels for each LiDAR point, with labels publicly available for sequences 00-10. We utilize KITTI for model training following [11] and evaluate performance in SemanticKITTI sequences. Both SemanticKITTI and KITTI include the ground truth poses, enabling the generation of ground truth trajectories for evaluation and straightforward implementation of relative pose estimation for point cloud aggregation. The step size, neighbor radius, and number of iterations of Field-RRT* are set to 1 m, 2 m, and 1000, respectively. The RRT tree is down-sampled half along the x- and y-axis for fast running.

To train our model, no OSM routes or manual driving trajectories are provided for supervision. The free space for training is estimated by the LiDAR segmentation method [31]. We compare our method with the two baselines. To train the end-to-end baseline model, authors in [11] obtained OSM routes from the OSM website as input and ground-truth driving trajectories from the dataset collected by manual driving as supervision. We directly obtain the checkpoint released by the authors. The segmentation baseline uses the free space estimated by the same method used for generating training samples for our model. Five SemanticKITTI sequences (sequences 08, 13, 15, 16, 18, and 19) are used for testing as they are in complex road topologies. For evaluation, the OSM routes are downloaded from the OSM website and registered with the ground-truth driving trajectories. During inference, these OSM routes are converted into initial OrField (Section III-A) for network input.

Table I presents the quantitative results, where the Field-RRT* and Field-Bezier planners based on the learned OrField outperform the others in most sequences. However, the end-to-end baseline achieves better for metric in distance error, i.e., ADE and FDE, when evaluate at $20$ meters in sequence 18 and evaluated at $10$ meters in sequences 16 and 18. This is because the end-to-end baseline trained the network overfits smooth driving, while sequences 16 and 18 are the two smooth driving sequences with the least frames categorized as sharp turns. This alignment between training and evaluation conditions results in stronger performance. Nonetheless, in real-world scenarios, where conditions are more diverse, an overfitted predictor may fail to generalize effectively.

The qualitative comparison is shown in Fig. 8. The plotted trajectories are generated by fusing outputs from each time step, based on past frames. We observe that the end-to-end baseline trained with OSM guidance, as well as the segmentation baseline relying on heuristic designs, does not perform well in complex scenarios (especially at turns), as shown in the zoom-in at each sequence of Fig. 8. In contrast, our method with Field-RRT* and Field-Bezier planners demonstrates superior understanding in both straight-road and intersection scenarios, thanks to unbiased training and independent trajectory planning. Notably, despite our network being trained without exposure to the realistic OSM route, i.e., the extent and structure of OSM routes are unknown during training, it generalizes effectively to real-world applications.

From Table I, we see that Field-Bezier outperforms Field-RRT* for most metrics in most sequences. This observation motivates us to investigate the benefits of the behavior priors introduced by the trajectory planner. Field-RRT* imposes no prior knowledge, estimating trajectories purely based on orientations, whereas Field-Beier enforces strong constraints, restricting trajectories to Bezier curves. To analyze whether incorporating behavior priors benefits trajectory estimation, we classify the frames into ”keep-straight” or ”turn” scenarios by examining the direction suggested by the OSM trajectory, as depicted in Fig. 9. The majority of the frames correspond to keep-straight, while turns are required at specific intersections. We evaluate the performance for these two scenarios separately to verify the strengths of the methods under different conditions. The evaluation results (Table II) reveal that Field-Beier performs better in straight-line scenarios, while Field-RRT* excels during turns. Since the metrics for turns reflect the ability to make correct road choices at intersections, we conclude that our method with Field-RRT* planner is more advantageous for robot navigation as it prioritizes successful traversal through intersections.

We created a campus dataset consisting of five scenes (scenes 1, 2, 3, 4, and 5) for both offline and online testing. The dataset includes LiDAR scans captured at $10$ Hz using a Livox Mid-360 LiDAR. Compared to the Velodyne HDL-64, this LiDAR produces significantly lower point density with shorter sensor range, making trajectory planning more challenging. The ground truth poses are estimated by the SLAM algorithms [36, 37, 38, 39]. To accommodate the new dataset, the step size, neighbor radius, and number of iterations of Field-RRT* are set to 0.75 m, 1.5 m, and 1000, respectively. The RRT tree is down-sampled half along the x- and y-axis for fast running.

Offline Testing. The experiment settings of offline testing are similar to the previous experiment on SemanticKITTI. Scenes 1 and 2 are used for training while the rest scenes are used for testing. To train our model, no OSM routes or manual driving trajectories are provided for supervision. The free space for training is estimated by a random forest classifier trained by LiDAR points from $30$ frames. To train the end-to-end baseline model, we obtained OSM routes from the OSM website as input and ground-truth driving trajectories collected by manual driving as supervision. The segmentation baseline uses the free space estimated by the same method used for generating training samples for our model. For evaluation, the OSM routes are downloaded from the OSM website and registered with the ground-truth driving trajectories. During inference, these OSM routes are converted into initial OrField (Section III-A) for network input. The results are shown in Table III from which we see that our method achieves supreme performance again across all the testing scenes for all the testing metrics. This shows that our method is also robust to low-density data and short sensor range.

Online Testing. To evaluate the methods in real-world applications, we conduct online experiments where trained models are tested on live data in scenes 3, 4, and 5, which cover large, medium, and small areas, respectively, as shown in Fig. 10. The experiments are conducted on campus using a ground vehicle equipped with an Intel NUC 11 computer. To assess online performance, we calculate the Coverage after the vehicle automatically traverses a scene and returns to the starting position. Specifically, we at first manually control the vehicle to perfectly traverse the scene and record the driving trajectory. By removing the trajectory points that duplicate in their nearby, we get a collection that contains the trajectory points defining the area covered by OSM routes. For each point in the collection, we define it as covered if the traversal trajectory of the automatic vehicle intersects the circle centered at the point with a radius of $r$ meters. Then we calculate the Coverage as the ratio of the number of the covered points to the number of all the points in the collection. Besides, we introduce a metric called Intervention, defined as the number of manual interventions required during the vehicle traversal. An intervention occurs when the vehicle fails during navigation and must be manually steered onto the correct track. Failures include getting stuck in road ditches or choosing infeasible branches at intersections when following the OSM trajectory, preventing successful navigation. The experiments were carried out three times for each scene at three different starting positions. For calculating Coverage, we set $r$ from $2$ meters to $5$ meters every $0.1$ meter and plot the result for each $r$. The results of average Coverage and Interventions are shown in Fig. 10. Our method demonstrates fewer interventions and achieves a larger coverage of the target area compared to the end-to-end baseline [11]. This is mainly because our approach excels in selecting the correct branches at intersections, even when the OSM routes significantly deviate from actual roads. Additionally, our method can execute turnarounds after encountering dead ends while following OSM routes. In contrast, the baseline struggles with turnarounds due to the absence of such behavior in its supervision. Furthermore, the baseline tends to predict smooth trajectories extrapolated from previous vehicle poses, which are often inconsistent with the precise trajectories required in narrow lanes. Figure 10 visualizes the trajectories and maps of the evaluated scenes, showcasing the advantages of our approach in real-world scenarios.

In this section, we do ablation studies to confirm the best one among different OrFields including those derived before and after optimization by our deep network. Meanwhile, we investigate the superiority of planning with orientation representation over the binary free space representation without the orientation information.

Before Network Optimization. The OrFields before optimization by our deep network are categorized into two types based on whether they incorporate sensor information. For the OrFields incorporating sensor information, we estimate the free space using environmental data collected from onboard sensors. Based on this free space estimation, orientations are calculated using the Dijkstra algorithm from the local target point, referred to as Dijkstra. Section III-B1 introduces how orientations for training samples are generated by operating on the gradient of the free space. Using the same method, orientations are calculated here and denoted as Gradient. To highlight the importance of future frames, we aggregate them with the past and current frames to estimate free space. Orientations are then generated using the Dijkstra and Gradient methods, denoted as Dijkstra-Full and Gradient-Full, respectively. These OrFields, which leverage future frames, are unavailable during real-world deployment. Thus, they serve as a reference upper bound for the performance of OrFields without deep network optimization.

Another approach is to estimate orientations solely based on OSM routes, without incorporating sensor information. For this, the orientation of a grid is assigned as the tangent of its nearest OSM route edge, calculated from the OSM topology. We denote this method as Nearest. Additionally, the initial OrField for network input in Section III-A, referred to as InitialOrField, is a smoothed version of orientations derived from the OSM topology.

Table IV highlights several findings: 1. The improvement of Gradient (Gr.) over Dijkstra (Di.) demonstrates the validity of calculating orientations from gradient directions, as discussed in Section III-B1. 2. Aggregating future frames reduces the loss of surrounding information. The improvement from Gradient (Gr.) to Gradient-Full (Gr.^′) illustrates this point and emphasizes the importance of full observations in trajectory planning. This further supports the rationale for designing a network to estimate orientations from partial observations. 3. Meanwhile, the underperformance of Nearest (Ne.) and InitialOrField (IOr.) shows the necessity of optimization on the orientations derived solely from OSM routes with the existence of noise.

OrField After Network Optimization. Following the evaluation of heuristically designed OrFields without network optimization, we analyze the performance of OrFields after optimization by our deep network, referred to as DeepOrField. If free space information is available, it can be integrated into DeepOrField by setting the orientation length in non-free space areas to zero, thereby discouraging traversal in those regions. This variant is denoted as FS-DeepOrField.

Table IV shows a clear performance gap between the orientations before and after optimization, demonstrating the effectiveness of our deep network. When comparing the optimized orientations, the integration of occupancy information in FS-DeepOrField (FS-DOr.) resulted in a degraded performance in most sequences compared to using all orientations from the network in DeepOrField (DOr.). This indicates our deep network provides valuable orientation information not only in the free space but also in the obstacle and unobserved area. Our optimized orientations are more accurate than the ”oracle” one Gradient-Full (Gr.^′) which sees the future frames.

Point Planner versus Field Planner. The point planner is a traditional RRT* planner that approaches a target point by expanding an exploration tree following the estimation of the free space and the selection of the local target in a step-by-step manner akin to [7, 8], denoted as Modular. Instead, our field planner expands the tree constrained by the orientations along the edge. We demonstrate the superiority of our field planner over the point planner by comparison.

The results are shown in Table IV from which we see that compared to the point planner (Mo.), our field planner (Gr.) which incorporates constraints on orientation achieves better performance when evaluated at distances of $20$ meters ahead. This aligns with the findings of previous studies [9, 10], which optimized trajectories using the Euclidean Distance Transform (EDT) to encourage paths that align with the road centerline. Meanwhile, this indicates that the field planner outperforms the traditional point planner in scenarios requiring planning over longer distances which are more susceptible to occlusion, encouraging us to design a network to estimate more accurate orientations both within and beyond the sensor range.

Based on these ablation studies, we conclude that our method significantly enhances orientation accuracy, even under partial observations and noisy OSM data. Examples of both successful and failure cases are illustrated in Fig. 11.

The inputs for the network consist of the LiDAR BEV, initial OrField, and distance map. To investigate the best chosen for the network inputs, we do ablation studies by removing the distance map and initial OrField, respectively. The network loses the position information about the OSM route without the distance map, while the network loses the orientation information about the OSM route without the initial OrField. The ablation results are shown in Table V from which we can see a clear improvement when using the initial OrField as network input. Although the distance map is enough to prompt the underlying direction for navigation, it is difficult to decide when the OSM route renders large uncertainties. Besides, the improvement when using the distance map, especially in sequences 08, 13, and 19 of large scale, indicates the usefulness of incorporating it because provides the confidence of position of the OSM route.

We analyze the robustness of our method that plans with OrField compared with the end-to-end trajectory planning method in different cases at the same intersection. Specifically, we generate different cases by rotating the OSM route from $0$ to $360$ degree every $6$ degree at a typical 4-branches intersection. In this way, $60$ cases are generated, in which OSM routes lead to different branches. An example of an original OSM route and the one rotated by $180$ degree are shown in the first and second rows of Fig. 12 (a), respectively. The paths planned by the end-to-end baseline [11] method are shown in Fig. 12 (b). We can see that the baseline predicts forward paths, while the OSM route indicates a left turn and a right turn. Instead, our method plans the correct path, as shown in Fig. 12 (d). For evaluation, we collect all the planned paths in all the cases with different rotated angles and plot them in Fig. 12 (e) and (f). We find the paths most forward for the baseline and are separated into different branches for our method. This means that the distribution of paths planned by our method goes beyond the biased distribution learned from manual driving. We calculate the percentage of the points of planned paths lying in the free space. As shown in Fig. 12 (e) and (f), the baseline plans many paths across the obstacles and only $82\%$ planned points lying in free space, while our method plans most paths lying in the free space and achieves $93\%$. We find similar conclusions in other intersections.