CN110081878A - 一种多旋翼无人机的姿态及位置确定方法 - Google Patents
一种多旋翼无人机的姿态及位置确定方法 Download PDFInfo
- Publication number
- CN110081878A CN110081878A CN201910411892.7A CN201910411892A CN110081878A CN 110081878 A CN110081878 A CN 110081878A CN 201910411892 A CN201910411892 A CN 201910411892A CN 110081878 A CN110081878 A CN 110081878A
- 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.)
- Granted
Links
- 238000000034 method Methods 0.000 title claims abstract description 27
- 230000001133 acceleration Effects 0.000 claims abstract description 36
- 238000001914 filtration Methods 0.000 claims abstract description 36
- 230000004927 fusion Effects 0.000 claims abstract description 18
- 239000011159 matrix material Substances 0.000 claims description 25
- 239000013598 vector Substances 0.000 claims description 21
- 238000005259 measurement Methods 0.000 claims description 18
- 238000006243 chemical reaction Methods 0.000 claims description 13
- 230000009466 transformation Effects 0.000 claims description 6
- 238000005070 sampling Methods 0.000 claims description 5
- 230000005484 gravity Effects 0.000 claims description 4
- 230000005358 geomagnetic field Effects 0.000 claims description 3
- 238000007781 pre-processing Methods 0.000 claims description 3
- 238000012545 processing Methods 0.000 claims description 3
- 238000005516 engineering process Methods 0.000 abstract description 2
- 230000000694 effects Effects 0.000 description 4
- 238000010586 diagram Methods 0.000 description 3
- 230000008569 process Effects 0.000 description 3
- 230000003068 static effect Effects 0.000 description 2
- 208000035473 Communicable disease Diseases 0.000 description 1
- 230000009286 beneficial effect Effects 0.000 description 1
- 230000000295 complement effect Effects 0.000 description 1
- 238000007499 fusion processing Methods 0.000 description 1
- 208000015181 infectious disease Diseases 0.000 description 1
- 238000007689 inspection Methods 0.000 description 1
- 230000007774 longterm Effects 0.000 description 1
- 238000013507 mapping Methods 0.000 description 1
- 238000012986 modification Methods 0.000 description 1
- 230000004048 modification Effects 0.000 description 1
- 238000012544 monitoring process Methods 0.000 description 1
- 230000004044 response Effects 0.000 description 1
- 230000004905 short-term response Effects 0.000 description 1
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对应的旋转矩阵分别为
步骤3.1.2：将多旋翼无人机的三次旋转表示为
其中，三次旋转为：多旋翼无人机从初始位置开始，绕导航坐标系的zn轴旋转角度ψk，再绕旋转后的坐标系的y'n轴旋转角度θk，再绕旋转后坐标系的x”n轴旋转角度γk；为导航坐标系下的三轴角速度；
步骤3.1.3：由公式(4)得到欧拉角方程为：
步骤3.1.4：对式(5)进行一阶线性离散化，得到：
步骤3.2：利用低通滤波后的三轴加速度数据和三轴磁力值数据解算多旋翼无人机的姿态：
步骤3.2.1：计算导航坐标系到机体坐标系的坐标转换矩阵为
从而机体坐标系到导航坐标系的坐标转换矩阵为
步骤3.2.2：机体坐标系中的三轴加速度Abk＝[axk,ayk,azk]T转换到导航坐标系中为Ank＝[0,0,g]T，计算得到
由于加速度信息与偏航角ψk无关，令ψk＝0，得到
其中，g为重力加速度；
步骤3.2.3：机体坐标系中的三轴磁力值Mbk＝[mxk,myk,mzk]T转换到导航坐标系中为Mnk＝[m'xk,m'yk,m'zk]T：
计算得到地磁场水平分量为
计算得出偏航角为
步骤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；对非线性的f(X1,k-1)用扩展卡尔曼滤波进行线性化，得到
从而多旋翼无人机姿态的状态方程可以表示为
步骤3.3.2：估计第k时刻姿态的状态变量为
步骤3.3.3：计算第k时刻姿态的均方误差为
步骤3.3.4：计算第k时刻姿态的滤波增益为
步骤3.3.5：用第k时刻姿态的观测量对第k时刻姿态的估计量进行修正，得到第k时刻姿态的最优估计值为
将作为多旋翼无人机在第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：
其中，a为WGS-84坐标系中的长轴半径，e为WGS-84坐标系的固定参数；
步骤4.1.2：将ECEF坐标系中的位置数据[xEk,yEk,zEk]T转换为ENU坐标系中的位置数据[xk,yk,zk]T：
其中，[xE0,yE0,zE0]T为导航坐标系的原点在ECEF坐标系中的坐标表示：[λ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； 为第k时刻多旋翼无人机在导航坐标系中的水平加速度，为中的前两项，中的γk、θk、ψk由式(23)计算得出；Γ为噪声驱动矩阵，Γ＝B，W2k、V2k均为2×1维系统噪声向量，W2k的方差统计特性完全由uk的统计特性决定，Φ、B、H均为常数矩阵，
步骤4.2.2：估计第k时刻水平位置的状态变量为
步骤4.2.3：计算第k时刻水平位置的均方误差为
其中，P2,k/k-1不随每次滤波迭代，P均为常数矩阵，
步骤4.2.4：计算第k时刻水平位置的滤波增益为
其中，为常数矩阵，
步骤4.2.5：用第k时刻水平位置的观测量对第k时刻水平位置的估计量进行修正，得到第k时刻的最优估计值为
将为多旋翼无人机在第k时刻的最终水平位置和水平速度；
步骤4.3：利用距地面高度数据解算多旋翼无人机的垂直位置：将距地面高度hvk作为多旋翼无人机在第k时刻的的垂直位置Szk。
所述步骤2中，低通滤波的方法为算术平均滤波法，将三轴加速度的m个连续采样值相加后取算术平均值作为低通滤波后的值：
所述步骤3.2.3中，对式(14)进行修正得到：
本发明的有益效果为：
本发明通过不同数据的融合，得到多旋翼无人机的最优姿态和位置：利用多旋翼无人机的三轴角速度数据和低通滤波后的三轴加速度数据、三轴磁力值数据分别解算多旋翼无人机的姿态，然后对解算出的姿态进行扩展卡尔曼滤波融合，得到多旋翼无人机的最优姿态；并利用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个连续采样值相加后取算术平均值作为低通滤波后的值：对多旋翼无人机的y轴加速度数据进行低通滤波的效果图如图2所示。其中，散点是未处理的数据，线条是处理后的数据。可以看出，经过低通滤波后得到的加速度数据曲线比较平滑。
步骤3：解算多旋翼无人机的姿态：
导航坐标系选的是ENU即东-北-天坐标系，其x轴为正东方向、y轴为正北方向、z轴则与重力方向相反向上。无人机的偏航角ψk、翻滚角θk、俯仰角γk都是以导航坐标系为参考系。
步骤3.1：利用三轴角速度数据解算多旋翼无人机的姿态：
步骤3.1.1：计算多旋翼无人机的偏航角ψk、翻滚角θk、俯仰角γk对应的旋转矩阵分别为
步骤3.1.2：将多旋翼无人机的三次旋转表示为
其中，如图3所示，三次旋转为：多旋翼无人机从初始位置开始，绕导航坐标系的zn轴旋转角度ψk，再绕旋转后的坐标系的y'n轴旋转角度θk，再绕旋转后坐标系的x'n'轴旋转角度γk；为导航坐标系下的三轴角速度；
步骤3.1.3：由公式(4)得到欧拉角方程为：
步骤3.1.4：对式(5)进行一阶线性离散化，得到：
步骤3.2：利用低通滤波后的三轴加速度数据和三轴磁力值数据解算多旋翼无人机的姿态：
步骤3.2.1：计算导航坐标系到机体坐标系的坐标转换矩阵为
从而机体坐标系到导航坐标系的坐标转换矩阵为
无人机在导航坐标系下处于任意姿态并保持静止时，加速度计测量输出三轴加速度Abk＝[axk,ayk,azk]T。由于重力存在，其加速度计输出值Abk转换到导航坐标系下后一定为Ank＝[0,0,g]T，从而：
步骤3.2.2：机体坐标系中的三轴加速度Abk＝[axk,ayk,azk]T转换到导航坐标系中为Ank＝[0,0,g]T，计算得到
由于加速度信息与偏航角ψk无关，令ψk＝0，得到
其中，g为重力加速度；
步骤3.2.3：机体坐标系中的三轴磁力值Mbk＝[mxk,myk,mzk]T转换到导航坐标系中为Mnk＝[m'xk,m'yk,m'zk]T：
计算得到地磁场水平分量为
计算得出偏航角为
所述步骤3.2.3中，对式(14)进行修正得到：
步骤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；对非线性的f(X1,k-1)用扩展卡尔曼滤波进行线性化，得到
从而多旋翼无人机姿态的状态方程可以表示为
其中，无人机姿态信息融合系统为非线性系统，式(16)、式(17)为其离散时间姿态状态模型。
步骤3.3.2：估计第k时刻姿态的状态变量为
步骤3.3.3：计算第k时刻姿态的均方误差为
其中，P1,k/k-1是经过式(20)后所产生的方差；
步骤3.3.4：计算第k时刻姿态的滤波增益为
其中，滤波增益v1k代表通过加速度计和磁力计所得到的观测值Z1k在融合结果中所占的权重。
步骤3.3.5：用第k时刻姿态的观测量对第k时刻姿态的估计量进行修正，得到第k时刻姿态的最优估计值为
将作为多旋翼无人机在第k时刻的最终姿态；
步骤3.3.6：更新均方误差为：
P1k＝[I-v1kHk]P1,k/k-1 (24)
其中，I为单位矩阵；P1k是经过式(23)后产生的新方差，为k+1时刻所用。本实施例中，P1k的初始值为Qk、Rk均为常数矩阵，
步骤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
扁率：
长半轴a、短半轴b和扁率f之间的关系：
步骤4.1.1：将WGS-84坐标系中的gps位置数据[λk,φk,hk]T转换为ECEF坐标系中的位置数据[xEk,yEk,zEk]T：
其中，a为WGS-84坐标系中的长轴半径，e为WGS-84坐标系的固定参数；
步骤4.1.2：将ECEF坐标系中的位置数据[xEk,yEk,zEk]T转换为ENU坐标系中的位置数据[xk,yk,zk]T：
其中，[xE0,yE0,zE0]T为导航坐标系的原点在ECEF坐标系中的坐标表示：[λ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； 为第k时刻多旋翼无人机在导航坐标系中的水平加速度，为中的前两项，中的γk、θk、ψk由式(23)计算得出；Γ为噪声驱动矩阵，Γ＝B，W2k、V2k均为2×1维系统噪声向量，W2k的方差统计特性完全由uk的统计特性决定，Φ、B、H均为常数矩阵，
其中，无人机水平位置信息融合系统为非线性系统，式(27)、式(28)为其离散时间水平位置状态模型。
步骤4.2.2：估计第k时刻水平位置的状态变量为
步骤4.2.3：计算第k时刻水平位置的均方误差为
其中，P2,k/k-1不随每次滤波迭代，P均为常数矩阵，
其中，gps的长期静态性能较好而短期响应速度比较差，卡尔曼滤波最终达到的效果是让每次融合的结果方差最小。由于gps响应慢，其解算出来的值易较长时间保持不变，所以在融合过程中更容易占更大的比例，显然这样的结果不是我们想要的。解决方法是让状态一步预测均方误差矩阵P2,k/k-1不随每次滤波迭代，使其在滤波开始就是确定的常数矩阵P，从而滤波增益也会是一个常数矩阵。Φ、Γ、和H都是常数矩阵，如此便不会使得由状态一步预测得到的结果所起的作用越来越小。
步骤4.2.4：计算第k时刻水平位置的滤波增益为
其中，为常数矩阵，
本实施例中，
步骤4.2.5：用第k时刻水平位置的观测量对第k时刻水平位置的估计量进行修正，得到第k时刻的最优估计值为
将为多旋翼无人机在第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：进行数据预处理：对三轴加速度Ak进行低通滤波处理；
步骤3：解算多旋翼无人机的姿态：
步骤3.1：利用三轴角速度数据解算多旋翼无人机的姿态：
步骤3.1.1：计算多旋翼无人机的偏航角ψk、翻滚角θk、俯仰角γk对应的旋转矩阵分别为
步骤3.1.2：将多旋翼无人机的三次旋转表示为
其中，三次旋转为：多旋翼无人机从初始位置开始，绕导航坐标系的zn轴旋转角度ψk，再绕旋转后的坐标系的y'n轴旋转角度θk，再绕旋转后坐标系的x″n轴旋转角度γk；为导航坐标系下的三轴角速度；
步骤3.1.3：由公式(4)得到欧拉角方程为：
步骤3.1.4：对式(5)进行一阶线性离散化，得到：
步骤3.2：利用低通滤波后的三轴加速度数据和三轴磁力值数据解算多旋翼无人机的姿态：
步骤3.2.1：计算导航坐标系到机体坐标系的坐标转换矩阵为
从而机体坐标系到导航坐标系的坐标转换矩阵为
步骤3.2.2：机体坐标系中的三轴加速度Abk＝[axk,ayk,azk]T转换到导航坐标系中为Ank＝[0,0,g]T，计算得到
由于加速度信息与偏航角ψk无关，令ψk＝0，得到
其中，g为重力加速度；
步骤3.2.3：机体坐标系中的三轴磁力值Mbk＝[mxk,myk,mzk]T转换到导航坐标系中为Mnk＝[m'xk,m'yk,m'zk]T：
计算得到地磁场水平分量为
计算得出偏航角为
步骤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；对非线性的f(X1,k-1)用扩展卡尔曼滤波进行线性化，得到
从而多旋翼无人机姿态的状态方程可以表示为
步骤3.3.2：估计第k时刻姿态的状态变量为
步骤3.3.3：计算第k时刻姿态的均方误差为
步骤3.3.4：计算第k时刻姿态的滤波增益为
步骤3.3.5：用第k时刻姿态的观测量对第k时刻姿态的估计量进行修正，得到第k时刻姿态的最优估计值为
将作为多旋翼无人机在第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：
其中，a为WGS-84坐标系中的长轴半径，e为WGS-84坐标系的固定参数；
步骤4.1.2：将ECEF坐标系中的位置数据[xEk,yEk,zEk]T转换为ENU坐标系中的位置数据[xk,yk,zk]T：
其中，[xE0,yE0,zE0]T为导航坐标系的原点在ECEF坐标系中的坐标表示：[λ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； 为第k时刻多旋翼无人机在导航坐标系中的水平加速度，为中的前两项，中的γk、θk、ψk由式(23)计算得出；Γ为噪声驱动矩阵，Γ＝B，W2k、V2k均为2×1维系统噪声向量，W2k的方差统计特性完全由uk的统计特性决定，Φ、B、H均为常数矩阵，
步骤4.2.2：估计第k时刻水平位置的状态变量为
步骤4.2.3：计算第k时刻水平位置的均方误差为
其中，P2,k/k-1不随每次滤波迭代，P均为常数矩阵，
步骤4.2.4：计算第k时刻水平位置的滤波增益为
其中，为常数矩阵，
步骤4.2.5：用第k时刻水平位置的观测量对第k时刻水平位置的估计量进行修正，得到第k时刻的最优估计值为
将为多旋翼无人机在第k时刻的最终水平位置和水平速度；
步骤4.3：利用距地面高度数据解算多旋翼无人机的垂直位置：将距地面高度hvk作为多旋翼无人机在第k时刻的的垂直位置Szk。
2.根据权利要求1所述的多旋翼无人机的姿态及位置确定方法，其特征在于，所述步骤2中，低通滤波的方法为算术平均滤波法，将三轴加速度的m个连续采样值相加后取算术平均值作为低通滤波后的值：
3.根据权利要求1所述的多旋翼无人机的姿态及位置确定方法，其特征在于，所述步骤3.2.3中，对式(14)进行修正得到：
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 true CN110081878A (zh) | 2019-08-02 |
| CN110081878B 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) |
Cited By (9)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN110598370A (zh) * | 2019-10-18 | 2019-12-20 | 太原理工大学 | 基于sip和ekf融合的多旋翼无人机鲁棒姿态估计 |
| CN110955258A (zh) * | 2019-11-28 | 2020-04-03 | 深圳蚁石科技有限公司 | 四轴飞行器的控制方法、装置、控制器和存储介质 |
| CN112486215A (zh) * | 2020-12-02 | 2021-03-12 | 北京特种机械研究所 | 一种辅助飞行器装填的测控系统 |
| CN112577484A (zh) * | 2019-09-29 | 2021-03-30 | 北京信息科技大学 | 一种小型气象探测设备应用的遥测装置 |
| CN112649001A (zh) * | 2020-12-01 | 2021-04-13 | 中国航空工业集团公司沈阳飞机设计研究所 | 一种小型无人机姿态与位置解算方法 |
| CN113237478A (zh) * | 2021-05-27 | 2021-08-10 | 哈尔滨工业大学 | 一种无人机的姿态和位置估计方法及无人机 |
| CN113311456A (zh) * | 2021-05-18 | 2021-08-27 | 武汉大学 | 一种基于卡尔曼滤波的qar数据噪声处理方法 |
| CN115826602A (zh) * | 2022-11-17 | 2023-03-21 | 众芯汉创（北京）科技有限公司 | 一种基于无人机的飞行动态精准定位管理系统和方法 |
| CN117389322A (zh) * | 2023-12-08 | 2024-01-12 | 天津天羿科技有限公司 | 无人机控制方法 |
Citations (2)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN106643737A (zh) * | 2017-02-07 | 2017-05-10 | 大连大学 | 风力干扰环境下四旋翼飞行器姿态解算方法 |
| CN108645404A (zh) * | 2018-03-30 | 2018-10-12 | 西安建筑科技大学 | 一种小型多旋翼无人机姿态解算方法 |
-
2019
- 2019-05-17 CN CN201910411892.7A patent/CN110081878B/zh not_active Expired - Fee Related
Patent Citations (2)
| 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)
| Title |
|---|
| 王龙等: "改进扩展卡尔曼滤波的四旋翼姿态估计算法", 《计算机应用》 * |
Cited By (14)
| 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融合的多旋翼无人机鲁棒姿态估计 |
| CN110598370A (zh) * | 2019-10-18 | 2019-12-20 | 太原理工大学 | 基于sip和ekf融合的多旋翼无人机鲁棒姿态估计 |
| CN110955258A (zh) * | 2019-11-28 | 2020-04-03 | 深圳蚁石科技有限公司 | 四轴飞行器的控制方法、装置、控制器和存储介质 |
| CN112649001A (zh) * | 2020-12-01 | 2021-04-13 | 中国航空工业集团公司沈阳飞机设计研究所 | 一种小型无人机姿态与位置解算方法 |
| CN112649001B (zh) * | 2020-12-01 | 2023-08-22 | 中国航空工业集团公司沈阳飞机设计研究所 | 一种小型无人机姿态与位置解算方法 |
| CN112486215A (zh) * | 2020-12-02 | 2021-03-12 | 北京特种机械研究所 | 一种辅助飞行器装填的测控系统 |
| CN113311456A (zh) * | 2021-05-18 | 2021-08-27 | 武汉大学 | 一种基于卡尔曼滤波的qar数据噪声处理方法 |
| CN113237478A (zh) * | 2021-05-27 | 2021-08-10 | 哈尔滨工业大学 | 一种无人机的姿态和位置估计方法及无人机 |
| CN113237478B (zh) * | 2021-05-27 | 2022-10-14 | 哈尔滨工业大学 | 一种无人机的姿态和位置估计方法及无人机 |
| CN115826602A (zh) * | 2022-11-17 | 2023-03-21 | 众芯汉创（北京）科技有限公司 | 一种基于无人机的飞行动态精准定位管理系统和方法 |
| CN115826602B (zh) * | 2022-11-17 | 2023-11-17 | 众芯汉创（北京）科技有限公司 | 一种基于无人机的飞行动态精准定位管理系统和方法 |
| CN117389322A (zh) * | 2023-12-08 | 2024-01-12 | 天津天羿科技有限公司 | 无人机控制方法 |
| CN117389322B (zh) * | 2023-12-08 | 2024-03-01 | 天津天羿科技有限公司 | 无人机控制方法 |
Also Published As
| Publication number | Publication date |
|---|---|
| CN110081878B (zh) | 2023-01-24 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN110081878B (zh) | 一种多旋翼无人机的姿态及位置确定方法 | |
| CN106959110B (zh) | 一种云台姿态检测方法及装置 | |
| CN106643737B (zh) | 风力干扰环境下四旋翼飞行器姿态解算方法 | |
| CN112630813B (zh) | 基于捷联惯导和北斗卫星导航系统的无人机姿态测量方法 | |
| CN107247459B (zh) | 抗干扰飞行控制方法及装置 | |
| CN106767785B (zh) | 一种双回路无人机的导航方法及装置 | |
| CN112066985B (zh) | 一种组合导航系统初始化方法、装置、介质及电子设备 | |
| CN106403940B (zh) | 一种抗大气参数漂移的无人机飞行导航系统高度信息融合方法 | |
| CN107063262A (zh) | 一种用于无人机姿态解算的互补滤波方法 | |
| CN106249744B (zh) | 一种基于二级互补滤波的小型旋翼飞行器高度控制方法 | |
| CN110793515A (zh) | 一种基于单天线gps和imu的大机动条件下无人机姿态估计方法 | |
| CN106352897B (zh) | 一种基于单目视觉传感器的硅mems陀螺误差估计与校正方法 | |
| CN108444468B (zh) | 一种融合下视视觉与惯导信息的定向罗盘 | |
| US20200141969A1 (en) | System and method for determining airspeed | |
| CN103712598A (zh) | 一种小型无人机姿态确定系统与确定方法 | |
| CN110873563B (zh) | 一种云台姿态估计方法及装置 | |
| CN111238469A (zh) | 一种基于惯性/数据链的无人机编队相对导航方法 | |
| CN109521785A (zh) | 一种随身拍智能旋翼飞行器系统 | |
| Shiau et al. | Unscented kalman filtering for attitude determination using mems sensors | |
| Ascorti | An application of the extended Kalman filter to the attitude control of a quadrotor | |
| CN108759814A (zh) | 一种四旋翼飞行器横滚轴角速度和俯仰轴角速度估计方法 | |
| CN113465596B (zh) | 一种基于多传感器融合的四旋翼无人机定位方法 | |
| García et al. | Analysis of sensor data and estimation output with configurable UAV platforms | |
| Emran et al. | A cascaded approach for quadrotor's attitude estimation | |
| Li et al. | Quaternion-based robust extended Kalman filter for attitude estimation of micro quadrotors using low-cost MEMS |
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 |
Granted publication date: 20230124 |
|
| CF01 | Termination of patent right due to non-payment of annual fee |