GPS 定位信息分析：航向角分析及经纬度坐标转局部XY坐标

Prejudices2023-11-25 10:39

GPS 定位信息分析（1）

从下面的数据可知，raw data 的提取和经纬度的计算应该是没问题的

在相同的经纬度下， x 和 y 还会发生变化，显然是不正确的

cpp 复制代码 
raw data:3150.93331124	11717.59467080	5.3
latitude: 31.8489	longitude: 117.293	heading_angle: 5.3
raw data:3150.93332581	11717.59468186	59.4
latitude: 31.8489	longitude: 117.293	heading_angle: 59.4
x: 0.0270035	y: 0.017412
---------
raw data:3150.93333013	11717.59468264	81.7
latitude: 31.8489	longitude: 117.293	heading_angle: 81.7
x: 0.03501	y: 0.01864
---------
raw data:3150.93333688	11717.59468779	77.2
latitude: 31.8489	longitude: 117.293	heading_angle: 77.2
x: 0.0475202	y: 0.0267478
---------
raw data:3150.93333483	11717.59468236	89.2
latitude: 31.8489	longitude: 117.293	heading_angle: 89.2
x: 0.0437208	y: 0.0181992
---------
raw data:3150.93334090	11717.59465670	337.7
latitude: 31.8489	longitude: 117.293	heading_angle: 337.7
x: 0.0549707	y: 0.022198
---------

调整输出后在 MATLAB 中处理显示

  • clockwise_pro.txt
  • anticlockwise_pro.txt
  • path_pro.txt

clockwise_pro 和 anticlockwise_pro 有助于分析航向角信息，path 参考意义不大

航向角信息分析

clockwise_pro 航向角信息

anticlockwise_pro 航向角信息

GPS 获取的是真北航迹方向，deg

Heading定向是指双天线接收机的主天线（ANT1）与从天线（ANT2）之间构成一个基线向量 ，确定此基线向量逆时针方向与真北的夹角

首先要明确基线向量与正北方向夹角的大小以及正负关系，分析如下，红色表示正北方向，蓝色表示基线向量，绿色表示航向角（真北航迹方向）


正北方向表示 0 度或 360 度位置，非负值，沿正北方向顺时针旋转是角度增加的方向

此时分析 clockwise_pro 和 anticlockwise_pro 的航向角变化结果就比较容易，示意图如下

以顺时针为例，起始位姿航向角 50 度，顺时针旋转至于正北方向重合，角度一直增大至 360 度

重合后继续顺时针旋转，此时会先从 360 度突变至 0 度，再继续增加

问题分析

1、由于手动安装主从天线，基线向量本身存在夹角，导致航向角始终存在偏差

下图能够说明该问题

2、GPS 获得的航向角信息是在大地或者正北天坐标系下的，该信息并不可以使用，需要转换到局部坐标系下，涉及到坐标转换的问题

3、基线向量和主从天线有关，接收机 com1 口对应主天线，但仅靠线缆无法区分，本次也没有贴标签注明哪一根接前天线，哪一根接后天线，下次接线若与本次不同可能会有影响

轨迹信息分析

clockwise_pro 轨迹信息

anticlockwise_pro 轨迹信息

对于原地旋转而言经纬度信息保持不变，此时在 X 方向和 Y 方向还有如此大的位移，是有问题的

GPS 定位信息分析（2）

对于局部 XY 坐标信息的异常，从公式和编程两方面考虑

XY 坐标计算

公式部分应该没什么问题，主要由"根据经纬度计算两地之间的距离"推得

查找资料的过程中还发现"大地坐标系与空间直角坐标系的转换"，可以作为储备知识


以起始的 GPS 坐标为全局坐标系原点（init_pose），以当前纬度为 X轴，当前经度为 Y 轴

计算目标点 X 坐标时，假设纬度相同，根据经度差计算 X 坐标值

计算目标点 Y 坐标时，假设经度相同，根据纬度差计算 Y 坐标值

陆师兄 gps_path.cpp 中的代码如下，感觉存在一些问题

cpp 复制代码 
//初始化
    if(!init)
    {
        init_pose.latitude = gps_msg_ptr->latitude;
        init_pose.longitude = gps_msg_ptr->longitude;
        init_pose.altitude = gps_msg_ptr->altitude;
        init = true;
    }
    else
    {
    //计算相对位置
        double radLat1 ,radLat2, radLong1,radLong2,delta_lat,delta_long;
		radLat1 = rad(init_pose.latitude);
        radLong1 = rad(init_pose.longitude);
		radLat2 = rad(gps_msg_ptr->latitude);
		radLong2 = rad(gps_msg_ptr->longitude);
        //计算x
		delta_lat = radLat2 - radLat1;
        delta_long = 0;
        double x = 2*asin( sqrt( pow( sin( delta_lat/2 ),2) + cos( radLat1 )*cos( radLat2)*pow( sin( delta_long/2 ),2 ) ));
        x = x*EARTH_RADIUS*1000;

        //计算y
		delta_lat = 0;
        delta_long = radLong1  - radLong2;
        double y = 2*asin( sqrt( pow( sin( delta_lat/2 ),2) + cos( radLat2 )*cos( radLat2)*pow( sin( delta_long/2 ),2 ) ) );
        y = y*EARTH_RADIUS*1000;

        //计算z
        double z = gps_msg_ptr->altitude - init_pose.altitude;

        //发布轨迹
        ros_path_.header.frame_id = "path";
        ros_path_.header.stamp = ros::Time::now();  

        geometry_msgs::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);

        //ROS_INFO("( x:%0.6f ,y:%0.6f ,z:%0.6f)",x ,y ,z );
	cout<<x<<","<<y<<","<<z<<endl;
        state_pub_.publish(ros_path_);
    }
}

简单调整如下，只是将 X 变为 Y，Y 变为 X，影响不大

cpp 复制代码 
        		// 计算x
            delta_lat = 0;
            delta_long = radLong1 - radLong2;           
            double x = 2 * asin(sqrt(pow(sin(delta_lat / 2), 2) + cos(radLat1) * cos(radLat2) * pow(sin(delta_long / 2), 2)));
            x = x * EARTH_RADIUS * 1000;
            cout << "delta_lat: " << delta_lat << "\tdelta_long: " << delta_long << "\tx: " << x << endl;

            // 计算y
            delta_lat = radLat2 - radLat1;
            delta_long = 0;
            double y = 2 * asin(sqrt(pow(sin(delta_lat / 2), 2) + cos(radLat2) * cos(radLat2) * pow(sin(delta_long / 2), 2)));
            y = y * EARTH_RADIUS * 1000;
            cout << "delta_lat: " << delta_lat << "\tdelta_long: " << delta_long << "\ty: " << y << endl;

XY 坐标误差

另外对于原地旋转经纬度不变情况下存在较大 XY 方向位移的问题，打印过程中各变量分析

cpp 复制代码 
      			// 计算相对位置
            double radLat1, radLat2, radLong1, radLong2, delta_lat, delta_long;
            radLat1 = rad(init_pose.latitude);
            radLong1 = rad(init_pose.longitude);
            radLat2 = rad(latitude);
            radLong2 = rad(longitude);
            cout << "radLat1: " << radLat1 << "\tradLong1: " << radLong1 << "\tradLat2: " << radLat2 << "\tradLong2: " << radLong2 << endl;
            // 计算x
            delta_lat = radLat2 - radLat1;
            delta_long = 0;
            double x = 2 * asin(sqrt(pow(sin(delta_lat / 2), 2) + cos(radLat1) * cos(radLat2) * pow(sin(delta_long / 2), 2)));
            x = x * EARTH_RADIUS * 1000;
            cout << "delta_lat: " << delta_lat << "\tdelta_long: " << delta_long << "\tx: " << x << endl;

            // 计算y
            delta_lat = 0;
            delta_long = radLong1 - radLong2;
            double y = 2 * asin(sqrt(pow(sin(delta_lat / 2), 2) + cos(radLat2) * cos(radLat2) * pow(sin(delta_long / 2), 2)));
            y = y * EARTH_RADIUS * 1000;
            cout << "delta_lat: " << delta_lat << "\tdelta_long: " << delta_long << "\tx: " << y << endl;

            cout << "---------" << endl;
cpp 复制代码 
latitude: 31.8489	longitude: 117.293	heading_angle: 5.3
latitude: 31.8489	longitude: 117.293	heading_angle: 59.4
radLat1: 0.555868	radLong1: 2.04715	radLat2: 0.555868	radLong2: 2.04715
delta_lat: 4.23824e-09	delta_long: 0	x: 0.0270035
delta_lat: 0	delta_long: -3.21722e-09	x: 0.017412
---------
latitude: 31.8489	longitude: 117.293	heading_angle: 81.7
radLat1: 0.555868	radLong1: 2.04715	radLat2: 0.555868	radLong2: 2.04715
delta_lat: 5.49488e-09	delta_long: 0	x: 0.03501
delta_lat: 0	delta_long: -3.44412e-09	x: 0.01864
---------
latitude: 31.8489	longitude: 117.293	heading_angle: 77.2
radLat1: 0.555868	radLong1: 2.04715	radLat2: 0.555868	radLong2: 2.04715
delta_lat: 7.45837e-09	delta_long: 0	x: 0.0475202
delta_lat: 0	delta_long: -4.94219e-09	x: 0.0267478
---------
latitude: 31.8489	longitude: 117.293	heading_angle: 89.2
radLat1: 0.555868	radLong1: 2.04715	radLat2: 0.555868	radLong2: 2.04715
delta_lat: 6.86205e-09	delta_long: 0	x: 0.0437208
delta_lat: 0	delta_long: -3.36267e-09	x: 0.0181992
---------
latitude: 31.8489	longitude: 117.293	heading_angle: 337.7
radLat1: 0.555868	radLong1: 2.04715	radLat2: 0.555868	radLong2: 2.04715
delta_lat: 8.62774e-09	delta_long: 0	x: 0.0549707
delta_lat: 0	delta_long: 4.10152e-09	x: 0.022198
---------

发现 radLat1 和 radLat2 相等，但是 delta_lat 却一直在变化，经度同理，这是 XY 坐标变化的主因

这里涉及到计算机中小数的表示，浮点数无法被精确表示，虽然 radLat1 和 radLat2 前几位都是 0.555868，但最后的尾部无法始终相同，因此相减后始终存在极小的误差，e-09 级别

round() 函数可以用于保留小数，详见 2022-07-30 C++：round函数用法

在进行角度和弧度转换时限定浮点数精度，调整 cout 输出精度后输出如下

cpp 复制代码 
// 角度制转弧度制
double deg2rad(double deg)
{
    double rad = deg * M_PI / 180.0;
    // 保留有限小数
    rad = round(rad * 1000000000) / 1000000000;
    return rad;
}

		cout.setf(ios::fixed);
    cout.precision(9);
cpp 复制代码 
/home/redwall/catkin_ws/src/gps_sensor/log/2023-11-10/clockwise.txt
latitude: 31.848888521	longitude: 117.293244513	heading_angle: 5.300000000
latitude: 31.848888763	longitude: 117.293244698	heading_angle: 59.400000000
delta_lat: 0.000000000	delta_long: -0.000000003	x: 0.016236402
delta_lat: 0.000000004	delta_long: 0.000000000	y: 0.025485572
---------
latitude: 31.848888835	longitude: 117.293244711	heading_angle: 81.700000000
delta_lat: 0.000000000	delta_long: -0.000000004	x: 0.021648536
delta_lat: 0.000000005	delta_long: 0.000000000	y: 0.031856965
---------
latitude: 31.848888948	longitude: 117.293244797	heading_angle: 77.200000000
delta_lat: 0.000000000	delta_long: -0.000000005	x: 0.027060668
delta_lat: 0.000000007	delta_long: 0.000000000	y: 0.044599750
---------
latitude: 31.848888914	longitude: 117.293244706	heading_angle: 89.200000000
delta_lat: 0.000000000	delta_long: -0.000000003	x: 0.016236402
delta_lat: 0.000000007	delta_long: 0.000000000	y: 0.044599750
---------
latitude: 31.848889015	longitude: 117.293244278	heading_angle: 337.700000000
delta_lat: 0.000000000	delta_long: 0.000000004	x: 0.021648534
delta_lat: 0.000000008	delta_long: 0.000000000	y: 0.050971144
---------
latitude: 31.848888630	longitude: 117.293243995	heading_angle: 331.400000000
delta_lat: 0.000000000	delta_long: 0.000000009	x: 0.048709202
delta_lat: 0.000000002	delta_long: 0.000000000	y: 0.012742786
---------
latitude: 31.848888411	longitude: 117.293243583	heading_angle: 238.000000000
delta_lat: 0.000000000	delta_long: 0.000000016	x: 0.086594137
delta_lat: -0.000000002	delta_long: 0.000000000	y: 0.012742786
---------
latitude: 31.848888926	longitude: 117.293243262	heading_angle: 269.000000000
delta_lat: 0.000000000	delta_long: 0.000000022	x: 0.119066939
delta_lat: 0.000000007	delta_long: 0.000000000	y: 0.044599750
---------
latitude: 31.848889280	longitude: 117.293242747	heading_angle: 265.900000000
delta_lat: 0.000000000	delta_long: 0.000000031	x: 0.167776140
delta_lat: 0.000000013	delta_long: 0.000000000	y: 0.082828109
---------

观察确实主要在 latitude 和 longtitude 的 6 ~ 9 位小数会发生变化，导致 XY 坐标的变化

保留有限的小数可以一定程度上限制数据的抖动，但也会使得 path.txt 无法输出轨迹，比较矛盾

cpp 复制代码 
/home/redwall/catkin_ws/src/gps_sensor/log/2023-11-10/path.txt
latitude: 31.848883337	longitude: 117.293247277	heading_angle: 35.900000000
latitude: 31.848883241	longitude: 117.293247357	heading_angle: 49.400000000
delta_lat: 0.000000000	delta_long: 0.000000000	x: 0.000000000
delta_lat: 0.000000000	delta_long: 0.000000000	y: 0.000000000
---------
latitude: 31.848883139	longitude: 117.293247392	heading_angle: 198.800000000
delta_lat: 0.000000000	delta_long: 0.000000000	x: 0.000000000
delta_lat: 0.000000000	delta_long: 0.000000000	y: 0.000000000
---------
latitude: 31.848883027	longitude: 117.293247506	heading_angle: 37.300000000
delta_lat: 0.000000000	delta_long: 0.000000000	x: 0.000000000
delta_lat: 0.000000000	delta_long: 0.000000000	y: 0.000000000
---------
latitude: 31.848883101	longitude: 117.293247264	heading_angle: 359.300000000
delta_lat: 0.000000000	delta_long: 0.000000000	x: 0.000000000
delta_lat: 0.000000000	delta_long: 0.000000000	y: 0.000000000
---------
latitude: 31.848882789	longitude: 117.293247238	heading_angle: 175.200000000
delta_lat: 0.000000000	delta_long: 0.000000000	x: 0.000000000
delta_lat: 0.000000000	delta_long: 0.000000000	y: 0.000000000

问题分析

考虑 GPS 本身的定位精度，以及移动距离过短两方面引起误差的产生

差分定位（Differential GPS，DGPS）是一种通过引入参考站观测数据来提高全球定位系统（GPS）接收机测量精度的技术。通常，差分GPS可以提供亚米级的位置精度，甚至更高，相比于普通的独立GPS。

差分GPS系统的基本原理是，参考站与GPS接收机接收相同的卫星信号，然后比较它们的观测值与已知的准确位置。由于参考站的位置已知，它可以检测到GPS信号由于大气层、电离层等环境因素引起的误差，并将这些误差信息传输给用户设备，从而实现位置精度的提高。

DGPS的精度取决于多个因素，包括：

  1. 基站位置的准确性： 基站的准确位置对差分定位的精度至关重要。
  2. 基站到用户接收机的距离： 基站越接近用户设备，传输的差分校正信息的精度就越高。
  3. 使用的卫星数量和分布： 使用更多的卫星可以提高定位的准确性。
  4. 大气层和电离层的变化： 大气层和电离层的变化会引起GPS信号的传播延迟，这可能影响定位的精度。

总体而言，在适当的条件下，差分GPS可以提供比普通独立GPS更高的位置精度，通常在亚米级别。然而，实际精度可能会受到环境、设备和使用条件的影响。

其实关于 GPS 数据在 Rviz 中显示，2020 年已经有相关技术博客

ROS：GPS坐标序列可视化（在Rviz中显示轨迹）
GPS坐标转换并实时显示轨迹
ROS入门:GPS坐标转换&Rviz显示轨迹
将GPS轨迹，从经纬度WGS-84坐标转换到真实世界xyz坐标系（东北天ENU）下（思路：计算出每个gps坐标相对与第一个坐标的距离（m为单位），比较相邻两点的经纬度变化，赋予位移的方向，然后累加得到轨迹）

陆辉东的代码和博客中几乎一样，应该也是学习借鉴了

对于精度的问题可以参考下面的博客解决

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


另外还可以将 GPS 数据在卫星地图中显示，获得比较酷炫的效果


ROS下如何将GPS数据在卫星地图显示（两种开源方法）
ROS采集GPS/北斗数据在百度地图中可视化位置
SLAM中将地图映射到谷歌地图上的方法------完美运行

相关推荐
EAI-Robotics1 小时前
机器人打包物品研究现状简述
机器人
肥猪猪爸1 小时前
使用卡尔曼滤波器估计pybullet中的机器人位置
数据结构·人工智能·python·算法·机器人·卡尔曼滤波·pybullet
清安无别事5 小时前
闲聊？泳池清洁机器人？
机器人
zhd15306915625ff5 小时前
库卡机器人维护需要注意哪些事项
安全·机器人·自动化
宋138102797209 小时前
Manus Xsens Metagloves虚拟现实手套
人工智能·机器人·vr·动作捕捉
禁默10 小时前
第六届机器人、智能控制与人工智能国际学术会议（RICAI 2024）
人工智能·机器人·智能控制
Robot2511 天前
Figure 02迎重大升级！！人形机器人独角兽[Figure AI]商业化加速
人工智能·机器人·微信公众平台
FreeIPCC1 天前
谈一下开源生态对 AI人工智能大模型的促进作用
大数据·人工智能·机器人·开源
施努卡机器视觉1 天前
电解车间铜业机器人剥片技术是现代铜冶炼过程中自动化和智能化的重要体现
运维·机器人·自动化
zhd15306915625ff1 天前
库卡机器人日常维护
网络·机器人·自动化·机器人备件
热门推荐
01(欧拉)openEuler系统添加网卡文件配置流程、(欧拉)openEuler系统手动配置ipv6地址流程、(欧拉)openEuler系统网络管理说明02PyTorch机器学习实现液态神经网络03【HarmonyOS】HUAWEI DevEco Studio 下载地址汇总04玄机平台应急响应—webshell查杀05Coze扣子平台完整体验和实践（附国内和国际版对比）06Ubuntu 20.04使用Livox mid 360 测试 FAST_LIO07怎样让音频速度变慢？请跟随以下方法进行操作08Unity中PICO实现 隔空取物 和 接触抓取物体09RAG 实践- Ollama+RagFlow 部署本地知识库10【目标跟踪】相机运动补偿