《智能驾驶之激光雷达算法详解》空间变换数学基础

lospring

《智能驾驶之激光雷达算法详解》空间变换数学基础 [复制链接]

本帖最后由 lospring 于 2024-9-8 23:50 编辑

下面介绍一些坐标系欧氏变换的基础数学。

在进行激光雷达的外参标定时，我们实际上是求解激光雷达坐标系相对车体坐标系的姿态和相对位置；在进行车辆定位时，我们通常是在求解车体坐标系的相对变化量 $T_{C_{ti}}^{C_{tj}}$ 及其相对世界参考坐标系的变化量 $T_{C_{w}}^{C_{tj}}$ 。这两个坐标系之间的位置和姿态变化构成了欧氏变换。其中，姿态描述的是一个坐标系的轴系相对另一个坐标系轴系的旋转或方向关系，相对位置描述的是一个坐标系的原点相对另一个坐标系原点的平移关系。

1.1、旋转和平移变换

首先考虑两坐标系间具有共同原点且仅有相对旋转的情况。则点P的坐标系1和坐标系2下的三维向量可分别表示为

$_{}^{1}\textrm{P}$ = $[p_{x}^{1}p_{y}^{1}p_{z}^{1}]_{}^{T}$

$_{}^{2}\textrm{P}$ = $[p_{x}^{2}p_{y}^{2}p_{z}^{2}]_{}^{T}$

结合假设两坐标系的单位正交基分别为 $x_{1},y_{1},z_{1}$ ； $x_{2},y_{2},z_{2}$ 。以及坐标系2在坐标系1下单位正交基的投影关系最终可得

$_{}^{1}\textrm{P}$ = $R_{2}^{1}$ $_{}^{2}\textrm{P}$

其中旋转矩阵 $R_{2}^{1}$ = $\begin{bmatrix} a_{x} & b_{x}&c_{x} \\ a_{y}& b_{y} &c_{y} \\ a_{z}& b_{z} & c_{z} \end{bmatrix}$

我们可以进一步分析得到旋转矩阵是行列式为1的正交阵，因此旋转矩阵R与其转置矩阵 $R_{}^{T}$ 的相乘结果为单位阵E，所以 $RR_{}^{T}=R_{}^{T}R=E$ ,所以 $R_{}^{T}=R_{}^{-1}$

再进一步可以看出物体相对于坐标轴的旋转和坐标轴相对于物体的等角度反向旋转在描述上是等效的。

齐次矩阵则是考虑了两坐标系间同时具有旋转和平移的情况。最终可得 $_{}^{1}\textrm{P}=R_{3}^{1}$ $_{}^{3}\textrm{P}+d$ 。

1.2、旋转的欧拉角表示

在使用旋转矩阵R表示三维空间中的旋转和姿态时，共需要9个变量来表示，而对应的旋转本身则通常只有3个自由度。这种表达明显带来参数冗余，使得求解复杂。所以采用欧拉角姿态表示。

根据旋转轴顺序的不同，欧拉角有多种形式。以RPY为例，绕X轴旋转角称为翻滚角R，绕Y轴旋转角称为俯仰角P，绕Z轴旋转角称为Y。欧拉角描述的空间旋转与其转动顺序强相关，且欧拉角的三个分量不具有互换性，在求解RPY的逆变换时，不能仅对角度取负实现，而应该按照相反的旋转顺序反转相应的角度。当俯仰角pitch= $\pm 90^{\circ}$ 时，欧拉角描述的旋转存在奇异性，此时滚动角和领航角无法区分，其描述的旋转会出现退化现象，称为欧拉角的万向锁现象。所以在优化和滤波等迭代算法中通常不使用欧拉角表示较大的旋转变换。

1.3、旋转的轴角表示/旋转向量表示

旋转向量到旋转矩阵的转换关系如下：

$R=cos\theta E+(1-cos\theta )nn^{T}+sin\theta n{}'$

1.4、旋转的单位四元数表示

四元数是将二维空间中的复数扩展至三维空间中得到的超复数： $Q=q_{0}+q_{1}i+q_{2}j+q_{3}k$

i,j,k为虚数单位，分别对应坐标系的三个轴，并满足 $i^{2}=j^{2}=k^{2}=1,ij=k,jk=i,ki=j,ji=-k,kj=-i,ik=-j$

采用矢量形式表示为 $Q=\begin{bmatrix} q_{0} &q_{1} &q_{2} & q_{3} \end{bmatrix}=\begin{bmatrix} s & v \end{bmatrix},s=q_{0} ,v=\begin{bmatrix} q_{1}&q_{2} & q_{3} \end{bmatrix}$

2.1 李群、李代数

为简化位姿估计相关的求解过程，引入李群和李代数。

群通常表示为由有限或无限个元素构成的集合加上一种运算的代数结构。具有群结构的光滑微分流形为李群，即若G为一个群，同时它又是D维空间的一个流形，并且其群乘积和取逆操作都是平滑函数，则G为一个李群。李代数是一个由集合V，数域F和一个李括号运算 $[\cdot ,\cdot ]$ 组成的代数结构，用于表示被赋予李括号运算的线性空间。在三维空间 $\mathbb{R}^{3}$ 中，向量的叉乘运算即为该空间的李括号运算，所以李代数实际上是李群在其幺元处的切空间，它能够完全捕获李群的局部结构，并且李群M，李代数m可表示为 $m=T_{1}M$ 。之后可通过李群和李代数的映射关系将流形空间中待求解的问题表示成对应的线性空间的李代数结构，从而使得利用线性空间中的模型和算法成为可能。