经纬度转换为UTM坐标

概念 :

UTM(通用横向墨卡托投影): 是一种以米为单位的坐标系统,用于地图和GPS导航。将地球划分为60个纵向的区域。每个区域宽6度,从赤道开始往南北两级方向划分。每个区域都有一个特定的字母。

转换过程:

  1. 将经纬度值转化为弧度值
 // 将经度归一化到-180到180度之间
  double LongTemp =
      (Long + 180) - static_cast<int>((Long + 180) / 360) * 360 - 180;

  double LatRad = Lat * DEG_TO_RAD;
  double LongRad = LongTemp * DEG_TO_RAD;
  1. 根据经纬度计算所在的UTM区域

计算UTM带号,即 zoneNumber,这是由所在位置的经度计算得到的,计算公式为zoneNumber = floor((longTemp + 180)/6) + 1,其中 floor()函数表示向下取整。

对于特殊区域(Norway 和 Svalbard)需要进行特殊处理。对于Norway,UTM带号为31;对于Svalbard,需要根据具体位置计算UTM带号。

// 纬度在56度到64度之间,经度在3度到12度之间的区域属于挪威,对应的UTM带号为32
  if (Lat >= 56.0 && Lat < 64.0 && LongTemp >= 3.0 && LongTemp < 12.0)
    ZoneNumber = 32;

  // Special zones for Svalbard
  // 如果经纬度在北极圈内,则根据经度范围判断属于哪个UTM区域。
  // 对于挪威的斯瓦尔巴群岛(Svalbard)有特殊的UTM区域设置。
  // 如果纬度在72到84度之间,则根据经度范围判断属于哪个UTM区域,分别为31、33、35、37区。
  if (Lat >= 72.0 && Lat < 84.0) {
    if (LongTemp >= 0.0 && LongTemp < 9.0)
      ZoneNumber = 31;
    else if (LongTemp >= 9.0 && LongTemp < 21.0)
      ZoneNumber = 33;
    else if (LongTemp >= 21.0 && LongTemp < 33.0)
      ZoneNumber = 35;
    else if (LongTemp >= 33.0 && LongTemp < 42.0)
      ZoneNumber = 37;
  }
  1. 根据维度值计算UTM纵向带
  2. 计算以赤道为基准的投影面的相对圆柱体坐标系(即UTM投影)的各种参数
    椭球体参数,包括长半轴a,扁率f,第一偏心率平方eccSquared,以及第二偏心率平方eccPrimeSquared。
  3. 根据投影参数,计算经纬度值对应的UTM坐标值
    计算UTM坐标时,需要使用以下几个参数:
  • N:卯酉圈曲率半径。
  • T:曲率平面的切线方位角正切值。 T = t a n 2 ( L a t R a d ) T=tan^2(LatRad) T=tan2(LatRad)
  • C:子午圈曲率半径。 C = e c c P r i m e S q u a r e d × c o s 2 ( L a t R a d ) C=eccPrimeSquared×cos^2(LatRad) C=eccPrimeSquared×cos2(LatRad)
  • A:中央经线到点的经线长度。 A = c o s ( L a t R a d ) × ( L o n g R a d − L o n g O r i g i n R a d ) A=cos(LatRad)×(LongRad−LongOriginRad) A=cos(LatRad)×(LongRadLongOriginRad)
  • M:纬度的子午线弧长。
    M = a ∗ ( ( 1 − e 2 4 ​ − 3 e 4 64 ​ − 5 e 6 256 ​ ) ∗ ϕ − ( 3 e 2 8 ​ + 3 e 4 32 ​ + 45 e 6 1024 ​ ) ∗ s i n ( 2 ϕ ) + ( 15 e 4 256 ​ + 45 e 6 1024 ​ ) ∗ s i n ( 4 ϕ ) − 35 e 6 3072 ​ ∗ s i n ( 6 ϕ ) ) M=a∗((1−\frac{e^2}{4}​-\frac{3e^4}{64}​−\frac{5e^6}{256}​)∗ϕ−(\frac{3e^2}{8}​+\frac{3e^4}{32}​+\frac{45e^6}{1024}​)∗sin(2ϕ)+(\frac{15e^4}{256}​+\frac{45e^6}{1024}​)∗sin(4ϕ)−\frac{35e^6}{3072}​∗sin(6ϕ)) M=a((14e2643e42565e6)ϕ(83e2+323e4+102445e6)sin(2ϕ)+(25615e4+102445e6)sin(4ϕ)307235e6sin(6ϕ))

其中,a是椭球体的半长轴,e是椭球体的离心率, ϕ \phi ϕ是转换前的纬度。
U T M E a s t i n g = k 0 × N × ( A + ( 1 − T + C ) × A 3 / 6 + ( 5 − 18 × T + T 2 + 72 × C − 58 × e c c P r i m e S q u a r e d ) × A 5 / 120 ) + 500000.0 U T M N o r t h i n g = k 0 × ( M + N × t a n ( L a t R a d ) × ( A 2 / 2 + ( 5 − T + 9 × C + 4 × C 2 ) × A 4 / 24 + ( 61 − 58 × T + T 2 + 600 × C − 330 × e c c P r i m e S q u a r e d ) × A 6 / 720 ) ) UTM_{Easting}=k_0×N×(A+(1−T+C)×A^3/6+(5−18×T+T^2+72×C−58×eccPrimeSquared)×A^5/120)+500000.0 \\ UTM_{Northing}=k_0×(M+N×tan(LatRad)×(A^2/2+(5−T+9×C+4×C^2)×A^4/24+(61−58×T+T^2+600×C−330×eccPrimeSquared)×A^6/720)) UTMEasting=k0×N×(A+(1T+C)×A3/6+(518×T+T2+72×C58×eccPrimeSquared)×A5/120)+500000.0UTMNorthing=k0×(M+N×tan(LatRad)×(A2/2+(5T+9×C+4×C2)×A4/24+(6158×T+T2+600×C330×eccPrimeSquared)×A6/720))

eccPrimeSquared = (eccSquared) / (1 - eccSquared);

N = a / sqrt(1 - eccSquared * sin(LatRad) * sin(LatRad));
T = tan(LatRad) * tan(LatRad);
C = eccPrimeSquared * cos(LatRad) * cos(LatRad);
A = cos(LatRad) * (LongRad - LongOriginRad);

M = a *
    ((1 - eccSquared / 4 - 3 * eccSquared * eccSquared / 64 -
      5 * eccSquared * eccSquared * eccSquared / 256) *
         LatRad -
     (3 * eccSquared / 8 + 3 * eccSquared * eccSquared / 32 +
      45 * eccSquared * eccSquared * eccSquared / 1024) *
         sin(2 * LatRad) +
     (15 * eccSquared * eccSquared / 256 +
      45 * eccSquared * eccSquared * eccSquared / 1024) *
         sin(4 * LatRad) -
     (35 * eccSquared * eccSquared * eccSquared / 3072) * sin(6 * LatRad));

UTMEasting = k0 * N * (A + (1 - T + C) * A * A * A / 6 + (5 - 18 * T + T * T + 72 *

你可能感兴趣的:(SLAM基础,算法,自动驾驶)