Quaternion(四元数)和旋转以及Yaw, pitch, roll 的含义

小咪咪 2022-08-05 12:27 154阅读 0赞

<table style="width:1031px; color:rgb(64,64,64); font-family:Verdana,Arial,Helvetica,Futura,sans-serif; font-size:13px; text-align:justify"> 
 <tbody> 
  <tr> 
   <td style="border-top-width:1px; border-top-style:solid; border-top-color:rgb(34,34,34); border-left-width:1px; border-left-style:solid; border-left-color:rgb(34,34,34); width:1011.671875px; padding-right:1em; background-color:rgb(242,248,255)"> 
    <div> 
     <div style="font-size:1.6em; padding-left:0.4em; padding-top:0.2em">
       Quaternion(四元数)和旋转 
     </div> 
     <div style="font-size:9pt; padding-left:0.8em; margin-top:0.8em; margin-left:0.8em"> 
      <strong>作者:</strong>&nbsp;刘鹏 
     </div> 
     <div style="font-size:9pt; padding-left:0.8em; margin-top:0.4em; margin-left:0.8em; width:506px"> 
      <strong>日期:&nbsp;</strong>2011-06-10 
     </div> 
     <div style="font-size:9pt; padding-left:0.8em; margin-top:0.8em; margin-left:0.8em">
       本文介绍了四元数以及如何在OpenGL中使用四元数表示旋转。 
     </div> 
    </div> </td> 
  </tr> 
 </tbody> 
</table>

<table style="width:1031px; color:rgb(64,64,64); font-family:Verdana,Arial,Helvetica,Futura,sans-serif; font-size:13px; text-align:justify"> 
 <tbody> 
  <tr> 
   <td> <h4 style="margin-bottom:0.2em; color:black; font-size:10pt; font-family:Verdana,Arial,Helvetica,Futura,sans-serif; padding-top:0.1em; padding-bottom:0.05em"> Quaternion 的定义</h4> <p style="margin:0.4em 0.3em 0.5em 0.7em; line-height:1.35em; font-size:10pt"> 四元数一般定义如下：</p> <pre style="padding:1em; color:black; background-color:rgb(221,221,153); line-height:1.1em; margin-left:1em; overflow:scroll; width:905.5px">    q=w+xi+yj+zk
</pre> <p style="margin:0.4em 0.3em 0.5em 0.7em; line-height:1.35em; font-size:10pt">其中 w 是实数，x,y,z是虚数。同时，有:</p> <pre style="padding:1em; color:black; background-color:rgb(221,221,153); line-height:1.1em; margin-left:1em; overflow:scroll; width:905.5px">    i*i=-1

j*j=-1

k*k=-1

</pre> <p style="margin:0.4em 0.3em 0.5em 0.7em; line-height:1.35em; font-size:10pt">四元数也可以表示为：</p> <pre style="padding:1em; color:black; background-color:rgb(221,221,153); line-height:1.1em; margin-left:1em; overflow:scroll; width:905.5px">    q=[w,v]
</pre> <p style="margin:0.4em 0.3em 0.5em 0.7em; line-height:1.35em; font-size:10pt">其中v=(x,y,z)是矢量，w是标量，虽然v是矢量，但不能简单的理解为3D空间的矢量，它是4维空间中的的矢量，也是非常不容易想像的。</p> <p style="margin:0.4em 0.3em 0.5em 0.7em; line-height:1.35em; font-size:10pt">通俗的讲，一个四元数（Quaternion）描述了一个旋转轴和一个旋转角度。这个旋转轴和这个角度可以通过 Quaternion::ToAngleAxis转换得到。当然也可以随意指定一个角度一个旋转轴来构造一个Quaternion。这个角度是相对于单位四元数而言的，也可以说是相对于物体的初始方向而言的。</p> <p style="margin:0.4em 0.3em 0.5em 0.7em; line-height:1.35em; font-size:10pt">当用一个四元数乘以一个向量时，实际上就是让该向量围绕着这个四元数所描述的旋转轴，转动这个四元数所描述的角度而得到的向量。</p> <h4 style="margin-bottom:0.2em; color:black; font-size:10pt; font-family:Verdana,Arial,Helvetica,Futura,sans-serif; padding-top:0.1em; padding-bottom:0.05em"> 四元组的优点<sup><a href="http://www.linuxgraphics.cn/graphics/opengl_quaternion.html#fn.1" style="text-decoration:none; color:rgb(0,0,255)" rel="nofollow">1</a></sup></h4> <p style="margin:0.4em 0.3em 0.5em 0.7em; line-height:1.35em; font-size:10pt"> 有多种方式可表示旋转，如 axis/angle、欧拉角(Euler angles)、矩阵(matrix)、四元组等。 相对于其它方法，四元组有其本身的优点：</p> 
    <ul style="line-height:1.5em; margin:0.3em 0px 0px 1.5em; padding:0px"> 
     <li style="margin-bottom:0.1em; font-size:10pt">四元数不会有欧拉角存在的 gimbal lock 问题</li> 
     <li style="margin-bottom:0.1em; font-size:10pt">四元数由4个数组成，旋转矩阵需要9个数</li> 
     <li style="margin-bottom:0.1em; font-size:10pt">两个四元数之间更容易插值</li> 
     <li style="margin-bottom:0.1em; font-size:10pt">四元数、矩阵在多次运算后会积攒误差，需要分别对其做规范化(normalize)和正交化(orthogonalize)，对四元数规范化更容易</li> 
     <li style="margin-bottom:0.1em; font-size:10pt">与旋转矩阵类似，两个四元组相乘可表示两次旋转</li> 
    </ul> <h4 style="margin-bottom:0.2em; color:black; font-size:10pt; font-family:Verdana,Arial,Helvetica,Futura,sans-serif; padding-top:0.1em; padding-bottom:0.05em"> Quaternion 的基本运算<sup><a href="http://www.linuxgraphics.cn/graphics/opengl_quaternion.html#fn.1" style="text-decoration:none; color:rgb(0,0,255)" rel="nofollow">1</a></sup></h4> <h5 style="margin-bottom:0.2em; color:black; font-weight:normal; font-size:10pt; padding-top:0em; padding-bottom:0em"> Normalizing a quaternion</h5> <pre style="padding:1em; color:black; background-color:rgb(221,221,153); line-height:1.1em; margin-left:1em; overflow:scroll; width:905.5px">// normalising a quaternion works similar to a vector. This method will not do anything
// if the quaternion is close enough to being unit-length. define TOLERANCE as something
// small like 0.00001f to get accurate results
void Quaternion::normalise()
{
	// Don't normalize if we don't have to
	float mag2 = w * w + x * x + y * y + z * z;
	if (  mag2!=0.f &amp;&amp; (fabs(mag2 - 1.0f) &gt; TOLERANCE)) {
		float mag = sqrt(mag2);
		w /= mag;
		x /= mag;
		y /= mag;
		z /= mag;
	}
}

</pre> <h5 style="margin-bottom:0.2em; color:black; font-weight:normal; font-size:10pt; padding-top:0em; padding-bottom:0em"> The complex conjugate of a quaternion</h5> <pre style="padding:1em; color:black; background-color:rgb(221,221,153); line-height:1.1em; margin-left:1em; overflow:scroll; width:905.5px">// We need to get the inverse of a quaternion to properly apply a quaternion-rotation to a vector
// The conjugate of a quaternion is the same as the inverse, as long as the quaternion is unit-length
Quaternion Quaternion::getConjugate()
{
	return Quaternion(-x, -y, -z, w);
}

</pre> <h5 style="margin-bottom:0.2em; color:black; font-weight:normal; font-size:10pt; padding-top:0em; padding-bottom:0em"> Multiplying quaternions</h5> <pre style="padding:1em; color:black; background-color:rgb(221,221,153); line-height:1.1em; margin-left:1em; overflow:scroll; width:905.5px">// Multiplying q1 with q2 applies the rotation q2 to q1
Quaternion Quaternion::operator* (const Quaternion &amp;rq) const
{
	// the constructor takes its arguments as (x, y, z, w)
	return Quaternion(w * rq.x + x * rq.w + y * rq.z - z * rq.y,
	                  w * rq.y + y * rq.w + z * rq.x - x * rq.z,
	                  w * rq.z + z * rq.w + x * rq.y - y * rq.x,
	                  w * rq.w - x * rq.x - y * rq.y - z * rq.z);
}

</pre> <h5 style="margin-bottom:0.2em; color:black; font-weight:normal; font-size:10pt; padding-top:0em; padding-bottom:0em"> Rotating vectors</h5> <pre style="padding:1em; color:black; background-color:rgb(221,221,153); line-height:1.1em; margin-left:1em; overflow:scroll; width:905.5px">// Multiplying a quaternion q with a vector v applies the q-rotation to v
Vector3 Quaternion::operator* (const Vector3 &amp;vec) const
{
	Vector3 vn(vec);
	vn.normalise();

Quaternion vecQuat, resQuat;
	vecQuat.x = vn.x;
	vecQuat.y = vn.y;
	vecQuat.z = vn.z;
	vecQuat.w = 0.0f;

resQuat = vecQuat * getConjugate();
	resQuat = *this * resQuat;

return (Vector3(resQuat.x, resQuat.y, resQuat.z));
}

sinAngle = sin(angle);

x = (vn.x * sinAngle);
	y = (vn.y * sinAngle);
	z = (vn.z * sinAngle);
	w = cos(angle);
}

</pre> <h5 style="margin-bottom:0.2em; color:black; font-weight:normal; font-size:10pt; padding-top:0em; padding-bottom:0em"> Quaternion from Euler angles</h5> <pre style="padding:1em; color:black; background-color:rgb(221,221,153); line-height:1.1em; margin-left:1em; overflow:scroll; width:905.5px">// Convert from Euler Angles
void Quaternion::FromEuler(float pitch, float yaw, float roll)
{
	// Basically we create 3 Quaternions, one for pitch, one for yaw, one for roll
	// and multiply those together.
	// the calculation below does the same, just shorter

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-&gt;x = sinr * cosp * cosy - cosr * sinp * siny;
	this-&gt;y = cosr * sinp * cosy + sinr * cosp * siny;
	this-&gt;z = cosr * cosp * siny - sinr * sinp * cosy;
	this-&gt;w = cosr * cosp * cosy + sinr * sinp * siny;

normalise();
}

</pre> <h5 style="margin-bottom:0.2em; color:black; font-weight:normal; font-size:10pt; padding-top:0em; padding-bottom:0em"> Quaternion to Matrix</h5> <pre style="padding:1em; color:black; background-color:rgb(221,221,153); line-height:1.1em; margin-left:1em; overflow:scroll; width:905.5px">// Convert to Matrix
Matrix4 Quaternion::getMatrix() const
{
	float x2 = x * x;
	float y2 = y * y;
	float z2 = z * z;
	float xy = x * y;
	float xz = x * z;
	float yz = y * z;
	float wx = w * x;
	float wy = w * y;
	float wz = w * z;

// This calculation would be a lot more complicated for non-unit length quaternions
	// Note: The constructor of Matrix4 expects the Matrix in column-major format like expected by
	//   OpenGL
	return Matrix4( 1.0f - 2.0f * (y2 + z2), 2.0f * (xy - wz), 2.0f * (xz + wy), 0.0f,
			2.0f * (xy + wz), 1.0f - 2.0f * (x2 + z2), 2.0f * (yz - wx), 0.0f,
			2.0f * (xz - wy), 2.0f * (yz + wx), 1.0f - 2.0f * (x2 + y2), 0.0f,
			0.0f, 0.0f, 0.0f, 1.0f)
}

</pre> <h5 style="margin-bottom:0.2em; color:black; font-weight:normal; font-size:10pt; padding-top:0em; padding-bottom:0em"> Quaternion to axis-angle</h5> <pre style="padding:1em; color:black; background-color:rgb(221,221,153); line-height:1.1em; margin-left:1em; overflow:scroll; width:905.5px">// Convert to Axis/Angles
void Quaternion::getAxisAngle(Vector3 *axis, float *angle)
{
	float scale = sqrt(x * x + y * y + z * z);
	axis-&gt;x = x / scale;
	axis-&gt;y = y / scale;
	axis-&gt;z = z / scale;
	*angle = acos(w) * 2.0f;
}

</pre> <h5 style="margin-bottom:0.2em; color:black; font-weight:normal; font-size:10pt; padding-top:0em; padding-bottom:0em"> 球形线性插值</h5> <p style="margin:0.4em 0.3em 0.5em 0.7em; line-height:1.35em; font-size:10pt">尽管线性插值很有效，但不能以恒定的速率描述q1到q2之间的曲线，这也是其弊端，我们需要找到一种插值方法使得q1-&gt;q(t)之间的夹角θ是线性的，即θ(t)=(1-t)θ1+t*θ2，这样我们得到了球形线性插值函数q(t)，如下：</p> <pre style="padding:1em; color:black; background-color:rgb(221,221,153); line-height:1.1em; margin-left:1em; overflow:scroll; width:905.5px">q(t)=q1 * sinθ(1-t)/sinθ + q2 * sinθt/sineθ

</pre> <p style="margin:0.4em 0.3em 0.5em 0.7em; line-height:1.35em; font-size:10pt">如果使用D3D，可以直接使用 D3DXQuaternionSlerp 函数就可以完成这个插值过程。</p> <h4 style="margin-bottom:0.2em; color:black; font-size:10pt; font-family:Verdana,Arial,Helvetica,Futura,sans-serif; padding-top:0.1em; padding-bottom:0.05em"> 用 Quaternion 实现 Camera 旋转</h4> <p style="margin:0.4em 0.3em 0.5em 0.7em; line-height:1.35em; font-size:10pt"> 总体来讲，Camera 的操作可分为如下几类：</p> 
    <ul style="line-height:1.5em; margin:0.3em 0px 0px 1.5em; padding:0px"> 
     <li style="margin-bottom:0.1em; font-size:10pt">沿直线移动</li> 
     <li style="margin-bottom:0.1em; font-size:10pt">围绕某轴自转</li> 
     <li style="margin-bottom:0.1em; font-size:10pt">围绕某轴公转</li> 
    </ul> <p style="margin:0.4em 0.3em 0.5em 0.7em; line-height:1.35em; font-size:10pt">下面是一个使用了 Quaternion 的 Camera 类：</p> <pre style="padding:1em; color:black; background-color:rgb(221,221,153); line-height:1.1em; margin-left:1em; overflow:scroll; width:905.5px">    class Camera {

private:
        Quaternion m_orientation;

public:
        void rotate (const Quaternion&amp; q);
        void rotate(const Vector3&amp; axis, const Radian&amp; angle);

void roll (const GLfloat angle);
        void yaw (const GLfloat angle);
        void pitch (const GLfloat angle);

};

void Camera::rotate(const Quaternion&amp; q)
    {
        // Note the order of the mult, i.e. q comes after
        m_Orientation = q * m_Orientation;

}

void Camera::rotate(const Vector3&amp; axis, const Radian&amp; angle)
    {
        Quaternion q;
        q.FromAngleAxis(angle,axis);
        rotate(q);
    }

void Camera::roll (const GLfloat angle) //in radian
    {

Vector3 zAxis = m_Orientation * Vector3::UNIT_Z;
        rotate(zAxis, angleInRadian);

}

void Camera::yaw (const GLfloat angle)  //in degree
    {

Vector3 yAxis;

{
            // Rotate around local Y axis
            yAxis = m_Orientation * Vector3::UNIT_Y;
        }

rotate(yAxis, angleInRadian);

}

void Camera::pitch (const GLfloat angle)  //in radian
    {

Vector3 xAxis = m_Orientation * Vector3::UNIT_X;
        rotate(xAxis, angleInRadian);

}

void Camera::gluLookAt() {
        GLfloat m[4][4];

identf(&amp;m[0][0]);
        m_Orientation.createMatrix (&amp;m[0][0]);

glMultMatrixf(&amp;m[0][0]);
        glTranslatef(-m_eyex, -m_eyey, -m_eyez);
    }

</pre> <h4 style="margin-bottom:0.2em; color:black; font-size:10pt; font-family:Verdana,Arial,Helvetica,Futura,sans-serif; padding-top:0.1em; padding-bottom:0.05em"> 用 Quaternion 实现 trackball</h4> <p style="margin:0.4em 0.3em 0.5em 0.7em; line-height:1.35em; font-size:10pt"> 用鼠标拖动物体在三维空间里旋转，一般设计一个 trackball，其内部实现也常用四元数。</p> <pre style="padding:1em; color:black; background-color:rgb(221,221,153); line-height:1.1em; margin-left:1em; overflow:scroll; width:905.5px">class TrackBall
{
public:
    TrackBall();

void push(const QPointF&amp; p);
    void move(const QPointF&amp; p);
    void release(const QPointF&amp; p);

QQuaternion rotation() const;

private:
    QQuaternion m_rotation;
    QVector3D m_axis;
    float m_angularVelocity;

QPointF m_lastPos;

};

void TrackBall::move(const QPointF&amp; p)
{

if (!m_pressed)
        return;

QVector3D lastPos3D = QVector3D(m_lastPos.x(), m_lastPos.y(), 0.0f);
    float sqrZ = 1 - QVector3D::dotProduct(lastPos3D, lastPos3D);
    if (sqrZ &gt; 0)
        lastPos3D.setZ(sqrt(sqrZ));
    else
        lastPos3D.normalize();

QVector3D currentPos3D = QVector3D(p.x(), p.y(), 0.0f);
    sqrZ = 1 - QVector3D::dotProduct(currentPos3D, currentPos3D);
    if (sqrZ &gt; 0)
        currentPos3D.setZ(sqrt(sqrZ));
    else
        currentPos3D.normalize();

m_axis = QVector3D::crossProduct(lastPos3D, currentPos3D);
    float angle = 180 / PI * asin(sqrt(QVector3D::dotProduct(m_axis, m_axis)));

m_axis.normalize();
    m_rotation = QQuaternion::fromAxisAndAngle(m_axis, angle) * m_rotation;

m_lastPos = p;

}

</pre> <h4 style="margin-bottom:0.2em; color:black; font-size:10pt; font-family:Verdana,Arial,Helvetica,Futura,sans-serif; padding-top:0.1em; padding-bottom:0.05em"> Yaw, pitch, roll 的含义<sup><a href="http://www.linuxgraphics.cn/graphics/opengl_quaternion.html#fn.3" style="text-decoration:none; color:rgb(0,0,255)" rel="nofollow">3</a></sup></h4> <p style="margin:0.4em 0.3em 0.5em 0.7em; line-height:1.35em; font-size:10pt"> Yaw – Vertical axis：</p> 
    <table> 
     <tbody> 
      <tr> 
       <td align="center"><img src="http://www.linuxgraphics.cn/images/graphics/opengl_yaw.gif" alt="yaw"></td> 
      </tr> 
      <tr> 
       <td align="center">yaw</td> 
      </tr> 
     </tbody> 
    </table> <p style="margin:0.4em 0.3em 0.5em 0.7em; line-height:1.35em; font-size:10pt">Pitch – Lateral axis</p> 
    <table> 
     <tbody> 
      <tr> 
       <td align="center"><img src="http://www.linuxgraphics.cn/images/graphics/opengl_pitch.gif" alt="pitch"></td> 
      </tr> 
      <tr> 
       <td align="center">pitch</td> 
      </tr> 
     </tbody> 
    </table> <p style="margin:0.4em 0.3em 0.5em 0.7em; line-height:1.35em; font-size:10pt">Roll – Longitudinal axis</p> 
    <table> 
     <tbody> 
      <tr> 
       <td align="center"><img src="http://www.linuxgraphics.cn/images/graphics/opengl_roll.gif" alt="roll"></td> 
      </tr> 
      <tr> 
       <td align="center">roll</td> 
      </tr> 
     </tbody> 
    </table> <p style="margin:0.4em 0.3em 0.5em 0.7em; line-height:1.35em; font-size:10pt">The Position of All three axes</p> 
    <table> 
     <tbody> 
      <tr> 
       <td align="center"><img src="http://www.linuxgraphics.cn/images/graphics/opengl_yaw_pitch_roll.png" alt="Yaw Pitch Roll"></td> 
      </tr> 
      <tr> 
       <td align="center">Yaw Pitch Roll</td> 
      </tr> 
     </tbody> 
    </table> <h4 style="margin-bottom:0.2em; color:black; font-size:10pt; font-family:Verdana,Arial,Helvetica,Futura,sans-serif; padding-top:0.1em; padding-bottom:0.05em"> Reference</h4> 
    <ol style="line-height:1.5em; margin:0.3em 0px 0px 3.2em; padding:0px"> 
     <li style="margin-bottom:0.1em; font-size:10pt"><a href="http://gpwiki.org/index.php/OpenGL:Tutorials:Using_Quaternions_to_represent_rotation" style="text-decoration:none; color:rgb(0,0,255)" rel="nofollow">Using Quaternions to represent rotation</a></li> 
     <li style="margin-bottom:0.1em; font-size:10pt"><a href="http://school.ogdev.net/ArticleShow.asp?id=6058&amp;categoryid=2" style="text-decoration:none; color:rgb(0,0,255)" rel="nofollow">四元数</a></li> 
     <li style="margin-bottom:0.1em; font-size:10pt"><a href="http://www.qingran.net/2010/06/yaw-pitch-roll%E7%9A%84%E5%90%AB%E4%B9%89/" style="text-decoration:none; color:rgb(0,0,255)" rel="nofollow">Yaw,pitch,roll的含义</a></li> 
     <li></li> 
    </ol> </td> 
  </tr> 
 </tbody> 
</table>