面倒な双一次変換の計算はPythonにやってもらおう

何がしたいの？

ロボットアームの先端を掴んで動かしてみたことはありますか？

イメージ的には重そうですよね。重いです。というか電源を入れてなかったらブレーキがなかったとしても動きません。（これをバックドライバビリティが低いと表現します。）

ですが、ロボットアームの各関節にはモーターが付いているので各モーターの出力を調整すれば、手先を任意の方向に自在に動かすことができます。イメージとしては人が力を加えた方向にロボットの手先を動かせばまるでロボットが軽い物体のように感じられるのです。

つまり、もし仮にロボットの先端を持った人に目隠しをすれば、重い物体、なんか動かすのに抵抗がある物体などを自在に体感させられるという意味です。（まあ現実にはレイテンシが存在するのでアレですが）

だから何？役に立つの？

それがどうした状態だと思うので一例をあげます。

手術支援ロボットというものをご存知でしょうか。
以下の写真は日本のとある会社が開発した手術支援ロボットです。医師が右側のコンソールの前に座り、左側のロボットの先端から出ている鉗子という器具を患者のお腹に入れます。医師が小さなアームの先端を動かすと、左側のロボットの先端が動きを何分の一の大きさにして動きを再現することによって、人間の手では難しい精密な動きを実現することができます。

ではお腹の中で鉗子が臓器に当たったとしたらどうしましょう。どのくらい硬い部位にどんな強さで当たっているのかを医師は知りたいでしょう。
つまり、我々はモーターにどのくらいの出力を与えたらどのくらい硬いものを触っているように感じるかを知りたいのです。

計算させてみよう

問題設定を簡単にするためにバネ・マス・ダンパ系で考えます。 現実世界は非線形ばかりですが、細かいことは気にしないのです。 バネとダンパがついた重りを人間が持っているとして、この重りに人間が F の力をかけると速度はどうなるか、を計算しその速度をロボットで再現することによって、そのロボットがまるで M の重さ、K の弾力、B の減衰性能を持った物体のように感じられるのです。

ここからが本題です。
上の写真の系についての運動方程式は、

$$M \ddot{x} + B \dot{x} + K x = f$$

ですね。ではこの物体を人間が握るものとして、アドミタンス提示型のインピーダンス制御をかけるましょう。つまり、アクチュエータを用いて任意の $M,B,K$ を表現したいのです。入力を$F(s)$、出力を$\dot{X}(s)$とすると、伝達関数$G$は

$$G = \frac{s}{Ms^2 + Bs + K}$$

という2型になります。(本当は剛性いらないので1型にできるが...)
では目標速度$\dot{x}$はどうやって計算しますか？ 僕は双一次変換を使います。今回の例の正解は、

$$\dot{x}[k] = \frac{1}{2BT_s+KT_s^2+4M} \{ 2T_s(f[k-2]+f[k]) + (8M-2KT_s^2)\dot{x}[k-1] \\ + (2BT_s-KT_s^2-4M)\dot{x}[k-2] \}$$

ですが、こんな面倒な計算を手でしたくはありません。そこでPythonにやってもらいましょう。ipynbファイルを以下のリポジトリに置きました。2型までですが、伝達関数を入力するだけで双一次変換を一瞬で計算してくれます。

SympyはSymbolicな演算をしてくれるだけでなく、簡単にTex表記もしてくれるので非常に便利ですね。

https://github.com/RE-yura/calculate_bilinear_transformation