一、实现原理
在小范围场景下，可以假设GPS经纬度值都在一个平面上，地理正东方向为经度正方向，正北方向为纬度正方向，正上方向为高度正方向，至此经纬高度坐标系已经建立。而我们要做的是将其转换到一个以米为度量单位的平面坐标系下，平面坐标系X轴指向地理正东方向、Y轴指向地理正北方向、Z轴指向正上方向，具体如下图所示：
在确定好各坐标系之后，计算出X轴方向单位米对应经度的变化量gridLon、Y轴方向单位米对应纬度变化量gridLat以及Z轴方向单位米对应高度变化量gridHei。接着选择一个起始点作为经纬高度坐标系原点（，，）和XYZ平面坐标系原点（，，）。则各点GPS经纬高度值（，，）转换为平面XYZ坐标值（，，）关系式如下所示：
如此，便可以实现小场景下GPS经纬高度值转换为平面XYZ坐标值。
二、代码实现
测试环境：Ubuntu18.04系统+ Ros-Melodic版本 + C++编程；
1、gps_to_xyz.cpp
#include<ros/ros.h>#include <nav_msgs/Path.h>#include <sensor_msgs/NavSatFix.h>#include <geometry_msgs/PoseStamped.h>using namespace std;//角度制转弧度制double rad(double d){return d * 3.14159265 / 180.0;}// ROS订阅者和发布者ros::Subscriber gps_data_sub;ros::Publisher path_pub;// 全局变量string gps_sub_topic = "";string output_frame_name ="";double z_rotate_value = 0.0;double origin_latitude_value = 0.0;double origin_longitude_value = 0.0;double origin_altitude_value = 0.0;double latitude_resolution = 0.0;double longitude_resolution = 0.0;double altitude_resolution = 0.0;bool init_flag = true;nav_msgs::Path path;void gps_to_xyz(const sensor_msgs::NavSatFix::ConstPtr& gps_msg){if (init_flag == true){origin_latitude_value = gps_msg->latitude;origin_longitude_value = gps_msg->longitude;origin_altitude_value = gps_msg->altitude;init_flag = false;}else{double gps_lat = gps_msg->latitude;double gps_lon = gps_msg->longitude;double gps_hei = gps_msg->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;if (path_pub.getNumSubscribers() > 0){geometry_msgs::PoseStamped this_pose_stamped;this_pose_stamped.header.frame_id = output_frame_name;this_pose_stamped.pose.position.x = gps_x;this_pose_stamped.pose.position.y = gps_y;this_pose_stamped.pose.position.z = gps_z;this_pose_stamped.pose.orientation.x = 0.0;this_pose_stamped.pose.orientation.y = 0.0;this_pose_stamped.pose.orientation.z = 0.0;this_pose_stamped.pose.orientation.w = 1.0;path.poses.push_back(this_pose_stamped);path.header.frame_id = output_frame_name;}path_pub.publish(path);}}int main(int argc,char **argv){ros::init(argc,argv,"gps_deal");ros::NodeHandle nh;nh.param<string>("gps_sub_topic", gps_sub_topic, "/gps");nh.param<string>("output_frame_name", output_frame_name, "map");nh.param<bool>("auto_get_origin_gps", init_flag, true);nh.param<double>("z_rotate_value", z_rotate_value, 1.0);nh.param<double>("origin_latitude_value", origin_latitude_value, 1.0);nh.param<double>("origin_longitude_value", origin_longitude_value, 1.0);nh.param<double>("origin_altitude_value", origin_altitude_value, 1.0);nh.param<double>("latitude_resolution", latitude_resolution, 1.0);nh.param<double>("longitude_resolution", longitude_resolution, 1.0);nh.param<double>("altitude_resolution", altitude_resolution, 1.0);gps_data_sub = nh.subscribe<sensor_msgs::NavSatFix>(gps_sub_topic, 1000, gps_to_xyz);path_pub = nh.advertise<nav_msgs::Path>("/gpsTrack",1, true);ros::spin();return 0;}
2、gps_to_xyz.launch
<launch><param name="gps_sub_topic" value = "/gps" /> // 订阅GPS数据话题名称<param name="output_frame_name" value = "map" />// 输出frame名称<param name="auto_get_origin_gps" value = "true" /> // 是否自动获取起始点（数据集第一个GPS数据）经纬高度信息<param name="z_rotate_value" value = "0.0" /> // 对整个轨迹进行旋转操作<param name="origin_longitude_value" value = "1.0" />// 手动输入起始点经度值<param name="origin_latitude_value" value = "1.0" /> // 手动输入起始点纬度值<param name="origin_altitude_value" value = "1.0" /> // 手动输入起始点高度值<param name="longitude_resolution" value = "1.09244689e-05" /> // X轴单位米对应经度变化量<param name="latitude_resolution" value = "9.01006167e-06" /> // Y轴单位米对应纬度变化量<param name="altitude_resolution" value = "1.0" /> // Z轴单位米对应高度变化量<node pkg="gps_to_xyz" name="gps_to_xyz" type="gps_to_xyz" output="screen" /><node pkg="rviz" type="rviz" name="rviz" args="-d $(find gps_to_xyz)/rviz/gps_to_xyz.rviz" /></launch>
三、实现过程
1、首先选定一个GPS经纬高度数据点作为起始点和平面XYZ坐标系原点；
2、分别计算出X、Y、Z轴方向上单位米对应经度、纬度、高度变化量，即经纬高度分辨率，计算方式可以通过Google Earth（需要科学上网）进行测量计算;
3、修改gps_to_xyz.launch文件相应参数;
4、运行程序;
5、播放包含gps数据的rosbag包;
6、完成；
四、实现结果
GPS经纬高度值转换为平面XYZ坐标值并用RVIZ动态显示
五、总结
代码开源地址：/wccworld/gps_to_xyz
创作不易，如有错误，敬请批评指正。
