1. 理论准备

1.1 RTK工作原理

RTK：Real-Time Kinematic，实时载波技术，是一种高精度的卫星导航技术，可实现厘米级别的导航与定位要求。

现在的RTK分为：标准RTK、网络RTK以及ppp方式。

  • 标准RTK：
image-20240717182246856

其工作原理：

  1. 基准站先观测和接收卫星数据；
  2. 基准站通过旁边的无线电台（数据链），将观测数据实时发送给流动站（距离一般不超过20公里）；
  3. 流动站收到基准站数据的同时，也观测和接收了卫星数据；
  4. 流动站在基准站数据和自身数据的基础上，**根据相对定位原理，进行实时差分运算，**从而解算出流动站的三维坐标及其精度，其定位精度可达1cm~2cm。

  • 网络RTK：

    • 描述：利用多个基准站组成的网络，通过互联网将差分校正数据传输给移动站。移动站不需要独立设置基准站，只需接收网络提供的校正数据。
    • 优势：覆盖范围更广，无需自建基准站，适用于大面积区域的高精度定位。
    • 服务提供商：例如，千寻位置提供的高精度位置服务（如千寻FindCM）就是一种网络RTK服务。

  • PPP（Precise Point Positioning）：

    • 描述：利用全球导航卫星系统的精确轨道和钟差信息，通过精密单点定位算法实现高精度定位。

    • 优势：无需基准站，适用于全球范围，但收敛时间较长（通常在30分钟到几小时）。

    • 应用场景：适用于远离基准站网络的地区，如海上、偏远地区等。

1.2 坐标系

在GPS的使用中存在多种坐标系：惯性坐标系、地心坐标系以及导航坐标系。

image-20240717184629375

  • 惯性坐标系（ i 系 ）：

    • 以地球质心为原点，Z轴指向地球自转轴（即南北极轴，跟地球轨道面夹角66°34‘）， X轴位于赤道面指向空间任意点，Y轴与其构成右手系。
    • 该坐标系不随地球自转而转动，由于地球质心绕太阳公转以及太阳系绕银河系公转，因此，该系不是绝对惯性系， 但这些影响十分微弱，低于惯导的噪声水平，因而可以忽略不计，该系可以认为是一个惯性系。

  • 地心坐标系（ e 系 ）：

    • 又称：ECEF坐标系，其原点为地球的质心，X轴延伸通过本初子午线（0度经度）和赤道（0deglatitude）的交点， Z轴延伸通过的北极（即与地球自传轴重合），Y轴完成右手坐标系，穿过赤道和90度经度。

    • 使用最广泛的ECEF坐标系：WGS-84 （LLA）

image-20240717192727591

由于每年的标准不同，我们重点针对2005.0参考元年的WGS84坐标系进行讲解。

1.2.1 WGS84坐标系

image-20240717201007353

一句话解释就是：把前面提到的ECEF坐标系用在GPS中，就是WGS-84坐标系。
其中：
（1）：大地纬度是过用户点P的基准椭球面法线与赤道面的夹角。纬度值在-90°到+90°之间。北半球为正，南半球为负。

（2）：大地经度是过用户点P的子午面与本初子午线之间的夹角。经度值在-180°到+180°之间。

（3）：大地高度h是过用户点P到基准椭球面的法线距离，基准椭球面以内为负，以外为正。

  1. 参考历元2005.0：
    • 参考历元是用于定义坐标系的一个固定时间点，2005.0表示该版本的参考时间是2005年。
  2. 高精度地心坐标系：
    • WGS84是一个地心坐标系（地心地固坐标系，ECEF），其原点在地球质心，X轴指向格林尼治本初子午线与赤道的交点，Z轴指向地球的北极，Y轴垂直于X轴和Z轴，形成一个右手坐标系。
  3. 椭球参数：
    • 长半轴（a）：6378137.0米
    • 扁率（f）：1/298.257223563

坐标转换：

WGS84坐标（经度 λ\lambda、纬度 ϕ\phi、高度 hh）可以转换为地心地固坐标（ECEF）：

N=a1−e2sin⁡2(ϕ)X=(N+h)cos⁡(ϕ)cos⁡(λ)Y=(N+h)cos⁡(ϕ)sin⁡(λ)Z=(N(1−e2)+h)sin⁡(ϕ)\begin{aligned} &\text{N} =\frac a{\sqrt{1-e^2\sin^2(\phi)}} \\ &X=(N+h)\cos(\phi)\cos(\lambda) \\ &Y=(N+h)\cos(\phi)\sin(\lambda) \\ &Z=(N(1-e^{2})+h)\sin(\phi) \end{aligned}

其中：

  • aa 是地球的长半轴（6378137.0米）。
  • ee 是地球的偏心率（约0.0818191908426）。
  • NN 是卯酉圈曲率半径。

假设我们有一个点的WGS84坐标为：

  • 经度 λλ=45
  • 纬度 ϕ\phi=45
  • 高度 hh=100

我们可以计算其ECEF坐标：

N=6378137.01−0.08181919084262sin⁡2(45∘)≈6388838.29米X=(6388838.29+100)cos⁡(45∘)cos⁡(45∘)≈3194669.15米Y=(6388838.29+100)cos⁡(45∘)sin⁡(45∘)≈3194669.15米Z=(6388838.29⋅(1−0.08181919084262)+100)sin⁡(45∘)≈4487351.59米\begin{aligned} &\text{N} =\frac{6378137.0}{\sqrt{1-0.0818191908426^2\sin^2(45^\circ)}}\approx6388838.29\text{米} \\ &\text{X} =(6388838.29+100)\cos(45^\circ)\cos(45^\circ)\approx3194669.15\text{米} \\ &\text{Y} =(6388838.29+100)\cos(45^\circ)\sin(45^\circ)\approx3194669.15\text{米} \\ &\text{Z} =(6388838.29\cdot(1-0.0818191908426^2)+100)\sin(45^\circ)\approx4487351.59\text{米} \end{aligned}

这就是该点在2005年WGS84版本中的ECEF坐标。

1.2.2 ENU坐标系与UTM坐标系

ENU坐标系：

ENU（East-North-Up）坐标系是一种常用于局部坐标变换的坐标系，它的原点通常设置在某个已知点，如GPS基准站的所在地。

ENU坐标系相对于地球的局部平面定义，其中：

  • 原点：原点通常是一个已知的地理位置（经度 λ0\lambda_0，纬度 ϕ\phi_0$，高度 h0h_0）。
  • 轴方向：
    • E轴（东轴）：在局部水平面内指向正东方向。
    • N轴（北轴）：在局部水平面内指向正北方向。
    • U轴（上轴）：垂直于局部水平面向上指向天顶。

由于：三维直角坐标系来描述地球表面，实际应用较为困难，因此一般使用简化后的二维投影坐标系来描述。

UTM坐标系：

image-20240717202315053

UTM（Universal Transverse Mercator，通用横轴墨卡托）坐标系是一种基于墨卡托投影的平面坐标系，广泛用于大地测量和地图制作。UTM坐标系将地球表面划分为多个投影带，每个投影带覆盖6度的经度范围。

UTM坐标系的特点：

  1. 投影带：
    • 地球被划分为60个投影带，每个投影带覆盖6度的经度。
    • 投影带从西经180度开始，每6度为一个带，编号为1到60。
    • 每个投影带的中心经线称为中央经线。
  2. 北半球和南半球：
    • 北半球的UTM坐标使用N表示（例如，32N）。
    • 南半球的UTM坐标使用S表示（例如，32S）。
  3. 坐标单位：
    • UTM坐标采用米作为单位。
  4. 坐标原点：
    • 每个投影带的中央经线被赋予500,000米的东向伪东距（False Easting）。
    • 北半球的原点在赤道（0,000,000米），南半球的原点在赤道南5,000,000米。

UTM坐标转换：

将地理坐标（经度、纬度）转换为UTM坐标，主要涉及以下步骤：

  1. 确定投影带：

    • 投影带号 zone\text{zone} 由公式计算：

    zone=⌊longitude+1806⌋+1\text{zone}=\left\lfloor\frac{\text{longitude}+180}{6}\right\rfloor+1

  2. 计算投影坐标：

    • 使用高斯-克吕格公式或类似方法将地理坐标转换为平面坐标。

  3. 应用伪东距和伪北距：

    • 伪东距为500,000米。
    • 北半球没有伪北距，南半球伪北距为10,000,000米。

UTM坐标系的优点：

  • 精度高：适合大范围区域的精确测量和地图制作。
  • 计算简单：平面坐标便于距离和方向的计算。
  • 全球适用：可以覆盖全球范围。

下面将演示基于ROS环境下ENU坐标系转为UTM坐标系：

  • 安装
#include <ros/ros.h>
#include <sensor_msgs/NavSatFix.h>
#include <geometry_msgs/Point.h>
#include <cmath>

class WGS84ToUTMConverter
{
public:
    WGS84ToUTMConverter(const std::string& gps_topic, const std::string& utm_topic)
    {
        // 订阅NavSatFix消息
        gps_sub_ = nh_.subscribe(gps_topic, 10, &WGS84ToUTMConverter::gpsCallback, this);

        // 发布UTM坐标
        utm_pub_ = nh_.advertise<geometry_msgs::Point>(utm_topic, 10);
    }

    void gpsCallback(const sensor_msgs::NavSatFix::ConstPtr& msg) // 可改为自定义消息类型
    {
        double lat = msg->latitude;
        double lon = msg->longitude;
        double northing, easting;
        int zone;

        LLtoUTM(lat, lon, northing, easting, zone);

        geometry_msgs::Point utm_point;
        utm_point.x = easting;
        utm_point.y = northing;
        utm_point.z = msg->altitude;

        utm_pub_.publish(utm_point);
    }

private:
    ros::NodeHandle nh_;
    ros::Subscriber gps_sub_;
    ros::Publisher utm_pub_;

    void LLtoUTM(double Lat, double Long, double &UTMNorthing, double &UTMEasting, int &Zone)
    {
        // 常数
        double a = 6378137.0; // WGS84 长半轴
        double f = 1 / 298.257223563; // WGS84 曲率
        double k0 = 0.9996; // 比例因子

        double eccSquared = 2 * f - f * f;
        double eccPrimeSquared = eccSquared / (1 - eccSquared);
        double N, T, C, A, M;
        
        double LongOrigin;
        double LatRad = Lat * M_PI / 180.0;
        double LongRad = Long * M_PI / 180.0;
        double LongOriginRad;
        int ZoneNumber;

        // 分区
        ZoneNumber = int((Long + 180) / 6) + 1;

        // 特殊分区 Norway
        if (Lat >= 56.0 && Lat < 64.0 && Long >= 3.0 && Long < 12.0)
            ZoneNumber = 32;

        // 特殊分区 Svalbard
        if (Lat >= 72.0 && Lat < 84.0) {
            if (Long >= 0.0 && Long < 9.0) ZoneNumber = 31;
            else if (Long >= 9.0 && Long < 21.0) ZoneNumber = 33;
            else if (Long >= 21.0 && Long < 33.0) ZoneNumber = 35;
            else if (Long >= 33.0 && Long < 42.0) ZoneNumber = 37;
        }

        // 计算中央子午线
        LongOrigin = (ZoneNumber - 1) * 6 - 180 + 3; // +3 puts origin in middle of zone
        LongOriginRad = LongOrigin * M_PI / 180.0;

        N = a / sqrt(1 - eccSquared * sin(LatRad) * sin(LatRad));
        T = tan(LatRad) * tan(LatRad);
        C = eccPrimeSquared * cos(LatRad) * cos(LatRad);
        A = cos(LatRad) * (LongRad - LongOriginRad);

        M = a * ((1 - eccSquared / 4 - 3 * eccSquared * eccSquared / 64 - 5 * eccSquared * eccSquared * eccSquared / 256) * LatRad
            - (3 * eccSquared / 8 + 3 * eccSquared * eccSquared / 32 + 45 * eccSquared * eccSquared * eccSquared / 1024) * sin(2 * LatRad)
            + (15 * eccSquared * eccSquared / 256 + 45 * eccSquared * eccSquared * eccSquared / 1024) * sin(4 * LatRad)
            - (35 * eccSquared * eccSquared * eccSquared / 3072) * sin(6 * LatRad));

        UTMEasting = (k0 * N * (A + (1 - T + C) * A * A * A / 6
            + (5 - 18 * T + T * T + 72 * C - 58 * eccPrimeSquared) * A * A * A * A * A / 120)
            + 500000.0);

        UTMNorthing = (k0 * (M + N * tan(LatRad) * (A * A / 2 + (5 - T + 9 * C + 4 * C * C) * A * A * A * A / 24
            + (61 - 58 * T + T * T + 600 * C - 330 * eccPrimeSquared) * A * A * A * A * A * A / 720)));

        if (Lat < 0)
            UTMNorthing += 10000000.0; // 避免南半球出现负数坐标 增加10000000米偏移量

        Zone = ZoneNumber;
    }
};

int main(int argc, char** argv)
{
    ros::init(argc, argv, "wgs84_to_utm_converter");

    // 获取参数服务器中的话题名称
    ros::NodeHandle private_nh("~");
    std::string gps_topic;
    std::string utm_topic;
    
    private_nh.param("gps_topic", gps_topic, std::string("/gps_fix"));
    private_nh.param("utm_topic", utm_topic, std::string("/utm_point"));

    WGS84ToUTMConverter converter(gps_topic, utm_topic);

    ros::spin();
    return 0;
}

1.2.3 车体坐标系

车体坐标系通常以车辆的前方向为x轴，右方向为y轴，上方向为z轴。假设车辆的当前位置为参考点，并且车辆的航向角为 θ\theta（相对于北方向的偏转角），则将ENU坐标转换为车体坐标系的方法如下：

旋转矩阵：

[xyz]=[cos⁡(θ)sin⁡(θ)0−sin⁡(θ)cos⁡(θ)0001][ENU]\begin{bmatrix}x\\y\\z\end{bmatrix}=\begin{bmatrix}\cos(\theta)&\sin(\theta)&0\\-\sin(\theta)&\cos(\theta)&0\\0&0&1\end{bmatrix}\begin{bmatrix}E\\N\\U\end{bmatrix}

基于车身坐标系，就可以完成IMU、雷达、相机坐标系等传感器的坐标系变换了。

同时在实际使用RTK模块的时候一定要注意：

  • RTK类型，如：标准RTK、网络RTK、PPP
  • 配置信息，如：千寻挂载端口（端口不同对应坐标系不同）、参考元年等
  • RTK模块对坐标系的定义，如：ENU、END等

2. RTK的ROS驱动

不同的RTK设备，输出的数据格式不一样，通常存在两种方式：NMEA、二进制流

i. NMEA

  • 安装驱动：nmea_navsat_driver
cd catkin_ws/src
git clone https://github.com/ros-drivers/nmea_navsat_driver/
cd ..
catkin_make -DCATKIN_WHITELIST_PACKAGES='nmea_navsat_driver'
  • 参数设定：launch
<launch>

  <!-- 对nmea_serial_driver node的简单启动文件设计 -->
  <arg name="port" default="/dev/gps" />    <!-- 端口号 -->
  <arg name="baud" default="115200" />     <!--波特率-->
  <arg name="frame_id" default="gps" />     <!--参考坐标系-->
  <arg name="use_GNSS_time" default="False" />
  <arg name="time_ref_source" default="gps" />
  <arg name="useRMC" default="False" />

  <node name="nmea_serial_driver" pkg="nmea_navsat_driver" type="nmea_serial_driver" output="screen">
    <param name="port" value="$(arg port)"/>
    <param name="baud" value="$(arg baud)" />
    <param name="frame_id" value="$(arg frame_id)" />
    <param name="use_GNSS_time" value="$(arg use_GNSS_time)" />
    <param name="time_ref_source" value="$(arg time_ref_source)" />
    <param name="useRMC" value="$(arg useRMC)" />
  </node>

</launch>
  • PS：nmea_navsat_driver提供四个节点，nmea_topic_driver，nmea_serial_driver，nmea_topic_serial_reader、nmea_socket_driver
    • nmea_topic_serial_reader：此节点从串口读入GPS数据，然后封装为nmea_msgs/Sentence数据格式，发布话题 nmea_sentence。
    • **nmea_topic_driver：**此节点订阅话题nmea_sentence，然后根据NMEA0184协议解析，并发布解析后的数据，发布话题为：经纬度/fix，速度/vel，gps时间/time_reference和航向角/heading.
    • **nmea_serial_driver：**此节点从串口读数据，直接解析发布数据，相当于nmea_topic_driver节点和nmea_topic_serial_reader节点的结合体。
  • 上述代码发布话题：
    /fix : gps经纬度
    /vel : gps速度
    /time_reference : gps时间
    /heading : 航向角
    /extend_fix : GPSFix消息包含GPS卫星状态和定位信息

ii. 二进制流

该方法需要结合对应通讯协议，下面将基于NAV680AG组合导航设备进行ROS驱动设计，并将其封装。