点列で与えた経路の走行

概要

ロボットを道なりに走らせる場合など、走行させたい経路を点列で与えると便利な場合があります。 ここでは、点列で定義した連続直線(線分)に沿って走行させる方法を説明します。

点列で定義される連続直線の走行イメージ

点列と直線の定義

連続直線を構成する点が３つ与えられた場合、直線は２本定義できます。

３点から作られる２本の連続直線

ここで、連続直線を追従走行させるために、適当なタイミングで走行する直線を変えながら走行させることにします。
具体的には、何番目の直線を走行しているかを保持しておき、追従する直線を変更するタイミングまで移動したかを監視することにします。

追従する直線を変更するタイミング

追従する直線を変更するタイミングは、図の点 B を通り直線 AB に垂直な直線に、ある距離だけ点 A 側に寄せた直線です。
点 B に到達する少し前に走行する直線を指示することにより、滑らかに直線の乗り換えが行えます。

これらを実現するサンプルを以下に示します。

#include <iostream>
#include <vector>
#include "Roomba_driver.h"
#include "robot_utils.h"
#include "angle_utils.h"
#include "ticks.h"
#include "delay.h"
#include "log_printf.h"

using namespace hrk;
using namespace std;


namespace
{
    void create_follow_lines(vector<PositionF>& lines,
                             const vector<PointF>& points)
    {
        if (points.size() <= 1) {
            return;
        }

        // 始点と終点があったときに、終点の位置を通る直線を定義する
        vector<PointF>::const_iterator from_it = points.begin();
        for (vector<PointF>::const_iterator to_it = from_it + 1;
             to_it != points.end(); ++to_it, ++from_it) {
            Angle angle = angle_to(*from_it, *to_it);
            lines.push_back(PositionF(to_it->x(), to_it->y(), angle));
        }
    }


    void follow(Run_driver& robot, const vector<PositionF>& lines)
    {
        const double Line_change_distance = robot.path_change_distance();
        size_t line_index = 0;
        while (true) {
            // 直線への走行を開始する
            PositionF current_line = lines[line_index];
            robot.follow_line(current_line);

            // 直線を乗り換えるタイミングまで待つ
            while (robot.distance_to_perpendicular(current_line) <
                   -Line_change_distance) {
                // 本来は、センサによる計測などの処理をここで行う
                delay_sec(0.1);
            }

            size_t next_line_index = line_index + 1;
            if (lines.size() <= next_line_index) {
                // 次に追従すべき直線がなくなったら、戻る
                return;
            }

            line_index = next_line_index;
        }
    }
}


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

    Roomba_driver robot;
    if (!robot.open("/dev/ttyUSB0")) {
        cerr << FILE_POSITION() << robot.what() << endl;
        return 1;
    }

    // 点列の定義
    vector<PointF> points;
    points.push_back(PointF(0.0, 0.0));
    points.push_back(PointF(1.0, 0.0));
    points.push_back(PointF(1.0, 1.0));
    points.push_back(PointF(2.0, 1.0));

    // 追従する直線の生成
    vector<PositionF> lines;
    create_follow_lines(lines, points);

    // 走行の開始
    follow(robot, lines);

    // 次に走行すべき直線がなくなったら停止させる
    robot.stop();
    wait_stable(robot, 0.1);

    return 0;
}

実際は、このサンプルにセンサ情報の監視などの処理を追加する必要がありますが、基本はこのようなコードになります。