способ определения пространственного положения и угловой ориентации тележки транспортного средства

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

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

где ( ₁, ₂, ₃), ( ₁, ₂, ₃) - угловые положения маяков, измеренные при помощи датчиков,

_ip(x_Ол, y _Ол, z_Ол, , , ), _ip(x_Ол, y _Ол, z_Ол, , , ), где i=1, 2, 3, расчетные значения соответствующих углов, определяемые по следующим зависимостям:

где - радиус-вектор i-го маяка в локальной системе координат тележки,

где x_iл, y _iл, z_iл - видимые с тележки локальные координаты маяков, рассчитываемые по формулам:

x _iл=(cos cos +sin sin sin )(x_i-x_Ол)+(-cos sin +sin sin cos )( _i-y_Ол)+(sin cos )(z_i-z_Ол)-x _лi;

y_iл=(cos sin )(x_i-x_Ол)+(cos cos )(y_i-y_Ол)-sin (z_i-z_Ол)- _лi;

z_iл =(-sin cos +cos sin sin )(x_i-x_Ол)+(sin sin +cos sin cos )(y_i-y_Ол)+(cos cos )(z_i-z_Ол)-z _лi,

где x_i, _i, z_i - абсолютные координаты маяков;

х_Ол, y _Ол, z_Ол - абсолютные декартовые координаты центра тележки;

, , - углы поворота тележки вокруг осей x, y, z.

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

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

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

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

Также известен мобильный робот (Патент РФ №2210492, кл. МКИ В25J 9/00, 2003), который оснащен устройством определения местоположения, соединенным с управляющей частью, и содержащим обзорную видеокамеру для создания обзорного изображения потолка помещения и распознавания базового знака на потолке.

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

Наиболее близкой по совокупности признаков является система (In Procedings of the SPIE, volume 4868 (SPIE2002) pp.269-276. Boston, MA. August 2002 Sensor Coverage using Mobile Robots and Stationary Nodes Maxim A. Batalina and Gaurav S. Sukhatmea University of Southern California, Robotics Research Lab, Los Angeles, CA 90089, USA), включающая сеть статических многоцелевых датчиков, используемых, в том числе, для идентификации местоположения мобильных роботов. Используемая при этом радиосвязь позволяет найти координаты мобильного робота очень приближенно.

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

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

где ( ₁, ₂, ₃), ( ₁, ₂, ₃) - угловые положения маяков, измеренные при помощи датчиков.

_ip(x_Ол, y _Ол, z_ол, , , ), _ip(x_Ол, y _Ол, z_ол, , , ), где i=1, 2, 3 - расчетные значения соответствующих углов, определяемые по следующим зависимостям:

где - радиус-вектор i-го маяка в локальной системе координат тележки;

где х_iл, y _iл, z_iл - видимые с тележки локальные координаты маяков, рассчитываемые по формулам

х _iл=(cos cos +sin sin sin )(x_i-x_Ол)+(-cos sin +sin sin cos )(y_i-y_Ол)+(sin cos )(z_i-z_Ол)-x _iл;

y_iл=(cos sin )(x_i-x_Ол)+(cos cos )(y_i-y_Ол)-sin (z_i-z_Ол)-y _iл;

z_iл=(-sin cos +cos sin sin )(x_i-x_Ол)+(sin sin +cos sin cos )(y_i-y_Ол)+(cos cos )(z_i-z_Ол)-z _iл;

где x_i, y _i, z_i - абсолютные координаты маяков;

х_Ол, y_Ол, z _Ол - абсолютные декартовые координаты центра тележки;

, , - углы поворота тележки вокруг осей x, y, z.

На фиг.1 схематически дан общий вид тележки 1 и помещения, в котором указана абсолютная система координат Oxyz. На тележке 1 указана локальная система координат О_лx _лy_лz_л, а также система координат O_лx' _лy'_лz'_л , у которой оси ориентированы параллельно осям абсолютной системы координат Oxyz. Также на тележке 1 жестко закреплены датчики углового положения 2, 3, 4, имеющие локальные координаты (x _л1, y_л1, z_л1 ), (x_л2, y_л2, z _л2), (x_л3, y_л3 , z_л3). В помещении в точках с абсолютными координатами (x₁, y₁ , z₁), (х₂, y ₂, z₂), (х₃ , y₃, z₃) неподвижно установлены маяки 5, 6, 7.

На фиг.2 показана проекция на абсолютную плоскость Oxy тележки и маяков при приближенном расчете - при допущениях z_Ол=h, =0, =0, x_iл 0, y_iл 0, z_iл 0, (i=1, 2, 3).

Практически способ реализуется следующим образом. Следящие датчики 2, 3, 4 определяют углы склонения ( ₁, ₂, ₃) относительно плоскости О _лx_лy_л и поворота ( ₁, ₂, ₃) относительно плоскости O _лx_лz_л, положения маяков 5, 6, 7 и передают данные величины вычислительному устройству. Поскольку величины данных углов связаны с величинами необходимых декартовых координат (х_Ол, y _Ол, z_Ол) центра локальной декартовой системы координат тележки О_л и углов поворота ( , , ) тележки вокруг осей z, y, x сложной зависимостью, то для ее раскрытия предлагается двухступенчатый алгоритм.

Первый шаг вычислений

Поскольку тележка перемещается по поверхностям, близким к горизонтальным, то положение точки O _л имеет координату по оси z, близкую к h, а углы перекоса тележки вокруг абсолютных осей x,y близки к нулю: z _Ол=h, =0, =0. Также величины локальных координат (x _л1, y_л1, z_л1 ), (х_л2, y_л2, z _л2), (х_л3, y_л3 , z_л3) значительно меньше абсолютных декартовых координат (х_Ол, y_Ол ) центра тележки, поэтому при приближенном расчете можно принять допущение о том, что датчики 2, 3, 4 находятся в центре тележки О_л: х_лi 0, y_лi 0, z_лi 0 (i=1, 2, 3).

При данных допущениях приближенные значения абсолютных декартовых координат (х*_Ол , y*_Ол) центра О_л и угла поворота * тележки вокруг оси z могут быть найдены только по величинам углов поворота ( ₁, ₂, ₃) относительно плоскости О _лx_лz_л положения маяков 5, 6, 7 (фиг.2).

Геометрическое место искомой точки O_л относительно маяков 5, 6 является дуга окружности, у которой угол равен ( ₁- ₂).

Введя расстояние между маяками 5 и 6: ₁₂=((x₁-х ₂)²+(y₁-y ₂)²)^0,5, координаты (x₁₂, y₁₂) центра окружности O₁₂ и ее радиус R ₁₂ предложено рассчитать по формулам

Аналогично можно рассчитать:

- координаты (x₃₁, y₃₁) центра O₃₁ и радиус R₃₁ окружности, проходящей через О_л и маяки 5, 7;

- координаты (x₂₃, y ₂₃) центра О₂₃ и радиус R ₂₃ окружности, проходящей через О_л и маяки 6, 7.

Для определения приближенных значений абсолютных декартовых координат (x*_Ол, y* _Ол) центра тележки О_л предложено использовать систему уравнений

Она сводится к системе, линейной относительно неизвестных (x*_Ол, y*_Ол):

где

Решение системы имеет вид

где

Для определения приближенного значения угла поворота * тележки вокруг оси z по найденным приближенным значениям абсолютных декартовых координат (х*_Ол, y*_Ол) центра тележки О_л вычисляем угол _1абс, образованный направлением на маяк 5 с абсолютной осью x из соотношений:

=((x*_Ол-x_М1 )²+(y*_Ол-y _М1)²)^0,5,

cos _1абс=(x_М1-x* _Ол)/ ;

sin _1абс=(y_М1-y* _Ол)/ .

Приближенное значение угла поворота * равно: *= _1абс- ₁.

Второй шаг вычислений

Принимая в качестве начального приближения величины координат (х*_Ол, y*_Ол, h, *, 0, 0) точные их значения (х_Ол, y_Ол, z_Ол, , , ) определяем путем численного решения системы уравнений

где ( ₁, ₂, ₃), ( ₁, ₂, ₃) - угловые положения маяков 5, 6, 7, измеренные при помощи датчиков 2, 3, 4.

_ip(x_Ол, y _Ол, z_Ол, , , ), _ip(x_Ол, y _Ол, z_Ол, , , ), (i=1, 2, 3) - расчетные значения соответствующих углов, определяемые по следующим зависимостям.

Для определения последних величин сначала находим видимые локальные координаты маяков их соответствующих датчиков:

х_iл =(cos cos +sin sin sin )(x_i-x_Ол)+(-cos sin +sin sin cos )(y_i-y_Ол)+(sin cos )(z_i-z_Ол)-x _iл;

y_iл=(cos sin )(x_i-x_Ол)+(cos cos )(y_i-y_Ол)-sin (z_i-z_Ол)-y _iл;

z_iл=(-sin cos +cos sin sin )(x_i-x_Ол)+(sin sin +cos sin cos )(y_i-y_Ол)+(cos cos )(z_i-z_Ол)-z _iл;

Потом по найденным координатам рассчитываем видимые значения углов:

;

Пример

1. Общая характеристика окружающей среды и конструкции тележки

В окружающей среде помещены три маяка 5, 6, 7, координаты (х₁, y ₁, z₁), (x₂ , y₂, z₂), (х ₃, y₃, z₃) которых в абсолютной декартовой системе Oxyz будут следующими:

Расстояния между маяками в плоскости Oxy:

₁₂=((x₁-x ₂)²+(y₁-y ₂)²)^0,5=((2,2) ²+(5,0)²)^0,5 =5,463 м,

₃₁=((x₁-x ₃)²+(y₁-y ₃)²)^0,5=((1,8) ²+(5,0)²)^0,5 =5,314 м,

₂₃=((x₂-x ₃)²+(y₂-y ₃)²)^0,5=((4,0) ²+(0)²)^0,5=4,000 м.

На тележке на высоте h=0,25 м жестко закреплены датчики 2, 3, 4 углового положения, имеющие локальные координаты:

2. Выполненные измерения

По показаниям датчиков 2, 3, 4 определены наблюдаемые с них углы склонения ( ₁, ₂, ₃) относительно плоскости О _лx_лy_л и поворота ( ₁, ₂, ₃) относительно плоскости О _лx_лz_л положения маяков 5, 6, 7:

₁=37°2'; ₂=42°18'; ₃=34°48'; ₁=343°49'; ₂=152°39'; ₃=87°18'.

3. Вычисления

Первый шаг

Вначале рассчитываем координаты центров и радиусы окружностей, проходящих через проекции маяков в плоскости Оху и центр тележки:

Затем рассчитываем вспомогательные величины

det =(-9,68)(-1,78)-(1,71)(-5,63)=21,19.

Решение системы имеет вид:

Для определения приближенного значения угла поворота * тележки вокруг оси z вначале находим характеристики угла _1абс, образованного направлением на маяк 5 с абсолютной осью x:

=((x*_Ол-Х_М1 )²+(y*_Ол-y _М1)²)^0,5=((3,92-4,2) ²+(2,82-0)²)^0,5 =2,84;

cos _1абс=(x_М1-x* _Ол)/ =1,13/2,78=-0,621;

sin _1абс=(y_М1-y* _Ол)/ =-1,43/2,78=0,786.

По данным характеристикам вычисляем величину _1абс:

_1абс=-84°25'.

Завершаем первый шаг вычислением приближенного значения угла поворота *:

*= _1абс- ₁=-84°25'-343°49'=-68°10'.

Таким образом, на первом шаге определены приближенные значения абсолютных декартовых координат (х*_Ол y* _Ол) центра О_л и угла поворота * тележки вокруг оси z.

Второй шаг

Принимаем в качестве начального приближения для численного решения системы уравнений методом Розенброка величины координат:

x _Ол=х*=3,92 м, y_Ол=y* _Ол=2,82 м, z_Ол=h=0,25 м,

= *=-68°10', =0, =0.

Первая итерация численного расчета

Вычисляем видимые с тележки локальные координаты маяков:

x _1л=(cos cos +sin sin sin )(x₁-x_Ол)+(-cos sin +sin sin cos )(y₁-y_Ол)+(sin cos )(z₁-z_Ол)-x _л1=2,251;

x_2л=(cos cos +sin sin sin )(x₂-x_Ол)+(-cos sin +sin sin cos )(y₂-y_Ол)+(sin cos )(z₂-z_Ол)-x _л2=-3,01;

х_3л=(cos cos +sin sin sin )(x₃-x_Ол)+(-cos sin +sin sin cos )(y₃-y_Ол)+(sin cos )(z₃-z_Ол)-х _л3=-0,26;

y_1л=(cos sin )(x₁-x_Ол)+(cos cos )(y₁-y_Ол)-sin (z₁-z_Ол)-y _л1=-1,68;

y_2л=(cos sin )(x₂-x_Ол)+(cos cos )(y₂-y_Ол)-sin (z₂-z_Ол)-y _л2=-0,001;

y_3л=(cos sin )(x₃-x_Ол)+(cos cos )(y₃-y_Ол)-sin (z₃-z_Ол)-y _л3=2,98;

z_1л=(-sin cos +cos sin sin )(x₁-x_Ол)+(sin sin +cos sin cos )(y₁-y_Ол)+(cos cos )(z₁-z_Ол)-z _л1=2,75;

z_2л=(-sin cos +cos sin sin )(x₂-x_Ол)+(sin sin +cos sin cos )(y₂-y_Ол)+(cos cos )(z₂-z_Ол)-z _л2=2,75;

z_3л=(-sin cos +cos sin sin )(x₃-x_Ол)+(sin sin +cos sin cos )(y₃-y_Ол)+(cos cos )(z₃-z_Ол)-z _л3=2,75

Потом по найденным координатам рассчитываем видимые значения углов:

_1p=-36,79°; _1p=62,75°;

_2р=179,99°; _2p=56,3°;

_3p=94,96°; _3p=56,5°.

Зададим целевую функцию и критерий оптимальности в виде

При найденных значениях

F(x _Ол, y_Ол, z_Ол , , , )=0,775.

Для решения системы уравнений использован метод Розенброка. Получены последовательные приближения декартовых координат и углов поворота тележки, которым соответствуют следующие значения целевой функции:

Таким образом, в результате получены уточненные величины декартовых координат х_Ол=3,02 м, y_Ол=2,04 м; z_Ол =3,25 м и углов поворота тележки =-44,69°; =0,08°; =0,07°, определяющие пространственное положение и угловую ориентацию тележки мобильного робота, что подтверждает предполагаемый результат.