Устройство для вычисления местоположения транспортного средства

СОЮЗ СОВЕТСНИХСОЦИАЛИСТИЧЕСКИХРЕСПУБЛИН 119) 01 51)5 С 06 Р 15/50 ГОСУДАРСТВЕННЫПО ИЗОБРЕТЕНИЯМПРИ ГННТ СССР НОМИТЕТОТНРЫТИЯМ ПИСАНИЕ ИЗОБРЕТЕНА ВТОРСНОМУ СВИДЕТЕЛЬСТВУ 824/21,888.90. огс Каляе рев и 3:62- нт США(57) Изобретение относится к вычислительной технике и может быть исполь-.зовано в системах управления авто"номными транспортными работами,Целью изобретения является повышениеточности за счет учета возможностиперемещения транспортного средствапо сыпучим грунтам. Устройство содержит блок 1 датчиков ориентации,блок 2 курсоуказателя, блок 3 измерения пройденного пути, блок 4 микропрограммного управления, блок 5преобразования координат, блок бсуммирования приращений. 8 ил.Изобретение относится к нычислительной технике и может использоваться в системах управления автономными транспортными роботами. . Цель изобретения - повышение точности эа счет учета возможности перемещения транспортного средства по сыпучим грунтам.На Фиг.1 приведена структурная схема устройства; на Фиг.2 " блок датчиков ориентации; на фиг. 3- блок курсоуказателя; на фиг.4 - конструктивные особенности свободнокатящегося колеса; на Фиг. 5 - блок измерения пройденного пути; на фиг, 6 - блок микропрограммного управления; на фиг. 7 - блок преобразования координат; на фиг. 8 - блок суммирования приращений,10 20 Блок 3 измерения пройденного пути предназначен для того,.чтобы вырабатывать сигналы перемещения в блок 4 и величины этого приращения и угла отклонения плоскости свобод 55 Устройство содержит блок 1 датчиков ориентации, блок 2 курсоуказателя, блок 3 измерения пройденного пути, блок 4 микропрограммного упранления, блок 5 преобразования координат, блок 6 суммированйя приращений,гировертикаль 7, гирогориэонт 8, цифровые датчики 9 и О, курсоуказатель 11, .цифровой датчик 12, свободно катящееся колесо 13 поперечного перемещения, цатчик 14 оборотов, свободнокатящееся колесо 15 продольногоперемещения, датчик 16 оборотон,штанги 17, ось 18 вращения, втулку 19, 35корпус 20 транспортного средства,цифровой датчик 21, Формирователи .22"25 прямоугольных импульсов,триггеры 26 - 29, реверсинный счетчик 30, элемент 31 НЕ, сумматор 32, 40триггер.33, элемент 34 И-ИЛИ, цыЪровые датчики 35 и 36, регистры 37 и38, узел 39 арифметических операций,узлы 40 ц 41 памяти, триггеры 42 и43, элемент 44 ИЛИ, триггер 45,элементы 46, 47 и 48 И, счетчик 49. мик"рокоманд, узел 50 памяти микрокоманд, генератор 51 тактовых импульсов, регистры 52 - 56, узлы 57 и 58памяти, регистры 59 - 65, мультиплексори 66 и 67, сумматоры 68, 69 и 70,регистры 71 - 76, элементы 77 и 78задержки.(2) где 1- суммарная длина звеньев конструкции свободнокатящегося колеса; Ь и11 - соответственно изменениеугла поворота свободнокатящегося колеса и абсолютное значение пути пройденного колесом поперечного смещения за время,;и - число импульсов с датчикарасстояния от поперечного нокатящегося. колеса от нормали корпуса в блок 5 преобразования координат. Блок 3 измерения пройденногопути (Фиг,5) содержит дна свободнокатящихся колеса (фиг.Ьа) поперечного. 13 и продольного 15 перемещенийс закрепленными на оси по дна импульсных датчика 14 и 16 оборотон,в качестве которого можно применитьлюбой импульсный датчик (например,датчик Холла). Эти датчики должныбыть понернуты на оси один относительно другого так, чтобы Формироватьэлектрические импульсы, сдвинутыеоодин относительно другого на 90эти импульсы пропорциональны расстоянию, пройденному колесами и направлению их движения. Штанги 17 с расположенными на них колесами 13 и 15,закрепленные во вращающейся втулке,ось 18 вращения которой совпадаетс Е 1, Ось вращается во втулке 19,гдерасположен цифровой датчик угол-код,закрепленный на корпусе 20 транспорт"ного средства,Работу устройства можно пояснитьследующими математическими Формулами:1к-где Ч - вектор текущих координаттранспортного средства;Ч, , - вектор перемещения снободо;.,;нокатящегося колеса завремяЧ - вектор поперечного смещек.д,киия снободнокатящегосяколеса.смещения свободнокатящегося колеса.Устройство определения местоположения транспортного средства работает следующим образом,5В момент включения питания осущестнляется сброс всех регкстров,триггеров и счетчиков в исходное нулевое состояние, При перемещекии свободнокатящихся колес 13 и 15 по понерхности электрические импульсы отдатчиков 21 и 35 для поперечного ыпродольного перемещения поступают наблок 3 измерения пройденного пути, 15где формирователи 22-25 вырабатываютимпульсы по Аронту и срезу импульса,приходящего с датчика 21, а триггеры2 б " 29 вырабатывают импульсы насчетчик 30 в зависимости от направления движения катящегося колеса.либо на вход сложения, либо на входвычитания, счетчика, Счетчик 30 после поступления определенного количества импульсов (заранее заданных н 25соответствии с требуемой частотойпросчета очередных приращений координат) через схему преобразованиядополнительного кода в прямой (элементы 31-34), управляемой триггером 3033, перебрасывает триггер 42 н состояние лог. "1" и через элемент 44ИЛИ перебрасывает триггер 45 в состояние лог, "1". Открывается элемент48 И, разрешается подача тактовых35импульсов с прямого выхода генератора51 на счетный вход счетчика 49 микрокоманд, выходы которого служат адресом для угла 50 памяти микрокоманди идет перебор этих адресов.Последовательность микрокоманд,поступающихс ныхода узла 50 на блоки 3 - 6,обеспечивает выполнение операций всоответствии с алгоритмом работы системы в виде математических Аормул 45(1) и (2). Перная микрокоманда реализует сброс блока 5 в исходное состояние. Счедующая микрокоманда реализует запись цифровых двоичных кодовзначения углов диААерента, крена , курса 1 ив регистры 52 -55 блока 5 преобразсьания координат.В регистре 59 хранится значение компоненты Х исходного вектора Ч Н,а в регистре 5 б - значение компонен. ты Уф исходного вектора ЧС ,. Пос-,ледовательность дальнейших вычислений определяется выражением (1). Блок5 преобразования координат . реализует последовательное умножение трех- компонентного вектора на матрицы преобразования координат,Елок 6 суммирования приращений после окончания работы блока 5 осущест-вляет суммирование приращений координат по компонентам.Х, 7, Е.Очередной запуск устройства осуществляется из блока 3 подачей сигнала н блок 4 на входы триггеров 42 и 43 при перемещении транспортного средства на заданное расстояние.Формула изобретенияУстройство для нычисления местоположения транспортного средства,содержащее блок датчиков ориентации, блок курсоуказателя и блок измерения пройденного пути, инАормационные вы" ходы которых соединены с инАормационными входами блока преобразования координат соответственно, выход блока преобразования координат соединен с входом блока суммирования приращений, выходы которого являются выходами координат устройства, управляющие входы блока преобразования координат и блока суммирования приращений соединены с соответствующими выходами блока микропрограммного управления, о т л и ч а ю щ е е с я тем, что, с целью повышения точности, за счет учета возможности перемещения транспортного средства по сыпучим грунтам, в нем блок измерения пройденного пути содержит канал Аормирования импульсов продольного перемещения и канал Формирования импульсов поперечного перемещения, выполненных идентично, датчик угла поворота, два регистра, узел ариАметических операций и два узла памяти, выходы которых являются инАормационным выходом блока, в каждом канале Аормирования импульсов первый выход циАрового датчика оборотов соединен с первыми входами первого и второго Аормирователей прямоугольных импульсов и с инАормационным входом первого триггера, а через первый элемент задержки - с вторым входом второго Фдрмиронателя прямоугольных импульсов, выход которого соединен с вторым входом первого Аормироватгля прямсугольных импульсов, выход которого соединен с синхронизируюшим входом второго триггера, второй выход цифрового датчика оборо589292тов соединен с первыми входамитретьего и четвертого Аормирователейпрямоугольных импульсов и с ннформа"циониым входом второго триггера, ачерез второй элемент задержки с вторым входом четвертого формирователяпрямоугольных импульсов выход кОтОрого подключен к второму входутретьего формирователя прямоугольныхимпульсов, выход которого соединенс синхрониэирующим входом первоготриггера, выход первого триггера соединен с синхронизирующим входомтретьего триггера и с установочнымвходом четвертого триггера, выходвторого триггера соединен с синхронизирующим входом четвертого триггера и с установочным входом третьеготриггера, информационные входы третье го и четвертого триггеров соединеныс единичной константой, выходы третьего и четвертого триггеров соединены,с входом сложения и входом вычитанияреверсивного счетчика соответственно, 25выход переполнения и выход обнуленияреверсивного счетчика соединены с единичным и нулевым входами пятого триггера соответственно, единичный и нулевойвыходы которого соединены с первым и вто-;О рым входами элемента И-ИЛИ, выход признака счетчика соединен с третьим входомэлемента И"ИЛИ и с входом элемейтаНЕ, выход которого подключен к входусумматора, выход которого соединен счетвертым входом элемента И-ИЛИ,выходы элементов И-ИЛИ обоих каналовподключены к соответствующим входамблока микропрограммного управления,единичные выходы пятых триггеров канала формирования импульсов продольного перемещения и канала Аормированияимпульсов поперечного перемещенияподключены к синхронизирующим входампервого и второго узлов памяти соответственно, управляющие входы которыхсоединены с соответствующими выходами блока микропрограммного управления, информационный вход второго узла памяти соединен с выходом узлаарифметических операций, входы которо"о соединены с выходами первого ивторого регистров соответственно,выход первого регистра соединен .с вхо"дом второго регистра, выход датчикауглов поворота соединен с .входом пер-,вого регистра и информационным входом блока преобразования координат,,Кучерявая Коррек Редакто олинич В о-издательский комбинат "Патент", г. Ужгород, ул. Гагарина, 10 оизводс оставитель А.Жереиоехред А;Кравчук 2542 Тираж 569 Государственного комитета по йзоб 113035, Москва, Ж, Ра Подписноеениям и открытиям при ГКНТ ССРкая наб., д. 4/5