文章目录
- 旋转矩阵
- 平移
- 变换矩阵与齐次坐标
- TF中发布坐标变换
在空间中表示两个物体之间的位姿关系，通常可以分解为一个纯平移和一个纯旋转。也称为一个欧式变换。
旋转矩阵
这里介绍一种用旋转矩阵表示旋转的方式。旋转矩阵有一些特殊的性质，他是一个行列式为1的正交矩阵。因此它属于正交群。此外由于所有旋转矩阵都满足行列式为1，是一种特殊的正交矩阵，我们称旋转矩阵属于特殊正交群（Special Orthogonal Group）。表示三维旋转可以写成
S
O
(
3
)
=
{
R
∈
R
n
×
n
∣
R
R
T
=
I
,
det
(
R
)
=
1
}
SO(3) = \{R\in\mathbb R^{n\times n} | RR^T = I,\det (R) = 1\}
SO(3)={R∈Rn×n∣RRT=I,det(R)=1}
由于旋转矩阵是一个正交矩阵，它的逆矩阵（也就是它的转置）描述了一个相反的旋转，按照上面的定义
a
′
=
R
−
1
a
=
R
T
a
a' = R^{-1}a = R^Ta
a′=R−1a=RTa
平移
在欧式变换中，除了旋转还有平移。考虑世界坐标系下一个向量
a
a
a，经过一次旋转（用
R
R
R描述）和一次平移变换
t
t
t之后，得到了
a
′
a'
a′，那么把平移和旋转合到一起可以表示为：
a
′
=
R
a
+
t
a' = Ra + t
a′=Ra+t
这样我们就完整的描述了一个欧式空间的坐标变换。我们通常定义坐标系1和坐标系2，用
R
12
R_{12}
R12表示两个坐标系之间的旋转（通常不表示吧坐标系2中的向量旋转到坐标系1中），向量
a
a
a在两个坐标系下的坐标分别为
a
1
,
a
2
a_1,a_2
a1,a2，那么他们之间的关系应该是
a
1
=
R
12
a
2
+
t
12
a_1 = R_{12}a_2 + t_{12}
a1=R12a2+t12
关于平移
t
12
t_{12}
t12，通常是知坐标系1原点指向坐标系2原点的向量。
变换矩阵与齐次坐标
假设我们进行了两次变换，分别为
R
1
,
t
1
R_1,t_1
R1,t1和
R
2
,
t
2
R_2,t_2
R2,t2
b
=
R
1
a
+
t
1
,
c
=
R
2
b
+
t
2
b = R_1a+t_1,c = R_2b + t_2
b=R1a+t1,c=R2b+t2
那么从
a
a
a到
c
c
c的变换可以表示为
c
=
R
2
(
R
1
a
+
t
1
)
+
t
2
c = R_2(R_1a+t_1) + t_2
c=R2(R1a+t1)+t2
这样的形式会显得非常啰嗦，而且随着变换则增加，嵌套会显得非常冗长。因此我们映入齐次坐标和坐标变换矩阵
[
a
′
1
]
=
[
R
t
0
T
1
]
[
a
1
]
=
T
[
a
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]
[a′1]=[R0Tt1][a1]=T[a1]
我们采用在向量末位补1的数学技巧，这样就可以表示为
b
~
=
T
1
a
~
,
c
~
=
T
2
b
~
⟹
c
~
=
T
2
T
1
a
~
\tilde b = T_1 \tilde a,\tilde c = T_2\tilde b \implies \tilde c = T_2T_1\tilde a
b~=T1a~,c~=T2b~⟹c~=T2T1a~
称矩阵
T
T
T为变换矩阵，他具有比较特别的结构，左上三角为旋转矩阵，右侧为平移。这种矩阵又称特殊欧式群（Special Euclidean Group）。
S
E
(
3
)
=
{
T
=
[
R
t
0
T
1
]
∈
R
4
×
4
∣
R
∈
S
O
(
3
)
,
t
∈
R
3
}
SE(3) = \{T = \left[ \begin{matrix} R&t\\0^T & 1 \end{matrix}\right]\in\mathbb R^{4\times 4} | R\in SO(3), t\in\mathbb R^3 \}
SE(3)={T=[R0Tt1]∈R4×4∣R∈SO(3),t∈R3}
与
S
O
3
SO3
SO3一样，求该矩阵的逆，可以得到一个反向的变换
T
−
1
=
[
R
T
−
R
T
t
0
T
1
]
T^{-1} = \left[\begin{matrix} R^T & -R^Tt \\ 0^T&1 \end{matrix}\right]
T−1=[RT0T−RTt1]
TF中发布坐标变换
我们常用的机器人描述中，通常有静态的变换（例如雷达相对于车的位置安装完成后就保持不变了），和动态的变换（例如小车在世界坐标系中移动，车声坐标系和世界坐标系的相对位置就是移动的）。前者可以通过TF的静态Transform命令实现，前者和后者都可以通过以下方式实现。
假设我的无人机机身坐标系为"uav"，世界坐标系为"world"，无人机上的雷达坐标系为"lidar"，当我们获得无人机当前在世界坐标系下的位姿时，我们就可以通过以下代码发布坐标变换：
#include <tf/transform_broadcaster.h>
// 你也可以不用Eigen
#include <Eigen/Geometry>
#include <Eigen/Dense>
typedef Eigen::Matrix<double, 3, 3> Mat33;
typedef Eigen::Matrix<double, 3, 1> Vec3;
void publishTF(Mat33 rotation, Vec3 translation){
// 创建一个坐标变换发布者
static tf::TransformBroadcaster broadcaster;
// 采用四元数和平移变换描述
tf::Quaternion quad;
tf::Transform trans;
Vec3 eulerAngle = rotation.eulerAngles(0,1,2);
// 发布机身到世界的动态变化
trans.setOrigin(tf::Vector3(translation.x(), translation.y(), translation.z()));
quad.setRPY(eulerAngle[0],eulerAngle[1],eulerAngle[2]);
trans.setRotation(quad);
broadcaster.sendTransform(tf::StampedTransform(trans, ros::Time::now(),"world","uav"));
// 发布雷达到机身的静态变换
trans.setOrigin(tf::Vector3(0,0,0.08));
trans.setRotation( tf::Quaternion(-0.707107, 0, 0, 0.707107));
broadcaster.sendTransform(tf::StampedTransform(trans, ros::Time::now(),"uav","laser"));
}