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

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

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

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

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

拡張カルマンフィルタ(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()