【SLAM】4.SLAM十四讲——三维空间刚体运动

前言

 

一.坐标系

1.1 正交矩阵

A'A=AA'=E，即乘以自身的转置等于单位矩阵

1.2 机器人中的坐标系

世界坐标系world

机体坐标系body

传感器坐标系sensor

1.3 向量运算

点乘

叉乘

 其中a^意味着将一个向量转为反对称阵，口诀为对角线000，左下角321，左下角符号为正负正，如下所示

1.4 向量

向量即在某个坐标系下的坐标值

 

二.位姿变化

2.1 旋转变换

SO(3),Special Orthogonal Group，三维空间的旋转，是个正交阵

a=Rb，因为R正交，所以R'a=b

2.2 欧式变换

a=Rb+c，即在SO(3)的基础上再加一个平移，即SE(3)变换

 注意欧式变换时需要先旋转再平移，反向时平移已经受到了旋转的影响

 

三.描述刚体的旋转

3.1 描述方式

旋转矩阵SO(3)

旋转向量/角轴，总有1个轴1个角可以描述该旋转

欧拉角，一个旋转分为3次绕轴旋转，Roll-Pitch-Yaw

四元数

3.2 欧拉角RPY

旋转次序：常用绕RPY或者YPR的顺序旋转，旋转顺序的不同欧拉角的描述也不同，后旋受到前旋的影响

动轴/定轴问题：转完之后，再继续转时用之前的坐标系还是用新轴方向？

动轴：好交流描述，但受万向锁影响

定轴：编程方便，不受万向锁影响，但是不好描述

3.3 奇异性问题

 凡是用三个量描述三维空间的旋转，都具有奇异性或称不连续性

（1）旋转向量：从0~2Pi的跳变

（2）欧拉角：万向锁问题，不能在一个旋转结果上叠加旋转，虽然绕定轴旋转不会死锁但也不能增量调整

旋转向量、欧拉角的方式难用连续变化的参数描述连续变化的运动，不便匀速变化。

 

四.四元数Quaternion

4.1 形式

二维旋转:

 三维旋转:

 其中i、j、k为右手系，q0为实数，[q1,q2,q3]为实向量

运算：不支持交换律、数乘、取模、共轭

4.2 求逆

记住四元数运算的这两个符号的含义

4.3 使用

 （1）将三维点扩充至四维，

 （2）四元数运算

 p'的后三维即旋转后的坐标

（3）Eigen直接运算

 已对*作了重载，无须手动拓维

4.4提醒

（1）四元数好用、连续，但难以理解，欧拉角好理解但难用

（2）旋转的左乘和右乘不等价

（3）四种旋转描述方式是等价的，不必纠结转换问题

（4）位姿和旋转，有时表达静态，有时表达运动量

 

五.实际操作

VSCode的配置、Eigen、CMake

5.1 VSCode的配置

基本插件：C++、CMake、ROS、Chinese

存在头文件引用问题时，尝试一下自动修复或Ctrl+Shift+P对EditConfiguration配置文件进行修改

命令工具：locate xxx | grep yyy　　//需要安装mlocate工具

在VSCode中打开嵌入式终端：Ctrl+`

5.2 使用Eigen矩阵运算库

Eigen库的安装

在十四讲的ch3中有示例工程

ROS会自动安装Eigen库

特点：纯头文件、高效

矩阵的常用操作在Eigen中都有，例如矩阵分解

矩阵运算的加速：QR分解、正定矩阵的Cholesky分解

可以便捷地在欧拉角、角轴、四元数之间转换

5.3 CMake

大小写问题：cmake指令不区分大小写，变量参数区分大小写

log管理：message还是warning

CMake添加依赖库：

（1）用findpackage找到库路径,findpackage(xxx)，找到xxx.cmake的路径

（2）找到的xxx用于后面的include_dictories、target_link_lib