SLAM~Unscentedカルマンフィルタ

すみません、ただいま作成中です。

0

ロボット・ドローン部品お探しなら、ロボット翔をご利用下さい

SLAM~拡張カルマンフィルタ

概要

SLAM(ロボットの自己位置推定、マッピング同時に行うこと)に使用可能なセンサは、色々とあるが、それぞれ単一のセンサでは完璧ではないため、複数のセンサ情報を、統計的・確率的に組み合わせて、より精密で安定した自己位置を得る手法が一般的だ。状態値(位置、姿勢またはポーズ)、観測値とも正規分布(ガウス分布)に近似可能の場合、カルマンフィルタKFが適用される。また、非線形問題に対して、拡張カルマンフィルタEKFが適用される。勿論、UKF他のフィルタが存在する。なぜかROSに公式に採用されたのはEKFのようだ。ここでは、タイヤパルスセンサ(ホール式や、光学式エンコーダ)からのodom情報と距離センサ(レーザや、カメラなど)からの位置情報を組み合わせて、マッピング地図がある前提でロボットの自己位置、姿勢に拡張カルマンフィルタをかけて推定してみる。

状態空間モデル

$$x_t = f_{t-1}(x_{t-1}^{true}+p_{t-1}) + q_{t-1}\\=\scriptsize\begin{bmatrix}x_{t-1}+v_{t-1}\cdot Δt\cdot cosψ_{t-1} & 0 & 0 \\0 & y_{t-1}+v_{t-1}\cdot Δt\cdot sinψ_{t-1} & 0\\0 & 0 & ψ_{t-1}+ω_{t-1}\cdot Δt\end{bmatrix}+q_{t-1}$$
$$y_t = H_t(x_t^{true}) + r_t\\=\begin{bmatrix}x_t\\y_t\end{bmatrix}+ r_t$$

拡張カルマンフィルタの適用

予測ステップ

事前状態推定値

$$\bar{x}_t=\scriptsize\begin{bmatrix}\hat{x}_{t-1}+v_{t-1}\cdot Δt\cdot cos\hat{ψ}_{t-1} & 0 & 0 \\0 & \hat{y}_{t-1}+v_{t-1}\cdot Δt\cdot sin\hat{ψ}_{t-1} & 0\\0 & 0 & \hat{ψ}_{t-1}+ω_{t-1}\cdot Δt\end{bmatrix}$$

線形化近似(ヤコビアン行列)

$$\hat{F}_t =\frac{\partial f_t(x)}{\partial x}|_{x=\hat{x}_{t-1}}\\=\begin{bmatrix}1 & 0 & -v_{t-1}\cdot Δt\cdot sin\hat{ψ}_{t-1} \\0 & 1 & v_{t-1}\cdot Δt\cdot cos\hat{ψ}_{t-1}\\0 & 0 & 1\end{bmatrix}$$

事前誤差共分散行列

$$\bar{P_t} = \hat{F}_{t-1}\hat{P}_{t-1}\hat{F}_{t-1}^T + Q_{t-1}$$

フィルタリングステップ

カルマンゲイン行列

$$K_t = \bar{P_t}H_t^T(H_t\bar{P_t}H_t^T + R_t)^{-1}$$

状態推定値

$$\hat{x_t} = \bar{x_t} + K_t(y_t – H_t\bar{x_t})$$

事後誤差共分散行列

$$\hat{P_t} = (I-K_tH_t)\bar{P_t}$$

関連記事

研究開発用 台車型ロボット キット

1+

ロボット・ドローン部品お探しなら、ロボット翔をご利用下さい

YDLidar G4のSLAMパラメータ

移動ロボット用レーザースキャナとしてのYDLidar G4のgmappingとhector mappingのパラメータが以下のようにご参考まで。必要に応じてパラメータをご変更してみてください。

gmapping

gmappingに関わるROSパッケージをインストールしておく。

$sudo apt-get install ros-indigo-gmapping
$sudo apt-get install ros-indigo-slam-gmapping

gmappingパラメータ設定の例。

<launch>
  <node pkg="tf" type="static_transform_publisher" name="base_link_2_laser" args="0 0 0 0 0 0 base_link laser 100" /> <!--change -->
  <node pkg="tf" type="static_transform_publisher" name="odom_2_base_link" args="0 0 0 0 0 0 odom base_link 100" /> <!--change -->
  <node pkg="gmapping" type="slam_gmapping" name="simple_gmapping" output="screen"><!--change -->
      <!--remap from="scan" to="base_scan"/-->
      <param name="map_update_interval" value="5.0"/>
      <param name="maxUrange" value="16.0"/>
      <param name="sigma" value="0.05"/>
      <param name="kernelSize" value="1"/>
      <param name="lstep" value="0.05"/>
      <param name="astep" value="0.05"/>
      <param name="iterations" value="5"/>
      <param name="lsigma" value="0.075"/>
      <param name="ogain" value="3.0"/>
      <param name="lskip" value="0"/>
      <param name="minimumScore" value="50"/>
      <param name="srr" value="0.1"/>
      <param name="srt" value="0.2"/>
      <param name="str" value="0.1"/>
      <param name="stt" value="0.2"/>
      <param name="linearUpdate" value="0.3"/>
      <param name="angularUpdate" value="0.4"/>
      <param name="temporalUpdate" value="3.0"/>
      <param name="resampleThreshold" value="0.5"/>
      <param name="particles" value="30"/>
      <param name="xmin" value="-5.0"/>
      <param name="ymin" value="-5.0"/>
      <param name="xmax" value="5.0"/>
      <param name="ymax" value="5.0"/>
      <param name="delta" value="0.05"/>
      <param name="llsamplerange" value="0.01"/>
      <param name="llsamplestep" value="0.01"/>
      <param name="lasamplerange" value="0.005"/>
      <param name="lasamplestep" value="0.005"/>
  </node>
</launch>

ROS wiki:gmapping

hector mapping

hector mappingに関わるROSパッケージをインストールしておく。

$sudo apt-get install ros-indigo-hector-slam

hector mappingパラメータ設定の例。

<launch>
  <node pkg="tf" type="static_transform_publisher" name="base_link_2_laser" args="1 0 0 0 0 0 base_link laser 100" /> <!--change -->
  <node pkg="hector_mapping" type="hector_mapping" name="hector_height_mapping" output="screen">
    <param name="scan_topic" value="scan" />
    <param name="base_frame" value="base_link" />
    <param name="odom_frame" value="base_link" />
    <param name="output_timing" value="false"/>
    <param name="advertise_map_service" value="true"/>
    <param name="use_tf_scan_transformation" value="true"/>
    <param name="use_tf_pose_start_estimate" value="false"/>
    <param name="pub_map_odom_transform" value="true"/>
    <param name="map_with_known_poses" value="false"/>
    <param name="map_pub_period" value="0.5"/>
    <param name="update_factor_free" value="0.45"/>
    <param name="map_update_distance_thresh" value="0.02"/>
    <param name="map_update_angle_thresh" value="0.1"/>
    <param name="map_resolution" value="0.05"/>
    <param name="map_size" value="1024"/>
    <param name="map_start_x" value="0.5"/>
    <param name="map_start_y" value="0.5"/>
  </node>
</launch>

ROS wiki:hector mapping

YDLidar G4販売店

YDLIDAR G4 360度レーザースキャナー開発キット

以上

1+

ロボット・ドローン部品お探しなら、ロボット翔をご利用下さい