BetaFlight深入传感设计之七:GPS&Baro高度数据融合
Posted lida2003
tags:
篇首语:本文由小常识网(cha138.com)小编为大家整理,主要介绍了BetaFlight深入传感设计之七:GPS&Baro高度数据融合相关的知识,希望对你有一定的参考价值。
BetaFlight深入传感设计之七:GPS&Baro高度数据融合
传感器数据融合最主要的目的是为了数据的精准。同时也可以通过多个传感数据源来判断和纠正异常数据。
最近在飞BetaFlight的时候,总是感觉高度数据异常,明明在天上飞的,结果高度是负值。另外,BF团队也在考虑在4.4版本逐步引入高精度的GPS Resucre功能,甚至有Return To Home,然后Auto Landing的功能。
所以感觉有必要更深入了解下这方面的机制和误差的来源。
1. 现象
OSD显示的数据显示高度负值,比较明显。视频可以大家看下:
— 注意观察负值高度,采用视频时间戳。第一次起飞 —
01:24 起飞校准 0米, GPS卫星数量11颗
01:28 起飞[lift off],出现一个-10米的数据(尴尬)
01:53 持续飞30秒,该飞地是一个茶叶坡地,所以一直在升高,直到30秒OSD显示正值高度
04:06 飞行2分43秒,靠近起飞地点,高度开始出现负值
04:34 飞行3分11秒,降落瞬间,高度显示-12.5米
— 注意起飞校准时刻0米高度,起飞瞬间-10米数据,降落书剑-12.5米。第二次起飞 —
05:31 再次起飞 校准 0米,GPS卫星数量10颗
05:34 起飞[lift off], 出现一个-1米的数据,后续正常攀升(正值)
08:09 飞行2分37秒,接近起飞点,出现负值高度
08:13 飞越公路桥,在起飞点上方56米高度,但是OSD显示-5.4米(尴尬~)
08:56 再次靠近起飞点,OSD仍然显示负值-3.8米
09:55 重新飞第一次飞行路径,OSD显示高度负值(持续,稳定负值;随实际海拔高度提升,高度逐步回升)
11:41 再次公路桥高度附近飞行,OSD显示高度持续为负
14:04 降落瞬间,OSD显示高度 - 5.4米
00:10 起飞校准 高度0米,GPS卫星14颗,晴天
00:12 起飞瞬间 高度-2.4米,随后马上正常
05:34 接近起飞点,高度显示0米(波动,随后显示1-2米高度)
05:41 再次出现一个-0.1米的高度(波动,随后显示正常)
05:56 降落瞬间,OSD显示高度0米(非常正常)
00:01 起飞校准 高度0米,GPS卫星16颗,晴天
00:02 起飞瞬间 高度-3.2米,随后正常
01:31 飞行一段时间,目前高度已经过围墙,显然不止2米,但是OSD显示2米
02:12 飞行中,OSD显示高度0米【异常】
03:18 飞过起飞点上方,OSD显示高度-2.2米【异常】,后面全程可以关注,靠近地面的基本都是负值高度
12:39 降落瞬间,高度-3米【异常】
所以,BF 4.3.1固件所使用的GPS+Baro传感器数据融合方法,精度并不理想,甚至出现尴尬的负值。
2. 分析
2.1 程序逻辑
先根据之前研读一些内容,整理下传感高度数据融合的计算逻辑:
【1】BetaFlight模块设计之九:气压计任务分析
【2】BetaFlight模块设计之十四:高度计算任务分析
【3】BetaFlight深入传感设计之一:Baro传感模块
【4】BetaFlight深入传感设计之四:GPS传感模块
- Step1: 上电时,使用数学平均值算法,记录baroGroundAltitude
- Step2: 定时更新气压计高度pressureToAltitude,计算BaroAlt_tmp = pressureToAltitude - baroGroundAltitude
- Step3: 做一个低通滤波baro.BaroAlt = baro.BaroAlt * barometerConfig()->baro_noise_lpf + BaroAlt_tmp * (1.0f - barometerConfig()->baro_noise_lpf)
- Step4: 为防止上电时刻与解锁时刻高度差异值,解锁时计算高度差baroAltOffset = baroAlt
- Step5: 实际飞行时,气压计高度baroAlt -= baroAltOffset
- Step6: 【传感融合】【第一次GPS高度有效时刻】计算gpsAltOffset = gpsAlt - baroAlt
- Step7: 【传感融合】【GPS高度有效】gpsTrust = 100.0 / gpsSol.hdop //但不超过 0.9, 至少保留10%的气压计高度
- Step8: 【GPS有效性判断】有效判断依据:> altNumSatsGpsUse; 无效回退判断依据:< altNumSatsBaroFallback
- Step9: 【高度计算】
【GPS + baro】estimatedAltitudeCm = gpsAlt * gpsTrust + baroAlt * (1 - gpsTrust)
【仅GPS】 estimatedAltitudeCm = gpsAlt
【仅baro】estimatedAltitudeCm = baroAlt
注:默认GPS高度有效性判断依据。
position_alt_gps_min_sats = 10
Allowed range: 4 - 50position_alt_baro_fallback_sats = 7
Allowed range: 3 - 49
2.2 GPS精度
DOP是Dilution of Precision的缩写,直译为“精度强弱度”,通常翻译为“精度因子”。精度因子:一种描述纯粹因卫星几何因素对定点精度的影响,指出了在测量时被跟踪卫星几何结构上的强度。包括:
- GDOP (Geometrical):包括经度,纬度,高程和时间等因子,称为几何精度因子。
- PDOP (Positional):范围:0.5 - 99.9,包括经度,纬度和高程等因子,称为三维(空间)位置精度因子
- HDOP (Horizontal):范围:0.5 - 99.9,包括经度和纬度等因子,称为水平(平面)位置精度因子
- VDOP (Vertical):范围:0.5 - 99.9,仅包括高程因子,称为高程精度因子
- TDOP (Time): 钟差几何精度因子
精度衰减因子(DOP)是位置质量的指示器。它是考虑每颗卫星相对于星座(几何位置)中其它卫星的位置来预计用该星座能得到的位置精度的计算结果。小的DOP值表示强的卫星几何位置和精度的较高概率。高的DOP值表示弱的卫星几何位置和精度的较低概率.
高度误差按理应该采用VDOP,但是BetaFlight代码中使用的是HDOP,与理论有些不符。为什么???
2.3 数值分析
这里我们结合一组DEBUG的数据,分析了BetaFlight 4.3.1固件对高度的计算,实际一直在飞,所以理论上不可能出现高度为负值的情况。
表格中数据显示:
- GPS高度被采用时刻==》正值
- GPS高度后面随着时间/高度变化==》出现负值
- Baro高度起飞瞬间==》负值
- Baro其他飞行时间==》正值
因此,相对来说Baro气压计的高度更加精准。
DEBUG数据 | 起飞校准 | 起飞瞬间 | GPS有效时刻前 | GPS有效时刻后 | HDOP1变化前 | HDOP1变化后 | HDOP2变化前 | HDOP2变化后 |
---|---|---|---|---|---|---|---|---|
GPS trust *100 | 0 | 0 | 0 | 43 | 43 | 28 | 28 | 43 |
Baro Altitude | 0 | -385 | 341 | 336 | 229 | 225 | 104 | 100 |
GPS Altitude | 8896 | 8746 | 10014 | 340 | 40 | 39 | -110 | -146 |
3. 总结
根据上面视频以及数据的情况来看:BetaFlight的高度计算无法很好的支撑Auto Landing功能,真的基于气压计+GPS高度和这种融合算法来做Auto Landing一定是缺乏安全的软着陆(比如:接近地面时降低下降率,检测碰撞来判断是否着陆)。
同时,我们也可以看到第二个视频,除了起飞瞬间气垫效应影响了高度,出现负值,,全程整体效果还是不错的。也许该天气下VDOP的精度比较高,但是这个对GPS卫星依赖的情况无法很好支持实际应用Auto Landing功能。
PS:通过iNav的代码,我们可以比较好的发现,其高度计算已经与cleanFlight最初的代码逻辑不一样,整体重构过了,还处理了接地或者低空接地飞行的气垫效应。
以上是关于BetaFlight深入传感设计之七:GPS&Baro高度数据融合的主要内容,如果未能解决你的问题,请参考以下文章