مسیر یادگیری ROS 2  ·  کتاب آموزشی تصویری

فصل نهم: Odometry و Sensor Fusion

وقتی یک حسگر به‌تنهایی کافی نیست
پیش‌نیاز: فصل ۵ و ۸
پروژه پیوسته: ربات ARCHO
ابزار: robot_localization (EKF)
زمان مطالعه: ۹۰ تا ۱۱۰ دقیقه
در این فصل چه می‌خوانیم ۹.۱سؤال ساده: ARCHO چقدر حرکت کرده؟ ۹.۲Wheel Odometry و مشکل Drift ۹.۳IMU: حسگر دومی که کمک می‌کند ۹.۴Sensor Fusion با robot_localization ۹.۵Covariance: زبان عدم قطعیت ۹.۶چرا LiDAR خام وارد EKF نمی‌شود ۹.۷بازرسی نتیجه Fusion ۹.۸جمع‌بندی، واژه‌نامه و تمرین‌ها

۹.۱سؤال ساده: ARCHO چقدر حرکت کرده؟

در فصل هشتم، diff_drive_controller از روی چرخش Encoderهای دو چرخ، مسیر ARCHO را تخمین زد و روی /odom منتشر کرد. این خیلی خوب به‌نظر می‌رسد — تا وقتی که یک روز متوجه شوی بعد از نیم ساعت حرکت در انبار، موقعیت گزارش‌شده ربات با موقعیت واقعی‌اش چند متر فاصله دارد. این پدیده Drift نام دارد و دقیقاً همان مشکلی است که این فصل حلش می‌کند.

🧠 چرا یک حسگر به‌تنهایی کافی نیست

Wheel Odometry مثل شمردن قدم‌هایت با چشم بسته است — دقیق و سریع در کوتاه‌مدت، اما هر لغزش کوچک چرخ روی زمین به خطای تجمعی تبدیل می‌شود. IMU مثل حس تعادل گوش داخلی توست — می‌فهمد چقدر چرخیده‌ای، اما نمی‌داند چقدر جلو رفته‌ای. هیچ‌کدام به‌تنهایی کامل نیستند؛ اما وقتی با هم ترکیب شوند، نقاط ضعف یکدیگر را جبران می‌کنند. این ترکیب را Sensor Fusion می‌گویند.

۹.۲Wheel Odometry و مشکل Drift

ببینیم دقیقاً چه چیزی روی /odom منتشر می‌شود:

dev@archo:~$ ros2 topic echo /diff_drive_controller/odom --once pose: pose: position: {x: 1.842, y: 0.113, z: 0.0} ... covariance: [0.001, 0, 0, ...] twist: twist: linear: {x: 0.3, y: 0.0, z: 0.0} angular: {z: 0.02}

پیام Odometry سه بخش اصلی دارد:

بخشمعنی
poseموقعیت و جهت تخمینی فعلی ربات
twistسرعت خطی و زاویه‌ای لحظه‌ای
covarianceمیزان عدم قطعیت این تخمین (بخش بعد به‌طور کامل توضیح می‌دهد)
⚠️ از کجا Drift می‌آید

Wheel Odometry فرض می‌کند چرخ‌ها دقیقاً به اندازه چرخش‌شان روی زمین می‌لغزند — نه بیشتر، نه کمتر. اما در واقعیت، سطح انبار ممکن است لیز باشد، چرخ‌ها کمی روی جا بچرخند، یا شعاع واقعی چرخ به‌خاطر سایش کمی از مقدار نوشته‌شده در controllers.yaml فصل قبل فرق داشته باشد. هر کدام از این خطاهای کوچک، لحظه به لحظه جمع می‌شوند — دقیقاً مثل خطای گرد‌کردن در یک محاسبه طولانی.

۹.۳IMU: حسگر دومی که کمک می‌کند

IMU (که در فصل چهارم Frame فیزیکی‌اش را ساختیم) شتاب خطی و سرعت زاویه‌ای را با نرخ بالا اندازه می‌گیرد. برخلاف Wheel Odometry، IMU هیچ فرضی درباره لغزش چرخ ندارد — مستقیم حرکت واقعی بدنه را حس می‌کند. اما IMU هم ضعف خودش را دارد: نویز بالا در بلندمدت باعث می‌شود تخمین موقعیتش به‌تنهایی هم Drift کند، فقط از نوع دیگری.

حسگرنقطه قوتنقطه ضعف
Wheel Odometryدقیق در کوتاه‌مدت، نرخ پایدارلغزش چرخ باعث Drift تجمعی می‌شود
IMUمستقیماً چرخش و شتاب واقعی بدنه را حس می‌کندنویزی و مستعد Drift در بلندمدت، مخصوصاً برای موقعیت

۹.۴Sensor Fusion با robot_localization

برای ترکیب این دو منبع، از یک بسته استاندارد ROS 2 به‌نام robot_localization استفاده می‌کنیم که یک فیلتر کالمن توسعه‌یافته (EKF — Extended Kalman Filter) پیاده‌سازی می‌کند.

flowchart LR A["Wheel Odometry
/diff_drive_controller/odom"] --> C["ekf_filter_node"] B["IMU
/imu/data"] --> C C --> D["/odometry/filtered"] C --> E["Transform: odom → base_link"] style C fill:#eef0ff,stroke:#3d4bf5,color:#211f1a,font-weight:bold style D fill:#eafaf3,stroke:#0e9e6e,color:#211f1a 
# archo_bringup/config/ekf.yaml
ekf_filter_node:
  ros__parameters:
    frequency: 50.0
    sensor_timeout: 0.1
    two_d_mode: true
    publish_tf: true
    map_frame: map
    odom_frame: odom
    base_link_frame: base_link
    world_frame: odom

    odom0: /diff_drive_controller/odom
    odom0_config: [false, false, false,
                   false, false, false,
                   true,  true,  false,
                   false, false, true,
                   false, false, false]

    imu0: /imu/data
    imu0_config: [false, false, false,
                  false, false, true,
                  false, false, false,
                  false, false, true,
                  true,  false, false]
    imu0_remove_gravitational_acceleration: true
📖 چطور این ماتریس‌های true/false را بخوانیم

هر _config پانزده مقدار true/false دارد که به ترتیب به X، Y، Z، Roll، Pitch، Yaw، Vx، Vy، Vz، Vroll، Vpitch، Vyaw، Ax، Ay، Az اشاره می‌کنند. true یعنی «از این مقدار این حسگر استفاده کن». مثلاً برای odom0، فقط Vx و Vy و Vyaw (سرعت خطی و زاویه‌ای) روشن است — چون به سرعت چرخ‌ها اعتماد داریم، نه به موقعیت مطلق آن‌ها. برای imu0، Yaw و Vyaw و شتاب طولی (Ax) روشن‌اند — چون IMU در تشخیص چرخش دقیق‌تر از حرکت خطی است.

🔧 چرا imu0_remove_gravitational_acceleration مهم است

شتاب‌سنج IMU همیشه گرانش زمین را هم اندازه می‌گیرد، حتی وقتی ربات کاملاً ساکن است. اگر این جزء حذف نشود، فیلتر فکر می‌کند ربات دائماً در حال شتاب‌گرفتن است — یکی از رایج‌ترین دلایل «ربات ساکنی که در RViz آرام‌آرام سر می‌خورد».

۹.۵Covariance: زبان عدم قطعیت

Covariance عددی است که می‌گوید یک اندازه‌گیری چقدر قابل‌اعتماد است. عدد بسیار کوچک یعنی «تقریباً مطمئنم»؛ عدد بزرگ یعنی «این اندازه‌گیری را با احتیاط در نظر بگیر». EKF از این اعداد استفاده می‌کند تا تصمیم بگیرد به کدام حسگر، در کدام لحظه، بیشتر وزن بدهد.

🌍 مثال ملموس

فرض کن ARCHO روی یک سطح خیلی صاف حرکت می‌کند — Wheel Odometry تقریباً بدون خطاست، پس Covariance پایینی دارد و EKF بیشتر به آن اعتماد می‌کند. اما وقتی از روی یک ناهمواری کوچک رد می‌شود، لغزش چرخ زیاد می‌شود؛ اینجا IMU (که این نوع اختلال را حس نمی‌کند) به‌طور نسبی وزن بیشتری می‌گیرد. تمام این توازن، خودکار و لحظه‌به‌لحظه توسط EKF انجام می‌شود.

۹.۶چرا LiDAR خام وارد EKF نمی‌شود

سؤال منطقی: چرا داده /scan را هم مستقیم به EKF نمی‌دهیم؟ چون LiDAR خام یک LaserScan است — فهرستی از فاصله‌ها در جهت‌های مختلف — نه یک Pose یا Odometry. EKF به داده‌ای با ساختار حرکتی (موقعیت، سرعت، شتاب) نیاز دارد. اگر بخواهیم از LiDAR برای تصحیح موقعیت استفاده کنیم، باید اول آن را از طریق SLAM یا مقایسه با نقشه (که در فصل دهم و یازدهم می‌بینیم) به یک Pose تبدیل کنیم؛ آن Pose، نه Scan خام، وارد Fusion می‌شود.

۹.۷بازرسی نتیجه Fusion

ros2 run tf2_ros tf2_echo odom base_link
ros2 run tf2_tools view_frames
ros2 topic echo /odometry/filtered
⚠️ خطای رایج: Lookup would require extrapolation

همان‌طور که در فصل پنجم دیدیم، این خطا معمولاً یعنی Timestampها هماهنگ نیستند یا use_sim_time روی همه Nodeهای درگیر (از جمله ekf_filter_node) یکسان تنظیم نشده است. در شبیه‌سازی، این تنظیم را هرگز فراموش نکن.

✅ Checkpoint فصل نهم
  • Topic /odometry/filtered با نرخ پایدار (مثلاً ۵۰ هرتز) منتشر می‌شود.
  • Transform odom → base_link بدون خطای extrapolation در دسترس است.
  • وقتی ARCHO می‌ایستد، موقعیت فیلترشده هم ثابت می‌ماند (نه سرخوردن به‌خاطر گرانش IMU).
  • Covariance خروجی EKF کوچک‌تر از Covariance هرکدام از حسگرهای تنها است.

۹.۸جمع‌بندی فصل نهم

ARCHO حالا به یک منبع تک‌حسگری برای تخمین موقعیت متکی نیست. Wheel Odometry و IMU، هرکدام با نقاط قوت و ضعف خودشان، توسط robot_localization ترکیب می‌شوند تا یک تخمین پایدارتر روی /odometry/filtered تولید شود — پایه‌ای که SLAM و Nav2 در فصل‌های بعد بدون آن اصلاً نمی‌توانند درست کار کنند.

🌍 ارتباط با پروژه اصلی

پروژه ARCHO اکنون یک ekf.yaml کامل دارد و زنجیره odom → base_link که در فصل پنجم فقط تعریفش را دیدیم، حالا واقعاً با داده ترکیبی Wheel Odometry و IMU پر می‌شود.

فصل بعد چه چیزی اضافه می‌کند

در فصل دهم، ARCHO برای اولین بار بدون هیچ نقشه از پیش‌آماده‌ای وارد یک محیط ناشناخته می‌شود و با SLAM Toolbox، هم‌زمان محیط را کشف می‌کند و نقشه می‌سازد — درست همان‌جایی که تخمین دقیق موقعیت این فصل، پیش‌نیاز مطلق آن است.

واژه‌نامه فصل نهم

Odometry
تخمین موقعیت و سرعت ربات بر اساس حرکت داخلی (چرخش چرخ‌ها یا شتاب‌سنج)، بدون نگاه به محیط بیرونی.
Drift
خطای تجمعی در تخمین موقعیت که به‌مرور زمان بزرگ‌تر می‌شود.
Sensor Fusion
ترکیب چند منبع حسگری برای رسیدن به یک تخمین دقیق‌تر و پایدارتر از هرکدام به‌تنهایی.
EKF (Extended Kalman Filter)
الگوریتم آماری برای ترکیب اندازه‌گیری‌های نویزی چند حسگر با وزن‌دهی بر اساس عدم قطعیت هرکدام.
Covariance
معیار عددی میزان عدم قطعیت یک اندازه‌گیری؛ عدد کوچک‌تر یعنی اعتماد بیشتر.
robot_localization
بسته استاندارد ROS 2 برای Sensor Fusion موقعیت با استفاده از EKF یا UKF.

خطاهای رایج فصل نهم — جمع‌بندی

فصل نهم از کتاب «مسیر یادگیری ROS 2»
فصل بعد: SLAM — ARCHO نقشه انبار را می‌سازد →