移動と自己位置の取得

概要

ここでは、ロボット(ルンバ)を移動台車として移動させる方法を説明します。
このライブラリでは、ロボットの走行軌跡を座標系上の直線、および曲線で指示します。

例えば、座標系の X 軸に沿ってロボットを走行させたい場合の C++ コードは以下のようになります。

hrk::Roomba_driver robot;
...
robot.follow_line(Position<long>(0, 0, deg(0)));

この処理は、ロボットがエンコーダ情報を用いた自己位置を元に実現されています。また、自己位置を推定しているため、ロボットの現在位置を取得することも可能です。

hrk::Roomba_driver robot;
...
const hrk::Position<double> position = robot.position();
cout << position.x() << ", " << position.y() << ", " << position.angle().to_deg() << endl;

このページでは、ロボットの移動させるときに用いる主なコマンドを紹介していきます。
ここで紹介していないコマンドについては hrk::Roomba_driver をご覧下さい。

自己位置の取得

ロボットの自己位置を取得します。
位置は、二次元座標系の X, Y および向きの３つの値で構成されます。

自己位置の初期値は (0, 0, deg(0)) で、原点上で X 軸の方を向いた状態です。 また、取得できる自己位置はエンコーダ情報を元に推定された位置なため、走行距離が長くなるにつれて実際の位置とは、ずれてきます。

以下は、直進しながら自己位置を表示するサンプルです。(完全なソースコード print_position.cpp)

    // 移動の開始
    robot.follow_line(PositionF(0, 0, deg(0)));

    // しばらく位置を表示してから停止させる
    const double Print_period_sec = 2.0;
    const double first_sec = ticks_sec();
    while ((ticks_sec() - first_sec) < Print_period_sec) {
        print_mm_position(robot.position());
        delay_sec(0.1);
    }

    robot.stop();
    wait_stable(robot, 0.1);

void print_mm_position(const hrk::PositionF& position)
{
    int mm_x = position.x() * 1000;
    int mm_y = position.y() * 1000;
    int degree = position.angle().to_deg();
    cout << mm_x << ", " << mm_y << ", " << degree << endl;
}

停止

ロボットに停止するよう指示します。
ただし、ロボットは急には止まれないため停止コマンドを発行してから停止するまで少し時間がかかります。 wait_stable() で待機することにより、実際に停止した状態になります。

    robot.stop();
    wait_stable(robot, 0.1);

直線追従

直線の経路を走行させる場合、その直線は座標系で指定します。
ここでは、直線追従の使い方を示すために、直角に交わる経路の走行を２通りの方向で実装します。

直線の曲り角で一旦停止する方法

曲り角で減速して一旦停止し、そこで 90 度だけ転回してから、再度走行する方法です。

• 発行する経路追従コマンド
  • 直線追従のコマンド (１回目)
  • 直線上に停止するコマンド
  • 90 度だけ転回するコマンド
  • 直線追従のコマンド (２回目)

このコマンドの処理を状態遷移図で表現すると、以下のようになります。

これらの処理を実現する C++ コードは、以下のようになります。

follow_lines_stop.cpp より抜粋

    // 移動の開始
    // 直線の点を最後の位置(1.0, 0.0)で定義してあるのは、
    // 残りの距離を計算するときに便利なため。
    const PositionF first_line(1.0, 0.0, deg(0));
    robot.follow_line(first_line);

    // 次の直線に近付くまでは移動させ続ける
    const PositionF next_line(1.0, 0.0, deg(90));
    const double Stop_distance = 0.3; // 次の直線への停止を開始する距離
    while (robot.distance_to_perpendicular(first_line) < -Stop_distance) {
        delay_sec(0.1);
    }

    // 直線上に停止させる
    robot.stop_to_line(first_line);
    wait_stable(robot, 0.1);

    // 90 度だけ転回
    robot.spin_to_direction(deg(90));
    wait_stable(robot, 0.1);

    // 次の直線追従
    robot.follow_line(next_line);

    // 停止させて終了する
    delay_sec(3.0);
    robot.stop();

曲がるタイミングで次の直線コマンドを発行する方法

曲り角で停止せずに、弧を描きながら次の直線に追従する方法です。

直線追従コマンドを発行すると、ロボットは漸近しながら指示された直線に走行しようとするよう実装されているため、このような軌跡は、２つめの直線追従コマンドを適切なタイミングで発行すると実現できます。

• 発行する経路追従コマンド
  • 直線追従コマンド (１回目)
  • 直線追従コマンド (２回目)

以下に、状態遷移図とプログラムを示します。

follow_lines_smooth.cpp より抜粋

    // 移動の開始
    const PositionF first_line(1.0, 0, deg(0));
    robot.follow_line(first_line);

    // 次の直線に近付くまでは移動させ続ける
    const double Line_change_distance = robot.path_change_distance();
    while (robot.distance_to_perpendicular(first_line) <
           -Line_change_distance) {
        delay_sec(0.1);
    }

    // 次の直線追従のコマンドを発行する
    PositionF next_line(1.0, 0.0, deg(90));
    robot.follow_line(next_line);

    // 停止させて終了する
    delay_sec(3.0);
    robot.stop();

円弧追従

走行させる円の軌跡は、円の中心と半径の長さを指定することで行います。また、半径の値が正の値のときは反時計回りで、半径の値が負のときには時計回りで走行します。

follow_circle.cpp より抜粋

    // 前方の半径 30 [cm] の円弧に追従させる
    const double radius = 0.3;
    const PointF center(0.5, 0.0);
    robot.follow_circle(center, radius);

    // 数秒だけ走行させてから停止する
    delay_sec(10.0);
    robot.stop();

指定された方向を向く

座標系上のどの方向を向くかを指定します。 hrk::Run_drive::spin() が移動する角度幅を指定するのに対し hrk::Run_driver::spin_to_direction() では、向かせたい方向を指定します。
ロボットが時計回りと反時計回りのどちらで指定した方向を向くかは、ロボットの状態に依存して決まります。

spin_to_direction.cpp より抜粋

    // 座標系で 120 [deg] の方を向かせる (ロボットの初期位置は (0, 0, deg(0))
    Angle target_angle = deg(120);
    robot.spin_to_direction(target_angle);

    // 向きが目的の角度まで移動したら終了する
    wait_stable(robot, 0.1);

指定された角度だけ旋回する

どれだけ回転するかを指定します。360 [deg] 以上の回転が指定できるので、ぐるぐると向きを変え続けるような行動を実現できます。

spin.cpp より抜粋

    // 時計まわりに 360 [deg] 転回させる
    robot.spin(deg(360));

    // コマンドの動作が安定するまで待つ
    wait_stable(robot, 0.1);

後進させる

ロボットを後ろに進ませるには、並進速度を負の値にして移動コマンドを発行します。

back_line.cpp より抜粋

    // 直線追従で後進させる
    double current_velocity = robot.straight_velocity();
    robot.set_straight_velocity(-current_velocity);

    robot.follow_line(PositionF(0, 0, deg(0)));

    // 数秒後に停止させる
    delay_sec(2.0);
    robot.stop();

back_circle.cpp より抜粋

    // 直線追従で後進させる
    double current_velocity = robot.straight_velocity();
    robot.set_straight_velocity(-current_velocity);

    robot.follow_line(PositionF(0, 0, deg(0)));

    // 数秒後に停止させる
    delay_sec(2.0);
    robot.stop();