腿足机器人之四- 卡尔曼滤波

和人类类似，基于无机体的智能机器人需要四模块，认知推理模块、运动/行动模块、视觉和听觉模块，当前深度神经网络的发展已经使得机器在视觉（识别，理解）和听觉（语音识别、语音合成）一些方面已经超越人类平均水平（如识别的准确性），而多模态大语言模型的发展使得认知推理模块的发展也较为迅速，而运动/行动模块的发展相对滞后一些，当运动/行动模块发展好了之后，会应该模块之间的融合。

因此行动放在了这一系列博文的开端。

为了处理机器人感知和行动中不确定性，衍生出了如混合动力系统（Hybrid Systems）、概率机器人学（Probabilistic robotics）、深度学习（Deep Learning）、强化学习（Reinforcement Learning）等范式，每一种都提供了处理不确定性的独特方法，它们可以单独使用，也可以与其他方法结合起来，以优化四足和人形机器人在多变环境中的表现。

概率机器人学（Probabilistic robotics）的关键思想是使用概率论的微积分明确地表示不确定性，这和规划算法相比，需要的算力会高不少。

概率机器人专著

概率学基础

对于概率密度、高斯分布、混合高斯分布不清楚的，可以网上搜索关键词或者查找概率论相关的著作，这里展示概率论中两个比较核心的点，贝叶斯准则和熵。

贝叶斯准则

贝叶斯准则（Bayes’ Theorem），也称为贝叶斯法则或贝叶斯更新，是概率论中的一个核心概念，用于计算条件概率。也就是说，它描述了在已知某些信息的情况下如何更新事件发生的概率。贝叶斯准则是基于先前的知识或信念对未知概率的一种修正方式。

贝叶斯准则可以表示为：

$$P(A\mid B)=\frac{P(B\mid A)P(A)}{P(B)}$$

其中：

$P(A|B)$是在事件 (B) 发生的条件下事件 (A) 发生的概率，称为后验概率。

$P(B|A)$是在事件 (A) 发生的条件下事件 (B) 发生的概率。

$P(A)$ 和 $P(B)$分别是 (A) 和 (B) 发生的边缘概率。

概率的理解和计算
边缘概率$P(A)$ 和 $P(B)$: 这是不考虑其他因素影响下单独某事件发生的概率。

条件概率$P(B|A)$: 表示在已知事件 (A) 发生的情况下，事件 (B) 发生的概率。

后验概率$P(A|B)$: 表示在已知事件 (B) 发生的情况下，事件 (A) 发生的概率。这是贝叶斯准则计算的主要目标，也是基于新证据对原有信念进行更新的过程。

概率密度用小写字母表示，如$p$，概率用大写字母，如$P$，这是因为概率是通过概率密度积分（连续情况，如观察火箭深空时尾焰温度）或者求和（离散情况，如抛硬币）。

还是以一个机器人行动的例子说明贝叶斯准则是如何使用的。

场景设定
假设一个机器人在一个已知环境中导航，但机器人的确切位置有不确定性。机器人可以通过传感器读取周围环境（如墙壁的距离）并移动，但这些传感器和移动的命令执行都有误差。

问题定义
机器人的任务是根据传感器读取和之前的移动历史估计自己的当前位置。这里的关键是如何根据新的传感器数据更新机器人对当前位置的估计。

几个贝叶斯准则涉及的概率
先验概率 $P(x)$: 在获取新的传感器数据之前，机器人对自己可能位置的估计。这称为“先验概率”，基于以往的位置信息和移动命令。

似然概率 $P(z\mid x)$ 在已知机器人位于特定位置 $x$ 的情况下，得到当前传感器读数$z$的概率。这反映了传感器的特性，如传感器误差等。

证据 $P(z)$: 这通常是用全概率定理计算的，表示在所有可能位置下得到当前传感器读数 $z$的概率。

后验概率 $P(x\mid z)$: 给定新的传感器读数 $z$ 后，机器人位置为 $x$ 的概率。这是我们希望计算的结果——在新证据的基础上更新的位置估计。

显然后验概率要比先验概率得到的位置更准，因为其使用了传感器的数据进行了“校准”，

通过贝叶斯准则，我们可以结合先验概率和新的传感器信息来得到后验概率：
$$P(x|z)=\frac{P(z|x)P(x)}{P(z)}$$
这里，$P(x\mid z)$告诉我们在观察到新数据后，各个位置的可能性。

在实践中，该过程会不断重复，每次机器人获取新的传感器信息或移动时，都会使用贝叶斯准则来更新位置的估计。这个过程使得机器人能够综合所有可用信息，并不断改进自身对于当前位置的估计。

另外$P(z)$是不需要求的，因为上面不断重复的过程中，分母都是$P(z)$，其类似一个归一化因子的作用。

另外需要注意的是，机器人一般都是多传感器的，这里增加传感器$y$，这样就涉及到两个传感器，其联合概率密度的贝叶斯公式，如下：
$$P(x|z,y)=\frac{P(y|x,z)P(x|z)}{P(y|z)}$$

熵

熵（Entropy）是信息论中的一个核心概念，用以量化随机变量的不确定性或者信息的随机性。对于离散随机变量，熵被定义为其概率分布的期望信息量，这可以被理解为如果你知道随机变量的实际取值，你将会获得多少信息。

熵的数学表达
对于一个离散随机变量，其熵 $H(P)$ 可以通过下面的表达式给出：
$$H(P)=-\sum _{x}p(x){\log }_{2}p(x)$$

其中:

• $p(x)$ 是随机变量取特定值 $x$ 的概率。
• ${\log }_{2}p(x)$是以 2 为底 $p(x)$的对数。

信息量通常定义为 $-{\log }_{2}p(x)$，这表示如果事件 $x$ 发生，你将获得的信息量。这里的对数以 2 为底，因而结果的单位是比特（bits），即在二进制表述中表示信息的单位。前面加负号是因为 $p(x)$ 是一个概率值，所以它位于 0 和 1 之间。对这种概率值取对数将会得到一个负值（因为对小于 1 的正数取对数结果为负），为了使信息量为正，我们在前面加上一个负号。

熵实际上是信息量 $-{\log }_{2}p(x)$的期望值，即当随机变量随机地取各种可能值时，平均每次观测到的信息量。这个期望通过所有可能的 $x$ 值的 $p(x){\log }_{2}p(x)$ 的加权总和（权重即为概率 $p(x)$）来计算。

熵度量了随机变量的不确定性。一个熵值高的随机变量其结果不可预料性更强，因而不确定性更高。反之，熵值低表示随机变量的结果可预料性更强，不确定性更低。

如果对于机器人行动状态求熵，如果其熵值等于0，那么意味着在某事件上的概率 ( p(x) = 1 )（即总是发生同一个结果），也即机器人永远只有一个状态。

在深度学习领域还有一个交叉熵，这个容易混淆，也做一个简单的介绍。

在深度学习中，交叉熵（Cross-Entropy）是一种常用的损失函数（Loss Function），用于评估模型的预测概率分布与真实标签的概率分布之间的差异。特别是在分类任务中，交叉熵损失非常流行，因为它可以有效地衡量预测结果的准确性。

交叉熵的定义
对于两个概率分布 ( p ) 和 ( q )，交叉熵定义为：
$$H(p,q)=-\sum _{x}p(x)\log q(x)$$
其中：

• $p(x)$ 是真实分布的概率，通常表示为标签的分布。
• $q(x)$ 是模型预测的概率分布。

交叉熵提供了一个衡量两个概率分布之间差异的方法。在理想情况下，如果模型的预测$q(x)$完全准确，那么它应该等于真实分布$p(x)$，这时交叉熵会达到最低值（理论上为0，当 $q(x)=p(x)$ 时）。当预测值与实际值偏离越大，交叉熵值就越高。

卡尔曼滤波

状态转移和观测模型都是线性的，对于高斯状态的状态转移和测量的卡尔曼滤波算法如下：

一个高斯分布的概率密度可以由其均值和方差确定，这里$u_t$表示的是控制器/驱动器均值向量（比如仰角、步速等），${\sum }_{t-1}$表示的是控制器协方差矩阵，之所以使用均值向量和协方差矩阵是因为机器人具有多个自由度。

$z_t$是传感器测量值，实际是一个向量，比如可以由多个距离、扭力、测力计等等构成。

首先我们可以根据驱动器信息$u_t$和机器人的前一时刻状态$x_{t-1}$(位置、或者姿势)来估算当前的时刻的姿势$x_t$，然后再根据传感器的观测值$z_t$来修正机器人在各个状态下概率值，最后以概率最高的状态作为机器人的状态。

第2行和3行就是获得当前状态的${\overline{x}}_t$(先验分布)的均值${\overline{\mu }}_t$和方差${\overline{\sum }}_t$信息(这是因为高斯分布可以由均值向量和协方差矩阵确定)。

第4行$K_t$是卡尔曼增益，该增益用于对不同的观测值加不同的权重，也即不同的传感器观测值对于最终的状态$x_t$影响是不同的。

第5和6行是使用当前观测向量$z_t$修正当前的状态$x_t$(是由均值向量${\mu }_t$和协方差矩阵${\sum }_t$唯一表示，修正之后是后验分布，所以没有上划线了)。

卡尔曼滤波器是一种有效的线性估计工具，用于处理线性动态系统中的噪声测量数据，然而实际的机器人应用中可能不成立。例如，机器人在执行任务时可能会执行复杂的非线性运动（如圆周运动），这种情况下，其状态变化不能简单地用线性方程描述。此外，机器人的实际运动可能受多种因素影响，导致其状态分布是多峰的或非高斯的，这超出了普通卡尔曼滤波器的处理范围。

因此，对于复杂或高度非线性的机器人系统，更先进的滤波技术（如扩展卡尔曼滤波器（EKF）、无迹卡尔曼滤波器（UKF）或粒子滤波器）更加适合，因为这些方法能够更好地处理非线性问题和非高斯噪声。

扩展卡尔曼滤波

扩展卡尔曼滤波extended Kalman filter (EKF)将下一个状态的概率和观测概率使用非线性函数表示：
$$x_t=g(u_t,x_{t-1})+{ϵ}_t$$
$$z_t=h(x_t)+{\delta }_t$$
其中$g$和$h$是非线性函数，$x_t$可以表示机器人的状态（空间中三维坐标叠加俯仰角姿势，即位置+姿势信息），$U-tt$是控制信息（比如各个驱动器，马达或者液压的驱动量），$z_t$是观测量（距离传感器、压力传感器，刚性力测量等），另外两个是扰动或者观测噪声。

当一个高斯分布通过这样的非线性函数处理时，输出通常不再是高斯分布，非线性函数$g$将替换上面的$A_t$和$B_t$，而$h$会取代$C_t$，这就导致状态的概率分布将不再是高斯分布，因而状态的概率分布精确解也无法求得。

EKF通过计算真实状态的概率分布的近似值来处理这一问题。它仍然使用高斯分布来表示这个近似的状态的概率分布，其中，任一时刻 (t) 的状态的概率分布 $x_t$由均值 ${\mu }_t$和协方差 ${\Sigma }_t$描述。具体方法是将非线性函数线性化。

线性化常用的技术包括泰勒展开（Taylor expansion）。泰勒展开是一种数学方法，用于在已知函数在某点的值及其导数的情况下，构建该函数在该点的线性近似。

对于EKF线性化做法是在高斯的均值处将函数 $g$近似为一条切线的方法，通过计算非线性函数$g$ 在高斯均值处的一阶导数（偏导数），可以构建一个与 $g$在该点切线相同的线性函数，通过这个线性近似函数而非原始的非线性函数来传播高斯分布，可以使后验分布仍然保持高斯形态。

对于感测非线性函数$h$可以做类似的线性化，这意味着，尽管处理的是非线性系统，EKF通过近似线性化使得处理过程类似于线性系统的处理。由于线性化仅是近似方法，EKF可能在高度非线性的情况下表现不佳，特别是当系统偏离线性化点很远时，这是可能需要更高级的滤波技术，如无迹卡尔曼滤波器（UKF）或粒子滤波器。

EKF是一种计算效率非常高的状态估计工具。每次更新所需的时间复杂度为 $(O(k^{2.8}+n))$，其中 (k) 是测量向量 (z) 的维度，(n) 是状态向量 (x) 的维度。与可能需要指数级时间的其他算法（例如粒子滤波器）相比，EKF的效率优势尤为明显。

EKF保持了原始卡尔曼滤波器的简单性，使用多变量高斯分布来表示信念（即状态的概率分布）。因为高斯分布是单峰的，所以通常可以理解为对状态的单一估计，并附带一个不确定性椭圆来描述估计的可靠性。

EKF通常只能提供对状态的单一估计，无法处理多模态（multimodal）的情况，即可能存在多个不同的合理假设。例如，一个机器人可能在空间中有两个完全不同的位置假设，而这两个位置的算术平均值并不是一个可能的位置。

为了解决EKF在表示多模态信念方面的不足，一种常见的扩展是使用高斯混合模型（Gaussian Mixtures）来表示后验信念。一个高斯混合可以形式化为多个高斯分布的和，这允许表示和处理多个独立假设。

信息滤波器

信息滤波器和卡尔曼滤波器都是用来估计系统状态（比如机器人的位置，机器人的姿态等）的概率滤波器，且都假设信念（belief，即状态概率分布）是高斯分布的。

在KF及其非线性版本EKF中，状态更新和预测相对直观和计算效率高，但如果要计算协方差的逆等操作则相对复杂。

信息滤波器（IF， information filter）使用高斯分布的典范表示（canonical representation），这包括信息矩阵（information matrix）$\Omega ={\Sigma }^{-1}$和信息向量（information vector）$\xi ={\Sigma }^{-1}\mu$。信息矩阵是协方差矩阵的逆，信息向量是协方差矩阵的逆乘以均值向量。更新信息矩阵和信息向量（这涉及到矩阵的逆和乘法）可能更为直观和容易，尤其是在信息矩阵已知的情况下。但转换回常规的均值和协方差表示则相对复杂。

对于一个多元高斯分布，其概率密度函数如下：
$$p(x)=\det (2\pi \Sigma {)}^{-1/2}{e}^{-\frac{1}{2}(x-\mu {)}^T{\Sigma }^{-1}(x-\mu )}$$

使用信息矩阵和信息向量替换之后，其概率密度可以表示为：
$$p(x)=\eta e^{-\frac{1}{2}{x}^T\Omega x+x^T\xi }$$

在信息滤波器中，状态的全局不确定性的表示非常简单，只需将信息矩阵（Ω）设为0。而在卡尔曼滤波器中，这需要协方差矩阵有无限大的值。
信息滤波器天然适合处理多机器人集成的问题，尤其是需要集成多个机器人收集的分散感测数据的场景。信息滤波器通过简单地对信息进行加和来实现集成，这一过程是可交换的，可以在任意顺序、任意延迟及完全分散的方式下进行。但在高维状态空间的情况下，一般认为其计算效率低于卡尔曼滤波器。这也是为什么扩展卡尔曼滤波器（EKF）比扩展信息滤波器（EIF）更为流行的原因之一。