โครงรางวทยานพนธ (THESIS PROPOSAL)

ชอเรอง (ภาษาไทย) การระบตาแหนงและสรางแผนทในสามมตดวยกลองวดโอแบบออมน

ชอเรอง (ภาษาองกฤษ) 3D SLAM for Omni-directional Camera

เสนอโดย นายยทธนา สทธสภา

รหสนสต 5070413421

หลกสตร วศวกรรมศาสตรมหาบณฑต

ภาควชา วศวกรรมคอมพวเตอร

คณะ วศวกรรมศาสตร จฬาลงกรณมหาวทยาลย

สถานทตดตอ ภาควชาคอมพวเตอร วศวกรรมศาสตร จฬาลงกรณมหาวทยาลย

โทรศพท 08-6916-2001

อเมล Yuttana.S@student.chula.ac.th

อาจารยทปรกษา ผศ. ดร.อรรถวทย สดแสง คาสาคญ (ภาษาไทย) กลองออมน, วธการระบตาแหนงและสรางแผนท

คาสาคญ (ภาษาองกฤษ) Omni-directional Camera, Simultaneous localization and mapping

(SLAM)

การระบตำแหนงและสรางแผนทในสามมตดวยกลองออมน(3D SLAM for Omni-directional Camera)

นายยทธนา สทธสภา

11 ธนวาคม 2551

1 ทมาและความสำคญของปญหา

การสรางแผนทของหนยนต คอการทหนยนตสามารถอธบายสภาพแวดลอม ในบรเวณทหนยนตสนใจได โดยหนยนตอาจจะอธบายสงแวดลอม ดวยตำแหนงของจดสำคญตาง ๆ ของสงแวดลอม หรออาจจะอธบายไดเปน โครงสรางความสมพนธของสงแวดลอมกเปนได สวนการระบตำแหนงของหนยนต คอการท

หนยนต สามารถระบตำแหนงของตนเองในแผนทได โดยแผนทนนอาจจะเปนแผนททกำหนดไวแลว หรอ

อาจจะเปนแผนท ทหนยนตสรางขนเอง ซงในการใชงานหนยนตอตโนมตนน ปญหาการระบตำแหนง และการสรางแผนทของหนยนต มความสำคญสำหรบหนยนตเปนอยางมาก เนองจากวาหนยนต จำเปนตองใชขอมลแผนท ประกอบกบขอมลตำแหนงของหนยนตในการวางแผน เพอกระทำกจกรรมตาง ๆ ตอบสนอง

ตามสถาพแวดลอมนน อาทเชน การเคลอนทสำหรบหนยนตสำรวจอตโนมต, หนยนตกภย, หนยนตแมบานหรอ แมแตการหยบจบสงของ หนยนตกจำเปนจะตองรตำแหนงของ สงของทจะหยบจบ และตำแหนงของมอตนเองดวย

วธการระบตำแหนงพรอมกบการสรางแผนท (Simultaneous Localization and Mapping: SLAM) [1] เปนกระบวนการทหนยนตจะสรางแผนทของสภาพแวดลอมในขณะทกำลงเคลอนท และระบตำแหนงของตวเอง

ไปพรอม ๆ กน โดยทหนยนตนนไมมขอมลของสงแวดลอมมากอน ซงปญหาของ SLAM นนมความสำคญ

เปนอยางมากสำหรบหนยนตทตองการการโตตอบแบบทนการณ โดย SLAM นนจะอธบาย ตำแหนงของ

หนยนต และ แผนท ในรปของความนาจะเปน ทขนอยกบขอมลการวดจากเซนเซอร

สำหรบวธการระบตำแหนงพรอมกบการสรางแผนท (SLAM) นน มการประยกตใชงานกบเซนเซอรหลายรปแบบ อาทเชน กลองวดโอ, โซนาร (Sonar) หรอ อปกรณวดระยะดวยเลเซอร (Laser Range Finder) ซงเซนเซอรแตละชนดกจะม วธการใชงานและขอดขอเสย ทแตกตางกน เชน

กลองวดโอนน จะใหขอมลของสงแวดลอมเปนปรมาณมาก (ขอมลเปนภาพ) มความแมนยำของขอมล

พอประมาณ ขนกบความละเอยดของกลอง มการตอบสนองตอขอมลสงแวดลอมเรวทนการณ (real-time)

ราคาไมสงนก พกพาสะดวก แต การนำขอมลมาใชนนมความยงยาก ใชเวลาในการประมวลผลนาน และยงตองอาศยแสงจากภายนอกเพอชวยในการทำงานในขณะรบภาพ จงทำใหการเปลยนแปลงของแสงจากสภาพแวดลอม สงผลกระทบตอการทำงานของกลองไดงาย และกลองยงไมอาจทำงานไดในสภาพแวดลอมทมแสงนอยจนเกนไป

โซนารเปนเซนเซอรซงวดระยะทางโดยอาศยการจบเวลาของเลยงสะทอน มจดเดนในดานราคาทไมสงมาก และความสะดวกในการใชงาน แตมปญหาในเรองความแมนยำ และถกรบกวนไดงายจากปจจยภายนอกเชนผวสะทอนทแตกตางกน หรอ อณหภมของอากาศ

อปกรณวดระยะดวยเลเซอร เปนอปกรณวดระยะทใชหลกการสะทอนของแสงซงถกรบกวนไดยากกวาเสยง จงเปนสาเหตใหเลเซอรมความแมนยำ และความเรวในการวดคาสงกวาโซนาร แต กจดวาเปนอปกรณทมราคาสงชนดหนง

เมอไมนานมาน การใชงานกลองออมน เปนเซนเซอรสำหรบงานระบตำแหนงพรอมกบการสรางแผนท

เรมมการแพรหลายมากขน [2, 3, 4] ซงจดเดนของกลองออมนอยทความสามารถในการมองเหนรอบทศทางจงสามารถตรวจหาสงแวดลอมไดโดยงาย และยงมขอดอน ๆ คลายกลองวดโอทวไป คอใหขอมลเปนภาพสมขอมลจำนวนมาก ราคาไมสง แตปญหาหลกของกลองออมนกคอ ภาพทไดจากกลองเมอเทยบกบ Field of

View แลว มความละเอยดคอนขางตำ อกทงมมมองของภาพยงแตกตางไปจากกลองทวไป แตเมอพจารณาถงขอดของกลองออมนแลว จะเหนไดวามความนาสนในทจะพฒนาอลกอรทมตาง ๆ สำหรบกลองออมนดงนนในงานวจยนจะนำเสนออลกอรทมสำหรบการระบตำแหนงพรอมไปกบการสรางแผนทสำหรบกลอง

ออมน โดยทตำแหนงของกลองและแผนทของสงแวดลอมทสรางไดนนจะแสดงในสามมต และกลองออมนจะสามารถเคลอนทไปมาไดอยางอสระ โดยทไมจำเปนตองทราบทศทางการเคลอนทของกลอง

(a) (b)

รปท 1: (a) ภาพจากกลองออมน (b) ภาพแสดงการระบตำแหนงพรอมกบการสรางแผนท โดยใชกลองออมน

2 งานวจยทเกยวของ

วธการระบตำแหนงพรอมกบการสรางแผนท (Simultaneous Localization and Mapping: SLAM) ถก

กลาวถงเปนครงแรกเมอป ค.ศ. 1986 โดย Smith และ Cheeseman [5] ซงไดนำเสนอวธในการประมาณความสมพนธ และคาความแปรปรวนรวม (error covariance) ระหวางโคออดเนตเฟรมทงหลาย ซงเปนตวแทนของวตถตาง ๆ ดวยวธการทางความนาจะเปน (probabilistic methods) และหลงจากนนไมนาน Moutarlier และ

Chatila [6, 7] กไดพฒนาวธแกปญหาสำหรบ SLAM ขนครงแรกโดยใช Extend Kalman Filter (EKF) ในการประมาณคาตำแหนงของหนยนตและแผนท ซงภายหลง Extended Kalman Filter กถกใชในการแกปญหาSLAM อยางกวางขวาง อยางไรกตาม กยงไดมการพฒนาอลกอรทมอน ๆ อกมากมายสำหรบ SLAM เชนFastSLAM [8, 9], UKF-SLAM [10], SEIF-SLAM [11] ฯลฯ

วธแกปญหา SLAM นนถกนำไปใชอยางแพรหลาย ในงานนำรองของหนยนต ในสภาพแวดลอมหลาย

รปแบบ เชน สภาพแวดลอมในอาคาร[12, 13], สภาพแวดลอมนอกอาคาร [14], ใตนำ [15], ใตดน [16, 17]

หรอแมกระทง การสำรวจดาวเคราะห [18, 19] นอกจากนยงมการใชงานกบอปกรณเซนเซอรทแตกตางกน

ไป ตวอยางเชน กลองวดโอแบบทวไป [20], กลองแบบออมน [21], อปกรณวดระยะดวยเลเซอร (LaserRange Finder) [22], เรดาร (Radar), โซนาร (Sonar), GPS และอน ๆ

สำหรบเซนเซอรตาง ๆ สำหรบงานระบตำแหนงพรอมกบการสรางแผนท (SLAM) กลองวดโอ ถอเปน

อปกรณหนงทมการใชในงานวจยตาง ๆ เปนจำนวนมาก [23, 24] ทงนเนองมาจากขอดของกลองวดโอ คอ

มราคาถก ใชงานงาย ใหขอมลเปนจำนวนมาก อกทงมความเรวในการรบขอมลสง (Real-time) แตปญหา

สำหรบกลองวดโอ คอ ขอมลทไดมาจากกลองนนเปนภาพในสองมต ซงเกดจากการ projection สงแวดลอม

ในสามมตลงมาบนภาพ จงทำใหมขอมลทสญหายไป ซงกคอขอมลระยะทางจากกลองถงวตถในนน ดงนน

การสรางแผนทสำหรบสงแวดลอมซงตองอาศยขอมลระยะจงมความยงยากขน ในงานวจยหลาย ๆ งานจงมเทคนคในการชวยหาขอมลระยะ เชน การใชกลองสเตอรโอ (Stereo Camera) ชวยในการหาระยะ [25], การใชกลองวดโอ รวมกบเซนเซอรวดระยะอน [26], การประมาณขอมลระยะทางของสงแวดลอมจากการเคลอนทของกลอง โดยตรวจวดขอมลการเคลอนทจาก อปกรณตรวจจบการเคลอนทจากความเฉอย (inertia sensors)

หรอ อปกรณตรวจวดความเรวจากลอ (wheel encoders) หรอ GPS หรออน ๆ ประกอบ แตถงกระนนกยงมงานวจยทอาศยขอมลรปภาพจากกลองเพยงอยางเดยวในการระบตำแหนงพรอมกบการสรางแผนท (SLAM)

MonoSLAM [20] ซงถกนำเสนอโดย Davison เปนงานหนงทมความโดดเดน สำหรบงานระบตำแหนง

พรอมกบการสรางแผนทดวยกลองวดโอ (Vision-based SLAM) โดย Davison นนไดเสนออลกอรทมแบบReal-time ทใชการหาเสนทางการเคลอนทในสามมตของกลองตวเดยว ทเคลอนทในสงแวดลอมทไมคนเคย

มากอน สำหรบ MonoSLAM นน ถอไดวาเปนงานแรก ๆ ทประสบความสำเรจสำหรบ Vision- based SLAMทอาศยขอมลรปภาพจากกลองเพยงอยางเดยว โดยไมอาศยขอมลการเคลอนทของกลองจากเซนเซอรอน

หลงจากนนกมงานทคลายคลงกบ MonoSLAM อกมากมายเชน Eade และ Drummond [27] ไดเสนอ SLAMโดยใชกลองตวเดยวรวมกบ FastSLAM หรอ Sunderhauf [28] กไดเสนอ SLAM สำหรบ เครองบนไรคนขบ(UAV) โดยใชกลองตวเดยวและใช Unscented Kalman Filter (UKF) ในการแกปญหา SLAM

ถงแมวา MonoSLAM จะทำงานไดดในสงแวดลอมทไมมโครงสรางแนนอน แตเนองจากมมมองของกลอง(field of view) ทแคบจงทำให MonoSLAM ทำงานไดไมดนกกบการเคลอนท ทเปลยนมมมองเรวจนเกนไปจนไมสามารถสรางแผนทไดทน และ MonoSLAM ยงไมสามารถทำงานไดกบการเคลอนทของกลองบางรปแบบอกดวย

ในปค.ศ. 2003 Kim และ Chung [29] ไดเสนอวธการใชงานกลองออมน กบงานระบตำแหนงพรอมกบการสรางแผนท (SLAM) โดยเปนการทำงานรวมกนระหวาง กลองออมนแบบสเตอรโอ (Omni-directionalstereo vision) กบ การหาโครงสรางสามมตจากการเคลอนท (structure from motion: SFM) และเนองจากกลองออมน เปนเซนเซอรทมมมมองของภาพทกวาง จงทำใหเซนเซอรตรวจวดสงแวดลอมไดทวถง สงผลใหอลกอรทมของ SLAM มความเสถยร และทนทานตอการเคลอนทของกลอง ภายหลง การใชงานกลองออมน สำหรบงานระบตำแหนงพรอมกบการสรางแผนท จงเรมมการแพรหลายมากขน Murillo, Guerrero และ

กลม [21] ไดเสนออลกอรทมสำหรบกลองออมน ในการระบตำแหนงของหนยนตในสองมต โดยใชแนวเสนตง

กบกลองเปนจดสงเกต และใชเสนตรงในสามมมมองเพอคำนวณ Trifocal Tensor จากนนจงใช tensor เพอการระบตำแหนงของหนยนต Valgren, Lilienthal และ กลม [30] ไดเสนออลกอรทมสำหรบสรางแผนทแบบโครงสราง (topological maps) โดยใชขอมลจากกลองกลองออมน เพยงอยางเดยว Saedan, Lim และ กลม[31] ไดเสนอวธในการระบตำแหนงโดยใช Appearance-based ซงมความสามารถในการสรางแผนทขนาด

ใหญ และสามารถตรวจจบ สถานทเดมทเคยเขาถงมากอน นอกจากนยงม งานวจยทเกยวของกบการระบตำแหนงพรอมกบการสรางแผนท (SLAM) โดยใชกลองออมน อกมากมาย [2, 3, 4]

จากงานวจยทกลาวมาขางตน จะเหนไดวา มการใชกลองวดโอในงานระบตำแหนงของหนยนตพรอมกบการสรางแผนท (SLAM) อยางแพรหลายมาก แตเนองจากปญหามมมองของกลองทแคบ ทำใหอลกอรทม

ของกลองวดโอ ไมทนทานตอการเคลอนทมากนก ดงนนการใชกลองมมมองกวาง อยางเชน กลองออมน จงเปนทางออกทดสำหรบ SLAM อยางไรกตาม การพฒนาอลกอรทมสำหรบกลองออมน ยงยดตดกบการระบ

ตำแหนงของหนยนตในสองมตอยมาก อกทงเนองจากกลองออมนมความละเอยดของภาพทตำ จงทำใหการ

ทำงานของอลกอรทมทงหลายทถกนำเสนอไมดเทาทควร วทยานพนธนจงมเปาหมายทจะพฒนาอลกอรทมสำหรบระบตำแหนงและหาเสนทางการเคลอนทของกลอง พรอมไปกบการสรางแผนทของสงแวดลอม

โดยทงเสนทางการเคลอนทของกลอง และแผนทของสงแวดลอมทสรางไดนนจะแสดงในสามมต โดยกลองออมนสามารถเคลอนทไปมาไดอยางอสระ โดยทกลองออมน อาจถกตดตงไปกบหนยนต หรออาจถกจบเคลอนทไปมาโดยมนษย ซงการเคลอนทของกลองออมนจะเปนแบบไมสามารถประมาณการเคลอนทได และ

สำหรบอลกอรทมการระบตำแหนงพรอมกบการสรางแผนทนน จะอาศยขอมลเปนภาพจากกลองออมนเพยงอยางเดยว ไมอาศยขอมลจากเซนเซอรอน ๆ รวมอก โดยมจดมงหมายวา อลกอรทมจะสามารถทำงานไดในสภาพแวดลอมภายในหอง และสามารถนำไปใชงานรวมกบงานอน ๆ ตอไป

3 วตถประสงคของงานวจย

เพอพฒนาอลกอรทมสำหรบการระบตำแหนงและหาเสนทางการเคลอนทของกลองในสามมต พรอมไปกบการสรางแผนทของสงแวดลอม โดยอาศยขอมลเปนภาพจากกลองออมนเพยงอยางเดยว โดยการเคลอนท

ของกลองออมน เปนแบบไมสามารถควบคมได (Uncontrolled) ซงกลองออมนอาจถกตดตงอยบนหนยนต

หรอ กลองออมนอาจถกถอเคลอนทโดยมนษยทงนระบบจะไมทราบขอมลการเคลอนท และ model การเคลอนทของกลอง

4 ทฤษฎและอปกรณทเกยวของกบงานวจย

4.1 กลองออมน (Omni-directional Camera)

กลองออมน (Omni-directional Camera) เปนกลองทสามารถรบภาพไดรอบทศทาง โดยอาศยการสะทอนภาพจากกระจกทมลกษณะตาง ๆ เชน กรวย พาราโบลา ไฮเปอรโบลา และ อานมา ซงโดยทวไปแลวจะใหมมมองในแนวตงรอบตวกลอง 360 องศา และ มมมองในมมกมเงยประมาณ 90 องศา ซงกลองออมนนน

ถกใชในงานทหลากหลาย ทงในงานนำรองของหนยนตอตโนมต หรอการถายภาพทวไป ทงนเนองจากกลอง

ออมนนนมขอดเหนอกลองวดโอทวไปคอ

• จดสงเกต (landmark) ทมองเหนไดจากกลองแทบจะอยใน field of view ของกลองเสมอ ยกเวนกรณทมการบดบงกน ซงการทสามารถสงเกตเหนจดสงเกตไดเปนเวลานานทำใหสามารถพฒนาอลกอรทมทใชในการระบตำแหนงทแมนยำได

• มมมองรอบทศทางทไดจากกลองออมนนนเกดการการถายภาพเพยงครงเดยว จงทำใหไมมปญหาเรองการ synchronization ของภาพ

• กลองออมนนนไมม กลไกทมการเคลอนท จงทำใหมความทนทาน และมความนาเชอถอของภาพสงเมอเทยบกบกลอง pan-tilt ทใชระบบหนกลองไปมาเพอเพม field of view

สำหรบกลองออมนสำหรบงานวจยนเปนแบบกระจกสองชนโคง รปทรงพาลาโบลารน D40 ของ RemoteReality ทมลกษณะดงรปท 2(a) และภาพทไดจากกลองออมมดงกลาวจะปรากฏดงรปท 2(b) โดยตวกลองม

คณสมบตดงน

• ระบบภาพ NTSC ความถ 29.97 ภาพตอวนาท

• ความละเอยด 640x480 พกเซลแบบสลบเสน (Interlace)

• ภาพทไดจากกลองออมนตามรปท 2(b) มความละเอยดประมาณ 480x480 พกเซล

(a) (b)

รปท 2: (a) ภาพกลองออมนทใชในงานวจย (b) ภาพถายจากกลองออมน

4.2 การหาพารามเตอรการเรยงตวชนสวนของกลอง (Camera Calibration)

สำหรบกลองโดยทวไปแลว หลกการทำงานกคอการ โปรเจค วตถในพกดสามมตใหมาอยในพกดสองมต

ของรป ซงการเรยงตวของชนสวนของกลองกจะแตกตางกนไป

ในงานทางดาน Robotics ทมความเกยวของกบ Vision นน มความจำเปนอยางยงทจะตองทราบคาพารามเตอร และ Mapping Function ระหวางตำแหนงของวตถในสามสต และ ตำแหนงในภาพ เพอใชใน

การคำนวณทมความแมนยำสง ดงนนสงทขาดไปมได สำหรบงานทางดาน Vision กคอการ Calibrate เพอหาคาพารามเตอร และ Mapping Function ของกลอง

ในงานวจย [32, 33] ไดนำเสนอหลกการในการประมาณพารามเตอรของกลองออมน (Omni CameraCalibration) ซงจะเปนการหาความสมพนธระหวาง จดสองมตในภาพ (p) กบตำแหนงในโลกสามมต (P)

ทสะทอนมาจากกระจกดงรปท 3 ซงในความเปนจรงแลวในกระบวนการนมความจำเปนตองหา intrinsicparameters ของกลอง และ intrinsic parameters ของกระจก อยางไรกตาม โมเดลของกลองออมน ในงานวจย [33] จะถกมองเปนระบบพเศษระบบหนง ซงจะไมสนใจวาจะใช กระจก หรอ เลนสตาปลา ในกลองออมน

ในงานวจย [33] ไดมสมมตฐานวา กลองจะเปนแบบ central ดงนนจะตองมจดจดหนงในกระจกซงเปน

จดตดของ รงสของแสงทงหมด ซงจะกำหนดใหจดจดนนเปนจดกำเนด ของระบบโคออดเนต XYZ ของกลอง, กลองและกระจกไดวางอยในแนวเดยวกน, กระจกมความสมมาตรในการหมนในแกนของมน, และการ Distortion ของเลนส จะไมถกพจารณา ซงเหตผลทไมสนใจการ Distortion ของเลนส เปนเพราะ กลอง

ออมน ทใชกระจก โดยปรกตแลวจะตองการ ความยาวโฟกสทมากเพอทจะได โฟกสภาพไดบนกระจก ดงนนการ Distortion ของเลนสสามารถละเลยได

รปท 3: แสดงการ project ของแสงทสะทอนเขามายงกลองออมน

u′

v′

u

v

รปท 4: แสดงโคออดเนตของพกเซล (u′, v′) และโคออดเนตทแกไขการ distortion แลว (u, v)

พจารณา โมเดลของกลองออมน

สมมตใหกลองและกระจกวางอยในแนวเดยวกน จากรปท 4 กำหนดให (u′, v′) เปนโคออดเนตของพกเซล

สวน (u, v) เปนโคออดเนตทแกไขการ distortion แลว จะเขยนการแปลง u′ และ v′ ไดดงน[u′

v′

]=

[c d

e 1

]·

[u

v

]+

[xc′

yc′

](1)

จะไดวา [u

v

]=

[1/det −d/det−e/det c/det

]·

[u′ − xc′

v′ − yc′

], det = c− e · d (2)

ซง c, d, e, xc′ และ yc′ เปน parameter ทไดมาจากการ calibrate โดย xc′ และ yc′ จะเปนจดศนยกลางภาพ สวน c, d และ e จะเปน affine transform parameter

จากภาพท 3 จะหา vector จากกลองชไปยงจดใน 3 มตไดเปน

r =

rx

ry

rz

=

u

v

f(ρ)

(3)

ρ =√u2 + v2 , f(ρ) = a0 + a0ρ+ a0ρ

2 + a0ρ3 + a0ρ

4 + · · · (4)

จากสมการขางตนฟงกชน f(ρ) จะถกประมาณดวย Polynomial function โดยคา ρ จะมคาเปน√u2 + v2

ซงจะเปนคาระยะทางจากจดศนยกลางกลองโดยจากภาพท 3 แสดงใหเหนวาระยะทางจากจดศนยกลางจะสอดคลองกบความเอยงของกระจก ซงจะสงผลกบคาในแกน z ดวย สวนสมประสทธของการประมาณจะหาไดจากการ calibrate ซงในทนจะใช polynomial degree 4 ในการประมาณ

4.3 การระบตำแหนงพรอมกบการสรางแผนท (Simultaneous Localization and Mapping:SLAM)

การระบตำแหนงทมความเชอถอได มความสำคญเปนอยางยงสำหรบหนยนตอตโนมต ซงปญหาการระบตำแหนงของหนยนต ในสภาพแวดลอมทรแผนทอยแลว แตกตางกบ การระบตำแหนงในสภาพแวดลอมทมไมคนเคยเปนอยางมาก วธการระบตำแหนงพรอมกบการสรางแผนท (SLAM) [1] เปนกระบวนการทหนยนตจะสรางแผนทของสภาพแวดลอมในขณะทกำลงเคลอนท และระบตำแหนงของตวเองในเวลาพรอม

ๆ กน โดยทหนยนตนนไมมขอมลของสงแวดลอมมากอน ซงปญหาการระบตำแหนง และการสรางแผนทเปนปญหาทเกยวพนกนเปนอยางมาก ในกรณเรมตน หนยนตจะไมรตำแหนงของตนเองมากอน และจะคอย

ๆ สรางแผนทของสงแวดลอมขนมาเรอย ๆ ในขณะเดยวกนกใชแผนทนน ๆ ในการระบตำแหนงของหนยนต

จงอาจกลาวไดวา ความไมแนนอนของตำแหนงของหนยนต จะขนกบความไมแนนอนของแผนท และ ความ

ไมแนนอนของแผนท กขนกบความไมแนนอนของตำแหนงของหนยนต ดวย ซงวธการของ SLAM นจะใช

วธการทางความนาจะเปน (probabilistic methods) มาจดการความไมแนนอนเหลาน

เพอความสะดวกในการอธบาย จะขอกำหนดตวแปรตอไปนเมอหนยนตเคลอนทผานสงแวดลอมพบจดสงเกต (Landmarks) ณ เวลา k กำหนดใหxk เปนสถานะของหนยนต ประกอบดวย ขอมลตำแหนงและทศทางของหนยนต เมอเวลา kuk เปนคำสงของหนยนตทสงใหหนเคลอนทจากเวลา k − 1 ไปสถานะ xk ทเวลา kmi เปนเวกเตอรบอกถงตำแหนงของจดสงเกต (Landmarks) จดท i โดยสมมตวาตำแหนงทแทจรง

ไมเปลยนแปลงตามเวลาzi,k เปนตำแหนงของจดสงเกตท i ทวดไดจากหนยนตทเวลา k ซงสามารถเขยนรวมทกจดสงเกต

ไดเปน zk

นอกจากนยงขอกำหนดใหX0:k = {x0, x1, · · · , xk} = {X0:k−1, xk} : เปนบนทก (history) ของสถานะหนยนตU0:k = {u0, u1, · · · , uk} = {U0:k−1, uk} : เปนบนทก (history) ของคำสงของหนยนตm = m0,m1, · · · ,mn : เปนเซตของจดสงเกตทงหมดZ0:k = {z0, z1, · · · , zk} = {Z0:k−1, zk} : เปนเซตของขอมลการวดจดสงเกตทงหมดจากหนยนต

4.3.1 การระบตำแหนงพรอมกบการสรางแผนทในเชงความนาจะเปน

รปแบบของความนาจะเปนของปญหาการระบตำแหนงพรอมกบการสรางแผนท (SLAM) สามารถเขยน

ในรปแบบ การกระจายของความนาจะเปน (probability distribution) ไดดงน

P (xk,m|Z0:k, U0:k, x0) (5)

จากสมการแสดง การกระจายความนาจะเปน ของความหนาแนนรวมในภายหลง (joint posterior density)ของสถานะหนยนต (xk) ซงในทนคอตำแหนงของหน และตำแหนงของจดสงเกต (m) ทเวลา k เมอกำหนด

เซตของจดสงเกตทงหมดทวดไดจากหนยนต (Z0:k), คำสงทงหมดของหนยนต (U0:k) ตงแตเวลา 0 ถง k

และ สถานะเรมตนหนยนต (x0)ซงโดยทวไปแลวผลลพธแบบเวยนบงเกด (recursive solution) ของปญหา SLAM จะเรมจากการประมาณ

การกระจายของ P (xk−1,m|Z0:k−1, U0:k−1) ทเวลา k− 1 หลงจากนนจงคำนวณ joint posterior จาก คำสงของหนยนต uk และ จดสงเกตทงหมดทวดไดจากหนยนต zk ทเวลา k โดยอาศย Bayes theorem ซงการ

คำนวณนนจำเปนจะตองอาศยโมเดลการเปลยนแปลงของสถานะหนยนต (state transition model) และ โมเดลการวดจดสงเกต (observation model) ทแปรผนตามคำสงของหนยนต และ จดสงเกต ตามลำดบ

โมเดลการวดจดสงเกต (observation model) จะ บรรยายความนาจะเปน ของ จดสงเกตทงหมดทวดไดจากหนยนต (zk) เมอรสถานะหนยนต (xk) และ ตำแหนงของจดสงเกต m ทแนนอน ซงเขยนไดเปน

P (zk|xk,m) (6)

ในทนจะสมมตวา จดสงเกตทวดได (zk) จะขนอยกบ สถานะหนยนต (xk) และตำแหนงของจดสงเกต m

ปจจบนเทานนโมเดลการเปลยนแปลงของสถานะหนยนต (state transition model) จะบรรยาย การกระจายความนาจะเปน

ไดดงน

P (xk|xk−1, uk) (7)

state transition ทงหมดถกสมมตใหเปน Markov process ไดซงสถานะถดไป (xk) จะขนอยกบสถานะ

กอนหนา (xk−1) และคำสงของหนยนต (uk) เทานนโดยขนกบจดสงเกตทวดได (zk) และแผนท

SLAM อลกอรทม จะสามารถอธบายไดดวย อลกอรทมแบบเวยนบงเกดสองขนตอนคอ การประมาณคา(prediction, time-update) และการปรบแกคา (correction, measurement- update) ซงอยในรป

Time-update

P (xk,m|Z0:k−1, U0:k, x0) =∫P (xk|xk−1, uk) · P (xk−1,m|Z0:k−1, U0:k−1, x0) · dxk−1 (8)

Measurement Update

P (xk,m|Z0:k, U0:k, x0) =P (zk|xk,m) · P (xk,m|Z0:k−1, U0:k, x0)

P (zk|Z0:k−1, U0:k)(9)

ในสมการท 8 และ 9 เปนการคำนวณแบบเวยนบงเกด สำหรบคำนวณหา ความนาจะเปนรวมภายหลง(joint posterior) P (xk,m|Z0:k, U0:k, x0) สำหรบสถานะหนยนต (xk) และ เซตของจดสงเกตทงหมดในแผนท

m ณ เวลา k โดยขนกบ เซตของจดสงเกตทงหมดทวดได (Z0:k) และ คำสงทงหมดของหนยนต (U0:k) การ

คำนวณแบบเวยนบงเกดน จะอยในรปโมเดลการเปลยนแปลงของสถานะหนยนต (state transition model)P (xk|xk−1, uk) และ โมเดลการวดจดสงเกต (observation model) P (zk|xk,m)

สำหรบปญหาการสรางแผนทเพยงอยางเดยวสามารถเขยน การกระจายความนาจะเปน ไดเปนP (m|X0:k, Z0:k, U0:k) โดยกำหนดวารสถานะหนยนต (xk) ทแนนอนตลอดเวลา ดงนนแผนทจะเปนการรวมกนระหวางจดสงเกตทตำแหนงของหนยนตตาง ๆ กน ในทางกลบกน สำหรบปญหาทตองการการระบตำแหนงเพยงอยางเดยวสามารถเขยนการกระจายความนาจะเปน ไดเปน P (xk|Z0:k, U0:k,m) โดยทสมมต

วารตำแหนงของจดสงเกตทแนนอนตลอดเวลา ดงนนจดมงหมายจะกลายเปนการประมาณสถานะหนยนต

เทยบกบจดสงเกตทงหลาย

4.3.2 การแกปญหา SLAM

การแกปญหา SLAM เปนการหาการรปแบบการนำเสนอของ โมเดลการเปลยนแปลงของสถานะหนยนต(state transition model) (7) และ โมเดลการวดจดสงเกต (observation model) (6) ทมประสทธภาพเพยงพอทจะคำนวณ การประมาณคา (8) และการปรบแกคา (9) ซงโดยทวไปแลวจะถกเสนออยในรป โมเดลของstate-space ทใช Gaussian noise เขารวมดวย วธแกปญหาของ SLAM ทนยมใชกนมาก กคอการใชExtended Kalman filter (EKF) [1] ในการแกปญหา

EKF-SLAM

อลกอรทมของ SLAM นนอยบนสมมตฐานทวา เราร Kinematic Model ของระบบ ซงเปาหมายของ

SLAM คอการระบตำแหนงของหนยนต ในแผนททสรางขนโดยอาศยขอมลจากการวดจดสงเกต โดยทจะตดเซนเซอรบนตวหน ซงเซนเซอรจะมความสามารถในการวดคาจดสงเกต สมพทธกบตวหนยนต ดงรปท 5

รปท 5: เซนเซอรวดคาจดสงเกตในสงแวดลอม สมพทธกบตวหนยนต

Process Model

สถานะของระบบ จะประกอบดวย ตำแหนงและทศทางของหนยนต นอกจากนจะม ตำแหนงของจดสงเกต(Landmark) โดยทสถานะของหนยนตเขยนแทนดวย xk เมอเคลอนทผานสงแวดลอม สามารถเขยน model

การเคลอนทไดดงน

xk = fv(xk−1, uk) + wk (10)

โดยท fv(a) เปน ฟงกชนการเคลอนทของหนยนต, uk เปน คำสงการเคลอนทของหนยนต, wk เปน

noise ทไมขนกบระบบหนยนต มคาเฉลย (mean) เปน 0 และ ความแปรปรวนรวม (covariance) เปน Qk

ตำแหนงของจดสงเกต (Landmark) จดท i จะแทนดวย mi โดยจะสมมตวาตำแหนงทแทจรงของจดสงเกต ไมเปลยนแปลงตามเวลา จะได state transition model สำหรบ landmark ตวท i เปน

mi,k = mi,k−1 = mi (11)

เมอนำสถานะของหนยนต และ ตำแหนงของจดสงเกตมาเขยนรวมกนจะได

Xk =

xk

m0

...mn

=

fv(xk−1, uk)

m0

...mn

+

wk

0...0

= f(Xk−1, uk) +Wk (12)

โดยท f(a) เปน State transition model ของระบบทงหมด

Observation Model

เมอหนยนตตดเซนเซอรบนตวหน ซงเซนเซอรมความสามารถในการวดคาจดสงเกต สมพทธกบตว

หนยนตดงนนจะไดวา observation model ของ landmark ตวท i จะเขยนไดเปน

zi,k = h(xk,mi) + vk (13)

โดยท h(a) เปน ฟงกชนการวดของเซนเซอร, vk เปน noise ทไมขนกบ model การวด มเฉลย (mean)เปน 0 และ ความแปรปรวนรวม (covariance) เปน Rk สาเหตทฟงกชน h(a) รบสถานะของหนยนตเปน

พารามเตอรดวย นนเปนเพราะการวดคาจดสงเกตจะขนอยกบตำแหนงของหนยนตดวย

Estimation Process

Extended Kalman Filter จะประมาณสถานะของระบบXk ดวยวธเวยนบงเกด (recursive) ซง model ของระบบจะประมาณตามสมการ 12 และ model การวดคาจดสงเกตจะเปนไปตามสมการ 13 ซงในกระบวนการ

ประมาณนนจะเสมอนเปนการหา

Xk|k = E [Xk|Z0:k] (14)

โดย Z0:k เปนเซตของจดสงเกตทงหมดทวดไดจากหนยนตตงแตเวลา 0 ถง k

และ error จากการประมาณน และ คาความแปรปรวนรวม (Covariance) ของของการประมาณ เขยนไดเปน

Xk|k = Xk|k −Xk (15)

Pk|k = E[Xk|kX

Tk|k|Z0:k

](16)

อลกอรทมของ Extended kalman filter มการทำงานอย 3 ขนคอ Prediction, Observation และ Update

Prediction: กำหนดใหม model ตามสมการ 12 และ 13 และ คาประมาณสถานะของระบบ Xk−1|k−1

และ คาประมาณความแปรปรวนรวมของระบบ Pk−1|k−1 จะทำนายสถานะของระบบ, คาประมาณการวดจดสงเกตท i และ ความแปรปรวนรวมทเวลา k ไดเปน

Xk|k−1 = f(Xk−1|k−1, uk) (17)

zi,k|k−1 = hi(Xk|k−1) (18)

Pk|k−1 = FkPk−1|k−1FTk +Qk (19)

ตามลำดบ โดยท Fk คอ Jacobian ของ f(a) เทยบกบการเปลยนแปลงของ Xk−1|k−1 และ uk

Observation: หลงจากการทำนายสถานะแลว เมอทราบคาการวดของจดสงเกต zi,k จดท i จะคำนวณคา innovation และ innovation covariance matrix ไดจาก

yi,k = zi,k − zi,k|k−1 (20)

Sk = HkPk|k−1HTk +Rk (21)

ตามลำดบ โดยท Hk คอ Jacobian ของ h(a) เทยบกบการเปลยนแปลงของ Xk|k−1

Update: สถานะประมาณของระบบ หลงจากทราบคาการวดจดสงเกต และ ความแปรปรวนรวมทเวลาk ไดเปน

Xk|k = Xk|k−1 +Kkyk (22)

Pk|k = Pk|k−1 −KkSkKTk (23)

โดยทตวคณ Kk หาไดจาก

Kk = Pk|k−1HTk S−1k (24)

5 แนวคดของการวจยและวธการดำเนนงาน

การพฒนาอลกอรทมสำหรบกลองออมน เพอใชในงานระบตำแหนงพรอมกบการสรางแผนท มเปาหมาย

เพอใหสามารถระบตำแหนงของกลองออมนได ในแผนททถกสรางขนมาพรอมกน โดยทกลองออมนอาจถก

ตดตงอยบนหนยนตหรอ กลองออมนอาจถกถอไปมาโดยมนษยเอง ทงน ระบบจะไมทราบขอมลการเคลอนท

ของกลอง จะทราบเพยงแตขอมลรปภาพทไดจากกลองเทานน

เพอการประมาณตำแหนงของกลอง และ ลกษณะสงแวดลอมทกลองตรวจวดได หวใจสำคญของงานนกคอการนำ SLAM มาใชในการประมาณสถานะตาง ๆ โดยผลลพธทไดจากการประมาณของ SLAM นนจะเปน แผนทความนาจะเปน (Probabilistic map) ซงแสดงถงความไมแนนอนของสถานะสงแวดลอมทงหมดทกลองตรวจวดได รวมไปถงความไมแนนอนของตำแหนงของกลองดวย สำหรบงานวจยน จะใช ExtendedKalman Filter (EKF) มาใชในการแกปญหาของ SLAM โดยท ณ เวลาหนง ๆ Probabilistic map จะถกUpdate ดวยคาการวดทไดจากกลองในขณะทกลองเคลอนทไปเรอย ๆ ในสงแวดลอม

ปญหาของงานวจยนประกอบดวย 3 ปญหาหลกคอ

1. จะใช Features อะไรในภาพ มาใชในการวดคาจดสงเกต (measurement) ซง Feature ทดนนจะตองมความโดดเดนและมลกษณะเฉพาะ เมอเทยบกบ Features อน ๆ จะตองทนทานตอการเปลยนแปลงตาง ๆ และจะตองตรวจหาไดงายดวยวธการทาง Image Processing

2. เนองจากงานวจยน มงหมายทจะระบตำแหนงของกลอง โดยไมทราบรปแบบการเคลอนท จงทำให

เกดปญหาวา ในสวน Prediction ของ SLAM นนจะใช state transition model อะไรมาประมาณการเคลอนทของกลอง และยงมปญหาอกวาจะประมาณขอมล control ของกลองมาไดอยางไร

3. เนองจากงานวจยน มงเนนจะระบตำแหนงของกลอง พรอมกบสรางแผนทของสงแวดลอมในสามมต

แตกตางจากงานวจยอน ๆ ซงทำงานในสองมตเทานน จงทำให degree of freedom ของสถานะทตองการประมาณเพมสงขน อกทงขอมล measurement จากเซนเซอรนนยงขาดขอมลระยะทาง(range) จากกลองถงวตถในสงแวดลอม จงเกดเปนปญหาวา Observation model อะไรทมประสทธภาพ

เพยงพอทจะประมาณสถานะของระบบไดอยางเหมาะสม ทำใหสถานะของระบบนนลเขาโดยเรว และมเสถยรภาพสง

สำหรบ SLAM แบบทวไปนน ในกระบวนการ Update เพอประมาณสถานะของระบบหลงจากทราบคาการวดจดสงเกตในเวลาหนง ๆ จะใชขอมล measurement ปจจบนเทานนมาใชในการ Update ซงสำหรบขอมลการวดทขาดขอมลระยะทางแลว จะไมสามารถประมาณสถานะของจดสงเกตดวยขอมลในเวลาเดยวได

ถงแมวาการประมาณซำ ๆ ในหลาย ๆ ชวงเวลาจะพอประมาณสถานะของจดสงเกตได แตจะใหผลลพธทไม

ดนก เนองจากสถานะของจดสงเกต จะขนอยกบการวดครงสดทายเสยมาก เพอแกปญหาน ในงานวจยนจะ

ใชขอมล measurement ในหลาย ๆ ชวงเวลามาชวยในการ Update ดงแสดงในรปท 6(b) การประมาณเชนน

จะประมาณสถานะของจดสงเกตไดดกวา และทำใหอลกอรทมลเขาเรวขน อกทงอลกอรทมยงมเสถยรภาพสง แตการประมาณดวยขอมล measurement ในหลาย ๆ ชวงจำตองอาศยสถานะของกลองในอดตควบคกน

ดวยเหตน SLAM จะตองประมาณ สถานะของกลองในอดตเพม ซงภายหลงจะเรยกสถานะของกลองในอดตวา "กรอบอางองยอย"

Landmark

Global Reference Frame

Camera State

(a)

Landmark

Global Reference Frame

Camera State Old Camera State

Old Camera State

(b)

รปท 6: (a) การประมาณดวย Measurement เพยงคาเดยว (b) การประมาณดวย Measurement หลายคา

สำหรบแนวทางการแกปญหาการระบตำแหนงของกลอง โดยไมทราบรปแบบการเคลอนทนน สำหรบงานวจยอน ๆ มกจะใชวธประมาณ Model การเคลอนทดวยสมการ polynomial [1] โดยคาคงทของสมการจะเปนสถานะของกลอง การประมาณวธนไมสามารถเลอก degree ของ polynomial ไดสงมากนก เนองจาก

จะทำใหขนาดของสถานะของกลองใหญขนซงสงผลใหประมาณคาไดยาก งานวจยนจะใชการประมาณการเคลอนทดวยสมการการเคลอนทแบบทวไปของวตถทตองอาศยความเรวและความเรวเชงมมของวตถในการ

ประมาณ และงานวจยนจะหาขอมล control input จากการประมาณการเคลอนทของกลองดวยวธการอนแทน

ซงจะไดอธบายตอไปกรอบอางองทจะใชในงานวจยนจะมอย 3 ประเภท คอ กรอบอางองหลก (World Frame) แทนดวย W ,

กรอบอางองยอย (Reference Frame) แทนดวย R และ กรอบอางองของกลอง (Camera Frame) แทนดวยC ซงจากรปท 7 World Frame จะใชเปน global Coordinate สวน Camera Frame จะใชอางองตำแหนงตาง

ๆ สมพทธกบกลองซงจะเปลยนแปลงไปเมอกลองมการเคลอนท สำหรบ Reference Frame นนจะใชเปนจด

อางองพเศษ ซงในทนจะเปนกรอบอางองในอดต ของกลอง

World Frame

Reference Frame

Camera Frame

Landmark

CamerayW

yC

yR

รปท 7: แสดงกรอบอางองทจะใชในงานวจย

สำหรบ Probabilistic map ณ เวลาหนง ๆ จะประกอบดวย เวกเตอรสถานะ (state vector) (x) และ คาความแปรปรวนรวมของสถานะ (covariance matrix) (P ) ซงเวกเตอรสถานะ จะมขอมลสามสวนคอ สถานะของกลอง (camera state) (xc), สถานะของกรอบอางอง (reference frame states) (xr) และ สถานะของจดสงเกต (landmark states) (xy)

x =

xc

xr

xy

P =

PxcxcPxcxr

Pxcxy

PxrxcPxrxr

Pxrxy

PxyxcPxyxr

Pxyxy

(25)

xc =[cWC qWC

]T

(26)

xr =[r1 r2 · · ·

]T

ri =[cWRi qWR

i

]T

(27)

xy =[y1 y2 · · ·

]T

(28)

สำหรบสถานะของกลอง (xc) นนจะประกอบไปดวย ตำแหนงของกลองสามมตใน World Frame (cWC )และ ทศทางของกลองสามมตใน World Frame (qWC ), สถานะของจดสงเกต (xy) จะประกอบไปดวย

พารามเตอรของจดสงเกต (yj) ในสามมตจำนวนมาก ซงใชในการอธบายสงแวดลอมทงหมดทกลองตรวจวด

ได สวน สถานะของกรอบอางอง (xr) ในทนจะกำหนดใหเปนสถานะของกลองในอดต ซงสถานะของกรอบอางองจะประกอบดวย กรอบอางอง (ri) หลาย ๆ กรอบ ซงแตละกรอบอางองจะประกอบไปดวย ตำแหนงของกรอบอางองเทยบกบ World Frame (cWR

i ) และ ทศทางของกลองเทยบกบWorld Frame (qWRi ) โดยเมอ

พจารณะ สถานะของกรอบอางอง ซงประกอบไปดวยตำแหนงและทศทางของกลอง ในอดตจำนวนมากแลวจะเหนไดวา สถานะของกรอบอางอง กเปรยบเสมอนเสนทางการเคลอนทของกลองนนเอง ซงกรอบอางองตาง ๆ เหลาน จะถกนำไปใชประโยชนในการประมาณสถานะตาง ๆ ตอไป

สำหรบขนตอนการทำงานของอลกอรทมนน จะแบงออกเปน 3 สวนคอ1. การตรวจหาจดสงเกตสำคญ ในภาพทไดจากกลองออมน และการวดคาจดสงเกต (measurement)2. การประมาณการเคลอนทของกลองออมน โดยอาศยการวดคาของจดสงเกต ณ ภาพปจจบน เทยบกบภาพอางอง ในอดต3. การประมาณตำแหนงของกลอง และตำแหนงของจดสงเกตในแผนทดวย อลกอรทมของ SLAM

5.1 การตรวจหาจดสงเกตสำคญ ในภาพทไดจากกลองออมน และการวดคาจดสงเกต

ภาพทไดจากกลองวดโอนน ประกอบดวย Array ของจดสจำนวนมาก ซงเปนขอมลทคอมพวเตอรไม

สามารถเขาใจได หนาทสำคญของขนตอนนคอการตรวจหาจดสงเกตสำคญ ทมลกษณะเดนในสงแวดลอมยกตวอยางเชน ขอบวตถ, ขอบหนาตาง, เสา, มมหอง หรอวตถตาง ๆ ทมลกษณะเดนเฉพาะซงสามารถแยกแยะได และแปลงมาเปนคา parameter ของจดสงเกตทคอมพวเตอรสามารถเขาใจได

ซงลกษณะเดนในสงแวดลอมเหลานน เมอมาปรากฏบนภาพกมกจะมลกษณะเปน ขอบ (edge) หรอ มม(corner) ซงจะเปน สวนทมลกษณะเฉพาะในภาพ (features) โดยสามารถตรวจวดไดดวยวธการทาง ImageProcessing ตอไป

เมอได Feature จากภาพแลวจงหาคาการวดจดสงเกต (measurement) ไดจาก model การวดของกลองออมน ตามทไดเสนอไปในสวนทฤษฏทเกยวของ

5.1.1 Feature Detection

ตวอยางการตรวจหา Features ในภาพจากกลองออมนแสดงไดดงน

การตรวจหาจด ทมลกษณะเดนในภาพ

Feature แบบจดทมลกษณะเดนในภาพ มกจะเปนสวนประกอบของมม หรอ รายละเอยดของวตถตาง ๆในสงแวดลอม ซงจะเปนบรเวณทมความเปลยนแปลงของคาสสง อลกอรทมทเปนทนยมใชในการตรวจหาจดคอ Harris Detector

หลกการทำงานของ Haris Dector จะอาศยการตรวจจบจดทเปนสวนของ corner ในภาพ ซงในสวนการ

วเคราะหหา corner ของภาพนน corner ของภาพจะเปนจดทคา eigenvalue สงทงสองภาพ (eigenvectorจะแสดงใหเหนถง ทศทางของการเปลยนแปลงคาส สวน eigenvalue จะแสดงใหเหนถง ปรมาณของการเปลยนแปลงคาส ถา eigenvalue มคาสงทงสองคาแสดงวาบรเวณนนเปน corner)

การตรวจหาเสนตรงในภาพจากกลองออมน

สำหรบภาพจากกลองวดโอทวไป การตรวจหาเสนตรง นนทำไดงายโดยใช Hough (line detecting)transform สวนสำหรบภาพจากกลองออมน การตรวจหาเสนตรงกสามารถทำไดโดยใช Hough transformเชนกนแตอลกอรทมการทำงานนนจะมความแตกตางออกไป ซง Ying และ Hu [34] ไดเสนอวธการหาเสนตรงจากกลองออมน ดวย Hough transform ไวแลว (ตวอยางแสดงในรปท 9)

(a) (b)

(c) (d)

รปท 8: ขนตอนของ Harris Detector (a) ภาพปรกต (b) ภาพ eigen value (c) รป threshold (d) feature ทตรวจพบได

(a) (b)

รปท 9: (a) เสนตรงทตรวจวดไดจากกลองทวไป (b) เสนตรงทตรวจวดไดจากกลองออมน

การตรวจหาวตถในภาพ

สำหรบการตรวจหาวตถในภาพจากกลองวดโอทวไป สามารถหาไดโดยใช SIFT algorithm [35] ซงการ

ตรวจหาวตถโดยใช SIFT นนจำเปนจะตองใช template เปนภาพของวตถทตองการตรวจหาเสยกอน สวน

การตรวจหาวตถในภาพจากกลองออมนนน ปจจบนยงไมม SIFT algorithm บนภาพจากกลองออมน แตกพอจะสามารถตรวจหาไดโดยการประมาณจาก SIFT algorithm สำหรบกลองวดโอทวไป (ตวอยางแสดงในรปท 10)

5.1.2 Feature Associations

นอกจากการตรวหา Features ในภาพแลว ขนตอนสำคญอกขนตอนหนง กคอการหาความสมพนธระหวาง Feature ทตรวจหาไดจากภาพ กบจดสงเกต (landmark) ทกลองเคยตรวจเจอแลวในอดต วธการหาความสมพนธ ระหวาง Feature กบ landmark สามารถแบงออกไดเปน 2 แบบ คอ

(a) (b)

รปท 10: (a) รปแสดงการตรวจหาวตถในภาพจากกลองทวไป (b) รปแสดงการตรวจหาวตถในภาพจากกลองออมน

1. การหาความสมพนธจากภาพเปนการหาความสมพนธของ Feature ปจจบน เทยบกบ Feature ในภาพกอนโดยพจารณาความสมพนธจากภาพ ตวอยางอลกอรทมทหาความสมพนธของ Feature จากภาพเชน optical flow [36] จะเปนอลกอรทมซงใชการ track Feature เพอตดตามตำแหนงของ Feature ทำใหไดขอมลวา Feature จากภาพในอดต ไดเลอนมาเปน Feature ในภาพปจจบบนทตำแหนงใดบาง แลวจงสามารถกำหนดความสมพนธของ Featureปจจบนกบ Landmark ได

2. การหาความสมพนธจากขอมลการวดการหาความสมพนธจากขอมลการวด จะใชกตอเมอไมสามารถหาความสมพนธของ Feature และ landmark

จากภาพได สาเหตเนองจาก landmark ทเคยถกตรวจพบนนไดหายไปจาก Fiew of View กลองแลว แตได

กลบเขามาใน Fiew of View ของกลองอกครง ดวยเหตนจงทำใหไมสามารถใชการ track Feature ได สวน

วธหาความสมพนธนน จะใชเปรยบเทยบความตางระหวางคาการวดจดสงเกตจรง และ คาการวดทประมาณ

ไดจาก สถานะจดสงเกตของ SLAM ดวยวธ Mahalanobis distance สำหรบขนตอนการหาความสมพนธจาก

ขอมลการวดน จะตองหาหลงจากท SLAM ได Predict state เปนเรยบรอยแลวจงจะเกดความแมนยำ

5.2 การประมาณการเคลอนทของกลองออมน

เนองจากวาในงานวจยน ไมทราบคาการเคลอนทของกลองออมน อกทงยงไมทราบ Model การเคลอนทของกลองอกดวย ดงนน การทำนายการเปลยนแปลงสถานะ (predict state) ของ SLAM ยอมเปนไปไมไดเวนแตวาจะประมาณการเคลอนทของกลอง โดยอาศยขอมลจากภาพ และขอมลสถานะในอดต แลวนำไปเปนขอมลการเคลอนท สำหรบทำนายการเปลยนแปลงสถานะของ SLAM

ในขนตอนนจะเปนการประมาณตำแหนงของกลอง เทยบกบกรอบอางองหนง ๆ ซงเคยเปนตำแหนงของกลองในอดต ดงนน เมอ รความสมพนธระหวางคาการวดจดสงเกต (measurement) ณ ตำแหนงปจจบน กบ

คาการวดของกรอบอางองนน ๆ อกทงยงรคาประมาณตำแหนงของ landmark จากสถานะของ SLAM จงพอจะประมาณตำแหนงของกลองไดดวยวธ Non-Linear Least Squares

ความสมพนธของคาการวดจดสงเกต (measurement) ณ ตำแหนงปจจบน กบคาการวดของกรอบอางอง

จดท i สามารถทจะเขยน model function อยในรป yi = f(xi, β) เมอ xi เปนตวแปรตน ซงในทนจะเปนmeasurement ณ ตำแหนงปจจบน, yi เปนตวแปรตาม ซงจะเปน measurement ของกรอบอางอง และ β เปน

ตวแปรทตองการประมาณคา ในทนคอ transformation parameter ประกอบดวย การเลอนท (translation)และ และการหมน (rotation) เมอมคขอมล (xi, yi) i = 1, . . . ,m หลายค ทำใหไมสามารถหาคา β ทเหมาะสมสำหรบทกคาได จงจะประมาณ β คาททำใหผลรวมของผลตางกำลงสองระหวาง yi และ f(xi, β) ทกคนอยสด

S =m∑

i=1

r2i , ri = yi − f(xi, β) (29)

สำหรบ Non-Liner Least Squares คา parameters β หาไดจาก iterative refinement โดยท βk+1j =

βkj +∆βj เมอ k เปนจำนวนรอบการ iterate และ model function สามารถประมาณไดจาก first order Taylor

series เปน

f(xi, β) = f(xi, βk) +

∑j

∂f(xi, β)∂βj

(βj − βkj ) = f(xi, β

k) +∑

j

Jij∆βj (30)

เมอ J เปน Jacobian Matrix ของ f(xi, β) เทยบกบ β ดงนนคาผลตางระหวาง yi และ f(xi, β) เปน

ri = yi − f(xi, βk)−

∑j

Jij∆βj = ∆yi −∑

j

Jij∆βj (31)

คานอยสดของ Least Square หาไดจากการกำหนดให gradient ของ S เทยบ β เปน 0

∂S

∂βj= 2

∑i

ri∂ri∂βj

= 0, j = 1, . . . , n (32)

−2∑

i

∂f(xi, β)∂βj

ri = 0, j = 1, . . . , n (33)

−2∑

i

Jij(∆yi −∑

k

Jik∆βk) = 0, j = 1, . . . , n (34)

m∑i=1

n∑k=1

JijJik∆βk =m∑

i=1

Jij∆yi, j = 1, . . . , n (35)

ดงนนจงหาคา ∆β ไดจากสมการขางตน และทำการปรบปรงคา β ดวยวธ Iterative ไปเรอย ๆ จนกวาerror จะยอมรบได

สำหรบการประมาณการเคลอนทของกลองออมนนน อาจใช Extended Kalman Filter แบบ iteration ในการประมาณไดเชนกนซงการประมาณดวย Extended Kalman Filter นนกเปน Least Squares รปแบบหนง

5.3 การประมาณตำแหนงของกลอง และตำแหนงของจดสงเกตในแผนทดวย อลกอรทมของ SLAM

SLAM นนเปนหวใจสำคญของงานวจยน มหนาทสำคญคอการประมาณตำแหนงของกลอง, ตำแหนงของจดสงเกต รวมไปถงตำแหนงของกรอบอางอง โดยใชความนาจะเปนในการประมาณ ขนตอนของ SLAM นนประกอบดวย 3 สวนหลก คอสวน Prediction, สวน Measurement Update, สวน Landmark Augmentationและสวน Reference Frame Augmentation

สวน Prediction: เนองจากระบบไมทราบการเคลอนทของกลอง อกทงยงไมทราบ model การเคลอนทของกลองอกดวย จงทำใหทำนายสถานะของระบบไดยาก ดงนนจงไดประมาณการเคลอนทของกลองในหวขอกอนหนาน เพอเปนขอมลสำหรบการ Prediction ของ SLAM

สวน Measurement Update: จะใชคาการวดทหาไดจาก Feature จากภาพของกลองออมน มาใชในการ Update สถานะของระบบ นอกจากนจะใชคาการวดในอดต ทเกยวของกบ Reference Frame มาชวยในการ Update สถานะของระบบดวย

สวน Landmark Augmentation: จะเพม Landmark เขาไปในระบบเมอมการตรวจพบ landmark ใหม

สวน Reference Frame Augmentation: การเพม Reference Frame เขาไปในระบบนน จะเลอกจากสถานะของกลองในอดตทมระยะหางระหวาง ตำแหนงของ Reference Frame ทตองการจะเพม กบตำแหนงของ Reference Frame อน ๆ เปนระยะทางทเหมาะสม

5.4 การวดผลการทดลอง

ผลของการประมาณตำแหนงของกลอง และ สรางแผนทจากอลกอรทมทไดนำเสนอ จะถกวดผลเพอหาคาความผดพลาดของการประมาณตำแหนงดวยวธวดผล 2 วธ

1. การวดผลการประมาณเทยบกบขอมล ground truth วธนจะใชหนยนตเพอตดตงกลองออมน แลวจง

บงคบใหหนยนตวงไปตามเสนทางทกำหนดไวแลว จากนนจงเปรยบเทยบตำแหนงกลองทประมาณไดกบ เสนทางทกำหนดไว และเปรยบเทยบ ตำแหนงจดสงเกตทประมาณได กบ แผนทททราบคาอยแลว

2. การวดผลการประมาณตำแหนงของกลองจากอลกอรทมทไดนำเสนอ เทยบกบ อปกรณวดตำแหนง ท

มความเชอถอได เชน กลองทตดตงเพอวดคาแบบ bird eye view

เนองจากวาอลกอรทมทไดนำเสนอนนใชขอมลเปนภาพจากกลองออมนเพยวอยางเดยว ดงนน หนวย

ของระยะตาง ๆ ของผลลพธนนจงสามารถ Scale ขนหรอลงได ขนอยกบคาคงทของระบบทตงไว ดงนนจะ

ถอวาความคลาดเคลอนของการประมาณดวยวธวดผล 2 วธทงสองวธนน มคาเทากบ error ทนอยทสด เมอใชตวคณใด ๆ ในการ Scale คาการประมาณของอลกอรทมทไดนำเสนอ

6 ขอบเขตของงานวจย

1. งานวจยนจะพฒนาอลกอรทมทเหมาะสมสำหรบปญหาการระบตำแหนงพรอมกบการสรางแผนทของกลองออมน

2. กลองออมนสามารถเคลอนทไดอสระใน 3 มต โดยไมจำเปนตองทราบการเคลอนทและ model การเคลอนท

3. สภาพแวดลอมทสนใจจะเปน สภาพแวดลอมภายในอาคาร โดยระบบจะทำงานภายในหองปดเทานน

4. สภาพแวดลอมจะถอวา อยนงตลอดการทดลอง

5. สภาพแวดลอมจะไมมการจดสภาพไวกอน แตสภาพแวดลอมจะเปนโครงสรางทมรปแบบ มความหลากหลายและ ซบซอนของ Feature ทกลองสามารถตรวจจบได

7 ขนตอนการดำเนนงาน

1. ศกษาทฤษฏพนฐานของกลองออมน และการหาพารามเตอรการเรยงตวชนสวนของกลอง (CameraCalibration)

2. ศกษาทฤษฏพนฐานของ SLAM และศกษาวธการตรวจวดจดสงเกตสำคญ ในภาพทไดจากกลองออมน และการวดคาจดสงเกต (measurement)

3. ศกษาและพฒนาอลกอรทม สำหรบตรวจหา Features จากภาพจากกลองออมน และ การหาความสมพนธระหวาง Feature และ landmark

4. พฒนาอลกอรทมการประมาณการเคลอนทของกลองออมน

5. พฒนาอลกอรทมการประมาณตำแหนงของกลอง และตำแหนงของจดสงเกตในแผนทดวย SLAM

6. วดผลการทดลอง

7. สรปผลการทดลองพรอม จดทำรปเลม และเนอหาวทยานพนธ

8 เอกสารอางอง

[1] H. Durrant-Whyte and T. Bailey, “Simultaneous localization and mapping:part i,ii,” IEEE Robotics& Automation Magazine, vol. June, 2006.

[2] H. Tamimi, H. Andreasson, A. Treptow, T. Duckett, and A. Zell, “Localization of mobile robotswith omnidirectional vision using particle filter and iterative sift,” in In Proceedings of the 2005European Conference on Mobile Robots, 2005.

[3] J. Kim, K.-J. Yoon, J.-S. Kim, and I. Kweon, “Visual slam by single-camera catadioptric stereo,”in SICE-ICASE International Joint Conference, 2006.

[4] H. Andreasson, A. Treptow, and T. Duckett, “Localization for mobile robots using panoramicvision, local features and particle filter,” in International Conference on Robotics and Automation,2005.

[5] R. C. Smith and P. Cheeseman, “On the representation and estimation of spatial uncertainty,”The International Journal of Robotics Research, vol. 5, p. 56, 1986.

[6] P. Moutarlier and R. Chatila, “An experimental system for incremental environment modeling byan autonomous mobile robot,” in In 1st International Symposium on Experimental Robotics, 1989.

[7] R. Chatila and P. Moutarlier, “Stochastic multisensory data fusion for mobile robot location andenvironment modeling,” in In 5th Int. Symposium on Robotics Research, 1989.

[8] D. Hahnel, W. Burgard, D. Fox, and S. Thrun, “A highly efficient fastslam algorithm for generatingcyclic maps of large-scale environments from raw laser range measurements,” in InternationalConference on Intelligent Robots and Systems, 2003.

[9] M. Montemerlo, S. Thrun, D. Roller, and B. Wegbreit, “Fastslam 2.0: An improved particlefiltering algorithm for simultaneous localization and mapping that provably converges,” In Proc.of the Int. Conf. on Artificial Intelligence (IJCAI), p. 1151, 2003.

[10] X. Wang and H. Zhang, “A upf-ukf framework for slam,” in International Conference on Roboticsand Automation, 2007.

[11] M. R. Walter, R. M. Eustice, and J. J. Leonard, “Exactly sparse extended information filters forfeature-based slam,” The International Journal of Robotics Research, vol. 26, p. 335, 2007.

[12] M. Bosse, J. Leonard, and S. Teller, “Large-scale cml using a network of multiple local maps,” inWorkshop Notes of the ICRA Workshop on Concurrent Mapping and Localization for AutonomousMobile Robots, 2002.

[13] D. Fox, H. Hexmoor, and M. Mataric, “A probabilistic approach to concurrent mapping andlocalization for mobile robots,” Machine Learning and Autonomous Robots, p. 29, 1998.

[14] T. Bailey, “Mobile robot localisation and mapping in extensive outdoor environments,” Ph.D.dissertation, Australian Centre for Field Robotics Department of Aerospace, Mechanical andMechatronic Engineering The University of Sydney, 2002.

[15] S. Williams, G. Dissanayake, and H. Durrant-Whyte, “Towards terrain-aided navigation forunderwater robotics,” Advanced Robotics, 2001.

[16] S. Thrun, D. Hahnely, D. Ferguson, M. Montemerlo, R. Triebel, W. Burgard, C. Baker,Z. Omohundro, S. Thayer, and W. Whittaker, “A system for volumetric robotic mapping ofabandoned mines,” in IEEE International Conference on Robotics and Automation, 2003.

[17] S. Scheding, E. M. Nebot, M. Stevens, and H. Durrant-Whyte, “Experiments in autonomousunderground guidance,” in IEEE International Conference on Robotics and Automation, 1997.

[18] R. LI, F. MA, F. XU, L. MATTHIES, C. OLSON, and Y. XIONG, “Large scale mars mapping androver localization. using descent and rover imagery,” in Proceedings of ISPRS 19th Congress,2000.

[19] J. Uhlmann, M. Lanzagorta, and S. Julier, “The nasa mars rover: A testbed for evaluatingapplications of covariance intersection,” in In Proceedings of the SPIE 13th Annual Symposiumin Aerospace/Defence Sensing, Simulation and Controls, 1999.

[20] A. J. Davison, I. D. Reid, N. D. Molton, and O. Stasse, “Monoslam: Real-time single cameraslam,” IEEE Transactions on Pattern analysis and Machine Intelligence, vol. 29, 2007.

[21] A. C. Murillo, J. J. Guerrero, and C. Sagues, “Robot and landmark localization using sceneplanes and the 1d trifocal tensor,” in International Conference on Intelligent Robots and Systems,2006.

[22] A. Diosi and L. Kleeman, “Laser scan matching in polar coordinates with application to slam,” inInternational Conference on Intelligent Robots and Systems, 2005.

[23] W. Y. Jeong and K. M. Lee, “Visual slam with line and corner features,” in InternationalConference on Intelligent Robots and Systems, 2006.

[24] P. Gemeiner, W. Ponweiser, P. Einramhof, and M. Vincze, “Real-time slam with a high-speedcmos camera,” in International Conference on Image Analysis and Processing, 2007.

[25] C. Han, Z. Xiang, J. Liu, and E. Wu, “Stereo vision based slam in outdoor environments,” inIEEE International Conference on Robotics and Biomimetics, 2007.

[26] S. Fu, H.-y. Liu, L.-f. Gao, and Y.-x. Gai, “Slam for mobile robots using laser range finder andmonocular vision,” in International Conference on Mechatronics and Machine Vision in Practice,2007.

[27] E. Eade and T. Drummond, “Scalable monocular slam,” in IEEE Computer Society Conferenceon Computer Vision and Pattern Recognition, 2006.

[28] N. Sunderhauf, S. Lange, and P. Protzel, “Using the unscented kalman filter in mono-slam withinverse depth parametrization for autonomous airship control,” in In Proc. of IEEE InternationalWorkshop on Safety Security and Rescue Robotics, 2007.

[29] J.-H. Kim and M. J. Chung, “Slam with omni-directional stereo vision sensor,” in InternationalConference on Intelligent Robots and Systems, 2003.

[30] C. Valgren, A. Lilienthal, and T. Duckett, “Incremental topological mapping using omnidirectionalvision,” in International Conference on Intelligent Robots and Systems, 2006.

[31] M. Saedan, C. W. Lim, and M. H. Ang, “Appearance-based slam with map loop closing usingan omnidirectional camera,” in Advanced Intelliget Mechatronics, 2007.

[32] D. Scaramuzza, A. Martinelli, and R. Siegwart, “A flexible technique for accurate omnidirectionalcamera calibration and structure from motion,” in IEEE International Conference on ComputerVision Systems, 2006.

[33] D. Scaramuzza, R. Siegwart, and A. Martinelli, “A toolbox for easily calibrating omnidirectionalcameras,” in International Conference on Intelligent Robots and Systems, 2006.

[34] X. Ying and Z. Hu, “Catadioptric line features detection using hough transform,” in InternationalConference on Pattern Recognition, 2004.

[35] D. G. Lowe, “Distinctive image features from scale-invariant keypoints,” International Journal ofComputer Vision, 2004.

[36] J.-Y. Bouguet, “Pyramidal implementation of the lucas kanade feature tracker description of thealgorithm,” Intel Corporation, Microprocessor Research Labs, Tech. Rep., 2000.