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

3.3K Views

July 24, 22

スライド概要

profile-image

■ドローンやロボットを自作することを通じて制御や関連技術の生涯勉強情報を提供■工学博士■防大航空宇宙→筑波大博士■陸自→対戦車誘導弾等の装備品開発→高専教員→大学教員■ロボットランサー優勝→マイクロマウスニューテクノロジー賞受賞■指導者としてつくばチャレンジバンナム賞→飛行ロボコンマルチコプタ部門1位等々■北海道函館出身

シェア

またはPlayer版

埋め込む »CMSなどでJSが使えない場合

関連スライド

各ページのテキスト
1.

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

2.

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

3.

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

4.

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

5.

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

6.

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

7.

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

8.

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

9.

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

10.

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

11.

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

12.

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

13.

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

14.

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

15.

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

16.

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

17.

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

18.

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

19.

付録︓⼆乗の総和の偏微分(回転成分) $ 𝐽=) !"# 𝑑𝐽 𝑑𝜃 $ =) +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