---

前言

代码：github
原文：原文

摘要——搜索和救援操作要求移动机器人在非结构化的室内和室外环境中导航。特别是，主动稳定的多旋翼无人机需要精确的运动数据以保持平衡并避免障碍物。将芯片上的雷达的径向速度与MEMS惯性传感相结合，已被证明能提供强健、轻量且一致的状态估计，即使在视觉或几何条件恶劣的环境中也是如此。统计测试增强了这些估计器对雷达异常值的鲁棒性。然而，现有的二元异常值滤波器缺乏对各种硬件配置和环境的适应性。其他研究主要在手持静态环境或汽车上下文中进行了测试。本文介绍了一种用于四旋翼飞行的鲁棒气压-雷达-惯性测距（BRIO）m-估计器，适用于典型的GNSS失效场景。在城市和森林中进行的大量实际闭环飞行展示了该系统对移动物体和虚假目标的鲁棒性，保持了每 traveled 0.5%到3.2%的漂移性能。公共数据集上的基准测试验证了该系统的泛化能力。

图 1：我们雷达稳定四旋翼无人机的机载视图。点表示1秒内累计的雷达检测，颜色表示其径向速度（绿色：零，橙色：朝向雷达，蓝色：远离雷达）。

一、介绍

移动机器人在灾难响应中受到广泛研究。小型多旋翼无人机（UAV）非常适合探索，因为它们可以穿越废墟和障碍物。在封闭空间中，无人机需要机载定位来校正惯性测量单元（IMU）的漂移。已经提出了视觉、激光雷达和热导航解决方案，但这些方案易受到视觉退化、几何模糊或弱温度梯度的影响。随着汽车单芯片频率调制连续波（FMCW）雷达的问世，Doer和Trommer首次演示了雷达-惯性稳定的四旋翼飞行。由于其主动感知原理，雷达对视觉退化（例如黑暗或雾霭）具有鲁棒性。此外，雷达测量线性位移的能力与IMU测量方向变化的能力，使其成为估计标准导航状态（如位置、速度和方向）的出色传感器组合。本文考虑将方向多普勒雷达检测与IMU测量进行融合。我们研究及其他研究的一个核心假设是，所有雷达检测都是静态的。然而，由多路径传播或电磁噪声引入的移动物体和虚假目标常常会违反这一假设。图1a显示这些异常值与机器人运动不一致的径向速度，因此统计异常值测试增强了雷达-惯性测距（RIO）估计器的鲁棒性。我们的工作表明，鲁棒优化能够有效抑制现实环境中的噪声。与二元异常值拒绝（例如RANSAC）相比，m-估计只需一个调节参数。估计机体运动的第二个必要条件是至少三个具有线性独立到达方向（DoA）的雷达检测。我们的实验表明，在人为和自然环境中通常能够获得足够的独立检测。即使在方向角分布条件不良的场景中（如图1b中的稠密地面反射），估计往往也是稳定的。最后，DoA可能存在偏差，导致漂移，特别是在受重力影响的高度方向上。差分气压测量有效减轻垂直漂移。鲁棒优化也有助于剔除由于螺旋桨下洗引发的噪声。

本工作展示了BRIO在城市和自然环境中用于四旋翼导航的实验验证。我们推导出一个具有鲁棒方向多普勒和差分气压因子的因子图m-估计器。大量实验表明该估计器对常见异常值的鲁棒性，如骑自行车的人、行人、电车、多路径检测和气动效应。估计的轨迹显示出低漂移和足够的平滑度，适用于多旋翼反馈控制。总结我们的贡献如下：

• 一个具有鲁棒方向多普勒、鲁棒差分气压测量和零速度因子的BRIO m-估计器。
• 通过雷达稳定的四旋翼飞行在现实自然和人造环境中的实验调试、鲁棒性和性能分析。
• 在公共数据集基准上展示了泛化能力和先进性能。
• 开源估计器、传感器驱动程序和数据集。

图 2：四旋翼无人机的侧视图，展示了坐标系和两个雷达检测

二、相关工作

Yokoo 等人[7]提出了从稀疏的 FMCW 雷达方向多普勒和 IMU 测量中进行位姿估计的思想。随后，Kellner 等人[8]展示了成本效益高的单芯片 FMCW 雷达能够为自主驾驶提供自我运动估计。他们表明，两个线性独立的径向速度测量足以计算车辆的线速度。Doer 和 Trommer[6]将这一概念推广到三维导航。他们引入了一种结合 IMU 和气压计的导航滤波器，利用至少三个径向速度和最小二乘法计算线速度。然而，他们的二元雷达异常值测试需要 16 个调节参数，这使得设置适应变得复杂。他们的气压计忽略了气动干扰和偏差可观测性。他们的后续工作使用多个雷达，并通过曼哈顿世界假设引入偏航估计[9]。他们与额外传感器的结果不尽相同，而这一假设不适合自然环境。相反，我们的结果表明，使用开机偏差校准可以在消费级 IMU 上实现足够的偏航精度。Huang 等人[10]研究了雷达分辨率如何影响状态估计精度。鲁棒优化从多个雷达检测中计算线速度并提取协方差。滑动窗口估计器将其与 IMU 数据融合。我们的实验表明，较简单的单雷达-气压计配置在现实环境中为空中机器人提供了良好的性能和顺畅的状态估计。Michalczyk 等人[11]在滤波框架中将瞬时雷达速度更新与雷达地标跟踪结合。后续工作实现了 0.17% 到 1.62% 的漂移[12]。然而，特征跟踪和映射增加了复杂性，是误差的来源[13]。特别地，由于雷达反射的波动性，特征关联的概括性较具挑战性[14, pp. 106-108]。Michalczyk 等人[12]仅在一个小型实验室环境中展示了结果，环境中人为放置了角反射器。我们将跟踪限制在静态场景，以检测静止。因此，我们避免了对雷达目标的严格假设。我们的方法具有相似的精度，同时显示出更广泛的适用性。Kramer 和 Heckman[15]提出了一种固定滞后平滑器，类似于总最小二乘问题。他们共同估计机器人速度、方向、雷达检测的到达方向（DoA）和多普勒速度。然而，为了稳定优化器，他们需要在鲁棒损失的基础上进行 MLESAC 预滤波。我们的方法将雷达反射视为固定，具有更少的优化变量，并且不需要预滤波。Kramer 等人[16]的工作与我们的方法最为相近。他们直接在鲁棒优化中融合各个方向多普勒测量，使用 Cauchy 损失以拒绝雷达异常值。然而，我们的工作远远超出了他们的研究。我们比较了不同的损失函数，并展示了鲁棒的气压计融合、闭环四旋翼控制、对动态物体的鲁棒性、位置估计、开机偏差校准和零速度跟踪。我们的工作已开放源代码。
最后，针对地面机器人的多种雷达状态估计方法已经存在[17]。然而，大多数方法，如完整的 SLAM 流水线或自主汽车上的大型旋转雷达，都不适合四旋翼控制，因为四旋翼控制需要轻量级的速度估计，而不是密集的地图.

三、 符号

大写字母表示右手坐标系的框架。特定坐标系的原点由相同的字母表示。矩阵和向量用粗体表示。${}_A{t}_{BC}$是从源点$B$到目标点$C$的向量，以坐标框架$A$表示。${}_A{e}_B\in S^2$是指向坐标系原点$A$到位置$B$的单位向量。方向余弦矩阵$R_{AB}\in SO(3)$表示旋转并将向量${}_B{t}_{AB}$从框架$B$映射到框架$A$。

$${}_A{t}_{AB}={\mathbf{R}}_{AB}\cdot {}_B{t}_{AB}\text{\hspace{1em}(1)}$$

因此，旋转 ${\mathbf{R}}_{AC}$ 可以表示为

$${\mathbf{R}}_{AC}={\mathbf{R}}_{AB}{\mathbf{R}}_{BC}.\text{\hspace{1em}(2)}$$

刚性变换$T_{AB}\in SE(3)$是旋转$R_{AB}\in SO(3)$和平移${}_A{t}_{AB}$的组合。

四、 雷达惯性状态估计

该系统由三个坐标框架描述，用大写字母$I$、$B$和$R$表示，如图2所示。单位向量${}_I\mathbf{ex}$、${}_I\mathbf{ey}$、${}_I\mathbf{ez}$定义了惯性框架$I$，其中${}_I\mathbf{ex}$和${}_I\mathbf{ey}$构成与地球椭球面切平面的平面，而${\mathbf{I}}_{ez}$指向上方。$B$表示与IMU轴一致的机体框架，其中${\mathbf{B}}_{ex}$朝前，${\mathbf{B}}_{ey}$朝左，${\mathbf{B}}_{ez}$指向上方。$R$是雷达框架，其中${\mathbf{R}}_{ex}$指向正方位角方向，${\mathbf{R}}_{ey}$指向前视，${\mathbf{R}}_{ez}$指向正高程方向。坐标框架通过刚性变换$T_{IB}$和$T_{BR}$相连接。$T_{IB}$是在线估计的时变机器人位姿。$T_{BR}$是IMU与雷达之间的外部标定，来源于CAD。

A. 系统状态

IMU位姿$T_{IB}^i\in SE(3)$由方向$R_{IB}^i\in SO(3)$和位移$t_{IB}^i\in {\mathbb{R}}^3$组成，机体速度${\mathbf{I}}_{IB}^v\in {\mathbb{R}}^3$、陀螺仪偏置${\mathbf{B}}_g^i\in {\mathbb{R}}^3$，以及加速度计偏置${\mathbf{B}}_a^i\in {\mathbb{R}}^3$形成15维状态向量$x$在时间$i$。我们的框架还追踪位置为零速度雷达检测$t^j$与轨迹索引$j$。

图3：因子图表示，包含两个、一个和四个多普勒测量，以及从第一状态和第二状态观察到的零速度地标。

$t_{IT}^{i,m}$是第$m$个雷达检测在时间$i$的估计位置，跟踪多个帧$K_j\subseteq K_k$：

$$x^i={\left[R_{IB}^i,t_{IB}^i,I_{IB}^v,B{b}_g^i,B{b}_a^i\right]}^T,t^j={\left[t_{IT}^{i,m}\right]}^T\text{\hspace{1em}(3)}$$

估计器为每个雷达框架创建一个新状态。$X_k$是所有估计状态的集合，$K_k$是截至时间$k$的所有雷达框架的集合。类似地，$T_k$是截至时间$k$的所有估计零速度检测位置的集合，$L_k$是所有轨迹的集合。

X k = { x i } i ∈ K k , T k = { t j } j ∈ L k (4) X_k = \{ x^i \}_{i \in K_k}, \quad T_k = \{ t^j \}_{j \in L_k} \tag{4} Xk​={xi}i∈Kk​​,Tk​={tj}j∈Lk​​(4)

B. 测量

我们系统的输入包括方向多普勒雷达检测、IMU测量和可选的气压测量。我们用$R^i$表示时间$i$的所有检测的集合。$I^{i,i+1}$是雷达框架$i$和$i+1$之间的IMU测量集合。$B^i$是最接近时间$i$的气压测量。截至时间$k$的测量集合为：

Z k = { R i , I i , i + 1 , B i } ( i , i + 1 ) ∈ K k (5) Z_k = \{ R^i, I^{i,i+1}, B^i \}_{(i,i+1) \in K_k} \tag{5} Zk​={Ri,Ii,i+1,Bi}(i,i+1)∈Kk​​(5)

C. 优化准则

在线估计器评估状态$X_k$和零速度轨迹$T_k$的后验概率最大值$MAP$，给定所有可用的观测$Z_k$。

$$X_k^{MAP}=\arg \underset{X_k,T_k}{\max }p(X_k,T_k|Z_k)\text{\hspace{1em}(6)}$$

图3中的因子图表示未归一化的后验[18]。

$$X_k^{MAP}=\arg \underset{X_k,T_k}{\min }{\Vert r^0\Vert }_{{\Sigma }_p}^2+\sum _{(i,i+1)\in K_k}{\Vert r_I^{i,i+1}\Vert }_{{\Sigma }_I}^2(IMU)\text{\hspace{1em}(7)}$$

$$+\sum _{j\in L_k}\sum _{i\in K_j}{\Vert r^{i,j}\Vert }_{{\Sigma }_T}^2(零速度轨迹)$$

$$+\sum _{i\in K_k}\sum _{m\in R^i}\rho \left(\frac{\Vert r_D^{i,m}\Vert }{{\sigma }_D}\right){\Vert r_D^{i,m}\Vert }_{{\Sigma }_D}^2(稳健的方向多普勒)$$

$$+\sum _{i\in K_k}\rho \left(\frac{\Vert r_B^i\Vert }{{\sigma }_B}\right){\Vert r_B^i\Vert }_{{\Sigma }_B}^2(稳健的气压测量)$$

D. 稳健的方向多普勒因子

我们考虑具有标准检测处理的FMCW雷达。在接收到异步触发信号后，多输入多输出（MIMO）雷达执行预定义的FMCW脉冲序列。返回的模拟信号与传输信号混合，转换为数字、窗口化、范围和多普勒处理，经过恒定虚警率（CFAR）滤波和到达方向（DoA）处理。CFAR检测提取出已分离的强反射信号，处于范围-多普勒空间。CFAR检测是一种有效的数据缩减步骤，作为DoA处理和数据传输，主机计算机只需针对少量数据点执行。此外，CFAR有效地过滤来自噪声的反射信号。

结果是在时间$i\in K_k$上形成一个稀疏的点云，包含检测 $R^i$。检测 $T^{i,m}\in R^i$ 包含到达方向 $R{E}_T^{i,m}=\frac{r_T^{i,m}}{\Vert r_T^{i,m}\Vert }$、多普勒速度 $v_T^{i,m}$、范围噪声 $w_T^{i,m}=\Vert r_T^{i,m}\Vert$、信噪比（SNR）$SN{R}_T^{i,m}$。图1显示这些检测大多发生在几何边缘和相对介电常数的强变化处，例如金属窗框。我们使用DoA方向来约束身体速度，如图2所示。
方向多普勒残差 $r_D^{i,m}$ 是估计雷达线性速度 $R{V}_{IR}^i$ 投影到检测方向与测量的多普勒之间的差值。它依赖于状态变量 $R_{IB}^i$、$I{V}_B^i$ 和 $B_g^i$、雷达测量 $R{E}_T^{i,m}$ 和 $v_T^{i,m}$、以及角速度 $B{\omega }_{IB}^i$：

$$r_D^{i,m}=-\left((R{V}_{IR}^i{)}^T\cdot R{E}_T^{i,m}-v_T^{i,m}\right)\text{\hspace{1em}(8)}$$

$$=-\left(\left((R_{IB}^i{R}_{BR}{)}^T\cdot I{V}_{IR}^i\right)\cdot R{E}_T^{i,m}-v_T^{i,m}\right)\text{\hspace{1em}(9)}$$

其中雷达速度 $I{V}_{IR}^i$ 是从身体速度推导出的：

$$I{V}_{IR}^i=I{V}_{IB}^i+R_{IB}^i\left((B{\omega }_{IB}^i-B{b}_g^i)\times B{T}_{BR}\right)\text{\hspace{1em}(10)}$$

在静态场景中，方向多普勒残差噪声通常呈正态分布，标准差为 ${\sigma }_D$，如图4b所示。然而，雷达偶尔会检测到虚假目标或移动物体，产生长尾的残差噪声。例如，图4a中的电车产生接近（橙色/红色）或后退（蓝色/紫色）径向速度异常。

图4b显示了相应的方向多普勒残差误差分布，明显的离群值集位于10到50个标准差之间。后续的损失函数[19]可以在不同程度上抑制这些离群值。

我们的实验表明，在移动物体的情况下，应选择合适的损失函数，因为加权函数 $\rho$ 趋近于零。图4c显示了电车通过时的速度估计。该估计应该与图中方向的检测一致。

图4：基于电车经过悬停四旋翼的稳健方向多普勒损失函数分析。雷达检测在10秒的优化窗口内累积。

接近于零。纯二次损失会导致显著的零速度估计。此外，Fair和Huber的损失不足以抑制电车的检测。即使Cauchy损失在存在异常值的情况下仍然会预测一些运动。Welsch损失显示出最稳定的悬停估计，并被选用于估计问题（7）。仅使用稳健损失进行异常值剔除的一个优势是，相较于额外的统计测试，如RANSAC，只有${\sigma }_D$需要调整以归一化残差（7）。我们在所有实验中保持 ${\sigma }_D$恒定。我们没有观察到与任何评估的$\rho$ 函数相关的优化不稳定性。

E. 稳健的气压因子

从理论上讲，雷达因子足以观察身体速度和积分位置，前提是测量三个线性独立的径向速度。然而，系统性误差这些测量中的偏差，加上外部校准、重力常数和IMU偏差的误差，会导致高度估计漂移。为了补偿高度的误差，我们测量差分气压。气压残差 $r_B^i$ 是估计高度与偏差 $z_p^0$ 和测量高度 $z_p^i$ 之间的差值。

$$r_B^i=I{e}_z^T\cdot r_{IB}^i+z_p^0-z_p^i,\text{\hspace{1em}(11)}$$

其中 $z_p^i$ 是在时间 $i$ 的压力 $p^i$ 的函数，来自一个地球大气模型 [20]。

$$z_p^i=\frac{288.08{\left(\frac{p^i}{101290}\right)}^{\frac{1}{5.256}}-273.1-15.04}{-0.00649}\text{\hspace{1em}(12)}$$

气压测量受到环境温度变化的影响 [21]。与$Doer$和$Trommer$ [6]不同，我们不使用气压偏差 $z_p^0$ 的估计，而是将其在首次气压测量时设为常数。我们注意到，因天气变化导致的环境气压漂移非常小，因此 $z_p^0$ 并不可观察，远小于RIO高度估计的漂移。然而，我们的估计器对突发干扰（如着陆时的地面效应）是稳健的（图5a）。在没有抑制的情况下，这种噪声可能会导致垂直速度估计偏离，如图5b中以红色显示的那样。Fair稳健损失函数确保估计平滑。除了少数空气动力学场合，气压残差如图5c所示，通常呈正态分布。

F. 零速检测跟踪

在起飞前和着陆后，所有静态反射都有零多普勒速度，无法通过CFAR检测器在多普勒空间中分离。因此，雷达点云通常只包含一个或两个目标，代表可以按距离分离的最强反射。这不足以计算线性本体速度，因此状态估计将漂移在一个球体或圆圈上。由于环境和机器人都是静态的，连续反射在传感器分辨率内是相同的。我们跟踪这些目标，使完整状态在零速度下可观测。

令 $T_i^{i,m}\in {\mathbb{R}}^i$ 为在时间 $i$ 的检测 $m$。轨迹在被观察到后经过一段固定的雷达帧数后将被丢弃。目标只有在测量相同的情况下才会关联。

$$\left(r_{RT}^{i,m}=r_{RT}^{i,m}\right)\wedge \left(v_T^{i,m}=v_T^{i,m}=0\right)\wedge \left(SN{R}_T^{i,m}=SN{R}_T^{i,m}\right)\wedge \left(w_T^{i,m}=w_T^{i,m}\right)\Rightarrow T_i^{i,m}=T^{i,m}$$

优化器估计每个轨迹在惯性框架中的位置。残差在笛卡尔坐标系中表示为估计检测位置 $t^j$ 与测量检测位置 $R_{RT}^{i,m}$ 之间的差异。

$$r_T^{i,j}={\left(R_{IB}^i{R}_{BR}\right)}^T\left(t^j-r_{IB}^i\cdot R_{IB}^i\cdot B_{BR}\right)-R_{RT}^{i,m}\text{\hspace{1em}(14)}$$

G. IMU 因子

将高频率IMU测量作为单个因子整合，快速增加优化中的变量，使其在计算上不可处理。Forster等人 [22] 提出了一种将一组IMU测量结果预积分到单个因子的方法。该方法连续计算方向、速度和位置的相对变化。两个状态之间的组合IMU残差为：

$$r_I^{i,i+1}=\left[r_{\Delta R_{IB}^{i,i+1}},r_{\Delta t_{IB}^{i,i+1}},r_{\Delta v_{IB}^{i,i+1}},r_{b_g^{i,i+1}},r_{\Delta b_a^{i,i+1}}\right]\text{\hspace{1em}(15)}$$

H. 先验因子

适当的优化器初始化确保从一开始就能得到一致的状态估计。偏航偏差和方向需要特别处理。观察偏航角是困难的，因为小舵臂 $b_{TB}$ 和相对较大的雷达噪声 ${\sigma }_D$ [9]。我们通过平均前几条消息来校准陀螺仪偏差，并以较高的确定性进行初始化。

Madgwick滤波器 [23] 将方向与姿态控制器对齐（图6）。所有其他值初始化为零。

$$r_{R_{IB}}^0=\log \left({\left(R_{IB}^0\right)}^T{\tilde{R}}_{IB}^0\right)\text{\hspace{1em}(16)}$$

$$r_{t_{IB}}^0=t_I^0-{\tilde{t}}_{IB}^0\text{\hspace{1em}(17)}$$

$$r_{v_{IB}}^0=v_I^0-{\tilde{v}}_{IB}^0\text{\hspace{1em}(18)}$$

$$r_{b_g}^0=b_g^0-{\tilde{b}}_g^0\text{\hspace{1em}(19)}$$

$$r_{b_a}^0=b_a^0-{\tilde{b}}_a^0\text{\hspace{1em}(20)}$$

配合对数映射将旋转转换为其切向速度 [24]，波浪号表示初始值。组合的先验因子为：

$$r_P^0=\left[r_{R_{IB}}^0,r_{t_{IB}}^0,r_{v_{IB}}^0,r_{b_g}^0,r_{b_a}^0\right]\text{\hspace{1em}(21)}$$