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 - 50

position_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固件对高度的计算,实际一直在飞,所以理论上不可能出现高度为负值的情况。

表格中数据显示:

  1. GPS高度被采用时刻==》正值
  2. GPS高度后面随着时间/高度变化==》出现负值
  3. Baro高度起飞瞬间==》负值
  4. Baro其他飞行时间==》正值

因此,相对来说Baro气压计的高度更加精准。

DEBUG数据起飞校准起飞瞬间GPS有效时刻前GPS有效时刻后HDOP1变化前HDOP1变化后HDOP2变化前HDOP2变化后
GPS trust *1000004343282843
Baro Altitude0-385341336229225104100
GPS Altitude88968746100143404039-110-146

3. 总结

根据上面视频以及数据的情况来看:BetaFlight的高度计算无法很好的支撑Auto Landing功能,真的基于气压计+GPS高度和这种融合算法来做Auto Landing一定是缺乏安全的软着陆(比如:接近地面时降低下降率,检测碰撞来判断是否着陆)。

同时,我们也可以看到第二个视频,除了起飞瞬间气垫效应影响了高度,出现负值,,全程整体效果还是不错的。也许该天气下VDOP的精度比较高,但是这个对GPS卫星依赖的情况无法很好支持实际应用Auto Landing功能。

PS:通过iNav的代码,我们可以比较好的发现,其高度计算已经与cleanFlight最初的代码逻辑不一样,整体重构过了,还处理了接地或者低空接地飞行的气垫效应。

以上是关于BetaFlight深入传感设计之七:GPS&Baro高度数据融合的主要内容,如果未能解决你的问题,请参考以下文章

BetaFlight深入传感设计之十:传感器物理特性方向对齐

BetaFlight深入传感设计:传感模块设计框架

BetaFlight深入传感设计:传感模块设计框架

BetaFlight深入传感设计之二:Mag传感模块

BetaFlight深入传感设计之二:Mag传感模块

BetaFlight深入传感设计之三:IMU传感模块