【摘抄】SLAM中的位姿

咚咚咔的粉思

已于 2022-07-20 22:03:29 修改

阅读量1.9k

点赞数 1

分类专栏：其他文章标签：矩阵

于 2022-05-04 20:01:57 首次发布

本文链接：https://blog.csdn.net/weixin_43118084/article/details/124479486

版权

其他专栏收录该内容

2 篇文章 0 订阅

订阅专栏

http://zhaoxuhui.top/blog/2021/05/14/APE-RPE-ATE-RTE-Mmetric-in-SLAM.html#2%E4%BD%8D%E5%A7%BF%E8%AF%84%E4%BB%B7%E6%8C%87%E6%A0%87
方便自己做笔记用，侵删

在空间里的点的坐标是三维的，那为什么要用4维矩阵才能进行旋转平移和缩放呢？
解答：如刚性变换（只允许平移和旋转两种，变换后任意两点的距离不变）
公式：P( x)= Ax+ B （B是平移量），则无法用三维矩阵唯一表示

1.位姿变换的表示

“位姿”包含位置和姿态两部分内容，是位置(位移)+姿态(旋转)的简称，不是打错的“位置”。一般情况下，旋转可以用旋转矩阵R表示，平移可以用平移向量t表示。当然我们也可以把旋转和位移统一在一个变换矩阵T中，以此来表示位姿。需要注意的一点是，位姿和位姿的变化都是用旋转平移或者变换来表示（？）。这类似于在坐标系中的坐标点与向量，如果我们从坐标角度看，它就是一个状态、一个点，而如果我们从向量的角度来看，它就是一个矢量。前面说了，位姿一定是相对于某个参考系的。而位姿的变化(位移+旋转)除了相对于参考系，还有一个重要的属性就是方向。任何一个位姿的变化必然有方向，不然就没有意义了。例如，我们说旋转30°，这个旋转可以是顺时针，也可以是逆时针，缺少了方向就会产生歧义。因此我们可以说位姿的变换包含两个要素：参考系和方向。当我们判断两个变换是不是相同时就可以看它们的这两个要素是不是相同。这个概念需要在后面介绍内容的时候时刻记住，不然就容易把自己绕晕了。

上面，我们说了，位姿的变换包含参考系和方向。如下图所示，对于两个状态A和B，从A到B的变换和从B到A的变换就是两个相反的变换。因为从A到B的变换参考系是A，而从B到A的变换参考系是B。
1

在这里插入图片描述
下面再从数学的角度介绍一下旋转矩阵、平移向量和变换矩阵的一些性质。对于旋转矩阵，三维旋转就是一个3×3的正交矩阵，也就是说矩阵的逆等于矩阵的转置。而且矩阵的逆等于一个逆变换。而对于变换矩阵T，按下式构成。
在这里插入图片描述
与旋转矩阵类似的，变换矩阵的逆表示一个反变换，所以TAB=T−1BA。但这里需要说明的一点是，我们并不能想当然地以为是对各部分求逆或取负,尤其是对于平移部分，当有旋转的时候，不能直接添加一个负号作为其逆变换。下面解释为什么。对于一个由R和t构成的变换矩阵T，其逆变换矩阵T−1表示为：

不能直接将T的各部分取反，原因如下：

举例，从A变换为B，没有旋转。
仅有尺度是不够的，还得有方向，更进一步说就是运算的顺序：是B状态减去A状态还是A状态减去B状态。一般而言，对于一个从A到B的变换，我们习惯用B状态(终止状态)减去A状态(起始状态)来作为运算顺序。这样我们可以得到平移向量为t=(4,2,−2)。因此，根据上面的介绍，我们可以写出：在这里插入图片描述
而对于逆变换，我们有两种求法。一种就是简单粗暴地直接对TAB求逆，另一种是根据上面提到的T−1和R、t的关系来构造。从A到B的过程，由于我们认为只有平移，所以旋转矩阵R=I，为一个3×3的单位阵。平移向量t=(4,2,−2)，所以，自然地，我们可以构造出如下矩阵：在这里插入图片描述
对于−RTt这部分可能不是很好理解。这里因为R为单位阵，所以该部分就是−t。为什么会有负号呢？还是按照刚刚我们说的变换两要素就可以明白了。我们要进行的是一个由B到A的变换，参考系是B状态。因此，按照终止状态减去起始状态的原则，我们可以得到t=(10−14,8−10,6−4)=(−4,−2,2)，而这恰恰就是从A到B变换的平移向量取负号。这也就是T−1公式中负号的来历。可以发现，两者殊途同归，得到了同样的结果。

上面的例子比较简单，因为我们没有考虑旋转的情况。接下来我们就单独考虑一下纯旋转的情况，看看会发生什么，如下图所示
（红色为X轴，绿色为Y轴，蓝色为Z轴，顺时针旋转为正）
在这里插入图片描述

我们从A状态开始，绕着x轴顺时针旋转45°，求A到B的变换矩阵TAB。对于旋转，我们可以使用==【https://blog.csdn.net/yhx956058885/article/details/108579270】==中关于【旋转矩阵推导】的知识，就可以很容易得到旋转矩阵R如下。
在这里插入图片描述
推导T−1公式：
从A状态变换到B状态，旋转为RAB，平移为tAB，它们共同构成了变换TAB。当我们求反变换TBA的时候，除了直接对TAB求逆，还可以根据物理意义求解。变换由旋转和平移构成，所以只要得到对应部分，填到变换矩阵T的对应位置就可以了。对于旋转部分，根据上面说的，逆旋转等于对旋转矩阵求逆，而旋转矩阵的逆又等于转置。所以旋转部分，就是RT。比较麻烦的是平移部分，根据上面的介绍，只有当两个坐标或平移的参考系相同时运算才有意义。对于现在我们已知的平移tAB，其参考系是旋转了RAB以后的A坐标系，方向是从A到B。B是在这个坐标系下按照平移向量移动得到的，所以B坐标系在旋转RAB后的A坐标系下的坐标就是tAB。我们对其添加一个负号，得到−tAB，即表示从B到A的平移。所以，对其乘上一个旋转矩阵的逆，就可以得到其在原始A坐标系下的坐标。最终也就写成了−RTt，这也就是T−1公式里的平移部分了。至此，我们也就推导得到T−1公式。

另外，对于旋转和平移的顺序问题，也需要明确一下。一般而言，在SLAM中，都是先旋转再平移的顺序。
对于某个从A到B的变换T，由旋转R和平移t组成。对于A中的某个坐标点xA，按照先旋转再平移的顺序，有如下公式。
xB=RxA+t
而按照先平移再旋转，则可以得到。
xB=R(xA+t)
可以看到，公式是不一样的。但这里需要注意的是，平移t也是不一样的。

简单小结一下。
对于旋转，通过3×3的正交阵表示，它的逆等于它的转置，通过矩阵求逆得到其逆旋转。对于平移，通过1×3的向量表示，通过添加正负号表示平移方向。对于变换，通过4×4的矩阵表示，通过矩阵求逆得到其逆变换。