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

Abstract

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

Description

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

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

Claims (1)

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

   

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

   α_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.

Images