毫米波雷达SLAM：利用TI IWR1443+ROS+NVIDIA JTX+TurtleBot2实现（下）

毫米波雷达原理

线性调频信号

该雷达的核心是一种特别的线性调频脉冲信号，如图所示。线性调频脉冲是频率随时间线性增长的正弦波，或者说是一种频率随时间以线性方式进行调制的连续波。

为了更好的理解，我们将线性调频信号放在（频率-时间）图当中分析。信号频率以初始频率$f_s$开始，随时间线性
增加，所以我们在图中可以看到一条斜率为$S$的直线；最后，假设信号带宽为$B$，那么最终信号将在$T_c$时刻后以$f_s+B$的频率结束。这样的发射波形称之为一个调频连续波信号，在一个这样的信号发射完毕后，发射天线会开始再次从初始频率发射相同的信号，不断重复这个过程。

一个简化的单发-单收调频连续波雷达框图如图所示，该雷达由频综产生线性调频信号，发射天线$T_x$射出，经目标反射后形成回波由接收天线$R_x$接收进混频器。

混频器具有两个输入和一个输出，除了回波信号之外的另一个输入是发射信号的一个拷贝，且在发射信号发出时就进入混频器。
该混频器有两个性质：1、输出的瞬时频率为两个瞬时输入频率差值。2、输出的相位等于两输入信号的相位差。
就是说，混频器输出一个中频信号IF，该中频信号的频率和相位分别为：

$$\begin{gathered} IF=f_{Tx}-f_{Rx} \\ {\phi }_{IF}={\phi }_{Tx}-{\phi }_{Rx} \end{gathered}$$

距离估计

中频频率与回波时延$\tau$直接有关，而回波时延是波速为$c$的电磁波在目标与雷达之间的往返距离。因此，
在线性调频斜率$S$一定或信号脉宽$T_c$与带宽$B$一定的情况下，目标与雷达间的距离为：
$$d=\frac{c\cdot \tau }{2}=\frac{c\cdot IF}{2S}=\frac{c\cdot IF\cdot T_c}{2B}$$
可以看出距离估计与中频频率之间的正比例关系。

角度估计

角度估计至少需要两个接收天线，设两天线有长为$D$的间距，目标偏离接收天线1径向$\theta$，则在两个天线上产生中频相位差，由相差公式$\Delta \phi =2\pi \tau \cdot IF=\frac{4\pi \Delta d}{{\lambda }_{IF}}$有：
$$\begin{gathered} \Delta \phi =\frac{2\pi D\sin (\theta )}{{\lambda }_{IF}} \\ \theta =\arcsin (\frac{{\lambda }_{IF}\Delta \phi }{2\pi D}) \end{gathered}$$

可以看到，角度估计计算相位差的中频信号不是在时间上（帧-帧）分开的，而是在空间上分开的（天线-天线）。利用天线阵的参数和中频信号相位差，可以实现对目标的角度估计。

定位与建图算法

引言

介绍SLAM算法之前，我们先对后文涉及到的传感器种类做一个简单的定义。并先对一些随机过程和参数估计当中常用的算法做一个简单的推导。
对于有一定基础的读者来说，本节可以跳过。

内部传感与外部传感

内部传感记录运动过程，外部传感获取当前状态信息。

定位与建图技术的核心是由传感器数据来估计状态。我们将传感器分为两类：内部传感器（Internal）和外部传感器（Proximity）。

• 内部传感器：里程计，惯性导航，陀螺仪等一系列能够提供机器人运动过程数据的传感器，可以当作是一种控制器的反馈。

• 外部传感器：声纳，激光雷达以及本论文详细讨论的雷达等通过一些介质（声、光、电磁波等）与外界接触的传感器，可以当作是对外界信息的一种感知。

  利用这两类传感，我们既可以在机器人运动的过程中掌握其实时位置，也能了解到机器人在该位置所感知到的环境信息。这就是定位与建图算法的基本思路。

马尔可夫假设

假设一个状态能够完整包含预估下一时刻状态的所有信息。

马尔可夫假设也称作完整状态假设，这个假设并不是说未来${\mathbf{x}}_{t+1}$状态是一个基于当前状态
${\mathbf{x}}_t$的确定映射，未来可以是随机的，但是未来状态不会先于当前状态变化，除非这个变化是依赖于当前状态${\mathbf{x}}_t$的。
这些按时间序依赖的事件，也被称之为马尔可夫链（Markov Chains）。
本文以下的所有算法都是基于假设\ref{markov}的，详细来讲，就是我们假设状态$\mathbf{x}$是一个能够概括机器人所有状态的向量，甚至是一些细节——车轮的磨损、机器人的电量等等。但实际上，$\mathbf{x}$是一个机器人的位置和姿态而已，我们只是假设其完整。

实际上，在这种将不完整的状态变量视作完整状态变量的假设下，估计算法的表现往往比完整的状态变量下更好。因为即使我们有办法精确且完整的获取机器人的内部状态，但是外界环境永远是未知的、随机的。而这种不完整的状态变量保留了估计结果的不确定性。

因而在讨论状态生成的过程时，我们认为位姿作为状态是完整的，而位姿由控制量$\mathbf{u}$生成，测量量$\mathbf{z}$则由状态量$\mathbf{x}$生成。
这样的时间生模型也称作隐马尔科夫模型（Hidden Markov Model,HMM），或者动态贝叶斯网络（Dynamic Bayes Network,DBN），如图所示。

在这样的假设下，我们能够得到状态变量概率密度函数依据传感器观测值更新的递推关系：
$$\begin{gathered} p({\mathbf{x}}_t|{\mathbf{x}}_{0:t-1},{\mathbf{z}}_{1:t-1},{\mathbf{u}}_{1:t})=p({\mathbf{x}}_t|{\mathbf{x}}_{t-1},{\mathbf{u}}_t) \\ p({\mathbf{z}}_t|{\mathbf{x}}_{0:t-1},{\mathbf{z}}_{1:t-1},{\mathbf{u}}_{1:t})=p({\mathbf{z}}_t|{\mathbf{x}}_t) \end{gathered}$$
可以得到两个生成条件概率$p({\mathbf{x}}_t|{\mathbf{x}}_{t-1},{\mathbf{u}}_t)$和$p({\mathbf{z}}_t|{\mathbf{x}}_t)$。
$p({\mathbf{x}}_t|{\mathbf{x}}_{t-1},{\mathbf{u}}_t)$描述了机器人从一个状态${\mathbf{x}}_{t-1}$经过一个对运动过程的反馈${\mathbf{u}}_t$得到新状态${\mathbf{x}}_t$的概率，故称之为
状态转移概率（state transition probability），对状态转移概率密度函数的估计称之为运动模型。
$p({\mathbf{z}}_t|{\mathbf{x}}_t)$描述了在当前状态${\mathbf{x}}_t$下对环境的观测值${\mathbf{z}}_t$的概率，故称之为测量概率（measurement probability），对测量概率的估计，称之为测量模型或者传感器模型。结合这两种模型，我们就得以推测出机器人的状态分布。

贝叶斯滤波

通过运动过程推测当前状态，并借由当前外部信息滤除不可能的状态。

贝叶斯准则在概率推理中起着主导作用，我们希望能够利用传感器的观测值作为后验信息，推测机器人在何位置以何种姿态能够产生这样的观测值。这种推理依赖于另一个重要的概念——置信度（Belief）。

置信度为所有可能的位置和姿态分配一个概率（对连续的情景是概率密度）。我们设任意时刻$t$对机器人运动过程的观测信息为${\mathbf{u}}_t$，在此时此地对外部环境的感知可用传感器的感知模型表示${\mathbf{z}}_t$。

若初始位置是已知的我们可以根据条件独立下的贝叶斯准则$p=\frac{p(y|x,z)p(x|z)}{p(y|z)}$设计一种递归算法。

贝叶斯滤波的正确性由归纳法给出，在此不再赘述。

粒子滤波

在此处可以看作基于数理统计理论的贝叶斯滤波。
粒子滤波（PF）可以看作是贝叶斯滤波的一种非参数实现，它用有限个参数来近似后验概率密度，可以理解为是用一系列从后验得到的随机采样表示后验置信度$bel({\mathbf{x}}_t)$。
这些随机采样被称为粒子（paeticles），若每个时刻的粒子数由$m$表示：
$${\mathcal{X}}_t:={{\mathbf{x}}_t}^{[1]},{{\mathbf{x}}_t}^{[2]},\dots ,{{\mathbf{x}}_t}^{[m]}$$
只要$m$的取值$M$不是太小，那么粒子群${\mathcal{X}}_t$对于置信度$bel({\mathbf{x}}_t)$的近似就可以认为是准确的。我们可以由构建递推算法:

该算法输入的是上一时刻的粒子集${\mathcal{X}}_{t-1}$和当前状态下的观测值${\mathbf{u}}_t$，${\mathbf{z}}_t$。
首先为当前时刻的置信度采样构造一个空粒子集$\overline{{\mathcal{X}}_t}$，该粒子集用于存放置信度$\overline{bel}({\mathbf{x}}_t)$的采样。
$\overline{{\mathcal{X}}_t}$中的每个粒子${{\mathbf{x}}_t}^{[m]}$基于输入粒子${{\mathbf{x}}_{t-1}}^{[m]}$和运动反馈${\mathbf{u}}_t$得到，即是对状态转移分布$p({\mathbf{x}}_t|{\mathbf{x}}_{t-1},{\mathbf{u}}_t)$采样，$M$次迭代后可以近似得到$\overline{bel}({\mathbf{x}}_t)$。

定义${{\mathbf{w}}_t}^{[m]}$是每个粒子${{\mathbf{x}}_t}^{[m]}$的重要性因子（importance factor），直观的表现为测量值${\mathbf{z}}_t$在${{\mathbf{x}}_t}^{[m]}$粒子处出现的概率，测量值出现的概率越大，说明该粒子在状态估计时越重要。

依据每个粒子的重要性因子进行重采样。依旧是进行$M$次采样，但是重要性高的粒子被重复抽取的概率更高，重要性低的粒子可能会被舍弃。由此得到的对$bel({\mathbf{x}}_t)$的粒子滤波估计${\mathcal{X}}_t$。

定位算法

假设地图信息$\mathbf{m}$可以用某种方式得到。

马尔可夫定位

拥有先验地图的贝叶斯滤波。

在贝叶斯滤波当中很重要的一个步骤是通过外部传感获取的信息，判断估计的置信度。因此我们需要一个参考，来作为外部传感信息的判别标准，因此马尔可夫定位是贝叶斯滤波的变种，可以认为是拥有先验地图$\mathbf{m}$的贝叶斯滤波。

假设机器人运动环境的环境地图$\mathbf{m}$已经提供。没有有效的地图作为参考，任何定位都是没有意义的。先验地图对于运动模型和传感器模型的估计都会产生影响，但是对测量模型的影响更大，且并不总是会干涉运动模型。

我们可以举一个《概率机器人》上经典的例子证明这一点，如图，是一个小机器人在一条地图已知的走廊上运动。图（a）是机器人在初始状态下对自己的位置做出的置信度预测，由于除地图外没有任何信息输入，所以机器人只能通过走廊的长度生成一个均匀分布来预测自己的位置。

紧接着，图（b）当中机器人第一次观测值$z$产生了，机器人看到了一扇门。结合先验地图，机器人对自己的位置估计发生了改变，以地图上三个门的坐标为中心生成了三个服从高斯分布的预测。

在当前时刻的末尾，图（c）中机器人收到了运动的反馈信息，知道自己向$x$轴正向移动了一段距离，于是将对自己位置的估计也整体向$x$轴正向平移了一段距离。

图（d），机器人进行了第二次观测，又看到了一扇门。结合第一次的观测和运动反馈信息，机器人更新了自己的位置信息，此时它对自己在第二扇门附近的置信明显增加了。

最后，图（e）机器人再次接收运动反馈信息，向前运动一段距离。由于先验地图与该运动没有冲突的地方，故单纯的将估计平移即可。

蒙特卡洛定位

拥有先验地图的粒子滤波。

一种流行的非参数估计方法叫做蒙特卡洛定位（MCL）算法或称粒子滤波（PF）定位算法，是通过把马尔可夫定位中中合适的运动模型和感知模型带入到粒子滤波算法，有：

建图算法

假设机器人的瞬时位姿信息${\mathbf{x}}_t$可以用某种方式得到。

占据栅格地图

利用外部传感器的先验精度推算目标的实际位置。

马尔可夫定位并不会对地图的形式做出要求。可是在连续空间上定义的地图很可能具有无穷多维。因此我们提出一种离散化的地图，称之为占据栅格地图（Occupancy grid map）。

在机器人的实时位姿已知的情况下，讨论建图问题会更加纯粹，只需要将注意力放在地图$\mathbf{m}$和感知传感器上$\mathbf{z}$上即可：
$$p(\mathbf{m}|{\mathbf{z}}_{1:t},{\mathbf{x}}_{1:t})$$
即利用位姿和在该位姿处的观测计算可能地图的后验概率的建图思想。其中$\mathbf{m}$，也就是占据栅格地图。占据栅格地图一般可以视作是在二维平面上分割出了许多的精细栅格单元，$m_i$表示第$i$个栅格变量。

最基本的情况下，这些栅格变量是用于表示栅格是否被占用的布尔型。如果栅格被占用则表现为“1”（true），若未被占用则表现为“0”（false）。$p(m_i=1)$或简记为$p(m_i)$表示该格被占用的概率。

其中$l_{t-1,i}=\log \frac{p(m_i|{\mathbf{z}}_{1:t},{\mathbf{x}}_{1:t})}{1-p(m_i|{\mathbf{z}}_{1:t},{\mathbf{x}}_{1:t})}$。常数$l_0$是用对数让步比表示的先验占用概率$l_0=\log \frac{p(m_i=1)}{p(mi=0)}$。

程序输入前序时刻地图估算概率$l_{t-1,i}$和当前时刻的位姿${\mathbf{x}}_t$与观测值${\mathbf{z}}_t$。观测用传感器，以毫米波雷达为例，其每次观测范围是由角量程和距离量程规定的测量锥${\mathbb{Z}}_t$。

对于在测量锥范围内的栅格单元$m_i$，更新其占用概率：
$$l_{t,i}=l_{t-1,i}+\log \frac{p(m_i|{\mathbf{z}}_t,{\mathbf{x}}_t)}{1-p(m_i|{\mathbf{z}}_t,{\mathbf{x}}_t)}-l_0$$
而对于未在测量锥范围内的栅格点，本次更新不影响他们的取值。

同时定位与建图算法

默认初始位置和地标可靠，通过重复检测先前的地标确定新位置，再通过可靠的新位置添加新的可靠地标。

在之前的讨论中，我们发现定位算法需要依赖先验地图，而建图算法需要依赖定位信息，因此“同时定位与建图”看起来像是一个“先有鸡，还是先有蛋”的逻辑死循环。

为了解决这个死循环，SLAM技术的关键，在于“回环”。也就是通过重复检测同一块地标来修正推演结果。

首先先给定一个初始位姿，在这个位姿上进行一次观测，称该观测为初始地标。对初始位姿和初始地标的置信都为1，此时可以认为我们拥有一个可以参考的局部地图。

紧接着，第一个运动信息输入，机器人位姿发生改变，当这个该变量不是很大的时候，我们认为在此处既可以看到初始地标$z_0$也可以看到新的地标$z_1$。通过$z_0$可以对当前的位姿$x_1$进行滤波矫正，而矫正后的$x_1$又可以确定$z_1$。

此时的$z_1$就可以作为一个置信量添加进局部地图当中，依次类推，直到机器人又回到初始位姿，完成建图。

小结

从马尔可夫假设和贝叶斯滤波以及粒子滤波的原理切入，分别介绍了马尔可夫定位的具体算法和他应用粒子滤波技术的离散形式蒙特卡洛定位算法。并介绍了全文建图的基础理论——占据栅格地图。以及同时定位与建图在工程上是如何实现的。

参考文献

1、《概率机器人》
2、AtsushiSakai/PythonRobotics：https://github.com/AtsushiSakai/PythonRobotics
3、Openai：https://www.openai.com/
4、ROS wiki