统一高效因子图加速器设计

问题和解决

问题

定位问题稀疏矩阵，用处理器浪费计算资源。

通常使用加速器堆叠算法，芯片面积很大。

先前工作

1. 定位

P-BA ： 针对 ORB-based SLAM 系统，将计算密集型的Bundle Adjustment (BA) 任务的核心部分（Jacobian 计算和 Schur 消元）放在 FPGA 上加速，其余在软件中实现。

BAX ： 完整的 BA 硬件加速器，使用通用向量单元，但它只针对 BA 这一特定子任务。

2. 规划

BLITZCRANK： 利用因子图抽象来减少优化问题的规模。通过优化因子图的推理顺序**，它相比 CPU 软件实现，实现了 7.4 倍加速和 29.7 倍能耗降低。

3. 控制

Lin et al. ： 开发了一种基于采样的运动控制加速器，旨在最大化控制率和轨迹时间步数量，相比现有技术 [38] 实现了 22 倍和 26 倍的提升。

后面都可以看看

解决

提出新型位姿表示法，结合因子图抽象，为定位、规划与控制算法构建了统一的因子计算模型

针对系数矩阵J提出高效稀疏数据压缩格式，有效减少索引信息存储量，并优化矩阵访问与更新效率

设计高速高能效因子图加速器，支持因子图计算模型与稀疏格式。

前置知识

因子图计算

线性代数快忘完了，QR分解是将将矩阵分解为一个正交矩阵Q和一个上三角矩阵R的乘积

姿态表示方法

不同的姿态表示方法

算法	表示方法	核心操作
运动规划	特殊欧几里得群 $SE(3)$ 和李代数 $\mathfrak{se}(3)$	旋转通过乘法运算实现（群乘法）。
定位	四元数 $q$ 和位置向量 $T(3)$	旋转通过加法运算实现。

数学，已畏惧，下面是查了下的快乐数学补课部分

$SE(3)$

在三维空间中，一个点的姿态有六个自由度，包括三个平移自由度（位置）和三个旋转自由度（方向）。

表示方式： 姿态变换可以用一个 $4 \times 4$ 的齐次坐标变换矩阵 $\mathbf{T}$ 来描述，该矩阵属于特殊欧几里得群 $SE(3)$ 。
矩阵结构： $\mathbf{T}$ $T$ 矩阵由两部分组成：
1. 方向 (Orientation)： 一个 $3 \times 3$ 的旋转矩阵 $\mathbf{R}$ ，属于特殊正交群 $SO(3)$ ( $\mathbf{R} \in SO(3) \in \mathbb{R}^{3 \times 3}$ )。
2. 位置 (Position)： 一个 $3 \times 1$ 的平移向量 $\mathbf{t}$ ( $\mathbf{t} \in T(3) \in \mathbb{R}^{3}$ )。

\mathbf{T} = \begin{pmatrix} \mathbf{R} & \mathbf{t} \\ \mathbf{0}^T & 1 \end{pmatrix}

冗余性

旋转矩阵 $\mathbf{R}$ 的冗余： 旋转本身只有 3 个自由度，但却使用一个包含 9 个变量的 $\mathbf{R}$ 矩阵来表示。（原因：这 9 个变量之间存在 6 个约束条件（正交性 $\mathbf{R}^T \mathbf{R} = \mathbf{I}$ 和行列式 $\text{det}(\mathbf{R})=1$ ）。

变换矩阵 $\mathbf{T}$ 的冗余： 完整的姿态只有 6 个自由度，却使用一个 $4 \times 4$ 矩阵，即 16 个变量来表示。（原因：除了旋转矩阵本身的 6 个约束外，底部一行（ $\mathbf{0}^T$ 和 1）是固定的。）

$\mathfrak{se}(3)$

李代数 $\mathfrak{se}(3)$ 的元素由一个六维向量 $\boldsymbol{\xi}$ 确定，这个向量精确地包含了姿态的所有 6 个独立自由度。

\boldsymbol{\xi} = \begin{pmatrix} \boldsymbol{\rho} \\ \boldsymbol{\phi} \end{pmatrix} \in \mathbb{R}^6

其中 $\boldsymbol{\rho} \in \mathbb{R}^3$ 表示平移分量， $\boldsymbol{\phi} \in \mathbb{R}^3$ 表示旋转轴角分量。

这个六维向量 $\boldsymbol{\xi}$ 被映射为一个 $4 \times 4$ 的反对称矩阵 $\mathbf{\Phi}$ ，即李代数 $\mathfrak{se}(3)$ 的元素：

\mathbf{\Phi} = \boldsymbol{\xi}^{\wedge} = \begin{pmatrix} \boldsymbol{\phi}^{\wedge} & \boldsymbol{\rho} \\ \mathbf{0}^T & 0 \end{pmatrix} \in \mathfrak{se}(3) \subset \mathbb{R}^{4 \times 4}

其中 $\boldsymbol{\phi}^{\wedge} \in \mathbb{R}^{3 \times 3}$ 是由 $\boldsymbol{\phi}$ 向量构造的反对称矩阵。

需要涉及复杂扰动导数模型的变换：

\mathbf{T} = \exp(\mathbf{\Phi}) = \exp(\boldsymbol{\xi}^{\wedge})

\frac{\partial f}{\partial \boldsymbol{\xi}} = \frac{\partial f}{\partial \mathbf{T}} \cdot \frac{\partial \mathbf{T}}{\partial \boldsymbol{\xi}}

$\frac{\partial \mathbf{T}}{\partial \boldsymbol{\xi}}$ 中 $\mathbf{T}$ 是 $\boldsymbol{\xi}$ 的非线性指数函数。

新姿态表示 $< \mathfrak{so}(n), T(n) >$

姿态结构： 姿态 $\boldsymbol{\xi}$ 被表示为一个二元组 $< \boldsymbol{\phi}, \mathbf{t} >$ 。

旋转部分 ( $\boldsymbol{\phi}$ ): 使用李代数 $\mathfrak{so}(n)$ 的元素，即 旋转向量 $\boldsymbol{\phi}$ 。它通过 $\mathbf{R}_i = \text{Exp}(\boldsymbol{\phi}_i)$ 映射到旋转矩阵 $\mathbf{R}_i$ 。

平移部分 ( $\mathbf{t}$ ): 使用一个简单的 平移向量 $\mathbf{t}$ 。

广义运算：

计算模型

Lidar因子误差

\mathbf{e} (\boldsymbol{\xi}_i, \boldsymbol{\xi}_j) = (\boldsymbol{\xi}_i \ominus \boldsymbol{\xi}_j) \ominus \Delta \boldsymbol{\xi}_{\text{obs}} \tag{3}

到

\mathbf{e} (\boldsymbol{\xi}_i, \boldsymbol{\xi}_j) = < \text{Log}(\Delta \mathbf{R}_{\text{obs}}^T \mathbf{R}_j^T \mathbf{R}_i), \Delta \mathbf{R}_{\text{obs}}^T (\mathbf{R}_j^T (\mathbf{t}_i - \mathbf{t}_j) - \Delta \mathbf{t}_{\text{obs}}) > \tag{4}

被数学轰炸了，还是有点一知半解，但这样看的话确实避免了冗余和太复杂的计算

稀疏矩阵存储

稀疏矩阵压缩格式

线性二次调节器 (LQR)：

稀疏，分块，连续

特征	传统的 CSR 格式	基于连续性优化的新格式
非零块数据 (`VAL`)	大小： $NZ$ 块 (存储所有非零块)	大小： $NZ$ 块 (存储所有非零块)
列索引 (`COL_IND`)	大小： $NZ$ (存储每个非零块的列索引)	大小： $M$ (只存储每行第一个非零块的列索引)
行信息数组	`ROW_PTR` (大小： $M+1$ )，记录每行起始位置	`NUM_NZ` (大小： $M$ )，记录每行非零块数量/跨度