Сравнение различных численных методов для решения задачи ультразвукового позиционирования подвижного робота в закрытом пространстве

Ключевые слова: ультразвуковое позиционирование, позиционирование мобильного робота, численный метод, простейший градиентный метод, метод Левенберга-Марквардта.

Способность определять свое местоположение является самым основным требованием для мобильных устройств и роботов, так как она является основой планирования и контроля траектории в режиме реального времени [1, 2, 3].Одной из задач позиционирования в закрытом помещении является определение координат мобильного робота в рабочем пространстве. Методы и технологии использующиеся в глобальном позиционировании на открытом пространстве (GPS/GLONASS, GSM) не подходят для небольшого закрытого пространства так как, качество связи внутри помещения не всегда является стабильным. Другие технологии (Wi-Fi, Bluetooth) также основаны на применении радиоволн и, следовательно, требуют использования высокоточных синхронизированных часов. Ультразвуковой метод не требует наличия высокоточных часов, обладает при этом достаточно малой погрешностью (до 0,3 см), не является источником электромагнитных помех, и относительно дешевый в эксплуатации.

Ультразвуковая система позиционирования представляет собой ряд датчиков приемников расположенных по периметру помещения, с известными координатами и датчик излучатель, расположенный на мобильном роботе, чьи координаты требуется вычислить [4]. Минимальное количество датчиков приемников для такой системы равно трём, однако, большее количество приемников увеличивают точность решения. В прямоугольной зоне целесообразно использовать четыре датчика.

Существует два подхода к решению задачи позиционирования: измерение времени приходов сигнала от излучателя к каждому из приемников (синхронный метод), измерение разностей времен прихода сигнала к приемникам (асинхронный метод). Более предпочтительным является асинхронный метод, так как для его реализации не нужны синхронизированные для двух подсистем часы [5].

Таким образом, постановка задачи сводится к следующему. Мобильный робот имеет неизвестные координаты робота R = (X_R,Y_R). Четыре приемника (M1, M2, M3, M4) имеют известные координаты (X_M1,Y_M1), (X_M2,Y_M2), (X_M3,Y_M3), (X_M4,Y_M4). Четыре расстояния d1,d2,d3,d4 от R до M1,M2,M3,M4 неизвестны, однако разности между ними m2=d2-d1, m3=d3-d2, m4=d4-d3 измеряются ультразвуковым методом и являются наблюдениями. Нужно найти координаты R = (X_R,Y_R) путем решения системы уравнений (1), которая содержит три измерения m2,m3,m4 и n=3 неизвестных параметра (X_R,Y_R,d1).

Расстояние от каждого приемника до робота можно описать системой нелинейных уравнений:

(1)

В такой постановке задача решалась в источнике [5]. Авторы применили подход, заключающийся в линеаризации исходной модели и применении к ней метода наименьших квадратов [6]. В данной статье ставится задача оценки возможности применения численных методов (простого градиентного и метода Левенберга-Марквардта) к решению поставленной задачи.

Запишем исходную систему уравнений (1) в виде набора минимизируемых функции:

Тогда вектор R невязок решения будет:

Решается задача нахождения таких значений неизвестных параметров x,y,d1 при которых компоненты вектора R стремятся к нулю. Для решения данной задачи проведено сравнение двух методов: простой градиентный метод и метод Левенберга-Марквардта.

Простой градиентный метод является итерационным. Вычисление параметра на очередном шаге выполняется путем вычитания градиента функции, умноженного на заданный положительный коэффициент[7]:

Градиент функции можно представить как произведение транспонированной матрицы первых производных системы (матрицы Якоби) и самого вектора R невязок [7]. Тогда формула примет вид:

(2)

Составляем матрицу Якоби:

Алгоритм поиска неизвестных параметров x, y, d1 c помощью простого градиентного метода (2) реализован в программной среде MathCad. Ниже представлены графики изменения вектора R невязок функций при двух наборах исходных данных.

Рис. 1-а. Рис. 1-б.

Рис.1. Зависимость модуля суммарной невязки от числа шагов для градиентного метода

На рис. 1 а. показан результат расчета для объекта с координатами x=104.042, y=122.851 и значением d1=160.988; рассчитанные значения составили x=104.306, y=122.173, d1=160.716. Получившаяся погрешность соответственно х=0,25% , y=0.55%, d1=0.17%. Величина шага подобрана вручную и составляет л=0,000001.

На рис. 2. приведен результат расчета для объекта с координатами x=22.103, y=207.214 и значением d1=208.166; рассчитанные значения x=67.974, y=158.362, d1=152.479. Погрешность составила для х=209% , y=24%, d1=26.8%. Величина шага подобрана вручную и составляет л=0,000001.

Из графика на рис.1-а видно, что алгоритму потребовалось 200 шагов для того, чтобы невязка стала менее 1. Во втором случае (рис. 1-б) даже после 200 шагов невязка остается достаточно большой (более 10 000).

Недостатками простейшего градиентного метода являются: большое количество шагов алгоритма поиска минимума функции, значение коэффициента находится вручную. Также недостатком являются различные проблемы сходимости, при некоторых значениях л, так как шаги которые он делает в направлении поиска минимума функции являются одинаковыми по величине и определяются величиной . [8].

Метод Левенберга-Марквардта автоматически учитывает кривизну поверхности, изменяя величину шага. Он обладает лучшей сходимостью по сравнению с градиентным методом и подходит для реализации на ЭВМ. Правило вычисления параметра на очередном шаге по методу Левенберга-Марквардта [8]:

,

где H - матрица вторых частных производных, вычисленная в точке (матрица Гессе), л - заданный положительный коэффициент, I - единичная матрица 3x3.

Используя аппроксимацию Гессиана [9] формула примет вид:

(3)

Алгоритм использует правило автоматической коррекции параметра л в зависимости от успешности очередного шага. Данное правило используется следующим образом: если на очередной итерации невязка сокращается, это значит, что предположение о квадратичности f(x) работает, и мы уменьшаем л (обычно в 10 раз), чтобы понизить влияние градиентного спуска. С другой стороны, если невязка увеличивается, необходимо следовать направлению градиента, и мы увеличиваем л (во столько же раз) [10].

Алгоритм поиска неизвестных параметров x,y,d1 с помощью метода Левенберга-Марквардта реализован в программной среде MathCad. Ниже (рис. 2-а,б) представлены графики изменения модуля вектора невязок R в зависимости от числа шагов.

Исходные данные выбирались такими же как и в градиентном алгоритме. Параметр л задавался равным единице. В обоих случаях итоговая погрешность не превысила 1•10^-11.

Рис 2 а. Рис 2 б.

Рис.2. Зависимость модуля суммарной невязки от числа шагов для метода Левенберга-Марквардта

Как видно из графиков алгоритму потребовалось 5 шагов для нахождения минимума вектора R.

По результатам экспериментов, можно сделать вывод о преимуществе метода Левенберга - Марквардта над простейшим градиентным методом. Алгоритму Левенберга-Марквардта при различных наборах исходных данных требовалось не более 10 шагов для получения решения. Кроме того, он показал большую устойчивость по сравнению с градиентным методом. Он не требует точного начального выбора параметра л, так как автоматически корректирует его. Дальнейшим исследованием будет разработка реального макета системы, состоящей из ультразвуковых датчиков и мобильного робота. В качестве вычислительного устройства предполагается использовать 32-разрядный микроконтроллер.

Литература

1. Кульченко А. Е. Структурно-алгоритмическая организация автопилота робота-вертолета // Инженерный вестник Дона, 2011, №1 URL: ivdon.ru/ru/magazine/archive/n1y2011/330

2. Хусаинов Н. Ш., Кравченко П. П., Салов В. В. Об исследовании бортовой интегрированной системы управления движением летательного аппарата с коррекцией координат // Инженерный вестник Дона, 2013, №4 URL: ivdon.ru/ru/magazine/archive/n4y2013/2038

3. Duchoт, F. and L. Juriљica, 2011. Ultrasonic hybrid map for navigation of mobile robot. The 18th International Conference on Process Control, Slovak University of Technology in Bratislava, Institute of Information Engineering, Automation, and Mathematics, pp: 167-173.

4. Soo-Yeong, Y. and C. Byoung-Wook, 2007. Chapter 17: Autonomous Navigation of Indoor Mobile Robot Using Global Ultrasonic System. Mobile Robots: Perception & Navigation, Pro Literatur Verlag, ARS, pp: 383-394.

5. Filonenko, V., C. Cullen and J.D. Carswell, 2013. Indoor Positioning for Smartphones Using Asynchronous Ultrasound Trilateration. ISPRS International Journal of Geo-Information, 2. pp. 598-620.

6. Filonenko V., Cullen C., Carswell J. D. Asynchronous ultrasonic trilateration for indoor positioning of mobile phones //Web and Wireless Geographical Information Systems. - Springer Berlin Heidelberg, 2012. - pp. 33-46.

7. Математический синтез оптических наноструктур: учебное пособие / Ловецкий К.П., Севастьянов Л.А., Паукшто М. В., Бикеев О.Н., М.: РУДН, 2008. 25 с.

8. Ranganathan A. The Levenberg-Marquardt algorithm. Date Views 6.04.2016 URL: ananth.in/docs/lmtut.pdf.

9. Дэннис Дж. мл., Шнабель Р. Численные методы безусловной оптимизации и решения нелинейных уравнений. М.: Мир, 1988. 440 с.

10. Numerical Optimization / Nocedal J., Wright S.J., New York: springer, 1999. 631 p.

References