一、无人机控制中的坐标系

  无人机运动学中，有三种需要了解的坐标系  

1.1、地球中心坐标系（ECEF）

    地球中心坐标系，即坐标系原点位于地心。X轴通过格林尼治线和赤道线的交点，正方向为原点指向交点方向。 Z轴通过原点指向北极。 Y轴与X，Z轴构成右手坐标系。   而将该坐标系下的点，用GPS坐标表示出来即为WGS-84坐标系，其中在WGS-84坐标系中，X轴指向BIH(国际时间服务机构)1984.0定义的零子午面(Greenwich)和协议地球极(CTP)赤道的交点。Z轴指向CTP方向。Y轴与X，Z轴构成右手坐标系GPS可以获得在WGS-84中的速度向量，为了方便使用速度向量进行无人机控制，我们要把它转换在无人机所在位置的“平面坐标系”。  

1.2、NED坐标系

  由于地心坐标系建立产生的GPS信号是以前面的形式存在，而现实无人机控制中需要在平面，因此引入了NED坐标系，如图绿色部分即为NED坐标系。通常我们以NED为确定无人机位姿的参考坐标系，惯性系。     NED坐标系是在导航计算时使用的坐标系。向量分别指向北，东，地，因此NED坐标系也经常称为北东地坐标系。也就是NED坐标系的三个方向是不变的。   当无人机搜星满足起飞条件1，则该点所确定  

1.3、机体坐标系

    机体坐标系与飞行器固联，坐标系符合右手法则，原点在飞行器重心处，x轴指向飞行器机头前进方向，y轴由原点指向飞行器右侧，z轴方向通过x,y由右手法则确定。机体坐标系是无人机惯性导航的基础坐标系，IMU中获得的加速度状态信息就是该坐标系下的数值。当我们获取IMU输出的x轴加速度信息时不能直接应用在NED坐标系下。 无人机中的坐标转换，即为NED坐标系（惯性系）与机体坐标系之间的转换。  

二、无人机IMU单元

  IMU单元，惯性测量单元，用于测量物体三轴姿态角（角速率）以及加速度的装置。 IMU由陀螺仪和加速度计组成，其中： 陀螺仪：检测载体相对于导航坐标系的角速度信号； 加速度计：三个单轴的加速度计检测物体在坐标系独立三轴的加速度信号 其精度直接影响到惯性系统的精度。  

三、无人机中矩阵的确定与变换

 

3.1、无人机的姿态

    一个空间中的无人机可通过给定位置和方向而完全描述，令O-xyz为标准正交坐标系，xyz为其单位向量。在上图中，设无人机为O^{'},其在空间中的位置可表述为     O^{'}的坐标可简洁的写为     通过坐标系O-xyz以及可以约束无人机的位置，但无人机的方向仍没有约束，接下来大幅篇章将开始角度坐标系的描述。 为了确定无人机的方向，简便的方法即为加入一个附在无人机机身上的坐标系，O^{'}-x^{'}y^{'}z^{'},其中x^{'}y^{'}z^{'}为坐标系中的单位向量，与O-xyz的转换关系为    

3.2、旋转矩阵（注：3.2中的旋转矩阵与3.1上式各分量定义不一致，可对比根据式子定义来理解）

  将描述无人机方向的x^{'}y^{'}z^{'}用矩阵表示为     这里第三个矩阵中，由于x为单位向量（1,0,0），故与向量x^{'}的装置相乘，即为x_{x}^{'}的值。 上诉矩阵R被称为旋转矩阵。 由于x^{'}y^{'}z^{'}是正交坐标系下的单位向量，所以他们之间彼此正交，且向量模为1     R是一个正交矩阵，故     如果坐标系是右手法则则det(R)=1，若为左手法则确定则det(R)=-1。  

3.2.1、基本旋转(旋转回原坐标系下所需要相乘的矩阵，也可以理解为旋转量)

  坐标系O^{'}-x^{'}y^{'}z^{'}可被视为由坐标系O-xyz通过基本旋转而成。这些旋转如果是逆时针的，则其旋转值为正。 假定坐标系O-xyz绕z轴旋转一定角度a，如图其单位向量可表述为：       因此，绕z轴旋转的矩阵可表述为     同样，按此方法可表述按x轴和按y轴旋转时的旋转矩阵     对于描述空间中容易一个旋转，这些矩阵将会是非常有用的。 通过上诉的矩阵，可很容易的证明，旋转矩阵有    

3.2.2、一个向量的旋转表述

  为了进一步理解旋转矩阵的几何意义，将两坐标系原点重合，即o^{'}=0,其中0表示为0向量。点P能够表述为：         其中，p和p^{'}分别对应于相同的点P在坐标系O-xyz和O^{'}-x^{'}y^{'}z^{'}的坐标。根据之前对于o^{'}下坐标转换至o，可得     即     旋转矩阵R表述了向量从矩阵O^{'}-x^{'}y^{'}z^{'}转换至O-xyz，回顾R^{T}R=I,可得    

例3.1：

  设两坐标系具有相同原点，且通过按z轴旋转a而得，如图，由几何关系可得         由此可见，其坐标转换即为p=Rp^{'},旋转矩阵不仅仅描述了一个坐标系的方向，还描述了基于同一个原点的一个向量从一个坐标系到另一个坐标系的转换。  

3.2.3 一个向量的旋转

  上一节中，利用旋转矩阵实现了向量从O^{'}-x^{'}y^{'}z^{'}坐标系转换到O-xyz坐标系下，旋转矩阵还可以表示为向量在同一个坐标系下的旋转。令p^{'}是坐标系O-xyz下的一个向量，Rp^{'}产生一个与p^{'}相同模的向量p，相同的模可以被证明由p^{T}p=p^{'T}R^{T}Rp^{'}和。  

例3.2：

  在下图坐标系下，令位于xy平面的向量p^{'}绕z轴旋转a角。(p_{x}^{'},p_{y}^{'},p_{z}^{'})为向量p^{'}坐标，则向量p可表示为：     即可以简单的用旋转矩阵表示为     可以得到一个旋转矩阵，首先描述两个坐标系之间的转换和两个坐标系向量之间的转换，此外还可以实现在同一坐标系下向量的旋转。  

3.3、旋转矩阵的组合

  为了得到旋转矩阵的组合规则，思考一个向量在不同坐标系下的表述是有用的。令O-x_{0}y_{0}z_{0},O-x_{1}y_{1}z_{1},O-x_{2}y_{2}z_{2}为共同原点O所对应的三个坐标系。p^{0}，p^{1}，p^{2}为向量p在三个坐标系下的表述。   首先，思考坐标系2下p^{2}与坐标系1下p^{1}之间的关系。如果R_{i}^{j}表示坐标系i转换至坐标系j的旋转矩阵，则     相似地，有     上诉三式中，将第一和第三代入第二式，可得关系     上一关系式可以表述为连续旋转的组合。思考一个坐标系最初为O-x_{0}y_{0}z_{0}，通过R_{2}^{0}旋转能够被视为包含以下两步：   （1）基于现有坐标系根据R_{1}^{0}旋转，以便与坐标系O-x_{1}y_{1}z_{1}相联系 （2）经过第一步的旋转，已于O-x_{1}y_{1}z_{1}联系，根据R_{2}^{1}，以便与坐标系O-x_{2}y_{2}z_{2}进行联系   （这里这样表述可能大家感到不甚理解，不是由O-x_{1}y_{1}z_{1}的坐标乘上R_{1}^{0}转换至O-x_{0}y_{0}z_{0}么，这样表述是为了在以后无人机坐标系转换理解更加便捷，如将之后的一个坐标系转换至最初的一个坐标系）   可注意到所有的旋转可表示为一系列微小旋转的组合。每一个旋转定义为基于之前的一个坐标系，将要发生转换的坐标系称为当前坐标系。连续旋转的组合是通过旋转矩阵自右乘来实现的，回顾可得：     连续的旋转也能够详述为不断的参考他们至最初的坐标系，在这种情况下，旋转可以被发生基于一个固定的坐标系。令R_{1}^{0}为O-x_{1}y_{1}z_{1}基于固定坐标系O-x_{0}y_{0}z_{0}的旋转矩阵。令\bar{R}_{2}^{0}表示坐标系O-x_{2}y_{2}z_{2}基于坐标系O-x_{0}y_{0}z_{0}的旋转矩阵，这其中包含了坐标系1基于矩阵\bar{R}_{2}^{1}的旋转。     其中\bar{R}_{2}^{0}是不同于R_{2}^{0}的，\bar{R}_{2}^{0}可表述为基于一个固定坐标系连续旋转的组合，其由单个旋转矩阵自左乘得到的。 旋转组合的一个重要问题是矩阵的产生是不可交换的。鉴于这一点，能够推断两个旋转矩阵一般是不能交换的并且它的组成取决于单个旋转的顺序。  

例3.3：

  思考在一个物体上系有坐标系，下图展示了该物体对于当前坐标系改变旋转次序的两次旋转变化，可以明显的注意在两种情形到最后物体的方向是不同的。    

4、欧拉角

  旋转矩阵对于坐标系的方向给予了一个过多的描述；事实上，由于正交矩阵的性质，所具有的九个特征元素收到六个约束，彼此之间并非独立的。这意味着对于描述一个刚性物体的方向有三个参数即是足够的。   （六个约束详解地址：https://blog.csdn.net/qq_35379989/article/details/102761195）   依据三个参数组成的方向描述为一个最小表示，一个最小表示能够被获得通过使用一个三个角度的集合\phi =\left [ \varphi ,\upsilon ,\psi \right ]^{T}。一个坐标系表述基本旋转的旋转矩阵（三种基本旋转矩阵）作为一个单个角度的功能且要保证两次连续的旋转，这意味着共有27种不同的组合共有12种符合，即第一次旋转有三个轴可以选择，第二次旋转要排除第一个轴已转的有两种选择，第三次旋转要排除第二次旋转的轴有两次旋转，共有12种可能。这12种每一种都为欧拉角的一种。接下来主要简绍的是ZYX（Roll-Pitch-Yaw）（滚转-俯仰-偏航）。  

4.1、RPY角

  RPY（最初起源于航海领域，他们属于（ZYX）角，也被称为Roll-Pitch-Yaw（滚转-俯仰-偏航），它能表述一个飞行器在空中姿态的变化。角\phi =\left [ \varphi ,\upsilon ,\psi \right ]^{T}可表示固定在飞行器质点上的坐标系。   翻滚-俯仰-偏航角度的获取可包括以下：     该图只为了让大家有一个具体空间角度变化的了解，将坐标系轴线位置设置不同则滚仰偏定义对应的轴线可能不同，如以下，定义中，设置绕x轴旋转为偏航，绕y轴旋转为俯仰，绕z轴旋转为翻滚，结合坐标系，判断一下坐标系在飞机上怎么固定呢？   机头方向指向z，x正方向为机身向上方向，如下图：     （1）绕x轴旋转角度\psi，即偏航角，通过矩阵R_{x}(\psi )来表示     （2）绕y轴旋转角度\upsilon，即俯仰角，通过矩阵R_{y}(\psi )来表示     （3）绕z轴旋转角度\varphi，即滚转角，通过矩阵R_{z}(\psi )来表示     坐标系旋转的结果可由基于固定坐标系旋转的组合获得，能够通过基本旋转矩（上3.2.1）阵自左乘获得。 R(\phi )=R_{z}(\varphi )R_{y}(\upsilon )R_{x}(\psi )   即     由以上R(\phi )对应给予一个旋转矩阵R，使得R为：   R=\begin{bmatrix} r_{11} & r_{12} &r_{13} \\ r_{21}&r_{22} &r_{23} \\ r_{31}&r_{32} & r_{33} \end{bmatrix}   通过比较R(\phi )能够获得角度为：   若\upsilon角度范围在(-\pi/2,\pi /2 ),则     若\upsilon角度范围在(\pi/2,3\pi /2 ),则     其中Atan2(y,x)为计算y/x反正切，即arctan(y/x),可通过y与x的正负来判别角度所在象限，从而判别在2\pi范围内的角。  

4.2、无人机中的欧拉角

  经过上诉的部分已经将旋转矩阵方面的知识描述完毕，接下来为具体在无人机运用上的矩阵变换。 无人机中我们选择旋转z轴产生的为偏航角，旋转y轴产生的为俯仰角，旋转x轴为滚转角。     上文3.2.1中讲述了三种基本旋转矩阵，上述的旋转矩阵R可将变换后的位移、速度、力转换为变换前原坐标系下的位置、速度、力，由此我们如果获得无人机相对于惯性系（NED坐标系）的角度，即可得到在惯性系下的运动。当然我们也可以设定无人机的机体坐标系为原始坐标系，这时如果想要获得在惯性系下的相关量，则需要乘上旋转矩阵的逆，上面我们有旋转矩阵的装置为旋转矩阵的逆，固有如下：     R_{b/nv}表示为以机体坐标系为原坐标系，机体坐标系下相关量想要转换至惯性系下坐标量所需乘的旋转矩阵  

4.3、无人机中的万向锁

  陀螺仪的结构为一根轴线穿过一个圆盘，且圆盘处于高速旋转状态，为陀螺仪的转子。根据陀螺的定轴性，陀螺的自转轴即竖轴在惯性空间内的方向保持不变