LF-3PM: a LiDAR-based Framework
for Perception-aware Planning with Perturbation-induced Metric

Assume the robot’s pose is denoted by $\boldsymbol{x}$, which has $n$ dimensions. Data captured at pose $\boldsymbol{x}$ by sensors can be utilized to construct error-based observations, denoted as $\boldsymbol{h}(\boldsymbol{x})=[h_{1}(\boldsymbol{x}),h_{2}(\boldsymbol{x}),...,h_{m}(\boldsymbol{x})]^{\text{T}}$. An example of $h_{j}(\boldsymbol{x})$ is given by Eq.(25) in Sec. V, which means the distance from point $j$ to its nearest plane. We aim to develop a metric to evaluate LiDAR observation loss that quantifies the effect of a LiDAR observation $\boldsymbol{h}(\boldsymbol{x})$ on the robot LAS. The metric can be formulated as:

$$q=Q(\boldsymbol{h}(\boldsymbol{x})),$$

(1)

where $Q$ denotes the metric, and scalar $q$ is the observation loss. Regions with low $q$ values are more conducive to maintaining good LAS.

As mentioned in Sec. I, mainstream localization algorithms [2, 4] are essentially solving a Least Square Problem. To derive a concrete form of $Q$, we explore how observations $\boldsymbol{h}(\boldsymbol{x})$ influence robots’ LAS in the linear Least Square Problem context. Consider the pose estimation problem as the following nonlinear Least Square Problem:

$$\boldsymbol{x}^{*}=\arg\mathop{\min}\limits_{\boldsymbol{x}}\frac{1}{2}\sum_{j=1}^{m}\lVert h_{j}(\boldsymbol{x})\rVert^{2}_{2}.$$

(2)

Usually, we solve Eq.(2) iteratively with $\boldsymbol{x}_{0}$, an initial estimate of $\boldsymbol{x}$. Suppose we iterate one step using Gauss-Newton method[21] to obtain $\boldsymbol{x}_{\text{opt}}$, which is equivalent to solving the following optimization problem:

$\displaystyle\boldsymbol{x}_{\text{opt}}=\arg\mathop{\min}\limits_{\boldsymbol{x}}\frac{1}{2}\lVert A(\boldsymbol{x}-\boldsymbol{x}_{0})-\boldsymbol{b}\rVert_{2}^{2},$

(3)

where $A\in\mathbb{R}^{m\times n},\boldsymbol{b}\in\mathbb{R}^{m}$. Each row of $A$ is $\nabla_{\boldsymbol{x}}^{\text{T}}h_{j}|_{\boldsymbol{x}=\boldsymbol{x}_{0}}$, each row of $\boldsymbol{b}$ is $-h_{j}(\boldsymbol{x}_{0})$, and optimal pose estimation $\boldsymbol{x}_{\text{opt}}$ is directly affected by observations $\boldsymbol{h}(\boldsymbol{x})$.

In fact, the observations usually contain sensor noise, erroneous associations, etc., which can be regarded as a perturbation applied to the exact observations. So we actually solve for $\hat{\boldsymbol{x}}_{\text{opt}}$ under disturbed observations $\hat{\boldsymbol{h}}(\boldsymbol{x})$. Naturally, we expect the displacement of the solution $\|\hat{\boldsymbol{x}}_{\text{opt}}-{\boldsymbol{x}}_{\text{opt}}\|$ caused by perturbations as small as possible.

According to the linear Least Square Problem sensitivity theory [15], given a certain disturbance on observations, the displacement of $\boldsymbol{x}_{\text{opt}}$ depends on $\boldsymbol{h}(\boldsymbol{x})$. In other words, the attributes of $\boldsymbol{h}(\boldsymbol{x})$ decide the robustness of the solution. Hence, if the metric $Q$ effectively captures the perturbation-induced displacement of $\boldsymbol{x}_{\text{opt}}$, it can serve as a reliable measurement of observation loss at the given pose $\boldsymbol{x}$.

If we have a smooth enough perturbation mapping $\mathcal{V}_{\delta\boldsymbol{v}}$ and a perturbation $\delta\boldsymbol{v}\in\mathbb{R}^{\text{V}}$ that change the observations $\boldsymbol{h}$ to $\hat{\boldsymbol{h}}$, where $\mathcal{V}_{\delta\boldsymbol{v}}:\boldsymbol{h}(\boldsymbol{x})\mapsto\hat{\boldsymbol{h}}(\boldsymbol{x},\delta\boldsymbol{v})$, $\boldsymbol{x}_{\text{opt}}$ will be changed to $\hat{\boldsymbol{x}}_{\text{opt}}$. Note that the perturbation mapping $\mathcal{V}_{\delta\boldsymbol{v}}$ with different expressions will lead to different changes in $\boldsymbol{x}_{\text{opt}}$. In this work, we assume that the mapping $\mathcal{V}_{\delta\boldsymbol{v}}$ has the form of a linear transformation with a bias, which can be expressed as the following equations:

	$\displaystyle\hat{\boldsymbol{h}}(\boldsymbol{x},\delta\boldsymbol{v})$	$\displaystyle=(I+\delta K)\boldsymbol{h}(\boldsymbol{x})+\delta\boldsymbol{t},$		(4)
	$\displaystyle\delta K$	$\displaystyle=g(\delta\boldsymbol{k})\in\mathbb{R}^{m\times m},$		(5)
	$\displaystyle\delta\boldsymbol{v}$	$\displaystyle=[\delta\boldsymbol{k},\delta\boldsymbol{t}]^{\text{T}},$		(6)

where $\delta\boldsymbol{t}\in\mathbb{R}^{m},\text{V}=m^{2}+m$. The role of $g$ is to resize the vector $\delta\boldsymbol{k}\in\mathbb{R}^{m^{2}}$ to the matrix $\delta K$ in row-major order [22]. Substituting $\hat{\boldsymbol{h}}(\boldsymbol{x})$ into Problem (3), $\hat{\boldsymbol{x}}_{\text{opt}}$ can be obtained by solving the following optimization problem:

	$\displaystyle\hat{\boldsymbol{x}}_{\text{opt}}=\arg\mathop{\min}\limits_{\boldsymbol{x}}\frac{1}{2}\lVert$	$\displaystyle(I+\delta K)A(\boldsymbol{x}-\boldsymbol{x}_{0})$
		$\displaystyle-(\boldsymbol{b}-\delta K\boldsymbol{b}-\delta\boldsymbol{t})\rVert_{2}^{2}.$		(7)

Let $\Delta\boldsymbol{x}=\boldsymbol{x}-\boldsymbol{x}_{0}$, we have:

	$\displaystyle\boldsymbol{x}_{\text{opt}}$	$\displaystyle=\arg\mathop{\min}\limits_{\Delta\boldsymbol{x}}\frac{1}{2}\lVert A\Delta\boldsymbol{x}-\boldsymbol{b}\rVert^{2}_{2}+\boldsymbol{x}_{0},$		(8)
	$\displaystyle\hat{\boldsymbol{x}}_{\text{opt}}$	$\displaystyle=\arg\mathop{\min}\limits_{\Delta\boldsymbol{x}}\frac{1}{2}\lVert(A+\Delta A)\Delta\boldsymbol{x}-(\boldsymbol{b}-\Delta\boldsymbol{b})\rVert^{2}_{2}+\boldsymbol{x}_{0},$		(9)

where $\Delta A=g(\delta\boldsymbol{k})A$ and $\Delta\boldsymbol{b}=g(\delta\boldsymbol{k})\boldsymbol{b}+\delta\boldsymbol{t}.$

What we care is how much small $\delta\boldsymbol{k}$ and $\delta\boldsymbol{t}$ can affect the displacement norm of the solution $\|\hat{\boldsymbol{x}}_{\text{opt}}-{\boldsymbol{x}}_{\text{opt}}\|$. We find that this problem can be converted to a sensitivity analysis problem for the optimal solution of a linear Least Square Problem. Work[15] summarizes a number of methods for obtaining the upper bound on the variation of the solution caused by $\Delta A$ and $\Delta\boldsymbol{b}$. Let the expression for the upper bound be $E(\lVert\Delta A\rVert,\lVert\Delta\boldsymbol{b}\rVert)$, we have

$$\lVert\hat{\boldsymbol{x}}_{\text{opt}}-\boldsymbol{x}_{\text{opt}}\rVert\leq E(\lVert\Delta A\rVert,\lVert\Delta\boldsymbol{b}\rVert).$$

(10)

Writing the perturbations $\delta\boldsymbol{k}$ and $\delta\boldsymbol{t}$ as: $\delta\boldsymbol{k}=r_{\boldsymbol{k}}\boldsymbol{\alpha},\delta\boldsymbol{t}=r_{\boldsymbol{t}}\boldsymbol{\beta}$, where $\boldsymbol{\alpha}^{\text{T}}\boldsymbol{\alpha}=\boldsymbol{\beta}^{\text{T}}\boldsymbol{\beta}=1,r_{\boldsymbol{k}}\geq 0,r_{\boldsymbol{t}}\geq 0$, and substituting them into the upper bound $E$, we have:

	$\displaystyle E(\lVert\Delta A\rVert,\lVert\Delta\boldsymbol{b}\rVert)$	$\displaystyle=E(\lVert g(\delta\boldsymbol{k})A\rVert,\lVert g(\delta\boldsymbol{k})\boldsymbol{b}+\delta\boldsymbol{t}\rVert)$
		$\displaystyle\triangleq F(r_{\boldsymbol{k}}\boldsymbol{\alpha},r_{\boldsymbol{t}}\boldsymbol{\beta}).$		(11)

Treating $\boldsymbol{\alpha},\boldsymbol{\beta}$ as constants first and linearizing $F$ near $r_{\boldsymbol{k}}=r_{\boldsymbol{t}}=0$ (which means the intensities of the perturbations tend to zero), we have

	$\displaystyle F(r_{\boldsymbol{k}}\boldsymbol{\alpha},r_{\boldsymbol{t}}\boldsymbol{\beta})$	$\displaystyle\approx\left.\frac{\partial F}{\partial r_{\boldsymbol{k}}}\right|_{r_{\boldsymbol{k}}=r_{\boldsymbol{t}}=0}r_{\boldsymbol{k}}+\left.\frac{\partial F}{\partial r_{\boldsymbol{t}}}\right|_{r_{\boldsymbol{k}}=r_{\boldsymbol{t}}=0}r_{\boldsymbol{t}}$
		$\displaystyle\triangleq d_{1}(\boldsymbol{\alpha,\beta})r_{\boldsymbol{k}}+d_{2}(\boldsymbol{\alpha},\boldsymbol{\beta})r_{\boldsymbol{t}}.$		(12)

We note that $d_{1}$ and $d_{2}$ serve as an amplifier of perturbations, which, in other words, denotes the sensitivity of $F$ to the perturbations. As the upper bound of the displacement of the solution $\|\hat{\boldsymbol{x}}_{\text{opt}}-{\boldsymbol{x}}_{\text{opt}}\|$, we expect $F$ less sensitive to the perturbations, which will constrain the sensitivity of displacement to the perturbations at a level no more than the sensitivity of $F$, because the inequality sign still holds for the derivation of both sides of Inequality (10) at zero. This can be easily proved by considering the functions on the left and right sides of Inequality (10) as $f_{1}$ and $f_{2}$ in Lemma 1, respectively. Thus, we define the metric $Q$ as the sensitivity of the upper bound $F$ to the perturbations:

$$q=\sqrt{w_{1}d_{1}^{2}(\boldsymbol{\alpha}_{\mathcal{G}},\boldsymbol{\beta}_{\mathcal{G}})+w_{2}d_{2}^{2}(\boldsymbol{\alpha}_{\mathcal{G}},\boldsymbol{\beta}_{\mathcal{G}})},$$

(13)

where $\boldsymbol{\alpha}_{\mathcal{G}},\boldsymbol{\beta}_{\mathcal{G}}$ are user-chosen directions of perturbations, $w_{1}>0,w_{2}>0,w_{1}+w_{2}=1$ are the weights of $d_{1},d_{2}$, respectively. Here, instead of performing sensitivity analysis on the derivation of $\lVert\hat{\boldsymbol{x}}_{\text{opt}}-\boldsymbol{x}_{\text{opt}}\rVert$ directly, we perform sensitivity analysis on the upper bound $E$ because the expanded expression for $\lVert\hat{\boldsymbol{x}}_{\text{opt}}-\boldsymbol{x}_{\text{opt}}\rVert$ is too complicated to be analyzed. After simplifying, what we need to do is pursue observations $\boldsymbol{h}(\boldsymbol{x})$ with lower $q$ value to keep low sensitivity of displacement $\|\hat{\boldsymbol{x}}_{\text{opt}}-{\boldsymbol{x}}_{\text{opt}}\|$ to perturbations, which is conducive to improving the LAS.

The intuition for defining the metric as Eq.(13) is that the directions of the perturbations can affect the sensitivity of the upper bound $E$. From a mathematical point of view, it is impossible to obtain the sensitivity of $E$ by direct derivation of $\delta\boldsymbol{k}$ and $\delta\boldsymbol{t}$ without treating $\boldsymbol{\alpha},\boldsymbol{\beta}$ as constants first, since the derivatives of the norms usually do not exist at zero. To enhance the clarity of the abstract mathematical procedure, we present an example when $m=2,\ w_{2}=1$, visualizing $\lVert\partial F/\partial\delta\boldsymbol{t}\rVert_{2}^{2}$ and $\lvert q_{o}\rvert=\sqrt{w_{1}d_{1}^{2}+w_{2}d_{2}^{2}}$ in Fig. 2.

In this work, we choose one of the upper bounds derived in the work [23] to compute the metric. According to the results of the work[23], suppose that $A$ has full column rank and $\lVert\Delta A\rVert_{2}<\sigma_{1}$, where $\sigma_{1}$ is the smallest singular value of $A$, we have

	$\displaystyle E(\lVert\Delta A\rVert,\lVert\Delta\boldsymbol{b}\rVert)$	$\displaystyle=\frac{\lVert\Delta\boldsymbol{x}^{*}\rVert_{2}+\sigma_{1}^{-1}\lVert\boldsymbol{r}\rVert_{2}}{\sigma_{1}-\lVert\Delta A\rVert_{2}}\lVert\Delta A\rVert_{F}$
		$\displaystyle\quad+\frac{1}{\sigma_{1}-\lVert\Delta A\rVert_{2}}\lVert\Delta\boldsymbol{b}\rVert_{2},$		(14)

where $\Delta\boldsymbol{x}^{*}=\arg\mathop{\min}\limits_{\Delta\boldsymbol{x}}\frac{1}{2}\lVert A\Delta\boldsymbol{x}-\boldsymbol{b}\rVert^{2}_{2},\boldsymbol{r}=A\Delta\boldsymbol{x}^{*}-\boldsymbol{b}$.

Writing $\boldsymbol{\alpha}$ as $\boldsymbol{\alpha}=[\delta\boldsymbol{k}_{1}^{\text{T}},\delta\boldsymbol{k}_{2}^{\text{T}},...,\delta\boldsymbol{k}_{m}^{\text{T}}]^{\text{T}}$ and calculating the corresponding derivatives (Details can be found in Appendix. -B), we can obtain:

$$q(\boldsymbol{\alpha})=\frac{1}{\sigma_{1}}\sqrt{w_{1}\sum_{j=1}^{m}\delta\boldsymbol{k}_{j}^{\text{T}}\Phi\delta\boldsymbol{k}_{j}+w_{2}},$$

(15)

where $\Phi=\xi^{2}AA^{\text{T}}+\boldsymbol{b}\boldsymbol{b}^{\text{T}},\xi=\lVert\Delta\boldsymbol{x}^{*}\rVert_{2}+\lVert\boldsymbol{r}\rVert_{2}/\sigma_{1}$.

Noting that $\Phi$ is a symmetric semi-positive definite matrix and $m\geq n$, we can diagonalize it: $\Phi=P\Lambda P^{\text{T}}$, where $PP^{\text{T}}=I,\Lambda=\text{diag}(\lambda_{m},\lambda_{m-1},...,\lambda_{1})$, $0=\lambda_{1}=\lambda_{2}=...=\lambda_{m-n-1}\leq\lambda_{m-n}\leq...\leq\lambda_{m}$ are eigenvalues of $\Phi$. Since $\boldsymbol{\alpha}^{\text{T}}\boldsymbol{\alpha}=1$, Eq.(15) is equivalent to:

$$q(\boldsymbol{\alpha})=\frac{1}{\sigma_{1}}\sqrt{w_{1}\sum_{j=1}^{m}\delta\boldsymbol{k}_{j}^{\text{T}}\Lambda\delta\boldsymbol{k}_{j}+w_{2}}.$$

(16)

As illustrated in Fig. 2$(b)$, the evaluation is affected by the direction of perturbations. With eigenvalue analysis, we can obtain the range of evaluation result: $\sum_{j=1}^{m}\delta\boldsymbol{k}_{j}^{\text{T}}\Lambda\delta\boldsymbol{k}_{j}\geq\sum_{j=1}^{m}\lambda_{1}\delta\boldsymbol{k}_{j}^{\text{T}}\delta\boldsymbol{k}_{j}=\lambda_{1}$ and $\sum_{j=1}^{m}\delta\boldsymbol{k}_{j}^{\text{T}}\Lambda\delta\boldsymbol{k}_{j}\leq\sum_{j=1}^{m}\lambda_{m}\delta\boldsymbol{k}_{j}^{\text{T}}\delta\boldsymbol{k}_{j}=\lambda_{m}$.

Thus, choosing different value of $\sum_{j=1}^{m}\delta\boldsymbol{k}_{j}^{\text{T}}\Lambda\delta\boldsymbol{k}_{j}$, we can obtain different forms of $q$. The three most representative forms are:

$$q^{(\min)}=\frac{1}{\sigma_{1}}\sqrt{w_{1}\lambda_{1}+w_{2}}=\frac{\sqrt{w_{2}}}{\sigma_{1}}.$$

(17)

$$q^{(n)}=\frac{1}{\sigma_{1}}\sqrt{w_{1}\lambda_{m-n+1}+w_{2}},$$

(18)

$$q^{(\max)}=\frac{1}{\sigma_{1}}\sqrt{w_{1}\lambda_{m}+w_{2}},$$

(19)

Different strategies actually represent different conservativeness. $q^{(\max)}$ is the most conservative estimate, choosing the direction in which the perturbation has the greatest impact. On the contrary, $q^{(\min)}$ is the least conservative strategy, which is actually the degradation factor in literature [14]. $q^{(n)}$ is an intermediate strategy between $q^{(\max)}$ and $q^{(\min)}$, where $n$ denotes the dimension of the state $\boldsymbol{x}$ to be estimated. In Sec. V, we will show the effect of strategies with different levels of conservativeness in observation evaluation and trajectory generation.

The upper part of Fig. 3 shows the process of SOLM calculation, including initialization, updating, and saving.

To start with, we initialize the SOLM according to the robot’s potential state space. Assume the robot’s state is $\boldsymbol{x}=[x,y,\theta]^{\text{T}}\in SE(2)$. Given the resolution on each dimension, we discretize the $SE(2)$ state space a grid structure with the size of $a,b$, and $c$ for dimensions $x$, $y$, and $\theta$, respectively. The obstacles within the given point cloud map will be marked on the SOLM, where the observation evaluation will be skipped to reduce unnecessary calculations.

Then, we evaluate the observation loss at each grid. Firstly, the information of a non-obstacle $\text{grid}(i,j,k)$ is taken out from the SOLM. Secondly, we simulate a LiDAR scan at the state represented by the center of this grid. A ray-casting-based scan simulator with GPU acceleration can significantly speed up the simulation process, like the one used in work [24]. The simulated scan will formulate observations $\boldsymbol{h}(\boldsymbol{x})$ follow the observation model like Eq. (25). Thirdly, we evaluate the observation loss at state $\boldsymbol{x}$ with the metric $Q$ derived in Sec. III. Finally, we update the evaluation result $q$ into $\text{grid}(i,j,k)$. It is noted that since the evaluation process at different grids is independent of each other, the process can be performed in parallel. Once all the non-obstacle grids have been evaluated, we obtain the SOLM that can be used for motion planning. In practice, due to the similar structure of the picture, we store the SOLM as multi-channel images for the $SE(2)$ case. For higher-dimensional state spaces, like $SE(3)$, we store the SOLM as a tensor.

The lower part of Fig. 3 shows the process of motion planning, including path searching and trajectory optimization.

After loading SOLM from stored images, we restore the knowledge of observation loss in the robot’s state space. For front-end path searching, we adopt the SOLM as the cost function and perform Breadth-First-Search (BFS) in the grid map, which allows for full consideration of all topologies and orientations in the environment to emphasize the effectiveness in improving the robot’s LAS of the proposed Perception-aware Planning framework.

For trajectory optimization, similar to the work[25], we use piecewise polynomials $\boldsymbol{x}(t)=[x(t),y(t),\theta(t)]^{\text{T}}$ to represent state trajectories, where $[x,y]\in\mathbb{R}^{2}$ denotes the position of the robot in the world frame, $\theta\in SO(2)$ denotes the yaw angle of the robot, as mentioned in last subsection. In this work, the cubic spline is chosen to represent the trajectory for the smoothness. We formulate the perception-aware trajectory planning problem for ground robots as the following optimization problem:

	$\displaystyle\min_{\boldsymbol{c},\boldsymbol{T}}\int_{0}^{T_{s}}q(\boldsymbol{x}(t))dt+\rho_{T}T_{s}$		(20)
	$\displaystyle s.t.\ \ \boldsymbol{M}\boldsymbol{c}=\boldsymbol{b},\quad\boldsymbol{T}\geq\boldsymbol{0},$		(21)
	$\displaystyle\quad\quad\text{Nonholo}(\boldsymbol{x})=0,$		(22)
	$\displaystyle\quad\quad\dot{\boldsymbol{x}}^{2}-([v_{\text{mlon}},v_{\text{mlat}},w_{\text{max}}]^{\text{T}})^{2}\leq[0,0,0]^{\text{T}},$		(23)
	$\displaystyle\quad\quad\text{SDF}(\boldsymbol{x})\geq r_{\text{safe}},$		(24)

where $\boldsymbol{c}\in\mathbb{R}^{4\text{N}\times 3}$ denote coefficient matrix, N denotes the number of segments of the piecewise polynomials. Each element in $\boldsymbol{T}=[T_{1},T_{2},...,T_{\text{N}}]^{\text{T}}\in\mathbb{R}^{\text{N}}$ denotes the duration of a piece of the trajectory. In the object function, $q(\boldsymbol{x}(t))$ denote observation loss at state $\boldsymbol{x}(t)$, $T_{s}=T_{1}+T_{2}+...+T_{\text{N}}$ is the total duration of the trajectory, $\rho_{T}$ is a positive weight to ensure the aggressiveness of the trajectory.

Conditions (21) denote the continuity constraints of the cubic spline at interpolation points and the positive time, where the inequality is taken element-wise. Conditions (22) denote the possible nonholonomic constraints of the robot. For car-like robots or differential robots, it is $\dot{\boldsymbol{x}}\sin\theta-\dot{\boldsymbol{y}}\cos\theta=0$. While for omnidirectional robots, this constraint does not exist. Conditions (23) are dynamic feasibility constraints, where the inequality and square operation are also taken element-wise. $v_{\text{mlon}},v_{\text{mlat}},w_{\text{max}}$ denote maximum longitude velocity, latitude velocity, and angular velocity, respectively. Condition (24) denotes safety constraint, where $\text{SDF}(\cdot)$ denotes the signed distance function (SDF) at position $[x,y]^{\text{T}}$ to the nearest obstacle. $r_{\text{safe}}$ denotes the safety distance related to the size of the robot. In this work, we compute the SDF by the algorithm proposed in work[26].

To make the above optimization problem easier to solve, we adopt the MINCO trajectory class and differential homogeneous mapping used in work[27] to eliminate the constraints (21). For ease of solving the problem, we also replace the integral in the objective function with a summation approximation. When the equation constraint (22) exists, PHR Augmented Lagrange Multiplier method[28] (PHR-ALM) is used to solve the simplified problem for higher accuracy. Conversely, we utilize the penalty function method, which is sufficient to obtain an acceptable solution. Besides, an efficient quasi-Newton method L-BFGS[29] is chosen as the unconstrained optimization algorithm to work with PHR-ALM and the penalty function method.

To validate the effectiveness and efficiency, we apply the proposed Perception-aware Planning framework on an omnidirectional vehicle (RoboMaster AI-2020) with a 70-degree Field of View (FoV) LiDAR (Livox Mid-70¹¹1https://www.livoxtech.com/mid-70). In real-world experiments, the odometry is provided by FAST-LIO2 [2], a high-precision Lidar-Inertial Odometry (LIO) system. The ground truth of robot pose is provided by the motion tracking system (NOKOV²²2https://www.nokov.com/). All the computation is performed on an on-board computer with Intel i7-1165G7. The observation model takes the distance from the scan point to the nearest plane, the same model defined in work [2]:

$\displaystyle h_{j}({{}^{\text{L}}}{\boldsymbol{p}_{j}},\boldsymbol{x})={{}^{\text{G}}}{\boldsymbol{u}_{j}}^{\text{T}}({{}^{\text{G}}}{\boldsymbol{R}_{L}}{{}^{\text{L}}}\boldsymbol{p}_{j}+{{}^{\text{G}}}{\boldsymbol{t}_{L}}-\boldsymbol{q}_{j}),$

(25)

where $\boldsymbol{x}$ indicates the robot position ${{}^{\text{G}}}\boldsymbol{t}_{\text{L}}$ and orientation ${{}^{\text{G}}}\boldsymbol{R}_{\text{L}}$ to be estimated, $\boldsymbol{p}_{j}$ is a LiDAR point, $\boldsymbol{u}_{j}$ is the normal vector of the nearest plane, $\boldsymbol{q}_{j}$ is a point on the nearest plane.

To see if the metric is reliable in evaluating observation loss, we adopt an indicator Mean Disturbance-induced Error (MDE) serving as the evaluation ground truth, which is similar to $\boldsymbol{e}_{p}$ defined in work [30] as follows:

$\displaystyle\text{MDE}=\frac{1}{N}\sum_{j=1}^{N}\rVert\log(\boldsymbol{T}^{-1}_{\text{gt}}\boldsymbol{T}_{\text{regi},j})^{\vee}\lVert_{2}^{2},$

(26)

where $N$ denotes the number of disturbances, $\boldsymbol{T}_{\text{regi},j}$ is the $j_{th}$ registration result start from a disturbed $\boldsymbol{T}_{\text{gt}}$ with iterated point-to-plane registration.

In this subsection, we demonstrate the effectiveness and characteristics of the proposed metrics in evaluating observation loss in several representative scenes built in UE5³³3https://www.ue5.com.

As shown in Fig. 4, scene (a) is rich in planar features where the robot can localize itself stably and robustly to observation perturbations. Scene (d) is a typical degenerate scenario. Scene (c) is an unstructured scenario with roughly the same features, where the robot’s pose estimation is sensitive to the initial guess $\boldsymbol{x}_{0}$. Scene (b) shows some structured features in the unstructured grasslands.

We first simulate a LiDAR scan at each scene to obtain observations $\boldsymbol{h}(\boldsymbol{x})$ with the LiDAR model described in Eq. (25). Then we use metrics Eq. (17)-(19) to evaluate the loss of the simulated observation. Besides, we calculate MDE for each scene as the evaluation’s ground truth. The result is shown in TABLE. I. By comparing the ground truth with the evaluations by different metrics, we can judge which metric can provide the most reliable evaluation. It’s important to note that we are only concerned with the relative loss of different scenarios by the same metric.

In Table. I, all of the three metrics can tell the observation loss in scene (d) is the worst, while only the evaluation with metric $q^{(n)}$ is consistent with the ground truth in the four scenes. For $q^{(\text{min})}$, which is actually the degeneracy factor proposed in literature [14], cannot distinguish scenarios ($a$), ($b$), and ($c$) well because this metric only focuses on the structural richness of scenarios. In addition, the reason why the evaluation with $q^{(\text{max})}$ does not distinguish scenarios ($b$) and ($c$) well is the introduction of over-conservative maximum eigenvalues, which leads to the evaluation influenced by the best-observed dimension of the state to be estimated.

To demonstrate the effectiveness of the proposed framework in scenarios with large ranges, we build a Gobi desert in UE5 as shown in Fig. 5. We require the robot with the configuration in Sec. V-A to reach a distant warehouse from the inside of a small town. Stones are evenly distributed on the ground outside of towns to help robots locate themselves while traveling, and part of the stones are surrounded by weeds and bushes. To further illustrate the characteristics of the framework with different metrics, we show two trajectories guided by metrics $q^{(\text{min})}$ and $q^{(n)}$ respectively.

As we can see in Fig. 5, the planner utilizing $q^{(\text{min})}$ chooses a trajectory with grass and bush, whereas the planner utilizing $q^{(\text{n})}$ prefers the path with well-structured features and devoid of grass and bush, which is because $q^{(\text{min})}$ only considers the feature richness of observations and make the robot pursuit area with rich features no matter whether it is preferable for robot localization.

To evaluate how much effect the observations along the trajectories have on LAS, we calculate MDE for each pose sampled along the trajectories at a constant time interval. As shown in Fig. 6, while the trajectory guided by $q^{(n)}$ is less rich in features compared to $q^{(\text{min})}$, the clear geometrical features result in smaller accumulated MDE ($S=43.66$) compared to the trajectory guided by $q^{(\text{min})}$ ($S=64.98$).

After showing the different features of the proposed metrics, we adapt our Perception-aware Planning framework to the real-world scenario. As shown in Fig. 7(a), a vehicle with configurations in Sec. V-A is required to navigate from point A to point B or C, during which process it has to localize itself with boxes and floor within the motion capture area. According to Sec. V-B and V-C, metric $q^{(n)}$ has more holistic performance that provides reliable evaluations. Here, we show trajectories (#2 and #4) guided by metric $q^{(n)}$ and trajectories (#1 and #3) without metrics for comparison.

As shown in Fig. 7(b), the vehicle guided by $q^{(n)}$ is able to choose topologies and orientations with low observation loss. We calculate the localization error of the robot when moving along each trajectory by $\rVert\log(\boldsymbol{T}^{-1}_{\text{gt}}\boldsymbol{T}_{\text{odom}})^{\vee}\lVert_{2}$, where $\boldsymbol{T}_{\text{gt}}$ is given by Motion Tracking System and $\boldsymbol{T}_{\text{odom}}$ is given by FAST-LIO2 [2]. As we can see in Fig. 7(c), trajectories #2 and #4 have lower localization error than #1 and #3, which demonstrates that the trajectories generated by the proposed framework are conducive to improving LAS.

	$\displaystyle\left.\frac{d\lVert\Delta A\rVert_{F}}{dr_{\boldsymbol{k}}}\right|_{r_{\boldsymbol{k}}=0}$	$\displaystyle=\left.\frac{d\sqrt{\sum_{j=1}^{m}\lVert r_{\boldsymbol{k}}A^{\text{T}}\delta\boldsymbol{k}_{j}\rVert_{2}^{2}}}{dr_{\boldsymbol{k}}}\right|_{r_{\boldsymbol{k}}=0}$
		$\displaystyle=\lim_{r_{\boldsymbol{k}}\rightarrow 0^{+}}\sqrt{\frac{r_{\boldsymbol{k}}^{2}\sum_{j=1}^{m}\delta\boldsymbol{k}_{j}^{\text{T}}AA^{\text{T}}\delta\boldsymbol{k}_{j}}{r_{\boldsymbol{k}}^{2}}}$
		$\displaystyle=\sqrt{\sum_{j=1}^{m}\delta\boldsymbol{k}_{j}^{\text{T}}AA^{\text{T}}\delta\boldsymbol{k}_{j}},$		(27)

	$\displaystyle\left.\frac{\partial\lVert\Delta\boldsymbol{b}\rVert_{2}}{\partial r_{\boldsymbol{t}}}\right|_{r_{\boldsymbol{k}}=r_{\boldsymbol{t}}=0}$	$\displaystyle=\left.\frac{\partial\lVert g(\delta\boldsymbol{k})b+\delta\boldsymbol{t}\rVert_{2}}{\partial r_{\boldsymbol{t}}}\right|_{r_{\boldsymbol{k}}=r_{\boldsymbol{t}}=0}$
		$\displaystyle=\lim_{r_{\boldsymbol{t}}\rightarrow 0^{+}}\sqrt{\frac{r_{\boldsymbol{t}}^{2}\boldsymbol{\beta}^{\text{T}}\boldsymbol{\beta}}{r_{\boldsymbol{t}}^{2}}}=1,$		(28)
	$\displaystyle\left.\frac{\partial\lVert\Delta\boldsymbol{b}\rVert_{2}}{\partial r_{\boldsymbol{k}}}\right|_{r_{\boldsymbol{k}}=r_{\boldsymbol{t}}=0}$	$\displaystyle=\lim_{r_{\boldsymbol{k}}\rightarrow 0^{+}}\sqrt{\frac{\sum_{j=1}^{m}\lVert r_{\boldsymbol{k}}\boldsymbol{b}^{\text{T}}\delta\boldsymbol{k}_{j}\rVert_{2}^{2}}{r_{\boldsymbol{k}}^{2}}}$
		$\displaystyle=\sqrt{\sum_{j=1}^{m}\delta\boldsymbol{k}_{j}^{\text{T}}\boldsymbol{b}\boldsymbol{b}^{\text{T}}\delta\boldsymbol{k}_{j}},$		(29)