三维空间刚体运动的描述总结

三维空间刚体运动的描述方式有很多种，大概是：

旋转矩阵、平移向量--->欧式变换矩阵--->旋转向量--->欧拉角--->四元数

下面对它们简单的做一个整理，顺一下思路。

欧式变换中，坐标系的变换有：旋转、平移两种情况（只有旋转、只有平移、两者都有）

1、旋转矩阵、平移向量

假设此时，相机坐标系相对于世界坐标系的旋转矩阵为R、平移向量为t，则 $P_{w}=RP_{c}+\vec{t}$

这里要注意的就是“R、t是从哪个坐标系到哪个坐标系的，一定不能弄错了”，R是一个行列式为1的正交矩阵。

2、欧式变换矩阵

用R、t来表示欧式空间的旋转和平移存在一个问题：变换关系不是一个线性关系，多次变换之后计算公式会变得过于复杂。

于是就引入了齐次坐标：在三维向量的末尾添加一维（为数字1），将其变成四维向量，这种形式的坐标就是齐次坐标。

在齐次坐标里边，某个点 $x$ 的具体坐标不是唯一确定的，比如说[1, 1, 1, 1]和[2, 2, 2, 2]表示的是同一个点。但是，如果最后一项不为0，我们可以将其转换为“归一化坐标”---所有坐标除以最后一项，也就是将齐次坐标转换为非齐次坐标，对于每一个点来说，它的归一化坐标一定是唯一的。用公式表示就是： $\vec{x}=[x,y,z,w]=[x/w,y/w,z/w,1]$ 。

引入了其次坐标之后，我们可以将R、t写在一个矩阵里面，如下：

$\begin{bmatrix} P_{w}^{'} \\ 1 \end{bmatrix} = \begin{bmatrix} R & \vec{t} \\ 0 & 1 \end{bmatrix} \begin{bmatrix} P_c^{'} \\ 1 \end{bmatrix}$

中间的矩阵称为欧式变换矩阵，使用这种形式的欧式变换矩阵的好处就是它是一种线性变换的形式，多次变换直接将欧式变换矩阵左乘就可以得到最终的变换矩阵。

3、旋转向量

欧式变换可以很好的描述的刚体的运动，并且还是线性的形式。为了还需要旋转向量？

欧式变换矩阵用9个变量描述6个自由度的运动，信息有冗余
欧式变换矩阵本身是行列式为1的正交矩阵，这样的约束会使得后面优化的求解变得苦难。

旋转向量表示旋转的思想就是：任意一个旋转都可以用旋转轴和旋转角来表示。

因此，我们可以让旋转向量的方向和旋转轴相同，旋转向量的模表示旋转角。

旋转向量用3个自由度描述旋转，再用平移向量来描述平移向量，这样刚好可以用6个自由度来表达一次变换。

旋转向量和旋转矩阵之间的转换---罗德里格斯公式：

若旋转轴为 $\vec{n}$ ，旋转角为 $\theta$ ，则旋转矩阵为： $R=cos\theta I+(1-cos\theta )nn^{T}+sin\theta n^{\Lambda }$
若已经旋转矩阵，求旋转角：

$tr(R)=cos\theta tr(I)+(1-cos\theta )tr(nn^{T})+sin\theta tr(n^{\Lambda })$

$=3cos\theta +1-cos\theta =1+2cos\theta$

于是： $\theta =arccos((tr(R)-1)/2)$

同时，旋转轴经过旋转之后不变，则有 $R\vec{n}=\vec{n}$ ，解方程即可得到旋转轴。

4、欧拉角

相比旋转欧式、欧式变换矩阵、旋转向量，欧拉角以一种更加直观的方式来表示旋转---三个轴上分离的转角。

欧拉角的一个缺点就是存在“万向锁问题”。

5、四元数

四元数的形式如下： $q=q_0+q_1i+q_2j+q_3k$ 或者 $q=[r,\vec{s}]$ ，其中 $r=q_0\epsilon R,\vec{n}=[q_1,q_2,q_3]\epsilon R^{3}$

单位四元数可以用来表示三维空间中任意一个旋转，这种表达方式和欧式变换矩阵、欧拉角是等价的。

（1）四元数和旋转向量之间的转换

假设某个旋转是绕单位向量 $\vec{n}=[n_1,n_2,n_3]$ 进行了角度为 $\theta$ ，那么这个旋转对应的四元数为：

$q=[cos\theta /2,n_xsin\theta /2,n_ysin\theta /2,n_zsin\theta /2]$

相反，若已知一个单位四元数为 $q=[q_0,q_1,q_2,q_3]$ ，可以从它计算出旋转轴和夹角：

$\theta =2arccosq_0$ ， $\vec{n}=[q_1,q_2,q_3]^{T}/sin\theta /2$

上面式子中的 ${\color{Red} \theta}$ 加上 ${\color{Red} 2\pi }$ ，得到一个完全相同的旋转，但是对应的四元数却成为一个负的。在四元数中，任意一个旋转，都可以用两个互为相反数的四元数来表示。

（2）用四元数表示旋转

假设一个空间三维点 $p$ ，以及一个由轴角指定的旋转 $\vec{n},\theta$ ，经过旋转之后变为点 $p^{'}$ 。

它们之间的关系可以用下列式子来表达：

把三维空间点用一个纯四元数来描述： $p=[0,x,y,z]$
把旋转用四元数表示： $q=[cos\theta /2,\vec{n}sin\theta /2]$
旋转之后的点为： $p^{'}=qpq^{-1}$

可以验证，计算结果的实部为0，也就是一个纯四元数。虚部的3个分量表示旋转后的点的坐标。

（3）四元数到旋转矩阵的转换---见SLAM十四讲P55。

下面是一道练习题：

机器人1号、2号分别位于世界坐标系中。

1号的位姿：q1=[0.25,0.2,0.3,0.1]，t1=[0.3,0.1,0.1]

2号的位姿：q2=[-0.5,0..4,-0.1,0.2]，t2=[-0.1,0.5,0.3]

注意：这里的q、t表达的是Tcw，且未进行归一化。

设点p在机器人1号的坐标系下的坐标为p=[0.5,0,0.2]，求它在机器人2号的坐标系下的坐标。

#include <iostream>
#include <Eigen\Core>
#include <Eigen\Dense>
//Geometry模块提供了各种旋转和平移的表示
#include <Eigen\Geometry>

using namespace std;

int main()
{
	//一号的姿态
	Eigen::Quaterniond q1(0.35, 0.2, 0.3, 0.1);
	Eigen::Vector3d t1(0.3, 0.1, 0.1);

	Eigen::Isometry3d Tcw_1 = Eigen::Isometry3d::Identity();
	Tcw_1.rotate(q1.normalized().matrix());	//q1.matrix()将四元数转为AngleAxis旋转向量
	Tcw_1.pretranslate(t1);

	//点P
	Eigen::Vector3d Pc1(0.5, 0, 0.2);
	Eigen::Vector3d Pw = Tcw_1.inverse()*Pc1;

	//二号的姿态
	Eigen::Quaterniond q2(-0.5, 0.4, -0.1, 0.2);
	Eigen::Vector3d t2(-0.1, 0.5, 0.3);

	Eigen::Isometry3d Tcw_2 = Eigen::Isometry3d::Identity();
	Tcw_2.rotate(q2.normalized().matrix());
	Tcw_2.pretranslate(t2);

	Eigen::Vector3d Pc2 = Tcw_2*Pw;

	cout << "p向量在小萝卜二号的坐标系下的坐标为：" << endl;
	cout << "(" << Pc2.x() << "," << Pc2.y() << "," << Pc2.z() << ")" << endl;

	return 0;
}

不服输的小白

发布了47 篇原创文章 · 获赞 23 · 访问量 8万+

私信关注

三维空间刚体运动的描述总结

猜你喜欢