在导航应用程序中调整指南针环绕答案

【问题标题】：Adjusting for compass wrap around in a navigation application在导航应用程序中调整指南针环绕
【发布时间】：2011-02-09 15:21:56
【问题描述】：

我有一个应用程序，我在指南针方向上引导车辆，但当车辆从 360 度穿越到 0 度时我遇到了问题。

在这种情况下，控制回路中没有智能来计算转弯以跟随航向的最近方式。

例如，如果指示车辆跟随 360 度的航向，它将不可避免地向以太侧漂移几度。如果它漂移到 0+ 度，控制回路会发疯并尝试将车辆完全转向以使其再次达到 360 度。

有没有优雅的方法来处理这个问题？

navigation 函数的编写方式，我使用了一个外部 PID 控制器类，我这样计算航向：

            lock (steering)
        {
            if (!Engaged)
            {
                return;
            }

            double mv = 90 + Trim + pidController.CalculateCorrection(flyHeading, currentHeading);

            steering.Degree = mv;
        }

谢谢！

【问题讨论】：

标签： c# embedded control-theory

【解决方案1】：

假设您无法更改CalculateCorrection 方法，请在调用前添加这些行以确保增量在合理范围内。

if (flyHeading - currentHeading > 180) currentHeading+=360;
else if (flyHeading - currentHeading< -180) currentHeading-=360;

如果你能修复CalculateCorrection，把夹子放在那里，就像@Greg Buehler 的例子一样。

【讨论】：

我的猜测是你实际上想要调整 currentHeading 而不是 flyHeading （假设 flyHeading 是你想要跟踪的）；否则，您将通过在 360 度分开的目标之间翻转来混淆 PID 控制器的组成部分。
@Brooks Moses，好点，已更正。（我做了一个错误的假设，即想要的和测量的）
另外，flyHeading 是我想要跟踪的标题。 currentHeading 是我目前正在使用的（又名：指南针的实时读数）。对不起，令人困惑的命名方案......

【解决方案2】：

这听起来可能过于简单，但您不能只使用目标矢量和当前矢量之间的增量，以便始终测量从 0 开始的距离，并且您的方位指示现在在 -180 和 180 处换行吗？

我也不确定您是如何计算方位的，但快速查看Ardupilot 会显示方位计算为：

int calc_bearing(float flat1, float flon1, float flat2, float flon2)
{
  float calc;
  float calc2;
  float bear_calc;
  float diflon;
  //I've to spplit all the calculation in several steps. If i try it to do it in a single line the arduino will explode.
  flat1=radians(flat1);
  flat2=radians(flat2);

  diflon=radians((flon2)-(flon1));

  calc=sin(diflon)*cos(flat2);
  calc2=cos(flat1)*sin(flat2)-sin(flat1)*cos(flat2)*cos(diflon);

  calc=atan2(calc,calc2);

  bear_calc= degrees(calc);

  if(bear_calc<=1){
    bear_calc=360+bear_calc;
  }
  return bear_calc;
}

并通过夹紧来纠正航向错误：

int heading_error(int PID_set_Point, int PID_current_Point)
{
 float PID_error = PID_set_Point - PID_current_Point;

 if (PID_error > 180) {
   PID_error -= 360;
 }

 if (PID_error < -180) {
   PID_error += 360;
 }

 return PID_error;
}

【讨论】：

是的，我的方位计算与您列出的非常接近，但是我得到了 0-360 度数。我可以随时切换并将其转换为 -180 到 180。

【解决方案3】：

作为 hack，您是否尝试过以 720° 航向发送它？

否则，我必须查看您正在调用的属性和方法的更多详细信息才能进一步了解。

【讨论】：