3

I have odometry data $(x, y, angle)$ of a real two-wheeled robot, who received control commands $(forward speed, angular speed)$.

Now I want to code a motion model (in C++ (/ROS)) which should follow the same trajectory, given the same control commands.

Normally, the kinematics should look something like this:

$$ \begin{align} v_{fwd} &= control_{fwd} \\ v_{ang} &= control_{ang} \\ x &= x_{old} + 0.5(control_{fwd} + v_{fwd,old}) * \cos(angle) * dt \\ y &= y_{old} + 0.5(control_{fwd} + v_{fwd,old}) * \sin(angle) * dt \\ angle &= angle_{old} + 0.5(control_{ang} + v_{ang,old}) * dt \end{align} $$

And I thought about just setting

$$ \begin{align} v_{fwd} &= control_{fwd} + k_1 v_{fwd,old} + k_2 v_{fwd,old}^2 + k_3 v_{ang,old} + k_4 v_{ang,old}^2 \\ v_{ang} &= \text{ ...analog...} \\ x, y, angle &\text{ unchanged} \end{align} $$

and then just search the minimum of the squared distance of the computed trajetory to the real one - depending on the values of $k_i$. This would mean either a good optimization algorithm or just brute-forcing / randomly testing a lot of values.

Is this the way to go here? I tried the 2nd approach, but the results so far are not that good.

So, as you might guess now, I'm pretty new at this, so any help is appreciated.

Bending Unit 22
  • 1,435
  • 10
  • 13
user3926
  • 31
  • 2
  • 1
    How great are the error values of wheel slip, and angle inaccuracy? The model could be perfect, but if your real-world errors are high, than the model will deviate strongly from reality, regardless of algorithm, unless this is taken into effect. – Mhz4.77 Apr 04 '14 at 01:41

1 Answers1

1

What are you trying to reach is called "system identification": for given raw data of a real system, a computersimulation has to build with the same behavior. The challange is hard, because the simulation is done on a computer and the computer needs a turing-like program. The program consists of loops, if-then-statements and variables. To automate the generation of such a program is not possible. Nevertheless there is research on this field which is called "model learning". Until now, this research is not good enough for a practical demonstration. The alternative to automatic system identification is to programm the simulation manually. The results are physics-engines like ODE which are used in Gazebo or box2D which is well suited for 2D simulation. It is possible to tune these simulators for fitting to real world by adjustig gravity parameter or damping-values.

A more advanced technique is to build a motion model from scratch. This can be done on a basis of a gui, where the objects are visable and a program which control these objects. A good starting point is the motion model of the Original Post which calculate new x/y value with help of the old value and a sin(angle) addition. The limit of this motion model is that no obstacle is detected. A tutorial for programming a better bouncing ball physics engine in pygame is give here.

Manuel Rodriguez
  • 794
  • 3
  • 17