slam 估计的轨迹转为 WGS84?

1. 这句话什么意思? WGS84 和 GPS 什么关系

When GPS information is available, the estimated trajectory is transformed to WGS84 coordinates after time synchronized automatically.

这句话的意思是:

当 GPS 信息可用时,估计的轨迹会在时间同步后自动转换为 WGS84 坐标系中的坐标。

WGS84 和 GPS 的关系

  1. WGS84(World Geodetic System 1984) 是一种全球通用的地理坐标系,定义了地球的形状、参考椭球、重力场模型等。WGS84 坐标通常由 经度(Longitude)、纬度(Latitude)和海拔(Altitude) 表示。
  2. GPS(Global Positioning System) 是美国的全球定位系统,依赖于卫星提供的定位数据。GPS 输出的位置数据默认采用 WGS84 坐标系

这句话具体描述的过程

  • 你的系统(比如 VINS-Fusion)估计出的轨迹最开始可能是本地坐标系(如惯性系、相机坐标系、局部 UTM 坐标系等)中的轨迹。
  • 当 GPS 可用时,系统会通过时间同步 GPS 数据,然后将估计轨迹转换到 WGS84 坐标系(即全球坐标)。
  • 这样可以使轨迹与全球坐标系统对齐,而不是仅在一个局部坐标系中工作。

简单来说,GPS 提供的是 WGS84 坐标,而 VINS-Fusion 这类视觉惯性 SLAM 估计的是局部坐标系中的轨迹。当 GPS 数据可用时,系统会进行时间对齐,然后把轨迹转换成 WGS84 坐标系,使其成为全球可用的坐标数据。

2. 将估计轨迹转换到 WGS84 坐标系的方法?

1. 了解坐标系

  • 估计轨迹(SLAM/VO/VINS 轨迹):通常是在某个局部坐标系(惯性坐标系、相机坐标系、ENU坐标系、UTM坐标系等)。
  • GPS 提供的坐标:默认是 WGS84(经纬度 + 海拔)

为了转换,通常需要找到局部坐标系和 WGS84 之间的关系。


2. 具体转换步骤

(1) 选定参考点

在 GPS 可用的情况下,选定一个初始时刻的 GPS 位置 (lat₀, lon₀, alt₀),对应局部坐标系中的原点 (x₀, y₀, z₀)

  • lat₀, lon₀, alt₀:来自 GPS(单位:度、米)。
  • x₀, y₀, z₀:估计轨迹的局部坐标(单位:米)。

(2) 将局部坐标转换为地球坐标(ECEF)
  1. GPS 的 WGS84 坐标 (lat, lon, alt) 需要转换为地心地固坐标系(ECEF - Earth-Centered, Earth-Fixed)

\[ X = (N + h) \cos(lat) \cos(lon) \]

\[Y = (N + h) \cos(lat) \sin(lon) \]

\[Z = \left( \frac{b^2}{a^2} N + h \right) \sin(lat) \]

  • (X, Y, Z) 是地心坐标(单位:米)。
  • lat, lon, h 是 GPS 经纬度和高度。
  • a = 6378137.0(WGS84 长半轴)。
  • b = 6356752.3142(WGS84 短半轴)。
  • N = a / sqrt(1 - e² sin²(lat))(卯酉圈半径)。
  • e² = (a² - b²) / a²(偏心率平方)。
  1. 同样地,把参考点 (lat₀, lon₀, alt₀) 也转换到 ECEF (X₀, Y₀, Z₀)

(3) 计算局部坐标系到 ECEF 的变换

在局部坐标系中,通常采用 ENU(East-North-Up) 坐标系,其中:

  • X 轴指向东(East)。
  • Y 轴指向北(North)。
  • Z 轴指向天顶(Up)。

构造ENU 到 ECEF 的旋转矩阵:

\[R = \begin{bmatrix} -\sin(lon₀) & \cos(lon₀) & 0 \\ -\sin(lat₀) \cos(lon₀) & -\sin(lat₀) \sin(lon₀) & \cos(lat₀) \\ \cos(lat₀) \cos(lon₀) & \cos(lat₀) \sin(lon₀) & \sin(lat₀) \end{bmatrix} \]

然后,把局部轨迹点 (x, y, z) 转换到 ECEF:

\[\begin{bmatrix} X' \\ Y' \\ Z' \end{bmatrix} = R \cdot \begin{bmatrix} x - x₀ \\ y - y₀ \\ z - z₀ \end{bmatrix} + \begin{bmatrix} X₀ \\ Y₀ \\ Z₀ \end{bmatrix} \]


(4) 从 ECEF 变换回 WGS84

把新的 (X', Y', Z') 转换回经纬度:

  • 迭代计算纬度:

    \[lat' = \tan^{-1} \left( \frac{Z'}{\sqrt{X'^2 + Y'^2} \cdot (1 - e²)} \right) \]

  • 计算经度:

    \[lon' = \tan^{-1} \left( \frac{Y'}{X'} \right) \]

  • 计算高度:

    \[h' = \frac{\sqrt{X'^2 + Y'^2}}{\cos(lat')} - N \]


3. 代码示例

这里用 Python 代码演示 WGS84 和局部坐标之间的转换:

import numpy as np
from pyproj import Proj, transform

# WGS84 椭球参数
a = 6378137.0  # 长半轴
b = 6356752.3142  # 短半轴
e2 = (a**2 - b**2) / a**2  # 偏心率平方

def wgs84_to_ecef(lat, lon, alt):
    lat, lon = np.radians(lat), np.radians(lon)
    N = a / np.sqrt(1 - e2 * np.sin(lat)**2)
    X = (N + alt) * np.cos(lat) * np.cos(lon)
    Y = (N + alt) * np.cos(lat) * np.sin(lon)
    Z = (b**2 / a**2 * N + alt) * np.sin(lat)
    return np.array([X, Y, Z])

def ecef_to_wgs84(X, Y, Z):
    lon = np.arctan2(Y, X)
    p = np.sqrt(X**2 + Y**2)
    lat = np.arctan2(Z, p * (1 - e2))  # 初始估计
    N = a / np.sqrt(1 - e2 * np.sin(lat)**2)
    alt = p / np.cos(lat) - N
    return np.degrees(lat), np.degrees(lon), alt

def local_to_wgs84(x, y, z, ref_lat, ref_lon, ref_alt):
    """ 将局部坐标 (x, y, z) 转换到 WGS84 """
    # 计算参考点的 ECEF 坐标
    ref_ecef = wgs84_to_ecef(ref_lat, ref_lon, ref_alt)

    # 计算 ENU 到 ECEF 旋转矩阵
    ref_lat, ref_lon = np.radians(ref_lat), np.radians(ref_lon)
    R = np.array([
        [-np.sin(ref_lon), np.cos(ref_lon), 0],
        [-np.sin(ref_lat) * np.cos(ref_lon), -np.sin(ref_lat) * np.sin(ref_lon), np.cos(ref_lat)],
        [np.cos(ref_lat) * np.cos(ref_lon), np.cos(ref_lat) * np.sin(ref_lon), np.sin(ref_lat)]
    ])

    # 局部坐标转换到 ECEF
    ecef_coords = R @ np.array([x, y, z]) + ref_ecef

    # ECEF 转回 WGS84
    return ecef_to_wgs84(*ecef_coords)

# 示例:假设局部坐标 (x, y, z) = (100, 50, 10)
# 参考点 GPS 坐标 (lat, lon, alt) = (30.0, 114.0, 50)
lat, lon, alt = local_to_wgs84(100, 50, 10, 30.0, 114.0, 50)
print(f"WGS84: lat={lat}, lon={lon}, alt={alt}")

4. 总结

  1. GPS 坐标是 WGS84,而 VINS-Fusion 估计轨迹是局部坐标,必须进行转换。
  2. 步骤
    • 选定参考点(GPS 原点)。
    • 局部坐标 → ECEF 坐标
    • ECEF 坐标 → WGS84 坐标(经纬度 + 高度)。
  3. 代码实现用 ECEF 作为中间转换,可以准确完成变换。

你可以根据 VINS-Fusion 的原点选取策略,调整参考点选择方式。这样就可以让你的估计轨迹在 GPS 可用时,与 WGS84 对齐! 🚀

3. 可是单目 slam 系统,估计的局部坐标存在尺度问题,这样转换可以吗?

你说得对!单目 SLAM 的局部坐标系存在尺度模糊性(scale ambiguity),直接转换到 WGS84 可能会导致误差,因为 SLAM 估计的轨迹坐标单位是相对尺度,而 GPS 提供的是真实世界尺度(米)。

核心问题:尺度不匹配

  • 单目 SLAM 轨迹(VO/VINS/OpenVSLAM)

    • 轨迹是相对坐标,没有绝对尺度信息,单位是“未知比例的米”。
    • 估计的位移和 GPS 真实位移之间可能相差一个尺度因子 s
  • GPS 坐标(WGS84 / ECEF)

    • 真实世界的绝对坐标(单位:米)。
    • 没有尺度模糊性。

所以,如果直接把 SLAM 轨迹 x, y, z 转换为 ECEF/WGS84,可能会导致转换后的轨迹缩放错误


如何解决单目 SLAM 的尺度问题?

方法 1:使用 GPS 进行尺度对齐

如果 GPS 轨迹是可用的(即在一定时间段内有可靠的 GPS 数据),可以计算一个全局尺度因子,使 SLAM 轨迹与 GPS 轨迹匹配:

\[s = \frac{\sum d_{\text{GPS}}}{\sum d_{\text{SLAM}}} \]

其中:

  • $ d_{\text{GPS}} $ 是 GPS 轨迹的欧几里得距离(从 WGS84 转换到 UTM/ECEF 后计算)。
  • $ d_{\text{SLAM}} $ 是单目 SLAM 估计的轨迹间的欧几里得距离。
  • $ s $ 是尺度因子。

然后,把 SLAM 轨迹按尺度 $ s $ 缩放

\[(x', y', z') = s \cdot (x, y, z) \]

之后,再按照 ECEF → WGS84 进行转换,就可以得到合理的 WGS84 坐标。


方法 2:利用 IMU 进行尺度恢复

如果你的单目 SLAM 结合了 IMU(如 VINS-Fusion、OKVIS、MSCKF),IMU 可以提供加速度计,从而帮助估计真实世界的尺度。
在 VINS-Fusion 里,IMU 的单位是 m/s²,可以用于恢复轨迹的真实尺度,这样 SLAM 估计的坐标就不再是任意尺度,而是可以直接用于坐标转换。


方法 3:用已知地图点进行尺度校准

如果在环境中有已知的地标点(比如 GPS 已知的固定位置点),可以通过对齐这些点来确定单目 SLAM 轨迹的真实比例尺

  • 例如,选取 SLAM 轨迹中的两个点 A 和 B,已知它们的GPS 位置(经纬度),计算它们之间的真实距离 $ d_{\text{GPS}} $。
  • 计算单目 SLAM 估计的 A 和 B 之间的欧几里得距离 $ d_{\text{SLAM}} $。
  • 计算尺度因子:

    \[s = \frac{d_{\text{GPS}}}{d_{\text{SLAM}}} \]

  • 重新缩放整个 SLAM 轨迹:

    \[(x', y', z') = s \cdot (x, y, z) \]


总结

单目 SLAM 不能直接转换为 WGS84,必须先解决尺度问题

  • 如果有 GPS 数据,可以计算全局尺度因子 $ s $ 并缩放 SLAM 轨迹。
  • 如果有 IMU,可以用 VINS-Fusion 自动恢复真实尺度。
  • 如果有已知地标点,可以基于这些点计算尺度因子。

最终步骤:

  1. 尺度恢复(GPS、IMU、地标点等方法)。
  2. 局部坐标 → ECEF(使用 ENU 旋转矩阵)。
  3. ECEF → WGS84(转换为经纬度)。

如果你有 GPS 轨迹和 SLAM 轨迹的数据,我可以帮你计算合适的尺度因子!🚀

注意:chatgpt 复制的 公式格式是 \(\)\[\]

博客园看起来不支持公式格式:

\( e^{\pi i} + 1 = 0\)

\[
E = mc^2
\]

\(\)替换为 $, \[\]替换为 $$

posted @ 2025-02-25 10:04  Zenith_Hugh  阅读(70)  评论(0)    收藏  举报