Как реверс-инжиниринг чужой инерциальной навигационной системы перерос в свою собственную разработку

Сегодня, благодаря MEMS-датчикам, инженеры начинают использовать инерциальные навигационные системы везде, где есть движение. В зависимости от требуемой точности как по углу, так и по координатам, применяют МЕМS-датчики разного уровня цены и интегрированности: от "все датчики в одной микросхеме" до "один датчик - одна микросхема". А сама инерциальная навигация, как часть инженерных систем, впервые появилась в торпедах, кораблях, ракетах и самолетах.

Вступление

Итак, как заметил Виктор Олегович Пелевин, различные беспилотные летательные аппараты действительно стали распространены во всем мире. И не только для военных целей, но и в быту. А что способствовало распространению их для гражданского применения?

- Прежде всего, снижение стоимости микропроцессоров и микроконтроллеров, а также рост их вычислительной мощности;

- Появление и распространение аккумуляторов с удельной энергоемкостью 150…250 Вт*ч/кг, литий-ионных и литий-полимерных;

- Широкое распространение вентильных (бесколлекторных) электродвигателей, скоростью вращения которых можно управлять в широком диапазоне — от нуля до нескольких тысяч оборотов в минуту — без применения механических редукторов;

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

- Широкое распространение инерциальных систем, построенных на микроэлектромеханических (МЭМС) сенсорах: датчиках угловой скорости (ДУС), акселерометрах, магнитометрах и датчиках давления. Про инерциальные навигационные системы мы и поговорим.

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

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

Главное свойство карданова подвеса состоит в том, что если в него закрепить вращающееся тело, то оно будет сохранять направление оси вращения независимо от ориентации самого подвеса. Таким образом, передвигающийся объект, на котором закреплен гироскоп, может постоянно наблюдать свое первоначальное угловое положение в пространстве.

Первые серийные навигационные приборы, основанные на гироскопах, появились в самом конце 19-го века на морских судах, подлодках и торпедах. На судах это были гирокомпасы, позволяющие определять направление на север вне зависимости от текущей широты, в отличие от магнитных компасов, которые в высоких широтах (близких к полюсам Земли) не работают из-за больших возмущений магнитного поля Земли.

В авиации навигация - еще более насущный вопрос, поэтому приборы на основе гироскопов стали применять и там. Например, на русском самолете «Илья Муромец» (1917 год) использовался гироскопический указатель поворотов, что позволяло сохранять направление при пилотировании «вслепую» в облаках. Первый известный автопилот с более масштабным использованием гироскопов стоял на ракете Фау-2, где применены гировертикаль (ракета с вертикальным стартом с земли), гирогоризонт и измеритель скоростей. С увеличением времени и дальности полета ракет и самолетов совершенствовались и системы навигации с непременным использованием гироскопов.

В настоящее время в задачу навигации входит: определение координат местоположения и других параметров поступательного движения объекта (скорости и ускорения) как материальной точки, и ориентация – определение углового положения и других параметров вращательного движения объекта вокруг его центра масс.

Любая ИНС имеет в своём составе датчики линейного кажущегося ускорения (акселерометры) и гироскопы (или ДУС). С помощью интегрирования их данных определяется отклонение связанной с корпусом объекта системы координат относительно системы координат, связанной с Землёй, выражаемое в углах ориентации: курсе, тангаже и крене, также определяются отклонения координат (широта, долгота и высота) относительно референц-эллипсоида Земли при условии задания исходных координат перед началом счисления. Получается, что алгоритмически ИНС состоит из курсовертикали и системы определения координат, но тем не менее эти подсистемы не независимы.

Изначально инерциальные навигационные системы были только платформенными (далее ПИНС). Что такое гиростабилизированная платформа? Для стабилизации датчиков в пространстве используется такая гироплатформа, которая физически реализует опорную систему координат на движущемся объекте. Гироплатформа имеет минимум 3 вращательные степени свободы относительно объекта, которые обеспечивают сохранение платформе неизменного углового положения в пространстве при угловых эволюциях. Гироплатформы бывают 2 типов:

- с тремя гироскопами, которые имеют 2 степени свободы каждый;

- с двумя гироскопами, которые имеют 3 степени свободы каждый.

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

В ПИНС геометрического типа координаты местоположения объекта определяются углами, которые образует физически реализованная ось местной вертикали с материализованными гироплатформой, осями и плоскостями навигационной системы координат. Эти углы измеряются соответствующими датчиками. Т.е. в наличии две платформы – одна материализует навигационную систему, другая горизонтированную систему координат.

В ПИНС аналитического типа гироплатформа материализует (при околоземной навигации) навигационную систему координат с обеспечением учета вращения Земли, местная вертикаль определяется аналитически. Координаты объекта получаются в вычислителе, в котором обрабатываются сигналы, снимаемые с акселерометров и устройств, определяющих поворот самого объекта относительно гироскопов и акселерометров.

Полуаналитическая система имеет платформу, которая непрерывно стабилизируется по местному горизонту, т.е. одна ось трехгранника платформы постоянно направлена вдоль местной вертикали. На платформе установлены гироскопы и акселерометры, сигналы которых и управляют платформой.

А вот в бесплатформенной ИНС акселерометры и гироскопы (или ДУС) жестко связаны с корпусом прибора. Гироплатформа отсутствует, что сразу снижает массу и объем такой ИНС. Все системы координат существуют только в вычислителе, без механической материализации.

Если раньше любая инерциальная навигационная система (ИНС) занимала не менее 10 литров и имела соответствующую массу, то сейчас набор датчиков для такой ИНС может заключаться в одной-единственной микросхеме (3 гироскопа, 3 акселерометра и может быть также встроен трехосевой магнитометр) размером с человеческий ноготь.

Такие инерциальные MEMS-модули очень привлекательны для применения в разных устройствах (не только в коптерах): цена не превышает 1000 руб. в рознице, интерфейс взаимодействия I2C или SPI, в микросхему встроен датчик температуры, можно на ходу менять максимальный измеряемый размах угловой скорости и кажущегося ускорения. Для более серьезных применений набор MEMS-датчиков с вычислителем заключены в модуль объемом 0,1..0,3 литра, где каждый гироскоп и акселерометр выполнен в отдельной микросхеме.

Однако по точности измерения угловой скорости инерциальные модули общего применения на основе MEMS-датчиков сильно проигрывают подавляющему большинству тех старых инерциальных систем. Наиболее важный показатель, влияющий на погрешность определения координат в БИНС (ошибка по координате, обусловленная им, растет быстрее, чем в третьей степени от времени) – это исходное смещение ноля и зависимость этого смещения от температуры для датчика угловых скоростей (ДУС).

В таблице приведены интересующие нас параметры для характерных современных устройств.

MPU-9250, BMI0556, ICM-20689 и другие подобные микросхемы фирм Bosch, Invensense или STMicroelectronics используются в современных полетных контроллерах для любительских беспилотных коптеров и самолетов. Это, например, контроллеры PixHawk, Naza и другие, чуть более высокого или низкого класса по функциональным и ценовым характеристикам. Все они применяются на беспилотниках класса «для хобби» или околопрофессионального – полетов для наблюдений с помощью камеры на небольшие расстояния и время. Работа полетных контроллеров с подобными ИНС без постоянного комплексирования с данными от приемника GPS\ГЛОНАСС в таких режимах «как зависание на точке», «возврат на точку старта» или «автоматический полет по координатным точкам» немыслима. Кроме этого, у них есть и другие особенности, которые иногда могут затруднять их использование, а иногда и приводить к авариям: ухудшение стабильности на температурах ниже –5…–10 ⁰C, влияние вибраций от работы винтов на отработку углов горизонта, нестабильность определения истинного курса, возникновение «toilet bowling» («унитазинг», самопроизвольные круговые движения коптера по возрастающему радиусу).

Однако все это недопустимо при применении в коммерческих целях, при массе БПЛА до 30 кг, и более. Кстати, 30 кг – это, по измененным законам РФ, максимальная масса беспилотного воздушного судна, которое можно эксплуатировать без согласования с диспетчерскими службами использования воздушного пространства, с некоторыми дополнительными ограничениями.

В общем, если говорить о промышленных или сельскохозяйственных дронах, таких как DJI Agras T20 или DJI серии Matrice c временем полета до 40-50 минут и стоимостью в районе одного миллиона рублей, то здесь требуются более надежные и точные ИНС. Тем более, что при таких массах остро стоит вопрос безаварийности не только в смысле сохранения дрона, но и, что куда важнее, в смысле безопасности окружающих людей. Да и потом, не следует забывать про развитие и распространение всяких приборов и устройств, подавляющих радиосигналы, используемые дроном для управления и навигации. А эти приборы впоследствии могут использовать не только госслужбы.

Когда в Миландре появилась необходимость использовать инерциальную навигационную систему в собственной разработке, то выбор такой системы проходил по принципу максимальной точности при достаточной миниатюрности. По сути, выбирался инерциальный модуль на основе MEMS c наилучшими характеристиками. Вопрос цены не стоял остро, так как предполагалось первичное применение в прототипе устройства. Выбор пал на модуль ADIS16480 от Аnalog Devices, на тот момент это был лучший вариант среди имеющихся в свободной продаже. Данный модуль является курсовертикалью, в состав которой входят: ДУСы, акселерометры, трехосевой магнитометр и датчик давления (бародатчик). Выходные данные – угловые скорости, кажущиеся ускорения, углы Эйлера относительно горизонта и истинный курс (магнитный курс с учетом заданного склонения), а также соответствующий этим углам кватернион (или, взамен углов и кватерниона – матрица ориентации). Для коррекции углов используется калмановская фильтрация с применением данных от акселерометров и магнитометра. Также в модуле присутствует возможность подвергать цифровой фильтрации «сырые» данные угловых скоростей и кажущихся ускорений.

Однако, начиная с некоторого момента, из-за введенных санкций правительства США данный модуль перестал быть доступным в продаже в России. И было принято решение разработать собственный аналог.

Реверс-инжиниринг ADIS16480

По результатам вскрытия

выявлен состав данного модуля и некоторые его схемотехнические особенности. Модуль выполнен в виде гибко-жесткой платы, на основе процессора Blackfin ADSP-BF512, также в нем есть 4 АЦП AD7689BCPZ, датчик давления MS560702BA03-00, магнитометр HMC1043, 6 двухосевых акселерометров AD22037Z и по несколько ДУС в каждой из 3 осей.

Наше устройство по набору датчиков похоже на устройство Analog Devices и выполнено на собственном цифровом сигнальном процессоре К1986ВН04BG. Однако функционально мы его расширили.

Прежде всего, это уже не только курсовертикаль, но целая бесплатформенная инерциальная навигационная система (БИНС). В режиме БИНС дополнительно к информации, выдаваемой в режиме курсовертикали, выдаются следующие данные:

- текущие географические координаты объекта (широта, долгота и высота над референц-эллипсоидом) в системе координат ПЗ-90.02;

- вектор скорости объекта относительно Земли в проекциях на северную, восточную и вертикальную оси.

Для того, чтобы модуль работал в режиме БИНС, необходимо при включении передать в него начальные координаты относительно Земли: широту, долготу и высоту над референц-эллипсоидом. С этого момента начинается интегрирование показаний датчиков и счисление координат, скоростей и углов ориентации. Кроме того, знание текущих координат позволяет включить в алгоритм курсовертикали учёт вращения Земли. Таким образом, в отличие от ADIS16480, наш инерциальный навигационный модуль может работать не только в режиме курсовертикали, но и как полноценная инерциальная навигационная система. Курсовертикаль обеспечивает счисление ориентации, точность которой непосредственно влияет на точность счисления координат – так, часто характеристикой точности инерциальных систем для самолётов является погрешность  в формате ухода координат «миль в час».

Также, в отличие от ADIS16480, в нашу БИНС добавлена возможность записи телеметрической информации на microSD-карту, которая оперативно снимается и устанавливается без вскрытия корпуса. Можно сказать, что в наш модуль введена функция «черного ящика».

Разработка алгоритмов и математики, а также математическое моделирование

Алгоритм ориентации основан на использовании кватернионов и матриц ориентации, с предварительным интегрированием «сырых» данных ДУС и акселерометров. В расчетах используются константы «Параметров Земли 1990 года (ПЗ-90.02)».

В алгоритмах фильтрации и комплексирования вместо N-мерного фильтра Калмана используется система алгоритмов субоптимальной фильтрации, основанных на линейных моделях ошибок датчиков ДУС, акселерометров, магнитометров и бародатчика: инклинометрический, бароинерциальный и гиромагнитный фильтры. Все фильтры собственной разработки, использующие известные математические модели, основанные на решении линейных дифференциальных уравнений.

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

Бароинерциальный фильтр – используется для коррекции текущей высоты и вертикальной скорости БИНС по данным бародатчика. Также основан на линейном дифференциальном уравнении.

Гиромагнитный фильтр в процессе работы оценивает ошибку счисления инерциального угла и величину дрейфа истинного курса, используя в качестве «объективного» значения этого угла величину, выработанную магнитным указателем курса.

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

В оригинальном приборе используется адаптивный расширенный фильтр Калмана. Этот фильтр оценивает угловую ориентацию, используя комбинацию информации ДУС, акселерометров и магнитометров. Акселерометры обеспечивают угловые измерения по отношению к гравитации Земли. Магнитометры обеспечивают угловые измерения, основываясь на измерениях магнитного поля Земли. В мануале от Analog Devices есть отдельные рекомендации для настройки точности в статике и отдельно для динамики, эти настройки задаются записью в память ADIS16480 данных по ковариации акселерометров и магнитометра, а также уровня шума и дрейфа ДУС. В общем случае эти настройки требуют дополнительного анализа модели движения объекта, на котором устанавливается курсовертикаль, а также специального испытательного оборудования. При использовании ADIS16480 c заводскими настройками при совместных испытаниях с нашим модулем он показал неудовлетворительные результаты, однако эти настройки позволяли сохранять устойчивость работы фильтра даже при сильных вибрациях.

Прежде чем использовать математику и алгоритмы в железе, все это было проверено при помощи математического моделирования. В математическую модель, кроме математики и алгоритмов навигации, были заведены: достаточно простая модель ошибок датчиков (в том числе «ненули», гауссовский шум, размах измерения, разрядность АЦП и др.), параметры объекта движения (масса, коэффициент лобового сопротивления, площадь лобовой проекции). Траекторией для моделирования являлось движение по замкнутой линии типа «коробочка», т.е., по сути, по квадрату, где координаты начала движения и его окончания совпадают. Сначала моделирование проводилось на «чистом» (невозмущенном) движении, а потом и при воздействии вибрации:

- синусоидального сигнала в канале ДУС частотой 10 Гц и амплитудой 5 гр/с.;

- синусоидального сигнала в канале ДУС частотой 50 Гц и амплитудой 4 гр/с.;

- синусоидального сигнала в канале ДУС частотой 500 Гц и амплитудой 3 гр/с.;

- синусоидального сигнала в канале ДУС частотой 1000 Гц и амплитудой 2 гр/с.

Отдельным любопытным экспериментом было моделирование работы наших алгоритмов при подаче на их вход реальных «сырых» данных датчиков ADIS16480, которые были записаны в телеметрию в ходе полетов БПЛА.

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

Разработка и отладка программно-математического обеспечения в среде CM-Lynx и ОСРВ MACS

Разработка и отладка ПМО велась в проприетарной среде Миландра «CM-Lynx». Все ПМО написано на C/C++ с редкими вкраплениями ассемблера.

В алгоритмически сложной, насыщенной взаимодействиями с разнородной аппаратурой программной системе реального времени нельзя, разумеется, обойтись без использования соответствующей операционной системы. В нашем модуле применена ОСРВ MACS. В частности, работа ПО модуля существенно опирается на такие ее функциональные возможности, как:

-  Вытесняющий алгоритм планирования на основе приоритетов;

- Стандартные средства синхронизации потоков:

– Мьютексы,

– Семафоры,

– События,

- Средства для организации межпоточного информационного обмена (очереди сообщений).

Нужно признаться, что в данном случае, при применении нашего же ЦСП K1967ВН04BG выбора не было – ни в среде, ни в ОСРВ. Не без некоторых трудностей, но проект был завершен, отлажен и стал живым организмом.

Отдельно стоит сказать про процессор. Вообще, применение цифровых сигнальных процессоров в системах навигации и управления подвижных объектов до сих является экзотикой, особенно в России. Не будем углубляться в причины такой ситуации, но можно только сказать, что ЦСП являются не столь распространенными по сравнению с процессорами общего пользования. В нашем же проекте применение ЦСП оправдало себя полностью.

ЦСП 1967ВН04BG работает с тактовой частотой 200 МГц (при максимально возможной 230). Ядро процессора содержит два вычислительных устройства, называемых вычислительными модулями. Каждый вычислительный модуль содержит регистровый файл и четыре независимых вычислительных блока: ALU, CLU, умножитель и сдвиговое устройство. Вычислительные блоки способны обрабатывать данные в нескольких форматах представления с фиксированной и плавающей точкой. Форматы данных с плавающей точкой: 32-битное обычное слово (float); 64-битное двойное слово (double); 40-битное расширенное слово. Операции с плавающей точкой выполняются с одинарной, двойной и расширенной точностью, и все эти возможности реализованы аппаратно. Кроме того, у процессора развитая периферия: 3 канала интерфейса SPI, 2 канала UART, 2 резервированных канала магистрального интерфейса ГОСТ52070-2003 (МКИО), интерфейс ARINC, содержащий в своем составе восемь приемников и четыре передатчика по ГОСТ 18977-79 и некоторые другие интерфейсы, а также множество каналов DMA.

Испытания

Испытания проводились в основном на квадрокоптере.

В каждом испытании два модуля (наш и ADIS16480) были жестко закреплены друг относительно друга, и их соответствующие измерительные оси были коллинеарны c допуском ± 3…4 угловых градуса. Все испытательные включения происходили также одновременно, при этом в качестве эталона использовались данные географических координат и скоростей, получаемые от приемника спутникового навигационного сигнала. Еще одной опорой для анализа являлось то, что начальные и конечные координаты и угловые положения аппарата совпадают с визуальной точностью. На основе данных о географических скоростях от приемника спутникового навигационного сигнала (при достаточно интенсивном движении) мы получали данные об истинном курсе на траектории движения без учета так называемого угла сноса. Была специально выбрана траектория движения «коробочка», где направление движения каждого участка этой «коробочки» максимально совпадало с направлением на одну из сторон света: север, юг, восток или запад, путевая скорость составляла при этом 36 км/ч. Наш модуль производил счисления координат и углов в чисто автономном режиме (без поддержки от приемника сигнала GPS/ГЛОНАСС).

Здесь первые три верхних графика – углы, счисленные ADIS16480, три нижних – счисленные нашим БИНС. На участках прямолинейного движения заметно, как накапливается ошибка углов тангажа и крена ADIS16480, либо наоборот, они как бы медленно приближаются к истинному. Т.е. характеристики этих углов ADIS 16480 не в полной мере соответствуют «физике» полета квадрокоптера. При этом те же углы БИНС адекватно соответствуют этой «физике».

Частота обсчета углов и координат - 1000 Гц. Температура воздуха -8 °С. Приведенные в таблице максимальные ускорения, полученные по данным нашего модуля, во всех осях достаточно велики, при этом зарегистрированные по данным ADIS16480 максимальные ускорения не превышали 45 м/с² по оси Y, а по остальным осям составляли менее 25 м/с². Это объясняется расположением испытуемого нашего модуля БИНС, который имел приличное отстояние от центра масс коптера, когда как расположение ADIS16480 практически совпадало с этим центром масс. То же можно сказать и об угловых скоростях – максимальная угловая скорость у ADIS16480 по сути не превышает 100 гр/с.

Заключение

Проект по разработке собственной бесплатформенной инерциальной навигационной системы можно считать удачным. Мы пошли дальше простого повторения курсовертикали на замену ADIS16480. В данный момент изготавливаются несколько таких модулей в вариантах исполнения с диапазонами измерения ДУС: ± 450 и ± 250 гр/с.; и с диапазонами измерения по акселерометрам: ± 2, ± 5 и ± 10g. Предполагается испытать их не только на квадрокоптере, но и на других беспилотниках, и на самолете Як-12. Есть версия для автомобиля, с урезанным количеством датчиков и диапазоном их измерения. Также в плане разработка собственного полетного контроллера для БПЛА ответственного применения – коптеров и конвертопланов.