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

在机器人（四足、人形）领域，感知和行动的不确定性可能由多种因素引起，如传感器噪声、外部环境的变化、非精确控制以及实时性算力限制等。

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

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

为了处理机器人感知和行动中不确定性，衍生出了如混合动力系统（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 ∣ z ) P(x \mid z)