ИСПРАВЛЕНА ОШИБКА при использовании библиотеки проводов для получения направления по компасу

Question

ИСПРАВЛЕНА ОШИБКА при использовании библиотеки проводов для получения направления по компасу

i2c code-optimization wire-library magnetometer

Я использую модуль магнитометра HMC-5983 для определения направления компаса автономного автомобиля, который я создаю с использованием платформы arduino.Я использую Ось Z и ось X для получения заголовка.

Однако, когда переменная headingFiltered равна 359, а автомобиль превращается в ноль, значение headingFiltered становится равным 0 по всем значениям от 0 до 360, а не просто становится 0. Это приводит к тому, что моя машина постоянно поворачивается из стороны в сторону, когда движется в этом соответствующем направлении. Кроме того, если в любом случае есть возможность улучшить эту функцию, я открыт для предложений.

Функция, которую я использую, заключается в следующем.

void chead() {

  //---- Ось X
  Wire.beginTransmission(Magnetometer); // передача на устройство
  Wire.write(Magnetometer_mX1);
  Wire.endTransmission();
  Wire.requestFrom(Magnetometer,1);
  if(Wire.available()<=1)
  {
    mX0 = Wire.read();
  }
  Wire.beginTransmission(Magnetometer); // передача на устройство
  Wire.write(Magnetometer_mX0);
  Wire.endTransmission();
  Wire.requestFrom(Magnetometer,1);
  if(Wire.available()<=1)
  {
    mX1 = Wire.read();
  }

  //---- Ось Y
  Wire.beginTransmission(Magnetometer); // передача на устройство
  Wire.write(Magnetometer_mY1);
  Wire.endTransmission();
  Wire.requestFrom(Magnetometer,1);
  if(Wire.available()<=1)
  {
    mY0 = Wire.read();
  }
  Wire.beginTransmission(Magnetometer); // передача на устройство
  Wire.write(Magnetometer_mY0);
  Wire.endTransmission();
  Wire.requestFrom(Magnetometer,1);
  if(Wire.available()<=1)
  {
    mY1 = Wire.read();
  }

  //---- Ось Z
  Wire.beginTransmission(Magnetometer); // передача на устройство
  Wire.write(Magnetometer_mZ1);
  Wire.endTransmission();
  Wire.requestFrom(Magnetometer,1);
  if(Wire.available()<=1)
  {
    mZ0 = Wire.read();
  }
  Wire.beginTransmission(Magnetometer); // передача на устройство
  Wire.write(Magnetometer_mZ0);
  Wire.endTransmission();
  Wire.requestFrom(Magnetometer,1);
  if(Wire.available()<=1)
  {
    mZ1 = Wire.read();
  }

  //---- Ось X
  mX1=mX1<<8;
  mX_out =mX0+mX1; // Необработанные данные
  // Из таблицы данных: 0,92 мГ/цифра
  Xm = mX_out*0.00092; // Единица Гаусса
  // * Магнитное поле Земли колеблется от 0,25 до 0,65 Гаусса, так что это те значения, которые нам нужно получить приблизительно.

  //---- Ось Y
  mY1=mY1<<8;
  mY_out =mY0+mY1;
  Ym = mY_out*0.00092;

  //---- Ось Z
  mZ1=mZ1<<8;
  mZ_out =mZ0+mZ1;
  Zm = mZ_out*0.00092;
  // ==============================
  //Вычисление курса
  heading = atan2(Zm, Xm);// тангенс дуги z/x

  // Корректировка курса с учетом угла склонения в зависимости от вашего местоположения
  declination = 0.03717551307 ;
  heading += declination;

  // Исправление, когда знаки почитаются
  if(heading <0) heading += 2*PI;
  // Корректировка за счет добавления угла склонения
  if(heading > 2*PI)heading -= 2*PI;
  headingDegrees = heading * 180/PI; // Заголовок в градусах
  // Сглаживание выходного угла / Фильтр нижних частот
  headingFiltered = headingFiltered*0.85 + headingDegrees*0.15;
  //Отправка значения заголовка через последовательный порт в среду IDE обработки

  // это было сделано, чтобы исправить ошибку. которая заключалась в том, что когда компас был повернут на 180 °, полученные показания не изменились на 180 °.
  if(headingFiltered >= 0 && headingFiltered <= 89){
    c_head=map(headingFiltered,0,90,270,359);
  }
  if(headingFiltered > 89 && headingFiltered <= 204){
    c_head=map(headingFiltered,90,204,0,90);
  }
  if(headingFiltered > 204 && headingFiltered <= 290){
    c_head=map(headingFiltered,205,290,91,180);
  }
  if(headingFiltered > 290){
    c_head=map(headingFiltered,291,359,181,269);
  }

  //Дальнейшая калибровка
  if(c_head>=0 && c_head<8){
    c_head=352+c_head;
  }
  if(c_head>=8){
    c_head=c_head-8;
  }

  // Serial.print("Градусы - ");
  // Serial.println(c_head);

  float teta1 = radians(clatval);
  float teta2 = radians(dlatval);
  float delta1 = radians(dlatval-clatval);
  float delta2 = radians(dlongval-clongval); //================== Расчет формулы заголовка================//

  float y = sin(delta2) * cos(teta2);
  float x = cos(teta1)*sin(teta2) - sin(teta1)*cos(teta2)*cos(delta2);
  float brng = atan2(y,x);
  degris = degrees(brng);// радианы в градусах
  degris = ( ((int)degris + 360) % 360 );

  dif_h=c_head - degris;
  abs_dif_h=abs(dif_h);
}

@AfiJaabb, 👍2

1 ответ

Лучший ответ:

@*Edgar Bonet* · Accepted Answer · 2019-11-27 19:30-00:00

Проблема возникает из-за используемого вами фильтра нижних частот:

// Сглаживание выходного угла / Фильтр нижних частот
headingFiltered = headingFiltered*0.85 + headingDegrees*0.15;

Этот фильтр предназначен для сглаживания быстрых изменений измеряемого курса. Если курс внезапно перескочит с 359 ° на 0 °, фильтр заменит этот скачок непрерывным изменением, которое проходит через промежуточные значения.

Я могу придумать три различных варианта решения этой проблемы.

Вариант 1: отфильтровать декартовы координаты

В отличие от направления движения, компоненты магнитного поля в плоскости плавно изменяются при вращении транспортного средства. Вы можете отфильтровать эти компоненты перед вычислением заголовка следующим образом:

const float filter_constant = 0.15;

float filtered_angle(float y, float x)
{
    static float x_f, y_f;  // отфильтрованные компоненты
    x_f += filter_constant * (x - x_f);
    y_f += filter_constant * (y - y_f);
    return atan2(y, x);
}

Тогда вы бы заменили

heading = atan2(Zm, Xm);

Автор:

heading = filtered_angle(Zm, Xm);

Вариант 2: отфильтруйте развернутый заголовок

Другой возможный подход заключается в том, чтобы развернуть заголовок перед его фильтрацией. Это похоже на разворачивание фазы: вместо того, чтобы требовать, чтобы заголовок ограничивался интервалом [−π, π] (или [0, 360 °), если вы работаете с градусами), вы позволяете ему принимать произвольные значения. Каждый раз, когда ваш автомобиль совершает полный поворот, направление движения в развернутом положении меняется на ±360°. Вот простой разворачивающий фильтр:

// Разверните угол в радианах.
float unwrap(float angle)
{
    static int revolutions;
    static float previous_angle;
    float delta = angle - previous_angle;
    previous_angle = angle;
    if (delta >= M_PI)
        revolutions--;
    else if (delta < -M_PI)
        revolutions++;
    return 2*M_PI*revolutions + angle;
}

Развернутый заголовок является непрерывным и может быть безопасно отфильтрован по нижним частотам , не создавая проблем, которые у вас есть сейчас. После фильтра нижних частот вы можете снова обернуть его с помощью функции fmod(), если хотите.

Вариант 3: оберните различия в заголовках внутри фильтра

Предыдущую идею можно применить и в обратном направлении: вместо того, чтобы разворачивать заголовок, вы можете обернуть различия заголовков в фильтр нижних частот, чтобы сохранить их в пределах [−π, +π) (т. е. [−180°, +180°)). Затем, если отфильтрованный заголовок равен 359 °, и вы получаете новый необработанный заголовок 0 °, разница составляет -359 °, но вы переносите ее на +1 °. Это приведет к незначительному увеличению отфильтрованного выходного сигнала до 359,15° при выбранной вами константе фильтра 0,15.

Вот фильтр нижних частот, основанный на этой идее, который знает, как должны обтекаться углы:

const float filter_constant = 0.15;

// Заверните угол в [-pi, pi).
float wrap(float angle)
{
    while (angle < -M_PI) angle += 2*M_PI;
    while (angle >= M_PI) angle -= 2*M_PI;
    return angle;
}

// Фильтр нижних частот под углом.
float filter_angle(float angle)
{
    static float filtered_angle;
    filtered_angle += filter_constant * wrap(angle - filtered_angle);
    filtered_angle = wrap(filtered_angle);
    return filtered_angle;
}

Я думаю, что этот вариант мне нравится больше, чем два других, потому что он выполняет свою арифметику таким образом, который согласуется с тем, как предполагается переносить углы, а также потому, что он сохраняет меньше состояния (только одна статическая переменная). Кроме того, вы реализовали что-то очень похожее на описанную выше функцию wrap(), чтобы обернуть заголовок в [0, 2π). Вместо этого вы могли бы повторно использовать обернуть() следующим образом:

// Обернуть в [0, 2 пи).
heading = wrap(heading - M_PI) + M_PI;

Обновление: Существует недавно опубликованная библиотека Arduino, которая реализует этот третий вариант. Он “предоставляет класс runningAngle, который вычисляет экспоненциально взвешенное среднее значение ряда углов, таких как показания компаса. Он знает, как углы переносятся по модулю 360 ° ”. Библиотека называется runningAngle и доступна через диспетчер библиотек Arduino IDE.