基于Matlab现有函数下的内容
Matlab ——旋转矩阵,旋转向量,四元数,欧拉角之间的转换
旋转矩阵 dcm R
四元数 quat q = [q0 q1 q2 q3]
欧拉角 angle [row,pitch,yaw]/[r1,r2,r3]
注:以上表格是为了帮助理解的表示
roll(横滚) --X pitch(俯仰)--Y yaw(偏航/航向)-- Z
- - - - - - - - - - - - - - - - - - - - - - -- - - -- - - - - - - - - - - - - - - -- - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
转四元数
旋转矩阵转四元数
q =dcm2quat(R);
欧拉角转四元数
q=angle2quat(r1,r2,r3,S);
- - - - - - - - - - - - - - - - - - - - - - -- - - -- - - - - - - - - - - - - - - -- - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
转欧拉角
旋转矩阵转欧拉角
[r2,r2,r3]=dcm2angle(R, S)
注:得到的结果为弧度,若需要角度需进一步转化
四元数转欧拉角
[r1,r2,r3]=quat2angle([q0 q1 q2 q3],S)
注:S 的选择有12种,【'ZYX','ZYZ’,‘ZXY’,‘ZXZ’,‘YXZ’,‘YXY’,‘YZX’,‘YZY’,‘XYZ’,‘XYX’,‘XZY’,‘XZX’】
S 默认 ‘ZYX'
- - - - - - - - - - - - - - - - - - - - - - -- - - -- - - - - - - - - - - - - - - -- - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
转旋转矩阵
四元数转旋转矩阵
R=quat2dcm([q0 q1 q2 q3])
欧拉角转旋转矩阵
R=angle2dcm(r1,r2,r3,S);
R=angle2dcm(yaw/180*pi,pitch/180*pi,roll/180*pi)
注:根据欧拉角是弧度/角度,选择以上操作
- - - - - - - - - - - - - - - - - - - - - - -- - - -- - - - - - - - - - - - - - - -- - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
若已知旋转矩阵R,求四元数[q0 q1 q2 q2]
则对应的四元数为:
- - - - - - - - - - - - - - - - - - - - - - -- - - -- - - - - - - - - - - - - - - -- - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
旋转矩阵及旋转向量相互转化 Rodrigues矩阵及matlab实现
处理三维旋转问题时,通常采用旋转矩阵的方式来描述。一个向量乘以旋转矩阵等价于向量以某种方式进行旋转。除了采用旋转矩阵描述外,还可以用旋转向量来描述旋转,旋转向量的长度(模)表示绕轴逆时针旋转的角度(弧度)。旋转向量与旋转矩阵可以通过罗德里格斯(Rodrigues)变换进行转换。
算法过程如下:
式中,norm为求向量的模。反变换也可以很容易的通过如下公式实现:
matlab实现
% om=[0.02876123785972595215 -1.668149590492248535 -0.0360212959349155426]';
om=[ 0.0009597472380846738815 0.01455024257302284241 0.010445447638630867]';
theta=norm(om);
om=om./theta;
I=eye(3);
rom=[0 -om(3) om(2);
om(3) 0 -om(1);
-om(2) om(1) 0];
R = cos(theta)*I+(1-cos(theta))*om*om'+ sin(theta)*rom
q =dcm2quat(R)
matlab里有rotationVectorToMatrix和rotationMatrixToVector函数
以上为例
>> R=rotationVectorToMatrix(om)
R =
0.9998 0.0105 -0.0145
-0.0104 0.9999 0.0010
0.0146 -0.0009 0.9999
rotationVector = rotationMatrixToVector(R)
rotationVector = -0.0010 -0.0146 -0.0104
---------------------------------------------
欧拉角与旋转矩阵转换 matlab 函数 angle2dcm dcm2angle 方向余弦矩阵 机器人坐标变换
在工业机器人坐标变换中,旋转矩阵与欧拉角有几个概念比较容易混淆,根据自己的理解整理如下,如有问题,欢迎指正
因为欧拉角相对旋转矩阵更为直观,直接用绕坐标轴的旋转可以得到,因此将旋转矩阵转换为欧拉角会方便分析
在位姿变换相关资料中(可参照 J. J. Craig 《Introduction to Robotics》),位姿矩阵的描述如下
上面这样的位姿描述,指的是主动旋转矩阵,即向量或子坐标系绕坐标轴主动旋转的旋转矩阵,也是机器人坐标变换通常会采取的旋转方式,写成 matlab 函数如下(a输入‘X’,'Y','Z',B输入旋转角度)
function y = Rot3( a ,B )
b = deg2rad(B);
switch a
case 'X'
y=[1,0,0;0,cos(b),-sin(b);0,sin(b),cos(b)];
case 'Y'
y=[cos(b),0,sin(b);0,1,0;-sin(b),0,cos(b)];
case 'Z'
y=[cos(b),-sin(b),0;sin(b),cos(b),0;0,0,1];
end
与此区分的是被动旋转矩阵,向量或子坐标系不变,基坐标系旋转的旋转矩阵,这类被动旋转矩阵与上面的三个矩阵互为转置(互为逆矩阵,对于旋转矩阵是等价的),也可以理解为被动旋转矩阵让向量或子坐标系旋转到基坐标系位置,而 matlab 中的 dcm2angle,angle2dcm 均对应的是被动旋转:
Mat_t = [cos(pi/3),-sin(pi/3),0;sin(pi/3),cos(pi/3),0;0,0,1];
[ra,rb,rc]=dcm2angle(Mat_t,'ZYX');
%此处 ra 为 -60 度,可见 Matlab 中采用的是被动旋转,sin的符号和主动旋转矩阵相反
因此,在采用matlab 中 dcm2angle 计算欧拉角时,需要注意对旋转矩阵进行转置(R3'),以‘ZYX’欧拉角为例:
R3 = Rot3('Z',20)*Rot3('Y',60)*Rot3('X',40)
[ra,rb,rc]=dcm2angle(R3','ZYX');
M3 = Rot3r('Z',ra)*Rot3r('Y',rb)*Rot3r('X',rc)
d=[ra,rb,rc]./pi.*180
这样,计算结果 d = 20.0000 60.0000 40.0000 ,与常用主动旋转矩阵一致,R3与M3也一致,如果R3‘转置去掉,改为
[ra,rb,rc]=dcm2angle(R3,'ZYX');
则结果为从R3转换到基坐标的欧拉角,对应M3为R3的逆矩阵
需要注意的是,主动/被动旋转不等同于基坐标系、连体坐标系的变换,在主动旋转矩阵下,仍然要区分按照固定基坐标系坐标轴,还是按照旋转后的连体坐标新坐标轴旋转,按照右乘连体左乘基的方式进行计算
————————————————
旋转向量,旋转矩阵和四元数的关系
参考资料
1.向量旋转公式
2.旋转矩阵和旋转向量
2.1旋转矩阵微分和旋转向量的推导
2.2罗德里格旋转公式
3.四元数和旋转向量
3.1 由欧拉参数推导四元数
3.2旋转向量表示四元数
4.旋转矩阵和四元数
5.欧拉角和旋转向量角
参考资料
- Quaternion kinematics for the error-state KF
- barfoot《state estimation forrobotics》
- 袁信、郑锷《捷联式惯性导航原理》
- 以上书籍的下载链接链接:http://pan.baidu.com/s/1c1G0k5U 密码:jdsz
刚体在空间中的一次旋转可以用旋转矩阵,四元数和旋转向量三种方式表示,以下总结三者的数学转化关系。
1.向量旋转公式
- 旋转向量的定义:方向是旋转轴,大小是旋转角的向量,表示刚体在空间中的一次旋转。
- 定义向量x绕单位旋转轴u旋转角度
- ,将向量绕u轴和垂直u轴分解,并利用向量的点乘的几何意义得到:
- 平行于u轴的分量在旋转中保持不变,垂直分量则旋转
- 角度,公式(48)到(49)的推导利用了正交基的几何关系和上式证明的模长相等。
2.旋转矩阵和旋转向量
2.1旋转矩阵微分和旋转向量的推导
- 相对于笔记1,利用哥氏定理推导旋转矩阵的微分方程,这里给出了另外一种微分方程的推导,并从旋转角时间积分的角度给出一种旋转向量的定义。
2.2罗德里格旋转公式
- 利用李群SO(3)推导出以旋转向量表示的罗德里格旋转公式(Rodrigues rotation formula);其中计算时旋转轴u用哪个坐标系表示都是等价的,后面将证明。
- 利用罗德里格旋转公式推导向量旋转公式
3.四元数和旋转向量
3.1 由欧拉参数推导四元数
-
由旋转矩阵的欧拉参数(Euler Parameters)推导单位四元数的表示,这里参考barfoot书上的内容,这部分也证明了罗德里格旋转公式的旋转轴u用哪个坐标系表示都可以。1中也有说明,旋转轴在旋转过程中没有几何位置变化。
-
由此得到以欧拉参数推导出的单位四元数
3.2旋转向量表示四元数
- 根据3.1,由旋转向量表示四元数可以写成:
- 用四元数表示向量的旋转,这里和四元数相乘运算的三维向量都改写成四元数的形式(只有虚部,实部为0),并进行了证明(结果等于向量旋转公式结果):
4.旋转矩阵和四元数
- 由2.2和3.3,以向量旋转公式为纽带,得到旋转矩阵和四元数表示旋转的等价关系,进而得到单位四元数的元素表示的旋转矩阵,该结果和3.1由欧拉参数表示罗德里格公式的结论一致。3.1中的欧拉参数其实就是这里的单位四元数的实部和虚部。
- 其中这里的x是以四元数的形式表示的。
5.欧拉角和旋转向量角
- 我们提到欧拉角,一般指的是绝对姿态的欧拉角,即导航系下的机体俯仰滚转和偏航角,而旋转向量角指的是一次旋转过程的角度,一般是微小量。
- 两者的关系:机体在运动过程中欧拉角的更新可以看做是初始旋转矩阵(欧拉角表示)与每次旋转矩阵(旋转向量角表示)的积分。
欧拉角的转动顺序为Z→Y→X,转动的角度分别为(ψ,θ,ϕ)。
欧拉角介绍
欧拉角包括偏航角(yaw)ψ,俯仰角(pitch)θ,滚转角(roll)ϕ,导航坐标系S−Oxgygzg经过三次旋转可以得到机体坐标系B−xbybzb。需要注意的是,这里旋转的是坐标轴,不是向量。旋转过程遵循右手定则。
- RPY角与Z-Y-X欧拉角
描述坐标系{B}相对于参考坐标系{A}的姿态有两种方式。第一种是绕固定(参考)坐标轴旋转:假设开始两个坐标系重合,先将{B}绕{A}的X轴旋转γ
,然后绕{A}的Y轴旋转β,最后绕{A}的Z轴旋转α
,就能旋转到当前姿态。可以称其为X-Y-Z fixed angles或RPY角(Roll, Pitch, Yaw)。
Roll:横滚
Pitch: 俯仰
Yaw: 偏航(航向)