How to control PID Yaw

Question

My yaw angle varies from -180 degree to 180 degree.

                                        -170 170
                                   -135            135
                                 -90                  90
                                      45           45
                                          10 -10   

If my current heading is about 170 degree, then the wind makes it rotate to the left at about -170 degree, then how can PID control it to make it rotate back to the right at 170 degree.

Since, for PID ERROR = SETPOINT - INPUT In my case, SETPOINT = 170, and INPUT = -170, the the ERROR = 170 - (-170) = 340. So instead of moving to the right and apply PWM = 20, it rotate to the left and apply PWM = 340 and come back to the desired position, which is 170 degree?

Answer 1

The problem is here:

Since, for PID ERROR = SETPOINT - INPUT

This is only true for linear systems. In your system, your error never exceeds $180^\circ$ no matter how many times you rotate -- modular arithmetic is affecting your calculation. (For the PID to work properly for angles, you would need to keep track of the absolute yaw value, which would indicate how many rotations you've done in one direction or the other. That would only be relevant to cases where the error corresponds to the amount of "twist" in a rope or cable that connects 2 rotating elements.)

The simple solution is to say

ABS_ERROR = SETPOINT - INPUT
if ABS_ERROR < 180
    ERROR = ABS_ERROR
else
    ERROR = ABS_ERROR - 360

However, this could result in some instability if your system has a lot of momentum (large $K_d$ term). So a more correct solution would account for whether ABS_ERROR combined with that momentum should take the "long way" around instead of attempting to stop, change directions, and rotate the "shorter" distance.

Answer 2

While the books give general formulas, you need to have enough understanding to adjust them based on your specific application. The textbook definition of error is:

error = goal - position

where position is your current state and goal is the state you wish you were at. While this definition is ok for a cartesian space, in others, as you have observed, is not the best thing.

With your own system, where the goal and position are angles, you have in fact infinite possible values for error.

$goal = 2k'\pi+\alpha$

$position = 2k''\pi + \beta$

and therefore:

$error = 2k\pi + (\alpha - \beta)$

In your case, the error could be 340, 700, 1060, -20, -380 etc. Clearly, depending on what value you choose for the error (all of which are correct), the controller behaves differently. If you choose 1060 over 340 for example, the controller thinks it's much farther from the goal and therefore tries to get to it in much more of a hurry.

You would usually want the minimum movement to reach your goal, so from the values above you would want to choose -20. That's quite easy. Simply choose the value of error in the range $[-\pi, \pi)$ (or $[-180, 180)$ if you want). That would have the minimum absolute value among the infinite choices.

Answer 3

Shahbaz has the perfect explanation, the PID controller will not solve your problem by itself, what you have to do is figure out a piece of code that reads the angle and the reference, calculates the shortest distance from the reference to the current position and inputs it to the PID controller with the correct sign(+ or -). You can calculate the shortest distance by determining if where you want to go is less or more than 180º from where you are. The math would be simpler if you worked with angles between 0º and 360º because you wouldn't have to deal with negative angles.

                      |Ref|--->|"piece of code"|--->|PID|--->|Plant|----->
                                      ^                               |
                                      |_______________________________|