# Multiple-Hypothesis Path Planning with Uncertain Object Detections

Brian H. Wang, Beatriz Asfora, Rachel Zheng, Aaron Peng, Jacopo Banfi, and
Mark Campbell _Corresponding author: Brian H. Wang_ Brian H. Wang, Beatriz
Asfora, Rachel Zheng, Aaron Peng, and Mark Campbell are with the Sibley School
of Mechanical and Aerospace Engineering, Cornell University, Ithaca, NY 14850,
USA (e-mail<EMAIL_ADDRESS><EMAIL_ADDRESS><EMAIL_ADDRESS><EMAIL_ADDRESS>mc288@cornell.edu).Jacopo Banfi is with the Computer
Science and Artificial Intelligence Laboratory, Massachusetts Institute of
Technology, Cambridge, MA 02139, USA (e-mail: jbanfi@mit.edu).

###### Abstract

Path planning in obstacle-dense environments is a key challenge in robotics,
and depends on inferring scene attributes and associated uncertainties. We
present a multiple-hypothesis path planner designed to navigate complex
environments using obstacle detections. Path hypotheses are generated by
reasoning about uncertainty and range, as initial detections are typically at
far ranges with high uncertainty, before subsequent detections reduce this
uncertainty. Given estimated obstacles, we build a graph of pairwise
connections between objects based on the probability that the robot can safely
pass between the pair. The graph is updated in real time and pruned of unsafe
paths, providing probabilistic safety guarantees. The planner generates path
hypotheses over this graph, then trades between safety and path length to
intelligently optimize the best route. We evaluate our planner on randomly
generated simulated forests, and find that in the most challenging
environments, it increases the navigation success rate over an A* baseline
from 20% to 75%. Results indicate that the use of evolving, range-based
uncertainty and multiple hypotheses are critical for navigating dense
environments.

###### Index Terms:

Motion and Path Planning, Collision Avoidance, Probability and Statistical
Methods, Path Planning Under Uncertainty

##  I Introduction

Figure 1: Overview of the contributions of our multiple-hypothesis planner.

Autonomous navigation in unknown, unstructured, and obstacle-dense
environments requires a robot to recognize obstacles and plan a safe path
around them, typically by using a 3D sensor such as a stereo camera or lidar.
This problem becomes much more challenging as the desired navigation speed and
complexity of the environment increase. The further ahead the robot can
robustly perceive and react to upcoming obstacles, the more safely it will be
able to navigate through increasingly challenging and obstacle-dense
environments.

However, 3D sensors used in robotics are often affected by measurement errors
that increase with distance from the sensor. Stereo camera depth errors grow
quadratically with range, due to disparity estimation errors [1, 2]. Lidar
sensors, while more precise, return very sparse points at longer ranges.

Current pipelines for autonomous navigation in unknown environments plan
iteratively, by constructing a map of the robot’s immediate surroundings
(often as a 3D occupancy grid), optimizing a path to a global goal in this
map, then recursively updating the local map and planned trajectory as the
robot moves towards the goal and obtains new sensor measurements [3, 4]. These
methods have demonstrated significant successes in autonomous path planning.
However, due to increasing 3D sensor noise with range, existing methods
typically plan only in a short range near the robot, where sensor noise is
minimal and reliable map construction is possible. The robot discards noisier
sensor measurements beyond this range entirely, ultimately limiting the
robot’s ability to quickly and effectively traverse complex environments.

Ryll et al. [5] point out this constrained perception horizon as a key
limitation on the speed of safe autonomous navigation in agile unmanned aerial
vehicle (UAV) flight. With no knowledge of the environment past the short,
minimal-noise sensor range, the robot must plan conservatively, always ready
to come to a stop at the boundary of this sensor range in case new obstacles
appear. Ideally, the robot should be able to reason about potential obstacles
at far ranges, enabling preemptive avoidance of obstacle-dense areas.

Long-range sensor measurements, while noisy, still contain usable information
for recognizing obstacles. Works such as [5], [6], and [7] presented planning
methods that use camera image semantics to augment short range 3D sensor
measurements. Recent works in 3D object detection [1, 8, 9, 10, 11] have
demonstrated accurate object detection at longer ranges than reliable
occupancy grid mapping is typically possible. These methods use machine
learning to recognize patterns in 3D point clouds and detect objects even
within noisy sensor data.

These object detectors provide a promising way to extend the amount of
actionable information available to a robot, by enabling longer-range obstacle
recognition. However, object detections still contain errors in measuring
obstacle positions and sizes, as shown previously in [12]. The robot must
therefore reason about this sensing uncertainty in the path planning
algorithm, in order to effectively use these object detections.

As illustrated in Figure 1, failing to consider this uncertainty can result in
unsafe planned paths, while using uncertainty naively can result in
inefficient planning. In this paper, we present a multiple-hypothesis planner
which uses uncertain object detections to help the robot navigate to its goal,
while considering the length and safety of multiple candidate paths to
determine the overall best path to the goal.

Our work is motivated by navigation through a forest while avoiding tree
trunks, previously studied in [13], [14], and [15]. To model this problem, we
consider a robot navigating through an environment densely populated with
obstacles of a single class, but with different sizes and locations, that can
be detected with noise that varies with range. We first estimate the positions
and sizes of obstacles using these noisy object detections. We then construct
a graph representation of the scene based on pairwise connections between
obstacles, which stores the probability that the robot can safely navigate
between each pair of nearby obstacles in the world. This graph can be updated
in real-time based on new measurements, and pruned if edges do not provide
safe paths, thus providing probabilistic guarantees. Using this probabilistic
graph, we plan multiple path hypotheses between pairs of obstacles to the
navigation goal. The planner then intelligently optimizes the best route to
the goal by trading off between safety and expected path length.

Our contributions include the following:

  * •

A high-level graph representation of forest environments, that represents and
stores the probabilities of the robot being able to safely navigate between
pairs of obstacles in the forest.

  * •

A multiple-hypothesis path planning method which uses this probabilistic
navigation graph to generate and evaluate multiple candidate paths to the
goal, accounting for both expected path distance and safety.

  * •

An experimental simulation study in randomly generated forest environments,
demonstrating that our probabilistic model of long-range object detection,
when compared to a baseline planner using a 2D A* search, allows a robot to
more safely reach its goal. In the most challenging and obstacle-dense
forests, while the A* baseline is able to reach the goal in only 20% of
environments, our planner increases the navigation success rate to 75%.

  * •

An open-source, publicly available implementation of our planner and
simulation, available at https://github.com/brian-h-wang/multiple-hypothesis-
planner.

##  II Related work

###  II-A Path planning in unknown environments

Traditionally, path planning methods take as input a known map of the robot’s
surroundings, then use this map to plan a collision-free path to a given goal
state. Robot path planning methods include sampling-based approaches such as
rapidly-exploring random trees (RRTs) [16, 17], as well as minimum cost search
methods, often based on the A* algorithm [18]. Hybrid A* [19, 20] extends this
algorithm by associating each discrete step of the A* search with a continuous
vehicle state, ensuring that the final path is valid with respect to the robot
kinematics.

In practice, a robot generally lacks a detailed map of the environment in
advance and must construct one in real time using its onboard sensors.
Therefore, typical path planning pipelines work iteratively, re-planning as
the robot traverses the environment and updates its map with new sensor
measurements. A commonly used map structure is a binary 3D occupancy grid,
where cells are classified as either occupied or unoccupied. These occupancy
grids can be constructed using 3D sensors such as stereo cameras [3] or lidar
[21, 4, 22]. These works have significantly advanced the state of the art in
3D autonomous path planning, implementing the iterative re-planning approach
on 3D grids.

A key limiting factor on autonomous navigation is the maximum perception
horizon of the robot’s sensors [5]. At long ranges, sensor noise grows
significantly, with stereo cameras suffering from a quadratic increase in
depth estimation error with range [1, 2], and lidar point measurements
becoming sparse. Longer range planning carries computational costs as well;
for example, the cost to maintain a 3D occupancy grid grows cubically with the
size of the grid, forcing size- and weight-constrained robots to use a smaller
grid that does not make use of all available sensor information [4].

Planning only at short range limits the speed of safe robotic navigation, as
the robot must move slowly enough to be able to come to a stop at the boundary
of the perceived space, in case new obstacles are detected. [23] address this
issue by planning in the unknown space, allowing faster flight while
maintaining a safe backup trajectory that exists entirely in the nearer-range
known space. However, the planner in this work assumes the unknown space to
always be free. Ideally, a robot should be able to reason about uncertainty in
noisier, longer-range sensor measurements, and plan efficient paths that also
account for the imperfect knowledge of distant regions.

Additionally, complex and cluttered environments make efficient and agile
planning especially difficult. Banfi et al. [24] previously suggested that
reasoning about multiple path hypothesis could be beneficial when planning in
uncertain environments. However, the map model used in that work is based on a
3D occupancy grid, whose typical resolutions (15-30 cm, to enable real-time
processing) might preclude the existence of safe paths in dense environments.
The present work fills this gap.

Other previous works have specifically addressed navigation in forests, a
common outdoors environment which demonstrates the challenges of unstructured
and obstacle-dense environments. [25] and [14] develop theoretical guarantees
on the speed of flight through a forest modelled as a Poisson point process,
with the former additionally analyzing the effects of limiting the robot’s
sensing range. [26] and [27] develop methods for 2D localization in forests,
addressing the challenges of obstacle-dense environments with limited semantic
information differentiating tree trunk obstacles. [15] present a
computationally efficient method for path planning in a forest, by
constructing a lattice of candidate paths, then pruning it according to
detected obstacle locations. [22] present a forest search and rescue system,
mapping a UAV’s surroundings using a hybrid approach including a 3D occupancy
grid along with a data-efficient object-based representation of tree trunks,
then planning paths using A* search. This paper builds upon previous works by
reasoning about the _uncertainty_ in tree trunk obstacle estimates, when
planning paths through the forest.

###  II-B Perception for planning

Robotics researchers have begun using machine learning-based perception to
enable robots to more meaningfully understand their surroundings. [28], [29],
and [30] use 2D bounding box object detections as measurements for landmark-
based simultaneous localization and mapping (SLAM). In contrast to previous
SLAM methods, which heuristically pick out landmarks from 2D image pixels or
3D point cloud scans, the object detector enables the robot to reason at a
higher level, recognizing objects of interest, estimating their positions and
sizes, and localizing with respect to them.

Researchers have also used learned perception to augment robotic path planning
and navigation. [6] use a Bayesian neural network to identify potentially
unsafe regions in stereo images (for example, distinguishing muddy terrain
from a paved sidewalk), then use this information to plan safe paths that also
decrease the robot’s uncertainty about its surroundings, using a next-best
view planning approach. [7] train a neural network to predict drivable versus
unsafe terrain from sparse lidar scans. [31] extend A* search with semantic
weights applied to the graph edges. Finally, [5] use semantic labels on RGBD
camera images to identify from a distance regions where a UAV can safely fly
in urban environments. Ryll et al. [5] motivate their work with the specific
goal of raising the maximum speed for safe UAV flight, by increasing the
robot’s perception range. The common theme throughout these works is that
semantic information allows a robot to reason about further away regions in
the environment, where traditional 3D sensors give sparser and/or noisier
measurements. This capability increases the robot’s effective perception
range, allowing it to recognize potential unsafe areas at a further distance.

Similarly motivated, in this paper we explore the usage of 3D object detectors
for path planning. Recent works in 3D object detection have shown impressive
results for object detection using sensors such as lidar and stereo cameras.
Lidar provides highly precise 3D point clouds which have been used
successfully as inputs to 3D object detection methods [9, 10]. Recently, [1]
and [8], motivated by the lower cost, weight, and power consumption of stereo
cameras compared to lidar, proposed a pseudo-lidar data representation that
significantly closed the accuracy gap between lidar- and stereo camera-based
object detection methods.

One challenge with stereo camera 3D object detection is the fact that due to
significant noise, long-range stereo point clouds cannot be annotated with 3D
bounding boxes in a reliable and unbiased way. Previously, in [12], we
addressed this problem by introducing a self-supervised data labeling and
detector training pipeline. Our approach fuses a series of noisy point clouds
into a cleaned global point cloud, clusters obstacles in the fused point
cloud, then propagates obstacle locations back onto individual stereo point
clouds, using the pose history of the camera.

##  III Problem definition

###  III-A Forest environment

As an example unstructured and obstacle-dense environment for robotic path
planning, we consider the planar forest model previously studied in [25],
[14], and [15]. The robot’s objective is to navigate through the forest to a
2D goal point $\vec{x}_{\rm G}=\left(x_{\rm G},y_{\rm G}\right)$ specified in
a global coordinate frame, while avoiding densely distributed 2D obstacles
representing tree trunks. We model tree trunks as circular obstacles, as seen
from a top-down 2D perspective. The set

$\boldsymbol{O}=\\{\vec{O}_{i}=(x_{i},y_{i},d_{i}),\forall i\in[1,n_{\rm
obs}]\\}$ (1)

represents the actual obstacles in the environment, where $x_{i}$ and $y_{i}$
are the 2D coordinates of tree $i$ in the global reference frame, and $d_{i}$
is its diameter.

###  III-B Obstacle position and size estimation

The robot does not know the number of obstacles $n_{\rm obs}$, their
locations, or their sizes ahead of time, and estimates these variables using
noisy sensor measurements as it navigates.

We assume the robot estimates the 2D positions and diameters of obstacles as
Gaussian-distributed random variables. We define an obstacle estimate as

$\hat{O}_{i}\sim\mathcal{N}\left(\vec{\mu}_{O_{i}},\Sigma_{O_{i}}\right),$ (2)

where the estimate’s mean state vector contains the expected 2D position and
diameter of tree $i$,

$\vec{\mu}_{O_{i}}=\begin{bmatrix}\mu_{x,i}&\mu_{y,i}&\mu_{d,i}\end{bmatrix},$
(3)

and $\Sigma_{O_{i}}$ is a 3D covariance matrix encoding the uncertainty in the
estimate. The elements on the diagonal of $\Sigma_{O_{i}}$, which we denote
$\sigma_{xx,i}^{2}$, $\sigma_{yy,i}^{2}$, and $\sigma_{dd,i}^{2}$, are the
variances of the estimates of $x_{i}$, $y_{i}$, and $d_{i}$ respectively.
Gaussian estimates in this form can be produced by various estimation
algorithms such as the Kalman filter and its variants, or factor graph SLAM
[32].

These obstacle estimates are updated iteratively using newly received obstacle
measurements. We assume that at each time step $k$, the robot detects some
subset of the trees in the forest–for example, all visible trees within a
certain maximum range and angular field of view, except for any of those that
register as false negatives due to detector errors. We denote the set of trees
detected at time step $k$ as $\boldsymbol{D}_{k}\subseteq\boldsymbol{O}$.

For each tree $i$ in $\boldsymbol{D}_{k}$, the robot receives a range, bearing
and diameter measurement:

$\vec{z}_{i,k}=\begin{bmatrix}{r}_{i,k}&{\phi}_{i,k}&{d}_{i,k}\end{bmatrix}^{T}.$
(4)

We model each as a Gaussian variable,

${r}_{i,k}=\bar{{r}}_{i,k}+W_{i,k}^{r}\left(\bar{{r}}_{i,k}\right),$ (5)
${\phi}_{i,k}=\bar{{\phi}}_{i,k}+W_{i,k}^{\phi},$ (6)
${d}_{i,k}=\bar{{d}}_{i,k}+W_{i,k}^{d}\left(\bar{{r}}_{i,k},\bar{{d}}_{i,k}\right),$
(7)

where $\bar{{r}}_{i,k},~{}\bar{{\phi}}_{i,k}$ and $\bar{{d}}_{i,k}$ are the
measurement means, i.e. the true range and bearing from the robot to the
obstacle, and the obstacle’s true diameter. $W_{i,k}^{r},W_{i,k}^{\phi}$ and
$W_{i,k}^{d}$ are zero mean, Gaussian distributed random variables modeling
the sensor noise on these quantities. The range measurement noise depends on
the range from the robot to the obstacle, and we assume generally that the
measurement variance will increase with range, as studied in [2]. We also
assume that the diameter measurement noise may vary with range and with the
true size of the detected obstacle.

Given these range-bearing measurements, we calculate the 2D obstacle estimates
in (2) by solving a factor graph landmark SLAM problem, defining range and
bearing factors between the detected trees at time step $k$ and the
corresponding robot pose factor. Additionally, we estimate the obstacle sizes
from the diameter measurements.

###  III-C Local planner definition

Existing planning pipelines [4, 3, 23] generally include a _local planner_
which generates a dynamically feasible short-term trajectory for the robot.
This local planner complements the global planner, which plans a high-level
path that reaches all the way to the global goal, but does not necessarily
account for the robot dynamics due to computational constraints.

We assume the local planner takes as input a local goal position $\vec{x}_{\rm
L}=\left(x_{\rm L},y_{\rm L}\right)$, the robot dynamics function, and a map
representation, and outputs a series of 2D waypoints

$W_{\rm L}=\begin{bmatrix}x_{0}&y_{0}\\\ x_{1}&y_{1}\\\ \vdots&\vdots\\\
x_{n_{w}}&y_{n_{w}}\\\ x_{L}&y_{L}\end{bmatrix}$ (8)

which form a path from the robot’s current state
$\vec{q}_{0}=\begin{bmatrix}x_{0}&y_{0}&\theta_{0}\end{bmatrix}^{T}$ to the
goal position $\vec{x}_{\rm L}$ through $n_{w}$ waypoints. In the following
sections of this paper, we describe a modular high-level planner which
augments the local planner by providing a local goal computed using long-
range, uncertain object detections.

##  IV Multiple-hypothesis planning under uncertainty

Figure 2: Illustration of the components of our high-level planning method,
and their inputs and outputs.

We now present the steps of our multiple-hypothesis path planning method,
illustrated in Figure 2. Our planner takes as input the Gaussian obstacle
estimates defined in section III-B, constructs a navigation graph that
probabilistically represents possible routes through the forest, generates and
evaluates multiple path hypotheses, and finally outputs a local goal point for
the local planner described in section III-C.

###  IV-A Probability model for safe navigation between estimated obstacles

At a high level, a path through the forest is defined by a series of pairs of
obstacles, between which the robot passes as it moves towards the goal. In
order to reason about the safety of a given path through the forest, we derive
a model for calculating the probability that the robot will be able to safely
move between a pair of obstacles whose positions and sizes are estimated
assuming Gaussian uncertainty, as defined in section III-B. Figure 3
illustrates this problem.

Figure 3: Our approach models the probability that a robot of a given width is
able to safely pass between a pair of circular obstacles with Gaussian
distributed uncertainty in their 2D positions and radii. Figure 4:
Illustration of the width of the safe space between a pair of obstacles in 1D,
which is used to calculate the probability that the robot can safely pass
between the obstacles.

We begin by calculating the probability that a robot is able to safely move
between a pair of one-dimensional obstacles, then generalize this simpler
calculation to the two-dimensional case.

####  IV-A1 1D safety probability calculation

In one dimension, two obstacles $\vec{O}_{i}$ and $\vec{O}_{j}$ have scalar
positions $x_{i}$ and $x_{j}$, and widths $d_{i}$ and $d_{j}$. Assuming that
$x_{i}<x_{j}$, if the gap between the right edge of obstacle $\vec{O}_{i}$ and
the left edge of obstacle $\vec{O}_{j}$ is greater than the robot width, then
the robot will be able to safely move between the obstacles. For clarity in
the following derivations, we use the obstacle radii $r_{i}=\frac{d_{i}}{2}$,
$r_{j}=\frac{d_{j}}{2}$. The right edge point of $\vec{O}_{i}$ is then
$e_{i}=x_{i}+r_{i}$, and the left edge of $\vec{O}_{j}$ is
$e_{j}=x_{j}-r_{j}$. Then, the width of the free space between these obstacles
is

$\displaystyle S$ $\displaystyle=e_{j}-e_{i}$ (9)
$\displaystyle=x_{j}-x_{i}-(r_{i}+r_{j}).$ (10)

Figure 4 shows an example illustration of the free space $S$, calculated based
on the 1D positions and radii of a pair of obstacles.

Since we do not have perfect environment knowledge, we model the obstacle
positions and sizes as Gaussian random variables, with the uncertain position
and size of obstacle $\hat{O}_{i}$ being

$\hat{x}_{i}\sim\mathcal{N}\left(\mu_{x,i},\sigma^{2}_{x,i}\right),$ (11)
$\hat{r}_{i}\sim\mathcal{N}\left(\mu_{r,i},\sigma^{2}_{r,i}\right),$ (12)

and likewise for $\hat{O}_{j}$.

The estimated width of the free space $S$, as a linear combination of Gaussian
random variables according to (9), is then also Gaussian distributed as

$\hat{S}\sim\mathcal{N}\left(\mu_{S},\sigma^{2}_{S}\right),$ (13)

where

$\mu_{S}=\left(\mu_{x,j}-\mu_{r,j}\right)-\left(\mu_{x,i}+\mu_{r,i}\right),$
(14)
$\sigma^{2}_{S}=\sigma^{2}_{x,i}+\sigma^{2}_{r,i}+\sigma^{2}_{x,j}+\sigma^{2}_{r,j}.$
(15)

Let the known width of the robot be $w_{R}$. Then, the probability that our
robot can move between obstacles $\hat{O}_{i}$ and $\hat{O}_{j}$ is the
probability that the space between them is enough to avoid collision, i.e.
$P(S>w_{R})$. We can exactly compute this probability as $1$ minus the
cumulative distribution function (CDF) of $S$, giving us an expression for the
safety probability using the Gaussian CDF equation:

$P(S>w_{R})=\frac{1}{2}\left(1-\text{erf}\left(\frac{x-\mu_{S}}{\sigma_{S}\
\sqrt{2}}\right)\right),$ (16)

where the mean and standard deviation of $S$, respectively $\mu_{S}$ and
$\sigma_{S}$, are given by (14) and (15). erf denotes the error function used
to compute the Gaussian CDF.

####  IV-A2 2D safety probability calculation

We now solve for the probability that the robot can safely navigate between a
pair of 2D circular obstacles specified by their positions and diameters,
showing that the 2D problem can be transformed to the 1D case without loss of
generality.

Given a pair of 2D obstacle estimates
$\hat{O}_{i}=\left(\vec{\mu}_{\hat{O},i},\Sigma_{\hat{O},i}\right)$ and
$\hat{O}_{j}=\left(\vec{\mu}_{\hat{O},j},\Sigma_{\hat{O},j}\right)$, as
defined in (2), the angle between the world frame $x$-axis and the line
passing through both position estimate means is

$\theta_{ij}=\arctan\left(\frac{\mu_{y,j}-\mu_{y,i}}{\mu_{x,j}-\mu_{x,i}}\right).$
(17)

We can then compute the rotation matrix $R_{ij}\in SO(2)$, which rotates the
obstacle mean positions into a reference frame where the obstacle centers lie
along the transformed x-axis,

$R_{ij}=\begin{bmatrix}\cos{\left(\theta_{ij}\right)}&\sin{\left(\theta_{ij}\right)}\\\
-\sin{\left(\theta_{ij}\right)}&\cos{\left(\theta_{ij}\right)}\end{bmatrix}.$
(18)

Note that the negative $\sin$ term is located at the bottom-left of $R_{ij}$,
as this is a rotation by $-\theta_{ij}$.

We define the mean vector and covariance matrix which relate to only the
position of obstacle $i$, omitting the obstacle diameter, as

$\vec{\mu}_{xy,i}=\begin{bmatrix}\mu_{x,i}&\mu_{y,i}\end{bmatrix}^{T}$ (19)
$\Sigma_{xy,i}=\begin{bmatrix}\sigma^{2}_{xx,i}&\sigma^{2}_{xy,i}\\\
\sigma^{2}_{xy,i}&\sigma^{2}_{yy,i}\end{bmatrix},$ (20)

where $\Sigma_{xy,i}$ is the upper-left $2\times 2$ submatrix of the full
covariance matrix $\Sigma_{i}$.

We transform the position mean and covariance using $R_{ij}$, obtaining the
transformed mean and transformed covariance matrix

$\vec{\mu}_{xy,i}^{\prime}=R_{ij}\vec{\mu}_{xy,i}$ (21)
$\Sigma_{xy,i}^{\prime}=R_{ij}\Sigma_{xy,i}R_{ij}^{T},$ (22)

and likewise for the transformed mean and covariance of obstacle
$\hat{O}_{j}$.

We then marginalize over the transformed $y$-axis, to find the position mean
and variance along the transformed $x$ axis. This leave us with the simpler 1D
probability calculation, and we can then apply the equations for the 1-D safe
navigation probability, along the transformed $x$-axis.

Defining the components of the transformed mean and covariance of obstacle $i$
as

$\vec{\mu}_{xy,i}^{\prime}=\begin{bmatrix}\mu^{\prime}_{x,i}&\mu^{\prime}_{y,i}\end{bmatrix}^{T}$
(23)
$\Sigma_{xy,i}^{\prime}=\begin{bmatrix}\sigma^{2^{\prime}}_{xx,i}&\sigma^{2^{\prime}}_{xy,i}\\\
\sigma^{2^{\prime}}_{xy,i}&\sigma^{2^{\prime}}_{yy,i}\end{bmatrix},$ (24)

we can then express the mean and variance of the free space $S$ between the
two obstacles, _in the case of two-dimensional circular obstacles_ , as

$\mu_{S}=\left(\mu^{\prime}_{x,j}-\mu_{r,j}\right)-\left(\mu^{\prime}_{x,i}+\mu_{r,i}\right),$
(25)
$\sigma^{2}_{S}=\sigma^{2^{\prime}}_{xx,i}+\sigma^{2}_{r,i}+\sigma^{2^{\prime}}_{xx,j}+\sigma^{2}_{r,j}.$
(26)

Using these terms, we can then compute the probability of safe navigation
between the two obstacles using equation (16).

###  IV-B Navigation graph construction

Given estimates of the obstacle positions and sizes, we construct a graph
which represents the high-level paths the robot can take through the
environment to reach its goal. Our graph encodes information on _path
distance_ and _path safety_ , while also taking into account the _uncertainty
in obstacle estimates_. Figure 5 illustrates the graph construction, and
Algorithm 1 lists the steps of the process.

Algorithm 1 Constructing the navigation graph

Required: A method for computing the probability that the robot can safely
pass between a pair of obstacles, $P_{safe}(\hat{O}_{i},\hat{O}_{j})$, which
returns a probability in the set $\\{p\in\mathcal{R}\mid 0\leq p\leq 1\\}$.

1:

2:$\hat{O}_{i}=\left(\vec{\mu}_{O,i},\Sigma_{O,i}\right),i\in[1,n_{\rm obs}]$,
obstacle estimates.

3:$\vec{q}_{start}$, the robot’s starting state.

4:$\vec{x}_{\rm G}$, the global goal position.

5:$p_{\rm target}\in[0,1]$, the desired safety probability.

6:$r_{\rm short}\in\mathbb{R}_{\geq 0}$, the threshold between the short and
long range zones.

7:$w_{robot}\in\mathbb{R}_{\geq 0}$, the robot width.

8:

9:$V_{\rm N},E_{\rm N}$, the navigation graph structure.

10:$c_{\rm dist}$, the edge distance costs.

11:$p_{\rm safe}$, the vertex safety probabilities.

12:$R$, the vertex range zones.

13:$V_{\rm DT},E_{\rm DT}=DT(\hat{\boldsymbol{O}})$ // Delaunay triangulation.

14:$V_{\rm N}\leftarrow\emptyset$ // Initialize navigation graph vertices.

15:$E_{\rm N}\leftarrow\emptyset$ // Initialize navigation graph edges.

16:for all $e_{DT,ij}\in E_{\rm DT}$ do // Delaunay edge between obstacles i,
j.

17: // Compute distance from robot to obstacles.

18: $r_{i}=compute\\_distance(\vec{\mu}_{O,i},\vec{q}_{start})$

19: $r_{j}=compute\\_distance(\vec{\mu}_{O,j},\vec{q}_{start})$

20: if $r_{i}>r_{\rm short}$ or $r_{j}>r_{\rm short}$ then

21: $r\leftarrow long$

22: else

23: $r\leftarrow short$

24: end if

25: $prob=P_{safe}(\hat{O}_{i},\hat{O}_{j})$

26: if $prob<p_{\rm target}$ then

27: if $r=short$ then

28: $V_{new}=\emptyset$

29: else

30: $V_{new}=\\{midpoint(\vec{\mu}_{O,i},\vec{\mu}_{O,j})\\}$

31: end if

32: else

33: $V_{new}=\\{place\\_points(\hat{O}_{i},\hat{O}_{j},w_{robot})\\}$

34: end if

35: for all $v_{k}\in V_{new}$ do

36: $p_{\rm safe}(v_{k})=prob$

37: $c_{\rm safe}(v_{k})=-\log(prob)$

38: $R(v_{k})=r$

39: for all $v_{l}\in V_{\rm N}$ that share a Delaunay cell with $v_{k}$ do

40: $E_{\rm N}\leftarrow E_{\rm N}\cup\\{e_{kl}\\}$

41: $c_{\rm dist}(e_{kl})=distance(v_{k},v_{l})$

42: end for

43: end for

44: $V_{\rm N}\leftarrow V_{\rm N}\cup V_{new}$

45:end for

46:return $(V_{\rm N},E_{\rm N},c_{\rm dist},p_{\rm safe},c_{\rm safe},R)$

Figure 5: Steps of the graph construction process.

####  IV-B1 Delaunay triangulation cell decomposition

Previous works have used the Delaunay triangulation for localization in
forests, due to its ability to qualitatively describe regions in the
environment[26, 27]. Using the Delaunay triangulation algorithm [33], we
divide the workspace into triangular cells with obstacle center points as
their vertices. The Delaunay triangulation computes these cells such that each
triangular cell, defined by a triplet of obstacles, contains no other obstacle
center points within its boundaries. Figure 5.b) shows an example Delaunay
triangulation.

The edges between the Delaunay triangulation cells, which we will refer to as
_cell faces_ (to avoid confusion with the navigation graph edges which we will
define later), represent thresholds where the robot passes _between_ a pair of
obstacles, as described in [34].

The Delaunay triangulation is defined for our purposes as
$DT(\hat{\boldsymbol{O}})=(V_{\rm DT},E_{\rm DT},T_{\rm DT})$, where
$\hat{\boldsymbol{O}}$ is the set of estimated obstacles. $V_{\rm DT}$ are the
vertices of the triangulation, placed at the mean center positions of the
estimated obstacles, so that $V_{\rm
DT}=\\{\vec{\mu}_{xy,i}\mid\hat{O}_{i}\in\hat{\boldsymbol{O}}\\}$. $E_{\rm
DT}$ is the set of Delaunay triangulation cell faces, while $T_{\rm DT}$ is
the set of obstacle triplets that form the cells; therefore,
$\exists(\hat{O}_{i},\hat{O}_{j},\hat{O}_{k})\in T_{\rm DT}\iff\exists
e_{ij},e_{jk},e_{ik}\in E_{\rm DT}$. The circle circumscribed around the
vertices of the triangle formed by these obstacles contains no other obstacle
center points $\vec{\mu}_{\hat{O},l}$ such that $l\notin\\{i,j,k\\}$.

For each Delaunay cell face between a pair of obstacles $\hat{O}_{i}$ and
$\hat{O}_{j}$, we use Equation 16 to calculate the probability that the robot
can safely pass between $\hat{O}_{i}$ and $\hat{O}_{j}$, transitioning from
one Delaunay cell to the next. Since at a high level, a path through the
forest can be thought of as a series of obstacle pairs between which the robot
should pass, we can therefore equivalently define a path by a series of
Delaunay triangulation cells. In the following sections of this paper, we
define a graph structure over the Delaunay cells, over which we can search in
order to determine possible paths to the goal.

####  IV-B2 Definitions of graph terms

Before introducing our navigation graph construction procedure, we define the
following terms:

  * •

$p_{\rm target}\in[0,1)$, the _desired safety probability_ , specified as a
planner parameter. The planner will attempt to find a path whose safety (i.e.
the probability that the robot will be able to reach the global goal without
crashing into an obstacle by following this path) exceeds this threshold.

  * •

$G_{\rm N}=\left(V_{\rm N},E_{\rm N}\right)$, the navigation graph, defined by
its vertices and edges. The vertices of this graph are placed on the Delaunay
triangulation cell faces, and are connected by edges to other vertices on
adjacent cell faces. A search over this graph therefore finds a path passing
between a series of obstacle pairs in the forest. Figure 5.h) illustrates an
example navigation graph, showing the placement of vertices and edges.

  * •

$v_{i}\in V_{\rm N},e_{ij}\in E_{\rm N}$, the individual vertices and edges.

  * •

$c_{\rm dist}:e_{ij}\rightarrow\mathbb{R}_{\geq 0}$, a mapping which gives the
Euclidean distance cost for each navigation graph edge. $\mathbb{R}_{\geq 0}$
is the set of non-negative real numbers.

  * •

$p_{\rm safe}:v_{i}\rightarrow\\{p\in\mathbb{R}\mid 0\leq p\leq 1\\},\forall
i\in V_{\rm N}$, a mapping which gives the safety probability for each graph
vertex, i.e. the probability that the robot can safely move between the pair
of obstacles associated with this vertex.

  * •

$c_{\rm safe}:v_{i}\rightarrow\mathbb{R}_{\geq 0},$ defined as the safety cost
$c_{\rm safe}(v_{i})=-\log(p_{\rm safe}(v_{i})),\forall i\in V_{\rm N}$.

  * •

$\mathcal{R}=\\{short,long\\}$, the set of range zones. Our planner treats
vertices differently depending on their range zone, due to differing amounts
of sensor noise and estimation uncertainty at different sensor ranges.

  * •

$r_{\rm short}\in\mathbb{R}_{\geq 0}$, the range threshold defining the two
range zones. The threshold is defined as a planner parameter.

  * •

$R:v_{i}\rightarrow\mathcal{R}$, a range mapping which maps a vertex to the
range zone to which it belongs.

####  IV-B3 Vertex range zones

Obstacle estimation uncertainty varies significantly with distance from the
robot, due to increasing sensor measurement noise with range, and our graph
structure therefore stores obstacle range information so that the planner can
account for this variation.

As illustrated in Figure 5.b), we divide the estimated obstacles into two
_range zones_ : short and long, divided by the threshold $r_{\rm short}$.
Obstacles at distance less than $r_{\rm short}$ are considered short range,
and all others are considered long range. We assume that at short range,
obstacle position and size estimates are fairly confident, having been updated
recursively with multiple range-bearing measurements from detections. Beyond
the short range, obstacle estimates contain significant uncertainty due to
higher measurement noise at further ranges.

####  IV-B4 Navigation graph vertex placement

The placement of vertices in the graph depends on obstacle estimate safety
probabilities as well as their range zones. For safe Delaunay cell faces, i.e.
$p_{\rm safe}(e_{DT,ij})\geq p_{\rm target}$, we allow the robot to plan paths
between the estimated obstacles $\hat{O}_{i}$ and $\hat{O}_{j}$, and we add
corresponding vertices to the graph as shown in Figure 5.c).

For an unsafe Delaunay cell face, i.e. $p_{\rm safe}(e_{DT,ij})<p_{\rm
target}$, the decision on whether or not to add graph vertices along this cell
face depends on the range zone of obstacles $\hat{O}_{i}$ and $\hat{O}_{j}$.
At short range, we do not place graph vertices on unsafe Delaunay cell faces,
as demonstrated in Figure 5.e). When planning around close-by obstacles, the
robot may not have enough time to collect more measurements and update its
estimates of these obstacles. Therefore, the robot should not risk a crash by
passing between unsafe obstacle pairs in the short range.

In contrast, path planning between uncertain long-range obstacles should take
into account the fact that the robot will have time to safely collect
additional sensor information, and update its estimate of these obstacles
before closely approaching them. Preventing the robot from planning between
uncertain, far-off obstacles will likely lead to excessively conservative
planning behavior in obstacle-dense environments. During the graph
construction, we therefore place navigation graph vertices on Delaunay cell
faces between unsafe long-range obstacles, recognizing that as the robot
receives more detections, some of these uncertain vertices will resolve to
safe and viable navigation paths. The multiple-hypothesis planner we introduce
in the next section of this paper intelligently considers multiple of these
candidate paths, making use of uncertain information from the obstacle
estimates.

We place multiple vertices on long Delaunay cell faces which are determined to
be very safe. These cell faces correspond to very wide gaps between obstacles,
where the overall path distance may change significantly depending on whether
the robot decides to pass nearby one of the obstacles or the other. Placing
multiple vertices on these cell faces therefore allows the graph search to
better approximate the geometric length of paths.

For each new vertex added on Delaunay cell face $e_{DT,ij}$, we assign the
vertex to the short range zone if both obstacles $\hat{O}_{i}$ and
$\hat{O}_{j}$ are within the short range threshold $r_{\rm short}$, and assign
the vertex to the long range zone if _either_ obstacle is outside the short
range.

We record the Delaunay cell face which each graph vertex lies on, and we
additionally save the safety probability of the Delaunay cell face as $p_{\rm
safe}(v_{k})=P_{safe}(\hat{O}_{i},\hat{O}_{j}),\forall k\in V_{DT}$. The saved
range zone and safety probability information will later be used by the
multiple-hypothesis planner.

####  IV-B5 Graph edge construction

For each Delaunay cell, we add graph edges to $E_{\rm N}$ connecting each
vertex on one of the cell faces with all vertices on other cell faces, as
shown in Figure 1.d). For each $e_{ij}\in E_{\rm N}$, we set the distance cost
$c_{\rm dist}(e_{ij})$ to the 2D Euclidean distance between the 2D positions
of vertices $v_{i}$ and $v_{j}$.

Finally, we add the robot start position and goal positions as graph vertices
$v_{start}$ and $v_{goal}$, shown in Figure 1.h). If the start and/or goal lie
within a Delaunay triangulation cell, we connect them via graph edges,
weighted by 2D Euclidean distance, to all graph vertices that lie on the faces
of this cell. If either the start or goal lies outside all Delaunay cells, we
connect them to all visible vertices on the cell faces at the boundary of the
Delaunay triangulation (i.e. all vertices which can be connected by a straight
line segment in 2D to the start/goal, without the line segment passing through
any other cell faces). We set $p_{\rm safe}(v_{start})=p_{\rm
safe}(v_{goal})=1.0$, i.e. the robot does not incur any safety cost traveling
through the start and goal vertices.

###  IV-C Multiple-hypothesis planning under uncertainty

Figure 6: Example of the multiple-hypothesis planning procedure using the
navigation graph. The planner generates hypotheses of the environment, until
finding a safe path–in this case, the pink path–with a safety probability over
the desired safety $p_{\rm target}$, or until generating the maximum allowed
number of hypotheses.

We now present a high-level planning method which uses the constructed
navigation graph to generate and evaluate candidate paths to the goal, based
on multiple environment hypotheses that take into account the uncertainty in
obstacle estimates. Algorithm 2 outlines the multiple-hypothesis planning
algorithm.

####  IV-C1 Definitions of planner terms

We define the following terms, used in addition to those defined in IV-B2 to
describe our multiple hypothesis planning algorithm.

  * •

$H:v_{i}\rightarrow\\{0,1\\}$, a hypothesis of the environment, defined as a
mapping which takes as input a navigation graph vertex and outputs either a
$0$, indicating that _in this hypothesis_ , the vertex is not safe to pass
through, or a $1$, indicating that it is safe.

  * •

$P\subset V_{\rm N}$, a path through the graph, defined as an _ordered_ subset
of the graph vertices. For any vertex $v_{i}$ at index $P[k]$ in the path, and
vertex $v_{j}$ at index $P[k+1]$, there must exist a graph edge $e_{i,j}\in
E_{\rm N}$.

The planner uses the following parameters:

  * •

$N_{\rm hyp}\in\mathbb{N}_{\textbf{}>0}$, the maximum number of path
hypotheses to generate.

  * •

$p_{\rm min}\in[0,1)$, with $p_{\rm min}<p_{\rm target}$ a minimum safety
probability for a graph vertex to be considered by the multiple hypothesis
planner. This parameter is optional, and by default can be set to zero,
causing it to have no effect on the planner.

  * •

$\alpha_{\rm dist}\in\mathbb{R}_{\geq 0}$, the weight on path length, and
$\alpha_{\rm safe}\in\mathbb{R}_{\geq 0}$, the weight on path safety. These
weights are used to evaluate the candidate paths in order to decide on the
best overall path.

####  IV-C2 Hypothesis generation

Our planner requires a shortest path search algorithm $SP(V_{\rm N},E_{\rm
N},c_{\rm dist},H)$. The input $H$ is a _hypothesis_ , which excludes some
subset of the vertices $v_{i}\in V_{\rm N}$ that are considered to be unsafe
(i.e. $H(v_{i})=0$).

We first construct an initial hypothesis $H_{0}$ with all graph vertices
marked as safe, i.e. $\forall v_{i},H_{0}(v_{i})=1$. Optionally, if a minimum
probability $p_{\rm min}$ greater than zero is specified, for any vertices
with $p_{\rm safe}(v_{i})<p_{\rm min}$, we set $H_{0}(v_{i})$ to 0. The
threshold $p_{\rm min}$ can be used to exclude extremely unsafe vertices from
the multiple hypothesis planner, making the path search more efficient.

The planner then computes the shortest path $P_{0}=SP(V_{\rm N},E_{\rm
N},c_{\rm dist},H_{0})$, finding the shortest possible path to the goal
according only to the Euclidean distances given by the navigation graph edge
distance costs $c_{\rm dist}(e_{ij})$. We initialize a set of candidate paths
$\mathcal{P}$, at first containing only $P_{0}$. Figure 6.a) illustrates this
step.

The planner then generates additional candidate paths, up to $N_{\rm hyp}$ in
total, each planned under a different hypothesis of which vertices in the
graph are safe to pass through. We store the graph vertices $v_{i}\in P_{0}$
in a priority queue $\mathcal{Q}$. Each vertex is stored along with a copy of
$H_{0}$. We denote the priorities used for the queue as $\pi_{0,i}$ and
calculate the priority for each vertex $v_{i}$ as $\pi_{0,i}=-(1-p_{\rm
safe}(v_{i}))$, the negative of the likelihood that vertex $v_{i}$ is unsafe.
Computing the priorities in this way allows us to draw the vertex which is
most likely to be unsafe from the queue at each iteration of the multiple
hypothesis path search.

At each planner iteration, we use $\mathcal{Q}$ to find the graph vertex
$v_{i}$ which is most likely to be unsafe. We pop this vertex from the queue,
with its associated hypothesis $H_{j}$, then initialize a new hypothesis
$H_{k}$ as a copy of $H_{j}$. We then set $H_{k}(v_{i})=0$.

We then plan the shortest path _under this hypothesis_ , $P_{k}=SP(V_{\rm
N},E_{\rm N},c_{\rm dist},H_{k})$, finding the shortest path assuming that the
graph vertex $v_{i}$ is unsafe. This new path will be added to the set of
candidate paths if it is not a duplicate of an existing path, and if the
safety of the path evaluated only over the short range vertices in the path is
above $p_{\rm target}$. This condition guarantees that the robot will not
execute a path that is unacceptably unsafe in the short range.

If the new path $P_{k}$ passes these checks, we add it to the set of candidate
paths $\mathcal{P}$, and add all vertices on the path to the queue
$\mathcal{Q}$, along with hypothesis $H_{k}$. For each vertex $v_{l}\in
P_{k}$, the priority $\pi_{k,l}$ is stored as $(1-p_{\rm
safe}(v_{l}))\cdot\pi_{k,i}$, indicating that the likelihood of a hypothesis
depends conditionally on multiple graph vertices being unsafe. Figure 6.b-c
show the iterative process of planning, excluding graph vertices using the
priority queue, then planning again under the new hypothesis.

We terminate the search once the maximum number of hypotheses has been
reached, i.e. $|\mathcal{P}|=N_{\rm hyp}$, or a path $P_{i}$ is found such
that the safety of $P_{i}$ is above the desired safety probability, i.e.
$\prod_{v_{j}\in P_{i}}p_{\rm safe}(v_{j})\geq p_{\rm target}$. In calculating
the path safety in this way, we assume that the values $p_{\rm safe}(v_{i})$
of different graph vertices are independent of one another. The output of the
search process is a set of candidate paths through the navigation graph, as
shown in the example in Figure 6.d.

####  IV-C3 Candidate path evaluation

Given the set of paths $\mathcal{P}$, we calculate the _distance_ and _safety_
costs for each path, as

$C_{\rm dist}(P_{i})=\sum_{(v_{j},v_{k})\in P_{i}}c_{\rm dist}(e_{jk}),$ (27)

and

$C_{\rm safe}(P_{i})=\sum_{v_{j}\in P_{i}}c_{\rm safe}(v_{j}).$ (28)

Since the distance costs and safety costs are on different scales, we
normalize both across all paths. For all $P_{i}\in\mathcal{P}$:

$C_{\rm dist}(P_{i})=\frac{C_{\rm
dist}(P_{i})}{\max_{P_{j}\in\mathcal{P}}C_{\rm dist}(P_{j})},$ (29) $C_{\rm
safe}(P_{i})=\frac{C_{\rm safe}(P_{i})}{\max_{P_{j}\in\mathcal{P}}C_{\rm
safe}(P_{j})}.$ (30)

Finally, we compute the overall cost of each path

$C_{\rm total}(P_{i})=\alpha_{\rm dist}\cdot C_{\rm dist}(P_{i})+\alpha_{\rm
safe}\cdot C_{\rm safe}(P_{i}),$ (31)

and use the final path with minimum cost

$P^{*}=\operatorname*{arg\,min}_{P_{i}\in\mathcal{P}}\left(C_{\rm
total}(P_{i})\right).$ (32)

####  IV-C4 Local goal computation

Starting from the robot position, at vertex $v_{0}$ in $P^{*}$, we then
calculate the 2D goal point for the local planner, $\left(x_{\rm L},y_{\rm
L}\right)$, by finding the point along the 2D line segments $e_{ij}$, such
that $\left(v_{i},v_{j}\right)\in P^{*}$, which is ahead of the robot by a
plan-ahead distance $d_{\rm local}\in\mathbb{R}_{\geq 0}$. This approach uses
the high-level path $P^{*}$ to guide the local planner, which handles the
details of generating a dynamically-feasible trajectory for the robot and
accounting for the specific geometry of nearby obstacles.

Algorithm 2 Multiple-hypothesis planning in the navigation graph

Required: A shortest path graph search algorithm $SP(V_{\rm N},E_{\rm
N},c_{\rm dist},H)$ which returns a path $P$.

1:

2:$V_{\rm N},E_{\rm N}$, the graph structure.

3:$c_{\rm dist}$, the edge distance costs.

4:$p_{\rm safe}$, the vertex safety probabilities.

5:$R$, the vertex range zones.

6:$p_{\rm target}$, the desired safety probability.

7:$p_{\rm min}$, the minimum vertex safety to consider.

8:

9:$\mathcal{P}$, a set of candidate paths.

10:procedure Generate initial hypothesis $H_{0}$

11: for all $v_{i}\in V_{\rm N}$ do

12: if $p_{\rm safe}(v_{i})<p_{\rm min}$ then

13: $H_{0}(v_{i})\leftarrow 0$ // 0 indicates unsafe vertex.

14: else

15: $H_{0}(v_{i})\leftarrow 1$ // 1 indicates safe vertex.

16: end if

17: end for

18:end procedure

19:Find the shortest path $P_{0}=SP(V_{\rm N},E_{\rm N},c_{\rm dist},H_{0})$.

20:Initialize empty priority queue $\mathcal{Q}$.

21:for all $v_{i}\in P_{0}$ do

22: // Use vertex safeties to compute queue priorities.

23: $\pi_{0,i}\leftarrow-(1-p_{\rm safe}(v_{i}))$

24:
$queue\\_insert\left(\mathcal{Q},\pi_{0,i},\left(v_{i},H_{0}\right)\right)$

25:end for

26:$\mathcal{P}\leftarrow\\{P_{0}\\}$ // Initialize set of candidate paths.

27:procedure Multiple Hypothesis Search

28: while $|\mathcal{P}|<N_{\rm hyp}$ do

29: // Get most unsafe vertex from $\mathcal{Q}$.

30: $\pi_{j,i},v_{i},H_{j}=queue\\_pop(\mathcal{Q})$.

31: $H_{k}\leftarrow H_{j}$ // Copy previous hypothesis.

32: $H_{k}(v_{i})\leftarrow 0$ // Set new vertex to unsafe.

33: $P_{k}=SP(V_{\rm N},E_{\rm N},c_{\rm dist},H_{k})$ // Path search.

34: $P_{k,short}=\\{v_{l}\in P_{k}\mid R(v_{l})=short\\}$

35: if $safety(P_{k,short})<p_{\rm target}$ then

36: continue // Path unsafe in short range

37: end if

38: if $P_{k}\notin\mathcal{P}$ then // Disallow duplicate paths.

39: $\mathcal{P}\leftarrow\mathcal{P}\cup\\{P_{k}\\}.$

40: if $safety(P_{k})\geq p_{\rm target}$ then

41: break // Found safe path, end search.

42: end if

43: for all $v_{l}\in P_{k}$ do

44: $\pi_{k,l}\leftarrow(1-p_{\rm safe}(v_{l}))\cdot\pi_{j,i}$

45: $queue\\_insert(\mathcal{Q},\pi_{j,i},(v_{l},H_{k}))$

46: end for

47: end if

48: end while

49:end procedure

50:return $\mathcal{P}$

###  IV-D Planner safety guarantees

Given the graph structure, paths through the graph, and formal probabilities
of a robot passing safely between a pair of obstacles, one can calculate
probability guarantees for each path.

Given a total number of vertices in $P_{i}$, assuming each passing through a
pair of obstacles is an independent event, the probability of a collision for
$P_{i}$ is

$p_{\textrm{col}}(P_{i})=\left(1-\prod_{v_{j}\in P_{i}}p_{\rm
safe}(v_{j})\right)$ (33)

One could also simply consider the worst case probability, or

$p_{\textrm{col}}(P_{i})\geq\left(1-\min_{v_{j}\in P_{i}}p_{\rm
safe}(v_{j})\right)$ (34)

##  V Experiments and results

###  V-A Simulated forest environments

We evaluate our planner in a large-scale experimental trial, using randomly
generated simulated forest environments. We vary the complexity of the
simulated forests, in terms of the overall density of trees and their
distribution, in order to test our hypothesis that our probabilistic planner
should enable safer navigation in more complex and obstacle-dense
environments.

We generate our forests by specifying a desired tree density per area, then
sampling the total number of trees in the environment according to the Poisson
distribution as in [25]. Tree sizes are uniformly distributed in a defined
range, and we sample trees such that they do not overlap with each other. We
first conduct simulations in forest environments of varying densities, where
tree positions are uniformly sampled across the forest bounds. Noting that
through areas of uniformly distributed trees, the best path through the forest
mostly involves driving in a straight line to the goal with only small
deviations, we then conduct an additional experiment in the more challenging
case of forests containing three Gaussian-distributed clusters of trees (along
with additional uniformly distributed obstacles), which lie directly between
the robot and its goal. The robot generally cannot safely pass through these
regions, so identifying and avoiding them preemptively is the most efficient
way to reach the goal. Figure 7 shows two example simulated forests.

Figure 7: Example randomly generated forest environments, at the highest
density setting $\rho=0.3$ trees per square meter. The robot’s goal is to
navigate from $(x=0$\mathrm{m}$,y=5$\mathrm{m}$)$ to
$(x=40$\mathrm{m}$,y=5$\mathrm{m}$)$. Top: Uniformly distributed tree
positions. Bottom: Poisson forest with three Gaussian-distributed tree
clusters added.

###  V-B Path planner system implementation

####  V-B1 Obstacle estimation

We define a detection range noise model consistent with the noise model for
the Stereolabs ZED camera111https://support.stereolabs.com/hc/en-
us/articles/206953039-How-does-the-ZED-work. The standard deviation of the
range measurement noise is proportional to the distance from the sensor, with
this proportion growing quadratically with distance. The robot receives range
and bearing measurements of all obstacles within a given maximum range and
front-centered field of view, except for any obstacles which are fully
occluded.

In order to estimate the obstacle positions, we perform factor graph SLAM with
range and bearing obstacle measurements, as described in section III-B. We use
the iSAM2 [35] implementation provided by the GTSAM
library222https://gtsam.org/, and treat the obstacles as landmarks in landmark
SLAM. The range-bearing measurements define binary (i.e. involving two
variables in the graph) factors that probabilistically constrain the obstacle
positions with respect to the robot pose at each time step.

We assume the position and size of an obstacle are decoupled, so the cross-
covariance terms for position and size in $\Sigma_{O_{i}}$ are zero. We
estimate the landmark sizes using simple 1D Kalman filters, in parallel with
the SLAM obstacle position estimation.

In our simulation, the robot receives obstacle detections at a lower rate than
odometry measurements. At time steps where no detections are available, we
perform SLAM updates using odometry only. In order to isolate our study of the
path planner from localization error effects, our experiments use simulated
odometry measurements with negligible noise, so the localization problem is
effectively solved and the estimation practically reduces to a mapping
problem.

The true assignment of detections to landmark identities is unknown to the
robot. Following [32], we perform data association using the Mahalanobis
distance between new detections and existing landmark estimates, in order to
account for the estimate uncertainties during assignment. In order to compute
the Mahalanobis distance, we transform the obstacle estimate 2D position
uncertainty to the range-bearing space. We discard any data association
matches whose Mahalanobis distance falls over a set threshold, and initialize
new landmarks for any detections that remain unmatched after association.

####  V-B2 Hybrid A* local planner

We simulate a 2D differential drive robot which plans iteratively through the
2D forest environment. The robot uses a high-level global planner to specify a
short-term goal for a local planner to follow, as described in section III-C,
and re-plans as it moves and obtains new sensor measurements.

For our experiments, we implement a hybrid A* local planner based on [19],
[20], and [36]. Hybrid A* produces smooth, drivable paths which obey the robot
kinematics, and has previously been proven successful in practical robotic
systems. The local goal point for hybrid A* is generated by our multiple-
hypothesis planner using the approach described in section IV-C4. We note that
the multiple-hypothesis planner is agnostic to the specific choice of local
planner.

We run the hybrid A* search using the current obstacle estimate means as
obstacles. Since hybrid A* is a local planner, it operates only in a short
range where sensor noise is minimal and estimate means are reliable. We check
for obstacle collisions by bloating each obstacle’s radius by half the robot
width, and then modeling the robot as a point robot.

As mentioned in [19], a common pitfall with the hybrid A* planning algorithm
is that the returned path can contain unnecessary side-to-side turns, due to
the fixed motion primitives used by the algorithm, and can also pass very
close to obstacles due to the search finding the shortest possible path.
Therefore, as in Dolgov et al. [19], we use a gradient descent path smoother,
optimizing for the distance of the path from nearby obstacles as well as the
smoothness of the path.

####  V-B3 Baseline global planner

In our experiments, we compare our multiple-hypothesis planner to a baseline
global planner which uses a 2D A* search to find the shortest path to the
goal, but does not account for the uncertainty in obstacle estimates. The
baseline A* planner performs a search in 2D over a fixed-resolution grid. The
search does not pass through grid cells which are in collision with an
estimated obstacle mean. This search ignores the robot kinematics, and
therefore is much quicker to execute than the 3D hybrid A* search. This lower
computation time is required since the global planner searches over a much
longer distance to the goal, compared to the local planner which plans a path
only a few meters in length. After the 2D A* global planner computes a path to
the global goal, the point $d_{\rm local}$ distance ahead of the robot on this
path is selected and used as the goal for the hybrid A* local planner, in
order to find a short-range kinematically feasible path for the robot. We use
the same local planner for the baseline as we do for our multiple-hypothesis
planner, in order to perform a controlled comparison.

###  V-C Quantitative analysis

Figure 8: Results from simulating a differential drive robot navigating
through 20 randomly generated Poisson forests with _uniformly distributed tree
positions_. The straight-line distance from the start to the goal, ignoring
obstacles, is 40 meters. We run our experiments with three different settings
for $\rho$, the density of trees in the forest (indicated by the three
different colors), and with different maximum numbers of hypotheses $N_{\rm
hyp}$ allowed for the planner. Zero hypotheses indicates the 2D A* baseline
global planner. The height of the bars indicate the number of generated
forests in which the robot did not crash into an obstacle while navigating to
the goal. The colored portions of the bars indicate the number of forests in
which the robot successfully reached the goal. The gray portions of the bars
indicate runs where the robot stopped early (without crashing into an
obstacle), either due to the global planner believing there is no safe path to
the goal available, or due to hybrid A* planner failure. Left: Results using a
target safety probability of $p_{\rm target}=0.95$ for the multiple hypothesis
planner. Right: Results using $p_{\rm target}=0.999$. Figure 9: Results of
planner successes in forests which contain Gaussian-distributed tree clusters
between the robot’s starting position and the goal point. Subplot arrangement
is the same as in Figure 8. Left: Results using a target safety probability of
$p_{\rm target}=0.95$ for the multiple hypothesis planner. Right: Results
using $p_{\rm target}=0.999$

We experimentally evaluate our multiple hypothesis planner in randomly
generated forests, comparing it to the baseline 2D A* global planner. In order
to study our planner’s effectiveness in different types of environments, we
vary the forest density and distribution of trees (including forests with
uniformly distributed trees and forests containing nonuniform clusters of
trees, as described in Section V-A). We generate 20 random forests for each
combination of tree density and distribution, and run the multiple hypothesis
planner varying the parameters $N_{\rm hyp}$ and $p_{\rm target}$. The
simulated robot re-plans iteratively at a rate of 1Hz, and obtains new
detections at a rate of 2Hz. The robot is able to detect all non-occluded
obstacles within a maximum range of 20 meters that lie in a front-facing
sensor field of view of 110 degrees. We set the graph construction to ignore
estimated obstacles at a distance of greater than 15 meters, as we find that
near the sensor’s maximum range, obstacle estimates have received very few
associated detections (or only a single detection), and therefore contain too
high uncertainty to be useful for planning. The robot’s speed varies between
$1~{}$\mathrm{m}\mathrm{/}\mathrm{s}$$ and
$5~{}$\mathrm{m}\mathrm{/}\mathrm{s}$$, driving more slowly when an obstacle
is nearby. Finally, the planner internally adds a barrier of artificial
obstacles around the limits of the environment in order to keep the robot
within the boundaries of the simulated environment.

Figure 8 shows simulation results from the uniformly distributed forest
environments, with multiple numbers of maximum hypotheses $N_{\rm hyp}$ and
two different settings for $p_{\rm target}$. We plot the number of cases in
which the robot successfully reached the goal, indicated by the colored bars.
The _gray portions of the bars_ indicate cases where the robot was unable to
find a path to the goal and stopped, rather than crashing into an obstacle.
This can occur either when the hybrid A* local planner fails to find a path to
the local goal, or the global planner fails to find a path to the global goal.
In the case of the multiple hypothesis global planner, this failure indicates
that the planner believes no path above safety $p_{\rm target}$ exists in the
navigation graph.

The overall success rate of the planner is high in the uniform forest
environments, even when using the baseline global planner, as the robot can
generally avoid uniformly distributed obstacles by driving mostly in a
straight line towards the goal, making slight deviations around an obstacle
when needed. Still, we observe a higher rate of crashes using the baseline
planner in higher density forests. In the highest density forests, with
$\rho=0.3~{}$\mathrm{t}\mathrm{r}\mathrm{e}\mathrm{e}\mathrm{/}\mathrm{m}^{2}$$,
the baseline crashes 3 times out of 20 and stops once, compared to 1 crash for
the medium density and no crashes in the lowest density forest. These findings
show that denser obstacles, even in the relatively simple uniformly
distributed forests, are still more challenging for navigation.

Our multiple hypothesis planner with $p_{\rm target}=0.95$ closely matches the
performance of the baseline, likely due to the already low rate of crashes. In
the highest density forests, the 5-hypothesis planner crashes twice and stops
twice, out of 20 generated forests. With $p_{\rm target}=0.999$, we see that
the multiple hypothesis planner achieves a higher rate of safe navigation
compared to the baseline. The planner always safely reaches the goal in the
low and medium density forests, and in the high density case, the planner
never crashes with $N_{\rm hyp}=3$, and crashes once out of 20 forests for the
1, 2, and 5-hypotheses cases.

We then analyze the performance of our planner in non-uniformly distributed
forests, containing clusters of obstacles that the robot should avoid, with
results shown in Figure 9. We observe that in these environments, the 2D A*
baseline global planner performs poorly, reaching the goal safely only 4 out
of 20 times in the forests, and overall demonstrating a much lower rate of
safe navigation as compared to experiments in the uniform forests.

In these more challenging, non-uniform forests, the multiple-hypothesis
planner significantly increases the rate at which the robot is able to safely
reach the goal. At $p_{\rm target}=0.95$, using just a single hypothesis in
the high density forests triples the number of navigation successes compared
to the baseline, from 4 to 12. Using 5 hypotheses further increases the number
of successes to 15. We also see improvements in the multiple hypothesis
planner’s success rate, compared to the baseline, in the low and medium
density forests.

Increasing the target safety probability to $p_{\rm target}=0.999$ further
decreases the number of crashes in the non-uniformly distributed forests. At
this setting, the 5-hypothesis planner never crashes in the low and medium
density cluster forests, and crashes twice in the high density forests as
compared to four times for the $p_{\rm target}=0.95$ 5-hypothesis planner.
However, the increased $p_{\rm target}$ does cause the robot to stop more
often, due to being unable to find a safe enough path to the goal. The planner
stops most often in the high density forests, with the 5-hypothesis planner
succeeding 11 times, stopping 7 times, and crashing 2 times in the high
density forests. This effect is intuitive for the higher safety threshold, as
the planner will be more conservative overall. We note that depending on the
application, a robot stopping when unable to find a path is likely more
desirable than crashing into an obstacle. Overall, at both $p_{\rm target}$
settings, the multiple hypothesis planner significantly outperforms the
baseline at all forest densities, increasing the number of successful
navigation runs and/or reducing the number of crashes by stopping before
hitting obstacles.

We note that the actual observed success rate of the planner often does not
match the target safety probability $p_{\rm target}$ exactly. The planner
targets a safety probability $p_{\rm target}$ for each planned path, but since
multiple re-planning iterations are required to reach the goal, the overall
likelihood that _all_ of the planned paths are safe is below $p_{\rm target}$.
Additionally, the other components of the planning pipeline, including
measurement data association and the local hybrid A* planner, can occasionally
introduce other failures that cause crashes (for example, hybrid A* planning
too close to an obstacle due to approximations when checking collisions during
the motion primitives). Still, our findings do show increased safety for the
higher setting of $p_{\rm target}$, indicating that the path safeties
approximated using our safety probability model and high-level graph
representation do in fact correspond to higher rates of the robot actually
reaching its goal.

###  V-D Qualitative analysis

Figure 10: Behavior of the 1-hypothesis and 5-hypothesis planners, plotted
with the hybrid A* planner trajectory, in a high-density randomly generated
forest. The 1- and 5-hypothesis planners use $p_{\rm target}=0.95$.

We present several visualizations of the planner’s behavior in order to
demonstrate the effects of using multiple plan hypotheses, as well as of
adjusting the planner’s path safety threshold.

Figure 10 shows the trajectories taken by the robot through a randomly
generated forest (containing nonuniformly distributed obstacle clusters at the
highest obstacle density setting), using the 1-hypothesis, 5-hypothesis, and
hybrid A* baseline planners. The hybrid A* planner crashes into an obstacle,
as it attempts to navigate between a pair of close-together obstacles. The
single hypothesis planner successfully reaches the goal, but backtracks and
changes course multiple times, as it reacts to newly obtained sensor
measurements of nearby obstacles. The 5-hypothesis planner reaches the goal
with the smoothest path overall, as it accounts for further-away, uncertain
obstacles when planning.

(a) Zoomed-in view comparing paths generated with the planner using 1
hypothesis versus 5 hypotheses.

(b) Navigation graph constructed by the planner, and the path computed by the
single-hypothesis planner.

(c) Multiple path hypotheses generated by the multiple-hypothesis planner with
up to 5 maximum hypotheses allowed, showing the best path (green) and two
alternative hypotheses that were considered by the planner.

Figure 11: Comparison between the behaviors of the 1-hypothesis and
5-hypothesis versions of our planner. Both planners use $p_{\rm target}=0.95$.

Figure 11 zooms in on an example case to demonstrate how the planner behaves
differently when using 1 hypothesis versus 5 hypotheses. The robot
trajectories, plotted in Figure 11a show that the 1-hypothesis planner finds a
safe path around the cluster of obstacles ahead, but has to closely approach
these obstacles before determining there is no path through them with a 95%
likelihood of being safe. In contrast, the 5-hypothesis planner identifies
ahead of time that a safer, but slightly longer distance, path exists by
traveling around the cluster. Figures 11b and 11c illustrate the navigation
graph constructed by the planner, and the path hypotheses considered by the 1-
and 5-hypothesis versions of the planner. The 1-hypothesis planner computes
only the shortest path to the goal in the navigation graph, which passes
through an uncertain, narrow gap between two obstacles, ultimately leading to
the robot needing to backtrack away from these obstacles. In contrast, the
5-hypothesis planner identifies a path that takes a slightly longer route
around these obstacles, but has a safety probability near 1.0. The multiple-
hypothesis planner chooses this as the best path, trading off distance for a
much safer path.

Finally, Figure 12 demonstrates the effects of changing the desired path
safety probability $p_{\rm target}$, showing the behavior of the 5-hypothesis
planner with a desired path safety of $p_{\rm target}=0.95$ versus with
$p_{\rm target}=0.999$, in three different forest environments. In Figure 12a,
the lower-safety threshold planner crashes as it attempts to navigate a gap
between several obstacles, while the higher-safety planner identifies this
unsafe region and avoids it. However in other cases, the more conservative
behavior of the higher safety threshold planner can cause the robot to take
longer to reach the goal, as seen in Figure 12b, or to fail to find a path
when a safe path does in fact exist, as seen in Figure 12c. These multiple
cases show that the safety threshold $p_{\rm target}$ significantly affects
the planned path, and should be set depending on the desired trade-off of
safety with the time to reach the goal.

(a) A case where the 5-hypothesis planner crashes with the lower setting for
the safety probability threshold $p_{\rm target}$, but finds a safe route to
the goal using the higher setting.

(b) A case where the planner reaches the goal successfully with both settings
of $p_{\rm target}$, but takes a longer detour when using the higher setting.

(c) A case where the planner reaches the goal with the lower setting for
$p_{\rm target}$, but stops when using the higher setting, due to the planner
being unable to find a path that satisfies the safety threshold.

Figure 12: Multiple cases showing how the behavior of the 5-hypothesis planner
changes when the planner searches for a path with safety probability $p_{\rm
target}=0.95$ versus $p_{\rm target}=0.999$.

##  VI Conclusion

In this paper, we presented a method for path planning through complex,
obstacle-dense environments using noisy object detections, motivated by the
use of machine learning-based object detectors to enable robots to plan around
distant obstacles. Our approach constructs a probabilistic graph
representation of obstacles; we then plan multiple candidate paths through the
graph, accounting for path safety and expected distance to optimize the best
route to the goal. Simulation experiments in generated forest environments
demonstrated that our multiple hypothesis planner enables a differential drive
robot to reach its goal much more safely in complex, obstacle-dense forests as
compared to a baseline global planner.

Our graph representation provides a probabilistic framework that could be
extended with other sources of uncertain information. In particular, the
evaluation metric for the multiple path hypotheses is a promising area for
future improvement, building upon the current path cost function based on
safety and distance-to-goal. Including information gain as a weight in the
path evaluation, as in [6], would encourage the robot to follow paths that
allow it to view previously unobserved regions of the environment, or to
approach uncertain obstacles in order to improve its estimate certainty by
collecting additional measurements. Image semantics are another promising
source of information [6, 5], and could be used to weight candidate paths by
recognizing obstacle-dense regions from long range using semantic masks.

## References

  * [1] Y. Wang, W.-L. Chao, D. Garg, B. Hariharan, M. Campbell, and K. Q. Weinberger, “Pseudo-lidar from visual depth estimation: Bridging the gap in 3d object detection for autonomous driving,” in _Proceedings of the IEEE Conference on Computer Vision and Pattern Recognition_ , 2019, pp. 8445–8453.
  * [2] J. Monica and M. Campbell, “Vision only 3-d shape estimation for autonomous driving,” in _2020 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS)_. IEEE, 2020, pp. 1676–1683.
  * [3] M. Ryll, J. Ware, J. Carter, and N. Roy, “Efficient trajectory planning for high speed flight in unknown environments,” in _2019 International Conference on Robotics and Automation (ICRA)_ , 2019, pp. 732–738.
  * [4] K. Mohta, M. Watterson, Y. Mulgaonkar, S. Liu, C. Qu, A. Makineni, K. Saulnier, K. Sun, A. Zhu, J. Delmerico _et al._ , “Fast, autonomous flight in gps-denied and cluttered environments,” _Journal of Field Robotics_ , vol. 35, no. 1, pp. 101–120, 2018.
  * [5] M. Ryll, J. Ware, J. Carter, and N. Roy, “Semantic trajectory planning for long-distant unmanned aerial vehicle navigation in urban environments,” in _2020 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS)_. IEEE, 2020, pp. 1551–1558.
  * [6] Y. Han, H. Lin, J. Banfi, K. Bala, and M. Campbell, “DEEPsemanticHPPC: Hypothesis-based planning over uncertain semantic point clouds,” in _Proceedings of the International Conference on Robotics and Automation (ICRA)_ , 2020, pp. 4252–4258.
  * [7] A. Shaban, X. Meng, J. Lee, B. Boots, and D. Fox, “Semantic terrain classification for off-road autonomous driving,” in _5th Annual Conference on Robot Learning_ , 2021.
  * [8] Y. You, Y. Wang, W.-L. Chao, D. Garg, G. Pleiss, B. Hariharan, M. Campbell, and K. Q. Weinberger, “Pseudo-lidar++: Accurate depth for 3d object detection in autonomous driving,” _Proceedings of the International Conference on Learning Representations (ICLR)_ , 2020.
  * [9] S. Shi, X. Wang, and H. Li, “Pointrcnn: 3d object proposal generation and detection from point cloud,” in _Proceedings of the IEEE/CVF conference on computer vision and pattern recognition_ , 2019, pp. 770–779.
  * [10] A. H. Lang, S. Vora, H. Caesar, L. Zhou, J. Yang, and O. Beijbom, “Pointpillars: Fast encoders for object detection from point clouds,” in _Proceedings of the IEEE/CVF Conference on Computer Vision and Pattern Recognition_ , 2019, pp. 12 697–12 705.
  * [11] R. Qian, D. Garg, Y. Wang, Y. You, S. Belongie, B. Hariharan, M. Campbell, K. Q. Weinberger, and W.-L. Chao, “End-to-end pseudo-lidar for image-based 3d object detection,” in _Proceedings of the IEEE/CVF Conference on Computer Vision and Pattern Recognition_ , 2020, pp. 5881–5890.
  * [12] B. H. Wang, C. Diaz-Ruiz, J. Banfi, and M. Campbell, “Detecting and mapping trees in unstructured environments with a stereo camera and pseudo-lidar,” _IEEE International Conference on Robotics and Automation (ICRA)_ , 2021.
  * [13] S. Karaman and E. Frazzoli, “High-speed motion with limited sensing range in a poisson forest,” in _2012 IEEE 51st IEEE Conference on Decision and Control (CDC)_. IEEE, 2012, pp. 3735–3740.
  * [14] S. Choudhury, S. Scherer, and J. A. Bagnell, “Theoretical limits of speed and resolution for kinodynamic planning in a poisson forest,” in _Robotics: Science and Systems 2015_ , 2015.
  * [15] B. M. R. Junior and G. A. S. Pereira, “Fast path computation using lattices in the sensor-space for forest navigation,” in _2021 IEEE International Conference on Robotics and Automation (ICRA)_ , 2021.
  * [16] S. M. LaValle and J. J. Kuffner Jr, “Randomized kinodynamic planning,” _The international journal of robotics research_ , vol. 20, no. 5, pp. 378–400, 2001.
  * [17] Y. Kuwata, J. Teo, G. Fiore, S. Karaman, E. Frazzoli, and J. P. How, “Real-time motion planning with applications to autonomous urban driving,” _IEEE Transactions on control systems technology_ , vol. 17, no. 5, pp. 1105–1118, 2009.
  * [18] P. E. Hart, N. J. Nilsson, and B. Raphael, “A formal basis for the heuristic determination of minimum cost paths,” _IEEE transactions on Systems Science and Cybernetics_ , vol. 4, no. 2, pp. 100–107, 1968.
  * [19] D. Dolgov, S. Thrun, M. Montemerlo, and J. Diebel, “Path planning for autonomous vehicles in unknown semi-structured environments,” _The international journal of robotics research_ , vol. 29, no. 5, pp. 485–501, 2010\.
  * [20] J. Petereit, T. Emter, C. W. Frey, T. Kopfstedt, and A. Beutel, “Application of hybrid a* to an autonomous mobile robot for path planning in unstructured outdoor environments,” in _ROBOTIK 2012; 7th German Conference on Robotics_. VDE, 2012, pp. 1–6.
  * [21] K. Mohta, K. Sun, S. Liu, M. Watterson, B. Pfrommer, J. Svacha, Y. Mulgaonkar, C. J. Taylor, and V. Kumar, “Experiments in fast, autonomous, gps-denied quadrotor flight,” in _2018 IEEE International Conference on Robotics and Automation (ICRA)_ , 2018, pp. 7832–7839.
  * [22] Y. Tian, K. Liu, K. Ok, L. Tran, D. Allen, N. Roy, and J. P. How, “Search and rescue under the forest canopy using multiple uas,” in _International Symposium on Experimental Robotics_. Springer, 2018, pp. 140–152.
  * [23] J. Tordesillas, B. T. Lopez, M. Everett, and J. P. How, “Faster: Fast and safe trajectory planner for navigation in unknown environments,” _IEEE Transactions on Robotics_ , 2021.
  * [24] J. Banfi, L. Woo, and M. Campbell, “Is it worth to reason about uncertainty in occupancy grid maps during path planning?” in _2022 International Conference on Robotics and Automation (ICRA)_. IEEE, 2022, pp. 11 102–11 108.
  * [25] S. Karaman and E. Frazzoli, “High-speed flight in an ergodic forest,” in _2012 IEEE International Conference on Robotics and Automation_. IEEE, 2012, pp. 2899–2906.
  * [26] Q. Li, P. Nevalainen, J. Peña Queralta, J. Heikkonen, and T. Westerlund, “Localization in unstructured environments: Towards autonomous robots in forests with delaunay triangulation,” _Remote Sensing_ , vol. 12, no. 11, p. 1870, 2020.
  * [27] G. V. Nardari, A. Cohen, S. W. Chen, X. Liu, V. Arcot, R. A. Romero, and V. Kumar, “Place recognition in forests with urquhart tessellations,” _IEEE Robotics and Automation Letters_ , vol. 6, no. 2, pp. 279–286, 2020\.
  * [28] S. L. Bowman, N. Atanasov, K. Daniilidis, and G. J. Pappas, “Probabilistic data association for semantic slam,” in _2017 IEEE international conference on robotics and automation (ICRA)_. IEEE, 2017, pp. 1722–1729.
  * [29] L. Nicholson, M. Milford, and N. Sünderhauf, “Quadricslam: Dual quadrics from object detections as landmarks in object-oriented slam,” _IEEE Robotics and Automation Letters_ , vol. 4, no. 1, pp. 1–8, 2018.
  * [30] K. Ok, K. Liu, K. Frey, J. P. How, and N. Roy, “Robust object-based slam for high-speed autonomous navigation,” in _2019 International Conference on Robotics and Automation (ICRA)_. IEEE, 2019, pp. 669–675.
  * [31] J. Lim, O. Salzman, and P. Tsiotras, “Class-ordered lpa*: An incremental-search algorithm for weighted colored graphs,” in _2021 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS)_. IEEE, 2021, pp. 6907–6913.
  * [32] M. Kaess, A. Ranganathan, and F. Dellaert, “isam: Incremental smoothing and mapping,” _IEEE Transactions on Robotics_ , vol. 24, no. 6, pp. 1365–1378, 2008.
  * [33] D.-T. Lee and A. K. Lin, “Generalized delaunay triangulation for planar graphs,” _Discrete & Computational Geometry_, vol. 1, no. 3, pp. 201–217, 1986.
  * [34] D. C. Shah and M. E. Campbell, “A qualitative path planner for robot navigation using human-provided maps,” _The International Journal of Robotics Research_ , vol. 32, no. 13, pp. 1517–1535, 2013.
  * [35] M. Kaess, H. Johannsson, R. Roberts, V. Ila, J. J. Leonard, and F. Dellaert, “isam2: Incremental smoothing and mapping using the bayes tree,” _The International Journal of Robotics Research_ , vol. 31, no. 2, pp. 216–235, 2012\.
  * [36] J. Banfi, Y. Zhang, G. E. Suh, A. C. Myers, and M. Campbell, “Path planning under malicious injections and removals of perceived obstacles: A probabilistic programming approach,” _IEEE Robotics and Automation Letters_ , vol. 5, no. 4, pp. 6884–6891, 2020.


# Erratum to:A generalization of Taketa’s theorem on $\rm M$-groups

Zeinab Akhlaghi  Zeinab Akhlaghi, Faculty of Math. and Computer Sci.,
Amirkabir University of Technology (Tehran Polytechnic), 15914 Tehran, Iran.
and
School of Mathematics, Institute for Research in Fundamental Science(IPM) P.O.
Box:19395-5746, Tehran, Iran<EMAIL_ADDRESS>
###### Abstract.

In the recent paper [A generalization of Taketa’s theorem on $M$-groups,
Quaestiones Mathematicae, (2022),
https://doi.org/10.2989/16073606.2022.2081632], we give an upper bound $5/2$
for the average of non-monomial character degrees of a finite group $G$,
denoted by ${\mathrm{acd}}_{nm}(G)$, which guarantees the solvability of $G$.
Although the result is true, the example we gave to show that the bound is
sharp turns out to be incorrect. In this paper we find a new bound and we give
an example to show that this new bound is sharp. Indeed, we prove the
solvability of $G$, by assuming
${\mathrm{acd}}_{nm}(G)<{\mathrm{acd}}_{nm}({\mathrm{SL}}_{2}(5))=19/7$.

###### Key words and phrases:

Monomial character, Primitive character, Taketa’s Theorem, Average degree

###### 2000 Mathematics Subject Classification:

20C15,20D15

The author is supported by a grant from IPM (No. 1401200113).

##  1\. Introduction

This paper is a corrigendum to our paper [A generalization of Taketa’s Theorem
on $M$-groups, Quaestiones Mathematicae, (2022),
https://doi.org/10.2989/16073606.2022.2081632], which will refer to as [1],
hereafter. In [1], denoting by ${\mathrm{acd}}_{nm}(G)$ the average of non-
monomial irreducible characters of $G$, we proved that if
${\mathrm{acd}}_{nm}(G)<5/2$, then $G$ is solvable. This result is true,
however the bound is not sharp. In fact, after the paper was published, we
found a mistake in calculating ${\mathrm{acd}}_{nm}(\rm{S}_{5})$ which we have
miscalculated to be $5/2$, while it is equal to $10/3>5/2$. Thus, however the
proof of the main result in [1] is correct, the bound is not sharp. In this
paper we aim to to find the best possible bound and prove the same result for
this new bound. Indeed we prove the solvability of $G$, by assuming
${\mathrm{acd}}_{nm}(G)<{\mathrm{acd}}_{mn}({\mathrm{SL}}_{2}(5))=19/7$.

Let $N$ be a normal subgroup of $G$ and $\lambda\in{\mathrm{Irr}}(N)$. Then
${\mathrm{Irr}}(G|N)$, ${\mathrm{Irr}}_{nm}(G|N)$ and
${\mathrm{Irr}}(G|\lambda)$ denote the set of irreducible characters of $G$
whose kernels do not contain $N$, the set of the non-monomial irreducible
characters of $G$ whose kernels do not contain $N$ and the set of the
irreducible characters of $G$ above $\lambda$, respectively. By
${\mathrm{acd}}_{nm}(G|N)$ we mean the average degree of irreducible
characters in ${\mathrm{Irr}}_{nm}(G|N)$. We use the notation $n_{d}(G)$,
${\mathfrak{N}_{d}}(G)$, $n_{d}(G|N)$ and ${\mathfrak{N}_{d}}(G|N)$ for the
number of irreducible characters of $G$ of degree $d$, the number of non-
monomial irreducible characters of $G$ of degree $d$, the number of
irreducible characters of $G$ of degree $d$ whose kernels do not contain $N$
and the number of non-monomial irreducible characters of $G$ of degree $d$
whose kernels do not contain $N$, respectively. For the rest of notation, we
follow [5].

##  2\. Main Results

In the following we bring up some results and a generalized definition of a
monomial group which is taken from [7] and [10].

######  Definition.

A non-linear character $\chi\in{\mathrm{Irr}}(G)$ is called a multiply
imprimitive character (or $m.i$ character for short) induced from the pair
$(U,\lambda)$ if there exist a proper subgroup $U$ of $G$ and an irreducible
character $\lambda\in{\mathrm{Irr}}(U)$ such that $\lambda^{G}=m\chi$ for some
nonnegative integer $m$.

Clearly, if an irreducible character is not a $m.i$ character, then it is
primitive (in [10, Definition 3.2] they are called super-primitive character).
The proof of the following Lemma is essentially taken from [7].

######  Lemma 2.1.

If $S$ is a non-abelian simple group, then $S$ has a non-linear irreducible
character $\chi$ of degree at least $4$ which is extendible to
${\mathrm{Aut}}(S)$ and $\chi$ is not an $m.i$ character ($\chi$ is a super-
primitive character). In particular, if $S\not\cong{\rm A}_{5}$, then
$\chi(1)\geq 5$.

###### Proof.

First, let $S$ be a simple group of Lie type, then using [7, Lemma 3.8] the
Steinberg character of $S$ is not a $m.i$ character and it is extendable to
${\mathrm{Aut}}(S)$ (see for example [4]), except for the cases $S\cong
A_{5}$, ${\mathrm{PSL}}_{2}(7)$ or $S_{4}(3)$. One can check that in those
remaining cases $S$ has an irreducible character of degree $4$, $6$ and
$2^{6}$, respectively, which is not a $m.i$ character and it is extendable to
${\mathrm{Aut}}(S)$ (see [3]), as wanted.

Next, let $S$ be an alternating group of degree $n\geq 7$. By the proof of [7,
Proposition 4.1], we see that $S$ has an irreducible character $\chi$ of
degree $n-1\geq 6$ which is extendable to ${\mathrm{Aut}}(S)$ and it is not a
$m.i$ character.

At last, let $S$ be a sporadic simple group or the Tits group. Then according
to the the proof of [7, Proposition 5.1] if for each $S$ listed in the first
column of Table 2 of [7] we take $\chi$ to be the character in the third
column of Table 2 of [7], then $\chi$ satisfies the hypothesis of the Lemma.

######  Lemma 2.2.

(See [2, Lemma 5]) Let $N$ be a minimal normal subgroup of $G$ such that
$N=S_{1}\times\dots\times S_{t}$, where $S_{i}\cong S$, a non-abelian simple
group. If $\sigma\in{\mathrm{Irr}}(S)$ extends to ${\mathrm{Aut}}(S)$, then
$\sigma\times\dots\times\sigma\in{\mathrm{Irr}}(N)$ extends to $G$.

######  Lemma 2.3.

Assume $N\cong S^{k}$ is a non-abelian minimal normal subgroup of a finite
group $G$, for some simple group $S$ and integer $k$. Let either
$\phi\in{\mathrm{Irr}}(G/N)$ be primitive and $\sigma\in{\mathrm{Irr}}(S)$ be
a super-primitive character; or $\phi\in{\mathrm{Irr}}(G/N)$ be linear and
$\sigma\in{\mathrm{Irr}}(S)$ be primitive character. If $\chi$ is an extension
of the product of all different conjugate of $\sigma$ to $G$, then $\chi\phi$
is primitive.

###### Proof.

Note that $N$ and $G$ satisfies [10, Hypothesis 1.1] and $\chi$ is an
extension of $\theta$ the product of all distinct $G$-conjugate of $\sigma$
(see the explanation before [10, Lemma 3.1]). By applying [10, Corollary 3.4],
$\chi\phi$ is primitive.

######  Lemma 2.4.

(See [8, Lemma 2.8].) Let $1<N$ be a normal subgroup of $G$ with
$N=N^{\prime}$, and suppose that a non-trivial $\theta\in{\mathrm{Irr}}(N)$
extends to $\chi\in{\mathrm{Irr}}(G)$. If $\chi=\lambda^{G}$, where
$\lambda\in{\mathrm{Irr}}(H)$ for some subgroup $H$ of $G$, then the following
results are true:

(1) $HN=G$, and $\theta=(\lambda_{H\cap N})^{N}$, where $\lambda_{H\cap N}$ is
irreducible.

(2) If in addition $\theta(1)$ is minimal among all non-trivial irreducible
character degrees of $N$, then $\chi$ is primitive.

######  Theorem 2.5.

Let $G$ be a finite group. If ${\mathrm{acd}}_{nm}(G)<19/7$, then $G$ is
solvable.

###### Proof.

Assume, on the contrary, $G$ is an example with minimal order, such that $G$
is non-solvable and ${\mathrm{acd}}_{nm}(G)<19/7$. Thus,

${\mathrm{acd}}_{nm}(G)=\frac{\sum\limits_{\chi\in{\mathrm{Irr}}_{nm}(G)}\chi(1)}{|{\mathrm{Irr}}_{nm}(G)|}<19/7.$

Then, $\sum\limits_{\chi\in{\mathrm{Irr}}_{mn}(G)}\chi(1)=\sum\limits_{d\geq
1}d{\mathfrak{N}_{d}}(G)$ and $|{\mathrm{Irr}}_{mn}(G)|=\sum\limits_{d\geq
1}{\mathfrak{N}_{d}}(G)$. So by the above inequality we have

$\sum\limits_{d\geq
3}(7d-19){\mathfrak{N}_{d}}(G)<12{\mathfrak{N}}_{1}(G)+5{\mathfrak{N}}_{2}(G).\
\ \ \ \ \ \ \ (*)$

First, we claim that there is no non-solvable minimal normal subgroup of $G$
contained in $G^{\prime}$. On the contrary, let $M\leq G^{\prime}$ be a non-
solvable minimal normal subgroup of $G$. Then $M$ is a direct product of $k$
copies of a non-abelian finite simple group $S$, for some integer $k$. By the
hypothesis $M$ is contained in the kernel of every linear character of $G$. We
show that $M$ is contained in the kernel of every irreducible character of $G$
of degree $2$. Let $\chi\in{\mathrm{Irr}}(G)$ such that $\chi(1)=2$. Since,
non-abelian finite simple groups do not have any irreducible character of
degree $2$ and the only linear character of a simple group is the principle
character, then $\chi_{M}=2.1_{M}$. Therefore $M$ lies in the kernel of
$\chi$, as wanted. Hence $n_{d}(G)=n_{d}(G/M)$, for $d=1,2$ and so
${\mathfrak{N}}_{d}(G)={\mathfrak{N}}_{d}(G/M)$ for $d=1,2$.

By Lemmas 2.1, 2.2 and 2.3, $M$ has a primitive irreducible character $\theta$
with degree $d_{0}\geq 4$ which is extendable to a primitive irreducible
character of $G$. Note that if $\phi\in{\mathrm{Irr}}_{nm}(G/M)$ and
$\phi(1)=p$ for some prime $p$ then $\phi$ is primitive. Hence the number of
primitive characters of degree $p$ of $G/M$, for some prime $p$, is
${\mathfrak{N}}_{p}(G/M)$. Then, by Gallagher’s theorem (see [5, Corollary
6.17]) and Lemma 2.3, we have
${\mathfrak{N}}_{1}(G)+{\mathfrak{N}}_{2}(G)={\mathfrak{N}}_{1}(G/M)+{\mathfrak{N}}_{2}(G/M)\leq{\mathfrak{N}}_{d_{0}}(G|M)+{\mathfrak{N}}_{2d_{0}}(G|M)$
and so ${\mathfrak{N}}_{1}(G)+{\mathfrak{N}}_{2}(G)\leq\sum\limits_{d_{0}\mid
d}{\mathfrak{N}}_{d}(G|M)$.

Therefore,

${\mathfrak{N}}_{1}(G)+{\mathfrak{N}}_{2}(G)\leq\sum\limits_{d_{0}\mid
d}{\mathfrak{N}}_{d}(G).$

Hence,

$\sum\limits_{d_{0}\mid
d}(7d-19){\mathfrak{N}}_{d}(G)\geq(7d_{0}-19)({\mathfrak{N}}_{2}(G)+{\mathfrak{N}}_{1}(G)).\
\ \ \ \ (**)$

If $M\not\cong\rm{A}_{5}$, then $d_{0}\geq 5$ by Lemma 2.1, and the non-
equality $(**)$ contradicts $(*)$. Now, let $M\cong\rm{A}_{5}$ and set
$C=C_{G}(M)$. We know that $MC/C\leq
G/C\leq{\mathrm{Aut}}(MC/C)\cong\rm{S}_{5}$. First, let $G=MC\cong C\times M$.
Then, $M$ contains three non-linear primitive characters, two of degree $3$
and one of degree $4$. Note that in this case $(**)$ holds with $d_{0}=4$,
which means that

$\sum\limits_{4\mid d}(7d-19){\mathfrak{N}}_{d}(G)\geq
9({\mathfrak{N}}_{2}(G)+{\mathfrak{N}}_{1}(G)).$

On the other hand, by Lemma 2.3, the extensions of irreducible characters of
degree $3$ of $M$ to $G$ are primitive, yielding that
${\mathfrak{N}}_{3}(G)\geq{\mathfrak{N}}_{3}(G|M)=2{\mathfrak{N}}_{1}(G/M)=2{\mathfrak{N}}_{1}(G)$.
Hence,

$\sum\limits_{d\geq 3}(7d-19){\mathfrak{N}}_{d}(G)\geq\sum\limits_{d\geq
4}(7d-19){\mathfrak{N}}_{d}(G)+2{\mathfrak{N}}_{3}(G)\geq
9({\mathfrak{N}}_{2}(G)+{\mathfrak{N}}_{1}(G))+4{\mathfrak{N}}_{1}(G)\geq
5{\mathfrak{N}}_{2}(G)+12{\mathfrak{N}}_{1}(G),$

which is contradicting $(*)$. So, we may assume $G\not=CM$, implying that
$G/C\cong\rm{S}_{5}$. Recall that $M$ contains a character
$\chi\in{\mathrm{Irr}}(M)$ of degree $5$ which is extendable to $G$. We show
that all extensions of $\chi$ to $G$ are primitive. On the contrary, assume
$\chi_{0}\in{\mathrm{Irr}}(G)$ is an extension of $\chi$ that is not
primitive, which means that there exists a subgroup $H<G$ and a linear
character $\eta\in{\mathrm{Irr}}(H)$ such that $\chi_{0}=\eta^{G}$. Remark
that $G/C\cong{\rm S}_{5}$ has a primitive extension of $\chi$, say $\psi$. By
Gallagher’s theorem [5, Theorem 6.17], $\psi=\chi_{0}\lambda$ for some linear
character $\lambda\in{\mathrm{Irr}}(G/M)$. As
$\psi=\chi_{0}\lambda=\eta^{G}\lambda=(\eta\lambda_{H})^{G}$, (see [5, Problem
5.3]), we get that $\psi$ is not primitive, a contradiction. Hence, all
extensions of $\chi$ to $G$ are primitive. Thus,
${\mathfrak{N}}_{5}(G)\geq{\mathfrak{N}}_{5}(G|M)={\mathfrak{N}}_{1}(G/M)={\mathfrak{N}}_{1}(G).$
Again using, $(**)$, we have

$\sum\limits_{d\geq 4}(7d-19){\mathfrak{N}}_{d}(G)\geq\sum\limits_{4\mid
d}(7d-19){\mathfrak{N}}_{d}(G)+16{\mathfrak{N}}_{5}(G)\geq
9({\mathfrak{N}}_{2}(G)+{\mathfrak{N}}_{1}(G))+16{\mathfrak{N}}_{1}(G)\geq
5{\mathfrak{N}}_{2}(G)+12{\mathfrak{N}}_{1}(G),$

which is a contradiction.

Therefore, our claim is proved. Hence, we may assume that every minimal normal
subgroup of $G$ contained in $G^{\prime}$ is solvable. Let $M\unlhd G$ be
minimal such that $M$ is non-solvable. Notice that $M$ is a perfect group
contained in the last term of derived series of $G$. Let $T\leq M$, such that
$T$ is a minimal normal subgroup of $G$. In addition, if $[M,R]\not=1$, we
assume $T\leq[M,R]$, where $R$ is the radical solvable of $M$. Therefore
$T\leq M^{\prime}\leq G^{\prime}$, so $T$ is solvable and then $G/T$ is non-
solvable. As $G$ is a counterexample of minimal order, we have
${\mathrm{acd}}_{nm}(G/T)\geq 19/7$ and so it follows by arguing exactly as in
the first paragraph of the proof that

$\sum\limits_{d\geq
3}(7d-19){\mathfrak{N}}_{d}(G/T)\geq{\mathfrak{N}}_{2}(G/T)+12{\mathfrak{N}}_{1}(G/T).\
\ \ \ (**)$

Noting ${\mathfrak{N}}_{1}(G/T)={\mathfrak{N}}_{1}(G)$, we get that
${\mathfrak{N}}_{2}(G/T)<{\mathfrak{N}}_{2}(G)$ from $(*)$ and $(**)$. Hence
${\mathrm{Irr}}_{nm}(G|T)$ contains a character of degree $2$, say $\chi$. If
$K=\ker(\chi)$, then $G/K$ is a primitive linear group of degree 2 (see [5,
Chapter 14]). By the classification of the non-solvable primitive linear
groups of degree 2 (see [5, Theorem 14.23]) we have $G/C\cong A_{5}$, where
$C/K=Z(G/K)$. This implies that $G=MC$. Recall that $M/(M\cap C)\cong
MC/C=G/C$ and $M\cap C\unlhd M$ is a proper subgroup of $M$. Therefore, $M\cap
C$ is a subgroup of radical solvable subgroup of $M$ by the minimality of $M$.
Since $M/(M\cap C)$ is simple, we obtain that $M\cap C=R$, where $R$ is the
radical solvable subgroup of $M$, and hence $R\leq C$. Thus $[M,R]\leq K$. But
$T\not\leq K$, so we have $T\not\leq[M,R]$. By the choice of $T$, we have
$R={\bf Z}(M)$. Therefore, $M$ is a perfect central cover of the simple group
$M/{\bf Z}(M)=M/(M\cap C)\cong G/C$. Since $C$ and $M$ are both normal in $G$,
we have $[M,C]\leq C\cap M={\bf Z}(M)$, and so $[C,M,M]=[M,C,M]=1$. By the
three subgroups lemma, we deduce that $[M,M,C]=1$ and hence $[M,C]=1$ as $M$
is perfect. We conclude that $G=MC$ is a central product with a central
subgroup $M\cap C={\bf Z}(M)\not=1$. Thus, by the choice of $T$, we get that
$T=M\cap C={\bf Z}(M)$. As, ${\bf Z}(M)$ lies in the Schur multiplier of
$M/{\bf Z}(M)\cong\rm A_{5}$, we deduce that ${\bf Z}(M)=T\cong C_{2}$. So
$M\cong{\mathrm{SL}}_{2}(5)$ and
${\mathrm{Irr}}(G|T)={\mathrm{Irr}}(G|\lambda)$, where $\lambda$ is the only
non-trivial character of $T$. Recall that ${\mathrm{Irr}}(M|\lambda)$ contains
two primitive characters of degree $2$, one primitive character of degree $4$,
and one character of degree $6$ and ${\mathrm{Irr}}_{nm}(G|M)$ contains a
primitive character $\chi$ of degree $2$, which is an extension of one of the
irreducible characters of degree $2$ of $M$. By [6, Lemma 2],
$\chi(1)=\beta(1)\alpha(1)$ where $\beta\in{\mathrm{Irr}}(C|T)$ and
$\alpha\in{\mathrm{Irr}}(M|T)$. As ${\mathrm{Irr}}(M|T)$ does not contain any
linear character, $\beta$ is a linear character which means $\lambda$ extends
to $C$. Applying [6, Lemma 2], for every $\psi\in{\mathrm{Irr}}(M|T)$, we have
${\mathrm{Irr}}(G|T)$ contains a character of degree $\psi(1)\beta(1)$, which
clearly is the extension of $\psi$. By Lemma 2.4(1), we deduce that every
extension of $\psi$ is primitive, if $\psi(1)\in\\{2,4\\}$. Therefore,
${\mathfrak{N}}_{4}(G|T)={\mathfrak{N}}_{1}(G/M)=n_{1}(G/M)$ and
${\mathfrak{N}}_{2}(G|T)=2{\mathfrak{N}}_{1}(G/M)=2n_{1}(G/M)$. Then

${\mathrm{acd}}_{nm}(G|T)=\frac{4n_{1}(G/M)+4n_{1}(G/M)+\sum\limits_{d\geq
6}d{\mathfrak{N}}_{d}(G|T)}{2n_{1}(G/M)+n_{1}(G/M)+\sum\limits_{d\geq
6}{\mathfrak{N}}_{d}(G|T)}=\frac{8n_{1}(G/M)+\sum\limits_{d\geq
6}d{\mathfrak{N}}_{d}(G|T)}{3n_{1}(G/M)+\sum\limits_{d\geq
6}{\mathfrak{N}}_{d}(G|T)}.$

On the other hand, $G/T\cong C/T\times M/T\cong
C/T\times{\mathrm{PSL}}_{2}(5)$. Then using Lemma 2.4(1) all irreducible
characters $\lambda\times\mu\in{\mathrm{Irr}}(C/T\times{\mathrm{PSL}}_{2}(5))$
are non-monomial, provided that $\mu\in{\mathrm{Irr}}({\mathrm{PSL}}_{2}(5))$
is a non-monomial character and $\lambda$ is a linear character of $C/T$,
which means $\mu$ is one of those irreducible characters of
${\mathrm{PSL}}_{2}(5)$ of degree $3$ or $4$. Also, by 2.3, if $\mu$ is the
only super-primitive character of $M/T$ of degree $4$ and $\lambda$ is a
primitive character of $C/T$, then $\lambda\times\mu$ is primitive. We denote
by ${\mathfrak{M}}_{d}(G/T)$ the number of non-monomial irreducible characters
of $G/T$ of degree $d$ in form of $\lambda\times\mu$, where either
$\mu\in{\mathrm{Irr}}({\mathrm{PSL}}_{2}(5))$ has degree $1$ or $5$; $\mu$ has
degree $3$ and $\lambda$ is not linear; or $\mu$ has order $4$ and
$\lambda(1)>2$. Clearly
${\mathfrak{M}}_{d}(G/T)={\mathfrak{N}}_{d}(G/T)={\mathfrak{N}}_{d}(G/M)$ for
$d=1,2$ and ${\mathfrak{N}}_{1}(G/M)=n_{1}(G/M)$. Therefore, by the above
argument,

${\mathrm{acd}}_{nm}(G)=\frac{\sum\limits_{d\geq
1}d{\mathfrak{N}}_{d}(G|T)+\sum\limits_{d\geq
1}d{\mathfrak{N}}_{d}(G/T)}{|{\mathrm{Irr}}_{nm}(G|T)|+|{\mathrm{Irr}}_{nm}(G/T)|}=$
$\frac{10n_{1}(G/M)+8{\mathfrak{N}}_{2}(G/M)+\sum\limits_{1\leq d\leq
2}d{\mathfrak{N}}_{d}(G/M)+\sum\limits_{d\geq
3}d{\mathfrak{M}}_{d}(G/T)+8n_{1}(G/M)+\sum\limits_{d\geq
6}d{\mathfrak{N}}_{d}(G|T)}{3n_{1}(G/M)+{\mathfrak{N}}_{2}(G/M)+\sum\limits_{1\leq
d\leq 2}{\mathfrak{N}}_{d}(G/M)+\sum\limits_{d\geq
3}{\mathfrak{M}}_{d}(G/T)+3n_{1}(G/M)+\sum\limits_{d\geq
6}{\mathfrak{N}}_{d}(G|T)}=$
$\frac{19n_{1}(G/M)+10{\mathfrak{N}}_{2}(G/M)+\sum\limits_{d\geq
3}d{\mathfrak{M}}_{d}(G/T)+\sum\limits_{d\geq
6}d{\mathfrak{N}}_{d}(G|T)}{7n_{1}(G/M)+2{\mathfrak{N}}_{2}(G/M)+\sum\limits_{d\geq
3}{\mathfrak{M}}_{d}(G/T)+\sum\limits_{d\geq 6}{\mathfrak{N}}_{d}(G|T)}.$

Therefore,

$7(19n_{1}(G/M)+10{\mathfrak{N}}_{2}(G/M)+\sum\limits_{d\geq
3}d{\mathfrak{M}}_{d}(G/T)+\sum\limits_{d\geq 6}d{\mathfrak{N}}_{d}(G|T))$
$\geq 7(19n_{1}(G/M)+10{\mathfrak{N}}_{2}(G/M)+\sum\limits_{d\geq
3}3{\mathfrak{M}}_{d}(G/T)+\sum\limits_{d\geq 6}6{\mathfrak{N}}_{d}(G|T))$
$\geq 19(7n_{1}(G/M)+2{\mathfrak{N}}_{2}(G/M)+\sum\limits_{d\geq
3}{\mathfrak{M}}_{d}(G/T)+\sum\limits_{d\geq 6}{\mathfrak{N}}_{d}(G|T)),$

which means ${\mathrm{acd}}_{nm}(G)\geq 19/7$. This contradiction proves the
theorem.

Acknowledgment.  The author would like to thank Neda Ahanjide, for bringing
the error in [1] to her attention.

## References

  * [1] Z. Akhlaghi, A generalization of Taketa’s theorem on $M$-groups, Quaestiones Mathematicae, https://doi.org/10.2989/16073606.2022.2081632, (2022).
  * [2] M. Bianchi, D. Chillag, M. L. Lewis, E. Pacifici,  ’Character degree graphs that are complete graphs’,  Proc. Amer. Math. Soc. 135(3) (2007), 671–-676.
  * [3] J. H. Conway, R. T. Curtis, S. P. Norton, R.A. Parker and R.A. Wilson, Atlas of finite groups, Oxford University Press, London, (1984).
  * [4] W. Feit, Extending Steinberg characters, Linear algebraic groups and their representations, Contemp. Math. 153 (1993), 1–9.
  * [5] I. M. Isaacs, Character Theory of Finite Groups, New York NY: Academic Press 1976. Proc. Amer. Math. Soc. 91 (2) (1984), 192–194. finite group, Israel J. Math. 197 (2013), 55–67.
  * [6] A. Moretó, H. N. Neguyen, On the average character degree of finite groups, Bull. Lond. Math. Soc., 46 (2014), 454-462.
  * [7] T. Le, J. Moori and H. P. Tong-Viet, On a generalization of M-group, J. Algebra, 374 (2013), 27–41.
  * [8] G. Qian, Two results related to the solvability of M-groups, J. Algebra 323 (2010), 3134-3141.
  * [9] G. Qian, On the average character degree and the average class size in finite groups, J. Algebra 423 (2015), 1191-1212.
  * [10] G. Qian, Nonsolvable groups with few primitive character degrees, J. Group Theory 21 (2) (2018), 295-318.