アームと移動ロボットの可操作性楕円体をpythonで描画する

ロボットアームにおいて可操作性楕円体という概念があります。
手先位置がある角度においてどの方向に移動しやすいかということを評価する指標です。

今回はこれを2軸ロボットアームで求めたあと、メカナムホイールを使用した移動ロボットにおいても求めてみます。

2軸ロボットアーム

下図のような2軸ロボットアームを考えます。
機構学でよく扱うような簡単なやつです。

https://jp.mathworks.com/discovery/inverse-kinematics.html より引用

順運動学

このロボットアームの順運動学は以下の式で表されます。
まあ、ここまでは特に解説する必要はないですね。

\left\{\begin{array}{l}
x=L_{1} \cos \left(\theta_{1}\right)+L_{2} \cos \left(\theta_{1}+\theta_{2}\right) \\
y=L_{1} \sin \left(\theta_{1}\right)+L_{2} \sin \left(\theta_{1}+\theta_{2}\right)
\end{array}\right.

可操作性楕円体

ここから可操作性楕円体について説明します。
上記の順運動学の式の両辺を時間微分してやります。
一応書いてやると以下のような、先端速度と関節角速度の関係式が導けます。

\frac{dx}{dt}=
-L_{1}\dot{\theta_1} \sin \left(\theta_{1}\right)
-L_{2}\dot{\theta_1} \sin \left(\theta_{1}+\theta_{2}\right)
-L_{2}\dot{\theta_2} \sin \left(\theta_{1}+\theta_{2}\right) \\
\frac{dy}{dt}=
L_{1}{\theta_1} \cos \left(\theta_{1}\right)
+L_{2}{\theta_1} \cos \left(\theta_{1}+\theta_{2}\right)
+L_{2}{\theta_2} \cos \left(\theta_{1}+\theta_{2}\right)

さらに、この式を行列表示すると以下のようにまとめられます。

\left[
\begin{array}{ll}
\dot{x}\\
\dot{y}
\end{array}
\right]

=\left[\begin{array}{ll}
-L_{1} \sin \left(\theta_{1}\right)-L_{2} \sin \left(\theta_{1}+\theta_{2}\right) & -L_{2} \sin \left(\theta_{1}+\theta_{2}\right) \\
L_{1} \cos \left(\theta_{1}\right)+L_{2} \cos \left(\theta_{1}+\theta_{2}\right) & L_{2} \cos \left(\theta_{1}+\theta_{2}\right)
\end{array}\right]

\left[
\begin{array}{ll}
\dot{\theta_{1}}\\
\dot{\theta_{2}}
\end{array}
\right]

さらにそれぞれのベクトル、行列をまとめて以下のように表します。

\dot{\boldsymbol{r}}=\boldsymbol{J}(\theta) \dot{\boldsymbol{\theta}}

ただし、rは先端の位置ベクトル、θは関節ベクトルです。
また、Jはヤコビ行列(Jacobian matrix)といい、以下で表せられる行列です。
ここまでは習ったことがあるのではないでしょうか。

\begin{aligned}
\boldsymbol{J(\theta)} &=\left[\begin{array}{cc}
\frac{\partial x}{\partial \theta_{1}} & \frac{\partial x}{\partial \theta_{2}} \\
\frac{\partial y}{\partial \theta_{1}} & \frac{\partial y}{\partial \theta_{2}}
\end{array}\right] \\
&=\left[\begin{array}{ll}
-L_{1} \sin \left(\theta_{1}\right)-L_{2} \sin \left(\theta_{1}+\theta_{2}\right) & -L_{2} \sin \left(\theta_{1}+\theta_{2}\right) \\
L_{1} \cos \left(\theta_{1}\right)+L_{2} \cos \left(\theta_{1}+\theta_{2}\right) & L_{2} \cos \left(\theta_{1}+\theta_{2}\right)
\end{array}\right]
\end{aligned}

このヤコビ行列Jを用いて、以下のような正方行列Aを作成します。

\boldsymbol{A}=\boldsymbol{J(\theta)} \boldsymbol{J(\theta)}^{T}

この行列Aの行列式の平方根が可操作度wと呼ばれます。

w=\sqrt{\operatorname{det} \boldsymbol{A}}

また、この行列Aの固有値λと固有ベクトルvから可操作性楕円体を求めることが出来ます。
固有ベクトルvが操作性楕円体の軸方向を表し、固有値λの平方根が対応する軸方向についての楕円の大きさを表します。

実際に求めるコード

pythonで計算しました。描画はmatplotlib

draw_ManipulabilityEllipsoid_RobotArm.py

import numpy as np
import matplotlib.pyplot as plt
import matplotlib.patches as pat

fig, ax = plt.subplots(figsize=(10, 8))

def draw_ellipsoid(ax, a, b, x=0, y=0, deg=0):
    ellips = pat.Ellipse(xy = (x, y), width=a, height=b, alpha = 0.6, angle=deg, color="red", label="")
    ax.add_patch(ellips)

def calculate_Ellipsoid_from_Jacobian(J):
    # 正方行列Aを計算
    A = J * J.T
    # Aの固有値, 固有ベクトルを計算
    values, vectors = np.linalg.eig(A)
    #print(values, vectors)

    # 固有値^(1/2)を取得
    Lambda_1 = np.sqrt(values[0])
    Lambda_2 = np.sqrt(values[1])
    # 固有値から楕円の大きさを取得
    a = Lambda_2    # 横軸の長さ
    b = Lambda_1    # 縦軸の長さ

    # 固有ベクトルを取得
    Vector_1 = vectors[:,0]
    Vector_2 = vectors[:,1]
    # 楕円の傾きを取得
    # Aは対称行列なので固有ベクトルは直交する
    # なので楕円の傾きは固有ベクトルのどちらかを使えばよい
    rad = -np.arctan2(Vector_1[0],Vector_1[1])
    rad = np.arctan2(Vector_2[1],Vector_2[0])

    return a, b, rad

def get_robotarm_pose(L1, L2, theta1, theta2):
    # 順運動学
    X1 = L1*np.cos(theta1)
    Y1 = L1*np.sin(theta1)
    X2 = L1*np.cos(theta1)+L2*np.cos(theta1+theta2)
    Y2 = L1*np.sin(theta1)+L2*np.sin(theta1+theta2)
    return X1, Y1, X2, Y2

def get_Jacobian_robotarm_kinematics(L1, L2, theta1, theta2):
    # Jacobian Matrix
    J = np.matrix([[-L1*np.sin(theta1)-L2*np.sin(theta1+theta2), -L2*np.sin(theta1+theta2)], 
                    [L1*np.cos(theta1)+L2*np.cos(theta1+theta2), L2*np.cos(theta1+theta2)] ])

    return J

def draw_ellipsoid_from_robotarm(L1, L2, theta1, theta2):

    J = get_Jacobian_robotarm_kinematics(L1, L2, theta1, theta2)
    x1,y1,x2,y2 = get_robotarm_pose(L1, L2, theta1, theta2)
    a, b, rad = calculate_Ellipsoid_from_Jacobian( J )

    # 楕円の描画
    draw_ellipsoid(ax, a*0.5, b*0.5, x2, y2, np.rad2deg(rad))

    # ロボットアームの描画
    ax.plot([0, x1], [0, y1], 'k', linewidth = 10.0, zorder=1)
    ax.plot([x1, x2], [y1, y2], 'k', linewidth = 10.0, zorder=1)
    c0 = pat.Circle(xy=(0, 0), radius=.1, ec='w', fill=True, zorder=2)
    c1 = pat.Circle(xy=(x1, y1), radius=.1, ec='w', fill=True, zorder=2)
    ax.add_patch(c0)
    ax.add_patch(c1)

    # 図の調整
    ax.set_xlabel('x')
    ax.set_ylabel('y')
    ax.set_aspect('equal')
    ax.grid()
    plt.xlim([-2,2])
    plt.ylim([-0.5,2])

if __name__ == "__main__":
    L1 = 1.0
    L2 = 1.0
    theta1 = np.deg2rad(30)
    theta2 = np.deg2rad(40)
    draw_ellipsoid_from_robotarm(L1, L2, theta1, theta2)
    plt.show()

結果

実行結果は以下のようになります。
トラックバーなどを追加してぬるぬる動かしてみるのも楽しいのではないでしょうか。

メカナムホイール移動ロボット

次は移動ロボットについての可操作性楕円体を考えてみます。
ロボットアーム以外の可操作性楕円体の適用した文献は見つかりませんでしたが、考え方は同じです。

順運動学

メカナムホイールによって移動するロボットを以下の図のように定義します。

• メカナムホイールのローラ角度 φ
• 機体の縦幅 a
• 機体の横幅(トレッド幅) b としました。

Lは図より以下です。

L = \sqrt { a ^ { 2 } + b ^ { 2 } } \cos \alpha \\
\alpha = \varphi - \tan ^ { - 1 } \frac { a } { b }

図より、各ホイールの周速度v1,v2,v3,v4とすれば、幾何的に以下の関係が導けます。
(手でロボットを直接移動させることを想像してください。)

\begin{aligned} v _ { 1 } & = - \dot { x } \sin \varphi + \dot { y } \cos \varphi + L \dot { \theta } \\ v _ { 2 } & = - \dot { x } \sin \varphi - \dot { y } \cos \varphi + L \dot { \theta } \\ v _ { 3 } & = \dot { x } \sin \varphi - \dot { y } \cos \varphi + L \dot { \theta } \\ v _ { 4 } & = \dot { x } \sin \varphi + \dot { y } \cos \varphi + L \dot { \theta } \end{aligned}

行列表示すると以下のようになります。

 \begin{bmatrix} { v _ { 1 } } \\ { v _ { 2 } } \\ { v _ { 3 } } \\ { v _ { 4 } } \end{bmatrix} = \begin{bmatrix} { - \sin \varphi } & { \cos \varphi } & { L } \\ { - \sin \varphi } & { - \cos \varphi } & { L } \\ { \sin \varphi } & { - \cos \varphi } & { L } \\ { \sin \varphi } & { \cos \varphi } & { L } \end{bmatrix} \begin{bmatrix}{ \dot { x } } \\ { \dot { y } } \\ { \dot { \theta } } \end{bmatrix}

ただし、速度ベクトルはロボットフレーム{ R }から見た速度です。
実際に制御するときには慣性フレーム{ I }から見たらどう動けばいいのかを考えたいです。
ロボットフレームでの速度ベクトルは、回転行列を用いて、慣性フレームにおける速度ベクトルから以下の式で表されます。

\begin{bmatrix}{ \dot { x } } \\ { \dot { y } } \\ { \dot { \theta } }\end{bmatrix}  =  \begin{bmatrix} { \cos \theta } & { \sin \theta } & { 0 } \\ { - \sin \theta } & { \cos \theta } & { 0 } \\ { 0 } & { 0 } & { 1 } \end{bmatrix}   \begin{bmatrix} { \dot { x } _ { I } } \\ { \dot { y } _ { I } } \\ { \dot { \theta } _ { I } } \end{bmatrix} 

よって、前の式に代入して、以下の式が得られます。

 \begin{bmatrix} { v _ { 1 } } \\ { v _ { 2 } } \\ { v _ { 3 } } \\ { v _ { 4 } } \end{bmatrix} = \begin{bmatrix} { - \sin \varphi } & { \cos \varphi } & { L } \\ { - \sin \varphi } & { - \cos \varphi } & { L } \\ { \sin \varphi } & { - \cos \varphi } & { L } \\ { \sin \varphi } & { \cos \varphi } & { L } \end{bmatrix} \begin{bmatrix} { \cos \theta } & { \sin \theta } & { 0 } \\ { - \sin \theta } & { \cos \theta } & { 0 } \\ { 0 } & { 0 } & { 1 } \end{bmatrix}  \begin{bmatrix}{ \dot { x } _ { l } } \\ { \dot { y } _ { I } } \\ { \dot { \theta } _ { I } } \end{bmatrix}

以下の式のような形になっていることがわかります。

\boldsymbol{\dot{ \theta }} = \boldsymbol{X(\theta)} \boldsymbol{\dot{ x }}

ここで、式変形して以下のような形にできたとき、ヤコビ行列J(θ)がでてきます。

\boldsymbol{\dot{ x }} = \boldsymbol{J(\theta)} \boldsymbol{\dot{ \theta }}

ここではメカナムホイールロボットの可操作性楕円体を求めます。
ただし、回転に関する操作性は考慮しないため、使用する式を以下のように変えます。

 \begin{bmatrix} { v _ { 1 } } \\ { v _ { 2 } } \\ { v _ { 3 } } \\ { v _ { 4 } } \end{bmatrix} = \begin{bmatrix} { - \sin \varphi } & { \cos \varphi }  \\ { - \sin \varphi } & { - \cos \varphi } \\ { \sin \varphi } & { - \cos \varphi }  \\ { \sin \varphi } & { \cos \varphi } \end{bmatrix} \begin{bmatrix} { \cos \theta } & { \sin \theta } \\ { - \sin \theta } & { \cos \theta }  \end{bmatrix}  \begin{bmatrix}{ \dot { x } _ { l } } \\ { \dot { y } _ { I } } \end{bmatrix}

コード

上記のdraw_ManipulabilityEllipsoid_RobotArm.pyが必要です。

draw_ManipulabilityEllipsoid_Mecanum.py

import numpy as np
import matplotlib.pyplot as plt
import matplotlib.patches as pat
import matplotlib.transforms as transforms
from draw_ManipulabilityEllipsoid_RobotArm import *

def get_Jacobian_MecanumRobot_kinematics(theta, fai):
    # Jacobian Matrix
    A = np.matrix([ [-np.sin(fai),np.cos(fai)], [-np.sin(fai),-np.cos(fai)], [np.sin(fai),-np.cos(fai)],[np.sin(fai),np.cos(fai)] ])
    R = np.matrix([ [np.cos(theta),np.sin(theta)], [-np.sin(theta),np.cos(theta)] ])
    J = np.linalg.inv(R) * np.linalg.pinv(A);
    return J

def draw_ellipsoid_from_MacanumRobot(theta, fai):

    J = get_Jacobian_MecanumRobot_kinematics(theta, fai)
    a, b, rad = calculate_Ellipsoid_from_Jacobian( J )

    # 楕円の描画
    draw_ellipsoid(ax, a*3, b*3, 0, 0, np.rad2deg(rad))

    # ロボットの描画
    a = 1.0
    b = 1.0
    wheel_width = 0.3
    wheel_diameter = 0.6
    wheel_pos_list = [(b/2,a/2), (-b/2,a/2), (-b/2,-a/2), (b/2,-a/2)]
    ts = ax.transData
    tr = transforms.Affine2D().rotate_deg_around(0, 0, np.rad2deg(theta)) 
    t = tr + ts
    for pos in wheel_pos_list:
        wheel = pat.Rectangle(xy=(pos[0]-wheel_width/2,pos[1]-wheel_diameter/2),width=wheel_width,height=wheel_diameter,transform=t,color="k")
        ax.add_patch(wheel)
    robot = pat.Rectangle(xy=(-b/2+wheel_width/2,-a/2),width=b-wheel_width,height=a,transform=t,color="b")
    ax.add_patch(robot)

    # 図の調整
    ax.set_xlabel('x')
    ax.set_ylabel('y')
    ax.set_aspect('equal')
    ax.grid()
    plt.xlim([-1.5,1.5])
    plt.ylim([-1.5,1.5])

if __name__ == "__main__":
    theta = np.deg2rad(20)
    fai = np.deg2rad(30)
    draw_ellipsoid_from_MacanumRobot(theta, fai)
    plt.show()

結果

上記のdraw_ManipulabilityEllipsoid_RobotArm.pyが必要です。

θ=20°, φ=30°

実行結果は以下のようになります。それっぽい

θ=0°, φ=30°

まあ、そもそもロボットが回転しても楕円の形状には影響がないのです。
theta=0としてやると以下のようになります。

θ=0°, φ=45°

また、fai=45°にしてやると、x方向とy方向への移動のしやすさが等しくなり、以下のようになります。

θ=0°, φ=0°

ほかには、fai=0°とすると以下のようになります。よく見ると、ロボットの中心に縦線があります。
（わかりやすくするために本体の色を薄くしました。）

以下の図を考えればわかりやすいです。
fai=0だとx方向に動けなくなることがすぐに分かります。