旋转向量与旋转矩阵的联系、点乘点积的意义、飞机的姿态角

1、旋转向量与旋转矩阵的联系：

   处理三维旋转问题时，通常采用旋转矩阵的方式来描述。一个向量乘以旋转矩阵等价于向量以某种方式进行旋转。除了采用旋转矩阵描述外，还可以用旋转向量来描述旋转，旋转向量的长度（模）表示绕轴逆时针旋转的角度（弧度）。旋转向量与旋转矩阵可以通过罗德里格斯（Rodrigues）变换进行转换。

OpenCV实现Rodrigues变换的函数为

int cvRodrigues2( const CvMat* src, CvMat* dst, CvMat* jacobian=0 );

src为输入的旋转向量（3x1或者1x3）或者旋转矩阵（3x3）。

dst为输出的旋转矩阵（3x3）或者旋转向量（3x1或者1x3）。

jacobian为可选的输出雅可比矩阵（3x9或者9x3），是输入与输出数组的偏导数。

1.旋转角度

已知旋

3.  罗德里格旋转公式(Rodrigues' rotation formula)

3.1 公式

已知单位向量 ，将它旋转θ角。由罗德里格旋转公式，可知对应的旋转矩阵 ：

其中I是3x3的单位矩阵，

 是叉乘中的反对称矩阵r：

转前向量为P, 旋转后变为Q。由点积定义可知：

可推出P，Q之间的夹角为：

2. 旋转轴

由1中可知，旋转角所在的平面为有P和Q所构成的平面，那么旋转轴必垂直该平面。

假定旋转前向量为a(a1, a2, a3)，旋转后向量为b（b1, b2, b3)。由叉乘定义得：

所以旋转轴c(c1, c2, c3)为：

3. 罗德里格旋转公式(Rodrigues' rotation formula)

3.1 公式

已知单位向量 ，将它旋转θ角。由罗德里格旋转公式，可知对应的旋转矩阵 ：

公式（1）：

其中I是3x3的单位矩阵，

其中的R[0][0]本来应当是：1+cosa+（-Wz^2-Wy^2）(1-cosa),但这里使用的是：，是化简后的式子。

因为其在前面有个 r=r/norm(r)，这是对向量r进行归一化是的有个 X^2+Y^2+Z^2=1;

 是叉乘中的反对称矩阵r：

其C#代码实现过程为：

根据旋转前后的两个向量值，使用上面的方法，先求出旋转角度和旋转轴，然后用罗德里格旋转公式即可求出对应的旋转矩阵。

void Calculation(double[] vectorBefore, double[] vectorAfter){    double[] rotationAxis;    double rotationAngle;    double[,] rotationMatrix;    rotationAxis = CrossProduct(vectorBefore, vectorAfter);    rotationAngle = Math.Acos(DotProduct(vectorBefore, vectorAfter) / Normalize(vectorBefore) / Normalize(vectorAfter));    rotationMatrix = RotationMatrix(rotationAngle, rotationAxis);}double[] CrossProduct(double[] a, double[] b){    double[] c = new double[3];    c[0] = a[1] * b[2] - a[2] * b[1];    c[1] = a[2] * b[0] - a[0] * b[2];    c[2] = a[0] * b[1] - a[1] * b[0];    return c;}double DotProduct(double[] a, double[] b){    double result;    result = a[0] * b[0] + a[1] * b[1] + a[2] * b[2];    return result;}double Normalize(double[] v){    double result;    result = Math.Sqrt(v[0] * v[0] + v[1] * v[1] + v[2] * v[2]);    return result;}double[,] RotationMatrix(double angle, double[] u){    double norm = Normalize(u);    double[,] rotatinMatrix = new double[3,3];    u[0] = u[0] / norm;   //这里就是上面的归一化。    u[1] = u[1] / norm;    u[2] = u[2] / norm;    rotatinMatrix[0, 0] = Math.Cos(angle) + u[0] * u[0] * (1 - Math.Cos(angle));   //这就是上面的<span style="color: rgb(255, 0, 0); font-family: 宋体; font-size: 16px;">公式（1）矩阵</span>    rotatinMatrix[0, 1] = u[0] * u[1] * (1 - Math.Cos(angle) - u[2] * Math.Sin(angle));    rotatinMatrix[0, 2] = u[1] * Math.Sin(angle) + u[0] * u[2] * (1 - Math.Cos(angle));    rotatinMatrix[1, 0] = u[2] * Math.Sin(angle) + u[0] * u[1] * (1 - Math.Cos(angle));    rotatinMatrix[1, 1] = Math.Cos(angle) + u[1] * u[1] * (1 - Math.Cos(angle));    rotatinMatrix[1, 2] = -u[0] * Math.Sin(angle) + u[1] * u[2] * (1 - Math.Cos(angle));    rotatinMatrix[2, 0] = -u[1] * Math.Sin(angle) + u[0] * u[2] * (1 - Math.Cos(angle));    rotatinMatrix[2, 1] = u[0] * Math.Sin(angle) + u[1] * u[2] * (1 - Math.Cos(angle));    rotatinMatrix[2, 2] = Math.Cos(angle) + u[2] * u[2] * (1 - Math.Cos(angle));    return rotatinMatrix;}

小知识点：

1、点积的意义：

设向量A=(x1,y1,z1),B=(x2,y2,z2）

(一)向量的点积

向量的点积定义：
           AB=|A||B|cos<A,B>

其运算结果是一个常量。将其转换成矩阵乘法是

AB = matrix(A)*matrix(B)=(x1,y1,z1)*(x2,y2,z2)=x1*x2 + y1*y2 + z1*z2

其主要是用来求两个向量的夹角；这在图像处理的主要用来判定两条边是否是垂直，用于矩形判定。其是没有什么几何意义的。因为其是其中一条边长的投影与另一个边长的乘积，这个可以用来判断构成的矩形的边长是否足够长。

(二)向量的叉积

向量的叉积的模定义：

           |A×B|=|A||B|sin<A,B>

其几何意义是以|A|，|B|为边长的平行四边形的面积。

向量的叉积定义：

           A×B=( y1z2 - y2z1 , x1z2 - x2z1 , x1y2 - x2y1)

利用向量的叉乘性质可以求取一个平面的法向量。这里的叉乘是定义旋转轴的。

物理意义：。
        向量的叉积很有意思，按照爱因斯坦的相对论，在观察一个目标运动时，需要采用特定的参照系。
        利用向量的叉积可以创造出一个新的维度，而这个维度是独立于(垂直于)先前这个空间的，因此，在这个新的维度空间可以当成目标运动的参照系。

3、反对称矩阵：

反对称矩阵定义是：A= - AT

其中opencv里的罗德里格函数解算出来的矩阵是RxRyRz，其如下：

这时我们解算那个姿态角的时候就需要使用到公式：

因为不太的乘顺序，需要不同的解方法，其如下：

虽然矩阵的相乘符合结合律，但不符合掉换率。

4、飞机的姿态：

要了解飞机姿态，需要首先知道什么是地面坐标系和机体坐标系。
■地面坐标系（earth-surface inertial reference frame）Sg--------Oxgygzg


①在地面上选一点Og
②使xg轴在水平面内并指向某一方向
③zg轴垂直于地面并指向地心
④yg轴在水平面内垂直于xg轴，其指向按右手定则确定

■机体坐标系(Aircraft-body coordinate frame)Sb-------oxyz



①原点O取在飞机质心处，坐标系与飞机固连
②x轴在飞机对称平面内并平行于飞机的设计轴线指向机头
③y轴垂直于飞机对称平面指向机身右方
④z轴在飞机对称平面内，与x轴垂直并指向机身下方
■欧拉角/姿态角（Euler Angle）
 
机体坐标系与地面坐标系的关系是三个Euler角，反应了飞机相对地面的姿态。
俯仰角θ（pitch）：机体坐标系X轴与水平面的夹角。当X轴的正半轴位于过坐标原点的水平面之上（抬头）时，俯仰角为正，否则为负。

偏航角ψ（yaw）：
机体坐标系xb轴在水平面上投影与地面坐标系xg轴（在水平面上，指向目标为正）之间的夹角，由xg轴逆时针转至机体xb的投影线时，偏航角为正，即机头右偏航为正，反之为负。

滚转角Φ（roll）：机体坐标系zb轴与通过机体xb轴的铅垂面间的夹角，机体向右滚为正，反之为负。