способ определения параметров, характеризующих ориентацию тележки транспортного средства
Классы МПК: | G01C9/00 Приборы для измерения уклонов, например с помощью клинометров, нивелиров |
Автор(ы): | Гданский Николай Иванович (RU), Мальцевский Владислав Васильевич (RU), Засед Вера Валерьевна (RU), Михайлов Александр Александрович (RU) |
Патентообладатель(и): | Московский государственный университет инженерной экологии (RU) |
Приоритеты: |
подача заявки:
2005-12-14 публикация патента:
10.06.2007 |
Изобретение относится к робототехнике и предназначено для определения углового положения, а также подъема тележки мобильного робота при его перемещении по неровной поверхности в том случае, когда размеры препятствий по длине и ширине не превышают габариты тележки. Техническим результатом является автоматизированное определение углового положения относительно вертикали и подъема тележки мобильного робота. На тележке выбирается декартова система координат с горизонтальной продольной x, горизонтальной поперечной у и вертикальной осью z, образующими правую тройку. Причем тележка имеет четыре опорные точки, лежащие симметрично по отношению к центру системы координат на расстоянии А от оси x и на расстоянии В от оси у. На тележке выбирают точки с координатами (x 1, у1), (х2 , у2), (х3, у 3), не лежащие на одной прямой, в них стационарно устанавливают вертикальные сообщающиеся между собой трубки с жидкостью, снабженные датчиками уровня, и определяют в каждой из трубок уровень жидкости - z1, z2, z 3. После чего рассчитывают по формулам величины, характеризующие ориентацию тележки: угол поворота тележки вокруг горизонтальной продольной оси x, угол поворота вокруг горизонтальной поперечной оси у и величину h вертикального подъема центра системы координат:
=arcsin{[(у2-у1 )(z3-z1)-(у 3-у1)(z2-z 1)]/[(х3-x1 )(у2-у1)-(х 2-х1)(у3-у 1)]};
=arcsin{[(x2-x1 )(z3-z1)-(х 3-x1)(z2-z 1)]/([(х3-x1 ])(у2-у1)-(х 2-x1)(у3-у 1)]·cos )};
h=A·|sin |+В·|sin cos |.
Формула изобретения
Способ определения параметров, характеризующих ориентацию тележки транспортного средства при ее движении, заключающийся в выборе на тележке декартовой системы координат с горизонтальной продольной x, горизонтальной поперечной у и вертикальной осью z, образующими правую тройку, причем тележка имеет четыре опорные точки, лежащие симметрично по отношению к центру системы координат на расстоянии А от оси x и на расстоянии В от оси у, на тележке выбирают точки с координатами (x1, у 1), (x2, у2 ), (х3, у3), не лежащие на одной прямой, в них стационарно устанавливают вертикальные сообщающиеся между собой трубки с жидкостью, снабженные датчиками уровня, и определяют в каждой из трубок уровень жидкости - z 1, z2, z3, после чего рассчитывают по формулам величины, характеризующие ориентацию тележки: угол поворота тележки вокруг горизонтальной продольной оси x, угол поворота вокруг горизонтальной поперечной оси y и величину h вертикального подъема центра системы координат:
=arcsin{[(у2-у1 )(z3-z1)-(у 3-у1)(z2-z 1)]/[(х3-x1 )(у2-у1)-(х 2-х1)(у3-у 1)]};
=arcsin{[(x2-x1 )(z3-z1)-(х 3-x1)(z2-z 1)]/([(х3-x1 ])(у2-у1)-(х 2-x1)(у3-у 1)]·cos )};
h=A·|sin |+В·|sin cos |.
Описание изобретения к патенту
Изобретение относится к робототехнике и предназначено для определения углового положения, а также подъема тележки мобильного робота при его перемещении по неровной поверхности в том случае, когда размеры препятствий по длине и ширине не превышают габариты тележки. Крупные препятствия хорошо распознаются системами видеонаблюдения и их преодоление, как правило, происходит в режиме ручного управления. Основную сложность представляют небольшие препятствия. Неучет угловых смещений тележки и ее подъема значительно усложняет распознавание получаемой от него визуальной информации, управление приводами движителей, манипуляторов и др.
Известно устройство для определения углов наклона объектов (Патент РФ №2142613, кл. G01С 9/02, 1998), содержащее панель с подвешенным на ней маятником и шкалу, закрепленную на панели, при этом панель, плата, шкала и другие элементы конструкции выполнены с возможностью их взаимного перемещения и закрепления.
Недостатком данного устройства является необходимость дополнительных настроечных операций перед каждым измерением. Также он не предназначен для определения перемещений по вертикали.
Также известен сигнализатор угла наклона (Патент РФ №2156957, кл. 7 G01С 9/00), содержащий стакан с крышкой, в которой установлена шаровая опора с жестко закрепленными на ней двумя стержнями, нижний из которых соединен с маятником, расположенным внутри стакана, заполненного демпфирующей жидкостью, а верхний замыкает на массу контактные кольца при определенных углах крена машины, два регулировочных болта, каждый из которых соединен с одним из контактных колец так, что при их вывинчивании обеспечивается независимое перемещение контактных колец (соответственно вверх или вниз) вдоль оси сигнализатора.
Данный сигнализатор позволяет определять предельно допустимые углы наклона в вертикальных плоскостях, но не позволяет находить точное значение данных углов и их проекции на заданные ортогональные оси.
Задачей изобретения является автоматизированное определение углового положения относительно вертикали и подъема тележки мобильного робота в том случае, когда размеры препятствий по длине и ширине не превышают габариты тележки.
Поставленная задача достигается тем, что предложен способ контроля ориентации тележки транспортного средства при его движении, заключающийся в выборе на тележке декартовой системы координат с горизонтальной продольной x, горизонтальной поперечной у и вертикальной осью z, образующими правую тройку, причем тележка имеет четыре опорные точки, лежащие симметрично по отношению к центру системы координат на расстоянии А от оси x и на расстоянии В от оси у, на тележке выбирают точки с координатами (х 1, у1), (х2 , у2), (х3, у 3), не лежащие на одной прямой, в них стационарно устанавливают вертикальные сообщающиеся между собой трубки с жидкостью, снабженные датчиками уровня, и определяют в каждой из трубок уровень жидкости - z1, z2, z 3, после чего рассчитывают по формулам величины, характеризующие ориентацию тележки: угол поворота тележки вокруг горизонтальной продольной оси x, угол поворота вокруг горизонтальной поперечной оси у и величину h вертикального подъема центра системы координат:
=arcsin{[(у2-у1 )(z3-z1)-(у 3-у1)(z2-z 1)]/[(х3-x1 )(у2-у1)-(х 2-х1)(у3-у 1)]};
=arcsin{[(x2-x1 )(z3-z1)-(х 3-x1)(z2-z 1)]/([(х3-x1 ])(у2-у1)-(х 2-x1)(у3-у 1)]·cos )};
h=A·|sin |+В·|sin cos |.
На чертеже схематически на примере четырехколесного привода дан общий вид тележки мобильного робота в несмещенном положении, а также показано перемещение системы координат, жестко связанной с тележкой при повороте тележки вокруг оси x на угол и дополнительном повороте вокруг оси у на угол .
На тележке мобильного робота 1 показано исходное (без перекосов) положение горизонтальной продольной оси x, горизонтальной поперечной оси у и вертикальной оси z. Также показано положение осей после поворота тележки вокруг оси x на угол - х', у', z', а также после дополнительного поворота тележки вокруг оси у на угол - х", у", z". На тележке 1 в точках с координатами (x1, у1), (х 2, у2), (х3 , у3), не лежащих на одной прямой, установлены три вертикальные трубки 2, 3, 4 с жидкостью, образующие сообщающиеся сосуды и снабженные датчиками 5, 6, 7, определяющими в каждой из трубок уровень жидкости по координате z, z 1, z2, z3.
Рассмотрим функционирование данной конструкции. При отсутствии перекосов уровень жидкости во всех трех трубках одинаково. Обозначим его через z0. После поворота тележки вокруг оси x на угол , а также после дополнительного поворота вокруг оси у на угол положение жидкости относительно уровня z 0 в трубках 2, 3, 4 изменится. Обозначим измененные уровни жидкости в трубках через z1, z 2, z3. При этом, если трубка поднимается относительно среднего уровня, уровень жидкости в ней будет меньше z0 и наоборот, при опускании трубки - превышать z0. Сигналы с датчиков 5, 6, 7 по уровням z1, z2, z 3 передаются на вычислительное устройство. Рассмотрим алгоритм их обработки.
Матрицы, задающие элементарные преобразования координат трехмерных векторов при рассмотренных поворотах имеют вид:
Преобразование координат трехмерных векторов при рассмотренных поворотах задается умножением их слева на матрицу
Изменение уровня в каждой из трубок 1, 2, 3, равное (z0-z1), (z 0-z2), (z0-z 3), совпадает с третьей компонентой (по оси z) векторов
Из выражений для разностей уровней (z 2-z1) и (z3 -z1) получается следующая система уравнений:
С учетом того, что реальные углы наклона и лежат в интервале от - /2 до + /2, зависимость углов и от измененных уровней жидкости z1 , z2, z3 будет следующей:
=arcsin{[(у2-у1 )(z3-z1)-(у 3-у1)(z2-z 1)]/detMp};
=arcsin{[(x2-x1 )(z3-z1)-(х 3-x1)(z2-z 1)]/(detMp·cos )};
Очевидно, углы и можно рассчитать тогда и только тогда, когда detM p 0. Это условие эквивалентно тому, что точки (x 1, у1), (х2 , у2), (х3, у 3) не лежат на одной прямой. При любых значениях углов перекоса и величина подъема центральной точки тележки h равна
h=A·|sin |+В·|sin cos |.
Анализ случаев =0 и =0 дополнительно позволяет уточнить характер контакта опорных точек тележки с горизонтальной поверхностью.
Таким образом, расчеты углов , и смещения h позволяют полностью решить задачу автоматизированного контроля ориентации тележки в случае, когда размеры препятствий по длине и ширине не превышают ее габариты.
Пример.
1. Общая характеристика конструкции.
У четырехколесной тележки ширина просвета между колесами равна 2А=0,4 м, длина просвета между колесами равна 2В=0,6 м.
Вертикальные сообщающиеся между собой трубки с жидкостью, снабженные датчиками уровня, установлены на тележке в точках с координатами (x 1=0,1 м, у1=0,1 м), (х 2=0,1 м, у2=-0,1 м), (х 3=-0,1 м, у3=0 м).
Вспомогательный определитель Мр имеет вид:
Величина определителя detMp =-0,04.
2. Выполняемые действия.
При измерении в трубках получены следующие уровни жидкости: z 1=0,116 м, z2=0,141 м, z 3=0,129 м.
Рассчитываем угол поворота тележки вокруг горизонтальной продольной оси x, угол поворота вокруг горизонтальной поперечной оси у и величину h вертикального подъема центра системы:
=arcsin{[(у2-у1 )(z3-z1)-(у 3-у1)(z2-z 1)]/detMp}=arcsin{[(-0,2)(0,013)-(-0,1)(0,025)]/(-0,04)}=arcsin{0,0025}=8,6';
=arcsin{[(x2-x1 )(z3-z1)-(х 3-x1)(z2-z 1)]/(detMp·cos )}=arcsin{[0(0,013)-(-0,2)(0,025)]/(-0,04)}=arcsin{0,125}=7 0 10,8';
h=A·|sin |+B·|sin cos |=0,2·0,0025+0,3·0,125=0,0425 м=42,5 мм.
Таким образом, по показаниям датчиков уровня жидкости и по конструктивным параметрам тележки и измерительной системы рассчитаны все величины, характеризующие местную ориентацию тележки.
Класс G01C9/00 Приборы для измерения уклонов, например с помощью клинометров, нивелиров