四旋翼飞行器旋转矩阵公式推导！

四旋翼飞行器旋转矩阵公式推导！

方法一：

1.在二维平面中：如下图所示，在xoy平面中有一向量op⃗ =(x,
4000
y)T，旋转ϕ角后变为向量op⃗ ′=(x′,y′)T。 

　　



　　据图可得：x=|op⃗ |cosθ；y=|op⃗ |sinθ，经旋转ϕ角后有: 
　　x′=|op⃗ |cos(θ+ϕ)=|op⃗ |(cosθcosϕ−sinθsinϕ)=xcosϕ−ysinϕ 
　　y′=|op⃗ |sin(θ+ϕ)=|op⃗ |(sinθcosϕ+cosθsinϕ)=xsinϕ+ycosϕ； 
写成矩阵形式： 
　　(x′y′)=(cosϕsinϕ−sinϕcosϕ)(xy) 
　　2.在三维空间中：如下图所示，若以坐标系的三个坐标轴X、Y、Z分别作为旋转轴，则点实际上只在垂直坐标轴的平面上作二维旋转。 



　　例： op⃗ 绕X轴旋转ϕ角，有： 
旋转前：

 
旋转后：

 
写成矩阵形式：

 
则绕X轴旋ϕ角的旋转矩阵为： Rx(ϕ)=（1000cosϕ−sinϕ0sinϕcosϕ） 
同理可得绕X、Y、Z轴旋转的不同角度的旋转矩阵（方向余弦矩阵）分别为： 



　　最后，若op⃗ 
绕某一定轴旋转，从欧拉定律中可知，绕着固定轴做一个角值的旋转，可以被视为分别以坐标系的三个坐标轴X、Y、Z作为旋转轴的旋转的叠加。

方法二：

一、先来个平面旋转的分析：

两角和(差)公式

推导

旋转变换一般是按照某个圆心点，以一定半径 r 旋转一定的角度

α

，为了简单起见我们给出下面的情景

假定点A(x,y)想经过旋转变换到达B(x',y')，已知旋转角度

α

和点A坐标，计算出点B



要计算点B则分别计算他的x'和y'分量



根据矩阵乘法计算规则，可以推出 



只要给出旋转角度，计算出矩阵，然后使用这个矩阵分别左乘每一个点，就能计算出这个点旋转后的点坐标 这样我们就可以通过矩阵变换坐标了 

二、延伸到三维坐标：

    坐标的旋转变换在很多地方都会用到，比如机器视觉中的摄像机标定、图像处理中的图像旋转、游戏编程等。

    任何维的旋转可以表述为向量与合适尺寸的方阵的乘积。最终一个旋转等价于在另一个不同坐标系下对点位置的重新表述。坐标系旋转角度θ则等同于将目标点围绕坐标原点反方向旋转同样的角度θ。

    若以坐标系的三个坐标轴X、Y、Z分别作为旋转轴，则点实际上只在垂直坐标轴的平面上作二维旋转。

   假设三维坐标系中的某一向量

，其在直角坐标系中的图如图1所示。其中点P在XY平面、XZ平面、YZ平面的投影分别为点M、点P、点N。

 

                                              


                                                                 图1 直角坐标系XYZ

   1、

绕Z轴旋转θ角

    绕Z轴旋转，相当于

在XY平面的投影OM绕原点旋转，如下图所示，OM旋转θ角到OM'。

                                                 

                                                            


                                                                   图2 向量绕Z轴旋转示意图

   设旋转前的坐标为

，旋转后的坐标为

，则点M的坐标为

，点M'的坐标为

。由此可得：
                                                       


    对于

和

进行三角展开可得：
                                                      


    且有

;可得绕Z轴旋转

角的旋转矩阵为：

                         

                                                                   


   2、

绕X轴旋旋转θ角

   绕X轴旋转，相当于

在YZ平面的投影ON绕原点旋转，如下图所示，ON旋转θ角到ON'。

                                                            


                                                                   图3 向量绕X轴旋转示意图

   设旋转前的坐标为

，旋转后的坐标为

，则点N的坐标为

，点N'的坐标为

。由此可得：

                                                   


    对于

和

进行三角展开可得：

                                                


    且有

;可得绕X轴旋转

角的旋转矩阵为：

                         

                                                         


    3、

绕Y轴旋旋转θ角

   绕Y轴旋转，相当于

在XZ平面的投影OQ绕原点旋转，如下图所示，OQ旋转θ角到OQ'。
                                                 
                                                            


                                                                   图4 向量绕Y轴旋转示意图

   设旋转前的坐标为

，旋转后的坐标为

，则点Q的坐标为

，点Q'的坐标为

。由此可得：

                                               


    对于

和

进行三角展开可得：

                                             


    且有

;可得绕Y轴旋转

角的旋转矩阵为：

                         
                                           


   4、绕X、Y、Z轴旋转的旋转矩阵分别为：