Advertisement

第三讲 三维空间刚体运动

阅读量：

一、旋转矩阵

刚体在其运动过程中，在三维空间中占据特定的状态点，在工程学中将其运动状态称为位置坐标系与方向信息共同决定的结果，在机器人学领域则将此定义为完整的位形空间中的一个点，在此情况下需要明确位形参数的选择方案以确保表示的有效性与一致性

1、点、向量和坐标系

1）定义

点： 点是空间中的基本元素，没有长度和体积。

向量： 由一个点指向另外一个点就是一个向量。！！！本书中坐标用列向量表示！！！

坐标系： 点和向量要想与坐标联系起来，说先要选定一个坐标系。

2）点和向量的坐标

三维空间中点的坐标： 在给定坐标系下可以直接表示为（x,y,z)

三维空间中向量的坐标：

系与基的选取有关。其中基的长度为1，是空间中一组线性无关的向量。

3）向量之间的内积和外积

对于向量a,b,其内积可以写成

向量内积的结果是一个标量，大小为|a||b|cos<a,b>，描述向量间的投影关系。

向量外积的结果是一个矢量，大小为|a||b|sin<a,b>，方向垂直于这两个向量。

借助反对称符号^的作用，使得向量的外积表现为一个矩阵与一个列向量相乘的线性运算行为。

反对称矩阵性质：

2、坐标系间的欧式变换

在实际应用场景中的一般情况是，在应用环境中我们会建立多个参考框架以适应复杂的需求，在这种情况下同一向量在同一参考框架下的表示具有差异性，在这种情况下两者的转换关系涉及一个欧式变换矩阵T如上图所示用于直观展示各坐标的相对位置关系

在两个坐标系之间的转换通常表现为一次旋转变换后跟上一次平移操作。这通常被称为刚性变换（rigid transformation）。其显著特征是任何向量在不同坐标系中的长度及其之间的夹角均保持不变。

1)坐标系间的旋转变换

旋转变换公式

如图3-1所示

设世界坐标系为

{O_w}

，其中一组单位正交基为

，向量

p

在该坐标系下的坐标为{eft^T}

设机器人坐标系为

{O_c}

，其中一组单位正交基为

，向量

p

在该坐标系下的坐标为{eft^T}

当坐标系

{O_w}

和坐标系

{O_c}

间只存在旋转关系时，向量

p

在两坐标系下坐标关系为：

{p_w} = {R_{wc}}{p_c}

其中
{p_w} = {eft^T}，{p_c} = {eft^T}
{R_{wc}} = eft

称矩阵

R

为旋转矩阵。同时由于

{e_{iw}}

和

{e_{jc}}

均为单位向量，因此

e_{iw}^T{e_{jc}}

实际上是向量

{e_{iw}}

和

{e_{jc}}

的夹角余弦值，因此矩阵

R

也被称为方向余弦矩阵。

{R_{wc}}

读作从

{O_c}

到

{O_w}

的旋转矩阵。

旋转矩阵的性质

  • 矩阵
R

为旋转矩阵

eftrightarrow

矩阵

R

为正交矩阵且行列式为1 （正交矩阵：若

A{A^T} = I

,则

A

为正交矩阵）
*

n

维旋转矩阵的集合被称为特殊正交群：

{ext{SO}} = R n {athbb{R}^{n imes n}}|R{R^T} = I,et R = 1

同一向量在两坐标系下坐标的变换关系

**

egin{array}{*{20}{c}} {​{p_w} = {R_{wc}}{p_c}}&{​{p_w} = R_{cw}^T{p_c}} {​{p_c} = {R_{cw}}{p_w}}&{​{p_c} = R_{wc}^T{p_w}} nd{array}

**

2）坐标系间的旋转变换和平移变换

平移变换

当坐标系

{O_w}

和坐标系

{O_c}

在旋转变换的基础上还存在平移变换时，向量

p

在坐标系

{O_w}

和坐标系

{O_c}

下的坐标变换关系为：

{p_w} = {R_{wc}}{p_c} + {t_{wc}}

其中

{t_{wc}}

是坐标系

{O_w}

原点指向坐标系

{O_c}

原点的向量在坐标系

{O_w}

下的坐标。

！！！注意：

{t_{wc}}

是向量对应的坐标，而不是向量 ！！！ 所以

{t_{wc}} e {t_{cw}}
{t_{wc}}

是坐标系

{O_w}

原点指向坐标系

{O_c}

原点的向量在坐标系

{O_w}

下的坐标

{t_{cw}}

是坐标系

{O_c}

原点指向坐标系

{O_w}

原点的向量在坐标系

{O_c}

下的坐标

坐标

{t_{wc}}

和坐标

{t_{cw}}

所对应的空间中的向量是一对相反的向量，但坐标

{t_{wc}}

和坐标

{t_{cw}}

是这一对向量在两个不同坐标系

{O_w}

和

{O_c}

下的坐标，因此

{t_{wc}} e {t_{cw}}

坐标系间欧式变换公式（旋转+平移）

{p_w} = {R_{wc}}{p_c} + {t_{wc}}

其中
{p_w} = {eft^T}，{p_c} = {eft^T}分别为向量

p

在坐标系

{O_w}

和坐标系

{O_c}

下的坐标。
{R_{wc}} = eft 是从坐标系

{O_c}

到坐标系

{O_w}

的旋转矩阵

{t_{wc}}

是从坐标系

{O_w}

原点指向坐标系

{O_c}

原点的向量在坐标系

{O_w}

下的坐标。

3）坐标系间欧氏变换公式线性化

进行线性化原因

在实际应用过程中，通常需要在多个坐标系下进行多次坐标变换，而应用

{p_w} = {R_{wc}}{p_c} + {t_{wc}}

这样一个公式进行多次变换后会很复杂，如进行两次坐标变换后会变成

{p_3} = {R_{32}} + {t_{32}}

。因此要对该公式进行线性化。

线性化方法

我们通过增加一个维度将上述的欧式变换公式写成齐次形式：
eft = efteft = {T_{wc}}eft

我们称
{T_{wc}} = eft为从坐标系

{O_c}

到坐标系

{O_w}

的变换矩阵。

{T_{wc}}

这样的变换矩阵构成的集合成为特殊欧氏群

{ext{SE}}eft
{ext{SE}}eft = T = eft n {athbb{R}^{4 imes 4}}|R n {ext{SO}},t n {athbb{R}^3}

线性化结果

在不引起误会的情况下，我们将{eft^T}记作

p_w

，将{eft^T}记作

p_c

。（这个可以这么理解，对于

p_w

和

p_c

当他被要求呈现三维形态时，则表现为三维结构；当他被要求呈现四维形态时，则可以通过添加一个单位来实现相应的扩展。

则两个坐标间的变换公式可记作：

{p_w} = {T_{wc}}{p_c}

连续的坐标变换可表示为：

{p_3} = {T_{32}}{p_2} = {T_{32}}{T_{21}}{p_1}

这样同一向量在多个坐标系下进行多次变换后公式依然简洁。

二、C++开源线性代数库Eigen

1、库文件的安装和位置查询

库文件安装在终端输入以下命令

sudo apt-get install libeigen3-dev

库文件位置查询在终端输入以下命令

sudo updatedb

locate eigen3

说明

Eigen 是一个完全由头文件构建完成的静态库，并不包含.so和.a这样的符号链接文件。因此，在使用过程中无需引入额外的动态链接器即可直接运行代码。同时处理不同的静态链接库配置问题较为方便。此外，在Ubuntu系统中，默认安装路径位于/usr/include/eigen3/目录下。

2、Eigen库的引用和应用

1）Eigen库的引用

在CMakeLists.txt文件中：

#添加头文件,双引号中是eigen库所在路径，根据实际安装位置变化

include_directorise("/usr/include/eigen3")

在.cpp文件中：

//Eigen核心部分

#include <Eigen/Core>

//稠密矩阵的代数部分

#include <Eigen/Dense>

using namespace Eigen

2)Eigen库的使用

矩阵和向量的定义

//在Eigen库中，所有矩阵和向量均基于模板类Eigen::Matrix实现，并接受三个参数
//格式为：Matrix<数据类型, 行数, 列数>
//示例：创建一个浮点型元素的2行3列矩阵
Matrix<float, 2, 3> matrix_2_3;
//在Eigen库中预定义了许多特定类型的矩阵和向量
//这些类型的底层实现仍基于Eigen::Matrix
例如，在Eigen中可以使用Vector3d或Matrix<double, 3, 1>来表示三维列向量
//注：此处原书有误，请将float更改为double
//对于固定的维度（如3x3），可以直接使用简略形式
Matrix<double, 3, 3> matrix_3x3;
//当无法预先确定矩阵尺寸时，则可以选择动态大小的Matrix
如果无法预先确定矩阵尺寸，则可以选择动态大小的：
Matrix<double,Dynamic,Dynamic> matrix_dynamic;

为矩阵初始化

用数字初始化

在初始化阶段，在每一行依次填充数字，在接着在下一行继续填充。
经过这样的初始化后，
矩阵则表示为：
[
[1,2,3],
[4,5,6]
]

矩阵中元素置零

创建mxn维度的矩阵并初始化其所有元素为零。
生成一个3×3维度的单位矩阵并将其所有元素设为零。

矩阵中元素置随机数

matrix_mn = MatrixXd::Random(m,n)； //为mxn维的矩阵赋随机数

matrix_33 = Matrix::Random(); //特例，为3x3维矩阵赋随机数

访问矩阵中的元素

//直接用矩阵名称（行数，列数）就可以访问

//！！！注意：矩阵的行列在这里都是从0开始算的，如下访问矩阵的matrix_23第二行第二列元素为！！！

matrix_23(1,1)

//访问矩阵的第一行第一个元素

matrix_23(0,0)

矩阵类型的转换

在Eigen中无法进行隐式数据类型转换必须进行显式数据类型转换

//什么是隐式数据类型转换

int a = 3；

double b = 2；

double c；

c = a*b;

//在上述c=a*b中，a由原来的int型隐式转换为double型和double型的b相乘，最后得到了double型的c

//探讨显示数据类型的转换方式及其意义
c = (double)a*b;
其中：
(double)表示将数值a从当前类型转换为双精度浮点数，
与双精度浮点数b进行乘法运算，
计算结果存储在双精度浮点数c中。

在不同数据类型的矩阵运算中应采取明确的数据类型转换措施，并通过明确的数据类型转换实现这一目标。

矩阵名称.cast<要转换的数据类型>（） 来完成

创建一个浮点型矩阵matrix_23；
定义一个三维向量v_3d；
生成结果矩阵result；
将matrix_23转换为双精度后与v_3d相乘得到结果。

矩阵的维数

！！！在矩阵运算过程中，不能搞错矩阵的维数，否则也会报错。！！！

矩阵的常用计算

四则运算

直接用+-*/就可以

单矩阵运算（以3*3矩阵matrix_33为例）

该矩阵的转置操作

计算该矩阵内所有元素的总和

该矩阵的迹

计算该矩阵的逆

计算该矩阵的行列式

求矩阵的特征值

复制代码 
>       1. SelfAdjointEigenSolver<Matrix3d> eigen_solver(matrix_33.cast<double>())  
>  
>       2. //求解特征值和特征向量，并存储到变量eigen_solver中，eigen_solver()的()中为待求解的矩阵
>  
>  
>
复制代码 
>       1. cout << eigen_solver.eigenvalues() << endl;              //输出matrix_33的特征值

>  
>       2. cout << eigen_solver.eigenvectors() << endl;             //输出matrix_33的特征向量
>  
>  
>

说明：SelfAdjointEigenSolver<>()用于计算任意大小矩形区域内的特征值与特征向量。该算法支持处理不同维度的数据集，并通过自适应网格细化技术提高分析精度。

注意： 此函数仅限于处理double类型的矩阵及其相应的特征值与特征向量。为了实现数据类型的转换以满足计算需求，请将matrix\_33\cast显式地进行类型转换。

解矩阵方程matrix_NN * x = matrix_N1

复制代码 
>       1. Matrix<double,N,N> matrix_NN = MatrixXd::Random(N,N);     //定义NxN维的矩阵并赋随机数

>  
>       2. Matrix<double,N,1> matrix_N1 = MatrixXd::Random(N,1);     //定义Nx1维的矩阵并赋随机数
>  
>  
>

求解方法1：使用矩阵逆求解，但计算量大

复制代码 
>     Matrix<double,N,1> x = matrix_NN.inverse()*matrix_N1;     //使用x = matrix_N1 * matrix_NN.inverse()公式

求解方法2：使用QR分解求解，计算量小

复制代码 
>     Matrix<double,N,1> x = matrix_NN.colPivHouseholderQr().solve(matrix_N1);

求解方法3：使用cholesky分解求解，要求矩阵半正定

复制代码 
>     Matrix<double,N,1> x = matrix_NN.ldlt().solve(matrix_N1);
>

运算时间的计算

time_start被初始化为当前时间。
// clock_t是一种C++标准库中的数据类型
time_end被初始化为当前时间。
// CLOCKS_PER_SEC表示每秒有多少个CPU时钟周期
deltime被计算为程序运行所消耗的时间（以秒为单位）。
deltime被计算为程序运行所消耗的时间（以毫秒为单位）。

全部评论 (0)

还没有任何评论哟~

相关文章推荐

第三讲 三维空间刚体运动

一、旋转矩阵 对于刚体来说，其在空间中的状态由位置和姿态描述，称之为姿态。如，相机在的（0,0,0）处，这是在描述相机的位置。相机朝向为正前方，这是在描述相机的姿态。 1、点、向量和坐标系 1）定义 ...

第三讲 三维空间刚体运动

目标：理解三维空间的刚体运动描述方式：旋转矩阵、变换矩阵、四元数和欧拉角；掌握Eigen库的矩阵、几何模块的使用方法。 1、旋转矩阵 刚体运动：两个坐标系之间的运动由一个旋转加上一个平移组成，这种运动...

【视觉SLAM十四讲】第三讲 三维空间刚体运动

点与坐标系 右手系， 内积（点乘） 外积（叉乘） absin<a,b a×b=b×a 坐标系的变换 原点间的平移 三个轴的旋转 旋转矩阵 R为旋转矩阵 R是一个正交矩阵（R^TR=I） R的行列式为+...

视觉SLAM十四讲——第三讲三维空间刚体运动

@《视觉SLAM十四讲》知识点与习题 《视觉SLAM十四讲》第三讲知识点整理+习题 正在学习SLAM相关知识，将一些关键点及时记录下来。 知识点整理 本讲主要介绍VSLAM基本问题之一：一个刚体在三维...

【SLAM14讲】04 三维空间刚体运动

一、坐标系之间的欧式变换 xW,yW,zW定义的世界坐标系，xC,yC,zC定义的相机坐标系。 相机视野中某个向量p，它的坐标为pc，而从世界坐标系下看，它的坐标pw。 二、相机运动 相机运动是一个刚...

视觉slam14讲-第3讲-三维空间刚体运动

内容 第三讲其实就讲了一个事情旋转： 三维空间的旋转怎么表达，有几种表达方式，每个表达方式的优缺点，以及不同表达方式之间的转换方法。 介绍了四种表达方式，分别是： 1、旋转矩阵：R，缺点是计算起来有冗...

三维空间刚体运动

三维空间刚体运动 三维空间中的刚体运动涉及到刚体在空间中的位置（translation）和方向（orientation）的变化。在机器视觉和机器人领域，尤其是在同时定位与地图构建（SLAM，Simul...

三维空间刚体运动

三维空间的刚体运动 点与坐标系 旋转矩阵 旋转向量和欧拉角 四元数 点与坐标系 2D的情况：用两个坐标加旋转角表达 3D的情况：？ 在描述3D的情况前，可以先描述一些基本概念： 坐标系、点、向量、向量...

三维空间刚体运动

三维空间刚体运动 参考书目《视觉SLAM十四讲》高翔张涛等著 参考链接https://zhuanlan.zhihu.com/p/32937868 一、旋转矩阵 1\.点、向量和坐标系 我们一般用三维空...

SLAM十四讲-三维空间刚体运动

1旋转矩阵是正交矩阵 证明：旋转矩阵是正交矩阵