Способ определения положения транспортного средства относительно пунктирной линии дорожной разметки

Способ определения положения транспортного средства относительно пунктирной линии дорожной разметки

Иллюстрации

Показать все

Реферат

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

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

Известен способ определения положения контролируемого объекта или отдельных его точек в трехмерном пространстве [1], относящийся к системам технического зрения (СТЗ). Способ основан на трехмерной локации точки с помощью трех или двух измерительных систем, оптические оси которых направлены вдоль осей ортогональной системы координат (X, Y, Z) и пересекаются в ее начале (фиг.1).

Используя, например, два ОЛБ с системами отсчета координат внутри каждого, можно определить координаты точки А путем решения следующей системы уравнений:

x A = α x 1 ( − d 1 + α x 3 d 3 ) / ( 1 + α x 3 α x 1 ) ,   … …   С Т З   1

y A = α x 3 ( d 3 + α x 1 d 1 ) / ( 1 + α x 3 α x 1 ) ,   … …   С Т З   3

z A = − α y 3 ( d 3 + α x 1 d 1 ) / ( 1 + α x 3 α x 1 ) ,   … …   С Т З   3

z A = − α y 1 ( − d 1 + α x 3 d 3 ) / ( 1 + α x 3 α x 1 ) ,   … …   С Т З   1 ,

где α_xj - угловая координата точки A, отсчитанная от оси x в j-м ОЛБ; α_yj - угловая координата точки A, отсчитанная от оси y в j-м ОЛБ; d_j - расстояние до начала координат от передней узловой точки фотообъектива j-го ОЛБ.

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

Известен наиболее близкий по технической сущности к изобретению способ, описанный в [2], относящийся к системам дистанционного определения координат подвижных объектов.

Способ определения положения транспортного средства относительно линии дорожной разметки, основан на трехмерной локации точки с помощью оптико-локационного блока, измерении расстояния от линии дорожной разметки до транспортного средства и угла отклонения продольной оси транспортного средства относительно линии дорожной разметки. Посредством обработки оцифрованных изображений поверхности автодороги в поле изображения в одноименных вершинах штриховой линии дорожной разметки выбирают две идентичные точки R₁, R₂, ближайшие к транспортному средству, определяют координаты этих точек на фоточувствительных матрицах (Y_υ, Z_υ), м, где υ=1, 2 номер точки R₁, R₂ соответственно, вычисляют боковое отклонение от пунктирной линии дорожной разметки

Z б = h ⋅ ( Y 1 Z 2 − Y 2 Z 1 ) sin α + F ( Z 2 − Z 1 ) cos α [ ( Y 1 Z 2 − Y 2 Z 1 ) cos α ] 2 + [ F ( Z 2 − Z 1 ) sin α ] 2 + F 2 ( Y 1 − Y 2 ) 2 ,

где F - фокусное расстояние фотообъектива, h - высота расположения ОЛБ от дорожного полотна, α - угол наклона оптической оси ОЛБ от продольной оси транспортного средства вокруг его поперечной оси, вычисляют угол между продольной осью транспортного средства и линией дорожной разметки

ϕ = arccos F ( Y 1 − Y 2 ) [ F ( Y 1 − Y 2 ) ] 2 + [ ( Y 1 Z 2 − Y 2 Z 1 ) cos α − F ( Z 2 − Z 1 ) sin α ] 2 .

Недостатком описанного прототипа является методическая погрешность измерения параметров положения транспортных средств относительно линии дорожной разметки, обусловленная наличием в соотношениях α - угла наклона оптической оси ОЛБ от продольной оси транспортного средства вокруг его поперечной оси, что может привести к неустойчивости управления транспортным средством.

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

Технический результат изобретения достигается тем, что в способе определения положения транспортного средства относительно линии дорожной разметки, заключающемся в измерении отклонения продольной оси транспортного средства относительно линии дорожной разметки и расстояния от линии разметки до транспортного средства, измеряют боковое отклонение транспортного средства от пунктирной линии дорожной разметки и угол отклонения продольной оси транспортного средства от пунктирной линии дорожной разметки посредством системы технического зрения, включающей первую и вторую разнесенные цифровые фотокамеры, для этого осуществляют обработку оцифрованных изображений поверхности автодороги, снимаемых с фотоматриц первой и второй цифровых фотокамер, выбирают в поле изображения две точки в одноименных вершинах пунктирных линий дорожной разметки и одну оптически контрастную точку, не лежащую на пунктирной линии дорожной разметки, определяют координаты этих точек на фотоматрицах (y_1i, z_1i), (y_2i, z_2i), м, где первый индекс принимает значения 1, 2 - номер фотоматрицы, а второй индекс i=1, 2, 3 номер точки B₁, B₂, B₃ соответственно, вычисляют координаты точек B_i в системе координат OXYZ, связанной с транспортным средством

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

где F - фокусное расстояние первого и второго фотообъективов, м;

B - расстояние между фотоматрицами, м,

вычисляют боковое отклонение транспортного средства от линии дорожной разметки

Z б = { [ ( X 3 − X 1 ) ( Z 3 − Z 2 ) − ( X 3 − X 2 ) ( Z 3 − Z 1 ) ] ( Z 2 − Z 1 ) − − [ ( X 3 − X 2 ) ( Y 3 − Y 1 ) − ( X 3 − X 1 ) ( Y 3 − Y 2 ) ] ( Y 2 − Y 1 ) } ( X 3 − X 1 ) / | E ¯ | + + { [ ( X 3 − X 2 ) ( Y 3 − Y 1 ) − ( X 3 − X 1 ) ( Y 3 − Y 2 ) ] ( X 2 − X 1 ) − − [ ( Y 3 − Y 2 ) ( Z 3 − Z 1 ) − ( Y 3 − Y 1 ) ( Z 3 − Z 2 ) ] ( Z 2 − Z 1 ) } ( Y 3 − Y 1 ) / | E ¯ | + + { [ ( Y 3 − Y 2 ) ( Z 3 − Z 1 ) − ( Y 3 − Y 1 ) ( Z 3 − Z 2 ) ] ( Y 2 − Y 1 ) − − [ ( X 3 − X 1 ) ( Z 3 − Z 2 ) − ( X 3 − X 2 ) ( Z 3 − Z 1 ) ] ( X 2 − X 1 ) } ( Z 3 − Z 1 ) / | E ¯ |

где | E ¯ | = { [ ( X 3 − X 1 ) ( Z 3 − Z 2 ) − ( X 3 − X 2 ) ( Z 3 − Z 1 ) ] ( Z 2 − Z 1 ) − − [ ( X 3 − X 2 ) ( Y 3 − Y 1 ) − ( X 3 − X 1 ) ( Y 3 − Y 2 ) ] ( Y 2 − Y 1 ) } 2 ( X 3 − X 1 ) 2 + + { [ ( X 3 − X 2 ) ( Y 3 − Y 1 ) − ( X 3 − X 1 ) ( Y 3 − Y 2 ) ] ( X 2 − X 1 ) − − [ ( Y 3 − Y 2 ) ( Z 3 − Z 1 ) − ( Y 3 − Y 1 ) ( Z 3 − Z 2 ) ] ( Z 2 − Z 1 ) } 2 ( Y 3 − Y 1 ) 2 + + { [ ( Y 3 − Y 2 ) ( Z 3 − Z 1 ) − ( Y 3 − Y 1 ) ( Z 3 − Z 2 ) ] ( Y 2 − Y 1 ) − − [ ( X 3 − X 1 ) ( Z 3 − Z 2 ) − ( X 3 − X 2 ) ( Z 3 − Z 1 ) ] ( X 2 − X 1 ) } 2 ( Z 3 − Z 1 ) 2

вычисляют угол ψ отклонения продольной оси транспортного средства от пунктирной линии дорожной разметки

ψ = arcsin Z 2 − Z 1 ( X 2 − X 1 ) 2 + ( Y 2 − Y 1 ) 2 + ( Z 2 − Z 1 ) 2 .

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

выбор в поле изображения двух идентичных точек в одноименных вершинах пунктирной линии дорожной разметки, ближайших к транспортному средству, а также одной оптически контрастной точки, не лежащей на пунктирной линии дорожной разметки (фиг.3);

определение координат этих точек на фотоматрицах;

вычисление координат этих точек в системе координат OXYZ, связанной с транспортным средством;

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

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

На фиг.1 приведена схема трехмерной локации точки с помощью трех или двух измерительных систем, оптические оси которых направлены вдоль осей ортогональной системы координат (аналог).

На фиг.2 изображена схема измерения параметров положения транспортных средств относительно линии дорожной разметки посредством оптико-локационного блока(прототип).

На фиг.3 изображены пунктирные линии дорожной разметки, идентичные точки B₁, B₂, B₃, и система координат OXYZ, связанная с транспортным средством.

Способ осуществляется следующим образом.

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

Выберем в поле изображения дорожного полотна две разнесенные идентичные точки B₁, B₂, лежащие в одноименных вершинах одной пунктирной линии дорожной разметки и третью оптически контрастную точку B₃, не лежащую на одной прямой с линией B₁B₂ (например, лежащую на другой пунктирной линии дорожной разметки) (фиг.3). Выбор точек в поле изображения может осуществляться, например, по контурам пунктирных линий дорожной разметки методом оконтуривания [1]. Координаты этих точек на фотоматрицах обозначим (y_1i, z_1i), (y_2i, z_2i), где первый индекс 1, 2 - номер фотоматрицы, а второй индекс i=1, 2, 3 - номер точки B₁, B₂, B₃ соответственно.

Ось OX совпадает с продольной осью транспортного средства, OZ - поперечная ось.

Вычислим координаты точек B₁, B₂, B₃ в системе координат OXYZ, связанной с транспортным средством.

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

где F - фокусное расстояние первого и второго фотообъективов, м; B - расстояние между фотоматрицами, м.

Решим задачу по определению бокового отклонения точки О от пунктирной линии дорожной разметки B₁B₃ (фиг.3).

Найдем расстояние OO` от начала координат до плоскости П, проходящей через линию B<sub>1</sub>B<sub>2</sub>, перпендикулярной плоскости дороги, заданной точками B<sub>1</sub>, B<sub>2</sub>, B<sub>3</sub>. Вектор E ¯ коллинеарный вектору O O ` ¯ найдем следующим образом

E ¯ = | i j k X 3 − X 2 Y 3 − Y 2 Z 3 − Z 2 X 3 − X 1 Y 3 − Y 1 Z 3 − Z 1 | × [ i ( X 2 − X 1 ) + j ( Y 2 − Y 1 ) + k ( Z 2 − Z 1 ) ] = = | i j k ( Y 3 − Y 2 ) ( Z 3 − Z 1 ) − − ( Y 3 − Y 1 ) ( Z 3 − Z 2 ) ( X 3 − X 1 ) ( Z 3 − Z 2 ) − − ( X 3 − X 2 ) ( Z 3 − Z 1 ) ( X 3 − X 2 ) ( Y 3 − Y 1 ) − − ( X 3 − X 1 ) ( Y 3 − Y 2 ) X 2 − X 1 Y 2 − Y 1 Z 2 − Z 1 | = = i { [ ( X 3 − X 1 ) ( Z 3 − Z 2 ) − ( X 3 − X 2 ) ( Z 3 − Z 1 ) ] ( Z 2 − Z 1 ) − [ ( X 3 − X 2 ) ( Y 3 − Y 1 ) − ( X 3 − X 1 ) ( Y 3 − Y 2 ) ] ( Y 2 − Y 1 ) } + + j { [ ( X 3 − X 2 ) ( Y 3 − Y 1 ) − ( X 3 − X 1 ) ( Y 3 − Y 2 ) ] ( X 2 − X 1 ) − [ ( Y 3 − Y 2 ) ( Z 3 − Z 1 ) − ( Y 3 − Y 1 ) ( Z 3 − Z 2 ) ] ( Z 2 − Z 1 ) } + + k { [ ( Y 3 − Y 2 ) ( Z 3 − Z 1 ) − ( Y 3 − Y 1 ) ( Z 3 − Z 2 ) ] ( Y 2 − Y 1 ) − [ ( X 3 − X 1 ) ( Z 3 − Z 2 ) − ( X 3 − X 2 ) ( Z 3 − Z 1 ) ] ( X 2 − X ) }

Длину вектора O O ` ¯ найдем как проекцию вектора B 1 B 3 ¯ на единичный вектор E ¯ / | E ¯ |

Z б = | O O ' ¯ | = ( E ¯ ⋅ B 1 B 3 ¯ ) / | E ¯ | = { [ ( X 3 − X 1 ) ( Z 3 − Z 2 ) − ( X 3 − X 2 ) ( Z 3 − Z 1 ) ] ( Z 2 − Z 1 ) − − [ ( X 3 − X 2 ) ( Y 3 − Y 1 ) − ( X 3 − X 1 ) ( Y 3 − Y 2 ) ] ( Y 2 − Y 1 ) } ( X 3 − X 1 ) / | E ¯ | + + { [ ( X 3 − X 2 ) ( Y 3 − Y 1 ) − ( X 3 − X 1 ) ( Y 3 − Y 2 ) ] ( X 2 − X 1 ) − − [ ( Y 3 − Y 2 ) ( Z 3 − Z 1 ) − ( Y 3 − Y 1 ) ( Z 3 − Z 2 ) ] ( Z 2 − Z 1 ) } ( Y 3 − Y 1 ) / | E ¯ | + + { [ ( Y 3 − Y 2 ) ( Z 3 − Z 1 ) − ( Y 3 − Y 1 ) ( Z 3 − Z 2 ) ] ( Y 2 − Y 1 ) − − [ ( X 3 − X 1 ) ( Z 3 − Z 2 ) − ( X 3 − X 2 ) ( Z 3 − Z 1 ) ] ( X 2 − X 1 ) } ( Z 3 − Z 1 ) / | E ¯ |

где | E ¯ | = { [ ( X 3 − X 1 ) ( Z 3 − Z 2 ) − ( X 3 − X 2 ) ( Z 3 − Z 1 ) ] ( Z 2 − Z 1 ) − − [ ( X 3 − X 2 ) ( Y 3 − Y 1 ) − ( X 3 − X 1 ) ( Y 3 − Y 2 ) ] ( Y 2 − Y 1 ) } 2 ( X 3 − X 1 ) 2 + + { [ ( X 3 − X 2 ) ( Y 3 − Y 1 ) − ( X 3 − X 1 ) ( Y 3 − Y 2 ) ] ( X 2 − X 1 ) − − [ ( Y 3 − Y 2 ) ( Z 3 − Z 1 ) − ( Y 3 − Y 1 ) ( Z 3 − Z 2 ) ] ( Z 2 − Z 1 ) } 2 ( Y 3 − Y 1 ) 2 + + { [ ( Y 3 − Y 2 ) ( Z 3 − Z 1 ) − ( Y 3 − Y 1 ) ( Z 3 − Z 2 ) ] ( Y 2 − Y 1 ) − − [ ( X 3 − X 1 ) ( Z 3 − Z 2 ) − ( X 3 − X 2 ) ( Z 3 − Z 1 ) ] ( X 2 − X 1 ) } 2 ( Z 3 − Z 1 ) 2

Координата Z_б может быть использована для управления боковым движением транспортного средства в автоматическом режиме, с целью стабилизации его положения относительно пунктирной линии дорожной разметки. Косинус угла ψ найдем как скалярное произведение единичного вектора B 1 B 2 ¯ | B 1 B 2 ¯ | и орта k

cos ( π 2 − ψ ) = sin ψ = [ i ( X 2 − X 1 ) + j ( Y 2 − Y 1 ) + k ( Z 2 − Z 1 ) ] ⋅ k ( X 2 − X 1 ) 2 + ( Y 2 − Y 1 ) 2 + ( Z 2 − Z 1 ) 2 = Z 2 − Z 1 ( X 2 − X 1 ) 2 + ( Y 2 − Y 1 ) 2 + ( Z 2 − Z 1 ) 2

откуда

ψ = arcsin Z 2 − Z 1 ( X 2 − X 1 ) 2 + ( Y 2 − Y 1 ) 2 + ( Z 2 − Z 1 ) 2