gps转wgs转XY坐标系

GPS转WGS84
$GPGGA协议
WGS84转XY坐标
- 将GPS数据转成Ecef坐标系(地心坐标系)，原点是地心
- GPS转到自定义坐标系
WGS84转UTM

GPS转WGS84

接收机输出的数据格式：ddd°mm.mmm 度 . 分 . 分格式
比如：纬度 DDFF.FFFF
Wgs84 = DD +FFFFFF/60
经度
DDDFF.FFFF
Wgs84= DDD + FFFFFF/60

$GPGGA协议

G P G G A 语句包括 17 个字段：语句标识头，世界时间，纬度，纬度半球，经度，经度半球，定位质量指示，使用卫星数量， H D O P − 水平精度因子，椭球高，高度单位，大地水准面高度异常差值，高度单位，差分 G P S 数据期限，差分参考基站标号，校验和结束标记 ( 用回车符 < C R > 和换行符 < L F > ) ，分别用 14 个逗号进行分隔。格式示例： GPGGA 语句包括17个字段：语句标识头，世界时间，纬度，纬度半球，经度，经度半球，定位质量指示，使用卫星数量，HDOP-水平精度因子，椭球高，高度单位，大地水准面高度异常差值，高度单位，差分GPS数据期限，差分参考基站标号，校验和结束标记(用回车符<CR>和换行符<LF>)，分别用14个逗号进行分隔。格式示例： GPGGA语句包括17个字段：语句标识头，世界时间，纬度，纬度半球，经度，经度半球，定位质量指示，使用卫星数量，HDOP−水平精度因子，椭球高，高度单位，大地水准面高度异常差值，高度单位，差分GPS数据期限，差分参考基站标号，校验和结束标记(用回车符<CR>和换行符<LF>)，分别用14个逗号进行分隔。格式示例：GPGGA,014434.70,3817.13334637,N,12139.72994196,E,4,07,1.5,6.571,M,8.942,M,0.7,0016*79
该数据帧的结构及各字段释义如下：
$GPGGA,<1>,<2>,<3>,<4>,<5>,<6>,<7>,<8>,<9>,M,<10>,M,<11>,<12>*xx
$GPGGA：起始引导符及语句格式说明(本句为GPS定位数据)；
<1> UTC时间，格式为hhmmss.sss；
<2> 纬度，格式为ddmm.mmmm(第一位是零也将传送)；
<3> 纬度半球，N或S(北纬或南纬)
<4> 经度，格式为dddmm.mmmm(第一位零也将传送)；
<5> 经度半球，E或W(东经或西经)
<6> GPS状态， 0初始化， 1单点定位， 2码差分， 3无效PPS， 4固定解， 5浮点解， 6正在估算 7，人工输入固定值， 8模拟模式， 9WAAS差分
<7> 使用卫星数量，从00到12(第一个零也将传送)
<8> HDOP-水平精度因子，0.5到99.9，一般认为HDOP越小，质量越好。
<9> 海拔高度，-9999.9到9999.9米
M 指单位米
<10> 大地水准面高度异常差值，-9999.9到9999.9米
M 指单位米
<11> 差分GPS数据期限(RTCM SC-104)，最后设立RTCM传送的秒数量，如不是差分定位则为空
<12> 差分参考基站标号，从0000到1023(首位0也将传送)。

语句结束标志符
xx 从$开始到*之间的所有ASCII码的异或校验
回车符，结束标记
换行符，结束标记

WGS84转XY坐标

将GPS数据转成Ecef坐标系(地心坐标系)，原点是地心

Eigen::Vector3d LatLongAltToEcef(const double latitude, const double longitude,
const double altitude)
{
constexpr double a = 6378137.; // semi-major axis, equator to center.
constexpr double f = 1. / 298.257223563;
constexpr double b = a * (1. - f); // semi-minor axis, pole to center.
constexpr double a_squared = a * a;
constexpr double b_squared = b * b;
constexpr double e_squared = (a_squared - b_squared) / a_squared;
const double sin_phi = std::sin(rad(latitude));
const double cos_phi = std::cos(rad(latitude));
const double sin_lambda = std::sin(rad(longitude));
const double cos_lambda = std::cos(rad(longitude));
const double N = a / std::sqrt(1 - e_squared * sin_phi * sin_phi);
const double x = (N + altitude) * cos_phi * cos_lambda;
const double y = (N + altitude) * cos_phi * sin_lambda;
const double z = (b_squared / a_squared * N + altitude) * sin_phi;
std::cout <<“x:”<<x<<“y:”<<y<<“z:”<<z<<std::endl;
return Eigen::Vector3d(x, y, z);
}
Ecef坐标系

GPS转到自定义坐标系

第一帧gps数据为原点，建立自定义笛卡尔坐标系。
bool init_flag =true;
double origin_latitude_value;
double origin_longitude_value;
double origin_altitude_value;
double z_rotate_value;
double longitude_resolution = 1.09244689e-05;
double latitude_resolution = 9.01006167e-06;
double altitude_resolution = 1.0;
void gps_to_xyz(double latitude,double longitude,double altitude,double &x,double &y,double &z)
{
if (init_flag == true)
{
origin_latitude_value = latitude;
origin_longitude_value = longitude;
origin_altitude_value = altitude;
init_flag = false;
}else{
double gps_lat = latitude;
double gps_lon = longitude;
double gps_hei = altitude;
double gps_xx = (gps_lon - origin_longitude_value) / longitude_resolution;
double gps_yy = (gps_lat - origin_latitude_value) / latitude_resolution;
double gps_zz = (gps_hei - origin_altitude_value) / altitude_resolution;
double gps_x = cos(rad(z_rotate_value)) * gps_xx + (-sin(rad(z_rotate_value))) * gps_yy;
double gps_y = sin(rad(z_rotate_value)) * gps_xx + cos(rad(z_rotate_value)) * gps_yy;
double gps_z = gps_zz;
x = gps_x;
y = gps_y;
z = gps_z;
}
}

//R可取值 R = 6378137; // 赤道半径 6378137

WGS84转UTM

ros系统
1、准备，安装geodesy 软件包 sudo apt-get install ros-xxx-geodesy
2、CMakeLists.txt 添加 geodesy
3、代码实现

//头文件wgs2utm.h
#ifndef WGS2UTM_H
#define WGS2UTM_H#include <iostream>
#include <fstream>
#include <vector>
#include <Eigen/Geometry>
#include <geodesy/utm.h>
#include <ros/ros.h>using namespace std;class WGS2UTM
{public:/*** @brief 构造函数*/WGS2UTM();/*** @brief 获取选取原点的经纬度lla坐标* @return  所选取原点的经纬度lla坐标*/inline Eigen::Vector3d get_origin_lla() {return origin_LLA_;}/*** @brief 获取选取原点的经纬度utm坐标* @return  所选取原点的经纬度utm坐标*/inline Eigen::Vector3d get_origin_tum() {return origin_UTM_;}/***********************************************************//***********************************************************//*****利用geodesy功能包实现WGS84经纬度坐标转化成UTM坐标*****//***********************************************************//***********************************************************//*** @brief 把选取的原点的经纬度lla坐标转化为东北天utm坐标* @param origin_LLA    输入--原点经纬度lla坐标*/void set_origin_1(Eigen::Vector3d origin_LLA);/*** @brief 把经纬度lla坐标转化为东北天utm坐标* @param LLA    输入--lla坐标* @param UTM    输出--utm坐标*/void LLA2UTM_1(const Eigen::Vector3d& LLA, Eigen::Vector3d& UTM);/*** @brief 把经纬度lla坐标转化为相对于所选取原点的东北天utm坐标* @param LLA    输入--lla坐标* @param UTM    输出--utm坐标*/void LLA2UTM_origin_1(const Eigen::Vector3d& LLA, Eigen::Vector3d& UTM_origin);private:Eigen::Vector3d origin_LLA_;Eigen::Vector3d origin_UTM_;double torad = M_PI / 180;
};
#endif //WGS2UTM_H*************************************************************************************
//源文件wgs2utm.cpp
#include "wgs2utm.h"WGS2UTM::WGS2UTM() {}void WGS2UTM::set_origin_1(Eigen::Vector3d origin_LLA)
{origin_LLA_ = origin_LLA;LLA2UTM_1(origin_LLA_, origin_UTM_);
}void WGS2UTM::LLA2UTM_1(const Eigen::Vector3d& LLA, Eigen::Vector3d& UTM)
{geographic_msgs::GeoPointStampedPtr gps_msg(new geographic_msgs::GeoPointStamped());gps_msg->position.latitude  = LLA[0];gps_msg->position.longitude = LLA[1];gps_msg->position.altitude  = LLA[2];geodesy::UTMPoint utm_point;geodesy::fromMsg(gps_msg->position, utm_point);UTM[0] = utm_point.easting;UTM[1] = utm_point.northing;UTM[2] = utm_point.altitude;
}void WGS2UTM::LLA2UTM_origin_1(const Eigen::Vector3d& LLA, Eigen::Vector3d& UTM_origin)
{//经纬度转换为UTM投影Eigen::Vector3d UTM;LLA2UTM_1(LLA, UTM);//UTM2ENU, UTM投影平移至东北天坐标系, 东北天坐标系原点为第一个采样点UTM_origin = UTM - origin_UTM_;
}
***********************************************************
//应用
#include "wgs2utm.h"
using namespace std;
int main()
{WGS2UTM wgs2utm;Eigen::Vector3d origin_LLA;Eigen::Vector3d origin_UTM;origin_LLA[0] = 30.9543508;origin_LLA[1] = 121.4795984;origin_LLA[2] = 17.0970000;wgs2utm.set_origin_1(origin_LLA);origin_UTM = wgs2utm.get_origin_tum();cout << "origin_UTM: (" <<  origin_UTM[0] << ", " << origin_UTM[1] << ", " <<       origin_UTM[2] << ")" << endl;cout << "******************************" << endl;Eigen::Vector3d P_LLA;  // 任意一点的经纬度坐标Eigen::Vector3d P_UTM;  // 该点相对于所选取原点的utm坐标P_LLA[0] = 30.9543508;P_LLA[1] = 121.4755984;P_LLA[2] = 17.0970000;wgs2utm.LLA2UTM_origin_1(P_LLA, P_UTM);cout << "P_UTM: (" <<  P_UTM[0] << ", " << P_UTM[1] << ", " << P_UTM[2] << ")" << endl;cout << "******************************" << endl;
}