小场景下基于ROS的GPS经纬高度值转换为平面XYZ坐标值，并用RVIZ显示轨迹

最新推荐文章于 2025-03-12 21:08:43 发布

沐春风吖

最新推荐文章于 2025-03-12 21:08:43 发布

阅读量6.3k

点赞数 15

文章标签： c++ vscode

本文链接：https://blog.csdn.net/weixin_42174523/article/details/127926630

版权

本文介绍了将GPS经纬高度值转换为平面XYZ坐标的方法，通过建立坐标系并计算单位米对应的经度、纬度和高度变化量。提供了C++代码实现，并在ROS环境下进行GPS数据处理，将转换后的坐标动态显示在RVIZ中。同时，文章给出了配置参数的launch文件及实现步骤。

摘要生成于 C知道，由 DeepSeek-R1 满血版支持，前往体验 >

一、实现原理

在小范围场景下，可以假设GPS经纬度值都在一个平面上，地理正东方向为经度正方向，正北方向为纬度正方向，正上方向为高度正方向，至此经纬高度坐标系已经建立。而我们要做的是将其转换到一个以米为度量单位的平面坐标系下，平面坐标系X轴指向地理正东方向、Y轴指向地理正北方向、Z轴指向正上方向，具体如下图所示：

在确定好各坐标系之后，计算出X轴方向单位米对应经度的变化量gridLon、Y轴方向单位米对应纬度变化量gridLat以及Z轴方向单位米对应高度变化量gridHei。接着选择一个起始点作为经纬高度坐标系原点（ $lon_{0}$ ， $lat_{0}$ ， $hei_{0}$ ）和XYZ平面坐标系原点（ $x_{0}$ ， $y_{0}$ ， $z_{0}$ ）。则各点GPS经纬高度值（ $lon_{i}$ ， $lat_{i}$ ， $hei_{i}$ ）转换为平面XYZ坐标值（ $x_{i}$ ， $y_{i}$ ， $z_{i}$ ）关系式如下所示：

$x_{i} = x_{0} + gridLon * (lon_{i} - lon_{0})$

$y_{i} = y_{0} + gridLat * (lat_{i} - lat_{0})$

$z_{i} = z_{0} + gridHei * (hei_{i} - hei_{0})$

如此，便可以实现小场景下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、完成；

四、实现结果