ตัวกรองคาลมานเสริมด้วยเลเซอร์สแกน + แผนที่ที่รู้จัก

ขณะนี้ฉันกำลังทำงานในโครงการสำหรับโรงเรียนที่ฉันจำเป็นต้องใช้ตัวกรองคาลมานเพิ่มเติมสำหรับหุ่นยนต์จุดด้วยเครื่องสแกนเลเซอร์ หุ่นยนต์สามารถหมุนได้ด้วยรัศมีวงเลี้ยว 0 องศาและขับไปข้างหน้า การเคลื่อนไหวทั้งหมดเป็นแบบเชิงเส้นเป็นชิ้น ๆ (ไดรฟ์, หมุน, ขับ)

เครื่องจำลองที่เราใช้ไม่รองรับการเร่งความเร็วการเคลื่อนที่ทั้งหมดเป็นแบบทันที

นอกจากนี้เรายังมีแผนที่ที่รู้จัก (ภาพ png) ที่เราต้องทำการแปลเราสามารถทำการติดตามรอยในภาพเพื่อจำลองการสแกนด้วยเลเซอร์

คู่ของฉันและฉันสับสนเล็กน้อยเกี่ยวกับรูปแบบการเคลื่อนไหวและเซ็นเซอร์ที่เราจะต้องใช้

จนถึงตอนนี้เรากำลังจำลองสถานะเป็นเวกเตอร์ $(x,y,\theta)$.

เราใช้สมการการปรับปรุงดังนี้

void kalman::predict(const nav_msgs::Odometry msg){
    this->X[0] += linear * dt * cos( X[2] ); //x
    this->X[1] += linear * dt * sin( X[2] ); //y
    this->X[2] += angular * dt; //theta

    this->F(0,2) = -linear * dt * sin( X[2] ); //t+1 ?
    this->F(1,2) =  linear * dt * cos( X[2] ); //t+1 ?

    P = F * P * F.t() + Q;

    this->linear = msg.twist.twist.linear.x;
    this->angular = msg.twist.twist.angular.z;
    return;
}

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

สำหรับโมเดลการเคลื่อนที่เราใช้เมทริกซ์ต่อไปนี้สำหรับ F:

$F = \begin{bmatrix}1 & 0 & -v*\Delta t*sin(\theta) \\ 0 & 1 & v*\Delta t*cos(\theta) \\ 0 & 0 & 1 \end{bmatrix}$

ในฐานะที่เป็น Jacobian ของสูตรการอัพเดทของเรา ถูกต้องหรือไม่

สำหรับโมเดลเซนเซอร์เราประมาณ Jacobian (H) ด้วยการรับตำแหน่งที่แตกต่างกันของหุ่นยนต์ ,และและการติดตามรังสีในแผนที่ เราคุยกับ TA ผู้ซึ่งกล่าวว่าสิ่งนี้จะได้ผล แต่ฉันก็ยังไม่แน่ใจ ศาสตราจารย์ของเราไม่อยู่ดังนั้นเราจึงไม่สามารถถามเขาได้อย่างน่าเสียดาย เราใช้การวัดเลเซอร์ 3 ครั้งต่อการแก้ไขดังนั้น H คือ 3x3$x$$y$$\theta$

ปัญหาอื่น ๆ ที่มีวิธีการเตรียมใช้งาน P เราลอง 1,10,100 และพวกเขาทั้งหมดวางหุ่นยนต์นอกแผนที่ที่ (-90, -70) เมื่อแผนที่มีขนาด 50x50 เท่านั้น

รหัสสำหรับโครงการของเราสามารถพบได้ที่นี่: https://github.com/en4bz/kalman/blob/master/src/kalman.cpp

คำแนะนำใด ๆ ที่ชื่นชมอย่างมาก

แก้ไข:

ณ จุดนี้ฉันได้รับตัวกรองเพื่อทำให้เกิดความมั่นคงด้วยเสียงการเคลื่อนไหวขั้นพื้นฐาน แต่ไม่มีการเคลื่อนไหวจริง ทันทีที่หุ่นยนต์เริ่มเคลื่อนที่ตัวกรองจะเปลี่ยนทิศทางอย่างรวดเร็วและออกจากแผนที่

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

ฉันอยากจะแนะนำให้ดูที่ Monte Carlo Localization (ตัวกรองอนุภาค) สิ่งนี้จะไม่เพียงแก้ปัญหาด้วยรูปแบบการวัดของคุณ แต่ยังอนุญาตการแปลทั่วโลก (ไม่ทราบตำแหน่งเริ่มต้นภายในแผนที่โดยสมบูรณ์)

แก้ไข: ฉันขอโทษฉันลืมพูดถึงจุดสำคัญอื่น:

• หากคุณต้องการใช้ EKF รูปแบบการวัดของคุณรวมถึงรูปแบบการเคลื่อนไหวของคุณอาจมีเพียงเสียงเกาส์เซียนเท่านั้น ซึ่งหมายความว่าคุณต้องสามารถจดโมเดลการวัดของคุณเป็น$z_t = h(x_t) + N(0, Q_t)$. นี่เป็นข้อ จำกัด ที่รุนแรงเนื่องจากไม่อนุญาตให้มีแบบจำลองที่ซับซ้อนกว่าเล็กน้อยเช่น beam_range_finder_model ใน Probabilistic Robotics ซึ่งพิจารณาวัตถุแบบไดนามิกที่อยู่ด้านหน้าของหุ่นยนต์การวัดที่ไม่ถูกต้อง (สูงสุด) และการอ่านแบบสุ่มอย่างสมบูรณ์ คุณจะติดอยู่กับรังสีหล่อสำหรับ$h(x_t)$ เป็นส่วนหนึ่งและเพิ่มเสียงเสียน

ก่อนอื่นคุณไม่ได้พูดถึงชนิดของการแปลที่คุณใช้ มันมีการติดต่อที่รู้จักหรือไม่รู้จักหรือไม่?

ตอนนี้เพื่อรับ Jacobian ใน Matlab คุณสามารถทำสิ่งต่อไปนี้

>> syms x y a Vtran Vrotat t
>> f1 = x + Vtran*t*cos(a);
>> f2 = y + Vtran*t*sin(a);
>> f3 = a + Vrotat*t;
>> input  = [x y a];
>> output = [f1 f2 f3];
>> F = jacobian(output, input)

ผลลัพธ์

F =
[ 1, 0, -Vtran*t*sin(a)]
[ 0, 1,  Vtran*t*cos(a)]
[ 0, 0,               1]

ตัวกรองคาลมานแบบขยายต้องการให้ระบบเป็นเส้นตรงและเสียงจะเป็นแบบเกาส์เซียน ระบบที่นี่หมายถึงโมเดลการเคลื่อนที่และการสังเกตต้องเป็นแบบเส้นตรง เซ็นเซอร์เลเซอร์ให้ระยะและมุมไปยังจุดสังเกตจากกรอบของหุ่นยนต์ดังนั้นโมเดลการวัดจึงไม่เชิงเส้น เกี่ยวกับPถ้าคุณคิดว่ามันมีขนาดใหญ่ตัวประมาณค่า EKF ของคุณจะไม่ดีในตอนเริ่มต้นและขึ้นอยู่กับการวัดเนื่องจากเวกเตอร์สถานะก่อนหน้านี้ไม่พร้อมใช้งาน อย่างไรก็ตามเมื่อหุ่นยนต์ของคุณเริ่มเคลื่อนไหวและตรวจจับได้ EKF จะเริ่มดีขึ้นเนื่องจากใช้โมเดลการเคลื่อนที่และการวัดเพื่อประเมินท่าทางของหุ่นยนต์ หากหุ่นยนต์ของคุณไม่ตรวจจับจุดสังเกตใด ๆ ความไม่แน่นอนก็จะเพิ่มขึ้นอย่างมาก นอกจากนี้คุณต้องระวังมุมด้วย ใน C ++cos and sinมุมควรเป็นเรเดียน ไม่มีอะไรในรหัสของคุณเกี่ยวกับปัญหานี้ หากคุณสมมุติว่าเสียงของมุมเป็นองศาขณะที่การคำนวณเป็นเรเดียนคุณอาจได้ผลลัพธ์ที่แปลกเพราะเสียงหนึ่งองศาในขณะที่การคำนวณเรเดียนนั้นถือว่ามาก