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

1

Д

4

00

эо

о

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

О М

В систему управления фи9

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

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

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

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

транспортного средства, преобразователь 24 угол - код, датчик 25 оборотов, генератор 26 тактовых импульсов счетчик 27, триггер 28, элемент И 29 счетчик 30, узел 31 памяти микрокоманд, регистры 32 - 35, узлы 36 и 37 памяти, общие шины 38 и 39, регистры 40 и 41, арифметическо-логический узел (АЛУ) 42, регистр 43, умножител 44, регистры 45-49, связь 50, общую шину 51, сумматоры 52-54, регистры 55-60.

Функционирование устройства поясняется следующими математическими выражениями

;

(1)

вектор перемещения центра масс транспортного средства за время между моментами t , ,

t;;

вектор перемещения измерительного свободно

Р .

.4

катящегося колеса за время между моментами t {, и t; в системе координат, связанной с точкой закрепления свободно катящегося колеса к корпусу транспортного средства (ТС); ) - матрица, устанавливающая связь между системой координат, связанной с точкой закрепления свободно катящегося колеса к корпусу ТС, и координатами подвижной системы координат, связанной с корпусом транспортного средства (где ось X совпадает с продольной осью транспортного средства, а ось Y - с поперечной, а ось Z перпендикулярна плоскости X, Y) при повороте плоскости измерительного колеса на угол р

cos р; sinp; -sin ft; cos i;

О

О

(2)

Vf:

)

F«(9;),

матрицы, определяющие связь между нет подвижной и подвижной системами координат при повороте подвижной системы на и курса

(3)

(4)

50

(5)

55

Вектор перемещения измерительного свободно катящегося колеса определяется, как

х

h-bn О О

К

(6)

514

Устройство работает следующим образом,

В момент включения питания осуществляется сброс всех регистров, триггеров и счетчиков в исходное нулевое состояние. При перемещении свободно катящегося колеса по поверхности электрические импульсы от импульсного датчика 25 оборотов измерительного колеса 17 поступают на вход счетчика 27 блока 4, После поступления определенного количества импульсов (заранее заданного в соответствии с требуемой частотой просчета очередных приращений координат) срабатывает триггер 28, в результате чего логическая 1 поступает на вход элемента И 29 и разрешает поступление тактовых импульсов с генератора 26 на счетный вход счетчика 30, который, в свою очередь, вырабатывае последовательность адресов для адресного входа узла 31 памяти микрокоманд.

Последовательность микрокоманд, поступающих с выхода узла 31, обеспечивает выполнение операций в соответствии с алгоритмом работы системы в виде математических формул (1)- (6). Первая микрокоманда реализует запись цифровых двоичных кодов значения углов дифферента 0 , кренаЯ , курсами В в регистры 32-35 блока 5. В регистре 45 хранится значение компоненты X исходного вектора перемещения VIVM . Последовательност дальнейших вычислений определяется выражением (1). Блок 5 реализует последовательное умножение трехком- понентного вектора (6) на матрицы преобразования координат (2)-(5), которое происходит следующим образом

После умножения исходного вектора (6) на матрицы (2) получают промежуточные значения координат X1, Y1, Z1 в виде тождеств

X1 (i + у sin(5 ; Y1 + у cosfi ; (7) - Z z .

Это реализуется в блоке 5 следующим образом.

Значение X из регистра 45 выдается на общую шину 39 и записывается fc регистр умножителя 44.

Значение угла В из регистра 35 выдается на общую шину 38. Одновременно

8016

с этим включается в работу узел 37, где хранится функция cosft. Таким образом, появившееся на общей шине 39 значение cos (3 записывается в регистр умножителя 44.

Приходит команда умножения, по которой два сомножителя, записанные в регистрах умножителя 44, перемноQ жаются и в этом же такте управления записываются в регистр 40.

Значение у из регистра 46 (у 0, см. (6)) записывается в регистр умножителя 44.

5 Значение угла } из регистра 33 через узел 36 и шину 39 записывается в регистр умножителя 44.

Сомножители, записанные в регистрах умножителя 44, перемножаются, и

Q результат записывается в регистр 41. В этом же такте управления два слагаемых из регистров 40 и 41 выдаются на узел 42, где они складываются, а результат записывается в регистр

5 3, откуда через общую шину 39 переписывается в регистр 47.

Значение регистра 45 считывается и через общую шину 39 записывается в регистр умножителя 44.

0 Синус угла {3 из регистра 33 через общую шину 38 и узел 36 записывается в регистр умножителя 44.

Происходит умножение двух чисел в умножителе 44 и запись результата в регистр 40.

Значение у из регистра 46 записывается через общую шину 39 в регистр 41 .

В АЛУ 42 число из регистра 40

о вычитается из числа регистра 41 и разность записывается в регистр 43, откуда через общую шину записывается в регистр 49.

Код числа z (см. (6)) из регист5 pa 46 через общу шину 39 записывается в регистр 49.

В регистрах 47-49 расположены промежуточные результаты х , Yf, Z1 (см. (7)),

Q Исходные координаты X , Y, Z умножают на матрицу преобразования (3) и получают промежуточный вектор в виде

5

X х1;

У y .cosjf - a .sinjf ; (8) Z у , sin - z . cosj .

Вычисления (8) проводятся аналогичным образом, в результате чего получают промежуточные значения X, ZM в регистрах 47-49.

Следующее преобразование при вороте вектора на угол имеет слющий вид (см. (4))

к

cos

В +

Z1

z1 cos 9 - х sin б.

Осуществляя последнее преобразование, (см. (5)), получают окончательные приращения координат X, Y, Z в соответствии с выражением

X cosM + у -sirup;

Y -х|П sin(f + y .cosif; (10)

Блок 6 суммирования приращений

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

после окончания работы блока 5 осуще- 20содержащее блок измерения пройденного

ствляет суммирование приращений коор-пути, отличающееся тем,

динат по компонентам X, Y, Z, котороечто, с целью повышения точности, в

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

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

щения по координате X ( из регистра 25приращений, блок микропрограммного

47) на общую шину 51 блока 6. В этотуправления и блок преобразования момент срабатывает регистр 55 в режи- Запись с выдачей. Исходное чисме

ло, записанное в регистр 55, равняется нулю, поэтому на сумматор 52 поступают два числа: первое с общей шины 51 и второе с регистра 55, которые суммируются, и результат записывается в регистр 55, после чего происходит перезапись результата из регистра 55 в выходной регистр 58. Аналогичным образом происходит суммирование приращений по координате Y (из регистра 48 на общую шину 51 и вход сумматора 53, в регистр 56 и выходной регистр 59) и Z (из

координат, информационные входы которого соединены с информационными выходами датчика ориентации, блока

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

регистра 49 на общую шину 51 и вход сумматора 54, в регистр 57 и выходной регистр 60). Последняя микрокоманда от узла 31 блока 4 осуществляет запись значения параметра Z из регистра 57 в выходной регистр 60 и последний разряд микрокоманды, осуществляет сброс счетчиков 27 и 30 и триггера 28 и тем самым подготавливает блок 4 к очередному просчету координат, который осуществляется по приходу очередного сигнала от выхода счетчика 27 на вход триггера 28.

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

I

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

управления и блок преобразования

координат, информационные входы которого соединены с информационными выходами датчика ориентации, блока

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

Фиг, Z

П

8Јлок5

Фив. 5

Фиг.З

29

i

«о

V

2

28

31

OW/

Й

фив.7

От. 5локаЬ .SO

о

OmtMK f

1

I