CC BY
61
В рамках работы, в ответ на вызовы современных задач подземной навигации, были рассмотрены некоторые варианты начальной выставки гироинклинометров. Было показано, что для автономной азимутальной выставки в режиме компасирования необходимо применение высокоточных дорогостоящих чувствительных элементов, которые не только удорожают себестоимость конечного изделия, но и резко снижают его эксплуатационные характеристики. В то же время было продемонстрировано, что спутниковые навигационные системы в своем развитии достигли необходимых точностей определения ориентации. Принцип фазовых измерений, предложенный еще в середине прошлого века, позволил построить высокоточные спутниковые системы определения ориентации. В качестве источника внешнего курсоуказания было предложено воспользоваться существующими на данный момент приборами класса GPS-компас. Представлен вариант конструктивного решения, позволяющего связать базы приборов.
Следует отметить, что применение высокоточного внешнего курсоуказания значительно расширяет возможности использования современных инклинометров. Открываются широкие перспективы применения бесплатформенных гироинклинометров в областях, весьма далеких от добывающей промышленности - исторически сложившейся сферы применения инклинометров. Так, в качестве возможного варианта применения новой компоновки инклинометра можно предположить контрольные измерения хладагентных скважин метрополитена. Ранее подобные скважины выпадали из поля зрения инклинометрии в силу очень высоких требований к точности их измерения.
Литература
1. Калинин А.Г., Кульчицкий В.В. Естественное и искусственное искривление скважин: Учебное пособие для вузов. - Москва-Ижевск: НИЦ «Регулярная и хаотическая динамика»; Институт компьютерных исследований, 2006. - 640 с.
2. Биндер Я.И. Аналитическое компасирование в инклинометрии скважин малого диаметра // Гироскопия и навигация. - 2003. - № 2(41). - С. 38-46.
3. Пестряков В.Б. Радионавигационные угломерные системы. - М.: Госэнергоиздат, 1955. - 304 с.
4. Патент РФ № 2141118. Способ угловой ориентации объектов в пространстве / Фатеев Ю.Л., Чмых М.К. Опубл. 1999. Бюл. № 31.
5. Tans Vector: Проспект фирмы Trimble, 2004.
Первовский Владимир Сергеевич
Биндер Яков Исаакович
Санкт-Петербургский государственный университет информационных технологий, механики и оптики, аспирант, pwladimir@ya.ru
Санкт-Петербургский государственный университет информационных технологий, механики и оптики, кандидат технических наук, доцент, mail@elmech.ru
УДК 629.1.033
ПРОГРАММНОЕ ДВИЖЕНИЕ ДВУНОГОГО ШАГАЮЩЕГЕО РОБОТА В САГИТТАЛЬНОЙ ПЛОСКОСТИ
Р.А. Алексеев, Ю.П. Котельников
Рассматривается комфортабельное движение корпуса двуногого робота, для которого заданы в декартовых координатах программные траектории таза и стоп. Предлагается аналитический расчет задающих воздействий на приводы исполнительного механизма с использованием решения обратных задач кинематики.
Ключевые слова: двуногий робот, комфортабельное движение, программные траектории, обратная задача кинематики, точка нуль-момента.
Введение
Для получения задающих воздействий (ЗВ) на приводы исполнительного механизма (ИМ), осуществляющего комфортабельное движение [1, 2] корпуса двуногого шагающего робота (ДШР), необходимо задать в декартовых координатах либо согласованные (в смысле физической реализуемости кинематической схемой) траектории всех звеньев механизма [3], либо программные траектории части звеньев механизма [1,4-6]. Вычисление на их основании ЗВ составляет обратную задачу кинематики (ОЗК), решение которой неоднозначно для механизма с большим числом звеньев [5, 7-9]. В работе предложено аналитическое решение ОЗК методом разделения общей задачи на составляющие ее частные ОЗК в двузвенниках, для которых известен закон движения в декартовых координатах, с последующим нахождением ЗВ по теореме косинуса для треугольников, образованных этими двузвенниками.
Постановка задачи
Для заданной кинематической схемы ДШР (рис. 1) необходимо обеспечить комфортабельное в сагиттальной плоскости движение корпуса, т.е. движение таза с постоянной скоростью (¥Т) на постоянной высоте (И0) над опорной поверхностью. Рассмотрим решение поставленной задачи для случая, когда в декартовых координатах задано программное движение корпуса и маховой стопы и следует определить программные задающие воздействия на приводы ИМ, обеспечивающие комфортабельное движение корпуса.
Тазобедренные шарниры
Левая нога
Голеностопные шарниры
Рис. 1. Кинематическая схема двуногого шагающего робота
Фазы и параметры движения ДШР
Пусть движение ДШР [4, 10, 11] имеет следующие фазы (см. рис. 2): а (одноопорная) - перемещение корпуса и правой ноги с опорой на левую ногу; б (двухопорная) - перенос тяжести корпуса с левой ноги на правую ногу с опорой на обе ноги;
в (одноопорная) - перемещение корпуса и левой ноги с опорой на правую ногу; г (двухопорная) - перенос тяжести корпуса с правой ноги на левую ногу с опорой на обе ноги.
Параметрами, определяющими походку робота [7, 12], являются (рис. 2): И0 - желаемая высота движения таза (тазобедренных суставов), 1ш - длина одного шага, ¡а -протяженность носочной части стопы (от голеностопа до носка), ¡ъ - протяженность пяточной части стопы (от голеностопа до пятки), ^ - начальный фазовый сдвиг таза (относительно голеностопа), АИ - высота подъема стопы над грунтом.
АН
..............тСТ*—
Н,
Но
1 б/ 4
а б в г
Рис. 2. Фазы движения ДШР
Траектория корпуса, также как и определяющие походку робота траектории стоп, зададим в декартовых координатах. Из всего разнообразия рассмотрены следующие траектории стоп (рис. 3): П-образная траектория (рис. 3, а), содержащая последовательный подъем (подфазы А1/В1), перенос (подфазы А2/В2) и опускание (подфазы А3/В3) стопы и, совмещающие подъем и опускание с горизонтальным переносом, Л-образная (рис. 3, б) и дугообразная (рис. 3, в) траектории, различия между которыми определяются изменением скоростей во времени.
Н
Н
Н
0
0
б
а б в
Рис. 3. Траектории таза и стоп. Сплошными линиями показаны траектории (правой стопы и таза) при шаге правой ногой, пунктирными линиями - траектории (левой стопы и таза) при шаге левой ногой
Системы координат
Для математического описания ДШР введены (рис. 4) следующие декартовы правые системы координат (СК): СК О'0„х'0пу'0„, (далее СКп), связанная с правой стопой ДШР, СК О0х0у0, (далее СК0), связанная с корпусом ДШР, а СК О'олх'0лу'ол, (далее СКл), связанная с левой стопой ДШР.
СКп и СКл позволяют рассматривать ИМ в фазах ходьбы А-Б и В-Г.
Вектора обобщенных координат 0'н = [9'1н 0'2н 0'3н 0'4н 0'5н 0'бн]Г, где индекс «н» принимает значения «п» или «л», однозначно определяют в СКп или СКл расположение отдельных звеньев (корпус, бедра, голени, стопы) и конфигурацию ДШР в целом.
СК0 введена в работе для пересчета обобщенных координат из СКп и СКл с целью получения вектора 0 = [91п 01л 02п 02л 0зп 0зл]Г, определяющего относительные углы поворота звеньев. Координаты в СК0 однозначно определяют положение механизма при условии, что одна из стоп (опорная) прилегает к опорной поверхности.
Обобщенные координаты в СК0 можно однозначно получить [4, 10, 11], используя следующие выражения:
а) из СКп: б) из СКл:
бзл = п-е\л, (11) езп = п-е\п, (2.1)
02л =-е '2л , (12) е2п = -е '2п , (2.2)
е1л = -е 'зл , (13) е1п = е 'зп , (2.з)
ео =е \л+е'2л+е'зл-п , (14) ео =е\п+е'2п+е'зп-п, (2.4)
е1п = е'4л-п, (1.5) е1л = е'4п -п, (2.5)
е2п =е 5л , (1.6) е2л =е'5п , (2.6)
е3п =е 'бл ; (1.7) езл = е'бп . (2.7)
а б
Рис. 4. Системы координат, используемые для математического описания ДШР:
а - СКп, б - СК0, в - СКл
Условие кинематической реализуемости походки ДШР
Программное движение корпуса, бедер, голеней, стоп при заданной кинематической схеме и параметрах ДШР возможно только в случае, если выполняются условия кинематической реализуемости походки.
Исходя из геометрических параметров ИМ и параметров ходьбы ДШР, получено кинематическое условие реализуемости походки (рис. 5), имеющее вид
(/ш + 5)2 + /0 < (/ + /2)2, (3)
где /\ и /2 - длины бедер и голеней. Обращение неравенства (4) в равенство позволяет определить наибольшую длину шага (/ш тах) при фиксированных ^ и И0:
Стах =4(А + /2)2 - % - * . (4)
Условие отсутствия неуправляемого движения ДШР
Потребуем, чтобы походка ДШР в сагиттальной плоскости была устойчивой, другими словами, потребуем, чтобы во всех фазах движения стопа опорной ноги не отрывалась носком или пяткой от опорной поверхности под действием моментов сил тяжести звеньев. В противном случае ДШР будет совершать неуправляемое движение, обусловленное моментами сил тяжести звеньев, действующих относительно точки О'0н - начала координат опорной ноги (см. рис. 4, а, в), которые можно определить по формуле
ыг = 2
к=/
( (к-1 ( I Л
О,к • 2 // • С08(Ее;)
/ =/ V ] =1
+ /к ' С08(2е;)
]=1
(5)
JJ
где О/ - вес /-го звена, /с/ - расстояние от /-го сочленения до (/+1)-го сочленения, /с/ -расстояние от /-го сочленения до центра масс /-го звена, 0г- - угол поворота /-го звена относительно (/-1)-го звена. Как известно, во всех фазах движения [7, 13] существует точка нуль-момента (т0м) в которой совокупная реакция опоры (грунта) уравновешивает все силы тяжести звеньев ИМ:
(0)
^=2 о,
Из уравнения баланса моментов относительно точки О'
0н
^Е ' Хт0м
= 2 ы-0)
(6)
(7)
/=0
можно определить хт0м - продольную координату точки нуль-момента.
а б
Рис. 5. Силы, действующие на ДШР, при сменах фаз опоры: а - постановка ступни на грунт, б - отрыв ступни от грунта
/=0
0
Условием отсутствия неуправляемого движения ДШР (рис. 5) под действием статических моментов (6) является невыход т0м в одноопорных фазах движения за пределы опорной стопы,
4 < *том < 4, (8а)
а в двухопорных фазах движения - за пределы обоих стоп и промежутка между ними,
/ъ < хтом < /ш + /а. (8б)
Тогда на стыках фаз можно записать условия устойчивого отрыва стопы, 4 < *том, (9а)
и устойчивой постановки стопы,
*Т0м < /а . (9б)
Полученные условия должны быть выполнены при расчете программных траекторий движения ДШР.
Расчет программных траекторий в декартовых координатах
Движение ДШР с заданной траекторией таза определяется траекториями движения ног. В работе рассматривается решение поставленной задачи путем задания желаемых траекторий таза и стоп в декартовых координатах с последующим определением желаемых обобщенных координат через ОЗК. Для выработки желаемых траекторий движения стоп и таза необходимо формировать для шага правой ногой (фазы А-Б) траектории правой стопы (х '5л (^), у '5л (^)) и таза (х '2л (^), у '2л (^)) в СКл, а для шага левой
ногой (фазы В-Г ) - траектории левой стопы (х '5п (^), у '5п(^)) и таза (х '2п (^), у '2п (^)) в
СКп. Дополнительным условием при ходьбе является поддержание перемещаемой стопы всегда в горизонтальном положении, что соответствует выполнению условия
е1 +е2 +е3 +е4+е5 +е6 = 2п (рис. 4).
V
Ум
V
V
т1б т+1 1
С
Т
V. = V. - ■
1 1 рт
ч—I-
т1б т1+1 1
а б в
Рис. 6. Графики изменения линейной скорости, используемые в работе: а - постоянное, б - трапециидальное, в - треугольное. Этапы движения: Р - разгон, С - стабильное движение, Т - торможение
воздействие постоянное трапециидальное треугольное
наивысшая скорость = = У1М = 2 • V
время этапы скорость этапы скорость этапы скорость
- ^ t < -a "с" V * = V у x2м у M "р" V* —^ (t -Ti) Т. -Т. ia i "р" V--Vm— (t--) tia TiM
Tia ^ t < -М "с" V * = V 'i * iM
-М ^ t < -б "т" V, * --Vm— (Ti+! -1) ti+1 TiM
-б ^ t < -+1 "т" v (т.+x - t) ti+1 т1б
Таблица 1. Этапы движения и желаемая линейная скорость на них при разных видах
воздействий
В работе исследовано движение ДШР при разных заданных траекториях стопы маховой ноги: П-образной, Л-образной и дугообразной (рис. 3). При этом рассматривались желаемые движения с постоянными, трапециидальными и треугольными графиками изменения линейной скорости (рис. 6) при одинаковом времени выполнения заданного движения, из чего следует, что средняя линейная скорость V постоянна (табл. 1), где i -номер задаваемого движения (1 - подъем стопы, 2 - перенос стопы, 3 - опускание стопы), Vi * - требуемая средняя составляющая линейной скорости движения звена, VM -
максимальная величина составляющей линейной скорости движения звена, а V*(t) -текущая составляющая линейной скорости движения звена.
Решение ОЗК и исходное положение робота
По заданным желаемым траекториям таза и маховой стопы ДШР при неподвижной опорной стопе требуется определить задающие воздействия на шесть приводов ИМ, т.е. Qm(t), 0ы(О при i = 1, 2, 3. Задача упрощается, если из СКп или СКл перейти в СК0 и получить желаемые траектории голеностопов (x'2n*(t); y'2n*(t)), (x'2jl*(t); у'2л*(0) относительно начала координат О0 (таза ДШР). Таким образом, ОЗК для всего робота распадается на две независимые локальные ОЗК для двузвенников (правой и левой ног).
Используя теорему косинуса для треугольников О0О1О2 и О3О4О5, можно получить ЗВ на приводы ДШР (желаемые углы взаимной ориентации звеньев): /2 /2 |* 2 |* 2
02н =-arccos'1н +/2н -Х2н + y 2н , (10.1)
е* =- arccos x '2н • (/1н + 12н C0s 62И ) + y 'L • L sin 02И , ( 2)
1н £ + £ + 2/^ cos е2н ' V '
езн = -е,н-е2н, (10.3)
где н = п для правой ноги и н = л для левой ноги.
1°. Следует отметить, что знак минус перед функцией арккосинуса соответствует движениям «коленом вперед», что снимает неоднозначность решения ОЗК.
2°. Решив ОЗК при желаемом исходном положении робота, получаем обобщенные координаты исходного положения, которые предварительно должны быть заложены в исследуемую симуляционную модель ИМ.
Полученные в результате компьютерного моделирования в программном продукте Matlab-Simulink [4, 10, 11] программные траектории обобщенных координат, обеспечивающие комфортабельное движение ДШР, приведены в табл. 2. Здесь программные обобщенные координаты обозначены следующим образом: «квадрат» - е*п (правый тазобедренный), «треугольник» - е22п (правый коленный), «круг» - е*л (левый тазобедренный), «ромб» - е*2л (левый коленный).
В двух правых столбцах приведены последовательно все подфазы движения в цикле ходьбы: А1/В1 - подъем правой/левой ступни, А2/В2 - перенос правой/левой ступни, А3/В3 - опускание правой/левой ступни, Б/Г - перемещение корпуса, р - разгон, с - стабильное движение, т - торможение.
Анализ траекторий относительного движения звеньев (табл. 2) показал, что траектории изменения обобщенных координат могут быть описаны полиномами времени не выше второго порядка. Следовательно, приводы отдельных степеней подвижности должны обладать астатизмом по отношению ко входному воздействию как минимум третьего порядка [14]. Тогда в точке постановки стопы на опорную плоскость будет обеспечена нулевая ошибка позиционирования.
Вид желаемых траекторий движения
Графики задающих воздействий на приводы ДШР
Стадии движения при шаге правой ноги
Стадии движения при шаге левой ноги
П-образная траектория стопы с постоянными скоростями и одновременным перемещением корпуса с постоянной скоростью
А1с+Бс А2с+Бс А3с+Бс Бс
В1с+ Гс В2с+ Гс В3с+ Гс Гс
П-образная траектория стопы с трапеции-дальными скоростями и одновременным перемещением корпуса с постоянной скоростью
А1 р+Бс В1 р+ Гс
А1 с+Бс В1 с+ Гс
А1т+Бс В1 т+ Гс
А2р+Бс В2р+ Гс
А2с+Бс В2с+ Гс
А2т+Бс В2т+ Гс
А3р+Бс В3р+ Гс
А3с+Бс В3с+ Гс
А3т+Бс В3т+ Гс
Бс Гс
Л-образная траектория стопы с постоянными скоростями и одновременным перемещением корпуса с постоянной скоростью
А1 с+А2с+Бс А3с+А2с+Бс Бс
В1с+В2с+Гс В3с+В2с+Гс Гс
Л-образная траектория стопы с трапеции-дальными скоростями и одновременным перемещением корпуса с постоянной скоростью
А1р+А2р+Бс А1с+А2р+Бс А1 с+А2с+Бс А1т+А2с+Бс А3р+А2с+Бс А3с+А2с+Бс А3с+А2т+Бс А3т+А2т+Бс Бс
В1р+В2р+Гс В1с+В2р+Гс В1с+В2с+Гс В1т+В2с+Гс В3р+В2с+Гс В3с+В2с+Гс В3с+В2т+Гс В3т+В2т+Гс Гс
Табл. 3. Виды желаемых траекторий в обобщенных координатах при разных траекториях стоп ДШР
Заключение
В работе приведены аналитические выражения для вычисления программных траекторий движения таза и стоп в декартовых координатах и получения на их основе задающих воздействий на приводы ДШР. Рассмотрены разные виды программных траекторий корпуса и стоп ДШР, обеспечивающих комфортабельное движение корпуса робота. Приведен анализ полученных при этом задающих воздействий. Даются рекомендации по синтезу системы управления локальными приводами исполнительного механизма.
Литература
1. Белецкий В.В., Бербюк В.Е. Нелинейная модель двуногого шагающего аппарата, снабженного управляемыми стопами. - М.: Наука, 1982.
2. Chevallereau C., Abba G., Aoustin Y., Plestan F., Westervelt E. R., Canudas-de-Wit C. and Grizzle J.W. RABBIT: A Testbed for Advanced Control Theory // IEEE Control Systems Magazine. - October, 2003. - Vol. 23. - № 5. - Р. 57-79.
3. Формальский А.М. Перемещение антропоморфных механизмов. - М.: Наука, 1982.
4. Алексеев Р.А., Котельников Ю.П. Расчет задающих воздействий для двуногого робота // Проблемы машиноведения и машиностроения: Межвузовский сборник. -Выпуск 37. - СЗТУ, 2007. - С. 147-155.
5. Медведев В.С., Лесков А.Г., Ющенко А.С. Системы управления манипуляционных роботов. - М.: Наука, 1978.
6. Смалюк А.Ф. Модель устойчивой пассивной ходьбы // Математическое моделирование деформируемого твердого тела: Сборник статей / Под ред. О.Л. Шведа. -Минск: ИТК НАН Белоруссии, 1999.
7. Белецкий В.В. Двуногая ходьба. Модельные задачи динамики и управления. - М.: Наука, 1984.
8. Бербюк В.Е. Динамика и оптимизация робототехнических систем. - К.: Наукова думка, 1989.
9. Fujimoto Yasutaka, Obata Satoshi, Kawamura Atsuo Robust Biped Walking with Active Interaction Control between Foot and Ground // Proc. of IEEE Int. Conf. on Robotics and Automation. - Leuven, Belgium. - May 1998. - Р. 2030-2035.
10. Алексеев Р.А., Мирошник И.В. Разработка алгоритма ходьбы двуногого робота // Научно-технический вестник СПбГУ ИТМО. - 2006. - Вып. 28. - С. 123-132.
11. Алексеев Р. А. Моделирование циклических процессов при передвижении двуногого робота // Научно-технический вестник СПбГУ ИТМО. - 2006. - Вып. 33. - С. 35-47.
12. Чигарев А.В., Михасев Г.И. Биомеханика: Учебное пособие. - Минск: УП Техно-принт, 2004.
13. Вукобратович М.К. Шагающие роботы и антропоморфные механизмы. - М.: Мир, 1976.
14. Бессекерский В.А., Попов Е.П. Теория систем автоматического регулирования. -М.: Наука, 1972.
Алексеев Ростислав Александрович — Санкт-Петербургский государственный универси-
тет информационных технологий, механики и оптики, аспирант, RostAlexeev@mail.ru
Котельников Юрий Петрович — Санкт-Петербургский государственный универси-
тет информационных технологий, механики и оптики, кандидат технических наук, доцент, kotel@mail.ifmo.ru
