使用した機体

こちらの機体です。

詳細な解説記事はチームのブログに記載しています。気になる方はご覧ください。
https://pylot.kaijo-physics.club/blog/runa_commentry

使用したPCのスペック

i7-10875H
RTX 3080 laptop
Ram 16gb

非常につよつよなパソコンとなっています。今回の大会では、音声認識タスクや、顔識別タスクなどもあったため、このようなスペックとなっています。

今回このプログラムを使用した競技内容

今回こちらのプログラムは、RoboCup@Home 2025 JapanOpen Bridge Competition(日本大会)とRoboCup@Home OPL 2025 Salvador(世界大会)で、Help Me Carryのタスクの中で実際に使用されました。実際に動いている様子はこちらの動画にあります。

プログラムの概要

実際のプログラムはGithubにあります。(未整備です...すみません)

今回のプログラムでは、ハードウェアと

まず最初にプログラムの流れを示します。

今回制作したプログラムは2つのノードに分かれています。
一つのプログラムは、realsense_trackとしています。
機体の上部にあるrealsense D455から出ているRGBDトピックを受け取り、ultralyticsのYolov8-Poseで、人検出を行います。その後、2つの処理を行っています。一つは、PanTiltの制御のためにPID制御で、PanTiltで、人が画像の中止に来るようにDynamixelサーボの信号を生成し、送信しています。もう一つは、デプス画像から、ロボット座標系における認識物の座標を計算しています。処理の流れとしては、下の通りです。

カメラのデプス画像から座標変換する際はピンホールカメラモデルを利用しています。詳細な解説、図等は、他記事を参照していただければと思います。数式だけ示しておきます。
カメラの光学情報はrealsenseの/camera_infoトピックから配信されているため、こちらを使用しています。

パラメータ	記号	説明	単位
焦点距離（X方向）	$f_x$	X軸方向のピクセル単位焦点距離	pixel
焦点距離（Y方向）	$f_y$	Y軸方向のピクセル単位焦点距離	pixel
光学中心（X）	$c_x$	画像主点のX座標	pixel
光学中心（Y）	$c_y$	画像主点のY座標	pixel
深度	$Z$	カメラからの距離	meter

|

今回、深値 $Z$ が既知であるため、画像座標 $(u, v)$ から3D座標 $(X, Y, Z)$ を求められます。

同次座標系での表現：

$$
\begin{pmatrix}
u' \
v' \
w
\end{pmatrix}
=\mathbf{K} \cdot
\begin{pmatrix}
X \\
Y \\
Z
\end{pmatrix}
=\begin{pmatrix}
f_x & 0 & c_x \\
0 & f_y & c_y \\
0 & 0 & 1
\end{pmatrix}
\begin{pmatrix}
X \
Y \
Z
\end{pmatrix}
$$

正規化：

$$
u = \frac{u'}{w} = f_x \cdot \frac{X}{Z} + c_x
$$

$$
v = \frac{v'}{w} = f_y \cdot \frac{Y}{Z} + c_y
$$

$$
\boxed{
X = \frac{(u - c_x) \cdot Z}{f_x}
}
$$

$$
\boxed{
Y = \frac{(v - c_y) \cdot Z}{f_y}
}
$$

行列形式での変換：

$$
\begin{pmatrix}
X \
Y \
Z
\end{pmatrix}
= Z \cdot \mathbf{K}^{-1}
\begin{pmatrix}
u \
v \
1
\end{pmatrix}
= Z \cdot
\begin{pmatrix}
\frac{1}{f_x} & 0 & -\frac{c_x}{f_x} \\
0 & \frac{1}{f_y} & -\frac{c_y}{f_y} \\
0 & 0 & 1
\end{pmatrix}
\begin{pmatrix}
u \
v \
1
\end{pmatrix}
$$

光学座標系とロボット座標系では、x,y,zの座標系が違うため、座標系をきちんと変換しなきゃいけません。このプログラムを作成している時に、realsenseのノードから出ているTFのcamera_optical_frameの意味がやっとわかるようになりました。

Yolo+深度取得の工夫点

人追従ということで、基本的には、Yolov8のPoseモデルを使用しています。基本的にPoseで推定されるのは人なので,こちらを使用しました。
また、RoboCupでは、たくさん人がいる場面で人追従を行う必要があるため、人認識が一回だけ途切れてしまうことがあっても、一定時間までは同じ人を追うようにするという処理を加えました。
また、深度もKeypointから取得できるというメリットもあります。
深度に関しては、bboxの場合は、全体の深度のうち近い方から20%を使用するや、keypoint付近の5x5の平均値を使用するなどして、安定した深度取得を目指しました。

追従部分

次にperson_to_goalupdater.pyを説明します。

追従部分に関しては、今回は、Nav2に全部投げました。person_infoから受け取った情報を、map座標系に直してから、定期的なタイマー更新によって、NavigateToPoseで、追従を行っています。BehaivorTreeについては、公式のDocumentに記載されているものから引っ張ってきました。

終わりに

今回は、RoboCupで使用した人追従のプログラムについて話しました。チーム自作の人追従(A*+MPC)もあったのですが、Nav2と組み合わせたプログラムの方だと精度が高かったため、そちらを使用しました。人追従で遊びたい人がこのプログラムを参考にしてもらえると嬉しいです。