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

MyEnigma

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

差動二輪型ロボットのWheel Odometryによる自己位置推定プログラム

Robot Python

車輪型ロボットの自己位置推定技術の中で最も一般的である.

Wheel Odometry(WO)のプログラムの一例を掲載したいと思います.



WOはロボットに搭載された車輪の回転数(速度)のみの情報を元に

相対的なロボットの移動量を計算し,

それらの値を足しあわせて,自己位置推定を行う手法です.

参考記事:Dead Reckoning: デットレコニングの問題点 (ロボットの自己位置推定) - MY ENIGMA



今回は以下のような,差動二輪型のロボットのWOのプログラムを掲載したいと思います.



今回は,米国Willow Garage社のロボット専用OSであるROSを使用することを仮定します.

Documentation - ROS Wiki

また,言語はROSが採用している言語の1つであるPythonを用います.

このプログラムの入力としては,ロボットのタイヤの左右輪に取り付けられた

ホイールエンコーダのそれぞれのパルス値を用いて,

ロボットの前進速度(v),回頭速度(ω),二次元空間の位置(x,y),姿勢(θ)を計算します.

ロボットの初期値は原点とし,初期方位もx軸方向を原点としています.



プログラム内のアルゴリズムは,かなり単純なので,

自己位置推定を少しでもかじったことがある人は

以下のコードを見れば解ると思います.


誰かの助けになれば幸いです.



以下のプログラムをダウンロードできます.:(Download)

#!/usr/bin/env python
import roslib; roslib.load_manifest('wheel_odometry')
import rospy
import time
from std_msgs.msg import *
from nav_msgs.msg import Odometry
from numpy import *

#=========Initial Parameter=============
D_RIGHT=0.245462    #Pulse ratio of the right wheel encoder
D_LEFT=0.243768     #Pulse ratio of the left wheel encoder
GR=100.0            #Gear ratio
PR=32.0             #Pulse ratio of encoders
TREAD=1.002405      #Tread

rs=[0,0]; #pluse store
u=array([0,0]);
count=0
odocount=0

result=open('odo.txt','w')

def talker():
    rospy.init_node('infant_odometry', anonymous=True)
    rospy.Subscriber("/recover/odom", Odometry, odom_callback)
    pub = rospy.Publisher('/odom', Odometry)

    odo=Odometry()
    odo.header.frame_id='1'
    odo.child_frame_id='1'

    current_time=time.time()
    last_time=time.time()

    #======= Parameter Setting with Parameter Server=======
    #   * If the parameter server didn't work, the following sequence would be passed.
    if rospy.has_param('infant_learning_odometry/right_wheel_p'):
        global D_RIGHT
        D_RIGHT=rospy.get_param('infant_learning_odometry/right_wheel_p')
    if rospy.has_param('infant_learning_odometry/left_wheel_p'):
        global D_LEFT
        PRIGHT=rospy.get_param('infant_learning_odometry/left_wheel_p')
    if rospy.has_param('infant_learning_odometry/Tread'):
        global TREAD
        TREAD=rospy.get_param('infant_learning_odometry/Tread')

    #print PRIGHT
    #print PLEFT
    #print TREAD
    #===========================================================
    odo.pose.pose.position.x=0
    odo.pose.pose.orientation.w=0
    odo.twist.twist.linear.x=0
    odo.twist.twist.angular.z=0

    r = rospy.Rate(10)
    global dt
    global x
    global count
    x=array([0,0,toRadian(0.0)]);
    dt=0.1
    t=0;

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

        current_time=time.time()

        dt=current_time-last_time

        odo.header.seq=odocount
        odo.pose.pose.position.x=x[0]
        odo.pose.pose.position.y=x[1]
        odo.pose.pose.orientation.w=x[2]
        odo.twist.twist.linear.x=u[0]
        odo.twist.twist.angular.z=u[1]

        pub.publish(odo)

        if count%2000==0:
            print "%8.2f" %x[0],"%8.2f" %x[1],"%8.2f" %toDegree(x[2])
            #print (last_time-current_time)
            #print t

        result.write('%f,%f,%f,%f,%f\n' %(x[0],x[1],x[2],u[0],u[1]))

        last_time=time.time()
        t+=dt

        r.sleep()
    rospy.spin()

def calc_input(rs):
    u=array([0,0]);
    vr=rs[0]/(GR*PR)*pi*D_RIGHT
    vl=rs[1]/(GR*PR)*pi*D_LEFT
    v=(vr+vl)/2.0
    yawrate=(vr-vl)/(TREAD/2.0)
    u=array([v,yawrate])
    return u
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]=PItoPI(x[2])
    return x

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

def odom_callback(data):
    global u
    global x
    global dt
    global odocount
    rs=[data.pose.pose.position.x,data.pose.pose.position.y]
    odocount=data.header.seq

    u=calc_input(rs)

    x=MotionModel(x,u,dt)

def toDegree(angle):
    angle=angle*180.0/pi
    return angle

def toRadian(angle):
    angle=angle*pi/180.0
    return angle

if __name__ == '__main__':
    talker()