Устройство управления самоходным средством Советский патент 1991 года по МПК G05D1/02 

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

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

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

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

со

о о го

правления, датчики пройденного пути 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 буферные инверторы.

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

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

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

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

Импульсы фотодиода 5 через усилитель- формирователь 10 поступают на вход блока 11 управления записью сигнала датчика угла 11. Сигнал, поступающий на вход блока

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

0 световые импульсы. Ширина пачки импульсов определяется частотой вращения зеркала 6, приводимого в движение через кинематическую передачу 8 двигателем 9 вращения зеркала, а также величиной угла зрения

5 объектива 4, С выхода первого буферного инвертора 43, расположенного в блоке 11 управления записью, в момент прихода первого импульса пачки поступает сигнал на вход управления записью первого буферно0 го регистра 14. На вход управления записью второго буферного регистра 15 с выхода второго буферного инвертора 32, содержащегося в блоке 11 управления записью, поступают сигналы в момент прихода следу5 ющих импульсов пачки, начиная с N-ro, где N - номер разрешающего импульса пачки (например, N 4). В моменты поступления сигналов на входы управления записью буферных регистров 14 и 15 в них записывает0 ся с шины 18 датчика угла информация об угловом положении транспортного средства относительно маяка.

С выхода третьего буферного инвертора 42, расположенного в блоке 11 управления

5 записью, на входы управления записью первого 16 и второго 17 выходных регистров поступает сигнал, фиксирующий окончание пачки импульсов, поступившей на вход блока 11 управления записью, если количество

0 импульсов в пачке не меньше N. По этому сигналу информация с шин выходных линий первого 14 и второго 15 буферных регистров записывается соответственно в первый и второй выходные регистры. Таким образом,

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

0 направлении на маяк в момент окончания пачки импульсов. С шин выходных линий регистров 16 и 17 через первый и второй входы информация поступает в блок 13 вычисления координат. Информация о дли5 тельности импульсов, необходимая для. идентификации маяка, передается через шину 12 выходных линий регистра маяка одновременно с сигналом третьего буферного инвертора 42. Через третий вход эта информация поступает в блок 13.

На четвертый и пятый входы блока вычисления координат поступает информация с выходов гироскопического датчика 19 направления и датчика 20 пройденного пути. Текущее координаты самоходного средства X1, т на рабочей площадке (фиг.4) по сигналам датчиков направления 19 и пройденного пути 20 определяются в блоке 13 как

Х Х0+ 2 ASjCosVi; J

Y Y0+ X ASjSlnV); j

где X0, Y0 - координаты опорной точки;

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

1р - угол отклонения траектории самоходного средства от оси X на j-м прямолинейном участке траектории.

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

Маяки Ml, M2, МЗ располагаются на рабочей площадке с координатами (Xi, Yi), (Х2, Y2), (Хз, Y3), записанными в память вычислительного устройства. В точке Т с координатами X, Y, подлежащими определению, находится самоходное средство. По информации датчика 3 углов визирования маяков вычислительное устройство 13 определяет углы между направлениями на маяки и продольной осью самоходного средства ф, рг рз и рассчитывает углы между направлениями на маяки

«1 р - pi

с& .(2)

Прямые, проведенные через точки Т1, М1, Т, М2, Т, МЗ, характеризуются уравнениями

Yi - YYi - Y

i u - 1 v 4- v - . Y.

1 vv AI

III:

Xi -X Y2-Y

U

y X2-X Y3-Y

yXi -X

3

XIY Y3-Y-v

X Т, Л,

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

Kt -К2

19И Т+кГкГа :

2% (4)

где Ki (Yi - Y)/(Xi - X); I - 1, 2, 3 - номер маяка.

Таким образом

а (.X.-Y, )X + X,(Y,+Y)

°v15

п гУ2)х + (хг-хг)у.у2х3-у3хг X2-U2 v3)x tXjX Ya-fY tYjiY + YjY Система двух уравнений (5) с двумя неизвестными может быть представлена в видег

а2Х - а2(Х2 + Хз) - (Y3 - Y2) X + a2Y2 (Y2 + Y3) - (X2 - X3)Y Y2X3 - Y3Y2 - 20 - a2(X2X3 + Y2Y3);

a ,X - ai(Xi + X2) - (Y2 - Yi) X + aiY2 (Yi + Y2) - (Xi - X2)Y YiX2 - Y2Xi -ai(XiXz + YiY2)(6) или

25 x2 +P1x + y2 + q,y Ы;

x2 + P2x + y2 + q2y - b2(7)

где х X; y Y;

(Y2-Yi)-(Xi + X2);

Э1

(Xi-Xz)-(Yi + Y2):

bi (YiX2 - Y2X,i) - (XiX2 + YiY2); ai

P2 (Y3-Y2)-(X2 + X3);

Ј.

Q2

32

(X2 - Хз) - (Y2 + Y3): 1

02 (Y2X3 - Y3X2) -f- - (X2X3 + Y2Y3). (8) 32

Выражая х из первого уравнения (7) и подставляя во второе уравнение системы (7), получают квадратное уравнение относительно у

),iVl i1-2,vv(i,b,-l,

($-l.-b.-W-b.,-p,.

Решение квадратного уравнения (9) дает два значения искомой координаты yj. Подставляя эти значения в первое уравнение системы (7), получают два квадратных уравнения относительно х и соответственно четыре решения по х

х--Јl±Ybi-qiyi-y ; 1 1,2. (10)

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

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

У°УЕ-аг«дУ .(11)

Полученные координаты х, у могут быть использованы как координаты новой опорной точки для определения текущих координат X1, Y1 в соответствии с (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. (Ы+1}-й импульс пачки, проходящий черп «торой з емонт И-НЕ 25, устанавливает первый RS-григгер 33 в состояние 1 и тем самым открывает пятый элемент И-HF 36 для прохождения импульса второго одновибратора 35. Через промежуток времени т после прохождения последнего импульса пачки первый одно- вибратор 28 возвращается в исходное состояние, сигналом с инверсного выхода

0 сбрасывает в О первый счетчик 29 и через дифференцирующую цепочку 34 запускает второй одновибратор 3. Импульс второго одновибратора через пятый элемент И-НЕ 36 и третий буферный инвертор 42 подается

5 на входы управления записью первого 16 и второго 17 выходных регистров. Одновременно сигналом с инверсного выхода второго одновибратора выполняется сброс второго RS-триггера 37, первого триггера 33

0 и сброс второго счетчика 39.

На второй счетчик 39 поступают импульсы генератора 40 стабилизированной частоты. Период следования импульсов генератора задается в 5-10 раз меньше ми5 нимальной длительности импульсов маяков. Заполнение второго счетчика 39 происходит в интервалах прихода первых N импульсов пачки сигналов маяка. Запись содержимого счетчика 39 в регистр 41 маяка про0 изводится по сигналу, снимаемому с третьего буферного инвертора 42.

При попадании в датчик маяков случайного светового импульса помехи на входе блока управления записью появляется оди5 ночный электрический импульс. В результате первый счетчик 29 не набирает требуемого количества N импульсов и запись информации во второй буферный регистр 15 не выполняется. После окончания действия

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

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

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

0 средством, содержащее установленные на самоходном средстве приводы перемещения и разворота тележки, гироскопический датчик направления, датчик пройденного пути, датчик углов визирования маяков, а

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

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

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

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

и входом буферного усилителя.

Фиг. /

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

$ i

21

24

fix од

23

26

П

Ч

kMl

Ф

И

-А -V

зо -Нз/Н

52

5Я

Ш

55

36

3S

2 WJ

/Й/2. /

Ј-гяф

, о(Л1У1

°А

А

№

т(х, Y)

м2(х2,Уг)

М(Х3,Ъ)

Фиг. 5

х

Фиг. 6

х

Фиг. 7