调整导航应用程序中的指南针环绕

Question

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

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

例如，如果车辆被指示跟随360度的航向，它将不可避免地向以太侧漂移几度。如果它漂移到0+度，控制回路将变得疯狂，并试图将车辆一直转向360度，使其再次达到360度。

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

在编写导航函数时，我使用了一个外部PID控制器类，并像这样计算标题：

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

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

            steering.Degree = mv;
        }

谢谢!

Answer 1

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

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

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

Answer 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;
}

Answer 3

作为一个黑客，你有没有试过用720°的方向发送它？

否则，我将不得不查看您所调用的属性和方法的更多细节，才能进一步说明问题。