{'fraction_non_alphanumeric': 0.027185278126306986, 'fraction_numerical': 0.008699289000418236, 'mean_word_length': 4.632744405182568, 'pattern_counts': {'":': 0, '<': 0, '<?xml version=': 0, '>': 1, 'https://': 0, 'lorem ipsum': 0, 'www.': 0, 'xml': 0}, 'pii_count': 0, 'substrings_counts': 0, 'word_list_counts': {'cursed_substrings.json': 0, 'profanity_word_list.json': 0, 'sexual_word_list.json': 0, 'zh_pornsignals.json': 0}}
RAMP: A Risk-Aware Mapping and Planning Pipeline for Fast Off-Road Ground Robot Navigation

A key challenge in fast ground robot navigation in 3D terrain is balancing robot speed and safety. Recent work has shown that 2.5D maps (2D representations with additional 3D information) are ideal for real-time safe and fast planning. However, raytracing as a prevalent method of generating occupancy grid as the base 2D representation makes the generated map unsafe to plan in, due to inaccurate representation of unknown space. Additionally, existing planners such as MPPI do not reason about speeds in known free and unknown space separately, leading to slow plans. This work therefore first presents ground point inflation as a way to generate accurate occupancy grid maps from classified pointclouds. Then we present an MPPI-based planner with embedded variability in horizon, to maximize speed in known free space while retaining cautionary penetration into unknown space. Finally, we integrate this mapping and planning pipeline with risk constraints arising from 3D terrain, and verify that it enables fast and safe navigation using simulations and a hardware demonstration.


I. INTRODUCTION
Ground robot navigation in outdoor 3D terrain is a longstanding area of research, with challenges compounded by the need for high speed as mobile robot platforms become increasingly capable. Some emergent time-critical applications include planetary exploration [1]- [3], search-andrescue missions [4], and combat missions [5]. This paper aims to develop fast off-road navigation for ground robots on unmapped 3D terrain, as shown in Fig. 1, by presenting a 2.5D local planning pipeline, as shown in Fig. 2. Variable horizon path planning for fast navigation: A common representation of the environment for robot navigation involves classifying nearby space as known-free, knownoccupied, or unknown regions, either in 2D (e.g., occupancy grid mapping [6], more), 2.5D (e.g., costmap2d (Robot Operating System, ROS plugin), multi-layer grid map [7], other papers that use digital elevation maps) or 3D (e.g., octomap [8], surfel representation [9], pointcloud map [10], signed distance field [11]). For vehicles constrained to the ground, a 2.5D representation provides richer information than a purely 2D representation while maintaining computational tractability. (e.g., occupancy grid map [6], octomap [8]). As discussed in [12], a key challenge is how the planner should interpret this representation. Safety-guaranteed path planning methods [12]- [15] heavily penalize entering unknown space, which can lead to conservative plans and unnecessarily low robot speed. Alternatively, treating unknown space as free  allows for faster navigation, but could cause dangerous maneuvers resulting from high-speed unknown space penetration (such as being unable to stop before unexpected obstacles or getting airtime from a slope). It is therefore essential to bridge the gap between high robot speed and safe plans into unknown space.
Some works attempt to address this by penalizing high speed in unknown space [16]. Yet, these works do not incentivize moving quickly in free space, leading to slow average speeds over the mission duration and undesired plan curvature due to fixed planning horizons. To alleviate this issue, we present a planner cost function with horizon variability to generate fast, unknown space-aware trajectories in real-time. Our cost function allows for variable horizons [17] not just for the terminal time to reach the goal but also for the time of each known to unknown space transition. For real-time computation, our implementation builds on model predictive path integral (MPPI) [16], [18], where sampling and parallel optimization are performed. Ground point inflation for occupancy grid generation: Since a ground robot is constrained to move on the ground plane, we make the choice to plan on a 2D map using 2D robot dynamics to save time on map access (direct for a 2D array vs. tree traversal for octomap [8]) and MPPI rollout state inference. A major challenge in using 2D maps for 3D environments [19] is that the conventional method of raytracing used to project LiDAR information onto an occupancy grid does not check for ground plane support. This leads to a map where not all known free space is necessarily traversable, thereby feeding false information to the planner. To have an accurate and efficient known vs. unknown space classification of the environment, we present ground point inflation as a direct way of generating a ground-plane-aware occupancy grid from semantically classified point clouds. Planning constraints imposed by 3D terrain: A fully 2D  mapping/planning pipeline like the one described so far is incapable of dealing with risks arising directly from the 3D nature of the problem. These risks include the robot tipping over due to turning on a slope, climbing a steep slope, or taking tight turns at high speed. To round out our variable horizon planner, we add constraints for all three of these specific maneuvers, using information such as surface normal components and slope of the 3D terrain by employing the GridMap [7] library. This makes the pipeline akin to 2.5D by incorporating 3D constraints in a 2D planner.

A. Contributions
While navigating off-road terrains requires accurate 3D environment knowledge that is expensive to build and query, the desire to move at high speeds demands a high planning rate such that a robot can quickly react to unexpected hazards due to occlusion. These conflicting requirements thus necessitate a new map representation that requires less computation than existing 3D mapping techniques (e.g., octomap) and provides a reliable classification for occupied, known, and unknown space. To this end, this work proposes a new 3D-aware 2D map representation based on lidar point inflation for ground vehicles that are not susceptible to the pitfalls of existing 2D approaches based on ray-tracing, which can erroneously classify unknown space as known (see Fig. 3), while retaining the computational advantage of a 2D map structure. To address the effect of sparse ground points (thus sparse known cells), we design a persistent memory that fuses historic occupancy information to create a dense occupancy map desirable for path planning algorithms. Lastly, given reliable knowledge about the unknown space, we propose a novel risk-aware planner capable of planning high-speed maneuvers in known space and adjustable speed in the unknown space based on specified risk-level. In representative high-speed navigation tasks over a hill, we show that the proposed map representation and risk-aware planner are both indispensable for achieving fast and safe navigation.
In summary, our contributions include: • A novel 3D-aware 2D map representation based on lidar point inflation that can accurately assess the 3D occupancy information that's relevant for driving (thus the risk induced by wrongly classified known space), while retaining the computational advantage of a 2D map structure; • A risk-aware planner that uses variable time-horizon cost formulation to achieve high-speed navigation within known space and low speed in unknown space, where the speed in the unknown is adjustable based on risk-level; • A demonstration of our approach with proposed riskaware planner and map representation simultaneously achieving higher success rate and time-to-goal than approaches with existing 2D mapping techniques and planners in a high-fidelty Unity simulator.
The organization of this paper is as follows. Section II presents the ground point inflation procedure for populating the occupancy grid, along with the necessary map memory for information retention. Section III presents our variable horizon path planning for fast navigation. Section IV presents the details of the three kinds of 3D constraints imposed on the 2D planner. Section V demonstrates our framework via simulations and a hardware demonstration. Section VI concludes this paper and addresses our future work.

II. GROUND POINT INFLATION AND MAP MEMORY
Generating an occupancy grid for the planner from LiDAR data is the first step involved in our pipeline. Given a pointcloud classifier capable of labeling points as navigable (referred to as ground) or not (referred to as obstacle), all points classified as ground form the basis of ground plane support for the robot. Known free space in the 2D occupancy grid is then defined as the collection of circles of a fixed inflation radius around each ground point. This allows for a more accurate representation of unknown space, as regions without ground support information are not cleared as free.
The point cloud classifier used here is a simple geometric one that uses sharp increases in feature height as criteria for defining obstacles. However, several other methods exist that estimate semantic properties within the environment, such as Cylinder3D [19] and SalsaNet [20]. However, simply using this inflated ground point representation in its raw form also presents the following issues: a) Map sparsity at long range: Due to finite sensor resolution, the distance between LiDAR beams increases with increasing distance from the robot. When this distance between neighboring beams (and therefore hits) exceeds the ground point inflation radius, known free space is no longer continuous, and the map generated from a single LiDAR scan, therefore, becomes sparse at long range.
b) Tilt-induced map amnesia: Due to the uneven terrain of interest, any amount of robot tilt induces a nonuniform spread of LiDAR points. This leads to large parts of the map being classified as unknown space, which had previously been classified as known before the tilt. For large tilt values, entire sectors of the map around the robot might be classified as unknown space, even though they were known to be free before the tilt. For risk-aware planning into unknown space, here represent LiDAR hits classified as ground, light gray represents known free space, greenish-gray represents unknown space, and the objects with a magenta to cyan gradient represent known occupied space). The rocks on the hilltop and the surface of the hilltop itself aren't detectable by LiDAR, but raytracing erroneously clears it as known free space. Meanwhile, memory is essential to have a usable map for the planner, such that the robot is not surrounded by unknown space.
it is not ideal to have such an inaccurate classification of known space as unknown. c) LiDAR shadow: The 3D LiDAR sensors commonly used on robots today have a limited vertical field of view, so depending on sensor height, the closest LiDAR hits might not be in the immediate neighborhood of the robot. Additionally, some of the closest beams might have to be canceled out in pointcloud preprocessing to remove LiDAR self-hits. This creates a region in the immediate vicinity of the robot that has no LiDAR hits and is defined here as LiDAR shadow. Since there are no points to be classified, it translates to a ring of unknown space around the robot in the occupancy grid.
We address all these issues by implementing persistent map memory: filling in patches of unknown space as the robot moves based on information about each patch in prior maps. This is done by first aggregating the raw maps from the last t seconds, where t can vary. Next, any unknown space in the map is filled in with information about that location in the processed final map from the previous iteration of this memory pipeline: if it was previously known to be free or an obstacle, it is classified as such, otherwise, it is left as unknown space. This generates a much more densely populated and usable map for planning, as shown in Fig. 3.

III. VARIABLE HORIZON COST FUNCTION
This section presents a variable-horizon optimal control formulation, which builds on [16]. We consider a state trajectory solving where x t ∈ R n is a state at time t, and u t ∈ R m is a control input at t. The state represents the robot's position and angle, and the control represents each wheel's velocity, where our robot has four wheels. We use x 0:T to denote a state trajectory from 0 to T . We design a variable-horizon problem that enables the vehicle to move fast in known free space, slow down cautiously in unknown space, and prioritize the goal approach.
where α(> 0), β ∈ [0.5, 1], and γ ∈ [0, 1] are tunable coefficients, m is the number of the state trajectory's transitions from known to unknown space within the planning horizon T , t i is i-th transition time of the trajectory from known to unknown space, t i is i-th transition time of the trajectory from unknown to known space, TTR represents the time-to-reach within the planning horizon T TTR(x 0:T ) = min T, min where 'Goal' is the navigation goal, 1(Condition) is an indicator function for the 'Condition' and φ is the cost-to-go Here, p goal and p t are the positions of the goal and the robot at time t respectively, and s default is the post-rollout speed used as a heuristic for the time-to-go. (s default is usually set to a low number to encourage goal approach) Fig. 4 visually explains the notation t i , t i , and m.
In the first term, t i − t i−1 enables the vehicle to minimize travel time in free space (a source of horizon variability), and φ(x ti ) prevents the vehicle from greedily jumping to the nearest patch, instead pushing it to remain close to the goal at transition time. The second term results in the terminal state of the plan tending to approach the goal while minimizing the amount of time it takes to do so using TTR explicitly (another source of horizon variability). The third term prevents high  Overall, this cost function is able to provide us with the edge needed over default MPPI implementations due to the variability in goal and unknown space transition horizons, combined with unknown space speed cost.

IV. RISK CONSTRAINTS
Finally, as shown in in Fig. 5, three kinds of risks need to be accounted for by constraining the cost function: 1) Steep slopes: Since the input costmap to the planner is 2D, there needs to be an explicit constraint to prevent the robot from climbing slopes that are too steep to be traversed safely. (Normally, these would be captured as obstacles by the LiDAR classifier, but due to the unproven reliability of segmentation algorithms, it is important to have this failsafe.) Mathematically, the robot maneuvers on the ground whose slope satisfies where λ 1 is a threshold for safe slope.
2) Turning on slopes: This is one of the most dangerous maneuvers the robot can undertake, and is accounted for by limiting the deviation of the robot's heading from the direction of the steepest ascent/descent at every point on the plan with slope above a certain threshold. where ϕ is the robot's yaw angle, and Yaw(∇ψ) is the yaw angle of the gradient of the slope.
3) Sharp turns at high speed: Due to the fast speeds and large sizes of many robots used in such applications, all turns must be subjected to tipping constraints. This is done by simply checking the required centripetal acceleration for the turning radius commanded and ensuring it falls within the limits of what the platform's geometry allows The mapping pipeline above is used as the foundation for running both the baseline min-time cost function and the constrained cost function described above in a high-fidelity Unity-based simulation environment as well as on a Clearpath Husky robot platform. It is desirable to have an unknown space induced by a traversable terrain feature and obstacles in this unknown space that challenge the planner's safety. The ideal terrain feature that can produce this scenario is a hill, and therefore the tests done below involve climbing hill slopes.

A. Simulation
The simulation environment chosen is indicated in Fig.  3 (a). It consists of flat terrain which quickly leads into a moderately steep hill with a slope of about 30°. At the top of this hill, there is flat land for some distance, which is then followed by tall metallic structures that are detectable by LiDAR from the base of the hill. For the purpose of the experiments, rocks that are big enough to block the robot are spawned at random locations on the flat land in each trial. The robot used in this environment is a Clearpath Warthog, which can achieve a top speed of 5 m/s, which is also set as the maximum commanded speed here. All simulations were done in real-time using a desktop computer with a 3.6 GHz i7 CPU and an RTX2080 Super GPU.
For each trial, the robot starts on flat land some distance away from the base of the hill and is given a fixed location (same for each trial) near the back of the hilltop as the goal. Then, 20 such trials are conducted with each of the following pipelines: • Pipeline 1: Raytraced map and default MPPI that treats unknown space as free • Pipeline 2: Raytraced map and default MPPI that treats unknown space as impenetrable • Pipeline 3: Inflated ground point map and default MPPI that treats unknown space as impenetrable • Pipeline 4: RAMP (inflated ground point map, variable horizon MPPI, and 3D aware risk constraints) Here, the parameters for MPPI in Pipelines 1-3 as well as for RAMP are tuned to give the best results on flat land. α in (III) is swept between 0.25 and 0.75 to vary the speed in unknown space, for studying the speed vs safety tradeoff in RAMP. β is taken to be 0.95 for prioritizing the goal, and γ is set to 10 for high speed in known space. The 3D risk Fig. 9: When running default MPPI with unknown space treated as an obstacle, there is a clear benefit to using ground point inflation over raytracing for map generation. Raytracing prematurely clears the hilltop as known free space and results in the robot frequently colliding with the rocks beyond. Fig. 10: Out of the 4 pipelines tested, RAMP is able to achieve a full 100% success rate, thereby ensuring safety without sacrificing speed.
constraints are set so that RAMP cannot plan up slopes greater than λ 1 = 75°, the max deviation of the robot's heading from steepest ascent is λ 2 = 15°on any slope greater than 15°, and λ 3 is chosen to be 1 for a lenient constraint on sharp turns. Finally, the planning horizon T is set to be 10 seconds, which comfortably allows for long plans into unknown space.
The average speed (for successful trials) and success and collision rates are measured, and qualitative plan behavior is observed throughout. Here, the collision rate is defined as the fraction of trials that hit rocks on the hilltop, and the success rate is defined as the fraction of trials that reach the goal. (inability to reach the goal can arise from either hitting a rock or tipping due to turning on a slope) This leads to the following results: • Pipeline 1 fails almost every time due to heading up the hill at high speeds and crashing into a rock at the top. • Pipeline 2 also fails 85% of the time, despite the high unknown space cost, due to the raytraced map not representing unknown space accurately and MPPI planning a high-speed trajectory up the slope. This is shown in Fig. 6. It should be noted that Pipelines 1 and 2 have high average speeds in the few trials where they are successful due to the planner commanding a straight line trajectory at full speed up the hill. • Pipeline 3 does not hit rocks due to the accurate representation of unknown space by the inflated ground point map. This steep improvement in collision frequency is denoted in Fig. 9, showing the benefits of an inflated ground point map. • However, Pipeline 3 has a middling success rate of 80 %, despite having a low collision rate. This is because the fixed horizon of Pipeline 3 necessitates a long 10 s rollout to fit in a limited length of known free space. MPPI compensates for this by introducing curvature in the local plan, as well as very low robot speed, as seen in Fig. 7. This leads to a lot of wheel slip, sliding down the slope, and sometimes leading to the robot tipping over. • The drawbacks of Pipeline 3, arising from both turning on slope and inability to adjust plan horizon based on the distance to unknown space, are fixed by RAMP. As shown in Fig. 8, RAMP's plans with α = 0.5 are not only straight up the hill but also fast in known space while slowing down in unknown space. Additionally, α can be increased to 0.75 to reduce the speed in unknown space further, which reduces average speed while maintaining 100% success rate, or α can be reduced to 0.25 to get a small increase in average speed and go faster in unknown space, at a slight cost of success rate. These results are summarized in Fig. 10, which shows that RAMP breaks the tradeoff barrier between success rate and high speed for challenging off-road environments.

B. Hardware Demonstration
RAMP was successfully deployed on a Clearpath Husky robot, achieving a maximum speed of 1 m/s to achieve realtime safe navigation at the platform's top speed. This was done in the environment shown in Fig. 1: a hill in a forest with about the same steepness as the one used in the simulations above. For this scenarios, due to the risk of catastrophic damage to the robot, only RAMP was tested. The main computer on the robot was equipped with a 2.8 GHz i7 CPU and RTX2060 GPU. The results can be summarized as follows: • The robot successfully climbed straight up the hill at full speed. It was noticed that the robot's maximum speed was too low to observe the slowdown beyond the unknown space that RAMP produces. The state of the local map and plan for this hill is shown in Fig. 11. • It was also made clear that the advantages of RAMP only become readily apparent at high speeds, since the long stopping distances involved in those scales necessitate the improvements that RAMP brings. • Finally, the geometric classifier struggles with the ubiquitous tall grass in off-road terrain, and classifies even a single blade of grass as an obstacle. This leads to an obstacle-filled map on an empty forest floor, which is hard to plan in, and calls for a better LiDAR classifier using semantic information. Fig. 11: The Husky was able to climb up the hill fairly easily, however due to the low top speed the robot usually clears unknown space by the time the plan catches up to it. Also, the sheer number of obstacles stems from the unsuitability of a geometric LiDAR classifier in off-road terrain.

VI. CONCLUSIONS AND FUTURE WORK
This paper proposes a Risk-Aware Mapping and Planning (RAMP) pipeline for fast off-road ground robot navigation in unmapped 3D terrain. RAMP introduces a ground-plane support based map generation method by inflating classified ground points from LiDAR, combined with persistent memory for increasing map density. This map is then used by a novel MPPI-based planner, which introduces multiple layers of variable horizon to maximize speed in known free space while allowing for tunable speed in unknown space. When compared to default MPPI with different levels of mapping and unknown space costing improvements, RAMP delivers high success rates at high speed, and is interfaced with 3D aware risk constraints to allow for safe and fast navigation up hills in both simulation and on hardware.
In the near future, we plan to design a policy to tune the parameters of the cost function and have them vary online based on the environment, since it was found that the optimal values for these parameters vary based on the acceptable risk tolerance for terrain such as hills vs negative obstacles. Additionally, we plan to scale up by testing RAMP on high speed platforms such as a Clearpath Warthog, more challenging terrain such as a steep ravine. We also plan to use a better LiDAR classifier such as Cylinder3D [19] to get ground points for map generation, since the hardware test showed that geometric classifiers struggle with tall grass.