CN110081878B - 一种多旋翼无人机的姿态及位置确定方法 - Google Patents

一种多旋翼无人机的姿态及位置确定方法 Download PDF

Info

Publication number
CN110081878B
CN110081878B CN201910411892.7A CN201910411892A CN110081878B CN 110081878 B CN110081878 B CN 110081878B CN 201910411892 A CN201910411892 A CN 201910411892A CN 110081878 B CN110081878 B CN 110081878B
Authority
CN
China
Prior art keywords
unmanned aerial
aerial vehicle
coordinate system
rotor unmanned
attitude
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.)
Expired - Fee Related
Application number
CN201910411892.7A
Other languages
English (en)
Other versions
CN110081878A (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.)
Northeastern University China
Original Assignee
Northeastern University China
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 Northeastern University China filed Critical Northeastern University China
Priority to CN201910411892.7A priority Critical patent/CN110081878B/zh
Publication of CN110081878A publication Critical patent/CN110081878A/zh
Application granted granted Critical
Publication of CN110081878B publication Critical patent/CN110081878B/zh
Expired - Fee Related 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/005—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 with correlation of navigation data from several sources, e.g. map or contour matching
    • 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/20—Instruments for performing navigational calculations
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
    • G01S19/00—Satellite radio beacon positioning systems; Determining position, velocity or attitude using signals transmitted by such systems
    • G01S19/38—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system
    • G01S19/39—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system the satellite radio beacon positioning system transmitting time-stamped messages, e.g. GPS [Global Positioning System], GLONASS [Global Orbiting Navigation Satellite System] or GALILEO
    • G01S19/42—Determining position
    • G01S19/48—Determining position by combining or switching between position solutions derived from the satellite radio beacon positioning system and position solutions derived from a further system
    • G01S19/49—Determining position by combining or switching between position solutions derived from the satellite radio beacon positioning system and position solutions derived from a further system whereby the further system is an inertial position system, e.g. loosely-coupled

Landscapes

  • Engineering & Computer Science (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Automation & Control Theory (AREA)
  • Computer Networks & Wireless Communication (AREA)
  • Navigation (AREA)
  • Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)

Abstract

本发明涉及多传感器融合技术领域，提供一种多旋翼无人机的姿态及位置确定方法。首先利用加速度计、陀螺仪、磁力计、gps模块、超声波传感器分别采集多旋翼无人机的三轴加速度、三轴角速度、三轴磁力值、gps位置、距地面高度数据；然后进行数据预处理；接着利用三轴角速度和三轴加速度、三轴磁力值分别解算多旋翼无人机的姿态，再对上两步解算出的姿态进行扩展卡尔曼滤波融合；再利用gps位置解算多旋翼无人机的水平位置，结合三轴加速度和解算出的姿态，对上一步解算出的水平位置进行扩展卡尔曼滤波融合，利用距地面高度数据解算多旋翼无人机的垂直位置。本发明能够提高多旋翼无人机姿态及位置确定的精度及效率。

Description

一种多旋翼无人机的姿态及位置确定方法
技术领域
本发明涉及多传感器融合技术领域，特别是涉及一种多旋翼无人机的姿态及位置确定方法。
背景技术
随着多旋翼无人机问世以来，多旋翼无人机越来越受到人们的热烈追捧，也在各个方面广泛应用。目前，多旋翼无人机在航拍、农业、植保、微型自拍、快递运输、灾难救援、观察野生动物、监控传染病、测绘、新闻报道、电力巡检、影视拍摄等各个行业都起到了重要的作用，大大地拓展了无人机本身的用途，发达国家也在积极扩展行业应用与发展无人机技术。
多旋翼无人机的广泛应用，也使得无人机要面对更复杂的工作环境。目前无人机工作时的主要控制方法是人的手动遥控和程序的自动控制，手动遥控有安全性风险，易撞击坠毁，且易受到电磁信号干扰。
面对较为复杂的工作环境，无人机的自动控制比手动遥控更为安全可靠，相应地无人机也需要对自身物理状态(位置与姿态)有一个较为精确的估计，以便实现对无人机的精确控制。目前求解无人机位置与姿态的方法主要有卡尔曼滤波和互补滤波两种，求解姿态角融合加速度计与陀螺仪信息，求解位姿用到gps与加速度计以及姿态角。由于gps信息输出频率通常比较低，利用传统的方法融合求得的无人机位置离散化严重，不能满足精确的控制。
发明内容
针对现有技术存在的问题，本发明提供一种多旋翼无人机的姿态及位置确定方法，能够提高多旋翼无人机姿态及位置确定的精度及效率。
本发明的技术方案为：
一种多旋翼无人机的姿态及位置确定方法，所述多旋翼无人机配置有惯性测量单元、gps模块、超声波传感器，所述惯性测量单元包括加速度计、陀螺仪、磁力计，其特征在于，包括下述步骤：
步骤1：进行数据采集：以采样周期Ts，利用加速度计、陀螺仪、磁力计、gps模块、超声波传感器分别采集多旋翼无人机在第k时刻的三轴加速度Abk＝[axk,ayk,azk]T、三轴角速度Ωbk＝[wxk,wyk,wzk]T、三轴磁力值Mbk＝[mxk,myk,mzk]T、gps位置[λk,φk,hk]T、距地面高度hvk；其中，b代表数据是机体坐标系中的，多旋翼无人机的导航坐标系为ENU坐标系，多旋翼无人机的起飞原点为导航坐标系的原点，λk、φk、hk分别为WGS-84坐标系中多旋翼无人机在第k时刻的经度、纬度、海拔，k∈{0,1,2,...,K}，K为采样总数；
步骤2：进行数据预处理：对三轴加速度Ak进行低通滤波处理；
步骤3：解算多旋翼无人机的姿态：
步骤3.1：利用三轴角速度数据解算多旋翼无人机的姿态：
步骤3.1.1：计算多旋翼无人机的偏航角ψk、翻滚角θk、俯仰角γk对应的旋转矩阵分别为
Figure BDA0002063065280000021
Figure BDA0002063065280000022
Figure BDA0002063065280000023
步骤3.1.2：将多旋翼无人机的三次旋转表示为
Figure BDA0002063065280000024
其中，三次旋转为：多旋翼无人机从初始位置开始，绕导航坐标系的zn轴旋转角度ψk，再绕旋转后的坐标系的y'n轴旋转角度θk，再绕旋转后坐标系的x”n轴旋转角度γk；
Figure BDA0002063065280000025
为导航坐标系下的三轴角速度；
步骤3.1.3：由公式(4)得到欧拉角方程为：
Figure BDA0002063065280000031
步骤3.1.4：对式(5)进行一阶线性离散化，得到：
Figure BDA0002063065280000032
步骤3.2：利用低通滤波后的三轴加速度数据和三轴磁力值数据解算多旋翼无人机的姿态：
步骤3.2.1：计算导航坐标系到机体坐标系的坐标转换矩阵为
Figure BDA0002063065280000033
从而机体坐标系到导航坐标系的坐标转换矩阵为
Figure BDA0002063065280000034
步骤3.2.2：机体坐标系中的三轴加速度Abk＝[axk,ayk,azk]T转换到导航坐标系中为Ank＝[0,0,g]T，
Figure BDA0002063065280000035
计算得到
Figure BDA0002063065280000036
Figure BDA0002063065280000037
由于加速度信息与偏航角ψk无关，令ψk＝0，得到
Figure BDA0002063065280000038
其中，g为重力加速度；
步骤3.2.3：机体坐标系中的三轴磁力值Mbk＝[mxk,myk,mzk]T转换到导航坐标系中为Mnk＝[m'xk,m'yk,m'zk]T：
Figure BDA0002063065280000041
计算得到地磁场水平分量为
Figure BDA0002063065280000042
计算得出偏航角为
Figure BDA0002063065280000043
步骤3.3：对步骤3.1和步骤3.2中解算出的姿态进行扩展卡尔曼滤波融合；
步骤3.3.1：构建多旋翼无人机姿态的状态方程和观测方程分别为
X1k＝f(X1,k-1)+W1,k-1 (16)
Z1k＝HkX1k+V1k (17)
其中，X1k＝[γk,θk,ψk]T、Z1k＝[zγk,zθk,zψk]T分别为第k时刻多旋翼无人机的姿态状态向量、姿态测量向量，f(X1,k-1)表示的关系为式(6)表示的关系，zγk、zθk、zψk分别为式(10)中的γk、式(9)中的θk、式(14)中的ψk；W1k＝[δγk,δθk,δψk]T、V1k＝[vγk,vθk,vψk]T分别为第一噪声向量、第二噪声向量，均由惯性测量单元标定测量后确定，E[W1k]＝qk，Cov(W1k,W1k)＝Qk，E[V1k]＝rk，Cov(V1k,V1k)＝Rk，Cov(W1k,V1k)＝0；
Figure BDA0002063065280000044
对非线性的f(X1,k-1)用扩展卡尔曼滤波进行线性化，得到
Figure BDA0002063065280000045
从而多旋翼无人机姿态的状态方程可以表示为
Figure BDA0002063065280000051
步骤3.3.2：估计第k时刻姿态的状态变量为
Figure BDA0002063065280000052
步骤3.3.3：计算第k时刻姿态的均方误差为
Figure BDA0002063065280000053
步骤3.3.4：计算第k时刻姿态的滤波增益为
Figure BDA0002063065280000054
步骤3.3.5：用第k时刻姿态的观测量对第k时刻姿态的估计量进行修正，得到第k时刻姿态的最优估计值为
Figure BDA0002063065280000055
将
Figure BDA0002063065280000056
作为多旋翼无人机在第k时刻的最终姿态；
步骤3.3.6：更新均方误差为：
P1k＝[I-v1kHk]P1,k/k-1 (24)
其中，I为单位矩阵；
步骤4：解算多旋翼无人机的位置：
步骤4.1：利用gps位置数据解算多旋翼无人机的水平位置：
步骤4.1.1：将WGS-84坐标系中的gps位置数据[λk,φk,hk]T转换为ECEF坐标系中的位置数据[xEk,yEk,zEk]T：
Figure BDA0002063065280000057
其中，
Figure BDA0002063065280000058
a为WGS-84坐标系中的长轴半径，e为WGS-84坐标系的固定参数；
步骤4.1.2：将ECEF坐标系中的位置数据[xEk,yEk,zEk]T转换为ENU坐标系中的位置数据[xk,yk,zk]T：
Figure BDA0002063065280000061
其中，[xE0,yE0,zE0]T为导航坐标系的原点在ECEF坐标系中的坐标表示：
Figure BDA0002063065280000062
[λ0,φ0,h0]T为多旋翼无人机在起飞原点时gps模块输出的gps位置数据；
步骤4.2：结合三轴加速度数据和步骤3中解算出的多旋翼无人机在第k时刻的姿态数据，对步骤4.1中解算出的水平位置进行扩展卡尔曼滤波融合：
步骤4.2.1：构建多旋翼无人机水平位置的状态方程和观测方程分别为
X2k＝ΦX2,k-1+Buk-1+ΓW2,k-1 (27)
Z2k＝HX2k+V2k (28)
其中，X2k＝[Sxk,Syk,vexk,veyk]T、Z2k＝[Sgxk,Sgyk]T分别为第k时刻多旋翼无人机的水平位置状态向量、水平位置测量向量，(Sxk,Syk)、(vexk,veyk)分别为第k时刻多旋翼无人机在导航坐标系中的水平位置、水平速度，Sgxk、Sgyk分别为式(26)中的xk、yk；
Figure BDA0002063065280000063
Figure BDA0002063065280000064
为第k时刻多旋翼无人机在导航坐标系中的水平加速度，
Figure BDA0002063065280000065
为
Figure BDA0002063065280000066
中的前两项，
Figure BDA0002063065280000067
中的γk、θk、ψk由式(23)计算得出；Γ为噪声驱动矩阵，Γ＝B，W2k、V2k均为2×1维***噪声向量，W2k的方差统计特性完全由uk的统计特性决定，Φ、B、H均为常数矩阵，
Figure BDA0002063065280000068
步骤4.2.2：估计第k时刻水平位置的状态变量为
Figure BDA0002063065280000069
步骤4.2.3：计算第k时刻水平位置的均方误差为
Figure BDA0002063065280000071
其中，P2,k/k-1不随每次滤波迭代，
Figure BDA0002063065280000072
P均为常数矩阵，
Figure BDA0002063065280000073
步骤4.2.4：计算第k时刻水平位置的滤波增益为
Figure BDA0002063065280000074
其中，
Figure BDA0002063065280000075
为常数矩阵，
Figure BDA0002063065280000076
步骤4.2.5：用第k时刻水平位置的观测量对第k时刻水平位置的估计量进行修正，得到第k时刻的最优估计值为
Figure BDA0002063065280000077
将
Figure BDA0002063065280000078
为多旋翼无人机在第k时刻的最终水平位置和水平速度；
步骤4.3：利用距地面高度数据解算多旋翼无人机的垂直位置：将距地面高度hvk作为多旋翼无人机在第k时刻的的垂直位置Szk。
所述步骤2中，低通滤波的方法为算术平均滤波法，将三轴加速度的m个连续采样值相加后取算术平均值作为低通滤波后的值：
Figure BDA0002063065280000079
所述步骤3.2.3中，对式(14)进行修正得到：
Figure BDA00020630652800000710
本发明的有益效果为：
本发明通过不同数据的融合，得到多旋翼无人机的最优姿态和位置：利用多旋翼无人机的三轴角速度数据和低通滤波后的三轴加速度数据、三轴磁力值数据分别解算多旋翼无人机的姿态，然后对解算出的姿态进行扩展卡尔曼滤波融合，得到多旋翼无人机的最优姿态；并利用gps位置数据解算多旋翼无人机的水平位置，然后结合三轴加速度数据和解算出的姿态数据，对gps位置数据解算出的水平位置进行扩展卡尔曼滤波融合，得到多旋翼无人机的最优水平位置；由超声波传感器输出的距地面高度数据解算出多旋翼无人机的垂直位置，大大提高了多旋翼无人机姿态及位置确定的精度及效率。
附图说明
图1为本发明多旋翼无人机的姿态及位置确定方法的流程图；
图2为实施例中对加速度数据进行低通滤波的效果图；
图3为多旋翼无人机的旋转过程示意图；
图4为WGS-84坐标系的YOZ平面剖面示意图；
图5为WGS-84坐标系的XOY平面剖面示意图；
图6为实施例中未经滤波前直接由gps数据解算出的多旋翼无人机在水平面上的运动轨迹示意图；
图7为实施例中本发明多旋翼无人机的姿态及位置确定方法解算出的多旋翼无人机在水平面上的运动轨迹示意图。
具体实施方式
下面将结合附图和具体实施方式，对本发明作进一步描述。
如图1所示，为本发明多旋翼无人机的姿态及位置确定方法的流程图。本实施例中，所述多旋翼无人机为四旋翼无人机ParrortARDrone2.0。ParrortARDrone2.0是法国的一家无人机厂商生产的无人机产品，主要配置有惯性测量单元IMU、gps模块、超声波传感器、前置相机等，该超声波传感器为定高超声波传感器，所述惯性测量单元包括加速度计、陀螺仪、磁力计。本发明的多旋翼无人机的姿态及位置确定方法，其特征在于，包括下述步骤：
步骤1：进行数据采集：以采样周期Ts，利用加速度计、陀螺仪、磁力计、gps模块、超声波传感器分别采集多旋翼无人机在第k时刻的三轴加速度Abk＝[axk,ayk,azk]T、三轴角速度Ωbk＝[wxk,wyk,wzk]T、三轴磁力值Mbk＝[mxk,myk,mzk]T、gps位置[λk,φk,hk]T、距地面高度hvk；其中，b代表数据是机体坐标系中的，多旋翼无人机的导航坐标系为ENU坐标系，多旋翼无人机的起飞原点为导航坐标系的原点，λk、φk、hk分别为WGS-84坐标系中多旋翼无人机在第k时刻的经度、纬度、海拔，k∈{0,1,2,...,K}，K为采样总数。本实施例中，Ts＝0.02s，K＝4000。
其中，由于IMU是固连在无人机机体上的，所以加速度计、陀螺仪、磁力计输出的数据都是在机体坐标系下的。
步骤2：进行数据预处理：对三轴加速度Ak进行低通滤波处理；
无人机飞行时，加速度计的数据输出受到机体振动的影响较大，需要进行低通滤波。本实施例中，低通滤波的方法为算术平均滤波法，将三轴加速度的m＝16个连续采样值相加后取算术平均值作为低通滤波后的值：
Figure BDA0002063065280000091
对多旋翼无人机的y轴加速度数据进行低通滤波的效果图如图2所示。其中，散点是未处理的数据，线条是处理后的数据。可以看出，经过低通滤波后得到的加速度数据曲线比较平滑。
步骤3：解算多旋翼无人机的姿态：
导航坐标系选的是ENU即东-北-天坐标系，其x轴为正东方向、y轴为正北方向、z轴则与重力方向相反向上。无人机的偏航角ψk、翻滚角θk、俯仰角γk都是以导航坐标系为参考系。
步骤3.1：利用三轴角速度数据解算多旋翼无人机的姿态：
步骤3.1.1：计算多旋翼无人机的偏航角ψk、翻滚角θk、俯仰角γk对应的旋转矩阵分别为
Figure BDA0002063065280000092
Figure BDA0002063065280000093
Figure BDA0002063065280000094
步骤3.1.2：将多旋翼无人机的三次旋转表示为
Figure BDA0002063065280000101
其中，如图3所示，三次旋转为：多旋翼无人机从初始位置开始，绕导航坐标系的zn轴旋转角度ψk，再绕旋转后的坐标系的y'n轴旋转角度θk，再绕旋转后坐标系的x'n'轴旋转角度γk；
Figure BDA0002063065280000102
为导航坐标系下的三轴角速度；
步骤3.1.3：由公式(4)得到欧拉角方程为：
Figure BDA0002063065280000103
步骤3.1.4：对式(5)进行一阶线性离散化，得到：
Figure BDA0002063065280000104
步骤3.2：利用低通滤波后的三轴加速度数据和三轴磁力值数据解算多旋翼无人机的姿态：
步骤3.2.1：计算导航坐标系到机体坐标系的坐标转换矩阵为
Figure BDA0002063065280000105
从而机体坐标系到导航坐标系的坐标转换矩阵为
Figure BDA0002063065280000106
无人机在导航坐标系下处于任意姿态并保持静止时，加速度计测量输出三轴加速度Abk＝[axk,ayk,azk]T。由于重力存在，其加速度计输出值Abk转换到导航坐标系下后一定为Ank＝[0,0,g]T，从而：
步骤3.2.2：机体坐标系中的三轴加速度Abk＝[axk,ayk,azk]T转换到导航坐标系中为Ank＝[0,0,g]T，
Figure BDA0002063065280000111
计算得到
Figure BDA0002063065280000112
Figure BDA0002063065280000113
由于加速度信息与偏航角ψk无关，令ψk＝0，得到
Figure BDA0002063065280000114
其中，g为重力加速度；
步骤3.2.3：机体坐标系中的三轴磁力值Mbk＝[mxk,myk,mzk]T转换到导航坐标系中为Mnk＝[m'xk,m'yk,m'zk]T：
Figure BDA0002063065280000117
计算得到地磁场水平分量为
Figure BDA0002063065280000115
计算得出偏航角为
Figure BDA0002063065280000116
所述步骤3.2.3中，对式(14)进行修正得到：
Figure BDA0002063065280000121
步骤3.3：对步骤3.1和步骤3.2中解算出的姿态进行扩展卡尔曼滤波融合；
步骤3.3.1：构建多旋翼无人机姿态的状态方程和观测方程分别为
X1k＝f(X1,k-1)+W1,k-1 (16)
Z1k＝HkX1k+V1k (17)
其中，X1k＝[γk,θk,ψk]T、Z1k＝[zγk,zθk,zψk]T分别为第k时刻多旋翼无人机的姿态状态向量、姿态测量向量，f(X1,k-1)表示的关系为式(6)表示的关系，zγk、zθk、zψk分别为式(10)中的γk、式(9)中的θk、式(14)中的ψk；W1k＝[δγk,δθk,δψk]T、V1k＝[vγk,vθk,vψk]T分别为第一噪声向量、第二噪声向量，均由惯性测量单元标定测量后确定，E[W1k]＝qk，Cov(W1k,W1k)＝Qk，E[V1k]＝rk，Cov(V1k,V1k)＝Rk，Cov(W1k,V1k)＝0；
Figure BDA0002063065280000122
对非线性的f(X1,k-1)用扩展卡尔曼滤波进行线性化，得到
Figure BDA0002063065280000123
从而多旋翼无人机姿态的状态方程可以表示为
Figure BDA0002063065280000131
其中，无人机姿态信息融合***为非线性***，式(16)、式(17)为其离散时间姿态状态模型。
步骤3.3.2：估计第k时刻姿态的状态变量为
Figure BDA0002063065280000132
步骤3.3.3：计算第k时刻姿态的均方误差为
Figure BDA0002063065280000133
其中，P1,k/k-1是经过式(20)后所产生的方差；
步骤3.3.4：计算第k时刻姿态的滤波增益为
Figure BDA0002063065280000134
其中，滤波增益v1k代表通过加速度计和磁力计所得到的观测值Z1k在融合结果
Figure BDA0002063065280000135
中所占的权重。
步骤3.3.5：用第k时刻姿态的观测量对第k时刻姿态的估计量进行修正，得到第k时刻姿态的最优估计值为
Figure BDA0002063065280000136
将
Figure BDA0002063065280000137
作为多旋翼无人机在第k时刻的最终姿态；
步骤3.3.6：更新均方误差为：
P1k＝[I-v1kHk]P1,k/k-1 (24)
其中，I为单位矩阵；P1k是经过式(23)后产生的新方差，为k+1时刻所用。本实施例中，P1k的初始值为
Figure BDA0002063065280000138
Qk、Rk均为常数矩阵，
Figure BDA0002063065280000139
Figure BDA00020630652800001310
步骤4：解算多旋翼无人机的位置：
由于gps解算出的水平位置较为准确，而垂直误差较大，所以将位置解算分成两个部分：水平位置解算和垂直位置解算。
步骤4.1：利用gps位置数据解算多旋翼无人机的水平位置：
gps模块输出的经纬度和海拔数据是在WGS-84坐标系下的位置表示，需要把它转换到导航坐标系(ENU坐标系)下。这个过程分为两步：首先把WGS-84坐标系下的位置坐标转换到ECEF坐标系下，再将ECEF坐标系下的位置坐标转换到ENU坐标系下。如图4和图5所示，分别为WGS-84坐标系的YOZ平面、XOY平面剖面示意图。ECEF坐标系是三维直角坐标系，其原点与坐标轴与WGS-84坐标系重合。WGS-84坐标系的一些参数如下：
长轴半径：a＝6378137m
扁率：
Figure BDA0002063065280000141
长半轴a、短半轴b和扁率f之间的关系：
Figure BDA0002063065280000142
步骤4.1.1：将WGS-84坐标系中的gps位置数据[λk,φk,hk]T转换为ECEF坐标系中的位置数据[xEk,yEk,zEk]T：
Figure BDA0002063065280000143
其中，
Figure BDA0002063065280000144
a为WGS-84坐标系中的长轴半径，e为WGS-84坐标系的固定参数；
步骤4.1.2：将ECEF坐标系中的位置数据[xEk,yEk,zEk]T转换为ENU坐标系中的位置数据[xk,yk,zk]T：
Figure BDA0002063065280000145
其中，[xE0,yE0,zE0]T为导航坐标系的原点在ECEF坐标系中的坐标表示：
Figure BDA0002063065280000146
[λ0,φ0,h0]T为多旋翼无人机在起飞原点时gps模块输出的gps位置数据；
步骤4.2：结合三轴加速度数据和步骤3中解算出的多旋翼无人机在第k时刻的姿态数据，对步骤4.1中解算出的水平位置进行扩展卡尔曼滤波融合：
步骤4.2.1：构建多旋翼无人机水平位置的状态方程和观测方程分别为
X2k＝ΦX2,k-1+Buk-1+ΓW2,k-1 (27)
Z2k＝HX2k+V2k (28)
其中，X2k＝[Sxk,Syk,vexk,veyk]T、Z2k＝[Sgxk,Sgyk]T分别为第k时刻多旋翼无人机的水平位置状态向量、水平位置测量向量，(Sxk,Syk)、(vexk,veyk)分别为第k时刻多旋翼无人机在导航坐标系中的水平位置、水平速度，Sgxk、Sgyk分别为式(26)中的xk、yk；
Figure BDA0002063065280000151
Figure BDA0002063065280000152
为第k时刻多旋翼无人机在导航坐标系中的水平加速度，
Figure BDA0002063065280000153
为
Figure BDA0002063065280000154
中的前两项，
Figure BDA0002063065280000155
中的γk、θk、ψk由式(23)计算得出；Γ为噪声驱动矩阵，Γ＝B，W2k、V2k均为2×1维***噪声向量，W2k的方差统计特性完全由uk的统计特性决定，Φ、B、H均为常数矩阵，
Figure BDA0002063065280000156
其中，无人机水平位置信息融合***为非线性***，式(27)、式(28)为其离散时间水平位置状态模型。
步骤4.2.2：估计第k时刻水平位置的状态变量为
Figure BDA0002063065280000157
步骤4.2.3：计算第k时刻水平位置的均方误差为
Figure BDA0002063065280000158
其中，P2,k/k-1不随每次滤波迭代，
Figure BDA0002063065280000159
P均为常数矩阵，
Figure BDA00020630652800001510
其中，gps的长期静态性能较好而短期响应速度比较差，卡尔曼滤波最终达到的效果是让每次融合的结果方差最小。由于gps响应慢，其解算出来的值易较长时间保持不变，所以在融合过程中更容易占更大的比例，显然这样的结果不是我们想要的。解决方法是让状态一步预测均方误差矩阵P2,k/k-1不随每次滤波迭代，使其在滤波开始就是确定的常数矩阵P，从而滤波增益也会是一个常数矩阵。Φ、Γ、
Figure BDA0002063065280000161
和H都是常数矩阵，如此便不会使得由状态一步预测得到的结果所起的作用越来越小。
步骤4.2.4：计算第k时刻水平位置的滤波增益为
Figure BDA0002063065280000162
其中，
Figure BDA0002063065280000163
为常数矩阵，
Figure BDA0002063065280000164
本实施例中，
Figure BDA0002063065280000165
步骤4.2.5：用第k时刻水平位置的观测量对第k时刻水平位置的估计量进行修正，得到第k时刻的最优估计值为
Figure BDA0002063065280000166
将
Figure BDA0002063065280000167
为多旋翼无人机在第k时刻的最终水平位置和水平速度；
如图6所示，为本实施例中未经滤波前直接由gps数据解算出的多旋翼无人机在水平面上的运动轨迹；如图7所示，为本实施例中本发明多旋翼无人机的姿态及位置确定方法解算出的多旋翼无人机在水平面上的运动轨迹。
步骤4.3：利用距地面高度数据解算多旋翼无人机的垂直位置：将距地面高度hvk作为多旋翼无人机在第k时刻的的垂直位置Szk。
本实施例中，ParrortARDrone2.0自带一个定高的超声波传感器，超声波传感器输出的数据直接是无人机距离地面高度，精度为1毫米，有效范围在0.5米到12米，从而可以直接作为最终的垂直位置。
显然，上述实施例仅仅是本发明的一部分实施例，而不是全部的实施例。上述实施例仅用于解释本发明，并不构成对本发明保护范围的限定。基于上述实施例，本领域技术人员在没有做出创造性劳动的前提下所获得的所有其他实施例，也即凡在本申请的精神和原理之内所作的所有修改、等同替换和改进等，均落在本发明要求的保护范围内。

Claims (3)

1.一种多旋翼无人机的姿态及位置确定方法，所述多旋翼无人机配置有惯性测量单元、gps模块、超声波传感器，所述惯性测量单元包括加速度计、陀螺仪、磁力计，其特征在于，包括下述步骤：
步骤1：进行数据采集：以采样周期Ts，利用加速度计、陀螺仪、磁力计、gps模块、超声波传感器分别采集多旋翼无人机在第k时刻的三轴加速度Abk＝[axk,ayk,azk]T、三轴角速度Ωbk＝[wxk,wyk,wzk]T、三轴磁力值Mbk＝[mxk,myk,mzk]T、gps位置[λk,φk,hk]T、距地面高度hvk；其中，b代表数据是机体坐标系中的，多旋翼无人机的导航坐标系为ENU坐标系，多旋翼无人机的起飞原点为导航坐标系的原点，λk、φk、hk分别为WGS-84坐标系中多旋翼无人机在第k时刻的经度、纬度、海拔，k∈{0,1,2,...,K}，K为采样总数；
步骤2：进行数据预处理：对三轴加速度Ab k进行低通滤波处理；
步骤3：解算多旋翼无人机的姿态：
步骤3.1：利用三轴角速度数据解算多旋翼无人机的姿态：
步骤3.1.1：计算多旋翼无人机的偏航角ψk、翻滚角θk、俯仰角γk对应的旋转矩阵分别为
Figure FDA0002063065270000011
Figure FDA0002063065270000012
Figure FDA0002063065270000013
步骤3.1.2：将多旋翼无人机的三次旋转表示为
Figure FDA0002063065270000014
其中，三次旋转为：多旋翼无人机从初始位置开始，绕导航坐标系的zn轴旋转角度ψk，再绕旋转后的坐标系的y'n轴旋转角度θk，再绕旋转后坐标系的x″n轴旋转角度γk；
Figure FDA0002063065270000021
为导航坐标系下的三轴角速度；
步骤3.1.3：由公式(4)得到欧拉角方程为：
Figure FDA0002063065270000022
步骤3.1.4：对式(5)进行一阶线性离散化，得到：
Figure FDA0002063065270000023
步骤3.2：利用低通滤波后的三轴加速度数据和三轴磁力值数据解算多旋翼无人机的姿态：
步骤3.2.1：计算导航坐标系到机体坐标系的坐标转换矩阵为
Figure FDA0002063065270000024
从而机体坐标系到导航坐标系的坐标转换矩阵为
Figure FDA0002063065270000025
步骤3.2.2：机体坐标系中的三轴加速度Abk＝[axk,ayk,azk]T转换到导航坐标系中为Ank＝[0,0,g]T，
Figure FDA0002063065270000026
计算得到
Figure FDA0002063065270000027
Figure FDA0002063065270000028
由于加速度信息与偏航角ψk无关，令ψk＝0，得到
Figure FDA0002063065270000031
其中，g为重力加速度；
步骤3.2.3：机体坐标系中的三轴磁力值Mbk＝[mxk,myk,mzk]T转换到导航坐标系中为Mnk＝[m'xk,m'yk,m'zk]T：
Figure FDA0002063065270000032
计算得到地磁场水平分量为
Figure FDA0002063065270000033
计算得出偏航角为
Figure FDA0002063065270000034
步骤3.3：对步骤3.1和步骤3.2中解算出的姿态进行扩展卡尔曼滤波融合；
步骤3.3.1：构建多旋翼无人机姿态的状态方程和观测方程分别为
X1k＝f(X1,k-1)+W1,k-1 (16)
Z1k＝HkX1k+V1k (17)
其中，X1k＝[γk,θk,ψk]T、Z1k＝[zγk,zθk,zψk]T分别为第k时刻多旋翼无人机的姿态状态向量、姿态测量向量，f(X1,k-1)表示的关系为式(6)表示的关系，zγk、zθk、zψk分别为式(10)中的γk、式(9)中的θk、式(14)中的ψk；W1k＝[δγk,δθk,δψk]T、V1k＝[vγk,vθk,vψk]T分别为第一噪声向量、第二噪声向量，均由惯性测量单元标定测量后确定，E[W1k]＝qk，Cov(W1k,W1k)＝Qk，E[V1k]＝rk，Cov(V1k,V1k)＝Rk，Cov(W1k,V1k)＝0；
Figure FDA0002063065270000035
对非线性的f(X1,k-1)用扩展卡尔曼滤波进行线性化，得到
Figure FDA0002063065270000036
Figure FDA0002063065270000041
从而多旋翼无人机姿态的状态方程可以表示为
Figure FDA0002063065270000042
步骤3.3.2：估计第k时刻姿态的状态变量为
Figure FDA0002063065270000043
步骤3.3.3：计算第k时刻姿态的均方误差为
Figure FDA0002063065270000044
步骤3.3.4：计算第k时刻姿态的滤波增益为
Figure FDA0002063065270000045
步骤3.3.5：用第k时刻姿态的观测量对第k时刻姿态的估计量进行修正，得到第k时刻姿态的最优估计值为
Figure FDA0002063065270000046
将
Figure FDA0002063065270000047
作为多旋翼无人机在第k时刻的最终姿态；
步骤3.3.6：更新均方误差为：
P1k＝[I-v1kHk]P1,k/k-1 (24)
其中，I为单位矩阵；
步骤4：解算多旋翼无人机的位置：
步骤4.1：利用gps位置数据解算多旋翼无人机的水平位置：
步骤4.1.1：将WGS-84坐标系中的gps位置数据[λk,φk,hk]T转换为ECEF坐标系中的位置数据[xEk,yEk,zEk]T：
Figure FDA0002063065270000048
其中，
Figure FDA0002063065270000049
a为WGS-84坐标系中的长轴半径，e为WGS-84坐标系的固定参数；
步骤4.1.2：将ECEF坐标系中的位置数据[xEk,yEk,zEk]T转换为ENU坐标系中的位置数据[xk,yk,zk]T：
Figure FDA0002063065270000051
其中，[xE0,yE0,zE0]T为导航坐标系的原点在ECEF坐标系中的坐标表示：
Figure FDA0002063065270000052
[λ0,φ0,h0]T为多旋翼无人机在起飞原点时gps模块输出的gps位置数据；
步骤4.2：结合三轴加速度数据和步骤3中解算出的多旋翼无人机在第k时刻的姿态数据，对步骤4.1中解算出的水平位置进行扩展卡尔曼滤波融合：
步骤4.2.1：构建多旋翼无人机水平位置的状态方程和观测方程分别为
X2k＝ΦX2,k-1+Buk-1+ΓW2,k-1 (27)
Z2k＝HX2k+V2k (28)
其中，X2k＝[Sxk,Syk,vexk,veyk]T、Z2k＝[Sgxk,Sgyk]T分别为第k时刻多旋翼无人机的水平位置状态向量、水平位置测量向量，(Sxk,Syk)、(vexk,veyk)分别为第k时刻多旋翼无人机在导航坐标系中的水平位置、水平速度，Sgxk、Sgyk分别为式(26)中的xk、yk；
Figure FDA0002063065270000053
Figure FDA0002063065270000054
为第k时刻多旋翼无人机在导航坐标系中的水平加速度，
Figure FDA0002063065270000055
为
Figure FDA0002063065270000056
中的前两项，
Figure FDA0002063065270000057
中的γk、θk、ψk由式(23)计算得出；Γ为噪声驱动矩阵，Γ＝B，W2k、V2k均为2×1维***噪声向量，W2k的方差统计特性完全由uk的统计特性决定，Φ、B、H均为常数矩阵，
Figure FDA0002063065270000058
步骤4.2.2：估计第k时刻水平位置的状态变量为
Figure FDA0002063065270000061
步骤4.2.3：计算第k时刻水平位置的均方误差为
Figure FDA0002063065270000062
其中，P2,k/k-1不随每次滤波迭代，
Figure FDA0002063065270000063
P均为常数矩阵，
Figure FDA0002063065270000064
步骤4.2.4：计算第k时刻水平位置的滤波增益为
Figure FDA0002063065270000065
其中，
Figure FDA0002063065270000066
为常数矩阵，
Figure FDA0002063065270000067
步骤4.2.5：用第k时刻水平位置的观测量对第k时刻水平位置的估计量进行修正，得到第k时刻的最优估计值为
Figure FDA0002063065270000068
将
Figure FDA0002063065270000069
为多旋翼无人机在第k时刻的最终水平位置和水平速度；
步骤4.3：利用距地面高度数据解算多旋翼无人机的垂直位置：将距地面高度hvk作为多旋翼无人机在第k时刻的的垂直位置Szk。
2.根据权利要求1所述的多旋翼无人机的姿态及位置确定方法，其特征在于，所述步骤2中，低通滤波的方法为算术平均滤波法，将三轴加速度的m个连续采样值相加后取算术平均值作为低通滤波后的值：
Figure FDA00020630652700000610
3.根据权利要求1所述的多旋翼无人机的姿态及位置确定方法，其特征在于，所述步骤3.2.3中，对式(14)进行修正得到：
Figure FDA0002063065270000071
CN201910411892.7A 2019-05-17 2019-05-17 一种多旋翼无人机的姿态及位置确定方法 Expired - Fee Related CN110081878B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN201910411892.7A CN110081878B (zh) 2019-05-17 2019-05-17 一种多旋翼无人机的姿态及位置确定方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN201910411892.7A CN110081878B (zh) 2019-05-17 2019-05-17 一种多旋翼无人机的姿态及位置确定方法

Publications (2)

Publication Number Publication Date
CN110081878A CN110081878A (zh) 2019-08-02
CN110081878B true CN110081878B (zh) 2023-01-24

Family

ID=67420696

Family Applications (1)

Application Number Title Priority Date Filing Date
CN201910411892.7A Expired - Fee Related CN110081878B (zh) 2019-05-17 2019-05-17 一种多旋翼无人机的姿态及位置确定方法

Country Status (1)

Country Link
CN (1) CN110081878B (zh)

Families Citing this family (9)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN112577484A (zh) * 2019-09-29 2021-03-30 北京信息科技大学 一种小型气象探测设备应用的遥测装置
CN110598370B (zh) * 2019-10-18 2023-04-14 太原理工大学 基于sip和ekf融合的多旋翼无人机鲁棒姿态估计
CN110955258B (zh) * 2019-11-28 2023-04-28 深圳蚁石科技有限公司 四轴飞行器的控制方法、装置、控制器和存储介质
CN112649001B (zh) * 2020-12-01 2023-08-22 中国航空工业集团公司沈阳飞机设计研究所 一种小型无人机姿态与位置解算方法
CN112486215B (zh) * 2020-12-02 2022-12-02 北京特种机械研究所 一种辅助飞行器装填的测控***
CN113311456B (zh) * 2021-05-18 2022-08-16 武汉大学 一种基于卡尔曼滤波的qar数据噪声处理方法
CN113237478B (zh) * 2021-05-27 2022-10-14 哈尔滨工业大学 一种无人机的姿态和位置估计方法及无人机
CN115826602B (zh) * 2022-11-17 2023-11-17 众芯汉创（北京）科技有限公司 一种基于无人机的飞行动态精准定位管理***和方法
CN117389322B (zh) * 2023-12-08 2024-03-01 天津天羿科技有限公司 无人机控制方法

Citations (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN106643737A (zh) * 2017-02-07 2017-05-10 大连大学 风力干扰环境下四旋翼飞行器姿态解算方法
CN108645404A (zh) * 2018-03-30 2018-10-12 西安建筑科技大学 一种小型多旋翼无人机姿态解算方法

Patent Citations (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN106643737A (zh) * 2017-02-07 2017-05-10 大连大学 风力干扰环境下四旋翼飞行器姿态解算方法
CN108645404A (zh) * 2018-03-30 2018-10-12 西安建筑科技大学 一种小型多旋翼无人机姿态解算方法

Non-Patent Citations (1)

* Cited by examiner, † Cited by third party
Title
改进扩展卡尔曼滤波的四旋翼姿态估计算法;王龙等;《计算机应用》;20170410(第04期);第1122-1128页 *

Also Published As

Publication number Publication date
CN110081878A (zh) 2019-08-02

Similar Documents

Publication Publication Date Title
CN110081878B (zh) 一种多旋翼无人机的姿态及位置确定方法
CN106959110B (zh) 一种云台姿态检测方法及装置
CN106643737B (zh) 风力干扰环境下四旋翼飞行器姿态解算方法
CN108592950B (zh) 一种单目相机和惯性测量单元相对安装角标定方法
CN107247459B (zh) 抗干扰飞行控制方法及装置
CN106403940B (zh) 一种抗大气参数漂移的无人机飞行导航***高度信息融合方法
CN112066985B (zh) 一种组合导航***初始化方法、装置、介质及电子设备
CN106249744B (zh) 一种基于二级互补滤波的小型旋翼飞行器高度控制方法
CN111189442B (zh) 基于cepf的无人机多源导航信息状态预测方法
CN110793515A (zh) 一种基于单天线gps和imu的大机动条件下无人机姿态估计方法
CN111238469B (zh) 一种基于惯性/数据链的无人机编队相对导航方法
CN108592911A (zh) 一种四旋翼飞行器动力学模型/机载传感器组合导航方法
CN110873563B (zh) 一种云台姿态估计方法及装置
CN103712598A (zh) 一种小型无人机姿态确定***与确定方法
CN108444468B (zh) 一种融合下视视觉与惯导信息的定向罗盘
EP3676617A1 (en) System and method for determining airspeed
CN115574816B (zh) 仿生视觉多源信息智能感知无人平台
CN109521785A (zh) 一种随身拍智能旋翼飞行器***
CN108536163A (zh) 一种单面结构环境下的动力学模型/激光雷达组合导航方法
CN108759814B (zh) 一种四旋翼飞行器横滚轴角速度和俯仰轴角速度估计方法
Ascorti An application of the extended Kalman filter to the attitude control of a quadrotor
WO2022057350A1 (zh) 一种基于非线性积分补偿的组合运动测量***的惯性预积分方法
Shiau et al. Unscented kalman filtering for attitude determination using mems sensors
CN113129377A (zh) 一种三维激光雷达快速鲁棒slam方法和装置
Emran et al. A cascaded approach for quadrotor's attitude estimation

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
CF01 Termination of patent right due to non-payment of annual fee
CF01 Termination of patent right due to non-payment of annual fee

Granted publication date: 20230124