一种飞行器陀螺仪的零点漂移偏差和姿态估计方法与流程

本发明涉及飞行器陀螺仪校准的技术领域，尤其是一种飞行器陀螺仪的零点漂移偏差和姿态估计方法。

背景技术：

飞行器在姿态控制的时候，需要用到角速度和姿态。角速度一般可以由陀螺仪测量得到，但是由于供电和器件自身的原因，陀螺仪在每次上电的初始时刻将有一个不固定的常值，称之为零点漂移值，零点漂移值受供电电源和环境温度等因素的影响，会大大影响陀螺仪的精度。飞行器姿态可以通过陀螺仪的角速度数据对飞行器姿态运动学方程解算获得，也可以通过加速度计获取，但陀螺仪角速度数据存在误差，加速度计存在噪声干扰，导致了其获得的姿态并不精确。

在飞行器姿态控制的时候，由于飞行器姿态和角速度数据获取的不精确，导致飞行器的控制力矩将会有偏差，可能会导致飞行器的控制失效。另外飞行器在平稳飞行的时候，期望角速度一般为0，但由于存在零点漂移值，其所测得的角速度将不为0，会对飞行器的平稳控制产生极大的威胁，严重可能导致飞行器的震颤从而造成控制失效。

在现有的技术中，一般针对飞行器零点漂移采用物理标定偏差的办法，如“四旋翼飞行器航姿参考系统的误差补偿方法研究，杨志永,《计算机测量与控制》,2016年02期”，即在陀螺仪每次上电时候，对陀螺仪进行物理标定，该标定方法需要采集大量陀螺仪的数据，该标定方法不仅费时费力，而且精度不是很高。现有的技术中，姿态估计一般采用卡尔曼滤波算法，卡尔曼滤波对高斯噪声具有一定的抑制作用，但在实际飞行器系统中，传感器测量数据存在大量非高斯噪声。

技术实现要素：

为了克服上述现有技术中的缺陷，本发明提供一种飞行器陀螺仪的零点漂移偏差和姿态估计方法，基于飞行器姿态运动学方程设计有限时间观测器的数学模型，使得有限时间观测器更具有针对性，且具有更高的抗干扰性能和更快的收敛性，本发明的精度高、收敛性快，能够实时快速、准确地估计零点漂移偏差和飞行器姿态值。

为实现上述目的，本发明采用以下技术方案，包括：

一种飞行器陀螺仪的零点漂移偏差和姿态估计方法，包括以下步骤：

s1，设计有限时间观测器，所述有限时间观测器用于根据由加速度计测出的姿态测量值和由陀螺仪测出的角速度测量值，在有限的时间内估计出零点漂移偏差和飞行器姿态值；

s2，将由加速度计测出的姿态测量值和由陀螺仪测出的角速度测量值输入有限时间观测器中，通过有限时间观测器将零点漂移偏差和飞行器姿态值在有限的时间内估计出来；

s3，将陀螺仪测出的角速度测量值减去零点漂移偏差，即可得到角速度的真实值；有限时间观测器估计输出的飞行器姿态值即为飞行器真实姿态值。

步骤s1中，基于飞行器的姿态运动学方程设计所述有限时间观测器，具体过程如下所示：

飞行器的姿态运动学方程：

其中，σ为基于修正的罗德里格参数mrps所表示的姿态，σ为三维向量，σ＝[σ1,σ2,σ3]；为姿态σ对时间的导数；ω为实际角速度即角速度真实值；

g(·)为姿态矩阵，如下所示：

其中，i3为三阶单位矩阵；σ^×为如下所示矩阵：σ^t为σ的转置矩阵；

所设计的有限时间观测器，如下所示：

其中，为姿态估计值；为姿态估计值对时间的导数；

b为零点偏移偏差真实值；为零点偏移偏差估计值；为零点偏移偏差估计值对时间的导数；

k1为常数，k1＞0；k2为常数，k2＞0；α1为常数，0＜α1＜0.5；α2为常数，α2＝2α1-1；

ωg为陀螺仪的角速度测量值，角速度测量值ωg包括角速度真实值ω和零点偏移偏差真实值b，ωg＝ω+b；

sig(·)表示符号函数；是指sig(e1)的α1次方；是指sig(e1)的α2次方；

g(·)为姿态矩阵；

e1为由加速度计测出的姿态测量值σ与姿态估计值之间的相对误差，即飞行器期望姿态值σe与姿态估计值之间的向量差；

σ为输入有限时间观测器中的由加速度计测出的姿态测量值；

r(e1)为旋转矩阵；h(·)为矩阵，无实意，仅用于表示旋转矩阵为r(e1)为两个矩阵即h(σe)、相乘得到；i3为三阶单位矩阵；

符号用于计算两个姿态之间即由加速度计测出的姿态测量值σ与姿态估计值的相对误差e1，符号的具体计算方式如下所示：

其中，设则为如下所示的矩阵：

对所设计的有限时间观测器的合理性即收敛性进行分析，具体如下所示：

令从而得出有限时间观测器的误差系统的动态方程为：

其中，为零点偏移偏差真实值b对时间的导数；为零点偏移偏差估计值对时间的导数；为e1对时间的导数；为e2对时间的导数；e2为零点偏移偏差估计值与零点偏移偏差真实值b之间的差值；所述误差系统包括两个误差状态即e1、e2；

由于零点偏移偏差真实值b为常数值，从而可以得到：

根据有限时间的稳定性理论，可以得到误差系统中误差状态(e1,e2)将在有限时间t内收敛到零，即收敛时间在达到该有限时间t后，使得e1(t)≡0，e2(t)≡0，从而使得零点偏移偏差估计值在有限时间t内收敛到零点偏移偏差真实值b；且姿态估计值在有限时间t内收敛到由加速度计测出的姿态测量值σ，即收敛到飞行器真实姿态值。

本发明的优点在于：

(1)本发明首先设计飞行器的有限时间观测器，将飞行器陀螺仪测量出的带零点漂移误差的角速度测量值和飞行器的期望姿态输入有限时间观测器中，通过有限时间观测器可以估计零点漂移的偏差。本发明中的有限时间观测器是基于飞行器姿态运动学的数学模型，更具有针对性，且具有更高的抗干扰性能和更快的收敛性，因此本发明的精度高、收敛性快，能够实时快速、准确地估计负载值。

附图说明

图1为本发明的一种飞行器陀螺仪的零点漂移偏差和姿态估计方法的流程图。

图2为本实施例中的零点漂移偏差估计值的响应曲线。

图3为本实施例中的飞行器姿态估计值的响应曲线。

具体实施方式

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

由图1所示，本发明的一种飞行器陀螺仪的零点漂移偏差和姿态估计方法，包括以下步骤：

s1，设计一个有限时间观测器，所述有限时间观测器用于根据由加速度计测出的姿态测量值和由陀螺仪测出的角速度测量值，在有限的时间内估计出零点漂移偏差和飞行器姿态值。

s2，将由加速度计测出的姿态测量值和由陀螺仪测出的角速度测量值输入有限时间观测器中，通过有限时间观测器将零点漂移偏差和飞行器姿态值在有限的时间内估计出来；

通过估计陀螺仪的零点漂移偏差获得陀螺仪角速度的真实值，可通过飞行器姿态运动学，获得飞行器姿态估计值，即观测器的输出结果。该值通过真实陀螺仪角速度值计算所得，有较高的精度，可以用作飞行器的控制。

s3，将陀螺仪测出的角速度测量值减去零点漂移偏差，即可得到真实的角速度；有限时间观测器估计输出的飞行器姿态值即为飞行器真实姿态值。

步骤s1中，基于飞行器的姿态运动学方程设计所述有限时间观测器，并对所设计的有限时间观测器的合理性即收敛性进行分析，具体过程如下所示：

飞行器的姿态运动学方程：

其中，σ为基于修正的罗德里格参数(mrps)所表示的姿态，σ为三维向量，σ＝[σ1,σ2,σ3]；为姿态σ对时间的导数；ω为实际角速度即角速度真实值；

g(·)为姿态矩阵，如下所示：

其中，i3为三阶单位矩阵；σ^×为如下所示矩阵：

设计有限时间观测器为：

其中，为姿态估计值；为姿态估计值对时间的导数；

b为零点偏移偏差真实值；为零点偏移偏差估计值；为零点偏移偏差估计值对时间的导数；

k1为常数，k1＞0；k2为常数，k2＞0；α1为常数，0＜α1＜0.5；α2为常数，α2＝2α1-1；

ωg为陀螺仪的角速度测量值，角速度测量值ωg包括角速度真实值ω和零点偏移偏差真实值b；

e1为由加速度计测出的姿态测量值σ与姿态估计值之间的相对误差，即飞行器期望姿态值σe与姿态估计值之间的向量差；

其中，σe为输入有限时间观测器中的由加速度计测出的姿态测量值；

r(e1)为旋转矩阵；h(·)为矩阵，无实意，仅用于表示旋转矩阵为r(e1)为两个矩阵即h(σe)、相乘得到；

sig(·)表示符号函数；是指sig(e1)的α1次方；

符号用于计算两个向量即向量a与向量b之间相对误差，且向量a与向量b中的元素个数相同，符号的具体计算方式如下所示：

其中，设b＝[b1,b2,b3]，则b^×为如下所示的矩阵：

本发明中，即为计算两个姿态之间即由加速度计测出的姿态测量值σ与姿态估计值的相对误差e1，具体计算方式如下所示：

其中，设则为如下所示的矩阵：

针对所设计的有限时间观测器的合理性即收敛性进行分析，具体如下所示：

令从而得出误差系统的动态方程为：

由于零点偏移偏差真实值b为常数值，从而可以得到：

根据有限时间的稳定性理论，可以得到误差系统动态方程中状态(e1,e2)将在有限时间t内收敛到零，即收敛时间在达到该有限时间t后，使得e1(t)≡0，e2(t)≡0，从而使得零点偏移偏差估计值在有限时间t内收敛到零点偏移偏差真实值b，且姿态估计值在有限时间t内收敛到由加速度计测出的姿态测量值σ，即收敛到飞行器真实姿态值。因此，通过消减收敛后的零点偏移偏差估计值即相当于消减零点偏移偏差真实值b，就可以得到角速度真实值ω，ω＝ωg-b。

此外，观测器输出的飞行器姿估计值是利用角速度真实值ω通过姿态运动学方程计算的，即：

可以看出相对于由加速度计测出的姿态测量值σ，姿态估计值更具有参考意义，可以用于姿态控制。

本实施例中，飞行器上为三轴陀螺仪，分别对三个方位测量得到三个角速度测量值ωg1、ωg2、ωg3，实验采用matlab软件进行仿真，对飞行器姿态采用pd控制，控制参数为kp＝10，ki＝3，控制飞行器到达期望姿态σd＝[0.60.81]^t，此外设飞行器陀螺仪的零点漂移偏差数值为b＝[0.30.30.3]^trad/sec，取飞行器的转动惯量如下：

仿真环境下，选择有限时间观测器的控制参数：k1＝14，k2＝2，α1＝0.55，α2＝2α1-1＝0.1，选择有限时间观测器的初始值：

在有限时间观测器的作用下，可以得到三轴陀螺仪三个方位的零点偏移偏差真实值，由图2所示，第一个方位的零点偏移偏差估计值估计到零点偏移偏差真实值b1的时间为1.865s，第二个方位的零点偏移偏差估计值估计到零点偏移偏差真实值b2的时间为1.87s，第三个方位的零点偏移偏差估计值估计到零点偏移偏差真实值b3的时间为1.306s，由图3所示，飞行器估计姿态值的响应曲线，在1.87s有限时间观测器获得了真实的陀螺仪零点漂移偏差后，1.87s后后的飞行器估计姿态值即为飞行器真实姿态值，最终飞行器真实姿态值在10.02s到达期望姿态。由此可知，本发明所提出的一种基于有限时间观测器的飞行器陀螺仪的零点漂移偏差估计和姿态估计方法对零点偏移偏差的估计更快，获得的姿态估计值更精确，且本发明所设计的有限时间观测器具有较快的收敛性。

综上所述，本发明的一种基于有限时间观测器的飞行器陀螺仪的零点漂移偏差估计和姿态估计方法，精度高、收敛性快，能够实时快速、准确的估计出零点漂移偏差真实值和飞行器姿态值。

以上仅为本发明创造的较佳实施例而已，并不用以限制本发明创造，凡在本发明创造的精神和原则之内所作的任何修改、等同替换和改进等，均应包含在本发明创造的保护范围之内。