CN111854691B - 基于多传感器融合的高度解算方法、高度解算装置及无人机 - Google Patents

基于多传感器融合的高度解算方法、高度解算装置及无人机 Download PDF

Info

Publication number
CN111854691B
CN111854691B CN202010682781.2A CN202010682781A CN111854691B CN 111854691 B CN111854691 B CN 111854691B CN 202010682781 A CN202010682781 A CN 202010682781A CN 111854691 B CN111854691 B CN 111854691B
Authority
CN
China
Prior art keywords
height
data
gps
sensor
baro
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.)
Active
Application number
CN202010682781.2A
Other languages
English (en)
Other versions
CN111854691A (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.)
Wuhan Gaode Aircraft Technology Co ltd
Original Assignee
Wuhan Gaode Aircraft Technology Co ltd
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 Wuhan Gaode Aircraft Technology Co ltd filed Critical Wuhan Gaode Aircraft Technology Co ltd
Priority to CN202010682781.2A priority Critical patent/CN111854691B/zh
Publication of CN111854691A publication Critical patent/CN111854691A/zh
Application granted granted Critical
Publication of CN111854691B publication Critical patent/CN111854691B/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
    • G01C5/00—Measuring height; Measuring distances transverse to line of sight; Levelling between separated points; Surveyors' levels
    • G01C5/06—Measuring height; Measuring distances transverse to line of sight; Levelling between separated points; Surveyors' levels by using barometric means
    • B—PERFORMING OPERATIONS; TRANSPORTING
    • B64—AIRCRAFT; AVIATION; COSMONAUTICS
    • B64C—AEROPLANES; HELICOPTERS
    • B64C39/00—Aircraft not otherwise provided for
    • B64C39/02—Aircraft not otherwise provided for characterised by special use
    • 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
    • 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
    • G01S11/00—Systems for determining distance or velocity not using reflection or reradiation
    • G01S11/14—Systems for determining distance or velocity not using reflection or reradiation using ultrasonic, sonic, or infrasonic waves
    • 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
    • G01S15/00—Systems using the reflection or reradiation of acoustic waves, e.g. sonar systems
    • G01S15/02—Systems using the reflection or reradiation of acoustic waves, e.g. sonar systems using reflection of acoustic waves
    • G01S15/06—Systems determining the position data of a target
    • G01S15/08—Systems for measuring distance only
    • 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
    • G01S15/00—Systems using the reflection or reradiation of acoustic waves, e.g. sonar systems
    • G01S15/86—Combinations of sonar systems with lidar systems; Combinations of sonar systems with systems not using wave reflection
    • 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
    • B—PERFORMING OPERATIONS; TRANSPORTING
    • B64—AIRCRAFT; AVIATION; COSMONAUTICS
    • B64U—UNMANNED AERIAL VEHICLES [UAV]; EQUIPMENT THEREFOR
    • B64U2101/00—UAVs specially adapted for particular uses or applications

Abstract

本发明公开了一种基于多传感器融合的高度解算方法、高度解算装置及无人机，其包括如下步骤：S1、设置高度传感器，且通过每一高度传感器对应获取移动设备的有效高度数据；S2、将各高度传感器获取的高度数据进行高度对齐；S3、根据移动设备的运动速度计算气压计的置信度；以及S4、将气压高度、超声高度和GPS高度与加速度计数据进行融合，以获取移动设备当前的高度信息。本发明将气压计、GPS、超声测距仪等多种传感器数据同时与加速度计数据进行融合，利用不同传感器的优势弥补传感器自身的不足，从而得到更为精确可靠的高度信息和速度信息。

Description

基于多传感器融合的高度解算方法、高度解算装置及无人机
技术领域
本发明涉及无人机领域，具体为一种基于多传感器融合的高度解算方法、高度解算装置及无人机。
背景技术
目前市面上采用无人机进行测量高度时，其普遍使用的传感器有加速度计、气压计、GPS或者超声测距仪等，通常会从气压计、GPS、超声测距仪等传感器中择优选择一种传感器与加速度计进行数据融合计算，以获取高度信息。
但由于上述融合计算方式只采用一种传感器数据，但各传感器均具有其各自的短板，因此融合单一传感器数据不能有效弥补各传感器的不足之处，进一步会影响高度计算的精度，且计算结果不稳定。
发明内容
针对现有技术的不足，本发明提供了一种基于多传感器融合的高度解算方法、高度解算装置及无人机，其将气压计、GPS、超声测距仪等多种传感器数据同时与加速度计数据进行融合，利用不同传感器的优势弥补传感器自身的不足，从而得到更为精确可靠的高度信息和速度信息。
为实现上述目的，本发明提供如下技术方案：
一方面，提供了一种基于多传感器融合的高度解算方法，其包括如下步骤：
S1、设置高度传感器，且通过每一高度传感器对应获取移动设备的有效高度数据，所述高度传感器包括：超声传感器、气压计以及GPS；所述有效高度数据包括超声高度、气压高度、GPS高度；
S2、将各高度传感器获取的高度数据进行高度对齐；
S3、根据移动设备的运动速度计算气压计的置信度；
以及S4、将气压高度、超声高度和GPS高度与加速度计数据进行融合，以获取移动设备当前的高度信息。
优选的，所述步骤S2包括如下步骤：
根据公式(1-1)-(1-2)分别计算气压高度hbaro和超声高度hultra之间的偏移量Δh1，以及气压高度hbaro和GPS高度hGPS之间的偏移量Δh2；
Δh1＝hbaro-hultra (1-1)
Δh2＝hbaro-hGPS (1-2)；
根据公式(2-1)-(2-3)分别计算对齐后的GPS高度xGPS、对齐后的超声高度xultra以及对齐后的气压高度xbaro；
xultra＝hultra+Δh1 (2-1)
xGPS＝hGPS+Δh2 (2-2)
xbaro＝hbaro (2-3)；
对齐后的高度数据用于后续数据融合计算。
优选的，所述步骤S2还包括如下步骤：当某一传感器数据从无效标记切换为有效标记时也需要进行高度对齐，此时设该传感器当前获得的高度数据为h，融合高度为
Figure BDA0002586454790000021
则偏移量
Figure BDA0002586454790000022
对齐后的高度x＝h-Δx，然后再将对齐后的高度用于后续融合计算。
优选的，所述步骤S3包括：
根据公式(3-1)-(3-2)计算气压计的置信度δbaro：
Figure BDA0002586454790000023
Figure BDA0002586454790000024
其中，|v|为移动设备当前运动速度；vx、vy、vz分别为XYZ三轴对应的运动速度；vmax为移动设备最大运动速度；k为系数，取值范围为0-100；
同时，还根据公式(4-1)-(4-2)计算GPS垂直速度置信度εGPS和/或GPS高度置信度δGPS：
Figure BDA0002586454790000025
Figure BDA0002586454790000031
其中，vdop为垂直精度因子，取值范围为0.5-99.9；vacc为垂直精度，单位mm；sacc为垂直速度精度因子，单位mm/s。
优选的，所述步骤S4包括：
S41、根据公式(5-1)对加速度计数据进行积分，以得到垂直方向预测速度和预测高度；
Figure BDA0002586454790000032
其中，向量
Figure BDA0002586454790000033
和
Figure BDA0002586454790000034
分别为k时刻的预测高度和预测速度，
Figure BDA0002586454790000035
为k时刻预测加速度修正量；向量
Figure BDA0002586454790000036
xk-1、vk-1分别为k-1时刻的融合高度和融合速度；矩阵
Figure BDA0002586454790000037
矩阵
Figure BDA0002586454790000038
uk为垂直方向的加速度数据；abias为k-1时刻的加速度修正量；Δt为算法执行周期；
计算协方差矩阵
Figure BDA0002586454790000039
其中
Figure BDA00025864547900000310
为过程噪声矩阵；q为对预测模型误差的估计值，为可调参数，其取决于传感器自身的特性(不同型号的传感器测量误差不同)，预测模型越精准则q的取值越小，通常通过试凑的方式得出；
S42、判断气压计数据是否有效，若气压计数据有效，则根据公式(5-2)进行数据融合计算；若气压计数据无效，则直接进入步骤S43；
Figure BDA00025864547900000311
其中，
Figure BDA00025864547900000312
为卡尔曼增益；Hbaro＝[1 0 0]；
Figure BDA0002586454790000041
为观测噪声矩阵；更新协方差矩阵
Figure BDA0002586454790000042
r是对传感器数据误差的估计；
Figure BDA0002586454790000043
为当前时刻的高度信息；
S43、判断超声传感器数据是否有效，若超声传感器数据有效，则根据公式(5-3)进行数据融合计算；若超声传感器数据无效，则直接进入步骤S44；
Figure BDA0002586454790000044
其中，
Figure BDA0002586454790000045
为卡尔曼增益；观测噪声矩阵
Figure BDA0002586454790000046
为观测噪声矩阵；Hultra＝[1 0 0]；更新协方差矩阵
Figure BDA0002586454790000047
S44、判断GPS数据是否有效，若GPS数据有效，则根据公式(5-4)进行数据融合计算；若GPS数据无效，则直接返回步骤S1，进行下一步的高度解算；
Figure BDA0002586454790000048
其中，
Figure BDA0002586454790000049
为卡尔曼增益；
Figure BDA00025864547900000410
为观测噪声矩阵；
Figure BDA00025864547900000411
为当前时刻的高度信息；需要更新协方差矩阵
Figure BDA00025864547900000412
返回步骤S1进行下一步的高度解算。
优选的，所述移动设备包括无人机。
还提供一种能实现上述高度解算方法的高度解算装置，其包括：
至少2个高度传感器，且每一高度传感器均获取与其对应的传感器数据，所述高度传感器包括：超声传感器、气压计以及GPS，则对应的传感器数据为超声传感器数据、气压计数据和GPS数据；
有效性判断单元，其连接每一高度传感器，且当判断出超声传感器数据是在正常状况下获取到的、气压计数据是在正常状况下获取到的以及GPS数据是GPS搜星数量大于或等于设定阈值时获取到的时，确定此时的超声传感器数据、气压计数据和GPS数据为有效数据；
高度数据获取单元，其用于获取超声传感器数据、气压计数据和GPS数据为有效数据时，其对应从超声传感器、气压计以及GPS读取到的超声高度hultra、气压高度hbaro以及GPS高度hGPS；
高度对齐单元，其连接所述高度数据获取单元，用于根据公式(1-1)-(1-2)分别计算气压高度hbaro和超声高度hultra之间的偏移量Δh1，以及气压高度hbaro和GPS高度hGPS之间的偏移量Δh2，再根据公式(2-1)-(2-3)分别计算对齐后的GPS高度xGPS、对齐后的超声高度xultra以及对齐后的气压高度xbaro；
气压计置信度计算单元，其用于根据公式(3-1)-(3-2)计算气压计的置信度δbaro；
GPS垂直速度置信度/高度置信度计算单元，其用于根据公式(4-1)-(4-2)计算GPS垂直速度置信度εGPS和/或GPS高度置信度δGPS；
融合计算单元，其用于将气压高度hbaro、超声高度hultra和GPS高度hGPS与加速度计数据进行融合计算。
还提供一种无人机，其包括上述高度解算装置。
与现有技术相比，本发明具备以下有益效果：
本发明将气压计、GPS、超声测距仪等多种传感器数据同时与加速度计数据进行融合，且融合时自动剔除无效的传感器数据，通过传感器数据置信度决定传感器数据融合的权重，以利用不同传感器的优势弥补传感器自身的不足，保证最终获得高度信息的精确度，从而得到更为精确可靠的高度、速度信息。
附图说明
图1为本发明中基于多传感器融合的高度解算方法的步骤流程图。
具体实施方式
下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。
实施例一：
如图1所示，本发明提供了一种基于多传感器融合的高度解算方法，其包括如下步骤：
S1、设置至少2个高度传感器，且通过每一高度传感器对应获取移动设备的有效高度数据，本实施例中，所述高度传感器包括：超声传感器、气压计以及GPS，所述移动设备为无人机；
具体的，通过每一高度传感器对应获取移动设备的有效高度数据包括：
判断超声传感器数据是否有效，若有效，则读取超声传感器测得的超声高度hultra，该超声高度hultra即为超声传感器获取的有效高度数据；其中，超声传感器数据有效是指超声传感器在正常状况下获取到的数据是有效的，正常状况包括：超声波的发射功率、频率等均正常，探测范围满足预设条件，且数据传输无异常等，此时记为超声有效标志，且值为1；异常情况下获得的超声传感器数据为无效数据，此时记为超声无效标志，且值为0；
判断气压计数据是否有效，若有效，则读取气压计测得的气压高度hbaro，该气压高度hbaro即为气压计获取的有效高度数据；其中，气压计数据有效是指气压计在正常状况下获取到的数据是有效的，正常状况包括：气压计结构无损伤(如水银柱无破损等)，数据传输无异常等，此时记为气压有效标志，且值为1；异常情况下获得的气压计数据为无效数据，此时记为气压无效标志，且值为0；
判断GPS数据是否有效性，若有效，则读取GPS确定的GPS高度hGPS，该GPS高度hGPS即为定位装置获取的有效高度数据；其中，GPS数据有效是指GPS搜星数量大于或等于设定阈值(如搜星数量≥5等，具体阈值根据实际需要确定，在此不做限定)时获取到的数据是有效的，此时记为GPS有效标志，且值为1；搜星数量小于设定阈值时获得的数据为无效数据，此时记为GPS无效标志，且值为0；
需要说明的是，上述“正常状况”、“异常情况”均根据本领域技术人员具备的普通知识判定即可，且各传感器所获取的无效数据均不用于后续步骤。
S2、由于超声高度hultra是超声传感器到地面的距离，气压高度hbaro是移动设备机体相对地面的高度(在地面上为0)，而GPS高度hGPS是海拔高度，其各自获得的基准不一致，因此需要将各高度传感器获取的高度数据进行高度对齐，便与后续融合计算。
具体的，所述步骤S2包括如下步骤：
根据公式(1-1)-(1-2)分别计算气压高度hbaro和超声高度hultra之间的偏移量Δh1，以及气压高度hbaro和GPS高度hGPS之间的偏移量Δh2；
Δh1＝hbaro-hultra (1-1)
Δh2＝hbaro-hGPS (1-2)；
根据公式(2-1)-(2-3)分别计算对齐后的GPS高度xGPS、对齐后的超声高度xultra以及对齐后的气压高度xbaro；
xultra＝hultra+Δh1 (2-1)
xGPS＝hGPS+Δh2 (2-2)
xbaro＝hbaro (2-3)；
此时xultra＝xbaro＝xGPS＝hbaro，三个对齐后的高度数据即处于同一基准面，可用于后续数据融合计算；
另外，当某一传感器数据从无效标记切换为有效标记(如超声传感器的超声测量范围恢复正常，则此时超声传感器数据从超声无效标记切换为超声有效标记)时也需要进行高度对齐，此时设该传感器当前获得的高度数据为h，融合高度为
Figure BDA0002586454790000071
则偏移量
Figure BDA0002586454790000072
对齐后的高度x＝h-Δx，然后再将对齐后的高度用于后续融合计算；
S3、由于气压计受气流的影响较大，所以需要根据移动设备的运动速度计算气压计的置信度δbaro，具体的，根据公式(3-1)-(3-2)计算气压计的置信度δbaro：
Figure BDA0002586454790000073
Figure BDA0002586454790000074
其中，|v|为移动设备当前运动速度；vx、vy、vz分别为XYZ三轴对应的运动速度；vmax为移动设备最大运动速度；k为系数，取值范围为0-100；δbaro值越小表示气压计的数据可信度越高；
同时，还根据公式(4-1)-(4-2)计算GPS垂直速度置信度εGPS和/或GPS高度置信度δGPS：
Figure BDA0002586454790000081
Figure BDA0002586454790000082
其中，vdop为垂直精度因子，取值范围为0.5-99.9；vacc为垂直精度，单位mm；sacc为垂直速度精度因子，单位mm/s；vdop、vacc、sacc均可从GPS中读取，且δGPS和εGPS的值越小，则表示GPS数据的置信度越高。
以及S4、将气压高度hbaro、超声高度hultra和GPS高度hGPS与加速度计数据通过卡尔曼滤波算法等进行融合，以获取移动设备当前的高度信息；
具体的，步骤S4包括如下步骤：
S41、根据公式(5-1)对加速度计数据进行积分，以得到垂直方向预测速度和预测高度；
Figure BDA0002586454790000083
其中，向量
Figure BDA0002586454790000084
和
Figure BDA0002586454790000085
分别为k时刻的预测高度和预测速度，
Figure BDA0002586454790000086
为k时刻预测加速度修正量；向量
Figure BDA0002586454790000087
xk-1、vk-1分别为k-1时刻的融合高度和融合速度；矩阵
Figure BDA0002586454790000088
矩阵
Figure BDA0002586454790000089
uk为垂直方向的加速度数据；abias为k-1时刻的加速度修正量；Δt为算法执行周期，即当前时刻k和前一时刻k-1之间的间隔时间，例如算法的运行频率为1000HZ，则Δt为0.001s；
计算协方差矩阵
Figure BDA0002586454790000091
其中
Figure BDA0002586454790000092
为过程噪声矩阵；q为对预测模型误差的估计值，为可调参数，其取决于传感器自身的特性(不同型号的传感器测量误差不同)，预测模型越精准则q的取值越小，通常通过试凑的方式得出；AT为矩阵A的转置矩阵；
S42、判断气压计数据是否有效(即判断数据标记值为1还是0)，若气压计数据有效(即数据标记值为1)，则根据公式(5-2)进行数据融合计算；若气压计数据无效(即数据标记值为0)，则直接进入步骤S43；
Figure BDA0002586454790000093
其中，
Figure BDA0002586454790000094
为卡尔曼增益；Hbaro＝[1 0 0]；
Figure BDA0002586454790000095
为观测噪声矩阵；更新协方差矩阵
Figure BDA0002586454790000096
r是对传感器(即超声传感器、气压计以及GPS)数据误差的估计，该值的大小取决于传感器自身的特性(不同型号的传感器测量误差不同)，传感器数据越精准r的取值越小，通常通过试凑的方式得出；
Figure BDA0002586454790000097
为当前时刻的高度信息。
S43、判断超声传感器数据是否有效(即判断数据标记值为1还是0)，若超声传感器数据有效(即数据标记值为1)，则根据公式(5-3)进行数据融合计算；若超声传感器数据无效(即数据标记值为0)，则直接进入步骤S44；
Figure BDA0002586454790000098
其中，
Figure BDA0002586454790000099
为卡尔曼增益；观测噪声矩阵
Figure BDA00025864547900000910
为观测噪声矩阵；Hultra＝[1 0 0]；更新协方差矩阵
Figure BDA00025864547900000911
S44、判断GPS数据是否有效(即判断数据标记值为1还是0)，若GPS数据有效(即数据标记值为1)，则根据公式(5-4)进行数据融合计算；若GPS数据无效(即数据标记值为0)，则直接返回步骤S1；
Figure BDA0002586454790000101
其中，
Figure BDA0002586454790000102
为卡尔曼增益；
Figure BDA0002586454790000103
为观测噪声矩阵；
Figure BDA0002586454790000104
为当前时刻的高度信息；因高度测量过程是持续性的过程，且无人机的高度不断变化，只要无人机不断电就需要实时获取当前的高度信息，所以程序执行时需要不断循环计算当前的高度信息，因此，需要更新协方差矩阵
Figure BDA0002586454790000105
返回步骤S1进行下一步的高度解算。
实施例2：
本实施例提供了一种能实现实施例1中所述高度解算方法的高度解算装置，其包括：
至少2个高度传感器，且每一高度传感器均获取与其对应的传感器数据，本实施例中，所述高度传感器包括：超声传感器、气压计以及GPS，则对应的传感器数据即为超声传感器数据、气压计数据和GPS数据；
有效性判断单元，其连接每一高度传感器，且当判断出超声传感器数据是在正常状况下获取到的、气压计数据是在正常状况下获取到的以及GPS数据是GPS搜星数量大于或等于设定阈值时获取到的时，确定此时的超声传感器数据、气压计数据和GPS数据为有效数据；
高度数据获取单元，其用于获取超声传感器数据、气压计数据和GPS数据为有效数据时，其对应从超声传感器、气压计以及GPS读取到的超声高度hultra、气压高度hbaro以及GPS高度hGPS；
高度对齐单元，其连接所述高度数据获取单元，用于根据公式(1-1)-(1-2)分别计算气压高度hbaro和超声高度hultra之间的偏移量Δh1，以及气压高度hbaro和GPS高度hGPS之间的偏移量Δh2，再根据公式(2-1)-(2-3)分别计算对齐后的GPS高度xGPS、对齐后的超声高度xultra以及对齐后的气压高度xbaro；
气压计置信度计算单元，其用于根据公式(3-1)-(3-2)计算气压计的置信度δbaro；
GPS垂直速度置信度/高度置信度计算单元，其用于根据公式(4-1)-(4-2)计算GPS垂直速度置信度εGPS和/或GPS高度置信度δGPS；
融合计算单元，其用于将气压高度hbaro、超声高度hultra和GPS高度hGPS与加速度计数据通过卡尔曼滤波算法等进行融合计算，计算过程与步骤S4相同，在此不再赘述。
实施例3：
本实施例还提供了一种无人机，其包括实施例2中所述的高度解算装置。
综上所述，本发明将气压计、GPS、超声测距仪等多种传感器数据同时与加速度计数据进行融合，且融合时自动剔除无效的传感器数据，通过传感器数据置信度决定传感器数据融合的权重，以利用不同传感器的优势弥补传感器自身的不足，保证最终获得高度信息的精确度，从而得到更为精确可靠的高度、速度信息。
需要说明的是，上述实施例1-2中的技术特征可进行任意组合，且组合而成的技术方案均属于本申请的保护范围。在本文中，诸如术语“包括”、“包含”或者其任何其他变体意在涵盖非排他性的包含，从而使得包括一系列要素的过程、方法、物品或者设备不仅包括那些要素，而且还包括没有明确列出的其他要素，或者是还包括为这种过程、方法、物品或者设备所固有的要素。在没有更多限制的情况下，由语句“包括一个……”限定的要素，并不排除在包括所述要素的过程、方法、物品或者设备中还存在另外的相同要素。
尽管已经示出和描述了本发明的实施例，对于本领域的普通技术人员而言，可以理解在不脱离本发明的原理和精神的情况下可以对这些实施例进行多种变化、修改、替换和变型，本发明的范围由所附权利要求及其等同物限定。

Claims (5)

1.一种基于多传感器融合的高度解算方法，其特征在于，包括如下步骤：
S1、设置高度传感器，且通过每一高度传感器对应获取移动设备的有效高度数据，所述高度传感器包括：超声传感器、气压计以及GPS；所述有效高度数据包括超声高度、气压高度、GPS高度；
S2、将各高度传感器获取的高度数据进行高度对齐；
S3、根据移动设备的运动速度计算气压计的置信度；
以及S4、将气压高度、超声高度和GPS高度与加速度计数据进行融合，以获取移动设备当前的高度信息；
所述步骤S2包括如下步骤：
根据公式(1-1)-(1-2)分别计算气压高度hbaro和超声高度hultra之间的偏移量Δh1，以及气压高度hbaro和GPS高度hGPS之间的偏移量Δh2；
Δh1＝hbaro-hultra (1-1)
Δh2＝hbaro-hGPS (1-2)；
根据公式(2-1)-(2-3)分别计算对齐后的GPS高度xGPS、对齐后的超声高度xultra以及对齐后的气压高度xbaro；
xultra＝hultra+Δh1 (2-1)
xGPS＝hGPS+Δh2 (2-2)
xbaro＝hbaro (2-3)；
对齐后的高度数据用于后续数据融合计算；
所述步骤S2还包括如下步骤：当某一传感器数据从无效标记切换为有效标记时也需要进行高度对齐，此时设该传感器当前获得的高度数据为h，融合高度为
Figure FDA0003598408820000011
则偏移量
Figure FDA0003598408820000012
对齐后的高度x＝h+Δx，然后再将对齐后的高度用于后续融合计算；
所述步骤S3包括：
根据公式(3-1)-(3-2)计算气压计的置信度δbaro：
Figure FDA0003598408820000021
Figure FDA0003598408820000022
其中，|v|为移动设备当前运动速度；vx、vy、vz分别为XYZ三轴对应的运动速度；vmax为移动设备最大运动速度；k为系数，取值范围为0-100；
同时，还根据公式(4-1)-(4-2)计算GPS垂直速度置信度εGPS和/或GPS高度置信度δGPS：
Figure FDA0003598408820000023
Figure FDA0003598408820000024
其中，vdop为垂直精度因子，取值范围为0.5-99.9；vacc为垂直精度，单位mm；sacc为垂直速度精度因子，单位mm/s。
2.如权利要求1所述的高度解算方法，其特征在于，所述步骤S4包括：
S41、根据公式(5-1)对加速度计数据进行积分，以得到垂直方向预测速度和预测高度；
Figure FDA0003598408820000025
其中，向量
Figure FDA0003598408820000026
Figure FDA0003598408820000027
和
Figure FDA0003598408820000028
分别为k时刻的预测高度和预测速度，
Figure FDA0003598408820000029
为k时刻预测加速度修正量；向量
Figure FDA00035984088200000210
xk-1、vk-1分别为k-1时刻的融合高度和融合速度；矩阵
Figure FDA00035984088200000211
矩阵
Figure FDA00035984088200000212
uk为垂直方向的加速度数据；abias为k-1时刻的加速度修正量；Δt为算法执行周期；
计算协方差矩阵
Figure FDA0003598408820000031
其中
Figure FDA0003598408820000032
为过程噪声矩阵；q为对预测模型误差的估计值，为可调参数，其取决于传感器自身的特性(不同型号的传感器测量误差不同)，预测模型越精准则q的取值越小，通常通过试凑的方式得出；
S42、判断气压计数据是否有效，若气压计数据有效，则根据公式(5-2)进行数据融合计算；若气压计数据无效，则直接进入步骤S43；
Figure FDA0003598408820000033
其中，
Figure FDA0003598408820000034
为卡尔曼增益；Hbaro＝[1 0 0]；
Figure FDA0003598408820000035
为观测噪声矩阵；更新协方差矩阵
Figure FDA0003598408820000036
r是对传感器数据误差的估计；
Figure FDA0003598408820000037
为当前时刻的高度信息；
S43、判断超声传感器数据是否有效，若超声传感器数据有效，则根据公式(5-3)进行数据融合计算；若超声传感器数据无效，则直接进入步骤S44；
Figure FDA0003598408820000038
其中，
Figure FDA0003598408820000039
为卡尔曼增益；观测噪声矩阵
Figure FDA00035984088200000310
为观测噪声矩阵；Hultra＝[1 0 0]；更新协方差矩阵
Figure FDA00035984088200000311
S44、判断GPS数据是否有效，若GPS数据有效，则根据公式(5-4)进行数据融合计算；若GPS数据无效，则直接返回步骤S1，进行下一步的高度解算；
Figure FDA00035984088200000312
其中，
Figure FDA00035984088200000313
为卡尔曼增益；
Figure FDA00035984088200000314
为观测噪声矩阵；
Figure FDA00035984088200000315
Figure FDA00035984088200000316
为当前时刻的高度信息；需要更新协方差矩阵
Figure FDA00035984088200000317
返回步骤S1进行下一步的高度解算。
3.如权利要求1所述的高度解算方法，其特征在于，所述移动设备包括无人机。
4.一种能实现权利要求2中所述高度解算方法的高度解算装置，其包括：
至少2个高度传感器，且每一高度传感器均获取与其对应的传感器数据，所述高度传感器包括：超声传感器、气压计以及GPS，则对应的传感器数据为超声传感器数据、气压计数据和GPS数据；
有效性判断单元，其连接每一高度传感器，且当判断出超声传感器数据是在正常状况下获取到的、气压计数据是在正常状况下获取到的以及GPS数据是GPS搜星数量大于或等于设定阈值时获取到的时，确定此时的超声传感器数据、气压计数据和GPS数据为有效数据；
高度数据获取单元，其用于获取超声传感器数据、气压计数据和GPS数据为有效数据时，其对应从超声传感器、气压计以及GPS读取到的超声高度hultra、气压高度hbaro以及GPS高度hGPS；
高度对齐单元，其连接所述高度数据获取单元，用于根据公式(1-1)-(1-2)分别计算气压高度hbaro和超声高度hultra之间的偏移量Δh1，以及气压高度hbaro和GPS高度hGPS之间的偏移量Δh2，再根据公式(2-1)-(2-3)分别计算对齐后的GPS高度xGPS、对齐后的超声高度xultra以及对齐后的气压高度xbaro；
气压计置信度计算单元，其用于根据公式(3-1)-(3-2)计算气压计的置信度δbaro；
GPS垂直速度置信度/高度置信度计算单元，其用于根据公式(4-1)-(4-2)计算GPS垂直速度置信度εGPS和/或GPS高度置信度δGPS；
融合计算单元，其用于将气压高度hbaro、超声高度hultra和GPS高度hGPS与加速度计数据进行融合计算。
5.一种无人机，其包括权利要求4中所述的高度解算装置。
CN202010682781.2A 2020-07-15 2020-07-15 基于多传感器融合的高度解算方法、高度解算装置及无人机 Active CN111854691B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202010682781.2A CN111854691B (zh) 2020-07-15 2020-07-15 基于多传感器融合的高度解算方法、高度解算装置及无人机

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202010682781.2A CN111854691B (zh) 2020-07-15 2020-07-15 基于多传感器融合的高度解算方法、高度解算装置及无人机

Publications (2)

Publication Number Publication Date
CN111854691A CN111854691A (zh) 2020-10-30
CN111854691B true CN111854691B (zh) 2022-09-09

Family

ID=72983073

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202010682781.2A Active CN111854691B (zh) 2020-07-15 2020-07-15 基于多传感器融合的高度解算方法、高度解算装置及无人机

Country Status (1)

Country Link
CN (1) CN111854691B (zh)

Families Citing this family (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN113074697A (zh) * 2021-03-29 2021-07-06 宿迁学院 一种基于多传感器融合的无人机定高算法
CN112762893B (zh) * 2021-04-06 2021-06-25 北京三快在线科技有限公司 无人机状态确定方法、装置、介质、电子设备及无人机

Citations (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN104567799A (zh) * 2014-11-28 2015-04-29 天津大学 基于多传感器信息融合的小型旋翼无人机高度测量方法
CN106840093A (zh) * 2017-02-06 2017-06-13 北京京东尚科信息技术有限公司 一种无人机飞行高度的检测方法、装置及无人机

Family Cites Families (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US11644310B2 (en) * 2017-08-02 2023-05-09 Nextnav, Llc Systems and methods for positioning during atypical atmospheric conditions

Patent Citations (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN104567799A (zh) * 2014-11-28 2015-04-29 天津大学 基于多传感器信息融合的小型旋翼无人机高度测量方法
CN106840093A (zh) * 2017-02-06 2017-06-13 北京京东尚科信息技术有限公司 一种无人机飞行高度的检测方法、装置及无人机

Non-Patent Citations (2)

* Cited by examiner, † Cited by third party
Title
多旋翼飞行器的多传感器信息融合算法研究;张沛;《中国优秀硕士学位论文全文数据库 工程科技Ⅱ辑》;20170815;第29-31、32-35页 *
数据融合技术在无人机高度测量中的研究应用;李洪辉;《中国优秀硕士学位论文全文数据库 信息科技辑》;20130615;第16-26、第32-50页 *

Also Published As

Publication number Publication date
CN111854691A (zh) 2020-10-30

Similar Documents

Publication Publication Date Title
CN110196443B (zh) 一种飞行器的容错组合导航方法及系统
US6498996B1 (en) Vibration compensation for sensors
US10234292B2 (en) Positioning apparatus and global navigation satellite system, method of detecting satellite signals
JP5237723B2 (ja) 動的に較正されるセンサデータと、ナビゲーションシステム内の繰り返し拡張カルマンフィルタとを使用する、ジャイロコンパスの整合用のシステム及び方法
EP2072949B1 (en) Navigation system with dynamically calibrated pressure sensor
JP3682980B2 (ja) 航空機の高度センサを較正する方法
CN111854691B (zh) 基于多传感器融合的高度解算方法、高度解算装置及无人机
EP2187170A2 (en) Method and system for estimation of inertial sensor errors in remote inertial measurement unit
CN108051839B (zh) 一种车载三维定位装置及三维定位的方法
JP5074950B2 (ja) 航法装置
CN111156994A (zh) 一种基于mems惯性组件的ins/dr&gnss松组合导航方法
CN105509770B (zh) 一种gnss和mems组合导航系统中的气压计在线校正方法
CN112432642A (zh) 一种重力灯塔与惯性导航融合定位方法及系统
CN109725649A (zh) 一种基于气压计/imu/gps多传感器融合的旋翼无人机定高算法
KR101107219B1 (ko) 비행체의 항법 방법 및 이를 이용한 관성항법장치 필터 및 항법 시스템
CN115291253B (zh) 一种基于残差检测的车辆定位完好性监测方法及系统
CN115542349A (zh) 复杂环境多源融合定位自主完好性评估方法
CN115655269A (zh) 一种三轴超声波辅助下的行人自主定位导航方法
EP1058816B1 (en) Method and apparatus for determining altitude
JP3962403B2 (ja) 高度演算装置および方法
US20210190499A1 (en) Method for providing a navigation information, corresponding system and program product
CN114545475A (zh) 一种复杂环境下的多源组合导航定位方法
JP2013210235A (ja) 気圧高度計、位置計測システム、プログラム及び記録媒体
CN113959430B (zh) 飞行汽车的航姿确定方法、装置、车载终端及存储介质
JPH0785019B2 (ja) 船首方位計測装置

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
CB02 Change of applicant information

Address after: 430223 rooms 613 and 614, building 6, Wuhan Gaode infrared Industrial Park, No. 6, Huanglongshan South Road, Donghu New Technology Development Zone, Wuhan, Hubei Province

Applicant after: Wuhan Gaode Aircraft Technology Co.,Ltd.

Address before: 430070 rooms 613 and 614, building 6, Wuhan Gaode infrared Industrial Park, no.6, Huanglongshan South Road, Donghu New Technology Development Zone, Wuhan City, Hubei Province

Applicant before: Puzhou robot technology (Wuhan) Co.,Ltd.

CB02 Change of applicant information
GR01 Patent grant
GR01 Patent grant