一种应用于农用无人机植保作业的姿态解算方法技术

本发明专利技术公开了一种应用于农用无人机植保作业的姿态解算方法，该方法针对农用无人机在室外进行植保作业时，由于受到自然风场等干扰因素的影响导致传统滤波方法精度降低的问题，提出利用加速度和磁场强度的信息修正陀螺仪输出的角速度，并将根据修正后的角速度解算出的姿态四元数作为量测值，陀螺仪输出的角速度信息以及姿态四元数作为状态量，建立自适应扩展卡尔曼滤波的数学模型，并且对系统噪声协方差矩阵以及量测噪声协方差矩阵引入自适应因子，提高算法在状态突变时的鲁棒性以及无人机姿态解算的精度。姿态解算的精度。姿态解算的精度。

【技术实现步骤摘要】
一种应用于农用无人机植保作业的姿态解算方法


[0001]本专利技术属于无人机控制以及惯性导航领域，涉及测量、捷联惯导、滤波技术，具体涉及一种应用于农用无人机植保作业的姿态解算方法。

技术介绍

[0002]目前，多旋翼无人机的应用越来越广泛，特别是在农业领域，伴随着农业现代化的发展，应用无人机开展植保作业能够更高效率的帮助农户防治病虫草害。
[0003]然而在山林果园施药等需要靶向施药的场合中，由于自然风场的影响，四旋翼无人机喷洒的药滴往往会偏移需要施药的果树，造成施药不均甚至脱靶的问题。要解决这一问题就需要实现无人机的高精度控制，而由四旋翼无人机的飞行特点可知，无人机的姿态会直接影响到无人机的飞行位置，进而影响到药滴落点，因此无人机的准确的姿态信息获取至关重要。加速度计，陀螺仪以及磁力计是无人机获取姿态信息的主要传感器，但是三者都存在各自的不足，其中陀螺仪容易受到温度的影响，存在温度漂移，而加速度计容易受到高频微小震动的影响，磁力计则容易受到周围磁场的干扰。因此为了提高无人机姿态解算精度，依靠单一的传感器是不行的，需要将三者采集到的数据信息进行融合。
[0004]现有的无人机多传感器数据融合算法主要是互补滤波，根据加速度计、陀螺仪以及磁力计所具有不同频域波段的噪声，来设计不同的滤波器对传感器输出的值进行数据融合处理，就能得到有效的期望数据。这种方法有一定的局限性，陀螺仪的低频噪声通过高通滤波器进行滤除，加速度计的高频噪声通过设计一个低通滤波器进行滤除，但是低通滤波器和高通滤波器的转接频率不好确定，需要进行大量的实验获得，同时互补滤波只有在传感器噪声具有不同频率的情况下才适用，而农用无人机在植保作业时会受到自然风场的干扰，无法保证传感器输出的信号中携带的噪声具有不同的频率，从而降低了解算出来的姿态数据的精度。

技术实现思路

[0005]为解决上述问题，本专利技术公开了一种应用于农用无人机植保作业的姿态解算方法，利用加速度和磁场强度的信息修正陀螺仪输出的角速度，并将根据修正后的角速度解算出的姿态四元数作为量测值，建立自适应扩展卡尔曼滤波(Adaptive Extended Kalman Filtering,AEKF)的数学模型，并且对系统噪声协方差矩阵以及量测噪声协方差矩阵引入自适应因子，提高算法在状态突变时的鲁棒性以及无人机姿态解算的精度。
[0006]为达到上述目的，本专利技术的技术方案如下：
[0007]一种应用于农用无人机植保作业的姿态解算方法，包括以下步骤：
[0008]1)计算初始姿态角：根据方向余弦矩阵建立重力加速度和加速度计测量值的关系，方向余弦矩阵与磁力计测量值的关系，计算初始横滚角、俯仰角以及偏航角；
[0009]2)角速度修正：利用加速度计和磁力计输出的信息计算修正向量，通过PI控制器得到陀螺仪角速度的修正值，与陀螺仪输出的数据相加；
[0010]3)姿态解算：根据陀螺仪输出的数据，求解四元素微分方程得到姿态四元数Q
g
，根据修正后的角速度求解四元数微分方程，得到融合了加速度计、磁力计以及陀螺仪数据信息的姿态四元数Q
r
；
[0011]4)AEKF滤波器设计：将步骤3)得到的更新之后的姿态四元数Q
r
作为量测值建立量测方程，以姿态四元数和陀螺仪随机漂移作为状态向量建立状态方程，并对系统噪声协方差矩阵以及量测噪声协方差矩阵引入自适应因子；
[0012]5)针对AEKF算法进行时间更新和量测更新，得到滤波校正后的四元数，根据四元数与姿态角的关系，将其转换为姿态角。
[0013]进一步地，所述步骤1)中计算初始姿态角具体步骤包括：
[0014](1.1)建立导航坐标系与机体坐标系得转换关系：将地理坐标系作为导航坐标系n，方向为北
‑
东
‑
地，机体坐标系的方向为前
‑
右
‑
下，则方向余弦矩阵可以表示为：
[0015][0016](1.2)计算横滚角、俯仰角以及偏航角：根据方向余弦矩阵就能得到加速度计测量值[a
bx
，a
by
，a
bz
]T
与重力加速度[0，0，
‑
g]T
的关系：
[0017][0018]无人机横滚角φ与俯仰角θ为：
[0019][0020]将磁力计输出的值转换到导航坐标系，计算出n系下的磁场强度：
[0021][0022]其中，[m
bx
，m
by
，m
bz
]T
为磁力计输出的机体坐标系下的磁场强度m
b
，从而计算出航向角ψ：
[0023][0024]进一步地，所述步骤2)中角速度修正具体步骤包括：
[0025](2.1)计算误差向量：利用步骤1)得到的方向余弦矩阵将导航坐标系n下的单位重力加速度[0，0，
‑
1]T
转换到机体坐标系b得到k1：
[0026][0027]根据加速度计测量值a和机体坐标系下的重力向量k1，计算修正量α1:
[0028]α1＝a
×
k1[0029]根据磁力计的输出m
b
计算出n系下的磁场强度m
n
：
[0030][0031]将其投影到n系水平面得到β：
[0032][0033]计算偏航角在水平面上的偏差k2，计算修正量α2：
[0034][0035][0036]误差向量：e＝α1+α2。
[0037](2.2)计算修正量：将误差向量e输入到PI控制器，调节PI控制器的比例系数K
p
和积分系数K
i
，PI控制器输出值为角速度修正量δw
[0038][0039]根据陀螺仪输出的数据，得到融合了加速度计和磁力计信息之后的角速度修正值：w＝w
g
+δw，其中w
g
为陀螺仪输出的角速度。
[0040]进一步地，所述步骤3)中姿态解算具体步骤包括：
[0041](3.1)建立四元数微分方程：根据四元数Q与机体角速度w的运算关系建立四元数微分方程：
[0042][0043]其中：Q＝{q
0 q
1 q
2 q3]T
[0044](3.2)带入陀螺仪输出的角速度数据求解Q
g
：
[0045][0046]其中，以及为陀螺仪输出角速度ω
g
的三轴分量，并将步骤(1.2)得到的初始姿态角转换为初始姿态四元数Q0：
[0047][0048]求解上述微分方程，得到Q
g
。
[0049](3.3)根据步骤2)输出值求解Q
r
：根据步骤(2.2)修正之后的角速度求解四元数微分方程得到Q
r
[0050][0051]该微分方本文档来自技高网...

【技术保护点】

【技术特征摘要】
1.一种应用于农用无人机植保作业的姿态解算方法，其特征在于，包括以下几个步骤：1)计算初始姿态角：根据方向余弦矩阵建立重力加速度和加速度计测量值的关系，方向余弦矩阵与磁力计测量值的关系，计算初始横滚角、俯仰角以及偏航角；2)角速度修正：利用加速度计和磁力计输出的信息计算修正向量，通过PI控制器得到陀螺仪角速度的修正值，与陀螺仪输出的数据相加；3)姿态解算：根据陀螺仪输出的数据，求解四元素微分方程得到姿态四元数Q
g
，根据修正后的角速度求解四元数微分方程，得到融合了加速度计、磁力计以及陀螺仪数据信息的姿态四元数Q
r
：4)AEKF滤波器设计：将步骤3)得到的更新之后的姿态四元数Q
r
作为量测值建立量测方程，以姿态四元数和陀螺仪随机漂移作为状态向量建立状态方程，并对系统噪声协方差矩阵以及量测噪声协方差矩阵引入自适应因子；5)针对AEKF算法进行时间更新和量测更新，得到滤波校正后的四元数，根据四元数与姿态角的关系，将其转换为姿态角。2.如权利要求1所述的一种应用于农用无人机植保作业的姿态解算方法，其特征在于，所述步骤1)中计算初始姿态角具体步骤包括：(1.1)建立导航坐标系与机体坐标系得转换关系：将地理坐标系作为导航坐标系n，方向为北
‑
东
‑
地，机体坐标系的方向为前
‑
右
‑
下，则方向余弦矩阵表示为：(1.2)计算横滚角、俯仰角以及偏航角：根据方向余弦矩阵就能得到加速度计测量值[a
bx
，a
by
，a
bz
]
T
与重力加速度[0，0，
‑
g]
T
的关系：无人机横滚角φ与俯仰角θ无人机横滚角φ与俯仰角θ将磁力计输出的值转换到导航坐标系，计算出n系下的磁场强度：其中，[m
bz
，m
by
，m
bz
]
T
为磁力计输出的机体坐标系下的磁场强度m
b
，从而计算出航向角ψ：3.如权利要求1所述的一种应用于农用无人机植保作业的姿态解算方法，其特征在于，
所述步骤2)中角速度修正具体步骤包括：(2.1)计算误差向量：利用步骤1)得到的方向余弦矩阵将导航坐标系n下的单位重力加速度[0，0，
‑
1]
T
转换到机体坐标系b得到k1：根据加速度计测量值a和机体坐标系下的重力向量k1，计算修正量α1：α1＝a
×
k1根据磁力计的输出m
b
计算出n系下的磁场强度m
n
：将其投影到n系水平面得到β：计算偏航角在水平面上的偏差k2，计算修正量α2：：误差向量：e＝α1+α2(2.2)计算修正量：将误差向量e输入到PI控制器，调节PI控制器的比例系数K
p
和积分系数K
i
，PI控制器输出值为角速度修正量δw：根据陀螺仪输出的数据，得到融合了加速度计和磁力计信息之后的角速度修正值：w＝w
g
+δw，其中w
g
为陀螺仪输出的角速度。4.如权利要求1所述的一种应用于农用无人机植保作业的姿态解算方法，其特征在于，所述步骤3)中姿态解算具体步骤包括：(3.1)建立四元数微...

【专利技术属性】
技术研发人员：吴峻，张伦，张翔，
申请(专利权)人：东南大学，
类型：发明
国别省市：