カテゴリー: センサー回り

SLAM～Unscentedカルマンフィルタ

はじめに

UKF(Unscented kalman filter)では無損変換(Unscented Transform)の線形化手法を利用し、つまり(2*n次元+1)個Sigma Pointの線形回帰に基づき、確率変数の非線形関数を線形化する。確率変数の拡張を考慮するので、この線形化は拡張カルマンフィルタEKFで使用するテイラー級数線形化より比較的正確に状態推定できる他、EKFと違い、状態・観測ヤコビアン行列を求める必要でなく、但しEKFと同様にUKFも予測と更新の手順から時系列の状態推定を行う。状態変数、観測変数ともガウス分布に当てるのがカルマンフィルタKFの共通特徴という。またガウス分布に当てないパーティクル(和文粒子、英文Particle)フィルタPFがある。そもそもガウス分布に従う確率変数の非線形関数はガウス分布にならないから、EKF、UKFよりPFの方が精度よく、ROSのSLAM Gmappingアルゴリズムに組まれている。

Unscented Kalman Filter

状態空間モデル

状態モデル（状態方程式）
$$x_t = F \cdot x_{t-1} + B \cdot u_{t-1} + q_{t-1} \\\scriptsize=\begin{bmatrix}1&0&0&0\\0&1&0&0\\0&0&1&0\\0&0&0&0\end{bmatrix}\begin{bmatrix}x_{t-1}\\y_{t-1}\\ψ_{t-1}\\v_{t-1}\end{bmatrix}+\begin{bmatrix}Δt\cdot cosψ_{t-1}&0\\Δt\cdot sinψ_{t-1}&0\\0&Δt\\1&0\end{bmatrix}\begin{bmatrix}v_{t-1}\\ψ_{t-1}\end{bmatrix}+q_{t-1}$$

観測モデル（観測方程式）
$$z_t = H \cdot x_t + r_t \\=\begin{bmatrix}1&0&0&0\\0&1&0&0\end{bmatrix}\begin{bmatrix}x_t\\y_t\\ψ_t\\v_t\end{bmatrix}+r_t$$

予測ステップ

$$χ_{t-1}=(x_{t-1} , x_{t-1}+γ\sqrt{{\sum}_{t-1}} , x_{t-1}-γ\sqrt{{\sum}_{t-1}})\\
\bar{χ}_t^{*}=g(u_t,χ_{t-1})\\
\bar{x}_t=\sum_{i=0}^{2n} w_m^{[i]} \bar{χ}_t^{*[i]}\\
\bar{\sum}_t=\sum_{i=0}^{2n} w_c^{[i]}(\bar{χ}_t^{*[i]}-\bar{x}_t)(\bar{χ}_t^{*[i]}-\bar{x}_t)^T +R_t\\
\bar{χ}_{t}=(x_{t} , x_{t}+γ\sqrt{{\sum}_{t}} , x_{t}-γ\sqrt{{\sum}_{t}})\\
\bar{Z}_{t}=h(\bar{χ}_{t})\\
\hat{z}_{t}=\sum_{i=0}^{2n} w_m^{[i]} \bar{Z}_{t}^{[i]}\\
S_t=\sum_{i=0}^{2n} w_c^{[i]}(\bar{Z}_t^{[i]}-\hat{z}_t)(\bar{Z}_t^{[i]}-\hat{z}_t)^T +Q_t\\
\bar{\sum}^{x,z}_t=\sum_{i=0}^{2n} w_c^{[i]}(\bar{χ}_t^{[i]}-\bar{x}_t)(\bar{Z}_t^{[i]}-\hat{z}_t)^T
$$

更新ステップ

$$K_t=\bar{\sum}^{x,z}_t S^{-1}_t\\
x_t = \bar{x}_t+K_t(z_t-\hat{z}_t)\\
{\sum}_t = \bar{\sum}_t-K_t S_t K^T_t$$

参考文献

Probabilistic Robotics/Sebastian Thrun, Wolfram Burgard and Dieter Fox.

1+

ロボット・ドローン部品お探しなら
ROBOT翔・電子部品ストア

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}$$

関連記事

9軸IMUセンサ 6軸／9軸フュージョン 低遅延 USB出力 補正済み ROS対応
研究開発用 台車型ロボット キット

2+

ロボット・ドローン部品お探しなら
ROBOT翔・電子部品ストア

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度レーザースキャナー開発キット

以上

2+

ロボット・ドローン部品お探しなら
ROBOT翔・電子部品ストア

ROS launchの自動起動について

ロボットが立ち上がった際、自動的にコマンドの待ち受け状態に入り、launchファイルのロード、開始、停止、再開、アウトなどのコマンドを受け付けて、指定されたlaunchファイルを実行することが可能な仕組みを作る必要があった。

#!/usr/bin/env python
import roslaunch
import os
import rospy
class Ros_Launch(object):
    def __init__(self, ):
        self.launchfile = ""
        self.reinitialize()
    def __del__(self,):
        self.exit()
    def load_file(self, launch_file):
        if self.is_alive():
            raise RuntimeError("you need to stop before you load another launch file")
            return False  
        self.launchfile = os.path.abspath(os.path.expanduser(launch_file))
        self.launch.parent.config = roslaunch.config.load_config_default([self.launchfile], None)
        return True
    def start(self, ):
        local_machine = roslaunch.core.local_machine()
        for n in self.launch.parent.config.nodes:
            n.machine = local_machine
        self.launch.start()
    def is_alive(self, ):
        if self.launch.parent.pm is not None and self.launch.parent.pm.is_alive() and len(self.launch.parent.pm.get_active_names())>0:
            return True
        return False
    def current_file(self, ):
        return self.launchfile
    def reinitialize(self, ):
        self.launch = roslaunch.scriptapi.ROSLaunch()
    def resume(self, launch_file):
        self.launch.stop()
        self.reinitialize()
        self.load_file(launch_file)
    def stop(self, ):
        self.launch.stop()
        self.reinitialize()
        #self.load_file(self.launchfile)
    def exit(self, ):
        self.launch.stop()

以上

3+

ロボット・ドローン部品お探しなら
ROBOT翔・電子部品ストア

ROS+rosjava+android+studio

開発環境の整え

関連ツール、コンポーネントの互換性から予測せぬ、さまざまなコンパイルエラーに苦労してきた。強力なROS機能が込められたandroid app開発環境の整備を目指して、Ubuntu14.04.5（Trusty）の環境で、ROS Indigo→rosjava→android studio→android_coreの順番でインストールしていきましょう。

Ubuntu 14.04.5（Trusty）

ROS Indigoの公式に認定した動作OSなので、インストールしておくこと。

ROS Indigo

android向けでは、ROS Indigoの都合がよく、インストールしておくこと。

$sudo sh -c 'echo "deb http://packages.ros.org/ros/ubuntu trusty main" > /etc/apt/sources.list.d/ros-latest.list'
$wget http://packages.ros.org/ros.key -O - | sudo apt-key add -
$sudo apt-get update
$sudo apt-get install ros-indigo-desktop-full
$sudo rosdep init
$rosdep update
$source /opt/ros/indigo/setup.bash
$sudo apt-get install python-rosinstall
$mkdir -p ~/catkin_ws/src
$cd ~/catkin_ws
$catkin_make

rosjava

ROSのjava版で、java言語でもROS全ての機能を操るまたカスタマイズできることになるのでインストールしておくこと。

$sudo add-apt-repository ppa:openjdk-r/ppa
$sudo apt-get update
$sudo apt-get install openjdk-8-jdk
$export JAVA_HOME=/usr/lib/jvm/java-8-openjdk-amd64
$mkdir -p ~/rosjava/src
$wstool init -j4 ~/rosjava/src https://raw.githubusercontent.com/rosjava/rosjava/indigo/rosjava.rosinstall
$source /opt/ros/indigo/setup.bash
$cd ~/rosjava
$rosdep update
$rosdep install --from-paths src -i -y
$catkin_make
$mkdir -p ~/myjava/src
$cd ~/myjava/src
$wstool init -j4 https://raw.githubusercontent.com/me/rosinstalls/master/my_custom_msg_repos.rosinstall
$source ~/rosjava/devel/setup.bash
$cd ~/myjava
$rosdep update
$rosdep install --from-paths src -i -y
$catkin_make

android studio

android studio 171.4443003~zesty 3.01 linux versionをインストールしておくこと。

$mkdir ~/android
$cd ~/android
$wget https://dl.google.com/dl/android/studio/ide-zips/3.0.1.0/android-studio-ide-171.4443003-linux.zip
$unzip android-studio-ide-171.4443003-linux.zip
$mv android-studio studio
$sudo dpkg --add-architecture i386
$sudo apt-get update -y
$sudo apt-get install -y libncurses5:i386 libstdc++6:i386 zlib1g:i386
$echo export PATH=$PATH:~/android/sdk/tools:~/android/sdk/platform-tools:~/android/studio/bin >> ~/.bashrc
$echo export ANDROID_HOME=~/android/sdk >> ~/.bashrc
$source ~/.bashrc
$studio.sh

android_core

android app開発向けのcore moduleが含まれているので、インストールしておくこと。

$mkdir -p ~/android_core/src
$wstool init ~/android_core/src https://raw.github.com/rosjava/rosjava/indigo/android_core.rosinstall
$cd ~/android_core
$catkin_make

関連記事

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

以上

3+

ロボット・ドローン部品お探しなら
ROBOT翔・電子部品ストア