SLAM数学篇:位姿表述

  研究机器人的运动，离不开三维空间。三维空间由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$$
则点可由该向量在该基下的坐标来描述。

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

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

内积、外积、反对称矩阵

$$a\cdot b=a^{T}b=|a||b|cos<a,b>$$

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

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

欧式变换

• 旋转

  为了统一描述问题，通常我们需要选择一个共同的坐标系，将其它坐标系的数据变换到该坐标系。
  $$[e_1^{\prime },e_2^{\prime },e_3^{\prime }][a_1^{\prime },a_2^{\prime },a_3^{\prime }{]}^T=[e_1,e_2,e_3][a_1,a_2,a_3{]}^T$$
  左乘$[e_1^{\prime },e_2^{\prime },e_3^{\prime }{]}^T$，由内积性质得到单位矩阵，可得：
  $$[a_1^{\prime },a_2^{\prime },a_3^{\prime }{]}^T=R[a_1,a_2,a_3{]}^T$$

  $R$为旋转矩阵，行列式为1的正交矩阵（$R{R}^T=I,|R|=1$），其中$R^T=R^{-1}$,描述了相反的旋转。两个旋转矩阵相乘仍是旋转矩阵。
  R z , ψ = [ c o s ψ − s i n ψ 0 s i n ψ c o s ψ 0 0 0 1 ] , R y , θ = [ c o s θ 0 s i n θ 0 1 0