slam第二讲(三维空间刚体运动)

最新推荐文章于 2024-05-07 20:31:19 发布

bestrivern

最新推荐文章于 2024-05-07 20:31:19 发布

阅读量736

点赞数

分类专栏： slam 文章标签： slam

本文链接：https://blog.csdn.net/bestrivern/article/details/89455031

版权

slam 专栏收录该内容

4 篇文章 0 订阅

订阅专栏

一.旋转矩阵

1.点和向量，坐标系

外积：

$a\times b=\begin{Vmatrix} i &j &k \\ a1 & a2 &a3 \\ b1&b2 &b3 \end{Vmatrix}=\begin{bmatrix} a2b3-a3b2\\ a3b1-a1b3\\ a1b2-a2b1 \end{bmatrix}=\begin{bmatrix} 0 &-a3 &a2 \\ a3 &0 &-a1 \\ -a2 &a1 &0 \end{bmatrix}b =a^{\wedge} b$

$a^{\wedge}$ 表示矩阵a的反对称矩阵，以此来表示矩阵的外积运算。

2.坐标系间的欧式变换

坐标系1的单位正交基：[e1,e2,e3] 点在坐标系1中的坐标：[a1,a2,a3]

坐标系2的单位正交基：[e1',e2',e3'] 点在坐标系2中的坐标：[a1',a2',a3']

则转换关系如下：

$\begin{bmatrix} a1\\ a1\\ a3 \end{bmatrix}=\begin{bmatrix} e1^{T}e1' &e1^{T}e2' &e1^{T}e3' \\ e2^{T}e1'&e2^{T}e2' &e2^{T}e3' \\ e3^{T}e1'& e3^{T}e2' & e3^{T}e3' \end{bmatrix}\begin{bmatrix} a1'\\ a2'\\ a3' \end{bmatrix}=Ra'$

将中间的矩阵定义为矩阵R，R描述了旋转本身，又称旋转矩阵。

旋转矩阵是一个行列式为1的正交矩阵，反之，一个行列式为1的正交矩阵也是一个旋转矩阵。

旋转矩阵的集合定义如下：

$SO(n)=\{ R\in \mathbb{R}^{n \times n}}|RR^{T}=I,det(R)=1 \}$

SO(n)是特殊正交群，这个集合由n维空间的旋转矩阵组成。
旋转矩阵可以描述相机的旋转。
$R^{T}$ 刻画了一个相反的旋转

a'=Ra+t，其中t表示平移，这是一个完整的欧式变换。

3.变换矩阵与齐次坐标

问题：a'=Ra+t虽说是一个完整的欧式变换，但不是一个线性的表示方法，这会使得多次变换后的运算变得很复杂。

解决方法：引入齐次坐标和变换矩阵

齐次坐标：

$\widetilde{x}=[x,y,z,w]^{T}=[x/w,y/w,z/w,1]^{T}$

通过添加最后一维，用四个实数表示了一个三维向量，这显然多了一个自由度，但可以将变换写成线性形式。
在齐次坐标中，某个点x的分量同乘一个非0常数k后，仍然表示同一个点，故一个点的具体坐标值并不确定，但当最后一项不为0时，我们总可以把所有的坐标除以最后一项，强制最后一项为1，从而得到一个点的唯一的坐标表示(即转换为非齐次坐标)。

变换矩阵：

$\begin{bmatrix} a'\\ 1 \end{bmatrix}=\begin{bmatrix} R &t \\ 0^{T}& 1 \end{bmatrix}\begin{bmatrix} a\\ 1 \end{bmatrix}=T\begin{bmatrix} a\\ 1 \end{bmatrix}$

其中T为变换矩阵， $\widetilde{a}$ 表示a的齐次坐标。

特殊欧式群：

特点：左上角为旋转矩阵，右侧为平移向量，左下角为0向量，右下角为1。

$SE(3)=\{ T=\begin{bmatrix} R &t \\ 0^{T}&1 \end{bmatrix} \in \mathbb{R}^{4 \times 4}|R \in SO(3),t \in \mathbb{R}^3 \}$

与SO(3)一样，求解该矩阵的逆表示一个反向变换：

$T^{-1}=\begin{bmatrix} R &-R^{T}t \\ 0^{T}& 1 \end{bmatrix}$

二.旋转向量和欧拉角

存在问题：

SO(3)的旋转矩阵有九个量，但一次旋转只有3个自由度，因此这种表示方法是冗余的，同理变换矩阵用16个量表达了6个自由度的变换，也是可以有更为紧凑的表示。
旋转矩阵自身带有约束，它是一个行列式为1的正交矩阵，变换矩阵也是如此，当想要估计或者优化一个旋转矩阵和变换矩阵时，这些约束会使得求解变得很困难。

解决方法：旋转向量

1.旋转向量

任何旋转都可以用一个旋转轴和一个旋转角来表示。我们可以用一个向量，其方向与旋转轴一致，而长度等于旋转角，这种向量称为旋转向量(或轴角，Axis-Abgle)。

一个旋转轴为n,旋转角为 $\theta$ 的旋转，其对应的旋转向量为 $\theta$ n.

从旋转向量到旋转矩阵的变换过程由罗德里格斯公式表明。

旋转向量到旋转矩阵：

$R=\cos \theta I+(1-\cos \theta)nn^{T}+\sin \theta n^{\wedge}$

旋转矩阵到旋转向量：

$tr(R)=\cos \theta tr(I)+(1-\cos \theta)tr(nn^{T})+\sin \theta tr(n^{\wedge})=3\cos \theta +(1-\cos\theta)=1+2\cos \theta$

$\theta=\arccos (\frac{tr(R)-1}{2})$

Rn=n

对于转轴n,由于旋转轴上的向量在旋转后不发生改变，说明Rn=n,因此转轴n是矩阵R特征值1对应的特征向量，求解此方程，再归一化，就得到了旋转轴。

2.欧拉角

相对于以上所述的旋转矩阵和旋转向量，欧拉角便于人类的理解和想象。

绕物体的Z轴旋转，得到偏航角yaw
绕旋转之后的Y轴旋转，得到俯仰角pitch
绕旋转之后的X轴旋转，得到滚转角roll

此时可以用 $[r,p,y]^{T}$ 这样一个三维向量描述任何旋转。

重大缺陷：

会遇到万向锁问题(Gimbal Lock)，在俯仰角为+-90度时，第一次旋转和第三次旋转将使用同一个轴，使得系统丢失了一个自由度（由三次旋转变成了两次旋转）。

这被称为奇异性问题，切不可避免，故slam中很少使用欧拉角表示姿态，这种表示方法多用于人机相互。

三.四元数

四元数是紧凑的，也没有奇异性。

一个四元数q拥有一个实部和三个虚部，表示如下：

$q=q_{0}+q_{1}i+q_{2}j+q_{3}k$

其中i,j,k为四元数的三个虚部。

性质：

$i^{2}=j^{2}=k^{2}=-1$

四元数也可以用一个标量和一个向量来表示：

$q=[s,v],s=q_{0}\in \mathbb{R},v=[q_{1},q_{2},q_{3}]^{T}\in \mathbb{R}^{3}$

s称为四元数的实部，v称为它的虚部，若v=0，则为实四元数，若s=0，则为虚四元数

旋转向量与四元数相互转换：

旋转向量

旋转轴： $n=[n_{x},x_{y},n_{z}]^{T}$

旋转角： $\theta$

四元数： $q=[\cos \frac{\theta}{2},n_{x}\sin \frac{\theta}{2},n_{y}\sin \frac{\theta}{2},n_{z}\sin \frac{\theta}{2}]^{T}$

反之，单位四元数到旋转向量：

$\theta =2\arcos q_{0}$

$[n_{x},x_{y},n_{z}]^{T}=[q_{1},q_{2},q_{3}]^{T}/\sin \frac{\theta}{2}$

任意的旋转都可以由两个互为相反数的四元数表示，即q和-q这两个四元数表示同一个旋转。

四元数的运算：

1.加法和减法

$q_{a}+q_{b}=[s_{a}+s_{b},v_{a}+v_{b}]$

2.乘法

$q_{a}q_{b}=s_{a}s_{b}-x_{a}x_{b}-y_{a}y_{b}-z_{a}z_{b}+(s_{a}x_{b}+x_{a}s_{b}+y_{a}z_{b}-z_{a}y_{b})i+(s_{a}y_{b}-x_{a}z_{b}+y_{a}s_{b}+z_{a}x_{b})j+(s_{a}z_{b}+x_{a}y_{b}-y_{a}x_{b}+z_{a}s_{b})k$

向量形式表示：

$q_{a}q_{b}=[s_{a}s_{b}-v^{T}_{a}v_{b},s_{a}v_{b}+s_{b}v_{a}+v_{a}\times v_{b}]$

四元数乘法通常不可交换，除非va和vb在R3中共线，此时外积项为0

3.共轭

$q_{a}^{*}=s_{a}-x_{a}i-y_{a}j-z_{a}k=[s_{a},-v_{a}]$

$q^{*}q=qq^{*}=[s_{a}^{2}+v^{T}v,0]=\begin{Vmatrix}q\end{Vmatrix}^{2}$

4.模长

$\begin{Vmatrix} q_{a} \end{Vmatrix}=\sqrt{s_{a}^2+x_{a}^2+y_{a}^2+z_{a}^2}$

$\begin{Vmatrix}q_aq_b \end{Vmatrix}=\begin{Vmatrix}q_a\end{Vmatrix}\begin{Vmatrix}q_b \end{Vmatrix}$

5.逆

$q^{-1}=q^{*}/\begin{Vmatrix}q\end{Vmatrix}^{2}$

$qq^{-1}=q^{-1}q=1$

若q为单位四元数，其逆和共轭就是同一个量。

$(q_{a}q_{b})^{-1}=q_{b}^{-1}q_{a}^{-1}$

6.数乘与点乘

数乘：

点乘： $q_{a}q_{b}=s_{a}s_{b}+x_{a}x_{b}+y_{a}y_{b}+z_{a}z_{b}$

用四元数表示旋转：

三维空间中一点p=[0,x,y,z]=[0,v]

四元数表示旋转 $q=[\cos \frac{\theta}{2},n\sin \frac{\theta}{2}]$

则旋转之后的点p'可以表示为 $p'=qpq^{-1}$

四元数到旋转矩阵的转换：

四元数 $q=q_{0}+q_{1}i+q_{2}j+q_{3}k$

对应的旋转矩阵R：

$R=\begin{bmatrix} 1-2q_2^2-2q_3^2 &2q_1q_2-2q_0q_3 &2q_1q_3+2q_0q_2 \\ 2q_1q_2+2q_0q_3&1-2q_1^2-2q_3^2 &2q_2q_3-2q_0q_1 \\ 2q_1q_3-2q_0q_2 &2q_2q_3+2q_0q_1 &1-2q_1^2-2q_2^2 \end{bmatrix}$

反之，若旋转矩阵 $R=\{ m_{ij} \},i,j\in [1,2,3]$

则其对应的四元数q为：

$q_{0}=\frac{\sqrt{tr(R)+1}}{2},q_1=\frac{m_{23}-m_{32}}{4q_{0}},q_2=\frac{m_{31}-m_{13}}{4q_{0}},q_{3}=\frac{m_{12}-m_{21}}{4q_{0}}$

四.相似，仿射，射影变换

变换名称	矩阵形式	自由度	不变性质	示意图
欧式变换	$\begin{bmatrix} R &t \\ 0^{T}&1 \end{bmatrix}$ 1)旋转矩阵R是正交矩阵	6	长度，夹角，体积
相似变换	$\begin{bmatrix} sR &t \\ 0^{T}&1 \end{bmatrix}$ 1) 旋转部分多了一个缩放因子缩放因子 s，可以在x,y,z三个坐标上进行均匀的缩放；	7	体积比
仿射变换	$\begin{bmatrix} A &t \\ 0^{T}&1 \end{bmatrix}$ 1) 矩阵 A是一个可逆矩阵可逆矩阵，不必是正交矩阵； 2) 立方体经仿射变换之后不再是方的，但各个面仍是平行四边形；	12	平行比，体积比
射影变换	$\begin{bmatrix} A &t \\ a^{T}&v \end{bmatrix}$ 1) 左上角为可逆矩阵 A，右上为平移 t，左下为缩放 $a^{T}$ ； 2) 从真实世界到相机照片的变换可以看成一个射影变换； 3) 想象一个原来是方形的地板砖，在照片中不再是方形的，甚至不再是平行四边形，而是一个不规则的四边形	15	接触平面的相交与相切