matlab 罗德里格斯变换,修正罗德里格斯参数
修正罗德里格斯参数(modified Rodrigues parameters)又称为MRP。它是描述两坐标系之间方向关系的一种方法,且由欧拉四元数衍生定义:
\[{\sigma _i} = \frac{{{\beta _i}}}{{1 + {\beta _0}}},i = 1,2,3\]
三个参数组成的向量即为修正罗德里格斯参数$\hat \sigma$:
\[{\left[ {\sigma_1,{\sigma_2},{\sigma_3}} \right]^\prime } = \hat \sigma\]
性质
与主旋转矢量的关系
设主旋转矢量中主轴为$\hat e$,主角度为$\Phi$,则修正罗德里格斯参数$\hat q$可由两者导出:
\[\hat \sigma = \hat e\tan \frac{\Phi }{4}\]
在主角度$\Phi$小于$\pi$时,可认为:
\[\hat \sigma = \hat e\frac{\Phi }{4}\]
下图显示了主角度$\Phi
与欧拉四元数的关系
由定义式:
\[{\sigma _i} = \frac{{{\beta _i}}}{{1 + {\beta _0}}},i = 1,2,3\]
此外,反向关系为:
\[{\beta _0} = \frac{{1 - {{\hat \sigma }^T}\hat \sigma }}{{1 + {{\hat \sigma }^T}\hat \sigma }}\]
\[{\beta _i} = \frac{{2{\sigma _i}}}{{1 + {{\hat \sigma }^T}\hat \sigma }}\]
与经典罗德里格斯参数的关系
设经典罗德里格斯参数为$\hat q$,则有转化关系:
\[\hat q = \frac{2\hat \sigma }{1 - {\hat \sigma }^T \hat \sigma }\]
\[\hat \sigma = \frac{\hat q}{{1 + \sqrt {1 + {{\hat q}^T}\hat q} }}\]
与方向余弦矩阵的关系
方向余弦矩阵用修正罗德里格斯参数表示为:
\[\left[ C \right] = \left[ I \right]{\rm{ + }}\frac{{{\rm{8[}}\tilde \sigma {{\rm{]}}^2} - 4(1 - {{\hat \sigma }^T}\hat \sigma ){\rm{[}}\tilde \sigma {\rm{]}}}}{{{{(1 + {{\hat \sigma }^T}\hat \sigma )}^2}}}\]
其中$\left[ I \right]$为单位阵。展开式不能写成简单形式,故不在此列出。
且有性质:
\[{\left[ {C(\hat \sigma )} \right]^{ - 1}} = {\left[ {C(\hat \sigma )} \right]^T} = \left[ {C( - \hat \sigma )} \right]\]
在已知方向余弦矩阵的情况下求修正罗德里格斯参数,则首先定义中间参量$\xi$:
\[\xi = \sqrt {trace\left[ C \right] + 1} =2\beta_0\]
其中$\beta_0$为第一个欧拉四元数参数,$trace\left[ C \right]$为矩阵$\left[ C \right]$的迹。
进一步,有:
\[\hat \sigma = \frac{1}{\xi (\xi + 2)}\left( {\begin{array}{*{20}{c}}
{C_{23} - {C_{32}}}\\
{C_{31} - {C_{13}}}\\
{C_{12} - {C_{21}}}
\end{array}} \right)\]
$\hat \sigma$的叉乘矩阵$\left[ {\tilde \sigma} \right]$也可写为方向余弦矩阵的简单组合:
\[\left[ {\tilde \sigma} \right] = \frac{\left[ C \right]^T - \left[ C \right]}{\xi(+2)}\]
$\left[ {\tilde \sigma} \right]$也与凯莱变换有密切关系。
与角速度的关系
利用欧拉四元数的微分方程和四元数与修正罗德里格斯参数的关系,并经较复杂的推导与化简,最终可得较为简单的修正罗德里格斯参数对时间的微分与修正罗德里格斯参数以及角速度之间的关系:
\[\dot \sigma = \frac{1}{4}[(1 - {{\hat \sigma }^2})[I] + 2[\tilde \sigma ] + 2\hat \sigma {{\hat \sigma }^T}]\hat \omega = \frac{1}{4}[B(\hat \sigma )]\hat \omega \]
或者写为展开形式:
\[\dot \sigma = \frac{1}{4}\left[ {\begin{array}{*{20}{c}}
{1 - {\sigma ^2} + 2\sigma _1^2}&{2({\sigma _1}{\sigma _2} - {\sigma _3})}&{2({\sigma _1}{\sigma _3} + {\sigma _2})}\\
{2({\sigma _2}{\sigma _1} + {\sigma _3})}&{1 - {\sigma ^2} + 2\sigma _2^2}&{2({\sigma _2}{\sigma _3} - {\sigma _1})}\\
{2({\sigma _3}{\sigma _1} - {\sigma _2})}&{2({\sigma _3}{\sigma _2} + {\sigma _1})}&{1 - {\sigma ^2} + 2\sigma _3^2}
\end{array}} \right]\left[ {\begin{array}{*{20}{c}}
{\omega _1}\\
{\omega _2}\\
{\omega _3}
\end{array}} \right]\]
特别地,当主角度$\Phi$为0时,$[B(\hat \sigma )]$为单位阵。
反向转换,有:
\[\omega {\rm{ = 4}}{[B(\hat \sigma )]^{\rm{ - 1}}}\dot \sigma \]
$[B(\hat \sigma )]$是近似正交的,即:
\[{[B(\hat \sigma )]^{\rm{ - 1}}}{\rm{ = }}\frac{\rm{1}}{{{{(1 + {\sigma ^2})}^2}}}{[B(\hat \sigma )]^T}\]
从而有:
\[\hat \omega = \frac{4}{{{{(1 + {\sigma ^2})}^2}}}[(1 - {{\hat \sigma }^2})[I] + 2[\tilde \sigma ] + 2\hat \sigma {{\hat \sigma }^T}]\dot \sigma \]
奇点
由修正罗德里格斯参数定义,当$\beta _0=-1$时定义式分母为0,此时出现奇点。$\beta _0=-1$对应于主角度$\Phi= \pm 2\pi $。由于正转或反转360度与没有旋转结果等效,故可不考虑该情况。在数值计算中,常通过设计伴影集来避免(见后续介绍)。
不可叠加性
这里的不可叠加性指的两次旋转的修正罗德里格斯参数之和并不等于等效的修正罗德里格斯参数。但是,修正罗德里格斯参数可先转化为方向余弦矩阵,再通过方向余弦矩阵叠加。最后从总方向余弦矩阵中得出等效修正罗德里格斯参数。例如,坐标系${\cal N}$到${\cal B}$的转换可由坐标系${\cal N}$到${\cal F}$的转换和${\cal F}$到${\cal B}$的转换联合给出。即:
\[\left[ {C_{FN}(\hat \sigma)} \right] = \left[ {C_{FB}({\hat \sigma^{ \prime \prime}})} \right]\left[ {C_{BN}({\hat \sigma'})} \right]\]
最终可得等效的修正罗德里格斯参数为:
\[\hat \sigma = \frac{{(1 - |{{\hat \sigma }^\prime }{|^2}){{\hat \sigma }^{\prime \prime }} + (1 - |{{\hat \sigma }^{\prime \prime }}{|^2}){{\hat \sigma }^\prime } - 2{{\hat \sigma }^{\prime \prime }} \times {{\hat \sigma }^\prime }}}{{1 + |{{\hat \sigma }^\prime }{|^2}|{{\hat \sigma }^{\prime \prime }}{|^2} - 2{{\hat \sigma }^\prime } \cdot {{\hat \sigma }^{\prime \prime }}}}\]
也可写为另一种形式:
\[{{\hat \sigma }^{\prime \prime }} = \frac{{(1 - |{{\hat \sigma }^\prime }{|^2})\hat \sigma - (1 - |\hat \sigma {|^2}){{\hat \sigma }^\prime } + 2\hat \sigma \times {{\hat \sigma }^\prime }}}{{1 + |{{\hat \sigma }^\prime }{|^2}|\hat \sigma {|^2} + 2{{\hat \sigma }^\prime } \cdot \hat \sigma }}\]
可以发现,减法形式相比于加法形式,仅有将${{\hat \sigma }^{\prime \prime }}$与${\hat \sigma }$互换、${\hat \sigma }^\prime $变为$-{\hat \sigma }^\prime $这两个变化。
注意分母在转换时可能出现的奇点问题。若出现,则可将参与运算的两个修正罗德里格斯参数之一换为对应的伴影集(见后续介绍)。
几何意义
由欧拉四元数的约束关系可知,若将四元数理解为四维空间中的坐标,则该点在一个单位超球体上移动,而不会离开此超球体。现只考虑$\beta_0$与$\beta_i,i=1,2,3$构成的二维投影平面,则四元数的投影只能在该平面上的单位圆内移动。
由修正罗德里格斯参数的定义式:
\[{\sigma _i} = \frac{{{\beta _i}}}{{1 + {\beta _0}}},i = 1,2,3\]
可知,$\sigma _i$等于四元数投影与$(-1,0)$连线在y轴上的截距。每一个二维平面投影都是如此,则$\hat \sigma$为四元数与$(-1,0,0,0)$连线与超平面$\beta_0=0$的交点。
而四元数与原点的连线和$\beta_i=0,i=1,2,3$的夹角就是主角度$\frac{\Phi}{2}$。其场景如下图所示。
伴影集
定义
修正罗德里格斯参数的伴影集(shadow set)是保持其与主旋转角$\Phi$之间近似线性关系的一种措施。它由修正罗德里格斯参数的几何意义导出。如上一节所示,四元数与原点的连线反向延伸交超球体于$-\hat \beta$,$(-1,0,0,0)$与$-\hat \beta$连线交超平面$\beta_0=0$于$\hat \sigma^s$。可由此写出伴影集$\hat \sigma^s$的定义:
\[\sigma _i^s = \frac{ - \beta _i}{1 - \beta _0} = \frac{ - \sigma _i}{\sigma ^2}\]
也可用主轴$\hat e$和主角度$\Phi$来表示:
\[\hat \sigma _i^s = \tan \left( \frac{\Phi - 2\pi }{4} \right)\hat e\]
修正罗德里格斯参数和其伴影集总是一个在超球体内,一个在超球体外,在特殊情况下均在超球体上。上图即表示了这种特殊情况。当$\beta_0=0$时,$ \sigma _i$与$ \sigma _i^s$一正一负,均在超球体上。此外,$ \sigma _i$在原点时,$ \sigma _i^s$在无穷远处,反之亦然。
修正罗德里格斯参数并不与姿态一一对应。它与它的伴影集表示的是同一姿态。
应用
由于修正罗德里格斯参数和其伴影集表达的是同一姿态,两者之间的切换不会影响问题描述。为了避开奇点,并控制主角度$\Phi
应用与评价
修正罗德里格斯参数由三个参量表征了三自由度的坐标旋转,唯一性好、奇点问题可克服、与其他参数转化方便、微分方程较简单。因此,它具有很高的应用价值。在微分方程或其他计算的迭代,常使用它来表示两个坐标系之间的方向关系。
附录:Matlab程序
以下Matlab程序均来自
转换为方向余弦矩阵
function C = MRP2C(q)
% MRP2C
%
%C = MRP2C(Q) returns the direction cosine
%matrix in terms of the 3x1 MRP vector Q.
%
q1 = q(1);
q2 = q(2);
q3 = q(3);
d1 = q'*q;
S = 1-d1;
d = (1+d1)*(1+d1);
C(1,1) = 4*(2*q1*q1-d1)+S*S;
C(1,2) = 8*q1*q2+4*q3*S;
C(1,3) = 8*q1*q3-4*q2*S;
C(2,1) = 8*q2*q1-4*q3*S;
C(2,2) = 4*(2*q2*q2-d1)+S*S;
C(2,3) = 8*q2*q3+4*q1*S;
C(3,1) = 8*q3*q1+4*q2*S;
C(3,2) = 8*q3*q2-4*q1*S;
C(3,3) = 4*(2*q3*q3-d1)+S*S;
C = C/d;
转换为欧拉四元数
functionq = MRP2EP(q1)
% MRP2EP(Q1)
%
%Q = MRP2EP(Q1) translates the MRP vector Q1
%into the Euler parameter vector Q.
%
ps = 1+q1'*q1;
q(1) = (1-q1'*q1)/ps;
q(2) = 2*q1(1)/ps;
q(3) = 2*q1(2)/ps;
q(4) = 2*q1(3)/ps;
q=q';
转换为吉布斯矢量
functionq = MRP2Gibbs(q1)
% MRP2Gibbs(Q1)
%
%Q = MRP2Gibbs(Q1) translates the MRP vector Q1
%into the Gibbs vector Q.
%
q = 2*q1/(1-q1'*q1);
转换为主旋转矢量
functionq = MRP2PRV(q1)
% MRP2PRV(Q1)
%
%Q = MRP2PRV(Q1) translates the MRP vector Q1
%into the principal rotation vector Q.
%
tp = sqrt(q1'*q1);
p = 4*atan(tp);
q(1) = q1(1)/tp*p;
q(2) = q1(2)/tp*p;
q(3) = q1(3)/tp*p;
q=q';
转换为伴影集
function s = MRPswitch(q,s2)
% MRPswitch
%
%S = MRPswitch(Q,s2) checks to see if norm(Q) is larger than s2.
%If yes, then the MRP vector Q is mapped to its shadow set.
%
q2 = q'*q;
if (q2>s2*s2)
s = -q/q2;
else
s = q;
end
微分方程
下面的程序用来求$[B]$矩阵。它的定义是:
\[\dot \sigma = \frac{1}{4}\left[ B \right]\left[ {\begin{array}{*{20}{c}}
{ {\omega _1} }\\
{ {\omega _2} }\\
{ {\omega _3} }
\end{array}} \right]\]
function B = BmatMRP(q)
% BmatMRP(Q)
%
%B = BmatMRP(Q) returns the 3x3 matrix which relates the
%body angular velocity vector w to the derivative of
%MRP vector Q.
%
%dQ/dt = 1/4 [B(Q)] w
%
s2 = q'*q;
B(1,1) = 1-s2+2*q(1)*q(1);
B(1,2) = 2*(q(1)*q(2)-q(3));
B(1,3) = 2*(q(1)*q(3)+q(2));
B(2,1) = 2*(q(2)*q(1)+q(3));
B(2,2) = 1-s2+2*q(2)*q(2);
B(2,3) = 2*(q(2)*q(3)-q(1));
B(3,1) = 2*(q(3)*q(1)-q(2));
B(3,2) = 2*(q(3)*q(2)+q(1));
B(3,3) = 1-s2+2*q(3)*q(3);
或者反过来:
\[\left[ {\begin{array}{*{20}{c}}
{ {\omega _1} }\\
{ {\omega _2} }\\
{ {\omega _3} }
\end{array}} \right] = 4{\left[ B \right]^{ - 1}}\dot \sigma \]
下面的程序用来求$[B]$的逆矩阵。
function B = BinvMRP(q)
% BinvMRP(Q)
%
%B = BinvMRP(Q) returns the 3x3 matrix which relates
%the derivative of MRP vector Q to the
%body angular velocity vector w.
%
%w = 4 [B(Q)]^(-1) dQ/dt
%
s2 = q'*q;
B(1,1) = 1-s2+2*q(1)*q(1);
B(1,2) = 2*(q(1)*q(2)+q(3));
B(1,3) = 2*(q(1)*q(3)-q(2));
B(2,1) = 2*(q(2)*q(1)-q(3));
B(2,2) = 1-s2+2*q(2)*q(2);
B(2,3) = 2*(q(2)*q(3)+q(1));
B(3,1) = 2*(q(3)*q(1)+q(2));
B(3,2) = 2*(q(3)*q(2)-q(1));
B(3,3) = 1-s2+2*q(3)*q(3);
B = B/(1+s2)/(1+s2);
Schaub, H. & Junkins, J.L., Analytical Mechanics of Space Systems , 2nd edition, AIAA Education Series, 2009.
matlab 罗德里格斯变换,修正罗德里格斯参数相关推荐
- Matlab FFT变换细节(信号采样频率,FFT变换点数,频率分辨率)
问题: 在做深度学习的故障诊断中,发现代码直接将原始信号fft之后直接将实频域信号输入网络中进行诊断,虽说效果比较不错95% 但因为输入的是双边谱且频率范围远超故障特征频率同时由于单个样本的点数只有1 ...
- opencv通过dll调用matlab函数,图片作为参数
[blog 项目实战派]opencv通过dll调用matlab函数,图片作为参数 前文介绍了如何"csharp通过dll调用opencv函数,图片作为参数 ...
- arcpy投影(二)——基准面变换概念及参数、空间参考对象获取、变换关系获取方法梳理与解析(Spatial Reference、ListTransformations)
arcpy投影这一个专题从文件位置.文件含义.空间参照获取.转换关系查询.投影定义.自定义转换关系.投影变换这几个角度上系统的进行了介绍,整理出了: arcpy投影(一)--prj.gtf文件定义.路 ...
- 机器学习 | MATLAB实现BP神经网络newff参数设定(下)
机器学习 | MATLAB实现BP神经网络newff参数设定(下) 目录 机器学习 | MATLAB实现BP神经网络newff参数设定(下) 基本介绍 程序设计 参考资料 致谢 基本介绍 newff搭 ...
- 机器学习 | MATLAB实现BP神经网络newff参数设定(上)
机器学习 | MATLAB实现BP神经网络newff参数设定(上) 目录 机器学习 | MATLAB实现BP神经网络newff参数设定(上) 基本介绍 程序设计 参考资料 致谢 基本介绍 newff搭 ...
- 机器学习 | MATLAB实现BP神经网络newff参数设定(中)
机器学习 | MATLAB实现BP神经网络newff参数设定(中) 目录 机器学习 | MATLAB实现BP神经网络newff参数设定(中) 基本介绍 程序设计 参考资料 致谢 基本介绍 newff搭 ...
- 希尔伯特黄变换matlab,HHT变换的三种方法 Matla
压缩包 : f914a6a90d345a26f732d9223e682699.rar 列表 复件 HHT变换的三种方法 Matlab/G Rilling/document.doc 复件 HHT变换的三 ...
- MATLAB调用refprop计算物性参数详解
MATLAB调用refprop计算物性参数详解 欢迎使用Markdown编辑器 欢迎使用Markdown编辑器 REFPROP(REference Fluid PROPerties)是一款国际权威工质 ...
- MATLAB坐标系变换动画gif(附代码):坐标系旋转动画+坐标系平移动画代码
MATLAB坐标系变换动画gif(附代码) 以之前的文章中的例题为例,绘制一个向量和一个运动坐标系在空间中的变换过程,并生成gif动画. 已知坐标系{B}的初始位姿与{A}重合,首先{B}相对于{A} ...
最新文章
- HDU1892(二维树状数组)
- 斯坦福大学深度学习与自然语言处理第三讲:高级的词向量表示
- postman模拟post请求的四种请求体
- oracle删除后电脑卡,彻底删除oracle服务 -电脑资料
- ADVM/ACFS is not supported on centos-release-5-5.el5.centos 解决方法
- 线程中这么调用类_这些线程知识总结是真的到位!java开发两年的我看的目瞪口呆
- vestacp 远程mysql_免费使用VestaCP控制面板的文件管理器 | 雷雨博客
- burp intruder爆破出现 Payload set 1: Invalid number settings的解决办法
- C++之操作符重载探究(七):==运算符重载
- 【LeetCode】【字符串】题号:*8. 字符串转换整数 (atoi)
- 交互设计精髓pdf百度云_About Face 4:交互设计精髓 (Alan cooper艾伦·库伯等) 中文pdf扫描版[139MB]...
- 巨佬Jake Wharton谈Android对Java 8的支持
- Codeforces 364D Ghd(随机化)
- 计算机实习生听课记录,实习生听课记录
- latex 背景颜色设置
- TestCenter测试管理工具功能详解十(O)
- python实战:爬取优美图库,将图片格式的本地存储
- 真正牛逼的人,都是极简主义者!!
- 计算机网络论文 考试吧,2012年11月计算机网络学习心得体会
- directives
热门文章
- dede 获取当前栏目的上一级栏目名称,和链接
- 如何设置 HomePod?HomePod设置教程分享
- 2017.11.25 计算机一级,2017计算机一级MSOffice考试试题
- 正则表达式,Math类,System类,日期日历对象
- 学画画要什么地方开始学起?零基础的人!
- Java “constant string too long” 编译错误
- c语言中什么随机存取,C语言中对文件的随机存取
- readl()和writel()
- 影响新零售商城系统直播带货的四大因素
- Deepin15.11系统深度驱动助手并未检测出独显问题