Устройство для вычисления местоположения транспортного средства Советский патент 1990 года по МПК G06F15/50 

Л

и

Изобретение относится к вычислительной технике и может быть использовано в системах управления автономными транспортными работами. Целью изобретения является повышение точности за счет учета возможности перемещения транспортного средства по сыпучим грунтам. Устройство содержит блок 1 датчиков ориентации, блок 2 курсоуказателя, блок 3 измерения пройденного пути, блок 4 микропрограммного управления, блок 5 преобразования координат, блок 6 суммирования приращений. 8 ил.

тпгтг

д систему дпроВлеиия Фиг. 1

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

Цель изобретения - повышение точности за счет учета возможности перемещения транспортного средства по сыпучим грунтам,

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

Устройство содержит блок 1 датчиков ориентации, блок 2 курсоуказателя, блок 3 измерения пройденного пути i блок 4 микропрограммного управле- ния, блок 5 преобразования координат, блок 6 суммирования приращений, гировертикаль 7, гирогорияонт 8, цифровые датчики 9 и 10, курсоуказа- тель 11, .цифровой датчик 12, свободн катящееся колесо 13 поперечного перемещения, датчик 14 оборотов, свобод- нокатящееся колесо 15 продольного перемещения, датчик 16 оборотов,штанги 17, ось 18 вращения, втулку 19, корпус 20 транспортного средства, цифровой датчик 21, формирователи. 22-25 прямоугольных импульсов, триггеры 26 - 29, реверсив1а)й счетчик 30, элемент 31 НЕ, сумматор 32, триггер.33, элемент 34 И-ИЛИ, циЬро- вые датчики 35 и 36, регистры 37 и 38, узел 39 арифметических операций, узлы 40 н 41 памяти, триггеры 42 и 43, элемент 44 ИЛИ, триггер 45,эле- менты 46, 47 и 4В И, счетчик 49 MI;IK- рокоманд, узел 50 памяти микрокоманд, генератор 51 тактовых импулъ- сов, регистры 52 - 56, узлы 57 и 58 памяти, регистры 59 - 65, мультиплек соры 66 и 67, сумматоры 68, 69 и 70, регистры 71 - 76, элементы 77 и 78 задержки.

Блок 3 измерения пройденного пути предназначен дня того,, чтобы вы- рабатывать сигналы перемещения в блок 4 и величины этого превращения и угла отклонения плоскости свободнокатящегося. колеса от нормали корпуса в блок 5 преобразования координат. Блок 3 измерения пройденного пути (фиг.5) содержит дра свободно- катящихся колеса (фиг,4а) поперечного 13 и продольного 15 перемещений с закрепленными на оси по два импульсных датчика 14 и 16 оборотов, в качестве которого можно применить любой импульсный датчик (например, датчик Холла), Эти датчики должны быть повернуты на оси один относительно другого так, чтобы формировать электрические импульсы, сдвинутые один относительно другого на 90 , эти импульсы пропорциональны расстоянию, пройденному колесами и направлению их движения. 1Чтанги 17 с расположенными на них колесами 13 и 15, закрепленные во вращающейся втулке, ось 18 вращения которой совпадает с Z|, Ось вращается во втулке 19,где расположен цифровой датчик угол-код, закрепленный на корпусе 20 транспорного средства. .

Работу устройств можно пояснить следующими математическими (Формулами

,., - i;4,.,,, (О

i-

вектор текущих координат транспортного средства; вектор перемещения снободнокатящегося колеса за время t,., ; вектор поперечного смеше

ия свободнокатяшегося колеса.

Ски.к

и 1

UY| isinu(J- 0 I

- sin (arctg ))

C2)

где 1 - суммарная длина звеньев

конструкции свободнокатя- щегося колеса; и

- соответственно изменение

1

угла поворота свободнока- тящегося колеса и абсолютное значение пути пройденного колесом поперечного смещения за время Ь, Пц - число импульсов с датчика расстояния от поперечного

10

5,1589292

смещения свободнокатящегося колеса.

Устройство определения местоположения транспортного средства работает следующим образом,

В момент включения питания осуществляется сброс всех регистров,триггеров и счетчиков в исходное нулевое состояние. При перемещении .сво- боднокатящихся колес 13 и 15 по поверхности электрические импульсы от датчиков 21 и 35 для поперечного и продольного перемещения поступают на блок 3 измерения пройденного пути, где формирователи 22-25 вырабатывают импульсы по фронту и срезу импульса, приходящего с датчика 21, а триггеры 26-29 вырабатывают импульсы на счетчик 30 в зависимости от направления движения катящегося колеса. либо на вхсд сложения, либо на вход вычитания, счетчика. Счетчик 30 после поступления определенного количества импульсов (заранее заданных в соответствии с требуемой частотой просчета очередных приращений координат) через схему преобразования дополнительного кода в прямой (элементы 31-34), управляемой триггером 33, перебрасывает триггер 42 в состояние лог. 1 и через элемент 44 ИЛИ перебрасывает триггер 45 в сое- тояние лог. 1. Открывается элемент 48 И, разрешается подача тактовых импульсов с прямого выхода генератора 51 на счетный вход счетчика 49 микрокоманд, выходы которого служат адресом для угла 50 памяти микрокоманд и идет перебор этих адресов.Последовательность микрокоманд,поступающих с выхода узла 50 на блоки 3-6, обеспечивает выполнение операций в срответствии с алгоритмом работы системы в виде математических формул (1) и (2). Первая, микрокоманда реализует сброс блока 5 в исходное состояние. Спедующая микрокоманда реализует запись цифровых двоичных кодов значения углов дифферента У , крена у , курса и р в регистры 52 - 55 блока 5 преобразования координат. 59 хранится значение кометко об

ле вл ди

ще на и ср

15Ф

35

В регистре

поненты X исходного вектора Vf); , а в регистре 56 - значен и е компонен- ты Y исходного вектора с(. к

ледовательность дальнейших вы числе- НИИ.определяется выражением (1). Блок 5 преобразования координат реализуподе

20 бл пр хо ны ко

25 ка с ни ми вх

30 и .ди бл о с уч по в ти пу

40 на го но ра дв

45 ля в со об пе

50 мо ны пер вхо уго

55 Сое мир вых зир вто

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

Блок 6 суммирования приращений после окончания работы блока 5 осуществляет суммирование приращений координат по компонентам.X, Y, Z,

Очередной запуск устройства осуществляется из блока 3 подачей сигнала в блок 4 на входы триггеров 42 и 43 при перемещении транспортного средства на заданное расстояние.

15Формулаизоб

р е т е н и я

Устройство для вычисления местоположения транспор.тного средства, содержащее блок датчиков ориентации,

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

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

Соединен с вторым входом первого формирователя прямоугольных импульсов, ыход которого соединен с синхрони- зируюипш входом второго триггера, второй ззыход цифрового дат.чика оборотов соединен с первыми входами третьего и четвертого формирователей прямоугольных импульсов и с информа- 1щоиг ым входом второго триггера, а g через второй элемеит задержки с EIT O- рым входом четвертого формирователя прямоугольных импульсов, выход которого подключен к второму входу третьего формирователя прямоугольных ю импульсов, выход которого соединен с синхронизирующим входом первого триггера, выход первого триггера соединен с синхронизирующим входом третьего триггера и с установочга 1м 15 входом четвертого триггера, выход второго триггера соединен с синхронизирующим входом четвертого триггера и с установочным входом третьего триггера, информационные входы третье-20 го и четвертого триггеров соединены с единичной константой, выходы третьего и четвертого триггеров соединены .с входом сложения и входом вычитания реверсивного счетчика соответственно, -5 выход переполнения и выход обнуления реверсивного счетчика соединены с единичным и нулевым входами пятого триггера соответственно, единичный и нулевой выходы которого соединены с первым и нто- Ю

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

S 6flSK 5

Ю

Фиг.1

Фиг.З

мака 4

SSfOKS

Фаг. 5

От блокад

о

Фиг.а

;

OmSMKOtt

n.

Ф(/г..в