SLAM中的基础数学1:坐标变换


SLAM这个坑中的一些知识和体会,以后方便回顾。主要还是从高翔那里获得很多理解,但也添加了不少我的理解。好了,话不多说,今天就先从三维空间开始。

坐标变换

首先,这里给出其内积和外积的计算公式

内积:\begin{equation} \mathbf{a}\cdot\mathbf{b}=\sum{a_ib_i} \end{equation}

外积:\begin{equation}\mathbf{a}\times\mathbf{b}=\begin{bmatrix}\mathbf{i}&\mathbf{j}&\mathbf{k} \\ a_1&a_2&a_3\\b_1&b_2&b_3 \end{bmatrix}=\begin{bmatrix}a_2b_3-a_3b_2\\a_3b_1-a_1b_3\\a_1b_2-a_2b_1\end{bmatrix}\\=\begin{bmatrix}0&-a_3&a_2\\a_3&0&-a_1\\-a_2&a_1&0\end{bmatrix}\cdot\mathbf{b}=\mathbf{a}^\hat{}\mathbf{b}\end{equation}

我们用\(a^\hat{}\)来表示向量a的一种展开方式,即上式所表示的——把a展开成反对称矩阵。以后我们就有了\begin{equation}\mathbf{a}\times \mathbf{b}=\mathbf{a}^\hat{}\mathbf{b}\end{equation} 这个式子很有用。相反地,我们把一个反对称矩阵可以表示成一个向量:$$\mathbf{a}=A^{\vee}$$

对于坐标变换,我们可以将其拆分为旋转和平移两部分。在欧式空间下,平移很简单,只要将对应的坐标相加就可以了。对于旋转,我们有

\begin{equation} \mathbf{x_1}=\mathbf{R_{12}x_2} \end{equation}.这里的的R是一个旋转矩阵,可以推导出旋转矩阵是一个行列式为1的正交矩阵,所有的R构成了一个特殊正交群SO(3)。然后,我们的坐标变换可以写成\begin{equation}x_1 = R_{12}x_2+t1\end{equation}。这很好,但是当我们对坐标变换进行嵌套的时候,事情就变得有麻烦了\(x’ = R_1(R_2(R_3x_3+t_3)+t_2)+t_1\),我们发现变换变得不再线性了。所以,为了保证变换的线性,我们引入了齐次坐标的概念。\begin{equation}x=[x_1, x_2, x_3]^T \rightsquigarrow x=[x_1, x_2, x_3, 1]^T \rightsquigarrow x=[ax_1, ax_2, ax_3, a]^T \end{equation}.这里三个坐标表示的是同一个向量,第一个是非其次坐标,第二个和第三个是其次坐标。齐次坐标乘以一个非零常数的时候不变。从第一个到第二个式子可以看出,非齐次坐标最后添1转化为齐次坐标,齐次坐标最后一项归一化后掉1变为非齐次坐标。

所以前文的坐标变换在齐次坐标下可以写成\begin{equation}x_1 = T_{12}x_2\end{equation}其中,\(x_1 和 x_2\)是齐次坐标形式,且

\begin{equation}T_{12}=\begin{bmatrix}  R & t \\ 0^T & 1 \end{bmatrix} \end{equation}。这里的所有的T构成了一个特殊欧氏群SE(3)。为了避免转化麻烦,以后的公式中的坐标默认为符合转化的那一种,我们并不再做现式的转换了。

欧拉角、旋转向量与四元数

三维空间中的旋转只有三个只有度,而旋转矩阵有九个带约束的参数,这对于我们来说是不方便的。我们需要找一种紧凑且无约束的方式来表示旋转。这里涉及到了我们的旋转限量、欧拉角与四元数。

欧拉角

先从简单的说起吧,欧拉角就是我们常用的RPY角度,这对于我们人来说是非常直观的,但是对于计算机来说,最大的困难在于万向锁问题(这里不展开,有兴趣的可以自行google),这使得在方程中很少使用欧拉角,只是在人机交互与控制系统的执行端的时候才大量使用RPY格式。这里多说一句,万向锁其实是一个奇点问题,个人觉得用”自由度“个参数来描述系统,都会不可避免的碰到这种问题,比如,用经纬线表示地球表面坐标,但是子阿南北极点,经度就无意义。在二位平面,在无穷远点,不能用数来表达坐标,坐标无意义,因为无穷远存在一条无穷远直线经过所有无穷远点。当然了,在下是一个烂学校的烂工科生,也就不懂这是里面所蕴涵的更深的原因了。

旋转向量

我们在空间中的所有旋转总是可以合成为,绕空间中的某一固定轴旋转了某一角度。我们可以用固定轴上的单位向量作为旋转的方向,旋转的角度作为旋转的大小,于是我们就得到一个向量–旋转向量来表示空间中的旋转,这也叫做角轴,即沿指定轴旋转指定角度的变换。其中,旋转向量与旋转矩阵之间的关系有罗德里格斯公式给出:

\begin{equation} \mathbf{R}=cos\theta \mathbf{I}+(1-cos\theta)\mathbf{n}\mathbf{n}^T + sin\theta\mathbf{n}^\hat{} \end{equation}

对上式两边求迹,\begin{equation}tr(\mathbf{R})=1+2cos\theta\end{equation}所以,\begin{equation}\theta=arccos((tr(\mathbf{R})-1)/2)\end{equation},另外,旋转作用于旋转轴,旋转轴不发生变化,所以,\begin{equation}\mathbf{Rn}=\mathbf{n} \end{equation},即n是R的特征值为1的特征向量。

四元数

最后我们要着重说明一下四元数。

四元数的定义

要说四元数还真是个神奇的东西。它对于空间旋转的表达非常优秀,维二的缺点就是不够直观和计算稍微有点复杂。一个四元数的表示为:

\begin{equation} \mathbf{q}=q_0+q_1i+q_2j+q_3k \end{equation}其中\( i、j、k\)是四元数的虚部,且满足下列关系式:\begin{equation} i^=j^2=k^2=-1 \\ ij=k,  ji=-k \\ jk=i,  kj=-i \\ ki=j,  ik=-j \end{equation}为了简便,我们有时候用一个标量和一个向量(划分为实部和虚部)来表达四元数

\begin{equation} \mathbf{q}=[s,\mathbf{v}] \end{equation}

有了四元数,我们可以用四元数来表示前文说的角轴,假设角轴是绕旋转向量\(\mathbf{n}=[n_x, n_y, n_z]^T\)进行了\(\theta\)的旋转,那么可以表示为:

\begin{equation}\mathbf{q}=[cos\frac{\theta}{2},n_xsin\frac{\theta}{2},n_ysin\frac{\theta}{2},n_zsin\frac{\theta}{2} ] \end{equation}反之,我们从一个四元数中计算角轴:

\begin{equation}\left\{ \begin{aligned} \theta &=2arccosq_0 \\ [n_x,n_y,n_z] &= [q_1,q_2,q_3]/sin\frac{\theta}{2} \end{aligned} \right. \end{equation}我们可以试着在上式的\(\theta\)上加一个\(2\pi\),在我们知道,此时的旋转效果是不变的,但我们得到了一个相反的四元数,因此,任意一个旋转都可以由两个互为相反数的四元数来表示,也就是说表示不唯一。

四元数的运算

我们有两个四元数\(\mathbf{q_a,q_b}\),记作:

$$\mathbf{q}_a= s_a + x_ai +y_aj +z_ak=[s_a,\mathbf{v}_a],\\ \mathbf{q}_b= s_b + x_bi +y_bj +z_bk=[s_b,\mathbf{v}_b]$$

  1. 加法。对于加减法来说,四元数的运算同向量的加减法类似,只需要对应的坐标相加减就可以了。
  2. 乘法。四元数的乘法类似与多项式的乘法,\(\mathbf{q_a}与\mathbf{q_a}\)中的每一项都要相乘,虚部要按照前文的关系式进行。
  3. 共轭。四元数的共轭就是虚部取相反数。即\(\mathbf{q}^*_a=[s_a,-\mathbf{v}_a]\)。两个共轭的四元数相乘得到一个实四元数,实部为其模长的平方。即$$\mathbf{q}^*\mathbf{q}=\mathbf{q}\mathbf{q}^*=[s^2_a+\mathbf{v}^T\mathbf{v},\mathbf{0}]$$
  4. 模长。模长定义为:$$\left\| \mathbf{q}_a\right\|=\sqrt{s^2_a+x^2_a+y^2_a+z^2_a} $$。容易验证,四元数模长的积等于积的模长。即$$\left\| \mathbf{q}_a\right\|\left\| \mathbf{q}_b\right\|=\left\| \mathbf{q}_a\mathbf{q}_b\right\|$$
  5. 逆。四元数的逆定义为$$\mathbf{q}^{-1}=\mathbf{q}^* {\left\|\mathbf{q}\right\|}^2$$。按照此定义,四元数与自身的逆相乘结果为实四元数\(\mathbf{1}\)
  6. 数乘。四元数的数乘与向量的数乘类似,四元数的每个分量分别与数值相乘。

用四元数表示的旋转

首先我们把三位空间中的向量\(\mathbf{p}=[x,y,z]^T\)用纯虚四元数来表示$$\mathbf{p}=[0,x,y,z]=[0,\mathbf{v}]$$。旋转用$$\mathbf{q}=[cos\frac{\theta}{2},\mathbf{n}sin\frac{\theta}{2}]$$。那么我们旋转之后的结果用四元数来表示就是

\begin{equation}\mathbf{p}’=\mathbf{qpq^{-1}}\end{equation}需要说明的是,得到的结果是纯虚四元数,我验证过。另外,旋转四元数的模长为1,这很容易验证。

这里的旋转四元数对应的旋转是由前面“角轴–四元数”关系得到的。我们知道,根据罗德里格斯公式,角轴和旋转矩阵之间又存在对应关系,我们就建立起了关系。但是我们有四元数\(\mathbf{q}=[q_0,q_1i,q_2j,q_3k]\)直接到旋转矩阵\(\\mathbf{R})之间的转化公式:

\begin{equation} \mathbf{R}= \begin{bmatrix}1-2q^2_2-2^2q_3 & 2q_1q_2+2q_0q_3 & 2q_1q_3-2q_0q_2\\ 2q_1q_2-2q_0q_3 & 1-2q^2_q-2q^2_3 & 2q_2q_3+2q_0q_1 \\ 2q_1q_3 + 2q_0q_2 &2q_2q_3-2q_0q_1 & 1-2q^2_1-2q^2_2 \end{bmatrix}\end{equation}相反,如果\(\mathbf{R}=\{m_{ij} \}\),那么对应的四元数为\begin{equation} q_0=\frac{\sqrt{tr(\mathbf{R})+1}}{2},q_1=\frac{m_23-m_32}{4q_0},q_2=\frac{m_31-m13}{4q_0},q_3=\frac{m_12-m_21}{4q_0}\end{equation}

 

到此,本节就结束了,因为写blog匆忙,纰漏之处在所难免,万望包涵,不吝指正。

“SLAM中的基础数学1:坐标变换”的2个回复

发表评论

电子邮件地址不会被公开。 必填项已用*标注