RaspberryPi[66]　RTKを応用した設備の相対位置監視（変位検知）

2025年11月23日 最終更新日時 : 2025年11月23日 mack0113

概要

F9Pを用いたRTKを応用することで、各GPSの位置の変位を確認することができる。ZED-F9P が出す「基準局に対する相対位置（北・東・下）」のUBXバイナリメッセージであるRELPOSNED。これを見ることによって以下のことがわかります。

  • N：North（北方向相対距離）
  • E：East（東方向相対距離）
  • D：Down（高度差）
  • 単位：cm + mm の高精度成分

移動局を複数持ってそれぞれの位置関係を関数で表すことができれば、絶対値が不要な相対位置監視ができることになります。

RELPOSNED

RELPOSNEDはF9PのUBXの中にあるメッセージでこれを取り出せば目的の要素データを取得することができます。これは、RTKが動作しているときにしか出てきませｎ。したがって、構築はRTKができていることが最低要件になります。

GNSSには2種類の測位方式がある

方式元にする信号精度用途
コード測位 (Pseudo-range)擬似距離（C/Aコード）m〜数m単独測位・NMEA
位相測位 (Carrier phase)搬送波の位相mm〜cmRTK・相対測位・RELPOSNED

ZED-F9P が RELPOSNED を出せる条件：


搬送波位相を使っている（carrier phase tracking）
基準局からRTCMを受信している（差分）
整数アンビギュイティが解けている（FIX）
これによって
GPS衛星の位相差 → 2点間の相対距離を mm精度で求められる。

◆ 位相測位がなぜ正確か（仕組み）
搬送波L1の波長：

L1 = 約19cm
位相を 1/100 程度まで読めれば：

19cm / 100 ≈ 2mm精度
ただし位相は**波の周回数が分からない（アンビギュイティ問題）**ので、
RTK FIX = 周回数が整数一致した状態
RTK FLOAT = まだ整数決まってない状態

◆ RELPOSNEDの CARR が示すもの
CARR (bit3-4)
意味
状態
0
non-carrier / 未収束
単独 or RTK未成立
1
float（位相追跡してるが整数未決定）
cm〜十cm
**2
fix（整数アンビギュイティ決定）**
mm〜cm ←本命
つまり RELPOSNED の 精度指標 = CARR

◆ NMEAとの決定的な違い
出力
基準
精度
位相利用
NMEA GGA
地球座標（WGS84）
m〜cm（RTK FIX時）
利用するけど丸め大きい
RELPOSNED
基準局に対する相対（N/E/D）
mm〜cm
位相そのものを反映
NMEAは"ユーザー向けレポート"
RELPOSNEDは"測位エンジン内部値"に近い。
特に HP成分（mm） が位相由来の精度を担っている。

◆ まとめ
RELPOSNEDは 搬送波位相を使った相対測位結果
RTK FIX時は 整数アンビギュイティが解けて mm級
NMEAは結果を座標にして出すだけで、相対精度は落ちる
相対位置・姿勢・三角測量には RELPOSNED が最適

RELPOSNEDの取り出し

ラズパイによるRTKでRTKの構築は説明してありますし、UBXの取り出し方も記載しているので参考にしてください。ここでは、RELPOSNEDを取り出すコードから書いていきます。

#include <stdio.h>
#include <stdint.h>
#include <stdlib.h>

#define UBX_SYNC1 0xB5
#define UBX_SYNC2 0x62
#define CLS_NAV 0x01
#define ID_RELPOS 0x3C
#define PAYLOAD_LEN 64

static uint8_t get_byte(void)
{
    int c = getchar();
    if (c == EOF)
        exit(0);
    return (uint8_t)c;
}

static uint16_t get_u2(void)
{
    uint8_t b0 = get_byte();
    uint8_t b1 = get_byte();
    return (uint16_t)(b0 | (b1 << 8));
}

int main(void)
{
    while (1)
    {
        // ---- UBX 同期 ----
        uint8_t b;
        do
        {
            b = get_byte();
        } while (b != UBX_SYNC1);

        if (get_byte() != UBX_SYNC2)
            continue;

        uint8_t cls = get_byte();
        uint8_t id = get_byte();
        uint16_t len = get_u2();

        if (!(cls == CLS_NAV && id == ID_RELPOS && len == PAYLOAD_LEN))
        {
            // このフレームはスキップ（payload + checksum 2バイト）
            for (uint16_t i = 0; i < len + 2; i++)
                get_byte();
            continue;
        }

        // ---- ペイロード読み込み ----
        uint8_t buf[PAYLOAD_LEN];
        for (int i = 0; i < PAYLOAD_LEN; i++)
        {
            buf[i] = get_byte();
        }

        // チェックサム捨て
        get_byte();
        get_byte();

        // ---- cm成分 ----
        int32_t relPosN = *(int32_t *)(buf + 8);  // [cm]
        int32_t relPosE = *(int32_t *)(buf + 12); // [cm]
        int32_t relPosD = *(int32_t *)(buf + 16); // [cm]

        // ---- 高精度 mm 成分（±127mm）----
        int8_t relPosHPN = *(int8_t *)(buf + 20); // [mm]
        int8_t relPosHPE = *(int8_t *)(buf + 21); // [mm]
        int8_t relPosHPD = *(int8_t *)(buf + 22); // [mm]

        // ---- cm + mm を合成して mm に統一 ----
        int32_t relPosN_mm = relPosN * 10 + relPosHPN; // [mm]
        int32_t relPosE_mm = relPosE * 10 + relPosHPE; // [mm]
        int32_t relPosD_mm = relPosD * 10 + relPosHPD; // [mm]

        // ---- flags（offset 60）----
        uint32_t flags = *(uint32_t *)(buf + 60);

        int gnssFixOK = (flags >> 0) & 0x01;
        int diffSoln = (flags >> 1) & 0x01;
        int relPosValid = (flags >> 2) & 0x01;
        int carrSoln = (flags >> 3) & 0x03; // 0:none 1:float 2:fix
        int isMoving = (flags >> 5) & 0x01;
        int refPosMiss = (flags >> 6) & 0x01;
        int refObsMiss = (flags >> 7) & 0x01;
        int relPosHeadingValid = (flags >> 8) & 0x01;

        // ---- 表示 ----
        printf("N=%d mm E=%d mm D=%d mm  ",
               relPosN_mm, relPosE_mm, relPosD_mm);

        printf("FLAGS=0x%08X gnssFixOK=%d diffSoln=%d relPosValid=%d ",
               flags, gnssFixOK, diffSoln, relPosValid);

        printf("CARR=%d (0:none 1:float 2:fix) isMoving=%d refPosMiss=%d refObsMiss=%d relPosHeadOK=%d\n",
               carrSoln, isMoving, refPosMiss, refObsMiss, relPosHeadingValid);

        fflush(stdout);
    }
}

使い方

sudo stty -F /dev/ttyAMA0 115200 raw -echo -onlcr -icanon
sudo ./relpos < /dev/ttyAMA0

結果

N=620 mm E=-850 mm D=30 mm  FLAGS=0x00000117 gnssFixOK=1 diffSoln=1 relPosValid=1 CARR=2 (0:none 1:float 2:fix) isMoving=0 refPosMiss=0 refObsMiss=0 relPosHeadOK=1

これだと毎回、stty の設定が必要なので、プログラム内で設定する版です。

#include <stdio.h>
#include <stdint.h>
#include <stdlib.h>
#include <unistd.h>
#include <fcntl.h>
#include <termios.h>
#include <string.h>
#include <errno.h>

#define UBX_SYNC1 0xB5
#define UBX_SYNC2 0x62
#define CLS_NAV   0x01
#define ID_RELPOS 0x3C
#define PAYLOAD_LEN 64

// ----- シリアル設定 -----
int open_serial(const char *dev, speed_t baud)
{
    int fd = open(dev, O_RDONLY | O_NOCTTY);
    if (fd < 0) {
        perror("open_serial");
        return -1;
    }

    struct termios tio;
    if (tcgetattr(fd, &tio) < 0) {
        perror("tcgetattr");
        close(fd);
        return -1;
    }

    cfmakeraw(&tio);              // rawモード

    cfsetispeed(&tio, baud);
    cfsetospeed(&tio, baud);

    tio.c_cflag |=  (CLOCAL | CREAD);
    tio.c_cflag &= ~(PARENB | CSTOPB | CSIZE);
    tio.c_cflag |=  CS8;          // 8N1

    if (tcsetattr(fd, TCSANOW, &tio) < 0) {
        perror("tcsetattr");
        close(fd);
        return -1;
    }

    return fd;
}

// ----- 読み取りユーティリティ -----
static uint8_t get_byte(int fd)
{
    uint8_t b;
    ssize_t n;

    while (1) {
        n = read(fd, &b, 1);
        if (n == 1) return b;
        if (n < 0) {
            if (errno == EINTR) continue;
            perror("read");
            exit(1);
        }
        // n == 0: EOF 相当だけどシリアルなので基本来ない
    }
}

static uint16_t get_u2(int fd)
{
    uint8_t b0 = get_byte(fd);
    uint8_t b1 = get_byte(fd);
    return (uint16_t)(b0 | (b1 << 8));
}

// ----- main -----
int main(int argc, char *argv[])
{
    const char *dev = "/dev/ttyAMA0";
    if (argc >= 2) {
        dev = argv[1];    // 引数でデバイス指定可能
    }

    int fd = open_serial(dev, B115200);
    if (fd < 0) {
        fprintf(stderr, "Failed to open serial: %s\n", dev);
        return 1;
    }

    while (1) {
        // UBX同期
        uint8_t b;
        do {
            b = get_byte(fd);
        } while (b != UBX_SYNC1);

        if (get_byte(fd) != UBX_SYNC2) continue;

        uint8_t cls = get_byte(fd);
        uint8_t id  = get_byte(fd);
        uint16_t len = get_u2(fd);

        if (!(cls == CLS_NAV && id == ID_RELPOS && len == PAYLOAD_LEN)) {
            // payload + checksum を読み捨て
            for (uint16_t i = 0; i < len + 2; i++) {
                get_byte(fd);
            }
            continue;
        }

        uint8_t buf[PAYLOAD_LEN];
        for (int i = 0; i < PAYLOAD_LEN; i++) {
            buf[i] = get_byte(fd);
        }

        // チェックサム捨て
        get_byte(fd);
        get_byte(fd);

        // cm成分
        int32_t relPosN = *(int32_t *)(buf + 8);
        int32_t relPosE = *(int32_t *)(buf + 12);
        int32_t relPosD = *(int32_t *)(buf + 16);

        // mm成分
        int8_t relPosHPN = *(int8_t *)(buf + 20);
        int8_t relPosHPE = *(int8_t *)(buf + 21);
        int8_t relPosHPD = *(int8_t *)(buf + 22);

        int32_t relPosN_mm = relPosN * 10 + relPosHPN;
        int32_t relPosE_mm = relPosE * 10 + relPosHPE;
        int32_t relPosD_mm = relPosD * 10 + relPosHPD;

        // flags
        uint32_t flags = *(uint32_t *)(buf + 60);

        int gnssFixOK          = (flags >> 0) & 0x01;
        int diffSoln           = (flags >> 1) & 0x01;
        int relPosValid        = (flags >> 2) & 0x01;
        int carrSoln           = (flags >> 3) & 0x03;   // 0:none 1:float 2:fix
        int isMoving           = (flags >> 5) & 0x01;
        int refPosMiss         = (flags >> 6) & 0x01;
        int refObsMiss         = (flags >> 7) & 0x01;
        int relPosHeadingValid = (flags >> 8) & 0x01;

        printf("N=%d mm E=%d mm D=%d mm  ", relPosN_mm, relPosE_mm, relPosD_mm);
        printf("FLAGS=0x%08X gnssFixOK=%d diffSoln=%d relPosValid=%d ",
               flags, gnssFixOK, diffSoln, relPosValid);
        printf("CARR=%d (0:none 1:float 2:fix) isMoving=%d refPosMiss=%d refObsMiss=%d relPosHeadOK=%d\n",
               carrSoln, isMoving, refPosMiss, refObsMiss, relPosHeadingValid);

        fflush(stdout);
    }

    close(fd);
    return 0;
}

実行はこれ

sudo ./relpos_serial /dev/ttyAMA0

カテゴリー
RaspberryPi
前の記事
RaspberryPi[13] WiringPi
2022年9月7日