SLAM数学篇:位姿表述

原创

已于 2022-09-24 15:58:32 修改 · 1.3k 阅读

5 ·

CC 4.0 BY-SA版权

于 2020-08-14 17:00:47 首次发布

本文深入探讨了机器人在三维空间中的位姿表示，包括旋转矩阵、旋转向量、欧拉角和四元数等方法。通过分析各种表示的优缺点，强调了四元数在SLAM中的应用。此外，还讨论了坐标变换、奇异性问题和群的概念，为理解机器人运动学提供了基础。

摘要生成于 C知道，由 DeepSeek-R1 满血版支持，前往体验 >

研究机器人的运动，离不开三维空间。三维空间由3个轴组成，一个空间点位置便可以由三个坐标确定，但机器人作为刚体不光有位置，还有自身的姿态。

机器人位姿示意图

基本概念

点、向量、坐标系

为了描述三维空间中的点 $a_1,a_2,a_3)^T$ ，我们在该空间中选定一组基 $e_1,e_2,e_3)$ ，然后可以找一向量 $a$ 满足：
$a=[e_1,e_2,e_3][a_1,a_2,a_3]^T$
则点可由该向量在该基下的坐标来描述。

基又被称为坐标系，作为描述的载体存在，三维空间可以有很多坐标系，拿机器人问题来说，不同的传感器都有自身的坐标系，则同一点在不同坐标系下将有不同的描述。坐标系又可分为左手系、右手系，第三个轴方向相反，使用的时候要特别注意。

注：此处基使用行向量，坐标使用列向量。

内积、外积、反对称矩阵

$\cdot b=a^Tb=|a||b|cos<a,b>$

$\times b=\begin{bmatrix} 0 & -a_3 & a_2 \\\\ a_3 & 0 & -a_1 \\\\ -a_2 & a_1 & 0 \end{bmatrix}b=a^ \wedge b,a=[a_1,a_2,a_3]^T,|a \times b|=|a||b|sin<a,b>$

若 $\times n$ 阶矩阵 $A$ 满足 $A^T=-A$ ，则 $A$ 为反对称矩阵。考虑矩阵的重复元素，可将反对称矩阵与向量对应起来，可表示为 $a^\wedge=A$ ， $A^\vee=a$ 。

欧式变换

旋转

为了统一描述问题，通常我们需要选择一个共同的坐标系，将其它坐标系的数据变换到该坐标系。
$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$
左乘 $e'_1,e'_2,e'_3]^T$ ，由内积性质得到单位矩阵，可得：
$a'_1,a'_2,a'_3]^T=R[a_1,a_2,a_3]^T$

$R$ 为旋转矩阵，行列式为1的正交矩阵（ $RR^T=I,|R|=1$ ），其中 $R^T=R^{-1}$ ,描述了相反的旋转。两个旋转矩阵相乘仍是旋转矩阵。
$R_{z,\psi}=\begin{bmatrix} cos\psi & -sin\psi & 0 \\\\ sin\psi & cos\psi & 0 \\\\ 0 & 0 & 1 \end{bmatrix},R_{y,\theta}=\begin{bmatrix} cos\theta & 0 & sin\theta \\\\ 0 & 1 & 0 \\\\ -sin\theta & 0 & cos\theta \end{bmatrix},R_{x,\varphi}=\begin{bmatrix} 1 & 0 & 0 \\\\ 0 & cos\varphi & -sin\varphi \\\\ 0 & sin\varphi & cos\varphi \end{bmatrix}$