LoginSignup
0
0

Delete article

Deleted articles cannot be recovered.

Draft of this article would be also deleted.

Are you sure you want to delete this article?

@haruhiro1020

2軸ロボットアーム Part1 (順運動学)

Last updated at Posted at 2025-06-15

はじめに

私がロボットに関して興味を持っている．特にロボットの経路生成に興味がある．
前記事では，経路生成手法であるRRT と干渉判定用ライブラリであるpython-fcl を説明した．
(https://qiita.com/haruhiro1020/items/45e1e0d92c74a3ac5ff9)
最終的にはロボットアームにRRT + python-fcl を合体させたいと考えている．
本記事では，2軸ロボットアームの順運動学を説明する．
順運動学とは，関節角度から手先位置を計算することです．
手先位置を算出するために，三角関数(sin, cos, tan)を使用する．以下サイトはわかりやすいです．
・https://www.meikogijuku.jp/meiko-plus/study/trigonometric-function.html

本記事で実装すること

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

本記事では実装できないこと (将来実装したい内容)

・2軸ロボットアームの逆運動学 (逆運動学 ... ロボットの手先位置からロボットの関節角度を算出する)
・2軸ロボットアームに経路生成RRT を組み合わせる

動作環境

・macOS Sequoia (バージョン15.5)
・Python3 (3.10.9)
・Numpy (1.23.5) (数値計算用ライブラリ)
・Matplotlib (3.7.0) (アニメーション作成用ライブラリ)

順運動学

順運動学について説明する．
今回は下図のような2軸ロボットアームを考える．
ForwardKinematics_V0.drawio.png
l1はリンク1の長さ[m]，l2はリンク2の長さ[m]です．
$\theta_1$は関節1の角度[rad]，$\theta_2$は関節2の角度[rad]です．
手先位置P を各関節角度(theta_1，theta_2)から計算する．

上図を一度下図のようにして考えてみる．
ForwardKinematics_V1.drawio.png

はじめにリンク1の先端位置P1を計算する．
P1(x1, y1)は三角関数を使用することで，以下となる．

\displaylines{
x_{1} = l_{1} * \cos(\theta_{1}) \\
y_{1} = l_{1} * \sin(\theta_{1}) \\
}

次に手先位置Pを計算する．
P(x, y)は三角関数とP1(x1, y1)を使用することで，以下となる．

\displaylines{
x = x_{1} + l_{2} * \cos(\theta_{1} + \theta_{2}) \\
y = y_{1} + l_{2} * \sin(\theta_{1} + \theta_{2}) \\
}

手先位置P(x, y)はP1(x1, y1)を使用しない場合は，以下となる．

\displaylines{
x = l_{1} * \cos(\theta_{1}) + l_{2} * \cos(\theta_{1} + \theta_{2}) \\
y = l_{1} * \sin(\theta_{1}) + l_{2} * \sin(\theta_{1} + \theta_{2}) \\
}

順運動学のソースコード

定数を定義するファイル (constant.py)

定数を定義するファイルを下記に記す．

constant.py
# 複数ファイルで使用する定数の定義


# 次元数を定義
DIMENTION_NONE            = -1          # 未定義
DIMENTION_2D              =  2          # 2次元
DIMENTION_3D              =  3          # 3次元

2軸ロボットアームを定義するファイル (robot.py)

2軸ロボットアームを定義するファイルを下記に記す．

robot.py
# 2自由度ロボットの運動学を記載

# ライブラリの読み込み
import numpy as np

# 自作モジュールの読み込み
from constant import *              # 定数


class Robot:
    """
    ロボットのベースクラス(抽象クラス)
    
    プロパティ
        _links(numpy.ndarray): ロボットのリンク長 [m]
    
    メソッド
        public
            forward_kinematics(): 順運動学 (ロボットの関節角度からロボットの手先位置を算出)
            links(): _linksプロパティのゲッター
    """
    # 定数の定義
    _DIMENTION_POSE  = DIMENTION_NONE       # 手先位置の次元数
    _DIMENTION_THETA = DIMENTION_NONE       # 関節角度の次元数
    _DIMENTION_LINK  = DIMENTION_NONE       # リンク数
    
    def __init__(self, links):
        """
        コンストラクタ

        パラメータ
            links(numpy.ndarray): ロボットのリンク長 [m]
        """
        if np.size(links) != self._DIMENTION_LINK:
            # 異常
            raise ValueError(f"links's size is abnormal. correct is {self._DIMENTION_Link}")
        
        # プロパティの初期化
        self._links = links

    @property
    def links(self):
        """
        _linksプロパティのゲッター
        """
        return self._links

    def forward_kinematics(self, thetas):
        """
        順運動学 (ロボットの関節角度からロボットの手先位置を算出)

        パラメータ
            thetas(numpy.ndarray): ロボットの関節角度 [rad]
        
        戻り値
            pose(numpy.ndarray): ロボットの手先位置 (位置 + 姿勢) [m] + [rad]
        """
        raise NotImplementedError("forward_kinematics()をオーバーライドしてください.")



class Robot2DoF(Robot):
    """
    2軸ロボットクラス
    
    プロパティ
        _links(numpy.ndarray): ロボットのリンク長
        _rot(Rotation): 回転行列クラス
    
    メソッド
        public
            forward_kinematics(): 順運動学 (ロボットの関節角度からロボットの手先位置を算出)
    """
    # 定数の定義
    _DIMENTION_POSE  = DIMENTION_2D         # 手先位置の次元数
    _DIMENTION_THETA = DIMENTION_2D         # 関節角度の次元数
    _DIMENTION_LINK  = DIMENTION_2D         # リンク数
    
    def __init__(self, links):
        """
        コンストラクタ

        パラメータ
            links(numpy.ndarray): ロボットのリンク長 [m]
        """
        # 親クラスの初期化
        super().__init__(links)
    
    def forward_kinematics(self, thetas):
        """
        順運動学 (ロボットの関節角度からロボットの手先位置を算出)

        パラメータ
            thetas(numpy.ndarray): ロボットの関節角度 [rad]
        
        戻り値
            pose(numpy.ndarray): ロボットの手先位置 (位置) [m]
        """
        # パラメータの次元数を確認
        if np.size(thetas) != self._DIMENTION_THETA:
            raise ValueError(f"thetas's size is abnormal.\nthetas's size is {np.size(thetas)}")

        # あらかじめ三角関数を算出する
        sin1  = np.sin(thetas[0])
        sin12 = np.sin(thetas[0] + thetas[1])
        cos1  = np.cos(thetas[0])
        cos12 = np.cos(thetas[0] + thetas[1])

        theta1  = np.array([cos1,  sin1])
        theta12 = np.array([cos12, sin12])

        # ロボットの手先位置を算出
        pose = self._links[0] * theta1 + self._links[1] * theta12
        
        return pose

メイン(プロットなど)処理 (main.py)

メイン処理ファイルを下記に記す．

main.py
# メイン処理

# ライブラリの読み込み
import numpy as np                  # 数値計算
import matplotlib.pyplot as plt     # 描画

# 自作モジュールの読み込み
from robot import Robot2DoF         # ロボットクラス


LINE_WIDTH = 3                      # プロット時の線の太さ


def main():
    """
    メイン処理
    """
    # 2軸ロボットのリンク長
    links = np.array([1.0, 1.0])
    # 2軸ロボットのインスタンスを作成
    robot = Robot2DoF(links)

    # 関節角度 [deg] を定義
    deg_thetas = [0, 30, 60, 90]
    # [deg]から[rad]へ変換
    candidate  = np.deg2rad(deg_thetas)
    all_thetas = np.array([[theta1, theta2] for theta1 in candidate for theta2 in candidate])

    # 順運動学により，位置を算出
    for thetas in all_thetas:
        pose = robot.forward_kinematics(thetas)
        plot(robot, thetas)

    # X軸，Y軸の名称
    plt.xlabel("X [m]")
    plt.ylabel("Y [m]")

    # X軸，Y軸の描画範囲
    plt.xlim(-2.5, 2.5)
    plt.ylim(-2.5, 2.5)

    plt.grid()
    # plt.legend(bbox_to_anchor=(1.05, 1), loc='upper left', borderaxespad=0, fontsize=10)
    plt.legend(fontsize=6)
    plt.show()


def plot(robot, thetas):
    """
    ロボットをプロット (将来的にはクラス化する)

    パラメータ
        robot(Robot2DoF): ロボット
        thetas(numpy.ndarray): 回転角度
    """
    # 回転角度とリンク長をローカル変数に保存
    theta1 = thetas[0]
    theta2 = thetas[1]
    link1  = robot.links[0]
    link2  = robot.links[1]

    # 三角関数を計算
    s1  = np.sin(theta1)
    c1  = np.cos(theta1)
    s2  = np.sin(theta2)
    c2  = np.cos(theta2)
    s12 = np.sin(theta1 + theta2)
    c12 = np.cos(theta1 + theta2)

    # 各リンクの位置を算出
    base_pos  = np.zeros(2)
    link1_pos = link1 * np.array([c1, s1])
    link2_pos = link1_pos + link2 * np.array([c12, s12])

    # 全リンクの位置を算出
    all_link_pos = np.array([base_pos, link1_pos, link2_pos])
    # print(f"all_link_pos = {all_link_pos}")
    # 全リンクの回転角度を算出
    all_link_theta = np.array([theta1, theta2])

    # 線プロット (小数点1桁まで記載)
    plt.plot(all_link_pos[:, 0], all_link_pos[:, 1], linewidth=LINE_WIDTH, label=f"theta1={np.rad2deg(theta1):.1f} [deg], theta2={np.rad2deg(theta2):.1f} [deg]")
    # 点プロット
    plt.scatter(all_link_pos[:, 0], all_link_pos[:, 1])


if __name__ == "__main__":
    # 本ファイルがメインで呼ばれた時の処理
    main()

処理結果

main.pyを実行し，順運動学が正しいかのグラフは下図である．
plot_2drob_forward_kinematics.png
左下に判例を記載してある．グラフを見る限り，順運動学の処理は正常であることがわかる．

おわりに

本記事では，Pythonを使用して，下記内容を実装しました．
・2軸ロボットアームの順運動学 (順運動学 ... ロボットの関節角度からロボットの手先位置を算出する)

次記事では，下記内容を実装していきます．
・2軸ロボットアームの逆運動学 (逆運動学 ... ロボットの手先位置からロボットの関節角度を算出する)
(https://qiita.com/haruhiro1020/items/9416bea4b5b0757f4f36)

0
0
0

Register as a new user and use Qiita more conveniently

  1. You get articles that match your needs
  2. You can efficiently read back useful information
  3. You can use dark theme
What you can do with signing up
Sign upLogin
0
0

Delete article

Deleted articles cannot be recovered.

Draft of this article would be also deleted.

Are you sure you want to delete this article?