УСТРОЙСТВО ДЛЯ ОПРЕДЕЛЕНИЯ УГЛОВ ПРОСТРАНСТВЕННОЙ ОРИЕНТАЦИИ ПОДВИЖНОГО ОБЪЕКТА Российский патент 2015 года по МПК G01C21/08 G01R33/02 

Изобретение относится к области измерительной техники и может быть использовано в магнитной навигации, в частности, для определения углов пространственной ориентации летательных аппаратов (ЛА).

Достоинствами магнитных датчиков являются их автономность, малое время готовности, скрытность работы и отсутствие накопления ошибок во времени. В связи с этим их применение для решения задач ориентации подвижных объектов, в том числе летательных аппаратов, продолжает оставаться актуальным. Однако могут возникать трудности, связанные с неоднозначностью решения.

Известно устройство для определения углового положения подвижного объекта (патент RU 2285931 C1, МПК G01R 33/02, G01P 3/42, G01C 21/08, опубл. 20.10.2006), включающее трехкомпонентный магнитометр, два трехкомпонентных акселерометра, у которых оси чувствительности коллинеарны строительным осям системы координат OXYZ подвижного объекта с началом координат в точке О и осям магниточувствительного датчика трехкомпонентного магнитометра, при этом акселерометры размещены на продольной строительной оси системы координат OXYZ подвижного объекта симметрично относительно начала координат в точке О, выбранной в месте центра тяжести подвижного объекта, регистрирующий блок, к которому подключены выходы трехкомпонентного магнитометра и трехкомпонентных акселерометров, вычислительное устройство, соединенное с выходом регистрирующего блока, инерциальное устройство, к которому подключен выход вычислительного устройства, и катушку индуктивности, соединенную с магнитометром и размещенную на инерциальном устройстве так, что ось катушки ориентирована по вертикали.

Недостатком известного устройства является то, что оно не обеспечивает требуемой точности определения углов пространственной ориентации ЛА, а наличие, в частности, катушки индуктивности усложняет его конструкцию и снижает технологичность.

Известно другое устройство для определения углового положения подвижного объекта (патент RU 2278356 C1, МПК G01C 21/08, G01R 33/02, опубл. 20.06.2006), которое по технической сущности и достигаемому техническому эффекту наиболее близко к заявляемому изобретению. Данное устройство включает в себя трехкомпонентный магнитометр, четыре трехкомпонентных акселерометра, оси чувствительности которых коллинеарны строительным осям системы координат OXYZ подвижного объекта с началом координат в точке О, выбранной в месте центра тяжести подвижного объекта, и осям магниточувствительного датчика трехкомпонентного магнитометра, при этом первый и второй трехкомпонентные акселерометры размещены на одной оси, проходящей через начало координат - точку О, но по разные стороны от начала координат, третий и четвертый трехкомпонентные акселерометры размещены на другой оси, проходящей через начало координат - точку О, но по разные стороны от начала координат, регистрирующий блок, к входам которого подключены трехкомпонентный магнитометр и четыре трехкомпонентных акселерометра, и вычислительное устройство, к которому подключен регистрирующий блок.

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

Цель заявляемого изобретения - повышение точности автономного определения углов пространственной ориентации ЛА в условиях спокойного прямолинейного полета и интенсивного маневрирования.

Поставленная цель достигается за счет того, что в устройство определения углов пространственной ориентации подвижного объекта, содержащее трехкомпонентный магнитометрический датчик, трехкомпонентный акселерометр, оси чувствительности которых коллинеарны строительным осям системы координат OXYZ подвижного объекта с началом координат в точке О, выбранной в месте центра тяжести подвижного объекта, и вычислительное устройство, дополнительно введены второй трехкомпонентный магнитометрический датчик, оси которого коллинеарны осям первого трехкомпонентного магнитометрического датчика, но направлены в противоположные стороны, трехкомпонентный блок датчиков угловых скоростей, оси чувствительности которого коллинеарны строительным осям системы координат OXYZ подвижного объекта, и блок фильтрации Калмана, к входам которого подключены соответствующие выходы трехкомпонентных магнитометрических датчиков, трехкомпонентного блока датчиков угловых скоростей и вычислительного устройства, входы которого соединены с выходами трехкомпонентного акселерометра и первого трехкомпонентного магнитометрического датчика.

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

Устройство содержит первый трехкомпонентный магнитометрический датчик 1, с выхода которого получают значения проекций напряженности магнитного поля Земли на связанные (строительные) оси ЛА t_Bx; t_By t_Bz, второй трехкомпонентный магнитометрический датчик 2, с выхода которого получают значения проекций напряженности магнитного поля Земли на связанные оси ЛА t_Dx, t_Dy, t_Dz, трехкомпонентный блок 3 датчиков угловых скоростей, с выхода которого получают измеренные гироскопами значения угловых скоростей $${\widehat{\omega }}_x$$ , $${\widehat{\omega }}_y$$ , $${\widehat{\omega }}_z$$ , блок 4 фильтрации Калмана, вычислительное устройство 5, к соответствующим входам которого подключены выходы первого трехкомпонентного магнитометрического датчика 1 и трехкомпонентного акселерометра 6, с выхода которого снимаются сигналы, равные проекциям линейных ускорений на связанные оси ЛА n_X, n_Y, n_Z. Выходы магнитометрических датчиков 1 и 2, трехкомпонентного блока 3 датчиков угловых скоростей и вычислительного устройства 5 подключены к соответствующим входам блока 4 фильтрации Калмана, где осуществляется определение пространственной ориентации рекуррентным способом.

В отличие от традиционного представления будем рассматривать так называемое «магнитное» рыскание, которое будем понимать как угол поворота аппарата в горизонтальной плоскости, отсчитываемый от текущего положения плоскости магнитного меридиана против часовой стрелки в диапазоне 0-2π. Далее этот угол обозначается ψ, без какого-либо дополнительного индекса. Тогда магнитный курс, отсчитываемый по часовой стрелке, определяется как ψ_m=2π-ψ. Рассмотрим плоскость магнитного меридиана, которая является вертикальной плоскостью, проходящей через точку текущего местоположения летательного аппарата, она содержит в себе вектор напряженности геомагнитного поля, составляющие которого обозначим: $$t_n^T=[d_1{d}_2{d}_3]$$ , где d₁, d₂, d₃ - горизонтальная, вертикальная и поперечная составляющая, перпендикулярная плоскости магнитного меридиана, которая равна нулю d₃=0, то есть имеет место $$t_n^T=[d_1{d}_{2}0]$$ .

Ориентацию аппарата будем определять относительно инерциальной системы координат, сопровождающей ЛА, повернутой в горизонте в направлении магнитного меридиана. Тогда при произвольной ориентации аппарата трехкомпонентный магнитометр 1 измеряет проекции вектора t_n на оси чувствительности магнитометра. Далее полагаем, что оси чувствительности совпадают со строительными осями летательного аппарата.

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

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

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

Учитывая наличие ошибок датчиков, а также априорную неопределенность, рассматриваемая задача представляется в стохастической постановке и основывается на формировании уравнений объекта и наблюдений.

С учетом сделанных замечаний вектор состояния принимается в виде

Здесь: ψ - угол «магнитного» рыскания; ϑ - тангаж; γ -крен; c_x, c_y, c_z - переменные величины, учитывающие совместно дрейфы гироскопов и указанные дополнительные угловые скорости; d₁ и d₂ - горизонтальная и вертикальная составляющие вектора напряженности геомагнитного поля в плоскости магнитного меридиана.

Уравнения объекта являются непрерывными и имеют вид

,

,

,

,

,

,

,

Здесь: $${\widehat{\omega }}_x={\omega }_x+c_x$$ , $${\widehat{\omega }}_y={\omega }_y+c_y$$ , $${\widehat{\omega }}_z={\omega }_z+c_z$$ , где ω_x, ω_y, ω_z - угловые скорости летательного аппарата; $${\widehat{\omega }}_x$$ , $${\widehat{\omega }}_y$$ , $${\widehat{\omega }}_z$$ - измерения угловых скоростей гироскопами; T_x, T_y, T_z, T₁, T₂ - постоянные времени корреляции; w_ψ, w_ϑ, w_γ - возмущения, учитывающие неточность интегрирования параметров ориентации; v_x, v_y, v_z и u₁, u₂ - возмущения, учитывающие нестабильность дрейфов гироскопов и параметров геомагнитного поля.

Все возмущения принимаются нормальными процессами белого шума.

Уравнения (2) являются нелинейными относительно координат состояния, но линейными относительно вектора возмущений w и могут быть записаны в векторном виде

Здесь А(х) - векторная функция, компоненты которой определяются правыми частями дифференциальных уравнений в (2).

Соответствующая детерминированная модель имеет вид

Априорная информация о начальном состоянии принимается в виде нормального распределения вектора состояния для начального времени t₀:

Здесь ψ₀₀, ϑ₀, γ₀ - начальные значения магнитного рыскания, углов тангажв и крена; d₁₀ и d₂₀ - начальные значения горизонтальной и вертикальной составляющих вектора напряженности геомагнитного поля в плоскости магнитного меридиана.

Измерения вектора состояния являются дискретными, с малым шагом дискретизации по времени Δt=t_i-t_i-1.

Вектор измерений сигналов рассматриваемой пары магнитометров, обозначаемых далее индексами В и D, для дискретных моментов времени t₁ имеет вид

Для его составляющих имеют место соотношения

,

,

,

.

Здесь: ψ₀=π - угол разворота магнитометра D относительно магнитометра В; t_B, t_D - векторы измерений магнитометров; t_n - вектор напряженности геомагнитного поля в плоскости магнитного меридиана; B(t_i), D(t_i)- матрицы направляющих косинусов, проецирующие вектор t_n на оси чувствительности магнитометров; r_B(t_i) и r_D(t_i) - векторы ошибок измерений магнитометров, которые при условии предварительного списания девиации принимаются центрированными процессами дискретного белого шума.

С учетом (1) и (9) уравнение наблюдений представляется нелинейным относительно координат состояния, но линейным относительно ошибок измерения и может быть записано в векторном виде

Здесь h[x(t_i)] - векторная функция, компоненты которой определяются соотношениями (5); М - символ операции математического ожидания; r(t_i) - вектор ошибок измерений магнитометров с заданной ковариационной матрицей R.

Соответствующее детерминированное уравнение наблюдений имеет вид

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

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

На каждом шаге дискретизации Δt выполняются две вычислительные процедуры - прогноз плотности вероятности вектора состояния для очередного момента времени и оценивание апостериорной плотности с учетом вновь поступившего вектора измерений. При этом удерживаются две статистики апостериорной плотности вероятности вектора состояния - математическое ожидание и ковариационная матрица.

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

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

Для этого уравнения объекта (2) и наблюдений (8) линеаризуются известным способом в окрестности текущих значений компонент вектора состояния F=l+A-At. Непрерывной модели (2) соответствует дискретная модель, линеаризованная в окрестности текущих значений компонент вектора состояния:

Здесь: Q_ι - ковариационная матрица эквивалентных дискретных шумов возмущений. Линеаризованное уравнение наблюдения имеет вид

В вычислителе 5 осуществляется начальная выставка в горизонте по сигналам акселерометров. Углы крена γ₀ и тангажа ϑ₀ вычисляются с использованием результатов измерения трехкомпонентного акселерометра:

,

где n_x, n_y, n_z - перегрузки по трем осям.

Углы γ₀ и ϑ₀, отличающиеся от нуля, показывают отклонение ЛА от вертикали:

,

где n₀, n_y, n_z - перегрузки по трем осям.

Углы γ₀ и ϑ₀, отличающиеся от нуля, показывают отклонение ЛА от вертикали.

Тогда при точно известных значениях крена и тангажа определятся магнитный курс ЛА, используя выходные сигналы первого трехкомпонентного магнитометрического датчика:

,

Когда проходит прямолинейный полет, при отсутствии линейных ускорений выполняются условия невозмущенного полета:

, где Δn определяется погрешностями и шумами акселерометров.

Текущие углы крена и тангажа определяются по сигналам акселерометров

.

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

Шаг 1. Расчет статистик $${\overline{x}}_{i+1}$$ , $${\overline{P}}_{i+1}$$ нормальной плотности распределения $$x(t_{i+1/i})\in N\left\{{\overline{x}}_{i+1},{\overline{P}}_{i+1i}\right\}$$ , априорной для очередного (i+1)-го момента дискретного времени. Выполняется путем прогноза статистик $${\widehat{x}}_i$$ , $${\widehat{P}}_i$$ , апостериорных для предыдущего i-го момента времени. Исходным является апостериорное распределение для предыдущего шага $$x(t_{i/i})\in N\left\{{\widehat{x}}_{i+1},{\widehat{P}}_{i+1i}\right\}$$ .

Математическое ожидание $${\overline{x}}_{i+1}$$ вычисляется интегрированием детерминированных уравнений объекта (4), при этом начальными условиями является оценка вектора состояния, вычисленная на предыдущем шаге. Для начального шага берется математическое ожидание априорного распределения (5).

Шаг 2. Ковариационная матрица $${\overline{P}}_{i+1}$$ ^ вычисляется с помощью линеаризованных дискретных уравнений объекта (10) и наблюдений (11): $$\overline{P}=F\widehat{P}{F}^T+Q$$ .

Шаг 3. Определение коэффициента усиления фильтра: $$K=\overline{P}{H}^T{(HP{H}^T+R)}^{-1}$$

Шаг 4. Вычисление апостериорной ковариационной матрицы:

$$\widehat{P}=(I-KH)\overline{P}{(I-KH)}^T+KR{K}^T$$ .

Шаг 5. При вычислении невязки $$Z_{i+1}-h({\overline{x}}_{i+1})$$ используется оценка вектора измерений $$h({\overline{x}}_{i+1})$$ , вычисляемая с помощью нелинейных детерминированных уравнений наблюдения (7).

Шаг 6. Оценка вектора состояния: $${\widehat{x}}_{i+1}={\overline{x}}_i+K(Z_{i+1}-h(\overline{x_{i+1}}))$$ - переход к шагу 1. Оцененные значения углов магнитного курса, крена и тангажа поступают к потребителям.

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

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

Изобретение относится к области измерительной техники и может быть использовано в магнитной навигации, в частности, для определения углов пространственной ориентации летательных аппаратов (ЛА). Устройство для определения углового положения подвижного объекта содержит два трехкомпонентных магнитометра, трехкомпонентный блок датчиков угловых скоростей, трехкомпонентный акселерометр, вычислительное устройство и устройство фильтрации Калмана, размещенные на подвижном объекте и включенные между собой соответствующим образом. Определение углов пространственной ориентации осуществляется посредством блока фильтрации Калмана рекуррентным способом. Коррекция магнитного курса осуществляется по сигналам трехкомпонентного акселерометра. Техническим результатом использования изобретения является повышение точности и обеспечение автономного определения углов пространственной ориентации ЛА в условиях маневрирования в полете, а также малое время готовности, скрытность работы и отсутствие накопления ошибок во времени. 1 ил.

Устройство для определения углов пространственной ориентации подвижного объекта, содержащего трехкомпонентный магнитометрический датчик, трехкомпонентный акселерометр, оси чувствительности которых коллинеарны строительным осям системы координат OXYZ подвижного объекта с началом координат в точке О, выбранной в месте центра тяжести подвижного объекта, и вычислительное устройство, отличающееся тем, что в него дополнительно введены второй трехкомпонентный магнитометрический датчик, оси которого коллинеарны осям первого трехкомпонентного магнитометрического датчика, но направлены в противоположные стороны, трехкомпонентный блок датчиков угловых скоростей, оси чувствительности которого коллинеарны строительным осям системы координат OXYZ подвижного объекта, и блок фильтрации Калмана, к входам которого подключены соответствующие выходы трехкомпонентных магнитометрических датчиков, трехкомпонентного блока датчиков угловых скоростей и вычислительного устройства, входы которого соединены с выходами трехкомпонентного акселерометра и первого трехкомпонентного магнитометрического датчика.