【Gano剣】第11回 ROBO-剣(アーム型) 参加報告 – 制御編 – – 東京工業大学ロボット技術研究会公式ブログ

この記事はrogy Advent Calender 2020 15日目の記事です。

こんにちは。15の矢野です。

「第11回 ROBO-剣(アーム型)」の参加報告の最終記事です。

今日は機構編、画像処理編に続き制御編です。

私の担当は

ダミーロボットの位置データを基にロボットを動かす

部分でした。本記事では

カメラ・パソコン・モーターを含むシステムをどのような構成にしたか
軌道生成
モーターの制御

について解説していきます。

システム構成

まず使用したコンピューター（マイコンを含む）は

ノートパソコン (Ubuntu) ✕ 1
ESP32 (Arduino) ✕ 1

です。これら２つの役割は明確で

ESP32は関節の目標角度、角速度を受け取り、それに追従するようにモーターの制御を行う
ノートパソコンは各時刻で目標角度、角速度を計算しそれを送信する

となっています。下にシステムの構成図を示します。

ROS

上の図を見てわかるようにこのシステムではROSを使っています。今回ROSを使ってよかったと思ったことは

実機やカメラがなくても「実機を動かしているつもり」、「カメラからデータがきているつもり」で動作の確認ができる。
通信、ビジュアライズ、データロギングは既存のものを利用できる
ROS Serialを使うことでマイコンもROSのNodeとして扱える
ROS param を活用したマイコンへの書き込み回数を大幅に減らせた

といったところです。

さて画像処理編にある動画をROSの観点から見てみたいと思います。

右側に注目してもらいたいのですが、画像処理によって得られたダミーロボットの位置と、エンコーダーにより計測したGano剣（自分のロボット）の関節角データからこのようにビジュアライズすることができます。計測した数字だけ眺めていても人間にはいまいち状況がわからないのでビジュアライズするって大事ですね。

軌道生成

アームをどのように動かすかを考えます。本当は最適化とかしたかったんですが、時間がなかったので結局ポーズとポーズの間を線形補間するだけになりました。

ここでいうポーズとは各関節角により決まるものとします。

以下のように各ポーズのシーケンスで軌道を作ることを考えます。

中段構え
面振りかぶり
面
面振りかぶり
胴振りかぶり
胴
胴振りかぶり
中段構え

ポーズの更新

それぞれのポーズは予め基本のポーズを作っておき、動かす前にダミーロボットの位置を基に逆運動学を解いて更新しています。具体的には予め作成しておいた基本のポーズを初期値としてヤコビアンを用いた反復計算により逆運動学を解くので基本のポーズと近い解が求まります。

運動学の計算には RigidBodyDynamics.jl というjuliaのライブラリを用いています。 URDF形式のロボットモデルを読み込ませたら順運動学やヤコビアンを計算してくれるのでとても便利です。

速度計画

あるポーズから次のポーズへの動きを考えます。今回はとりあえず速度が連続になればいいかなという考えで簡単にできる台形加速を採用しました。パラメーターは到達にかかる時間、そのうち加速時間が占める割合、減速時間が占める割合の3つのみです。

台形加速の説明をするためのいい図がないかなと思ってググったら先輩の記事が1番にヒットしたので引用させていただきます。引用元：KERI's Lab

シミュレーション動画

ここまでの話を動画で確認してみます。この動画では「ダミーロボットの位置は取得できたつもり」ということでスライダーで位置を変えられるようにしてます。そしてその位置を基にポーズの更新をして軌道が生成できていることが確認できます。

あとはこの軌道を実機で再現できればいいということです！

モーターの制御

いよいよ実際に実機を動かしていきます。
エンコーダーて計測した関節角情報とPCから送られてきた目標角度を基にモーターに入力する電圧を決める部分です。

今回は以下のような制御速を採用しました。
PD制御 + 逆起電力補償（速度フィードフォワード） + 摩擦補償（摩擦一定）
$u = -K_p (q-q_d) -K_d (\dot{q}-\dot{q}_d) +K_{ff} v_d +K_{fr} sgn(v_d)$

$u$ は入力電圧
$encoding="application/x-tex">K_p,K_d,K_{ff},K_{fr}</annotation></semantics></math>$ はゲイン行列 (対角)
$q$ は関節角、 $encoding="application/x-tex">q_d</annotation></semantics></math>$ は目標関節角

特徴的なのは逆起電力補償（速度フィードフォワード）の部分です。
式の $encoding="application/x-tex">K_{ff} v_d</annotation></semantics></math>$ に該当します。

$encoding="application/x-tex">v_d</annotation></semantics></math>$ はモーターの目標角速度で、
今回の機体ではモーターの速度と関節の速度は同じにはなりません。
機構編で紹介があったようにワイヤー駆動による干渉があるからです。
—

実際に駆動したい関節より前の関節全てにワイヤーを中継しているため、動作の干渉という問題が生じます。
例えば下のGIF画像では一番根本のピッチ軸を手で動かしているのですが、その次の軸もそれにつられるように回転してしまっています。今回は全てのプーリー直径を等しくしたため、平行リンクと同様な動きです。

—
引用元：機構編

どういうことが起きているかというと、動かしたい関節の次の関節も同じだけ逆方向に回してしまうという状況です。
これが一番根本のyaw軸以外で起きています。

これを式で表すと、関節角速度 $encoding="application/x-tex">\dot{q} </annotation></semantics></math>$ とモーター角速度 $v$ の関係は以下のようになります。

$\dot{q} = B v$

$B = \begin{bmatrix} 1 & 0 & 0 & 0 & 0 \\ 0 & 1 & 0 & 0 & 0 \\ 0 & -1 & 1 & 0 & 0 \\ 0 & 0 & -1 & 1 & 0 \\ 0 & 0 & 0 & -1 & 1 \end{bmatrix}$

$q$ , $v$ は関節を根本の方から順に上から並べたベクトルです。

$B$ 行列が対角であれば干渉はないということですが、見てわかるように-1の要素があり、これが干渉を表しています。

今回制御では関節の目標角速度が与えられ、モーターの出すべき速度を求めたいので上の式の逆の関係を用います。
$v = B^{-1} \dot{q}$

$B^{-1} = \begin{bmatrix} 1 & 0 & 0 & 0 & 0 \\ 0 & 1 & 0 & 0 & 0 \\ 0 & 1 & 1 & 0 & 0 \\ 0 & 1 & 1 & 1 & 0 \\ 0 & 1 & 1 & 1 & 1 \end{bmatrix}$

これを見ると先端に行くほど前の関節の影響を受けてしまうというのがわかります。

以上がモーター制御の解説でした。動きは最後の動画をご覧ください。

計測

面白いことは特にないです。

計測部分はマグネットエンコーダーから1kHzでサンプリングし、10回の平均値フィルターをかけて角度、角速度を計算しています。マグネットエンコーダーが思ったよりノイジーだったのでこのようなフィルタリングが必要になりました。

あとこれは余談なのですが、SPIの線を一つのバスから5本も伸ばすのはよくないですね。4本まではちゃんと読めてたのに5本目つないだら通信おかしくなったとかいうのと格闘してました。今回一番苦労したのはここで、ケーブルにアルミホイル巻いたらいい感じに通信できるようになりました笑。差動通信とシールド線のありがたみを改めて実感しました。SPIをフラットケーブルで伸ばすのはやめましょう。

試合の様子

お待たせしました。

決勝戦の第一試合の様子をご覧ください。

これで制御は完璧！とまではいきませんでしたが、まあまあの速さでまあまあの追従制度を出すことができました。

せっかくワイヤー駆動の軽いアームを作ってもらったので本当はもっと速く動かしたかったのですが、それは今後の課題としたい思います。

また大会全体の様子はTwitchで配信されていたので、興味ある方はこちらからご覧ください。

以上で３日間に渡る【Gano剣】第11回 ROBO-剣(アーム型) 参加報告　を締めくくりたいと思います。

最後まで読んでいただきありがとうございます。

協賛企業