走行モジュールの作成 (記述中)

ルンバ用の走行モジュールの実装過程を説明しています。

作成する走行モジュール

ルンバに対し、走行経路を座標系で指定して走行させることのできるモジュールを作成します。走行経路を座標系で指定するために、ロボットが座標系のどこにいるかの管理を行います。ロボットの位置を推定するには、車輪に取り付けられたエンコーダの情報を利用します。
また、与えられた経路を走行させるために、ルンバの駆動輪を制御します。今回の実装では、ルンバの各輪に対して目標速度を与えて制御します。

• エンコーダ値を用いた自己位置推定
• 駆動輪の速度制御

エンコーダ値を用いた自己位置推定

ルンバの左右の駆動輪にはエンコーダが取り付けられています。このエンコーダの値を取得することにより、ルンバの移動情報を計算します。ルンバの移動情報は「ルンバがどの方向を向いているか」および「向いている方向にどれだけ移動したか」で計算します。

!!! 移動した図、に対して距離の値を書き込んだもの

ルンバの駆動輪は左右に取り付けられているため、両輪のエンコーダ変位の差がルンバの回転量を表します。また、両輪のエンコーダ変位の平均が、移動した距離を表します。

!!! ルンバの回転の変位、の式

!!! ルンバの移動した距離、の式

これらの式により、ルンバのエンコーダ単位系での X-Y 座標系と向きの情報が計算できます。あとは、ここまでの計算では X-Y 座標系の値はルンバのエンコーダ単位系の情報なので、これを [mm] 単位に変換するためにルンバのエンコーダが１だけ変位したときに、何 [mm] だけ移動するかを取得します。

エンコーダ１変位あたりの [mm] 移動距離の計測

エンコーダ変位を出力するプログラムを作成して、ある距離だけ手でルンバを押して直進させます。そして、移動後のルンバのエンコーダ変位の積分値から、エンコーダ１変位あたりの移動距離を導出します。

lib/robot/example/print_roomba_encoder.cpp

/*
  ルンバ用のエンコーダ値の表示するプログラム
*/

#include "roomba_utils.h"
#include "Connection.h"
#include "delay.h"

using namespace hrk;


int main(int argc, char *argv[])
{
    static_cast<void>(argc);
    static_cast<void>(argv);

    Connection* connection = roomba_open("/dev/ttyUSB0");
    if (!connection) {
        return 1;
    }
    change_to_full_mode(connection);

    print_encoder_values(connection);
    change_to_passive_mode(connection);

    return 0;
}

#include <iostream>
#include "roomba_utils.h"
#include "Serial.h"
#include "delay.h"

#include <cstdio>

using namespace hrk;
using namespace std;


namespace
{
    bool receive_header_character(Connection* connection, int timeout,
                                  char header, int max_retry_times)
    {
        for (int i = 0; i < max_retry_times; ++i) {
            char data;
            int n = connection->read(&data, 1, timeout);
            if (n != 1) {
                return false;
            }
            if (data == header) {
                return true;
            }
        }
        return false;
    }


    unsigned long decode_unsigned(const char* data, int size)
    {
        unsigned long value = 0;

        for (int i = 0; i < size; ++i) {
            value <<= 8;
            value += static_cast<unsigned char>(data[i]);
        }
        return value;
    }
}


void change_to_full_mode(Connection* connection)
{
    char full_command = 132;
    connection->write(&full_command, 1);
    delay_msec(10);
}


void change_to_passive_mode(Connection* connection)
{
    char passive_command = 128;
    connection->write(&passive_command, 1);
    delay_msec(10);
}


Connection* roomba_open(const char* device)
{
    static Serial serial;
    enum { Roomba_default_baudrate = 115200 };
    if (!serial.open(device, Roomba_default_baudrate)) {
        cout << "Serial::open(): " << serial.what() << endl;
        return NULL;
    }
    return &serial;
}


void print_encoder_values(Connection* connection)
{
#if 0
    while (true) {
        // 要求の送信
        enum {
            Query_list = 149,
            Encoder_counts_left = 43,
            Encoder_counts_right = 44,
            Angle = 20,
        };
        const char encoder_request_command[] = {
            Query_list, 3,
            Encoder_counts_left, Encoder_counts_right,
            Angle,
        };
        connection->write(encoder_request_command,
                          sizeof(encoder_request_command));

        // 応答の受信
        enum {
            Timeout = 100,
            Retry_times = 32,
            Header_character = 19,
        };
#if 0
        if (!receive_header_character(connection, Timeout,
                                      Header_character, Retry_times)) {
            cerr << "receive_header_character_failed." << endl;
            return;
        }
#endif

        enum { Buffer_size = 15 };
        char received_data[Buffer_size];
        int n = connection->read(received_data, Buffer_size, Timeout);
        for (int i = 0; i < n; ++i) {
            fprintf(stderr, "%d ", (unsigned char)received_data[i]);
        }
        fprintf(stderr, "\n");

        // !!!
    }


#else
    // 要求の開始
    enum {
        Stream_command = 148,
        Encoder_counts_left = 43,
        Encoder_counts_right = 44,
    };
    const char encoder_request_command[] = {
        static_cast<char>(Stream_command),
        static_cast<char>(2),
        static_cast<char>(Encoder_counts_left),
        static_cast<char>(Encoder_counts_right),
    };
    connection->write(encoder_request_command, sizeof(encoder_request_command));

    // エンコーダ情報
    bool encoder_value_initialized = false;
    unsigned short previous_encoder_left;
    unsigned short previous_encoder_right;
    int total_left_count = 0;
    int total_right_count = 0;
    int index = 0;

    // データの受信
    while (true) {
        enum {
            Timeout = 100,
            Header_character = 19,
            Retry_times = 32,
        };
        if (!receive_header_character(connection, Timeout,
                                      Header_character, Retry_times)) {
            cerr << "receive_header_character_failed." << endl;
            return;
        }

        enum { Buffer_size = 15 - 1 };
        char received_data[Buffer_size];
        int n = connection->read(received_data, Buffer_size, Timeout);
        if (n != Buffer_size) {
            cerr << "read data fail." << endl;
            return;
        }

        // 受信データのパース
        for (int i = 0; i < Buffer_size; ++i) {
            fprintf(stderr, "%d ", (unsigned char)received_data[i]);
        }
        fprintf(stderr, "\n");

        unsigned short encoder_left = decode_unsigned(&received_data[2], 2);
        unsigned short encoder_right = decode_unsigned(&received_data[5], 2);
        if (!encoder_value_initialized) {
            previous_encoder_left = encoder_left;
            previous_encoder_right = encoder_right;
            encoder_value_initialized = true;
        }
        short left_diff = encoder_left - previous_encoder_left;
        short right_diff = encoder_right - previous_encoder_right;
        fprintf(stderr, "[%d, %d]\n", left_diff, right_diff);
        previous_encoder_left = encoder_left;
        previous_encoder_right = encoder_right;
        total_left_count += left_diff;
        total_right_count += right_diff;

        cout << index << ", "
             << total_left_count << ", "
             << total_right_count << endl;
        ++index;
    }
#endif
}

このプログラムを用い、手でルンバを 1000 [mm] だけ押したときのエンコーダ変位の和のグラフを以下に示します。

この結果では 1000 [mm] あたりのエンコーダ変位は、約 2229 となりました。よって、エンコーダ１変位あたりの移動距離は 0.448 [mm] とみなします。

向き変位の計測

計測はルンバを手で押し、何周かその場で回転させて行います。そして、回転した角度量の和と左右エンコーダの差の和から、左右エンコーダの変位とルンバの向きの関係を導出します。

エンコーダ値の計測には lib/robot/example/print_roomba_encoder.cpp を利用します。このプログラムを用い、ルンバを１周ほど手で押して回転させたときの左右エンコーダ変位の積分値のグラフを以下に示します。

この結果では、ルンバが１周したときの左右エンコーダ変位の積分値の差は 3257 になりました。よって左右エンコーダ変位の差が１あるときに、ロボットは (2 * PI / 3257) だけ向きを変えることになります。

モジュールの動作確認

ここまでに導出した値を用いて、検算と動作確認を行います。

それでは、左右の車輪間の距離(トレッド)を用いた検算を行います。
まず、円周率と半径、円周の関係は !!! と表せます。今、ルンバがその場で１周したときの左右のエンコーダ変位の差が !!! になります。そのときの各輪の移動距離は エンコーダ１変位あたりの [mm] 移動距離の計測 で求めた値より !!! となります。この車輪の移動距離を円周の距離とみなしてトレッドを計算すると

!!! 式と値

となります。
実際にルンバの車輪間を実測すると !!! [mm] でした。この値より推定の計測は、おおよそ正しいように思えます。

また、ここまでの値を用いて取得したエンコーダ値からルンバの X-Y 座標での位置と向きを計算するプログラムを作成すると以下のようになります。

lib/robot/example/print_roomba_position.lua

-- ルンバの推定自己位置を表示する

require("setting")


-- ロボットとの接続を開く
local robot = setting.robot() or die(setting.what())

-- 位置を表示し続ける
while true do
   local position = robot:position()
   print("" .. position:x() .. ", " .. "" .. position:y() .. ", " .. "" .. position:angle():to_deg())
   delay(0.1)
end

また、このプログラムを手で適当に押したときの X, Y 位置を描画すると、以下のようになります。

!!! 手で押して、何かのコースを１周させたときの移動経路のグラフを貼りつける。

この結果から、とりあえずは動作しているのがわかります。
今回の値は手で押したときのデータを用いましたが、ロボットをプログラムで動作させての走行パラメータ推定は URG を用いた走行パラメータの推定 (記述中) にて行います。

駆動輪の速度制御

自己位置の推定がエンコーダ変位の差と平均とで計算できるように、移動する経路の制御も車輪速の差と平均とで定義できます。以降では、この車輪速の差を「回転速」と呼び、平均を「並進速」と呼ぶことにします。このプロジェクトでは、指定した経路を走行させる際に、並進速と回転速とを指定します。

!!! 並進、回転の速度から各輪の速度を求める計算式

加速度の計測

ルンバには各輪の車輪速を指定できるコマンドが用意されているため、車輪の制御にはこのコマンドを用いることにします。ただ、このコマンドを用いて制御ライブラリを作成する前に、このコマンドで指定した速度まで、どのように加速するかを確認しておきます。

加速度の確認には、エンコーダ変位を記録しつつ速度コマンドを発行するプログラムを利用します。

lib/robot/example/print_roomba_acceleration.cpp

/*
  ルンバのモータ回転の加速度を表示するためのプログラム
*/

#include "roomba_utils.h"
#include "Connection.h"
#include "delay.h"

using namespace hrk;


namespace
{
    void set_motor_velocity(Connection* connection,
                            short left_mm_per_sec, short right_mm_per_sec)
    {
        enum { Drive_direct_code = 145 };
        const char data[] = {
            static_cast<char>(Drive_direct_code),
            static_cast<char>(left_mm_per_sec >> 8),
            static_cast<char>(left_mm_per_sec & 0xff),
            static_cast<char>(right_mm_per_sec >> 8),
            static_cast<char>(right_mm_per_sec & 0xff),
        };
        connection->write(data, sizeof(data));
    }
}


int main(int argc, char *argv[])
{
    static_cast<void>(argc);
    static_cast<void>(argv);

    Connection* connection = roomba_open("/dev/ttyUSB0");
    if (!connection) {
        return 1;
    }
    change_to_full_mode(connection);

    // モータ回転速度の指示
    // 範囲は -500 から +500 まで。
    short mm_per_sec = +25;
    set_motor_velocity(connection, mm_per_sec, mm_per_sec);

    print_encoder_values(connection);

    change_to_passive_mode(connection);

    return 0;
}

このプログラムを用い、ルンバの車輪が接地しない状態で記録したグラフを以下に示します。データの記録は、速度を 10 [mm]/sec, 100 [mm]/sec にして行いました。

!!! グラフ

この結果から、車輪の速度をコマンドで指定したときは、その速度まで !!! の加速度で制御されることがわかります。

経路追従

自己位置の推定と速度の制御ができるようになったため、経路の追従を行う機能を作成していきます。

• 経路追従
  • 指定した直線に追従
  • 指定した直線に追従
  • 指定した角度だけ旋回
  • 指定した向きで停止

指定した直線に追従

!!! 図

与えたベクトルに沿って走行させます。

!!!

指定した直線に追従

!!!

指定した角度だけ旋回

!!!

指定した向きで停止

!!!