Навигатор 2ГИС: Экстраполяция позиции автомобиля
В приложении 2ГИС теперь есть навигатор. Мы научились «ехать» по треку, озвучивать манёвры, автоматически перестраивать маршрут, рассчитывать время в пути, доводить пользователя до входа в здание или организацию, учитывая заборы и шлагбаумы, — и всё это в честном офлайне. Пробки (вот разве что для них нужен интернет), разведённые мосты и перекрытые улицы учитываем давно. Пока в нашем навигаторе — необходимый минимум. Чуть позже научим его предупреждать о слишком высокой скорости, лежачих полицейских и камерах ГИБДД, настроим ночной режим, сделаем маршруты по платным и грунтовым дорогам опциональными. Чтобы воспользоваться им, нужно обновить 2ГИС в своем смартфоне или скачать в AppStore или Windows Store. Для Android обновление выходит постепенно, начиная с 22 августа (будет доступно на всю аудиторию к сентябрю).
А сегодня расскажем, как навигатор 2ГИС предугадывает положение автомобиля и плавно перемещает стрелочку по маршруту. Ведь именно качество ведения пользователя по маршруту определяет эргономику интерфейса любого современного навигатора, простоту ориентирования на местности и своевременность совершения манёвров.
Большую часть времени водитель автомобиля вынужден следить за дорогой, поэтому даже беглого взгляда на экран устройства с программой-навигатором должно быть достаточно, чтобы получить максимально точную и своевременную информацию о собственном местоположении относительно маршрута и окружающих объектов. Эта с виду простая функциональность требует решения множества технических проблем для своей реализации. Некоторые из них мы и рассмотрим.
Маркер GPS и маршрут
Чтобы обозначить местоположение пользователя на карте, многие навигаторы (и наш не стал исключением) используют специальный маркер GPS в виде наконечника стрелы или просто треугольника, который интуитивно понятным образом указывает направление движения. Кроме того, маркер должен быть хорошо заметен на карте, поэтому его цвет обычно сильно отличается от фона, края дополнительно обведены и т.д.
В самом простом случае можно отображать позицию устройства на местности, считывая координаты с датчика GPS и размещая маркер в соответствующее место на карте. Уже здесь мы сталкиваемся с первой проблемой — измерительной погрешностью, которая даже в условиях неплохого сигнала вполне может достигать 20–30 метров.
Для ответа на обычный вопрос «Где я нахожусь?» такого способа отображения будет вполне достаточно, особенно если вокруг маркера нарисовать ещё и круг точности с радиусом, равным оценке погрешности. Однако для навигации нужно придумать что-то получше, ведь водителя, движущегося по городской улице, вряд ли устроит маркер GPS, расположенный внутри соседнего дома или, того хуже, на каком-нибудь внутриквартальном проезде.
Решить проблему помогает маршрут, построенный программой до точки назначения и всегда присутствующий в сценарии навигации. При помощи некоторых ухищрений мы можем «притянуть» точку на карте к маршруту, нивелируя некоторую часть измерительной погрешности датчика GPS. В первом приближении притяжку можно рассматривать как проецирование точки на линию маршрута. Рассмотрение же нюансов, а также способов обнаружения схода с маршрута, к сожалению, выходит за рамки данной статьи.
Взяв на вооружение обозначенный приём притяжки, мы можем абстрагироваться от двумерных географических координат (широты-долготы или любых других) и перейти к одномерной координате — смещению относительно начала маршрута, измеряемому, например, в метрах. Такой переход упрощает как теоретические модели, так и вычисления, выполняемые на устройствах пользователей.
Отображение геопозиции во времени
Дискретный характер поступления данных от датчика GPS — ещё одна проблема при реализации ведения пользователя по маршруту. В идеальном случае координаты обновляются один раз в секунду. Рассмотрим несколько вариантов отображения геопозиции во времени и выберем наиболее подходящий для наших задач.
1. Самый простой способ заключается в том, чтобы при получении каждого нового отсчёта от датчика тут же выполнять притяжку к маршруту и отображать соответствующее местоположение на карте. Среди достоинств стоит отметить исключительную лёгкость реализации, высокую в некотором смысле точность (ведь здесь мы просто отображаем спутниковые данные, не внося в них каких-либо серьёзных изменений) и минимальную вычислительную трудоёмкость. Главный недостаток в том, что маркер в этом случае не движется по карте в привычном понимании, а «телепортируется» из точки в точку. В основном сценарии навигации камера (виртуальный наблюдатель — термин из области компьютерной графики) привязана к маркеру GPS, поэтому подобные его телепортации приводят к резкому «проматыванию» карты вдоль маршрута и, как следствие, к дезориентации водителя, особенно на высоких скоростях, когда за время между отсчётами геопозиции автомобиль преодолевает значительное расстояние. Наша задача — помочь пользователю, а не сбить его с толку, поэтому указанного изъяна уже достаточно, чтобы исключить данный вариант из рассмотрения.
Единственная возможность избежать дезориентации состоит в том, чтобы перемещать маркер GPS плавно, без «телепортаций», а значит, двигать его нужно существенно чаще, чем приходят отсчёты геопозиции. Чтобы обеспечить такое движение, требуется каким-либо образом вычислять промежуточные точки между реальными отсчётами с датчика и использовать их, пока не будет получен очередной отсчёт. Конкретному подходу к вычислению этих промежуточных точек стоит уделить особое внимание, так как он в конечном итоге сильно повлияет на общую эргономику программы-навигатора.
2. Второй способ отображать местоположение пользователя связан с самым очевидным подходом к генерации промежуточных точек — интерполяции между последними реальными отсчётами GPS. Смысл в том, чтобы двигать маркер от предпоследнего отсчёта к последнему в течение некоторого заданного времени, вычисляя промежуточные точки с требуемой частотой по одной из известных математических функций (простейший вариант — линейная интерполяция). Пользоваться навигатором при таком способе значительно удобнее, но недостатки у него тоже есть.
Один из самых безобидных — необходимость заранее задавать время интерполяции. Установка его в одну секунду будет хорошо работать только в упомянутом выше идеальном случае, когда именно столько времени будет проходить между отсчётами GPS. Если времени пройдёт меньше — не беда, можно просто начать двигаться из текущей позиции в новую целевую. А вот если больше — придётся маркеру стоять на месте и ждать новых координат от датчика, хотя автомобиль пользователя вполне может в это время двигаться.
Есть и более серьёзная проблема. В момент поступления нового отсчёта маркер в лучшем случае находится в предыдущей реальной точке. С точки зрения пользователя мы вносим ещё одну погрешность позиционирования, величина которой не меньше, чем расстояние, преодолеваемое автомобилем за время между отсчётами. При скорости в 100 км/ч это значение достигает почти 28 метров, что вкупе с возможной измерительной погрешностью делает информацию, выдаваемую пользователю, мягко говоря недостоверной.
Мы могли бы сделать маркер GPS огромных размеров и загородить им четверть экрана, тщательно маскируя недочёты описываемого способа позиционирования, но идти на прямой подлог было бы неуважением к пользователям и к самим себе. Точность и своевременность отображаемых данных — ничуть не менее важный критерий при разработке навигатора, чем внешняя красота и плавность движения.
3. С учётом появившегося требования к точности позиционирования стоит заметить, что теперь от нас требуется незадолго до прихода нового отсчёта GPS расположить маркер в точке, максимально приближенной к этому новому отсчёту. То есть, по сути, заглянуть будущее, пусть и ненадолго. Хотя с изобретением машины времени у человечества пока дела обстоят из рук вон плохо, для нас спасение всё же есть. Движение автомобиля инертно, поэтому скорость и направление его движения не могут меняться мгновенно, а раз так, мы можем попытаться с некоторой точностью спрогнозировать, где пользователь будет находиться в интервале между последним отсчётом позиции и будущим. Если нам удастся добиться того, что ошибка прогнозирования в большинстве случаев будет меньше, чем погрешность второго способа, то мы здорово облегчим жизнь пользователей нашего навигатора.
Такого рода прогнозирование в точных науках называется экстраполяцией. Именно этим путём мы пойдём в попытке разработать третий способ ведения по маршруту, удовлетворяющий всем перечисленным выше критериям. Далее нам придётся прибегнуть к более формальному языку изложения, коль скоро речь пойдёт о математических моделях.
Ведение по маршруту с экстраполяцией позиции
Ранее упоминалось, что благодаря притяжке геопозиции пользователя к маршруту навигации мы можем перейти от двумерных географических координат к одномерной координате — смещению относительно начала маршрута (для краткости дальше будем использовать термин «смещение» без уточнений).
Вспомним поступающие к нам данные и введём для них обозначения:
— реальные отсчёты смещения, получаемые притяжкой позиции GPS к линии маршрута;
— время прихода соответствующих отсчётов смещения.
На этом, собственно, список входных данных и заканчивается. Придётся выжимать из них максимум полезной информации.
В конечном итоге нам необходимо построить функцию экстраполяции смещения , которая будет приближена к реальной динамике автомобиля и при этом обеспечит плавность движения маркера GPS по всему нашему маршруту (его длина ни на что не повлияет, так как завершение маршрута обрабатывается отдельно, поэтому условно будем считать маршрут бесконечным). Для обеспечения хорошей визуальной плавности достаточно будет условия гладкости , то есть ни позиция, ни скорость маркера не должны меняться скачком. Другими словами, функция обязана быть непрерывной вместе со своей первой производной (здесь и далее — по времени) на всей области определения.
Обратим внимание, что каждый реальный отсчёт смещения несёт существенно новую информацию о движении. Например, если в течение длительного времени автомобиль ехал равномерно, а затем стал ускоряться, то «почувствовать» ускорение навигатор сможет только с приходом очередного отсчёта. Так как заглянуть в будущее на сколь угодно длительный срок мы не можем, все поступающие новые отсчёты GPS будут в общем случае изменять поведение искомой функции , что не позволяет задать её одним аналитическим выражением. Вместо этого попытаемся определить функцию кусочно. Для этого решим сперва более простую задачу.
Непосредственная кусочная экстраполяция
Построим такую функцию экстраполяции смещения , чтобы после -го отсчёта её значения предсказывали реальное расположение пользователя в течение достаточного времени до прихода -го отсчёта. Все полезные данные, которыми мы обладаем, — последовательность отсчётов до -го включительно вместе со временем получения каждого из них.
Вспомнив про конечные разности, отметим, что у нас есть возможность оценить скорость движения автомобиля в -й момент времени, разделив длину отрезка между последним и предпоследним смещением на соответствующий временной интервал:
, где — оценка скорости по отсчётам, а — производная экстраполяционной функции , которую мы пытаемся построить.
Аналогично для производных более высокого порядка — ускорения, рывка и т.д.:
Как видно из этих формул, для получения оценки всё более старших производных смещения требуется учитывать всё больше отсчётов, предшествующих текущему: для определения скорости нужны два отсчёта, для ускорения — три, для рывка — четыре и т.д. С одной стороны, чем больше динамических характеристик движения мы будем учитывать в своём прогнозе, тем большую моделирующую способность получим; с другой — полезная информация, содержащаяся во всё более «старых» отсчётах, драматически теряет актуальность. Например, тот факт, что мы ехали со скоростью 30 км/ч минуту назад ничем не поможет нам в текущий момент времени: с тех пор можно было несколько раз разогнаться, затормозить или вообще остановиться. По этой причине оценки всё более старших производных смещения становятся всё дальше от реальности; кроме того, вклад погрешности вычисления некоторой производной в общую аналитическую модель смещения тоже растёт с увеличением порядка этой производной. Раз так, то, начиная с какого-то порядка, динамические характеристики, оценённые при помощи конечных разностей, вместо уточнения будут только портить нашу модель.
По результатам проверок на реальных данных выяснилось, что оценка рывка , особенно в случаях «среднего» качества сигнала GPS, уже достаточно плоха, чтобы от неё было больше вреда, чем пользы. С другой стороны, к счастью, наиболее частые сценарии динамики автомобиля — это покой, равномерное и равнопеременное движение, описываемые полиномиальными уравнениями 0-й, 1-й и 2-й степени от времени соответственно.
Получается, квадратичной модели равнопеременного движения нам будет вполне достаточно для описания большинства дорожных ситуаций, и для неё у нас как раз хватает более-менее качественных оценок динамических характеристик — скорости и ускорения. Вспомнив школьный курс физики, мы уже можем вчерне составить аналитическое выражение для искомой экстраполяционной функции:
Осталось сделать всего один шаг: область определения начинается с момента времени , поэтому время в вычислениях удобнее считать с этого же момента.
В итоге функция примет вид:
Замечательной особенностью этой функции является её гладкость на всей области определения, что, как упоминалось ранее, входит в постановку нашей задачи.
Теперь возьмём несколько реальных отсчётов смещения с устройства и попробуем экстраполировать их на каждом интервале (хотя определена до , в момент прихода отсчёта будем сразу переходить к следующей функции , ведь она располагает более свежими данными):
Оговоримся, что для наглядности данные были сняты при сравнительно низком качестве сигнала GPS, однако ситуация на рисунке вполне реальна и может возникнуть у любого пользователя.
Гладкость каждого экстраполяционного полинома прекрасно видна на соответствующем временном интервале, но вот беда — на стыках интервалов общая серая кривая терпит разрывы, подчас весьма заметные.
Назовём величину разрыва в -й момент времени ошибкой экстраполяции . Действительно, именно это значение показывает, какую неточность имеет каждый наш прогноз к концу его временного интервала. Вычислить значение ошибки можно при помощи следующего выражения:
Увы, свести ошибку к нулю, варьируя сами функции , мы никак не можем, ведь это было бы эквивалентно стопроцентной точности видения будущего. Значит, для решения нашей исходной задачи построения единой функции придётся каким-то образом «склеить» между собой кусочные экстраполяционные полиномы, то есть скорректировать возникающие на стыках ошибки.
Подход к коррекции ошибок
В соответствии с выбранными выше обозначениями можно неформально сказать, что к моменту прихода нового отсчёта мы находимся в точке , т.е. сдвинуты относительно реальной позиции на величину погрешности , накопленной к моменту предыдущим экстраполяционным полиномом .
С одной стороны, с точки зрения соответствия выдаваемых пользователю данных реальности, наилучшим способом коррекции ошибки будет разрыв функции в точку начала следующего полинома , однако мы не можем так поступить, ведь в этом случае снова станем «телепортировать» маркер по карте и дезориентировать водителя.
Очевидно, что если мгновенное изменение значения недопустимо, коррекция ошибки будет занимать некоторое ненулевое время. Также понятно, что коррекцию ошибки желательно завершить до прихода следующего отсчёта, дабы не допустить накопления ошибки.
В силу стохастической природы временных интервалов между отсчётами смещения достоверно определить точное время коррекции не представляется возможным. Поэтому в первом приближении зафиксируем время коррекции ошибки в виде некоторой постоянной величины, конкретное значение которой подберём в будущем опытным путём.
Если снова говорить неформальным языком, для коррекции ошибки требуется из точки за время плавно «вернуться» на следующий экстраполяционный полином — кривую .
Для описания процесса коррекции ошибки удобно ввести отдельные функции коррекции таким образом, чтобы в момент времени соответствующая функция коррекции принимала значение , а начиная с момента становилась равна нулю:
Если сложить такую функцию коррекции с соответствующим интерполяционным полиномом, то в ключевых точках и мы обеспечим коррекцию ошибки смещения:
Назовём скорректированной функцией смещения сумму экстраполяционного полинома и соответствующей функции коррекции:
Заметим, что благодаря описанным выше свойствам функций коррекции мы получили очень важное свойство функций — они уже «сшиты по смещению», т.е. не терпят разрывов в точках :
Совокупность скорректированных функций могла бы претендовать на роль искомой модели смещения , определённой во все моменты времени, если бы не одно обстоятельство: несмотря на отсутствие разрывов смещения в точках , производные этой совокупности функций в общем случае всё ещё рвутся.
Конкретно нас интересует разрыв первой производной — скорости, потому что исходные требования содержат условие повсеместной гладкости , т.е. условие повсеместной непрерывности скорости. С учётом этого необходимо расширить требования к функциям коррекции , чтобы «сшить» ещё и производные скорректированных функций :
Это уравнение является условием гладкости совокупности скорректированных функций. Подставив в обе части уравнения определение скорректированных функций, получим
Ранее мы упоминали о том, что после истечения времени коррекции функция коррекции принимает нулевые значения. Добавим ещё одно требование к функции коррекции — пусть её производная после истечения времени коррекции также принимает нулевые значения:
Тогда, в предположении, что время коррекции всегда меньше интервала между отсчётами, можно считать, что производная -й функции коррекции к моменту прихода следующего отсчёта уже обратится в ноль. Тогда, возвращаясь к условию гладкости, получим:
Выразим отсюда :
Заметим, что представляет собой оценку скорости, сделанную при помощи конечных разностей, подставим её:
Правая часть представляет собой ошибку экстраполяции скорости — разность между скоростью, полученной по предыдущему экстраполяционному полиному, и «реальным» отсчётом скорости. Теперь мы можем собрать воедино граничные условия для функций коррекции:
Словами их можно описать так — необходимо найти функцию коррекции, чтобы:
- в начале интервала коррекции её значение совпадало с ошибкой экстраполяции смещения;
- в начале интервала коррекции значение её производной совпадало с ошибкой экстраполяции скорости;
- в конце интервала коррекции и далее значение самой функции и её производной было нулевым.
Выбор функции коррекции ошибок
Стоит отметить, что получить единое аналитическое выражение для функций коррекции , в точности удовлетворяющее вышеуказанным четырём условиям, очень сложно. Проблема заключается в той части области определения, которая идёт после истечения времени коррекции , — нужно добиться нулевых значений функции и её производной на всём остатке числовой оси. Для упрощения задачи сократим область определения искомого аналитического выражения функции коррекции до интервала коррекции , а после верхней его границы будем считать значение функции и её производной тривиально нулевым (благо, на уровне программного кода у нас есть такая возможность из-за наличия ветвлений).
Формально с учётом этого приёма функция коррекции кусочной — некоторое выражение для интервала коррекции и константа 0 далее, однако при соблюдении граничных условий в точке не будет разрыва ни самой функции коррекции, ни её первой производной. Так как разрывы более старших производных нас не интересуют (они не испортят гладкости искомой функции ), в дальнейшем не будем упоминать о нулевом «хвосте» функции коррекции, а граничные условия переформулируем в более удобном виде:
Обозначим ошибку экстраполяции скорости через :
Теперь нужно определить аналитическое выражение для . В связи с эргономическими требованиями к программе, помимо граничных условий, нужно, чтобы функция коррекции имела как можно меньше экстремумов и перегибов на интервале коррекции — чтобы маркер GPS «не дёргался».
Самой простой функцией, подходящей под эти требования, опять является полином — полином минимально возможной степени от времени (теоретически, сходными характеристиками среди элементарных функций обладает ещё и, например, синус, но вычислять его значение накладнее с точки зрения процессорного времени).
Так как граничные условия представляют собой систему из четырёх нетривиальных уравнений, то минимальной степенью полинома, обеспечивающей достаточную параметризацию функции коррекции, является третья. Учитывая то, что при построении аналитического выражения для удобнее считать время с момента -го отсчёта (ровно так же, как и в определении , нужный полином примет следующий вид:
Подставив это выражение в систему граничных условий и решив её относительно констант и , получим следующие значения:
В итоге, если определить функции коррекции описанным образом, то скорректированные функции сливаются в единую функцию экстраполяции , гладкую во все моменты времени. Полное выражение для приводить не будем в силу его громоздкости.
Примечание: последняя неточность осталась в допущении при выборе времени коррекции — наши рассуждения строились в свете условия, что всегда будет меньше интервала между отсчётами:
Приятной особенностью построенной модели является то, что нам достаточно выбрать таким образом, чтобы оно не превышало среднего времени между отсчётами: в случае, если отдельные интервалы будут меньше , то часть ошибки, которую мы не успели докорректировать на слишком коротком интервале, будет скорректирована на одном из следующих. Для этого достаточно будет вычислять ошибку экстраполяции не по обычной экстраполяционной функции , а по скорректированной :
На рисунке ниже изображён пример графика итоговой функции экстраполяции , построенный по реальным данным:
Формальная задача решена, полученная кривая удовлетворяет всем оговоренным условиям, да и выглядит вполне симпатично. Можно было бы на этом и расслабиться, но особенности реального мира представляют определённые трудности для построенной идеализированной системы.
Рассмотрим некоторые из них более подробно, оговорившись, что все принятые далее решения реализуются непосредственно в программном коде за пределами математической модели.
Адаптация математической модели к реальным условиям
Запрет движения маркера в обратном направлении
На последнем графике можно заметить, что в некоторых случаях функция начинает убывать, даже когда по реальным отсчётам пользователь едет исключительно вперёд по маршруту. Такое происходит, когда наш прогноз сильно переоценивает скорость движения. С другой стороны, в реальности автомобиль двигается в обратном направлении только по двум причинам: водитель действительно включил заднюю передачу и отправился назад (очень редкий случай), либо выполнил разворот.
В случае разворота дорожная ситуация существенно меняется, что требует перестроения навигационного маршрута; это представляет собой отдельную тему и никак не укладывается в рамки данной статьи.
Если мы воспользуемся результатами экстраполяции позиции по непосредственно, то из всех движений маркера в сторону начала маршрута соответствовать реальному движению автомобиля в том же направлении будет исчезающее меньшинство. В свете этого было принято решение вообще запретить маркеру двигаться назад без перестроения маршрута, дабы не вводить пользователей в заблуждение.
Такое жёсткое условие трудно описать на языке математики, но в программном коде реализовать сравнительно легко. Для начала учтём дискретный характер модельного времени — в силу особенностей функционирования вычислительных машин получать результаты экстраполяции мы в любом случае будем в некоторые выделенные моменты времени.
Раз так, то обеспечить неубывание экстраполированного смещения будет нетрудно: достаточно сравнить новое полученное значение с предыдущим, и если текущее окажется меньше, то подменить его предыдущим. Несмотря на кажущуюся грубость этого приёма, гладкости функции экстраполяции мы не нарушим, ведь, для того чтобы начать двигаться назад по гладкой функции, нужно сначала полностью остановиться.
В будущем режим работы, когда мы подменяем математически корректные значения более старыми, чтобы не допустить движения назад, будем называть режимом принудительной остановки.
Слишком большие ошибки экстраполяции и слишком долгие интервалы между отсчётами
Несмотря на то, что мы построили в некотором смысле качественную функцию , иногда ошибки экстраполяции могут достичь недопустимых величин. В этих случаях программа должна прекратить попытки скорректировать ошибки штатными средствами. Ещё одна ситуация, когда экстраполированные данные теряют актуальность, возникает, если новый отсчёт смещения по какой-либо причине не приходит слишком долго — моделирующая способность драматически падает с момента получения последнего отсчёта. Чтобы не перейти грань между попытками прогнозирования и бессовестной ложью, полагаться на модель обычно стоит не дольше трёх секунд.
Для простоты назовём первую негативную ситуацию некорректируемой ошибкой смещения, а вторую — некорректируемой ошибкой времени.
Работать с каждым из этих видов ошибок мы можем двумя способами:
- Входить в упомянутый выше режим принудительной остановки. Достоинство этого подхода — в сохранении плавности движения маркера геопозиции по карте местности. Однако чем дольше мы находимся в режиме принудительной остановки, тем хуже мы информируем пользователя о его реальном местоположении;
- Мгновенно телепортировать маркер GPS на место последнего отсчёта. Здесь мы, наоборот, жертвуем эргономикой ради достоверности информации, подаваемой пользователю.
Для нашего приложения был избран первый способ, так как плавности движения уделяется особо пристальное внимание.
Затянувшийся режим принудительной остановки
Любой вход в режим принудительной остановки сопряжён с выдачей менее точных данных о местоположении в угоду запрету обратного движения маркера GPS. Чтобы не дезинформировать пользователя в особо неблагоприятных случаях, наша модель дополнительно наделена возможностью прерывать режим принудительной остановки «телепортацией» маркера на последнее реальное положение по истечении заданного промежутка времени, вне зависимости от причины входа в режим (математический результат экстраполяции или некорректируемые ошибки смещения/времени). В этот момент даже плавность движений приходится принести в жертву ради «остатков» точности.
Выводы
В результате проделанной работы нам удалось улучшить ведение по маршруту так, чтобы обеспечить неплохой баланс между точностью выдаваемых данных и визуальной эргономикой их отображения. Пользователь будет чувствовать себя достаточно комфортно, особенно когда с датчика GPS благодаря хорошему сигналу поступают качественные данные.
Описанную систему экстраполяции можно применять в других приложениях, использующих геопозиционир