このノートについて

ロボットアームの順運動学と逆運動学を手を動かしながら学習しましょう。三角関数を使った順運動学の立式からニュートン法を使った関節角推定までを解説します。

Written by Yosuke Matsusaka (MID Academic Promotions, Inc. @yosuke)

このノートはCreative CommonsのBY-SAライセンスで公開します。CC BY-SA

準備

このノートはipython notebookを使った手を動かしながら学べる教材になっています。Ubuntu上では

sudo apt-get install imagemagick asymptote graphviz
sudo pip3 install jupyter matplotlib sympy graphviz
jupyter notebook

と入力することで環境を立ち上げることができます。

ipython notebookを立ち上げて新規ノートを作成したら、まずグラフ描画と数式処理のライブラリを読み込みます。

In [1]:
import matplotlib.pyplot as plt
import matplotlib.animation as animation
from IPython.display import Image
from sympy import *
init_printing()

以下も入力してください。この部分はアニメーションGIFを表示するためのおまじないです。

In [2]:
# Part of code is taken from: http://nbviewer.ipython.org/url/jakevdp.github.io/downloads/notebooks/AnimationEmbedding.ipynb
from tempfile import NamedTemporaryFile
import base64

GIF_TAG = """<img src="data:image/gif;bogus=ABCDEF;base64,{0}">"""

def anim_to_html(anim):
    if not hasattr(anim, '_encoded_video'):
        with NamedTemporaryFile(suffix='.gif') as f:
            anim.save(f.name, writer='imagemagick', fps=4)
            video = open(f.name, "rb").read()
        anim._encoded_video = base64.b64encode(video).decode('utf-8')
    return GIF_TAG.format(anim._encoded_video)

animation.Animation._repr_html_ = anim_to_html

グラフ領域を初期化します。このグラフ上でロボットアームの動作を確かめていきます。

In [3]:
# Part of code is taken from: http://studywolf.wordpress.com/2013/09/20/python-visualization-with-matplotlib/
fig = plt.figure(figsize=(4,4))
ax = fig.add_subplot(111, aspect='equal', autoscale_on=False,
                     xlim=(-1, 1), ylim=(-1, 1))
ax.grid()
line, = ax.plot([], [], 'o-', lw=4, mew=5)
infoline = ax.text(0.02, 0.95, '', transform=ax.transAxes)

def init():
    line.set_data([], [])
    infoline.set_text('')
    return line, infoline

以下は説明のための作図ライブラリです(インストールは任意です)。

In [4]:
import graphviz
In [5]:
import subprocess # to run asymptote
import shlex

では、はじめましょう。

ロボットアームの順運動学

「順運動学」とはロボットアームの各関節角が与えられたときに手先の位置を求める作業のことです。

In [6]:
graphviz.Source('digraph G { rankdir=LR; 関節角 -> 手先の位置 [label="求める"] }')
Out[6]:
G 関節角 関節角 手先の位置 手先の位置 関節角->手先の位置 求める

「学」の字が入っているので学問を連想するのですが、そのような大げさなものではありません。実際は以下のような作業です。

順運動学を求めるにはロボットアームの構造を知る必要があります。今回シミュレートするロボットアームは三菱重工製のPA10-7Cです。技術情報が以下のページで公開されています。

https://www.mhi.co.jp/products/detail/pa10_6c_7c.html

ロボットアームの各関節の配置と関節間の距離を以下から知ることができます。

https://www.mhi.co.jp/products/detail/images/7c_mass.jpg

PA10は本来7自由度(7関節)あるロボットアームですが、ここでは大幅に簡略化して上下方向の2自由度(2関節)のみを考慮します。

三菱重工の技術情報から、PA10は下図のような根元からたどって第1関節と第2関節の間が450mm、第2関節と手先の間が480mmある構造をしています。この情報を使って第1関節と第2関節の角度が与えられた時の手先の位置を求めてみましょう。

In [7]:
f=open('/tmp/pa10-simplified.asy', 'w')
f.write('''
import math; import graph; import geometry;
size(12cm); xaxis("$x$"); yaxis("$y$");
real theta1=pi/4; real theta2=pi/3;
real L1=0.45; real L2=0.48;
pair l1p = (L1*sin(theta1), L1*cos(theta1));
pair l2p = l1p + (L2*sin(theta1+theta2), L2*cos(theta1+theta2));
draw((0,0)--l1p--l2p); draw(l1p--l1p*1.5, dashed); dot(l2p);
label("$J1(0,0)$", (0,0), NW); label("$J2(u,v)$", l1p, NW); label("$H(x,y)$", l2p, NE);
draw("$L1=450mm$",(0,0)-0.02*I*l1p--l1p-0.02*I*l1p, red, Arrows, Bars, PenMargins);
draw("$L2=480mm$", l1p-0.02*I*l2p--l2p-0.02*I*l2p, red, Arrows, Bars, PenMargins);
draw("$\theta_1$", arc((0,1),(0,0),l1p,0.1), blue, PenMargins);
draw("$\theta_2$", arc(l1p,0.1,degrees(l1p),degrees(l2p-l1p),CW), blue, PenMargins);
''')
f.close()
subprocess.call(shlex.split('asy /tmp/pa10-simplified.asy -f png -o pa10-simplified.png'))
Out[7]:
$$0$$

PA10 simplified

はじめに各関節の角度を変数として定義します。

In [8]:
t1 = Symbol('theta_1')
t2 = Symbol('theta_2')

各関節間の距離を定数として定義します。

In [9]:
L1 = Symbol('L_1')
L2 = Symbol('L_2')

手先の位置を求める前に、まずは第2関節の位置を求めます。第2関節の位置は三角関数を使って以下のように求まるでしょう。

In [10]:
u = L1 * sin(t1)
v = L1 * cos(t1)
[u, v]
Out[10]:
$$\left [ L_{1} \sin{\left (\theta_{1} \right )}, \quad L_{1} \cos{\left (\theta_{1} \right )}\right ]$$

次に手先の位置を求めます。第2関節の位置に手先までの距離を加算します。

In [11]:
x = u + L2 * sin(t1+t2)
y = v + L2 * cos(t1+t2)
[x, y]
Out[11]:
$$\left [ L_{1} \sin{\left (\theta_{1} \right )} + L_{2} \sin{\left (\theta_{1} + \theta_{2} \right )}, \quad L_{1} \cos{\left (\theta_{1} \right )} + L_{2} \cos{\left (\theta_{1} + \theta_{2} \right )}\right ]$$

これで手先の位置が求まったはずですが、念の為、グラフで描いて確認します。

In [12]:
import math

subs = {
    L1: 450.0 / 1000.0,
    L2: 480.0 / 1000.0,
    t1: 0.0,
    t2: 0.0
}

def animate(i):
    if i < 10:
        subs[t1] = subs[t1] + math.pi / 30.0
    elif i < 20:
        subs[t2] = subs[t2] + math.pi / 30.0
    else:
        subs[t1] = subs[t1] - math.pi / 30.0
        subs[t2] = subs[t2] - math.pi / 30.0
    line.set_data([0, u.evalf(subs=subs), x.evalf(subs=subs)],
                  [0, v.evalf(subs=subs), y.evalf(subs=subs)])
    infoline.set_text('t1 = %.2f, t2 = %.2f' % (subs[t1], subs[t2]))
    return line, infoline

animation.FuncAnimation(fig, animate, frames=30,
                        interval=25, blit=True, 
                        init_func=init)
Out[12]:

問題無いようです。順運動学が無事に求まりました。

ロボットアームの逆運動学

逆運動学とは順運動学の逆に「手先の位置から各関節の角度を求める」作業です。

In [13]:
graphviz.Source('digraph G { rankdir=LR; 手先の位置 -> 関節角 [label="求める"] }')
Out[13]:
G 手先の位置 手先の位置 関節角 関節角 手先の位置->関節角 求める

ロボットアームを自由に操ってピックアンドプレースのような動作をさせるには逆運動学を求める必要があります。順運動学より多少難しくなりますが、「学」の字から想像されるほど難しいものではありません。

逆運動学を求めるには、解析的に求める方法と数値的に求める方法の2つがあります。

解析的に求める方法

順運動学はすでに求まっているので、その逆を求める、数学的に言うと逆関数を求める、ということになります。数学的なセンスでこのような逆関数を求めるのが解析的な方法です。

順運動学は最終的に以下の2式になりました。

In [14]:
[x, y]
Out[14]:
$$\left [ L_{1} \sin{\left (\theta_{1} \right )} + L_{2} \sin{\left (\theta_{1} + \theta_{2} \right )}, \quad L_{1} \cos{\left (\theta_{1} \right )} + L_{2} \cos{\left (\theta_{1} + \theta_{2} \right )}\right ]$$

解析的な方法では、式をパズルのように組み合わせ、試行錯誤しながら変換していきます。

三角関数の変換では式を2乗して足し合わせると整理できることがあることが知られています。

In [15]:
i = expand(x**2 + y**2)
i
Out[15]:
$$L_{1}^{2} \sin^{2}{\left (\theta_{1} \right )} + L_{1}^{2} \cos^{2}{\left (\theta_{1} \right )} + 2 L_{1} L_{2} \sin{\left (\theta_{1} \right )} \sin{\left (\theta_{1} + \theta_{2} \right )} + 2 L_{1} L_{2} \cos{\left (\theta_{1} \right )} \cos{\left (\theta_{1} + \theta_{2} \right )} + L_{2}^{2} \sin^{2}{\left (\theta_{1} + \theta_{2} \right )} + L_{2}^{2} \cos^{2}{\left (\theta_{1} + \theta_{2} \right )}$$

この式はまだ複雑に見えますが、以下に示す三角関数の各定理を用いると大幅に単純化することができます。

ピタゴラスの定理: $ cos^2 \theta + sin^2 \theta = 1 $

加法定理: $ cos(\alpha - \beta) = cos(\alpha) cos(\beta) + sin(\alpha) sin(\beta) $

これを踏まえつつ、ここではsympyに自動で整理してもらいます(手抜きです)。

In [16]:
j = simplify(i)
j
Out[16]:
$$L_{1}^{2} + 2 L_{1} L_{2} \cos{\left (\theta_{2} \right )} + L_{2}^{2}$$

整理すると$\theta_1$が消えてしまいます(いつもこのようなことが起きるわけでなくラッキーなパターンです)。

これだけ単純な式まで変換できると逆関数の計算は簡単です。$\theta_2$は$cos$の逆関数$acos$を使って以下のように解けます。解が2つ出てきますが、これについては後で解説します。

In [17]:
from sympy.solvers import solve
x2 = Symbol('x')
y2 = Symbol('y')
j2 = (x2**2 + y2**2)
k = solve(j2 - j, t2)
k
Out[17]:
$$\left [ - \operatorname{acos}{\left (- \frac{1}{2 L_{1} L_{2}} \left(L_{1}^{2} + L_{2}^{2} - x^{2} - y^{2}\right) \right )} + 2 \pi, \quad \operatorname{acos}{\left (\frac{1}{2 L_{1} L_{2}} \left(- L_{1}^{2} - L_{2}^{2} + x^{2} + y^{2}\right) \right )}\right ]$$

余談: sympyのsolveは高機能な求解アルゴリズムを提供してくれます。これを使えば逆運動学を直接解けるのでは?とも思ってしまうのですが、元の整理する前の順運動学の式を与えても自動で解くことはできません。sympyが低機能なわけでなく逆運動学に必要な変換がかなり複雑なのです。このノート末尾の「おまけ」の中で言及しているIKFastのような、式を解きやすい形で立式して自動で求解してくれる強力なライブラリも近年開発されています。

$\theta_2$が求まったので次に$\theta_1$を求めましょう。今回は、余弦定理を使って解いてみます。

まずは第1関節から手先まで補助線を引き、補助線と$x$軸の成す角を$\alpha$とします。

In [18]:
f=open('/tmp/pa10-firstjoint.asy', 'w')
f.write('''
import math; import graph; import geometry;
size(12cm); xaxis("$x$"); yaxis("$y$");
real theta1=pi/4; real theta2=pi/3;
real L1=0.45; real L2=0.48;
pair l1p = (L1*sin(theta1), L1*cos(theta1));
pair l2p = l1p + (L2*sin(theta1+theta2), L2*cos(theta1+theta2));
draw((0,0)--l1p--l2p); draw(l1p--l1p*1.5, dashed); dot(l2p); draw((0,0)--l2p, dashed); 
label("$J1(0,0)$", (0,0), NW); label("$J2(u,v)$", l1p, NW); label("$H(x,y)$", l2p, NE);
draw("$L1$",(0,0)-0.02*I*l1p--l1p-0.02*I*l1p, red, Arrows, Bars, PenMargins);
draw("$L2$", l1p-0.02*I*l2p--l2p-0.02*I*l2p, red, Arrows, Bars, PenMargins);
draw("$\\theta_1$", arc((0,1),(0,0),l1p,0.1), blue, PenMargins);
draw("$\\theta_2$", arc(l1p,0.1,degrees(l1p),degrees(l2p-l1p),CW), blue, PenMargins);
draw("$\\alpha$", arc((1,0),(0,0),l2p,0.2), blue, PenMargins);
''')
f.close()
subprocess.call(shlex.split('asy /tmp/pa10-firstjoint.asy -f png -o pa10-firstjoint.png'))
Out[18]:
$$0$$

PA10 fist joint

$\alpha$は$atan$を使って以下のように求まります。

In [19]:
alpha = atan(y2/x2)
alpha
Out[19]:
$$\operatorname{atan}{\left (\frac{y}{x} \right )}$$

ここでさらに

In [20]:
beta = pi / 2 - t1 - alpha
beta
Out[20]:
$$- \theta_{1} - \operatorname{atan}{\left (\frac{y}{x} \right )} + \frac{\pi}{2}$$

なる角$\beta$を仮定すると、第2関節を中心とした以下の様な三角形に書き下すことができます。

In [21]:
f=open('/tmp/pa10-secondjoint.asy', 'w')
f.write('''
import math; import graph; import geometry;
size(12cm); xaxis("$x$"); yaxis("$y$");
real theta1=pi*0.327; real theta2=pi/3;
real L1=0.45; real L2=0.48;
pair l1p = (L1*sin(theta1), L1*cos(theta1));
pair l2p = l1p + (L2*sin(theta1+theta2), L2*cos(theta1+theta2));
draw((0,0)--l1p--l2p); draw(l1p--l1p*1.5, dashed);
label("$J1(0,0)$", (0,0), NW); label("$J2(u,v)$", l1p, NW); label("$H(x,y)$", l2p, NE);
draw("$L1$",(0,0)-0.02*I*l1p--l1p-0.02*I*l1p, red, Arrows, Bars, PenMargins);
draw("$L2$", l1p-0.02*I*l2p--l2p-0.02*I*l2p, red, Arrows, Bars, PenMargins);
draw("$\\theta_2$", arc(l1p,0.1,degrees(l1p),degrees(l2p-l1p),CW), blue, PenMargins);
draw("$\\beta$", arc((1,0),(0,0),l1p,0.15), blue, PenMargins);
''')
f.close()
subprocess.call(shlex.split('asy /tmp/pa10-secondjoint.asy -f png -o pa10-secondjoint.png'))
Out[21]:
$$0$$

PA10 second joint

余弦定理(を導出する前の三角形の分解と同様の考え方)から$\beta$と$\theta_2$の間には以下の関係が成り立つはずです。

In [22]:
beta2 = atan((L2 * sin(t2))/(L1 + L2 * cos(t2)))
beta2
Out[22]:
$$\operatorname{atan}{\left (\frac{L_{2} \sin{\left (\theta_{2} \right )}}{L_{1} + L_{2} \cos{\left (\theta_{2} \right )}} \right )}$$

上記の式を$\beta$の式と組み合わせると$\theta_1$は以下のようにして計算できます。

In [23]:
k2 = solve(beta2 - beta, t1)
k2
Out[23]:
$$\left [ - \operatorname{atan}{\left (\frac{y}{x} \right )} - \operatorname{atan}{\left (\frac{L_{2} \sin{\left (\theta_{2} \right )}}{L_{1} + L_{2} \cos{\left (\theta_{2} \right )}} \right )} + \frac{\pi}{2}\right ]$$

これで$\theta_1$と$\theta_2$の両者が求まったはずですが、グラフで動きを確認します。試しに三角形の軌跡を描かせてみましょう。

In [24]:
subs = {
    L1: 450.0 / 1000.0,
    L2: 480.0 / 1000.0,
    x2: 0.2,
    y2: 0.2
}

subs2 = {
    L1: 450.0 / 1000.0,
    L2: 480.0 / 1000.0,
    t1: 0.0,
    t2: 0.0
}

def animate(i):
    if i < 10:
        subs[x2] = subs[x2] + 0.04
    elif i < 20:
        subs[y2] = subs[y2] + 0.04
    else:
        subs[x2] = subs[x2] - 0.04
        subs[y2] = subs[y2] - 0.04
    subs2[t2] = subs[t2] = k[0].evalf(subs=subs)
    subs2[t1] = k2[0].evalf(subs=subs)
    line.set_data([0, u.evalf(subs=subs2), x.evalf(subs=subs2)],
                  [0, v.evalf(subs=subs2), y.evalf(subs=subs2)])
    infoline.set_text('x = %.2f, y = %.2f' % (subs[x2], subs[y2]))
    return line, infoline

animation.FuncAnimation(fig, animate, frames=30,
                        interval=25, blit=True, 
                        init_func=init)
Out[24]:

指定した三角形の軌跡を手先で正しく描いてくれてはいるのですが、PA10の実物を想像すると、このままでは肘が地面にぶつかってしまいます。先ほどの計算で$\theta_2$には2つの解があったと思うのですが、2番目の解を使ってグラフを描いてみましょう。

In [25]:
subs = {
    L1: 450.0 / 1000.0,
    L2: 480.0 / 1000.0,
    x2: 0.2,
    y2: 0.2
}

subs2 = {
    L1: 450.0 / 1000.0,
    L2: 480.0 / 1000.0,
    t1: 0.0,
    t2: 0.0
}

def animate(i):
    if i < 10:
        subs[x2] = subs[x2] + 0.04
    elif i < 20:
        subs[y2] = subs[y2] + 0.04
    else:
        subs[x2] = subs[x2] - 0.04
        subs[y2] = subs[y2] - 0.04
    subs2[t2] = subs[t2] = k[1].evalf(subs=subs)
    subs2[t1] = k2[0].evalf(subs=subs)
    line.set_data([0, u.evalf(subs=subs2), x.evalf(subs=subs2)],
                  [0, v.evalf(subs=subs2), y.evalf(subs=subs2)])
    infoline.set_text('x = %.2f, y = %.2f' % (subs[x2], subs[y2]))
    return line, infoline

animation.FuncAnimation(fig, animate, frames=30,
                        interval=25, blit=True, 
                        init_func=init)
Out[25]:

これで意図した動作になったのではないかと思います。先ほど2つの解が出てきた意味もこれでわかったと思うのですが、冗長な自由度があるロボットは同じ手先座標にしようと思っても肘の位置により複数の解が求まり、数学的にはそのどちらも正解です。とはいえ実際には意図した肘の位置もあるので、どちらか解を選択する必要があるのです。

余談: ロボットの「肘の位置」は大きな問題となることがあります。今回は地面が障害になりましたが、狭い空間にロボットアームを伸ばして作業する場合、手先に気を使うと同時に肘も障害物に当たらないようにしなければなりません。今回は2自由度のロボットを考えましたが、同じ上下の軸に3以上のさらに冗長な自由度を持ったロボットを設計することも可能です。このようなロボットは同じ手先座標でも肘の位置をかなり自由に選択することが可能です。計算がめんどうくさくなる、という一方で、狭い空間でも肘の位置をコントロールして障害物を避けながら進むことができるため面白いロボットを作ることができます。

余談2: とは言っても、冗長な自由度を持ったロボットは計算が大変なだけでなく、長尺の構造材の歪や各モータ部分でガタ(バックラッシュ)が発生し誤差として蓄積するため、手先座標の精度の確保が難しくなります。実用上は、自由度が高いと同時に精度を出しやすい設計のロボットが多用されます。「スカラ型ロボット」はそのようなロボットの一つで、xy軸は今回計算したものと同じ2自由度の構造をしているのですが、手先近くにz軸方向の直動関節があります。機械的な剛性が確保しやすく、計算し易さと精度の高さを両立できる優れた設計のロボットです。

数値的に求める方法

「解析的な方法」では数学的に逆関数を求めていきましたが、今回のような大幅に簡略化したロボットであってもかなり骨が折れる作業だったと思います。より多くの関節を持った複雑なロボットの関節角を計算する場合は更に大変な作業になります。

解析的な方法では関節角を逆関数として求めようとしましたが、逆関数を用いず、コンピュータを使った繰り返し計算によって関節角を求めることもできます。そのような方法を「数値的な方法」と呼びます。ここではニュートン法を使った例を紹介します。

ニュートン法では以下の考え方で解に近づいていきます。

まずは、関節角に適当な初期値を与えます。「適当」は案外難しいのですが、ここではサイコロを振って決めたランダムな値と考えてください。

$$ \theta_1 = 初期値 $$

関節の初期値から順運動学の式を使って手先座標を求めます。

$$ r_1 = f(\theta_1) $$

ランダムに決めた初期値がたまたま正解だった場合を除いて、この手先座標は目標座標とは異なっているはずです。

目標座標と現在の手先座標との差分を計算します。この差分値は、手先座標のxy軸の空間の中で現在の座標と目標座標がどちらの方向にどれだけ離れているかという指標になります。

$$ r - r_1 $$

手先座標のxy空間での指標が得られたとは言っても、今回は関節角の調整でその指標に近づいていかなければなりません。ニュートン法ではここで関数の微分を使います。順運動学の微分の式に対して現在の関節角の値を代入すると「関節角をどれだけ動かすと手先座標のxy軸の空間で手先がどれだけ動くか」という傾きが定数として得られます。

$$ A_1 = f'(\theta_1) $$

元の順運動学の式が三角関数なので本来この傾きは関節角に応じて複雑な動きをするはずです。なので、ここで得られた定数は「現在の関節角の付近ではこの傾き」という暫定的な値を切り出したものでしかありません。ですが近似としては十分使えます。

上記の傾きを手先座標のxy空間と関節角をつなぐ「のり」のように使います。

式の左辺を手先座標のxy空間、右辺を関節角の空間と考えて、のりでつなぎ合わせます。ここで$r$と$\theta$はそれぞれ手先座標と関節の目標値です。

$$ r - r_1 = A_1 (\theta - \theta_1) $$

厳密にはこのような等式は成立するわけではなく、今この瞬間に限っては近似的にこのような関係が成り立つと考えよう、と信じるわけです。

上記の式を$\theta$に関して解くと以下のようになります。

$$ \theta = \theta_1 + A_1^{-1} (r - r_1) $$

$r_1$と$A_1$は順運動学から求めたので置き換えると、目標座標$r$と関節角の初期値$\theta_1$から正しい関節角$\theta$を求める以下の式になります。

$$ \theta = \theta_1 + f'(\theta_1)^{-1} (r - f(\theta_1)) $$

$f'(\theta_1)$の値に関してはあくまで$\theta_1$付近での近似にすぎないので、実際は以下のようにして何度も繰り返し計算して精度を高めます。

$$ \theta_{i+1} = \theta_i + k f'(\theta_i)^{-1} (r - f(\theta_i)) $$

ここで$k$は$A=f'(\theta)$が近似としてあまり信用できない場合に使う手加減の係数です(今回もその信用できない場合にあたります)。この係数を小さめにすることで、小刻みに解に近づこうとするため計算量は増えますが収束がより確実になります。

ニュートン法では、関数の微分のみを使って解を求められるのがポイントです。複雑な変換が必要な逆関数とは異なり微分は簡単に導出することができます。

In [26]:
xdt1 = diff(x, t1)
xdt2 = diff(x, t2)
ydt1 = diff(y, t1)
ydt2 = diff(y, t2)
j = [[xdt1, ydt1], [xdt2, ydt2]]
j
Out[26]:
$$\left [ \left [ L_{1} \cos{\left (\theta_{1} \right )} + L_{2} \cos{\left (\theta_{1} + \theta_{2} \right )}, \quad - L_{1} \sin{\left (\theta_{1} \right )} - L_{2} \sin{\left (\theta_{1} + \theta_{2} \right )}\right ], \quad \left [ L_{2} \cos{\left (\theta_{1} + \theta_{2} \right )}, \quad - L_{2} \sin{\left (\theta_{1} + \theta_{2} \right )}\right ]\right ]$$

適当な初期値から$A$と$A^{-1}$を求めてみましょう。

In [27]:
subs = {
    L1: 450.0 / 1000.0,
    L2: 480.0 / 1000.0,
    t1: 0.1,
    t2: 0.1
}

A = Matrix([[xdt1.evalf(subs=subs), ydt1.evalf(subs=subs)], [xdt2.evalf(subs=subs), ydt2.evalf(subs=subs)]])
A
Out[27]:
$$\left[\begin{matrix}0.918183831738908 & -0.140286316272702\\0.470431957363796 & -0.0953612787816294\end{matrix}\right]$$

$A^{-1}$は$A$の逆関数ですが、$A$は定数なので簡単に求まります。$A$が1次元であれば

$$A^{-1} = \frac{1}{A}$$

ですが、ここでは式の入出力がそれぞれ$r = (x, y)$、$\theta = (\theta_1, \theta_2)$の2次元であるため、$A$も2行2列の行列になります。2行2列の行列の逆関数(逆行列)は以下のように計算できます。

$$A^{-1} = \begin{pmatrix} a & b \\ c & d \end{pmatrix}^{-1} = \frac{1}{ad-bc}\begin{pmatrix} d & -b \\ -c & a \end{pmatrix}$$

一見複雑な式に見えますが、各行列要素の掛け算と割り算だけなので一瞬で計算できます。

In [28]:
AI = A.pinv()
AI
Out[28]:
$$\left[\begin{matrix}4.42224073456897 & -6.50557406790228\\21.8155984407579 & -42.5794409892144\end{matrix}\right]$$

ここでは逆行列の計算に通常のinv()ではなく疑似逆行列を計算するpinv()を用いています。今回は深く説明しませんが、擬似逆行列を使うことでロボットの手が伸びきった状態などでの計算の安定性が向上します。

繰り返し計算で解を求めるプログラムを書いてみましょう。

In [29]:
subs = {
    L1: 450.0 / 1000.0,
    L2: 480.0 / 1000.0,
    t1: 0.0,
    t2: 0.0
}

def newton(t1_initial, t2_initial, target_x, target_y, times):
    subs[t1] = t1_initial
    subs[t2] = t2_initial
    for i in range(0, times):
        A = Matrix([[xdt1.evalf(subs=subs), ydt1.evalf(subs=subs)], [xdt2.evalf(subs=subs), ydt2.evalf(subs=subs)]])
        AI = A.pinv()
        t1_next = subs[t1] + 0.1 * (AI[0,0]*(target_x - x.evalf(subs=subs)) + AI[0,1]*(target_y - y.evalf(subs=subs)))
        t2_next = subs[t2] + 0.1 * (AI[1,0]*(target_x - x.evalf(subs=subs)) + AI[1,1]*(target_y - y.evalf(subs=subs)))
        subs[t1] = t1_next
        subs[t2] = t2_next
    return [subs[t1], subs[t2]]

グラフを描いて確認しましょう。

In [30]:
subs2 = {
    L1: 450.0 / 1000.0,
    L2: 480.0 / 1000.0,
    t1: -0.5,
    t2: 3.0
}

target_x = 0.2
target_y = 0.2

def animate(i):
    global target_x, target_y
    if i < 10:
        target_x = target_x + 0.04
    elif i < 20:
        target_y = target_y + 0.04
    else:
        target_x = target_x - 0.04
        target_y = target_y - 0.04
    [subs2[t1], subs2[t2]] = newton(subs2[t1], subs2[t2], target_x, target_y, 10)
    line.set_data([0, u.evalf(subs=subs2), x.evalf(subs=subs2)],
                  [0, v.evalf(subs=subs2), y.evalf(subs=subs2)])
    infoline.set_text('x = %.2f, y = %.2f' % (target_x, target_y))
    return line, infoline

animation.FuncAnimation(fig, animate, frames=30,
                        interval=25, blit=True, 
                        init_func=init)
Out[30]:

解を正しく推定できているようです。

数値的な方法を使うことで逆関数を導出することなく逆運動学を計算することができました。

ここで利用したニュートン法は、逆関数を導出することが難しい複雑な関数であっても近似的にパラメータを推定することができる非常に強力なアルゴリズムで、ロボットの制御にとどまらず様々な最適化計算に用いられています。 その一方で収束性が保証されていなかったり、初期値をうまく取らないと解が発散するという使用上の注意点もありますので、それらの特性を理解した上でうまく利用する必要があります。

数値的な方法は利用上の注意点がいくつかあるとはいえ、解析解を得ることが難しい複雑な構造のロボットであっても多少の繰り返し計算のみで容易に逆運動学を導出できます。近年のコンピュータの高速化は著しく、小型の組込みプロセッサでも逆運動学の計算が実用的な速度で動いてしまうため、数値的な方法は高度なロボットシステムで多く用いられています。

ロボットへの実装

Pythonによるプログラミングかと思いきや実際は数式の変換の連続で嫌になってきたころだと思いますが、それもここまでです。

いよいよ上記の逆運動学をC++でプログラミングしてロボットを動かしてみます。

前回の記事でPA10ロボット用のPID制御コントローラを実装して調整しました。また、RTMを使うとシステムを「コンポーネント」という部品に分割して開発できることを知りました。今回は前回開発したPID制御コントローラを組み替えることで以下の設計のシステムを組んでみます。