前回のブログでは、「Vagrant」と「watchdog」を使ってシミュレータ上のロボットを動作させるための環境作りをしました。

今回はPID制御とRTMによる開発の詳細について解説します。

「PID制御」とはロボットに限らず様々な制御分野で用いられる制御方式で以下の式によって表されます。

制御理論の教科書を読むとこの式を変換して、理論面での議論が展開されることもあるので面食らいますが、PID制御は元は非常に実用的な観点で作られた制御方法であり、理論的な解析よりもまず実用的な使い方を学習することが大切です。このブログでは実用面に絞って解説を進めます。

まずは上の式をざっと見てみましょう。$e(t)$は制御対象の状態$y(t)$と目標値$r(t)$とのズレです。このズレをゼロに近づける（ロボットを指定した姿勢に近づける）のが制御の目的です。$u(t)$が制御対象に加える出力なのですが、この式は$K_p, K_i, K_d$の３つの部分からなる式の和で表現されています。

最初の$K_p$の部分は「比例ゲイン」と呼ばれます。この式によって、「制御対象がプラス方向にズレていればマイナス方向に力をかける」、「マイナス方向にズレていればプラス方向に力をかける」、「大きくズレていれば大きな力をかける」、「小さくズレていれば小さな力をかける」ことができます。$K_p$は人が設定するパラメータで、このパラメータで「力加減」を調整します。

前回セットアップしたロボット制御シミュレーションを動かしながらパラメータ調整の方法を学習しましょう。

まずは前回のブログを参考にVagrantの環境を立ち上げてください:

1
2
3

$ cd vagrant-robotics-boilderplate
$ vagrant up
$ vagrant ssh

弊社設定済みのVagrant環境を使うと高速なネットワーク環境下であればシミュレーション環境をすぐに立ち上げることができますが、hrpsys-baseインストール済みのLinux環境が既にある場合は、その環境をそのまま使うこともできます。その場合には、以下のコマンドでwatchdogを追加でインストールしてください：

1

$ sudo pip install -U watchdog

今回のブログ用のレポジトリは以下のURLで公開しています：

https://github.com/devrt/learn-control-pid

レポジトリをチェックアウトしてwatchdogを起動します：

1
2
3
4

(以下Vagrantまたは実Linuxマシンログイン後のコマンド)
$ git clone https://github.com/devrt/learn-control-pid.git
$ cd learn-control-pid
$ ./run.sh

「run.sh」を起動すると小さな窓が現れてプロジェクト内のファイルの監視が始まります。ファイルが変更されると自動でコンパイルとシミュレータの起動が行われます。

エディタを起動して「PA10Controller.cpp」ファイルを編集します。

PID制御とパラメータ定義が行われているのは以下の部分です：

1
2
3
4
5
6
7
8
9

#define K_P 100.0
#define K_D -100.0
#define MaxTau 400.0
  for (size_t i = 0; i < m_qRef.data.length(); i++){
    double u = K_P * (m_qRef.data[i] - m_angles.data[i]) + K_D * m_vels.data[i];
    u = std::min( MaxTau, u);
    u = std::max(-MaxTau, u);
    m_torque.data[i] = u;
  }

今は$K_p$が小さな値になっているため、以下の動画のようにロボットが脱力した状態です。パラメータ部分を編集して係数を少しずつ大きくしてみましょう。

力を加えすぎると制御が発散します。下は$K_p$を大きくしすぎた例：

下は複数回の調整をした結果です。これぐらいが調整の限界でしょうか：

比例ゲインは目標からのズレに対して加える力加減のパラメータですが、上の例のように最適まで調整しても振動が残ってしまいます。

ここで役に立つのが$K_d$です。$K_d$はズレの時間微分に対する係数で「微分ゲイン」と呼ばれます。

動画をよく観察するとロボットの腕が上下して停止した瞬間に大きな振動が発生しています。係数$K_p$は目標値からのズレに比例して加える力を定義するのですが、ロボットのような制御対象の場合、腕には重量があるため慣性が発生します。慣性があるとズレがゼロになって力を抜いても、その位置から行きすぎてしまい振動が発生するのです。

これを防ぐためには、ズレがゼロに近づきかけた状態で、早めに力を抜いてあげる必要があります。「微分ゲイン」でこのような早めのブレーキを実現することができます。

「PA10Controller.cpp」ファイルを編集して$K_d$を大きくしてみましょう。

微分ゲインを大きくすると、目標位置付近での姿勢維持が弱くなるので比例ゲインも合わせて大きくします。両者を調整すると以下のような安定した制御になります：

今回はさほど気になりませんでしたが、重いロボットを使う場合、$K_p, K_d$の両者を調整しても自身の重さに負けて腕の位置が低い状態で維持されてしまうことがあります。そのような時に使うのが「積分ゲイン$K_i$」で、この係数の調整によってズレた姿勢が長時間継続するのを防ぐことができます。

また、今回は全ての軸に対して同じPID係数を適用しましたが、調整済みの動画をよくよく見ると、腕の左右の細かい振動が発生してしまっています。大きな慣性がかかる上下方向の制御と、それよりは軽い左右方向の制御では最適なPID係数が異なるのです。今回は手抜きしましたが、本当はロボットの各軸に対してPID係数を調整してあげる必要があります。

最後にRTMを使った開発についても説明したいと思います。今回、腕の上下動を実現しましたが、上下の動きを生成する部分は以下のように実装されています：

1
2
3
4
5
6

  if (count > 1000)
    m_qRef.data[1] = 0.8;
  else
    m_qRef.data[1] = 0.0;
  count++;
  if (count > 2000) count = 0;

次回以降、複雑な動作を生成するシミュレーションを紹介していこうと思うのですが、今回作成したPID制御プログラム部分は再利用する予定です。その際、コピペしないで使いまわせるのが理想です。

RTMでは「コンポーネント」という単位でソフトウェアを分割して開発することができます。今回の環境では「PA10コントローラ」と「シミュレータ」の二つのコンポーネントがつながった状態で動作しています：

ここで、PA10コントローラの「上下動を実現する部分」と「PID制御を実現する部分」を分割して以下のような構成にすることを考えてみます：

このような構成にすると、他の動作であってもPID制御部分は使い回すことができます：

RTM（ROSもそうですが）はこのようなコンポーネント作成と接続・再接続を容易にするライブラリを提供してくれます。具体的な方法は次回以降説明しますが、今回使ったプログラム全体を読んでみるとポート定義部分から何となく想像できるかもしれません。

次回はロボットにより複雑な動作をさせるために必要な「逆運動学」の概念について説明するとともに、RTMによる開発についてもより深めていきたいと思います。