ArduPilot 地形数据真值校验基于测距仪日志与外部 DEM 的对比分析指南【免费下载链接】ardupilotArduPlane, ArduCopter, ArduRover, ArduSub source项目地址: https://gitcode.com/GitHub_Trending/ar/ardupilot导读ArduPilot 的机载地形数据库TERRAIN为固定翼、多旋翼等平台提供地形跟随、避障与仿地飞行能力但其精度究竟如何往往需要用实测数据说话。本指南以 Tools/terrain-tools 提供的 Jupyter notebook 为核心讲解如何把飞行日志BIN 格式中的 AHR2 姿态位置、RFND 测距仪读数与 TERR 机载地形高度逐时刻对齐利用姿态旋转把朝下的测距仪测量投影到地面坐标再与 OpenTopography 下载的 COP30、ALOS World 3D 等外部 DEM 栅格做高程对比最终绘制出多源高程随时间变化的对比曲线。读完本文你将掌握一套可复现的地形真值校验流程并了解 ArduPilot 地形系统的日志字段、配置参数与底层实现原理。一、为什么需要做地形真值校验ArduPilot 的地形系统通过维护一块本地地形数据库默认从 SD 卡/APM/TERRAIN目录缓存网格块为飞行控制提供当前位置的地形高度估计。飞行中机载系统会按需向地面站请求地形数据块相关逻辑由 libraries/AP_Terrain 实现。用户可通过 AP_Terrain.cpp 中的参数配置其行为TERRAIN_ENABLE启用/禁用地形数据库默认值依机型而异在libraries/AP_Terrain/AP_Terrain.cpp中定义。TERRAIN_SPACING网格间距单位米默认 100对应 AP_Terrain.cpp 的AP_GROUPINFO(SPACING, 1, AP_Terrain, grid_spacing, 100)。TERRAIN_CACHE_SZ缓存块数量AP_Terrain.cpp。TERRAIN_OFS_MAX与参考位置偏移用于在起飞点在地面时校准机载高度与地形高度的偏差见set_reference_location()与 TERR 日志中的ROfs字段。机载地形数据库的质量、缓存缺失、网格插值方式以及 DEM 本身的误差都会影响地形跟随的实际表现。要评估这些误差最直接的办法就是把机载地形高度TERR 消息的TerrH与真实测得的离地高度朝下测距仪读数以及第三方高精度 DEM 高程放在同一时间轴上比较——这正是 terrain-tools 提供的校验思路。二、工具结构与运行环境Tools/terrain-tools目录包含两个文件README.md使用说明概述了比较不同地形数据源、将机载地形数据与实测测距仪数据对比的流程。terrain_rangefinder_postprocess.ipynb完整的 Jupyter notebook 处理脚本所有数据处理步骤均在其中实现。运行前置条件安装 Jupyter 并启动内核安装以下 Python 依赖notebook 中直接importpymavlink提供mavutil.mavlink_connection用于解析 BIN 日志pandas数据帧组织与时间索引对齐pyprojGeod地理反算用于 WGS84 椭球上的经纬度投影scipyscipy.spatial.transform.Rotation用于姿态旋转osgeo.gdal读取 GeoTIFF 高程栅格plotly交互式对比绘图numpy/math数值计算。准备一份包含 AHR2、RFND、TERR 消息的 BIN 日志DataFlash 日志。三、输入日志准备与参数修改打开 notebook 后第一件事是修改首个代码单元格顶部的input_log变量指向你的 BIN 日志路径from pathlib import Path input_log Path(00000125.BIN) stem input_log.stem # 00000125 from 00000125.BIN output_csv Path(fprojected_{stem}.csv) # Open .bin log log mavutil.mavlink_connection(str(input_log))input_logDataFlash BIN 日志文件路径stem日志文件名主干用于输出文件命名如projected_00000125.csvoutput_csv投影后的地面点结果 CSV 路径。notebook 用一条注释给出了关键假设BIN 日志解析器目前假设第一个测距仪朝下安装The BIN log parser currently assumes the first rangefinder is downward facing.。在校验脚本用测距仪读数推算地面点时这个假设至关重要——只有朝下的测距仪测量值才能直接反映飞机到地面的距离。多测距仪布局下请确保第一个测距仪指向下方否则结果会失真。3.1 日志解析循环核心解析逻辑按消息类型分流存入三个列表Ahrs namedtuple(Ahrs, [timestamp, lat, lon, alt, roll, pitch, yaw]) Rangefinder namedtuple(Rangefinder, [timestamp, distance]) Terrain namedtuple(Terrain, [timestamp, terr_height]) while True: msg log.recv_match(blockingFalse) if msg is None: break t datetime.fromtimestamp(msg._timestamp) mtype msg.get_type() if msg.get_type() AHR2: ahrs_positions.append(Ahrs( timestampt, latmsg.Lat, lonmsg.Lng, altmsg.Alt, rollmath.radians(msg.Roll), pitchmath.radians(msg.Pitch), yawmath.radians(msg.Yaw), )) elif msg.get_type() RFND and msg.Stat 4: # Only use healthy readings rangefinder.append(Rangefinder(timestampt, distancemsg.Dist)) elif msg.get_type() TERR: terrain_data.append(Terrain(timestampt, terr_heightmsg.TerrH))解析完成后三个列表分别转为以时间戳为索引、排序后的 DataFrameahrs_df pd.DataFrame(ahrs_positions).set_index(timestamp).sort_index() rfnd_df pd.DataFrame(rangefinder).set_index(timestamp).sort_index() terr_df pd.DataFrame(terrain_data).set_index(timestamp).sort_index()3.2 依赖的三类日志消息AHR2AHRS 输出的姿态与位置估计。字段Lat、Lng度、Alt米以及Roll、Pitch、Yaw脚本中转为弧度。AHR2 是经过 EKF 融合后的滤波解比原始 IMU 消息更适合做几何投影。RFND测距仪消息。字段Dist距离与Stat状态。脚本要求Stat 4对应AP_RangeFinder::Status::Good健康读数。该状态枚举定义于 AP_RangeFinder.cpp 的Status与后端状态机见 AP_RangeFinder_Backend.cpp只有健康数据才可用于地面点推算。RFND 消息的日志格式定义在 LogStructure.hTimeUS,Instance,Dist,Stat,Orient,Quality,Temp。TERR地形数据库消息。字段TerrH即机载地形高度。TERR 消息由 AP_Terrain::log_terrain_data() 周期性写入包含Status,Lat,Lng,Spacing,TerrH,CHeight,Pending,Loaded,ROfs格式见 LogStructure.h。其中TerrH为地形高度米CHeight为飞行器距地高度ROfs为起飞点地形参考偏移。四、姿态投影把测距仪读数换算成地面坐标测距仪测到的是机体坐标系下的斜向距离朝下安装时近似垂直。要把该读数落回地球表面需要经历“机体距离 → NED 偏移 → 地理坐标”三步变换这由 notebook 的第二个代码单元格完成。from scipy.spatial.transform import Rotation as R ground_points [] geod Geod(ellpsWGS84) for t, row in rfnd_df.iterrows(): dist row.distance # Efficient nearest neighbor lookup with .asof() for datetime index try: ahrs_row ahrs_df.loc[ahrs_df.index.asof(t)] terr_row terr_df.loc[terr_df.index.asof(t)] except KeyError: continue # skip if no data available at or before timestamp lat, lon, alt ahrs_row.lat, ahrs_row.lon, ahrs_row.alt roll, pitch, yaw ahrs_row.roll, ahrs_row.pitch, ahrs_row.yaw terr_height terr_row.terr_height # Build the rotation using Euler angles (in radians) rotation R.from_euler(xyz, [roll, pitch, yaw]) # Rotate the down vector (in body frame) ned_vector rotation.apply([0, 0, dist]) north_offset, east_offset, down_offset ned_vector ground_alt alt - down_offset # subtract to get estimated ground height # Project horizontal lat/lon offset azimuth math.degrees(math.atan2(east_offset, north_offset)) horizontal_distance math.hypot(north_offset, east_offset) g_lon, g_lat, _ geod.fwd(lon, lat, azimuth, horizontal_distance) ground_points.append(GroundPoint( timestampt, rng_latg_lat, rng_long_lon, rng_elevground_alt, ahrs_latlat, ahrs_lonlon, ahrs_altalt, distancedist, terr_heightterr_height, ))关键点说明时间对齐由于 AHR2、TERR 与 RFND 的消息频率不同脚本用 pandas 的asof()做“截至该时刻最近一次”的查找把每条 RFND 读数关联到最近一帧 AHR2 姿态与 TERR 地形高度。若时间戳早于任何可用数据如日志回放早期 EKF 尚未收敛则跳过该样本。姿态旋转将机体坐标系下的朝下向量[0, 0, dist]经R.from_euler(xyz, [roll, pitch, yaw])旋转到 NED 坐标系得到北向、东向与向下分量。旋转公式来自scipy.spatial.transform.Rotation属于标准欧拉角旋转。地面高程估算ground_alt alt - down_offset即用 AHRS 海拔高度减去向下分量得到测距仪照射点的估算地面高程米。水平位置投影用atan2(east_offset, north_offset)求方位角、hypot(north_offset, east_offset)求水平距离再通过Geod.fwd()WGS84 椭球大地反算从当前经纬度沿该方位角推进水平距离得到地面点的经纬度rng_lat / rng_lon。断言保护脚本对三个数据源都做了assert如assert ahrs_positions, No AHR2 messages found任一数据源缺失都会立刻报错避免产生无意义的对比结果。若最终ground_points为空则输出 Error: No projected ground point data found. 并退出。投影结果由第三个单元格汇总为 DataFramedf pd.DataFrame([gp.__dict__ for gp in ground_points])每行包含timestamp、rng_lat/rng_lon/rng_elev测距仪推算的地面点、ahrs_lat/ahrs_lon/ahrs_alt飞行器位置与海拔、distance原始测距读数、terr_height机载地形高度。五、确定下载区域并与 OpenTopography DEM 对比投影完成后第四个单元格自动从地面点数据框算出经纬度范围作为在 OpenTopography 下载 DEM 时的裁剪范围min_lat df[rng_lat].min() max_lat df[rng_lat].max() min_lon df[rng_lon].min() max_lon df[rng_lon].max() assert min_lat max_lat assert min_lon max_lon print(fLatitude range: ({min_lat},{max_lat})) print(fLongitude range: ({min_lon},{max_lon}))5.1 推荐数据集README 建议优先使用 OpenTopography 的以下全球 DEMCOP30Copernicus GLO-30 高程数据30 米分辨率OpenTopography 栅格服务中对应OTSDEM.032021.4326.3。ALOS World 3D 30 meter DEMJAXA 的 AW3D30同样为 30 米分辨率OpenTopography 中对应OT.112016.4326.2。下载时应覆盖上面打印出的经纬度范围并确保范围包含整条飞行轨迹建议留出一定余量避免边缘采样越界。更多全球地形数据集可参考 OpenTopography 公布的 SRTM 椭球版、ALOS World 3D 与 GMRT 相关公告。5.2 用 GDAL 读取 GeoTIFF将下载解压后的栅格文件放入脚本目录重命名并更新dataset_paths列表notebook 默认期待cop30.tif与AW3D30.tif然后运行加载单元格from osgeo import gdal dataset_paths [Path(cop30.tif), Path(AW3D30.tif)] for dataset_path in dataset_paths: dataset gdal.Open(str(dataset_path)) assert dataset is not None, fDataset {dataset_path} failed to load band dataset.GetRasterBand(1) transform dataset.GetGeoTransform() # Convert lat/lon to row/col def latlon_to_rowcol(lat, lon): inv_transform gdal.InvGeoTransform(transform) px, py gdal.ApplyGeoTransform(inv_transform, lon, lat) return int(py), int(px) elevations [] for _, row in df.iterrows(): r, c latlon_to_rowcol(row.rng_lat, row.rng_lon) grid band.ReadAsArray(c, r, 1, 1) if grid is not None: elevation grid[0][0] else: # Out of range of the dataset, common with LOG_REPLAY before the EKF initializes elevation math.nan elevations.append(elevation) df[dataset_path.stem _elevation] elevations要点支持任意数量的数据集对比Add as many datasets for comparison as you likeGDAL 支持多种栅格格式不只是 GeoTIFF。latlon_to_rowcol()通过 GDAL 的 GeoTransform 逆变换把经纬度换算成栅格行列号单点采样band.ReadAsArray(c, r, 1, 1)读取高程。采样越界栅格返回None时置为NaN——脚本注释指出这在日志回放LOG_REPLAY早期、EKF 尚未初始化时很常见此时位置估计无效对应样本应在绘图阶段被忽略。六、对比绘图与结果导出6.1 Plotly 交互式曲线notebook 使用 Plotly 的Scattergl绘制高程随时间的变化把多种数据源叠加在同一张图上import plotly.graph_objs as go fig go.Figure() for col in df.columns: stems [f{p.stem}_elevation for p in dataset_paths] cols (ahrs_alt, rng_elev, terr_height, *stems) if col in cols: y pd.to_numeric(df[col], errorscoerce) fig.add_trace(go.Scattergl(xdf[timestamp], yy, modelinesmarkers, namecol)) fig.update_layout( titlefTerrain Comparison Over Time for {stem}, xaxis_titleTime, yaxis_titleAltitude (m), height600, ) fig.show()图中包含四类曲线曲线来源含义ahrs_altAHR2.Alt飞行器海拔高度rng_elev测距仪投影实测地面点高程真值参考terr_heightTERR.TerrH机载地形数据库高程{stem}_elevation外部 DEMCOP30 / AW3D30 等栅格采样高程通过观察terr_height与rng_elev、外部 DEM 的偏离程度可以直观判断机载地形数据的系统性偏差、缓存缺失或网格插值误差ahrs_alt与rng_elev之差则对应实际离地高度可用于验证测距仪数据的合理性。Plotly 交互图适合小于约 30 分钟的小型日志超大日志几十万条消息建议改用更轻量的静态渲染或降采样避免浏览器卡顿。6.2 导出 HTML 与 CSV最后两个单元格分别完成结果保存import plotly.io as pio pio.write_html(fig, fterrain_pyplot_{stem}.html)交互图导出为terrain_pyplot_{stem}.html可离线分享投影后的地面点数据保存在前面定义的projected_{stem}.csv由首个单元格的output_csv指定df构造后需自行to_csv落盘可供后续在 GIS 软件或统计分析中复用。七、底层实现原理速览为了更好理解对比结果这里补充几个与脚本直接相关的底层实现TERR 日志的产生AP_Terrain::log_terrain_data()AP_Terrain.cpp通过height_amsl(loc, terrain_height)查询地形高度、height_above_terrain(current_height, true)计算距地高度并把grid_spacing、缓存统计pending/loaded与参考偏移一并写入log_TERRAIN结构体。terr_height字段即脚本中的TerrH。RFND 健康状态脚本只用Stat 4的测距读数。该状态值对应RangeFinder::Status::Good在 AP_RangeFinder.cpp 的状态转换中后端只有在测量有效如信号质量达标、距离在量程内时才会进入该状态。使用健康读数可剔除无效/饱和采样保证地面点质量。栅格采样与 EKF 收敛notebook 注释明确提到采样越界常见于 LOG_REPLAY before the EKF initializes。这意味着若在 Mission Planner 等工具中对 BIN 日志做离线回放回放开始阶段的 AHR2 位置尚未收敛此时投影出的地面点经纬度不可信对应的 DEM 高程采样也会落在错误位置应在分析时剔除图中体现为NaN断点。WGS84 椭球模型Geod(ellpsWGS84)使用大地线反算而非平面近似在长距离、高纬度区域比简单的米制偏移如offset_bearing的平面假设更准确适合覆盖数公里范围的地形校验任务。八、完整操作流程总结准备一架装有朝下测距仪并启用地形功能的飞行器完成一次包含爬升、巡航、地形跟随的飞行收集 BIN 日志确认包含 AHR2、RFND、TERR 消息。安装 Jupyter 及pymavlink、pandas、pyproj、scipy、gdal、plotly等依赖。打开 terrain_rangefinder_postprocess.ipynb修改input_log为你的日志路径依次运行各单元格。根据脚本输出的经纬度范围在 OpenTopography 下载覆盖该区域的 COP30 或 AW3D30 数据区域需覆盖整条轨迹并留余量。将 DEM 放入脚本目录、重命名为cop30.tif/AW3D30.tif或按需增删dataset_paths运行加载单元格完成栅格采样。查看 Plotly 对比图与projected_*.csv分析terr_height与实测/外部 DEM 的偏差评估机载地形数据库精度如需排查缓存问题可结合 TERR 消息的Pending/Loaded统计见 LogStructure.h判断是否因缓存未命中导致地形高度缺失。通过这套流程你可以量化 ArduPilot 机载地形数据在不同飞行场景下的误差为地形跟随参数的调优、DEM 数据源的选型以及日志回放中的 EKF 收敛分析提供实测依据。更多地形系统内部机制网格缓存、请求调度、GCS 交互可继续阅读 AP_Terrain.cpp 及其配套的 TerrainIO.cpp、TerrainGCS.cpp 与 TerrainUtil.cpp。【免费下载链接】ardupilotArduPlane, ArduCopter, ArduRover, ArduSub source项目地址: https://gitcode.com/GitHub_Trending/ar/ardupilot创作声明：本文部分内容由AI辅助生成（AIGC），仅供参考 ArduPilot 地形数据真值校验：基于测距仪日志与外部 DEM 的对比分析指南
ArduPilot 地形数据真值校验基于测距仪日志与外部 DEM 的对比分析指南【免费下载链接】ardupilotArduPlane, ArduCopter, ArduRover, ArduSub source项目地址: https://gitcode.com/GitHub_Trending/ar/ardupilot导读ArduPilot 的机载地形数据库TERRAIN为固定翼、多旋翼等平台提供地形跟随、避障与仿地飞行能力但其精度究竟如何往往需要用实测数据说话。本指南以 Tools/terrain-tools 提供的 Jupyter notebook 为核心讲解如何把飞行日志BIN 格式中的 AHR2 姿态位置、RFND 测距仪读数与 TERR 机载地形高度逐时刻对齐利用姿态旋转把朝下的测距仪测量投影到地面坐标再与 OpenTopography 下载的 COP30、ALOS World 3D 等外部 DEM 栅格做高程对比最终绘制出多源高程随时间变化的对比曲线。读完本文你将掌握一套可复现的地形真值校验流程并了解 ArduPilot 地形系统的日志字段、配置参数与底层实现原理。一、为什么需要做地形真值校验ArduPilot 的地形系统通过维护一块本地地形数据库默认从 SD 卡/APM/TERRAIN目录缓存网格块为飞行控制提供当前位置的地形高度估计。飞行中机载系统会按需向地面站请求地形数据块相关逻辑由 libraries/AP_Terrain 实现。用户可通过 AP_Terrain.cpp 中的参数配置其行为TERRAIN_ENABLE启用/禁用地形数据库默认值依机型而异在libraries/AP_Terrain/AP_Terrain.cpp中定义。TERRAIN_SPACING网格间距单位米默认 100对应 AP_Terrain.cpp 的AP_GROUPINFO(SPACING, 1, AP_Terrain, grid_spacing, 100)。TERRAIN_CACHE_SZ缓存块数量AP_Terrain.cpp。TERRAIN_OFS_MAX与参考位置偏移用于在起飞点在地面时校准机载高度与地形高度的偏差见set_reference_location()与 TERR 日志中的ROfs字段。机载地形数据库的质量、缓存缺失、网格插值方式以及 DEM 本身的误差都会影响地形跟随的实际表现。要评估这些误差最直接的办法就是把机载地形高度TERR 消息的TerrH与真实测得的离地高度朝下测距仪读数以及第三方高精度 DEM 高程放在同一时间轴上比较——这正是 terrain-tools 提供的校验思路。二、工具结构与运行环境Tools/terrain-tools目录包含两个文件README.md使用说明概述了比较不同地形数据源、将机载地形数据与实测测距仪数据对比的流程。terrain_rangefinder_postprocess.ipynb完整的 Jupyter notebook 处理脚本所有数据处理步骤均在其中实现。运行前置条件安装 Jupyter 并启动内核安装以下 Python 依赖notebook 中直接importpymavlink提供mavutil.mavlink_connection用于解析 BIN 日志pandas数据帧组织与时间索引对齐pyprojGeod地理反算用于 WGS84 椭球上的经纬度投影scipyscipy.spatial.transform.Rotation用于姿态旋转osgeo.gdal读取 GeoTIFF 高程栅格plotly交互式对比绘图numpy/math数值计算。准备一份包含 AHR2、RFND、TERR 消息的 BIN 日志DataFlash 日志。三、输入日志准备与参数修改打开 notebook 后第一件事是修改首个代码单元格顶部的input_log变量指向你的 BIN 日志路径from pathlib import Path input_log Path(00000125.BIN) stem input_log.stem # 00000125 from 00000125.BIN output_csv Path(fprojected_{stem}.csv) # Open .bin log log mavutil.mavlink_connection(str(input_log))input_logDataFlash BIN 日志文件路径stem日志文件名主干用于输出文件命名如projected_00000125.csvoutput_csv投影后的地面点结果 CSV 路径。notebook 用一条注释给出了关键假设BIN 日志解析器目前假设第一个测距仪朝下安装The BIN log parser currently assumes the first rangefinder is downward facing.。在校验脚本用测距仪读数推算地面点时这个假设至关重要——只有朝下的测距仪测量值才能直接反映飞机到地面的距离。多测距仪布局下请确保第一个测距仪指向下方否则结果会失真。3.1 日志解析循环核心解析逻辑按消息类型分流存入三个列表Ahrs namedtuple(Ahrs, [timestamp, lat, lon, alt, roll, pitch, yaw]) Rangefinder namedtuple(Rangefinder, [timestamp, distance]) Terrain namedtuple(Terrain, [timestamp, terr_height]) while True: msg log.recv_match(blockingFalse) if msg is None: break t datetime.fromtimestamp(msg._timestamp) mtype msg.get_type() if msg.get_type() AHR2: ahrs_positions.append(Ahrs( timestampt, latmsg.Lat, lonmsg.Lng, altmsg.Alt, rollmath.radians(msg.Roll), pitchmath.radians(msg.Pitch), yawmath.radians(msg.Yaw), )) elif msg.get_type() RFND and msg.Stat 4: # Only use healthy readings rangefinder.append(Rangefinder(timestampt, distancemsg.Dist)) elif msg.get_type() TERR: terrain_data.append(Terrain(timestampt, terr_heightmsg.TerrH))解析完成后三个列表分别转为以时间戳为索引、排序后的 DataFrameahrs_df pd.DataFrame(ahrs_positions).set_index(timestamp).sort_index() rfnd_df pd.DataFrame(rangefinder).set_index(timestamp).sort_index() terr_df pd.DataFrame(terrain_data).set_index(timestamp).sort_index()3.2 依赖的三类日志消息AHR2AHRS 输出的姿态与位置估计。字段Lat、Lng度、Alt米以及Roll、Pitch、Yaw脚本中转为弧度。AHR2 是经过 EKF 融合后的滤波解比原始 IMU 消息更适合做几何投影。RFND测距仪消息。字段Dist距离与Stat状态。脚本要求Stat 4对应AP_RangeFinder::Status::Good健康读数。该状态枚举定义于 AP_RangeFinder.cpp 的Status与后端状态机见 AP_RangeFinder_Backend.cpp只有健康数据才可用于地面点推算。RFND 消息的日志格式定义在 LogStructure.hTimeUS,Instance,Dist,Stat,Orient,Quality,Temp。TERR地形数据库消息。字段TerrH即机载地形高度。TERR 消息由 AP_Terrain::log_terrain_data() 周期性写入包含Status,Lat,Lng,Spacing,TerrH,CHeight,Pending,Loaded,ROfs格式见 LogStructure.h。其中TerrH为地形高度米CHeight为飞行器距地高度ROfs为起飞点地形参考偏移。四、姿态投影把测距仪读数换算成地面坐标测距仪测到的是机体坐标系下的斜向距离朝下安装时近似垂直。要把该读数落回地球表面需要经历“机体距离 → NED 偏移 → 地理坐标”三步变换这由 notebook 的第二个代码单元格完成。from scipy.spatial.transform import Rotation as R ground_points [] geod Geod(ellpsWGS84) for t, row in rfnd_df.iterrows(): dist row.distance # Efficient nearest neighbor lookup with .asof() for datetime index try: ahrs_row ahrs_df.loc[ahrs_df.index.asof(t)] terr_row terr_df.loc[terr_df.index.asof(t)] except KeyError: continue # skip if no data available at or before timestamp lat, lon, alt ahrs_row.lat, ahrs_row.lon, ahrs_row.alt roll, pitch, yaw ahrs_row.roll, ahrs_row.pitch, ahrs_row.yaw terr_height terr_row.terr_height # Build the rotation using Euler angles (in radians) rotation R.from_euler(xyz, [roll, pitch, yaw]) # Rotate the down vector (in body frame) ned_vector rotation.apply([0, 0, dist]) north_offset, east_offset, down_offset ned_vector ground_alt alt - down_offset # subtract to get estimated ground height # Project horizontal lat/lon offset azimuth math.degrees(math.atan2(east_offset, north_offset)) horizontal_distance math.hypot(north_offset, east_offset) g_lon, g_lat, _ geod.fwd(lon, lat, azimuth, horizontal_distance) ground_points.append(GroundPoint( timestampt, rng_latg_lat, rng_long_lon, rng_elevground_alt, ahrs_latlat, ahrs_lonlon, ahrs_altalt, distancedist, terr_heightterr_height, ))关键点说明时间对齐由于 AHR2、TERR 与 RFND 的消息频率不同脚本用 pandas 的asof()做“截至该时刻最近一次”的查找把每条 RFND 读数关联到最近一帧 AHR2 姿态与 TERR 地形高度。若时间戳早于任何可用数据如日志回放早期 EKF 尚未收敛则跳过该样本。姿态旋转将机体坐标系下的朝下向量[0, 0, dist]经R.from_euler(xyz, [roll, pitch, yaw])旋转到 NED 坐标系得到北向、东向与向下分量。旋转公式来自scipy.spatial.transform.Rotation属于标准欧拉角旋转。地面高程估算ground_alt alt - down_offset即用 AHRS 海拔高度减去向下分量得到测距仪照射点的估算地面高程米。水平位置投影用atan2(east_offset, north_offset)求方位角、hypot(north_offset, east_offset)求水平距离再通过Geod.fwd()WGS84 椭球大地反算从当前经纬度沿该方位角推进水平距离得到地面点的经纬度rng_lat / rng_lon。断言保护脚本对三个数据源都做了assert如assert ahrs_positions, No AHR2 messages found任一数据源缺失都会立刻报错避免产生无意义的对比结果。若最终ground_points为空则输出 Error: No projected ground point data found. 并退出。投影结果由第三个单元格汇总为 DataFramedf pd.DataFrame([gp.__dict__ for gp in ground_points])每行包含timestamp、rng_lat/rng_lon/rng_elev测距仪推算的地面点、ahrs_lat/ahrs_lon/ahrs_alt飞行器位置与海拔、distance原始测距读数、terr_height机载地形高度。五、确定下载区域并与 OpenTopography DEM 对比投影完成后第四个单元格自动从地面点数据框算出经纬度范围作为在 OpenTopography 下载 DEM 时的裁剪范围min_lat df[rng_lat].min() max_lat df[rng_lat].max() min_lon df[rng_lon].min() max_lon df[rng_lon].max() assert min_lat max_lat assert min_lon max_lon print(fLatitude range: ({min_lat},{max_lat})) print(fLongitude range: ({min_lon},{max_lon}))5.1 推荐数据集README 建议优先使用 OpenTopography 的以下全球 DEMCOP30Copernicus GLO-30 高程数据30 米分辨率OpenTopography 栅格服务中对应OTSDEM.032021.4326.3。ALOS World 3D 30 meter DEMJAXA 的 AW3D30同样为 30 米分辨率OpenTopography 中对应OT.112016.4326.2。下载时应覆盖上面打印出的经纬度范围并确保范围包含整条飞行轨迹建议留出一定余量避免边缘采样越界。更多全球地形数据集可参考 OpenTopography 公布的 SRTM 椭球版、ALOS World 3D 与 GMRT 相关公告。5.2 用 GDAL 读取 GeoTIFF将下载解压后的栅格文件放入脚本目录重命名并更新dataset_paths列表notebook 默认期待cop30.tif与AW3D30.tif然后运行加载单元格from osgeo import gdal dataset_paths [Path(cop30.tif), Path(AW3D30.tif)] for dataset_path in dataset_paths: dataset gdal.Open(str(dataset_path)) assert dataset is not None, fDataset {dataset_path} failed to load band dataset.GetRasterBand(1) transform dataset.GetGeoTransform() # Convert lat/lon to row/col def latlon_to_rowcol(lat, lon): inv_transform gdal.InvGeoTransform(transform) px, py gdal.ApplyGeoTransform(inv_transform, lon, lat) return int(py), int(px) elevations [] for _, row in df.iterrows(): r, c latlon_to_rowcol(row.rng_lat, row.rng_lon) grid band.ReadAsArray(c, r, 1, 1) if grid is not None: elevation grid[0][0] else: # Out of range of the dataset, common with LOG_REPLAY before the EKF initializes elevation math.nan elevations.append(elevation) df[dataset_path.stem _elevation] elevations要点支持任意数量的数据集对比Add as many datasets for comparison as you likeGDAL 支持多种栅格格式不只是 GeoTIFF。latlon_to_rowcol()通过 GDAL 的 GeoTransform 逆变换把经纬度换算成栅格行列号单点采样band.ReadAsArray(c, r, 1, 1)读取高程。采样越界栅格返回None时置为NaN——脚本注释指出这在日志回放LOG_REPLAY早期、EKF 尚未初始化时很常见此时位置估计无效对应样本应在绘图阶段被忽略。六、对比绘图与结果导出6.1 Plotly 交互式曲线notebook 使用 Plotly 的Scattergl绘制高程随时间的变化把多种数据源叠加在同一张图上import plotly.graph_objs as go fig go.Figure() for col in df.columns: stems [f{p.stem}_elevation for p in dataset_paths] cols (ahrs_alt, rng_elev, terr_height, *stems) if col in cols: y pd.to_numeric(df[col], errorscoerce) fig.add_trace(go.Scattergl(xdf[timestamp], yy, modelinesmarkers, namecol)) fig.update_layout( titlefTerrain Comparison Over Time for {stem}, xaxis_titleTime, yaxis_titleAltitude (m), height600, ) fig.show()图中包含四类曲线曲线来源含义ahrs_altAHR2.Alt飞行器海拔高度rng_elev测距仪投影实测地面点高程真值参考terr_heightTERR.TerrH机载地形数据库高程{stem}_elevation外部 DEMCOP30 / AW3D30 等栅格采样高程通过观察terr_height与rng_elev、外部 DEM 的偏离程度可以直观判断机载地形数据的系统性偏差、缓存缺失或网格插值误差ahrs_alt与rng_elev之差则对应实际离地高度可用于验证测距仪数据的合理性。Plotly 交互图适合小于约 30 分钟的小型日志超大日志几十万条消息建议改用更轻量的静态渲染或降采样避免浏览器卡顿。6.2 导出 HTML 与 CSV最后两个单元格分别完成结果保存import plotly.io as pio pio.write_html(fig, fterrain_pyplot_{stem}.html)交互图导出为terrain_pyplot_{stem}.html可离线分享投影后的地面点数据保存在前面定义的projected_{stem}.csv由首个单元格的output_csv指定df构造后需自行to_csv落盘可供后续在 GIS 软件或统计分析中复用。七、底层实现原理速览为了更好理解对比结果这里补充几个与脚本直接相关的底层实现TERR 日志的产生AP_Terrain::log_terrain_data()AP_Terrain.cpp通过height_amsl(loc, terrain_height)查询地形高度、height_above_terrain(current_height, true)计算距地高度并把grid_spacing、缓存统计pending/loaded与参考偏移一并写入log_TERRAIN结构体。terr_height字段即脚本中的TerrH。RFND 健康状态脚本只用Stat 4的测距读数。该状态值对应RangeFinder::Status::Good在 AP_RangeFinder.cpp 的状态转换中后端只有在测量有效如信号质量达标、距离在量程内时才会进入该状态。使用健康读数可剔除无效/饱和采样保证地面点质量。栅格采样与 EKF 收敛notebook 注释明确提到采样越界常见于 LOG_REPLAY before the EKF initializes。这意味着若在 Mission Planner 等工具中对 BIN 日志做离线回放回放开始阶段的 AHR2 位置尚未收敛此时投影出的地面点经纬度不可信对应的 DEM 高程采样也会落在错误位置应在分析时剔除图中体现为NaN断点。WGS84 椭球模型Geod(ellpsWGS84)使用大地线反算而非平面近似在长距离、高纬度区域比简单的米制偏移如offset_bearing的平面假设更准确适合覆盖数公里范围的地形校验任务。八、完整操作流程总结准备一架装有朝下测距仪并启用地形功能的飞行器完成一次包含爬升、巡航、地形跟随的飞行收集 BIN 日志确认包含 AHR2、RFND、TERR 消息。安装 Jupyter 及pymavlink、pandas、pyproj、scipy、gdal、plotly等依赖。打开 terrain_rangefinder_postprocess.ipynb修改input_log为你的日志路径依次运行各单元格。根据脚本输出的经纬度范围在 OpenTopography 下载覆盖该区域的 COP30 或 AW3D30 数据区域需覆盖整条轨迹并留余量。将 DEM 放入脚本目录、重命名为cop30.tif/AW3D30.tif或按需增删dataset_paths运行加载单元格完成栅格采样。查看 Plotly 对比图与projected_*.csv分析terr_height与实测/外部 DEM 的偏差评估机载地形数据库精度如需排查缓存问题可结合 TERR 消息的Pending/Loaded统计见 LogStructure.h判断是否因缓存未命中导致地形高度缺失。通过这套流程你可以量化 ArduPilot 机载地形数据在不同飞行场景下的误差为地形跟随参数的调优、DEM 数据源的选型以及日志回放中的 EKF 收敛分析提供实测依据。更多地形系统内部机制网格缓存、请求调度、GCS 交互可继续阅读 AP_Terrain.cpp 及其配套的 TerrainIO.cpp、TerrainGCS.cpp 与 TerrainUtil.cpp。【免费下载链接】ardupilotArduPlane, ArduCopter, ArduRover, ArduSub source项目地址: https://gitcode.com/GitHub_Trending/ar/ardupilot创作声明：本文部分内容由AI辅助生成（AIGC），仅供参考