case 1： {
衣带渐宽终不悔；为伊消得人憔悴；
四元数、欧拉角及方向余弦矩阵的相互转换公式
四元数、欧拉角及方向余弦矩阵的相互转换公式
1. 欧拉角转四元数（常用来初始化四元数）
按Z-Y-X的旋转变换顺序有：
2. 四元数与旋转矩阵（常用来作坐标变换）
1.b系到R系的坐标变换矩阵
2.R系至b系的坐标变换矩阵公式
3. 欧拉角转方向余弦矩阵
由以上两式可得，经三次基本旋转对应的坐标变换为：
从上述方向余弦矩阵中可方便提取欧拉角
4. 四元数转欧拉角
5. 附C程序
void eulerAnglesToQuaternion(void)
cosRoll = cosf(roll * 0.5f);
sinRoll = sinf(roll * 0.5f);
cosPitch = cosf(pitch * 0.5f);
sinPitch = sinf(pitch * 0.5f);
cosHeading = cosf(hdg * 0.5f);
sinHeading = sinf(hdg * 0.5f);
q0 = cosRoll * cosPitch * cosHeading + sinRoll * sinPitch * sinH
q1 = sinRoll * cosPitch * cosHeading - cosRoll * sinPitch * sinH
q2 = cosRoll * sinPitch * cosHeading + sinRoll * cosPitch * sinH
q3 = cosRoll * cosPitch * sinHeading - sinRoll * sinPitch * cosH
void quaternionToRotationMatrix(void)
float q1q1 = sq(q1);
float q2q2 = sq(q2);
float q3q3 = sq(q3);
float q0q1 = q0 * q1;
float q0q2 = q0 * q2;
float q0q3 = q0 * q3;
float q1q2 = q1 * q2;
float q1q3 = q1 * q3;
float q2q3 = q2 * q3;
rMat[0][0] = 1.0f - 2.0f * q2q2 - 2.0f * q3q3;
rMat[0][1] = 2.0f * (q1q2 + -q0q3);
rMat[0][2] = 2.0f * (q1q3 - -q0q2);
rMat[1][0] = 2.0f * (q1q2 - -q0q3);
rMat[1][1] = 1.0f - 2.0f * q1q1 - 2.0f * q3q3;
rMat[1][2] = 2.0f * (q2q3 + -q0q1);
rMat[2][0] = 2.0f * (q1q3 + -q0q2);
rMat[2][1] = 2.0f * (q2q3 - -q0q1);
rMat[2][2] = 1.0f - 2.0f * q1q1 - 2.0f * q2q2;
void quaternionToEulerAngles(void)
roll = atan2f(2.f * (q2q3 + q0q1), q0q0 - q1q1 - q2q2 + q3q3);
pitch = asinf(2.f * (q0q2 - q1q3));
yaw = atan2f(2.f * (q1q2 + q0q3), q0q0 + q1q1 - q2q2 - q3q3);
没有更多推荐了，posts - 26,&
comments - 11,&
trackbacks - 0
一．四元组基础
Q(x,y,z,w)，其中x，y，z用来确定旋转轴，w为旋转的角度
Q=w+xi+yj+zk，i，j，k为三个虚轴的单位分量
| i&& &&j&&&& k|&|a1 &b1 &c1|&|a2 &b2 &c2|&=(b1c2-b2c1,c1a2-a1c2,a1b2-a2b1)
c也为一个向量，且c的长度为|a||b|sin(theta)，垂直于a和b所在的平面，方向由右手法则来判定，用右手的四指先表示向量a的方向，然后手指朝着手心的方向摆动到向量b的方向，大拇指所指的方向就是向量c的方向
1．四元组相乘：
Q1=w1+x1i+y1j+z1k=(w1,v1)
Q2=w2+x2i+y2j+z2k=(w2,v2)
Q1*Q2=(w1*w2-&v1,v2&,w1*v2+w2*v1+v1xv2)
( w1+x1i+y1j+z1k)*( w2+x2i+y2j+z2k)
&=w1*w2-x1*x2-y1*y2-z1*z2+
& (W1*x2+x1*w2+y1*z2-z1-y2)i+
(y1*w2+w1*y2+z1*x2-x1*z2)j+
(w1*z2+z1*w2+x1*y2-y1*x2)k
&&&&&&&& 对于其中的轴部分，假如v1//v2，则有v1 x v2=0（平行向量的叉乘结果为0）
2．四元组的点乘，点乘积为数值：
Q1.*Q2=w1*w2+&v1,v2&=w1*w2+x1*x2+y1*y2+z1*z2;
&&&&&&&& s为一实数，q为四元组，则有sq=qs
p=(w,v)，则p*=(w,-v)
(pq)*=q*p*
N(q)=w2+x2+y2+z2
q-1=q*/N(q)---------------&显然可得qq-1=(1,0)
二．使用四元数旋转向量
假如有一表示向量的四元组q=(w,v)，对其应用旋转量p后的结果为：
&&&&&&& q&=pqp-1=(w,v&)
从上可以看出，计算的结果q&的实部和q的实部是相等的，并且有N(v)=N(v&)
如果N(q)=1，则可以令q=(cosa,usina)，u也为一个单位向量，则q&是q绕u旋转2a个弧度的结果
假如S(q)表示q的实部，则有2S(q)=q+q*
2S(pqp-1)= pqp-1+( pqp-1)*=pqp*+(pqp*)*=pqp*+pq*p*=p(q+q*)p*=2S(q)
（这里由于p是单位四元数，所以有p-1等于p*）
欧拉角到四元数的转换
&定义pitch, yaw, roll分别为绕X轴、Y轴、Z轴的旋转弧度
float p = pitch * PIOVER180 / 2.0;
&&& float y = yaw * PIOVER180 / 2.0;
&&& float r = roll * PIOVER180 / 2.0;
&&& float sinp = sin(p);
&&& float siny = sin(y);
&&& float sinr = sin(r);
&&& float cosp = cos(p);
&&& float cosy = cos(y);
&&& float cosr = cos(r);
&&& this-&x = sinr * cosp * cosy - cosr * sinp *
&&& this-&y = cosr * sinp * cosy + sinr * cosp *
&&& this-&z = cosr * cosp * siny - sinr * sinp *
&&& this-&w = cosr * cosp * cosy + sinr * sinp *&
&&& normalise();
三．使用matlab进行相关计算
计算两个向量v1和v2之间的旋转量四元数p，使得v1应用p后到达v2
假如v1转到v2的旋转轴为v，旋转角为theta，则q=[v*cos(theta/2) &sin(theta/2)]
Matlab代码：
function q=vector2q(v1,v2)
%..normalize....
len1=sqrt(v1*v1');
len2=sqrt(v2*v2');
v1=v1/len1;
v2=v2/len2;
angle=v1*v2';
axis=cross(v1,v2);
alen=sqrt(axis*axis');
axis=axis/
t=acos(angle);
q(1)=axis(1)*sin(t);
q(2)=axis(2)*sin(t);
q(3)=axis(3)*sin(t);
q(4)=cos(t);
计算出了q之后，可以获得对应的旋转矩阵，旋转矩阵的计算
Matlab里面的矩阵是以列为主顺序的
function r=q2rot(q)
r=zeros(3,3);
r(1,1)=1-2*y*y-2*z*z;
r(1,2)=2*x*y+2*w*z;
r(1,3)=2*x*z-2*w*y;
r(2,1)=2*x*y-2*w*z;
r(2,2)=1-2*x*x-2*z*z;
r(2,3)=2*z*y+2*w*x;
r(3,1)=2*x*z+2*w*y;
r(3,2)=2*y*z-2*w*x;
r(3,3)=1-2*x*x-2*y*y;
同时，也可以根据四元数来计算欧拉角
function R=q2euler(q)
t11=2*(w*x+y*z);
t12=1-2*(x*x+y*y);
R(1)=atan2(t11,t12);
t2=2*(w*y-z*x);
R(2)=asin(t2);
t31=2*(w*z+x*y);
t32=1-2*(y*y+z*z);
R(3)=atan2(t31,t32);
计算出来的欧拉角rx，ry，rz，分别为绕X轴、Y轴和Z轴的旋转角，假如有：
Rotq=q2rot(q)
R=q2euler(q)
[rotx roty rotz]=Rotation(R)
可以发现Rotq==rotz*roty*rotx
从这里可以看出，上面使用四元数这样计算出来的旋转矩阵的旋转顺序分别是X轴、Y轴和Z轴的
qz=[0 0 -sin(ra) cos(ra)]& %绕z旋转-90度
qy=[0 sin(ra) 0 cos(ra) ]& %绕y旋转90度
qyz=qmult(qy,qz)
r=q2euler(qyz)
上面的r得出的结果为
r = -1.5708&&& 0.0000&& -1.5708
也就是说其几何意义变成先绕X轴旋转-90度，再绕Z轴旋转-90度，而根据qy和qz的相乘我们实际进行的操作却是先绕Z轴旋转-90度，再绕Y轴旋转90度，但是结果却是这两种操作等价，这说明由四元数到欧拉角可以有多个解
两个四元数，假如它们的方向是相反的，用它们作用于向量得到的新向量的值仍然相等
q1=[0....862594];
arm=[-8...36776];
rot1=q2rot(q1);
rot2=q2rot(q2);
v1=rot1*arm'
v2=rot2*arm'
上面计算出来的v1等于v2
四元数的余弦值为它们的内积
假如余弦值小于0，则需要将其中的一个取反，因为上面我们知道一个四元数和它的反方向的四元数对一个向量起相同的作用
四元数的相乘，代表旋转的累积
rotp=q2rot(p);
rotq=q2rot(q);
rotpq=q2rot(pq);
rotmul=rotp*
这里rotpq与rotmul相等
四． OGRE中Quaternion类的几个函数
1.四元数到旋转向量
void Quaternion::ToRotationMatrix (Matrix3& kRot) const
2.旋转量到四元数
根据1中的表格，有：
4 *(1-qx2-qy2-qz2)&= 1 + m00 + m11 + m22
又qw2=1-qx2-qy2-qz2，可得
4 *qw2= 1 + m00 + m11 + m22
这里解qw必须保证1 + m00 + m11 + m22&=0，如果不是的话，就构造其他的等式来计算，OGRE中分成两种情况，一种是m00 + m11 + m22&=0，就可以直接先解出qw，否则的采用另外的等式计算
3．Local axis
Vector3 xAixs(void)
取得旋转矩阵的第一列，旋转矩阵和一个向量相乘的话，第一列的数据均和向量的x分量相乘
Vecotr3 yAxis(void)
取得旋转矩阵的第二列，旋转矩阵和一个向量相乘的话，第二列的数据均和向量的y分量相乘
Vecotr3 zAxis(void)
取得旋转矩阵的第三列，旋转矩阵和一个向量相乘的话，第三列的数据均和向量的z分量相乘
阅读(...) 评论()三维空间旋转（欧拉角、四元数、旋转矩阵）
时间： 20:51:49
&&&& 阅读：374
&&&& 评论：
&&&& 收藏：0
标签：&&&&&&&&&&&&&&&&&&&&&&&&&&&　　姿态角（欧拉角）　　
　　姿态角即RPY（roll, pitch,yaw）又叫欧拉角，是由三个角组成的。
　　俯仰角（pitch）
　　翻滚角（roll）
　　偏航角（yaw）
　　其中最直观的就是其绕刚体自身的X、Y、Z三个轴分别旋转的角度，这就是欧拉角（Euler Angle）表示方法。
　　需要注意的是，欧拉角的表示方式里，yaw、pitch、roll的顺序对旋转的结果是有影响的。
　　给定一组欧拉角角度值，比如yaw=45度，pitch=30度，roll=60度，按照yaw-pitch-roll的顺序旋转和按照yaw-roll-pitch的顺序旋转，最终刚体的朝向是不同的！
　　另外需要注意，在欧拉角中，三个旋转轴一般是随着刚体一起运动的。
　　欧拉角的表示方式比较直观，但是有几个缺点：
　　(1) 欧拉角的表示方式不唯一。给定某个起始朝向和目标朝向，即使给定yaw、pitch、roll的顺序，也可以通过不同的yaw/pitch/roll的角度组合来表示所需的旋转。比如，同样的yaw-pitch-roll顺序，(0,90,0)和(90,90,90)会将刚体转到相同的位置。这其实主要是由于引起的，关于万向锁的解释，有条件的同学看看或许会比较直观。
　　(2) 欧拉角的插值比较难。
　　(3) 计算旋转变换时，一般需要转换成旋转矩阵，这时候需要计算很多sin, cos，计算量较大。
　　方向余弦矩阵（DCM）和旋转矩阵（RM）　　
　　在计算坐标变换时，旋转更方便的表示形式是。
　　三维空间的旋转矩阵可以表示成3x3的矩阵，将欧拉角转换为旋转矩阵的计算方式如下，
　　假设欧拉角yaw、pitch、roll的角度为alpha, beta, gamma，则旋转矩阵可以计算如下：
　　这里也可以看出，如果yaw、pitch、roll的顺序有改变，矩阵相乘的顺序需要作出相应改变，所得的旋转矩阵结果也会有所改变。
　　而最主要的是万向节锁（Gimbal lock）的问题，详情见youtub上有很多演示视频（例如）。
　　这里文字解释一下，一个旋转万向节如下：
　　最终间的旋转轴代表偏航角，第二层的旋转代表俯仰角，最外面的旋转轴代表翻滚角。
　　万向节锁的问题是，当锁死其中一个自由度的时候，其中两个自由度会重叠，例如锁死俯仰和翻滚的轴的时候（即最外面的两层），翻滚和偏航角的旋转轴会重叠（即表示的是同一个自由度）。
　　也就是说这个Gimble lock（万向锁）是由旋转顺序中的第二个旋转轴产生的，即XYZ旋转顺序，Y轴会产生万向锁。当我们知道有一个轴经常会旋转到90度时，那就不要让这个轴成为第二个旋转的轴。
&　　详情见（，为youtub 需FQ），另一个视频可以详细讲解欧拉角和四元数（）
&（未完待续）
　　标签：&&&&&&&&&&&&&&&&&&&&&&&&&&&原文：https://www.cnblogs.com/TIANHUAHUA/p/8135446.html
教程昨日排行
&&国之画&&&& &&&&&&
&& &&&&&&&&&&&&&&
鲁ICP备号-4
打开技术之扣，分享程序人生！&>&欧拉角，四元数，方向余弦矩阵转换代码
欧拉角，四元数，方向余弦矩阵转换代码
上传大小：3KB
欧拉角，四元数和方向余弦矩阵三者之间相互转换的代码，采用北-东-地坐标系
综合评分：5
{%username%}回复{%com_username%}{%time%}\
/*点击出现回复框*/
$(".respond_btn").on("click", function (e) {
$(this).parents(".rightLi").children(".respond_box").show();
e.stopPropagation();
$(".cancel_res").on("click", function (e) {
$(this).parents(".res_b").siblings(".res_area").val("");
$(this).parents(".respond_box").hide();
e.stopPropagation();
/*删除评论*/
$(".del_comment_c").on("click", function (e) {
var id = $(e.target).attr("id");
$.getJSON('/index.php/comment/do_invalid/' + id,
function (data) {
if (data.succ == 1) {
$(e.target).parents(".conLi").remove();
alert(data.msg);
$(".res_btn").click(function (e) {
var parentWrap = $(this).parents(".respond_box"),
q = parentWrap.find(".form1").serializeArray(),
resStr = $.trim(parentWrap.find(".res_area_r").val());
console.log(q);
//var res_area_r = $.trim($(".res_area_r").val());
if (resStr == '') {
$(".res_text").css({color: "red"});
$.post("/index.php/comment/do_comment_reply/", q,
function (data) {
if (data.succ == 1) {
var $target,
evt = e || window.
$target = $(evt.target || evt.srcElement);
var $dd = $target.parents('dd');
var $wrapReply = $dd.find('.respond_box');
console.log($wrapReply);
//var mess = $(".res_area_r").val();
var mess = resS
var str = str.replace(/{%header%}/g, data.header)
.replace(/{%href%}/g, 'http://' + window.location.host + '/user/' + data.username)
.replace(/{%username%}/g, data.username)
.replace(/{%com_username%}/g, data.com_username)
.replace(/{%time%}/g, data.time)
.replace(/{%id%}/g, data.id)
.replace(/{%mess%}/g, mess);
$dd.after(str);
$(".respond_box").hide();
$(".res_area_r").val("");
$(".res_area").val("");
$wrapReply.hide();
alert(data.msg);
}, "json");
/*删除回复*/
$(".rightLi").on("click", '.del_comment_r', function (e) {
var id = $(e.target).attr("id");
$.getJSON('/index.php/comment/do_comment_del/' + id,
function (data) {
if (data.succ == 1) {
$(e.target).parent().parent().parent().parent().parent().remove();
$(e.target).parents('.res_list').remove()
alert(data.msg);
//填充回复
function KeyP(v) {
var parentWrap = $(v).parents(".respond_box");
parentWrap.find(".res_area_r").val($.trim(parentWrap.find(".res_area").val()));
评论共有1条
下载看看，应该可以用的
综合评分：
积分/C币：3
VIP会员动态
CSDN下载频道资源及相关规则调整公告V11.10
下载频道用户反馈专区
下载频道积分规则调整V1710.18
spring mvc+mybatis+mysql+maven+bootstrap 整合实现增删查改简单实例.zip
资源所需积分/C币
当前拥有积分
当前拥有C币
输入下载码
为了良好体验，不建议使用迅雷下载
欧拉角，四元数，方向余弦矩阵转换代码
会员到期时间：
剩余下载个数：
剩余积分：0
为了良好体验，不建议使用迅雷下载
积分不足！
资源所需积分/C币
当前拥有积分
您可以选择
程序员的必选
绿色安全资源
资源所需积分/C币
当前拥有积分
当前拥有C币
为了良好体验，不建议使用迅雷下载
资源所需积分/C币
当前拥有积分
当前拥有C币
为了良好体验，不建议使用迅雷下载
资源所需积分/C币
当前拥有积分
当前拥有C币
您的积分不足，将扣除 10 C币
为了良好体验，不建议使用迅雷下载
无法举报自己的资源
你当前的下载分为234。
你还不是VIP会员
开通VIP会员权限，免积分下载
你下载资源过于频繁，请输入验证码
您因违反CSDN下载频道规则而被锁定帐户，如有疑问，请联络:!
若举报审核通过，可返还被扣除的积分
被举报人：
请选择类型
资源无法下载 （ 404页面、下载失败、资源本身问题）
资源无法使用 （文件损坏、内容缺失、题文不符）
侵犯版权资源 （侵犯公司或个人版权）
虚假资源 （恶意欺诈、刷分资源）
含色情、危害国家安全内容
含广告、木马病毒资源
*投诉人姓名：
*投诉人联系方式：
*版权证明：
*详细原因：
欧拉角，四元数，方向余弦矩阵转换代码1，方向余弦阵
坐标变换矩阵的每一个元素都是i系某个坐标系与b系坐标系的夹角的余弦。这个转换方法也同样适用于二维坐标转换。只不过二维坐标转换不仅需要考虑旋转，同时也需要考虑坐标的平移变换。
如上图所示，表示只进行旋转的时候二维坐标转换公式。此时旋转矩阵表示为P=
&经典的欧拉角有多种定义，下面只介绍内旋方式的定义。
所以，欧拉角只是绕着两个本机坐标轴在旋转，第三个坐标轴就没有用到。
之后提出 Tait-Bryan 角
经常说的yaw，pitch，roll就是用Tait-Bryan角实现的。并且yaw指的是绕z轴旋转的角度值，pitch指的是绕y轴旋转的角度值，roll指的是绕x轴旋转的角度值。
通常yaw角也用ψ表示，pitch角也用θ表示，roll角也用φ表示。并且yaw和roll的范围是[-∏,∏],pitch的范围是[-Π/2,Π/2].
不同的旋转组合虽然可以得到相同的坐标转换结果，但是得到的roll，pitch，yaw是不同的。
按照x，y，z顺序旋转得到由roll，pitch，yaw表示的方向余弦如下
由方向余弦得到roll pitch yaw的表达式如下
roll(φ)=atan2(R(3,2),R(3,3));
pitch(θ)=-asin(R(3,1));
yaw(ψ)=atan2(R(2,1),R(1,1));
四元数的理解可以参考http://www.qiujiawei.com/understanding-quaternions/（理解四元数，比较通俗的解释了四元数）
推导请查看秦永元版惯性导航中相关章节。
四元数的几何意义：实部表示旋转角度的1/2的余弦值。虚部表示旋转轴与旋转角度的1/2的正弦值的乘积。
四元数得到方向余弦矩阵的表达式
上式和下式是相同的，因为四元数的平方和等于1。
由方向余弦阵到四元数的转换可以参考以下matlab程序
function q=dcm2q(R)
T = 1 + R(1,1) + R(2,2) + R(3,3);
if T & 10^-8
S = 0.5 / sqrt(T);
qw = 0.25 / S;
qx = ( R(3,2) - R(2,3) ) * S;
qy = ( R(1,3) - R(3,1) ) * S;
qz = ( R(2,1) - R(1,2) ) * S;
if (R(1,1) & R(2,2)) && (R(1,1) & R(3,3))
S = sqrt( 1 + R(1,1) - R(2,2) - R(3,3)) * 2; % S=4*qx
qw = (R(3,2) - R(2,3)) / S;
qx = 0.25 * S;
qy = (R(1,2) + R(2,1)) / S;
qz = (R(1,3) + R(3,1)) / S;
elseif (R(2,2) & R(3,3))
S = sqrt( 1 + R(2,2) - R(1,1) - R(3,3) ) * 2; %S=4*qy
qw = (R(1,3) - R(3,1)) / S;
qx = (R(1,2) + R(2,1)) / S;
qy = 0.25 * S;
qz = (R(2,3) + R(3,2)) / S;
S = sqrt( 1 + R(3,3) - R(1,1) - R(2,2) ) * 2; % S=4*qz
qw = (R(2,1) - R(1,2)) / S;
qx = (R(1,3) + R(3,1)) / S;
qy = (R(2,3) + R(3,2)) / S;
qz = 0.25 * S;
q = [qx qy qz qw]';
4，方向余弦阵的微分
5，四元数微分
详细的推导参看秦永元版惯性导航
阅读(...) 评论()