I'm not quite sure how to apply a Proportional-Integral-Derivative control to a quaternion representing orientation. There are a few things I don't understand.
Given the PID control formula of:
output = proportionalTerm * error + integralTerm * errorSum + derivativeTerm * derivative
I am not sure if quaternion additions should be used (Which is maybe only correct for infinitesimal rotations, which I don't believe I understand properly). Or if quaternion multiplications should somehow be used in their place as they compose successive rotations?
From my understanding the error should be calculated as:
error = targetOrientation * conjugate(currentOrientation)
if error.w < 0 then error = conjugate(error)
But I'm not sure if the potential flip for the smaller angle, caused by the real part will somehow cause a problem?
I understand that an approximation to the derivative with respect to time is:
0.5 * quaternion_xyzw(angularVelocity.x, angularVelocity.y, angularVelocity.z, 0) * orientation * dt
So I could probably use that for the error quaternion, but do I simply scale the error quaternion by a linear proportionalTerm? Similarly for the derivative?
To compute the errorSum can I simply use a running quaternion addition? Do I need to normalise them if I do? Or is it more complicated as it rather has to be composed like quaternion multiplication?
For the first formula I stated I believe I have to run the whole thing across the x/y/z components of the quaternions involved but have no need for the resulting w real component? I am unsure where to normalise things across that if needed? And then how to correctly form the torque from that?
Best Answer
You also have to keep in mind what the output of the controller represents. In your case I assume that the output of the controller is equal to the input of the kinematic system, thus the angular velocity. However, the error should be of the same size as the output of the controller, which is a three dimensional vector and thus you can't directly use the error quaternion. In order to define this error I believe usually the axis multiplied by the smallest angle is used, which can be defined using
$$ e = 2 \tan^{-1}\left(\frac{\|q_v\|}{q_r}\right) \frac{q_v}{\|q_v\|}, $$
with $q_r$ and $q_v$ the real and vector part of the error quaternion respectively.