ROS2(二) - nav2_amcl節點概況

AMCL是一個針對二維移動機器人的機率定位系統。它實作了自适應(或KLD采樣)蒙特卡羅定位方法(如Dieter Fox所述)，該方法使用粒子過濾器來跟蹤機器人在已知地圖上的姿勢。

算法

從Probabilistic Robotics書中的算法

• sample_motion_model_odometry
• beam_range_finder_model
• likelihood_field_range_finder_model
• Augmented_MCL, and KLD_Sampling_MCL

amcl接收基于雷射的地圖，雷射掃描資料和tf轉換消息，并輸出姿态估計pose estimates。啟動時，AMCL根據提供的參數初始化其粒子過濾器。請注意，由于預設設定，如果未設定任何參數，則初始過濾狀态将是以(0，0，0)為中心的中等大小的粒子雲

訂閱的主題

• scan (sensor_msgs/LaserScan)

  Laser scans.轉換到odom_frame_id

• (tf/tfMessage)

  Transforms.

• initialpose (geometry_msgs/PoseWithCovarianceStamped)

  Mean and covariance with which to (re-)initialize the particle filter.用于（重新）初始化粒子濾波器的平均值和協方差

• map (nav_msgs/OccupancyGrid)

  When the use_map_topic parameter is set, AMCL subscribes to this topic to retrieve the map used for laser-based localization. 當設定了use_map_topic參數時，AMCL訂閱此主題以檢索用于基于雷射的本地化的映射

釋出的主題

• amcl_pose (geometry_msgs/PoseWithCovarianceStamped)

  Robot’s estimated pose in the map, with covariance.機器人在地圖上的估計姿态，具有協方差後驗位姿和一個6*6的協方差矩陣（xyz+三個轉角）

• particlecloud (geometry_msgs/PoseArray)

  The set of pose estimates being maintained by the filter.由過濾器維護的姿态估計集合。粒子位姿的數組

• tf (tf/tfMessage)

  Publishes the transform from odom (which can be remapped via the ~odom_frame_id parameter) to map. 釋出從odom到map的transform。

服務

• global_localization（std_srvs / Empty）

  啟動全局定位，其中所有粒子随機分散通過地圖中的自由空間。沒有給定初始位姿的情況下在全局範圍内初始化粒子位姿。callback調用pf_init_model，然後調用uniformPseGenerator，在地圖上随機生成pf -> max_samples個粒子。

• request_nomotion_update:沒有運動模型更新的情況下也更新粒子群。
• dynamix_configure:動态參數配置器

粒子濾波

通過尋找一組在狀态空間中傳播的随機樣本來近似的表示機率密度函數，用樣本均值代替積分運算，進而獲得系統狀态的最小方差估計的過程，這些樣本被形象的稱為“粒子”，故而叫粒子濾波。

• 初始化粒子群
• 按照上一時刻的 x t − 1 x_{t-1} xt−1​及控制資料 u t u_t ut​（可能是編碼器資料，可能是odom）,産生t時刻的假象狀态 x t x_t xt​（從 p ( x t ∣ u t , x t − 1 ) p(x_t|u_t,x_{t-1}) p(xt​∣ut​,xt−1​)中采樣）
• 計算每個 x t x_t xt​的重要性因子。 w t = p ( z t ∣ x t ) w_t = p(z_t|x_t) wt​=p(zt​∣xt​)
• 粒子 x t x_t xt​和權重 w t w_t wt​構成t時刻的新的粒子集
• 重采樣/重要性采樣:
  • 從包含M個粒子的粒子集中，按照每個粒子的權重，作為機率，抽取M次。這樣粒子集的分布發生變化。這時粒子集中有許多重複粒子。

具體參考ros官方navigation功能介紹