I was wondering how I could determine a robot's distance from a fixed point when the robot itself is constantly changing positions. I can keep encoders on the wheels and can also get data from a gyroscope and an accelerometer.
Determining a robot's distance from a certain point when the robot's position is constantly changing
$\begingroup$
$\endgroup$
Add a comment
|
3 Answers
$\begingroup$
$\endgroup$
If you know the position of the point at the begin, an easy solution would be to implement Dead Reckoning using the encoder value. Knowing the position of the robot at time t, compare it to its initial position and you can easily find where the fixed point is, in the robot frame (and thus calculate the distance).
Then to compensate the drift create by the dead reckoning process, you can use the values given by your IMU (gyro and accelerometer).
$\begingroup$
$\endgroup$
In addition to Malc's answer, a simple Dead Reckoning Algorythm might look like this:
$$ X_k = X_{k-1} + f(u) $$
where: $$ X=\left(\! \begin{array}{c} PosX \\ PosY \\ Heading \end{array} \!\right) \phantom {AAAAA} u=\left(\! \begin{array}{c} EncLeft \\ EncRight \end{array} \!\right) $$
$f(u)$ is the change of the state $X$ since the last measurement. To calculate this change you need the Measurement $u$, which is in the most simple version just your Encoder values. In this case the encoder values are already transformed to their respective distance, therefore the unit is in meters or something similar.
In addition you need your wheelbase $r$ and the SamplingTime $Ts$ of your algorythm.
The update of $PosX$ might be:
$$ PosX_{(k)} =PosX_{(k-1)} + \cos {(Heading_{(k)})} \cdot \frac{EncLeft_{(k)} +EncRight_{(k-1)}} 2 $$
Similar for $PosY$:
$$ PosY_{(k)} =PosY_{(k-1)} + \sin {(Heading_{(k-1)})} \cdot \frac{EncLeft_{(k)} +EncRight_{(k)}} 2 $$
The change in heading might be expressed with:
$$ Heading_{(k)}=Heading_{(k-1)}+ \arctan \left(\! \frac{EncLeft-EncRight} r \!\right) $$
Some Points, like the calculation of the heading, are pretty simplified, but may work in "homemade" application
Further improvements might be done with feedback from the IMU sensor.
$\begingroup$
$\endgroup$
This document has a good overview of mobile robot kinematics which is required to perform dead reckoning.
Adding the IMU provides more information and can correct for drift of the encoders. But now you need to fuse the data, typically with a kalman or particle filter. Traditionally, mobile robots will also have a planar laser range finder (LIDAR) sensor such as a Hokuyo. By doing incremental scan matching you can further improve accuracy. In addition to now being able to map and such. This is approaching SLAM.
Additionally, determining "ground truth" location is achieved through the use of other sensor modalities. For example a motion capture system like Optitrack, or a webcam pointed at the ceiling with April tags on it.
