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?

@carp1046(1046 carp)

FANUC KARELで3D姿勢推定その2 ― 異方性誤差を軽量に扱う

0
Posted at

FANUC KARELで3D姿勢推定その2 ― 異方性誤差を軽量に扱う

はじめに

前回の記事では、FANUCロボット、2Dカメラ、距離センサなどから得た複数の3D点を使い、SVDや固有値分解を使わずに位置姿勢を推定する方法を試しました。

前回はこちら。

対応するベクトル同士の外積を足し合わせ、その結果を疑似的なトルクとして少しずつクォータニオンへ反映する方法です。

大まかには、

点群A
   ↓
重心を原点へ移動
   ↓
各ベクトルを正規化
   ↓
A × B を加算
   ↓
Torque方向へ少し回転
   ↓
繰り返す

という処理でした。

思ったより普通に動いたのですが、実際の設備へ持っていこうとすると一つ問題があります。

センサのXYZを全部同じ精度として扱ってよいわけではありません。

今回は、この「方向によって測定精度が違う」という異方性誤差を、FANUC KAREL上であまり重くならない形で扱ってみます。

2DカメラのZは信用できない

例えば2Dカメラで位置を計測するとします。

画像平面上の位置は比較的正確に求められても、カメラの光軸方向について同じ精度があるとは限りません。

イメージとしては、

          Z
          ↑
          │   ← この方向は弱い
          │
          ●────→ X
         /
        /
       Y

       XY方向は比較的強い

という状態です。

距離センサなら逆です。

レーザ距離計などでは、

             測定方向
                ↓
センサ  ───────────→ ●
                ↑
          この方向は強い

ですが、その直交方向の位置を測定しているわけではありません。

つまり測定誤差は、

$\sigma_x=\sigma_y=\sigma_z$

ではなく、

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

です。

こういう方向依存の誤差を異方性誤差として扱います。

真面目にやると重い

異方性誤差をきちんと扱うなら、各計測点について例えば、

\Sigma_i =
\begin{pmatrix}
\sigma_x^2 & * & * \\
* & \sigma_y^2 & * \\
* & * & \sigma_z^2
\end{pmatrix}

のような誤差共分散行列を持ち、

その逆行列を使って、

この方向の誤差は信用する
この方向の誤差は信用しない

という計算を行う方法が考えられます。

PC上なら別に困りません。

しかし、今回の計算場所はFANUCのKARELです。

欲しいのは、

  • 逆行列
  • 共分散行列
  • 大量の行列積

ではありません。

できれば、

  • 足し算
  • 掛け算
  • 外積
  • 平方根

くらいで済ませたいところです。

そして計算はロボットの動作中にも行いたいので、重たい処理を毎回Torqueの反復ループへ入れるのも避けたいです。

そこで、異方性誤差を全部Torque法内部で解決するのをやめました。

異方性誤差を2種類に分ける

今回、誤差を次の2種類に分けて考えました。

異方性誤差
│
├─ 方向による信頼度
│
│   2DカメラのZ方向
│   距離センサの直交方向
│   など
│
└─ 点そのものの信頼度

    センサ精度
    点の配置
    重心からの距離
    測定条件
    など

そして処理する場所も分けます。

方向による信頼度
       ↓
計測時・前処理側

点ごとの信頼度
       ↓
Torque側の w_coef

これでTorque本体をほとんど変更せずに済みます。

方向による誤差は先に落とす

例えば2Dカメラなら、カメラが信頼できる平面へ観測結果を投影します。

単純なXY平面なら、

$(x,y,z)$

を、

$(x,y,0)$

として扱うイメージです。

実際にはカメラ座標系や基準面に合わせて投影するので、

$\mathbf p' = P\mathbf p$

のような処理になります。

これをTorque反復中に毎回行う必要はありません。

計測したときに一度だけ処理しておけば、

2Dカメラ
    ↓
XYZ計測値
    ↓
信用しない方向を除去
    ↓
Torque用点群へ登録

で終わります。

距離センサでも同じです。

センサが観測している方向だけを使って位置を更新し、それ以外の成分については基準位置を使用します。

つまり、

観測できていない値を、観測した値としてTorqueへ渡さない

というだけです。

これだけでもかなり扱いやすくなります。

点ごとの信頼度だけTorqueへ渡す

方向依存の問題を先に処理したら、Torque側へ渡す重みはスカラー1個にします。

現在のKAREL版では、

w_coef :ARRAY[max_points] OF REAL

として各点に1個ずつ重みを持たせています。

Torque計算本体は、

FOR i = 1 TO num_point DO
    a_n = A[i]
    b_n = B[i]

    normalized(a_n)
    normalized(b_n)

    torque = torque + w_coef[i] * (a_n # b_n)
ENDFOR

です。

数式にすると、

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

です。

前回とほとんど変わっていません。

違うのは、

$w_i$

が付いただけです。

なぜ重心から遠い点を重くするのか

前回のTorque法では、各ベクトルを正規化していました。

つまり、

$\hat{\mathbf a}=\frac{\mathbf a}{|\mathbf a|}$

です。

これには利点があります。

点が重心から100mm離れていても1000mm離れていても、ベクトルの長さによってTorqueが極端に変化しなくなります。

一方で欠点もあります。

姿勢推定では一般に、

回転中心から遠い点ほど、角度変化による位置変化が大きくなります。

ところが全部正規化すると、

重心から  50 mm の点
重心から 500 mm の点

が同じ長さのベクトルとしてTorqueへ入ります。

せっかく500mm離れた場所を測ったのに、その情報量を捨てています。

そこで、正規化前の距離を重みとして残します。

現在の前処理では、

w_coef[i] =
        b[i].x * b[i].x +
        b[i].y * b[i].y +
        b[i].z * b[i].z

として、基準点Bの重心からの距離を取得しています。

センサの信頼度も掛ける

さらに、点ごとに使用しているセンサの信頼度を掛けます。

現在の実装では概念的に、

w_coef[i] = distance_weight

w_coef[i] =
    sensor_alpha *
    w_coef[i]

としています。

つまり、

$w_i=d_is_i$

です。

ここで、

  • $(d_i)$：重心からの距離
  • $(s_i)$：センサ信頼度

です。

例えば、

2Dカメラの特徴点       1.0
距離センサ             0.8
少し不安定な計測点     0.5

のように設定できます。

この数値自体に絶対的な意味を持たせる必要はありません。

最終的にはすべて正規化するからです。

重みの総和を1にする

前処理の最後で、

sum_weight = sum_weight + w_coef[i]

として全重みを加算します。

その後、

FOR i = 1 TO num_point DO
    w_coef[i] = w_coef[i] / sum_weight
ENDFOR

として、

$\sum_i w_i = 1$

になるよう正規化します。

するとTorqueは、

\boldsymbol{\tau}
=
\sum_i
w_i
(\hat{\mathbf a}_i\times\hat{\mathbf b}_i)

という重み付き平均に近い形になります。

点数が増えてもTorqueのスケールが大きく変化しにくくなります。

ここでalphaを点数で割らない

初期版ではTorqueの更新係数を、

alpha = alpha0 / num_point

としていました。

各点の外積を単純加算していたため、点数が増えるほどTorqueが大きくなるのを抑える目的でした。

しかし現在は、

$\sum_i w_i = 1$

です。

つまり重み付け側ですでに平均化されています。

そこで、

angle = alpha0 * angle

としています。

これで、

点数を増やしたら急に収束速度が変わった

ということも起こりにくくなります。

固定データは暇なときに計算する

今回は前処理を別プログラムにしています。

前処理プログラムでは、

  • 基準点群Bを取得
  • 重心計算
  • 原点移動
  • 重心からの距離を計算
  • センサ信頼度を掛ける
  • 重みを正規化

まで行います。

一方、実際に計測した後に動かすプログラムでは、

  • 計測点Aの取得
  • 重心移動
  • 反復計算
  • クォータニオン更新
  • 平行移動推定
  • RMSE計算

だけを行います。

つまり、

設備待機中
   ↓
trq_ref_map
   ↓
基準点・重みを準備

---------------------

計測完了
   ↓
torque_align
   ↓
姿勢推定

としました。

基準点やセンサ信頼度は毎回かわりません。

ならロボットが暇なときに計算しておけばよい、という考えです。

古いコントローラでは、この手の事前計算が案外効きます。

Torque本体はほとんど変わらない

結果として、異方性誤差へ対応したからといってTorque本体が大きくなったわけではありません。

中心部分は今でも、

REPEAT

    torque.x = 0
    torque.y = 0
    torque.z = 0

    FOR i = 1 TO num_point DO

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

        normalized(a_n)
        normalized(b_n)

        torque =
            torque +
            w_coef[i] * (a_n # b_n)

    ENDFOR

    angle = vec_norm(torque)

    angle =
        alpha0 *
        angle *
        to_degree

    IF angle > eps THEN

        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)

        quat_normal(delta_Q)

        Q = quat_mul(delta_Q, Q)

        quat_normal(Q)

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

    ENDIF

UNTIL
    (n >= num_repeat)
    OR
    (angle < eps)

です。

異方性誤差を扱うために、

3×3行列
逆行列
共分散行列
固有値分解

などは追加していません。

厳密な異方性最小二乗ではない

ここは重要なので書いておきます。

今回の方法は、

異方性誤差を統計的に厳密な最適化しているわけではありません。

完全にやるなら、方向ごとの誤差共分散を持ち、それを姿勢推定の目的関数へ入れる方法が必要です。

今回やっているのは、

方向ごとの信頼度
       ↓
計測時に整理

点ごとの信頼度
       ↓
スカラー重み w_coef

その後
       ↓
従来のTorque法

という簡略化です。

なので、

異方性誤差対応Torque法

というより、

異方性誤差を前処理してから、重み付きTorque法へ渡す

と言った方が正確です。

なぜこの形にしたのか

今回の目的は、

最高精度の姿勢推定アルゴリズムを作ることではありません。

そもそも使っているのが、

  • 2Dカメラ
  • 距離センサ
  • ロボット
  • ロボット本体によるタッチアップした時の位置データ

などです。

高精度な3D点群が必要なら、最初から専用の3Dセンサを使った方がよいでしょう。

今回欲しいのは、

設備にすでに存在する安価なセンサを寄せ集めて、教示位置を少し補正
高価な3Dセンサを使うことを時間的に躊躇うような治具やワークの段替え時の簡易位置計測
そもそも最新設備がつけられないような旧型ロボットに安価なセンサを取り付けて計測作業

のような簡易的なものを考えています。

そのためにPCを常設したり、巨大なライブラリを用意したりするのも少し大げさです。

だったら、

分かる方向だけ使う

分からない方向は捨てる

信頼できる点を少し重くする

外積を足す

少し回す

くらいでもよいのではないか、と考えました。

実際には前処理の方が重要かもしれない

今回いろいろ試していて思ったのですが、

姿勢推定アルゴリズムそのものより、

センサから何を観測値として取り出すか

の方が重要かもしれません。

2Dカメラに存在しないZ精度を無理に持たせても、計算が賢くなるわけではありません。

距離センサにXY位置を期待しても同じです。

それぞれ、

2Dカメラ
→ 平面内の位置

距離センサ
→ 測定軸方向

ロボットTCP
→ 接触点

治具
→ 幾何拘束

という得意分野があります。

それらを前処理で一度整理してから、姿勢推定側にはなるべく単純なデータを渡す。

今回のKAREL版ではこの構成が一番扱いやすくなりました。

おわりに

前回作ったTorque法は、

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

というかなり単純なものでした。

今回はそこへ、

$w_i$

を追加して、

\boldsymbol{\tau}
=
\sum_i
w_i
(
\hat{\mathbf a}_i
\times
\hat{\mathbf b}_i
)

としました。

ただし、本当に重要だったのはこの式よりも、

異方性誤差をどこで処理するか

だった気がします。

方向依存の誤差は計測時に処理する。

点ごとの信頼性はスカラー重みにする。

固定できる重みはロボットが暇なときに計算する。

Torque反復そのものは極力単純なまま残す。

結果として、FANUC KARELでも無理なく扱える程度に収まりました。

PCで計算するなら、もっと高度な方法はいくらでもあります。

でも工場では、

今そこにあるセンサとロボットだけで、あと少し賢くしたい

という場面も結構あります。

そういうときには、この程度の割り切り方も使えるのではないかと思います。

次はPython側とKAREL側を両方単精度条件に揃えて、SVD/Kabschなどと比較してみる予定です。

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?