読者です 読者をやめる 読者になる 読者になる

MyEnigma

とあるエンジニアのブログです。#Robotics #Programing #C++ #Python #MATLAB #Vim #Mathematics #Book #Movie #Traveling #Mac #iPhone

自己位置推定フィルタ開発のための擬似センサデータ出力ノード@ROS

Robot Python

移動ロボットの自己位置推定では,

複数のセンサの情報を統合するための,

フィルタシステムの開発が欠かせません.



移動ロボットの自己位置推定では,

拡張カルマンフィルタ(EKF)や

パーティクルフィルタ(PF)が

使用されることが一般的です.[1][2]



その他にも,

沢山フィルタのアルゴリズムは存在するのですが,

自己位置推定システムを開発では,

これらのフィルタのアルゴリズムを実装することが

第一のステップになります.



しかし,実際には

論文に書かれているような

これらのフィルタのアルゴリズム表,

またはフィルタの数式を見ながら

フィルタを実装していくのですが,

いきなり実際のセンサデータのログを使用して,

フィルタを実装すると,

ある問題が生じます.



それは,

プログラムのデバックが

難しくなるということです.

なぜなら,

これらのフィルタは逐次,

確率的にロボットの状態量を推定するのですが,

正確に確率計算を行わないと,

簡単に計算が発散してしまいます.



しかし,

実際にフィルタを実装している時に,

確率計算が発散したとしても,

その原因が

フィルタプログラムのバグなのか

それとも,

そもそものセンサデータが不適切なのかが

分かりにくいのです.



つまり,まず初めに

フィルタの計算が発散しないような

センサ情報を使用して

フィルタの実装を行い,

その後,実際のセンサデータを使用して

フィルタの調整をしていくというのが

最もデバックし易い開発方法だと思います.



そこで今回,

以上のステップでフィルタを実装するために,

擬似的な自己位置推定センサデータを出力するROSノードを作成しました.

プログラムは以下の通りです.

以下のプログラムをダウンロード(link)


#!/usr/bin/env python
'''

fakelocalization

File         :       fakelocalization.py

Run Command  :       Rosrun fakelocalization fakelocalization.py

Environment  :       Python and ROS

Latest Update:          2010/11/24                                 

Author(s)    :       a.sakai (AMSL)   

CopyRight    :       Autonomous Mobile Systems Laboratory, Meiji Univ.

'''

import roslib; roslib.load_manifest('fakelocalization')
import rospy
from numpy import *
import time
from string import *
from struct import *
from math import *
from random import *
from nav_msgs.msg import Odometry


def talker():
     pub_xtrue = rospy.Publisher('/fake/xtrue', Odometry)
     pub_xd = rospy.Publisher('/fake/odom', Odometry)
     pub_gps = rospy.Publisher('/fake/gps', Odometry)
     rospy.init_node('fakelocalization')
     r = rospy.Rate(10)

     t=0;

     Q=array([[0.1,0],
                [0,to_radian(30)]]);
     Q=Q**2

     R=array([[1.5,0,0],
                [0,1.5,0],
                [0,0,to_radian(5)]]);
     R=R**2

     xtrue=array([0,0,0]);    
     xd=array([0,0,0]);    
     gps=array([0,0,0]);
              
     xtrue_msg=Odometry()
     xd_msg=Odometry()
     gps_msg=Odometry()

     dt=0.1

     count=0
     while not rospy.is_shutdown():
          count+=1
          t1=time.time()

          u=array([1.0,to_radian(5)])     #input vector
         
          xtrue=MotionModel(xtrue,u,dt)

          ud=add_process_noise(u,Q)

          xd=MotionModel(xd,ud,dt)

          gps=add_observation_noise(xtrue,R)

          if count%10==0:
               print xtrue
               print xd
               print gps
               print t

          xtrue_msg.pose.pose.position.x=xtrue[0]
          xtrue_msg.pose.pose.position.y=xtrue[1]
          xtrue_msg.pose.pose.orientation.w=pi_to_pi(xtrue[2])
          xtrue_msg.twist.twist.linear.x=u[0]
          xtrue_msg.twist.twist.angular.z=u[1]
         
          xd_msg.pose.pose.position.x=xd[0]
          xd_msg.pose.pose.position.y=xd[1]
          xd_msg.pose.pose.orientation.w=pi_to_pi(xd[2])
          xd_msg.twist.twist.linear.x=ud[0]
          xd_msg.twist.twist.angular.z=ud[1]
         
          gps_msg.pose.pose.position.x=gps[0]
          gps_msg.pose.pose.position.y=gps[1]
          gps_msg.pose.pose.orientation.w=pi_to_pi(gps[2])
         
          pub_xtrue.publish(xtrue_msg)
          pub_xd.publish(xd_msg)
          pub_gps.publish(gps_msg)
         
          r.sleep()
          t2=time.time()
          dt=t2-t1
          t=t+dt

     rospy.spin()

def MotionModel(x,u,dt):
     F=eye(3)
     B=array([[dt*cos(x[2]),0],
               [dt*sin(x[2]),0],
               [0,dt]])
         
     x=dot(F,x)+dot(B,u)
     x[2]=pi_to_pi(x[2])
     return x

def add_process_noise(u,Q):
     uw=array([gauss(0,Q[0,0]),gauss(0,Q[1,1])])
     ud=u+uw
     return ud
    
def add_observation_noise(x,R):
     xw=array([gauss(0,R[0,0]),gauss(0,R[1,1]),gauss(0,R[2,2])])
     xgps=x+xw
     return xgps

def pi_to_pi(angle):
     while angle>=pi:
          angle-=2*pi
     while angle<=-pi:
          angle+=2*pi
     return angle

def to_radian(angle):
     return angle*pi/180.0
    
if __name__ == '__main__':
     talker()


基本的には,プログラムを見れば自己位置推定システムの開発者なら解ると思いますが,

環境はPythonで,

二次元空間におけるロボットの位置x,yと方位θを

推定対象の状態ベクトルとした自己位置推定を行うことを仮定します.



使用するセンサは,

入力値(前進速度,回頭速度)としてWheel Encoderと

観測値(大域的位置情報,大域的方位)としてGPS,電子コンパス

を利用することを仮定しています.


それぞれのセンサデータには,

プログラムの上部で定義した共分散行列に応じて,

ガウスノイズを付加してセンサデータをパブリッシュします.

あとは,このデータを使用してフィルタの実装をすれば良いわけです.



このノードが出力するトピックは以下の通りです.

すべてのトピックの型は,nav_msgs/Odometryを使用し,統一しています.

  1. /fake/xtrue
    自己位置推定の真値(位置,方位,入力値)
  2. /fake/odom
    Wheel Odometryによる入力値
  3. /fake/gps
    GPSと電子コンパスによる観測値

最後に,以下の図はこのノードを使用してフィルタを開発した時の

自己位置推定の結果です.

青線が自己位置推定の真値(Ground Truth)で,

黒線は,今回のノードから出力される入力値を使用して

自己位置推定を行った場合のDead Reckoningの結果,

緑の点は今回のノードから出力される観測値であるGPSの位置情報

そして,赤線はフィルタの推定結果です.



以上のように,赤線と青線を比較することにより,

ちゃんとフィルタが機能しているかどうかを確認することができます.



めでたしめでたし.

参考文献

Probabilistic Robotics (Intelligent Robotics and Autonomous Agents series)

Probabilistic Robotics (Intelligent Robotics and Autonomous Agents series)

  • 作者: Sebastian Thrun,Wolfram Burgard,Dieter Fox
  • 出版社/メーカー: The MIT Press
  • 発売日: 2005/08/19
  • メディア: ハードカバー
  • 購入: 1人 クリック: 6回
  • この商品を含むブログを見る

myenigma.hatenablog.com
myenigma.hatenablog.com
myenigma.hatenablog.com
myenigma.hatenablog.com
myenigma.hatenablog.com
myenigma.hatenablog.com
myenigma.hatenablog.com
myenigma.hatenablog.com
myenigma.hatenablog.com
myenigma.hatenablog.com
myenigma.hatenablog.com
myenigma.hatenablog.com
myenigma.hatenablog.com
myenigma.hatenablog.com
myenigma.hatenablog.com
myenigma.hatenablog.com
myenigma.hatenablog.com