旋转向量与旋转矩阵的联系、点乘点积的意义、飞机的姿态角
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：
根据旋转前后的两个向量值，使用上面的方法，先求出旋转角度和旋转轴，然后用罗德里格旋转公式即可求出对应的旋转矩阵。
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)); //这就是上面的公式（1）矩阵
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;
}
(一)向量的点积
向量的点积定义：
AB=|A||B|cos
其运算结果是一个常量。将其转换成矩阵乘法是
AB = matrix(A)*matrix(B)=(x1,y1,z1)*(x2,y2,z2)=x1*x2 + y1*y2 + z1*z2
(二)向量的叉积
向量的叉积的模定义：
|A×B|=|A||B|sin
其几何意义是以|A|，|B|为边长的平行四边形的面积。
向量的叉积定义：
利用向量的叉乘性质可以求取一个平面的法向量。这里的叉乘是定义旋转轴的。
物理意义：。向量的叉积很有意思，按照爱因斯坦的相对论，在观察一个目标运动时，需要采用特定的参照系。
利用向量的叉积可以创造出一个新的维度，而这个维度是独立于(垂直于)先前这个空间的，因此，在这个新的维度空间可以当成目标运动的参照系。
■地面坐标系（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轴的正半轴位于过坐标原点的水平面之上（抬头）时，俯仰角为正，否则为负。
机体坐标系xb轴在水平面上投影与地面坐标系xg轴（在水平面上，指向目标为正）之间的夹角，由xg轴逆时针转至机体xb的投影线时，偏航角为正，即机头右偏航为正，反之为负。