自动驾驶中各种坐标系辨析

坐标系辨析

  • 0. 地球椭圆体
  • 1. 大地坐标系
  • 2. eci地心惯性坐标系
  • 3. 地心地固坐标系(ECEF坐标系,E系)
  • 4. 站心坐标系(ENU坐标系)
  • 5. UTM坐标系
  • 6. LTM坐标系
  • 7. IMU坐标系
  • 8. 代码部分
    • 8.1 LLA(大地坐标系坐标、经纬度海拔)坐标转LTM系(ENU系)下的三维笛卡尔坐标
    • 8.2 LLA坐标转化为ECEF坐标
    • 8.3 ECEF坐标系转LLA坐标系
    • 8.4 ENU坐标系与LLA坐标系之间的转换
  • 9. 引用

0. 地球椭圆体

地球表面是一个凸凹不平的表面,而对于地球测量而言,地表是一个无法用数学公式表达的曲面,这样的曲面不能作为测量和制图的基准面。假想一个扁率极小的椭圆,绕大地球体短轴旋转所形成的规则椭球体称之为地球椭球体。地球椭球体表面是一个规则的数学表面,可以用数学公式表达,所以在测量和制图中就用它替代地球的自然表面。因此就有了地球椭球体的概念。地球椭球体有长半径和短半径之分,长半径(a)即赤道半径,短半径(b)即极半径。f=(a-b)/a为椭球体的扁率,表示椭球体的扁平程度,a、b、f被称为地球椭球体的三要素。
在这里插入图片描述

1. 大地坐标系

大地坐标系(也叫WGS-84坐标系,LLA坐标系,经纬高坐标系,全球地理坐标系)。

任意点都可以描述为经度(Longitude)、纬度(Latitude)和高度(Altitude),也就是lla坐标。经度(单位 o ^o o)是指地球表面上某一点与本初子午线(通常取格林威治子午线)之间的角度,以东经和西经表示。纬度(单位 o ^o o)是指地球表面上某一点与赤道之间的角度,以北纬和南纬表示。高度(单位: m m m)或海拔是指某一点相对于参考面(通常是海平面)的垂直距离。

优点: 能够准确地描述地球表面上任意点的位置, 并且可以在地图上直观地表示地球的形状和特征

我们把地球椭球体和基准面结合起来看,在此我们把地球比做是“马铃薯”,表面凸凹不平,而地球椭球体就好比一个“鸭蛋”,那么按照我们前面的定义,基准面就定义了怎样拿这个“鸭蛋”去逼近“马铃薯”某一个区域的表面,X、Y、Z轴进行一定的偏移,并各自旋转一定的角度,大小不适当的时候就缩放一下“鸭蛋”,那么通过如上的处理必定可以达到很好的逼近地球某一区域的表面。

因此,从这一点上也可以很好的理解,每个国家或地区均有各自的基准面,我们通常称谓的北京54坐标系、西安80坐标系实际上指的是我国的两个大地基准面。我国参照前苏联从1953年起采用克拉索夫斯基(Krassovsky)椭球体建立了我国的北京54坐标系,1978年采用国际大地测量协会推荐的1975地球椭球体(IAG75)建立了我国新的大地坐标系–西安80坐标系,目前大地测量基本上仍以北京54坐标系作为参照,北京54与西安80坐标之间的转换可查阅国家测绘局公布的对照表。 WGS1984基准面采用WGS84椭球体,它是一地心坐标系,即以地心作为椭球体中心,目前GPS测量数据多以WGS1984为基准。

椭球体与基准面之间的关系是一对多的关系,也就是基准面是在椭球体基础上建立的,但椭球体不能代表基准面,同样的椭球体能定义不同的基准面。地球椭球体和基准面之间的关系以及基准面是如何结合地球椭球体从而实现来逼近地球表面的可以通过下图一目了然。
在这里插入图片描述

https://www.whu-cveo.com/2018/07/26/coordinate-projection/

2. eci地心惯性坐标系

在这里插入图片描述
红色O-XYZ坐标系表示地球坐标系,其中低新惯性坐标系(I系)的原点位于地球原点,Z轴沿地轴指向北极,X轴和Y轴位于赤道平面内,满足右手法则,且分别指向两个恒星。
特点:他的特点就是xy不动,不随着地球的自转而转动,可以作为地球附近传感器输出的惯性坐标系。imu检测到或者计算到探测到的加速度,角速度都是相对于地心惯性坐标系的

3. 地心地固坐标系(ECEF坐标系,E系)

在这里插入图片描述
如图,图中绿色 0 − x y z 0-xyz 0xyz坐标系即为地心地固坐标系(e系),原点位于地球原点,z轴沿着地轴指向北极,y轴沿着赤道平面与格林威治子午面的交线上,y轴在赤道平面与x轴z轴满足右手法则。
特点: 该坐标系与地球固连在一起,x轴和y轴的方向随地球自转而变化

4. 站心坐标系(ENU坐标系)

在这里插入图片描述
如图,蓝色的坐标系就是站心坐标系,在北半球被成为ENU坐标系。ENU坐标系的原点位于载体所在的地球表面,x轴和y轴在当地水平面内,分别指向东向和北向,z轴垂直向上,与x轴y轴满足右手法则。
基于地心坐标系的坐标都是很大的值,这样的值是不太方便进行空间计算的,所以很多时候可以选取一个站心点,将这个很大的值变换成一个较小的值。
另外在LLA坐标转化为ENU坐标系下的三维空间坐标系坐标的时候, 因为距离LLA原点越远,在计算三维坐标的时候距离,距离原点位置越远,误差越大。所以可以选择一个站点坐标系,将待转换的LLA点,转化到这个站点坐标系下,得到的三维空间点位置会比较准确(我的理解:误差原因在于,使用椭圆模型半径和经纬度角度差计算的弧长的时候,距离LLA原点越远,最后得到的弧长误差越大。)

5. UTM坐标系

UTM坐标系(通用横轴麦卡托投影(Universal Transverse Mercator Projection))的坐标原点位于本初子午线与赤道交点,以正东方向为x轴正方向(UTM Easting),正北方向为y轴正方向(UTM Northing)。
麦卡托投影Wiki

在这里插入图片描述

某个点在UTM坐标系下的表达方式为: 经度区纬度区以东以北,其中以东表示从经度区的中心子午线的投影距离,而以北表示距离赤道的投影距离。这个两个值的单位均为米。举例来说,使用 UTM 表示经/纬度坐标 61.44,25.40 的结果就是35V 414668 6812844;而经/纬度坐标 -47.04,-73.48 的表示结果为18G 615471 4789269。

可以简单理解UTM坐标系的原理为:UTM(Universal Transverse Mercator)坐标系是一种平面直角坐标系,用于将地球表面的点表示为二维坐标。它将地球的表面分成60个纵向的带,每个带为6度经度,然后将每个带内的点投影到一个平面上,使得该带内的经线变为垂直于横轴的直线,这样可以消除地球表面的曲率。 这就是麦卡托投影的基本原理。

  • 麦卡托投影的畸变
    由于麦卡托投影在高纬度过分放大,低纬度又过分缩小,因此会产生有趣的错觉。
    比如世界第一大岛高纬度的格陵兰比澳洲看起来还大好几倍。
    世界第二大岛低纬度的新几内亚和日本差不多大小,然而新几内亚岛面积足足是日本的2倍。
麦卡托投影展开后的世界地图不同纬度相同大小球形的形变比较
在这里插入图片描述在这里插入图片描述

所以在应用中,涉及UTM系的表达的时候,会使用一个局部的LTM系,这样相当于以LTM系为原点展开,距离LTM原点较近(几百米)的时候,基本上无形变

6. LTM坐标系

LTM坐标系是局部切面坐标系(Local Tangent Plane),通常用于描述地球表面上某一点周围的局部地理信息,特别是在地图制作、航空航天、地理勘测、天文观测等领域中常被使用。这个坐标系是相对于某一点(通常是观测点或参考点)建立的局部坐标系,以该点为中心建立的坐标系,可以更准确地描述该点周围的地理位置和方向。

ENU坐标系(East-North-Up)和LTM坐标系(Local Tangent Plane)都是用于描述局部区域的局部坐标系,但它们之间有一些区别:

  • 坐标轴方向:
    ENU坐标系的坐标轴方向是东(East)、北(North)和向上(Up)。其中,东方向(E)指的是与地球表面的经线方向平行的方向,北方向(N)指的是与地球表面的纬线方向平行的方向,向上(U)指的是垂直于地球表面向上的方向。
    LTM坐标系的坐标轴方向取决于局部区域的特定情况,它是相对于某一点的局部坐标系。通常情况下,LTM坐标系的坐标轴方向与ENU坐标系的方向是一致的,即东、北和向上

  • 原点选择:
    ENU坐标系的原点可以是任意选择的点,通常选择为局部区域的某个参考点或中心点。
    LTM坐标系的原点也是局部区域的某个参考点或中心点,它是相对于该点建立的局部坐标系。

  • 应用领域:
    ENU坐标系常用于导航、飞行控制、地图制作等领域,特别是描述移动物体相对于参考点的位置和方向。
    LTM坐标系常用于地理勘测、航空航天、天文观测等领域,描述局部区域内的地理信息或天文观测点的位置和方向。

  • 数学表示:
    ENU坐标系的坐标通常用三维直角坐标表示,即以东、北、向上的分量表示位置。
    LTM坐标系的坐标也是三维直角坐标,通常与ENU坐标系一样以东、北、向上的分量表示位置。
    总的来说,ENU坐标系和LTM坐标系都是用于描述局部区域的局部坐标系,它们的主要区别在于坐标轴方向的定义和应用领域的不同。

项目中,LTM坐标系原点可以设定为项目位置所在的区域的某个位置(LTM系的坐标轴方向一般情况下与ENU坐标系一致), World系可以定义为系统开机的第一帧IMU位置(方向可以设置为第一帧的姿态,IMU相对LTM系或者ECEF系的姿态,这个具体可以根据需求设定)

7. IMU坐标系

车辆的IMU(惯性测量单元)用于测量车辆的姿态(姿势)信息,包括角速度(角速度测量单位为弧度/秒)和加速度(加速度测量单位为米/秒^2)。IMU可以通过姿态更新算法来估计车辆的姿态,从而实现车身的姿态跟踪(Vehicle Pose Tracking)。

以下是车身IMU的姿态更新原理的基本步骤和算法:

  • 数据获取:
    IMU通过其内置的加速度计和陀螺仪获取车辆的角速度和加速度数据。
    角速度数据用于计算车辆在空间中的角度变化,而加速度数据则可以用于辅助姿态估计。

  • 姿态估计:
    初始时刻,可以根据IMU的加速度计数据估计车辆的倾斜角(pitch)和侧倾角(roll)。
    姿态估计通常采用互补滤波器(Complementary Filter)或卡尔曼滤波器(Kalman Filter)等算法,结合角速度和加速度数据来估计车辆的姿态。

  • 姿态更新:
    姿态更新是指根据IMU获取的最新数据对车辆的姿态进行更新。
    角速度数据可以用于连续地更新车辆的姿态,例如通过积分计算角度变化并更新姿态。
    加速度数据可以用于校准姿态估计结果,例如检测车辆的加速度变化来调整姿态估计的误差。

  • 姿态跟踪:
    通过持续的姿态更新,车辆的IMU可以实现对车身姿态的跟踪,包括倾斜角、偏航角等信息。
    姿态跟踪对于车辆的导航、控制和定位非常重要,可以用于实现车辆的姿态稳定控制、路径规划和定位校准等功能。
    总的来说,车身IMU的姿态更新原理基于获取的角速度和加速度数据,结合姿态估计算法和姿态更新算法,持续地更新车辆的姿态信息,从而实现姿态的跟踪和稳定控制。

在实际应用中,可以将开机时第一帧的IMU姿态作为基准姿态或初始姿态(Reference Pose),然后根据后续每一帧的IMU数据计算相对于基准姿态的相对姿态(Relative Pose)或增量姿态(Incremental Pose)。
这种相对姿态的计算通常采用姿态积分(Orientation Integration)的方法,通过累积角速度数据来估计车辆每一时刻的姿态变化。这样可以实现车辆在运动过程中的姿态跟踪,并相对于初始姿态计算出相对的姿态信息。
需要注意的是,姿态积分过程中可能会积累误差,特别是在长时间运行或高动态环境下。为了减小误差的累积,通常会结合其他传感器(如GPS、磁力计等)进行姿态校正或更新,以提高姿态估计的准确性和稳定性。

8. 代码部分

8.1 LLA(大地坐标系坐标、经纬度海拔)坐标转LTM系(ENU系)下的三维笛卡尔坐标

  1. 已知LTM坐标系原点的LLA坐标 O l l a O_lla Olla、点P为LTM系下的一个点,点P的LLA坐标为 P l l a P_lla Plla, 求点P的LTM系坐标 P l t m P_ltm Pltm
#include <iostream>
#include <cmath>struct LLA {double longitude;double latitude;double altitude;
};struct LTM {double x;double y;double z;
};// Convert LLA to LTM
// 把LLA坐标转化为LTM下的三维笛卡尔坐标
LTM llaToLTM(const LLA& origin, const LLA& point) {const double earthRadius = 6371008.8;  // Earth's radius in metersdouble deltaLongitude = (point.longitude - origin.longitude) * (M_PI / 180.0);double deltaLatitude = (point.latitude - origin.latitude) * (M_PI / 180.0);double x = earthRadius * deltaLongitude * std::cos(origin.latitude * (M_PI / 180.0));double y = earthRadius * deltaLatitude;double z = point.altitude - origin.altitude;return {x, y, z};
}int main() {// LTM坐标系原点对应的LLA坐标LLA origin {115.97040640463173133, 32.328346393150695803, 0.9999999722222 };// LTM系下的四个点的LLA坐标LLA point1 { 115.968928, 32.327075 };LLA point2 { 115.968938, 32.326945 };LLA point3 { 115.9676360, 32.3269011 };LLA point4 { 115.9676295, 32.3270185 };LTM ltmPoint1 = llaToLTM(origin, point1);LTM ltmPoint2 = llaToLTM(origin, point2);LTM ltmPoint3 = llaToLTM(origin, point3);LTM ltmPoint4 = llaToLTM(origin, point4);std::cout << "LTM coordinates of point 1: (" << ltmPoint1.x << ", " << ltmPoint1.y << ", " << ltmPoint1.z << ")\n";std::cout << "LTM coordinates of point 2: (" << ltmPoint2.x << ", " << ltmPoint2.y << ", " << ltmPoint2.z << ")\n";std::cout << "LTM coordinates of point 3: (" << ltmPoint3.x << ", " << ltmPoint3.y << ", " << ltmPoint3.z << ")\n";std::cout << "LTM coordinates of point 4: (" << ltmPoint4.x << ", " << ltmPoint4.y << ", " << ltmPoint4.z << ")\n";return 0;
}
  1. 已知LTM坐标系原点的LLA坐标 O l l a O_lla Olla,以及自车相对于LLA坐标系的姿态角(roll/pitch/yaw),点P为LTM系下的一个点,点P的LLA坐标为 P l l a P_lla Plla, 求点P的LTM系坐标 P l t m P_ltm Pltm
#include <iostream>
#include <cmath>struct LLA {double longitude;double latitude;double altitude;
};struct LTM {double x;double y;double z;
};// Convert degrees to radians
double degToRad(double degrees) {return degrees * (M_PI / 180.0);
}// Convert LLA to LTM
LTM llaToLTM(const LLA& originLLA, const LLA& pointLLA, double roll, double pitch, double yaw) {const double earthRadius = 6371000.0;  // Earth's radius in meters// Convert roll, pitch, and yaw from degrees to radiansdouble rollRad = degToRad(roll);double pitchRad = degToRad(pitch);double yawRad = degToRad(yaw);// Convert LLA to ECEF (Earth-Centered, Earth-Fixed) coordinatesdouble deltaLongitude = (pointLLA.longitude - originLLA.longitude) * (M_PI / 180.0);double deltaLatitude = (pointLLA.latitude - originLLA.latitude) * (M_PI / 180.0);double cosYaw = cos(yawRad);double sinYaw = sin(yawRad);double cosPitch = cos(pitchRad);double sinPitch = sin(pitchRad);double cosRoll = cos(rollRad);double sinRoll = sin(rollRad);double x = deltaLongitude * earthRadius * cos(originLLA.latitude * (M_PI / 180.0)) * cosYaw +deltaLatitude * earthRadius * sinYaw +(pointLLA.altitude - originLLA.altitude) * cosYaw * cosPitch;double y = -deltaLongitude * earthRadius * cos(originLLA.latitude * (M_PI / 180.0)) * sinYaw +deltaLatitude * earthRadius * cosYaw -(pointLLA.altitude - originLLA.altitude) * sinYaw * cosPitch;double z = deltaLongitude * earthRadius * sin(originLLA.latitude * (M_PI / 180.0)) +deltaLatitude * earthRadius * sinPitch +(pointLLA.altitude - originLLA.altitude) * cosPitch;return {x, y, z};
}int main() {LLA originLLA {115.97040640463173133, 32.328346393150695803, 0.9999999722222  };double roll = 0;double pitch = 0;double yaw = 0;LLA point1LLA { 115.968928, 32.327075 };LLA point2LLA { 115.968938, 32.326945 };LLA point3LLA { 115.9676360, 32.3269011 };LLA point4LLA { 115.9676295, 32.3270185 };LTM ltmPoint1 = llaToLTM(originLLA, point1LLA, roll, pitch, yaw);LTM ltmPoint2 = llaToLTM(originLLA, point2LLA, roll, pitch, yaw);LTM ltmPoint3 = llaToLTM(originLLA, point3LLA, roll, pitch, yaw);LTM ltmPoint4 = llaToLTM(originLLA, point4LLA, roll, pitch, yaw);std::cout << "LTM coordinates of point 1: (" << ltmPoint1.x << ", " << ltmPoint1.y << ", " << ltmPoint1.z << ")\n";std::cout << "LTM coordinates of point 2: (" << ltmPoint2.x << ", " << ltmPoint2.y << ", " << ltmPoint2.z << ")\n";std::cout << "LTM coordinates of point 3: (" << ltmPoint3.x << ", " << ltmPoint3.y << ", " << ltmPoint3.z << ")\n";std::cout << "LTM coordinates of point 4: (" << ltmPoint4.x << ", " << ltmPoint4.y << ", " << ltmPoint4.z << ")\n";return 0;
}

辨析:自车在LLA坐标系下的pose和LTM坐标系下的pose辨析
在LLA(经纬度高度)坐标系和LTM(局部切面坐标系)坐标系下,车辆的roll(横滚角)、pitch(俯仰角)和yaw(偏航角)可能有不同的定义和计算方式,具体取决于坐标系的定义和使用场景。

  • LLA坐标系下的姿态角:
    在LLA坐标系中,通常roll、pitch和yaw角度是相对于地球表面的经度、纬度和高度来定义的。
    Roll(横滚角)通常指车辆绕其纵轴(对应地球上的经度轴)的旋转角度。
    Pitch(俯仰角)通常指车辆绕其横轴(对应地球上的纬度轴)的旋转角度。
    Yaw(偏航角)通常指车辆绕其竖轴(对应地球上的垂直轴)的旋转角度。

  • LTM坐标系下的姿态角:
    在LTM坐标系中,roll、pitch和yaw角度的定义取决于局部坐标系的建立方式和使用场景。
    Roll、pitch和yaw角度通常是相对于LTM坐标系的局部坐标轴来定义的,具体取决于LTM坐标系的建立方式和物体在局部坐标系中的方向。

辨析2:
平时说到的坐标系A到坐标系B的变换 T A B ( T A 2 B ) T_A^B(T_{A2B}) TAB(TA2B)的含义:点P在坐标系A中的坐标值转化到坐标系B中

T A B T_A^B TAB是在数学中的表达,代码中大概率会写为 T A 2 B T_{A2B} TA2B

以坐标系A到坐标系B的变换为例,指的是点P在坐标系A中的坐标 P A P_A PA,将其转化到坐标系B中。这里目标坐标系为B。变换矩阵的可以理解为坐标系B做了什么变换可以与坐标系A完全重叠。

8.2 LLA坐标转化为ECEF坐标

在这里插入图片描述LLA坐标系下的 ( l o n , l a t , a l t ) (lon,lat,alt) (lon,lat,alt)转换为ECEF坐标系下点 ( X E C E F , Y E C E F , Z E C E F ) (X_{ECEF},Y_{ECEF},Z_{ECEF}) (XECEFYECEFZECEF)
{ X = ( N + alt  ) cont ⁡ ( lat  ) cos ⁡ ( lon  ) Y = ( N + alt  ) cos ⁡ ( lat  ) sin ⁡ ( lon  ) Z = ( N ( 1 − f ) 2 + alt  ) sin ⁡ ( lat  ) ) \left\{\begin{array}{c} X=(N+\text { alt }) \operatorname{cont}(\text { lat }) \cos (\text { lon }) \\ Y=(N+\text { alt }) \cos (\text { lat }) \sin (\text { lon }) \\ \left.Z=\left(N(1-f)^{2}+\text { alt }\right) \sin (\text { lat })\right) \end{array}\right. X=(N+ alt )cont( lat )cos( lon )Y=(N+ alt )cos( lat )sin( lon )Z=(N(1f)2+ alt )sin( lat ))
其中,f为极扁率,N为基准椭球体的曲率半径
N = a 1 − f ( 2 − f ) ∗ sin ⁡ 2 ( l a t ) N=\frac{a}{\sqrt{1-f(2-f) * \sin ^{2}(l a t)}} N=1f(2f)sin2(lat) a
一开始lon是未知的,可以假设为0,经过计策迭代之后就能收敛

8.3 ECEF坐标系转LLA坐标系

ECEF坐标系下点 ( X E C E F , Y E C E F , Z E C E F ) (X_{ECEF},Y_{ECEF},Z_{ECEF}) (XECEFYECEFZECEF)转换为LLA坐标系下的 ( l o n , l a t , a l t ) (lon,lat,alt) (lon,lat,alt)
lon  = arctan ⁡ ( y x ) alt  = p cos ⁡ ( lat ⁡ ) − N lat  = arctan ⁡ [ z p ( 1 − e 2 N N + a l t ) − 1 ] p = x 2 + y 2 \begin{array}{c} \text { lon }=\arctan \left(\frac{y}{x}\right) \\ \text { alt }=\frac{p}{\cos (\operatorname{lat})-N} \\ \text { lat }=\arctan \left[\frac{z}{p}\left(1-e^{2} \frac{N}{N+a l t}\right)^{-1}\right] \\ p=\sqrt{x^{2}+y^{2}} \end{array}  lon =arctan(xy) alt =cos(lat)Np lat =arctan[pz(1e2N+altN)1]p=x2+y2

8.4 ENU坐标系与LLA坐标系之间的转换

记站心坐标系(ENU坐标系)的原点P在地心地固坐标系(ECEF)下的坐标为 ( X p , Y p , Z p ) (X_p,Y_p,Z_p) (Xp,Yp,Zp)这个坐标也可以直接通过点P的经纬度计算出来,见7.2,点P所在的经纬度是 ( L , B ) (L,B) (L,B)
记ENU坐标系到ECEF坐标系的平移矩阵为 t E N U E C E F t_{ENU}^{ECEF} tENUECEF,反之ECEF坐标系到ENU坐标系的平移矩阵为 t E C E F E N U = t_{ECEF}^{ENU}= tECEFENU= − t E N U E C E F -t_{ENU}^{ECEF} tENUECEF;ENU坐标系到ECEF坐标系的旋转矩阵为 R E N U E C E F R_{ENU}^{ECEF} RENUECEF,反之ECEF坐标系到ENU坐标系的平移矩阵为 R E C E F E N U = R_{ECEF}^{ENU}= RECEFENU= ( R E N U E C E F ) − 1 {(R_{ENU}^{ECEF}})^{-1} (RENUECEF)1

ENU转换到ECEF系,先旋转再平移。ECEF系转换到ENU系,先平移后旋转。旋转过程如下:

  • 从ENU转换到ECEF
    首先,绕X轴旋转 p i 2 − B { pi \over 2} - B 2piB,然后绕Z轴旋转 p i 2 + L { pi \over 2} + L 2pi+L。得,
    R x ( θ 1 ) = [ 1 0 0 0 0 c o s θ 1 − s i n θ 1 0 0 s i n θ 1 cos ⁡ θ 1 0 0 0 0 1 ] , R z ( θ 2 ) = [ c o s θ 2 − s i n θ 2 0 0 s i n θ 2 c o s θ 2 0 0 0 0 1 0 0 0 0 1 ] R_x (\theta_1) = \begin{bmatrix} 1 & 0 & 0 & 0 \\ 0 & cos\theta_1 & -sin\theta_1 & 0\\ 0 & sin\theta_1 & \cos\theta_1 & 0\\ 0 & 0 & 0 & 1 \end{bmatrix} , R_z (\theta_2) = \begin{bmatrix} cos\theta_2 & -sin\theta_2 & 0 & 0 \\ sin\theta_2 & cos\theta_2 & 0 & 0\\ 0 & 0 & 1 & 0\\ 0 & 0 & 0 & 1 \end{bmatrix} Rx(θ1)= 10000cosθ1sinθ100sinθ1cosθ100001 ,Rz(θ2)= cosθ2sinθ200sinθ2cosθ20000100001
    其中, θ 1 = p i 2 − B \theta_1={ pi \over 2} - B θ1=2piB θ 2 = p i 2 + L \theta_2={ pi \over 2} + L θ2=2pi+L
    进一步推导得,

R E N U E C E F = R z ( p i 2 + L ) ⋅ R x ( p i 2 − B ) = [ − sin ⁡ L − sin ⁡ B cos ⁡ L cos ⁡ B cos ⁡ L 0 cos ⁡ L − sin ⁡ B sin ⁡ L cos ⁡ B sin ⁡ L 0 0 cos ⁡ B sin ⁡ B 0 0 0 0 1 ] R_{ENU}^{ECEF} = R_z({ pi \over 2} + L)\cdot R_x({ pi \over 2} - B)=\left[\begin{array}{cccc} -\sin L & -\sin B \cos L & \cos B \cos L & 0 \\ \cos L & -\sin B \sin L & \cos B \sin L & 0 \\ 0 & \cos B & \sin B & 0 \\ 0 & 0 & 0 & 1 \end{array}\right] RENUECEF=Rz(2pi+L)Rx(2piB)= sinLcosL00sinBcosLsinBsinLcosB0cosBcosLcosBsinLsinB00001

  • 从ECEF转换到ENU
    首先,绕Z轴旋转 − ( p i 2 + L ) -({ pi \over 2} + L) (2pi+L),然后绕X轴旋转 − ( p i 2 − B ) -({ pi \over 2} - B) (2piB)。得,

R E C E F E N U = R x ( − ( p i 2 − B ) ) ⋅ R z ( − ( p i 2 + L ) ) = R E C E F E N U − 1 = R − 1 = [ − sin ⁡ L cos ⁡ L 0 0 − sin ⁡ B cos ⁡ L − sin ⁡ B sin ⁡ L cos ⁡ B 0 cos ⁡ B cos ⁡ L cos ⁡ B sin ⁡ L sin ⁡ B 0 0 0 0 1 ] R_{ECEF}^{ENU} =R_x(-({ pi \over 2} - B)) \cdot R_z(-({ pi \over 2} + L)) = {R_{ECEF}^{ENU}}^{-1} = R^{-1}=\left[\begin{array}{cccc} -\sin L & \cos L & 0 & 0 \\ -\sin B \cos L & -\sin B \sin L & \cos B & 0 \\ \cos B \cos L & \cos B \sin L & \sin B & 0 \\ 0 & 0 & 0 & 1 \end{array}\right] RECEFENU=Rx((2piB))Rz((2pi+L))=RECEFENU1=R1= sinLsinBcosLcosBcosL0cosLsinBsinLcosBsinL00cosBsinB00001


综上所述,
ENU坐标系转到ECEF坐标系的齐次变换矩阵 T E N U E C E F T_{ENU}^{ECEF} TENUECEF为,
T E N U E C E F = t E N U E C E F ⋅ R E N U E C E F = [ 1 0 0 X p 0 1 0 Y p 0 0 1 Z p 0 0 0 1 ] [ − sin ⁡ L − sin ⁡ B cos ⁡ L cos ⁡ B cos ⁡ L 0 cos ⁡ L − sin ⁡ B sin ⁡ L cos ⁡ B sin ⁡ L 0 0 cos ⁡ B sin ⁡ B 0 0 0 0 1 ] T_{ENU}^{ECEF}=t_{ENU}^{ECEF} \cdot R_{ENU}^{ECEF}=\left[\begin{array}{cccc} 1 & 0 & 0 & X_{p} \\ 0 & 1 & 0 & Y_{p} \\ 0 & 0 & 1 & Z_{p} \\ 0 & 0 & 0 & 1 \end{array}\right]\left[\begin{array}{cccc} -\sin L & -\sin B \cos L & \cos B \cos L & 0 \\ \cos L & -\sin B \sin L & \cos B \sin L & 0 \\ 0 & \cos B & \sin B & 0 \\ 0 & 0 & 0 & 1 \end{array}\right] TENUECEF=tENUECEFRENUECEF= 100001000010XpYpZp1 sinLcosL00sinBcosLsinBsinLcosB0cosBcosLcosBsinLsinB00001

ECEF坐标系转到ENU坐标系的齐次变换矩阵 T E C E F E N U T_{ECEF}^{ENU} TECEFENU为,
T E C E F E N U = t E C E F E N U ⋅ R E C E F E N U = [ − sin ⁡ L cos ⁡ L 0 0 − sin ⁡ B cos ⁡ L − sin ⁡ B sin ⁡ L cos ⁡ B 0 cos ⁡ B cos ⁡ L cos ⁡ B sin ⁡ L sin ⁡ B 0 0 0 0 1 ] [ 1 0 0 − X p 0 1 0 − Y p 0 0 1 − Z p 0 0 0 1 ] T_{ECEF}^{ENU}=t_{ECEF}^{ENU} \cdot R_{ECEF}^{ENU}=\left[\begin{array}{cccc} -\sin L & \cos L & 0 & 0 \\ -\sin B \cos L & -\sin B \sin L & \cos B & 0 \\ \cos B \cos L & \cos B \sin L & \sin B & 0 \\ 0 & 0 & 0 & 1 \end{array}\right]\left[\begin{array}{cccc} 1 & 0 & 0 & -X_{p} \\ 0 & 1 & 0 & -Y_{p} \\ 0 & 0 & 1 & -Z_{p} \\ 0 & 0 & 0 & 1 \end{array}\right] TECEFENU=tECEFENURECEFENU= sinLsinBcosLcosBcosL0cosLsinBsinLcosBsinL00cosBsinB00001 100001000010XpYpZp1

9. 引用

https://blog.csdn.net/ohayiye/article/details/120973844
https://cloud.tencent.com/developer/article/1888676
https://www.whu-cveo.com/2018/07/26/coordinate-projection/

本文来自互联网用户投稿,该文观点仅代表作者本人,不代表本站立场。本站仅提供信息存储空间服务,不拥有所有权,不承担相关法律责任。如若转载,请注明出处:http://www.mzph.cn/news/790721.shtml

如若内容造成侵权/违法违规/事实不符,请联系多彩编程网进行投诉反馈email:809451989@qq.com,一经查实,立即删除!

相关文章

线上线下陪玩,APP小程序H5。源码交付,支持二开!

线下陪玩的风险与管理方式 1、陪玩者的身心健康风险 线下陪玩的模式决定了陪玩者需要与不同的需求方见面&#xff0c;并满足他们的陪伴和娱乐需求。这种工作方式可能会给陪玩者带来身心上的压力和负担。因为陪玩者需要面对各种需求方的要求&#xff0c;有时还需要虚拟出一种完…

HTML - 你知道b与strong标签的区别吗

难度级别&#xff1a;初级及以上 提问概率&#xff1a;50% 不单单是初学者&#xff0c;即便是有好几年工作经验的前端开发工作者&#xff0c;也会有一大部分人把这两个标签搞混&#xff0c;甚至在工作中&#xff0c;很大一部人不会使用这两个标…

212 基于matlab的双稳态随机共振的算法

基于matlab的双稳态随机共振的算法&#xff0c;分析信噪比随系统参数a,b及乘性噪声和加性噪声的增益变化曲线&#xff0c;60个数据样本可供选择。程序已调通&#xff0c;可直接运行。 212 双稳态随机共振 信噪比增益变化曲线 - 小红书 (xiaohongshu.com)

浅谈分布式光伏电站的运维管理

摘要&#xff1a;随着近些年我国对节能降耗关注力度的持续加大&#xff0c;为满足人们不断增长的电能需求&#xff0c;光伏发电产业得到迅猛发展&#xff0c;其中分布式光伏发电的比重持续增长。在打赢脱贫攻坚战的大背景下&#xff0c;国家电网公司探索出一条“阳光扶贫”的扶…

Spring Cloud微服务入门(一)

微服务的演变过程 //controller 视图交互层 前端数据处理传给service //1.DAO RequestBody userDAO String id;String type; {"id":"lcs", "type":"lcs"} //2.GET请求 findUserById&#xff1f;id1&typelcsRequestParam String …

5.3.1 配置交换机 SSH 管理和端口安全

5.3.1 实验1:配置交换机基本安全和 SSH管理 1、实验目的 通过本实验可以掌握&#xff1a; 交换机基本安全配置。SSH 的工作原理和 SSH服务端和客户端的配置。 2、实验拓扑 交换机基本安全和 SSH管理实验拓扑如图所示。 交换机基本安全和 SSH管理实验拓扑 3、实验步骤 &a…

全面的Docker快速入门教程(详细)

前言&#xff1a; 都2024年了&#xff0c;你还在为了安装一个开发或者部署环境、软件而花费半天的时间吗&#xff1f;你还在解决开发环境能够正常访问&#xff0c;而发布测试环境无法正常访问的问题吗&#xff1f;你还在为持续集成和持续交付&#xff08;CI / CD&#xff09;工…

flink1.18源码本地调试环境

01 源码本地调试环境搭建 1. 从github拉取源码创建本地项⽬ https://github.com/apache/flink.git 可以拉取github上官⽅代码 https://github.com/apache/flink.git GitHub - apache/flink: Apache Flink 2. 配置编译环境 ctrlaltshifts &#xff08;或菜单&#xff09;打…

OpenHarmony Neptune开发板-MQTT连接华为IoT平台

本示例将演示如何在Neptune开发板上使用MQTT协议连接华为IoT平台,使用的是ATH20温湿度传感器模块与Neptune开发板 本示例实现AHT20温湿度数据上报华为IoT平台,IoT平台下发命令控制LED灯的开关 使用W800 SDK功能包中libemqtt来实现连接华为IoT平台 程序设计 初始化 一、MQT…

上位机图像处理和嵌入式模块部署(qmacvisual亮度检测)

【 声明&#xff1a;版权所有&#xff0c;欢迎转载&#xff0c;请勿用于商业用途。 联系信箱&#xff1a;feixiaoxing 163.com】 前面我们说过&#xff0c;在机器视觉当中&#xff0c;对于光源的处理要非常小心。这里面不仅包括了选择什么样的光源&#xff0c;还取决于怎样使用…

WPF文本框TextEdit不以科学计数法显示

WPF文本框TextEdit不以科学计数法显示 一个float或者double类型的数值&#xff0c;如果小数点后0的个数≥4&#xff0c;在界面上就会自动以科学计数法显示&#xff0c; 比如&#xff1a;0.00003会显示成这样 但是很多时候我并不希望它这样显示&#xff0c;因为这样不方便编辑…

Python 基于 OpenCV 视觉图像处理实战 之 OpenCV 简单视频处理实战案例 之一 简单视频放大抖动效果

Python 基于 OpenCV 视觉图像处理实战 之 OpenCV 简单视频处理实战案例 之一 简单视频放大抖动效果 目录 Python 基于 OpenCV 视觉图像处理实战 之 OpenCV 简单视频处理实战案例 之一 简单视频放大抖动效果 一、简单介绍 二、简单视频放大抖动效果实现原理 三、简单视频放大…

C# WPF编程-命令

C# WPF编程-命令 概述WPF命令模型ICommand接口RoutedCommand类RoutedUICommand类命令库 概述 使用路由事件可以响应广泛的鼠标和键盘事件&#xff0c;这些事件是低级的元素。在实际应用程序中&#xff0c;功能被划分成一些高级的任务。这些任务可通过各种不同的动作和用户界面…

Spring Boot 学习(2)——HelloWorld

HelloWorld&#xff01;全宇宙码农的第一个&#xff08;行&#xff09;程序&#xff08;代码&#xff09;。 1、创建项目 打开idea&#xff0c;新建一个maven项目。 1&#xff09;选择项目sdk&#xff08;本例是1.8&#xff09; 2&#xff09;输入GroupId&#xff08;co…

阿里云服务器租用价格表,100元可以买哪些配置?

2024年阿里云服务器优惠价格表&#xff0c;一张表整理阿里云服务器最新报价&#xff0c;阿里云服务器网aliyunfuwuqi.com整理云服务器ECS和轻量应用服务器详细CPU内存、公网带宽和系统盘详细配置报价单&#xff0c;大家也可以直接移步到阿里云CLUB中心查看 aliyun.club 当前最新…

CEF的了解

(14 封私信 / 80 条消息) CEF和Electron的区别是什么&#xff1f; - 知乎 (zhihu.com) Electron面向的开发者&#xff1a;会用JavaScript,HTML,CSS&#xff0c;不会C CEF面向的开发者&#xff1a;会用JavaScript,HTML,CSS&#xff0c;会C (14 封私信 / 80 条消息) liulun - …

认识什么是Git

目录 1. 认识Git 1.1. 问题引入 1.2. 概念 1.3. 作用 1.4. 如何学 1.5. Git 安装 1.6. Git配置用户信息 2. Git仓库 2.1. Git 仓库&#xff08;repository&#xff09; 2.2. 创建 2.3. 需求 3. Git的三个区域 3.1. Git 使用时的三个区域 3.2. 工作区的内容&#…

怎么快速上手虚拟化(容器)技术——以 Docker 为例

Docker 整体介绍 Docker 是一种使用 Go 语言开发的容器工具。所谓容器&#xff0c;实际上是一种虚拟化技术&#xff0c;用于为应用提供虚拟化的运行环境&#xff0c;相较于虚拟机具有轻量级、低延迟的特性。 下面是对上述介绍的说明&#xff1a; 应用程序运行需要一定的依赖…

HTML - 你如何使H5页面禁止手动缩放

难度级别:初级及以上 提问概率:40% 我们知道,这道题其实是在考察meta标签的viewport属性,正常情况下设置viewport的代码为 <head><meta name="viewport" content="width=device-width,initial-scale=1.0" …

【MATLAB】PSO_BP神经网络时序预测算法

有意向获取代码&#xff0c;请转文末观看代码获取方式~ 1 基本定义 PSO_BP神经网络时序预测算法是一种结合了粒子群优化(PSO)算法和反向传播(BP)神经网络的时序预测方法。它利用了PSO算法的全局搜索能力和BP神经网络的优化能力&#xff0c;能够更准确地预测时序数据。 具体步…