垂直起降固定翼无人机姿态估计中加速度补偿方法及系统与流程

本发明涉及飞行器导航技术领域，尤其是一种垂直起降固定翼无人机姿态估计中加速度补偿方法及系统。

背景技术：

在多旋翼无人机的姿态估计中，常采用互补滤波、卡尔曼滤波等数据融合方法，对陀螺仪和加速度计的数据进行融合，从而得到无人机的姿态信息。

在利用加速度计数据进行姿态解算时，其误差来源于两个方面。其一是加速度计自身的噪声，另一方面是无人机运动造成的加速度叠加在加速度计的测量值上，使得加速度测量值不能准确反映因姿态改变引起的重力加速度计测量值（由于重力加速度是垂直向下的，所以在无人机上固定安装了一个三轴的加速度计，重力加速度在三个轴上的加速度分量可以反映无人机的姿态，从而可以将其用于无人机的姿态解算。如果无人机运动产生了加速度，加速度计测量值就是重力加速度与这个加速度的叠加值，但是姿态解算只希望测量得到的值为重力加速度的值，此时就会带来误差）。对于旋翼机而言，加速度计自身噪声可以通过低通滤波解决，由于飞行速度不快，加速也比较平稳，所以常忽略无人机自身运动引起的加速度，其误差可以通过数据融合算法来消除。

而上述处理方法在垂直起降固定翼无人机的姿态解算时遇到了问题。垂直起降固定翼无人机除了旋翼起飞和旋翼降落阶段，还有固定翼飞行阶段。在固定翼飞行阶段，由于无人机总是以空速作为控制量之一，当在有风环境下飞行时，需要不停地加减地速以保持空速，从而引起较大的加速度叠加在重力加速度的测量值上，无法忽略。另一方面，由于固定翼具有较大的前飞速度，在飞机转弯过程中容易产生科氏加速度，也会对加速度测量值产生干扰。因此，垂直起降固定翼无人机的姿态估计必须考虑以上因素，对加速度计测量数据进行补偿，以满足旋翼飞行和固定翼飞行的姿态估计。

技术实现要素：

本发明提供一种垂直起降固定翼无人机姿态估计中加速度补偿方法及系统，用于克服现有技术中姿态估计方法无法同时满足垂直起降固定翼无人机在旋翼飞行阶段和固定翼飞行阶段的姿态估计要求等缺陷。

为实现上述目的，本发明提出一种垂直起降固定翼无人机姿态估计中加速度补偿方法，包括：

利用gps获得当前时刻绝对坐标下无人机的速度信息；

获取上一时刻无人机的姿态转换矩阵，根据所述姿态转换矩阵和所述绝对坐标下无人机的速度信息，得到当前时刻机体坐标系下无人机的速度信息；所述机体坐标系包括x轴、y轴和z轴三个坐标轴，x轴为机头方向，z轴为垂直向下，y轴与x轴、z轴构成右手坐标系；

对所述机体坐标系下无人机的速度信息进行求导，得到无人机在x轴、y轴和z轴三个方向上的加速度信息；

根据垂直起降固定翼无人机在固定翼飞行时的状态和所述无人机在x轴、y轴和z轴三个方向上的加速度信息，获得第一加速度补偿量；

利用陀螺仪测量获得当前时刻无人机的角速度，对所述角速度进行低通滤波，得到平滑角速度；

根据科氏加速度计算法则，利用所述机体坐标系下无人机的速度信息和所述平滑角速度，计算获得科氏加速度；

根据科氏加速度对垂直起降固定翼无人机姿态的影响，获得第二加速度补偿量；

根据所述第一加速度补偿量和所述第二加速度补偿量，并利用罚函数，获得最终无人机的加速度补偿量。

为实现上述目的，本发明还提出一种垂直起降固定翼无人机姿态估计中加速度补偿系统，包括：

速度信息获取模块，用于利用gps获得当前时刻绝对坐标下无人机的速度信息；获取上一时刻无人机的姿态转换矩阵，根据所述姿态转换矩阵和所述绝对坐标下无人机的速度信息，得到当前时刻机体坐标系下无人机的速度信息；所述机体坐标系包括x轴、y轴和z轴三个坐标轴，x轴为机头方向，z轴为垂直向下，y轴与x轴、z轴构成右手坐标系；

速度信息处理模块，用于对所述机体坐标系下无人机的速度信息进行求导，得到无人机在x轴、y轴和z轴三个方向上的加速度信息；根据垂直起降固定翼无人机在固定翼飞行时的状态和所述无人机在x轴、y轴和z轴三个方向上的加速度信息，获得第一加速度补偿量；

角速度信息获取模块，用于利用陀螺仪测量获得当前时刻无人机的角速度，对所述角速度进行低通滤波，得到平滑角速度；

角速度信息处理模块，用于根据科氏加速度计算法则，利用所述机体坐标系下无人机的速度信息和所述平滑角速度，计算获得科氏加速度；根据科氏加速度对垂直起降固定翼无人机姿态的影响，获得第二加速度补偿量；

加速度补偿模块，用于根据所述第一加速度补偿量和所述第二加速度补偿量，并利用罚函数，获得最终无人机的加速度补偿量。

为实现上述目的，本发明还提出一种计算机设备，包括存储器和处理器，所述存储器存储有计算机程序，所述处理器执行所述计算机程序时实现上述所述方法的步骤。

与现有技术相比，本发明的有益效果有：

本发明提供的垂直起降固定翼无人机姿态估计中加速度补偿方法，基于机体坐标系下的速度信息以及陀螺仪的角速度信息，计算得到三个方向上的线性加速度信息以及科氏加速度，选取两种加速度中影响较大的分量进行组合，再利用罚函数来最终确定无人机的加速度补偿量。本发明提供的加速度补偿方法同时兼顾了垂直起降固定翼无人机在旋翼飞行阶段和固定翼飞行阶段的姿态估计，可实现垂直起降固定翼全程飞行过程中加速度的平滑补偿，从而更准确地反映因姿态变化引起的重力加速度测量值变化，提高无人机姿态估计的精度；此外，可有效避免因传感器数据噪声和运算带来的影响。

附图说明

为了更清楚地说明本发明实施例或现有技术中的技术方案，下面将对实施例或现有技术描述中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本发明的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图示出的结构获得其他的附图。

图1为本发明提供的垂直起降固定翼无人机姿态估计中加速度补偿方法流程图。

本发明目的的实现、功能特点及优点将结合实施例，参照附图做进一步说明。

具体实施方式

下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明的一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有作出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。

另外，本发明各个实施例之间的技术方案可以相互结合，但是必须是以本领域普通技术人员能够实现为基础，当技术方案的结合出现相互矛盾或无法实现时应当认为这种技术方案的结合不存在，也不在本发明要求的保护范围之内。

本发明提出一种垂直起降固定翼无人机姿态估计中加速度补偿方法，如图1所示，包括：

101：利用gps获得当前时刻绝对坐标下无人机的速度信息；

gps，全球定位系统，是一种以空中卫星为基础的高精度无线电导航的定位系统，它在全球任何地方以及近地空间都能够提供准确的地理位置、车行速度及精确的时间信息。

绝对坐标，是一个固定的坐标位置，使用它输入的点坐标不会因参照物的不同而不同。

102：获取上一时刻无人机的姿态转换矩阵，根据所述姿态转换矩阵和所述绝对坐标下无人机的速度信息，得到当前时刻机体坐标系下无人机的速度信息；所述机体坐标系包括x轴、y轴和z轴三个坐标轴，x轴为机头方向，z轴为垂直向下，y轴与x轴、z轴构成右手坐标系；

机体坐标系，是指固定在飞行器或者飞机上的遵循右手法则的三维正交直角坐标系，其原点位于飞行器的质心，ox轴位于飞行器参考平面内平行于机身轴线并指向飞行器前方，oy轴垂直于飞行器参考面并指向飞行器右方，oz轴在参考面内垂直于xoy平面，指向航空器下方。

103：对所述机体坐标系下无人机的速度信息进行求导，得到无人机在x轴、y轴和z轴三个方向上的加速度信息；

104：根据垂直起降固定翼无人机在固定翼飞行时的状态和所述无人机在x轴、y轴和z轴三个方向上的加速度信息，获得第一加速度补偿量；

垂直起降固定翼无人机在固定翼飞行时仅有前飞螺旋桨提供动力进行加速。

105：利用陀螺仪测量获得当前时刻无人机的角速度，对所述角速度进行低通滤波，得到平滑角速度；

陀螺仪，是用高速回转体的动量矩敏感壳体相对惯性空间绕正交于自转轴的一个或二个轴的角运动检测装置。

低通滤波，是一种过滤方式，规则为低频信号能正常通过，而超过设定临界值的高频信号则被阻隔、减弱。

106：根据科氏加速度计算法则，利用所述机体坐标系下无人机的速度信息和所述平滑角速度，计算获得科氏加速度；

在转动参考系中，物体在做牵连运动的同时沿旋转半径做相对运动，由牵连运动和相对运动交互耦合而形成的加速度为科氏加速度。

107：根据科氏加速度对垂直起降固定翼无人机姿态的影响，获得第二加速度补偿量；

科氏加速度主要影响无人机y轴向的加速度。

108：根据所述第一加速度补偿量和所述第二加速度补偿量，并利用罚函数，获得最终无人机的加速度补偿量。

罚函数用于判断最终无人机的加速度补偿量的大小。

步骤101~104，是为了获得第一加速度补偿量；

步骤105~107，是为了获得第二加速度补偿量；

本发明提供的垂直起降固定翼无人机姿态估计中加速度补偿方法中，第一加速度补偿量和第二加速度补偿量的获得是同时进行的，以提高方法效率。

在其中一个实施例中，对于步骤102，获取上一时刻无人机的姿态转换矩阵，根据所述姿态转换矩阵和所述绝对坐标下无人机的速度信息，得到当前时刻机体坐标系下无人机的速度信息，包括：

201：根据历史数据，获取上一时刻无人机的姿态转换矩阵；

202：将所述绝对坐标下无人机的速度信息乘以所述姿态转换矩阵，得到当前时刻机体坐标系下无人机的速度信息，为

（4）

式中，为机体坐标系下无人机的速度信息；为绝对坐标下无人机的速度信息；为姿态变换矩阵；为机体坐标系；为惯性系；x、y、z为机体坐标系中三个坐标轴上的分量；为速度；为转置符号。

在另一个实施例中，对于步骤103，对所述机体坐标系下无人机的速度信息进行求导，得到无人机在x轴、y轴和z轴三个方向上的加速度信息，包括：

通过微分环节的传递函数对所述机体坐标系下无人机的速度信息进行求导，得到无人机在x轴、y轴和z轴三个方向上的加速度信息，

（5）

式中，为速度求导；为机体坐标系；x、y、z为机体坐标系中三个坐标轴上的分量；为转置符号。

在某个实施例中，所述微分环节的传递函数为

（1）

式中，为决定跟踪速度的速度因子；为自变量，是时间经过拉氏变换后的量。

在下一个实施例中，对于步骤104，考虑到垂直起降固定翼无人机在固定翼飞行时仅有前飞螺旋桨提供动力进行加速，其他两个方向上的加速度多为噪声，因此取加速度信息的第一个分量作为第一加速度补偿量，即

（6）

式中，为速度求导；为机体坐标系；x为机体坐标系中x坐标轴上的分量；为转置符号。

在另一个实施例中，对于步骤105，利用陀螺仪测量获得当前时刻无人机的角速度，对所述角速度进行低通滤波，得到平滑角速度，包括：

501：利用陀螺仪测量获得当前时刻无人机的角速度；

502：通过低通滤波的传递函数对所述角速度进行低通滤波处理，得到平滑角速度，

（7）

式中，，，分别为平滑角速度在机体坐标系中三个坐标轴上的分量；为转置符号。

在某个实施例中，所述低通滤波的传递函数为

（2）

式中，为时间常数；k为放大系数；为自变量。

在下一个实施例中，对于步骤106，根据科氏加速度计算法则，利用所述机体坐标系下无人机的速度信息和所述平滑角速度，计算获得科氏加速度，

（8）

式中，为平滑角速度；为机体坐标系下无人机的速度信息；为加速度；为机体坐标系；x、y、z为机体坐标系中三个坐标轴上的分量；为转置符号。

在下一个实施例中，对于步骤107，根据科氏加速度对垂直起降固定翼无人机姿态的影响，获得第二加速度补偿量，

考虑到科氏加速度主要影响无人机y轴方向的加速度，因此取科氏加速度的第二个分量作为第二加速度补偿量，即

（9）

式中，为加速度；为机体坐标系；y为机体坐标系中y坐标轴上的分量；为转置符号。

在下一个实施例中，对于步骤108，根据所述第一加速度补偿量和所述第二加速度补偿量，并利用罚函数，获得最终无人机的加速度补偿量，包括：

801：将所述第一加速度补偿量和所述第二加速度补偿量相加，作为加速度补偿量；

由于传感器噪声影响，计算得到的加速度补偿量不可避免地存在误差，对于旋翼阶段而言，如果直接以所述加速度补偿量进行补偿容易引起额外的姿态估计误差，因此还需以无人机空速作为判断标准进行补偿，以进一步提高补偿方法的精确度。

802：根据无人机的固定翼飞行巡航速度设置第一空速阈值和第二空速阈值，通过空速计测量得到当前时刻无人机的空速；

，为无人机固定翼飞行的巡航速度。

当空速大于阈值时，无人机处于固定翼飞行状态，加速度计测量值需要补偿；

当空速小于时，无人机处于旋翼飞行状态，加速度计测量值不需要补偿。

803：根据所述第一空速阈值、第二空速阈值和当前时刻无人机的空速，建立加速度补偿罚函数；

加速度补偿罚函数是为了使得当空速处于和之间时加速度补偿值能够平滑地过渡。

804：根据所述加速度补偿罚函数加速度补偿和所述加速度补偿量，计算获得最终无人机的加速度补偿量，

（10）

式中，为加速度补偿罚函数；为加速度补偿量。

将加速度计测量得到的加速度减去，得到重力加速度在机体坐标系下的修正值，进而结合陀螺仪的数据对无人机姿态进行估计，可以得到比较准确的姿态数据。

在某个实施例中，所述加速度补偿罚函数为

（3）

式中，为当前时刻无人机的空速；为第一空速阈值；为第二空速阈值。

本发明还提供一种垂直起降固定翼无人机姿态估计中加速度补偿系统，包括：

速度信息获取模块，用于利用gps获得当前时刻绝对坐标下无人机的速度信息；获取上一时刻无人机的姿态转换矩阵，根据所述姿态转换矩阵和所述绝对坐标下无人机的速度信息，得到当前时刻机体坐标系下无人机的速度信息；所述机体坐标系包括x轴、y轴和z轴三个坐标轴，x轴为机头方向，z轴为垂直向下，y轴与x轴、z轴构成右手坐标系；

速度信息处理模块，用于对所述机体坐标系下无人机的速度信息进行求导，得到无人机在x轴、y轴和z轴三个方向上的加速度信息；根据垂直起降固定翼无人机在固定翼飞行时的状态和所述无人机在x轴、y轴和z轴三个方向上的加速度信息，获得第一加速度补偿量；

角速度信息获取模块，用于利用陀螺仪测量获得当前时刻无人机的角速度，对所述角速度进行低通滤波，得到平滑角速度；

角速度信息处理模块，用于根据科氏加速度计算法则，利用所述机体坐标系下无人机的速度信息和所述平滑角速度，计算获得科氏加速度；根据科氏加速度对垂直起降固定翼无人机姿态的影响，获得第二加速度补偿量；

加速度补偿模块，用于根据所述第一加速度补偿量和所述第二加速度补偿量，并利用罚函数，获得最终无人机的加速度补偿量。

本发明还提供一种计算机设备，包括存储器和处理器，所述存储器存储有计算机程序，所述处理器执行所述计算机程序时实现上述所述方法的步骤。

以上所述仅为本发明的优选实施例，并非因此限制本发明的专利范围，凡是在本发明的发明构思下，利用本发明说明书及附图内容所作的等效结构变换，或直接/间接运用在其他相关的技术领域均包括在本发明的专利保护范围内。