GPS学习(一):在ROS2中将GPS经纬度数据转换为机器人ENU坐标系,在RVIZ中显示坐标轨迹

本文主要是介绍GPS学习(一):在ROS2中将GPS经纬度数据转换为机器人ENU坐标系,在RVIZ中显示坐标轨迹,希望对大家解决编程问题提供一定的参考价值,需要的开发者们随着小编来一起学习吧!

文章目录

  • 一、GPS模块介绍
  • 二、坐标转换
    • 转换原理
      • 参数解释:
    • 增加回调函数
    • 效果演示

本文记录在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( d r \frac{d}{r} rd)=hav( ψ 2 \psi_{2} ψ2- ψ 1 \psi_{1} ψ1)+cos( ψ 1 \psi_{1} ψ1)cos( ψ 2 \psi_{2} ψ2)hav( λ 2 \lambda_{2} λ2- λ 1 \lambda_{1} λ1)

hav 是半正矢函数的缩写:

          haversin( θ \theta θ)= s i n 2 sin^{2} sin2( ( θ ) 2 \frac{(\theta)}{2} 2(θ))= 1 − c o s ( θ ) 2 \frac{1-cos(\theta)}{2} 21cos(θ)

参数解释:

d :两点之间的距离(沿大圆,见球面距离);
r :球的半径;
ψ 1 \psi_{1} ψ1 ψ 2 \psi_{2} ψ2 :点 1 的纬度和点 2 的纬度,以弧度制度量;
λ 1 \lambda_{1} λ1 λ 2 \lambda_{2} λ2:点 1 的经度和点 2 的经度,以弧度制度量。
等号左边的 d r \frac{d}{r} rd:圆心角,单位是弧度。

所以,可以通过应用反半正矢函数(如果可以查到值)或通过使用反正弦函数来解出d :
           d = r ∗ a r c h a v ( h ) d=\sqrt[]{r*archav(h)} d=rarchav(h)                   =  2r*arcsin h \sqrt[]{h} h )

将h=hav d r \frac{d}{r} rd代入后可得:
   在这里插入图片描述
对于中短距离来说,半正矢公式是计算地球表面 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后,显示效果为:
在这里插入图片描述

这篇关于GPS学习(一):在ROS2中将GPS经纬度数据转换为机器人ENU坐标系,在RVIZ中显示坐标轨迹的文章就介绍到这儿,希望我们推荐的文章对编程师们有所帮助!



http://www.chinasem.cn/article/314158

相关文章

Spring 请求之传递 JSON 数据的操作方法

《Spring请求之传递JSON数据的操作方法》JSON就是一种数据格式,有自己的格式和语法,使用文本表示一个对象或数组的信息,因此JSON本质是字符串,主要负责在不同的语言中数据传递和交换,这... 目录jsON 概念JSON 语法JSON 的语法JSON 的两种结构JSON 字符串和 Java 对象互转

python实现svg图片转换为png和gif

《python实现svg图片转换为png和gif》这篇文章主要为大家详细介绍了python如何实现将svg图片格式转换为png和gif,文中的示例代码讲解详细,感兴趣的小伙伴可以跟随小编一起学习一下... 目录python实现svg图片转换为png和gifpython实现图片格式之间的相互转换延展:基于Py

使用Python从PPT文档中提取图片和图片信息(如坐标、宽度和高度等)

《使用Python从PPT文档中提取图片和图片信息(如坐标、宽度和高度等)》PPT是一种高效的信息展示工具,广泛应用于教育、商务和设计等多个领域,PPT文档中常常包含丰富的图片内容,这些图片不仅提升了... 目录一、引言二、环境与工具三、python 提取PPT背景图片3.1 提取幻灯片背景图片3.2 提取

C++如何通过Qt反射机制实现数据类序列化

《C++如何通过Qt反射机制实现数据类序列化》在C++工程中经常需要使用数据类,并对数据类进行存储、打印、调试等操作,所以本文就来聊聊C++如何通过Qt反射机制实现数据类序列化吧... 目录设计预期设计思路代码实现使用方法在 C++ 工程中经常需要使用数据类,并对数据类进行存储、打印、调试等操作。由于数据类

SpringBoot使用GZIP压缩反回数据问题

《SpringBoot使用GZIP压缩反回数据问题》:本文主要介绍SpringBoot使用GZIP压缩反回数据问题,具有很好的参考价值,希望对大家有所帮助,如有错误或未考虑完全的地方,望不吝赐教... 目录SpringBoot使用GZIP压缩反回数据1、初识gzip2、gzip是什么,可以干什么?3、Spr

C#实现将Excel表格转换为图片(JPG/ PNG)

《C#实现将Excel表格转换为图片(JPG/PNG)》Excel表格可能会因为不同设备或字体缺失等问题,导致格式错乱或数据显示异常,转换为图片后,能确保数据的排版等保持一致,下面我们看看如何使用C... 目录通过C# 转换Excel工作表到图片通过C# 转换指定单元格区域到图片知识扩展C# 将 Excel

C++使用printf语句实现进制转换的示例代码

《C++使用printf语句实现进制转换的示例代码》在C语言中,printf函数可以直接实现部分进制转换功能,通过格式说明符(formatspecifier)快速输出不同进制的数值,下面给大家分享C+... 目录一、printf 原生支持的进制转换1. 十进制、八进制、十六进制转换2. 显示进制前缀3. 指

SpringBoot集成Milvus实现数据增删改查功能

《SpringBoot集成Milvus实现数据增删改查功能》milvus支持的语言比较多,支持python,Java,Go,node等开发语言,本文主要介绍如何使用Java语言,采用springboo... 目录1、Milvus基本概念2、添加maven依赖3、配置yml文件4、创建MilvusClient

使用Python开发一个带EPUB转换功能的Markdown编辑器

《使用Python开发一个带EPUB转换功能的Markdown编辑器》Markdown因其简单易用和强大的格式支持,成为了写作者、开发者及内容创作者的首选格式,本文将通过Python开发一个Markd... 目录应用概览代码结构与核心组件1. 初始化与布局 (__init__)2. 工具栏 (setup_t

SpringValidation数据校验之约束注解与分组校验方式

《SpringValidation数据校验之约束注解与分组校验方式》本文将深入探讨SpringValidation的核心功能,帮助开发者掌握约束注解的使用技巧和分组校验的高级应用,从而构建更加健壮和可... 目录引言一、Spring Validation基础架构1.1 jsR-380标准与Spring整合1