【原创】SLAM学习笔记（一）三维刚体运动旋转

三维空间的刚体运动

三维空间中，刚体的运动可以用两个概念来表示：旋转和平移。平移比较简单一些，一般用一个表示位移的向量来表示。而旋转则有多种表示方法，例如旋转矩阵、旋转向量等等，不同的表示方法各有优劣。

旋转矩阵

设原始向量$p$可以表示为$(e_1,e_2,e_3)(a_1,a_2,a_3{)}^T$, 旋转以后的向量$p^{\prime }$表示为$(e_1^{\prime },e_2^{\prime },e_3^{\prime })(a_1^{\prime },a_2^{\prime },a_3^{\prime }{)}^T$。
对于向量$p$,在两坐标系下应该相等，即
$(e_1,e_2,e_3)(a_1,a_2,a_3{)}^T=(e_1^{\prime },e_2^{\prime },e_3^{\prime })(a_1^{\prime },a_2^{\prime },a_3^{\prime }{)}^T$
化简得
$(a_1,a_2,a_3{)}^T=(\begin{array}{ccc}{e}_1^T{e}_1^{\prime }& e_1^T{e}_2^{\prime }& e_1^T{e}_3^{\prime }\\ e_2^T{e}_1^{\prime }& e_2^T{e}_2^{\prime }& e_2^T{e}_3^{\prime }\\ e_3^T{e}_1^{\prime }& e_3^T{e}_2^{\prime }& e_3^T{e}_3^{\prime }\end{array})(a_1^{\prime },a_2^{\prime },a_3^{\prime }{)}^T$

我们把中间的矩阵拿出来，定义成一个矩阵R ，它描述了旋转本身，又称为旋转矩阵，是由两组基之间的内积组成的。而且它是一个行列式为1的正交矩阵。它的逆（亦即转置）描述了一个相反的的旋转。

代码

旋转矩阵的表示

class MotionMatrix(Motion):
    """
    旋转矩阵
    """
    def __init__(self, matrix: np.ndarray):
        self.matrix = npa(matrix)
        assert self.matrix.shape == (3, 3)
        assert np.isclose(np.linalg.det(self.matrix), 1)

旋转向量

旋转矩阵可以很好的描述相机的旋转了，但也有两个问题：

• 一次旋转只有3个自由度，但旋转矩阵却有9个量。显然这冗余了；
• 旋转矩阵自身带有两个约束。在进行估计和优化时，我们显然不想要这些附加的约束。
  所以我们想要一个更紧凑的表达，最好就是3个量。而旋转向量就可以。

由于任意旋转都可以用一个旋转轴和一个旋转角来描述。那么，我们可以用一个向量，其方向为旋转轴 $n$ 的方向，大小则为旋转角 $\theta$，这种向量就是旋转向量，表示为$\theta n$ . 前面说到，外积可以用来表示旋转就是因为外积可以用来表示旋转向量：考虑两个不平行的向量 $a$ 和 $b$，在右手法则下，用右手的4个指头从 $a$转向$b$ ，大拇指的朝向就是旋转向量的方向，即 $a\times b$的方向，而它的大小则由 $a$和 $b$ 的夹角决定。

旋转向量和旋转矩阵的转换可以用罗德里格斯公式表示：
$$R=\cos \theta I+(1-\cos \theta )n{n}^T+\sin \theta n\hat{}$$
两边同时取迹
$$tr(R)=\cos \theta tr(I)+(1-\cos \theta )tr(n{n}^T)+\sin \theta tr(n\hat{})=1+2\cos \theta$$
得到
$$\theta =\arccos (\frac{tr(R)-1}{2})$$

而旋转轴上的向量在旋转后不发生改变，所以
$$Rn=n$$

说明 $n$是旋转矩阵 R的特征值1所对应的特征向量。求解后并归一化则可得到$n$ .

代码

旋转角的表示

class MotionAxisAngle(Motion):
    """
    旋转角
    """
    def __init__(self, theta, n):
        self.theta, self.n = theta, npa(n)

旋转矩阵与旋转角的相互转化

class MotionMatrix(Motion):
    def to_axis_angle(self):
        _, n, _ = solve_mat(self.matrix - 1, np.zeros(3))  # 需要导入 astar-math包，求解齐次线性方程组
        return MotionAxisAngle(np.trace(np.arccos(self.matrix) / 2 - 0.5), n)

class MotionAxisAngle(Motion):
    def to_matrix(self):
        cos_theta = np.cos(self.theta)
        sin_theta = np.sin(self.theta)
        r = np.eye(3) * cos_theta + (1 - cos_theta) * self.n @ self.n.T + sin_theta * skew_symmetric(self.n)
        return MotionMatrix(r)

欧拉角

除了旋转向量，我们也可以用欧拉角来紧凑地描述旋转。一个旋转可以分解成3次分别绕X、Y、Z 轴的旋转来表示，在航空摄影测量中，一般用“翻转 - 航偏 - 俯仰”（roll - yaw - pitch)，也即XZY来表示。先绕X 轴旋转roll 角度，再绕Y 轴旋转yaw 角度，最后按照Z 轴旋转pitch 角度。这三个旋转矩阵相乘就得到了总的旋转矩阵。

此时，就可以用 $(r,p,y{)}^T$这三个量来描述任意旋转，这种表示方式会比其他方式更直观、更易理解。但是出现异常的"万向锁"–一旦选择±90°作为pitch角，就会导致第一次旋转和第三次旋转等价，整个旋转表示系统被限制在只能绕竖直轴旋转，丢失了一个表示维度。

代码

欧拉角表示

class MotionRPY(Motion):
    def __init__(self, r, p, y):
        self.r, self.p, self.y = r, p, y

旋转矩阵和欧拉角的转化

class MotionMatrix(Motion):
    def to_rpy(self):
        alpha = np.arctan(self.matrix[2, 1] / self.matrix[2, 2])
        beta = np.arctan(-self.matrix[2, 0] / npl.norm(self.matrix[2, 1:]))
        gamma = np.arctan(self.matrix[1, 0] / self.matrix[0, 0])
        return MotionRPY(alpha, beta, gamma)

四元数

因为欧拉角和旋转向量具有奇异性, 因此我们需要用到四元数，它既是紧凑的，也没有奇异性。

定义：一个四元数包含一个实部和三个虚部。
$$q=q_0+q_{1}i+q_{2}j+q_{3}k=[s,v]$$
其中，后面的等式将四元数表达成一个标量和一个向量， $i,j,k$ 表示四元数的三个虚部，满足：
$$\{\begin{array}{c}{i}^2=j^2+k^2=-1\\ ik=k,ji=-k\\ jk=i,kj=-i\\ ki=j,ik=-j\end{array}$$

若一个四元数虚部全为0，则它是一个实四元数；如其实部为零，则称它为虚四元素。而且，一个虚四元数对应一个空间点。

我们能用单位四元数来表示三维空间中的任意一个旋转。我们先考虑下复数。在复数中，乘以$i$ 表示在复平面内旋转90度。但在四元数中，情形却有微妙的不同：乘以i 表示旋转180度，这样才能保证 $ij=-k$的性质。而 $i^2=-1$ ，说明绕i 轴旋转360度后得到一个相反的东西，而要旋转720度（两周）才能得到它原先的样子。

假设某个旋转的旋转向量为 $\theta n$ , 则

$$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\arccos q_0$$$$(n_x,n_y,n_z{)}^T=(q_1,q_2,q_3{)}^T/\sin \frac{\theta }{2}$$

上式给人一种“转了一半”的感觉。将上式中的 加上 [公式] 后得到一个相同的旋转，但是对应的四元数却变成了 $-q$. 所以，在四元数中，任意的旋转都可以由两个互为相反数的四元数表示。

而四元数和旋转矩阵的关系为：

$$R=\left(\begin{array}{ccc}1-2q_2^2-2q_3^2& 2q_1{q}_2-2q_0{q}_3& 2q_1{q}_3+2q_0{q}_2\\ 2q_1{q}_2+2q_0{q}_3& 1-2q_1^2-2q_3^2& 2q_2{q}_3-2q_0{q}_1\\ w{q}_1{q}_3-2q_0{q}_2& 2q_2{q}_3+2q_0{q}_1& 1-2q_1^2-2q_2^2\end{array}\right)$$

设矩阵 $R=\{m_{ij}\},i,j\in \{1,2,3\}$ , 则由上式可以推得：

$$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}$$

注意: 由于$q$和 $-q$ 表示同一个旋转，所以一个旋转矩阵对应的四元数表示并不惟一且存在其他转换公式。在实际中，如果 $q_0$ 接近于0，会造成其他3个数的解不稳定，应采用其他公式。

四元数的运算

加减法、乘法

现有两个四元数 $q_a,q_b$, 向量表示分别为 $[s_a,v_a],[s_b,v_b]$ 。

• 加减法
  $q_a\pm q_b=[s_a\pm s_b,v_a\pm v_b]$

• 乘法
  乘法将两个四元数的每一项两两相乘然后相加。注意其满足之前所述的四元数的性质。
  $q_a\times q_b=[s_a\times s_b,v_a\cdot v_b]$

该乘法定义下，两个实四元数的乘积仍然是实四元数。由于最后一项外积的存在，四元数的乘法不满足交换律，除非 $v_a$ 和 $v_b$ 在 $R^3$ 中共线（此时外积为零）。

共轭

共轭即将虚部取为相反数：

$$q{\ast }_a=[s_a,-v_a]$$

四元数共轭与其自身相乘可得一个实四元数，实部为模长的平方。

$$q\ast q=qq\ast =[s_a^2+v^{T}v,0]$$

模长

$$||q_a||=\sqrt{s_a^2+x_a^2+y_a^2+z_a^2}$$
可知，两个四元数乘积的模即为二者模的乘积。所以，单位四元数相乘后仍然得到单位四元数。
$$||q_a{q}_b||=||q_a||||q_b||$$

逆

$$q^{-1}=\frac{q\ast }{||q||}$$
可见，对于单位四元数，其逆和共轭相同。按照定义，四元数和自己逆的乘积为实四元数1 . 最后，四元数乘积的逆有和矩阵相乘相似的性质：

$$(q_a{q}_b{)}^{-1}=q_b^{-1}{q}_a^{-1}$$

数乘和点乘

四元数的数乘与点乘与向量相同：
$$kq=[ks,kv]$$$$q_a{q}_b=s_a{s}_b+x_a{x}_{b}i+y_a{y}_{b}j+z_a{z}_{b}k$$

用四元数表示旋转

要用四元数来计算旋转，首先，用一个虚四元数来表示三维空间点：
$$p=[0,x,y,z{]}^T=[0,v]$$
则旋转后的点$p^{\prime }$则为
$p^{\prime }=qp{q}^{-1}$

变换矩阵

解决了旋转问题，就可以考虑平移了。平移可以简单地用一个平移向量 $t$来表示。则变换后的坐标为
$$a^{\prime }=Ra+t$$
但是，上面这个式子并不是线性的。所以我们引入了齐次坐标。在一个三维向量末尾加上一个1，就得到一个四维向量，成为齐次坐标。上式就可以写成
$$\left[\begin{array}{c}{a}^{\prime }\\ 1\end{array}\right]=\left[\begin{array}{cc}R& t\\ 0^T& 1\end{array}\right]\left[\begin{array}{c}a\\ 1\end{array}\right]=T\left[\begin{array}{c}a\\ 1\end{array}\right]$$$$S{E}_3=\left[\begin{array}{cc}R& t\\ 0^T& 1\end{array}\right]\in R^{4\times 4}|R\in S{O}_3,t\in R^3$$

代码

四元数的表示

class MotionQuaternion(Motion):
    def __init__(self, quaternion=None, s=None, v=None):
        if quaternion is not None:
            assert len(quaternion) == 4
            self.quaternion = npa(quaternion)
        elif s is not None and v is not None:
            self.quaternion = npa([s, *v])
        else:
            raise ParameterValueError("error init params")

    @property
    def s(self):
        return self.quaternion[0]

    @property
    def x(self):
        return self.quaternion[1]

    @property
    def y(self):
        return self.quaternion[2]

    @property
    def z(self):
        return self.quaternion[3]

    @property
    def v(self):
        return self.quaternion[1:]

四元数和旋转矩阵的转化

from liepack.domain.liealgebras import so
npa = np.array


def skew_symmetric(a: (np.ndarray, list)):
    """
    获取反对称矩阵
    :param a:
    :return: 反对称矩阵
    """
    mat = so(len(a))
    mat.set_vector(a)
    return mat


class MotionMatrix(Motion):
    def to_quaternion(self):
        s_4 = np.sqrt(np.trace(self.matrix) + 1) * 2
        x = (self.matrix[1, 2] - self.matrix[2, 1]) / s_4
        y = (self.matrix[2, 0] - self.matrix[0, 2]) / s_4
        z = (self.matrix[0, 1] - self.matrix[1, 0]) / s_4
        s = s_4 / 4
        return MotionQuaternion([s, x, y, z])
        
class MotionQuaternion(Motion):
    def to_matrix(self):
        v_up = skew_symmetric(self.v)  # 安装liepack
        return MotionMatrix(self.v @ self.v.T + self.s * self.s * np.eye(3) + 2 * self.s * v_up + v_up @ v_up)

完整代码

代码完整版在这里