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

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

Способ определения параметров, характеризующих ориентацию тележки транспортного средства при ее движении, заключающийся в выборе на тележке декартовой системы координат с горизонтальной продольной 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 |.

Описание изобретения к патенту

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

Известно устройство для определения углов наклона объектов (Патент РФ №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 мм.

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