GPS 定位信息获取（1）
首先回顾北斗星通 GPS 数据获取（1）~（5）
gps_pub.cpp将接收到的串口数据转化为GPS的经纬度信息gps_path.cpp将经纬度信息转化为全局坐标系下的XY值，以第一个GPS经纬度为原点
目前已确定可以接收差分数据，但是 GPGGA 指令只能获取经纬度信息，无法获取航向信息
GPRMC 指令
查阅指令手册发现 GPRMC 指令可以位置、速度、航向等期望的信息
Heading 模式指令
要获取航向角信息需要满足一定的条件，即需要开启 Heading 模式
💡 本指令用于设置支持单板卡（模块）双天线定向的接收机（UB482、UM482、UM442）
车载接收机的型号是 NC502 系列，内置板卡 UB482，支持 Heading 模式
接收机及蘑菇头安装
按照《NPOS220S接收机操作使用说明书》中的说明，接收机和双天线的安装需要满足如下要求
但是由于接收机型号不同，本接收机机身并未标有 X/Y/Z 轴指向，可能需要根据实际的数据判断
GPS 定位信息获取（2）
下午把车开出去跑了一下，没有开下坡，直接从台阶那边搬下去，没有开很远，所幸没坏
用 cutecom 获取数据，原始数据如下
[16:52:13:433] $command,gprmc 1,response: OK*24␍␊
[16:52:14:643] $GPRMC,085115.00,A,3150.93331124,N,11717.59467080,E,0.014,5.3,101123,5.6,W,D*21␍␊
[16:52:15:643] $GPRMC,085116.00,A,3150.93332581,N,11717.59468186,E,0.041,59.4,101123,5.6,W,D*1C␍␊
[16:52:16:641] $GPRMC,085117.00,A,3150.93333013,N,11717.59468264,E,0.066,81.7,101123,5.6,W,D*1E␍␊
[16:52:17:642] $GPRMC,085118.00,A,3150.93333688,N,11717.59468779,E,0.071,77.2,101123,5.6,W,D*16␍␊
[16:52:18:645] $GPRMC,085119.00,A,3150.93333483,N,11717.59468236,E,0.038,89.2,101123,5.6,W,D*1C␍␊
[16:52:19:646] $GPRMC,085120.00,A,3150.93334090,N,11717.59465670,E,0.061,337.7,101123,5.6,W,D*23␍␊
[16:52:20:646] $GPRMC,085121.00,A,3150.93331782,N,11717.59463967,E,0.082,331.4,101123,5.6,W,D*24␍␊
[16:52:21:646] $GPRMC,085122.00,A,3150.93330466,N,11717.59461499,E,0.089,238.0,101123,5.6,W,D*26␍␊
为了更接近串口通信获取到的数据，进行一些简单的处理
sed 's/^\[.*\] //' input.txt > output.txt
sed 's/..$//' input.txt > output.txt
s/^\[.*\] //：替换每一行开头的**[16:52:14:643]为空白。这里使用正则表达式^\[.*\]匹配每一行开头的[16:52:14:643]**，然后用空白替换- 使用正则表达式
..$匹配每一行末尾的最后两个字符，然后将其替换为空白
处理后如下
$command,gprmc 1,response: OK*24
$GPRMC,085115.00,A,3150.93331124,N,11717.59467080,E,0.014,5.3,101123,5.6,W,D*21
$GPRMC,085116.00,A,3150.93332581,N,11717.59468186,E,0.041,59.4,101123,5.6,W,D*1C
$GPRMC,085117.00,A,3150.93333013,N,11717.59468264,E,0.066,81.7,101123,5.6,W,D*1E
$GPRMC,085118.00,A,3150.93333688,N,11717.59468779,E,0.071,77.2,101123,5.6,W,D*16
$GPRMC,085119.00,A,3150.93333483,N,11717.59468236,E,0.038,89.2,101123,5.6,W,D*1C
$GPRMC,085120.00,A,3150.93334090,N,11717.59465670,E,0.061,337.7,101123,5.6,W,D*23
$GPRMC,085121.00,A,3150.93331782,N,11717.59463967,E,0.082,331.4,101123,5.6,W,D*24
注意进行 3 组实验
- 原地顺时针转向 — clockwise.txt
- 原地逆时针转向 — anticlockwise.txt
- 任意轨迹运动 — path.txt
GPRMC 指令的纬度、经度、航向角信息分别对应 4、6、9 字段
参考陆辉东编写的 gps_sensor 功能包编写解析代码如下
#include <iostream>
#include <string.h>
#include <stdio.h>
#include <boost/algorithm/string.hpp>
#include <vector>
#include <cstdlib>
#include <sstream>
#include <fstream>
#include <math.h>
using namespace std;static double EARTH_RADIUS = 6371.393;//6378.137;//地球半径struct my_pose
{double latitude;double longitude;
};// 角度制转弧度制
double rad(double d)
{return d * 3.1415926 / 180.0;
}int main(int argc, char **argv)
{const string filePath = "/home/redwall/catkin_ws/src/gps_sensor/log/2023-11-10/clockwise.txt";ifstream log_file(filePath);if (!log_file.is_open()){cerr << "Failed to open the file!" << endl;return -1;}bool is_init_pose = false;my_pose init_pose;string one_line;vector<string> raw_strs;while (getline(log_file, one_line)){// split one line data by ","boost::split(raw_strs, one_line, boost::is_any_of(","));cout << "raw data:" << raw_strs[3] << "\t" << raw_strs[5] << "\t" << raw_strs[8] << endl;double latitude, longitude, heading_angle;vector<string> data_strs;boost::split(data_strs, raw_strs[3], boost::is_any_of("."));latitude = atoi(data_strs[0].c_str()) / 100 + (atoi(data_strs[0].c_str()) % 100 + atoi(data_strs[1].c_str()) / 100000000.0) / 60.0;boost::split(data_strs, raw_strs[5], boost::is_any_of("."));longitude = atoi(data_strs[0].c_str()) / 100 + (atoi(data_strs[0].c_str()) % 100 + atoi(data_strs[1].c_str()) / 100000000.0) / 60.0;heading_angle = stod(raw_strs[8]);cout << "latitude: " << latitude << "\tlongitude: " << longitude << "\theading_angle: " << heading_angle << endl;if (!is_init_pose){init_pose.latitude = latitude;init_pose.longitude = longitude;is_init_pose = true;}else{// 计算相对位置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);// 计算xdelta_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;// 计算ydelta_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 << "x: " << x << "\ty: " << y << endl;cout << "---------" << endl;}}return 0;
}
输出如下，与原始数据中的对应字段相符
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
---------
raw data:3150.93331782 11717.59463967 331.4
latitude: 31.8489 longitude: 117.293 heading_angle: 331.4
x: 0.0121951 y: 0.0490088
---------
raw data:3150.93330466 11717.59461499 238.0
latitude: 31.8489 longitude: 117.293 heading_angle: 238
x: 0.0121951 y: 0.0878631
---------
raw data:3150.93333556 11717.59459573 269.0
latitude: 31.8489 longitude: 117.293 heading_angle: 269
x: 0.0450738 y: 0.118185
---------
raw data:3150.93335679 11717.59456484 265.9
latitude: 31.8489 longitude: 117.293 heading_angle: 265.9
x: 0.0844207 y: 0.166816
---------
raw data:3150.93336170 11717.59454041 282.2
latitude: 31.8489 longitude: 117.293 heading_angle: 282.2
x: 0.0935207 y: 0.205276
---------
raw data:3150.93338996 11717.59451834 294.5
latitude: 31.8489 longitude: 117.293 heading_angle: 294.5
x: 0.145897 y: 0.240022
---------
raw data:3150.93342933 11717.59450323 334.3
latitude: 31.8489 longitude: 117.293 heading_angle: 334.3
x: 0.218864 y: 0.26381
---------
raw data:3150.93345386 11717.59449815 338.9
latitude: 31.8489 longitude: 117.293 heading_angle: 338.9
x: 0.264327 y: 0.271807
---------
raw data:3150.93348823 11717.59450537 10.2
latitude: 31.8489 longitude: 117.293 heading_angle: 10.2
x: 0.328027 y: 0.260441
---------
raw data:3150.93353467 11717.59451156 6.5
latitude: 31.8489 longitude: 117.293 heading_angle: 6.5
x: 0.414097 y: 0.250696
---------
raw data:3150.93358054 11717.59453627 24.7
latitude: 31.8489 longitude: 117.293 heading_angle: 24.7
x: 0.499111 y: 0.211794
---------
raw data:3150.93360846 11717.59458635 56.9
latitude: 31.8489 longitude: 117.293 heading_angle: 56.9
x: 0.550857 y: 0.132952
---------
raw data:3150.93366693 11717.59460345 14.0
latitude: 31.8489 longitude: 117.293 heading_angle: 14
x: 0.659223 y: 0.106031
---------
raw data:3150.93370425 11717.59465149 47.7
latitude: 31.8489 longitude: 117.293 heading_angle: 47.7
x: 0.72839 y: 0.0304002
---------
raw data:3150.93372192 11717.59471207 63.2
latitude: 31.8489 longitude: 117.293 heading_angle: 63.2
x: 0.761139 y: 0.0649724
---------
raw data:3150.93370758 11717.59477295 87.8
latitude: 31.8489 longitude: 117.293 heading_angle: 87.8
x: 0.734562 y: 0.160817
---------
raw data:3150.93369450 11717.59483801 93.8
latitude: 31.8489 longitude: 117.293 heading_angle: 93.8
x: 0.71032 y: 0.263243
---------
raw data:3150.93367488 11717.59488022 116.6
latitude: 31.8489 longitude: 117.293 heading_angle: 116.6
x: 0.673957 y: 0.329695
---------
raw data:3150.93365466 11717.59492160 118.6
latitude: 31.8489 longitude: 117.293 heading_angle: 118.6
x: 0.636482 y: 0.394841
---------
raw data:3150.93361987 11717.59494557 122.0
latitude: 31.8489 longitude: 117.293 heading_angle: 122
x: 0.572003 y: 0.432577
---------
raw data:3150.93356921 11717.59496880 130.5
latitude: 31.8489 longitude: 117.293 heading_angle: 130.5
x: 0.478112 y: 0.469149
---------
raw data:3150.93353325 11717.59498063 148.4
latitude: 31.8489 longitude: 117.293 heading_angle: 148.4
x: 0.411465 y: 0.487773
---------
raw data:3150.93347774 11717.59498615 164.2
latitude: 31.8489 longitude: 117.293 heading_angle: 164.2
x: 0.308585 y: 0.496464
---------
raw data:3150.93341225 11717.59497409 183.9
latitude: 31.8489 longitude: 117.293 heading_angle: 183.9
x: 0.187208 y: 0.477477
---------
raw data:3150.93335876 11717.59495099 200.5
latitude: 31.8489 longitude: 117.293 heading_angle: 200.5
x: 0.0880718 y: 0.44111
---------
raw data:3150.93331556 11717.59490935 227.1
latitude: 31.8489 longitude: 117.293 heading_angle: 227.1
x: 0.00800653 y: 0.375555
---------
raw data:3150.93328314 11717.59486159 233.1
latitude: 31.8489 longitude: 117.293 heading_angle: 233.1
x: 0.0520795 y: 0.300366
---------
raw data:3150.93326676 11717.59479839 239.3
latitude: 31.8489 longitude: 117.293 heading_angle: 239.3
x: 0.0824376 y: 0.200868
---------
raw data:3150.93325654 11717.59472604 255.8
latitude: 31.8489 longitude: 117.293 heading_angle: 255.8
x: 0.101379 y: 0.0869658
---------
raw data:3150.93325328 11717.59474098 253.8
latitude: 31.8489 longitude: 117.293 heading_angle: 253.8
x: 0.107421 y: 0.110486
---------
raw data:3150.93324540 11717.59474752 255.1
latitude: 31.8489 longitude: 117.293 heading_angle: 255.1
x: 0.122025 y: 0.120782
---------
raw data:3150.93324398 11717.59474844 280.4
latitude: 31.8489 longitude: 117.293 heading_angle: 280.4
x: 0.124657 y: 0.122231
---------
raw data:3150.93324677 11717.59474572 270.3
latitude: 31.8489 longitude: 117.293 heading_angle: 270.3
x: 0.119486 y: 0.117948
---------
需要在 MATLAB 中进一步分析结果
和fgets())
