SU1589292A1 - Устройство дл вычислени местоположени транспортного средства - Google Patents

Abstract

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

Description

Л

и

тпгтг

д систему дпроВлеии  Фиг. 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 при перемещении транспортного средства на заданное рассто ние.

Claims (1)

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

р е т е н и  

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

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

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

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

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

S 6flSK 5

Ю

Фиг.1

Фиг.З

мака 4

SSfOKS

Фаг. 5

От блокад

о

Фиг.а

;

OmSMKOtt

n.

Ф(/г..в