User Tools

Site Tools


turtlebot:adventures:sensing101_thetaerror

The orientation estimate is published as a quaternion for the Turtlebot. However, the turtlebot is a planar creature, so the full quaternion is never used. What parts are used? Which of the four values in the odometry quaternion correspond to the real value? The other one would then be the complex value. Which is it?

It turns out that these two parts used correspond to a planar quaternion, which is basically just a complex number $ z = x + jy$. Be careful though because the quaternion version has half the phase. When you unpack the quaternion into a complex number, be sure to square it first so that it has the correct phase. Also make sure that you set the proper parts to be the real and the complex coordinates. OK, let's assume that you have a desired orientation that gets created as a complex number $$ z_{des} = e^{j \theta_{des}} = \cos(\theta_{des}) + j \sin(\theta_{des})$$ In python, one would use the cmath package and the rect function to create the complex number. Naturally, since only rotations are the concern, the magnitude should be 1 (to create a unit length complex number). Basically $z_{des}$ lives on the unit circle, which makes it equivalent to an orientation.

OK, now that we have $z_des$, and there is a callback function that unpacks the quaternion orientation into a complex number too, called $z_{curr}$ for the current orientation, we need to create the error function. This is easily done via: $$z_{err} = z_{curr}^{-1} z_{des} = z_{des} \,/\, z_{curr},$$ where we took advantage of the fact that complex numbers behave more or less like scalar real number when performing multiplication by the inverse (e.g., performing division).

The variable $z_{err}$ is the orientation error as a complex number. To get the error in angular units, just recover the phase: $$ \theta_{err} = \text{phase}(z_{err})$$ which also happens to be a cmath python command. These units should be in radians which is compatible with the Turtlebots angular command inputs.

The last step is to convert this error into a feedback controlled signal, which is done through a gain $$ \omega = k_{turn} \theta_{err}. $$ Of course, in real life, this might lead to crazy-high control signals when executed on the Turtlebot. Turn rates that are not achievable by the Turtlebot should be avoided. What people do in practice is implement some kind of saturation function, $$ \omega = \text{saturate}(\omega, \omega_{max})$$ which clips the control signal at a max value if it is too high relative to $\omega_{max}$ (or a min value if it is too low relative to $-\omega_{max}$. Python does not have a saturation function, so you'll have to code one up with if-else statements. It's best to make it an actual function so you can reuse it.

There are other ways to do this, but the above generalizes to when you have to do full 3D rotations, plus it also is compatible with more complicated nonlinear controllers for the Turtlebot. Thus, it is a simple version of more advanced nonlinear control strategies. If you get this one, then you'll get the others automatically.


Sensing 101

turtlebot/adventures/sensing101_thetaerror.txt · Last modified: 2024/08/20 21:38 by 127.0.0.1