МЕХАНИКА ТВЕРДОГО ТЕЛА <3 • 2008
УДК 531.383
© 2008 г. Е.Ю. АЛЕХОВА
МАТЕМАТИЧЕСКАЯ МОДЕЛЬ БЛОКА ЧУВСТВИТЕЛЬНЫХ ЭЛЕМЕНТОВ ДЛЯ ОТРАБОТКИ БОРТОВЫХ АЛГОРИТМОВ НАВИГАЦИОННЫХ СИСТЕМ
В последнее время получают широкое распространение бескарданные навигационные гироскопические системы. В подобных системах чувствительные элементы, гироскопы и акселерометры размещаются непосредственно на борту объекта, а вместо стабилизированной гироскопической платформы используется воображаемый аналитический трехгранник. Ориентация объекта по отношению к аналитическому трехграннику вычисляется путем численного решения уравнений Пуассона по показаниям гироскопических чувствительных элементов, измеряющих угловые скорости объекта. Параметры взаимной ориентации позволяют перепроектировать кажущееся ускорение, измеренное акселерометрами в осях объекта, на оси аналитического трехгранника. В аналитическом трехграннике навигационная задача решается так же, как она решалась в платформенных системах, но в целом функции бортовых алгоритмов бескарданных систем существенно сложнее, чем в платформенных системах. Возможность детальной отработки бортовых алгоритмов имеет существенное значение для обеспечения точности навигационной системы в целом.
В данной работе сформированы алгоритмы, позволяющие воспроизвести точные показания идеальных гироскопических датчиков и точные показания идеальных акселерометров в условиях работы системы на стенде при угловом движении, имитирующим эволюцию ориентации объекта и возможную угловую вибрацию. Одновременно вычисляются точные параметры положения и ориентации, с которыми можно сравнить результаты работы бортового алгоритма.
С блоком гироскопических чувствительных элементов (ЧЭ) свяжем трехгранник е с осями e1e2e3. Неподвижный трехгранник обозначим o, его оси o1o2o3. Будем считать, что приборный трехгранник получается из неподвижного следующими последовательными поворотами (промежуточные положения приборного трехгранника обозначим буквами a, b, с, d): первый - вокруг оси o3 на угол Qí, получается трехгранник a; второй поворот вокруг оси a1 на угол а, получается трехгранник b; третий поворот вокруг оси b3 на угол raí, получается трехгранник с; четвертый - вокруг оси c1 на угол в, имеем трехгранник d; последний, пятый - вокруг d3 на угол - raí, блок принимает ориентацию трехгранника е.
Перечисленным поворотам соответствуют кватернионы:
Лоа = { cos (Qí/2) 0 0 sin (Qí/2)} ЛаЬ = { cos (а/2) sin (а/2) 0 0}
ЛЬс = { cos (ra í/2) 0 0 sin (ra í/2)} Лы = { cos (в/2) sin (в/2) 0 0} (1)
ЛЛе = { cos (raí/2) 0 0 -sin (raí/2)} Суммарный поворот соответствует произведению этих кватернионов
Лое = Лоа ° Л ab ° ЛЬс ° Kd ° Ке (2)
Движение по конусу с угловой скоростью О имитирует эволюцию ориентации объекта. Второе коническое движение с угловой скоростью ю вокруг оси Ь3 и с угловой скоростью -ю вокруг оси й3, составляющей с осью Ь3 угол в, имитирует вибрацию блока ЧЭ. Матрицы поворотов от одного трехгранника к другому
А
ba
Adc -
1 0 0 0 cos a sin а
0 -sin а cos а
1 0 0 0 cos в sin в 0 -sin в cos в
Aed -
cos юt sin юt 0 -sin rnt cos rat 0 0 0 1
cos rat -sin rat 0 sin юt cos юt 0 0 0 1
(3)
(4)
позволяют записать угловые скорости каждого трехгранника в проекциях на собственные оси
Wa - Q|| 0 0 11T, Wb - AbaWa - Q|| 0 sin a cos a||T
Wc - AcbWb + ||0 0 ю||T - l|Q sin a sin ю t Q sin a cos ю t Q cos a + ю||T
« - AdcW -
Q sin a sin ю t Q sin a cos юt cos в + (Q cos a + ю) sin в - Q sin a cos юt sin в + (Q cos a + ю) cos в
(5)
(6)
(7)
We - AedWd 410 0 ю||T
We
Q sin a sin ю t cos юt - [Q sin a cos юt cos в + (Q cos a + ю) sin в] sin юt Q sin a sin юt sin юt + [Q sin a cos ю t cos в + (Q cos a + ю) sin в]cos юt - Q sin a cos ю t sin в + (Q sin a + ю) cos в - ю
(8)
Верхний индекс у вектора угловой скорости указывает трехгранник, к которому относится вычисляемое значение. С использованием соотношения
2 2 2 sin юt + cos юt cos в - 0.5(1 + cos в + 1 - cos в) sin юt +
+ 0.5( 1 + cos в - 1 + cos в) cos юt - 0.5 (1 + cos в) -0.5 (1 - cos в) cos2 ю t вектор угловой скорости приборного трехгранника можно записать покомпонентно: Q1 - 0.5Q sin a( 1 - cos в) sin2юt - (Q cos a + ю) sin в sin юt
(9)
Qe
0.5Q sin a( 1 + cos в) - 0.5Q sin a( 1 - cos в) cos2юt + (Q cos a + ю) sin в cos юt (10)
Q3 - Q cos a cos в - (1 - cos в)ю t - Q sin a sin в cos ю t
Показания с чувствительных элементов блока снимаются в дискретные моменты времени, кратные интервалу съема показаний т. С каждого гироскопического датчика снимается значение интеграла от соответствующей проекции угловой скорости за ин-
тервал съема показаний. Для точного вычисления этих интегралов достаточно вычислить интегралы от тригонометрических функций:
(k + 1)т
f . ^ sin (ЮТ/2) . 1 I sin ю tdt = т — -sin (k + 1/2)ют = qsT
.sin ( юх /2)^ ют /2
кт
(к +1)т
í sin2raídí = т sin (ю т ) sin (2k + 1 )ют = j ют
кт (11) (к +1)т 4 7
г ^ sin (ют/2) 1
I cos югЛ = т— ,„ - cos (к + 1/2)ют = а; J ют/2 c
í cos2юídí = т sin (ю т ) cos (2к +1 )ют = а2ст J ют 2c
кт (к + 1)т
^п (ют), ют
кт
Показания гироскопических датчиков на интервале с номером к будут следующими:
51 (к) = 0.5 0тsin а( 1 - cos в) q2s - (Q^os а + ют) sin в qs
Se2( к) = 0.5 0тsin а( 1 - cos в) -0.50тsin а( 1 - cos в) q2c + (От cos а + ют) sin в qc (12)
53 (к) = От cos а cos в - ют( 1 - cos в) - От sin а sin вqc
Таким образом, формулы (12), с расшифровкой величин q с индексами по формулам (11), могут служить моделью идеального блока ЧЭ, измеряющего угловое движение. Формула (2), с расшифровками (1), дает истинную ориентацию блока для любого момента времени, в том числе и для любого момента кт.
Полные выражения для показаний ЧЭ на интервале с номером к можно записать в виде:
51( к) = 0.5 От sin а( 1 - cos в) sin((°T ) sin (2к +1 )ют -
ют
,„ . . Qsm(ют/2) . 1
- (От cos а + ют) sin в —^-т-—--sin (к + 1/2)ют ют/2
52 (к) = 0.5 От sin а( 1 - cos в) -0.50тsin а( 1 - cos в) cos (2 к +1 )ют + (13)
+ (От cos а + ют) sin В sin ( 0)т ) cos (к + 1/2)ют ют
53 (к) = От cos а cos в - ют( 1 - cos в) - От sin а sin в ^^т/^2'' cos (к + 1/2)ют
В формулах (13) удобно ввести обозначения для коэффициентов, не зависящих от номера интервала:
„ .„ . Q4sin(ют) ,„ . . Qsin(ют/2)
rsu = 0.5От sin а( 1 - cos В) —-—- rs12 = (От cos а + ют) sin В —4 ,„ ' 11 ют 12 ют/2
rs21 = 0.5От sinа( 1 + cos в) rs31 = Отcosаcosв - ют( 1 - cos в) (14)
„ . . Qsin(ют/2) rs32 = От sin а sin В —^ ,,, ' 32 г ют/2
Тогда формулы (13) можно записать в виде: 51 (k) = ri nsin (2 k +1 )ют - rs12sin (k + 1/2)ют
S2(k) = rs21 - rsncos(2k +1 )ют + rs12cos(k + 1/2)ют (15)
Se3(k) = rs31 - rs32cos(k + 1/2)ют
Тригонометрические функции, зависящие от номера интервала, удобно вычислять, учитывая постоянный прирост аргумента при каждом переходе к следующему интервалу:
sin (2k +1 )ют = sin (2 k -1 )ют cos2 ют + cos (2 k -1 )ют sin2 ют cos (2k +1 )ют = cos (2k -1 )ют cos2 ют -sin (2 k -1 )®Tsin2raT
(16)
sin (k + 1/2)ют = sin (k-1/2)ют cos ют + cos (k - 1/2)ют sin ют cos (k + 1/2) ют = cos (k - 1/2)ют cos ют -sin (k -1/2)ют sin ют
Функции sinou, cosart, sin2wr, со$2ют, не зависящие от номера, можно вычислить заранее. Формулы (15), (16) могут быть использованы в качестве математической модели блока чувствительных элементов для отработки и тестирования бортовых алгоритмов вычисления ориентации. Для отработки алгоритмов начальной выставки и опробования полетных алгоритмов в условиях стенда эти же формулы можно использовать, положив величину Q равной угловой скорости суточного вращения Земли, а угол а равным разности углов 90° и угла географической широты. Точные показания идеальных акселерометров в виде вектора прироста кажущейся скорости за интервал съема показаний вычисляются согласно формуле
(k + 1)т
Д Vе (k) = J Wedt (17)
kx
где We - вектор кажущегося ускорения в проекциях на оси приборного трехгранника. Этот вектор можно вычислить по формуле
We = AedAdcAcbWb (18)
считая, что Wb = ||0 0 g||T - вектор кажущегося ускорения в географическом трехграннике. Используя выражения (3), (4) для матриц, получаем
We = g||-sin в sin ю t sin в cos ю t cos PllT (19)
Учитывая (19) и формулы (11) для интегралов, выражение (17) для &Ve(k) можно привести к виду
Д Ve (k) = gi||-qs sin в qc sin в cos в||T (20)
Полные выражения для показаний акселерометров на интервале с номером k записываются покомпонентно в виде
Д V1 (k) = ^т sin в ^-l/psin (k + 1/2)ют
sin (ют/2) ,, 1 (21) -¡i-¿cos ( k +1/2) ют ^^
Д V3 (k) = gT cos в
Д V2(k) = gT sin p ЮТ/2 cos (k + 1/2)ют
Для коэффициентов, не зависящих от номера интервала, удобно ввести обозначения
. Qsin(ют/2) п
rs 41 = g т Sin ß—ЮГ/2 , rs 42 = g тcos ß (22)
и формулы для компонент прироста кажущейся скорости записать так:
А Vi (k) = -rs41sin (k + 1/2)ют
e e (23)
А V2 (k) = rs41cos (k + 1/2)ют, AV3(k) = rs42
Таким образом, полная математическая модель блока чувствительных элементов гироскопической навигационной системы реализуется вычислением показаний идеальных гироскопов по формулам (15) и показаний идеальных акселерометров по формулам (23). При этих вычислениях используются значения тригонометрических функций, вычисляемые на каждом шаге по формулам (16). Коэффициенты rs с индексами в формулах (15) и (23) вычисляются заранее в подготовительном блоке программы. Точная ориентация подвижного трехгранника в системе отсчета, неподвижной относительно звезд, вычисляется на каждом шаге по формуле (2) с учетом выражений (1). Преобразование вычисленных векторов произвольным постоянным кватернионом позволяет моделировать произвольную ориентацию блока чувствительных элементов на испытательном стенде.
В качестве иллюстрации приведем пример использования предложенных алгоритмов для сравнительного анализа точностных характеристик двух вариантов бортовых алгоритмов вычисления ориентации.
В первом варианте по вектору малого поворота S формируется соответствующий ему кватернион (с использованием разложения в ряд тригонометрических функций):
Л = 1 - r/8 + r2/348 + (0.5 - r/48 + r2/3840)S, r = S • S
Численное интегрирование ура
Для дальнейшего прочтения статьи необходимо приобрести полный текст. Статьи высылаются в формате PDF на указанную при оплате почту. Время доставки составляет менее 10 минут. Стоимость одной статьи — 150 рублей.