关于罗德里格斯公式(Rodrigues‘sFormula)的详细推导过程

这篇具有很好参考价值的文章主要介绍了关于罗德里格斯公式(Rodrigues‘sFormula)的详细推导过程。希望对大家有所帮助。如果存在错误或未考虑完全的地方,请大家不吝赐教,您也可以点击"举报违法"按钮提交疑问。

1 旋转向量

实际上,任意旋转都可以用一个旋转轴和一个旋转角来刻画。我们可以使用一个向量 u u u,其方向与旋转轴一致,其长度等于旋转角 θ \theta θ,那么向量 θ u \theta u θu就可以描述这个旋转,这种向量称为旋转向量(或轴角/角轴,Axis-Angle),只需一个三维向量即可描述旋转。同样,对于一个变换矩阵,使用一个旋转向量和一个平移向量即可表达一次变换,此时变量维数正好是六维。

2 罗德里格斯公式

2.1 罗德里格斯公式定义

维基百科给出的罗德里格斯旋转公式(Rodrigues’ rotation formula)定义如下:
如果 v v v R 3 \mathbb{R}^3 R3中的向量,并且 k k k是描述旋转轴的单位向量,根据右手法则 v v v围绕该轴旋转角度 θ \theta θ,则旋转向量 v r o t v_{rot} vrot的罗德里格斯公式为 v r o t = v c o s θ + ( k × v ) s i n θ + k ( k ⋅ v ) ( 1 − c o s θ ) . v_{rot}=vcos\theta+(k \times v)sin \theta +k(k\cdot v)(1-cos\theta). vrot=vcosθ+(k×v)sinθ+k(kv)(1cosθ).视觉SLAM十四讲中给出的用罗德里格斯公式转换的结果为: R = c o s θ I + ( 1 − c o s θ ) n n T + s i n θ n ∧ R=cos\theta I+(1-cos\theta)nn^T+sin\theta n^{\land} R=cosθI+(1cosθ)nnT+sinθn其中, R R R表示一个旋转,单位长度的向量 n n n为一个假设的旋转轴,角度为 θ \theta θ,符号 ∧ ^\land 是向量到反对称矩阵的转换符,具体含义见文章:[视觉SLAM十四讲]学习笔记1-刚体运动之旋转矩阵与变换矩阵。
我们可以看出,原始的定义和书中给出的转换结果形似,这说明这两个公式有一定的关系,下一节将通过推导过程来展示这个关系的转换。

2.2 罗德里格斯公式推导

我们可以通过三维空间中的旋转和旋转分解来进行罗德里格斯公式的推导。
首先,我们给出一些定义:令 k k k为定义旋转轴的单位向量,令 v v v为围绕 k k k旋转角度 θ \theta θ的任意向量(遵循右手法则,图中为逆时针方向)。
在图中,向量 v v v围绕 k k k旋转 θ \theta θ度得到向量 v r o t v_{rot} vrot,将向量 v v v分解为平行于旋转轴 k k k的向量 v ∣ ∣ v_{||} v∣∣和正交于(垂直)旋转轴 k k k的向量 v ⊥ v_{⊥} v v = v ∣ ∣ + v ⊥ v=v_{||}+v_{⊥} v=v∣∣+v
简单来说,根据2.1中的转换结果和下图可知, v r o t = R v v_{rot}=Rv vrot=Rv,为了求解出转换关系即 R R R我们只需要找到 v r o t v_{rot} vrot的表达式即可。
罗德里格斯公式推导,视觉SLAM学习,线性代数,人工智能,矩阵
根据下图,我们可以看出,向量 v r o t v_{rot} vrot同样被分解成了两个向量 v ∣ ∣ r o t v_{||rot} v∣∣rot v ⊥ r o t v_{⊥rot} vrot,但这幅图不便于我们进行公式的推导和观察,我们可以换一种分解方式,类似于第一个图,这时向量 v ∣ ∣ v_{||} v∣∣和向量 v ∣ ∣ r o t v_{||rot} v∣∣rot将平行(重合)于旋转轴 k k k
罗德里格斯公式推导,视觉SLAM学习,线性代数,人工智能,矩阵
1. v ∣ ∣ v_{||} v∣∣的旋转及 v r o t ∣ ∣ v_{rot||} vrot∣∣的表示
首先我们看一下向量 v v v的分解示意图(图中 u u u为旋转轴):
罗德里格斯公式推导,视觉SLAM学习,线性代数,人工智能,矩阵
通过上图,我们可以看到, v ∣ ∣ v_{||} v∣∣其实就是 v v v u u u上的正交投影(Orthogonal Projection),根据正交投影的公式,我们可以得出: v ∣ ∣ = p r o j u ( v ) = u ⋅ v u ⋅ u u = u ⋅ v ∥ u ∥ 2 u = ( u ⋅ v ) u v_{||}=proj_{u}(v)=\frac{u\cdot v}{u \cdot u}u=\frac{u\cdot v}{\Vert u \Vert^2}u=(u\cdot v)u v∣∣=proju(v)=uuuvu=u2uvu=(uv)u这里, ∥ u ∥ 2 = u ⋅ u , ∥ u ∥ = 1 \Vert u \Vert^2=u \cdot u,\Vert u \Vert=1 u2=uuu=1,从而由 v = v ∣ ∣ + v ⊥ v=v_{||}+v_{⊥} v=v∣∣+v,我们可以得到 v ⊥ = v − v ∣ ∣ = v − ( u ⋅ v ) u v_{⊥}=v-v_{||}=v-(u\cdot v)u v=vv∣∣=v(uv)u,,所以 v ⊥ = v − v ∣ ∣ = v − ( u ⋅ v ) u v_{⊥}=v-v_{||}=v-(u\cdot v)u v=vv∣∣=v(uv)u
从之前的图中我们可以看到, v ∣ ∣ v_{||} v∣∣其实没有被旋转,仍然与旋转轴 u u u重合,所以: v ∣ ∣ = v ∣ ∣ r o t v_{||}=v_{||rot} v∣∣=v∣∣rot
2. v ⊥ v_{⊥} v的旋转及 v ⊥ r o t v_{⊥rot} vrot的表示
通过之前的图,我们可以看出因向量 v ⊥ v_{⊥} v正交于向量 u u u,这个旋转可以看做是平面内的一个旋转。因为旋转不改变 v ⊥ v_{⊥} v的长度,所以路径是一个圆。通过下图我们可以清晰的看到旋转的情况(第一个图为旋转情况,第二个图为旋转情况的俯视图):
罗德里格斯公式推导,视觉SLAM学习,线性代数,人工智能,矩阵
有了旋转情况的俯视图,我们就可以通过平面上的旋转来分析三维旋转的情况了,为了表示旋转,我们可以构造一个同时正交于 u u u v ⊥ v_{⊥} v的向量 w w w,通过外积(叉乘),我们可以表示出来 w = u × v ⊥ w=u \times v_{⊥} w=u×v,由于我们一直使用的是右手坐标系统,所以在表示时要注意叉乘顺序。按照右手定则,新定义的向量 w w w指向的是 v ⊥ v_{⊥} v逆时针旋转 π / 2 \pi /2 π/2后的方向,同时和 v ⊥ v_{⊥} v一样位于正交于 u u u的平面内,由 ∥ u ∥ = 1 \Vert u \Vert=1 u=1,我们可以得到以下关系: ∥ w ∥ = ∥ u × v ⊥ ∥ = ∥ u ∥ ⋅ ∥ v ⊥ ∥ ⋅ s i n ( π / 2 ) = ∥ v ⊥ ∥ . \Vert w \Vert=\Vert u \times v_{⊥}\Vert=\Vert u\Vert \cdot \Vert v_{⊥}\Vert \cdot sin(\pi/2)=\Vert v_{⊥}\Vert. w=u×v=uvsin(π/2)=v∥.这里 π / 2 \pi/2 π/2 u u u v ⊥ v_{⊥} v的夹角,同时我们可以看出, w w w v ⊥ v_{⊥} v具有相同的模长,即 w w w也位于这个圆上, w w w v ⊥ v_{⊥} v即成为了一个平面上的两个坐标轴。这样,我们就可以把 v ⊥ r o t v_{⊥rot} vrot投影到 w w w v ⊥ v_{⊥} v上,我们可以将 v ⊥ r o t v_{⊥rot} vrot分解为 v r w v_{rw} vrw v r v v_{rv} vrv,通过三角函数的一些知识,我们可以得到以下关系: v ⊥ r o t = v r v + v r w = c o s ( θ ) v ⊥ + s i n ( θ ) w = c o s ( θ ) v ⊥ + s i n ( θ ) ( u × v ⊥ ) v_{⊥rot}=v_{rv}+v_{rw}=cos(\theta)v_{⊥}+sin(\theta)w=cos(\theta)v_{⊥}+sin(\theta)(u \times v_{⊥}) vrot=vrv+vrw=cos(θ)v+sin(θ)w=cos(θ)v+sin(θ)(u×v)
3. v r o t v_{rot} vrot的表示
通过1和2得到的结果,我们可以得到 v r o t v_{rot} vrot的表达式为: v r o t = v r o t ∣ ∣ + v r o t ⊥ = v ∣ ∣ + c o s ( θ ) v ⊥ + s i n ( θ ) ( u × v ⊥ ) v_{rot}=v_{rot||}+v_{rot⊥}=v_{||}+cos(\theta)v_{⊥}+sin(\theta)(u \times v_{⊥}) vrot=vrot∣∣+vrot=v∣∣+cos(θ)v+sin(θ)(u×v)对于 u × v ⊥ u \times v_{⊥} u×v,我们可以采用分配律进行变形 u × v ⊥ = u × ( v − v ∣ ∣ ) = u × v − u × v ∣ ∣ = u × v u \times v_{⊥}=u \times (v-v_{||})=u \times v-u \times v_{||}=u \times v u×v=u×(vv∣∣)=u×vu×v∣∣=u×v这里 u u u v ∣ ∣ v_{||} v∣∣平行,所以 u × v ∣ ∣ = 0 u \times v_{||}=0 u×v∣∣=0
最后代入 v ∣ ∣ = ( u ⋅ v ) u v_{||}=(u \cdot v)u v∣∣=(uv)u v ⊥ = v − ( u ⋅ v ) u v_{⊥}=v-(u \cdot v)u v=v(uv)u,得到下面的结果 v r o t = ( u ⋅ v ) u + c o s ( θ ) ( v − ( u ⋅ v ) u ) + s i n ( θ ) ( u × v ) v_{rot}=(u \cdot v)u+cos(\theta)(v-(u \cdot v)u)+sin(\theta)(u \times v) vrot=(uv)u+cos(θ)(v(uv)u)+sin(θ)(u×v) = v c o s ( θ ) + ( u × v ) s i n ( θ ) + ( u ⋅ v ) u ( 1 − c o s ( θ ) ) =vcos(\theta)+(u \times v)sin(\theta)+(u \cdot v)u(1-cos(\theta)) =vcos(θ)+(u×v)sin(θ)+(uv)u(1cos(θ))这样,我们就得到了3D空间中任意一个向量 v v v沿着单位向量(旋转轴) u u u旋转 θ \theta θ角度之后的 v r o t v_{rot} vrot
到这里,我们推导出了罗德里格斯公式的基本形式,但显然,书中给出的旋转 R R R是矩阵形式,接下来我们要将内积和外积的运算进行变形,用矩阵形式表示。
对于 ( u ⋅ v ) u (u \cdot v)u (uv)u,通过内积的展开规则 x ⋅ y = [ x , y ] = x T y x \cdot y=[x,y]=x^Ty xy=[x,y]=xTy及交换律可以得到 ( u ⋅ v ) u = ( u ⋅ u ) v = ( u ⋅ u T ) v = u u T v (u \cdot v)u=(u \cdot u)v=(u \cdot u^T)v=uu^Tv (uv)u=(uu)v=(uuT)v=uuTv同时, u × v u \times v u×v可以通过之前在:[视觉SLAM十四讲]学习笔记1-刚体运动之旋转矩阵与变换矩阵中介绍的叉乘矩阵的形式,即为 u × v = [ ( u × v ) x ( u × v ) y ( u × v ) z ] = [ u y v z − u z v y u z v x − u x v z u x v y − u y v x ] = [ 0 − k z k y k z 0 − k x − k y k x 0 ] [ v x v y v z ] = U v u \times v=\begin{bmatrix}(u \times v)_x \\ (u \times v)_y \\ (u \times v)_z \end{bmatrix}=\begin{bmatrix}u_yv_z-u_zv_y \\u_zv_x-u_xv_z \\u_xv_y-u_yv_x \end{bmatrix}=\begin{bmatrix}0&-k_z&k_y \\ k_z&0&-k_x \\ -k_y&k_x&0\end{bmatrix}\begin{bmatrix}v_x \\ v_y \\ v_z \end{bmatrix}=Uv u×v= (u×v)x(u×v)y(u×v)z = uyvzuzvyuzvxuxvzuxvyuyvx = 0kzkykz0kxkykx0 vxvyvz =Uv这里 U = u ∧ = [ 0 − k z k y k z 0 − k x − k y k x 0 ] U=u^{\land}=\begin{bmatrix}0&-k_z&k_y \\ k_z&0&-k_x \\ -k_y&k_x&0\end{bmatrix} U=u= 0kzkykz0kxkykx0 将转换后的 ( u ⋅ v ) u (u \cdot v)u (uv)u u × v u \times v u×v矩阵形式带入推导的结果,变形结果如下: v r o t = v c o s ( θ ) + ( u × v ) s i n ( θ ) + ( u ⋅ v ) u ( 1 − c o s ( θ ) ) v_{rot}=vcos(\theta)+(u \times v)sin(\theta)+(u \cdot v)u(1-cos(\theta)) vrot=vcos(θ)+(u×v)sin(θ)+(uv)u(1cos(θ)) = v c o s ( θ ) + ( 1 − c o s ( θ ) ) u u T v + s i n ( θ ) u ∧ v =vcos(\theta)+(1-cos(\theta))uu^Tv+sin(\theta)u^{\land}v =vcos(θ)+(1cos(θ))uuTv+sin(θ)uv = ( c o s ( θ ) I + ( 1 − c o s ( θ ) ) u u T + s i n ( θ ) u ∧ ) v =(cos(\theta)I+(1-cos(\theta))uu^T+sin(\theta)u^{\land})v =(cos(θ)I+(1cos(θ))uuT+sin(θ)u)v则旋转矩阵为 R = c o s ( θ ) I + ( 1 − c o s ( θ ) ) u u T + s i n ( θ ) u ∧ R=cos(\theta)I+(1-cos(\theta))uu^T+sin(\theta)u^{\land} R=cos(θ)I+(1cos(θ))uuT+sin(θ)u I I I为单位矩阵,证毕。

3 旋转矩阵到旋转向量的转换

有了上节的推导和转换结果,我们可以在转换公式的基础上,进行旋转矩阵到旋转向量的转换。
首先对转换公式两边取迹,得 t r ( R ) = c o s ( θ ) t r ( I ) + ( 1 − c o s ( θ ) ) t r ( u u T ) + s i n ( θ ) t r ( u ∧ ) tr(R)=cos(\theta)tr(I)+(1-cos(\theta))tr(uu^T)+sin(\theta)tr(u^{\land}) tr(R)=cos(θ)tr(I)+(1cos(θ))tr(uuT)+sin(θ)tr(u) = 3 c o s ( θ ) + ( 1 − c o s ( θ ) =3cos(\theta)+(1-cos(\theta) =3cos(θ)+(1cos(θ) = 1 + 2 c o s ( θ ) =1+2cos(\theta) =1+2cos(θ)因此: θ = a r c c o s t r ( R ) − 1 2 \theta =arccos\frac{tr(R)-1}{2} θ=arccos2tr(R)1对于旋转轴 u u u和之前的图及结果,我们可以知道,旋转轴上的向量在旋转后不会发生改变,说明: R u = u Ru=u Ru=u显然,旋转轴 u u u是旋转矩阵 R R R特征值 I I I对应的特征向量。求解这个方程,进行归一化,就可以得到旋转轴。


创作不易,希望大家支持,多多点赞收藏!!!!非常感谢!!!!

参考资料:
视觉SLAM十四讲:从理论到实践(第2版)(ISBN:9787121369421)
维基百科-罗德里格斯的旋转公式
四元数与三维旋转文章来源地址https://www.toymoban.com/news/detail-596490.html

到了这里,关于关于罗德里格斯公式(Rodrigues‘sFormula)的详细推导过程的文章就介绍完了。如果您还想了解更多内容,请在右上角搜索TOY模板网以前的文章或继续浏览下面的相关文章,希望大家以后多多支持TOY模板网!

本文来自互联网用户投稿,该文观点仅代表作者本人,不代表本站立场。本站仅提供信息存储空间服务,不拥有所有权,不承担相关法律责任。如若转载,请注明出处: 如若内容造成侵权/违法违规/事实不符,请点击违法举报进行投诉反馈,一经查实,立即删除!

领支付宝红包 赞助服务器费用

相关文章

  • 紫光展锐联合罗德与施瓦茨在MWC上海共同展示RedCap测试方案

    在6月28日-30日举办的2023上海世界移动大会上(MWC上海),紫光展锐联合罗德与施瓦茨共同演示RedCap射频与吞吐量测试,确保RedCap终端功能和性能达到预期。在本次联合测试中,RedCap下行吞吐量可达220Mbps,上行吞吐量可达116Mbps,充分验证了紫光展锐RedCap芯片平台V517的能力。使

    2024年02月12日
    浏览(39)
  • 矩阵公式(转置公式+求导公式)

    矩阵-转置公式 ( m A ) T = m A T (mA)^T=mA^T ( m A ) T = m A T ( A + B ) T = A T + B T (A+B)^T=A^T+B^T ( A + B ) T = A T + B T ( A B ) T = B T A T (AB)^T=B^TA^T ( A B ) T = B T A T ( A T ) T = A (A^T)^T=A ( A T ) T = A 矩阵-求导公式 注:对X求偏导 ( X T ) ′ = I (X^T)\\\'=I ( X T ) ′ = I ( A X T ) ′ = A (AX^T)\\\'=A ( A X T ) ′ = A ( X T A )

    2024年02月13日
    浏览(36)
  • VBA【公式】依据 “=”将多行矩阵公式变成单行公式

    软件OCR生成的公式,有时候公式糊成一块了。 将糊成一块的多行公式一行行分开(并且只对糊的公式操作),操作完后标记颜色为红色 1. 属性:ActiveDocument.OMaths.ParentFunction ActiveDocument.OMaths的 ParentFunction 如果公式为 多行矩阵 ActiveDocument.OMaths.ParentFunction 返回对象 如果公式为

    2024年04月09日
    浏览(55)
  • 全概率公式和贝叶斯公式

    一、完备事件组 设E是随机试验,Ω是相应的样本空间,A1,A2,...,An为Ω的一个事件组, 若两两事件互斥且所有事件的并集为全集,则称A1A2...An为样本空间的一个完备事件组。 二、条件概率 设A,B是两个事件,且P(B)0,则在事件B发生的条件下,事件A发生的条件概率: P(A|B)=P(

    2024年02月12日
    浏览(38)
  • 逻辑代数的基本公式和常用公式

    1、基本公式 0-1律 A*0 = 0 A+1 = 1 自等律 A*1 = 1 A+1 = A 重叠率 A*A = A A+A = A 互补律 A* = 0 A+ = 1 交换律 A*B = B*A A+B = B+A 结合律 A*(B*C) = (A*B)*C A+(B+C) = (A+B)+C 分配律 A*(B+C) = AB+AC A+B*C = (A+B)(A+C) 吸收律 A(A+B) = A A+AB = A 反演律  = +  = * 双重否定律    2、常用公式 公式一 AB+A = A 公式二

    2024年02月12日
    浏览(33)
  • word中使用latex多行公式,矩阵公式

    的引起的那条竖线可以通过backspace或者delete删除

    2024年01月21日
    浏览(34)
  • word修改公式默认字体并打出漂亮公式

    word公式简介 word 自带的公式字体 Cambria Math 不可否认很丑,要打出 latex 格式的漂亮字体很困难。 使用 Markdown 工具很多只能导出为不能修改的pdf格式,并且学习成本很高。 传统方法1——mathtype 个人使用体验不佳,安装较为复杂,使用界面很不好看很拥堵,持续点击相对来说

    2024年02月07日
    浏览(42)
  • 玩转LaTeX(三)【数学公式(基础)、​矩阵、多行公式】

    数学公式基础 导言区(引包) 正文区 矩阵: 导言区:(自命令) 正文区: 多行公式 导言区(导包): 正文区

    2024年02月04日
    浏览(42)
  • 通达信组合副图指标公式-通达信公式

    A:=REF((CO),2); A1:=REF((CO),1); A2:=CO OR C=O; A3:=REF(O,1)REF(C,2); A4:=OREF(C,1); A5:=REF((C-O),1)REF((C-O),2); A6:=(C-O)REF((C-O),1); A7:=REF(C,2)REF(O,7)*1.05; 节奏1:A AND A1 AND A2 AND A3 AND A4 AND A5 AND A6 AND A7; 节奏2:A AND A1 AND A2 AND A3 AND A5 AND A6 AND A7; Q:=REF((CO),2); Q1:=REF((CO),1); Q2:=CO; Q3:=REF((O-C),1)REF((O-C),2); Q4:=(O-C)RE

    2024年02月16日
    浏览(44)
  • word将mathtype公式批量转为latex公式

    最近,由于工作学习需要,要将word里面的mathype公式转为latex公式。 查了查资料,有alt+的操作,这样太慢了。通过下面链接的操作,结合起来可以解决问题。 某乎:https://www.zhihu.com/question/532353646 csdn:https://blog.csdn.net/m0_52232795/article/details/130617795 某乎比较详细,但是配合c

    2024年02月13日
    浏览(44)

觉得文章有用就打赏一下文章作者

支付宝扫一扫打赏

博客赞助

微信扫一扫打赏

请作者喝杯咖啡吧~博客赞助

支付宝扫一扫领取红包,优惠每天领

二维码1

领取红包

二维码2

领红包