differential drive odometry equations in python
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
@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.
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, you should be using
dtthat 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 computex,yandthnot onlyxandy. And then do the accumulation of the three variables, that is:If you notice you are computing
dxanddywith the accoumulatedthfrom the last cycle not the current one.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)
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_timeis the time at which the first robot movement started. You are already calculating the cycle with thecurrent_time. As for the orientation you should use thepthbecause is the accumulated value and you want to reflect in your odometry how much the robot moved from the initial position, same with thexandy, 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.