Навигация внутри помещений с использованием BLE-маячков

Использование BLE-маячков для навигации внутри помещений обсуждается уже несколько лет. Покопавшись в Интернете, вы найдете немало статей, как теоретических, посвященных предпосылкам работы технологий, так и практических – об опыте использования маячков. При изучении материалов выяснилось, что из-за высокой чувствительности маячков к ряду факторов, технология слабо подходит для навигации внутри помещений.

В этой статье мы приводим код алгоритмов, который позволит написать демо-версию навигационного приложения. Экспериментируя с более сложными алгоритмами и подходами для улучшения точности результата, можно улучшить работу приложения.

Подходы к определению местоположения

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

Выделяют 2 группы алгоритмов, различающихся по методам сбора данных:

  • Первая основана на предварительном сборе «отпечатков» сигналов в разных точках помещения с заданным интервалом, например, каждые 3 метра. Маячки отправляют сигналы, которые система сравнивает с имеющейся базой и выдает результат о расположении предмета.
  • Вторая группа берет за основу только информацию о силе сигнала, собираемой в момент определения местоположения.

Чтобы выбрать подходящую группу алгоритмов, нужно учесть минусы каждой. Первая группа требует предварительных затрат на первоначальный сбор базы сигналов. Также стоит учитывать, что существуют сроки валидности такой базы. При перемещении предметов в помещении придется составлять ее заново, ведь сигналы маяков обладают высокой чувствительностью к внешней среде. У второй группы такой проблемы нет, но в больших помещениях и при низкой плотности сети точность результатов снижается.

Возникает ряд вопросов. А что значит ниже, насколько? Какая точность является приемлемой и какой уровень точности достижим? Чтобы ответить на эти вопросы, мы решили реализовать один из самых простых методов второй группы — трилатерация на основе расстояний до ближайших трех маячков. Расстояние до маячка рассчитывается на основе мощности его сигнала.

Тестовая среда

Схема тестовой среды

Рис. 1 Схема расположения маячков в помещении

Комната 6*9м. Маячки расположены по краям одной стены и по центру на противоположной. Мощность излучения (Tx Power) – 12 dBm, что должно покрывать около 15 метров (согласно результатам из отчета компании Aislelabs). Формулы для расчета преобразования мощности сигнала в расстояние

Рис. 2 Формулы для расчета преобразования мощности сигнала в расстояние

Итак, мощность сигнала преобразуется в расстояние по формуле (1).

За d0 удобнее взять расстояние в 1 метр, тогда RSSI(d0) – это индикатор силы сигнала на расстоянии 1 метра от маячка. Это значение зачастую вписывается в сообщение, транслируемое самим маячком.

n — это path loss exponent, которая обычно берется равной от 2 до 4. Эту величину лучше рассчитывать по факту. Для этого выражаем ее из первой формулы и получаем формулу (2). Мы измеряли n на расстоянии 1, 2, 3, …, 7 метров и брали среднее (в нашем случае, от 1.5 до 2.3).

Определившись со всеми неизвестными, получаем итоговую формулу (3).

Полученные расстояния и координаты нахождения маячков подставляем в алгоритм трилатерации:

public static PointF trilaterate(PointF a, PointF b, PointF c, float distA, float distB, float distC) {
  float P1[] = { a.x, a.y, 0 };
  float P2[] = { b.x, b.y, 0 };
  float P3[] = { c.x, c.y, 0 };
  // ex = (P2 - P1)/(numpy.linalg.norm(P2 - P1))
  float ex[] = { 0, 0, 0 };
  float P2P1 = 0;
  for (int i = 0; i < 3; i++) {
      P2P1 += Math.pow(P2[i] - P1[i], 2);
  }
  for (int i = 0; i < 3; i++) {
      ex[i] = (float) ((P2[i] - P1[i]) / Math.sqrt(P2P1));
  }
  // i = dot(ex, P3 - P1)
  float p3p1[] = { 0, 0, 0 };
  for (int i = 0; i < 3; i++) {
      p3p1[i] = P3[i] - P1[i];
  }
  float ivar = 0;
  for (int i = 0; i < 3; i++) {
      ivar += (ex[i] * p3p1[i]);
  }
  // ey = (P3 - P1 - i*ex)/(numpy.linalg.norm(P3 - P1 - i*ex))
  float p3p1i = 0;
  for (int  i = 0; i < 3; i++) {
      p3p1i += Math.pow(P3[i] - P1[i] - ex[i] * ivar, 2);
  }
  float ey[] = { 0, 0, 0};
  for (int i = 0; i < 3; i++) {
      ey[i] = (float) ((P3[i] - P1[i] - ex[i] * ivar) / Math.sqrt(p3p1i));
  }
  // ez = numpy.cross(ex,ey)
  // if 2-dimensional vector then ez = 0
  float ez[] = { 0, 0, 0 };
  // d = numpy.linalg.norm(P2 - P1)
  float d = (float) Math.sqrt(P2P1);
  // j = dot(ey, P3 - P1)
  float jvar = 0;
  for (int i = 0; i < 3; i++) {
      jvar += (ey[i] * p3p1[i]);
  }
  // from wikipedia
  // plug and chug using above values
  float x = (float) ((Math.pow(distA, 2) - Math.pow(distB, 2) + Math.pow(d, 2)) / (2 * d));
  float y = (float) (((Math.pow(distA, 2) - Math.pow(distC, 2) + Math.pow(ivar, 2)
          + Math.pow(jvar, 2)) / (2 * jvar)) - ((ivar / jvar) * x));
  // only one case shown here
  float z = (float) Math.sqrt(Math.pow(distA, 2) - Math.pow(x, 2) - Math.pow(y, 2));
  if (Float.isNaN(z)) z = 0;
  // triPt is an array with ECEF x,y,z of trilateration point
  // triPt = P1 + x*ex + y*ey + z*ez
  float triPt[] = { 0, 0, 0 };
  for (int i = 0; i < 3; i++) {
      triPt[i] =  P1[i] + ex[i] * x + ey[i] * y + ez[i] * z;
  }
  // convert back to lat/long from ECEF
  // convert to degrees
  float lon = triPt[0];
  float lat = triPt[1];
  return new PointF(lon, lat);
}

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

public class KalmanFilter {
  private float R = 1.0f;
  private float Q = 1.0f;
  private float A = 1.0f;
  private float B = 0.0f;
  private float C = 1.0f;
  private float cov = Float.NaN;
  private float x = Float.NaN;
  public KalmanFilter(float r, float q) {
      R = r;
      Q = q;
  }
  public float filter(float z) {
      return filter(z, 0.0f);
  }
  public float filter(float z, float u) {
      if (Float.isNaN(this.x)) {
          this.x = (1 / this.C) * z;
          this.cov = (1 / this.C) * this.Q * (1 / this.C);
      } else {
          // Compute prediction
          float predX = (this.A * this.x) + (this.B * u);
          float predCov = ((this.A * this.cov) * this.A) + this.R;
          // Kalman gain
          float K = predCov * this.C * (1 / ((this.C * predCov * this.C) + this.Q));
          // Correction
          this.x = predX + K * (z - (this.C * predX));
          this.cov = predCov - (K * this.C * predCov);
      }
      return this.x;
  }
  public float lastMeasurement() {
      return this.x;
  }
  public void setMeasurementNoise(float noise) {
      this.Q = noise;
  }
  public void setProcessNoise(float noise) {
      this.R = noise;
  }
}

Вышеприведенный класс используется следующим образом:

KalmanFilter kalmanFilter = new KalmanFilter(0.01f, 3.0f);
float filteredRssi = kalmanFilter.filter(rssi);

Первый параметр — это шум процесса, второй — это шум измерений. Исходим из того, что погрешность маячка при трансляции сигнала минимальна, т.е. шум процесса низкий, а основной шум вызван погрешностями измерений. Чем сильнее фильтруется сигнал, тем сильнее падает отзывчивость к изменениям сигнала. Т.е. если резко уйти в другую часть комнаты, фильтрованный сигнал еще какое-то время будет отражать предыдущую позицию. Меняя степень фильтрации можно подобрать некоторый баланс между уровнем шума и отзывчивостью. Но даже с фильтрованным сигналом говорить о высокой точности не приходится. В лучшем случае, можно понять в какой половине комнаты находится устройство, спустя некоторое время после того как оно там появилось.

Используя формулы, ссылки на описания алгоритмов и их примеры реализации в коде, описанные выше, вы сможете создать демо-версию навигационного приложения. Применяя более сложные алгоритмы и дополнительные датчики, или комбинируя этот метод с другими, можно попробовать повысить уровень точности. Интересных вам экспериментов!