How do I calculate the direction robot is facing, pose from velocity and yaw rate?

Question

I have a simple car robot with four wheels moving relatively slowly (5~10 mph). The robot has a velocity magnitude and yaw rate sensor and the sensor samples about every 0.1 seconds. I want to map the location of the robot in the room in a global coordinates using these two values. What I am doing is robot starts out at location (x=0,y=0). I calculate the location by saying

x = velocity*cos(yaw_rate)*sample_interval
y = velocity*sin(yaw_rate)*sample_interval

So far with this method, the location calculated is actually fairly good as long as the yaw rate is not so large. But if my understanding is correct, yaw rate is the rate of angle change and not the angle changed and to get a better model and the actual direction the vehicle is facing, I will need to say integrate the yaw rate. I want the angle the robot's face normal vector makes to vertical axis so I can figure out the direction robot is facing or it's pose.

Now this must be a very well known equation in robotics and mechanical engineering but my back ground is in programming and I have no idea where to start. I tried searching on google but my search words must not be right since I can't find a good page. Can you point me to the right direction, let me know of a starter page that I can look at?

Answer 1

Close.

A simple solution is to first track the heading over time using the yaw rate sensors. If you're getting measurements every $\delta t$ seconds ...

$$ H(t+1) = H(t) + Yaw(t)*dt $$

The, you can use basically what you had to track position relative to starting configuration, by:

$$ x(t+1) = x(t) + \cos(H(t))*v(t)*\delta t $$ $$ y(t+1) = y(t) + \sin(H(t))*v(t)*\delta t $$

Note, this assumes, $x(0)=y(0)=H(0)=0$, so you'll have to "seed" $x(0),y(0), H(0)$ to get the "real" position and heading at time $t$.

This will do OK, but confounding factors are:

• The discrete heading is an approximation, the continuous yaw should ideally be integrated and a better movement model used
• Wheels slip, producing motion not well modeled
• You'll eventually want to fuse estimates of the robots actual location
• Note, this is called ackerman steering! Use that for your google-foo.

But this'll do for simple projects.

Answer 2

First of all, two equations you have mentioned above is completely wrong. If we consider the units, velocity - m/s, yaw rate -rad/s, time - s and after multiplying an additional 1/s is remaining. Maybe you are getting the yaw, instead of yaw rate. So its better to clearly mention what types of sensors you are using.

For these kind of position estimation, we usually use Extended Kalman Filter to fuse the measurements of several sensors. I recommend you to understand what is happening inside an EKF. There are plenty of online articles and youtube videos available. Further MATLAB has an inbuilt function for this.

After understanding concepts behind the EKF, you can identify two major steps are carried out when fusing measurements ; prediction step and update step. But since you dont have an absolute positioning system you can use only the prediction step. This is also known as dead reckoning. The main disadvantage of this method is that the error of the position estimation accumulates with the time.