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

Изобретение относится к робототехнике и предназначено для определения углового положения, а также подъема тележки мобильного робота при его перемещении по неровной поверхности в том случае, когда размеры препятствий по длине и ширине не превышают габариты тележки. Техническим результатом является автоматизированное определение углового положения относительно вертикали и подъема тележки мобильного робота. На тележке выбирается декартова система координат с горизонтальной продольной x, горизонтальной поперечной у и вертикальной осью z, образующими правую тройку. Причем тележка имеет четыре опорные точки, лежащие симметрично по отношению к центру системы координат на расстоянии А от оси x и на расстоянии В от оси у. На тележке выбирают точки с координатами (x₁, у₁), (х₂, у₂), (х₃, у₃), не лежащие на одной прямой, в них стационарно устанавливают вертикальные сообщающиеся между собой трубки с жидкостью, снабженные датчиками уровня, и определяют в каждой из трубок уровень жидкости - z₁, z₂, z₃. После чего рассчитывают по формулам величины, характеризующие ориентацию тележки: угол поворота α тележки вокруг горизонтальной продольной оси x, угол поворота β вокруг горизонтальной поперечной оси у и величину h вертикального подъема центра системы координат:

β=arcsin{[(у₂-у₁)(z₃-z₁)-(у₃-у₁)(z₂-z₁)]/[(х₃-x₁)(у₂-у₁)-(х₂-х₁)(у₃-у₁)]};

α=arcsin{[(x₂-x₁)(z₃-z₁)-(х₃-x₁)(z₂-z₁)]/([(х₃-x₁])(у₂-у₁)-(х₂-x₁)(у₃-у₁)]·cosβ)};

h=A·|sinβ|+В·|sinαcosβ|.

1 ил.

 

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

Известно устройство для определения углов наклона объектов (Патент РФ №2142613, кл. G01С 9/02, 1998), содержащее панель с подвешенным на ней маятником и шкалу, закрепленную на панели, при этом панель, плата, шкала и другие элементы конструкции выполнены с возможностью их взаимного перемещения и закрепления.

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

Также известен сигнализатор угла наклона (Патент РФ №2156957, кл. 7 G01С 9/00), содержащий стакан с крышкой, в которой установлена шаровая опора с жестко закрепленными на ней двумя стержнями, нижний из которых соединен с маятником, расположенным внутри стакана, заполненного демпфирующей жидкостью, а верхний замыкает на массу контактные кольца при определенных углах крена машины, два регулировочных болта, каждый из которых соединен с одним из контактных колец так, что при их вывинчивании обеспечивается независимое перемещение контактных колец (соответственно вверх или вниз) вдоль оси сигнализатора.

Данный сигнализатор позволяет определять предельно допустимые углы наклона в вертикальных плоскостях, но не позволяет находить точное значение данных углов и их проекции на заданные ортогональные оси.

Задачей изобретения является автоматизированное определение углового положения относительно вертикали и подъема тележки мобильного робота в том случае, когда размеры препятствий по длине и ширине не превышают габариты тележки.

Поставленная задача достигается тем, что предложен способ контроля ориентации тележки транспортного средства при его движении, заключающийся в выборе на тележке декартовой системы координат с горизонтальной продольной x, горизонтальной поперечной у и вертикальной осью z, образующими правую тройку, причем тележка имеет четыре опорные точки, лежащие симметрично по отношению к центру системы координат на расстоянии А от оси x и на расстоянии В от оси у, на тележке выбирают точки с координатами (х₁, у₁), (х₂, у₂), (х₃, у₃), не лежащие на одной прямой, в них стационарно устанавливают вертикальные сообщающиеся между собой трубки с жидкостью, снабженные датчиками уровня, и определяют в каждой из трубок уровень жидкости - z₁, z₂, z₃, после чего рассчитывают по формулам величины, характеризующие ориентацию тележки: угол поворота α тележки вокруг горизонтальной продольной оси x, угол поворота β вокруг горизонтальной поперечной оси у и величину h вертикального подъема центра системы координат:

β=arcsin{[(у₂-у₁)(z₃-z₁)-(у₃-у₁)(z₂-z₁)]/[(х₃-x₁)(у₂-у₁)-(х₂-х₁)(у₃-у₁)]};

α=arcsin{[(x₂-x₁)(z₃-z₁)-(х₃-x₁)(z₂-z₁)]/([(х₃-x₁])(у₂-у₁)-(х₂-x₁)(у₃-у₁)]·cosβ)};

h=A·|sinβ|+В·|sinαcosβ|.

На чертеже схематически на примере четырехколесного привода дан общий вид тележки мобильного робота в несмещенном положении, а также показано перемещение системы координат, жестко связанной с тележкой при повороте тележки вокруг оси x на угол α и дополнительном повороте вокруг оси у на угол β.

На тележке мобильного робота 1 показано исходное (без перекосов) положение горизонтальной продольной оси x, горизонтальной поперечной оси у и вертикальной оси z. Также показано положение осей после поворота тележки вокруг оси x на угол α - х', у', z', а также после дополнительного поворота тележки вокруг оси у на угол β - х", у", z". На тележке 1 в точках с координатами (x₁, у₁), (х₂, у₂), (х₃, у₃), не лежащих на одной прямой, установлены три вертикальные трубки 2, 3, 4 с жидкостью, образующие сообщающиеся сосуды и снабженные датчиками 5, 6, 7, определяющими в каждой из трубок уровень жидкости по координате z, z₁, z₂, z₃.

Рассмотрим функционирование данной конструкции. При отсутствии перекосов уровень жидкости во всех трех трубках одинаково. Обозначим его через z₀. После поворота тележки вокруг оси x на угол α, а также после дополнительного поворота вокруг оси у на угол β положение жидкости относительно уровня z₀ в трубках 2, 3, 4 изменится. Обозначим измененные уровни жидкости в трубках через z₁, z₂, z₃. При этом, если трубка поднимается относительно среднего уровня, уровень жидкости в ней будет меньше z₀ и наоборот, при опускании трубки - превышать z₀. Сигналы с датчиков 5, 6, 7 по уровням z₁, z₂, z₃ передаются на вычислительное устройство. Рассмотрим алгоритм их обработки.

Матрицы, задающие элементарные преобразования координат трехмерных векторов при рассмотренных поворотах имеют вид:

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

Изменение уровня в каждой из трубок 1, 2, 3, равное (z₀-z₁), (z₀-z₂), (z₀-z₃), совпадает с третьей компонентой (по оси z) векторов

Из выражений для разностей уровней (z₂-z₁) и (z₃-z₁) получается следующая система уравнений:

С учетом того, что реальные углы наклона α и β лежат в интервале от -π/2 до +π/2, зависимость углов α и β от измененных уровней жидкости z₁, z₂, z₃ будет следующей:

β=arcsin{[(у₂-у₁)(z₃-z₁)-(у₃-у₁)(z₂-z₁)]/detM_p};

α=arcsin{[(x₂-x₁)(z₃-z₁)-(х₃-x₁)(z₂-z₁)]/(detM_p·cosβ)};

Очевидно, углы α и β можно рассчитать тогда и только тогда, когда detM_p≠0. Это условие эквивалентно тому, что точки (x₁, у₁), (х₂, у₂), (х₃, у₃) не лежат на одной прямой. При любых значениях углов перекоса α и β величина подъема центральной точки тележки h равна

h=A·|sinβ|+В·|sinαcosβ|.

Анализ случаев α=0 и β=0 дополнительно позволяет уточнить характер контакта опорных точек тележки с горизонтальной поверхностью.

Таким образом, расчеты углов α, β и смещения h позволяют полностью решить задачу автоматизированного контроля ориентации тележки в случае, когда размеры препятствий по длине и ширине не превышают ее габариты.

Пример.

1. Общая характеристика конструкции.

У четырехколесной тележки ширина просвета между колесами равна 2А=0,4 м, длина просвета между колесами равна 2В=0,6 м.

Вертикальные сообщающиеся между собой трубки с жидкостью, снабженные датчиками уровня, установлены на тележке в точках с координатами (x₁=0,1 м, у₁=0,1 м), (х₂=0,1 м, у₂=-0,1 м), (х₃=-0,1 м, у₃=0 м).

Вспомогательный определитель М_р имеет вид:

Величина определителя detM_p=-0,04.

2. Выполняемые действия.

При измерении в трубках получены следующие уровни жидкости: z₁=0,116 м, z₂=0,141 м, z₃=0,129 м.

Рассчитываем угол поворота α тележки вокруг горизонтальной продольной оси x, угол поворота β вокруг горизонтальной поперечной оси у и величину h вертикального подъема центра системы:

β=arcsin{[(у₂-у₁)(z₃-z₁)-(у₃-у₁)(z₂-z₁)]/detM_p}=arcsin{[(-0,2)(0,013)-(-0,1)(0,025)]/(-0,04)}=arcsin{0,0025}=8,6';

α=arcsin{[(x₂-x₁)(z₃-z₁)-(х₃-x₁)(z₂-z₁)]/(detM_p·cosβ)}=arcsin{[0(0,013)-(-0,2)(0,025)]/(-0,04)}=arcsin{0,125}=7⁰ 10,8';

h=A·|sinβ|+B·|sinαcosβ|=0,2·0,0025+0,3·0,125=0,0425 м=42,5 мм.

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

Способ определения параметров, характеризующих ориентацию тележки транспортного средства при ее движении, заключающийся в выборе на тележке декартовой системы координат с горизонтальной продольной x, горизонтальной поперечной у и вертикальной осью z, образующими правую тройку, причем тележка имеет четыре опорные точки, лежащие симметрично по отношению к центру системы координат на расстоянии А от оси x и на расстоянии В от оси у, на тележке выбирают точки с координатами (x₁, у₁), (x₂, у₂), (х₃, у₃), не лежащие на одной прямой, в них стационарно устанавливают вертикальные сообщающиеся между собой трубки с жидкостью, снабженные датчиками уровня, и определяют в каждой из трубок уровень жидкости - z₁, z₂, z₃, после чего рассчитывают по формулам величины, характеризующие ориентацию тележки: угол поворота α тележки вокруг горизонтальной продольной оси x, угол поворота β вокруг горизонтальной поперечной оси y и величину h вертикального подъема центра системы координат:

β=arcsin{[(у₂-у₁)(z₃-z₁)-(у₃-у₁)(z₂-z₁)]/[(х₃-x₁)(у₂-у₁)-(х₂-х₁)(у₃-у₁)]};

α=arcsin{[(x₂-x₁)(z₃-z₁)-(х₃-x₁)(z₂-z₁)]/([(х₃-x₁])(у₂-у₁)-(х₂-x₁)(у₃-у₁)]·cosβ)};

h=A·|sinβ|+В·|sinαcosβ|.