differential drive odometry equations in python

asked 2020-08-04 04:00:23 -0500

bfdmetu gravatar image

updated 2020-08-04 04:11:48 -0500

I am working on a ros 2wd robot project. I am using ros kinetic.

When i calculate the odom y axis will get huge error. For example i drive the car go forward 10 meters.Then get back the initial position (without any turn theta=0 ). x and y will ok. But if i turn 180 degress and get back to initial position , calculation of position shows me x=0 y= -4 meters. Y error is huge i think. There should be a mistake in my code.

#dt cycle time
#vty theta 
self.vx= (self.vxr+self.vxl)/2.0  #vx velocity
self.vth= (self.vxr-self.vxl)/(0.6)   #theta
self.dth = self.vth * dt  #angular distance in 1 cycle
 self.pth += self.dth   #angular last position

self.vxx= self.vx * math.cos(self.vth)   #velocity in x axis
self.vxy= (-1*self.vx * math.sin(self.vth))  #velocity in y axis

self.dx = (self.vxx * math.cos(self.pth)-self.vxy*sin(self.pth)) * dt    #distance in x axis in 1 cycle 
 self.dy = (self.vxx * math.sin(self.pth)+ self.vxy*cos(self.pth)) * dt   #distance in y axis in 1 cycle
self.px += self.dx            #last position in x axis
self.py += self.dy             #last position in y axis
edit retag flag offensive close merge delete


@bfdmetu, Technically there is no problem in that since it is the natural behaviour of every odometry computed from encoders. Since the robot does not have a perfect movement: maybe the surface is slipery or whatever other physical or mechanical factor; you always end up accumulating the errors computed from wheels. That's one of the reason why SLAM with only wheel odometry is pretty inaccurate.

You are doing something very similar to the approach the navigation tutorials explains here. And as I said it is not something related with the code but with the natural behaviour of odometry calculations. Nervertheless I will recommend you not only basiing your localization in wheel odometry but try different approaches and odometry fusion like robot_localization. Or if you have sensor probabilistic localization like AMCL.

Weasfas gravatar image Weasfas  ( 2020-08-04 05:24:28 -0500 )edit

First of all thanks for your response. i confused some parts of the code. in the last point of the code (calculating dx and dy ) should i use dth ( angular distance in 1 cycle ) or pth ( angular last position)

and also i am not sure are dx and dy equations true or false ?

bfdmetu gravatar image bfdmetu  ( 2020-08-04 05:48:57 -0500 )edit

@bfdmetu, you should be using dt that is the time difference between the beginning of time and the current computed time, since you need to keep track of the time passed since the beginning of computations. In fact in that part you need to compute x,y and th not only x and y. And then do the accumulation of the three variables, that is:

dt = (current_time-last_time) (in secs)

self.dx = ...
self.dy = ...
self.dth = ...

self.px += self.dx
self.py += self.dy
self.pth += self.dth

If you notice you are computing dx and dy with the accoumulated th from the last cycle not the current one.

Weasfas gravatar image Weasfas  ( 2020-08-04 06:01:17 -0500 )edit

current_time = rospy.Time.now() dt = (current_time - self.last_time).to_sec() self.last_time = rospy.Time.now()

In my code dt is equals only one cycle time. Do i need one cycle time or total time from begining. And i update self.last_time after calculations of dt every cycle. In that way i try to calculate one cycle time...

In addition i compute th (in my code pth - last angular position ) before the compute dx and dy. Because i use pth in equations of dx and dy. dth is angular distance in one cycle . if i turn my robot 180 degrees and go forward , will i use dth or pth ? If i understand right way, you suggest me that i should use dth (only 1 cycle angular difference)

bfdmetu gravatar image bfdmetu  ( 2020-08-05 02:23:18 -0500 )edit

You need the time from the beginning of computation if not, how you are supposed to accumulate odometry values? You really need to know the time passed since you started the initial movement. That is the reason why last_time is the time at which the first robot movement started. You are already calculating the cycle with the current_time. As for the orientation you should use the pth because is the accumulated value and you want to reflect in your odometry how much the robot moved from the initial position, same with the x and y, if you do not accumulate the values like that, the final result will not be real odometry values since they would not be calculated as an accumulation of the origin and they really cannot tell you where you are globally and how much the robot moved.

Weasfas gravatar image Weasfas  ( 2020-08-05 04:51:41 -0500 )edit