常用导航坐标系 及 转换关系 （理论+程序）

一、坐标系定义与符号约定

在捷联惯导中，涉及到多种坐标系，其中 惯性坐标系、地心地固坐标系、导航坐标系 的示意图如下：

 

 

 

下面给出常用的坐标系的定义 ：

1）惯性坐标系（i 系）：

以地球质心为原点， 轴指向地球自转轴， 轴位 于赤道面指向空间任意点， 轴与其构成右手系。

该坐标系不随地球自转而转动， 但是由于地球质心绕太阳公转以及太阳系绕银河系公转，因此，该系

不是绝对惯 性系， 然而这些影响十分微弱，低于惯导的噪声水平， 因而可以忽略不计，该系 可以认

为 是一个惯性系。

 

2）地心地固系（e 系）：

与大地测量中的 ECEF 系一致

 

理解：某一时刻，载体在e系中的坐标 在 i 系的表达，可通过方向余弦矩阵（ Direction Cosine Matrix, DCM）

实现，其表达如下：

 

 

 

3）地理坐标系 （Geographic Coordinate System）：

是使用三维球面来定义地球表面位置，以实现通过 经纬度 对地球表面点位引用的坐标系。

一个地理坐标系包括 角度测量单位、本初子午线和参考椭球体三部分。在球面系统中，水平线是等

纬度线或纬线。垂直线是等经度线或经线。地理坐标系依据其所选用的本初子午线、参考椭球的不

同而略有区别。地理坐标系可以确定地球上任何一点的位置。首先将地球抽象成一个规则的逼近原

始自然地球表面的椭球体，称为参考椭球体，然后在参考椭球体上定义一系列的经线和纬线构成经

纬网，从而达到通过经纬度来描述地表点位的目的。需要说明的是经纬地理坐标系不是平面坐标系，

因为度不是标准的长度单位，不可用其直接量测面积长度。

 

地理坐标系 与 地心地固系（e 系）之间的转换关系如下：

程序如下：

//地理坐标系 转换到 地心地固坐标系
Eigen::Vector3d GlobalOptimization::BLH2XYZWGS84(const Eigen::Vector3d &BLH) // rad,rad,m
{
    Eigen::Vector3d XYZ;
	const static double a = 6378137.0, f = 1 / 298.257223563;
	double e2 = f * (2 - f);
	double sinfai = sin(BLH[0]);
	double cosfai = cos(BLH[0]);
	double sinlam = sin(BLH[1]);
	double coslam = cos(BLH[1]);

	double N = a / sqrt(1 - e2 * sinfai * sinfai);

	XYZ[0] = (N + BLH[2]) * cosfai * coslam;
	XYZ[1] = (N + BLH[2]) * cosfai * sinlam;
	XYZ[2] = (N * (1 - e2) + BLH[2]) * sinfai;
    
    return XYZ;
}

 

4）当地水平坐标系（L 系）：

原点位于载体质心， 轴沿参考椭球卯酉圈 指向东 E ， 轴沿参考椭球子午线指向北 N ， 轴沿参考椭

球法线指向天 U ， 从而形成 ENU 坐标系

 

 

5）导航坐标系（n 系）：

是惯性导航算法的基本参考系，运动物体在导航坐标系内进行位置，速度，姿态确 定，可选取 e 系

或 L 系做为导航系。 在 e 系内导航，可以直接确定地心地固系下 的导航参数， 便于和 GNSS 等大地

测量手段相结合，且动态模型简单，但不 利于 本质规律的研究。而在 L 系下导航，物理意义明确，

便于理论分析和误差规律探 寻，利于内部控制， 但动态模型复杂， 且不能直接与 GNSS 等大地测量

手段相结 合。对于组合导航解算，应选用 e 系，而对于理论分析，应选用 L 系；

 

一般采用当地水平坐标系 L，采用 NED 坐标系作为导航坐标系

 

 

 

 

6）载体坐标系（b 系）：

捷联惯导硬件内部定义了坐标系，其原点一般位于 硬件中心，而惯性元件安装在三个正交方向上形成

XYZ 轴。 将捷联惯导安装在 载体上后， 惯导硬件的坐标系就成为载体的坐标系，两者固联在一起。

一般将 Z 轴朝上， Y 轴朝前进方向， X 轴沿前进方向朝右

 

 

7）平台坐标系（p 系）、

在平台式惯导中，惯导通过自身调节，始终维持为 一个水平指北平台，这个就是 p 系，但由于各种

误差的影响， p 系与真实的 L 系 并不重合， 两者存在一个失准角 。在捷联惯导中，这种平台由计算

平台所取代，计算平台由 b 系转向 n 的姿态矩阵 来维持，由于解算误差的存在，所计算得 到的是存

在失准角的姿态矩阵 ， 与 存在一个失准角 。

 

 

8）计算机坐标系（c 系）：

L 系是由实际经纬度确定的当地坐标系，而惯导 自身可以解算出带有误差的经纬度 和 ，由 和 确定的

当地坐标 系称为计 算机坐标系。 c 系实质是模型简化过程中衍生出来的一个坐标系，并没有具体的

物理解释，在后续导 出 角误差模型时会详细说明。 在推导惯导误差模型时， 清晰自明的符号约定是十

分重要的，符号的混淆与 定义 不一致往往导致很多文献结果的不一致， 在相互引用时不能自洽。 

 

二、坐标系旋转

描述两个坐标系之间的旋转关系可以采用欧拉角、旋转矩阵、四元数和旋转 矢量四种数学工具 [57] 。

欧拉角 和旋转矩阵有密切关系， 两者统称为转角系统。 转角系统定义有如下 3 个要素：​​​​​​​1）旋转顺序；

2 ）欧拉角的 符号与域值定义； 3 ）奇点问题。  转角系统定义如图 3.2 ：

旋转顺序为 Z -> X -> Y ，第一次绕 Z 轴旋转 ，称为航向角 Yaw，第二次 X 绕 轴旋转 ，称为俯仰角 Pitch，

第三次绕 Y 轴旋转 ，称为翻滚角 Roll。 得 到的单轴旋转矩阵分别为：

欧拉角的值域为：

 

 

 

 

三、程序

大地坐标系(WGS-84)、地心地固坐标系(ECEF) 与 东北天坐标系(ENU)的相互转换C语言代码 

//convert.h
#include
#include
#include

#define M_PI 3.1415926

const double dSemiMajorAxis = 6378137.0;//椭球长半轴
const double e2 = 0.0066943799013;//第一偏心率的平方

//WGS-84坐标系
typedef struct tagWGS
{
double latitude;
double longitude;
double height;
}WGS;
typedef WGS *PWGS;

//空间笛卡尔坐标系 ECEF
typedef struct tagECEF
{
double x;
double y;
double z;
}ECEF;
typedef ECEF *PECEF;

//东北天坐标系ENU
typedef struct tagENU
{
double northing;
double easting;
double upping;
}ENU;
typedef ENU *PENU;

//载体坐标系BODY
typedef struct tagBODY
{
double pitch;
double yaw;
double roll;
double bx;
double by;
double bz;
double q0;
double q1;
double q2;
double q3;
}BODY;
typedef BODY *PBODY;

//GPS采集到的经纬度数据无法直接使用，需要先转换
double transfer(double val);

//WGS84 ---> ECEF
//pcg为WGS-84坐标系结构体指针，pcc为ECEF坐标系结构体指针
void WGSToECEF(PWGS pcg, PECEF pcc);

//ECEF ---> WGS84
//pcg为WGS-84坐标系结构体指针，pcc为ECEF坐标系结构体指针
void ECEFToWGS(PWGS pcg, PECEF pcc);

//ECEF ---> ENU
//pcc为ECEF坐标系结构体指针，pccCenter为东北天坐标原点的指针，pct为东北天坐标系结构体指针
void ECEFToENU(PECEF pcc, PECEF pccCenter, PENU pct);

//BODY ---> ENU
//pcb为载体坐标系指针，pct为东北天坐标系结构体指针
void BODYToENU(PBODY pcb, PENU pct);

------------------分割线--------------------------------------------
//convert.c
//GPS采集到的经纬度数据无法直接使用，需要先转换
double transfer(double val)
{
double deg = ((int)(val / 100));
val = deg + (val - deg * 100) / 60;
return val;
}

//WGS84 ---> ECEF
//pcg为WGS-84坐标系结构体指针，pcc为ECEF坐标系结构体指针
void WGSToECEF(PWGS pcg, PECEF pcc)
{
double B = pcg->latitude; //纬度
double L = pcg->longitude; //经度
double H = pcg->height;  //高度
double N; //卯酉圈曲率半径
double B_, L_;

//经纬度转换成弧度 通过GPS采集的经纬度需要先经过转换

//测试正常经纬度数据使用
B_ = B*M_PI/180;
L_ = L*M_PI/180;

N = dSemiMajorAxis / sqrt(1.0 - e2 * sin(B_) * sin(B_));
pcc->x = (N + H) * cos(B_) * cos(L_);
pcc->y = (N + H) * cos(B_) * sin(L_);
pcc->z = (N * (1.0 - e2) + H) * sin(B_);
}

//ECEF ---> WGS84
//pcg为WGS-84坐标系结构体指针，pcc为ECEF坐标系结构体指针
void ECEFToWGS(PWGS pcg, PECEF pcc)
{
double B0, R, N;
double B_, L_;
double X = pcc->x;
double Y = pcc->y;
double Z = pcc->z;

R = sqrt(X * X + Y * Y);
B0 = atan2(Z, R);

while (1)
{
N = dSemiMajorAxis / sqrt(1.0 - e2 * sin(B0) * sin(B0));
B_ = atan2(Z + N * e2 * sin(B0), R);
if (fabs(B_ - B0) < 1.0e-10)
break;
B0 = B_;
}

L_ = atan2(Y, X);
pcg->height = R / cos(B_) - N;

//弧度转换成经纬度
pcg->latitude = B_ * 180 / M_PI;
pcg->longitude = L_ * 180 / M_PI;
}

//ECEF ---> ENU
//pcc为ECEF坐标系结构体指针，center为东北天坐标原点的指针，pct为东北天坐标系结构体指针
//坐标原点center要用GPS采到的第一个点的数据
void ECEFToENU(PECEF pcc, PWGS center, PENU pct)
{
double dX, dY, dZ;
PECEF Geodetic;
Geodetic = (PECEF)malloc(sizeof(ECEF));
WGSToECEF(center, Geodetic);

dX = pcc->x - Geodetic->x;
dY = pcc->y - Geodetic->y;
dZ = pcc->z - Geodetic->z;

double B, L, H;
B = center->latitude;
L = center->longitude;
H = center->height;

pct->easting = -sin(L) * dX + cos(L) * dY; //X轴
pct->northing = -sin(B) * cos(L) * dX - sin(B) * sin(L) * dY + cos(B) * dZ; //Y轴
pct->upping = cos(B) * cos(L) * dX + cos(B) * sin(L) * dY + sin(B) * dZ;  //Z轴

free(Geodetic);
}

//ENU ---> ECEF
//pcc为ECEF坐标系结构体指针，center为东北天坐标原点的指针，pct为东北天坐标系结构体指针
//坐标原点center要用GPS采到的第一个点的数据
void ENUToECEF(PECEF pcc, PWGS center, PENU pct)
{
PECEF Geodetic;
Geodetic = (PECEF)malloc(sizeof(ECEF));
WGSToECEF(center, Geodetic);

double B, L, H;
B = center->latitude;
L = center->longitude;
H = center->height;

pcc->x = -sin(B) * cos(L) * pct->northing - sin(L) * pct->easting + cos(B) * cos(L) * pct->upping + Geodetic->x;
pcc->y = -sin(B) * sin(L) * pct->northing + cos(L) * pct->easting + cos(B) * sin(L) * pct->upping + Geodetic->y;
pcc->z = cos(B) * pct->northing + sin(B) * pct->upping + Geodetic->z;

free(Geodetic);
}

int main()
{
//定义所有坐标变量
PWGS pcg; //GPS坐标
pcg = (PWGS)malloc(sizeof(WGS));
PECEF pcc; //空间直角坐标
pcc = (PECEF)malloc(sizeof(ECEF));
PWGS center; //东北天坐标原点对应的空间直角坐标
center = (PWGS)malloc(sizeof(WGS));
PENU pct; //东北天坐标
pct = (PENU)malloc(sizeof(ENU));
//经纬度数据自行设置
center->latitude = ;
center->longitude = ;
center->height = ;

pct->easting = ;
pct->northing = ;
pct->upping = ;

//函数请自行调用

free(pcc);
free(pcg);
free(center);
free(pct);
free(pcb);

return 0;
}

 

感谢：http://blog.sina.com.cn/s/blog_ea0bc96f0102ylxu.html