つぶやきロボット工学（点群の重ね合わせ）

最⼩⼆乗法に よる点群の 重ね合わせ つぶやきロボット⼯学 1

最⼩⼆乗法による点群の重ね合わせ ２つの点群データを重ねる 2

最⼩⼆乗法による点群の重ね合わせ ２つの点群データを重ねる 点群データには誤差が 含まれ完全に⼀致する ことは無い 3

最⼩⼆乗法による点群の重ね合わせ 回転移動量と平⾏移動量を求める 𝜃回転し 4

最⼩⼆乗法による点群の重ね合わせ 回転移動量と平⾏移動量を求める Δ𝑥, Δ𝑦平⾏移動 Δ𝑦 Δ𝑥 5

最⼩⼆乗法による点群の重ね合わせ 点同⼠の対応は既知とする 𝑃! 𝑃" 𝑃# 𝑃$ 𝑄# 𝑃% 𝑄! 𝑄" 𝑄$ 𝑄% 6

最⼩⼆乗法による点群の重ね合わせ 重ね合わせの試み 𝑃! 𝑄!& 点とその座標について 𝑃" 𝑃# 𝑃$ 𝑄 & # & 𝑄" 𝑄$& ⽬標点群𝑃! (𝑥! , 𝑦! ) 𝑄! (𝑥!" , 𝑦!" ) 中間点群𝑄!" (𝑥!"" , 𝑦!"" ) 𝑃% Ｑ点群のＰ点群への重ね 合わせを試みる 回転 ＋平 ⾏移 動 𝑄# 𝑄" & 𝑄% 𝑄! 𝑄$ 𝑄% 7

最⼩⼆乗法による点群の重ね合わせ 最⼩⼆乗法 𝑃! (𝑥! , 𝑦! ) 対応する２点の距離の ２乗の総和が最⼩になるように 回転と平⾏移動量を求める 𝑃! & 𝑄! 𝑄!" (𝑥!"" , 𝑦!"" ) 𝑃" 𝑄"& 𝑃# 𝑃$ 𝑄 & # 𝑄$& 𝑃% & 𝑄% 8

最⼩⼆乗法による点群の重ね合わせ 最⼩⼆乗法による解法とその⼿順 ① 点𝑄! を𝜃回転し、∆𝑥、∆𝑦平⾏移動した点𝑄!" の式表現を求める ② 距離 𝑃𝑄" を求める ③ 距離 𝑃𝑄" の２乗の総和Ｊを最⼩にする𝜃 、 ∆𝑥、∆𝑦を⾒つける （最⼩⼆乗法と同じことをする） ④ Ｊの最⼩値を求めるため𝜃 、 ∆𝑥、∆𝑦でそれぞれ偏微分した式を ０とする３式からなる連⽴⽅程式を⽴てる ⑤ 連⽴⽅程式を解いて𝜃 、 ∆𝑥、∆𝑦を求める 9

最⼩⼆乗法による点群の重ね合わせ ① Ｑ点群の回転と平⾏移動 ! 𝑸 = 𝑹𝑸 + 𝒕 )) 𝑥( )) 𝑦( ) 𝑥( ) 𝑦( ∆𝑥 cos 𝜃 − sin 𝜃 = + ∆𝑦 sin 𝜃 cos 𝜃 ) ) 𝑥( cos 𝜃 − 𝑦( sin 𝜃 + ∆𝑥 = ) ) 𝑥( sin 𝜃 + 𝑦( cos 𝜃 + ∆𝑦 Q点群とQʼ点群との対応は以上のようになる 10

最⼩⼆乗法による点群の重ね合わせ ② 点と点の距離 𝑃! (𝑥! , 𝑦! ) 𝑃" 𝑃! 𝑄!& 点𝑃! と点𝑄!" との距離 𝑃! 𝑄!" は 𝑃! 𝑄!" = = 𝑥!"" − 𝑥! # + 𝑦!"" − 𝑦! 𝑃# 𝑄"& 𝑄!" (𝑥!"" , 𝑦!"" ) # 𝑥!" cos 𝜃 − 𝑦!" sin 𝜃 + ∆𝑥 − 𝑥! 𝑃$ 𝑄 & # 𝑄$& 𝑃% & 𝑄% # + 𝑥!" sin 𝜃 + 𝑦!" cos 𝜃 + ∆𝑦 − 𝑦! # 11

最⼩⼆乗法による点群の重ね合わせ ③ 2乗の総和 % 𝐽=, 𝑃! 𝑄! !#$ % =, & { 𝑥!" cos 𝜃 − 𝑦!" sin 𝜃 + Δ𝑥 − 𝑥! !#$ + 𝑥!" & sin 𝜃 + 𝑦!" cos 𝜃 + Δ𝑦 − 𝑦! & } 12

最⼩⼆乗法による点群の重ね合わせ ④２乗の総和の最⼩値 • ２乗の総和𝐽を最⼩にする∆𝑥， ∆𝑦，𝜃を求めるため∆𝑥， ∆𝑦，𝜃 で𝐽を編微分し、それぞれを0とする連⽴⽅程式をつくる。 • 連⽴⽅程式を解き𝐽を最⼩にする∆𝑥， ∆𝑦，𝜃を求める • それらの値が点群の重ね合わせを実現する。 𝐽を編微分 するとこう なります。 $ 𝑑𝐽 = ) 2 𝑥!% cos 𝜃 − 𝑦!% sin 𝜃 + Δ𝑥 − 𝑥! 𝑑∆𝑥 !"# ・・・・（１） $ 𝑑𝐽 = ) 2 𝑥!% sin 𝜃 + 𝑦!% cos 𝜃 + Δ𝑦 − 𝑦! 𝑑∆𝑦 !"# ・・・ ・（２） $ 𝑑𝐽 = ) 2 −Δ𝑥𝑥!% − Δ𝑦𝑦!% + 𝑥! 𝑥!% + 𝑦! 𝑦!% sin 𝜃 + −Δ𝑥𝑦!% + Δ𝑦𝑥!% + 𝑥! 𝑦!% − 𝑥!% 𝑦! cos 𝜃 𝑑𝜃 !"# ・・（３） 13

最⼩⼆乗法による点群の重ね合わせ ④２乗の総和の最⼩値 • ２乗の総和𝐽を最⼩にする∆𝑥， ∆𝑦，𝜃を求めるため∆𝑥， ∆𝑦，𝜃 で𝐽を編微分し、それぞれを0とする連⽴⽅程式をつくる。 • 連⽴⽅程式を解き𝐽を最⼩にする∆𝑥， ∆𝑦，𝜃を求める • それらの値が点群の重ね合わせを実現する。 𝑁∆𝑥 − & 𝑥! + cos 𝜃 & 𝑥!" − sin 𝜃 & 𝑦!" = 0 ・・・・（１） 𝑁∆𝑦 − & 𝑦! + sin 𝜃 & 𝑥!" + cos 𝜃 & 𝑦!" = 0 ・・・ ・（２） −Δ𝑥 & 𝑥!" − Δ𝑦 & 𝑦!" + & 𝑥! 𝑥!" + & 𝑦! 𝑦!" sin 𝜃 + −Δ𝑥 & 𝑦!" + ∆𝑦 & 𝑥!" + & 𝑥! 𝑦!" − & 𝑥!" 𝑦! cos 𝜃 = 0 ・・（３） 14

最⼩⼆乗法による点群の重ね合わせ (1)式(2)式から ∑ 𝑥! ∑ 𝑥!" ∑ 𝑦!" ∆𝑥 = − cos 𝜃 + sin 𝜃 𝑁 𝑁 𝑁 ∑ 𝑦! ∑ 𝑥!" ∑ 𝑦!" ∆𝑦 = − sin 𝜃 − cos 𝜃 𝑁 𝑁 𝑁 これらを(3)式に代⼊して整理すると " " ∑ ∑ 𝑥! ∑ 𝑥!" ∑ 𝑦! ∑ 𝑦!" ∑ ∑ ∑ 𝑥 𝑦 𝑥 𝑦! ! ! ! − − + & 𝑥! 𝑥!" + & 𝑦! 𝑦!" sin 𝜃 + − + + & 𝑥! 𝑦!" − & 𝑥!" 𝑦! cos 𝜃 𝑁 𝑁 𝑁 𝑁 =0 上式の⾊がついた部分を記号で置き換えます ∑ 𝑥! ∑ 𝑥!" ∑ 𝑦! ∑ 𝑦!" 𝐴=− − + & 𝑥! 𝑥!" + & 𝑦! 𝑦!" 𝑁 𝑁 ∑ 𝑥! ∑ 𝑦!" ∑ 𝑥!" ∑ 𝑦! 𝐵=− + + & 𝑥! 𝑦!" − & 𝑥!" 𝑦! 𝑁 𝑁 すると、以下のように簡単に表記できます 𝐴 sin 𝜃 + 𝐵 cos 𝜃 = 0 15

最⼩⼆乗法による点群の重ね合わせ 得られた以下の式を解いて𝜃を求める。 𝐴 sin 𝜃 + 𝐵 cos 𝜃 = 0 のグラフを描いてみると より、三⾓関数の合成を⽤いると 𝐴5＋𝐵5sin 𝜃 + 𝜙 = 0 ここで cos 𝜙 = sin 𝜙 = 従って 𝐴 𝐴5＋𝐵5 𝐵 𝐴5＋𝐵5 𝐵 67 𝜙 = tan 𝐴 ここで、ある場合のJの𝜃での微分 𝐽8 𝜃 = 𝐴 sin 𝜃 + 𝐵 cos 𝜃 交点の𝜃が⼆つ求められるが、⼀⽅が極⼩値と なる値となりそちらが答えである。 図のように位相遅れの𝜙を⽤いると、求める𝜃 の値は 𝜃 = −𝜙 となる事が、わかる 16

最⼩⼆乗法による点群の重ね合わせ 点群の重ね合わせ 回転量 𝜃= − tan67 ここで 𝐵 𝐴 点群 𝑥'& , 𝑦'& を 点群 𝑥' , 𝑦' に 重ね合わせる ∑ 𝑥! ∑ 𝑥!" ∑ 𝑦! ∑ 𝑦!" 𝐴=− − + & 𝑥! 𝑥!" + & 𝑦! 𝑦!" 𝑁 𝑁 ∑ 𝑥! ∑ 𝑦!" ∑ 𝑥!" ∑ 𝑦! 𝐵=− + + & 𝑥! 𝑦!" − & 𝑥!" 𝑦! 𝑁 𝑁 平⾏移動量 ∑ 𝑥9 ∑ 𝑥98 ∑ 𝑦98 ∆𝑥 = − cos 𝜃 + sin 𝜃 𝑁 𝑁 𝑁 ∑ 𝑦9 ∑ 𝑥98 ∑ 𝑦98 ∆𝑦 = − sin 𝜃 − cos 𝜃 𝑁 𝑁 𝑁 17

付録︓⼆乗の総和の偏微分（平⾏移動成分） $ 𝑥!% cos 𝜃 − 𝑦!% sin 𝜃 + Δ𝑥 − 𝑥! 𝐽=) !"# & + 𝑥!% sin 𝜃 + 𝑦!% cos 𝜃 + Δ𝑦 − 𝑦! & $ 𝑑𝐽 = ) 2 𝑥!% cos 𝜃 − 𝑦!% sin 𝜃 + Δ𝑥 − 𝑥! = 0 𝑑∆𝑥 !"# $ ) !"# 𝑥!% cos 𝜃 − 𝑦!% sin 𝜃 + Δ𝑥 − 𝑥! = 0 𝑁∆𝑥 − ) 𝑥! + cos 𝜃 ) 𝑥!% − sin 𝜃 ) 𝑦!% = 0 同様にして 𝑁∆𝑦 − ) 𝑦! + sin 𝜃 ) 𝑥!% + cos 𝜃 ) 𝑦!% = 0 18

付録︓⼆乗の総和の偏微分（回転成分） $ 𝐽=) !"# 𝑑𝐽 𝑑𝜃 $ =) +2 & + 𝑥!% sin 𝜃 + 𝑦!% cos 𝜃 + Δ𝑦 − 𝑦! & {2 𝑥!% cos 𝜃 − 𝑦!% sin 𝜃 + Δ𝑥 − 𝑥! (−𝑥!% sin 𝜃 − 𝑦!% cos 𝜃) !"# 𝑥!% sin 𝜃 $ =) 𝑥!% cos 𝜃 − 𝑦!% sin 𝜃 + Δ𝑥 − 𝑥! + 𝑦!% cos 𝜃 + Δ𝑦 − 𝑦! (𝑥!% cos 𝜃 − 𝑦!% sin 𝜃)} 2 −Δ𝑥𝑥!% − Δ𝑦𝑦!% + 𝑥! 𝑥!% + 𝑦! 𝑦!% sin 𝜃 + −Δ𝑥𝑦!% + Δ𝑦𝑥!% + 𝑥! 𝑦!% − 𝑥!% 𝑦! cos 𝜃 !"# 0とすると $ ) !"# −Δ𝑥𝑥!% − Δ𝑦𝑦!% + 𝑥! 𝑥!% + 𝑦! 𝑦!% sin 𝜃 + −Δ𝑥𝑦!% + Δ𝑦𝑥!% + 𝑥! 𝑦!% − 𝑥!% 𝑦! cos 𝜃 = 0 Δ𝑥 & 𝑥!" + Δ𝑦 & 𝑦!" − & 𝑥! 𝑥!" − & 𝑦! 𝑦!" sin 𝜃 + Δ𝑥 & 𝑦!" − ∆𝑦 & 𝑥!" − & 𝑥! 𝑦!" + & 𝑥!" 𝑦! cos 𝜃 = 0 19