0

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

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

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

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

导航函数的编写方式，我使用外部 PID 控制器类，我计算标题如下：

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

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

            steering.Degree = mv;
        }

谢谢！

4

3 回答 3

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;
}
于 2011-02-07T23:40:58.460 回答
2

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

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

如果可以修复CalculateCorrection，请将夹子放在那里，就像在@Greg Buehler 的示例中一样。

于 2011-02-08T00:39:07.827 回答
0

作为一个黑客，您是否尝试过以 720° 的航向发送它？

否则，我必须查看您正在调用的属性和方法的更多详细信息才能更有意义。

于 2011-02-07T23:24:50.363 回答