LS-Sb89550
.pdfМИНОБРНАУКИ РОССИИ
–––––––——————————–––––––
Санкт-Петербургский государственный электротехнический университет «ЛЭТИ»
–––––––——————————–––––––
Интегрированные системы навигации
Учебное пособие
Санкт-Петербург Издательство СПбГЭТУ «ЛЭТИ»
2013
УДК 629.3.05(075) ББК В213.33я7+К948я7 И73
Авторы: А. М. Боронахин, Л. Н. Подгорная, А. Н. Ткаченко,
Р. В. Шалымов.
И73 Интегрированные системы навигации: учеб. пособие. СПб.: Изд-во СПбГЭТУ «ЛЭТИ», 2013. 44 с.
ISBN 978-5-7629-1467-3
Рассмотрено изменение кинематических характеристик подвижного объекта при его движении по поверхности Земли и определение этих характеристик с использованием методов инерциальной навигации. Значительное внимание уделено моделированию сигналов гироскопов и акселерометров, а также алгоритмов их обработки в ППК MatLab (Simulink).
Предназначено для магистрантов, обучающихся по направлению «Приборостроение», для подготовки к практическим занятиям по дисциплинам «Инерциальные навигационные системы» и «Разработка и испытания интегрированных систем навигации».
УДК 629.3.05(075) ББК В213.33я7+К948я7
Рецензенты: кафедра аэрокосмических систем ориентации, навигации и стабилизации СПбГУАП; д-р техн. наук О. А. Степанов (ОАО «Концерн ЦНИИ “ Электроприбор”»).
Утверждено редакционно-издательским советом университета
в качестве учебного пособия
ISBN 978-5-7629-1467-3 |
© СПбГЭТУ «ЛЭТИ», 2013 |
Практическая работа № 1 МОДЕЛИРОВАНИЕ СИГНАЛОВ ГИРОСКОПОВ
И АКСЕЛЕРОМЕТРОВ
Цель работы – моделирование сигналов гироскопов и акселерометров в ППК MatLab (Simulink).
Методика проведения работы предусматривает моделирование параметров движения объекта по поверхности Земли (законы изменения линейной скорости и законы изменения углового положения относительно горизонта и направления на север), на основании которых с использованием основных кинематических соотношений, описывающих проекции векторов абсолютной угловой скорости и линейного ускорения на оси связанной с объектом системы координат, формируются сигналы триады гироскопов и акселерометров.
Результаты практической работы используются при выполнении практических работ № 2 и 3.
1.1.Основные теоретические сведения
Вкачестве базовой системы координат наиболее часто используется географическая топоцентрическая система координат (географический сопровождающий трехгранник) OENh. Ее начало совмещено с центром масс объекта, ось OE направлена на восток по касательной к параллели, ось ON – по касательной к меридиану на север, а ось Oh – вдоль вертикали места вверх.
С объектом (например, морским) жестко свяжем систему координат Oxyz, ось Oх которой совпадает с поперечной осью объекта, ось Oу – с продольной осью объекта и направлена к его носу, ось Oz направлена перпендикулярно плоскости палубы (пола для случая наземного объекта) вверх.
Взаимная угловая ориентация трехгранников Oxyz и OENh (рис. 1.1) определяется последовательными поворотами связанной с объектом системы координат относительно географической на угол курса K (по часовой стрелке
вокруг оси Oh), дифферента (тангажа) ψ и крена θ. Угол курса K, как прави-
ло, содержит два компонента:
K = Kо + ϕр ,
3
гдеK о – медленно меняющийся по определенному закону заданный курс объекта; ϕр – угол рыскания относительно заданного курса. Углы ψ и θ
применительно к морским объектам называют углами килевой и бортовой качек соответственно.
Переход из связанной системы координат Oxyz в географическую осуществляется с помощью матрицы направляющих косинусов В в соответствии
|
z2 |
h, z1 |
|
с выражением |
|
|
|
|
|
|
|
|||||
|
|
|
|
[xyz]= B[ENh], |
|
|
|
|
||||||||
z |
|
|
|
|
|
|
|
|
||||||||
|
ψ |
|
|
|
|
|
|
|
||||||||
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
причем |
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
B = BθBψBK , |
(1.1) |
|||||||||
|
|
− Kɺ |
|
где |
|
|
|
|
|
|
|
|
|
|
|
|
|
|
y |
|
|
|
|
|
|
cos θ |
0 |
−sin θ |
|
|
|
||
|
|
θɺ |
N |
|
|
= |
|
|
; |
|
||||||
|
ɺ |
|
B |
|
|
|
|
0 |
1 |
0 |
|
|
||||
|
ψ |
O |
|
θ |
|
|
|
|
|
|||||||
|
|
|
|
|
|
|
sin θ |
0 |
cos θ |
|
|
|
|
|||
|
|
y1 |
|
|
|
|
|
|
|
|
|
|||||
x1 θ K |
|
|
|
Bψ = |
|
0 |
|
0 |
|
|
|
|
||||
|
|
|
|
1 |
|
|
|
|
|
|||||||
x |
|
|
|
|
0 |
cos ψ |
sin ψ |
; |
||||||||
E |
|
|
|
|
||||||||||||
|
|
|
|
|
|
|
0 |
− sin ψ cos ψ |
|
|
||||||
Рис. 1.1. Взаимная угловая ориентация |
|
|
|
|
cos K |
−sin K |
0 |
|
||||||||
|
|
|
|
|
||||||||||||
связанного и географического |
|
BK = |
|
|
||||||||||||
|
sin K |
cos K |
0 |
|
||||||||||||
сопровождающего трехгранников |
|
|
||||||||||||||
|
|
|
|
|
|
|
0 |
|
0 |
1 |
|
|||||
|
|
|
|
|
|
|
|
|
|
|
|
|||||
– матрицы элементарных поворотов на углы θ, ψ, K соответственно. После их перемножения в соответствии с выражением (1.1) получим
|
cos θ cos K + |
− cos θ sin K + |
− sin θ cos ψ |
|
|
|
|
|
|
||||
|
+ sin θ sin ψ sin K |
+ sin θ sin ψ cos K |
|
|
|
|
B = |
cos ψ sin K |
cos ψ cos K |
sin ψ |
|
. |
(1.2) |
|
sin θcos K − |
− sin θ sin K − |
cos θ cos ψ |
|
|
|
|
− cos θ sin ψ sin K |
− cos θ sin ψ cos K |
|
|
|
|
|
|
|
|
|
||
|
|
|
|
|
|
|
Волновые гироскопы, установленные на борту объекта, измеряют проекции абсолютной угловой скорости на их измерительные оси. Как правило, трехосный волновой гироблок располагают на объекте таким образом, чтобы его измерительные оси совпадали с осями объекта.
Абсолютная угловая скорость объекта является суммой угловой скоро-
сти вращения объекта относительно географического трехгранника ω и переносной угловой скорости географического трехгранника u , вращающегося вместе с Землей и за счет движения объекта по земной сфере
4
|
|
|
|
|
|
|
|
|
|
|
(1.3) |
|||
|
|
|
|
Ω = ω + u . |
|
|
|
|
|
|
||||
Проектируя векторное равенство (1.3) на связанные с объектом оси, по- |
||||||||||||||
лучим |
|
|
|
|
|
|
|
|
|
|
|
|
||
Ω = |
|
ωx |
|
= ω+ u = |
|
p |
|
+ B |
|
uE |
|
|
|
|
|
|
|
|
|
|
|
||||||||
|
ω y |
|
|
q |
|
|
u N |
|
, |
(1.4) |
||||
|
|
ωz |
|
|
|
|
r |
|
|
|
uh |
|
|
|
где p, q и r – проекции относительной угловой скорости на связанные оси. Они могут быть определены следующим образом (см. также рис. 1.1):
|
p |
|
|
0 |
|
ψ |
|
0 |
|
|
|
|
|
|
|
|
|
||||
|
|
|
|
|
|
ɺ |
|
|
|
|
|
q |
= Bθ Bψ |
0 |
+ Bθ |
0 |
+ |
θɺ |
. |
|
|
|
r |
|
|
− Kɺ |
|
0 |
|
0 |
|
|
Выполнив вычисления, получим |
|
|
|
|
|
|||||
|
|
|
|
ɺ |
|
|
|
|
|
|
|
p = ψ cos θ + K sin θ cos ψ; |
|
||||||||
|
|
ɺ |
|
|
|
|
|
|
|
|
|
q = θɺ − Kɺ sin ψ; |
|
|
(1.5) |
||||||
|
|
|
|
|||||||
|
|
|
|
ɺ |
|
|
|
|
||
|
r = ψ sin θ + K cos θ cos ψ. |
|
||||||||
|
|
ɺ |
|
|
|
|
|
|
|
|
Полученные кинематические уравнения Эйлера легко разрешаются от- |
||||||||||
|
ɺ |
ɺ |
ɺ |
|
|
|
|
|
||
носительно переменных K |
, ψ , |
θ : |
|
|
|
|
|
|||
Kɺ |
= ( p sin θ − r cos θ) |
cos ψ ; |
|
|||||||
ψ = p cos θ + r sin θ; |
|
|
|
|
(1.6) |
|||||
|
|
|
|
|||||||
ɺ |
|
|
|
|
|
|
|
|
|
|
θɺ = q + tg ψ( p sin θ − r cos θ).
Таким образом, углы ориентации объекта K, ψ, θ, образующие матрицу направляющих косинусов В, могут быть определены в результате интегрирования системы (1.6) нелинейных дифференциальных уравнений. Проекции угловой скорости объекта p, q, r на связанные с ним оси определяются в соответствии с выражением (1.4) исходя из показаний волновых гироскопов
ωx , ω y , ωz и проекций |
угловой |
скорости географического |
трехгранника |
||||||||||||
OENh на его оси, которые имеют вид |
|
|
|
|
|
|
|
|
|||||||
uE = −ϕ = − |
vN |
; |
|
|
|
|
|
|
|
|
|||||
|
|
|
|
|
|
|
|
|
|||||||
|
|
|
ɺ |
|
|
R2 |
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
v |
|
|
|
|
|
uN = (ΩE + λɺ)cos ϕ = ΩE cos ϕ + |
E |
|
|
|
|||||||||||
|
; |
|
(1.7) |
||||||||||||
|
|
||||||||||||||
|
|
|
|
|
|
|
|
|
|
|
R1 |
|
|
||
u |
h |
= (Ω |
E |
+ λɺ)sin ϕ = Ω |
E |
sin ϕ + |
vE |
tg ϕ, |
|
||||||
|
|
||||||||||||||
|
|
|
|
|
|
|
R1 |
|
|
|
|
||||
|
|
|
|
|
|
|
|
|
|
|
|
|
|
||
5
где ϕ, λ – широта и долгота объекта; vN и vE – северный и восточный ком-
поненты скорости; |
R2 = |
|
a(1 − e) |
|
|
|
|
|
|
|
||||||
|
|
|
|
– радиус кривизны меридиональ- |
||||||||||||
|
|
|
|
|||||||||||||
|
|
|
||||||||||||||
|
|
|
|
|
|
(1 − e 2 sin 2 ϕ)3 |
|
|
|
|
|
|
|
|||
ного эллипса; ΩE – |
|
|
|
|
|
|
|
|
R1 = |
|
a |
|||||
угловая скорость вращения Земли; |
|
|
|
; |
||||||||||||
|
|
|
||||||||||||||
|
||||||||||||||||
|
|
|
|
|
|
|
|
|
|
|
|
|
|
1 − e2 sin 2 ϕ |
||
R1 cos ϕ – радиус параллели; e – |
эксцентриситет земного эллипсоида, причем |
|||||||||||||||
e 2 = |
a 2 − b 2 |
; а и b – |
его полуоси. Выражения для |
R |
и R |
2 |
можно упро- |
|||||||||
|
||||||||||||||||
|
a 2 |
|
|
|
|
|
|
|
|
1 |
|
|
|
|
||
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
стить, разложив их |
в ряд, с |
точностью до e 2 , |
в результате получим |
|||||||||||||
R ≈ a(1 + 0.5e2 sin2 ϕ); |
R = a(1 − e2 +1.5e2 sin2 ϕ) , |
где |
для |
эллипсоида |
||||||||||||
1 |
|
|
|
|
2 |
|
|
|
|
|
|
|
|
|
|
|
Ф. Н. Красовского а = 6 378 245 м, b = 6 356 856 м.
1.2.Порядок выполнения работы
1.Запускается программа MatLab (ярлык программы 
находится на «Рабочем столе»).
2.Загружается Simulink (для этого на панели инструментов MatLab
нажать 
).
3.Создается блок-схема (схема 1), моделирующая показания гироскопов
иакселерометров.
Согласно кинематическим уравнениям для формирования сигналов датчиков необходимо знание априорной информации о движении объекта по поверхности Земли (законы изменения линейной скорости и законы изменения углового положения относительно горизонта и направления на север).
Выбор элементов блок-схемы осуществляется в окне «Simulink Library Browser» (меню «View», пункт «Library browser» или кнопка 
на панели инструментов). Для того чтобы выбранный блок оказался в рабочей области программы Simulink, данный блок выделяется в окне «Simulink Library Browser» и при нажатой правой кнопке мыши перетаскивается в рабочую область.
Двойным нажатием правой кнопки мыши на элементе открывается окно свойств блока «Block Parameters» (рис. 1.2), в котором можно изменять параметры блока.
6
3.1. Формирование скорости и ускорения движения объекта. Закон изменения скорости задается как сумма начального значения скорости и переменной составляющей, определяемой ускорением объекта. Для задания начальной скорости используется блок Constant. В
свойствах блока в строке Value задается значение, например 10, что соответ-
ствует v0 = 10 м/с. Так как ускорение объекта может иметь различный закон изменения, целесообразно использовать блок Signal Builder. При двойном нажатии на блоке появится окно, в котором можно создать график зависимости ускорения от времени (рис. 1.3).
Рис. 1.3. Окно блока Signal Builder
В меню Axes данного окна выбирается пункт Set Y display limits и в появившемся окне задаются максимальное и минимальное значения по оси Y: –0.005 и 0.045 соответственно. Нажать кнопку OK. Затем выбирается пункт Set T display limits и задаются следующие значения по оси T: 0 и 500. Далее
7
формируется закон изменения ускорения (выделяя и перемещая красные точки в окне параметров блока Signal Builder) в виде ступеньки высотой 0.004 и продолжительностью от 0 до 40 с.
Для формирования переменной составляющей скорости сигнал с выхода Signal Builder подается на вход блока Integrator. Сигнал с выхода Integrator подается на один из входов сумматора (блок Sum), на другой вход сумматора подается начальное значение скорости. Примерный вид модели формирования скорости и ускорения объекта представлен на рис. 1.4.
Блок Scope (осциллограф) используется для просмотра результатов, т. е. отображает график зависимости скорости и ускорения объекта от времени.
3.2. Формирование углов курса (K ) , тангажа (ψ) и крена (θ) . В работе предполагается, что курс может быть как постоянным, так и изменяться с течением времени. Поэтому, как и в предыдущем пункте, используются блоки Constant и Signal Builder, сигналы от которых поступают на входы сумматора
(рис. 1.5).
10
0
Рис. 1.4. Модель формирования |
|
скорости и ускорения объекта |
Рис. 1.5. Модель формирования курса |
Параметры блоков Constant и Signal Builder устанавливаются в соответствии с указаниями преподавателя.
На данном этапе можно не задаваться законом изменения курса, для этого между блоками Signal Builder и Sum помещается усилитель (блок Gain), в параметрах которого задается коэффициент усиления, равный нулю.
Углы тангажа и крена определяются случайной качкой объекта. В практической работе качка задается как гармоническая функция. Следовательно,
для моделирования углов ψ, θ необходимо использовать блок SinWave. В
свойствах данного блока задаются амплитуда, частота и начальная фаза колебания (рис. 1.6, а).
8
Если необходимо реализовать колебания относительно некоторого уровня, то сигнал с выхода SinWave подается на один из входов сумматора, а на другой вход подается сигнал с выхода блока Constant, в котором задается значение этого уровня (рис. 1.6, б).
Согласно номеру варианта из таблицы выбираются соответствующие параметры для задания угловой ориентации.
Параметр |
|
1 |
2 |
3 |
4 |
|
Амплитуда, |
|
ψ |
0.10 |
0.09 |
0.20 |
0.08 |
рад |
|
θ |
0.12 |
0.11 |
0.13 |
0.05 |
Частота, |
|
ψ |
0.44 |
3.14 |
5.2 |
1.26 |
рад/с |
|
θ |
0.44 |
2.50 |
6.5 |
1.00 |
Начальная |
|
ψ |
0.1 |
0.2 |
0 |
0.5 |
фаза, рад |
|
θ |
0 |
0.4 |
0.3 |
0.1 |
3.3. Формирование скорости изме-
нения углов курса (Kɺ) , тангажа (ψɺ) и
крена (θɺ) . Скорость изменения углов ориентации объекта определяется как первая производная от соответствующего угла ориентации по времени. В программе Simulink данная операция осуществляется с использованием блока De-
rivative.
Для определения скорости изменения курса сигнал с выхода сумматора (см. рис. 1.5) подается на вход блока Derivative. Аналогичные действия осу-
ществляются для углов ψ, θ.
3.4. Формирование широты места
(ϕ) . Текущая широта места определяется исходной широтой (широтой точки отсчета) с учетом изменения широты, вызванного движением объекта по поверхности Земли.
Значение исходной широты задается в блоке Constant.
а
б
Рис. 1.6. Окно параметров блока SinWave (а); модель формирования угла тангажа (б)
9
Изменение широты зависит от северной составляющей линейной скоро-
сти движения объекта (vN ) . Так как на данном этапе разработки блок-схемы значение vN не может быть определено, то коррекция будет произведена позже.
3.5. Формирование текущего радиуса Земли (R). В работе Земля рассматривается как эллипсоид Ф. Н. Красовского. При этом радиус Земли зависит от широты места и определяется как
|
|
2 |
|
a |
2 |
− b |
2 |
||
|
|
|
|
|
|
||||
R = a 1 |
+ 0.5sin |
|
ϕ |
|
|
a |
2 |
|
, |
|
|
|
|
|
|
|
|
|
|
где a = 6 378 245 м, b = 6 356 856 м – полуоси эллипсоида Красовского.
В программе Simulink для моделирования радиуса Земли необходимо использовать мультиплексор (блок Mux), в свойствах которого задается количество входов (3). На первые два входа мультиплексора подаются посредством блоков Constant значения a, b. На третий вход подается текущее значение широты места (см. п. 3.4). Для удобства используется пара блоков Goto и From. На вход блока Goto подается текущее значение широты, в параметрах блока необходимо написать название, например «fi». Блок From соединяется со входом блока Mux, в свойствах блока From необходимо выбрать соответствующее название блока Goto, т. е. «fi».
Блок Mux формирует вектор, состоящий в данном случае из трех эле-
ментов: u =[a b ϕ].
Сигнал с выхода блока Mux подается на блок Fcn (рис. 1.7). В свойствах последнего вводится
формула для определения радиуса, причем a, b, φ записываются как элементы вектора u, например, a соответствует u(1).
При моделировании показаний датчиков необходимо использовать значения гравитационного ускорения (g = 9.819м/с2 )и угловой скорости су-
|
2p |
|
||
точного вращения Земли W = |
|
|
рад/с . Для этого вводятся два блока |
|
24 × 3600 |
||||
|
|
|||
Constant.
10