Control System - problem with pitch at >90 yaw error

[btmoore]

There is a potential issue with the control system. I believe I have a fix - I need somebody to add it to the control system and test it in the simulator.

The problem is that for relative orientation goals (from current orientation, not absolute) with yaws greater than +/- 90 degrees, any pitch error is going to have positive feedback.

Test 1: -set yaw P = very small, I = 0, D = 0 so that yaw rotation is slow. -set submarine to hold orientation [Roll Pitch Yaw ] = [0 0 0] -Once there, set goal to [0 5 180]. --Observe results - see if submarine begins to pitch excessively. If possible, track pitch error and see if it grows.

This should confirm the problem exists.

To fix, the pitch error needs to be multiplied by the cosine of the yaw error.

ePitch = ePitch * cosd(eYaw);

To fix, add the following code at line 392:

389> Vector3d rotation_error = ir3D( 390> r3D(state_vector.segment<3>(3)).transpose() * 391> r3D(rotation_goals)); 392>

//            rotation_error.segment<1>(1) *= cosd(rotation_error.segment<1>(2));    //

393> / 394> Update the current error vector with the calculated errors. 395> */ 396> current_error = Vector6d::Zero(); 397> current_error << translation_error, rotation_error; 398>

Test 2: -set yaw P = very small, I = 0, D = 0 so that yaw rotation is slow. -set submarine to hold orientation [Roll Pitch Yaw ] = [0 0 0] -Once there, set goal to [0 30 180]. --Observe results - see if submarine begins to pitch up or down. If possible, observe pitch error and see if it goes towards zero.

Test 3: -set yaw PID to normal so that rotation is normal. -set submarine to hold orientation [Roll Pitch Yaw ] = [0 0 0] -Once there, set goal to [0 30 180]. --Observe results - see if submarine begins to pitch up or down initially. See if it properly starts going up once yaw<90. If possible, observe pitch error and see if it goes towards zero.

This should hopefully solve the problem.

There is a second problem, which is that the submarine is unlikely to acomplish exotic rotations like [180, 45, 160] very elegantly. It may even fight itself. scaling Pitch error by cos(Yaw) should help. But there is something else that must be done.

I'm less certain about this change than the other one, so it needs good testing. I think I can declare the problem to exist when the yaw and roll error are both greater than +/- 90. If they are, the fix seems to be making the yaw parameter pursue the supplement angle to the goal.

if(abs(eYaw) > 90 && abs(eRoll) > 90) { eYaw = rollover(eYaw-180); eRoll = rollover(eRoll-180); }

The syntax of which I believe would be:

392>if( (fabs(rotation_error.segment<1>(2) >90) && (fabs(rotation_error.segment<1>(0) >90)){ 393>{ if(rotation_error.segment<1>(2) > 0) { rotation_error.segment<1>(2) -= 180; }else { rotation_error.segment<1>(2) += 180; } if(rotation_error.segment<1>(0) > 0) { rotation_error.segment<1>(0) -= 180; }else { rotation_error.segment<1>(0) += 180; } }

rotation_error.segment<1>(1) *= cosd(rotation_error.segment<1>(2)); //first change//

or some equivalent. A rollover function in the rotation_engine.cpp might be useful.

Test 4: -set yaw,pitch,roll PID regular. -set submarine to hold orientation [Roll Pitch Yaw ] = [0 0 0] -Once there, set goal to [180 45 180]. --Observe results - see if submarine appropriately flips over on its back.

Test 5: -set yaw,pitch,roll PID regular. -set submarine to hold orientation [Roll Pitch Yaw ] = [0 0 0] -Once there, set goal to [180 45 135]. --Observe results - See how submarine behaves. Does its motion look fluid and efficient?

Test 6: -set yaw,pitch,roll PID regular. -set submarine to hold orientation [Roll Pitch Yaw ] = [0 0 0] -Once there, set goal to [-135 -45 100]. --Observe results - See how submarine behaves. Does its motion look fluid and efficient?

[skallaher]

Test 1 results.

[skallaher]

Test 2 results, pending.

[btmoore]

That looks like test 1 shows the positive-feedback for pitch, ramping up close to 90, and then kind of falling/rotating over itself. Correct?

[skallaher]

That was what it looked like, yes.

[btmoore]

Cool. Nice to know things are failing catastrophically the way I predicted.

...wait

[skallaher]

Yep. I'm attempting to implement your fix and I'm not familiar enough with Eigen. What kind of cosine is it? Is it supposed to be matrix-wise, entry-wise, coefficient-wise or otherwise? I tried the syntax you specified and there isn't a cosd function defined.

[btmoore]

I was going based off the functions in the rotation_engine.cpp

https://github.com/PalouseRobosub/robosub/blob/dev/src/movement/rotation_engine.cpp

I don't know if those initial helper functions extended outside the engine. Perhaps not.

The second element of rotation_error (ePitch) is supposed to be multiplied by the cosine of the 3rd element (eYaw) and stored as the second element of that same vector. maybe *= doesn't work either with matrices, so maybe make it explicit on both sides of the equation.

I think you can reference the matrix elements by rotation_error(m,n) but I didn't see any obvious examples so I just use the segment syntax.

[skallaher]

Ah, okay. Eigen just returns odd data types when segmented the way you mentioned. I'll look into other ways to access the data and see if that helps.

[skallaher]

I found what it was. The rotation engine doesn't expose the cosd, sind and such functions. The only ones available (currently) are r3D, r3Dv, ir3D and pinv.

[btmoore]

Ah. I''m surprised it's gone this long without them being used elsewhere.

If Dustin continues to build the localization stuff, I can pretty much guarantee he'll need those. Probably worth changing them to be exposed as well.

[skallaher]

I'll do the test manually using the calculation within cosd for now.

[btmoore]

sounds good. Sorry about that.

[skallaher]

No problem. Here's the results for Test 2.

[skallaher]

Followed by Test 3.

[skallaher]

I wasn't sure if I had the PID values correct for Test 2, so I've updated the image just in case.

[btmoore]

Well, Test 2 it looks like it's not trying to do a really sad backflip anymore. And that roll looks like it's the right amount.

Did it look nice/efficient visually, or was it still jerky at all? Ideally it should kind of have close to zero pitch around the half-way point. The roll seems like its oscillating a bit, but that's just a tuning issue.

Test 3 is a bit confusing. It seemed to way overshoot its goal for yaw. Is that using the regular PID values? Seems like D needs to be higher - it's acellerating all the way up to the 180 degree mark.

[ryan-summers]

cosd, sind, and etc were recently added for use as cosine (degrees) sin(degrees) etc for the rotation engine. We weren't doing degrees with cosines and sines anywhere else. I don't see why we shouldn't namespace them into the repository.

[skallaher]

I just realized that I've been changing the values for roll, not yaw... I'll update the images accordingly for each test.

[skallaher]

Results updated. Test 3 uses the current default values for yaw, I don't know if they've been tuned at all or not.

[btmoore]

Interesting. The values for Test 3 take twice as long to stabilize as Test 2. I think more than anything that just indicates a need for better tuning.

Test 2 accomplishes what it's supposed to in about 6 seconds compared with roughly 10 seconds for Test 1, and it does it without going nose-up, so overall I'd call that a success.

Providing integral offsets based on orientation should also improve this significantly, as roll and yaw require a lot of force to maintain non-zero orientations, and that builds up powerful integral terms that have to be reversed at the drop of a hat when the submarine rolls-over. @irwineffect was asking about physically modeling the buoyancy, and inside the control system would be the place to do it, basically by treating gravity and buoyancy as thrusters and calculating against the forces/torques they exert. That's outside the scope of this issue, but something to think about.

@ryan-summers ah, that makes sense. I didn't know if you calculating things in degrees or not elsewhere. I had to rotation engine done in degrees because when debugging it, it's so much easier to read degrees than radians for cessing out calculation errors. Whether you want to use degrees to calculate in any of your other programs is up to you.

@skallaher The next step when you have time would be to try the second modification I talked about and try Test 4 and 5. I'd suggest sticking to the PID values you used in Test 2 vs Test 3 for now. Ideally for test 4 the submarine will align itself with the opposite desired roll and yaw, and flip itself over backwards. Test 5 is similar, but it shouldn't flip entirely over its back so much as kind of roll with the side. Similar to what it did in test 1 - but only because flipping over backwards should be more efficient in this case,whereas it should be less efficient to do so for Test 1's goal.

[skallaher]

Test 4 resulted in the sub fighting itself and eventually reaching a stable point at about [50,0,110].

I used P=0.01, I=0, D=0 as per Test 2. Also as a note that Test 3's values were P=0.07, I=0.01, D=1, I believe.

[skallaher]

Test 5 was similar, though stabilizing at [-50,0,-150]

[btmoore]

Gotcha. That is a really low setting. Explains why it takes 5-10 seconds to turn around.

I'll have to think a bit about why test 4 and 5 are fighting each other.

Can you run test 3 again, first with derivative set to D = 0.4, and second with it set to D = -0.4. I want to see the behavior change, and I also want to make sure that the derivative term has the proper sign, because that exponential growth towards yaw=180 could just be the integral term winding up, or the derivative exerting a positive feedback on motion.