自適應蒙特卡洛定位，簡稱AMCL，主要提供定位功能并以/tf形式輸出

蒙特卡洛算法的基本思想：當所要求的問題是某種事件出現的概率或者是某個變量的期望值時，它們可以通過某種"試驗"的方法，得到這種事件出現的概率，并用它們作為問題的解

粒子濾波

粒子濾波就是使用了蒙特卡洛思想的方法。把粒子濾波用在定位上就成為蒙特卡洛定位。其核心思想是通過一組隨機采樣粒子（即假設的狀態）及其權重來近似后驗概率分布，并通過重采樣避免粒子退化。

粒子濾波實現的蒙特卡洛定位過程如下：

1. 初始化：生成初始粒子集，表示機器人可能的初始位姿分布，注意一般粒子要均勻分布在環境的自由空間（非障礙物區域）。如果有初始位姿（有先驗，比如初始GPS或人工指定位置），則粒子集中在初始位姿附近的高斯分布中。這里，生成的粒子數量作為參數可調，粒子數量?NN?影響計算效率和定位精度
2. 權重計算：根據傳感器觀測數據（如激光雷達、深度相機）調整粒子權重，反映其與真實位姿的匹配程度。計算每個粒子的觀測似然?p(zt∣xt(i))，即當前傳感器數據在該粒子位姿?xt(i)xt(i)??下與地圖的匹配程度。以雷達為例，對每個激光束，在地圖中計算其端點與最近障礙物的距離誤差，誤差越小權重越高。
3. 重采樣：淘汰低權重粒子，復制高權重粒子，避免粒子退化（即多數粒子權重趨近0）。拋棄舊粒子重新生成相同數量的新粒子，新粒子集中出現在權重大的舊粒子位置上，并且有大量的新粒子疊加在同一個位置。因為所有的新粒子都是重采樣得到的，所以它們現在有相同的權重
4. 狀態轉移：根據機器人運動模型（如里程計）傳播粒子，預測下一時刻的位姿分布。傳感器不是絕對精準的，也就是存在誤差，測出來的速度和方向是一個范圍值，粒子在這些范圍內隨機取樣，導致狀態轉移后的粒子不完全重疊
5. 狀態轉移后再進行權重計算階段，重復過程234。加權平均或最高權重粒子作為最終位姿。
6. 不斷重復上述過程，會在某一時刻完成定位，即大部分粒子都集中到了小車附近，這時小車移動，蒙特卡洛的粒子集中區域就會跟隨移動，實現定位效果

由amcl定位的原理可以得知，剛開始時，粒子均勻撒在整個地圖上，真正接近真實位姿的粒子可能很少。?小車還沒運動或只運動了一點點，激光雷達/深度相機等傳感器數據不足以讓權重計算準確區分正確粒子。為了解決這個問題，可以在小車啟動后先不運動，保持靜止幾秒鐘，讓傳感器數據（如激光雷達）反復計算權重，淘汰錯誤粒子，但是如果環境特征不明顯（比如空曠走廊），可能仍然不準。也可以在定位未收斂時，限制小車最大速度，讓它緩慢運動，避免劇烈碰撞。

# 在move_base或局部規劃器中設置
initial_planner:max_vel_x: 0.1  # 初始最大速度（通常0.5m/s，這里降到0.1）convergence_threshold: 0.8  # 粒子置信度>80%才全速運動

也可以啟動時使用更多粒子（如5000→10000），等收斂后再減少。

amcl:initial_particles: 10000  # 初始粒子數after_convergence_particles: 2000  # 收斂后減少到2000

?

當機器人定位基本完成時，即這些粒子都集中在一塊了，這個時候重采樣的粒子就可以自適應的減少一些?。當遭遇劫持綁架(下面會介紹)，即粒子的平均分數(權重)突然降低，在全局重新撒一些粒子來重新找位置。上面所說的自適應調整粒子數量就是自適應解決的問題。

機器人定位分為三大問題：全局定位、位姿跟蹤、綁架劫持。

• 全局定位：機器人剛啟動，不清楚初始位置，這時靠粒子濾波等方法以及自身的運動來確定自己在地圖中的位姿
• 位姿跟蹤：已知自身的位姿，持續估計自身在地圖中的實時位置和朝向
• 綁架劫持：已知自身的位姿，被人為的放到另一個環境中，這時上一時刻的位姿數據還在，粒子很集中，但是下一時刻計算權重時，發現所有的粒子與測量值不符(所有粒子權重都比較小)。?

官方提供的amcl功能包(包括在navigation中)輸入為地圖，雷達和小車自身的tf信息，最后以tf轉換的形式輸出定位信息?

參數?

以下為amcl_omni.launch文件，用于全向移動小車。這些參數取自navigation/amcl/cfg/AMCL.cfg。具體參數含義在最后

<launch><arg name="initial_pose_x" default="$(optenv initial_pose_x 0.0)"/><arg name="initial_pose_y" default="$(optenv initial_pose_y 0.0)"/><arg name="initial_pose_a" default="$(optenv initial_pose_a 0.0)"/>
<node pkg="amcl" type="amcl" name="amcl"><!-- Publish scans from best pose at a max of 10 Hz --><!--<param name="initial_pose_x" value="$(arg initial_pose_x)" />-->  <!-- 根據實際初始位置設置x坐標，單位米 --><!--<param name="initial_pose_y" value="$(arg initial_pose_y)" />-->  <!-- 根據實際初始位置設置y坐標，單位米 --><!--<param name="initial_pose_a" value="$(arg initial_pose_a)" />-->  <!-- 根據實際初始朝向設置角度，單位弧度 --><param name="odom_model_type" value="omni"/><param name="odom_alpha5" value="0.1"/><param name="transform_tolerance" value="0.2" /> <!--0.1--><param name="gui_publish_rate" value="10.0"/><param name="laser_max_beams" value="50"/><param name="use_map_topic" value="false"/>  <!-- //當設置為true時，AMCL將會訂閱map話題，而不是調用服務返回地圖。也就是說，當設置為true時，有另外一個節點實時的發布map話題，也就是機器人在實時的進行地圖構建，并供給amcl話題使用；當設置為false時，通過map server，也就是調用已經構建完成的地圖。在navigation 1.4.2中新加入的參數。 --><param name="first_map_only" value="true"/>  <!-- //當設置為true時，AMCL將僅僅使用訂閱的第一個地圖，而不是每次接收到新的時更新為一個新的地圖，在navigation 1.4.2中新加入的參數。 --><param name="min_particles" value="1000"/><param name="max_particles" value="10000"/><param name="kld_err" value="0.03"/><param name="kld_z" value="0.99"/><param name="odom_alpha1" value="0.2"/><param name="odom_alpha2" value="0.2"/><!-- translation std dev, m --><param name="odom_alpha3" value="0.8"/><param name="odom_alpha4" value="0.2"/><param name="laser_z_hit" value="0.5"/><param name="laser_z_short" value="0.05"/><param name="laser_z_max" value="0.05"/><param name="laser_z_rand" value="0.5"/><param name="laser_sigma_hit" value="0.2"/><param name="laser_lambda_short" value="0.1"/><param name="laser_lambda_short" value="0.1"/><!-- <param name="laser_model_type" value="likelihood_field"/> --><param name="laser_model_type" value="beam"/><param name="laser_likelihood_max_dist" value="2.0"/><param name="update_min_d" value="0.15"/> <!--0.2--><param name="update_min_a" value="0.3"/>  <!--0.5--><param name="odom_frame_id" value="odom"/><param name="resample_interval" value="1"/><param name="transform_tolerance" value="0.1"/><param name="recovery_alpha_slow" value="0.0"/><param name="recovery_alpha_fast" value="0.0"/><!-- <param name="odom_frame_id" value="odom"/>   --><!-- //里程計默認使用的坐標系 --><!-- <param name="base_frame_id" value="base_link"/>   --><!-- //用作機器人的基坐標系 --><!-- <param name="global_frame_id" value="map"/>   --><!-- //由定位系統發布的坐標系名稱 --><!-- <param name="tf_broadcast" value="false"/>   --><!-- //設置為false阻止amcl發布全局坐標系和里程計坐標系之間的tf變換 -->
</node>
</launch>

amcl_diff.launch用于差速小車。?

<launch>
<node pkg="amcl" type="amcl" name="amcl" output="screen"><!-- Publish scans from best pose at a max of 10 Hz --><param name="odom_model_type" value="diff"/><param name="odom_alpha5" value="0.1"/><param name="transform_tolerance" value="0.2" /><param name="gui_publish_rate" value="10.0"/><param name="laser_max_beams" value="12"/><param name="min_particles" value="1000"/><param name="max_particles" value="10000"/><param name="kld_err" value="0.05"/><param name="kld_z" value="0.99"/><param name="odom_alpha1" value="0.2"/><param name="odom_alpha2" value="0.2"/><!-- translation std dev, m --><param name="odom_alpha3" value="0.8"/><param name="odom_alpha4" value="0.2"/><param name="laser_z_hit" value="0.5"/><param name="laser_z_short" value="0.05"/><param name="laser_z_max" value="0.05"/><param name="laser_z_rand" value="0.5"/><param name="laser_sigma_hit" value="0.2"/><param name="laser_lambda_short" value="0.1"/><param name="laser_lambda_short" value="0.1"/><!-- <param name="laser_model_type" value="likelihood_field"/> --><param name="laser_model_type" value="beam"/><param name="laser_likelihood_max_dist" value="2.0"/><param name="update_min_d" value="0.2"/><param name="update_min_a" value="0.5"/><param name="odom_frame_id"   value="odom"/><param name="resample_interval" value="1"/><param name="transform_tolerance" value="0.1"/><param name="recovery_alpha_slow" value="0.0"/><param name="recovery_alpha_fast" value="0.0"/><!--  <param name="odom_frame_id"   value="odom"/><param name="base_frame_id"   value="base_link"/><param name="global_frame_id" value="map"/><param name="scan_frame_id" value="laser_frame"/> --><!-- <param name="scan_topic" value="scan"/>  <remap from="scan"  to="lidar/scan"/> -->
</node>
</launch>

以下是我導航總的啟動文件，可以看到對amcl launch文件的調用?

<launch><!-- Run the map server --><include file="$(find ucar_controller)/launch/base_driver.launch" > </include><include file="$(find ydlidar)/launch/ydlidar.launch" > </include><node name="map_server" pkg="map_server" type="map_server" args="$(find ucar_nav)/maps/yf.yaml" output="screen"><param name="frame_id" value="map" /></node> <!-- 啟動AMCL重啟控制器 --><!--<node name="restart_amcl" pkg="ucar_nav" type="restart_amcl.py" output="screen"/>--><include file="$(find ucar_nav)/launch/config/amcl/amcl_omni.launch" > </include><node pkg="move_base" type="move_base" respawn="false" name="move_base" output="screen"><param name="base_global_planner" value="global_planner/GlobalPlanner"/><param name="base_local_planner" value="teb_local_planner/TebLocalPlannerROS"/><rosparam file="$(find ucar_nav)/launch/config/move_base/costmap_common_params.yaml" command="load" ns="global_costmap" /><rosparam file="$(find ucar_nav)/launch/config/move_base/costmap_common_params.yaml" command="load" ns="local_costmap" /><rosparam file="$(find ucar_nav)/launch/config/move_base/global_planner_params.yaml" command="load" /><rosparam file="$(find ucar_nav)/launch/config/move_base/tra_local_planner_params.yaml" command="load" /><rosparam file="$(find ucar_nav)/launch/config/move_base/local_costmap_params.yaml" command="load" /><rosparam file="$(find ucar_nav)/launch/config/move_base/global_costmap_params.yaml" command="load" /><rosparam file="$(find ucar_nav)/launch/config/move_base/laser_filters.yaml" command="load" /><remap from="scan" to="/raw_scan" />  <!-- 輸入原始激光話題 --><remap from="scan_filtered" to="/scan" />  <!-- 輸出過濾后話題 --></node><node pkg="rviz" type="rviz" name="rviz" args="-d $(find ucar_nav)/launch/config/rviz/rviz.rviz" /></launch>

?

 <--------里程計模型參數-------->
odom_model_type:
含義: 里程計模型類型。對于全向驅動機器人，設置為 omni。
調整建議: 如果機器人是全向驅動的，保持為 omni；如果是差分驅動的，改為 diff。odom_alpha1 到 odom_alpha5:
含義: 這些參數用于調整里程計模型的噪聲。具體含義如下：
odom_alpha1: 旋轉噪聲（由于旋轉引起的平移噪聲）。
odom_alpha2: 平移噪聲（由于平移引起的旋轉噪聲）。
odom_alpha3: 平移噪聲（由于平移引起的平移噪聲）。
odom_alpha4: 旋轉噪聲（由于旋轉引起的旋轉噪聲）。
odom_alpha5: 旋轉噪聲（由于平移引起的旋轉噪聲）。
調整建議: 如果機器人的里程計噪聲較大，可以適當增加這些值；如果噪聲較小，可以減少這些值。<--------粒子濾波器參數-------->
min_particles:
含義: 粒子濾波器的最小粒子數量。
調整建議: 增加粒子數量可以提高定位精度，但會增加計算負載。默認值 1000 通常足夠。
max_particles:
含義: 粒子濾波器的最大粒子數量。
調整建議: 增加粒子數量可以提高定位精度，但會增加計算負載。默認值 10000 通常足夠。
kld_err:
含義: KLD（Kullback-Leibler Divergence）誤差的上限，用于動態調整粒子數量。
調整建議: 減小該值會增加粒子數量，提高定位精度，但會增加計算負載。默認值 0.05 通常足夠。
kld_z:
含義: KLD 的置信度參數。
調整建議: 增加該值會增加粒子數量。默認值 0.99 通常足夠。<--------激光雷達參數-------->
laser_max_beams:
含義: 用于定位的激光雷達光束的最大數量。
調整建議: 增加該值可以提高定位精度，但會增加計算負載。默認值 50 通常足夠。
laser_model_type:
含義: 激光雷達模型類型。beam 表示使用光束模型，likelihood_field 表示使用似然場模型。
調整建議: likelihood_field 模型通常更精確，但計算量更大。如果激光雷達數據質量較高，可以嘗試使用 likelihood_field。
laser_z_hit:
含義: 激光雷達命中模型的權重。
調整建議: 增加該值可以提高對激光雷達命中數據的信任度。默認值 0.5 通常足夠。
laser_z_short:
含義: 激光雷達短距離模型的權重。
調整建議: 增加該值可以提高對短距離數據的信任度。默認值 0.05 通常足夠。
laser_z_max:
含義: 激光雷達最大距離模型的權重。
調整建議: 增加該值可以提高對最大距離數據的信任度。默認值 0.05 通常足夠。
laser_z_rand:
含義: 激光雷達隨機噪聲模型的權重。
調整建議: 增加該值可以提高對隨機噪聲的容忍度。默認值 0.5 通常足夠。
laser_sigma_hit:
含義: 激光雷達命中模型的標準差。
調整建議: 增加該值可以放寬對激光雷達數據的精度要求。默認值 0.2 通常足夠。
laser_lambda_short:
含義: 激光雷達短距離模型的衰減系數。
調整建議: 增加該值可以提高對短距離數據的信任度。默認值 0.1 通常足夠。
laser_likelihood_max_dist:
含義: 激光雷達似然場模型的最大距離。
調整建議: 增加該值可以擴大似然場的范圍。默認值 2.0 通常足夠。<--------更新參數-------->
update_min_d:
含義: 觸發定位更新的最小平移距離（米）。當機器人移動的距離超過 update_min_d 時，amcl 會觸發一次定位更新。
調整建議: 減小該值可以提高定位頻率，但會增加計算負載。默認值 0.2 通常足夠。
update_min_a:
含義: 觸發定位更新的最小旋轉角度（弧度）。當機器人旋轉的角度超過 update_min_a(弧度) 時，amcl 會觸發一次定位更新。
調整建議: 減小該值可以提高定位頻率，但會增加計算負載。默認值 0.5 通常足夠<--------其他參數-------->
transform_tolerance:
含義: 坐標系變換的容忍時間（秒）。它定義了在查詢坐標變換時，允許的最大時間偏差。如果坐標變換的時間戳與當前時間的偏差超過 transform_tolerance，系統會認為該變換無效。
調整建議: 增加該值可以容忍更大的時間偏差。默認值 0.1 通常足夠。
resample_interval:
含義: 重采樣間隔（更新次數）。用于控制 粒子濾波器的重采樣頻率。它的作用是決定 AMCL 在多少次更新后才對粒子進行重采樣。合理設置 resample_interval 可以提高定位的精度和效率。
調整建議: 增加該值可以減少重采樣頻率，降低計算負載。resample_interval 設置為 1，則每次更新后都會進行重采樣。
recovery_alpha_slow 和 recovery_alpha_fast:
含義: 用于恢復行為的慢速和快速衰減系數。它們的作用是控制 AMCL 在定位失敗或丟失時如何調整粒子濾波器的分布，以嘗試重新定位機器人。
recovery_alpha_slow控制 AMCL 在定位失敗時，緩慢增加粒子分布的隨機性。這個過程較慢，適合在定位失敗初期使用。取值范圍：0.0 到 1.0。
recovery_alpha_fast控制 AMCL 在定位失敗時，快速增加粒子分布的隨機性。當機器人定位失敗較嚴重時，AMCL 會快速增加粒子的隨機性，以嘗試快速覆蓋更大的區域。這個過程較快，適合在定位失敗較嚴重時使用。
recovery_alpha_slow適用場景：定位失敗較輕（例如機器人位置稍有偏差）。環境變化較小（例如機器人移動到一個已知區域但定位稍有偏差）。不會顯著改變粒子分布，避免過度隨機化導致定位結果不穩定。
recovery_alpha_fast適用場景：定位失敗較嚴重（例如機器人完全丟失定位）。環境變化較大（例如機器人被移動到未知區域）。快速覆蓋更大的區域，增加重新定位的可能性。
調整建議: 默認情況下，recovery_alpha_slow 和 recovery_alpha_fast 都設置為 0.0，即禁用恢復行為。這是因為恢復行為可能會引入額外的計算開銷，且在某些場景下并不需要。

?