GPS学习（一）：在ROS2中将GPS经纬度数据转换为机器人ENU坐标系，在RVIZ中显示坐标轨迹

本文记录在Ubuntu22.04-Humbel中使用NMEA协议GPS模块的过程，使用国产ROS开发板鲁班猫(LubanCat )进行调试。

一、GPS模块介绍

在淘宝找了款性价比较高的轮趣科技GPS北斗双模定位模块作为入门学习使用，支持GNSS系统(北斗、GPS、GLONASS、日本的QZSS以及卫星增强系统SBAS)，定位精度在2.5m左右，属于民用级别。引出了常用的几个外部接口，主要用type-c调试比较方便，售后技术也相当给力。接线示意图如下，注意：测试时需要将GPS天线放置在室外，否则模块将没有GPS信号

二、坐标转换

机器人基于Ubuntu-ROS平台做开发调试，所以需要将GPS模块的经纬度和高度信息转化为机器人所能认识的本地坐标系(局部笛卡尔坐标系)。参考其他博主案例，使用半正矢公式来将经纬度转为xyz坐标值。

转换原理

半正矢公式介绍:
半正矢公式是一种根据两点的经度和纬度来确定大圆上两点之间距离的计算方法，在导航有着重要地位。它是球面三角学中“半正矢定理”公式的特例，该定理涉及了球面三角形的边和角。
对于任何球面上的两点，圆心角的半正矢值可以通过如下公式计算：

          hav($\frac{d}{r}$)=hav(${\psi }_2$-${\psi }_1$)+cos(${\psi }_1$)cos(${\psi }_2$)hav(${\lambda }_2$-${\lambda }_1$)

hav 是半正矢函数的缩写：

          haversin($\theta$)=$si{n}^2$($\frac{(\theta )}{2}$)=$\frac{1-cos(\theta )}{2}$

参数解释：

d ：两点之间的距离（沿大圆，见球面距离）；
r ：球的半径；
${\psi }_1$${\psi }_2$ ：点 1 的纬度和点 2 的纬度，以弧度制度量；
${\lambda }_1$${\lambda }_2$：点 1 的经度和点 2 的经度，以弧度制度量。
等号左边的$\frac{d}{r}$：圆心角，单位是弧度。

所以，可以通过应用反半正矢函数（如果可以查到值）或通过使用反正弦函数来解出d ：
          $$d=\sqrt[]{r\ast archav(h)}$$                  =  2r*arcsin$\sqrt[]{h}$)

将h=hav$\frac{d}{r}$代入后可得：
   
对于中短距离来说，半正矢公式是计算地球表面 GPS 坐标之间距离的有用且相对准确的方法，在距离较长时其准确性可能会降低。

增加回调函数

这里简单解释一下gpsCallback回调函数的内容，此处订阅sensor_msgs::msg::NavSatFix数据格式的GPS话题进行数据处理。

//将纬度和经度值从度数转换为弧度。
double rad(double d) 
{
	return d * 3.1415926 / 180.0;
}
void gps_callback(const sensor_msgs::msg::NavSatFix::SharedPtr gps_msg)
{
    if (!pose_init)
    {
        // Initialization code remains the same.
        //使用接收到的 GPS 消息中的纬度、经度和高度false进行初始化，只初始化一次
        init_pose.latitude = gps_msg->latitude;
        init_pose.longitude = gps_msg->longitude;
        init_pose.altitude = gps_msg->altitude;
        init = true;
    }
    else
    {
        //计算当前 GPS 位置与初始 GPS 位置之间的纬度和经度变化
        double radLat1 ,radLat2, radLong1,radLong2,delta_lat,delta_long,x,y;
        double deltaLat_y ,deltaLong_x;
        deltaLat_x = 0; 
        deltaLong_y = 0;
        radLat1 = rad(init_pose.latitude);//初始化位置的纬度
        radLong1 = rad(init_pose.longitude);//初始化位置的经度
        radLat2 = rad(gps_msg->latitude);//当前位置的纬度
        radLong2 = rad(gps_msg->longitude);//当前位置的经度
        deltaLat = radLat2 - radLat1; 
        deltaLong = radLong2 - radLong1;
 
        // 代入公式进行计算x的坐标值
        x = 2 * asin(sqrt(pow(sin(delta_lat / 2), 2) + cos(radLat1) * cos(radLat2) * pow(sin(deltaLong_y / 2), 2));
        
        //公式计算结果默认为弧度，所以这里要转换成m的单位
        x = x * EARTH_RADIUS * 1000; 

        //  代入公式进行计算y的坐标值
        y = 2 * asin(sqrt(pow(sin(deltaLat_x / 2), 2) + cos(radLat2) * cos(radLat2) * pow(sin(deltaLong / 2), 2));
        y = y * EARTH_RADIUS * 1000; // Convert to meters.

        // 将高度差计算为z的坐标值
        double z = gps_msg->altitude - init_pose.altitude;

        //将计算得到的xyz值发布为ROS中的PoseStamped数据，作为路径发布
        
        ros_path_.header.frame_id = "path";
        ros_path_.header.stamp = rclcpp::Node::now();
        geometry_msgs::msg::PoseStamped pose;
        pose.header = ros_path_.header;
        pose.pose.position.x = x;
        pose.pose.position.y = y;
        pose.pose.position.z = z;
        ros_path_.poses.push_back(pose);
        state_pub_.publish(ros_path_);
    }
}

效果演示

使用nmea_navsat_driver驱动启动GPS模块，源码链接：https://github.com/ros-drivers/nmea_navsat_driver/tree/ros2

ros2 launch nmea_navsat_driver nmea_serial_driver.launch.py

然后启动坐标转换节点，将经纬度数据从WGS-84 坐标转换到机器人真实世
界 xyz 坐标系下（一般为东北天方向），以第一个经纬度数据为原点，不断描绘实时经纬度数据的位移方向，显示为机器人运行轨迹图。

ros2 launch gps_path gps_path.launch.py

打开rviz2，修改 Fixed Frame 为path，电机左下角[add]，根据topic添加[gps_path]->path后，显示效果为：