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 组实验

1. 原地顺时针转向 — clockwise.txt
2. 原地逆时针转向 — anticlockwise.txt
3. 任意轨迹运动 — 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);
            // 计算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;

            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 中进一步分析结果