วิธีการหลอมข้อมูลเชิงเส้นและเชิงมุมจากเซ็นเซอร์

ทีมของฉันและฉันกำลังตั้งค่าหุ่นยนต์กลางแจ้งที่มีตัวเข้ารหัสIMUเชิงพาณิชย์และGPSเซ็นเซอร์หุ่นยนต์มีไดรฟ์ถังพื้นฐานดังนั้นเครื่องเข้ารหัสจะจัดหาเห็บเพียงพอจากล้อซ้ายและขวา IMU แสดงการหมุน, พิทช์, หันเหและการเร่งความเร็วเชิงเส้นใน x, y และ z เราสามารถเพิ่ม IMU อื่น ๆ ในภายหลังซึ่งจะให้ความซ้ำซ้อน แต่ยังสามารถให้อัตราการหมุนมุมและพิสัยได้เพิ่มเติม GPS เผยแพร่พิกัด x, y และ z ทั่วโลก

การรู้ตำแหน่งและหัวเรื่องของหุ่นยนต์จะเป็นประโยชน์สำหรับหุ่นยนต์ในการแปลและแผนที่สภาพแวดล้อมในการนำทาง ความเร็วของหุ่นยนต์อาจเป็นประโยชน์สำหรับการตัดสินใจการเคลื่อนไหวที่ราบรื่น มันเป็นหุ่นยนต์ภาคพื้นดังนั้นเราจึงไม่สนใจแกน z มากเกินไป หุ่นยนต์ยังมีเซ็นเซอร์lidarและกล้องดังนั้นการหมุนและระยะพิทช์จะเป็นประโยชน์สำหรับการแปลงข้อมูล lidar และกล้องเพื่อให้ได้แนวที่ดีขึ้น

ฉันกำลังพยายามหาวิธีหลอมตัวเลขทั้งหมดเหล่านี้เข้าด้วยกันในวิธีที่ใช้ประโยชน์จากความแม่นยำของเซ็นเซอร์ทั้งหมดอย่างเหมาะสม ตอนนี้เรากำลังใช้ตัวกรองคาลมานเพื่อสร้างการประมาณ[x, x-vel, x-accel, y, y-vel, y-accel]ด้วยเมทริกซ์การเปลี่ยนแปลงอย่างง่าย:

[[1, dt, .5*dt*dt, 0,  0,        0],
 [0,  1,       dt, 0,  0,        0],
 [0,  0,        1, 0,  0,        0],
 [0,  0,        0, 1, dt, .5*dt*dt],
 [0,  0,        0, 0,  1,       dt],
 [0,  0,        0, 0,  0,        1]]

ตัวกรองจะประเมินสถานะเฉพาะตามการเร่งความเร็วที่ได้รับจาก IMU (IMU ไม่ใช่คุณภาพที่ดีที่สุดภายในประมาณ 30 วินาทีมันจะแสดงหุ่นยนต์ (ที่เหลือ) ลอยไปในระยะ 20 เมตรจากตำแหน่งเริ่มต้น) ฉันต้องการทราบวิธีใช้ม้วนสนามและหันเหจาก IMU และอัตราการหมุนระยะห่างและอัตราการหันเหข้อมูลเข้ารหัสจากล้อและข้อมูล GPS เพื่อปรับปรุงการประเมินสถานะ

ด้วยการใช้คณิตศาสตร์เราสามารถใช้ตัวเข้ารหัสสองตัวเพื่อสร้าง x, y และหัวเรื่องข้อมูลบนหุ่นยนต์รวมถึงความเร็วเชิงเส้นและเชิงมุม ตัวเข้ารหัสมีความแม่นยำมาก แต่พวกเขาสามารถไวต่อการลื่นไถลบนสนามกลางแจ้ง

สำหรับฉันดูเหมือนว่ามีชุดข้อมูลสองชุดแยกกันซึ่งยากที่จะหลอมรวม:

1. ค่าประมาณของ x, x-vel, x-accel, y, y-vel, y-accel
2. การประมาณการหมุนระยะพิทซ์และอัตราการหมุนพิทช์และการหันเห

แม้ว่าจะมีครอสโอเวอร์ระหว่างสองชุดนี้ แต่ฉันมีปัญหาในการหาเหตุผลมารวมกัน ตัวอย่างเช่นหากหุ่นยนต์ไปที่ความเร็วคงที่ทิศทางของหุ่นยนต์ที่กำหนดโดย x-vel และ y-vel จะเหมือนกับ yaw ของมัน แม้ว่าหากหุ่นยนต์หยุดพักการหันเหไม่สามารถกำหนดความเร็วและ x ได้อย่างแม่นยำ นอกจากนี้ข้อมูลที่ได้จากเครื่องเข้ารหัสซึ่งแปลเป็นความเร็วเชิงมุมอาจเป็นการปรับปรุงอัตราการหันเห ...

การใส่ตัวเลขทั้ง 12 ลงในตัวกรองเดียวกันนั้นเป็นเรื่องปกติหรือไม่ มีวิธีการพัฒนาที่ดีในการจัดการกับปัญหาประเภทนี้หรือไม่?

สองสิ่ง.

1. หากคุณวางแผนที่จะทำการแมปคุณจำเป็นต้องใช้อัลกอริทึมการแปลและการแมป (SLAM) พร้อมกันเต็มเปี่ยม ดู: พร้อมกันรองรับหลายภาษาและการทำแผนที่ (สแลม): Part I สำคัญอัลกอริทึม ใน SLAM การประเมินสถานะของหุ่นยนต์เป็นปัญหาเพียงครึ่งเดียว วิธีการทำนั้นเป็นคำถามที่ใหญ่กว่าที่สามารถตอบได้ที่นี่

2. เกี่ยวกับการแปล (ประมาณสถานะของหุ่นยนต์) นี่ไม่ใช่งานของตัวกรองคาลมาน การเปลี่ยนแปลงจาก เพื่อx ( T + 1 $x(t)=[x,y,\dot{x},\dot{y},\theta,\dot{\theta}]$$x(t+1)$ไม่ใช่ฟังก์ชันเชิงเส้นเนื่องจากการเร่งเชิงมุมและความเร็ว ดังนั้นคุณต้องพิจารณาตัวประมาณค่าแบบไม่เชิงเส้นสำหรับงานนี้ ใช่มีวิธีมาตรฐานในการทำเช่นนี้ ใช่พวกเขามีอยู่ในวรรณกรรม ใช่โดยปกติแล้วอินพุตทั้งหมดจะใส่ลงในตัวกรองเดียวกัน ตำแหน่ง, ความเร็ว, ทิศทางและความเร็วเชิงมุมของหุ่นยนต์ถูกใช้เป็นเอาท์พุท และใช่ฉันจะนำเสนอสั้น ๆ เกี่ยวกับชุดรูปแบบทั่วไปของพวกเขาที่นี่ เส้นทางขึ้นเครื่องหลักคือ

   1. รวมอคติ Gyro และ IMU ในรัฐของคุณไม่เช่นนั้นการประมาณของคุณจะเปลี่ยนไป
   2. ขยายคาลมานกรอง (EKF)เป็นที่นิยมใช้สำหรับปัญหานี้
   3. การใช้งานสามารถเกิดขึ้นได้จากศูนย์และไม่จำเป็นต้อง "ค้นหา"
   4. Implementaitons มีอยู่สำหรับการแปลส่วนใหญ่และปัญหา SLAM ดังนั้นอย่าทำงานมากกว่าที่คุณต้องทำ โปรดดูที่: ระบบปฏิบัติการหุ่นยนต์ROS

ตอนนี้เพื่ออธิบาย EKF ในบริบทของระบบของคุณ เรามี IMU + Gyro, GPS และ odometry หุ่นยนต์ที่มีปัญหานั้นเป็นไดรฟ์ที่ต่างกันดังที่กล่าวไว้ งานการกรองคือการใช้การประมาณการท่าปัจจุบันของหุ่นยนต์ xเสื้อปัจจัยการผลิตการควบคุมยูที , และการวัดจากเซ็นเซอร์แต่ละที่ซีทีและการผลิตประมาณการในขั้นตอนเวลาถัดไป x T + 1 เราจะเรียกวัด IMU ฉันt , จีพีเอสจีทีและ odometry, โอที${\hat{x}_t}$$u_t$$z_t$$\hat{x}_{t+1}$$I_t$$G_t$$O_t$

ผมถือว่าเรามีความสนใจในการประมาณท่าหุ่นยนต์เป็น θ ปัญหาเกี่ยวกับ IMU และ Gyros นั้นล่องลอยไป มีอคติไม่คงที่ในการเร่งความเร็วซึ่งคุณต้องคำนึงถึงใน EKF สิ่งนี้เสร็จสิ้น (โดยปกติ) โดยการวางอคติลงในสถานะโดยประมาณ สิ่งนี้ช่วยให้คุณสามารถประเมินอคติโดยตรงในแต่ละขั้นตอน x T = x , Y , ˙ x , ˙ Y , θ , ˙ θ , $x_t={x,y,\dot{x},\dot{y},\theta,\dot{\theta}}$$x_t={x,y,\dot{x},\dot{y},\theta,\dot{\theta},b}$สำหรับเวกเตอร์ของอคติข$b$

ฉันสมมติว่า:

1. = การวัดระยะทางสองทางแทนระยะทางที่ดอกยางเดินทางไปด้วยการเพิ่มขึ้นของเวลาเล็กน้อย$O_t$
2. = วัดสามปฐมนิเทศแอลฟา, β , θและสาม accelleration วัด ¨ x , ¨ Y , ¨ Z$I_t$${\alpha, \beta, \theta}$${\ddot{x},\ddot{y},\ddot{z}}$
3. = ตำแหน่งของหุ่นยนต์ในที่ทั่วโลกกรอบ G x T , G Yเสื้อ$G_t$$^Gx_t,^Gy_t$

โดยทั่วไปแล้วผลลัพธ์ของอินพุตควบคุม (ความเร็วที่ต้องการสำหรับแต่ละดอกยาง) เป็นการยากที่จะแมปกับเอาท์พุท (การเปลี่ยนแปลงในท่าทางของหุ่นยนต์) ในสถานที่ของมันเป็นเรื่องธรรมดา (ดูThrun , Odometry คำถาม ) เพื่อใช้ odometry เป็น "ผล" ของการควบคุม สมมติฐานนี้ใช้ได้ดีเมื่อคุณไม่ได้อยู่บนพื้นผิวที่ไม่มีแรงเสียดทาน IMU และ GPS สามารถช่วยแก้ไขการลื่นไถลได้ดังที่เราจะเห็น$u$

x T + 1 = F ( x T , U T ) ในกรณีของหุ่นยนต์ค่าไดรฟ์, คำทำนายนี้สามารถรับโดยตรงจากวรรณกรรม (ดูในจลนศาสตร์ของล้อยางมือถือหุ่นยนต์หรือการรักษาที่รัดกุมมากขึ้นในตำราหุ่นยนต์ที่ทันสมัย) หรือมาจากรอยขีดข่วนตามที่แสดงไว้ที่นี่: Odometry คำถาม$\hat{x}_{t+1} = f(\hat{x}_t,u_t)$

ดังนั้นตอนนี้เราสามารถคาดการณ์x T + 1 = F ( x T , O T ) นี่คือขั้นตอนการแพร่กระจายหรือการทำนาย คุณสามารถใช้งานหุ่นยนต์ได้โดยการเผยแพร่ ถ้าค่าโอทีมีความถูกต้องสมบูรณ์คุณจะไม่ได้มีการประมาณการxซึ่งไม่เท่ากับรัฐที่แท้จริงของคุณ สิ่งนี้ไม่เคยเกิดขึ้นในทางปฏิบัติ$\hat{x}_{t+1} = f(\hat{x}_t, O_t)$$O_t$$\hat{x}$

สิ่งนี้ให้ค่าที่คาดการณ์จากการประมาณการครั้งก่อนเท่านั้นและไม่ได้บอกเราว่าความแม่นยำของการประมาณค่าลดลงตามเวลาอย่างไร ดังนั้นเพื่อเผยแพร่ความไม่แน่นอนคุณต้องใช้สมการ EKF (ซึ่งเผยแพร่ความไม่แน่นอนในรูปแบบปิดภายใต้สมมติฐานเสียงเกาส์เซียน) ตัวกรองอนุภาค (ซึ่งใช้วิธีการสุ่มตัวอย่างตาม) *, UKF (ซึ่งใช้จุดที่ชาญฉลาด การประมาณความไม่แน่นอน) หรือหนึ่งในตัวแปรอื่น ๆ

ในกรณีของ EKF เราดำเนินการดังนี้ ให้เป็นเมทริกซ์ความแปรปรวนร่วมของสถานะหุ่นยนต์ เราปรับฟังก์ชั่นเชิงเส้นf โดยใช้การขยายแบบเทย์เลอร์เพื่อให้ได้ระบบเชิงเส้น ระบบเชิงเส้นสามารถแก้ไขได้อย่างง่ายดายโดยใช้ตัวกรองคาลมาน สมมติแปรปรวนของประมาณการในขณะที่เสื้อเป็นP Tและความแปรปรวนของปลอมเสียงใน odometry ที่จะได้รับเป็นเมทริกซ์ยูที (มักจะเป็นเส้นทแยงมุม2 × 2เมทริกซ์เช่น0.1 × ฉัน2 × 2 ) ในกรณีของฟังก์ชันfเราจะได้รับยาโคบ$P_t$$f$$t$$P_t$$U_t$$2\times2$$.1\times I_{2\times 2}$$f$ และFu=∂$F_x=\frac{\partial f}{\partial x}$แล้วเผยแพร่ความไม่แน่นอนเป็น$F_u=\frac{\partial f}{\partial u}$

ตอนนี้เราสามารถเผยแพร่ประมาณการและความไม่แน่นอน สังเกตว่าความไม่แน่นอนนั้นจะเพิ่มขึ้นตามเวลา สิ่งนี้คาดว่า ในการแก้ไขปัญหานี้สิ่งที่ทำได้โดยทั่วไปคือใช้และG tเพื่ออัปเดตสถานะที่คาดการณ์ไว้ สิ่งนี้เรียกว่าขั้นตอนการวัดของกระบวนการกรองเนื่องจากเซ็นเซอร์ให้การวัดทางอ้อมของหุ่นยนต์$I_t$$G_t$

ก่อนอื่นให้ใช้เซ็นเซอร์แต่ละตัวเพื่อประเมินสถานะของหุ่นยนต์บางส่วนเนื่องจากฟังก์ชั่นและh i ( )สำหรับ GPS, IMU รูปแบบที่เหลือหรือนวัตกรรมซึ่งเป็นความแตกต่างของค่าที่คาดการณ์และวัด จากนั้นให้ประเมินความแม่นยำของเซ็นเซอร์แต่ละตัวในรูปของเมทริกซ์ความแปรปรวนร่วมRสำหรับเซ็นเซอร์ทั้งหมด ( R g , R iในกรณีนี้) ในที่สุดค้นหา Jacobians ของhและอัปเดตการประเมินสถานะดังนี้$h_g()$$h_i()$$R$$R_g$$R_i$$h$

สำหรับแต่ละเซ็นเซอร์กับรัฐประมาณการZ s ( ต่อไปนี้รายการวิกิพีเดีย )$s$$z_s$

S s = H s * P T + 1 * H T s + R s K = P T + 1 * H T s S - 1 s x T + 1 = x T + 1 - K *

$z_g=h_g()$$H_g$$R_g$

$z_i=h_i()$$p_i$$R_i$$p_i$. การรวมการอัปเดตสำหรับอคตินั้นยากขึ้นและออกจากความเชี่ยวชาญของฉัน อย่างไรก็ตามเนื่องจากคุณมีความสนใจในการเคลื่อนที่แบบระนาบคุณอาจจะทำให้ปัญหาง่ายขึ้น คุณจะต้องมองหาวรรณกรรมในเรื่องนี้

การอ้างอิงที่ไม่ได้อยู่บนหัวของฉัน:

1. การปรับปรุงความถูกต้องของ Odometry Visual-Inertial-Inertial

2. เครื่องมือประมาณ EKF ที่สอดคล้องกับความสามารถในการสังเกตสำหรับการแปลหลายภาษาแบบร่วมมือกันหลายหุ่นยนต์

3. ปรับ EKF สองขั้นตอนสำหรับระบบคู่ INS-GPS แบบหลวม ๆ พร้อมกับอคติผิดพลาดที่ไม่รู้จัก

ฟิลด์นี้มีความเป็นผู้ใหญ่มากพอที่ Google (นักวิชาการ) อาจพบว่าคุณใช้งานได้จริง หากคุณกำลังจะทำงานมากในพื้นที่นี้ฉันขอแนะนำให้คุณรับหนังสือเรียนที่มั่นคง อาจเป็นได้ว่าหุ่นยนต์ Probablistic โดย S. Thrunจากชื่อเสียงของ Google Car (ฉันพบว่ามันเป็นข้อมูลอ้างอิงที่เป็นประโยชน์สำหรับการนำไปใช้ในช่วงดึก)

* มีตัวประมาณ PF หลายตัวที่มีอยู่ใน ระบบปฏิบัติการหุ่นยนต์ (ROS) อย่างไรก็ตามสิ่งเหล่านี้ได้รับการปรับให้เหมาะกับการใช้งานภายในอาคาร ตัวกรองอนุภาคจัดการกับ PDF แบบโมดัลซึ่งอาจเป็นผลมาจากการแปลตามแผนที่ (ฉันอยู่ใกล้ประตูนี้หรือประตูนั้น ) ฉันเชื่อว่าการใช้งานกลางแจ้งส่วนใหญ่ (โดยเฉพาะอย่างยิ่งที่สามารถใช้ GPS อย่างน้อยเป็นระยะ ๆ ) ใช้ตัวกรองแบบขยาย Kalman (EKF) ฉันใช้ตัวกรองแบบขยายของคาลมานได้สำเร็จสำหรับรถแลนด์โรเวอร์บนพื้นดินที่มีไดรฟ์ต่าง

คุณสามารถทำให้ปัญหาง่ายขึ้นอย่างมากในกรณีทั่วไปส่วนใหญ่:

• IMus "เกรดเชิงพาณิชย์" จำนวนมาก (เช่น Xsens) มีเครื่องเร่งความเร็วที่มีเสียงดังมาก อย่าแม้แต่จะหลอมรวมพวกมันเพื่อให้ได้ความเร็ว แต่ Odometry ก็มีขนาดเท่ากันแล้ว ข้อมูลที่ใช้ได้เพียงอย่างเดียวที่ IMU กำลังจะจัดเตรียมคือ pitch และ roll และในระดับหนึ่งส่วนหัว (ดูจุดถัดไป)
• ส่วนหัวจาก IMU นั้นไม่น่าเชื่อถือ มันใช้ magetometers และจะแสดงก้อนหินขนาดใหญ่ (สูงถึง 25 องศาในระยะ 2 เมตรในกรณีของเรา) ใกล้กับ ferromagnetic เช่นเช่นที่คุณสามารถพบได้ในผนังอาคาร สิ่งที่เราทำเพื่อแก้ไขปัญหานี้คือการใช้ส่วนหัว IMU แต่ประมาณอคติส่วนหัว
• หากคุณอยู่นอกอาคารอย่าลืมว่าการเดินทาง 10 ม. บนความลาดเอียง 10 องศานั้นไม่ได้นำไปสู่การเปลี่ยนแปลงใน X และ Y แบบเดียวกันมากกว่าการเดินทาง 10 ม. บนภูมิประเทศที่ราบเรียบ สิ่งนี้มักจะคิดโดยการประมาณ Z แต่ฉันเดาว่ามันสามารถประมาณได้แตกต่างกัน
• จีพีเอสเป็นสุนัขตัวเมียที่โกหก บวกกับ GPS คุณภาพต่ำ (และแม้กระทั่งในบางเงื่อนไข) GPSes มีแนวโน้มที่จะรายงานค่าเบี่ยงเบนมาตรฐานที่ผิดมาก เราใช้การทดสอบไคสแควร์ง่ายๆเพื่อตรวจสอบว่าการวัด GPS โดยเฉพาะควรรวมหรือไม่ (เช่นการตรวจสอบว่าตรงกับประมาณการปัจจุบันของตัวกรองถึงจุดหนึ่ง) ซึ่งทำให้เราได้ผลลัพธ์ที่ดี

วิธีแก้ปัญหา "ทั่วไป" สำหรับเราคือการใช้ odometry + IMU เพื่อรับการประเมินอัตตาการเคลื่อนไหวจากนั้นใช้ GPS เพื่อแก้ไข X, Y, Z และการตั้งค่าส่วนหัว

นี่คือการใช้ EKF ที่เราใช้อย่างกว้างขวาง หากคุณต้องการที่จะประเมินการวางแนวของ IMU (คือถ้ามันไม่ได้มีอยู่แล้วในตัวกรอง) คุณยังสามารถใช้งานบนของทั้งสองตัวกรอง: ยูเคเอฟและEKF