0
1

Delete article

Deleted articles cannot be recovered.

Draft of this article would be also deleted.

Are you sure you want to delete this article?

@carp1046(1046 carp)

FANUCロボットと2Dカメラ・距離センサだけで3D位置姿勢推定をしてみた

0
Last updated at Posted at 2026-08-30

FANUCロボットと2Dカメラ・距離センサだけで3D位置姿勢推定をしてみた

― SVDや固有値分解なしの剛体位置合わせ ―

産業用ロボットでワークの3次元位置・姿勢を測定したい場合、素直な方法は3Dカメラや3Dスキャナを使うことです。

実際、それが一番正しいと思います。

ただし問題があります。

高い。

そこで、

  • FANUCロボット
  • 2Dカメラ
  • 距離センサ

を組み合わせて複数の3次元点を取得し、それらからワークの位置と姿勢を推定してみることにしました。

さらに、

せっかくFANUCを使っているのだから、姿勢推定までKARELだけでできないだろうか？

と考えました。

普通ならPCへ点群を送り、NumPyやEigenなどでSVDを使って剛体位置合わせを行えば終わる話です。

しかし今回は、あえてロボットコントローラ側だけで完結させます。

やりたいこと

       2Dカメラ
          ↓
      画像上のXY位置

       距離センサ
          ↓
    センサZ軸上の距離

       計測結果
          ↓
   センサ座標系上の3D点
          +
   ロボット現在姿勢
          ↓
   ロボット座標系へ変換
          ↓
       3D測定点

をつくります。

まず、2Dカメラと距離センサの計測結果からセンサ座標系上の3D点を作り、
ロボットの現在姿勢と組み合わせてロボット座標系上の3D測定点へ変換します。
本記事では、センサ値から3D点を生成する部分ではなく、得られた複数の対応3D点からワークの位置・姿勢を推定する部分を扱います。
座標変換やベクトル合成などは、多くの産業用ロボットで標準機能として用意されているため、ここでは省略します。

基準となる点群を

$$
B={\mathbf b_1,\mathbf b_2,\ldots,\mathbf b_N}
$$

実際に測定した点群を

$$
A={\mathbf a_1,\mathbf a_2,\ldots,\mathbf a_N}
$$

とします。

求めたいのは、

$$
\mathbf b_i
\approx
R\mathbf a_i+\mathbf t
$$

となる回転 $R$ と平行移動 $\mathbf t$ です。

要するに、

測定した点群 A

           ・
       ・
              ・
          ・

        ↓ 回して移動する

基準点群 B

    ・　 ・

           
    ・　 ・

となる変換を求めます。

一般的な3D剛体位置合わせでは、Kabsch / Arun法 などを使い、共分散行列を作ってSVDする方法がよく使われます。

PCであれば非常に簡単です。

U, S, Vt = np.linalg.svd(H)
R = Vt.T @ U.T

しかしKARELにはNumPyはありません。

当然ながら、

SVD(H, U, S, V)

などという都合の良い関数もありません。

SVDを自作する手もありますが、今回そこまでしたいわけではありません。できる気もしません。

一方、ロボットメーカー各社の組み込み用言語では

1点目：原点

2点目：X方向を決める点

3点目：XY平面を決める点

という一番素朴な姿勢推定がありますが、測定ノイズに非常に弱いです。

欲しいのは、3点だけで座標系を決める方法よりも複数の測定点を利用でき、測定ノイズの影響を平均化しやすく、軽量で設備点数を増やさない

ロボットの中で完結する3D姿勢推定

です。

SVDを使わず、反復計算で回転を求める

参考にしたのが、SVDや固有値分解を使わず、反復計算によって剛体位置合わせや姿勢推定を行う手法です。

特に Fast Rigid 3D Registration Solution: A Simple Method Free of SVD and Eigen-Decomposition では、点群から作った相互共分散行列をもとに、外積を使った反復計算によって回転行列を求めています。

この考え方を見て、

「SVDや固有値分解を使わなくても、外積から得られる回転誤差を少しずつ補正していけば姿勢を求められるのではないか」

と考えました。

さらに、Mahony filter や Madgwick filter などの姿勢推定手法、クォータニオンの解説資料なども読み、自分で理解できた部分を組み合わせてKAREL向けの簡単な実装にしています。

今回の実装は、これらの論文やアルゴリズムを忠実に移植したものではありません。

対応ベクトルの外積を集めて回転方向の誤差を求め、その誤差から小さな回転クォータニオンを作り、姿勢を数回反復更新するという形に単純化しています。

まずは等方的な測定誤差を仮定した単純な実装を紹介します。異方性誤差や点ごとの重み付けについては、今後試していく予定です。

以下のコードは、KARELというPascalベースのFANUCロボット専用の組み込み言語で記述しています。

ただし、使用している処理はベクトルの内積・外積やクォータニオン演算など基本的なものが中心で、KAREL固有の組み込み関数にはほとんど依存していません。そのため、同じ考え方は他の言語でも実装できます。

まず重心を原点へ移す

最初に両点群の重心を求めます。

$$
\bar{\mathbf a}=
\frac{1}{N}\sum_i\mathbf a_i
$$

$$
\bar{\mathbf b}=
\frac{1}{N}\sum_i\mathbf b_i
$$

KARELでは

FOR i = 1 TO num_point DO
    org_a.x = org_a.x + A[i].x
    org_a.y = org_a.y + A[i].y
    org_a.z = org_a.z + A[i].z

    org_b.x = org_b.x + B[i].x
    org_b.y = org_b.y + B[i].y
    org_b.z = org_b.z + B[i].z
ENDFOR

org_a.x = org_a.x / num_point
org_a.y = org_a.y / num_point
org_a.z = org_a.z / num_point

org_b.x = org_b.x / num_point
org_b.y = org_b.y / num_point
org_b.z = org_b.z / num_point

そして、

FOR i = 1 TO num_point DO
    A[i] = A[i] - org_a
    B[i] = B[i] - org_b
ENDFOR

として両方の重心を原点へ移します。

これで平行移動成分を一旦無視して、回転だけ考えられます。

外積を足して「回したい方向」を作る

このプログラムの中心部分は非常に単純です。

FOR i = 1 TO num_point DO

    a_n = A[i]
    b_n = B[i]

    normalized(a_n)
    normalized(b_n)

    torque = torque + (a_n # b_n)

ENDFOR

FANUC KARELでは # がベクトルの外積です。
"normalized()" はベクトル型変数を正規化する関数です。

つまりやっていることは、

$$
\boldsymbol{\tau}=
\sum_i
\left(
\hat{\mathbf a}_i
\times
\hat{\mathbf b}_i
\right)
$$

だけです。

ここでは説明しやすいので、このベクトルを torque と呼んでいます。

なぜ外積で回転方向が分かるのか

2本のベクトル

$$
\mathbf a,\mathbf b
$$

があるとします。

外積

$$
\mathbf a\times\mathbf b
$$

の向きは、$\mathbf a$ を $\mathbf b$ へ近づけるための回転軸になります。

大きさは、

$$
|\mathbf a\times\mathbf b|=
|\mathbf a||\mathbf b|\sin\theta
$$

です。

今回は両方を正規化しているので、

$$
|\hat{\mathbf a}\times\hat{\mathbf b}|=
\sin\theta
$$

となります。

つまり、

方向 → どの軸回りへ回すか
大きさ → どれくらい姿勢がずれているか

という情報が外積一本から得られます。

1組だけならノイズに弱いですが、複数の対応点について全部足せば、

点1「こっちへ回したい」
点2「だいたいこっち」
点3「こっちかな」
点4「私もこっち」

        ↓

全部加算

        ↓

全体として回すべき方向

を作れます。

一気に回さず、少しずつ回す

求めた torque をそのまま回転量にはしません。

alpha = alpha0 / num_point

外積を点数分だけ加算しているため、点数が増えても更新量が極端に大きくならないよう、ゲインを点数で割っています。

このプログラムでは、テストの上でゲインを

    alpha0 = 0.5

としました。

回転量は、

angle = vec_norm(torque)
angle = alpha * angle * to_degree

として作っています。

"vec_norm()"はベクトル型変数のユークリッドノルムを返す関数。
FANUC側の三角関数との単位を合わせるため、ラジアン相を角度へ変換しています。

KARELでの実装に合わせて微小なクォータニオンを逐次合成して、姿勢を収束させる方式に変更しました。クォータニオンによる姿勢更新はMadgwickフィルタを参考にしました。

  • 回転行列が単精度演算の繰り返しにより直交性が崩れる可能性ある、チェックのコストが大きい。再直交化とかありえない...
  • 回転行列は9要素、クォータニオンは4要素でデータを保持できるので効率が良い
  • 単位クォータニオンなら、正規化だけで姿勢表現の単位性を保ちやすく、その計算コストも比較的小さい

微小回転をクォータニオンにする

回転軸を正規化します。

vec = torque
normalized(vec)

そして半角を使って、

half_angle = angle / 2
sin_val = SIN(half_angle)

delta_Q.x = vec.x * sin_val
delta_Q.y = vec.y * sin_val
delta_Q.z = vec.z * sin_val
delta_Q.w = COS(half_angle)

とすれば、小回転クォータニオン

$$
\Delta q=
\left(
u_x\sin\frac{\Delta\theta}{2},
u_y\sin\frac{\Delta\theta}{2},
u_z\sin\frac{\Delta\theta}{2},
\cos\frac{\Delta\theta}{2}
\right)
$$

が得られます。

点群そのものも少しずつ回す

小回転クォータニオンをつかって点群A自体を少しずつ点群Bへ向かって回していきます。

FOR i = 1 TO num_point DO
    A[i] = quat_rotv(delta_Q, A[i])
ENDFOR

ベクトルのクォータニオン回転も、わざわざ

$$ q p q^{-1} $$

をそのまま計算すると重たいので、

$$
q=(\mathbf{q}_v, q_w)
$$

$\mathbf{q}_v$ はクォータニオンの虚部ベクトル

$q_w$ はクォータニオンの実部のスカラー

$$
\mathbf{t}=
2(\mathbf{q}_v\times\mathbf{p})
$$

$$
\mathbf{p}'=
\mathbf{p}+q_w\mathbf{t}+\mathbf{q}_v\times\mathbf{t}
$$

としています。
これを"quat_rotv()"として実装しました

これは単位クォータニオンによるベクトル回転を展開したものです。

こうすると必要なのは、

  • 外積
  • スカラー倍
  • ベクトル加算

程度です。

KARELでも十分書けます。

あとは繰り返すだけ

全体は、

対応ベクトルの外積を足す
        ↓
回転軸を求める
        ↓
ほんの少し回す
        ↓
A点群も一緒に回す
        ↓
もう一度外積を計算
        ↓
……

という反復計算になります。

点群 A から 点群 B への回転量は

各反復で得られた微小回転クォータニオン $\Delta q_k$ を現在の姿勢クォータニオン $Q_k$ に左から順次乗算し、総回転クォータニオン $Q$ を更新することで得ることができます。

$$
Q_{k+1}=
\frac{
\Delta q_k \otimes Q_k
}{
\lVert
\Delta q_k \otimes Q_k
\rVert
}
$$

これはKarelだと

Q = quat_mul(delta_Q, Q)
quat_normal(Q)

です。

quat_mul()はクォータニオン積。
quat_normal()はクォータニオンの正規化として実装しました。

今回の用途では最大反復数を制限しています。

産業用ロボットでは、速さだけでなく

必ず有限時間で処理が終わる

ことも大切なので、反復回数には上限を設けています。

また計算資源の節約のため、torqueが規定値以下になっても反復を終えます。

回転が求まったら平行移動量を求める

回転 $Q$ が求まったら、最初に保存していた測定点群Aの重心を回転します。

org_a = quat_rotv(Q, org_a)

そして、向きが揃ったので

trans.x = org_b.x - org_a.x
trans.y = org_b.y - org_a.y
trans.z = org_b.z - org_a.z

とすれば平行移動量も求まります。

式で書けば、

$$
\mathbf t=\bar{\mathbf b}-R\bar{\mathbf a}
$$

です。

これで、

$$R,\mathbf t$$

の両方が揃いました。

最後にRMSEを計算する

位置姿勢が求まっただけでは、それを信用してよいか分かりません。

そこで変換後のAとBの残差からRMSEも出しています。

$$
RMSE=
\sqrt{
\frac{1}{N}
\sum_i
\lVert
\mathbf a_i-\mathbf b_i
\rVert^2
}
$$

KARELでも普通に、

FOR i = 1 TO num_point DO

    dx = A[i].x - B[i].x
    dy = A[i].y - B[i].y
    dz = A[i].z - B[i].z

    rmse = rmse + dx * dx + dy * dy + dz * dz

ENDFOR

rmse = SQRT(rmse / num_point)

です。

最終的に、

  • 推定位置姿勢 → 位置レジスタ
  • RMSE → レジスタ

へ書き戻します。

つまりTP側から見ると、

イチレジ[n] : 推定されたXYZWPR
レジ[n]  : 推定結果のRMSE

だけを見ればよい構成です。

なぜPCを使わずKARELでやったのか

ここまで読んで、

いやPCでやればよくない？

と思った人。

正しいです。

PCが自由に使えて、Ethernet通信も許されていて、PythonやC++の実行環境を保守できるなら、私はそちらを勧めます。

当然、計算精度、計算速度ともPCには劣ります。

今回、KARELへ詰め込んだ理由は

  • 保守対象になる設備を増やしたくない
  • 通信相手を増やしたくない
  • 工場の隅っこで馴染みのないC++やPythonなどの実行環境の保守が難しい
  • PCだってコスト増や故障の原因
  • 数点程度しか扱わない
  • 数msを競う処理でもない

という条件があります。

その場合、

ロボットが自分で測って、自分で計算して、自分のレジスタへ結果を書く

という構成はかなり扱いやすくなります。

2Dカメラと距離センサでも3D点は作れる

今回のもう一つの狙いはこちらです。

高価な3Dセンサがなくても、

2Dカメラ
    +
距離センサ
    +
ロボット自身の位置

を組み合わせれば3次元点を取得できます。

さらにロボットを動かせば、センサそのものを複数視点へ移動できます。

固定された一台のセンサですべてを見るのではなく、

ロボットそのものを測定機として使う

考え方です。

もちろん専用3Dカメラほど高速、高精度ではありません。

しかし、

  • 数点だけ測ればよい
  • サイクルタイムに余裕がある
  • 既存のロボットを利用できる
  • センサ価格を抑えたい

という条件では成立する場合があります。

この初期版の弱点

ここまで紹介したものは最初に作った版です。

最大の問題は、

すべての測定方向を同じように信用している

ことです。

例えば実際の設備では、

2Dカメラ

X : 精度が良い
Y : 精度が良い
Z : 苦手

だったり、

距離センサ

距離方向 : 精度が良い
横方向   : 情報を持っていない

ロボットの位置決め能力

X : バックラッシュがでやすい
Y : バックラッシュが出にくい
Z : 重力に引っ張られる

だったりします。

しかし、この初期版では、

torque = torque + (a_n # b_n)

としかしていません。

各点・各方向に対する誤差共分散も持たせていません。

つまり、

誤差の方向性を考慮しない、等方的な扱い

です。

シミュレーション上でXYZすべてに同程度のノイズを与える場合なら扱いやすいのですが、実センサはそう綺麗にはでません。

この問題は、その後かなり手を入れることになりました。

そして魔改造が始まる

実際のセンサを考えると、

$$
\sigma_x = \sigma_y = \sigma_z
$$

とは限りません。

むしろ、

$$
\sigma_x
\neq
\sigma_y
\neq
\sigma_z
$$

の方が普通です。

また今回の実装では計算を単純化するため、各対応ベクトルを正規化してから外積を求めています。

そのため、重心から各測定点までの距離情報はtorqueの大きさに反映されません。

例えば、重心から遠い測定点は本来わずかな角度変化でも大きな位置変化として観測できるため、姿勢推定には有利です。
一方で重心に近い測定点は、同じ測定誤差でも角度誤差への影響が大きくなります。

しかし初期版では、各ベクトルを正規化しているため、その距離情報を意図的に捨て、方向だけを同じ重みで扱っています。

そのため、測定点をどこに配置するかによって推定精度が大きく左右されます。

そこで現在は、

  • センサごとの得意・不得意方向
  • XYZそれぞれの観測精度
  • 回転軸ごとの信頼度
  • 点ごとの重み

などを考慮する方向へ拡張しています。

最初は、

外積を全部足せば回るじゃん

だったものが、

待てよ

このカメラ、
Z方向はそんなに信用できないぞ
計測点がどうしても左右非対称になっちゃう

となり、

じゃあ方向ごと、点ごとの信頼度を
トルクへ入れよう
でも遅いのはいや！！

という流れです。

このあたりは別の記事にしようと思います。

まとめ

今回やったことをまとめると、

FANUCロボット
      ↓
2Dカメラ・距離センサで複数点測定
      ↓
計測結果を位置レジスタへ格納
      ↓
重心を除去
      ↓
対応ベクトルの外積を加算
      ↓
微小回転を求める
      ↓
回転クォータニオンで点群を少し回す
      ↓
指定回数か、torqueが小さくなるまで反復
      ↓
回転＋平行移動を推定
      ↓
推定結果を位置レジスタへ出力
RMSEをレジスタへ出力

となります。

必要だった数学は主に、

  • ベクトル
  • 外積
  • 正規化
  • クォータニオン
  • RMSE

です。

SVD、固有値分解を使っていません。

高性能なPCも必要ありません。

手元のR-30iBでは、4点・今回の反復条件で1秒未満でした。

またPC上のSVDによる解と比較すると、今回試した条件ではおおむね小数点以下4〜5桁目付近から差が現れました。

最初から専用の3Dセンサを買えば、こんなことをしなくても済みます。

しかし工場設備では、

技術的にできること

と、

予算的にできること

が一致するとは限りません。

そこで、

安価なセンサと既存のロボットを、アルゴリズムで少し賢くする。

という寄せ集め軍団があってもよいのではないかと思います。

参考にした資料

0
1
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
1

Delete article

Deleted articles cannot be recovered.

Draft of this article would be also deleted.

Are you sure you want to delete this article?