SLAM 几何基础

Reference:

1. 深蓝学院-多传感器融合

1. 向量运算及其几何意义

1.1 内积

1.1.1 内积定义

内积，又叫数量积，是向量的点乘。
$$\begin{gathered} \mathbf{a}=(x_1,y_1,z_1),\mathbf{b}=(x_2,y_2,z_2) \\ \mathbf{a}\cdot \mathbf{b}=x_1{x}_2+y_1{y}_2+z_1{z}_2 \end{gathered}$$

1.1.2 内积几何意义

当 $\mathbf{b}$ 为单位向量时，内积就是 $\mathbf{a}$ 在 $\mathbf{b}$ 上的投影分量。
$$\mathbf{a}\cdot \mathbf{b}=|\mathbf{a}||\mathbf{b}|cos\theta$$

1.1.3 内积微分性质

从内积的定义出发，有：
$$\frac{\partial \mathbf{a}\bullet \mathbf{b}}{\partial \mathbf{a}}=\mathbf{b}$$证明：
$$\begin{array}{rl}& \frac{\partial \mathbf{a}\bullet \mathbf{b}}{\partial x_1}=\frac{\partial \left(x_1{x}_2+y_1{y}_2+z_1{z}_2\right)}{\partial x_1}=x_2\\ & \frac{\partial \mathbf{a}\bullet \mathbf{b}}{\partial y_1}=\frac{\partial \left(x_1{x}_2+y_1{y}_2+z_1{z}_2\right)}{\partial y_1}=y_2\\ & \frac{\partial \mathbf{a}\bullet \mathbf{b}}{\partial z_1}=\frac{\partial \left(x_1{x}_2+y_1{y}_2+z_1{z}_2\right)}{\partial z_1}=z_2\end{array}$$

1.2 外积

1.2.1 外积定义

外积，又叫叉积、向量积，是向量的叉乘。
$$\begin{array}{rl}& \mathbf{a}=\left(x_1,y_1,z_1\right)\\ & \mathbf{b}=\left(x_2,y_2,z_2\right)\\ & \mathbf{a}\times \mathbf{b}=\left|\begin{array}{ccc}\mathrm{i}& \mathrm{j}& \mathrm{k}\\ x_1& y_1& z_1\\ x_2& y_2& z_2\end{array}\right|=\left(y_1{z}_2-y_2{z}_1\right)i-\left(x_1{z}_2-x_2{z}_1\right)j+\left(x_1{y}_2-x_2{y}_1\right)k\end{array}$$

1.2.2 外积几何意义

外积模长等于 $\mathbf{a}$ 和 $\mathbf{b}$ 组成的平行四边形的面积，外积的方向满足右手定则，$\mathbf{a}$ 和 $\mathbf{b}$ 张成平面的单位法向量为：$\mathbf{n}=\frac{\mathbf{a}\times \mathbf{b}}{|\mathbf{a}\times \mathbf{b}|}$。
$$|\mathbf{a}\times \mathbf{b}|=|\mathbf{a}||\mathbf{b}|sin\theta$$

1.2.3 外积微分性质

根据外积的定义，有：
$$\mathbf{a}\times \mathbf{b}={\mathbf{a}}^{\wedge }\mathbf{b}$$其中 $a^{\wedge }$ 为 $a$ 的反对称矩阵：
$${\mathbf{a}}^{\wedge }=\left[\begin{array}{ccc}0& -z_1& y_1\\ z_1& 0& -x_1\\ -y_1& x_1& 0\end{array}\right]$$则：
$$\begin{array}{rl}{\mathbf{a}}^{\wedge }\mathbf{b}& =-{\mathbf{b}}^{\wedge }\mathbf{a}\\ \frac{\partial {\mathbf{a}}^{\wedge }b}{\partial a}& =-\frac{b^{\wedge }\partial a}{\partial a}=-{\mathbf{b}}^{\wedge }\end{array}$$

2. 线面特征运算

2.1 点到直线距离

点 $A$ 到直线 $BC$ 的距离为：$|\overrightarrow{AD}|=\frac{|\overrightarrow{AB}\times \overrightarrow{AC}|}{|\overrightarrow{BC}|}$，即平行四边形面积除以对角线长度。

2.2 点到平面距离

平面 $BCD$ 的单位法向量为 $\mathbf{n}=\frac{\overrightarrow{BC}\times \overrightarrow{BD}}{|\overrightarrow{BC}\times \overrightarrow{BD}|}$ 点 $A$ 到平面 $BCD$ 的距离为：
$$|\overrightarrow{AE}|=|\overrightarrow{AB}|\cos \theta =\overrightarrow{AB}\bullet \mathbf{n}$$(即先计算单位向量给出方向，再使用点乘计算距离)

3. IMU相关

3.1 线速度与角速度

粒子在坐标系中 $z=h$ 的平面做圆周运动，坐标为：$\mathbf{r}=(a\cos \theta ,a\sin \theta ,h{)}^{\top }$ 对坐标求导得：
$$\begin{array}{rl}\dot{\mathbf{r}}& =(-a\dot{\theta }\sin \theta ,a\dot{\theta }\cos \theta ,0{)}^{\top }\\ & =\left[\begin{array}{ccc}0& -\dot{\theta }& 0\\ \dot{\theta }& 0& 0\\ 0& 0& 0\end{array}\right]\left[\begin{array}{c}a\cos \theta \\ a\sin \theta \\ h\end{array}\right]\\ & =\mathbf{\omega }\times \mathbf{r}=[\mathbf{\omega }{]}_{\mathbf{x}}\mathbf{r}\end{array}$$其中 $\omega =\dot{\theta }\mathbf{z}$，$|\dot{\theta }|$ 是角速度大小。对上面两边取模得：
$$|\dot{\mathbf{r}}|=|\mathbf{\omega }||\mathbf{r}|\sin \varphi =a|\dot{\theta }|$$