玩转四旋翼无人机(四元数)
2016-08-27 16:45
411 查看
四元数介绍 Quaternions and spatial rotation
单位四元数,也就是规范化四元数,提供一种方便的数学表示来表达多维空间中的方向和物体的旋转。相对于欧拉角来说,四元数更简单,避免了万向节锁。对比与旋转矩阵,四元数的数值稳定,更加高校。四元数在计算机图形,计算机视觉,机器人,导航和分子动力学,飞行器动力学等广泛应用。四元数在16.oct.1843被发现,wiliiam Rowan Hamiton花费数年的时间使用一个3D数表示3D旋转,后来失败,于是发现4维才能工作。
这个图给我们关于四元数的直观的理解,这个图给人一种错误的提示就是,+1,-1代表180度,i代表90度。实际上想要它正常工作,那么i表示180度,i^2表示360度,+1,-1表示同一种变换。由Axis-Angle 转四元数的公式可知,当旋转角为180度的时候,旋转向量为[1,0,0],q=i.也就是旋转180度。
-1表示旋转了360度,也就是没有变化,按说应该是1.事实上一个四元数中所有项都取反,得到一个不同的四元数,但是旋转是相同的。
Unit quaternions, also known as versors, provide a convenient mathematical notation for representing orientations and rotations of objects in three dimensions. Compared to Euler angles they are simpler to compose and avoid the problem of gimbal lock. Compared to rotation matrices they are more numerically stable and may be more efficient. Quaternions have applications in computer graphics, computer vision, robotics, navigation, molecular dynamics, flight dynamics, orbital mechanics of satellites and crystallographic texture analysis.
基本特点
不可互换分布率
不满足结合律
not commutative
associative
distributive over sum
product of two quaternions
q1⊗q2=[q1]Lq2
and
q1⊗q2=[q2]Rq1
使用四元数表示3D旋转矩阵
一个单独的四元数无法包括平移,如果我们想要rotate, reflect and scale围绕一个点而不是原点,我们需要单独处理平移.- For Reflection & scaling: Pout=q⊗Pin⊗q
- For Rotation & scaling: Pout=q⊗Pin⊗q∗
对于大部分应用只涉及旋转,在该前提之下四元数变为具备单位幅值。
欧拉角
我们使用欧拉角来表示3维旋转,分解旋转维3各独立的角。 当我们考虑3D旋转的时候,欧拉角是最最然也是直觉上容易理解的方式。但是使用欧拉角会遇到很多问题。因此介绍欧拉角的目的就是获取直观的理解,然后转到四元数或者旋转矩阵。这里使用右手坐标系。有一些顺序是等价的。通过结合2各平面的旋转可以构造任意3D旋转。因此总共12种可能排列xyz, yzx, zxy or reversing the order zyx, xzy, yxz and 按照先后旋转的顺序 xyx, xzx, yxy, yzy, zxz and zyz。线性旋转
我们不能在3维中得到线性旋转,这里介绍为什么要引入四元数。为了研究欧拉旋转的非线性根源,我们尝试把角显示在线性的3D网格中,把旋转映射其中。在欧拉角和3D旋转的其他表示之间的相互转换时,我们遇到奇异点。首先考虑这样一个问题,如何表示2D的球面。
这里只涉及到经度和纬度。在数学中有多重奇异点,这些情形中我们这里考虑
- 在一种表示中,很多被映射成为一个点到另外一种表示中。
- 在一种表示中,在靠近奇异点的无穷小的变换在另外一种表示中引起巨大的变化。
-
类似的,欧拉角转换到四元数的时候,围绕0,1,0轴的旋转(一维空间)在欧拉角中为一个二维平面
在红色的平面内,对角线围绕轴0,1,0旋转,角度=heading+bank。
even if we do apply the angles in our chosen order it does not necessarily mean that each point on the diagram represents a unique rotation, it may be that the same rotation is represented at several places on the diagram, this happens at the singularities attitude= ±π/2.
conversions
euler angle to quaternion
(cos(ψ/2) + i * sin(ψ/2)) * (cos(θ/2) + j * sin(θ/2)) * (cos(φ/2) + k * sin(φ/2))first method:
w = c1 c2 c3 - s1 s2 s3
x = s1 s2 c3 +c1 c2 s3
y = s1 c2 c3 + c1 s2 s3
z = c1 s2 c3 - s1 c2 s3
where:
c1 = cos(heading / 2)
c2 = cos(attitude / 2)
c3 = cos(bank / 2)
s1 = sin(heading / 2)
s2 = sin(attitude / 2)
s3 = sin(bank / 2)
An alternative form is:
w = Math.sqrt(1.0 + C1 * C2 + C1*C3 - S1 * S2 * S3 + C2*C3) / 2
x = (C2 * S3 + C1 * S3 + S1 * S2 * C3) / (4.0 * w)
y = (S1 * C2 + S1 * C3 + C1 * S2 * S3) / (4.0 * w)
z = (-S1 * S3 + C1 * S2 * C3 + S2) /(4.0 * w)
where:
C1 = cos(heading)
C2 = cos(attitude)
C3 = cos(bank)
S1 = sin(heading)
S2 = sin(attitude)
S3 = sin(bank)
Conversion Quaternion to Euler
Conversion Quaternion to EulerEquations
heading = atan2(2*qy*qw-2*qx*qz , 1 - 2*qy2 - 2*qz2)
attitude = asin(2*qx*qy + 2*qz*qw)
bank = atan2(2*qx*qw-2*qy*qz , 1 - 2*qx2 - 2*qz2)
except when qx*qy + qz*qw = 0.5 (north pole)
which gives:
heading = 2 * atan2(x,w)
bank = 0
and when qx*qy + qz*qw = -0.5 (south pole)
which gives:
heading = -2 * atan2(x,w)
bank = 0
这里
Heading = rotation about y axis
Attitude = rotation about z axis
Bank = rotation about x axis
airplane | telescope | symbol | angular velocity | |
---|---|---|---|---|
applied first | heading | azimuth | (θ) | yaw |
applied second | attitude | elevation | (ϕ) | pitch |
applied last | bank | tilt | (ψ) | roll |
quaternion to axis angle:
angle = 2 * acos(qw)x = qx / sqrt(1-qw*qw)
y = qy / sqrt(1-qw*qw)
Conversion Euler to Axis-Angle
angle = 2 * acos(c1c2c3 - s1s2s3)x = s1 s2 c3 +c1 c2 s3
y = s1 c2 c3 + c1 s2 s3
z = c1 s2 c3 - s1 c2 s3
to normalise divide x,y and z by:
sqrt(x2 + y2 + z2) = sqrt((s1 s2 c3 +c1 c2 s3)2+(s1 c2 c3 + c1 s2 s3)2+(c1 s2 c3 - s1 c2 s3)2)
where:
c1 = cos(heading / 2)
c2 = cos(attitude / 2)
c3 = cos(bank / 2)
s1 = sin(heading / 2)
s2 = sin(attitude / 2)
s3 = sin(bank / 2)
Euler to matrix (RPY -> matrix)
R=⎡⎣⎢ch∗casa−sh∗ca−ch∗sa∗cb+sh∗sbca∗cbsh∗sa∗cb+ch∗sbch∗sa∗sb+sh∗cb−ca∗sb−sh∗sa∗sb+ch∗cb⎤⎦⎥where:
sa=sin(attitude)
ca=cos(attitude)
sb=sin(bank)
cb=cos(bank)
sh=sin(heading)
ch=cos(heading)
axis angle to matrix
该转换也称为通用旋转变换,根绝旋转轴以及旋转角度计算旋转矩阵。
⎡⎣⎢c+a2x(1−c)ayax(1−c)+azsazax(1−c)−aysaxay(1−c)−azsc+a2y(1−c)azay(1−c)+axsaxaz(1−c)+aysayaz(1−c)−axsc+a2z(1−c)⎤⎦⎥
where s and c are shorthand for θ and cos θ,
quaternion to matrix
一个四元数的旋转可以转化为一个矩阵旋转,通过简化四元数的乘法以及通用旋转变换。To get the rotation of a vector p about an arbitrary axis we getp′=Rp
Although care should be taken the axis and angle can be extracted via:
q=qr+qii+qjj+qkk
θ=2arccosqr=2arcsinq2i+q2j+q2k−−−−−−−−−−√
(ax,ay,az)=1sin12θ(qi,qj,qk)
Note that the θ equality holds only when qr is non-negative.
Alternatively, the rotation matrix can be expressed as
R=⎡⎣⎢⎢1−2q2j−2q2k2(qiqj+qkqr)2(qiqk−qjqr)2(qiqj−qkqr)1−2q2i−2q2k2(qjqk+qiqr)2(qiqk+qjqr)2(qjqk−qiqr)1−2q2i−2q2j⎤⎦⎥⎥
As with other schemes to apply rotations, the centre of rotation must be translated to the origin before the rotation is applied and translated back to its original position afterwards.
matrix to quaternion
qw= √(1 + m00 + m11 + m22) /2qx = (m21 - m12)/( 4 * qw)
qy = (m02 - m20)/( 4 * qw)
qz = (m10 - m01)/( 4 * qw)
quaternion | matrix | Euler | Axis-Angle | |
---|---|---|---|---|
quaternion | – | o | o | o |
matrix | ||||
Euler | o | o | - | o |
Axis-Angle | o | o | o | - |
相关文章推荐
- 玩转四旋翼无人机(Nvidia Jetson TK1 使用)
- 玩转四旋翼无人机(sensor数据融合)
- 玩转四旋翼无人机(DJI SDK 使用)
- 玩转四旋翼无人机(攒机基础1)
- 玩转四旋翼无人机(GPS基础)
- 玩转四旋翼无人机(DJI SDK LIB)
- 玩转四旋翼无人机(攒机基础2)
- 玩转四旋翼无人机(DJI OnBoard SDK ROS)
- 玩转四旋翼无人机(四旋翼运动学简介)
- 玩转四旋翼无人机(DJI_QT_SDK)
- 玩转四旋翼无人机(pixhawk飞控校准)
- 玩转四旋翼无人机(传感器)
- 玩转四旋翼无人机(DJI ROS SDK开发包及使用方法)
- 玩转四旋翼无人机(仿真)
- 玩转四旋翼无人机(姿态控制 in ArduPilot)
- 玩转四旋翼无人机(在rviz中显示飞机)
- 玩转Xwindow 终端[转载]
- 四元数插值
- 30分钟玩转Blog定制