自动驾驶中的SLAM技术

第2章: 基础数学知识回顾

习题 1

分别使用左右扰动模型，计算 $\frac{\partial {\mathbf{R}}^{-1}\mathbf{p}}{\partial \mathbf{R}}$。

左扰动模型
$$\begin{array}{rl}\frac{\partial {\mathbf{R}}^{-1}\mathbf{p}}{\partial \mathbf{R}}& =\underset{\delta \mathbf{\varphi }\to 0}{\lim }\frac{{\left(\mathrm{Exp}\left(\delta \mathbf{\varphi }\right)\mathbf{R}\right)}^{-1}\mathbf{p}-{\mathbf{R}}^{-1}\mathbf{p}}{\delta \mathbf{\varphi }}\\ & =\underset{\delta \mathbf{\varphi }\to 0}{\lim }\frac{{\mathbf{R}}^{-1}\mathrm{Exp}\left(-\delta \mathbf{\varphi }\right)\mathbf{p}-{\mathbf{R}}^{-1}\mathbf{p}}{\delta \mathbf{\varphi }}\\ & =\underset{\delta \mathbf{\varphi }\to 0}{\lim }\frac{{\mathbf{R}}^{-1}\left(\mathbf{I}-\delta {\mathbf{\varphi }}^{\wedge }\right)\mathbf{p}-{\mathbf{R}}^{-1}\mathbf{p}}{\delta \mathbf{\varphi }}\\ & =\underset{\delta \mathbf{\varphi }\to 0}{\lim }\frac{-{\mathbf{R}}^{-1}\delta {\mathbf{\varphi }}^{\wedge }\mathbf{p}}{\delta \mathbf{\varphi }}\\ & =\underset{\delta \mathbf{\varphi }\to 0}{\lim }\frac{{\mathbf{R}}^{-1}{\mathbf{p}}^{\wedge }\delta \mathbf{\varphi }}{\delta \mathbf{\varphi }}\\ & ={\mathbf{R}}^{-1}{\mathbf{p}}^{\wedge }\end{array}$$

右扰动模型
$$\begin{array}{rl}\frac{\partial {\mathbf{R}}^{-1}\mathbf{p}}{\partial \mathbf{R}}& =\underset{\delta \mathbf{\varphi }\to 0}{\lim }\frac{{\left(\mathbf{R}\mathrm{Exp}\left(\delta \mathbf{\varphi }\right)\right)}^{-1}\mathbf{p}-{\mathbf{R}}^{-1}\mathbf{p}}{\delta \mathbf{\varphi }}\\ & =\underset{\delta \mathbf{\varphi }\to 0}{\lim }\frac{\mathrm{Exp}\left(-\delta \mathbf{\varphi }\right){\mathbf{R}}^{-1}\mathbf{p}-{\mathbf{R}}^{-1}\mathbf{p}}{\delta \mathbf{\varphi }}\\ & =\underset{\delta \mathbf{\varphi }\to 0}{\lim }\frac{\left(\mathbf{I}-\delta {\mathbf{\varphi }}^{\wedge }\right){\mathbf{R}}^{-1}\mathbf{p}-{\mathbf{R}}^{-1}\mathbf{p}}{\delta \mathbf{\varphi }}\\ & =\underset{\delta \mathbf{\varphi }\to 0}{\lim }\frac{-\delta {\mathbf{\varphi }}^{\wedge }{\mathbf{R}}^{-1}\mathbf{p}}{\delta \mathbf{\varphi }}\\ & =\underset{\delta \mathbf{\varphi }\to 0}{\lim }\frac{{\left({\mathbf{R}}^{-1}\mathbf{p}\right)}^{\wedge }\delta \mathbf{\varphi }}{\delta \mathbf{\varphi }}\\ & ={\left({\mathbf{R}}^{-1}\mathbf{p}\right)}^{\wedge }\end{array}$$

习题 2

分别使用左右扰动模型，计算 $\frac{\partial \mathbf{R}1\mathbf{R}{2}^{-1}}{\partial {\mathbf{R}}_2}$。

注: 不能直接说 ${\mathbf{R}}_1{\mathbf{R}}_2$ 对 ${\mathbf{R}}_1$ 或 ${\mathbf{R}}_2$ 的导数，那样就变成矩阵对向量求导，无法使用矩阵来描述。在不引入张量的前提下，我们最多只能求向量到向量的导数。因此分子部分必须加上 Log 符号，取为矢量。

左扰动模型
$$\begin{array}{rl}\frac{\partial \mathrm{Log}\left({\mathbf{R}}_1{\mathbf{R}}_2^{-1}\right)}{\partial {\mathbf{R}}_2}& =\underset{\mathbf{\varphi }\to 0}{\lim }\frac{\mathrm{Log}\left({\mathbf{R}}_1{\left(\mathrm{Exp}\left(\mathbf{\varphi }\right){\mathbf{R}}_2\right)}^{-1}\right)-\mathrm{Log}\left({\mathbf{R}}_1{\mathbf{R}}_2^{-1}\right)}{\mathbf{\varphi }}\\ & =\underset{\mathbf{\varphi }\to 0}{\lim }\frac{\mathrm{Log}\left({\mathbf{R}}_1{\mathbf{R}}_2^{-1}\mathrm{Exp}\left(-\mathbf{\varphi }\right)\right)-\mathrm{Log}\left({\mathbf{R}}_1{\mathbf{R}}_2^{-1}\right)}{\mathbf{\varphi }}\\ & =\underset{\mathbf{\varphi }\to 0}{\lim }\frac{\mathrm{Log}\left({\mathbf{R}}_1{\mathbf{R}}_2^{-1}\right)+{\mathbf{J}}_r^{-1}\left(\mathrm{Log}\left({\mathbf{R}}_1{\mathbf{R}}_2^{-1}\right)\right)\left(-\mathbf{\varphi }\right)-\mathrm{Log}\left({\mathbf{R}}_1{\mathbf{R}}_2^{{}^{-1}}\right)}{\mathbf{\varphi }}\\ & =\underset{\mathbf{\varphi }\to 0}{\lim }\frac{-{\mathbf{J}}_r^{-1}\left(\mathrm{Log}\left({\mathbf{R}}_1{\mathbf{R}}_2^{-1}\right)\right)\mathbf{\varphi }}{\mathbf{\varphi }}\\ & =-{\mathbf{J}}_r^{-1}\mathrm{Log}\left({\mathbf{R}}_1{\mathbf{R}}_2^{-1}\right)\end{array}$$

右扰动模型
$$\begin{array}{rl}\frac{\partial \mathrm{Log}\left({\mathbf{R}}_1{\mathbf{R}}_2^{-1}\right)}{\partial {\mathbf{R}}_2}& =\underset{\mathbf{\varphi }\to 0}{\lim }\frac{\mathrm{Log}\left({\mathbf{R}}_1{\left({\mathbf{R}}_2\mathrm{Exp}\left(\mathbf{\varphi }\right)\right)}^{-1}\right)-\mathrm{Log}\left({\mathbf{R}}_1{\mathbf{R}}_2^{-1}\right)}{\mathbf{\varphi }}\\ & =\underset{\mathbf{\varphi }\to 0}{\lim }\frac{\mathrm{Log}\left({\mathbf{R}}_1\mathrm{Exp}\left(-\mathbf{\varphi }\right){\mathbf{R}}_2^{-1}\right)-\mathrm{Log}\left({\mathbf{R}}_1{\mathbf{R}}_2^{-1}\right)}{\mathbf{\varphi }}\\ & =\underset{\mathbf{\varphi }\to 0}{\lim }\frac{\mathrm{Log}\left({\mathbf{R}}_1{\mathbf{R}}_2^{-1}\mathrm{Exp}\left(-{\mathbf{R}}_2\mathbf{\varphi }\right)\right)-\mathrm{Log}\left({\mathbf{R}}_1{\mathbf{R}}_2^{-1}\right)}{\mathbf{\varphi }}\\ & =\underset{\mathbf{\varphi }\to 0}{\lim }\frac{\mathrm{Log}\left({\mathbf{R}}_1{\mathbf{R}}_2^{-1}\right)+{\mathbf{J}}_r^{-1}\left(\mathrm{Log}\left({\mathbf{R}}_1{\mathbf{R}}_2^{-1}\right)\right)\left(-{\mathbf{R}}_2\mathbf{\varphi }\right)-\mathrm{Log}\left({\mathbf{R}}_1{\mathbf{R}}_2^{-1}\right)}{\mathbf{\varphi }}\\ & =\underset{\mathbf{\varphi }\to 0}{\lim }\frac{-{\mathbf{J}}_r^{-1}\left(\mathrm{Log}\left({\mathbf{R}}_1{\mathbf{R}}_2^{-1}\right)\right){\mathbf{R}}_2\mathbf{\varphi }}{\mathbf{\varphi }}\\ & =-{\mathbf{J}}_r^{-1}\left(\mathrm{Log}\left({\mathbf{R}}_1{\mathbf{R}}_2^{-1}\right)\right){\mathbf{R}}_2\end{array}$$

注:
$$\mathrm{Log}\left(\mathbf{AB}\right)\approx \{\begin{array}{l}\mathrm{Log}\left(\mathbf{B}\right)+{\mathbf{J}}_l^{-1}\left(\mathrm{Log}\left(\mathbf{B}\right)\right)\mathrm{Log}\left(\mathbf{A}\right),\text{当}\mathbf{A}\approx \mathbf{I}\\ \mathrm{Log}\left(\mathbf{A}\right)+{\mathbf{J}}_r^{-1}\left(\mathrm{Log}\left(\mathbf{A}\right)\right)\mathrm{Log}\left(\mathbf{B}\right),\text{当}\mathbf{B}\approx \mathbf{I}\end{array}$$
$$\mathrm{Exp}\left(\mathbf{\varphi }\right)\mathbf{R}=\mathbf{R}\mathrm{Exp}\left({\mathbf{R}}^{\top }\mathbf{\varphi }\right)$$