pid - 180度偏航角不连续如何处理

Question

我正在研究车辆控制并使用 9DOF 传感器（加速度计、磁力计和陀螺仪）。对于偏航角，我在 pi rad 处存在不连续性问题。（180 度）。我正在使用 PID 控制器控制车辆，当车辆转弯超过 180 度时，标志突然改变（从 180 到 -180），这使得控制器行为怪异。当它转动超过-180度时也会出现同样的问题。

作为方法，我使用方向余弦矩阵来计算欧拉角。（Sparkfun 传感器的推荐方法。）

我的问题是我应该使用什么样的方法？在使用PID控制器控制偏航角的情况下如何处理这种不连续性。

Answer 1

我有同样的问题，我正在做以下事情：

#define MOD(a) ((a > 180.0) ? (a - 360.0) : ((a < -180.0) ? (a + 360.0) : a))

用这样的公式可以简单地将2个角度的差放回-180d/+180d。

Answer 2

这是你的问题吗？

After desired angle changes it's sign(ie. 180->-180) 
then suddenly control input(ie. P * error) becomes bigger.

例如。

--------------------------------
|  desired  |  real  |  error  |
|-----------|--------|---------|
|    170    |   160  |   10    |
|   -170    |   160  |  -330   |
--------------------------------

如果你的问题和我理解的一样，这个怎么样？在我们开始之前，请注意假设每个角度值都在 [-pi,pi] 的范围内。

error = desired - real;
if(error > 180)
   error = error - 360;
else if(error < -180)
   error = error + 360;
else
   error = error;//do nothing

这种方法总是选择一个方向，使控制输入变小。（你知道方向有两种选择）