EurekaMoments

This is my studying logs about Autonomous driving, Machine learning technologies and etc.

ロボットを制御するためのモデル化の基礎とPythonプログラム

Pythonによる制御工学入門

Pythonによる制御工学入門

  • 作者:裕樹, 南
  • 発売日: 2019/05/22
  • メディア: 単行本

目次

背景・目的

ここ最近、制御工学を勉強したい熱が高まって
きたので早速取り組み始めました。主に、
ロボットの自律移動や車の自動運転のための
制御を学んでいきます。
今回は記念すべき第1回。移動ロボットとして
一般的な、対向2輪型ロボットを制御するための
運動モデルの定義と、その動作を学ぶための
Pythonサンプルプログラムについて書きます。

f:id:sy4310:20200420212343g:plain

対向2輪型ロボット

2個の車輪を左右対称にロボットに取り付け、
それぞれを独立に駆動する形態であり、両輪
の回転によって制御されます。
f:id:sy4310:20200405125851p:plain 図中の文字は、それぞれ下記を定義したもの
になります。
f:id:sy4310:20200405125803p:plain

モデル化

ロボットを制御するためには、その特徴を
数学的に捉えたモデルを作る必要があります。
世の中の制御対象の多くは動的なシステム
であり、現在の出力に過去の入力が関係する
システムです。
ここでは、システムに与える入力、得られる
出力、システムの状態を表す状態変数を
定義して、これらを一つの式でまとめた
状態空間モデルという形で表現します。

ja.wikipedia.org

前提条件

車輪移動ロボットの動作を数学的にモデル化
する際、それをシンプルで理解しやすいもの
とするために、「車輪は滑らない」という
前提の元でモデル化するのが一般的です。

車輪が滑らないとした場合、下記の事が言える
ようになり、ロボットの挙動が理解しやすく
なります。

  • 車輪は車軸に垂直な方向にしか移動しない。
  • 車軸方向へのドリフトはしない。
  • 運動中は、各瞬間で旋回中心が存在する。
  • 全ての車軸の回転軸は旋回中心を通る。

状態変数の定義

状態変数とは、任意の時点におけるシステム
全体の状態を表せる変数群の最小の部分集合
の事です。今回のような移動ロボットの場合、
ロボットの位置座標と方位角とするのが一般
的であり、それらを下記のようなベクトルの
形で表現します。

f:id:sy4310:20200405151100p:plain

入力の定義

車輪移動ロボットを動かすための入力は、
一般には進行方向速度vと旋回角速度ω
です。

この時、左右車輪の回転角速度がエンコーダ
などのセンサで取得できるとした場合、
入力vとωはロボットのトレッドと車輪半径
も考慮して下記のような式で決定されます。

f:id:sy4310:20200405154847p:plain

出力の定義

ロボットの制御プログラムは、基本的
には一定の時間間隔で車輪のエンコーダ
などの情報を読み取り処理を行う事から、
離散的で時変なシステムと考える事が
出来ます。

これに従い、状態空間モデルは下記の
ように定義されます。

f:id:sy4310:20200405171442p:plain

上段の式を状態方程式、下段の式を
出力方程式と呼びます。

ある時刻kにおけるx(k)を状態ベクトル、
y(k)を出力ベクトル、u(k)を入力ベクトル
と呼びます。

また、A(k)を状態行列、B(k)を入力行列、
C(k)を出力行列、D(k)を直達行列と
呼びます。

ただし、大抵の場合は単純化するために、
直達行列D(k)は零行列とする、つまり
システムには直達項が無いものとして
扱う事が多いです。

以上を踏まえて、今回の2輪ロボットの
状態方程式は下記のようになります。

f:id:sy4310:20200405174717p:plain

また出力方程式は、基本的には状態ベクトル
x(k)がそのままシステムとしての出力となる
ので、

f:id:sy4310:20200405175343p:plain

となります。

Pythonプログラム

上記のモデルによりロボットがどのように
振る舞うかをシミュレートするサンプル
プログラムをPythonで作りました。

システムに与える入力は、前半5秒は
右車輪の回転速度を速くして左旋回、
後半5秒は左車輪の回転速度を速くして
右旋回させるようにしています。

このプログラムを実行すれば、記事の冒頭に
載せているような移動シミュレーションが
実行されます。

"""
Two wheel motion model sample

author: Shisato Yano (@4310sy)
"""

import numpy as np
from math import cos, sin
import matplotlib.pyplot as plt
from PIL import Image
import os

show_plot = True

DELTA_TIME = 0.1 # time tick [s]
SIM_TIME = 10 # simulation time [s]

class TwoWheelModel:

    def __init__(self, tr, td):
        """
        Initialize two wheel model

        tr: tire radius [m]
        td: tread [m]
        """

        self.tire_radius_m = tr
        self.tread_m = td

    def calculate_input(self, tasr, tasl):
        """
        tasr: tire angular speed right [rad/s]
        tasl: tire angular speed left [rad/s]
        :return: speed input [m/s], yaw rate input [rad/s]
        """

        mat = np.array([[self.tire_radius_m/2, self.tire_radius_m/2],
                         [self.tire_radius_m/self.tread_m, -self.tire_radius_m/self.tread_m]])

        vec = np.array([[tasr], [tasl]])

        input_vec = np.dot(mat, vec)

        return input_vec

    def control_input(self, time):
        if time <= 5.0:
            tasr = 0.6 # [rad/s]
            tasl = 0.1 # [rad/s]
        else:
            tasr = 0.1 # [rad/s]
            tasl = 0.6 # [rad/s]

        input_vec = self.calculate_input(tasr, tasl)

        return input_vec

    def output_equation(self, state, input_vec):
        out_mat = np.array([[1, 0, 0],
                            [0, 1, 0],
                            [0, 0, 1]])

        dirc_mat = np.array([[cos(state[2, 0]) * DELTA_TIME, 0],
                             [sin(state[2, 0]) * DELTA_TIME, 0],
                             [0, DELTA_TIME]])

        out_vec = np.dot(out_mat, state) + np.dot(dirc_mat, input_vec)

        return out_vec

    def create_gif(self, im_num):
        images = []

        for num in range(im_num):
            im_name = str(num) + '.png'
            im = Image.open(im_name)
            images.append(im)
            os.remove(im_name)

        images[0].save('TwoWheelModel.gif', save_all=True, append_images=images[1:], loop=0, duration=60)

def main():
    print("Run " + __file__)

    # set parameters
    tire_radius_m = 0.08
    tread_m = 0.26

    # initialize
    two_wheel = TwoWheelModel(tire_radius_m, tread_m)

    # elapsed time
    time = 0.0

    # state vector: x, y, yaw
    st_vec = np.zeros((3, 1))

    # figure
    ax_xy = plt.subplot(1, 1, 1)
    plt_xy, = ax_xy.plot([], [], '.', c='b', ms=10)
    ax_xy.set_xlim([0.0, 0.26])
    ax_xy.set_ylim([-0.05, 0.15])
    ax_xy.set_xlabel("X [m]")
    ax_xy.set_ylabel("Y [m]")
    ax_xy.grid(True)

    # image number
    im_num = 0

    # simulation
    st_x = []
    st_y = []
    st_yaw = []
    while SIM_TIME >= time:
        time += DELTA_TIME

        input_vec = two_wheel.control_input(time)

        st_vec = two_wheel.output_equation(st_vec, input_vec)

        st_x.append(st_vec[0, 0])
        st_y.append(st_vec[1, 0])
        st_yaw.append(st_vec[2, 0])

        if show_plot:
            plt_xy.set_data(st_x, st_y)
            plt.savefig(str(im_num) + '.png')
            im_num += 1
            plt.pause(0.001)

    if im_num > 0:
        two_wheel.create_gif(im_num)

    if show_plot:
        plt.plot(st_x, st_y, ".b")
        plt.grid(True)
        plt.axis("equal")
        plt.xlabel("X [m]")
        plt.ylabel("Y [m]")
        plt.show()

if __name__ == '__main__':
    main()

GitHubリポジトリ

上記のコードは、こちらのGitHub
リポジトリで公開しています。

github.com

参考資料

myenigma.hatenablog.com

www.mech.tohoku-gakuin.ac.jp