高翔视觉slam十四讲书籍笔记(第三讲04-罗德里格斯公式推导)

罗德里格斯(Rodrigues)旋转方程

此公式有2种形式,故而也有2种推导方法。这里只推导书籍里面的这个公式。
书籍里的条件如下:假设有一个旋转轴为n, 角度为 θ 旋转,显然,它对应的旋转向量为θn

罗德里格斯公式推导

假设V在三维空间中,绕单位旋转轴 k ,旋转 θ ,到Vrot
在这里插入图片描述

从上图可以看出,对于向量 V 可以分出两个分向量:

V = V + V

其中 V 平行于旋转轴 k ,由点乘的投影几何意义可得(V•k为标量,所以再乘k得到一个矢量):

V = (V · k)k

**V**垂直于旋转轴 k,根据向量减法可得:

V = V - V = V - ( k · V) k = - k × ( k × V)

V - (k · V)k = -k × (k × V)不知道是什么怎么实现变换的,但是从图上可以看出来,二者相等

在这里我们将两个分向量带进去得到:

V = ( kV) kk × ( k × V)

向量k×V可以看作 Vk 反向旋转90°。
看一下向量叉乘的几何意义:

在这里插入图片描述

叉乘公式和点乘公式转换:

a × ( b × c) = ( ac) b − ( ab) c

旋转之后的 Vrot 为分向量为 V‖rot

V ‖rot = V
| V ⊥rot| = | V|
V~⊥rot ~= b + a = cos θV + sin θk x V

a和b是由Vrot⊥正交分解得到的矢量,既有大小又有方向,所以在求解时,我们要对其大小和方向分别求解。 一、b的求解
1.大小 由图得:|b| = cosθ’ |V⊥rot| ( θ’为bV⊥rot的夹角 ) 又因为|V⊥rot| = |V| ,θ’= π - θ 所以:|b| = cos(π -
θ) |V| = (- cosθ)|V|

2.方向 由 b 的方向与 V 方向相反可得
b的单位方向向量为:-V / |V| 综上可得:b= (-V / |V| ) |b| = cosθV

二、a的求解
1.大小 |a| = sinθ’ |V⊥rot| = sinθ’ |V| = sin(π - θ) |V| = sinθ|V|
= sinθsinθ’’|V| (θ’‘为Vk的夹角)
= sinθsinθ’’|V| |K| (因为 |K|=1,所以可直接加上) 其中sinθ’’|V| |K| = |K x V| 所以:|a| = sinθ
|K x V|

2.方向 由叉乘方向可得 a 的单位方向向量为
K x V / |K x V| 综上可得:a = (K x V / |K x V|) sinθ |K x V| = sinθ K x V

其中:

k × V = k × (V − V) = k × V − k × V = k × V

因此:

V⊥rot = cosθV + sinθk × V

旋转后的向量 Vrot为:

Vrot = V∥rot + V⊥rot
Vrot = V∥ + cosθv + sinθk × V
=V∥ + cosθ(V−V) + sinθk × V
=cosθV + (1−cosθ)V + sinθk × V
=cosθV + (1−cosθ)(k⋅V)k + sinθk × V

其中:

(k⋅V)k = k(k⋅V) = k(kTV) = (kkT)V

所以(I 为大写的 i):

Vrot = (cosθI + (1 - cosθ)kkT + sinθk^)V

注意,如果在

Vrot = V + cosθV + sinθk x V

中,将V替换为 V - V,再将V替换为 -k x (k x V) 则:

在这里插入图片描述

在这里插入图片描述

已标记关键词 清除标记
相关推荐
©️2020 CSDN 皮肤主题: 大白 设计师:CSDN官方博客 返回首页