Беспилотники. Ч4: Неортогональный блок и Матрица Направляющих Косинусов.

Всем привет! Пристегните ремни... начинается серьезная математика / механика.
В этой статье пойдет более детальный рассказ о взаимном расположении осей чувствительности (ОЧ) микромеханических инерциальных датчиков в неортогональном блоке. Я собираюсь доходчиво объяснить почему при использовании двукомпонентных датчиков образуется пара инерциальных конусов. Также постараюсь объяснить как строится матрица направляющих конусов неортогонального блока.

Ссылки на статьи серии

Беспилотники. Ч1: что это за игрушка и как ею управляют
Беспилотники. Ч2: дроноприменение
Беспилотники. Ч3: Схемы ИНС. Ортогональная против Избыточной
Беспилотники. Ч4: Неортогональный блок и Матрица Направляющих Косинусов.

Неортогональный блок

Рис. 1. Принципиальная схема расположения 4-х двукомпонентных iMEMS датчиков в неортогональном блоке.
*** Взято из моего дипломного проекта

Итак, на Рис. 1 в схематическом виде представлена одна из возможных конструкций неортогонального блока. Эта конструкция придумана не мной, а одним из ветеранов каф. ПБС СГТУ. Это своего рода хак, позволяющий вместо пирамиды, которая сложна в изготовлении, использовать обычный параллелепипед. Хитрость в том, что вместо наклона боковых граней сам датчик вращается в плоскости вертикальной грани на угол α. Таким образом, если обе ОЧ датчика располагаются в плоскости микросхемы, то одна из них смещается на угол α от вертикали параллелепипеда, а вторая смещается на тот же угол от горизонтали. Теперь, если совместить начала систем координат датчиков в центре параллелепипеда (основания), то мы получим два конуса, по боковым поверхностям которых распределены разные ОЧ датчиков. У одного конуса угол полураствора будет равен [α] градусов, а у другого - [90-α].

В статье Л.В. Водичевой представлено рассчетное обоснование конкретного угла полураствора конуса (см. источник 1). Он равен 54,75 градуса (в десятичных единицах), т.е. примерно 55 градусов. Таким образом, если у одной неортогональной четверки угол полураствора конуса будет равен 55 градусам, то у второго он уже будет равен 35 градусам. Насколько критично это отклонение скажется на качестве измерений - вопрос для кандидатской диссертации. Для любительских беспилотников считаем, что некритично.

Еще один важный момент - ориентация системы координат, привязанной к основанию неортогонального блока (Связанная Система Координат, Связанная СК). Для нормальной работы неортогонального блока нельзя допускать, чтобы две и более ОЧ были параллельны или ортогональны друг другу. Этому требованию мы удовлетворяем просто разделив ОЧ по четыре на два отдельных конуса (алгоритмически). Также рекомендуется ОЧ располагать так, чтобы они не были параллельны осям горизонтальной плоскости блока (X, Y). Поэтому оси X и Y Связанной СК направлены по диагоналям основания.

Примечание: я видел решения, когда одну ОЧ располагают параллельно оси Z блока, т.е. информация по оси Z вычисляется избыточным способом (по нескольким ОЧ на поверхности конуса) + классическим (один датчик измеряет ось Z эксклюзивно). Слишком избыточное решение, как мне кажется.

Матрица направляющих косинусов

Небольшое отступление о важности правильного выбора последовательности поворотов:

На гиктаймс была опубликована статья Окончательно разбираемся с градусами: причина аварии «Фрегата». В ней проводится анализ причин недавней аварии разгонного блока "Фрегат" при содействии ракеты-носителя "Союз 2.1b". Если кратко, то последовательность поворотов ракеты-носителя оказалась несогласованной с последовательностью поворотов Фрегата. В результате Фрегат стал вращаться не в ту сторону, которая требовалась. Разные последовательности конечных поворотов дают разные конечные положения несмотря на то, что углы каждого отдельного (конечного) поворота совпадают.

Рис. 2. Схема конечных поворотов "СК Блока -> СК датчика".

Итак, расшифровываем то, что изображено на Рис. 2. Там представлена так называемая схема конечных поворотов (см. Источник 2). Согласно методике конечных поворотов переход твердого тела из одной ориентации в другую произвольную можно представить в виде последовательности трёх элементарных поворотов вокруг каждой из трёх осей системы координат (СК).

На Рис. 2 оси ξ, η, и ζ - это оси XYZ Связанной СК (СК нашего инерциального блока). Оси, обозначенные символами XYZ на Рис. 2 совпадают с осями СК инерциального датчика (микросхемы, примонтированной к боковой грани основания).

Простите за конфуз с обозначениями - изображения были взяты из разных работ, поэтому получился конфликт символов.

Итак, схема на Рис. 2 иллюстрирует переход от Связанной СК блока к СК конкретного датчика.

Первый поворот мы совершаем вокруг оси ξ и получаем новую СК ξ1 η1 ζ1. При этом оси ξ и ξ1 совпадают.
Второй поворот делаем вокруг оси η1 и получаем новую СК ξ2 η2 ζ2. Поэтому η1 и η2 совпадают.
Ну и последний поворот совершаем вокруг ζ2. ζ2 при этом переименовывается в Z (ось датчика). Вместо ξ3 и η3 также пишем X и Y.

Для каждого такого поворота можно составить простую алгебраическую систему из двух тригонометрических уравнений, описывающую перевод компонентов вектора из одной СК в другую.

Рис. 3. Преобразование координат из одной СК в другую.

На Рис. 3 проиллюстрирован второй поворот из описанной выше последовательности. На этой схеме мы видим вектор Q, который мы хотим выразить через координаты в конечной СК (на схеме обозначена осями 1' и 2'). Инами словами, нам нужно спроецировать компоненты вектора в исходной СК (оси 1 и 2) на оси конечной СК (оси 1' и 2'). Для составления системы тригонометрических уравнений используется элементарная Пифагорова геометрия (прямоугольные треугольники).

Длина катета равна длине гипотенузы, умноженной на синус противолежащего угла (угол между другим катетом и гипотинузой).

Длина катета равна длине гипотенузы, умноженной на косинус прилежащего угла (угол между данным катетом и гипотинузой).

Если представить полученную таким образом систему в матричном виде, то получим следующее:

источник

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

В самом начале я написал, что для избыточного блока четырёх двукомпонентных датчиков мы рассматриваем только две из осей датчика. Чаще всего ось Z датчика ориентирована по перпендикуляру к плоскости микросхемы. Поэтому из полученной МНК датчика нам следует выбросить одну строку, которая соответствует оси Z датчика. Далее нам следуют разделить оставшиеся строки. Мы их поместим в две отдельные МНК избыточного блока. Помните я писал выше, что мы оси чувствительности датчиков делим на два конуса с разными углами при вершине. Поэтому и избыточных матриц направляющих косинусов будет две. Одна МНК будет для осей на конусе с полууглом раствора 55 градусов и вторая МНК - для конуса с полууглом в 35 градусов. Датчиков всего у нас четыре, поэтому каждая избыточная МНК будет иметь 4 строки и 3 столбца.
Иными словами, значение проекции угловой скорости (или ускорения) на одну ось датчика (напр., X) будет вычисляться как нелинейная комбинация проекций той же угловой скорости на оси XYZ Связанной СК блока.
*** Вот тут я вынужден прерваться. И совесть меня мучает, т.к. я понимаю что без символьного представления текст выше воспринимать трудновато. В следующей статье постараюсь представить пример вычислений, а также перерисованные схемы каждого из поворотов.