CN112179342A - 一种估计无人机姿态的方法及系统 - Google Patents
一种估计无人机姿态的方法及系统 Download PDFInfo
- Publication number
- CN112179342A CN112179342A CN202010998123.4A CN202010998123A CN112179342A CN 112179342 A CN112179342 A CN 112179342A CN 202010998123 A CN202010998123 A CN 202010998123A CN 112179342 A CN112179342 A CN 112179342A
- Authority
- CN
- China
- Prior art keywords
- time
- attitude
- gyroscope
- accelerometer
- value
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Granted
Links
Images
Classifications
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
- G01C21/10—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration
- G01C21/12—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning
- G01C21/16—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation
- G01C21/18—Stabilised platforms, e.g. by gyroscope
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C1/00—Measuring angles
Landscapes
- Engineering & Computer Science (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Automation & Control Theory (AREA)
- Navigation (AREA)
- Gyroscopes (AREA)
Abstract
Description
技术领域
本发明涉及无人机技术领域,具体而言,涉及一种估计无人机姿态的方法及系统。
背景技术
随着无人机在快递运输、救灾、环境监测和电力巡检等各个领域的广泛应用,其准确的飞行姿态测量变得越来越重要。当前无人机通常使用由陀螺仪和加速度计组成的低成本惯性测量单元(IMU)进行飞行姿态测量,但低成本陀螺仪具有积分累积误差,且加速度计在动态姿态测量中,容易受到高频噪声的影响,导致无人机冬天姿态测量精度较低。
针对上述问题,国内外许多学者进行了大量的研究工作。目前,主流姿态估计方法是基于卡尔曼滤波和互补滤波,或是两种结合。线性卡尔曼滤波可以降低滤波器的复杂度,但是姿态估计准确度不高。互补卡尔曼滤波需要计算大量的三角函数,并且在计算中容易产生奇异值。扩展卡尔曼滤波,无损卡尔曼滤波,粒子滤波等也被用于姿态估计,在一定程度上提高了姿态估计的准确性。但是,卡尔曼滤波容易出现发散问题,且噪声模型设计和协方差计算等也存在一些理论和实际问题。相关技术中的互补滤波由于增益是定量且不能动态调整,因此当无人机移动太快而外部干扰太大时,估计精度较差。另有基于高斯牛顿法和互补滤波的方法,虽然可以有效抑制电磁干扰和动态条件下的姿态角估计误差,但在姿态估计过程中增加了计算量。
发明内容
为解决上述问题,本发明的目的在于提供一种估计无人机姿态的方法及系统,能够自适应地调整参数,准确估计出无人机的真实姿态,同时解决了人为调整参数耗时耗力的问题,可以很好地应用于各种飞行载体的姿态估计。
本发明提供了一种估计无人机姿态的方法,所述方法包括:
通过互补滤波器对加速计在t时刻的姿态估计值和陀螺仪在t时刻的姿态估计值进行融合处理,得到t时刻的无人机姿态估计值 其中,kt是所述陀螺仪在t时刻的姿态估计值的发散速率β的加权系数,1-kt是所述加速计在t时刻的姿态估计值的收敛速率的加权系数;
作为本发明进一步的改进,加速度计在t时刻的加速度测量值bat用四元数bat=[0ax ay az]表示,其中,ax、ay、az分别表示在无人机坐标系中x轴、y轴、z轴的加速度测量值分量,所述加速度测量值bat归一化后的加速度向量用表示,
其中,为所述互补滤波器在t-Δt时刻融合得到的无人机姿态估计值,Δt为所述互补滤波器的采样时间,为所述目标函数的梯度值,J表示雅可比行列式, 为所述陀螺仪在t时刻的姿态估计值的时间导数,α为比例因子。
作为本发明进一步的改进,所述方法还包括:
基于陀螺仪在t时刻的角度漂移值bωd,t对陀螺仪在t时刻的角速度测量值bωt进行补偿,得到补偿后的角速度值bωc,t;
作为本发明进一步的改进,所述角速度测量值用四元数bωt=[0 ωx ωy ω表示,其中,ωx、ωy、ωz分别表示在无人机坐标系中x轴、y轴、z轴的角速度测量值分量,
其中,基于陀螺仪在t时刻的角度漂移值bωd,t对陀螺仪在t时刻的角速度测量值bωt进行补偿,得到补偿后的角速度值bωc,t,基于所述补偿后的角速度值bωc,t和互补滤波器在t-Δt时刻融合得到的无人机姿态估计值得到所述陀螺仪在t时刻的姿态估计值包括:
基于所述陀螺仪在t时刻的角度漂移值bωd,t对所述角速度测量值bωt进行补偿,确定补偿后的角速度值bωc,t,bωc,t=bωt-bωd,t;
作为本发明进一步的改进,通过所述陀螺仪在t时刻的误差bωe,t
所述漂移补偿参数ζ表示消除所述陀螺仪非零平均测量误差的收敛速度:
本发明还提供了一种估计无人机姿态的系统,所述系统包括:
融合估计模块,用于通过互补滤波器对加速计在t时刻的姿态估计值和陀螺仪在t时刻的姿态估计值进行融合处理,得到t时刻的无人机姿态估计值 其中,kt是所述陀螺仪在t时刻的姿态估计值的发散速率β的加权系数,1-kt是所述加速计在t时刻的姿态估计值的收敛速率的加权系数;
作为本发明进一步的改进,加速度计在t时刻的加速度测量值bat用四元数bat=[0ax ay az]表示,其中,ax、ay、az分别表示在无人机坐标系中x轴、y轴、z轴的加速度测量值分量,所述加速度测量值bat归一化后的加速度向量用表示,
其中,为所述互补滤波器在t-Δt时刻融合得到的无人机姿态估计值,Δt为所述互补滤波器的采样时间,为所述目标函数的梯度值,J表示雅可比行列式, 为所述陀螺仪在t时刻的姿态估计值的时间导数,α为比例因子。
作为本发明进一步的改进,所述融合估计模块还用于:
基于陀螺仪在t时刻的角度漂移值bωd,t对陀螺仪在t时刻的角速度测量值bωt进行补偿,得到补偿后的角速度值bωc,t;
作为本发明进一步的改进,所述角速度测量值用四元数bωt=[0 ωx ωy ω表示,其中,ωx、ωy、ωz分别表示在无人机坐标系中x轴、y轴、z轴的角速度测量值分量,
其中,基于陀螺仪在t时刻的角度漂移值bωd,t对陀螺仪在t时刻的角速度测量值bωt进行补偿,得到补偿后的角速度值bωc,t,基于所述补偿后的角速度值bωc,t和互补滤波器在t-Δt时刻融合得到的无人机姿态估计值得到所述陀螺仪在t时刻的姿态估计值包括:
基于所述陀螺仪在t时刻的角度漂移值bωd,t对所述角速度测量值bωt进行补偿,确定补偿后的角速度值bωc,t,bωc,t=bωt-bωd,t;
所述漂移补偿参数ζ表示消除所述陀螺仪非零平均测量误差的收敛速度:
本发明还提供了一种电子设备,包括存储器和处理器,所述存储器用于存储一条或多条计算机指令,其中,所述一条或多条计算机指令被处理器执行以实现所述的方法。
发明还提供了一种计算机可读存储介质,其上存储有计算机程序,其特征在于,所述计算机程序被处理器执行以实现所述的方法。
本发明的有益效果为:采用梯度下降法对加速度计数据的姿态四元数进行优化,通过自适应增益互补滤波将优化的加速度计姿态四元数与陀螺仪数据进行融合,能够自适应地调整参数β,准确估计出无人机的真实姿态,同时解决了人为调整参数耗时耗力的问题,可以很好地应用于各种飞行载体的姿态估计。
附图说明
为了更清楚地说明本发明实施例或现有技术中的技术方案,下面将对实施例或现有技术描述中所需要使用的附图作简单地介绍。显而易见地,下面描述中的附图仅仅是本发明的一些实施例,对于本领域普通技术人员来讲,在不付出创造性劳动性的前提下,还可以根据这些附图获得其他的附图。
图1为本发明一示例性实施例所述的一种估计无人机姿态的方法的原理框图;
图2为本发明一示例性实施例所述的导航坐标系(n)和无人机坐标系(b)之间的关系以及基于单位四元数的相应变换的示意图;
图3为本发明一示例性实施例所述的对互补滤波器调整参数的实验结果,其中,(a)为滚动角的实验结果,(b)为俯仰角的实验结果,(c)为偏航角的实验结果,(d)为三个欧拉角插补后的实验结果;
图4为本发明一示例性实施例所述的对互补滤波器采用β=0.1和ζ=0.01的姿态估计结果,其中,(a)为滚动角的姿态估计结果,(b)为俯仰角的姿态估计结果,(c)为偏航角的姿态估计结果;
图5为本发明一示例性实施例所述的对互补滤波器采用β=0.1和ζ=0.01的姿态估计误差,其中,(a)为滚动角的姿态估计误差,(b)为俯仰角的姿态估计误差,(c)为偏航角的姿态估计误差;
图6为本发明一示例性实施例所述的对互补滤波器采用β=1和ζ=0.01的姿态估计结果,其中,(a)为滚动角的姿态估计结果,(b)为俯仰角的姿态估计结果,(c)为偏航角的姿态估计结果;
图7为本发明一示例性实施例所述的对互补滤波器采用β=1和ζ=0.01的姿态估计误差,其中,(a)为滚动角的姿态估计误差,(b)为俯仰角的姿态估计误差,(c)为偏航角的姿态估计误差。
具体实施方式
下面将结合本发明实施例中的附图,对本发明实施例中的技术方案进行清楚、完整地描述,显然,所描述的实施例仅仅是本发明一部分实施例,而不是全部的实施例。基于本发明中的实施例,本领域普通技术人员在没有作出创造性劳动前提下所获得的所有其他实施例,都属于本发明保护的范围。
需要说明,若本发明实施例中有涉及方向性指示(诸如上、下、左、右、前、后……),则该方向性指示仅用于解释在某一特定姿态(如附图所示)下各部件之间的相对位置关系、运动情况等,如果该特定姿态发生改变时,则该方向性指示也相应地随之改变。
另外,在本发明的描述中,所用术语仅用于说明目的,并非旨在限制本发明的范围。术语“包括”和/或“包含”用于指定所述元件、步骤、操作和/或组件的存在,但并不排除存在或添加一个或多个其他元件、步骤、操作和/或组件的情况。术语“第一”、“第二”等可能用于描述各种元件,不代表顺序,且不对这些元件起限定作用。此外,在本发明的描述中,除非另有说明,“多个”的含义是两个及两个以上。这些术语仅用于区分一个元素和另一个元素。结合以下附图,这些和/或其他方面变得显而易见,并且,本领域普通技术人员更容易理解关于本发明所述实施例的说明。附图仅出于说明的目的用来描绘本发明所述实施例。本领域技术人员将很容易地从以下说明中认识到,在不背离本发明所述原理的情况下,可以采用本发明所示结构和方法的替代实施例。
本发明实施例所述的一种估计无人机姿态的方法,如图1所示,所述方法包括:
通过互补滤波器对加速计在t时刻的姿态估计值和陀螺仪在t时刻的姿态估计值进行融合处理,得到t时刻的无人机姿态估计值 其中,kt是所述陀螺仪在t时刻的姿态估计值的发散速率β的加权系数,1-kt是所述加速计在t时刻的姿态估计值的收敛速率的加权系数;
相关技术中,通常使用传感器来实现无人机的飞行姿态测量,例如加速度计和陀螺仪等惯性测量单元(IMU)。陀螺仪用于测量角速度,具有高动态特性,它是一个间接测量角度的器件,它测量的是角度的导数,即角速度,要将角速度对时间积分才能得到角度。由于噪声等误差影响,在积分作用下不断积累,会导致陀螺仪的低频干扰和漂移。加速度计用于测量当前加速度(包含重力加速度)的方向,对高频信号敏感,在振动环境中高频干扰较大。因此,各个传感器除了自身的测量误差外,还容易受到温度、噪声等多方面因素的影响,以致测量数据不准确。本发明所述的方法利用互补滤波器将各个传感器的数据进行融合,同时通过加速度计的误差对互补滤波器的参数β进行动态调整,以适应于无人机飞行过程中姿态急剧变化的情况,提高无人机姿态估计的精度。所述无人机例如为多旋翼无人机等,所述加速度计和所述陀螺仪固定于所述无人机的框架上,本发明所述的方法对应用的无人机类型不做具体限定。
本发明所述的方法采用计算量少的梯度下降法对加速度计的姿态数据进行优化,获取加速度计的最优姿态估计。其中,kt是的最佳值是所述陀螺仪在测量过程中因积分漂移导致的所述陀螺仪在t时刻的姿态估计值的加权发散速率,其等于所述加速计在t时刻的姿态估计值的加权收敛速率相等,即可得根据动态调整的参数β,即可对t时刻的无人机姿态估计值进行动态调整,由于参数β动态可调,权重(kt,1-kt)动态可调,提高了互补滤波器的自适应调整性能,可以实现对无人机姿态的精确估计。
如前述所述,加速度计低频特性比较好,因为加速度的角度可以直解算出来,没有累积误差,所以长时间测量后也比较准确,但容易受到高频噪声和运动加速度的影响。而陀螺仪可以在短时间内提供高精度的姿态信息,但容易受到累积漂移和低频噪声的影响,会造成输出误差比较大,甚至无法使用。因此,所述互补滤波器主要针对具有互补频率特性的两个噪声源进行设计,通过所述互补滤波器可以根据各个传感器的特性进行姿态估计,通过互补滤波融合来优化姿态估计结果。所述互补滤波器包括低通滤波器和高通滤波器,其中,低通滤波器用于对加速度计进行滤波,使加速度计过滤高频信号,滤除短时的变化,高通滤波器用于对陀螺仪进行滤波,使陀螺仪过滤低频信号,滤出变化比较缓慢的信号,消除漂移,再以动态的权重(kt,1-kt)加权求和,得到整个频带的信号(姿态角度,包括偏航角、俯仰角和滚动角)。其中,互补滤波要求两个信号的干扰噪声处在不同的频率,通过设置两个滤波器的截止频率,确保融合后的信号能够覆盖需求频率。可以理解的是,互补即是将滤波器的两个部分加起来成为一个滤波器,使其能输出一个准确的线性估计。
在一种可选的实施方式中,加速度计在t时刻的加速度测量值bat用四元数bat=[0ax ay az]表示,其中,ax、ay、az分别表示在无人机坐标系中x轴、y轴、z轴的加速度测量值分量,所述加速度测量值bat归一化后的加速度向量用表示,
其中,所述加速度计进行姿态估计时,从无人机坐标系变换到导航坐标系的转换矩阵是有误差的,本发明所述的方法利用加速度计表征的四元数矩阵误差,将该误差矩阵用所述目标函数来表示,将优化问题表述为求解来实现,通过梯度下降法确定目标函数的极值,该方法对最小化目标函数来说易于实现和计算。
其中,为所述互补滤波器在t-Δt时刻融合得到的无人机姿态估计值,Δt为所述互补滤波器的采样时间,为所述目标函数的梯度值,J表示雅可比行列式, 为所述陀螺仪在t时刻的姿态估计值的时间导数,α为比例因子。
其中,对上述转化后的目标函数求导即可得到对应的雅克比矩阵:
再利用所得雅克比矩阵的转置矩阵,根据前述梯度计算式确定最终的计算目标,使用梯度下降法进行迭代计算,消除旋转矩阵误差的点,得到所述加速计在t时刻的姿态估计值其中,每个样本只需要迭代一次,姿态的收敛速度由步长μt决定,步长μt和无人机陀螺仪在t时刻的姿态估计值的时间导数相关,和采样时间Δt相关。
在一种可选的实施方式中,所述方法还包括:
基于陀螺仪在t时刻的角度漂移值bωd,t对陀螺仪在t时刻的角速度测量值bωt进行补偿,得到补偿后的角速度值bωc,t;
在一种可选的实施方式中,所述角速度测量值用四元数bωt=[0 ωx ωy ω表示,其中,ωx、ωy、ωz分别表示在无人机坐标系中x轴、y轴、z轴的角速度测量值分量,
其中,基于陀螺仪在t时刻的角度漂移值bωd,t对陀螺仪在t时刻的角速度测量值bωt进行补偿,得到补偿后的角速度值bωc,t,基于所述补偿后的角速度值bωc,t和互补滤波器在t-Δt时刻融合得到的无人机姿态估计值得到所述陀螺仪在t时刻的姿态估计值包括:
基于所述陀螺仪在t时刻的角度漂移值bωd,t对所述角速度测量值bωt进行补偿,确定补偿后的角速度值bωc,t,bωc,t=bωt-bωd,t;
在一种可选的实施方式中,通过所述陀螺仪在t时刻的误差bωe,t
所述漂移补偿参数ζ表示消除所述陀螺仪非零平均测量误差的收敛速度:
由前述所述,陀螺仪偏置随时间、温度和运动的变化而漂移,因此需要对陀螺偏置进行补偿。本发明所述的方法,以梯度下降法补偿后的角速度值bωc,t作为观测向量,对陀螺漂移进行补偿。
如图3所示,对互补滤波器进行了测试,其中,横坐标表示时间,纵坐标分别表示滚动角、俯仰角、偏航角、三个欧拉角(滚动角、俯仰角、偏航角)插补后的角度,图中的曲线为手动调整参数β所得到的姿态估计结果,图中的横线为本发明所述的方法根据加速度计的误差实时自动调整参数β所得到的姿态估计结果,从结果可以看出,手动调整参数无法实时获取最优的估计性能,需要将参数调整到合适的值才能获得与本发明所述的方法相同的估计性能,然而通过手动调整参数时,需要进行大量的计算,耗时耗力,不适用于实时的估计。
如图4所示,采用β=0.1和ζ=0.01对互补滤波器进行了测试,其中,横坐标表示时间,纵坐标表示姿态估计结果,图中与标定好的真实数据曲线变化更为一致的是本发明所述的方法根据加速度计的误差实时自动调整参数β所得到的姿态估计结果,图中与标定好的真实数据曲线变化误差较大的是通过手动调整参数β所得到的姿态估计结果。如图5所示,横坐标表示时间,纵坐标表示姿态估计误差,图中与标定好的真实数据误差较小的曲线是本发明所述的方法根据加速度计的误差实时自动调整参数β所得到姿态估计误差,图中与标定好的真实数据误差较大的曲线是通过手动调整参数β所得到的姿态估计误差。从结果可以看出,本发明所述的方法可以很好地估计横滚角、俯仰角和滚动角,手动调整参数在运动姿态变化较快时,姿态估计误差较大。
如图6所示,采用β=1和ζ=0.01对互补滤波器进行了测试,其中,横坐标表示时间,纵坐标表示姿态估计结果,本发明所述的方法根据加速度计的误差实时自动调整参数β所得到的姿态估计结果,以及通过手动调整参数β所得到的姿态估计结果与标定好的真实数据曲线变化都基本一致。如图7所示,横坐标表示时间,纵坐标表示姿态估计误差,本发明所述的方法根据加速度计的误差实时自动调整参数β所得到的姿态估计误差,以及通过手动调整参数β所得到的姿态估计误差相差不多。从结果可以看出,当参数β调整到一定值(例如1)时,互补滤波器获得了较佳的估计性能,本发明所述的方法能够自适应的调整参数β,以满足姿态估计的需求,解决了人为调整参数耗时耗力的问题,可以很好地应用于各种飞行载体的姿态估计。
本发明实施例所述的一种估计无人机姿态的系统,所述系统包括:
融合估计模块,用于通过互补滤波器对加速计在t时刻的姿态估计值和陀螺仪在t时刻的姿态估计值进行融合处理,得到t时刻的无人机姿态估计值 其中,kt是所述陀螺仪在t时刻的姿态估计值的发散速率β的加权系数,1-kt是所述加速计在t时刻的姿态估计值的收敛速率的加权系数;
相关技术中,通常使用传感器来实现无人机的飞行姿态测量,例如加速度计和陀螺仪等惯性测量单元(IMU)。陀螺仪用于测量角速度,具有高动态特性,它是一个间接测量角度的器件,它测量的是角度的导数,即角速度,要将角速度对时间积分才能得到角度。由于噪声等误差影响,在积分作用下不断积累,会导致陀螺仪的低频干扰和漂移。加速度计用于测量当前加速度(包含重力加速度)的方向,对高频信号敏感,在振动环境中高频干扰较大。因此,各个传感器除了自身的测量误差外,还容易受到温度、噪声等多方面因素的影响,以致测量数据不准确。本发明所述的系统利用互补滤波器将各个传感器的数据进行融合,同时通过加速度计的误差对互补滤波器的参数β进行动态调整,以适应于无人机飞行过程中姿态急剧变化的情况,提高无人机姿态估计的精度。所述无人机例如为多旋翼无人机等,所述加速度计和所述陀螺仪固定于所述无人机的框架上,本发明所述的系统对应用的无人机类型不做具体限定。
本发明所述的系统采用计算量少的梯度下降法对加速度计的姿态数据进行优化,获取加速度计的最优姿态估计。其中,kt是的最佳值是所述陀螺仪在测量过程中因积分漂移导致的所述陀螺仪在t时刻的姿态估计值的加权发散速率,其等于所述加速计在t时刻的姿态估计值的加权收敛速率相等,即可得根据动态调整的参数β,即可对t时刻的无人机姿态估计值进行动态调整,由于参数β动态可调,权重(kt,1-kt)动态可调,提高了互补滤波器的自适应调整性能,可以实现对无人机姿态的精确估计。
如前述所述,加速度计低频特性比较好,因为加速度的角度可以直解算出来,没有累积误差,所以长时间测量后也比较准确,但容易受到高频噪声和运动加速度的影响。而陀螺仪可以在短时间内提供高精度的姿态信息,但容易受到累积漂移和低频噪声的影响,会造成输出误差比较大,甚至无法使用。因此,所述互补滤波器主要针对具有互补频率特性的两个噪声源进行设计,通过所述互补滤波器可以根据各个传感器的特性进行姿态估计,通过互补滤波融合来优化姿态估计结果。所述互补滤波器包括低通滤波器和高通滤波器,其中,低通滤波器用于对加速度计进行滤波,使加速度计过滤高频信号,滤除短时的变化,高通滤波器用于对陀螺仪进行滤波,使陀螺仪过滤低频信号,滤出变化比较缓慢的信号,消除漂移,再以动态的权重(kt,1-kt)加权求和,得到整个频带的信号(姿态角度,包括偏航角、俯仰角和滚动角)。其中,互补滤波要求两个信号的干扰噪声处在不同的频率,通过设置两个滤波器的截止频率,确保融合后的信号能够覆盖需求频率。可以理解的是,互补即是将滤波器的两个部分加起来成为一个滤波器,使其能输出一个准确的线性估计。
在一种可选的实施方式中,加速度计在t时刻的加速度测量值bat用四元数bat=[0ax ay az]表示,其中,ax、ay、az分别表示在无人机坐标系中x轴、y轴、z轴的加速度测量值分量,所述加速度测量值bat归一化后的加速度向量用表示,
其中,所述加速度计进行姿态估计时,从无人机坐标系变换到导航坐标系的转换矩阵是有误差的,本发明所述的方法利用加速度计表征的四元数矩阵误差,将该误差矩阵用所述目标函数来表示,将优化问题表述为求解来实现,通过梯度下降法确定目标函数的极值,该方法对最小化目标函数来说易于实现和计算。
其中,为所述互补滤波器在t-Δt时刻融合得到的无人机姿态估计值,Δt为所述互补滤波器的采样时间,为所述目标函数的梯度值,J表示雅可比行列式, 为所述陀螺仪在t时刻的姿态估计值的时间导数,α为比例因子。
其中,对上述转化后的目标函数求导即可得到对应的雅克比矩阵:
再利用所得雅克比矩阵的转置矩阵,根据前述梯度计算式确定最终的计算目标,使用梯度下降法进行迭代计算,消除旋转矩阵误差的点,得到所述加速计在t时刻的姿态估计值其中,每个样本只需要迭代一次,姿态的收敛速度由步长μt决定,步长μt和无人机陀螺仪在t时刻的姿态估计值的时间导数相关,和采样时间Δt相关。
在一种可选的实施方式中,所述融合估计模块还用于:
基于陀螺仪在t时刻的角度漂移值bωd,t对陀螺仪在t时刻的角速度测量值bωt进行补偿,得到补偿后的角速度值bωc,t;
在一种可选的实施方式中,所述角速度测量值用四元数bωt=[0ωxωyω表示,其中,ωx、ωy、ωz分别表示在无人机坐标系中x轴、y轴、z轴的角速度测量值分量,
其中,基于陀螺仪在t时刻的角度漂移值bωd,t对陀螺仪在t时刻的角速度测量值bωt进行补偿,得到补偿后的角速度值bωc,t,基于所述补偿后的角速度值bωc,t和互补滤波器在t-Δt时刻融合得到的无人机姿态估计值得到所述陀螺仪在t时刻的姿态估计值包括:
基于所述陀螺仪在t时刻的角度漂移值bωd,t对所述角速度测量值bωt进行补偿,确定补偿后的角速度值bωc,t,bωc,t=bωt-bωd,t;
所述漂移补偿参数ζ表示消除所述陀螺仪非零平均测量误差的收敛速度:
由前述所述,陀螺仪偏置随时间、温度和运动的变化而漂移,因此需要对陀螺偏置进行补偿。本发明所述的方法,以梯度下降法补偿后的角速度值bωc,t作为观测向量,对陀螺漂移进行补偿。
如图3所示,对互补滤波器进行了测试,其中,横坐标表示时间,纵坐标分别表示滚动角、俯仰角、偏航角、三个欧拉角(滚动角、俯仰角、偏航角)插补后的角度,图中的曲线为手动调整参数β所得到的姿态估计结果,图中的横线为本发明所述的系统根据加速度计的误差实时自动调整参数β所得到的姿态估计结果,从结果可以看出,手动调整参数无法实时获取最优的估计性能,需要将参数调整到合适的值才能获得与本发明所述的方法相同的估计性能,然而通过手动调整参数时,需要进行大量的计算,耗时耗力,不适用于实时的估计。
如图4所示,采用β=0.1和ζ=0.01对互补滤波器进行了测试,其中,横坐标表示时间,纵坐标表示姿态估计结果,图中与标定好的真实数据曲线变化更为一致的是本发明所述的系统根据加速度计的误差实时自动调整参数β所得到的姿态估计结果,图中与标定好的真实数据曲线变化误差较大的是通过手动调整参数β所得到的姿态估计结果。如图5所示,横坐标表示时间,纵坐标表示姿态估计误差,图中与标定好的真实数据误差较小的曲线是本发明所述的系统根据加速度计的误差实时自动调整参数β所得到姿态估计误差,图中与标定好的真实数据误差较大的曲线是通过手动调整参数β所得到的姿态估计误差。从结果可以看出,本发明所述的系统可以很好地估计横滚角、俯仰角和滚动角,手动调整参数在运动姿态变化较快时,姿态估计误差较大。
如图6所示,采用β=1和ζ=0.01对互补滤波器进行了测试,其中,横坐标表示时间,纵坐标表示姿态估计结果,本发明所述的系统根据加速度计的误差实时自动调整参数β所得到的姿态估计结果,以及通过手动调整参数β所得到的姿态估计结果与标定好的真实数据曲线变化都基本一致。如图7所示,横坐标表示时间,纵坐标表示姿态估计误差,本发明所述的系统根据加速度计的误差实时自动调整参数β所得到的姿态估计误差,以及通过手动调整参数β所得到的姿态估计误差相差不多。从结果可以看出,当参数β调整到一定值(例如1)时,互补滤波器获得了较佳的估计性能,本发明所述的系统能够自适应的调整参数β,以满足姿态估计的需求,同时解决了人为调整参数耗时耗力的问题,可以很好地应用于各种飞行载体的姿态估计。
本公开还涉及一种电子设备,包括服务器、终端等。该电子设备包括:至少一个处理器;与至少一个处理器通信连接的存储器;以及与存储介质通信连接的通信组件,所述通信组件在处理器的控制下接收和发送数据;其中,存储器存储有可被至少一个处理器执行的指令,指令被至少一个处理器执行以实现上述实施例中的方法。
在一种可选的实施方式中,存储器作为一种非易失性计算机可读存储介质,可用于存储非易失性软件程序、非易失性计算机可执行程序以及模块。处理器通过运行存储在存储器中的非易失性软件程序、指令以及模块,从而执行设备的各种功能应用以及数据处理,即实现方法。
存储器可以包括存储程序区和存储数据区,其中,存储程序区可存储操作系统、至少一个功能所需要的应用程序;存储数据区可存储选项列表等。此外,存储器可以包括高速随机存取存储器,还可以包括非易失性存储器,例如至少一个磁盘存储器件、闪存器件、或其他非易失性固态存储器件。在一些实施例中,存储器可选包括相对于处理器远程设置的存储器,这些远程存储器可以通过网络连接至外接设备。上述网络的实例包括但不限于互联网、企业内部网、局域网、移动通信网及其组合。
一个或者多个模块存储在存储器中,当被一个或者多个处理器执行时,执行上述任意方法实施例中的方法。
上述产品可执行本申请实施例所提供的方法,具备执行方法相应的功能模块和有益效果,未在本实施例中详尽描述的技术细节,可参见本申请实施例所提供的方法。
本公开还涉及一种计算机可读存储介质,用于存储计算机可读程序,所述计算机可读程序用于供计算机执行上述部分或全部的方法实施例。
即,本领域技术人员可以理解,实现上述实施例方法中的全部或部分步骤是可以通过程序来指令相关的硬件来完成,该程序存储在一个存储介质中,包括若干指令用以使得一个设备(可以是单片机,芯片等)或处理器(processor)执行本申请各实施例所述方法的全部或部分步骤。而前述的存储介质包括:U盘、移动硬盘、只读存储器(ROM,Read-OnlyMemory)、随机存取存储器(RAM,Random Access Memory)、磁碟或者光盘等各种可以存储程序代码的介质。
在此处所提供的说明书中,说明了大量具体细节。然而,能够理解,本发明的实施例可以在没有这些具体细节的情况下实践。在一些实例中,并未详细示出公知的方法、结构和技术,以便不模糊对本说明书的理解。
此外,本领域普通技术人员能够理解,尽管在此所述的一些实施例包括其它实施例中所包括的某些特征而不是其它特征,但是不同实施例的特征的组合意味着处于本发明的范围之内并且形成不同的实施例。例如,在权利要求书中,所要求保护的实施例的任意之一都可以以任意的组合方式来使用。
本领域技术人员应理解,尽管已经参考示例性实施例描述了本发明,但是在不脱离本发明的范围的情况下,可进行各种改变并可用等同物替换其元件。另外,在不脱离本发明的实质范围的情况下,可进行许多修改以使特定情况或材料适应本发明的教导。因此,本发明不限于所公开的特定实施例,而是本发明将包括落入所附权利要求范围内的所有实施例。
Claims (10)
6.如权利要求5所述的方法,其中,所述角速度测量值用四元数bωt=[0 ωx ωy ωz]表示,其中,ωx、ωy、ωz分别表示在无人机坐标系中x轴、y轴、z轴的角速度测量值分量,
其中,基于陀螺仪在t时刻的角度漂移值bωd,t对陀螺仪在t时刻的角速度测量值bωt进行补偿,得到补偿后的角速度值bωc,t,基于所述补偿后的角速度值bωc,t和互补滤波器在t-Δt时刻融合得到的无人机姿态估计值得到所述陀螺仪在t时刻的姿态估计值包括:
基于所述陀螺仪在t时刻的角度漂移值bωd,t对所述角速度测量值bωt进行补偿,确定补偿后的角速度值bωc,t,bωc,t=bωt-bωd,t;
9.一种电子设备,包括存储器和处理器,其特征在于,所述存储器用于存储一条或多条计算机指令,其中,所述一条或多条计算机指令被处理器执行以实现如权利要求1-7中任一项所述的方法。
10.一种计算机可读存储介质,其上存储有计算机程序,其特征在于,所述计算机程序被处理器执行以实现如权利要求1-7中任一项所述的方法。
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202010998123.4A CN112179342B (zh) | 2020-09-21 | 2020-09-21 | 一种估计无人机姿态的方法及系统 |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202010998123.4A CN112179342B (zh) | 2020-09-21 | 2020-09-21 | 一种估计无人机姿态的方法及系统 |
Publications (2)
Publication Number | Publication Date |
---|---|
CN112179342A true CN112179342A (zh) | 2021-01-05 |
CN112179342B CN112179342B (zh) | 2021-08-27 |
Family
ID=73956407
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN202010998123.4A Active CN112179342B (zh) | 2020-09-21 | 2020-09-21 | 一种估计无人机姿态的方法及系统 |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN112179342B (zh) |
Cited By (3)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN113282005A (zh) * | 2021-05-26 | 2021-08-20 | 惠州中国科学院遥感与数字地球研究所空间信息技术研究院 | 无刷云台调控方法、装置、计算机设备和存储介质 |
CN113672137A (zh) * | 2021-08-17 | 2021-11-19 | 北京字节跳动网络技术有限公司 | 光标位置更新方法、装置和电子设备 |
CN114608516A (zh) * | 2022-01-28 | 2022-06-10 | 北京航天发射技术研究所 | 一种小型化雷达动态测姿设备 |
Citations (6)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
US6212476B1 (en) * | 1998-05-14 | 2001-04-03 | Baker Hughes Incorporated | Apparatus to measure the earth's local gravity and magnetic field in conjunction with global positioning attitude determining |
CN104914874A (zh) * | 2015-06-09 | 2015-09-16 | 长安大学 | 一种基于自适应互补融合的无人机姿态控制系统及方法 |
CN108398128A (zh) * | 2018-01-22 | 2018-08-14 | 北京大学深圳研究生院 | 一种姿态角的融合解算方法和装置 |
CN109682377A (zh) * | 2019-03-08 | 2019-04-26 | 兰州交通大学 | 一种基于动态步长梯度下降的姿态估计方法 |
CN109883451A (zh) * | 2019-04-15 | 2019-06-14 | 山东建筑大学 | 一种用于行人方位估计的自适应增益互补滤波方法及系统 |
CN110319840A (zh) * | 2019-07-05 | 2019-10-11 | 东北大学秦皇岛分校 | 面向异常步态识别的共轭梯度姿态解算方法 |
-
2020
- 2020-09-21 CN CN202010998123.4A patent/CN112179342B/zh active Active
Patent Citations (6)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
US6212476B1 (en) * | 1998-05-14 | 2001-04-03 | Baker Hughes Incorporated | Apparatus to measure the earth's local gravity and magnetic field in conjunction with global positioning attitude determining |
CN104914874A (zh) * | 2015-06-09 | 2015-09-16 | 长安大学 | 一种基于自适应互补融合的无人机姿态控制系统及方法 |
CN108398128A (zh) * | 2018-01-22 | 2018-08-14 | 北京大学深圳研究生院 | 一种姿态角的融合解算方法和装置 |
CN109682377A (zh) * | 2019-03-08 | 2019-04-26 | 兰州交通大学 | 一种基于动态步长梯度下降的姿态估计方法 |
CN109883451A (zh) * | 2019-04-15 | 2019-06-14 | 山东建筑大学 | 一种用于行人方位估计的自适应增益互补滤波方法及系统 |
CN110319840A (zh) * | 2019-07-05 | 2019-10-11 | 东北大学秦皇岛分校 | 面向异常步态识别的共轭梯度姿态解算方法 |
Cited By (3)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN113282005A (zh) * | 2021-05-26 | 2021-08-20 | 惠州中国科学院遥感与数字地球研究所空间信息技术研究院 | 无刷云台调控方法、装置、计算机设备和存储介质 |
CN113672137A (zh) * | 2021-08-17 | 2021-11-19 | 北京字节跳动网络技术有限公司 | 光标位置更新方法、装置和电子设备 |
CN114608516A (zh) * | 2022-01-28 | 2022-06-10 | 北京航天发射技术研究所 | 一种小型化雷达动态测姿设备 |
Also Published As
Publication number | Publication date |
---|---|
CN112179342B (zh) | 2021-08-27 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
CN112179342B (zh) | 一种估计无人机姿态的方法及系统 | |
US20170123035A1 (en) | Autonomous magnetometer calibration | |
CN102692225B (zh) | 一种用于低成本小型无人机的姿态航向参考系统 | |
US20150177020A1 (en) | Method and apparatus for data fusion of a three-axis magnetometer and three axis accelerometer | |
CN109000612A (zh) | 设备的角度估算方法、装置、摄像组件及飞行器 | |
CN110081878B (zh) | 一种多旋翼无人机的姿态及位置确定方法 | |
CN110377056B (zh) | 无人机航向角初值选取方法及无人机 | |
CN109579836B (zh) | 一种基于mems惯性导航的室内行人方位校准方法 | |
CN106403952A (zh) | 一种动中通低成本组合姿态测量方法 | |
EP2674781B1 (en) | Latency compensation | |
CN102654404A (zh) | 一种提高航姿参考系统解算精度和系统抗干扰能力的方法 | |
CN110887480A (zh) | 基于mems传感器的飞行姿态估计方法及系统 | |
CN110887481A (zh) | 基于mems惯性传感器的载体动态姿态估计方法 | |
CN106370178B (zh) | 移动终端设备的姿态测量方法及装置 | |
KR101211703B1 (ko) | 시선벡터를 이용한 자장계 오차 보정방법 및 이를 이용한 통합 항법 시스템 | |
CN112066985A (zh) | 一种组合导航系统初始化方法、装置、介质及电子设备 | |
CN110793515A (zh) | 一种基于单天线gps和imu的大机动条件下无人机姿态估计方法 | |
CN103712598A (zh) | 一种小型无人机姿态确定系统与确定方法 | |
Wang et al. | Nonlinear multiple integrator and application to aircraft navigation | |
CN112070170A (zh) | 一种动态残差阈值自适应四元数粒子滤波姿态解算数据融合方法 | |
Ascorti | An application of the extended Kalman filter to the attitude control of a quadrotor | |
CN113175926B (zh) | 一种基于运动状态监测的自适应水平姿态测量方法 | |
CN110375773B (zh) | Mems惯导系统姿态初始化方法 | |
US20200293067A1 (en) | State estimation | |
CN110849364B (zh) | 基于动中通的自适应卡尔曼姿态估计方法 |
Legal Events
Date | Code | Title | Description |
---|---|---|---|
PB01 | Publication | ||
PB01 | Publication | ||
SE01 | Entry into force of request for substantive examination | ||
SE01 | Entry into force of request for substantive examination | ||
GR01 | Patent grant | ||
GR01 | Patent grant |