Способ измерения положения дорожно-строительной машины

Способ измерения положения дорожно-строительной машины

Иллюстрации

Показать все

Реферат

Изобретение относится к навигации и может быть использовано для автоматического управления движением дорожно-строительной машины (ДСМ).

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

Оптико-локационные блоки определяют направления на отдельные РИ (углы-пеленги РИ), используя которые определяют ориентацию подвижных объектов. В ОЛБ широко используются двумерные анализаторы плоских изображений, например, на основе матричных фотодетекторов в сочетании с дальномерным устройством [1]. В классической стереоскопической схеме построения устройств определения ориентации подвижных объектов измеряют углы-пеленги РИ с двух точек, разнесенных на известное расстояние, затем определяют координаты РИ и ориентацию подвижного объекта.

Известен способ [2] локации источников излучения, описанный в патенте на изобретение GB №2002986 A, опубл. 28.02.1979., размещаемых на подвижных объектах, в котором предусматривается использование двух цилиндрических объективов-анаморфотов для формирования изображений РИ в плоскости линейки фотодетекторов. Определив координату изображения РИ и зная фокусное расстояние цилиндрического объектива, определяют угол-пеленг РИ. Используя полученные данные, зная расстояние между формирователями изображения (точками, относительно которых производятся отсчеты углов-пеленгов), вычисляют координаты РИ.

Измерение ориентации подвижного объекта производится в три этапа:

определение углов-пеленгов;

вычисление координат РИ;

вычисление ориентации подвижного объекта с использованием полученных значений координат РИ.

Недостатком такого способа является размещение ОЛБ на неподвижном основании (что соответствует размещению на земле применительно к задаче определения положения ДСМ относительно автотрассы), а также недостаточная информативность, связанная с измерением только угловых координат подвижного объекта.

Известен способ [3] определения местоположения и углов ориентации летательного аппарата (ЛА) относительно взлетно-посадочной полосы (ВПП), описанный в патенте на изобретение РФ №2378664, опубл. 10.01.2010., основанный на формировании области излучения посадочной траектории, приеме сигналов от источников излучения и определении местоположения ЛА относительно ВПП, в качестве источников излучения используют два одинаковых лазерных маяка, регистрируют излучение каждого из двух лазерных маяков с известными координатами, установленных вдоль ВПП на полосах безопасности, посредством двух разнесенных оптико-локационных блоков, установленных на борту ЛА и выполненных каждый в виде плоской фотоматрицы, размещенной в фокальной плоскости фотообъектива, осуществляют обработку оцифрованного изображения, снимаемого с фотоматрицы для определения координат изображений лазерных маяков, вычисляют координаты двух лазерных маяков относительно ЛА.

Недостатками этого способа являются:

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

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

Единый технический результат изобретения достигается тем, что в способе определения местоположения и углов ориентации ДСМ относительно профиля автотрассы, основанном на приеме сигналов от источников излучения и определении местоположения ДСМ относительно профиля автотрассы, в качестве источников излучения используют три лазерных маяка, регистрируют излучение каждого из трех лазерных маяков с известными координатами, установленных вдоль профиля автотрассы, посредством двух разнесенных цифровых фотокамер, установленных на раме ДСМ и выполненных в виде фоточувствительных матриц (ФМ), размещенных в фокальной плоскости фотообъектива, осуществляют обработку оцифрованного изображения, снимаемого с фоточувствительных матриц для определения координат изображений лазерных маяков, вычисляют координаты трех лазерных маяков относительно ДСМ,

X O M i = F ( B Z 2 i − Z 1 i + 1 ) ,   Y O M i = Y 1 i B Z 1 i − Z 2 i , Z O M i = B 2 Z 1 i + Z 2 i Z 1 i − Z 2 i ,

где Y_1i, Z_1i, Y_2i, Z_2i - координаты изображений S_1i, S_2i лазерных маяков M_i на фоточувствительной матрице, первый индекс обозначает номер фоточувствительной матрицы, второй индекс i=1…3 - номер лазерного маяка, B - расстояние между центрами фоточувствительных матриц, F - фокусное расстояние фотообъективов,

вычисляют матрицу

где X M 1 ( 1 ) , Y M 1 ( 1 ) , Z M 1 ( 1 ) , X M 2 ( 1 ) , Y M 2 ( 1 ) , Z M 2 ( 1 ) , X M 3 ( 1 ) , Y M 3 ( 1 ) , Z M 3 ( 1 ) - координаты лазерных маяков в системе O⁽¹⁾ X⁽¹⁾ Y⁽¹⁾ Z⁽¹⁾, α_nm - коэффициенты матрицы (A) (n, m=1…3 номера строк и столбцов),

вычисляют углы ψ, υ, γ ориентации дорожно-строительной машины относительно автотрассы

ψ=-arctg(α₁₃/α₁₁),

υ=arcsinα₁₂,

γ=-arctg(α₃₂/α₂₂),

вычисляют координаты X 0 ( 1 ) , Y 0 ( 1 ) , Z 0 ( 1 ) дорожно-строительной машины относительно автотрассы

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

определение координат изображений трех лазерных маяков на фоточувствительных матрицах;

вычисление координат трех лазерных маяков в системе координат, связанной с ДСМ;

вычисление значений углов ориентации ДСМ относительно автотрассы; вычисление координат ДСМ относительно автотрассы. На фиг.1 показана схема измерения положения ДСМ относительно автотрассы.

Способ реализуется следующим образом. Пусть горизонтальная система координат O⁽¹⁾X⁽¹⁾Y⁽¹⁾Z⁽¹⁾ связана с профилем автотрассы (фиг.1), точка O⁽¹⁾ совпадает с направлением движения ДСМ, ось O⁽¹⁾X⁽¹⁾ - с осью автотрассы, ось O⁽¹⁾Y⁽¹⁾ - нормаль к поверхности профиля автотрассы. В окрестности профиля автотрассы установлено три одинаковых лазерных маяка M₁, M₂ и M₃, координаты которых X M 1 ( 1 ) , Y M 1 ( 1 ) , Z M 1 ( 1 ) , X M 2 ( 1 ) , Y M 2 ( 1 ) , Z M 2 ( 1 ) , X M 3 ( 1 ) , Y M 3 ( 1 ) , Z M 3 ( 1 ) в системе O⁽¹⁾X⁽¹⁾Y⁽¹⁾Z⁽¹⁾ известны.

Система координат OXYZ связана с системой технического зрения (СТЗ), которая, в свою очередь, установлена на ДСМ, точка O совпадает с центром отрезка, соединяющего геометрические центры ФМ, ось OX (продольная ось ДСМ) направлена параллельно оптическим осям фотообъективов, а ось OZ (поперечная ось ДСМ) совпадает с линией, проходящей через геометрические центры ФМ, ось OY добавляет другие оси до правой системы координат. Известны расстояние B между центрами ФМ и фокусные расстояния фотообъективов F₁=F₂=F.

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

X O M i = F ( B Z 2 i − Z 1 i + 1 ) ,   Y O M i = Y 1 i B Z 1 i − Z 2 i ,   Z O M i = B 2 Z 1 i + Z 2 i Z 1 i − Z 2 i ,                           ( 1 )

где Y_1i, Z_1i, Y_2i, Z_2i - координаты изображений S_1i, S_2i лазерных маяков M_i на фоточувствительной матрице, первый индекс обозначает номер фоточувствительной матрицы, второй индекс i=1…3 - номер лазерного маяка.

Рассмотрим очевидное векторное равенство

O ( 1 ) O → = O ( 1 ) M → + M O → = O ( 1 ) M → − O M → .

Перепишем это равенство в матричной форме в проекциях на оси системы координат O⁽¹⁾X⁽¹⁾Y⁽¹⁾Z⁽¹⁾.

( X 0 ( 1 ) Y 0 ( 1 ) Z 0 ( 1 ) ) T = ( X O ( 1 ) M i ( 1 ) Y O ( 1 ) M i ( 1 ) Z O ( 1 ) M i ( 1 ) ) T − ( A ) ( X O M i Y O M i Y O M i ) T ,             ( 2 )

где вектор ( X 0 ( 1 ) Y 0 ( 1 ) Z 0 ( 1 ) ) T     , характеризующий положение ДСМ в системе координат O⁽¹⁾X⁽¹⁾Y⁽¹⁾Z⁽¹⁾, матрица направляющих косинусов

( A ) = ( cos ψ cos υ sin υ − sin ψ cos υ sin ψ sin γ − sin υ cos ψ cos γ cos γ cos υ cos ψ sin γ + sin ψ sin υ cos γ sin ψ cos γ + c o p s ψ sin υ sin γ − cos υ sin γ cos ψ Л cos γ − sin ψ sin υ sin γ ) - матрица преобразования координат, углы ψ, υ, γ (при горизонтальном расположении плоскости O⁽¹⁾X⁽¹⁾Y⁽¹⁾ совпадают с углами поворота относительно вертикальной, продольной и поперечной оси ДСМ).

Применим соотношение (2) для трех маяков

( X 0 ( 1 ) Y 0 ( 1 ) Z 0 ( 1 ) ) T = ( X O ( 1 ) M 1 ( 1 ) Y O ( 1 ) M 1 ( 1 ) Z O ( 1 ) M 1 ( 1 ) ) T − ( A ) ( X O M 1 Y O M 1 Y O M 1 ) T ,

( X 0 ( 1 ) Y 0 ( 1 ) Z 0 ( 1 ) ) T = ( X O ( 1 ) M 2 ( 1 ) Y O ( 1 ) M 2 ( 1 ) Z O ( 1 ) M 2 ( 1 ) ) T − ( A ) ( X O M 2 Y O M 2 Y O M 2 ) T ,

( X 0 ( 1 ) Y 0 ( 1 ) Z 0 ( 1 ) ) T = ( X O ( 1 ) M 3 ( 1 ) Y O ( 1 ) M 3 ( 1 ) Z O ( 1 ) M 3 ( 1 ) ) T − ( A ) ( X O M 3 Y O M 3 Y O M 3 ) T .

Совокупность трех последних векторных равенств представляет собой систему 9 нелинейных алгебраических уравнений относительно шести неизвестных X 0 ( 1 ) Y 0 ( 1 ) Z 0 ( 1 ) , ψ, υ, γ, то есть их следует отнести к переопределенным. Решение таких систем находят специальными численными методами, но для бортового алгоритма этот путь не является наилучшим. Поэтому воспользуемся тем, что наши уравнения имеют особые свойства, в частности их структура определяется присутствием матрицы направляющих косинусов (A) и это позволяет найти аналитическое решение системы.

Вычитая из второго соотношения первое, а из третьего второе, получим

( A ) ( X O M 2 − X O M 1 Y O M 2 − Y O M 1 Z O M 2 − Z O M 1 ) T = = ( X O ( 1 ) M 2 ( 1 ) − X O ( 1 ) M 1 ( 1 ) Y O ( 1 ) M 2 ( 1 ) − Y O ( 1 ) M 1 ( 1 ) Z O ( 1 ) M 2 ( 1 ) − Z O ( 1 ) M 1 ( 1 ) ) T ,

( A ) ( X O M 3 − X O M 2 Y O M 3 − Y O M 2 Z O M 3 − Z O M 2 ) T = = ( X O ( 1 ) M 3 ( 1 ) − X O ( 1 ) M 2 ( 1 ) Y O ( 1 ) M 3 ( 1 ) − Y O ( 1 ) M 2 ( 1 ) Z O ( 1 ) M 3 ( 1 ) − Z O ( 1 ) M 2 ( 1 ) ) T .

Запишем также соотношение, описывающее преобразование координат вектора равного векторному произведению этих векторов

( A ) ( ( Y O M 2 − Y O M 1 ) ( Z O M 3 − Z O M 2 ) − − ( Y O M 3 − Y O M 2 ) ( Z O M 3 − Z O M 2 ) ( X O M 3 − X O M 2 ) ( Z O M 2 − Z O M 1 ) − − ( X O M 2 − X O M 1 ) ( Z O M 3 − Z O M 2 ) ( X O M 2 − X O M 1 ) ( Y O M 3 − Y O M 2 ) − − ( X O M 3 − X O M 2 ) ( Y O M 2 − Y O M 1 ) ) = ( ( Y O M 2 − Y O M 1 ) ( Z O M 3 − Z O M 2 ) − − ( Y O M 3 − Y O M 2 ) ( Z O M 3 − Z O M 2 ) ( X O M 3 − X O M 2 ) ( Z O M 2 − Z O M 1 ) − − ( X O M 2 − X O M 1 ) ( Z O M 3 − Z O M 2 ) ( X O M 2 − X O M 1 ) ( Y O M 3 − Y O M 2 ) − − ( X O M 3 − X O M 2 ) ( Y O M 2 − Y O M 1 ) ) .

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

( A ) ( X O M 2 − X O M 1 X O M 3 − X O M 2 ( Y O M 2 − Y O M 1 ) ( Z O M 3 − Z O M 2 ) − − ( Y O M 3 − Y O M 2 ) ( Z O M 3 − Z O M 2 ) Y O M 2 − Y O M 1 Y O M 3 − Y O M 2 ( X O M 3 − X O M 2 ) ( Z O M 2 − Z O M 1 ) − − ( X O M 2 − X O M 1 ) ( Z O M 3 − Z O M 2 ) Z O M 2 − Z O M 1 Z O M 3 − Z O M 2 ( X O M 2 − X O M 1 ) ( Y O M 3 − Y O M 2 ) − − ( X O M 3 − X O M 2 ) ( Y O M 2 − Y O M 1 ) ) = = ( X O ( 1 ) M 2 ( 1 ) − X O ( 1 ) M 1 ( 1 ) X O ( 1 ) M 3 ( 1 ) − X O ( 1 ) M 2 ( 1 ) ( Y O ( 1 ) M 2 ( 1 ) − Y O ( 1 ) M 1 ( 1 ) ) ( Z O ( 1 ) M 3 ( 1 ) − Z O ( 1 ) M 2 ( 1 ) ) − − ( Y O ( 1 ) M 3 ( 1 ) − Y O ( 1 ) M 2 ( 1 ) ) ( Z O ( 1 ) M 3 ( 1 ) − Z ( 1 ) O M 2 ( 1 ) ) Y O ( 1 ) M 2 ( 1 ) − Y O ( 1 ) M 1 ( 1 ) Y O ( 1 ) M 3 ( 1 ) − Y O ( 1 ) M 2 ( 1 ) ( X O ( 1 ) M 3 ( 1 ) − X O ( 1 ) M 2 ( 1 ) ) ( Z O ( 1 ) M 2 ( 1 ) − Z O ( 1 ) M 1 ( 1 ) ) − − ( X O ( 1 ) M 2 ( 1 ) − X O ( 1 ) M 1 ( 1 ) ) ( Z O ( 1 ) M 3 ( 1 ) − Z O ( 1 ) M 2 ( 1 ) ) Z O ( 1 ) M 2 ( 1 ) − Z O ( 1 ) M 1 ( 1 ) Z O ( 1 ) M 3 (