CN111982100A - 一种无人机的航向角解算算法 - Google Patents

一种无人机的航向角解算算法 Download PDF

Info

Publication number
CN111982100A
CN111982100A CN202010645235.1A CN202010645235A CN111982100A CN 111982100 A CN111982100 A CN 111982100A CN 202010645235 A CN202010645235 A CN 202010645235A CN 111982100 A CN111982100 A CN 111982100A
Authority
CN
China
Prior art keywords
unmanned aerial
aerial vehicle
angle
course angle
gyroscope
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
Application number
CN202010645235.1A
Other languages
English (en)
Other versions
CN111982100B (zh
Inventor
不公告发明人
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Guangdong University of Technology
Original Assignee
Guangdong University of Technology
Priority date (The priority date 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 date listed.)
Filing date
Publication date
Application filed by Guangdong University of Technology filed Critical Guangdong University of Technology
Priority to CN202010645235.1A priority Critical patent/CN111982100B/zh
Publication of CN111982100A publication Critical patent/CN111982100A/zh
Application granted granted Critical
Publication of CN111982100B publication Critical patent/CN111982100B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

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
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C25/00—Manufacturing, calibrating, cleaning, or repairing instruments or devices referred to in the other groups of this subclass
    • G01C25/005—Manufacturing, calibrating, cleaning, or repairing instruments or devices referred to in the other groups of this subclass initial alignment, calibration or starting-up of inertial devices
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01P—MEASURING LINEAR OR ANGULAR SPEED, ACCELERATION, DECELERATION, OR SHOCK; INDICATING PRESENCE, ABSENCE, OR DIRECTION, OF MOVEMENT
    • G01P13/00—Indicating or recording presence, absence, or direction, of movement
    • G01P13/02—Indicating direction only, e.g. by weather vane
    • G01P13/025—Indicating direction only, e.g. by weather vane indicating air data, i.e. flight variables of an aircraft, e.g. angle of attack, side slip, shear, yaw

Abstract

本发明公开了一种无人机的航向角解算算法，包括：采用无人机自身携带的加速度计和陀螺仪传感器，通过数据融合解算出无人机的姿态角度‑俯仰角和滚转角；建立无人机动力学模型，通过自适应算法估算出动力学模型中的参数；结合动力学模型方程所解算出来的航向角和惯性导航传感器融合解算出来的航向角，采用添加权重的形式估计当前的航向角，由此实现不受磁场干扰的航向角解算。本方法能解算出较为精确的无人机航向角，解决现有磁力计传感器测量容易受到强磁场干扰而出现航向角解算出现较大偏差的问题，为无人机飞行控制器提供更为准确的航向角信息；不需要额外添加其他传感器，仅用无人机自带现有传感器即可解决解算航向角受强磁场干扰问题。

Description

一种无人机的航向角解算算法
技术领域
本发明涉及无人机技术领域，具体涉及一种无人机的航向角解算算法。
背景技术
四旋翼飞行器是一种能实现垂直起降的小型飞行器，因其体型小巧，灵活性高，成本低，易维护等特点受到关注，它能在各种受限环境中工作，无论是丛林，地底或者是室内环境等场景。在农业方面，广泛地应用于农药的喷洒，农作物生长情况的监管。快递的派送、消防安防、电网的安全巡检、国土资源的勘测以及无人机的探测与无线电反制等领域中。但是由于无人机系统大多是运用于多干扰，复杂环境下完成一些复杂任务的智能控制系统，在遇到一些强磁干扰的情况之前，传统的基于磁力传感器解算出来的无人机偏航角将不再是可信的，然而在无人机缺失航向角的引导将缺失方向感；不准确的航向角将导致的无人机可能出现马桶式螺旋发散。
目前已公开文献中并没有针对无人机中磁力计传感器容易受干扰的缺陷给出缺失磁力计传感器后如何解算出航向角。
发明内容
本发明的目的是提供一种无人机的航向角结算算法，用以克服现有技术中无人机磁力计传感器容易受干扰而导致解算出的航向角不准确的问题。
为了实现上述任务，本发明采用以下技术方案：
一种无人机的航向角解算算法，包括以下步骤：
步骤1，采用无人机自身携带的加速度计和陀螺仪传感器，通过数据融合解算出无人机的姿态角度-俯仰角和滚转角；
步骤2，建立无人机动力学模型，通过自适应算法估算出动力学模型中的参数，具体步骤包括：
步骤2.1，建立无人机动力学模型，表示如下：
Figure BDA0002572880020000021
上式中，
Figure BDA0002572880020000022
表示无人机的位置信息，x,y,z为不同方向的位置信息；kx，ky，kz为待估计的阻力参数，m为无人机质量，g为重力加速度，u1为无人机电机的总拉力，参数上标一个圆点、两个圆点表示该参数的一阶导数、二阶导数；
步骤2.2，令
Figure BDA0002572880020000023
为速度，
Figure BDA0002572880020000024
为加速度，
Figure BDA0002572880020000025
则有：
Figure BDA0002572880020000026
令速度误差：
Figure BDA00025728800200000220
其中vx，d为期望的速度值，对速度误差求导得：
Figure BDA0002572880020000027
步骤2.3，设计控制输入：
Figure BDA0002572880020000028
其中
Figure BDA0002572880020000029
Figure BDA00025728800200000210
为自适应估计值，
Figure BDA00025728800200000211
为估计误差值；Kx为控制器输入的待调整参数；
步骤2.4，根据所设计的控制输入，设计待估计的阻力参数kx的自适应参数估计律为
Figure BDA00025728800200000212
然后根据自适应律迭代或积分的方式获得自适应估计值
Figure BDA00025728800200000213
根据步骤2.1中的无人机动力模型，按照步骤2.2～2.4同样的方法，令
Figure BDA00025728800200000214
为速度，
Figure BDA00025728800200000215
表示对变量*的一阶求导，
Figure BDA00025728800200000216
为速度误差，其中*分别表示为位置y，z的信息，从而得到对应的自适应估计律
Figure BDA00025728800200000217
最后得到待估计的阻力参数ky，kz的估计值
Figure BDA00025728800200000218
和
Figure BDA00025728800200000219
步骤3，结合动力学模型方程所解算出来的航向角和惯性导航传感器融合解算出来的航向角，采用添加权重的形式估计当前的偏航角，由此实现不受磁场干扰的航向角解算，具体步骤包括：
步骤3.1，将步骤2中建立的无人机动力学模型改写为：
Figure BDA0002572880020000031
其中u1x，u1y，u1z分别对应无人机坐标系x，y，z轴的虚拟控制输入，导航坐标系下的速度
Figure BDA0002572880020000032
和加速度
Figure BDA0002572880020000033
可以从无人机的定位系统或者是光流传感器中获取，因此参数
Figure BDA0002572880020000034
已知，由此可以根据上式计算出u1x，u1y，u1z虚拟控制输入量；
步骤3.2，由步骤3.1中的模型可得：
Figure BDA0002572880020000035
即：
Figure BDA0002572880020000036
步骤3.3，根据步骤3.1中的模型第三行公式可得：
Figure BDA0002572880020000037
步骤3.4，结合步骤3.2和步骤3.3可得航向角度：
Figure BDA0002572880020000038
步骤3.5，根据解算出的陀螺仪测量数据的漂移量bg，k，可以获得一个经过矫正后的陀螺仪测量数据，即：ωk＝ωm，k-bg，k，其中ωm，k为k时刻陀螺仪的测量数据值；
步骤3.6：根据
Figure BDA0002572880020000041
求得
Figure BDA0002572880020000042
其中
Figure BDA0002572880020000043
所以能获得当前的航向角速度
Figure BDA0002572880020000044
则航向角
Figure BDA0002572880020000045
其中ψk，ψk-1分别为当前和前一时刻的航向角度；
步骤3.7，修正航向角度：
ψcorr＝ψ*w1+ψk*w2
其中w1，w2分别为步骤3.4解算出的航向角的权重和步骤3.5的矫正陀螺仪测量数据后的航向角度的权重。
进一步地，所述采用无人机自身携带的加速度计和陀螺仪传感器，通过数据融合解算出无人机的姿态角度-俯仰角θ和滚转角
Figure BDA0002572880020000046
包括：
步骤1.1，根据无人机的陀螺仪数据和加速度计数据通过扩展卡尔曼进行数据融合，令姿态角度为
Figure BDA0002572880020000047
其中φ为无人机姿态的滚转角，θ为俯仰角，ψ为偏航角，则扩展卡尔曼方程的状态方程如下：
Figure BDA0002572880020000048
式中：系统的状态变量
Figure BDA0002572880020000049
其中
Figure BDA00025728800200000410
Figure BDA00025728800200000411
表示估计值，Φk-1为k-1时刻的状态转移矩阵，Γk-1为k-1时刻过程噪声增益矩阵，Wk-1为k-1时刻系统过程噪声，上式子的具体形式如下：
Figure BDA00025728800200000412
其中：bg，k为第k时刻陀螺仪的漂移误差，I3×3为单位矩阵；TS为无人机系统的采样时间；ωm，k-1为k-1时刻陀螺仪的测量数据值；
Figure BDA00025728800200000413
为第k-1时刻的陀螺仪的偏移量噪声，wg，k-1为第k-1时刻的陀螺仪的测量噪声；
步骤1.2，扩张卡尔曼观测模型：
Figure BDA00025728800200000414
其中：
Figure BDA0002572880020000051
表示加速度，amx，k、amy，k、amz，k分别为x、y、z方向的加速度，g表示重力加速度；
Figure BDA0002572880020000052
Figure BDA0002572880020000053
表示实数矩阵，
Figure BDA0002572880020000054
为加速度计测量噪声；
步骤1.3，利用扩展卡尔曼算法对所述状态方程进行迭代更新，可以得到系统状态量Xk的最优估计值，因此
Figure BDA0002572880020000055
则有：
Figure BDA0002572880020000056
即可以求得俯仰角θ和滚转角
Figure BDA0002572880020000057
一种终端设备，包括存储器、处理器以及存储在所述存储器中并可在所述处理器上运行的计算机程序，处理器执行计算机程序时实现无人机的航向角解算算法的步骤。
一种计算机可读存储介质，计算机可读存储介质存储有计算机程序，计算机程序被处理器执行时实现无人机的航向角解算算法的步骤。
与现有技术相比，本发明具有以下技术特点：
本发明提出的算法结合加速度计，陀螺仪和光流数据进行融合，解算出较为精确的无人机航向角，解决现有磁力计传感器测量容易受到强磁场干扰而出现航向角解算出现较大偏差的问题，为无人机飞行控制器提供更为准确的航向角信息。本发明不需要额外添加其他传感器，仅用无人机自带现有传感器即可解决解算航向角受强磁场干扰问题。
附图说明
图1为本发明方法的流程示意图。
具体实施方式
本发明提出了一种无人机的航向角解算算法，该算法结合加速度计，陀螺仪和光流数据进行融合，解算出较为精确的无人机航向角，解决现有磁力计传感器测量容易受到强磁场干扰而出现航向角解算出现较大偏差的问题，为无人机飞行控制器提供更为准确的航向角信息。本发明不需要额外添加其他传感器，仅用无人机自带现有传感器即可解决解算航向角受强磁场干扰问题。
参见图1，本发明的一种无人机的航向角解算算法，包括以下步骤：
前期准备工作：初始化校准测量传感器的数据，分别读取加速度计数据、陀螺仪数据，位置加速度数据和位置速度数据。
步骤1，采用无人机自身携带的加速度计和陀螺仪传感器，通过数据融合解算出无人机的姿态角度-俯仰角θ和滚转角
Figure BDA0002572880020000061
具体步骤如下：
步骤1.1，根据无人机的陀螺仪数据和加速度计数据通过扩展卡尔曼进行数据融合，令姿态角度为
Figure BDA0002572880020000062
其中φ为无人机姿态的滚转角，θ为俯仰角，ψ为偏航角，则扩展卡尔曼方程的状态方程如下：
Figure BDA0002572880020000063
式中：系统的状态变量
Figure BDA0002572880020000064
其中
Figure BDA0002572880020000065
Figure BDA0002572880020000066
表示估计值，Φk-1为k-1时刻的状态转移矩阵，Γk-1为k-1时刻过程噪声增益矩阵，Wk-1为k-1时刻系统过程噪声，上式子的具体形式如下：
Figure BDA0002572880020000067
其中：bg，k为第k时刻陀螺仪的漂移误差，I3×3为单位矩阵；TS为无人机系统的采样时间；ωm，k-1为k-1时刻陀螺仪的测量数据值；
Figure BDA0002572880020000068
为第k-1时刻的陀螺仪的偏移量噪声，是一个3×1的列向量；wg，k-1为第k-1时刻的陀螺仪的测量噪声，是一个3×1的列向量。
步骤1.2，扩张卡尔曼观测模型：
Figure BDA0002572880020000069
其中：
Figure BDA00025728800200000610
表示加速度，amx，k、amy，k、amz，k分别为x、y、z方向的加速度，g表示重力加速度；
Figure BDA00025728800200000611
Figure BDA00025728800200000612
表示实数矩阵，
Figure BDA00025728800200000613
为加速度计测量噪声。
步骤1.3，利用扩展卡尔曼算法对所述状态方程进行迭代更新，可以得到系统状态量Xk的最优估计值，因此
Figure BDA0002572880020000071
则有：
Figure BDA0002572880020000072
即可以求得俯仰角θ和滚转角
Figure BDA0002572880020000073
步骤2，建立无人机动力学模型，通过自适应算法估算出动力学模型中的参数，具体步骤包括：
步骤2.1，建立无人机动力学模型，表示如下：
Figure BDA0002572880020000074
上式中，
Figure BDA0002572880020000075
表示无人机的位置信息，x，y，z为不同方向的位置信息；kx，ky，kz为待估计的阻力参数，m为无人机质量，g为重力加速度，u1为无人机电机的拉力，上式中参数上标一个圆点、两个圆点表示该参数的一阶导数、二阶导数，下同。
步骤2.2，取其中
Figure BDA0002572880020000076
为例，令
Figure BDA0002572880020000077
为速度，
Figure BDA0002572880020000078
为加速度，
Figure BDA0002572880020000079
则有：
Figure BDA00025728800200000710
令速度误差：
Figure BDA00025728800200000716
其中vx，d为期望的速度值，对速度误差求导得：
Figure BDA00025728800200000711
步骤2.3，设计控制输入：
Figure BDA00025728800200000712
其中
Figure BDA00025728800200000713
Figure BDA00025728800200000714
为自适应估计值，
Figure BDA00025728800200000715
为估计误差值；Kx为待调整参数。
步骤2.4，根据所设计的控制输入，设计待估计的阻力参数kx的自适应参数估计律为
Figure BDA0002572880020000081
然后根据自适应律迭代或积分的方式获得自适应估计值
Figure BDA0002572880020000082
本方案中，所述的自适应估计值
Figure BDA0002572880020000083
最后趋近真实值kx，证明如下：
根据李雅普诺夫方程：
Figure BDA0002572880020000084
求导这个方程得：
Figure BDA0002572880020000085
因此
Figure BDA0002572880020000086
误差最终收敛到0点。
根据步骤2.1中的无人机动力模型，按照步骤2.2～2.4同样的方法，令
Figure BDA0002572880020000087
为速度，
Figure BDA0002572880020000088
表示对变量*的一阶求导，
Figure BDA0002572880020000089
为速度误差，其中*分别表示为位置y，z的信息，从而得到对应的自适应估计律
Figure BDA00025728800200000810
最后得到待估计的阻力参数ky，kz的估计值
Figure BDA00025728800200000811
和
Figure BDA00025728800200000812
步骤3，结合动力学模型方程所解算出来的姿态角度和IMU(惯性导航传感器)融合解算出来的姿态角度，采用添加权重的形式估计当前的偏航角，由此实现不受磁场干扰的航向角解算，具体步骤包括：
步骤3.1，将步骤2中建立的无人机动力学模型改写为：
Figure BDA00025728800200000813
其中u1x，u1yu1z分别对应x，y，z轴的虚拟控制输入，导航坐标系下的速度
Figure BDA00025728800200000814
和加速度
Figure BDA00025728800200000815
可以从无人机的定位系统或者是光流传感器中获取，因此参数
Figure BDA00025728800200000816
已知，由此可以根据上式计算出u1x，u1y u1z虚拟控制输入量。
步骤3.2，由步骤3.1中的模型可得：
Figure BDA00025728800200000817
即：
Figure BDA0002572880020000091
步骤3.3，根据步骤3.1中的模型第三行公式可得：
Figure BDA0002572880020000092
步骤3.4，结合步骤3.2和步骤3.3可得：
Figure BDA0002572880020000093
步骤3.5，根据步骤1.3通过扩展卡尔曼解算出较为准确的陀螺仪测量数据的漂移量bg，k，可以获得一个经过矫正后的陀螺仪测量数据，即：ωk＝ωm，k-bg，k，其中ωm，k为k时刻陀螺仪的测量数据值。
步骤3.6：根据
Figure BDA0002572880020000094
求得
Figure BDA0002572880020000095
其中
Figure BDA0002572880020000096
所以能获得当前的航向角速度
Figure BDA0002572880020000097
再根据
Figure BDA0002572880020000098
其中ψk，ψk-1分别为当前和前一时刻的航向角度。
步骤3.7，修正航向角度：
ψcorr＝ψ*w1+ψk*w2
其中ψ为步骤3.4解算出的角度，w1，w2分别为步骤3.4解算出的航向角的权重和步骤3.5的矫正陀螺仪测量数据后的航向角度的权重，需通过定位系统的定位精度更加具体地调整w1，w2的取值。
以上实施例仅用以说明本申请的技术方案，而非对其限制；尽管参照前述实施例对本申请进行了详细的说明，本领域的普通技术人员应当理解：其依然可以对前述各实施例所记载的技术方案进行修改，或者对其中部分技术特征进行等同替换；而这些修改或者替换，并不使相应技术方案的本质脱离本申请各实施例技术方案的精神和范围，均应包含在本申请的保护范围之内。

Claims (4)

1.一种无人机的航向角解算算法，其特征在于，包括以下步骤：
步骤1，采用无人机自身携带的加速度计和陀螺仪传感器，通过数据融合解算出无人机的姿态角度-俯仰角和滚转角；
步骤2，建立无人机动力学模型，通过自适应算法估算出动力学模型中的参数，具体步骤包括：
步骤2.1，建立无人机动力学模型，表示如下：
Figure FDA0002572880010000011
上式中，
Figure FDA0002572880010000012
表示无人机的位置信息，x,y,z为不同方向的位置信息；
kx，ky，kz为待估计的阻力参数，m为无人机质量，g为重力加速度，u1为无人机电机的拉力，参数上标一个圆点、两个圆点表示该参数的一阶导数、二阶导数；
步骤2.2，令
Figure FDA0002572880010000013
为速度，
Figure FDA0002572880010000014
为加速度，
Figure FDA0002572880010000015
则有：
Figure FDA0002572880010000016
令速度误差：
Figure FDA00025728800100000114
其中vx，d为期望的速度值，对速度误差求导得：
Figure FDA0002572880010000017
步骤2.3，设计控制输入：
Figure FDA0002572880010000018
其中
Figure FDA0002572880010000019
Figure FDA00025728800100000110
为自适应估计值，
Figure FDA00025728800100000111
为估计误差值；Kx为待调整参数；
步骤2.4，根据所设计的控制输入，设计待估计的阻力参数kx的自适应参数估计律为
Figure FDA00025728800100000112
然后根据自适应律迭代或积分的方式获得自适应估计值
Figure FDA00025728800100000113
根据步骤2.1中的无人机动力模型，按照步骤2.2～2.4同样的方法，令
Figure FDA0002572880010000021
为速度，
Figure FDA0002572880010000022
表示对变量*的一阶求导，
Figure FDA0002572880010000023
为速度误差，其中*分别表示为位置y，z的信息，从而得到对应的自适应估计律
Figure FDA0002572880010000024
最后得到待估计的阻力参数ky，kz的估计值
Figure FDA0002572880010000025
和
Figure FDA0002572880010000026
步骤3，结合动力学模型方程所解算出来的航向角和惯性导航传感器融合解算出来的航向角，采用添加权重的形式估计当前的偏航角，由此实现不受磁场干扰的航向角解算，具体步骤包括：
步骤3.1，将步骤2中建立的无人机动力学模型改写为：
Figure FDA0002572880010000027
其中u1x，u1y，u1z分别对应无人机坐标系x，y，z轴的虚拟控制输入，导航坐标系下的速度
Figure FDA0002572880010000028
和加速度
Figure FDA0002572880010000029
可以从无人机的定位系统或者是光流传感器中获取，因此参数
Figure FDA00025728800100000210
已知，由此可以根据上式计算出u1x，u1y，u1z虚拟控制输入量；
步骤3.2，由步骤3.1中的模型可得：
Figure FDA00025728800100000211
即：
Figure FDA00025728800100000212
步骤3.3，根据步骤3.1中的模型第三行公式可得：
Figure FDA00025728800100000213
步骤3.4，结合步骤3.2和步骤3.3可得航向角度：
Figure FDA00025728800100000214
步骤3.5，根据解算出的陀螺仪测量数据的漂移量bg，k，可以获得一个经过矫正后的陀螺仪测量数据，即：ωk＝ωm，k-bg，k，其中ωm，k为k时刻陀螺仪的测量数据值；
步骤3.6：根据
Figure FDA0002572880010000031
求得
Figure FDA0002572880010000032
其中
Figure FDA0002572880010000033
所以能获得当前的航向角速度
Figure FDA0002572880010000034
则航向角
Figure FDA0002572880010000035
其中ψk，ψk-1分别为当前和前一时刻的航向角度；
步骤3.7，修正航向角度：
ψcorr＝ψ*w1+ψk*w2
其中w1，w2分别为步骤3.4解算出的航向角的权重和步骤3.5的矫正陀螺仪测量数据后的航向角度的权重。
2.根据权利要求1所述的无人机的航向角解算算法，其特征在于，所述采用无人机自身携带的加速度计和陀螺仪传感器，通过数据融合解算出无人机的姿态角度-俯仰角θ和滚转角
Figure FDA0002572880010000036
包括：
步骤1.1，根据无人机的陀螺仪数据和加速度计数据通过扩展卡尔曼进行数据融合，令姿态角度为
Figure FDA0002572880010000037
其中φ为无人机姿态的滚转角，θ为俯仰角，ψ为偏航角，则扩展卡尔曼方程的状态方程如下：
Figure FDA0002572880010000038
式中：系统的状态变量
Figure FDA0002572880010000039
其中
Figure FDA00025728800100000310
Figure FDA00025728800100000311
表示估计值，Φk-1为k-1时刻的状态转移矩阵，Γk-1为k-1时刻过程噪声增益矩阵，Wk-1为k-1时刻系统过程噪声，上式子的具体形式如下：
Figure FDA00025728800100000312
其中：bg，k为第k时刻陀螺仪的漂移误差，I3×3为单位矩阵；TS为无人机系统的采样时间；ωm，k-1为k-1时刻陀螺仪的测量数据值；
Figure FDA0002572880010000041
为第k-1时刻的陀螺仪的偏移量噪声，Wg，k-1为第k-1时刻的陀螺仪的测量噪声，；
步骤1.2，扩张卡尔曼观测模型：
Figure FDA0002572880010000042
其中：
Figure FDA0002572880010000043
表示加速度，amx，k、amy，k、amz，k分别为x、y、z方向的加速度，g表示重力加速度；
Figure FDA0002572880010000044
Figure FDA0002572880010000045
表示实数矩阵，
Figure FDA0002572880010000046
为加速度计测量噪声；
步骤1.3，利用扩展卡尔曼算法对所述状态方程进行迭代更新，可以得到系统状态量Xk的最优估计值，因此
Figure FDA0002572880010000047
则有：
Figure FDA0002572880010000048
即可以求得俯仰角θ和滚转角
Figure FDA0002572880010000049
3.一种终端设备，包括存储器、处理器以及存储在所述存储器中并可在所述处理器上运行的计算机程序，其特征在于，处理器执行计算机程序时实现根据权利要求1或2中任一权利要求所述算法的步骤。
4.一种计算机可读存储介质，计算机可读存储介质存储有计算机程序，计算机程序被处理器执行时实现根据权利要求1或2中任一权利要求所述算法的步骤。
CN202010645235.1A 2020-07-07 2020-07-07 一种无人机的航向角解算算法 Active CN111982100B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202010645235.1A CN111982100B (zh) 2020-07-07 2020-07-07 一种无人机的航向角解算算法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202010645235.1A CN111982100B (zh) 2020-07-07 2020-07-07 一种无人机的航向角解算算法

Publications (2)

Publication Number Publication Date
CN111982100A true CN111982100A (zh) 2020-11-24
CN111982100B CN111982100B (zh) 2022-05-06

Family

ID=73438536

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202010645235.1A Active CN111982100B (zh) 2020-07-07 2020-07-07 一种无人机的航向角解算算法

Country Status (1)

Country Link
CN (1) CN111982100B (zh)

Cited By (11)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN112230192A (zh) * 2020-12-21 2021-01-15 东方微电科技(武汉)有限公司 基于磁传感与定位系统的雷达航向角测量方法及装置
CN112629529A (zh) * 2020-12-15 2021-04-09 西安工业大学 无人机室内自主导航方法
CN113049004A (zh) * 2021-02-28 2021-06-29 哈尔滨工业大学 一种航磁补偿校准质量的自动评估方法及装置
CN113325865A (zh) * 2021-05-10 2021-08-31 哈尔滨理工大学 一种无人机控制方法、控制装置及控制系统
CN114323008A (zh) * 2021-12-31 2022-04-12 杭州电子科技大学 基于机器学习分类的融合航向角估计方法及系统
CN114778887A (zh) * 2022-05-09 2022-07-22 中国人民解放军93213部队 一种基于改进三角矢量模型的无人机测风方法和装置
CN114877858A (zh) * 2022-05-06 2022-08-09 西安电子科技大学 一种高动态和磁干扰环境下的姿态估计算法
CN116193581A (zh) * 2023-05-04 2023-05-30 广东工业大学 一种基于集员滤波的室内无人机混合定位方法及系统
CN117053803A (zh) * 2023-10-10 2023-11-14 腾讯科技（深圳）有限公司 航向信息处理方法、装置、设备及计算机可读存储介质
CN117387630A (zh) * 2023-12-12 2024-01-12 江苏君立华域信息安全技术股份有限公司 一种强磁干扰环境下自动方向校正方法及系统
CN114778887B (zh) * 2022-05-09 2024-05-14 中国人民解放军93213部队 一种基于改进三角矢量模型的无人机测风方法和装置

Citations (9)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
RU145937U1 (ru) * 2014-03-14 2014-09-27 Федеральное Государственное Бюджетное Образовательное Учреждение Высшего Профессионального Образования "Саратовский Государственный Университет Имени Н.Г. Чернышевского" Устройство автономной коррекции курсового угла летательного аппарата
CN107479567A (zh) * 2017-09-13 2017-12-15 山东大学 动态特性未知的四旋翼无人机姿态控制器及方法
CN108427414A (zh) * 2018-03-31 2018-08-21 西北工业大学 一种自主水下航行器水平面自适应轨迹跟踪控制方法
CN108681331A (zh) * 2018-05-21 2018-10-19 济南大学 一种近空间飞行器的姿态跟踪控制方法
CN109343369A (zh) * 2018-11-19 2019-02-15 南京邮电大学 一种基于非线性观测器的四旋翼容错控制器设计方法
CN109976367A (zh) * 2019-03-27 2019-07-05 天津大学 针对倾转式三旋翼无人机的姿态系统控制方法
CN110502027A (zh) * 2019-09-16 2019-11-26 南京邮电大学 一种基于自适应终端滑模的四旋翼无人机姿态容错控制方法
CN111123967A (zh) * 2020-01-02 2020-05-08 南京航空航天大学 一种基于自适应动态逆的固定翼无人机着舰控制方法
RU2725640C1 (ru) * 2019-09-09 2020-07-03 Закрытое акционерное общество "АйТи Десижн" Способ захода на посадку беспилотного летательного аппарата в аварийных условиях

Patent Citations (9)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
RU145937U1 (ru) * 2014-03-14 2014-09-27 Федеральное Государственное Бюджетное Образовательное Учреждение Высшего Профессионального Образования "Саратовский Государственный Университет Имени Н.Г. Чернышевского" Устройство автономной коррекции курсового угла летательного аппарата
CN107479567A (zh) * 2017-09-13 2017-12-15 山东大学 动态特性未知的四旋翼无人机姿态控制器及方法
CN108427414A (zh) * 2018-03-31 2018-08-21 西北工业大学 一种自主水下航行器水平面自适应轨迹跟踪控制方法
CN108681331A (zh) * 2018-05-21 2018-10-19 济南大学 一种近空间飞行器的姿态跟踪控制方法
CN109343369A (zh) * 2018-11-19 2019-02-15 南京邮电大学 一种基于非线性观测器的四旋翼容错控制器设计方法
CN109976367A (zh) * 2019-03-27 2019-07-05 天津大学 针对倾转式三旋翼无人机的姿态系统控制方法
RU2725640C1 (ru) * 2019-09-09 2020-07-03 Закрытое акционерное общество "АйТи Десижн" Способ захода на посадку беспилотного летательного аппарата в аварийных условиях
CN110502027A (zh) * 2019-09-16 2019-11-26 南京邮电大学 一种基于自适应终端滑模的四旋翼无人机姿态容错控制方法
CN111123967A (zh) * 2020-01-02 2020-05-08 南京航空航天大学 一种基于自适应动态逆的固定翼无人机着舰控制方法

Non-Patent Citations (2)

* Cited by examiner, † Cited by third party
Title
姜鑫燃等: "小小小型型型无无无人人人直直直升升升机机机浸浸浸入入入–不不不变变变集集集自自自适适适应应应控控控制", 《控制理论与应用》 *
鲜斌等: "倾转式三旋翼无人飞行器抗扰非线性控制设计", 《控制与决策》 *

Cited By (13)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN112629529A (zh) * 2020-12-15 2021-04-09 西安工业大学 无人机室内自主导航方法
CN112230192A (zh) * 2020-12-21 2021-01-15 东方微电科技(武汉)有限公司 基于磁传感与定位系统的雷达航向角测量方法及装置
CN113049004A (zh) * 2021-02-28 2021-06-29 哈尔滨工业大学 一种航磁补偿校准质量的自动评估方法及装置
CN113325865A (zh) * 2021-05-10 2021-08-31 哈尔滨理工大学 一种无人机控制方法、控制装置及控制系统
CN114323008A (zh) * 2021-12-31 2022-04-12 杭州电子科技大学 基于机器学习分类的融合航向角估计方法及系统
CN114877858A (zh) * 2022-05-06 2022-08-09 西安电子科技大学 一种高动态和磁干扰环境下的姿态估计算法
CN114778887A (zh) * 2022-05-09 2022-07-22 中国人民解放军93213部队 一种基于改进三角矢量模型的无人机测风方法和装置
CN114778887B (zh) * 2022-05-09 2024-05-14 中国人民解放军93213部队 一种基于改进三角矢量模型的无人机测风方法和装置
CN116193581A (zh) * 2023-05-04 2023-05-30 广东工业大学 一种基于集员滤波的室内无人机混合定位方法及系统
CN116193581B (zh) * 2023-05-04 2023-08-04 广东工业大学 一种基于集员滤波的室内无人机混合定位方法及系统
CN117053803A (zh) * 2023-10-10 2023-11-14 腾讯科技（深圳）有限公司 航向信息处理方法、装置、设备及计算机可读存储介质
CN117053803B (zh) * 2023-10-10 2024-01-26 腾讯科技（深圳）有限公司 航向信息处理方法、装置、设备及计算机可读存储介质
CN117387630A (zh) * 2023-12-12 2024-01-12 江苏君立华域信息安全技术股份有限公司 一种强磁干扰环境下自动方向校正方法及系统

Also Published As

Publication number Publication date
CN111982100B (zh) 2022-05-06

Similar Documents

Publication Publication Date Title
CN111982100B (zh) 一种无人机的航向角解算算法
CN106643737B (zh) 风力干扰环境下四旋翼飞行器姿态解算方法
CN107729585B (zh) 一种对无人机的噪声协方差进行估算的方法
Lee et al. Autonomous landing of a VTOL UAV on a moving platform using image-based visual servoing
De Marina et al. Guidance algorithm for smooth trajectory tracking of a fixed wing UAV flying in wind flows
Jun et al. State estimation of an autonomous helicopter using Kalman filtering
Doukhi et al. Neural network-based robust adaptive certainty equivalent controller for quadrotor UAV with unknown disturbances
CN105203098A (zh) 基于九轴mems传感器的农业机械全姿态角更新方法
CN104764467B (zh) 空天飞行器惯性传感器误差在线自适应标定方法
CN109683628B (zh) 一种基于有限时间分布式速度观测器的航天器相对位置控制方法
CN110955261A (zh) 一种基于ros的固定翼无人机自动驾驶仪的仿真方法
US20230366680A1 (en) Initialization method, device, medium and electronic equipment of integrated navigation system
CN110793515A (zh) 一种基于单天线gps和imu的大机动条件下无人机姿态估计方法
CN111189442A (zh) 基于cepf的无人机多源导航信息状态预测方法
Goppert et al. Invariant Kalman filter application to optical flow based visual odometry for UAVs
Oliveira et al. Moving path following for autonomous robotic vehicles
CN115855048A (zh) 一种多传感器融合位姿估计方法
CN115576341A (zh) 基于函数微分和自适应变增益的无人机轨迹跟踪控制方法
Azid et al. Wind gust estimation for precise quasi-hovering control of quadrotor aircraft
CN113670314B (zh) 基于pi自适应两级卡尔曼滤波的无人机姿态估计方法
CN115033844A (zh) 一种无人机状态估计方法、系统、设备及可读存储介质
CN109211232A (zh) 一种基于最小二乘滤波的炮弹姿态估计方法
CN115826594B (zh) 不依赖动态模型参数的无人潜航器切换拓扑编队控制方法
Lavoie et al. An invariant extended H∞ filter
KR101340158B1 (ko) 고정 표적을 이용한 무인항공기의 표적 위치 보정 방법 및 컴퓨터 판독 가능한 기록매체

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