Устройство управления самоходным средством

 

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

COIO3 СОВЕТСКИХ

СОЦИАЛИСТИЧЕСКИХ

РЕСПУБЛИК (я)ю G 05 О 1/02

ГОСУДАРСТВЕННЫЙ КОМИТЕТ

ПО ИЗОБРЕТЕНИЯМ И ОТКРЫТИЯМ

ПРИ ГКНТ СССР

ОПИСАНИЕ ИЗОБРЕТЕНИЯ

К АВТОРСКОМУ СВИДЕТЕЛЬСТВУ (21) 4605564/24 (22) 14,11.88 (46) 07.11,91. Бюл. М 41 (71) Ленинградский институт текстильной и легкой промышленности им, С.M.Кирова (72) Е,Н.Ковалев, Э,О.Девликанов и Э,А,Толкачев (53) 62.50(088.8) (56) Авторское свидетельство СССР

М 1037216, кл. G 05 0 1/03, 1983.

Авторское свидетельство СССР

% 1317401, кл. G 05 D 1/00, 1987, Роботизация процесса окончательной обработки бетонных плит, Араи Казахико

Кидзима кэмсоцу Гидзюцу кзнкюдзе немпо, Annu Rept KaJlma inst, Constr. Technol.—

1986, 34, 101 — 106.

Рж "Промышленные роботы и манипуляторы", 1987, М 11, с.54.

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

Целью изобретения является повышение точности управления.

На фиг.1 представлена общая функциональная схема устройства управления самоходным средством; на фиг.2 — схема блока управления записью сигнала датчика угла; на фиг.3 —; на фиг.4 — 6 — схемы, поясняющие метод определения координат самоходного средства; на фиг.7 — временные диаграммы работы блока управления

- записью сигнала датчика угла, „,. ЖÄÄ 1689921 А1 (54) УСТРОЙСТВО УПРАВЛЕНИЯ САМОХОДНЫМ СРЕДСТВОМ (57) Изобретение относится к автоматизации технологических процессов и может быть использовано для обработки бетонных полов, а также внутрицеховых транспортных систем. Целью изобретения является повышение точности управления, Цель достигается за счет коррекции информации о положении самоходного средства, получаемой от гироскопического датчика направления и датчика пройденного пути, по информации об углах визирования трех маяков с известными координатами. 1 з,п, флы, 7 ил.

Устройство управления самоходным средством содержит установленные на рабочей площадке маяки 1, расположенный на подвижном средстве 2 датчик 3 углов визирования маяков, содержащий неподвижно закрепленный объектив 4 с фотодиодом 5, вращающееся зеркало 6, датчик 7 угла поворота зеркала, связанный посредством кинематической передачи 8 с двигателем 9 вращения зе ркала, усилитель-форми рователь 10, блок 11 управления записью сигнала датчика угла, шину 12 выходных линий регистра маяка, блок 13 вычисления координат, первый 14 и второй 15 буферные параллельные регистры, первый 16 и второй 17 выходные параллельные регистры, шину 18 . датчика угла, гироскопический датчик 19 на1689921 правления, датчики пройденного пути 20, привода перемещения 21 и разворота 22 тележки.

Блок управления записью сигнала датчика угла (фиг.2) содержит буферный усилитель 23, первый 24, второй 25, третий 26 и четвертый 27 элементы И-НЕ, первый одновибратор 28, первый счетчик 29, дешифратор 30, первый инвертор 31, второй буферный инвертор 32, первый триггер 33, дифференцирующую цепочку 34, второй одновибратор 35, пятый элемент И-НЕ 36, второй триггер 37, второй инвертор 38, второй счетчик 39, генератор 40 стабилизированной частоты, параллельный регистр 41 маяка, третий 42 и первый 43 буферные инверторы.

В каждом из расположенных на рабочей площадке маяков (фиг.3) установлена излучающая головка 44, генератор 45 идентифицирующих импульсов, транзисторный ключ 46, цепочка инфракрасных диодов 47, источник 48 питания. Генератор 45 идентифицирующих импульсов содержит два элемента И-НЕ 49 и 50, резистор 51 и кварцевый резонатор 52, два счетчика 53 и 54, RS-триггер 55, элемент И-HE 56 и буферный инвертор 57.

Устройство управления самоходным средством работает следующим образом.

На рабочей площадке устанавливается не менее трех маяков 1, например, в углах площадки. Координаты маяков записывают ся в память вычислительного устройства 13, Частота и длительность световых импульсов, излучаемых инфракрасными диодами, содержащимися в излучающей головке 44, определяется сигналом генератора 45 идентифицирующих импульсов, управляющим работой транзисторного ключа 46. Частота импульсов определяется генератором стабилизированной частоты, представляющим собой мультивибратор с кварцевой стабилизацией частоты, выполненный на двух элементах И-НЕ 49 и 50, Сигнал этого мультивибратора поступает на счетчики 53 и 54 и RS-триггер 55, формирующие импульсы различной длительности для различных маяков. Изменение длительности импульса осуществляется переключением связи между выходами счетчика 53 и R-входом триггера 55.

Световые импульсы маяков 1 принимаются датчиком 3 углов визирования маяков, расположенным на самоходном средстве 2.

При этом световые импульсы, отражаясь от вращающегося зеркала 6, попадают в объектив 4 и на фотодиод 5, в котором они преобразуются в электрические импульсы.

45 !

Импульсы фотодиода 5 через усилительформирователь 10 поступают на вход блока

11 управления записью сигнала датчика угла 11, Сигнал, поступающий на вход блока

11 управления записью представляет собой пачку импульсов, длительность импульсов в которой определяется сигналами генерато-. ра 45 идентифицирующих импульсов маяка, от которого в данный момент принимаются световые импульсы, Ширина пачки импуль- сов определяется частотой вращения зеркала 6, приводимого в движение через кинематическую передачу 8 двигателем 9 вращения зеркала, а также величиной угла зрения объектива 4. С выхода первого буферного инвертора 43, расположенного в блоке 11 управления записью, в момент прихода первого импульса пачки поступает сигнал на вход управления записью первого буферного регистра 14. На вход управления записью второго буферного регистра 15 с выхода второго буферного инвертора 32, содержащегося в блоке 11 управления записью, поступают сигналы в момент прихода следующих импульсов пачки, начиная с N-го, где

N — номер разрешающего импульса пачки (например, N = 4). В моменты поступления сигналов на входы управления записью буферных регистров 14 и 15 в них записывается с шины 18 датчика угла информация об угловом положении транспортного средства относительно маяка.

С выхода третьего буферного инвертора

42, расположенного в блоке 11 управления записью, на входы управления записью первого 16 и второго 17 выходных регистров поступает сигнал, фиксирующий окончание пачки импульсов, поступившей на вход блока 11 управления записью, если количество импульсов в пачке не меньше N. По этому сигналу информация с шин выходных линий первого 14 и второго 15 буферных регистров записывается соответственно в первый и второй выходные регистры. Таким образом, в первый выходной регистр записывается информация об угловом направлении на маяк в момент прихода первого импульса пачки световых импульсов, а во второй выходной регистр — информация об угловом направлении на маяк в момент окончания пачки импульсов. С шин выходных линий регистров 16 и 17 через первый и второй входы информация поступает в блок 13 вычисления координат. Информация о длительности импульсов, необходимая для, идентификации маяка, передается через шину 12 выходных линий регистра маяка одновременно с сигналом третьего буферного инвертора 42. Через третий вход эта информация поступает в блок 13, 10 — 2(1 1)(+b-b

,1(), Р Р )-о

На четвертый и пятый входы блока вычисления координат поступает информация с выходов гироскопического датчика 19 направления и датчика 20 пройденного пути, Текущее координаты самоходного средства 5

Х, Уна рабочей площадке (фиг,,4) по сигна1 лам датчиков направления 19 и пройденного пути 20 определяются в блоке 13 как

Х = Х, +,",„Л SI cos 1/ ;

Y = Y,+, ) ЛSjз1п1/1;!

15 где Хо, Y< — координаты опорной точки;

h, $1 — приращение пути за j-й промежуток времени движения от опорной точки, определяемое датчиком 20 пройденного пути;

20 ф — угол отклонения траектории самоходного средства от оси Х на 1-м прямолинейном участке траектории.

При определении координат самоходного средства в соответствии с (1) происхо- 25 дит накопление ошибки, обусловленной

"уходом" гироскопического датчика направления и погрешностью датчика пройденного пути. Коррекция информации о координатах самоходного средства выполняется путем определения координат новых опорных точек по информации, получаемой от датчика 3 маяков, Расчет координат опорных точек выполняется в блоке 13 в соответствии со следующим методом (фиг.5).

Маяки М1, М2, МЗ располагаются на рабочей площадке с координатами (Х1, Y1), (Хг, У2), (Хз, Уз), записанными в память вычислительного устройства. В точке Т с координатами Х, У, подлежащими определению, находится самоходное средство. По информации датчика 3 углов визирования маяков вычислительное устройство 13 определяет углы между направлениями на маяки и продольной осью самоходного средства p1, pz, 45 (рз и рассчитывает углы между направлениями на маяки а1 = P1 — ()2;

Gh=P2 Р3 (2)

Прямые, проведенные через точки Т1, 50

М1, Т, М2, Т, МЗ, характеризуются уравнениями

+У

У=

=Х1-Х У Х1-Х Х: х+У—

11: у= x+Y X, (3) — Уг — Y

III: у — х+ Y X.

Угол между двумя прямыми определяется соотношениями

К1 К2

19 а1 =- =- а1;

1+К)К2

19 а =- — аг, К2 КЗ (4)

1 + Кг Кз где К) = (Y) — Y)/(X) — Х); i =- 1, 2, 3 — номер маяка.

Таким образом (У -У,)Х+(Х,- Xz>YiY„X - (Х, Л х — (х,+ Х 2Х+ Х,Х,+У вЂ” (1i+ г У t YY Yz (Y -Y )х ° (Х,-X,)Y> YzX>- Y х, O = 3 2

Х вЂ” (Х Хз ) Х + Х Х + (— (У + ), E + Y Y

Система двух уравнений (5) с двумя неизвестными может быть представлена в виде агХ вЂ” (аг(Хг + Хз) — (Уз — Y2)) Х + агУ вЂ” (аг(У2 + Уэ) — (Хг — Хз))У = УгХз — УзУг— — аг(ХгХз + УгУэ); а, X- — (а1(Х1+ Хг) — (Y2 — Y1)) X + а1У вЂ” (а1(У1+ Уг) — (X1 — Хг))У = Y1X2 — Y2X1— — а1(Х1Х2 + У1Y2) (6) или х +Р„х+у +ц„у Ь1, г 2

x + P2x + уг + цгу = b2 (7) гдех=Х;

y= Y;

Р1 = — (Y2 — Y1) — (Х1+ Хг);

1 а1

q1 = — (X1 — X2) — (У1+ Y2);

1 а1 1

Ь1 = (Y1X2 — Y2X,1) — — (Х1Х2 + Y1Y2);

1 а1

Р2 = — (Y3 — Y2) — (Х2 + X3);

1 аг

Цг = — (Х2 X3) (Y2 + УЗ);

1 аг

Ьг = (УгХз — УзХг) — — (ХгХз + УгУз). (8)

1 аг

Выражая х из первого уравнения (7) и подставляя во второе уравнение системы (7), получают квадратное уравнение относительно у ((Р,- .) "(, 9,2) 3 (,(р рг)Решение квадратного уравнения (9) дает два значения искомой координаты yI, Подставляя зти значения в первое уравнение системы (7), получают два квадратных уравнения относительно х и соответственно четыре решения по х х = — — + Ь1 — q1 у1 —; 1- 1,2. (10)

Р1

68992!

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

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

Уг — Y

p= — arctg (11}

Х2 — Х

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

После определения координат самоходного средства на рабочей площадке в блоке

13 производится их сравнение с координатами заданной траектории перемещения и вырабатываются соответствующие команды, поступающие через устройство вывода на приводы перемещения 21 и разворота 22 тележки.

Работа блока 11 управления записью сигнала датчика угла (фиг.2) поясняется диаграммами на фиг.7, На вход блока управления записью поступает сигнал в виде пачки импульсов. Через буферный усилитель 23, третий элемент И-НЕ 26 первый импульс пачки попадает на первый буферный инвертор43, а затем подается на входуправления записью первого буферного регистра. Одновременно этот импульс устанавливает второй RS-триггер 37 в состояние "1", а сигналом с инверсного выхода этого триггера отключается прохождение дальнейших импульсов пачки на первый буферный инвертор 43. Первым же импульсом пачки запускается первый одновибратор 28, длительность импульса которого тустанавливается примерно в 1,5 раза больше периода следования импульсов маяка Т. Каждый последующий импульс пачки производит повторный запуск первого одновибратора 28, Таким образом, его результирующий импульс получается больше длительности пачки О на величину т, Начиная с первого импульса пачки, происходит заполнение первого счетчика 29, после прохождения N-ro импульса сигналом с дешифратора 30 через первый элемент И-НЕ 24 прохождение импульсов на счетчик прекращается и одновременно открывается второй элемент И-НЕ 25 для прохождения импульсов пачки на второй буферный инвертор 32. (N+1)-й импульс пач5

50 ки, проходящий через второй элемент И-НЕ

25, устанавливае. первый RS-триггер 33 в состояние "1" и ем самым открывает пятый элемент И-НЕ 36 для прохождения импульса второго одновибратора 35. Через промежуток времени т после прохождения последнего импульса пачки первый одновибратор 28 возвращается в исходное состояние, сигналом с инверсного выхода сбрасывает в "0" первый счетчик 29 и через . дифференцирующую цепочку 34 запускает второй одновибратор 3. Импульс второго одновибратора через пятый элемент И-НЕ

36 и третий буферный инвертор 42 подается на входы управления записью nepaoro 16 и второго 17 выходных регистров. Одновременно сигналом с инверсного выхода второго одновибратора выполняется сброс второго RS-триггера 37, первого триггера 33 и сброс второго счетчика 39, На второй счетчик 39 поступают импульсы генератора 40 стабилизированной частоты. Период следования импульсов генератора задается в 5 — 10 раз меньше минимальной длительности импульсов маяков.

Заполнение второго счетчика 39 происходит в интервалах прихода первых N импульсов пачки сигналов маяка. Запись содержимого счетчика 39 в регистр 41 маяка производится по сигналу, снимаемому с третьего буферного инвертора 42.

При попадании в датчик маяков случайного светового импульса помехи на входе блока управления записью появляется одиночный электрический импульс. В результате первый счетчик 29 не набирает требуемого количества N импульсов и запись информации во второй буферный регистр 15 не выполняется.. После окончания действия импульса первого одновибратора 28 сигналом второго одновибратора первый 33 и второй 37 триггеры, а также первый 29 и второй 39 счетчики, устанавливаются в исходное состояние. Таким образом, информация, находившаяся в первом 16 и втором 17 выходных регистрах случайным световым импульсом не искажается.

Формула изобретения

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

20

40

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

2. Устройство па п.1, отл ич а ю щеес я тем, что блок управления записью сигнала датчика угла содержит буферный усилитель, выход которого связан с первыми входами перво о, второго, третьего и четвертого элементов И-НЕ, э также с входом первого аднавибратарэ, выход первого элемента И-НЕ связан с входом первого счетчика, выход которого подключен к дешифратору, связанному своим выходам с вторыми входами первого и четвертого элементов И-НЕ и через первый инлертор — с вторым входом второго элемента И-НЕ, выход которога подключен к второму буферному инвертору и к S-входу первого

RS-триггера, прямой выход первого одновибратора подключен к третьему входу четвертого элемента И-НЕ, а инверсный выход— к входу установки в "0" первого счетчика и через дифференцирующую цепочку — к входу второго одновибратора, прямой выход которого подключен к первому входу пятого элемента И-НЕ, а инверсный выход — к Rвходам первого и второго RS-триггеров и через второй инвертор — к входу установки в "0" второго счетчика, выход первого RSтриггера подключены к второму входу пятого элемента И-НЕ, выход третьего элемента

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

1689921 гг о; i у уе

Уо х

1689921

1689921

ccz. 7

Редактор И.Шулла

Тираж Подписное

Заказ 3813 ниям и отк ытиям при ГКНТ СССР

ВНИИПИ Государственного комитета по изобретениям и от р

113035, Москва, Ж-35, Раушская наб., 4/5

Производственно-издательский комбинат "Патент", r, Ужгород, ул.Гагарина, 101

Пачка

oодdиeaыaх имлуль сод

Импульс пердого одкоЖдрапюр оыю Ьой

uwnynac г дно ка pg

Иипульсь! на ягоде первого

СЧегп чика на7йоде Йока 52

Импульс дторого

Одноби Ъратора

Составитель В.Завадский

Техред М,Моргентал Корректор Л.Бескид