Способ навигации подвижного объекта
METHOD OF NAVIGATING MOBILE OBJECT

Изобретение относится к навигации и предназначено для счисления координат (определения пространственного перемещения) подвижного объекта относительно земли. Может использоваться как для автономной работы, так и для работы в комплексе с другими навигационными системами.
Известен способ определения ориентации подвижного объекта по координатам реперных источников (РИ), включающий определение сигналов, соответствующих положению изображения РИ в плоскости фотодетектора, определение углов пеленгов каждого РИ по полученным сигналам с учетом фокусного расстояния объектива и определение координат РИ по данным углов-пеленгов с учетом расстояния между РИ.
Этот способ реализован в устройстве, содержащем реперные источники на подвижном объекте, а на неподвижном - два фотоприемных оптико-локационных блока и блок определения координат РИ и ориентации подвижного объекта, описанный в патенте на изобретение GB №2002986 А, опубл. 28.02.1979.
Недостатком описанного аналога является использование реперных источников с известным расположением на подвижном объекте, а также определение только ориентации подвижного объекта при неизвестном местоположении.
Известен наиболее близкий к заявляемому изобретению способ определения местоположения и угловой ориентации летательного аппарата относительно ВПП, основанный на приеме излучения трех наземных лазерных маяков, установленного на борту ЛА и системы (световых контрастов), описанный в патенте на изобретение RU №2347240, опубл. 20.02.2009. Определяют координаты изображений лазерных маяков на фоточувствительной матрице, а затем вычисляют координаты и угловое положение ЛА относительно ВПП.
Недостатком этого способа является использование наземных источников излучения с известными координатами, что лишает систему автономности.
Технический результат изобретения достигается тем, что в способе навигации подвижного объекта, основанном на фотоэкспонировании земной поверхности фотокамерой, обработке их оцифрованного изображения и вычислении координат местоположения подвижного объекта, осуществляют последовательное фотоэкспонирование земной поверхности фотокамерами стереопары, установленной на подвижном объекте, путем программной обработки изображений земной поверхности выделяют на каждой паре двух последовательных изображений группу, по меньшей мере, из трех особых точек, изображение которых можно устойчиво отличать от изображений всех соседних точек, причем особые точки не лежат на одной прямой, вычисляют их координаты в связанной системе координат, определяют угловое и линейное перемещение подвижного объекта относительно земной поверхности за интервал времени между двумя фотоэкспозициями, определяют угловое и линейное перемещение подвижного объекта за время движения, а также текущие координаты местоположения посредством учета его начального положения
Сущность изобретения заключается в том, что осуществляют последовательное фотоэкспонирование земной поверхности фотокамерами стереопары, установленной на подвижном объекте, путем программной обработки изображений земной поверхности выделяют на каждой паре двух последовательных изображений группу, по меньшей мере, из трех особых точек, изображение которых можно устойчиво отличать от изображений всех соседних точек, причем особые точки не лежат на одной прямой, вычисляют их координаты в связанной системе координат, определяют угловое и линейное перемещение подвижного объекта относительно земной поверхности за интервал времени между двумя фотоэкспозициями, определяют угловое и линейное перемещение подвижного объекта за время движения, а также текущие координаты местоположения посредством учета его начального положения
Сущность изобретения поясняется на фиг. 1 и 2.
n,m
1,m
2,m
m
m+1
1,m
1,m
1,m
2,m
2,m
2,m
1,m
1,m
2,m
2,m
1,m
1,m
2,m
2,m
1
2
1m
2m
n,m
1m+1
2m+1
(m)
(m)
(m)
(m)
(m)
(m)
(m)
(m)
(m)
(m)
(m)
(m)
(m)
На фиг. 1 показаны схема измерения посредством стереопары, которая включает в себя две фотокамеры с параллельными оптическими осями, в процессе определения пространственного перемещения подвижного объекта и группа из трех особых точек P (индекс n=1, 2, 3, … - номер особой точки). Здесь прямоугольная система координат OXYZ, связана с подвижным объектом в m-м пространственном положении (в m-й момент времени), фотоматрицы ФМ1, ФМ2 цифровых фотокамер стереопары расположены в плоскости OYZ, а оптические оси объективов параллельны оси OX. Центры фотоматриц O (О) расположены симметрично относительно начала координат O (O - его положение в m+1-й момент времени) на расстояниях В/2. Системы координат OYZ и OYZ лежат в плоскости фотоматриц, причем оси OY и OY параллельны оси OY, а оси OZ и OZ совпадают с осью OZ. В точках L и L располагаются геометрические центры объективов левой и правой фотокамер, формирующих оптические изображения S и S особой точки земной поверхности P, а в следующий момент времени - S и S.
1,m
2,m
3,m
1,m+1
2,m+1
3,m+1
1,m+1
2,m+1
4,m+1
1,m
2,m+2
4,m+2
3,m+2
На фиг. 2 показана последовательность изображений групп из трех особых точек на первой и второй фотоматрицах стереопары (ФМ1 и ФМ2). Причем для m-го m+1-го изображений выбирается группа особых точек (объединенных сплошной вогнутой линией) Р, Р, Р, (Р, Р, Р), а для m+1-го и m+2-го изображений выбирается группа особых точек (объединенных пунктирной выпуклой линией) P, P, P (P+2, P, P), поскольку особая точка Р вышла за пределы фотоматрицы ФМ2.
Выделение на изображении каждой фотокамеры по меньшей мере трех особых точек земной поверхности может быть выполнено путем программной обработки изображений земной поверхности, которая может осуществляться методами, описанными в статье Девятериков Е.А. Визуальный одометр / Девятериков Е.А., Михайлов Б.Б. // Вестник МГТУ им. Н.Э. Баумана. Сер. "Приборостроение". 2012. С. 68-82, например, методом ускоренной проверки сегментов (FAST) яркости точек, образующих окружность заданного радиуса.
1n,m
1n,m
2n,m
2n,m
n,m
Определение координат Y, Z, Y, Z изображений трех особых точек  на фотоматрицах из положений в моменты времени m-й фотоэкспозиции обеспечивает вычисление координат , ,  n-й особой точки P относительно подвижного объекта в связанной системе координат по следующим формулам:
ln,m
ln,m
2n,m
2n,m
n,m
где Y, Z, Y, Z - координаты изображений особых точек P на фоточувствительных матрицах, первый индекс обозначает номер фоточувствительной матрицы; F - фокусное расстояние фотообъектива; В - расстояние между первой и второй фотокамерами.
(m+1)
(m+1)
(m+1)
(m+1)
(m)
(m)
(m)
(m)
(m)
(m)
(m)
(m)
Положение системы координат OХYZ, связанной с подвижным объектом, относительно ее предыдущего положения OXYZзадается вектором смещения начала координат , а изменение углового положения системы OXYZ - матрицей направляющих косинусов
m
m
m
(m)
(m)
(m)
(m)
(m)
(m)
где β, φ, α - углы последовательных разворотов подвижного объекта вокруг осей OY, OZ, OХ соответственно за время между двумя последовательными фотоэкспозициями.
Запишем соотношение, связывающее измеряемые векторы
(m+1)
(m+1)
(m+1)
(m+1)
(m)
(m)
(m)
(m)
и в системах OXYZ и OXYZ соответственно
(m)
(m)
(m)
(m)
(m)
n,m
где  - вектор с началом в точке O, а конец в точке P, отсчитанный в системе координат OXYZ, а также очевидное векторное равенство, связывающее m и m+1 экспозиции
(m+1)
(m+l)
(m+1)
(m+l)
Поскольку вектор  измерить невозможно (методом прямых измерений), то для того чтобы воспользоваться равенством (3), необходимо выразить этот вектор в проекциях на оси системы координат OXYZ, получим
m
m
m
Полученное соотношение содержит шесть неизвестных величин β, φ, α, , , , а также векторы  и , доступные для прямых измерений, и для совокупности трех особых точек представляет собой систему 9 нелинейных алгебраических уравнений, которая может быть решена применением специальных численных методов, использующихся для таких переопределенных систем.
Чтобы избежать этого, найдем аналитическое решение задачи. Применим соотношение (4) для трех особых точек земной поверхности, на положение которых накладывается следующее условие, они образуют треугольник ненулевой площади (не лежат на одной прямой):
Вычитая из второго соотношения первое, а из третьего второе, получим
Запишем также соотношение, описывающее преобразование координат вектора, равного векторному произведению этих векторов, поскольку для нашей цели необходим вектор, не являющийся линейной комбинацией выбранных векторов
Три последних соотношения используем для записи матричного соотношения, в котором матрицы-столбцы занимают место столбцов в блочных матрицах, такая запись следует из правила умножения матриц
m
m
Из последнего матричного равенства выразим искомую матрицу (A) путем умножения справа обеих частей равенства на матрицу, обратную сомножителю матрицы (A). Отметим, что определитель этой матрицы не равен нулю, поскольку три вектора, координаты которых записаны в столбцах матрицы, являются линейно не зависимыми и, следовательно, обратная матрица существует
m
Поскольку все координаты векторов в выражении матрицы (A) непосредственно измеряются с помощью СТЗ в моменты m и m+1 экспозиций, то соотношение (6) используется для вычисления углового перемещения подвижного объекта относительно земной поверхности за интервал времени между двумя фотоэкспозициями. Для определения вектора линейного перемещения подвижного объектавоспользуемся соотношением (5) при n=1
Матрица, описывающая вращательное движение подвижного объекта из начального в конечное положение, записывается в виде произведения матриц, описывающих вращения за интервалы времени между двумя последовательными фотоэкспозициями
0
0
0
0
0
0
Для определения координат подвижного объекта необходимо учесть начальное угловое и пространственное его положение, которое зададим географическими координатами: ϕ - широтой, λ - долготой, R - удалением от центра земли, а его угловое положение относительно земли углами курса - ψ, крена - γ и тангажа - υ, тогда конечное угловое положение подвижного объекта определим посредством матрицы
где
- матрица вида (1), определяющая начальное угловое положение подвижного объекта.
Задача определения начального положения подвижного объекта в навигации известна как начальная выставка системы, для ее осуществления используется несколько методов, в том числе метод, описанный в патенте на изобретение RU №2347240, опубл. 20.02.2009, с использованием бортовой цифровой фотокамеры и трех наземных лазерных маяков.
Находим приращения географических координат за один m-й интервал времени между последовательными фотоэкспозициями, а путем суммирования выразим текущие координаты подвижного объекта в m-й (текущий) момент, при этом учтем начальное местоположение объекта. Вычисляем текущие координаты местоположения подвижного объекта по следующим формулам:
a
ij
где  - элементы матрицы направляющих косинусов вида (9), определяемые текущими углами крена, курса и тангажа подвижного объекта.
Таким образом, решение задачи об определении поступательного и углового перемещения подвижного объекта относительно земли основано на измерении координат изображений по крайней мере трех особых точек земной поверхности, фотоэкспозиция которых осуществляется из последовательных точек траектории и вычислении на основе этих измерений его пространственного перемещения. Отличие такого подхода от упомянутого в статье Девятериков Е.А. Визуальный одометр / Девятериков Е.А., Михайлов Б.Б. // Вестник МГТУ им. Н.Э. Баумана. Сер. "Приборостроение". 2012. С. 68-82, состоит в счислении угловых и линейных координат местоположения подвижного объекта, а не пройденного пути и угла курса.