はじめに
今回の記事では、
車輪型ロボットの自己位置推定技術の中で最も一般的である．
Wheel Odometry(WO)の概要とPythonプログラムの一例を掲載したいと思います．
Wheel Odometryの概要
WOはロボットに搭載された車輪の回転数(速度)のみの情報を元に
相対的なロボットの移動量を計算し，
それらの値を足しあわせて，自己位置推定を行う手法です．
今回は以下のような，差動二輪型のロボットを考えます．
すると、ロボットの前進速度vと回頭速度ωは
下記の線形方程式で計算できます。
また、左右車輪の前進速度ではなく、
左右車輪の回転角速度がエンコーダなどで取得できる場合は、
下記のような線形方程式になります。
上記の式において、R_rとR_lはそれぞれのタイヤの半径です。
Python サンプルコード
今回は，米国Willow Garage社のロボット専用OSであるROSを使用することを仮定します．
また，言語はROSが採用している言語の１つであるPythonを用います．
このプログラムの入力としては，ロボットのタイヤの左右輪に取り付けられた
ホイールエンコーダのそれぞれのパルス値を用いて，
ロボットの前進速度(v)，回頭速度(ω)，二次元空間の位置(x,y)，姿勢(θ)を計算します．
ロボットの初期値は原点とし，初期方位もｘ軸方向を原点としています．
プログラム内のアルゴリズムは，かなり単純なので，
自己位置推定を少しでもかじったことがある人は
以下のコードを見れば解ると思います．
#!/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()
以下のプログラムは下記からもダウンロードできます．：(Download)
