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

A.Star

已于 2022-05-30 17:26:01 修改

阅读量370

点赞数

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

于 2022-05-30 03:06:26 首次发布

本文链接：https://blog.csdn.net/ye_xiao_yu/article/details/125035646

版权

SLAM 专栏收录该内容

4 篇文章 1 订阅

订阅专栏

三维空间的刚体运动

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

旋转矩阵

设原始向量 $p$ 可以表示为 $e_1,e_2,e_3)(a_1,a_2,a_3)^T$ , 旋转以后的向量 $p^{'}$ 表示为 $e'_1,e'_2,e'_3)(a'_1,a'_2,a'_3)^T$ 。
对于向量 $p$ ,在两坐标系下应该相等，即
$e_1,e_2,e_3)(a_1,a_2,a_3)^T=(e'_1,e'_2,e'_3)(a'_1,a'_2,a'_3)^T$
化简得
$(a_1,a_2,a_3)^T=\Bigg(\begin{matrix} e_1^Te'_1 & e_1^Te'_2 & e_1^Te'_3\\ e_2^Te'_1 & e_2^Te'_2 & e_2^Te'_3\\ e_3^Te'_1 & e_3^Te'_2 & e_3^Te'_3\end{matrix}\Bigg) (a'_1,a'_2,a'_3)^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)nn^T+\sin\theta n\hat{}$
两边同时取迹
$tr(R)=\cos\theta tr(I)+(1-\cos\theta)tr(nn^T)+\sin\theta tr(n\hat{})=1+2\cos\theta$
得到
$\theta=\arccos(\frac{tr(R)-1}2)$

而旋转轴上的向量在旋转后不发生改变，所以
$R n = 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)

四元数

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

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

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

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

假设某个旋转的旋转向量为 $\theta \vec{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=\Bigg(\begin{matrix}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\\wq_1q_3-2q_0q_2&2q_2q_3+2q_0q_1&1-2q_1^2-2q_2^2\end{matrix}\Bigg)$

设矩阵 $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,\vec v_a], [s_b,\vec v_b]$ 。

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

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

共轭

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

$q*_a=[s_a,-\vec v_a]$

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

$[s^2_a+ v^Tv, 0]$

模长

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

逆

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

$q_aq_b)^{-1}=q_b^{-1}q_a^{-1}$

数乘和点乘

四元数的数乘与点乘与向量相同：
$k q = [k s, k v]$ $q_aq_b=s_as_b+x_ax_bi+y_ay_bj+z_az_bk$

用四元数表示旋转

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

变换矩阵

解决了旋转问题，就可以考虑平移了。平移可以简单地用一个平移向量 $t$ 来表示。则变换后的坐标为
$a^{'} = R a + t$
但是，上面这个式子并不是线性的。所以我们引入了齐次坐标。在一个三维向量末尾加上一个1，就得到一个四维向量，成为齐次坐标。上式就可以写成
$\left[\begin{matrix}a'\\1\end{matrix}\right]=\left[\begin{matrix}R&t\\0^T&1\end{matrix}\right]\left[\begin{matrix}a\\1\end{matrix}\right]=T\left[\begin{matrix}a\\1\end{matrix}\right]$ $SE_3=\left[\begin{matrix}R&t\\0^T&1\end{matrix}\right] \in R^{4\times4}|R\in SO_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)

完整代码

代码完整版在这里

A.Star

关注

0
点赞
踩
0

收藏

觉得还不错? 一键收藏
打赏
0
评论
【原创】SLAM学习笔记（一）三维刚体运动旋转

文章目录三维空间的刚体运动旋转矩阵旋转向量欧拉角四元数三维空间的刚体运动三维空间中，刚体的运动可以用两个概念来表示：旋转和平移。平移比较简单一些，一般用一个表示位移的向量来表示。而旋转则有多种表示方法，例如旋转矩阵、旋转向量等等，不同的表示方法各有优劣。旋转矩阵设原始向量ppp可以表示为(e1,e2,e3)(a1,a2,a3)T(e_1,e_2,e_3)(a_1,a_2,a_3)^T(e1,e2,e3)(a1,a2,a3)T, 旋转以后的向量p′p'p′表示为(e1′,e2′,e3′)(
复制链接

扫一扫