形象解说四元数

形象解说四元数
By daode1212 2016-03-16
前言:
四元数（Quaternions）是由爱尔兰数学家哈密顿(William Rowan Hamilton,1805-1865）在1843年发明的数学概念。

复数、向量、矩阵都是数学中的基本要素，就如同编程中的数组、对象、集合那样。

四元数是一种超复数，是复数与三维向量的复合体。

四元数也有加法、减法、乘法、但是四元数的乘法不符合交换律（commutative law），即a*b <> b*a，而且，还有转置、规范化、共轭三种运算。

由于它在描述三维旋转、姿态方面的一些特有优点，所以在飞行器（飞机，火箭，导弹等），机器人姿态的控制中常用到。

数学手册中在代数结构的“群-环-域”中稍有点介绍，它属于不可交换的除环，称哈密顿四元数体。

以下是一些四元数运算的效果图：
四元数理论创立人：William Rowan Hamilton,1805-1865
一，四元数的几种表示形式:
OpenTK中，为建立四元数提供了多种方式：
public Quaternion(float x, float y, float z, float w)；
public Quaternion(OpenTK.Vector3v, float w)；
例如用Quaternion(float x, float y, float z, float w)：
OpenTK.Quaternion q = new OpenTK.Quaternion(0.51f, -0.71f, 0.31f, 0.7071f);
1, 四元数建构方式一：
i^2=j^2=k^2=-1
ij=-ji=k，jk=-kj=i，ki=-ik=j
q=w+ix+jy+kz，i,j,k分别对应轴向量X(1,0,0),Y(0,1,0),Z(0,0,1)
2, 四元数建构方式二：转动角之半+轴向量的方向余弦：
3, 四元数建构方式三：转动角之半+单位球面上的点：
二，四元数的模
如q是四元数，OpenTK中有:
1, q.Length;
返回值是:
2, q.LengthSquared;
返回值是:，与点积（内积）q·q是一致的。

三，四元数的规范化[单位化]
OpenTK.Quaternion q2= OpenTK.Quaternion.Normalize(q1); //单位化,使平方和等于1
四元数建构方式二、方式三在这方面有明显优势。

四，四元数的共轭
OpenTK.Quaternion q2= OpenTK.Quaternion.Conjugate(q1);//共轭,仅向量取反方向若q=w+(ix+jy+kz); p=w-(ix+jy+kz);则q,p称为共轭,记q=p*或p=q*;
共轭为取四元数中的标量与向量提供了方便：
1、四元数的标量部：S=(q+q*)/2；
2、四元数的向量部：V=(q-q*)/2；
五，四元数的转置
OpenTK.Quaternion q2 = OpenTK.Quaternion.Invert(q1); //倒数,转置：方向相反，模互为倒数
六，四元数的逻辑运算
主要是相等，不相等的判断：
q1 == q2, q1 != q2
bool b = q2.Equals(q1);
七，四元数的算术运算
运算符有：+、-、*，对应有Add(q,p )、Sub(q,p )、 Multiply( q,*)
加法的定义：
public static OpenTK.Quaternion Add(OpenTK.Quaternion left, OpenTK.Quaternion right)
减法的定义：
public static OpenTK.Quaternion Sub(OpenTK.Quaternion left, OpenTK.Quaternion right)
乘法有二种：
public static OpenTK.Quaternion Multiply(OpenTK.Quaternion quaternion, float scale)
public static OpenTK.Quaternion Multiply(OpenTK.Quaternion left, OpenTK.Quaternion right)
注意，乘法没有交换律：q2*q1 != q1*q2
乘法的定义和数学算法如下：
例如：
四元数与标量相乘，是对原四元数在正方向或反方向进行伸缩：
OpenTK.Quaterniond q2 = OpenTK.Quaterniond.Multiply(q1, 0.5f); //q1*0.5
四元数与四元数相乘，是两种旋转的叠加作用：
OpenTK.Quaterniond q3 = OpenTK.Quaterniond.Multiply(q2, q1); //q2*q1
q3 = q1·q2 =
(a1a2-b1b2-c1c2-d1d2) +
i(a1b2+b1a2+c1d2-d1c2) +
j(a1c2+a2c1+b2d1-d2b1) +
k(a1d2+d1a2+b1c2-c1b2)
四元数与四元数相乘是不容易心算的游戏，就方向的变化也很难摸准，见下例：
//向量连乘效果图：
private void QuatMults() //向量连乘效果图
{
qs[0] = new OpenTK.Quaternion(.5f, -.6f, -.7f, .4f);
DwQuat(qs[0], Color.FromArgb(255, 255, 0), 4f);
qs[1] = new OpenTK.Quaternion(-.6f, -.5f, .4f, .6f);
DwQuat(qs[1], Color.FromArgb(0, 255, 255), 4f);
for (int i = 0; i < 255; i++)
{
qs[i + 2] = OpenTK.Quaternion.Multiply(qs[i], qs[i + 1]); qs[i + 2] = OpenTK.Quaternion.Normalize(qs[i + 2]);
DwQuat(qs[i + 2], Color.FromArgb(i, i, i), 2f);
}
}
//表现四元数：
private void DwQuat(OpenTK.Quaternion q,Color clr,float d)
{
//画线段：
GL.LineWidth(d);
GL.Begin(BeginMode.Lines);
GL.Color3((byte)clr.R, (byte)clr.G, (byte)clr.B);
GL.Vertex3(0, 0, 0);
GL.Vertex3(q.X, q.Y, q.Z);
GL.End();
//画球：
GL.PushMatrix();
GL.Translate(q.X, q.Y, q.Z);
GL.Color3((byte)clr.R, (byte)clr.G, (byte)clr.B);
Glu.Sphere(Glu.NewQuadric(), .05, 9, 9);//画球
GL.PopMatrix();
}
三维效果如下：
正是这一特点，可以让程序建构出很多艺术品出来。

四元数的除法，如果按乘法的逆运算是除法这一法则去找因数1、因数2，要解一个4X4的联列方程组，可参考下面的矩阵：
先用克莱姆法则判断一下，它的解可能唯一，可能无解，也可能有无数多组解。

设：q3 = q1*q2（q1:左四元数因子，q2:右四元数因子），
如果把左、右因子分别叫作左、右商，我们可以自己构造函数进行计算：
//除法1: 求左商,右商:
OpenTK.Quaternion q= QuaternionDiv(qs[1], qs[0], 1); // LR=1:返回右商
OpenTK.Quaternion q1 = QuaternionDiv(qs[1], qs[0], -1); // LR=-1:返回左商
//除法2: 以qs[0]的转置乘qs[1]
OpenTK.Quaternion q2 = OpenTK.Quaternion.Multiply(qs[2], qs[1]);
实际计算表明，两种除法不一致，所以不应该用转置来替代除法。

四元数连除三维图
为了测试255次连续相除，设计算法如下:
进行12次迭代后制作了下图
之前另一算法的效果图:
八，“轴+角”方式转四元数
public static OpenTK.Quaternion FromAxisAngle(OpenTK.Vector3axis, float angle)
其中OpenTK.Vector3axis 是轴向量，float angle 是以弧度表示的角
如：
OpenTK.Quaternion q3 = OpenTK.Quaternion.FromAxisAngle(new Vector3(0.4f, 0.4f, 0.8f), PI / 2f); 九，四元数转“轴+角”方式
1，public OpenTK.Vector4ToAxisAngle()
2，public void ToAxisAngle(out OpenTK.Vector3axis, out float angle)
其中OpenTK.Vector4 形如（float x, float y, float z, float angle）
float angle是以弧度表示的角。

如：OpenTK.Vector4 v4 = q3.ToAxisAngle();
十，四元数到欧拉角的转换
//四元数转欧拉角,欧拉角的球面RGB(用自定义类实现):
private void QuaternionToEulerAngle_BallRGB()
{
//声明类与结构:
Eular EL = new Eular();
Quaternion q = new Quaternion();
EulerAngle ea = new EulerAngle();
//利用三角式生成四元数:
double PI = 3.1416;
//double a = PI / 8; double b = PI / 2; double c = 0; //转轴:Y轴,转动角:PI/4
//double a = PI / 2; double b = 0; double c = 0;//转轴:X轴,转动角:PI
//double a = PI / 2; double b = PI / 2; double c = 0;//转轴:Y轴,转动角:PI
//double a = PI / 2; double b = 0; double c = PI / 2;//转轴:Z轴,转动角:PI
double a = PI/2; //转动角:2a(=PI)
double b = PI / 2;
double c = 0;
for (b = -PI; b < PI; b += .1)
{
for (c = -PI/2; c < PI/2; c += .1)
{
q.w = Math.Cos(a);
q.x = Math.Cos(b) * Math.Cos(c) * Math.Sin(a);
q.y = Math.Sin(b) * Math.Cos(c) * Math.Sin(a);
q.z = Math.Sin(c) * Math.Sin(a);
//四元数转欧拉角:
ea = EL.QuaternionToEulerAngle(q);
byte R = (byte)(255 * (PI + ea.pitch) / (2 * PI));
byte G = (byte)(255 * (PI + ea.yaw) / (2 * PI));
byte B = (byte)(255 * (PI + ea.roll) / (2 * PI));
GL.Color3(0f, 0f, 0f);
GL.LineWidth(4f);
GL.Begin(BeginMode.Lines);
GL.Vertex3(0, 0, 0);
GL.Color3(R, G, B);
GL.Vertex3(q.x, q.y, q.z);
GL.End();
}
}
}
四元数转欧拉角,欧拉角的球面RGB
四元数转欧拉角的数学算法如下，不同的坐标系可能要作适当的调整：
如果分别为绕Z轴、Y轴、X轴的旋转角度，则有：
十一，欧拉角到四元数的转换
private void EulerAngleToQuaternion()
{
double A = Math.PI / 6; double B = Math.PI / 4; double C = Math.PI / 2;
double c1 = Math.Cos(A / 2);
double s1 = Math.Sin(A / 2);
double c2 = Math.Cos(B / 2);
double s2 = Math.Sin(B / 2);
double c3 = Math.Cos(C / 2);
double s3 = Math.Sin(C / 2);
double w = c1 * c2 * c3 - s1 * s2 * s3;
double x = s1 * s2 * c3 + c1 * c2 * s3;
double y = s1 * c2 * c3 + c1 * s2 * s3;
double z = c1 * s2 * c3 - s1 * c2 * s3;
//窗体标题栏显示：
this.Parent.Text = "W,X,Y,Z=" + w+","+x+","+y+","+z;
}
欧拉角转四元数的数学算法如下，不同的坐标系可能要作适当的调整：
十二，向量绕四元数旋转
//向量绕四元数旋转,就是用四元数作用于向量：
private void VectorRotByQuaternion()
{
//======================================
Vector3 v01 = new Vector3(1f, 0f, -1f);
double PI = 4.1416;
float x = (float)(Math.Sin(PI / 8) * Math.Cos(PI / 4) * (float)Math.Cos(PI / 9)); float y = (float)(Math.Sin(PI / 8) * Math.Cos(PI / 4) * (float)Math.Sin(PI / 9)); float z = (float)(Math.Sin(PI / 8) * Math.Sin(PI / 9));
float w = (float)Math.Cos(PI / 8);
OpenTK.Quaternion q = new OpenTK.Quaternion(x, y, z, w);
Vector3 v5 = OpenTK.Vector3.Transform(v01, q);
GL.PointSize(6f);
GL.LineWidth(4f);
GL.Begin(BeginMode.Lines);
GL.Color3(.0f, 0f, .5f);
GL.Vertex3(0, 0, 0);
GL.Vertex3(v01);//深蓝色：原来的向量
GL.Color3(.5f, 0f, .0f);
GL.Vertex3(0, 0, 0);
GL.Vertex3(v5);//深红色：旋转后的V01
GL.Color3(1f, 1f, 1f);
GL.Vertex3(0, 0, 0);
GL.Vertex3(5 * x, 5 * y, 5 * z); //白色：四元数
GL.End();
//======================================
}
见下图(深蓝色：原来的向量,深红色：旋转后的向量, 白色：四元数)
十三，二个给定的向量的线性混合:
private void LineLerp()
{
//======================================
Vector3 v1 = new Vector3(1f, 0f, 2f);
Vector3 v2 = new Vector3(0f, 2f, 1f);
GL.PointSize(6f);
GL.LineWidth(4f);
GL.Begin(BeginMode.Lines);
for (float f = 0; f < 1; f += 0.1f)
{
GL.Color3(1f, f, 1 - f);
GL.Vertex3(0, 0, 0);
Vector3 vf = OpenTK.Vector3.Lerp(v1, v2, f); GL.Vertex3(vf);
}
GL.End();
//======================================
}
十四，2个给定的向量的球面插值:
private void BallLerp()
{
//======================================
Vector3 v1 = new Vector3(.7f, 1f, -.2f);
Vector3 v2 = new Vector3(-1f, .2f, .7f);
double PI = 3.1416;
GL.PointSize(6f);
GL.LineWidth(4f);
GL.Begin(BeginMode.Lines);
for (float f = 0.01f; f < 1; f += 0.01f)
{
GL.Color3(1f, f, 1 - f);
GL.Vertex3(0, 0, 0);
//余弦式球面插值:
Vector3 vMul_1 = OpenTK.Vector3.Multiply(v1, (float)(Math.Cos(f)));
Vector3 vMul_2 = OpenTK.Vector3.Multiply(v2, (float)(Math.Cos(1 - f)));
Vector3 vAdd = OpenTK.Vector3.Add(vMul_1, vMul_2);
////指数式插值:
//Vector3 vMul_1 = OpenTK.Vector3.Mult(v1, (float)( Math.Exp( -f*f)));
//Vector3 vMul_2 = OpenTK.Vector3.Mult(v2, (float)( Math.Exp(f*f-1)));
//Vector3 vAdd = OpenTK.Vector3.Add(vMul_1, vMul_2);
////正弦式球面插值:
//v1 = OpenTK.Vector3.Normalize(v1);
//v2 = OpenTK.Vector3.Normalize(v2);
//double g =Math.Acos(OpenTK.Vector3.Dot(v1, v2));
//Vector3 vMul_1 = OpenTK.Vector3.Multiply(v1, (float)(Math.Sin(f * g)/Math.Sin(g))); //Vector3 vMul_2 = OpenTK.Vector3.Multiply(v2, (float)(Math.Sin((1-f)*g)/Math.Sin(g))); //Vector3 vAdd = OpenTK.Vector3.Add(vMul_1, vMul_2);
GL.Vertex3(vAdd);
}
GL.End();
//======================================
}
余弦式球面插值
十五，四元数对飞行姿态的控制
当运用高阶、多元偏微后，可模拟各种姿态的，各种加速度效果的空间运动。

图15-1 螺旋线切线引导的方向
图15-2 球面切线引导的方向
这些姿态控制，在关节机器人自动路径与自动姿态生成程序中起关键作用。

十六，四元数的三维面貌
若以四元数建构方式三，将w 作为颜色参数之一，作函数
，
就可给出如下二图：
基于四元数的三维屏保效果
十七，对四元数应用递归
以下是从一个四元数出发,用加法与乘法多次递归运算,即运用递归表达式
)
(1d i i i Q Q Q Q +=+λ所得的三维图形：
十八，对四元数应用迭代[一]
利用三维单位球面的四元数点集,再利用四元数的各种运算(如加法,减法,乘法,转置,规范化等)去迭代,如运用表达式:
)
(1d i i i Q Q Q Q +=+λ
并配合收敛与发散的特性,可得到下面各三维图:
迭代的方式,有终点型的,也有增量型的.上述6图就是只取最后一次的结果,这样总的点数较少,下面是增量型的,显示每一次迭代的点集.
迭代的结果,有时向外延伸(上述6图),有时向内延伸(下面2图):
转置,或数量积等方法约束,如:用以下表达式进行12次迭代:
即得到如下三维图像:
用四元数描述空间运动的好处是,既有了三维运动的线路,又同时可以控制(pitch、yow、roll)三个角(参见欧拉角)，即姿态可控制。

真是控制机器人末端手关节的灵巧神经。

OpenTK则提供了强大的三维表现工具，使各种算法得以直观形象地显现出来。

十九，对四元数应用迭代[二]
递归与迭代的表达式不同，或模式不同，产生的三维效果也大不相同。

生成三维运动的轨迹是基础，理重要的是每一个点都有自己的姿态，这也正是我们重视四元数的根本所在。

如：
改变迭代表达式，配合内置GLU立体件，可出现类似“场”的三维效果:
建议浏览分辨率：1024*768
版权所有：daode1212(微信号)，QQ ：1501488900, 2016-03-16。