算法主要贡献：

1. 相较于通过observation的图像来进行规划，根据编码后的图像进行在潜在空间中的规划在时间复杂度上更具有优势，并且更加高效。
2. Recurrent state space model：本文提出的一种新型的网络单元结构，既包含了确定性因素也考虑了随机因素
3. Latent overshooting：将standard variational bound推广到多步预测上来改善模型的长期预测

核心流程：

总体框架

训练过程的框架如图所示：

建立一个POMDP问题，定义观测模型、转移模型、奖励模型和encoder，利用可观测量来估计状态隐变量及其分布。

decode的图像用于训练的过程，在部署规划的时候只使用绿色部分的隐藏空间。在规划部分使用了交叉熵方法（Cross-Entropy Method, CEM），这是一类应用于重要性采样与优化的蒙特卡洛方法，计算过程为：

1. 从一个指定的概率分布中抽样出N的样本。
2. 通过最小化当前分布与目标分布(target distribution)的KL散度(KL-divergence),对当前的概率分布进行更新，以便在下一次迭代中产生效果最好的样本。

规划的流程为：使用学习到的模型来针对不同的行动预测收益，并选取最好的下一步的行动。可表示为：

$$o_{<=t}, a_{<t} \rightarrow s_t \rightarrow transition model \rightarrow s_{t+1}, s_{t+2}, ...\\ \rightarrow reward\ model \rightarrow R \rightarrow CEM \rightarrow a_t$$

基于先前的观察使用encoder得到当前状态的估计，然后对于未来的行动序列进行采样，并结合transition model来预测未来的状态序列，同时使用reward model来估计获得奖励的期望值。这样对于一个给定的未来行动序列就能够得到一个期望收益了，把这一套当做一个黑盒子我们就能利用CEM来规划出一个好的未来行动序列了。取这个行动序列的第一个行动作为下一步我们选择的行动即可。

模型学习

通过采样得到的数据是观测$o_{1:T}$和行动$a_{1:T}$，我们需要学习一个模型来估计状态隐变量$s_{1:T}$的分布：

$$p(o_{1:T},s_{1:T}\mid a_{1:T}) = \prod_{t=1}^T p(o_t\mid s_t) p(s_t\mid s_{t-1}, a_{t-1})$$

使用一个近似后验$q(s_{1:T}\mid o_{1:T}, a_{1:T})=\prod_{t=1}^T q(s_t\mid o_{1:T}, a_{1:T})$来逼近真实后验$p(s_{1:T}\mid o_{1:T}, a_{1:T})$。

在原文中有一个很复杂的公式：

$$\begin{aligned}\ln p(o_{1:T} \mid a_{1:T})&\triangleq \ln \mathbb{E}_{p(s_{1:T} \mid a_{1:T})}\left[ \prod_{t=1}^{T} p(o_t \mid s_t) \right] \\&= \ln \mathbb{E}_{q(s_{1:T} \mid o_{1:T}, a_{1:T})}\left[ \prod_{t=1}^{T} \frac{p(o_t \mid s_t) p(s_t \mid s_{t-1}, a_{t-1})}{q(s_t \mid o_{\le t}, a_{<t})} \right] \\&\ge \mathbb{E}_{q(s_{1:T} \mid o_{1:T}, a_{1:T})}\left[ \sum_{t=1}^{T} \ln p(o_t \mid s_t) + \ln p(s_t \mid s_{t-1}, a_{t-1}) - \ln q(s_t \mid o_{\le t}, a_{<t}) \right] \tag{8} \\&= \sum_{t=1}^{T} \left(\underbrace{\mathbb{E}_{q(s_t \mid o_{\le t}, a_{<t})}\left[ \ln p(o_t \mid s_t) \right]}_{\text{reconstruction}}- \underbrace{\mathbb{E}_{q(s_{t-1} \mid o_{\le t-1}, a_{<t-1})}\left[ \text{KL}\left[ q(s_t \mid o_{\le t}, a_{<t}) \parallel p(s_t \mid s_{t-1}, a_{t-1}) \right] \right]}_{\text{complexity}}\right).\end{aligned}$$

通过对不等式右侧做随机梯度上升即可学到这三个模型。

推导步骤

步骤 1：观测对数似然的全概率展开

根据全概率公式，观测的边缘对数似然可以表示为：

$$\ln p(o_{1:T} \mid a_{1:T}) \triangleq \ln \mathbb{E}_{p(s_{1:T} \mid a_{1:T})} \left[ \prod_{t=1}^T p(o_t \mid s_t) \right]$$

这里我们对隐状态序列 $s_{1:T}$ 进行边缘化，将观测似然写成对动力学先验 $p(s_{1:T} \mid a_{1:T})$ 的期望形式。

步骤 2：引入近似后验的重要性加权

利用重要性采样技巧，将对动力学先验的期望转换为对近似后验 $q$ 的期望：

$$= \ln \mathbb{E}_{q(s_{1:T} \mid o_{1:T}, a_{1:T})} \left[ \prod_{t=1}^T \frac{p(o_t \mid s_t) p(s_t \mid s_{t-1}, a_{t-1})}{q(s_t \mid o_{\le t}, a_{<t})} \right]$$

根据重要性加权公式：

$$\mathbb{E}_{p(s)}[f(s)] = \mathbb{E}_{q(s)}\left[ f(s) \cdot \frac{p(s)}{q(s)} \right]$$

其中 $p(s_{1:T} \mid a_{1:T}) = \prod_{t=1}^T p(s_t \mid s_{t-1}, a_{t-1})$，$q(s_{1:T} \mid o_{1:T}, a_{1:T}) = \prod_{t=1}^T q(s_t \mid o_{\le t}, a_{<t})$，因此两者的比值可拆分为每一步的乘积形式。

步骤 3：应用 Jensen 不等式得到下界

由于对数函数是凹函数，根据 Jensen 不等式 $\ln \mathbb{E}[X] \ge \mathbb{E}[\ln X]$，可得：

$$\ge \mathbb{E}_{q(s_{1:T} \mid o_{1:T}, a_{1:T})} \left[ \sum_{t=1}^T \ln p(o_t \mid s_t) + \ln p(s_t \mid s_{t-1}, a_{t-1}) - \ln q(s_t \mid o_{\le t}, a_{<t}) \right]$$

将期望内的乘积取对数后转化为求和，得到对数似然的一个下界（即证据下界 ELBO）。最大化该下界即可间接最大化原始对数似然。

步骤 4：整理为重构项与复杂度项

将上式整理为按时间步分解的形式，可得到最终的目标函数：

$$= \sum_{t=1}^T \left( \underbrace{\mathbb{E}_{q(s_t \mid o_{\le t}, a_{<t})} [\ln p(o_t \mid s_t)]}_{\text{重构项}} - \underbrace{\mathbb{E}_{q(s_{t-1} \mid o_{\le t-1}, a_{<t-1})} \left[ \text{KL}\big(q(s_t \mid o_{\le t}, a_{<t}) \parallel p(s_t \mid s_{t-1}, a_{t-1})\big) \right]}_{\text{复杂度项}} \right)$$

• 重构项（Reconstruction）：
  衡量从隐状态 $s_t$ 重建观测 $o_t$ 的能力，与 VAE 中的重建损失作用相同，目标是让重建观测与真实观测尽可能一致。
• 复杂度项（Complexity）：
  用 KL 散度衡量近似后验 $q(s_t \mid o_{\le t}, a_{<t})$ 与动力学先验 $p(s_t \mid s_{t-1}, a_{t-1})$ 的差异，起到正则化作用，防止后验分布偏离动力学模型的先验规律，保证隐状态的转移符合环境动态。

最终目标函数（ELBO）

整理后的变分下界就是 PlaNET 世界模型的训练目标：

$$\mathcal{L} = \sum_{t=1}^T \left( \text{Recon}_t - \text{KL}_t \right)$$

训练时同时优化观测模型、动力学模型和近似后验模型，即可得到一个能够预测环境动态的世界模型，后续可在此模型中进行规划（如 CEM 算法），大幅提升强化学习的样本效率。

创建QQ机器人

打开QQ开放平台，扫码登录，然后创建一个机器人，此时手机QQ会自动弹出一个机器人的提示。

配置QQ与OpenClaw连接

1. 首先需要安装qqbot的环境，这一步最好能够连接代理：

1

openclaw plugins install @tencent-connect/openclaw-qqbot@latest

2. 连接QQ机器人

1

openclaw channels add --channel qqbot --token "****:****"

token在网页中会显示

3. 重启gateway

完成以上步骤即可。

配置docker-compose

使用OpenClaw的官方Docker，使用国内镜像加速，注意需要自行配置volumes的映射目录。

network_mode: host是为了使用mihomo的代理

如果不添加user:root将没有读取本地文件的权限

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23

version: '3.8'

services:
  openclaw-gateway:
    # 使用 GitHub Container Registry 的官方镜像
    image: ghcr.1ms.run/openclaw/openclaw:latest
    container_name: openclaw-gateway
    network_mode: host
    user: root
    restart: always
    ports:
      - "18789:18789"  # Web 控制台端口
    volumes:
      # 将配置和数据持久化到宿主机
      - /********/openclaw:/home/node/.openclaw
    environment:
      - NODE_ENV=production
    # 健康检查
    healthcheck:
      test: ["CMD", "node", "dist/healthcheck.js"]
      interval: 30s
      timeout: 10s
      retries: 3

配置OpenClaw

完成docker-compose之后，点击构建运行。首先会下载镜像，下载完成后自动构建。

在容器中进入SSH，极空间的SSH的终端尺寸默认为0，会导致OpenClaw的输出变形，因此首先输入：

1

stty cols 120 rows 40

然后输入

1

openclaw onboard

开始配置。本文不再赘述OpenClaw的配置方法，仅提供在极空间中配置可以使用代理的OpenClaw镜像方法。

前期准备

• GitHub账号
• 在GitHub的Tokens中新增一个API token

步骤

1. 在终端中登录github

   1	docker login ghcr.io

   用户名是GitHub用户名，密码是刚刚的token

2. 将待上传的容器commit为镜像（或直接准备好镜像）

   1 2 3 4 5

   # 容器 docker commit fyj_deform_slam docker.pkg.github.com/feng1909/deform-slam:1.0 # 镜像 docker tag docker.pkg.github.com/feng1909/deform-slam:1.0 ghcr.io/feng1909/deform-slam:1.0

3. 上传镜像

   1	docker push ghcr.io/feng1909/deform-slam:1.0

   上传完成后如图：
   

• 如果随机变量$X$的概率密度函数为$p(x)$，则$X$的熵定义为：

  $$H(X) = -\sum_x p(x) \log p(x)$$

  熵的量纲为比特。熵可以看作是随机变量的平均不确定度的度量，在平均意义下，是为了描述了该随机变量所需的比特数。

• 将单个随机变量的熵推广到两个随机变量$X$和$Y$，其联合熵$H(X,Y)$定义为：

  $$\begin{aligned}H(X,Y)&=-\sum_{x\in\mathcal{X}} \sum_{y \in \mathcal{Y}} p(x,y) \log p(x,y) \\&=-E\log p(X,Y)\end{aligned}$$

• 定义一个随机变量在给定另一随机变量下的条件熵，是条件分布熵关于起条件作用的随机变量取平均之后的期望值，若$(X,Y)\sim p(x,y)$，条件熵$H(X|Y)$定义为：

  $$\begin{aligned}H(X|Y) &= \sum_{x\in \mathcal{X}} p(x)H(Y|X=x) \\&= -\sum_{x\in \mathcal{X}}p(x)\sum_{y\in \mathcal{Y}}p(y|x)\log p(y|x) \\&= -\sum_{x\in \mathcal{X}} \sum_{y\in \mathcal{Y}} p(x,y) \log p(y|x) \\&= -E\log p(Y|X)\end{aligned}$$

• 由联合熵和条件熵的定义可以得到：

  $$H(X,Y)=H(X)+H(Y|X)=H(Y)+H(X|Y)$$

  推论：

  $$H(X,Y|Z)=H(X|Z)+H(Y|X,Z)$$

• 相对熵是两个随机分布之间距离的度量，$D(p\parallel q)$度量当真实分布为$p$，而假定分布为$q$时的无效性，相对熵或Kullback-Leibler距离定义为：

  $$\begin{aligned}D(p\parallel q) &= \sum_{x\in \mathcal{X}} p(x) \log \frac{p(x)}{q(x)} \\&= E_p\left[\log \frac{p(X)}{q(X)}\right]\end{aligned}$$

  相对熵是非负的，并且当且仅当$p=q$时相对熵为零。相对熵在统计学、机器学习和信息论中有广泛的应用，特别是在模型选择和概率分布之间的比较中。

• 单个随机变量的熵为该随机变量的不确定度，定义涉及两个随机变量的条件熵$H(X|Y)$，即一个随机变量在给定另一个随机变量的条件下的熵，由另一随机变量导致的原随机变量不确定度的缩减量成为互信息。

  $$\begin{aligned}I(H;Y)&=H(X)-H(X|Y) \\&=\sum_{x,y} p(x,y) \log \frac{p(x,y)}{p(x)p(y)} \\&=D(p(x,y)\parallel p(x)p(y)) \\&=E_{p(x,y)}\left[\log \frac{p(X,Y)}{p(X)p(Y)}\right]\end{aligned}$$

  互信息$I(X;Y)$衡量了随机变量$X$和$Y$之间的独立程度，关于$X$和$Y$对称，且永远为非负值，当且仅当$X$和$Y$独立时，互信息为零。

• 高斯熵
  以上介绍的都是离散熵，当随机变量是连续时，定义微分熵：

  $$H(X)=-\int p(x) \log p(x) dx$$

  但这个式子不好算，数值无意义，因此大多数机器人领域工作都假设信念是高斯分布$X\sim\mathcal{N}(\mu, \Sigma)$，其高斯熵的封闭形式为：

  $$H(X)=\frac{1}{2} \log \left((2\pi e)^d |\Sigma|\right)$$

  其中：

  • d是随机变量的维数
  • $\Sigma$是随机变量的协方差矩阵，约等于不确定性体积

• KL散度
  KL散度是衡量两个概率分布之间差异的非对称度量，在这里可以衡量一个信念和另一个信念之间的差异，是信息损失和认知落差。离散形式为：

  $$D_{KL}(P\parallel Q) = \sum_x P(x) \log \frac{P(x)}{Q(x)}$$

  连续形式为：

  $$D_{KL}(P\parallel Q) = \int P(x) \log \frac{P(x)}{Q(x)} dx$$

  在POMDP中，信息增益=信念更新前后分布的KL散度：

  $$IG=\mathbb{E}_z[D_{KL}(p(s|z,a)\parallel p(s))]$$

  对于两个高斯分布的KL散度：
  如果$p=\mathcal{N}(\mu_0, \Sigma_0)$和$q=\mathcal{N}(\mu_1, \Sigma_1)$，则：

  $$D_{KL}(p\parallel q) = \frac{1}{2} \left[ \ln\left(\frac{|\Sigma_1|}{|\Sigma_0|}\right) - d + \text{tr}(\Sigma_1^{-1} \Sigma_0) + (\mu_1 - \mu_0)^T \Sigma_1^{-1} (\mu_1 - \mu_0) \right]$$

  前两项为不确定性体积变化，后两项为均值变化。在主动感知中，可以只保留协方差项。

  这也就意味着：

  $$IG=H(b_t)-\mathbb{E}[H(b_{t+1})] \\IG=\mathbb{E}[D_{KL}(b_{t+1}\parallel b_t)]$$

  在信息论中是两种等价的写法。

$$\mathcal{M}=(\mathcal{S,A,O,T,Z})$$

相比于MDP，POMDP引入了对环境状态的不完全观测。在POMDP中，智能体无法直接观察到环境的真实状态，而是通过观测获得部分信息。因此，智能体需要维护一个信念状态（belief state），即对环境状态的概率分布，以便在不确定性下进行决策。观测$\mathcal{Z}$是所有观测量$z$构成的集合，观测函数$\mathcal{O}$得到观测$z$的概率分布：

$$\mathcal{O}(z|s',a)=P(z|s',a)$$

MDP问题的求解

MDP问题的求解目标是找到最优策略对应的状态值函数和动作值函数。

策略的定义为：

$$\pi(a|s)$$

代表策略在状态$s$下选择动作$a$的概率。对应的状态值函数和动作值函数分别为：

$$V^{\pi}(s)=E_{\pi}[G_t|s]$$

$$Q^{\pi}(s,a)=E_{\pi}[G_t|s,a]$$

其中，$E$代表期望，$G_t$表示从时间步$t$开始的累积折扣奖励，$\gamma$为折扣因子：

$$G_t=\sum_{k=0}^{\infty}\gamma^k r_{t+k+1}$$

不同的策略会导致不同的状态值函数和动作值函数。最优策略$\pi^*$对应的最优状态值函数和动作值函数分别为：

$$V^*(s)=\max_{\pi} [R(s,a)+\gamma\sum_{s'}\Pr(s'|s,a)V^*(s')]$$

$$Q^*(s,a)=R(s,a)+\gamma\sum_{s'}\Pr(s'|s,a)\max_{a'}Q^*(s',a')$$

贝尔曼方程为求解最优策略提供了递归关系。

贝尔曼期望方程

贝尔曼方程用于计算给定策略$\pi$时价值函数在策略指引下所采轨迹上的期望。考虑如下随机轨迹：

$$S_t \xrightarrow{A_t} R_{t+1}, S_{t+1} \xrightarrow{A_{t+1}} R_{t+2}, S_{t+2} \xrightarrow{A_{t+2}} R_{t+3}$$

累积汇报$G_t$定义为：

$$\begin{aligned}G_t &= R_{t+1} + \gamma R_{t+2} + \gamma^2 R_{t+3} + \dots, \\ &= R_{t+1} + \gamma(R_{t+2} + \gamma R_{t+3} + \dots), \\ &= R_{t+1} + \gamma G_{t+1}.\end{aligned}$$

状态值函数的贝尔曼方程为：

$$\begin{aligned}v_\pi(s) &\doteq \mathbb{E}_\pi[G_t \mid S_t = s] \\&= \mathbb{E}_\pi[R_{t+1} + \gamma G_{t+1} \mid S_t = s] \\&= \sum_a \pi(a|s) \sum_{s',r} p(s', r|s, a)\left[r + \gamma v_\pi(s')\right], \quad \forall s \in \mathcal{S}.\end{aligned}$$

推导：

$$\begin{aligned}v_\pi(s) &= \mathbb{E}[G_t \mid S_t = s] \\&= \mathbb{E}[R_{t+1} + \gamma G_{t+1} \mid S_t = s] \\&= \mathbb{E}[R_{t+1} \mid S_t = s] + \gamma \mathbb{E}[G_{t+1} \mid S_t = s].\end{aligned}$$

考虑第一部分$\mathbb{E}[R_{t+1} \mid S_t = s]$，应用全概率公式：

$$P(A)=\sum_{i} P(A|B_i)P(B_i),$$

得到：

$$\begin{aligned}\mathbb{E}[R_{t+1} \mid S_t = s] &= \sum_a \pi(a|s) \mathbb{E}[R_{t+1} \mid S_t = s, A_t = a] \\&= \sum_a \pi(a|s) \sum_r p(r|s,a) r.\end{aligned}$$

考虑第二部分$\mathbb{E}[G_{t+1} \mid S_t = s]$，应用全概率公式和马尔可夫性质（在给定当前状态的情况下，未来状态的条件概率分布仅依赖于当前状态，而与过去状态无关）：

$$\begin{aligned}\mathbb{E}[G_{t+1} \mid S_t = s] &= \sum_{s'} \mathbb{E}[G_{t+1} \mid S_t = s, S_{t+1} = s'] p(s' \mid s) \\&= \sum_{s'} \mathbb{E}[G_{t+1} \mid S_{t+1} = s'] p(s' \mid s) \\&= \sum_{s'} v_\pi(s') p(s' \mid s) \\&= \sum_{s'} v_\pi(s') \sum_a p(s' \mid s, a) \pi(a \mid s).\end{aligned}$$

合并两个部分：

$$\begin{aligned}v_\pi(s) &= \mathbb{E}[R_{t+1} \mid S_t = s] + \gamma \mathbb{E}[G_{t+1} \mid S_t = s], \\&= \underbrace{\sum_a \pi(a|s) \sum_r p(r|s,a) r}_{\text{mean of immediate rewards}} + \gamma \underbrace{\sum_a \pi(a|s) \sum_{s'} p(s'|s,a) v_\pi(s')}_{\text{mean of future rewards}}, \\&= \sum_a \pi(a|s) \left[ \sum_r p(r|s,a) r + \gamma \sum_{s'} p(s'|s,a) v_\pi(s') \right], \quad \forall s \in \mathcal{S} \\&= \sum_a \pi(a|s) \sum_{s',r} p(s', r|s,a) \left[ r + \gamma v_\pi(s') \right], \quad \forall s \in \mathcal{S}.\end{aligned}$$

策略迭代算法

给定策略$\pi$，利用贝尔曼期望方程，对状态值函数进行更新：

$$V^{k+1}(s) = \sum_a \pi(a|s) \left[ R(s,a) + \gamma \sum_{s'} P(s'|s,a) V^k(s') \right]$$

也就是使用第$k$轮的值函数，更新第$k+1$轮的值函数。重复该过程，直到值函数收敛至$V^\pi$。

当拿到$V^\pi(s)$后，可根据状态值函数对原有策略进行改进。也就是在每一个状态下都选择最优的动作：

$$\pi'(s) = \arg\max_a Q^\pi(s,a) = \arg\max_a \left\{ R(s,a) + \gamma \sum_{s'} P(s'|s,a) V^\pi(s') \right\}$$

值迭代算法

价值迭代算法单独维护一个状态值函数，不维护策略，并且直接利用贝尔曼最优方程进行状态价值函数的更新：

$$V^{k+1}(s) = \max_a \left[ R(s,a) + \gamma \sum_{s'} P(s'|s,a) V^k(s') \right]$$

价值迭代算法就是直接利用上述方程对状态值函数进行迭代更新，直到收敛至最优状态值函数$\left\| V^{k+1} - V^k \right\| \leq \Delta$。

POMDP问题的求解

MDP问题的求解结果是得到了值函数$V^*(s)$或$Q^*(s,a)$，相当于两个大表格，但是POMDP中智能体无法直接观测到环境状态$s$，因此无法直接使用MDP的求解方法，同时最优决策依赖的是历史信息的统计汇总，而不是当前的观测。在POMDP中，智能体需要维护一个信念状态(Belief State)$b(s)$，表示在当前观测$z$下环境状态为$s$的概率分布：

$$b_t(s)=\Pr(s_t=s|z_{1:t},a_{1:t-1})$$

$b(s)$存在无限多情况，不能通过表格来描述，引入信念空间后，POMDP的求解目标变成了计算信念状态下的最优值函数$V^*(b)$或动作值函数$Q^*(b,a)$，即在信念状态$b$下采取最优策略所能获得的最大期望累积奖励。

信念状态的更新

根据马尔可夫性质，根据上一时刻的信念状态$b_{t-1}$、采取的动作$a_{t-1}$和当前观测$z_t$，可以计算当前时刻的信念状态$b_t$：

$$\begin{aligned}b_t(s') &= \tau(b_{t-1}, a_{t-1}, z_t)(s') \\&= \frac{1}{\Pr(z_t|b_{t-1}, a_{t-1})} \Pr(z_t|a_{t-1}, s') \sum_{s \in S} \Pr(s'|s, a_{t-1}) b_{t-1}(s)\end{aligned} \\\Pr(z_t | b_{t-1}, a_{t-1}) = \sum_{s' \in S} \Pr(z_t | a_{t-1}, s') \sum_{s \in S} \Pr(s' | s, a_{t-1}) b_{t-1}(s)$$

其中，$\Pr(z_t|a_{t-1}, s')$是观测函数$\mathcal{O}$，$\Pr(s'|s, a_{t-1})$是转移函数$\mathcal{T}$。

Belief MDP

通过引入信念状态，POMDP可以转换为一个等价的MDP，称为Belief MDP。在Belief MDP中，状态空间是信念空间$\mathcal{B}$，动作空间仍然是$\mathcal{A}$。转移函数$\tau$和奖励函数$R$需要根据信念状态进行定义。

奖励函数定义为：

$$R_B(b,a) = \sum_{s \in S} b(s) R(s,a)$$

Belief MDP的状态值函数和动作值函数满足的贝尔曼方程为：

$$V^*(b) = \max_{a \in A} \left[ R_B(b, a) + \gamma \sum_{z \in Z} \Pr(z | b, a) V^*(\tau(b, a, z)) \right] \\Q^*(b, a) = R_B(b, a) + \gamma \sum_{z \in Z} \Pr(z | b, a) V^*(\tau(b, a, z))$$

POMCP(Partially Observable Monte Carlo Planning)

MASt3R-SLAM 是一种基于深度学习的实时单目 SLAM（Simultaneous Localization and Mapping）系统。它结合了传统 SLAM 方法和深度神经网络的优势，能够在环境中实现高精度的相机定位和三维地图构建。相比于先前的DROID-SLAM，进一步提升了优化的效率和精度，本文重点关注前端优化部分。

位姿优化

MASt3R-SLAM的位姿优化函数为gauss_newton_ray_cuda，通过射线对齐方法(Ray Alignment)来优化相机位姿。其核心思想是最小化观测点与地图点之间的距离误差。

• Sim3群表示

  平移$t\in \mathbb{R}^3$，旋转$q=[q_x,q_y,q_z,q_w]$，缩放$s\in \mathbb{R}^+$，则李代数参数化可表示为：

  $$\zeta=[\tau,\omega,\sigma]^T \in \mathbb{R}^7$$

  其中$\tau \in \mathbb{R}^3$表示平移，$\omega \in \mathbb{R}^3$表示旋转，$\sigma \in \mathbb{R}$表示尺度对数。

• 相对位姿计算

  给定两个相机位姿$T_i,T_j \in \mathrm{Sim}(3)$，其相对位姿$T_{ij}$可通过李代数指数映射计算：

  $$\begin{aligned}T_{ij} &= T_j \circ T_i^{-1}\\s_{ij}&=s_i^{-1}\cdot s_j\\q_{ij}&=q_i^{-1}\otimes q_j\\t_{ij}&=s_i^{-1}\cdot R_i^{-1} (t_j - t_i)\end{aligned}$$

残差定义

射线方向残差

将点归一化为单位射线：

$$\mathbf{r_i}=\frac{X_i}{\|X_i\|}$$

将$X_j$变换到相机$i$坐标系下：

$$X_{j,C_i}=s_{ij} R_{ij} X_j+t_{ij}$$

归一化变换后的点：

$$\mathbf{r_{j,C_i}}=\frac{X_{j,C_i}}{\|X_{j,C_i}\|}$$

射线方向残差定义为：

$$\mathbf{e_{ray}}=\mathbf{r_i}-\mathbf{r_{j,C_i}}$$

距离残差

$$e_{dist}=\|\mathbf{X_i}-\mathbf{X_{j,C_i}}\|$$

雅可比矩阵计算

目标：$J_i=\frac{\partial e}{\partial \zeta_i}, J_j=\frac{\partial e}{\partial \zeta_j}$

链式法则分解：

$$\frac{\partial e}{\partial \zeta_j} = \frac{\partial e}{\partial \mathbf{r_{j,C_i}}}\cdot \frac{\partial \mathbf{r_{j,C_i}}}{\partial X_{j,C_i}} \cdot \frac{\partial X_{j,C_i}}{\partial \zeta_{i,j}}\cdot \frac{\partial \zeta_{i,j}}{\partial \zeta_j}$$

归一化雅可比

$$\mathbf{r}=\frac{X}{\|X\|}=\frac{X}{\sqrt{X_x^2+X_y^2+X_z^2}}$$

令$n=\|X\|=\sqrt{X^TX}$:

$$\begin{aligned}\frac{\partial n}{\partial X}&=\frac{X^T}{n}\\\frac{\partial \mathbf{r}}{\partial X}&=\frac{\partial }{\partial X}(\frac{X}{n})\\&=\frac{1}{n}I - \frac{X}{n^2}\frac{\partial n}{\partial X}\\&=\frac{1}{n}(I-\frac{XX^T}{n^2})\\&=\frac{1}{\|X\|}(I - \mathbf{r}\mathbf{r}^T)\\&=\frac{1}{\|X\|}(I-\frac{XX^T}{\|X\|^2})\end{aligned}$$

$$\begin{aligned}\frac{\partial r_x}{\partial X_x}&=\frac{1}{\|X\|}-\frac{X_x^2}{\|X\|^3}\\\frac{\partial r_x}{\partial X_y}&=-\frac{X_x X_y}{\|X\|^3}\\\frac{\partial r_x}{\partial X_z}&=-\frac{X_x X_z}{\|X\|^3}\end{aligned}$$

同理可得$r_y,r_z$对$X$的偏导数

Sim3变换雅可比

$$X'=sRX+t$$

李代数扰动：

$$\delta \zeta=[\delta \tau,\delta \omega,\delta \sigma]^T$$

$$X'=\exp(\delta \boldsymbol{\zeta})\cdot X \approx X+\left.\frac{\partial X'}{\partial \zeta}\right|_{\zeta=0}\delta \zeta$$

平移：

$$\frac{\partial X'}{\partial \zeta}=\begin{bmatrix}\frac{\partial X'}{\partial \tau} & \frac{\partial X'}{\partial \omega} & \frac{\partial X'}{\partial \sigma}\end{bmatrix}$$

$$\frac{\partial X'}{\partial \tau} = I_{3\times 3}$$

旋转：

$$\begin{aligned}X'&=RX\\\delta R&=I+[\delta \omega]_\times\\X'&=(I+[\delta \omega]_\times)X=X+[\delta \omega]_\times X\end{aligned}$$

$$\frac{\partial X'}{\partial \omega} = -[X']_\times\\=\begin{bmatrix}0 & x'_z & -x'_y \\-x'_z & 0 & x'_x \\x'_y & -x'_x & 0\end{bmatrix}$$

尺度：

$$X'=sRX+t$$

$$\frac{\partial X'}{\partial s}=RX$$

设$s=e^\sigma$，则：

$$\frac{\partial X'}{\partial \sigma}=\frac{\partial X'}{\partial s}\cdot \frac{\partial s}{\partial \sigma}=sRX=X'-t\approx X'$$

$$\begin{aligned}\frac{\partial r}{\partial \sigma}&=\frac{\partial}{\partial \sigma}(\frac{X'}{\|X'\|})=\frac{1}{\|X'\|}(I-\frac{X'X'^T}{\|X'\|^2})\cdot \frac{\partial X'}{\partial \sigma}\\&=\frac{1}{\|X'\|}(I-\frac{X'X'^T}{\|X'\|^2})X'=0\end{aligned}$$

$$\begin{aligned}\frac{\partial e_{dist}}{\partial \sigma}&=\frac{\partial \|X'\|}{\partial \sigma}=\frac{\partial \|X'\|}{\partial X'}\cdot \frac{\partial X'}{\partial \sigma}=\frac{X'^T}{\|X'\|}\cdot X'=\|X'\|\end{aligned}$$

完整的雅可比为：

对于点$X'=sRX+t$:

$$J_{local}=\begin{bmatrix}I_{3\times 3} & -[X']_\times & X'\end{bmatrix}$$

射线方向误差：

$$\begin{aligned}J_{ray}^{local}&=\frac{\partial r}{\partial X'}\cdot J_{local}\\J_{i,j}&=\sum_{k=1}^3 \frac{\partial r_i}{\partial X'_k} \cdot \frac{\partial X'_k}{\partial \zeta_j}\end{aligned}$$

伴随变换

优化变量是全局位姿$\zeta_i,\zeta_j$，但雅可比是在局部坐标系$T_{ij}$下计算的，需要通过伴随变换转换：

$$J_{global}=\mathbf{Ad}_{T_{ij}}^{-T}J_{local}$$

Sim3伴随表示：

$$\mathbf{Ad}=\begin{bmatrix}sR & 0 & 0 \\[t]_\times sR & sR & 0 \\t^TsR & 0 & 1\end{bmatrix}$$

伴随矩阵的转置：

$$J_{global} = \mathbf{Ad}_{T_{ij}}^{-T} J_{local}$$

伴随逆公式：

$$\mathbf{Ad}^{-1}=\begin{bmatrix}s^{-1}R^T & 0 & 0 \\-s^{-1}[t]_\times R^T & s^{-1}R^T & 0 \\-s^{-1}t^TR^T & 0 & 1\end{bmatrix}$$

从相机$j$到相机$i$的雅可比：

$$\begin{aligned}J_j&=\frac{\partial e}{\partial \zeta_j} \\&=\frac{\partial e}{\partial X_{j,C_i}} \cdot \frac{\partial X_{j,C_i}}{\partial \zeta_j} \cdot \mathbf{Ad}_{T_{ij}}^{-T}\end{aligned}$$

对相机i的雅可比：

$$\begin{aligned}T_{ij}&=T_i^{-1} \circ T_j\\J_i&=\frac{\partial e}{\partial \zeta_i} \\&=-\frac{\partial e}{\partial X_{j,C_i}} \cdot \frac{\partial X_{j,C_i}}{\partial \zeta_{ij}} \cdot \mathbf{Ad}_{T_{ij}}^{-T} \\&=-J_j\end{aligned}$$

Gauss-Newton优化

Hessian矩阵：

对每条边(相机对i,j)的Hessian矩阵为：

$$\begin{aligned}H_{ii}&=\sum_k J_i^{kT}W_kJ_i^k\\H_{ij}&=\sum_k J_i^{kT}W_kJ_j^k\\H_{jj}&=\sum_k J_j^{kT}W_kJ_j^k\end{aligned}$$

梯度：

$$\begin{aligned}g_i&=\sum_k J_i^{kT}W_k e_k\\g_j&=\sum_k J_j^{kT}W_k e_k\end{aligned}$$

得到：

$$H\cdot \Delta \zeta = -g$$

DROID-SLAM 是一种基于深度学习的实时单目 SLAM（Simultaneous Localization and Mapping）系统。它结合了传统 SLAM 方法和深度神经网络的优势，能够在动态环境中实现高精度的相机定位和三维地图构建。相比于后来的MASt3R-SLAM，它能够保证全局一致性，本文重点关注后端优化部分。

高斯牛顿法

$$\begin{aligned}&\arg\min_x f(x) \\f(x)&=\frac{1}{2}\sum_i^m r_i(x)^2=\frac{1}{2}\|r(x)\|^2\end{aligned}$$

一阶泰勒展开：

$$\begin{aligned}r(x+\Delta x) &\approx r(x)+J(x)\Delta x \\J(x)&=\frac{\partial r(x)}{\partial x} \end{aligned}$$

近似后的优化问题：

$$\begin{aligned}&\arg\min_{\Delta x} \frac{1}{2}\|r(x)+J(x)\Delta x\|^2 \\f(x+\Delta x) &\approx \frac{1}{2}(r+J\Delta x)^T(r+J\Delta x) \\&=\frac{1}{2}(r^Tr + r^TJ\Delta x + \Delta x^TJ^Tr + \Delta x^TJ^TJ\Delta x) \end{aligned}$$

其中：$r^TJ\Delta x+\Delta x^TJ^Tr=2 r^TJ\Delta x$

定理：

$$\begin{aligned}\frac{\partial}{\partial x}(\frac{1}{2}x^TAx)&= Ax \\(C^Tx)'&=C\end{aligned}$$

令：

$$\frac{\partial f}{\partial\Delta x}=J^Tr+J^TJ\Delta x=0$$

可得正规方程：

$$J^TJ\Delta x=-J^Tr$$

迭代公式：