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 。由 点积 定义可知:
2. 旋转轴
由1中可知,旋转角所在的平面为有P和Q所构成的平面,那么旋转轴必垂直该平面。
假定旋转前向量为a(a1, a2, a3),旋转后向量为b(b1, b2, b3)。由叉乘定义得:
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)); //这就是上面的<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;
}
(一)向量的点积
向量的点积定义:
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|为边长的平行四边形的面积。
向量的叉积定义:
利用向量的叉乘性质可以求取一个平面的法向量。这里的叉乘是定义旋转轴的。
物理意义:。向量的叉积很有意思,按照爱因斯坦的相对论,在观察一个目标运动时,需要采用特定的参照系。
利用向量的叉积可以创造出一个新的维度,而这个维度是独立于(垂直于)先前这个空间的,因此,在这个新的维度空间可以当成目标运动的参照系。
■地面坐标系(earth-surface inertial reference frame)Sg--------Oxgygzg
![](http://www.123kuai.com/uploadfile/2014/0114/20140114075940985.jpg)
①在地面上选一点Og
②使xg轴在水平面内并指向某一方向
③zg轴垂直于地面并指向地心
④yg轴在水平面内垂直于xg轴,其指向按右手定则确定
■机体坐标系(Aircraft-body coordinate frame)Sb-------oxyz
![](http://www.123kuai.com/uploadfile/2014/0114/20140114080638890.png)
![](http://www.123kuai.com/uploadfile/2014/0114/20140114075600165.jpg)
①原点O取在飞机质心处,坐标系与飞机固连
②x轴在飞机对称平面内并平行于飞机的设计轴线指向机头
③y轴垂直于飞机对称平面指向机身右方
④z轴在飞机对称平面内,与x轴垂直并指向机身下方
■欧拉角/姿态角(Euler Angle)
![](http://www.123kuai.com/uploadfile/2014/0114/20140114081143959.jpg)
![](https://i-blog.csdnimg.cn/blog_migrate/9749511f1bacf86c191b657de0026fcd.png)
机体坐标系与地面坐标系的关系是三个Euler角,反应了飞机相对地面的姿态。
俯仰角θ(pitch):机体坐标系X轴与水平面的夹角。当X轴的正半轴位于过坐标原点的水平面之上(抬头)时,俯仰角为正,否则为负。
![](https://i-blog.csdnimg.cn/blog_migrate/cb2b81377e6f6286301d440ff0843c4e.png)
机体坐标系xb轴在水平面上投影与地面坐标系xg轴(在水平面上,指向目标为正)之间的夹角,由xg轴逆时针转至机体xb的投影线时,偏航角为正,即机头右偏航为正,反之为负。
![](https://i-blog.csdnimg.cn/blog_migrate/93f831ece7eb885cdad5292dc959a5e6.png)
![](https://i-blog.csdnimg.cn/blog_migrate/da0a5a1a1c87edc4a6cf8c4ec8254a26.png)