0
$\begingroup$

Rosanswers logo

Hello , I have an Ackerman car which I want to calculate its velocity using wheel encoders to feed it as odometry message to robot localization. I'm confused what should be the proper calculations for odometry message velocity for an Ackerman car. I assigned the child_frame_id of the message to the base footprint of the car , so the velocity is with respect to the base_footprint of the car (which is in the middle between the rear wheels).

I used 3 models and not sure which is the right one ??

1st model:

(Calculate right rear and left rear wheels velocities and get the steering angle of the car from another sensor)

speed= (vR + vL)/ 2.0
vx = speed * math.cos(wheelAngle)
vy = speed * math.sin(wheelAngle)
odomMsg.twist.twist.linear.x = vx
odomMsg.twist.twist.linear.y = vy
odomMsg.header.frame_id = 'odom'
odomMsg.child_frame_id = 'base_footprint'

2nd model:

(Calculate right rear and left rear wheels velocities and measure the distance between the 2 rear wheels)

LenghtBetweenTwoWheels=1.01
speed= (vR + vL)/ 2.0
v_th=(vR - vL)/ LenghtBetweenTwoWheels
vx = speed
vy = 0.0
odomMsg.twist.twist.linear.x = vx
odomMsg.twist.twist.linear.y = vy
odomMsg.twist.twist.angular.z = v_th
odomMsg.header.frame_id = 'odom'
odomMsg.child_frame_id = 'base_footprint'

3rd model:

(Calculate right rear and left rear wheels velocities and get the steering angle of the car from another sensor)

wheelBase=1.68
speed= (vR + vL)/ 2.0
v_th=speed*tan(wheelAngle)/ wheelBase
vx = speed
vy = 0.0
odomMsg.twist.twist.linear.x = vx
odomMsg.twist.twist.linear.y = vy
odomMsg.twist.twist.angular.z = v_th
odomMsg.header.frame_id = 'odom'
odomMsg.child_frame_id = 'base_footprint'

Any help ? Thank you


EDIT1:


a- My wheel encoders only ticks 52 each turn (The wheel diameter = 0.436m) so The difference between vR & vL wouldn't be very expressive, So I was hoping the third model is correct.

b- I tried the 3 equations: Used 3 sets of robot_localization nodes with same setup :

Setup:

ekf_local_node :

  • world_frame: odom
  • imu0: /imu
  • odom0:/wheel_odom(i)
  • output:/odometry/filtered1 "not used"

ekf_global_node :

  • world_frame: map
  • imu0: /imu
  • odom0:/wheel_odom(i)
  • output:/odom(i)

The only difference of the 3 set of nodes is that odom0 is /wheel_odom1 or /wheel_odom2 or /wheel_odom3 . Each of these topics calculate twist message according to one equation of last three models .

I turned 360 deg with the car ( used my max wheel angle ) and logged the results of the three ekf_nodes (odom)

The result was as shown in the pic:

yellow line : is the real turn ( using a 1 cm RTK gps with imu orientation)

red line : is the odom output from the first model equation (vx , vy)

green line : is the odom output from the second model equation (vx , vth) (vth=(vR - vL)/ LenghtBetweenTwoWheels)

blue line : is the odom output from the third model equation (vx , vth) (v_th=speed*tan(wheelAngle)/ wheelBase)

image description

-There is no difference between the second and third model result . Also , I tried to put vth = zero . And the same result happens. -All three equation doesn't give the right position.

Looks like robot_localization don't take vth into consideration. Is that true? , How can I feed the car velocity to robot localization properly?

Note: I'm using this setup only for debugging , My actual setup is one set of nodes with odometry/gps output from navsat_transform_node in the ekf_global node.


EDIT2:


My ekf setup: https://drive.google.com/open?id=1f4w1mnYbXsgBtpjhOFNz7w7uHDLrOVIZ

Sample sensor messages and 2 bag files of inputs and outputs : https://drive.google.com/open?id=1d2inyabSyBKxA76fs7oufuGZpYPMqtyJ

Excel sheet with input velocity ,input imu -yaw , output position and output orientation: https://drive.google.com/file/d/1XUTtrVWrUe3G60da8sXg5VIe1CssSBuN/view?usp=sharing

The reason I say vth may not be taken in consideration is : According to the excel sheet i attached vth in both equations returns different values. However the output is identical. Also I tried to edit one of the two input odometry messeges that outputs vx , vth so that : 1st: vx=speed vth=0.0 2nd: vx=speed vth=(vR - vL)/ LenghtBetweenTwoWheels

Also I saw that the 2 outputs are identical.


Originally posted by Mahmoud Kamel on ROS Answers with karma: 103 on 2018-07-03

Post score: 0


Original comments

Comment by Tom Moore on 2018-07-05:
The image is not showing up for me.

Comment by Mahmoud Kamel on 2018-07-05:
Sorry , I updated the pic

Comment by Tom Moore on 2018-07-05:
Please post your full EKF config, as well as a sample message from every sensor input.

Comment by Tom Moore on 2018-07-05:
By the way, if it didn't use theta velocity, you wouldn't see a circle. What makes you think it's ignoring it?

Comment by Mahmoud Kamel on 2018-07-05:
Thank you , I added my full config and sample messages .

Comment by Tom Moore on 2018-07-05:
Sorry, downloading bags and going through them is going to take more time than I have right now. Can you please paste your full EKF config and sample input messages inside the question itself? Use the code formatting.

Comment by aarontan on 2018-07-09:
what steering sensor were you planning on using?

Comment by Mahmoud Kamel on 2018-07-10:
pot slider ... some thing like that : http://www.surplussales.com/Potentiometers/Slider/Slider-1.html

$\endgroup$

1 Answer 1

0
$\begingroup$

Rosanswers logo

I'm looking at your config file, and I have a few comments:

  1. Are you trying to run all of these at the same time? That's going to cause all sorts of trouble in rviz, because they are all publishing the same transform. If you want to do an apples-to-apples comparison, I suggest you rostopic echo /some_filtered_topic -p > output.txt, and then plot the output using MATLAB or Octave.
  2. I'll tell you what I tell a lot of people: start simple. Forget the GPS for a moment, and forget testing all the models at once. Start with one simple EKF config that just fuses IMU and wheel encoder velocities from one of your models. Get that working well, and then plot the data. Then try with a different model, and plot that on top of it.
  3. You have two_d_mode turn on, but you are fusing roll and pitch in your IMU. It won't hurt anything, but it also won't actually do anything.
  4. Your first data sources are not fusing yaw velocity at all. I assume that's intentional.
  5. For wheel encoder data, you should always fuse the 0 value for y velocity (just set Y velocity to true). Give it a tiny covariance value, too. Your robot is not omnidirectional, so fusing a 0 value for Y enforces a kinematic constraint that stops the robot from moving sideways.

Originally posted by Tom Moore with karma: 13689 on 2018-07-05

This answer was ACCEPTED on the original site

Post score: 1


Original comments

Comment by Mahmoud Kamel on 2018-07-10:
Thank you Yes I run all nodes at the same time. , I'll do that and see the result.

$\endgroup$

Your Answer

By clicking “Post Your Answer”, you agree to our terms of service and acknowledge you have read our privacy policy.