Pose Estimation

From Team 449 Wiki

This page details the derivation of the algorithm we use to draw profiles. It finds the next position of a 2-sided tank drive given the past position and the distance traveled by each wheel since that position. It assumes velocity was constant on that interval. In this description I'll be referring to each interval as a "tic."


For the sake of these calculations, we treat the tank-drive robot as two wheels along an axis (corresponding to the center of the robot). During each tic, we assume the robot can be doing one of three things: driving in a straight line, turning along some circle, or standing still. The approximation of circular movement is convenient; it is easy to calculate and (assuming constant velocity) accurately accounts for robot movement with errors on the order of O(x^3).

If the robot is standing still, the next position is the same as the current position. If it's driving in a straight line, we just move its position in the direction it's facing by the measured distance moved. However, if it's turning, things get a bit more complicated. The robot is turning some angle around some point. We know that point must be in a line with the two wheels, but we don't know how far away it is (it can be between the wheels). The point it's turning around is known as the Instantaneous Center of Curvature, or ICC.


Diagram with labels showing the variables

\phi is the robot's previous heading (i.e. what angle it was pointed at at the beginning of the tic)

T is the distance between the wheels

D_{L} is the distance traveled by the left wheel this tic

D_{R} is the distance traveled by the right wheel this tic

r is the radius of the circle the robot is turning on

\theta is the angle the robot is turning around that circle

Finding the Sector

First, we'll find the r and \theta that describe the sector traced out by the robot's path. Theta is easy, it's just \frac{D_{L}-D_{R}}{T} radians. To see why this makes sense, try a few examples: 0 and 1, 1 and -1, and, to take a 90 degree (or \frac{\pi}{2} radian) pivot turn, the right side moves 0 and the left side moves a quarter of the circumference of the circle with a radius of T, or \frac{2T\pi}{4}. Determining r is a bit trickier. Something to note here is that if \theta is 0, the sector is actually just a rectangle so r is infinity. We ignore that case because it's easy enough to handle separately (the robot's just driving straight). The formula is

r=\frac{T}{2} \cdot \frac{D_{L}+D_{R}}{D_{L}-D_{R}}

To derive this formula, start with the observation that the ratio between the distances (\frac{D_{L}}{D_{R}}) is equal to the ratio between the radii (\frac{r+\frac{T}{2}}{r-\frac{T}{2}}). Think through this, especially with the sample cases above, and you'll see it makes sense. Then, solve for r:

\frac{D_{L}}{D_{R}} \cdot (r-\frac{T}{2}) = r+\frac{T}{2}
\frac{D_{L}}{D_{R}} \cdot r - \frac{D_{L}}{D_{R}} \cdot \frac{T}{2} = r+\frac{T}{2}
\frac{D_{L}}{D_{R}} \cdot r - r = \frac{T}{2} + \frac{D_{L}}{D_{R}} \cdot \frac{T}{2}
r(\frac{D_{L}}{D_{R}} - 1) = \frac{T}{2}(\frac{D_{L}}{D_{R}} + 1)
r = \frac{T}{2} \cdot \frac{\frac{D_{L}}{D_{R}} + 1}{\frac{D_{L}}{D_{R}} - 1}
r = \frac{T}{2} \cdot \frac{\frac{D_{L} + D_{R}}{D_{R}}}{\frac{D_{L} - D_{R}}{D_{R}}}
r=\frac{T}{2} \cdot \frac{D_{L}+D_{R}}{D_{L}-D_{R}}

Finding the Vector

A diagram of the sector with the vector (blue) overlaid

Next, we want to turn the sector we found into a change in Cartesian coordinate values. To do this, we'll draw a vector pointing from the center of the where the robot was last tic to where it is now. We'll call this vector \vec{a}, and call the angle between it and the X-axis \alpha. To find \left\|\vec{a}\right\| (the magnitude or length of \vec{a}) and \alpha, we can draw an isosceles triangle with legs of length r and a base of length \left\|\vec{a}\right\|, with a vertex angle of \theta and therefore base angles of \frac{\pi-\theta}{2}, as shown in the diagram. Knowing everything about this triangle, we can then see that \alpha=\frac{\pi-\theta}{2}-(\frac{\pi}{2}-\phi)=\phi-\frac{\theta}{2} because we take the angle between \vec{a} and the line connecting the robot's wheels last tic (\frac{\pi-\theta}{2}), and subtract the angle between that line and the X-axis (\frac{\pi}{2}-\phi), yielding the angle between \vec{a} and the X-axis. Finding \left\|\vec{a}\right\| is even easier because we can just use Law of Sines:

\frac{r}{\sin(\frac{\pi-\theta}{2})}=\frac{\left\|\vec{a}\right\|}{\sin \theta}
\left\|\vec{a}\right\|=r \cdot \frac{\sin \theta}{\sin(\frac{\pi-\theta}{2})}=r\cdot\frac{\sin{\theta}}{\cos{\frac{\theta}{2}}}=2\cdot r\cdot\sin{\frac{\theta}{2}} (note that this can also be seen directly by bisecting \theta and considering the resulting right triangle)

Now all we have to do is turn this vector into a change in Cartesian coordinates, which we can do by splitting \vec{a} into it component vectors. In order to find the vectors for the left and right wheels, replace r with r+\frac{T}{2} and r-\frac{T}{2} respectively.

Practical Implementation

In practice, encoder measurements are not perfectly reliable indicators of robot movement: wheels can slip, such as during hard acceleration or a collision, de-coupling the measured wheel movement from the actual movement of the robot. For this reason, pose estimation works best when the robot is moving in a smooth, well-controlled manner, such as during autonomous (especially when following motion profiles that have been chosen to limit slip-causing factors such as jerk and acceleration).

Even under relatively ideal conditions, however, tank drives inevitably experience some degree of slippage due to "wheel scrub" during turns. In practical terms, this leads to the calculated value of \theta ending up somewhat larger than actual angular change of the robot.

A "first-order" correction for this problem can be achieved by substituting an inflated, "effective" value of T for the actual geometrical value, thus operating under the assumption that the effect of wheel scrub is to decrease \theta by some constant factor relative to the theoretical value. In order to determine a suitable "effective" value of T, rather than measuring the robot, one can run a motion-profile that spins the robot in place with each side of the drive moving a known distance, and measure the resulting angular change; T can then be back-calculated from the equations above. Repeating the test for a variety of distances and averaging the results may yield a better value.

While this correction works reasonably well given slow, well-controlled conditions, it is still highly imperfect. A better solution involves the use of a gyroscope to directly measure \theta. The simplest implementation is to simply neglect to calculate \theta from the encoders, and instead directly use the measured value from the gyro. The encoders are thus used only to determine the value of r; assuming that wheel scrub contributes only to angular error, and not to translational error, we instead calculate r=\frac{D_{L}+D_{R}}{2\theta}. Note that no value of T is needed at all for this method.

More complex solutions may involve fusing gyro and encoder estimates for \theta in more rigorous ways, such as through a Kalman filter.