3
$\begingroup$

It's not technically robotics but:

I've been trying to reproduce in Simulink a spacecraft attitude simulation using quaternions, and the kinematics and dynamics seem to work fine, however I'm having a bit of trouble with the controller.

I followed the model give in the 7th chapter which seems to be some sort of a PD controller. The control equation I used is:

Control law $q_e$ is the quaternion error, $\omega_e$ is the rotation speed error

But my results seems to be off.

With : Initial quaternion and rotation speed are $q_i = [0;0;0;1]$ and $ \omega_i = [0;0;0]$

I give a desired reference of $q = [0;1;0;1]$ and $ \omega = [0;0;0]$.

I get the following response: Step Response

  • $q(1)$ and $q(3)$ are staying at zero as expected.

But :

  • $q(2)$ is going towards -1 instead of 1 (As far as I understand the sign ambiguity does not explain this since q(4) is staying around 1)

  • $q(4)$ is not maintaining at 1. (I am not sure if this is related to the fact that the controller is only a PD)

I've tried to add -1 gains but it doesn't seem to solve the problem.

Why would the step response of q(2) be going to -1 instead of 1 ? And why is q(4) decreasing ?

For reference I've added the simulink model: Simulink model

And the "Error quaternion" block: Error block

Edit: (Response after Chuck's answer) enter image description here

$\endgroup$
5
$\begingroup$

Welcome to Robotics, PaoloH! This is a fantastic question for Robotics - It has some Matlab/Simulink, some control theory, some spatial (quaternion) representations, etc. Robotics is the place to come when your question spans multiple fields!

In looking at your question, the thing that I noticed is that your reference quaternion is $[0; 1; 0; 1]$. It is not a unit quaternion, and I believe this may be your issue.

I looked over your block diagram, and I didn't see anything glaringly wrong there. As SteveO mentioned, the way you're treating the references is a little unusual, but the math all works out. I can't see what you're doing behind the "reference quaternion" or "error quaternion" blocks, but let's take a look at that unit quaternion.

Right now, the magnitude of your reference quaternion is

$$ \sqrt{0^2 + 1^2 + 0^2 + 1^2} = \sqrt(2) \approx 1.414 \\ $$

If you want to convert your reference quaternion to a unit quaternion, then you divide each term of the quaternion by the magnitude of the quaternion, and you wind up with a reference unit quaternion of:

$$ q_{\mbox{ref}} = \left[\begin{array}{ccc} 0 \\ \frac{1}{\sqrt{2}} \\ 0 \\ \frac{1}{\sqrt{2}} \\ \end{array}\right] \\ q_{\mbox{ref}} \approx \left[\begin{array}{ccc} 0 \\ 0.707 \\ 0 \\ 0.707 \\ \end{array}\right]; $$

You can review your quaternion output plot and see that q(2) and q(4) are both moving toward a numeric value of ~0.7. The only real problem seems to be that the polarity on q(2) is wrong.

I would guess your "reference quaternion" block is to make the skew symmetric matrix for the "error quaternion" block? The sign problem on your quaternion output could be hiding anywhere, but I'd double check the skew symmetric matrix (that it is actually skew symmetric; $\Omega^T == -\Omega$), and then I'd check the gains. When I said the math all works out on the reference handling, I double checked that just now and it works out for the $\omega$ speed handling. I can't actually tell for the quaternion handling.

Typically, error is (reference - feedback), then you apply gains and sum the scaled error terms.

You have your error as (feedback - reference), and then you apply gains and negate the scaled error terms. BUT, it looks like, for quaternion error, you actually ARE taking (reference - feedback) but then you're still inverting it anyways.

If this isn't enough to get your question resolved, please edit your question to show what's going on under the "Reference quaternion" and "error quaternion" blocks.

$\endgroup$
  • $\begingroup$ I think you nailed that one, I completely forgot about the normalization, I looked and as you predicted they were converging to 0.707... Then for the matrix I copied correctly the matrix from my reference in equation (7.2) but it wasn't a skew symmetric matrix so I double checked the with the original reference "Space Vehicle Dynamics and Control, Bong Wie", and see what the actual matrix was, and after correction it's now indeed converging toward +0.707... And just for the last part of your answer I added the error quaternion block, this looks good to me but i'm not sure. $\endgroup$ – PaoloH Aug 15 '18 at 16:32
  • $\begingroup$ @PaoloH - Great! Glad to help. Again, this question was a great one for the site. If you've got any others like it, please bring those here, too! Welcome again :) $\endgroup$ – Chuck Aug 15 '18 at 17:18
0
$\begingroup$

It looks like you are negating your reference inputs in the summing junctions.

$\endgroup$
  • $\begingroup$ But isn't that coherent with the control law I'm using ? $\endgroup$ – PaoloH Aug 15 '18 at 1:28
  • $\begingroup$ Comparing figure 7.2 with your topology it doesn’t seem so. I have to admit, though, that I haven’t worked out the equations fully. $\endgroup$ – SteveO Aug 15 '18 at 1:39
  • $\begingroup$ As far as I understand, if I compare with Figure 7.2, the measurement is subtracted to the reference because of the minus K_y gain and this becomes the demanded torque (N_dem). And in my model (which might be a bit confusing, I should have done it the same way they did). The reference is subtracted to the measurement (so the other way around) but it goes through the minus-minus junction before being the demanded torque so it cancels out. $\endgroup$ – PaoloH Aug 15 '18 at 1:56

Your Answer

By clicking “Post Your Answer”, you agree to our terms of service, privacy policy and cookie policy

Not the answer you're looking for? Browse other questions tagged or ask your own question.