AX=XB 求解

slam 中的手眼标定最后可以归结为求解如下问题
已知 $\boldsymbol A_i, \boldsymbol B_i \in SO(3)$ ，需要求解 $\boldsymbol X \in SO(3)$ ，满足
$\min \sum_{i = 1}^n d(\boldsymbol A_i \boldsymbol X - \boldsymbol X \boldsymbol B_i)$

其中 $SO(3)$ 即为旋转矩阵，网上有多种求解方法，这里只描述其中几种，首先介绍下一些基本的变换

1、基本变换

hat运算符 $\land$

把三维实向量 $\boldsymbol\omega$ 映射到 $3 \times 3$ 反对称阵 $\boldsymbol W$
$\boldsymbol \omega^{\land} =\begin{bmatrix} \omega_1\\ \omega_2\\ \omega_3 \end{bmatrix}^{\land} =\begin{bmatrix} 0 & -\omega_3 & \omega_2 \\ \omega_3 & 0 & -\omega_1 \\ -\omega_2 & \omega_1 & 0 \end{bmatrix} =\boldsymbol W$

性质1： $\boldsymbol a^\land \boldsymbol b = - \boldsymbol b^\land \boldsymbol a$ ，直接推导即可证明

性质2： $\boldsymbol \omega^\land \boldsymbol \omega^\land \boldsymbol \omega^\land = - \| \boldsymbol \omega \|^2 \boldsymbol \omega^\land$ ，直接推导即可证明

性质3： $\boldsymbol \omega \boldsymbol \omega^T - \boldsymbol \omega^\land \boldsymbol \omega^\land = \| \boldsymbol \omega \|^2 \boldsymbol I$ ，直接推导即可证明

性质4： $(\boldsymbol R \boldsymbol v)^\land = \boldsymbol R \boldsymbol v^\land \boldsymbol R^T$ ，证明如下
$\begin{aligned} &\forall \boldsymbol u \in \mathbb R^3, (\boldsymbol R \boldsymbol v) \times (\boldsymbol R \boldsymbol u) = \boldsymbol R(\boldsymbol v \times \boldsymbol u) \\ \Rightarrow &\forall \boldsymbol u \in \mathbb R^3, (\boldsymbol R \boldsymbol v)^\land (\boldsymbol R \boldsymbol u) = \boldsymbol R(\boldsymbol v^\land \boldsymbol u) \\ \Rightarrow &\forall \boldsymbol u \in \mathbb R^3, (\boldsymbol R \boldsymbol v)^\land \boldsymbol R \boldsymbol u = \boldsymbol R \boldsymbol v^\land \boldsymbol u \\ \Rightarrow &(\boldsymbol R \boldsymbol v)^\land \boldsymbol R = \boldsymbol R \boldsymbol v^\land \\ \Rightarrow &(\boldsymbol R \boldsymbol v)^\land = \boldsymbol R \boldsymbol v^\land \boldsymbol R^T \end{aligned}$

vee运算符 $\lor$

把 $3 \times 3$ 反对称阵 $\boldsymbol W$ 映射到三维实向量 $\boldsymbol\omega$
$\boldsymbol W^{\lor} =\begin{bmatrix} 0 & -\omega_3 & \omega_2 \\ \omega_3 & 0 & -\omega_1 \\ -\omega_2 & \omega_1 & 0 \end{bmatrix}^{\lor} =\begin{bmatrix} \omega_1\\ \omega_2\\ \omega_3 \end{bmatrix} =\boldsymbol\omega$

指数映射

把 $3 \times 3$ 反对称阵 $\boldsymbol W$ 映射到旋转矩阵 $\boldsymbol R$
$e^{\boldsymbol W} = \boldsymbol I + \frac{\sin \| \boldsymbol W^\lor \|}{\| \boldsymbol W^\lor \|} \boldsymbol W + \frac{1 - \cos \| \boldsymbol W^\lor \|}{\| \boldsymbol W^\lor \|^2} \boldsymbol W^2 = \boldsymbol R$

性质1：假设 $\theta = || \boldsymbol W^\lor||=\sqrt{\boldsymbol \omega_1^2 + \boldsymbol \omega_2^2 + \boldsymbol \omega_3^2}$ 为向量模长，即 $\boldsymbol \omega = \theta \hat{\boldsymbol \omega}$
$\begin{aligned} e^{\boldsymbol W} &= \sum_{n = 0}^\infty \frac{1}{n!} \boldsymbol W^n = \sum_{n = 0}^\infty \frac{1}{n!} (\boldsymbol \omega^\land)^n = \sum_{n = 0}^\infty \frac{1}{n!} (\theta \hat{\boldsymbol \omega}^\land)^n \\ &= \boldsymbol I + \theta \hat{\boldsymbol \omega}^\land + \frac{1}{2!}\theta^2 \hat{\boldsymbol \omega}^\land \hat{\boldsymbol \omega}^\land + \frac{1}{3!}\theta^3 \hat{\boldsymbol \omega}^\land \hat{\boldsymbol \omega}^\land \hat{\boldsymbol \omega}^\land + \frac{1}{4!}\theta^4 \hat{\boldsymbol \omega}^\land \hat{\boldsymbol \omega}^\land \hat{\boldsymbol \omega}^\land \hat{\boldsymbol \omega}^\land + \cdots \\ &= \boldsymbol I + \theta \hat{\boldsymbol \omega}^\land + \frac{1}{2!}\theta^2 \hat{\boldsymbol \omega}^\land \hat{\boldsymbol \omega}^\land - \frac{1}{3!}\theta^3 \hat{\boldsymbol \omega}^\land - \frac{1}{4!}\theta^4 \hat{\boldsymbol \omega}^\land \hat{\boldsymbol \omega}^\land + \cdots \\ &= \boldsymbol I + \hat{\boldsymbol \omega}^\land \hat{\boldsymbol \omega}^\land + \bigg(\theta - \frac{1}{3!} \theta^3 + \cdots \bigg) \hat{\boldsymbol \omega}^\land - \bigg(1 - \frac{1}{2!} \theta^2 + \frac{1}{4!} \theta^4 - \cdots \bigg) \hat{\boldsymbol \omega}^\land \hat{\boldsymbol \omega}^\land \\ &= \boldsymbol I + \hat{\boldsymbol \omega}^\land \hat{\boldsymbol \omega}^\land +\sin \theta \hat{\boldsymbol \omega}^\land - \cos \theta \hat{\boldsymbol \omega}^\land \hat{\boldsymbol \omega}^\land\\ &= \boldsymbol I + \sin \theta \hat{\boldsymbol \omega}^\land + (1 - \cos \theta) \hat{\boldsymbol \omega}^\land \hat{\boldsymbol \omega}^\land \end{aligned}$

这也正是罗德里格斯公式，可以从旋转几何出发进行推导

性质2： $\forall \boldsymbol X \in SO(3), e^{\boldsymbol X \boldsymbol W \boldsymbol X^T} = \boldsymbol X e^\boldsymbol W \boldsymbol X^T$ ，证明如下
$\begin{aligned} e^{\boldsymbol X \boldsymbol W \boldsymbol X^T} &= \sum_{n=0}^\infty \frac{1}{n!} (\boldsymbol X \boldsymbol W \boldsymbol X^T)^n \\ &= \sum_{n=0}^\infty \frac{1}{n!} \underbrace{\boldsymbol X \boldsymbol W \boldsymbol X^T \boldsymbol X \boldsymbol W\boldsymbol X^T \cdots \boldsymbol X \boldsymbol W\boldsymbol X^T}_n \\ &= \sum_{n=0}^\infty \frac{1}{n!} \boldsymbol X \boldsymbol W^n \boldsymbol X^T \\ &= \boldsymbol X \bigg(\sum_{n=0}^\infty \frac{1}{n!} \boldsymbol W^n \bigg) \boldsymbol X^T \\ &= \boldsymbol X e^{\boldsymbol W} \boldsymbol X^T \end{aligned}$

对数映射

把旋转矩阵 $\boldsymbol R$ 映射到 $3 \times 3$ 反对称阵 $\boldsymbol W$
$\log \boldsymbol R = \frac{\varphi (\boldsymbol R - \boldsymbol R^T)}{2 \sin \varphi} =\boldsymbol W$
其中 $\varphi = \arccos \bigg( \frac{tr(\boldsymbol R) - 1}{2} \bigg)$

2、Navy手眼标定算法

论文 "Robot sensor calibration: solving AX=XB on the Euclidean group"
考虑到 $\boldsymbol A_i, \boldsymbol B_i$ 均为旋转矩阵，因此存在三维实向量 $\boldsymbol \omega_{A_i} = (\log \boldsymbol A_i)^\lor, \boldsymbol \omega_{B_i} = (\log \boldsymbol B_i)^\lor$ 使得
$\boldsymbol A_i = e^{\boldsymbol \omega_{A_i}^\land}, \boldsymbol B_i = e^{\boldsymbol \omega_{B_i}^\land}$

利用前面的性质可以得到
$\begin{aligned} \boldsymbol A_i \boldsymbol X &= \boldsymbol X \boldsymbol B_i \\ \boldsymbol A_i &= \boldsymbol X \boldsymbol B_i \boldsymbol X^T \\ e^{\boldsymbol \omega_{A_i}^\land} &= \boldsymbol X e^{\boldsymbol \omega_{B_i}^\land} \boldsymbol X^T \\ e^{\boldsymbol \omega_{A_i}^\land} &= e^{\boldsymbol X \boldsymbol \omega_{B_i}^\land \boldsymbol X^T} \\ e^{\boldsymbol \omega_{A_i}^\land} &= e^{(\boldsymbol X \boldsymbol \omega_{B_i})^\land} \end{aligned}$

因此求解 $\boldsymbol A_i \boldsymbol X = \boldsymbol X \boldsymbol B_i$ 等价于求解 $\boldsymbol \omega_{A_i} = \boldsymbol X \boldsymbol \omega_{B_i}$ 因此原问题将会变为如下问题
$\min \sum_{i = 1}^n w_i d(\boldsymbol X \boldsymbol \omega_{\boldsymbol B_i} - \boldsymbol \omega_{A_i})$

如果定义距离 $d$ 为欧式距离，则该问题将会变成经典问题
$\min \sum_{i = 1}^n w_i \| \boldsymbol X \boldsymbol \omega_{\boldsymbol B_i} - \boldsymbol \omega_{A_i} \|^2$

具体推导在另一篇文章《点云精配准》求解旋转矩阵 $\boldsymbol R$ 中有详细描述，这里直接给出求解步骤：

找到一系列旋转矩阵 $\boldsymbol A_i, \boldsymbol B_i$
计算对应的三维向量 $\boldsymbol \omega_{A_i} = (\log \boldsymbol A_i)^\lor, \boldsymbol \omega_{B_i} = (\log \boldsymbol B_i)^\lor$
对协方差矩阵 $\boldsymbol \Sigma$ 进行SVD分解 $\boldsymbol \Sigma = \boldsymbol U \boldsymbol \Lambda \boldsymbol V^T$ ，其中协方差矩阵 $\boldsymbol \Sigma$ 为
$\boldsymbol \Sigma = \begin{bmatrix} \boldsymbol \omega_{\boldsymbol B_1} & \cdots & \boldsymbol \omega_{\boldsymbol B_n} \end{bmatrix} \begin{bmatrix} w_1 & 0 & \cdots & 0 \\ 0 & w_2 & \cdots & 0 \\ \vdots & \vdots & \ddots & \vdots \\ 0 & 0 & \cdots & w_n \end{bmatrix} \begin{bmatrix} \boldsymbol \omega_{\boldsymbol A_1}^T \\ \vdots \\ \boldsymbol \omega_{\boldsymbol A_n}^T \end{bmatrix}$
求解得到旋转矩阵 $\boldsymbol X^* = \boldsymbol V \begin{bmatrix} 1 & 0 & \cdots & 0 \\ 0 & 1 & \cdots & 0 \\ \vdots & \vdots & \ddots & \vdots \\ 0 & 0 & \cdots & |\boldsymbol V \boldsymbol U^T| \end{bmatrix}\boldsymbol U^T$

带有初值的近似解

如果有一个不错的初值 $\boldsymbol X_0 \in SO(3)$ ，问题变成需要求解得到一个 $\boldsymbol Y$ 使得
$\boldsymbol Y \boldsymbol X_0 \boldsymbol \omega_{\boldsymbol B_i} = \boldsymbol \omega_{A_i}, \quad i = 1,\cdots, n$

注意到 $\boldsymbol Y = \boldsymbol X^* \boldsymbol X_0^T$ ，因此 $\boldsymbol Y \in SO(3)$ 可以用欧拉角表示，同时注意到 $\boldsymbol X_0$ 是一个不错的初值，因此有 $yall, pitch, roll \to 0$ ，进而 $\boldsymbol Y$ 可以近似表示成
$\begin{aligned} \boldsymbol Y =& \boldsymbol R_z(yall) \boldsymbol R_y(pitch) \boldsymbol R_x(roll) \\ =& \begin{bmatrix} \cos y & -\sin y & 0\\ \sin y & \cos y & 0\\ 0 & 0 & 1 \end{bmatrix} \begin{bmatrix} \cos p & 0 & \sin p\\ 0 & 1 & 0\\ -\sin p & 0 & \cos p \end{bmatrix} \begin{bmatrix} 1 & 0 & 0\\ 0 & \cos r & -\sin r\\ 0 & \sin r & \cos r\\ \end{bmatrix} \\ \approx& \begin{bmatrix} 1 & -y & 0\\ y & 1 & 0\\ 0 & 0 & 1 \end{bmatrix} \begin{bmatrix} 1 & 0 & p\\ 0 & 1 & 0\\ -p & 0 &1 \end{bmatrix} \begin{bmatrix} 1 & 0 & 0\\ 0 & 1 & -r\\ 0 & r & 1\\ \end{bmatrix} \\ \approx& \begin{bmatrix} 1 & -y & p\\ y & 1 & -r\\ -p & r & 1\\ \end{bmatrix} \\ =& \boldsymbol I + \begin{bmatrix} r\\ p\\ y \end{bmatrix}^\land \end{aligned}$

带入前式可以得到
$\boldsymbol \omega_{A_i} = \begin{bmatrix} r\\ p\\ y \end{bmatrix}^\land \boldsymbol X_0 \boldsymbol \omega_{\boldsymbol B_i} + \boldsymbol X_0 \boldsymbol \omega_{\boldsymbol B_i} = -(\boldsymbol X_0 \boldsymbol \omega_{\boldsymbol B_i})^\land \begin{bmatrix} r\\ p\\ y \end{bmatrix} + \boldsymbol X_0 \boldsymbol \omega_{\boldsymbol B_i}$

因此原问题变成
$(\boldsymbol X_0 \boldsymbol \omega_{\boldsymbol B_i})^\land \begin{bmatrix} r\\ p\\ y \end{bmatrix} = \boldsymbol X_0 \boldsymbol \omega_{\boldsymbol B_i} - \boldsymbol \omega_{A_i}, \quad i = 1,\cdots,n$

将 $n$ 个方程可以写成矩阵形式
$\begin{bmatrix} (\boldsymbol X_0 \boldsymbol \omega_{\boldsymbol B_1})^\land \\ \vdots \\ (\boldsymbol X_0 \boldsymbol \omega_{\boldsymbol B_n})^\land \end{bmatrix} \begin{bmatrix} r\\ p\\ y \end{bmatrix} = \begin{bmatrix} \boldsymbol X_0 \boldsymbol \omega_{\boldsymbol B_1} - \boldsymbol \omega_{A_1} \\ \vdots \\ \boldsymbol X_0 \boldsymbol \omega_{\boldsymbol B_n} - \boldsymbol \omega_{A_n} \end{bmatrix}$

此问题为经典 $\boldsymbol A \boldsymbol x = \boldsymbol b$ 问题，其解为
$\boldsymbol x = (\boldsymbol A^T \boldsymbol A)^{-1} \boldsymbol A^T \boldsymbol b = \boldsymbol V \Lambda^+ \boldsymbol U^T \boldsymbol b$

其中 $\boldsymbol A = \boldsymbol U \Lambda \boldsymbol V^T$ ， $\Lambda^+$ 为伪逆
$\Lambda^+ = \begin{bmatrix} \frac{1}{\lambda_1}\\ & \ddots\\ && \frac{1}{\lambda_m}\\ &&& 0\\ &&&& \ddots\\ &&&&& 0 \end{bmatrix}$

误差分析

对于优化方程 $f(x, z_1,\cdots,z_n)$ ，其中 $x$ 为需要求解的变量， $z_i$ 为观测得到的变量，如果通过求导来求得最优解 $x^*$ ，那么有 $\frac{\partial f(x^*, z_1,\cdots,z_n)}{\partial x} = F(x^*, z_1,\cdots,z_n) = 0$ ，该方程通过计算可以得到 $x^* = g(z_1,\cdots,z_n)$ ，假设 $z_i$ 均在真值 $z_i^0$ 附近，将函数 $g$ 在真值处进行泰勒展开可以得到
$x^*=g(z_1,\cdots,z_n) \approx g(z_1^0,\cdots,z_n^0)+\sum_{i=1}^n\frac{\partial g}{\partial z_i}(z_i - z_i^0)$

如果观测量 $z_i$ 相互之间独立，则有
$cov(x) \approx \sum_{i = 1}^n \frac{\partial g}{\partial z_i} cov(z_i) \frac{\partial g}{\partial z_i}^T$

根据隐函数定理
$\begin{aligned} \frac{\partial g}{\partial z_i} =& -\bigg( \frac{\partial F}{\partial x} \bigg)^{-1}\frac{\partial F}{\partial z_i} \\ =& -\bigg( \frac{\partial^2 f}{\partial x^2} \bigg)^{-1} \frac{\partial^2 f}{\partial z_i \partial x} \end{aligned}$

由此可以得到最优解 $x^*$ 的协方差矩阵为
$cov(x) \approx \sum_{i = 1}^n \bigg( \frac{\partial^2 f}{\partial x^2} \bigg)^{-1} \frac{\partial^2 f}{\partial z_i \partial x} cov(z_i) \frac{\partial^2 f}{\partial z_i \partial x}^T \bigg( \frac{\partial^2 f}{\partial x^2} \bigg)^{-1}$

当前
$\begin{aligned} f(\boldsymbol{\omega}_{\boldsymbol{X}}, \boldsymbol{\omega}_{\boldsymbol{B}_i}, \boldsymbol{\omega}_{\boldsymbol{A}_i}) =& \sum_{i = 1}^n w_i \| e^{\boldsymbol{\omega}_{\boldsymbol{X}}^\land} \boldsymbol{\omega}_{\boldsymbol{B}_i}- \boldsymbol{\omega}_{\boldsymbol{A}_i} \|^2\\ =& \sum_{i = 1}^n w_i (\boldsymbol{\omega}_{\boldsymbol{B}_i}^T {e^{\boldsymbol{\omega}_{\boldsymbol{X}}^\land}}^T e^{\boldsymbol{\omega}_{\boldsymbol{X}}^\land} \boldsymbol{\omega}_{\boldsymbol{B}_i} - 2\boldsymbol{\omega}_{\boldsymbol{A}_i}^T e^{\boldsymbol{\omega}_{\boldsymbol{X}}^\land} \boldsymbol{\omega}_{\boldsymbol{B}_i} + \boldsymbol{\omega}_{\boldsymbol{A}_i}^T \boldsymbol{\omega}_{\boldsymbol{A}_i} )\\ =& \sum_{i = 1}^n w_i (\boldsymbol{\omega}_{\boldsymbol{B}_i}^T\boldsymbol{\omega}_{\boldsymbol{B}_i} - 2\boldsymbol{\omega}_{\boldsymbol{A}_i}^T e^{\boldsymbol{\omega}_{\boldsymbol{X}}^\land} \boldsymbol{\omega}_{\boldsymbol{B}_i} + \boldsymbol{\omega}_{\boldsymbol{A}_i}^T \boldsymbol{\omega}_{\boldsymbol{A}_i} ) \end{aligned}$

采用右扰动，推导得到一阶导
$\frac{\partial f}{\partial \boldsymbol{\omega_X}} = 2\sum_{i=1}^n w_i (\boldsymbol{X}^T \boldsymbol{\omega_{A_i}})^\land \boldsymbol{\omega_{B_i}} = -2\sum_{i=1}^n w_i \boldsymbol{\omega_{B_i}}^\land \boldsymbol{X}^T \boldsymbol{\omega_{A_i}}$

继续推导得到二阶导
$\begin{aligned} \frac{\partial^2 f}{\partial \boldsymbol{\omega_X}^2} =& -2\sum_{i=1}^n w_i (\boldsymbol{X}^T \boldsymbol{\omega_{A_i}})^\land \boldsymbol{\omega_{B_i}}^\land = -2\sum_{i=1}^n w_i (\boldsymbol{\omega_{B_i}}^\land)^2 \\ \frac{\partial^2 f}{\partial \boldsymbol{\omega_{B_i}}\partial \boldsymbol{\omega_X}} =& -2w_i (\boldsymbol{X}^T \boldsymbol{\omega_{A_i}})^\land = -2w_i \boldsymbol{\omega_{B_i}}^\land \\ \frac{\partial^2 f}{\partial \boldsymbol{\omega_{A_i}}\partial \boldsymbol{\omega_X}} =& 2w_i \boldsymbol{X} \boldsymbol{\omega_{B_i}}^\land \end{aligned}$

采用左扰动，推导得到一阶导
$\frac{\partial f}{\partial \boldsymbol{\omega_X}} = 2\sum_{i=1}^n w_i \boldsymbol\omega_{A_i}^\land \boldsymbol{X} \boldsymbol{\omega_{B_i}} = -2\sum_{i=1}^n w_i (\boldsymbol{X} \boldsymbol{\omega_{B_i}})^\land \boldsymbol{\omega_{A_i}}$

继续推导得到二阶导
$\begin{aligned} \frac{\partial^2 f}{\partial \boldsymbol{\omega_X}^2} =& -2\sum_{i=1}^n w_i (\boldsymbol{X}\boldsymbol{\omega_{B_i}})^\land \boldsymbol{\omega_{A_i}}^\land = -2\sum_{i=1}^n w_i (\boldsymbol{\omega_{A_i}}^\land)^2 \\ \frac{\partial^2 f}{\partial \boldsymbol{\omega_{B_i}}\partial \boldsymbol{\omega_X}} =& -2w_i \boldsymbol{X}^T \boldsymbol{\omega_{A_i}}^\land\\ \frac{\partial^2 f}{\partial \boldsymbol{\omega_{A_i}}\partial \boldsymbol{\omega_X}} =& 2w_i (\boldsymbol{X} \boldsymbol{\omega_{B_i}})^\land = 2w_i \boldsymbol{\omega_{A_i}}^\land \end{aligned}$

因此有
$\begin{aligned} \Delta\boldsymbol{\omega_X} =& -\sum_{i=1}^n \bigg( \frac{\partial^2 f}{\partial \boldsymbol{\omega_X}^2} \bigg)^{-1} \frac{\partial^2 f}{\partial \boldsymbol{\omega_{B_i}}\partial \boldsymbol{\omega_X}} \Delta\boldsymbol{\omega_{B_i}} \\ &- \sum_{i=1}^n \bigg( \frac{\partial^2 f}{\partial \boldsymbol{\omega_X}^2} \bigg)^{-1} \frac{\partial^2 f}{\partial \boldsymbol{\omega_{A_i}}\partial \boldsymbol{\omega_X}} \Delta\boldsymbol{\omega_{A_i}} \\ =& -\bigg( \sum_{i=1}^n w_i(\boldsymbol{\omega_{B_i}}^\land)^2 \bigg)^{-1} \sum_{i=1}^n w_i \boldsymbol{\omega_{B_i}}^\land \Delta\boldsymbol{\omega_{B_i}}\\ &+\bigg( \sum_{i=1}^n w_i(\boldsymbol{\omega_{A_i}}^\land)^2 \bigg)^{-1} \sum_{i=1}^n w_i \boldsymbol{\omega_{A_i}}^\land \Delta\boldsymbol{\omega_{A_i}} \end{aligned}$

AX=XB 求解

AX=XB 求解

1、基本变换

hat运算符 $\land$

vee运算符 $\lor$

指数映射

对数映射

2、Navy手眼标定算法

带有初值的近似解

误差分析

相关阅读更多精彩内容

友情链接更多精彩内容