Risk-aware Traversability in Extreme Environments

to be ordered correctly later: Kyon, Nikhilesh, David, Rohan, Ali
Targetting RAL/ICRA ¹XX and Ali-Akbar Agha-Mohammadi are with the Jet Propulsion Lab, Pasadena, CA {aliagha, XXXXX}@jpl.nasa.gov

Abstract

Risk-aware motion planning

Instructions

Paragraph header: Please start \ulineevery single paragraph with a paragraph header, summarizing the intention of that paragraph. This is mainly for iterations during the paper preparation. We can remove most of them for the final report.

Paper Scope

•

MPC-based local planning with risk-based velocity adjustments
•

Risk computation with multiple risk metrics
•

Global planning (A*) risk-based costs
•

…?

I Introduction

Ali Real world problem and example: Consider a robot tasked to … in a GPS-denied … Instructions (to be deleted asap): Maybe we can at the end mention DARPA is pushing to enable these behaviors by creating and organizing the subt challenge.

Refer to caption — Figure 1: Husky robot at Arch Coal Mine, West Virginia, USA.

Ali Risk-aware planning: The above problem is an instance of more generic problem of risk-aware plannign and traversability analysis… Instructions (to be deleted asap): talk aobut its importnace and its general formulation (e.g., mdp, pomdp, rhc, etc).

Ali Challenges and gap in state-of-practice: when deplying in the extreme environemnts and field, too much unertainty … current methods are not sufficient…

Ali and everyone System overview: In this paper, we will go over a system comprised of x, y, z elements… to address this challenge in the field…

Ali and everyone Contributions: We push the boundaries of the state-of-practice by giving enabling …. In particular, we …

1.

More accurate risk qunatitificaiton
2.

Enabling and demonstration of wheeled robotic platform to traverse terrains wiht x amount of undulation and y amount of …
3.

Enabling and demonstration of recovery behaviors in …. and covering x amount of ground in y amount of minutes

Outline: Outline of the papers: related work, design principles, approach, implementation details, experimental results, conclusion

II Problem Statement

In this section, we will give a formal definition of the problem we are going to address.

State/action/observation: Let $x_{k}$ , $u_{k}$ , $z_{k}$ denote the state, action, and observation at the $k$ -th time step. $\pi=\{x_{0},x_{1},\cdots\}$ is a sequence of poses, that is referred to as a path in this paper.

Map: The map at time $k$ is represented as $m_{k}=(m_{k}^{(1)},m_{k}^{(2)},\cdots)$ where $m_{k}^{i}$ is the $i$ -th element of the map (e.g., a cell in a grid map).

Robot Model: A robot model captures the physical properties of the vehicle’s mobility function, such as inertia, mass, dimensions, shapes, and the kinematic constraints.

	$\displaystyle x_{k+1}$	$\displaystyle=f(x_{k},u_{k})$		(1)
	$\displaystyle g(u_{k})$	$\displaystyle>0$		(2)

Traversability: Traversability is defined as the capability for a ground vehicle to reside over a terrain region under an admissible state [Papadakis2003]. It is typically represented by a binary or continuous value computed from a terrain model, the robotic vehicle model, kinematic constraints, and a set of criteria for optimality.

\displaystyle\tau=f(m,x)

(3)

Problem Description: The problem in this paper is formalized as follows. Given the initial robot configuration $x_{S}$ and the goal configuration $x_{G}$ , find a time-optimal trajectory $\mathcal{T}^{*}=\{x_{S},u_{S},\cdots,x_{G},u_{G}\}$ that maximizes the traversability index while satisfying all the kinematic and dynamics constraints.

III Risk-aware Motion Planning

Overview: Risk-based mapping. Hierarchical planning (geometric and tracking). We split the motion planning problem into geometric planning and kinodynamic planning. For geometric planning, we search for minimum-risk paths in a 2-d path space: $\pi^{*}=\{p^{*}_{k},p^{*}_{k+1},\cdots\}$ , where $p_{k}=[p^{x}_{k},p^{y}_{k}]$ consists of $x$ and $y$ positions. We then solve a kinodynamic planning problem to track the geometric path using the full state $x_{k}=[p^{x},p^{y},\theta,v^{x}]_{k}$ which consists of $x$ , $y$ position, orientation, and forward linear velocity.

III-A Risk-based Traversability Analysis

Risk Definition: As a measure of traversability assessment, we introduce the concept of risk.

\displaystyle r(m,x)\in[0,1]

(4)

We define the risk as the probability of mobility failure after the robot drives the same terrain for 1 [m]. The mobility failure includes the collision, stuck, high-slip, or whatever in-preferable conditions.

III-B Risk-aware Geometric Planning

Risk of path: For a given path $\pi\in\Pi$ , the total accumulated risk is computed as

\displaystyle\mathcal{R}(m)=1-\prod_{i}\{1-r(m,x_{i})\}^{d_{i}}

(5)

where $d_{i}$ is the length of path segment. We should include the history of states for risk computation (e.g., path tortuosity). we should also think about how to properly decompose the geometric and kinodynamic risk)

As a simplification, for the geometric planning problem we assume a path-dependent risk:

\displaystyle r^{p}(m,p)=\operatorname*{arg\,min}_{\theta,v^{x}}r(m,x)

(6)

We then define the accumulated minimum path risk:

\displaystyle\mathcal{R}^{p}(m)=1-\prod_{i}\{1-r^{p}(m,p_{i})\}^{d_{i}}

(7)

Optimization problem: We search the minimum risk path based on the latest map information

\displaystyle\pi^{*}=\operatorname*{arg\,min}_{\pi\in\Pi}\mathcal{R}^{p}(m_{k})

(8)

III-C Risk-aware Kinodynamic Planning

Optimization problem: The objective is to accurately track the input geometric path under kinematic and dynamics constraints. The above geometric planner produces a 2-d geometric path $\pi^{*}$ . The goal is to find controls $\mathcal{U}^{*}_{k}=\{u^{*}_{k},u^{*}_{k+1},\cdots,u^{*}_{k+T}\}$ within a planning horizon $T$ which produces states $x^{*}_{k}$ to track the path $\pi^{*}$ . The optimal control $u^{*}_{k}$ can be obtained by minimizing the following cost function

\displaystyle u^{*}=\operatorname*{arg\,min}\sum_{t=0}^{T}\bigg{[}\|x_{k+t}-p^{*}_{k+t}\|_{Q}\bigg{]}

(9)

such that

$\displaystyle x_{k+1}$	$\displaystyle=f(x_{k},u_{k})$	(10)
$\displaystyle g(u_{k})$	$\displaystyle>0$	(11)
$\displaystyle h(m,x_{k})$	$\displaystyle>0$	(12)

where the constraints $g(u)$ encode controller limits and $h(m,x_{k})$ encodes risk constraints. For instance, $h(m,x_{k})$ can constrain the speed of the vehicle in risky regions, as well as constrain the vehicle to avoid regions with lethal levels of risk.

IV Implementation

IV-A Sensing and Mapping

Prefiltering: Apply prefilters to remove noise and body occlusion.

Temporal fusion: For the pointcloud $z_{k}$ and the pose $x_{k}$ , a local map is obtained as a set of last $n$ measurements

\displaystyle Z_{k}=\{(z_{i},x_{i})\}_{i=k-n:k}

(14)

This is typically obtained as the output of LiDAR-based localization module.

Classification of points: The registered pointcloud is classified to two types: obstacle points $Z^{\mathrm{obstacle}}$ and ground points $Z^{\mathrm{ground}}$ . We use an efficient method to fit lines in cylindrical coordinates .

Normal Estimation: Compute surface normals from registered pointcloud.

IV-B Risk Assessment

The risk for a configuration is assessed as the combination of multiple immobilization sources.

IV-B1 Collision Risk

Localize obstacles and set higher risk if a configuration has a chance of collision.

\displaystyle r_{\mathrm{collision}}:(Z^{\mathrm{obstacle}},Z^{\mathrm{ground}})\mapsto[0,1]

(15)

IV-B2 Missing Data Risk

Missing data is an indication of negative obstacles. Combine ray tracing and pointclouds to detect such region.

\displaystyle r_{\mathrm{missing}}:(Z^{\mathrm{ground}})\mapsto[0,1]

(16)

IV-B3 Tilt Risk

\displaystyle r_{\mathrm{tilt}}:(\phi,\theta)\mapsto[0,1]

(17)

IV-B4 Roughness Risk

\displaystyle r_{\mathrm{roughness}}:(\phi,\theta,Z^{\mathrm{ground}})\mapsto[0,1]

(18)

IV-C Risk-aware A*

The optimization problem in (8) can be rewritten as

	$\displaystyle\pi^{*}$	$\displaystyle=\operatorname*{arg\,min}_{\pi\in\Pi}\log[1-\mathcal{R}^{\pi}(m_{k})]$		(19)
		$\displaystyle=\operatorname*{arg\,min}_{\pi\in\Pi}\sum_{i}d_{i}\log[1-r(m_{k},x_{i})]$		(20)

such that

	$\displaystyle\mathcal{R}^{\pi}(m_{k})$	$\displaystyle<R^{\mathrm{total}}$		(21)
	$\displaystyle r(m_{k},x)$	$\displaystyle<R^{\mathrm{inst}}\quad\forall x\in\pi$		(22)

where $R^{\mathrm{total}}$ and $R^{\mathrm{inst}}$ are the thresholds for accumulated and instantaneous risk, respectively. This reformulation allows us to apply the well-established shortest-path algorithm over graphs such as A*.

IV-D Velocity-adaptive kinodynamic planning

We outline our implementation of risk-aware kinodynamic MPC planner. See Figure (2) for an overview and Figure (3) for an example.

Optimization problem: We encode two risk constraints $h(m,x_{k})$ in the kinodynamic planning problem: 1) avoid risky regions above some threshold, which we call obstacle constraints $h^{o}(m,x_{k})$ and 2) limit velocities in locations with high risk, which we call velocity constraints $h^{v}(m,x_{k})$ . The optimization problem takes the form:

\displaystyle u^{*}=\operatorname*{arg\,min}\sum_{t=0}^{T}\bigg{[}\|x_{k+t}-p^{*}_{k+t}\|_{Q}\bigg{]}

(23)

such that

$\displaystyle x_{k+1}$	$\displaystyle=f(x_{k},u_{k})$	(24)
$\displaystyle g(u_{k})$	$\displaystyle>0$	(25)
$\displaystyle h^{o}(m,x_{k})$	$\displaystyle>0$	(26)
$\displaystyle h^{v}(m,x_{k})$	$\displaystyle>0$	(27)

Linearize dynamics:

Obstacle constraints: Determining obstacle constraints $h^{o}(m,x_{k})$ is a 5-step process:

1.

Threshold risk to identify obstacles
2.

Group obstacle locations into convex polygons
3.

Determine robot footprint convex polygon
4.

Compute convex-to-convex polygon signed distances
5.

Create convex constraints $h^{o}(m,x_{k})$ on vehicle motion

Velocity constraints: We constrain the velocity of the vehicle to be less than some scalar multiple of the risk in that region:

\displaystyle h^{v}(m,x_{k})=r^{p}(m,p_{k})-\lambda v^{x}_{k}

(29)

This effectively minimizes the kinodynamic risk the vehicle experiences.

QP Solver:

Linesearch:

Backup Stopping trajectory:

Tracking Controller:

IV-E Adaptive Risk Thresholding

Mission-based tuning: Based on the mission states and robot’s role, the risk thresholds are adaptively chosen by the higher-level planning module.

Recovery Behavior: When in stuck, the thresholds are gradually increased to find feasible paths out of the inescapable state, without risking vehicle’s safety too much.

V Architecture Overview

See link to Literature Review.

See Traversability slides

Figure 4 shows an overview of the overall architecture. Using images from camera and pointclouds from sensors the state estimator generates an estimate of the pose of the robot in $SE(3)$ . The temporal mapper fuses multiple pointclouds using these pose estimates to produce a 3D Occupancy Grid. Next, the traversability estimator uses this occupancy grid and the most recent pointcloud to generate a costmaps at different levels of fidelity and range. Finally, these costmaps are used by the planning layers to generate wheel velocity commands to drive the robot to the desired goal location.

VI Dealing with Uncertainty

blah blah

VI-A Challenges

•

Darkness
•

Dust
•

Self-similar environments
•

Rough Terrain
•

High slippage (Mud, Sand)
•

Negative Obstacles
•

Limited Comms
•

Unknown, unmapped environments

Whats the Challenge: As shown in Fig. 1, low-lighting, dust and rough terrain are key features of subterranean environments which make state estimation and mapping really challenging. Errors in perception percolate to the planner and controller and eventually can lead to failure of the complete system.

How do we solve it: To address this issue we developed a robust state estimator which leverages redundancy and heterogeneity.

VI-B Heterogeneous Redundant Odometry (HeRO)

Failures and philosophy: State estimation failures can be triggered by many factors like dust, low-light conditions, lack of features/landmarks, dynamic range issues in cameras, presence of symmetries in environments for lidar-based localization, etc. HeRO [hero2019isrr] is a state estimator designed to achieves robustness to these failures by exploiting redundancy and heterogeneity in sensing and odometry algorithms.

Algorithm details: The robot is equipped with redundant cameras and lidars of different wavelengths pointing in different directions to maximize the field-of-view. Many camera’s such as Intel Realsense T265 or Qualcomm snapdragon flight are equipped with on-board state estimators. HeRO is designed to leverage these robust commercial implementations of state estimators and multiplexes a continuous pose estimate from the most reliable source from these redundant estimates by performing confidence tests.

VI-C Multi-fidelity Traversability Estimator

Why multi-fidelity?: While HeRO provides robustness to complete loss of state estimates in several failure modes, the estimates still lack accuracy due to presence of drift in pose estimates. This drift in estimated pose ends up corrupting the temporal map. This implies that there is a trade-off between accuracy of the map and it’s range. To get larger range, more pointclouds need to be fused by using more number of noisy pose estimates leading to lesser accuracy. Traversability estimator produces a costmap using the occupancy grid from the temporal mapper.

Why stochastic maps are not enough: The stochastic temporal maps that implement belief updates such as log-odds [hornung2013octomap] partially mitigate this issue. However, like any other filter, optimizing the belief update parameters to remove noise introduces a lag in the estimates. Hence, our traversability estimator balances the trade-off between range and accuracy by producing maps at multiple fidelities as shown in Fig. 5.

Instantaneous Pointcloud Idea: Note that the costmap for the local kinodyanmic planner, designed to detect the smallest obstacles, doesn’t use the temporal map at all. A key idea to enable this was to re-plan short-range trajectories at high-speed in body frame using the most recent pointcloud [instantaneousPointclouds]. Furthermore, we need to ensure that a single scan provides sufficiently large field-of-view to ensure the set of trajectories are not too restrictive. Hence, the robot was equipped with three Velodyne VLP16 lidars.

For larger obstacles, relying on instantaneous pointcloud data from sensors which provide good coverage of the surrounding environment is sufficient. However, due to the presence of blindspots or sparse lidar data, detecting small obstacles can be difficult due to insufficient data. To address this issue, we fuse the past 1-5 seconds of lidar data which provides a richer and more complete picture of the environment. The longer this temporal window is, the more susceptible to localization noise our costmap will be. Therefore if our state estimator HeRO detects a failure or decrease in confidence, this temporal window can be dynamically reduced.

Negative Obstacles: Since detecting negative obstacles is vital to preserving the life and safety of the robot, we apply the idea of relying on instantaneous (or temporally fused) pointclouds and detect negative obstacles without having to rely on accurate localization. This eliminates the possibility that errors in localization will create virtual floors which the robot may wish to drive over. We take a simple geometric approach which detects gaps in the pointcloud which correspond to negative obstacles. Negative obstacles which do appear in the pointcloud are not handled with this approach, and are instead detected via the settling algorithm outlined in the next section.

Settling Algorithm: Traversability at different query points on the costmap is obtained by running a 3-D settling algorithm [krusi2017driving]. For a given query pose in $SE(2)$ , a settled pose in $SE(3)$ is computed by settling the robot on the ground to obtain its height, roll and pitch.

VI-D Recovery Behaviors

In the real world, failures are unavoidable. Hence, as a last line-of-attack, we design behaviors to recover the system from failures. The following recovery behaviors are triggered (in described order) when the any local planner fails to find a path:

•

Clearing the temporal map and start building it from scratch.
•

Rotate the robot, open-loop, in the direction of maximum clearance.
•

Terminate current goal and return to base.
•

Backwards wall-follow (implemented by giving a carrot goal in body frame).

VII Local Planning

Assessing traversability and risk is prerequisite to autonomous navigation in extreme environments. These traversability challenges, which have to be accounted for by a local planner, include high slopes, obstacles, holes, comm nodes dropped by the robot or sensor blind spots. In order to find a balance between efficient traversal and low risk to the platform, we construct a measure of accumulative risk for a given path through the environment. Let $g=(m^{1},\cdots,m^{n})$ be a grid of $n=n_{l}\times n_{w}$ cells with length $n_{l}$ and width $n_{w}$ , and let each $m^{i}\in[0,1]$ represent the probability that the robot safely traverses through the cell. Let $\pi=\{i_{t}\}_{t=0}^{T}$ be a sequence which defines a connected path through the grid, and define the solution space $\Pi$ as the set of connected paths through the grid which connects the current robot position with the goal. Then we define the total accumulated risk of the path $\pi\in\Pi$ as

\displaystyle\mathcal{R}^{\pi}=1-\prod_{t=0}^{T}m^{i_{t}}

(30)

which is the probability that the robot fails to safely traverse the path $\pi$ .

We combine multiple layers of grids which encode different types of risk. Assuming mutual exclusivity between each risk type, the total risk in each grid cell is simply the product of the individual risks of each layer. We consider the following sources of risk:

•

Obstacles (Walls, Rubble, etc.)
•

Steep Slopes
•

Negative Obstacles
•

Sensor Blind Spots
•

Mission Items (comm nodes, other robots, etc.)

To identify these sources of risk, we perform traversability analysis on a local map generated from KO/VO odometry and sensor measurements (depth camera, LiDAR). We fit local ground planes on the map to identify traversable ground regions. Ground planes which have too high of a slope are identified as “Steep Slope" regions. Points on the map which are above or below the ground plane which are higher than the ground clearance of the robot are marked as “Obstacles". Negative obstacles are identified by gaps in the map (where no sensor measurements are available). Sensor blind spots contribute a non-lethal risk and are identified by the respective sensor models. Finally, we add mission-specific items to the risk map (to avoid dropped comm nodes, other robots, etc.) For an example of the risk mapping identifying obstacles, see Figure 7.

We search for the minimum risk path using A*:

\displaystyle\pi^{*}=\arg\min_{\pi\in\Pi}\mathcal{R}^{\pi}(g)

(31)

Having found the minimum risk path, we then send a carrot waypoint 1m along this path to the low-level controller of the legged system. The low-level controller in the Spot robot received information from Spot’s sensors (not the payload sensors). Due to the configuration of Spot cameras and their limited field of view, it is much safer and more effective for the robot to prefer moving in the forward direction, where sensor coverage is better. In general, within the NeBula framework, we formalize this notion with a perception-aware planner.

We model each cell $m^{i}$ as a random variable, where $\hat{m}^{i}$ and $\sigma^{m^{i}}$ denote its mean and variance. The “information" about $m^{i}$ is captured in the $\sigma^{i}$ , where fully unknown and fully known cells have the highest and lowest $\sigma^{i}$ values, respectively. By explicitly capturing the learned information about each cell $m^{i}$ , we can treat all the cells (known, unknown, partially-known) in a unified manner.

This representation allows us to incorporate perceptual capabilities into the planning. Given the sensors available on the robot and their configuration and noise characteristics, we can derive models that predict the evolution of $\sigma^{i}$ based on the sensor configuration and most likely measurements along a given trajectory $\pi$ .

\displaystyle\sigma^{i}_{k}=\tau(\sigma^{i}_{0},z_{0:k}(\pi))

(32)

where the measurements $z_{0},\cdots,z_{k}$ are predicted at each of the first $k$ time steps along the path $\pi$ . This becomes increasingly important when the sensor configuration is highly asymmetric on a robot, which is the case for Spot as it has blind spots and areas where sensory measurement noise is considerably higher than other areas. We denote the predicted map $k$ time steps in the future along the trajectory $\pi$ by

\displaystyle g_{k}=\{p(m|m^{1}_{k},\sigma^{1}_{k}),\cdots,p(m|m^{n}_{k},\sigma^{n}_{k})\}

(33)

where $p(m|m^{i}_{k},\sigma^{i}_{k})$ is the probability distribution of $m^{i}$ parameterized by $m^{i}_{k}$ and $\sigma^{i}_{k}$ . As a result, we define a risk measure that takes perceptual capabilities and uncertainties into account when planning trajectories, by minimizing accordingly:

\displaystyle\mathcal{R}_{k}^{\pi}=1-\prod_{t=0}^{T}p(m|m_{k}^{i_{t}},\sigma_{k}^{i_{t}}),~{}~{}~{}\pi^{*}=\arg\min_{\pi\in\Pi}\mathbb{E}[\mathcal{R}_{k}^{\pi}(g_{k})]

(34)

Efficient methods for computing predicted risk uncertainty over a 2-D grid for a given sensor model have been considered in [CRM].

VIII Experiments

IX Limitations and Future work

•

Water (may be handled due to no lidar returns?)

X Conclusion

Appendix A BeliefCloud: A Map Representation For Resilient Navigation

A-A Introduction

A-B Literature review

See Literature Review.

See Traversability slides

1.

[InstPcWafr2016] uses instantaneous pointclouds to get robustness to noise in localization but has issues due to occlusions, field-of-view, etc.
2.
[nanomap2018] Nanomap
1. (a)
  
  Only use most recent frame in FOV
2. (b)
  
  Does not capture uncertainty in rotation (i.e. important if depth measurements are over a larger distance)
3. (c)
  
  Designed for quadrotor (free space estimation) vs traversabiltiy for ground robots
3.
Grid based approaches [hornung2013octomap] [agha2017CRM]
1. (a)
  
  Need to pick resolution i.e. a trade off between computation and performance.
4.

[sphericalImage] uses a spherical image to efficiently compute normals
5.
Fitting a plane once we have points with gaussian uncertainty
1. (a)
  
  [planeFitUnc2010] shows plane fitting with noise in range measurements of the sensor
2. (b)
  
  [hyperfit2015] shows hyper-plane fitting with general gaussian uncertainty in points
3. (c)
  
  Look at page 5,7 in this link showing APLCON package. This is a c++ implementation of it on github with a sample code here.

Appendix B Discussion

B-A Motivation

Inst Point Clouds –> don’t keep history (bad), but no noise (good) Voxblox –> don’t consider error in state estimation, everything in world frame

Many times the planner only needs to plan in the local frame, not in the world frame.

B-B Key Ideas

•

Larger tolerance for things that are far away
•

Plan in current body frame and inflate the covariance of previous estimate (degenerate to instant. pointclouds when using bad localization source)
•

landing on form (stochastic traversability)
•

Account for uncertainty in pitch
•
How estimation error is modelled?
- –
  
  Take the covariances from the estimation algorithm?
- –
  
  based on $v,a,\omega$
•
How sensor noise is modelled?
- –
  
  Find specific model for Velodyne, depth sensor,… \jesusI don’t like too much this idea, we will end up with an algorithm that will require experiments to find the sensor model for every different sensor. Would it be possible to find this in an online fashion? Or just at the start of each run, with the robot still, compute the variance in each direction (but this may not work to compute the bias wrt the ground truth?) Rohan: The idea is that you would only do this once per sensor. E.g. point the velodyne to a flat wall at a known distance and fit a plane and characterize the sensor noise.
•
How covariances are propagated backwards?
- –
  
  The previous frame, expressed in the current frame, has translation Distribution(mean, variance) and rotation Distribution(mean, variance)
- –
  
  Nanomap propagates only translation, but not rotation (and therefore the propagation is linear). Propagating both is not linear–> we should find a way to efficiently approximate this. Rohan: see [barfoot2014associating] [se3unc] but can directly use pos_cov_ops ros package.
•
Representation
- –
  
  Brute Force 1: for every point of every point cloud $n=0,...,n=N-1$ , keep mean (3 values), and covariance (other 3 values).
- –
  
  Selective choice of the
  - *
    
    $N$ point clouds that will be used to compute the fused map: If the robot is still, take few point clouds from that moment, but many point clouds when the robot was moving.
  - *
    
    Area of the point cloud that will be used: If two point clouds overlap, take points in that area corresponding to the point cloud closer to the current frame (i.e. point cloud with smaller $n$ ).
  Once the selective choice has been made, we end up with a single point cloud that contains points from every point cloud. As we have now the uncertainty in position and orientation of the frame of the lidar where that point was captured, we can compute the uncertainty in position of a specific point wrt to the frame $n=0$
  
  Figure 12: Selective Selection
- –
  
  Should we compute everything from scratch in every iteration? (i.e. should we fuse the $N$ selected point clouds in every iteration) or is there a way to reuse past computation?
•

Traversability computation
- –
  
  Take into account different yaws of the vehicle for the same point? (and not just the same orientation for the same point, as we are doing right now)
- –
  Stochastic normal estimation
  - *
    
    Brute force 1: for every point (random variable), sample a point–> obtain a surface, and compute the normal from this surface. Obtain a traversability value (# of points inside the box?) from this surface. Note that the points inside that box are also random variables. We could do something like traversability= intersection of the pdf of the box with the pdfs of the points / total volume of the box Do this several times, and the traversability mean and deviation are obtained from this sampling.
    
    Traversability should be a continuous value, not a boolean one (right now it’s yes or no).
  - *
    
    The stochastic normal estimation should be a distribution over a 3d unit sphere (https://en.wikipedia.org/wiki/Kent_distribution)
  Figure 13: Caption
•

Output The output should be an stochastic traversability value Distribution(mean, variance). Could planners benefit from this random variable (similar to CRM from Ali) or should we just keep the mean?