6軸ロボットの順運動学・逆運動学のPythonコード例

6軸ロボットのサンプルアニメーション TECH徒然

はじめに

6軸ロボットの順運動学・逆運動学をPythonで実装してみます.前回までの3D回転の話のついでに,ロボティクスの幾何学計算を少し.

ロボットの幾何モデルとその計算

ロボットの幾何モデル

6軸ロボットは6個の関節で7個のアームを繋いだ構造をしています.これを,下記のように隣接アーム間の相対的な位置姿勢 $H_{j-1,j}$ と関節の回転角 $\theta_j$ で表すことにします.

  • アーム[j-1]とアーム[j]は,アーム[j]のZ軸を回転軸とする関節[j] (回転角 $\theta_j$)で連結.
  • $\theta_j = 0$ のとき,アーム[j-1] に対するアーム[j]の位置姿勢は $H_{j-1,j}$.

アーム[-1] はロボットを設置する”ワールド座標系”,アーム[6] はロボット先端に取り付ける”ツール”のイメージです.なお修正DH記法の扱いはコードのほうで触れます.

順運動学(Forward Kinematics) の計算

順運動学は,関節の回転角 $\theta = \left[ \theta_0, \cdots, \theta_5 \right]^T \in \mathbb{R}^6$ からツールの位置姿勢 $H = H(\theta)$ の計算です.$H$ はいわゆる同次変換行列(Homogeneous Transformation Matrix)で表すことにします.すなわち,位置 $t \in \mathbb{R}^3$,姿勢 $R \in \mathrm{SO}(3)$ なら $H ~:=~ \left[ \begin{array}{c|c} R & t \\ \hline 0 & 1 \end{array} \right]$.

アームや関節に沿った平行移動・回転の繰返しは同次変換行列の積に対応することから,前節のモデルの順運動学 $H = H(\theta)$ は次式となります.$R_z (\theta_j)$ はZ軸まわり $\theta_j$ 回転(平行移動なし)に相当する同次変換行列.

\[ H(\theta) ~=~ H_{-1,0} \cdot R_z(\theta_0) \cdots H_{4,5} \cdot R_z(\theta_5) \cdot H_{5,6} \]

逆運動学 (Inverse Kinematics) の計算


逆運動学は順運動学の逆 $H \mapsto \theta$ の計算です.条件が良ければ局所的に解が一意に定まり,Newton法などで数値的に解けます.解析的に解ける場合もありますが今回は数値解でいきます.

一般には逆運動学に解の存在も一意性もありません.

  • リーチの外では解なし.
  • リーチの中では典型的には4解.位置決めに上手と下手の2解,姿勢決めにフォアとバックの2解.背面エビ反りの解も数えれば更に倍.
  • 特異点 (ヤコビアン $J(\theta)$ が full rank でない $\theta \in \mathbb{R}^6$,たとえば2軸が揃うポーズ) の近傍では,解が連なったり($\mathrm{Ker} (J)$ 方向),なくなったり($\mathrm{Im} (J)$ から外れる方向)します.

サンプルプログラム

ライブラリ部分

前半はロボットの運動学を計算する Robot クラスのコードです.

  • __init__() はロボット形状の指定です.関節角度がゼロのときの隣接アーム間の相対的な同次変換行列を与えます.(6-10行目)
  • FK() は順運動学 $\theta \mapsto H$ です.前出の式の実装.(12-18行目)
  • IK() は逆運動学 $H \mapsto \theta$ です.ある種のNewton法.(20-30行目)
  • J()FK() の実質的なヤコビアンです.$dH/d\theta \cdot H^{-1}$ の $6 \times 6$ 行列表示.(32-41行目)
  • 変数は,th[6]: 関節の回転角[rad], H[4,4]: 同次変換行列,などです.
"""6軸ロボットマニピュレータの順運動学&逆運動学の計算コード例"""

import numpy as np

class Robot:
    def __init__(self, homs):
        """ロボット初期化 - 隣接アームの相対的な位置姿勢(同次変換行列)で規定"""
        assert len(homs)==6+1  # 6関節7アーム
        for H in homs: assert is_homtrsf(H)  # 同次変換行列
        self.homs = [np.array(hom).copy() for hom in homs]

    def FK(self, th, k=6):
        """順運動学 - 関節角θ-->アーム[k]の位置姿勢H"""
        H = np.eye(4)
        for j in range(k+1):
            if j==6: H = H @ self.homs[j]  # ツールは固定
            else   : H = H @ self.homs[j] @ _Rz(th[j])  # 関節[j-1]まで*アーム[j]*関節[j]
        return H

    def IK(self, H1, th0, tol=1.e-8, maxiter=100):
        """逆運動学 - 目標位置姿勢H1, 初期近似解θ0, 許容誤差, 反復回数-->関節角度θ"""
        th = th0.copy()
        for _ in range(maxiter):
            dH = _mat2vec(self.FK(th) @ inv_homtrsf(H1))  # 残差 H・H1^-1 の6Dベクトル表現 ΔH
            dth = np.linalg.solve(self.J(th), dH)  # Newton法 Δθ=J^-1・ΔH
            th -= dth
            print('# DEBUG: IK(): iter=%d: |dth|=%.4e |dH|=%.4e' % (_, np.linalg.norm(dth), np.linalg.norm(dH)))
            if np.linalg.norm(dth) < tol: return th
        print('WARNING: IK(): maxiter=%s reached, |dH|=%.4e.' % (maxiter, np.linalg.norm(dH)), flush=True)
        return th0  # 未到達

    def J(self, th, k=6):
        """アーム[k]の順運動学のヤコビアン - dH/dθ・H^-1 の6Dベクトル表現"""
        jac = np.zeros([6,k])
        Hj = np.eye(4)
        for j in range(k+1):
            if j==6: continue  # ツールは固定
            Hj = Hj @ self.homs[j] @ _Rz (th[j])
            Jj = Hj @ _dRRT() @ inv_homtrsf(Hj)  # = dHj/dθj・Hj^-1 = dH/dθj・H^-1
            jac[:,j] = _mat2vec(Jj)  # 第j列 := (dH/dθj・H^-1 の6Dベクトル表示)
        return jac

def is_homtrsf(m, tol=1.e-8):  # HTMかどうか
    return np.abs(inv_homtrsf(m) @ m - np.eye(4)).max() < tol

def inv_homtrsf(m0):  # HTMの逆行列
    m = np.eye(4)
    m[:3,:3] = m0[:3,:3].T
    m[:3,3] = -m0[:3,:3].T @ m0[:3,3]
    return m

def _mat2vec(m):  # 4x4行列 m=H or m=dH・H^-1 の6Dベクトル表示
    v1 = m[:3,3]  # 位置or速度
    v2 = np.array([m[2,1]-m[1,2], m[0,2]-m[2,0], m[1,0]-m[0,1]]) / 2  # 姿勢or角速度
    return [*v1, *v2]

def _Rz(th):  # Z軸まわりθ回転のHTM
    c,s = np.cos(th),np.sin(th)
    m = np.eye(4)
    m[:2,:2] = [[c,-s],[s,c]]
    return m

def _dRRT(th=None):  # = (d/dθ _Rz)・_Rz^T
    m = np.zeros([4,4])
    m[:2,:2] = [[0,-1],[1,0]]
    return m

大雑把に言って,IK() では位置の方程式 $t(\theta) = t_1$ と姿勢の方程式 $\log \left(R(\theta) \cdot {R_1}^T \right) = 0$ をあわせて少々雑なNewton法(解く関数が反復ごとに変わる,logの計算がいい加減,etc.)で解いています.雑とはいえ,普通に2次収束します.

なお J() の具体ですが,関節の角速度を $\dot{\theta} \in \mathbb{R}^6$,ツールの速度を $v := \dot{t} \in \mathbb{R}^3$,角速度を $a := \dot{R} \cdot R^{-1} \in \mathrm{so}(3) \simeq \mathbb{R}^3$ とすると,$J \cdot \dot{\theta}$ の上3成分が $v – a \times t$,下3成分が $a$ です.

アプリ部分

後半はロボットを動かすシミュレーションの例です.冒頭のような動く絵を出します.

  • モデルはウチの Yaskawa ES280D-230 (後継は SP-235) 風です.(36-44行目)
    修正DHパラメタが既知なら mdh2hom() にて利用可能.(28-30行目)
  • ロボットを円形のパスに沿って動かしてみます.(21-26, 57-60行目)
  • パス上でロボットのコマ画像を作成し,アニメーションにします.(64-69, 73-75行目)
from robot import Robot
from mpl_toolkits.mplot3d import Axes3D  # 3Dグラフ
from matplotlib import animation  # 動画
import matplotlib.pyplot as plt
import numpy as np

def rot(u,th):
    """u∈R^3方向を軸にθ[rad]回転する行列 (Rodriguesの回転公式)"""
    u = np.array(u)/np.linalg.norm(u)
    a = np.array([[0,-u[2],u[1]], [u[2],0,-u[0]], [-u[1],u[0],0]])
    u = u.reshape([3,1])
    return np.cos(th)*np.eye(3) + np.sin(th)*a + (1-np.cos(th))*u@u.T

def rt2hom(R,t):
    """姿勢R∈SO(3), 位置t∈R^3 から同次変換行列Hを算出"""
    H = np.eye(4)
    H[:3,:3] = R
    H[:3, 3] = t
    return H

def phi2hom(phi):
    """ロボットのパスの例 - 中心(500,1000,1000)半径500で-Y軸まわりφ回転"""
    H = np.eye(4)
    H[:3,:3] = rot([0,-1,0],phi)  # 姿勢: Y軸まわり回転
    H[:3,3]  = rot([0,-1,0],phi) @ [500,0,0] + [500,1000,750]  # 位置: 円弧
    return H

def mdh2hom(alpha,a,theta,d):
    """修正Denavit Hartenbergパラメタ(α[j-1],a[j-1],θ[j],d[j])から同次変換行列を算出"""
    return rt2hom(rot([1,0,0],alpha), [a,0,0]) @ rt2hom(rot([0,0,1],theta), [0,0,d])

if __name__=="__main__":
    # ロボットモデル: Yaskawa Motoman ES280D-230 風
    # 修正DHパラメタがある場合は homs0[j] := mdh2hom(α[j-1],a[j-1],θ[j],d[j]) などとする
    deg = np.pi/180
    homs0 = [rt2hom(R,t) for (R,t) in
             [(rot([1,0,0],  0*deg), [   0,    0,  0]),  #0
              (rot([1,0,0],-90*deg), [   0,    0,500]),  #1
              (rot([1,0,0],  0*deg), [   0,-1000,  0]),  #2
              (rot([1,0,0], 90*deg), [   0,-1000,  0]),  #3
              (rot([1,0,0],-90*deg), [ 200,    0,  0]),  #4
              (rot([1,0,0], 90*deg), [   0, -200,  0]),  #5
              (rot([1,1,1],120*deg), [   0, -150,  0])]  #6 ツール,なければR=1,t=0
    ]
    robot = Robot(homs0)

    # 初期位置姿勢
    H1 = phi2hom(0*deg)
    th = robot.IK(H1, np.array([210, 330, 270, 290, 70, 50])*deg)

    # 描画領域を準備
    fig = plt.figure()
    ax  = fig.add_subplot(111, projection='3d')
    ims = []  # アニメーションのコマ画像の入れ物

    # ロボットをパスに沿って動かし,コマ画像を記録
    for phi in np.arange(0,360,20)*deg:
        # パスの位置姿勢とそのための関節角度
        H1 = phi2hom(phi)
        th = robot.IK(H1,th)
        homs = [robot.FK(th,j) for j in range(6+1)]  # 各アームjの位置姿勢

        # コマ画像 - アーム原点をつなぐ折れ線 + 各アームのXYZ軸マーク
        im1 = ax.plot(xs=[H[0,3] for H in homs], ys=[H[1,3] for H in homs], zs=[H[2,3] for H in homs], lw=5, c='y')
        for j,H in enumerate(homs):
            ux,uy,uz,pos = H[:3,0], H[:3,1], H[:3,2], H[:3,3]
            points = np.vstack([ux+uy, ux-uy, -ux-uy, -ux+uy, ux+uy, ux, ux*0, uy, uy*0, uz*2, -uz*2])*50 + pos
            im1 += ax.plot(xs=points[:,0], ys=points[:,1], zs=points[:,2], color='rgb'[j%3], lw=2)
        ims.append(im1)
        print('phi=%s: pos=%s th=%s' % (phi/deg, H[:3,3], th/deg), flush=True)

    # 描画実行
    ani = animation.ArtistAnimation(fig, ims, interval=250, blit=True)
    ani.save('robot_animation.gif', writer='pillow')
    plt.show()

逆運動学 IK() は反復解法なので解析的解法に比べ性能や安定性が心配ですが,この例では反復5回程度ですんなり収束します.初期近似解が悪かったり特異点近傍に行くと苦戦.

もう少しリアルに可視化した動画例はこちら.ロボット運動学(自前),衝突検知(PQP),3D CG(OpenGL)の3要素を C++ で実装しています.


Accutheraでは加速器・医療システム・機械学習・放射線シミュレーションなどの専門技術を軸に開発やコンサルティングを承っております。お困りの案件がございましたら、こちら からお気軽にお問合せください。

タイトルとURLをコピーしました