一、GPS定位基本原理

GPS（全球定位系统）通过接收卫星信号实现定位，核心步骤如下：

1.卫星信号接收

接收机同时接收至少4颗卫星发射的载波信号（包含卫星位置与精确时间戳）。

2.距离计算

通过信号传播时间差计算接收机到各卫星的距离（伪距）：

距离=光速（T接收-T发送）

3.三维定位解算

建立方程组求解接收机位置 (x,y,z)和时间偏差 ΔT：

其中 (,,)为卫星坐标，为信号传播时间。

二丶GPS坐标转换为车辆位姿坐标

1.各关键坐标系理解

WGS-84 (GPS坐标系)： 全球通用的地球固定坐标系。使用经度、纬度、高度表示位置。它是一个大地坐标系。

ECEF (地心地固直角坐标系)： 原点在地心。X轴指向本初子午线与赤道交点，Z轴指向北极，Y轴与X、Z轴构成右手坐标系。位置用(X, Y, Z)表示。WGS-84坐标可以精确转换为ECEF坐标

局部切平面坐标系 (LTP)： 在某个特定参考点（通常是车辆起始点或某个关键点）建立的局部坐标系。

        ENU (东-北-天)： 最常用的LTP。原点在参考点。X轴指向东，Y轴指向北，Z轴指向天顶      

        NED (北-东-地)： 另一种常用LTP。X轴指向北，Y轴指向东，Z轴指向地心（向下）。

车辆坐标系 (Body Frame)： 固定在车辆上的坐标系。

       通常定义：原点：车辆参考点（如后轴中心、质心、IMU位置）。

       X轴：指向车辆前进方向（纵向）。

       Y轴：指向车辆左侧（横向）。

       Z轴：指向上方（垂直于车身平面）。

       横摆角/偏航角： 绕Z轴的旋转角度（0度通常指向北/参考方向）。

       俯仰角： 绕Y轴的旋转角度（抬头为正）。

       翻滚角： 绕X轴的旋转角度（右侧抬起为正）。

2.转换原理与步骤：

1：GPS (WGS-84) -> ECEF (X, Y, Z)

     使用精确的椭球模型（如WGS-84椭球）将经纬高转换为ECEF直角坐标。有标准的转换公式：

X = (N + h) * cos(lat) * cos(lon)
Y = (N + h) * cos(lat) * sin(lon)
Z = ((1 - e²) * N + h) * sin(lat)

  lat, lon, h：纬度（弧度）、经度（弧度）、高度。

  a：椭球长半轴（WGS-84: 6378137.0米）。

  e：第一偏心率（WGS-84: ≈0.0818191908429655）。

  N = a / sqrt(1 - e² * sin²(lat))：卯酉圈曲率半径。

 2：选择局部参考点并计算其ECEF坐标

        选择一个合适的参考点作为局部坐标系的原点。通常是：

        车辆轨迹的起始点。

        当前时刻附近的一个固定点。

        地图匹配到的某个已知点。

        同样使用步骤1的公式计算该参考点的ECEF坐标 (X_ref, Y_ref, Z_ref)

3：ECEF -> 局部切平面 (ENU/NED)

        目标： 将车辆当前的ECEF坐标 (X_veh, Y_veh, Z_veh) 转换到以参考点 (X_ref, Y_ref, Z_ref) 为原点的ENU（或NED）坐标系中。

        原理： 计算车辆位置相对于参考点的向量在局部坐标系各轴上的投影。

          计算ENU坐标：

          计算ECEF下的位移向量：

   ΔX = X_veh - X_ref

   ΔY = Y_veh - Y_ref

   ΔZ = Z_veh - Z_ref

          构建旋转矩阵 R。这个矩阵将ECEF向量旋转到参考点处的ENU坐标系。R 由参考点的经纬度 (lat_ref, lon_ref) 决定：

   R = [[ -sin(lon_ref), cos(lon_ref), 0 ],

      [ -sin(lat_ref)*cos(lon_ref), -sin(lat_ref)*sin(lon_ref), cos(lat_ref) ],

      [ cos(lat_ref)*cos(lon_ref), cos(lat_ref)*sin(lon_ref), sin(lat_ref) ]]

           将位移向量乘以旋转矩阵得到ENU坐标：[E, N, U]ᵀ = R * [ΔX, ΔY, ΔZ]ᵀ

           计算NED坐标： NED是ENU的简单变换：N = N(ENU), E = E(ENU), D = -U(ENU)

4：获取车辆姿态 (横摆/俯仰/翻滚)

         仅靠标准GPS（单点定位）无法直接提供姿态信息。

        惯性测量单元 (IMU)： 包含陀螺仪（测量角速度）和加速度计（测量比力）。通过对角速度            积分可以得到姿态变化（但存在漂移），结合加速度计和磁力计（测量地磁方向）可以校正            漂移，解算完整的3D姿态（横摆、俯仰、翻滚）。这是最常用的方法。

5：局部切平面坐标 + 姿态 -> 车辆位姿

        位置： 在局部切平面坐标系（如ENU）中，车辆的位置就是步骤3计算得到的 (E, N,       U) 或 (N, E, D)。

        姿态： 步骤4获得的姿态角（横摆ψ、俯仰θ、翻滚φ）描述了车辆本体坐标系相对于该局部            切平面坐标系的朝向。

         完整的车辆位姿： 在选定的局部切平面坐标系下，车辆的位姿可以用6自由度表示：                       (X ,Y ,Z ,φ,θ,ψ) 

         这里的 (X, Y, Z)是位置，(φ, θ, ψ) 是姿态角（翻滚、俯仰、横摆）。