视觉SLAM十四讲-第三讲-三维空间刚体运动教学内容
(完整版)刚体的基本运动(可编辑修改word版)
第三章刚体力学§3.1 刚体运动的分析§3.2 角速度矢量§3.3 刚体运动微分方程§3.4 刚体平衡方程§3.5 转动惯量§3.6 刚体的平动与定轴转动§3.7 刚体的平面平行运动§3.1 刚体运动的分析一、描述刚体位置的独立变量1.刚体是特殊质点组 dr ij=0,注意:它是一种理想模型,形变大小可忽略时可视为刚体。
2.描述刚体位置的独立变数描述一个质点需(x,y,z), 对刚体是否用 3n 个变量？否,由于任意质点之间的距离不变, 如确定不在同一直线上的三点,即可确定刚体的位置,需 9 个变量，由于两点间的距离保持不变，所以共需 9-3=6 个变量即可。
刚体的任意运动=质心的平动+绕质心的转动，描述质心可用(x,y,z), 描述转轴可由α, β,γ。
二、刚体的运动分类1.平动：刚体在运动过程中，刚体上任意直线始终平行.任意一点均可代表刚体的运动，通常选质心为代表.需要三个独立变量,可以看成质点力学问题.(注意:平动未必是直线运动)2.定轴转动: 刚体上有两点不动,刚体绕过这两点的直线转动,该直线为转轴. 需要一个独立变量φ3.平面平行运动: 刚体上各点均平行于某一固定平面运动。
可以用平行于固定平面的截面代表刚体。
需要三个独立变量。
4.定点运动: 刚体中一点不动,刚体绕过固定点的瞬转转动。
需三个独立的欧拉角。
5.一般运动: 平动+转动§3.2 角速度矢量定轴转动时角位移用有向线段表示,右手法确定其方向.有向线段不一定是矢量,必须满足平行四边形法则,对定点转动时,不能直接推广,因不存在固定轴.ω = lim ∆n=d n刚体在 dt 时间内转过的角位移为 d n ，则角速度定义为角速度反映刚体转动的快慢。
∆t →0 ∆t dt线速度与角速度的关系：d r =d n ⨯r , ∴ v =d rdt=ω ⨯rF 1 F ⨯ M§3.3 刚体运动微分方程一、 基础知识1.力系：作用于刚体上里的集合。
《视觉SLAM十四讲》笔记（ch7）
《视觉SLAM⼗四讲》笔记（ch7）ch7 视觉⾥程计1本章⽬标：1.理解图像特征点的意义，并掌握在单副图像中提取出特征点及多副图像中匹配特征点的⽅法2.理解对极⼏何的原理，利⽤对极⼏何的约束，恢复出图像之间的摄像机的三维运动3.理解PNP问题，以及利⽤已知三维结构与图像的对应关系求解摄像机的三维运动4.理解ICP问题，以及利⽤点云的匹配关系求解摄像机的三维运动5.理解如何通过三⾓化获得⼆维图像上对应点的三维结构本章⽬的：基于特征点法的vo，将介绍什么是特征点，如何提取和匹配特征点，以及如何根据配对的特征点估计相机运动和场景结构，从⽽实现⼀个基本的两帧间视觉⾥程计。
特征点：⾓点、SIFT(尺度不变特征变换，Scale-Invariant Feature Transform)、SURF、、ORB（后三个是⼈⼯设计的特征点，具有更多的优点）特征点的组成：1.关键点：指特征点在图像⾥的位置2.描述⼦：通常是⼀个向量，按照某种⼈为设计的⽅式，描述了该关键点周围像素的信息。
相似的特征应该有相似的描述⼦（即当两个特征点的描述⼦在向量空间上的距离相近，认为这两个特征点是⼀样的）以ORB特征为代表介绍提取特征的整个过程：ORB特征：OrientedFAST关键点+BRIEF关键⼦提取ORB特征的步骤：1.提取FAST⾓点：找出图像中的“⾓点”，计算特征点的主⽅向，为后续BRIEF描述⼦增加了旋转不变特性FAST⾓点：主要检测局部像素灰度变化明显的地⽅特点：速度快缺点：1).FAST特征点数量很⼤且不确定，但是我们希望对图像提取固定数量的特征2).FAST⾓点不具有⽅向信息，并且存在尺度问题解决⽅式：1).指定要提取的⾓点数量N，对原始FAST⾓点分别计算Harris响应值，然后选取前N个具有最⼤响应值的⾓点作为最终的⾓点集合2).添加尺度和旋转的描述 尺度不变性的实现：构建图像⾦字塔，并在⾦字塔的每⼀层上检测⾓点（⾦字塔：指对图像进⾏不同层次的降采样，以获得不同分辨率的图像）特征旋转的实现：灰度质⼼法（质⼼：指以图像块灰度值作为权重的中⼼）2.计算BRIEF描述⼦：对前⼀步提取出的特征点周围图像区域进⾏扫描特点：使⽤随机选点的⽐较，速度⾮常快，由于使⽤了⼆进制表达，存储起来也⼗分⽅便，适⽤于实时的图像匹配在不同图像之间进⾏特征匹配的⽅法：1.暴⼒匹配：浮点类型的描述⼦，使⽤欧式距离度量⼆进制类型的描述⼦（⽐如本例中的BRIEF描述⼦），使⽤汉明距离度量缺点：当特征点数量很⼤时，暴⼒匹配法的运算量会变得很⼤2.快速近似最近邻（FLANN）：适合匹配特征点数量极多的情况实践部分：1.OpenCV的图像特征提取、计算和匹配的过程：演⽰如何提取ORB特征并进⾏匹配代码： 1 #include <iostream>2 #include <opencv2/core/core.hpp>3 #include <opencv2/features2d/features2d.hpp>4 #include <opencv2/highgui/highgui.hpp>56using namespace std;7using namespace cv;89int main(int argc,char** argv)10 {11if(argc!=3)12 {13 cout<<"usage:feature_extraction img1 img2"<<endl;14return1;15 }1617//读取图像18 Mat img_1=imread(argv[1],CV_LOAD_IMAGE_COLOR);19 Mat img_2=imread(argv[2],CV_LOAD_IMAGE_COLOR);2021//初始化22 vector<KeyPoint> keypoints_1,keypoints_2;//关键点,指特征点在图像⾥的位置23 Mat descriptors_1,descriptors_2;//描述⼦,通常是向量24 Ptr<ORB> orb=ORB::create(500,1.2f,8,31,0,2,ORB::HARRIS_SCORE,31,20);2526//第⼀步：检测OrientFAST⾓点位置27 orb->detect(img_1,keypoints_1);28 orb->detect(img_2,keypoints_2);2930//第2步：根据⾓点位置计算BRIEF描述⼦31 orb->compute(img_1,keypoints_1,descriptors_1);32 orb->compute(img_2,keypoints_2,descriptors_2);3334 Mat outimg1;35 drawKeypoints(img_1,keypoints_1,outimg1,Scalar::all(-1),DrawMatchesFlags::DEFAULT);36 imshow("1.png的ORB特征点",outimg1);37 Mat outimg2;38 drawKeypoints(img_2,keypoints_2,outimg2,Scalar::all(-1),DrawMatchesFlags::DEFAULT);39 imshow("2.png的ORB特征点",outimg2);4041//第3步：对两幅图像中的BRIEF描述⼦进⾏匹配，使⽤Hamming距离42 vector<DMatch> matches;43//特征匹配的⽅法：暴⼒匹配44 BFMatcher matcher(NORM_HAMMING);45 matcher.match(descriptors_1,descriptors_2,matches);46// for(auto it=matches.begin();it!=matches.end();++it)47// {48// cout<<*it<<" ";49// }50// cout<<endl;5152//第4步：匹配点对筛选53 distance是min_dist5455double min_dist=10000,max_dist=0;5657//找出所有匹配之间的最⼩距离和最⼤距离，即最相似的和最不相似的和最不相似的两组点之间的距离58for(int i=0;i<descriptors_1.rows;++i)59 {60double dist=matches[i].distance;61// cout<<dist<<endl;62if(dist<min_dist) min_dist=dist;63if(dist>max_dist) max_dist=dist;64 }6566 printf("--Max dist:%f\n",max_dist);67 printf("--Min dist:%f\n",min_dist);6869//当描述⼦之间的距离⼤于两倍的最⼩距离时，即认为匹配有误70//但有时候最⼩距离会⾮常⼩，设置⼀个经验值作为下限71 vector<DMatch> good_matches;72for(int i=0;i<descriptors_1.rows;++i)73 {74if(matches[i].distance<=max(2*min_dist,30.0))75 {76 good_matches.push_back(matches[i]);77 }78 }7980//第5步：绘制匹配结果81 Mat img_match;82 Mat img_goodmatch;83 drawMatches(img_1,keypoints_1,img_2,keypoints_2,matches,img_match);84 drawMatches(img_1,keypoints_1,img_2,keypoints_2,good_matches,img_goodmatch);85 imshow("所有匹配点对",img_match);86 imshow("优化后匹配点对",img_goodmatch);87 waitKey(0);8889return0;90 }实验结果：1.png中提取到的特征点2.png中提取到的特征点匹配结果： 所有点对匹配结果 优化后的匹配点对结果（筛选依据是Hamming距离⼩于最⼩距离的两倍）结果分析：尽管在这个例⼦中利⽤⼯程经验优化筛选出正确的匹配，但并不能保证在所有其他图像中得到的匹配都是正确的，所以，在后⾯的运动估计中，还要使⽤去除误匹配的算法。
三维空间刚体运动——（1）齐次坐标与旋转矩阵
三维空间刚体运动——（1）齐次坐标与旋转矩阵⽬录：什么是齐次坐标？简单的说：齐次坐标就是在原有坐标上加上⼀个维度：使⽤齐次坐标有什么优势1、能⾮常⽅便的表达点在直线或平⾯上在2D平⾯上，⼀条直线 l 可以⽤⽅程 ax + by + c = 0 来表⽰，该直线⽤向量表⽰的话⼀般记做我们知道点p = (x, y)在直线 l 上的充分必要条件是 ax + by + c = 0如果使⽤齐次坐标的话，点p的齐次坐标就是p'=(x, y, 1)那么 ax + by + c = 0 就可以⽤两个向量的内积（点乘）来表⽰：因此，点p在直线l上的充分必要条件就是直线l 与p的齐次坐标p'的内积：是不是很⽅便呢！同理，我们知道三维空间的⼀个平⾯A可以⽤⽅程 ax + by + cz + d = 0 来表⽰，三维空间的⼀个点P=(x, y, z) 的齐次坐标 P'=(x, y, z, 1)，类似的，点P在空间平⾯A上可以⽤两个向量的内积来表⽰，如下：因此，点P在平⾯A上的充分必要条件就是平⾯A 向量与P的齐次坐标P'的内积（点乘）：2、⽅便表达直线与直线，平⾯与平⾯的交点先给出结论，后⾯再具体解释：结论：在齐次坐标下，可以⽤两个点 p, q 的齐次坐标叉乘结果来表达⼀条直线 l，也就是l = p x q也可以使⽤两条直线 l, m 的叉乘表⽰他们的交点 xx = l x m见下⾯⽰例图。
之所以可以这么简洁的表⽰交点是因为采⽤了齐次坐标的表⽰⽅式。
那么这是为什么呢？先介绍⼀下叉乘（也称叉积、外积）的概念：两个向量 a和b 的叉乘仅在三维空间中有定义，写作 a x ba xb 是与向量 a, b都垂直的向量，其⽅向通过右⼿定则（见下图）决定。
其模长等于以两个向量为边的平⾏四边形的⾯积（见下图）。
叉乘可以定义为：其中θ表⽰a, b的夹⾓（0°到180°之间），||a||, ||b||是向量a, b的模长n则是⼀个与向量a, b所构成的平⾯垂直的单位向量根据叉乘定义：向量⾃⾝叉乘结果为0，因为夹⾓为0。
视觉slam介绍PPT课件
2021
第一讲 视觉SLAM概述
• 相机的本质
• 以二维投影形式记录了三维世界的信息 • 此过程丢掉了一个维度：距离
• 各类相机主要区别：有没有深度信息
• 单目：没有深度，必须通过移动相机产生深度 Moving View Stereo • 双目：通过视差计算深度 Stereo • RGBD：通过物理方法测量深度
• 主流方法：SIFT/SURF/ORB （OpenCV features2d模块）
2021
3.1.1 ORB特征
• 例子：ORB特征
• 关键点：Oriented FAST • 描述：BRIEF
• FAST
• 连续N个点的灰度有明显差异
• Oriented FAST • 在FAST基础上计算旋转
• BRIEF • BRIEF-128：在特征点附近的128次像素比较
整理之：
2021
第二讲 相机模型
• 成像平面到像素坐标
代入 得
2021
第二讲 相机模型
展开形式 矩阵形式
传统习惯
左侧是齐次坐标 中间矩阵称为内参数 右侧是非齐次坐标 内参通常在相机生产之后就已固定
2021
第二讲 相机模型
• 除内参外，相机坐标系与世界坐标系还相差一个变换：
• 这里 R, t 或 T 称为外参 • 外参是SLAM估计的目标
2021
实践 点云拼接
2021
第三讲 视觉里程计
Chapter 3: Visual Odometry
2021
3.1 特征点法
• 经典SLAM模型中以位姿——路标（Landmark）来描述SLAM过程 • 路标是三维空间中固定不变的点，能够在特定位姿下观测到
视觉SLAM
用形式更简单的 E 。
三角测量确定深度
• 在得到运动之后，下一步我们需要用相机的运动估计特征点的空间位置。在单目 SLAM 中，仅通过单张图 像无法获得像素的深度信息，我们需要通过三角测量（ Triangulation ）（或三角化）的方法来估计地图点
回环检测
• 回环检测，又称闭环检测（ Loop Closure Detection ），主要解决位置 估计随时间漂移的问题。 • 前端提供特征点的提取和轨迹、地图的初值，而后端负责对这所有的数据
进行优化。然而，如果像 VO 那样仅考虑相邻时间上的关联，那么，之前
产生的误差将不可避免地累计到下一个时刻，使得整个 SLAM 会出现累积 误差。长期估计的结果将不可靠，或者说，我们无法构建全局一致的轨迹
用对极几何恢复出在两帧之间摄像机的运动
• 现在，假设我们从两张图像中，得到了一对配对好的特征点，像图 7-7 里显示的那样。如果我们有若 干对这样的匹配点，就可以通过这些二维图像点的对应关系，恢复出在两帧之间摄像机的运动。
• 1. 根据配对点的像素位置，求出 E 或者 F ；
• 2. 根据 E 或者 F ，求出 R, t 。由于 E 和 F 只相差了相机
视觉 SLAM
蔺志强 苏 敬
• SLAM 是 Simultaneous Lo“同时定位与地图构
建”。它是指搭载特定传感器的主体，在没有环 境先验信息的情况下，于运动过程中建立环境的 模型，同时估计自己的运动。如果这里的传感器 主要为相机，那就称为“视觉 SLAM ”。
和地图。
核心问题是如何计算图像间的相似性
• 最简单的方式就是对任意两张图像都做一遍特征匹配，根据正确匹配的数量确定哪两个图像存在关联。但 计算量大，效率低下。 • 基于里程计的几何关系（ Odometry based ）， • 基于外观（ Appearanc e based ）仅根据两张图像的相似性确定回环检测关系，这种做法摆脱了累计误差， 使回环检测模块成为 SLAM 系统中一个相对独立的模块。
第七章--刚体的基本运动
ω1 α1
O2
ω2
某瞬时主动轮Ⅰ的角速度为ω1 ， 角加速度为α1，试求该瞬时从动
α2
轮Ⅱ 的角速度ω2和角加速度α2 ， 本例ω1、ω2、α1、α2都代表绝对
值。
第七章 刚体的基本运动
应用运动学
例题3
Ⅰ
a2ta1tv2v1
r1O1
Ⅱ
r2 M2M1
ω1 α1
O2
ω2
α2
解： 两齿轮节圆相切并无相对滑 动，故两轮啮合点M1与M2恒有 相同的速度与切向加速度。即
应用运动学
角速度矢量
角加速d度矢量(angular acceleration vector)
dt
加速度矢积
at R r
因此:
an
at 2
R
r
v
因此: an v
数z2。
Ⅴ DⅣ
ⅠⅡ
n1
Ⅲ
第七章 刚体的基本运动
应用运动学
思考题
解: 对于直接啮合的齿轮或用齿条联动的一对齿轮，转
速与齿数成反比。以 n 表示转速，则：
n2 n3 , n4 n5 ,
n2 z1 , n4 z3 n1 z2 n3 z4
Ⅴ DⅣ
因而
n4 n1
z1 z2
z3 z4
轮Ⅳ的转速为
应用运动学
例题2
vM
at
aM
M
O an
αω
A vA
aA
物体A作直线平移，轮缘上M点
随滑轮作圆周运动，由于细绳不能
伸长，物体A与M点的速度大小相等，
A的加速度与M点切向加速度的大小
也相等，于是有
vA vM aA at
刚体课件
K G G a与β , ω 的关系：
v G 2 an = an = ω v = ω r = r
2
G ω⊥v
7
G
§3-2 刚体的角动量、转动惯量
(Angular Momentum of a Rigid Body , Moment of Inertia)
一、刚体定轴转动的角动量 质元∆mi相对于转轴的角动量：
O′
O
当转轴固定不动时，刚体的转动称定轴转动。
应用程序
刚体的上任一点的位移总可以表示为一个随质心 的平动加上绕质心的转动。 刚体定轴转动的特征 1. 各质点都在垂直于转轴的平面内作圆周运动。
4
2. 各质点作圆周运动的半径在相同的时间内转过的 角度相同。 ◎所有质点都具有相同的角位 移、角速度、角加速度。 三、刚体定轴转动的描述 ★用角量描述最为方便。 角速度矢量 ω
ω⊥
N X
Y Z O L
∫ ωdω = ∫
0 0
π /2
3g cos θ d θ 2L
π /2
0
α
θ
mg
1 2 3g ω ⊥ = sin θ 2 2L
10
例：求质量为m,长为L的均匀细棒对下面三个转 轴的转动惯量： c转轴过棒中心O并与棒垂直； d转轴过棒的一端B并与棒垂直； e转轴过棒上距质心O为h的点A并与棒垂直。 解：将棒分割成许多质元dm.
I = dI = r dm =
B A h
∫
∫
2
∫
L
x2 x1
dm = λ dx
r 2 λ dx
λ = m/ L
+
β K T2 T1 ' m3 g K a1 T2 '
【视觉SLAM十四讲课后作业答案】第三讲：李群与李代数
第一节课习题屋卡2017年12月19日一、验证向量叉乘的李代数性质我们说向量和叉乘运算构成了李代数，现在请你验证它。
书中对李代数的定义为：李代数由⼀个集合V，⼀个数域 F 和⼀个⼀元运算[,] 组成。
如果它们满⼀以下⼀条性质，称(V,F,[,]) 为⼀个李代数，记作g二、推导SE(3) 的指数映射课上给出了SO(3) 的指数映射推导，但对于SE(3)，仅介绍了结论，没有给出详细推导。
请你完成SE(3) 指数映射部分，有关左雅可⼀的详细推导。
三、伴随在SO(3) 和SE(3) 上，有⼀个东西称为伴随（Adjoint）。
下⼀请你证明SO(3) 伴随的性质。
对于SO(3)，有：Rexp(p∧)RT = exp((Rp)∧).此时称Ad(R) = R。
解：四、轨迹的描绘我们通常会记录机器⼀的运动轨迹，来观察它的运动是否符合预期。
⼀部分数据集都会提供标准轨迹以供参考，如kitti、TUM-RGBD 等。
这些⼀件会有各⼀的格式，但⼀先你要理解它的内容。
记世界坐标系为W，机器⼀坐标系为C，那么机器⼀的运动可以⼀TWC 或TCW 来描述。
现在，我们希望画出机器⼀在世界当中的运动轨迹，请回答以下问题：1.事实上，TWC 的平移部分即构成了机器⼀的轨迹。
它的物理意义是什么？为何画出TWC 的平移部分就得到了机器⼀的轨迹？解：wc T 的平移部分构成机器人的轨迹，它的物理意义是机器人在现实三维空间中的坐标点。
wc T 包含平移和旋转部分 ，平移指的是机器人在空间中的运动位置，而旋转部分是指机器人在每个位置的方向状态。
2. 我为你准备了⼀个轨迹⼀件（code/trajectory.txt ）。
该⼀件的每⼀⼀由若⼀个数据组成，格式为[t,tx,ty,tz,qx,qy,qz,qw],其中 t 为时间，tx,ty,tz 为 TWC 的平移部分，qx,qy,qz,qw 是四元数表⼀的 TWC 的旋转部分，qw 为四元数实部。
同时，我为你提供了画图程序 draw_trajectory.cpp ⼀件。
移动机器人SLAM技术【ch07】视觉SLAM 教学课件
0 2 对极几何
假设两张图像中得到了一对配对好的特征点，对极几何如图7.8所示，若能够得到多 个匹配好的点，则可实现运动轨迹的推测。
如果没有特征点匹配，我们就没法确定p?到底在极线的哪个位置了。那时， 就必须在极线上搜索以获得正确的匹配。
0 2 对极几何
现在，我们从代数角度来看一下这里出现的几何关系。在第一帧的坐标系下，设P的空间位置为 两个像素点p、p?的像素位置为
除了使用线性方法，我们还可以把PnP问题构建成一个定义于李代数上的非线性最 小二乘问题。前面说的线性方法，往往要先求相机位姿，再求空间点位置，而非 线性优化则把它们都看成优化变量，放在一起优化。
02 3D-2D·PnP
考虑n个三维空间点P和它们的投影p,我们希望计算相机的位姿R、t,其李代数表 示为。假设某空间点的坐标为F=[X,,Y,Z,],其投影的像素坐标为μ=[x,,y]],那么 可得像素位置与空间点位置的关系如下：
02
PART ONE
视觉前端——视觉里程计
Hale Waihona Puke 02视觉里程计 7.2.1视觉里程计的数学模型
视觉里程计关心相邻图像之间的相机运动，最简单的情况当然是两张图像 之间的运动 关系。在计算机视觉领域，人类在直觉上看来十分自然的事情，在计算机 视觉中却非常困 难。图像在计算机里只是一个数值矩阵，而在视觉SLAM中，我们只能看到 一个个像素， 知道它们是某些空间点在相机的成像平面上投影的结果。所以，为了定量 地估计相机运动， 必须在了解相机与空间点的几何关系之后进行。
第七章 视觉SLAM
高等院校公共课系列精品教材
大学生礼仪
01
PART ONE
视觉SLAM概述
01视觉SLAM的糖念与框架
-刚体的定轴转动新
刚体是一种理想模型 二、刚体的基本运动形式：平动、转动 .
1、平动：刚体中所有点的运动轨迹都完全相同， 或者说刚体内任意两点间的连线总是平行于它们的初 始位置间的连线 .
刚体的角动量（动量矩）
则
M
dL
dt
1、定义：刚体定轴转动时， L J
（质点 p mv ）
2、矢量 3、物理意义：描述刚体定轴转动状态的物理量
4、单位： kgm2 s1 （动量.米）
5、刚体上一点的角动量与刚体角动量的关系
1）质点的角动量
z
Li Ji miri2
TA
mA
TA
mA
a
a mAg TA mAa
mAg
TB mB g mBa
TAr TBr J a r
a mA mB g
mA mB J / r 2
例3 一根长为 l ，质量为 m 的均匀细直棒，可绕轴 O 在 竖直平面内转动，初始时它在水平位置
求 它由此下摆 角时的β和
转动定律
M J
刚体绕定轴转动时，刚体的角加速度与它所受合外 力矩成正比，与刚体的转动惯量成反比
说明
1、转动定律是解决刚体问题的基本定律
F ma
M J
2、定律中的各量都对同一转轴而言 3、瞬时性、矢量性
三 转动惯量
J mjrj2 , J r2dm j
物理意义：转动惯性的量度 . M J
M
r
F
定轴转动பைடு நூலகம்，M 只有二方向
视觉SLAM：从理论到实践李群与李代数
• 至此，说明了 SO(3) 与 so(3) 的对应关系。
3. 指数映射和对数映射
• se(3)到SE(3)的指数映射：
其中J为雅可比矩阵（留作习题）
3. 指数映射和对数映射
4. 李代数求导与扰动模型
4. 求导与扰动模型
• SLAM的定位即位姿估计
• 但李群无加法：
导数无从定义
• 解决办法：
• 同理，SE(3)亦有李代数se(3)：
• 上尖尖^ 不再是反对称矩阵，但仍保留记法：
李括号：
2. 李群与李代数
• 习题部分请你验证se(3)和so(3)的李代数性质 • 注：
• 不同书籍对se(3)的平前的。
• 把李代数理解成向量形式或矩阵形式都是可以的。向量形式更加自然一些。
• 直观写法（以左乘为例）
• 在李群上左乘小量时，李代数上的加法相差左雅可比的逆
• 反之
• 李代数上进行小量加法时，相当于李群上左（右）乘一个带左（右）雅可比的量
• SE(3)比SO(3)更复杂： 这里不展开花体雅可比
4. 求导与扰动模型
• 通过BCH线性近似，可以定义李代数上的导数 • 考虑一个基本问题：旋转后的点关于旋转的导数
2. 李群与李代数
• 李代数：与李群对应的一种结构，位于向量空间。
• 通常记作小写的so(3)和se(3)。书中以哥特体突出显示。 • 事实上是李群单位元处的正切空间。
• 下面从旋转矩阵引出李代数 • 考虑任意旋转矩阵R，满足
2. 李群与李代数
• 令R随时间变化（连续运动），有： • 两侧对时间求导：
2. 李群与李代数
• 该式说明，对任意t，都可以找到一个R和一个 ������ 的对应关系 • 该关系称为指数映射（Exponential Map） • 这里的 ������ 称为SO(3)对应的李代数：so(3) • 问题：
- 1、下载文档前请自行甄别文档内容的完整性，平台不提供额外的编辑、内容补充、找答案等附加服务。
- 2、"仅部分预览"的文档,不可在线预览部分如存在完整性等问题,可反馈申请退款(可完整预览的文档不适用该条件!)。
- 3、如文档侵犯您的权益，请联系客服反馈,我们会尽快为您处理(人工客服工作时间：9:00-18:30)。
从理论到实践
高翔 清华大学 2016年冬
2
第三讲 三维空间刚体运动
Chapter 3: 3D Space Rigid Body Motion
3 第三讲 三维空间刚体运动
• 本讲目标
• 理解三维空间的刚体运动描述方式：旋转矩阵、变换矩阵、四元数和欧拉角。 • 掌握Eigen库的矩阵、几何模块使用方法。
23 第三讲 三维空间刚体运动
• 由于万向锁，欧拉角不适于插值和迭代，往往只用于人机交互中。 • 可以证明，用三个实数来表达三维旋转时，会不可避免地碰到奇异性问题。 • SLAM 程序中很少直接使用欧拉角表达姿态
24
3.4 四元数
25 第三讲 三维空间刚体运动
• 四元数（Quaternion）
• 角轴与旋转矩阵的不同
• 旋转矩阵：九个量，有正交性约束 和行列式值约束
• 角轴：三个量，没有约束
• 注意它们只是表达方式的不同，但表 达的东西可以是同一个
• 角轴也就是第四章要介绍的李代数
• 转换关系
• 轴角转旋转矩阵：罗德里格斯 公式（Rodrigues's Formula ）
• 旋转矩阵转轴角
10 第三讲 三维空间刚体运动
• 旋转
• 设某坐标系
发生了一次旋转，变成了
• 对于某个固定的向量 （向量不随坐标系旋转），它的坐标怎么变化？
• 坐标关系：
11 第三讲 三维空间刚体运动
• 左乘
，得：
12 第三讲 三维空间刚体运动
• 中间的矩阵 称为旋转矩阵 • 根据定义可以验证：
• R是一个正交矩阵； • R的行列式为+1。
1. 绕物体的Z轴旋转，得到偏航角yaw； 2. 绕旋转之后的Y轴旋转，得到俯仰角pitch； 3. 绕旋转之后的X轴旋转，得到滚转角roll。
22 第三讲 三维空间刚体运动
• 万向锁（Gimbal Lock）
• ZYX顺序中，若Pitch为正负90度，则第三次旋转和第一次绕同一个轴，使得系统丢失 了一个自由度——存在奇异性问题
17
3.2 实践部分：EIGEN
18
3.3 旋转向量、欧拉角
19 第三讲 三维空间刚体运动
• 除了旋转矩阵之外的旋转表示 • 三维旋转：三自由度，用 向量表示
• 方向为旋转轴、长度为转过的角度
• 称为角轴（Angle-Axis）或旋转向量（Rotation Vector）
20 第三讲 三维空间刚体运动
• 满足这两个性质的矩阵称为旋转矩阵
• Special Orthogonal Group 特殊正交群
• 旋转矩阵描述了两个坐标的变换关系 • 比如： • 反之： • 于是：
• 进一步，三个坐标系亦有：
13 第三讲 三维空间刚体运动
• 加上平移： • 两个坐标系的刚体运动可以由 完全描述。
14 第三讲 三维空间刚体运动
• 在SLAM中：
• 固定的世界坐标系和移动的机器人坐标系 • 机器人坐标系随着机器人运动而改变，每个时刻都有新的坐标系
9 第三讲 三维空间刚体运动
• 两个不同的坐标系 • 如何描述左侧到右侧的变化？ • 直观看来由两个部分组成：
• 原点间的平移 • 三个轴的旋转
• 平移是一个向量 • 旋转是什么？
• 称为特殊欧氏群（Special Euclidean Group）
16 第三讲 三维空间刚体运动
• 类似的，可定义反向的变换：
• 例子 • 在SLAM中，通常定义世界坐标系
与机器人坐标系 • 一个点的世界坐标Байду номын сангаас ，机器人坐标
系下为 ，那么满足关系：
• 反之亦然 • 在实际编程中，可使用 或 来
描述机器人的位姿。
• 一种扩展的复数 • 回忆：（单位圆上的）复数可以表达二维平面的旋转
• 四元数有三个虚部，可以表达三维空间中的旋转：
• 虚部之间的关系：
自己和自己的运算像复数 自己和别人的运算像叉乘
26 第三讲 三维空间刚体运动
• 和复数一样，单位四元数可以表达三维空间的一次旋转 • 四元数的一些运算和性质：
加减法 乘法
4
3.1 点、向量和坐标系，旋转矩阵
5 第三讲 三维空间刚体运动
• 点存在于三维空间之中 • 点和点可以组成向量 • 点本身由原点指向它的向量所描述
• 向量
• 带指向性的箭头 • 可以进行加法、减法等运算
6 第三讲 三维空间刚体运动
• 定义坐标系后，向量可以由 坐标表示
• 坐标系：由三个正交的轴组成
共轭 模长
逆
乘法
数乘 点乘
27 第三讲 三维空间刚体运动
• 四元数和角轴的关系
• 角轴到四元数： • 四元数到角轴：
• 类似可知四元数亦可转换为旋转矩阵、欧拉角
28 第三讲 三维空间刚体运动
• 如何用四元数旋转一个空间点？
• 设点 经过一次以 表示的旋转后，得到了
1. 将 的坐标用四元数表示（虚四元数）： 2. 旋转之后的关系为：
，它们关系如何表示？
可以验证这货也是虚四元数
• 四元数相比于角轴、欧拉角的优势：紧凑、无奇异性
29
3.5 *相似、仿射、射影变换（略）
30
3.6 实践部分：EIGEN几何模块
31
3.7 可视化演示
• 角度：
• 轴：
21 第三讲 三维空间刚体运动
• 欧拉角（Euler Angles）
• 将旋转分解为三次不同轴上 的转动，以便理解
• 例如：按 Z-Y-X 顺序转动 • 轴可以是定轴或动轴，顺序
亦可不同，因此存在许多种 定义方式不同的欧拉角
• 常见的有 yaw-pitch-roll （偏航-俯仰-滚转）角等等。
• 齐次坐标与变换矩阵 • 用旋转+平移方式有一点不便之处，比如发生了两次变换： • 这时： • 叠加起来过于复杂
15 第三讲 三维空间刚体运动
• 改变形式，写成：
•记 • 那么多次变换就可写成：
• 这种用四个数表达三维向量的做法称 为齐次坐标
• 引入齐次坐标后，旋转和平移可以放 入同一个矩阵，称为变换矩阵
• 构成线性空间的一组基 • 左手系和右手系
7 第三讲 三维空间刚体运动
• 向量的运算可以由坐标运算来表达 • 加减法 • 内积 • 外积
这个定义 之后还要用
8 第三讲 三维空间刚体运动
• 目前为止都很平凡 • 但是有一个基本问题：
坐标系之间是如何变化的？
• 进而：
如何计算同一个向量在不同坐标系里的坐标？