一种小型无人机姿态确定系统与确定方法

摘要

一种低成本、低功耗、算法简单、计算量小的小型无人机姿态确定系统与确定方法，确定系统包括：角速率陀螺芯片，用于测量无人机的滚动、俯仰与偏航角速率；三轴电子罗盘芯片，用于测量无人机飞行时的航迹方位角即偏航角；三轴加速度计芯片量重力在无人机本体坐标轴的分量；微控器芯片，通过I2C总线分别与角速率陀螺芯片、三轴电子罗盘芯片和三轴加速度计芯片连接，用于采集上述传感器的测量数据，并根据采集数据姿态确定方法确定无人机实时姿态。确定方法包括以下步骤：包括建立无人机本体坐标系与惯性坐标系之间初始方向余弦矩阵、更新姿态方向余弦矩阵、标准化方向余弦矩阵、计算无人机的修正三轴角速度和计算无人机三轴姿态角。

说明书

技术领域

 本发明涉及无人机飞行姿态确定技术领域，具体涉及一种基于多传感器数据融合的低成本、低功耗的小型无人机姿态确定系统与确定方法。

背景技术

无人机由于其成本低、无人员伤亡以及战场生存能力强等特点而广泛应用于军事目标的侦查与打击等众多领域，成为航空技术的重要发展方向，受到各国的高度关注。无人机的姿态控制系统是无人机正常运行并完成飞行任务的重要保障系统之一，而无人机姿态的确定是实现姿态控制的基础。因此，针对小型无人机低成本等特点，如何设计低成本高可靠性的姿态确定系统是目前研究的热点。

目前大多数的军事侦查无人机采用高精度的角速率陀螺进行姿态确定。该方法将角速率陀螺的测量值进行积分从而获得无人机的姿态信息。由于积分计算中存在一定的偏差，因此高精度的姿态确定需要高精度的陀螺。由于高精度陀螺存在成本高、重量大以及结构复杂等缺陷，从而使得采用高精度陀螺进行姿态确定的方案并不适用于低成本的小型无人机。事实上，低成本小型无人机通常采用低成本、体积小的微型电子机械传感器，但是此类传感器的精度低，积分角速率陀螺输出值将产生较大的漂移，从而无法提供高精度的姿态确定。

为提高低成本无人机的姿态确定精度，目前小型无人机常采用传感器融合技术将加速度计、磁强计及GPS等组成的无陀螺系统与角速率陀螺进行融合，确定无人机姿态。该方法利用角速率陀螺提供的姿态信息来“平滑”无陀螺系统，而无陀螺系统则用于校正角速率陀螺产生的漂移。因此采用适当的滤波器可将角速率陀螺与无陀螺系统结合，从而获得无偏的姿态信息。采用多传感器数据融合技术进行无人机姿态确定的核心在于融合不同传感器的滤波器。无人机姿态确定中常采用扩展卡尔曼滤波器或者非线性预测滤波器。然而这些滤波算法计算量大，算法复杂，显然不太适用于低成本、计算能力小的小型无人机姿态确定。

在低成本传感器的制约条件下，无人机的小型化、低成本化，迫切要求形成一种结构简单、计算量小、廉价但又具有较高精度和可靠性的姿态确定系统与确定方法。

发明内容

本发明的目的是为了解决现有技术存在的上述问题，提供一种低成本、低功耗、算法简单、计算量小的小型无人机姿态确定系统与确定方法，实现小型无人机低成本的姿态确定。

为解决上述技术问题，本发明采用如下技术方案：

一种小型无人机姿态确定系统，其包括： 

角速率陀螺芯片，用于测量无人机的滚动、俯仰与偏航角速率；

三轴电子罗盘芯片，用于测量无人机飞行时的航迹方位角即偏航角；

三轴加速度计芯片量重力在无人机本体坐标轴的分量；

微控器芯片，通过I²C总线分别与角速率陀螺芯片、三轴电子罗盘芯片和三轴加速度计芯片连接，用于采集上述传感器的测量数据，并根据采集数据姿态确定方法确定无人机实时姿态。

一种小型无人机姿态确定方法，包括以下步骤：

步骤一、采用欧拉角321坐标系旋转建立无人机本体坐标系与参考惯性坐标系之间的无人机姿态，并建立两坐标系之间的初始方向余弦矩阵                                                ；

步骤二、更新姿态方向余弦矩阵。根据修正的三轴角速度值，步骤一中建立的初始方向余弦值，微控器芯片采样时间，以及时刻的方向余弦信息，计算时刻的方向余弦矩阵为：

当时，修正的三轴角速度直接采用三轴角速率陀螺芯片测量的角速率值；

步骤三、标准化方向余弦矩阵。根据方向余弦阵的第一列、第二列与第三列，标准化方向余弦阵为，

式中

其中“”表示向量叉乘运算，且；

步骤四、根据三轴角速率陀螺芯片测量的无人机角速度以及比例积分反馈输出修正项，计算无人机修正的三轴角速度为：

；

步骤五、根据步骤三获得的标准化后的方向余弦矩阵，计算无人机姿态滚动、俯仰与偏航角分别为：

，，

其中分别表示方向余弦矩阵的第第的元素。

步骤一中初始方向余弦矩阵的建立过程为：

步骤a、设定无人机初始的滚动与俯仰姿态角均为零度，即与，根据电子罗盘芯片输出的三轴测量值，计算该测量值在无人机本体系x_b与y_b轴的投影分别为、；

步骤b、根据步骤a获得的与值计算初始偏航角的正余弦值分别为，；

步骤c、根据步骤a与b的姿态角以及欧拉角321坐标系旋转，建立无人机初始方向余弦矩阵为：

。

步骤四中获得比例积分反馈输出修正项为：

，

，

其中与分别为比例、积分因子，当时，为检测出的角速率陀螺总的偏移值。

计算角速率陀螺总的偏移值为：

其中表示偏航轴上的角速率陀螺偏移值，表示滚动与俯仰轴上的角速率陀螺偏移值。

偏航轴上的角速率陀螺偏移的计算过程为：

。

滚动与俯仰轴上的角速率陀螺偏移的计算过程为：

其中为三轴加速度计芯片的测量值。

本发明所提出的姿态确定系统由低成本的角速率陀螺芯片、三轴电子罗盘芯片、三轴加速度计芯片以及微控器芯片组成，与采用高精度惯性测量单元构成的姿态确定系统相比，本发明系统成本更低、质量更轻，更能够适合低成本低质量需求的小型无人机开发。

本发明所提出的姿态确定方法包括建立无人机本体坐标系与惯性坐标系之间初始方向余弦矩阵、更新姿态方向余弦矩阵、标准化方向余弦矩阵、计算无人机的修正三轴角速度和计算无人机三轴姿态角，设计简单的加减运算，与利用卡尔曼滤波等设计的姿态确定方法相比，本发明方法计算量小、结构简单，更能够有效提升姿态确定的速度。

本发明的有益效果在于：本发明通过使用廉价的姿态测量芯片进行姿态信息采集，降低了小型无人机姿态确定系统的成本与重量。本发明提出的一种非基于滤波的传感器数据融合姿态确定方法，该算法结构简单、数据计算量小，解决了传统的姿态确定方法算法复杂、计算量大等问题，适用于小型低成本无人机系统。

附图说明

图1是本发明的小型无人机姿态确定系统硬件实现结构图；

图2是本发明的小型无人机姿态确定方法结构示意图；

图3是本发明的小型无人机姿态确定方法实验结果图。

具体实施方式

下面将结合附图和实施例对本发明作进一步详细的说明。

请参阅图1，本发明设计了一种小型无人机姿态确定系统，其包括三个姿态测量器件：角速率陀螺芯片用于测量无人机角速度，三轴电子罗盘芯片用于测量无人机偏航角，三轴加速度计芯片用于测量重力在无人机本体坐标系下的分量。除此之外，该系统还包括一个微控器芯片，该芯片用于采集姿态测量器件的测量数据，并执行本发明的姿态确定算法方法确定无人机姿态。微控器芯片与角速率陀螺芯片、三轴电子罗盘芯片以及三轴加速度计芯片通过I²C总线进行通讯。微控器芯片对各姿态测量芯片测量的数据采用多次采样进行平均的方式进行数据采集，以提高测量精度。本实施例中采用角速率陀螺芯片ITG-3200、三轴电子罗盘芯片HMC5883L、三轴加速度计芯片ADXL345以及微控器芯片ATemga2560，三种低成本姿态测量芯片与微控器总成本不超过500元人民币，总质量极轻，且总功耗相当小，十分符合低成本、小功耗的小型无人机。

令三轴电子罗盘芯片三轴测量值为，而加速度计芯片测量值为，且三轴角速率陀螺芯片测量值为，则本发明公开的一种小型无人机姿态确定方法如下：

根据姿态确定系统硬件实现结构图，本发明的核心在于其姿态确定算法，该姿态确定方法结构示意图如图2所示，包含以下几个步骤：

步骤一：建立无人机本体坐标系与惯性坐标系之间初始方向余弦矩阵；

步骤二：更新姿态方向余弦矩阵；

步骤三：标准化方向余弦矩阵； 

步骤四：计算无人机的修正三轴角速度；

步骤五：计算无人机三轴姿态角。

步骤一中建立无人机本体坐标系与参考惯性坐标系之间的姿态方向余弦矩阵过程为： 

步骤I：为了描述无人机的姿态，首先需要定义相应的坐标系系统。由于小型无人机的飞行航程短，因此通常将机体本体坐标系选S_b-Ox_by_bz_b为动坐标系，而将导航坐标系S_n-Ox_ny_nz_n选为参考坐标系，定义为北东天参考系统，即北向为S_n的x_n轴，东向为S_n的y_n轴，指向地心为S_n的z_n轴。

步骤II：建立无人机本体坐标系S_b与导航坐标系S_n之间的姿态。通过本体坐标系S_b绕z_b轴旋转角，然后绕y_b轴旋转角，最后绕z_b轴旋转角便可建立坐标系S_b与导航坐标系S_n之间的方向余弦阵即姿态矩阵：

 （1）

其中、与分别称作滚动姿态角、俯仰姿态角与偏航姿态角。

步骤一中无人机姿态方向余弦矩阵的初始值建立过程为： 

步骤a、设定无人机初始的滚动与俯仰姿态角均为零度，即与，根据电子罗盘芯片输出的三轴测量值，计算该测量值在无人机本体坐标系x_b与y_b轴的投影分别为、。

步骤b、根据步骤a获得的与值计算初始偏航角的正余弦值分别为，。

步骤c、根据步骤a与b的姿态角以及欧拉角321坐标系旋转，由公式（1）建立无人机初始方向余弦矩阵为：

       （2）

步骤二中无人机姿态方向余弦矩阵的更新过程为：

由欧拉定理，根据时刻的姿态矩阵、修正的三轴角速度信息以及微控器芯片采样时间，计算时刻无人机的方向余弦矩阵为：

                  （3）

              （4）

当时，为三轴角速率陀螺芯片实际测量的角速率值。

由于公式（3）的获得使用了数值积分方法，因此存在一定的积分偏差。故无人机飞行一段时间后方向余弦阵可能不再满足正交性，为此步骤三中无人机姿态方向余弦矩阵的标准化过程为：

令、与分别表示姿态矩阵阵的第一、二与三列，采用如下的与来分别逼近、：

            （5）

           （6）

式中。令“”表示向量叉乘运算，则此时可根据正交性质，可用进行逼近：

                   （7）

至此，由公式（5）-公式（7）可采用如下方法把姿态矩阵标准化、归一化如下的形式：

  （8）

式中：

           （9）

              （10）

             （11）

步骤四中无人机修正的三轴角速度的计算过程为：

由三轴角速率陀螺芯片的测量值与比例积分反馈输出修正项，计算修正的无人机三轴角速度为：

               （12）

所述比例积分反馈输出修正项的计算过程为：

步骤1)：令为因角速率陀螺偏航轴上的偏移而引起的角速率陀螺偏移，分别表示方向余弦矩阵的第第的元素。由于根据三轴电子罗盘芯片测量值可获得无人机航向方向，为此可检测为：

               （13）

步骤2) ：令为因角速率陀螺滚动-俯仰轴上的偏移而引起的角速率陀螺偏移，则根据三轴加速度计芯片的测量值可检测出该偏移值为：

                       （14）

步骤3)：根据步骤1)与2)中检测的三轴角速率陀螺偏移值，则角速率陀螺总的偏移值可计算为：

                      （15）

步骤4)：由控制理论中比例-积分-微分控制方法可知，经过一个比例-积分控制环节后消除一定的误差。为此在本步骤中将对所检测的总的偏移进行比例-积分控制处理，从而计算比例积分反馈输出修正为：

           （16）

其中表示比例环节，而表示积分环节，且

                                （17）

            （18）

式中与分别为比例、积分因子，且当时。

步骤五中无人机三轴姿态角的确定过程为：

根据步骤三标准化、归一化后的方向余弦矩阵，计算无人机姿态滚动、俯仰与偏航角分别为：

，，        （19）

根据以上所述的小型无人机姿态确定系统的硬件实现结构与软件算法方法，取微控器芯片在执行算法时其采样时间为秒，将本发明的姿态确定系统与姿态确定方法应用于小型无人机，该无人机实际飞行的过程中由本发明姿态确定方法所确定的三轴姿态角如图3所示。从图3可知，本发明的姿态确定算法能够实现低成本的无人机姿态确定。

综上可见，本发明提出了一种由廉价的姿态测量芯片(角速率陀螺芯片、三轴电子罗盘芯片、三轴加速度计芯片)组成的无人机姿态确定系统与方法。该系统中的微控器芯片首先采集上述三个芯片的测量数据，然后融合采集数据，执行姿态确定算法确定了无人机的三轴姿态。本发明硬件设计简单、成本低廉，且姿态确定方法软件实现算法简单、计算量小。实际结果表明，本发明能准确地建立无人机飞行过程中的空间姿态，非常适用于低成本小型无人机。

这里本发明的描述和应用是说明性的，并非想将本发明的范围限制在上述实施例中。这里所披露的实施例的变形和改变是可能的，对于那些本领域的普通技术人员来说实施例的替换和等效的各种部件是公知的。本领域技术人员应该清楚的是，在不脱离本发明的精神或本质特征的情况下，本发明中姿态测量芯片以及微控器可以采用其它选型。