【Python/numpy】Numpyで2次元の同次変換行列を扱う

はじめに

この記事はロボティクスアドベントカレンダー3日目の記事です。

また、自分の勉強も兼ねて様々な言語、通信手法などを試しながらロボットシステムを作っていくシリーズの2つ目の記事です。
1つ目の記事はこちら

以下の記事でC++/Eigenで2次元の同次変換行列を扱う方法を紹介しました。
今回はPythonでも同じようなことをやっていこうと思います。

環境

以下の環境で動作を確認しました。

項目	バージョン
Ubuntu	24.04
Python	3.12.3
Numpy	2.3.5

具体例

以下に平行移動、回転、同次変換の例を示します。
先程紹介したEigenの例だと並進専用の行列を示すEigen::TranslationがありましたがNumpyにはありません。なのでベクトルを単純に足すことで平行移動を実現しています。

Pythonでの行列積はA@Bまたはnumpy.matmul(A,B)で計算することができます。

import numpy as np

# 2次元の初期位置
pose = np.array([1.0, 2.0])

print("初期位置:")
print(pose)

# 平行移動ベクトル
translation = np.array([1.0, 1.0])
print("平行移動ベクトル:")
print(translation)

# 平行移動
translated_pose = pose + translation
print("平行移動後の位置:")
print(translated_pose)
print()

# 回転角（度→ラジアン）
yaw = 45.0
theta = np.deg2rad(yaw)

# 回転行列
rotation = np.array([
    [np.cos(theta), -np.sin(theta)],
    [np.sin(theta),  np.cos(theta)],
])
print("回転行列:")
print(rotation)

# 回転
rotated_pose = rotation @ pose
print("回転後の位置:")
print(rotated_pose)
print()

# 同次変換行列（3x3）
transform = np.eye(3)
transform[0:2, 0:2] = rotation
transform[0:2, 2] = translation
print("同次変換行列:")
print(transform)
print()

# 同次座標へ
pose_h = np.array([pose[0], pose[1], 1.0])

# 同次変換の適用
transformed_pose_h = transform @ pose_h
transformed_pose = transformed_pose_h[:2]

print("適用後の位置:")
print(transformed_pose)

初期位置:
[1. 2.]
平行移動ベクトル:
[1. 1.]
平行移動後の位置:
[2. 3.]

回転行列:
[[ 0.70710678 -0.70710678]
 [ 0.70710678  0.70710678]]
回転後の位置:
[-0.70710678  2.12132034]

同次変換行列:
[[ 0.70710678 -0.70710678  1.        ]
 [ 0.70710678  0.70710678  1.        ]
 [ 0.          0.          1.        ]]

適用後の位置:
[0.29289322 3.12132034]

同次変行列の計算は数式にすると以下のような計算をしています。

\begin{align}
\begin{pmatrix}
x^{\prime} \\
y^{\prime}\\
1\\
\end{pmatrix}
&=
\left(
\begin{array}{}
\cos \theta & - \sin \theta & t_x \\
\sin \theta &   \cos \theta & t_y \\
0           & 0             & 1 
\end{array}
\right)
\begin{pmatrix}
x \\
y \\
1 \\
\end{pmatrix} \\
\end{align}

Numpyでロボットの位置姿勢を計算

例えば2次元の移動ロボットの座標を計算する場合、3×3の同次変換行列を自分で構築するのがいいでしょう。
上記のように並進、回転だけも計算できますが、同次変換行列であれば行列の積算だけで計算することができ、コードがすっきりすると思います。

以下に例を挙げます。
初期位置も3次元である必要があるので2次元の並進しかしない場合はもちろん無理に3x3の同次変換行列を使う必要はありません。

import numpy as np

# 初期位置（同次座標）
pose = np.array([1.0, 2.0, 1.0])  # 最後が 1 のベクトル

# 平行移動
tx, ty = 1.0, 1.0

# 平行移動行列（3×3）
T = np.array([
    [1, 0, tx],
    [0, 1, ty],
    [0, 0, 1]
])

# 回転角（ラジアン）
yaw = np.deg2rad(45.0)
cos_y = np.cos(yaw)
sin_y = np.sin(yaw)

# 回転行列（3×3）
R = np.array([
    [cos_y, -sin_y, 0],
    [sin_y,  cos_y, 0],
    [0,      0,     1]
])

print("初期位置:\n", pose[:2], "\n")

print("平行移動行列:\n", T, "\n")

translated_pose = T @ pose
print("平行移動後の位置:\n", translated_pose[:2], "\n")

print("回転行列:\n", R, "\n")

rotated_pose = R @ pose
print("回転後の位置:\n", rotated_pose[:2], "\n")

# 同次変換行列（平行移動→回転）
TR = T @ R
print("同次変換行列:\n", TR, "\n")

transformed_pose = TR @ pose
print("適用後の位置:\n", transformed_pose[:2])

初期位置:
 [1. 2.] 

平行移動行列:
 [[1. 0. 1.]
 [0. 1. 1.]
 [0. 0. 1.]] 

平行移動後の位置:
 [2. 3.] 

回転行列:
 [[ 0.70710678 -0.70710678  0.        ]
 [ 0.70710678  0.70710678  0.        ]
 [ 0.          0.          1.        ]] 

回転後の位置:
 [-0.70710678  2.12132034] 

同次変換行列:
 [[ 0.70710678 -0.70710678  1.        ]
 [ 0.70710678  0.70710678  1.        ]
 [ 0.          0.          1.        ]] 

適用後の位置:
 [0.29289322 3.12132034]

汎用性を上げる

これらの行列計算を使って座標変換などを簡単にできるクラスを作ってみました。
Pose2D同士の行列計算は積算（@）で、Pose2Dのオブジェクトと姿勢を持たない点（X, Y）とはtransform_pose()関数で座標変換ができます。

import dataclasses
import math

import numpy as np

@dataclasses.dataclass
class Pose2D:
    x: float
    y: float
    theta: float

    def as_matrix(self) -> np.ndarray:
        s = math.sin(self.theta)
        c = math.cos(self.theta)
        return np.array(
            [
                [c, -s, self.x],
                [s, c, self.y],
                [0, 0, 1.0],
            ]
        )

    def transform_point(self, point: tuple[float, float]) -> tuple[float, float]:
        px, py = point
        vec = np.array([px, py, 1.0])
        res = self.as_matrix() @ vec
        return float(res[0]), float(res[1])

    def __matmul__(self, other: "Pose2D") -> "Pose2D":
        m = self.as_matrix() @ other.as_matrix()
        return Pose2D(
            x=float(m[0, 2]),
            y=float(m[1, 2]),
            theta=float(math.atan2(m[1, 0], m[0, 0])),
        )

# 使用例

yaw = np.deg2rad(45.0)
pose_initial = Pose2D(1.0, 2.0, 0.0)
pose_translation = Pose2D(1.0, 1.0, 0.0)
pose_rotation = Pose2D(0.0, 0.0, yaw)

# 平行移動の適用
translated_point_pose2d = pose_translation.transform_point((pose_initial.x, pose_initial.y))
# pose_translation @ pose_initial でも良い
print("Pose2D 平行移動後の位置:")
print(translated_point_pose2d)
print()

# 回転の適用
rotated_point_pose2d = pose_rotation.transform_point((pose_initial.x, pose_initial.y))
# pose_rotation @ pose_initial でも良い
print("Pose2D 回転後の位置:")
print(rotated_point_pose2d)
print()

# 合成変換
composed_transform = pose_translation @ pose_rotation
composed_point_pose2d = composed_transform.transform_point((pose_initial.x, pose_initial.y))
# composed_transform @ pose_initial でも良い
print("Pose2D 合成変換後の位置:")
print(composed_point_pose2d)

ロボットの位置姿勢の計算に導入

開発しているロボットのシュミレータに導入してみましょう。

具体的には、ロボットの位置を計算する箇所です。
ロボットに対する速度指令から、微小なPoseを作り、それを現在の位置姿勢に掛けます。

このブランチに上げました

- self._theta += self._omega * dt
- new_x = self._x + self._v * math.cos(self._theta) * dt
- new_y = self._y + self._v * math.sin(self._theta) * dt
+ delta_pose = pose_util.Pose2D(self._v * dt, 0.0, self._omega * dt)
+ new_pose = self._robot_pose @ delta_pose

- if not self._collides(new_x, new_y, world):
-     self._x = new_x
-     self._y = new_y
+ if not self._collides(new_pose.x, new_pose.y, world):
+     self._robot_pose = new_pose

X,Y,Thetaを別々に計算しなくなり、プログラムがすっきりしたような気がしますよね。

前回からの差分は以下で見ることができます。

終わりに

Numpyで同次変換行列を計算する例と、実際に開発しているシミュレータに組み込んでみました。
簡単なのでぜひみなさんも使ってみてください。

まだまだ始まったばかり、ロボティクスアドベントカレンダーの投稿をお待ちしています！