Разработка и исследование мехатронной мобильной системы шарового типа

- При недостаточных коэффициентах в ПД управлении по углу объект начинал поворот слишком поздно и слишком слабо, так что его "заносило" и он сбивался с траектории;

- при слишком больших этих коэффициентах объект слишком резко реагировал на изменения угла и начинал слишком преждевременные и резкие повороты, что также сбивало его с траектории;

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

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

- маленькая интегральная составляющая в управлении по скорости не успевала компенсировать статическую ошибку пропорционального управления;

- большая составляющая интегрального управления по скорости приводила к колебательности при небольших скоростях, когда не работает ограничение по модулю.

Таким образом, были подобраны такие коэффициенты в управлении, что была получена траектория, очень близкая к заданной. Результат моделирования системы с такими коэффициентами представлен на рис. 3.4.

Рис. 3.4 - Результат запуска модели с оптимальными коэффициентами управления

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

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

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

Однако, теперь встает задача управления полной моделью объекта без упрощений. Такая модель объекта описана в разд. 2.5, а разработка системы описана в разд. 3.3.

3.3 Разработка и исследование в пакете Matlab системы управления роботом c учетом внутренней динамики объекта

Итак, в предыдущем пункте данной работы рассматривалась система управления упрощенной моделью объекта.

Напомню особенности этой упрощенной модели:

- 2 входа, углы б₁ и б₂;

- 2 основных выхода, скорость V и угол поворота скорости Fi;

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

- порядок математической модели объекта 3-ий.

В следствие этого, управление и было реализовано по скорости и ее углу отдельно, а регуляторы в этих контурах линейные и простые.

Однако, для полной модели имеем следующие особенности:

- 2 входа, управляющие сигналы на двигателях тележки;

- 5 выходов, скорость, угол скорости и углы положения тележки;

- необходимость сложного нелинейного управления исходя из всех пяти выходов;

- порядок математической модели объекта 5-ый.

Основная задача управления остается той же: необходимо по задаваемому вектору скорости получать необходимые углы б₁ и б₂ и отрабатывать их.

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

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

На этой плоскости есть декартова система координат, на осях которой отложены углы -б₂ и б₁. Тележка располагается на ней в виде ориентированного вектора n, угол к нормали которого от оси абсцисс и является б₃. Эта система координат выглядит следующим образом, представленным на рис. 3.5.

Размещено на http://www.allbest.ru/

Рис. 3.5 - Вид тележки на плоскости в системе координат с углами

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

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

- Для прохождения роботом траектории максимально далеко необходимо постоянно поддерживать его скорость на уровне задаваемой. Это требование будет наиболее выполнено при поддержании задаваемого угла б₁ в первую очередь.

- Движение прямо из расчета по углу б2 производится только в случае, если разность в углах б₃ и б_3З меньше 90^о.

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

Входные параметры алгоритма: текущие углы б₁, б₂, б₃, задаваемые углы б₁, б₂, коэффициенты регулировки управления k1 и k2, длина оси тележки.

Выходные параметры алгоритма: управляющие воздействия y1, y2, полученный задаваемый угол б₃. Далее приводится описание самого алгоритма:

1) Приведение всех входных углов к требуемому виду. Для б₁, б₂ это промежуток от -р до р, для б₃ это промежуток от 0 до 2р.

2) Расчет углов разности между текущим и требуемым значениями для угла б₁ и б₂. Приведение угла разности по б₂ к значению в промежутке от -р до р.

3) Расчет требуемого угла поворота б₃. Расчет угла разности для б₃. Если груз находится на дне сферы, то угол разности по б₃ берется равным углу разности по б₂. Приведение угла разности по б₃ к значению в промежутке от -р до р.

4) Если угол разности по б₁ превосходит пороговое значение чувствительности алгоритма (для проведенных испытаний модели это значение равно 0.01), то регулировка скорости прямолинейного движения осуществляется по нему. А именно:

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

- Если груз не на дне сферы и угол его поворота б₃ не равен р или -р, то скорость прямолинейного движения прямо пропорциональна углу разности по б₁ и обратно пропорциональна косинусу угла поворота б₃.

- Если же груз не на дне сферы, а угол поворота б₃ близок к р или -р, то при значении угла разности по б₃ по модулю большем, чем р, прямо не двигаемся, а при меньшем, чем р, скорость прямолинейного движения пропорциональная углу разности по б₂.

- Если угол разности по б₁ меньше порогового значения, то регулируем по углу разности по б₂. Для этого требуется, чтобы его значение превышало пороговое. Если это условие выполняется, груз находится не на дне сфера, а угол разности по б₃ меньше по модулю, чем р, то скорость прямолинейного движения пропорциональна углу разности по б₂. Во всех других случая скорость прямолинейного движения равно 0.

5) Скорость поворота пропорциональная углу разности по б₃.

6) Скорость левого колеса вычисляется как скорость прямолинейного движения плюс скорость поворота, умноженная на половину длинны оси тележки. Скорость для правого угла вычисляется аналогично, однако приведенная скорость поворота вычитается.

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

Этот алгоритм реализован в блоке логического управления, описание которого приведено в Приложении 1.

Для работы алгоритма необходимы задаваемые значения углов б₁ и б₂. Эти значения предполагается получать с регуляторов, аналогичных регуляторам из разд. 3.2.

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

Таким образом, модель системы приобретает вид, подробно представленный в Приложении 1.

Произведем запуск этой модели без интегральной и дифференциальной составляющих управления по скорости и углу. Результат запуска представлен на рис. 3.6. Запуск модели производится из скрипта scr_sys_2.m, описание которого приведено в Приложении 2.

Рис. 3.6 - Результат запуска модели без интегрального и дифференциального управления

Как видно по представленному рисунку, ситуация позднего начала поворота и статическая ошибка по скорости имеют место, как и в случае с системой с упрощенным объектом, рассматриваемой в разд. 3.2.

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

Результат запуска системы представлен на рис. 3.7. Запуск модели полностью аналогичен запуску из предыдущего раза.



Рис. 3.7 - Результат запуска системы с интегральной и дифференциальной составляющей в управлении

Как видно по рисунку объект достаточно точно проследовал по задаваемой траектории. Это свидетельствует о том, что совокупность нелинейного регулятора с полной моделью объекта и упрощенная модель объекта весьма схожи. Данный факт позволяет говорить об удовлетворительном построении полной модели и алгоритма ее управления (при выбранных параметрах самого "колобка").

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

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

4. Разработка и исследование системы прямого компьютерного управления роботом

4.1 Разработка структуры компьютерного управления

Основные принципы разрабатываемой системы уже были приведены (см. разд. 1.4). Однако рассмотрим их еще раз.

Исследуемый объект - шар, с перемещаемым внутри него центром тяжести. Этот центр тяжести представляет из себя одноосную тележку (см. разд. 2.1), которая ездит по внутренней поверхности сферы робота. Для точного управления объектом необходимо получать данные о его состоянии. Сюда входит его положение на плоскости и положение груза на внутренней поверхности сферы. Получение такой информации возможно только с помощью специальной системы определения местоположения (для определения положения на плоскости) и гироскопа (для определения отклонения груза по углам). Использование таких систем сложно, дорогостояще и требует использования микроконтроллера для сбора данных, передачу их на ЭВМ, получения управляющих сигналов и их реализацию.

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

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

Вывод таких сигналов возможно сделать с выводов LPT-порта, так как частота работы порта более или менее позволяет выдавать ШИМ с требуемыми параметрами, его электрических параметров достаточно для управления ключевым преобразователем на полевых транзисторах, его программирование просто, а интерфейс предоставляет достаточное количество параллельно работающих выходных линий.

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

Формирование ШИМ-сигнала, битовой карты для LPT-порта, производится в основной программе на инструментальной ЭВМ. Эта же программа производит подготовку данных с клавиатуры (в том числе и считывание их) или из файла.

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

Полученная структура системы представлена на рис. 4.1.

Размещено на http://www.allbest.ru/

Рис. 4.1 - Структура системы прямого компьютерного управления

Далее будет приведено описание аппаратной и программной частей системы.

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

4.2 Аппаратная составляющая компьютерной системы управления

Поделим всю систему на две части: передающая часть и принимающая.

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

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

На обеих микросхемах, передатчика и приемника, есть резисторы, отвечающие за резонаторы и частоту работы радиопередачи.

Ключевой преобразователь реализован на полевых транзисторах, использование которых удобно совместно с LPT-портом. Управляющий сигнал на затворе и нагрузка подключены в соответствии со схемой подключения, представленной в документации для данного транзистора. Принципиальная схема коммутации данных с LPT-порта через ключевой преобразователь на микросхему представлена на рис. 4.2.

Размещено на http://www.allbest.ru/

Рис. 4.2 - Принципиальная схема коммутации сигнала от ЭВМ к радиопередатчику

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

Принципиальная схема передающей и принимающей частей представлена на рис. 4.3.

Размещено на http://www.allbest.ru/

Рис. 4.3 - Принципиальная схема передающей и принимающей частей системы

Из описания микросхем взята лишь информация о ее контактах, отвечающих за частоту работы.

На этих контактах и находятся резисторы, значение сопротивления которых влияет на частоту работы микросхем. Были измерены производственные значения сопротивления этих резисторов, посчитано соотношение этих сопротивлений для передающей и принимающей микросхемы и изменены в соответствии с этим соотношением таким образом, чтобы частота работы микросхемы соответствовала частоте работы LPT-порта.

4.3 ПО системы управления

Итак, исследуемая система компьютерного управления имеет в себе несколько программных составляющих. Ее структура представлена на рис. 4.4. Описания составляющих приведены далее.

Размещено на http://www.allbest.ru/

Рис. 4.4 - Структура программной составляющей системы управления роботом

Полевой терминальный задатчик - программа, предоставляемая магистрантом Ларионовой Еленой, созданная ею в рамках ее магистерской работы и предоставляющая терминальное задание модуля вектора скорости и его направления во времени для моделей системы.

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

Вычислитель управления, программный ШИМ и драйвер LPT-порта являют собой ПО инструментально ЭВМ, которое описано далее.

Как упомянуто в постановке задачи (см. разд. 1.4), инструментальное ПО в системе должно выполнять 3 функции:

1) Считывание управляющих импульсов либо с клавиатуры, либо из файла со значениями терминального управления в режиме реального времени и передачу в ШИМ.

2) Формирование ШИМ полученных управляющих воздействий.

3) Выдача полученных ШИМ-сигналов на LPT-порт.

Для реализации этой функции была написана программа на языке C++ и Assembler, описание этой программы представлено в Приложении 3.

Предполагаемая структура программы представлена на рис. 4.5.

Размещено на http://www.allbest.ru/

Рис. 4.5 - Структура программы на инструментальной ЭВМ

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

Общая структура программы такова.

В программе есть основной цикл. В нем происходит считывание нажатой клавиши (сюда же может быть добавлено считывание управляющих воздействий из файла), вывод текущих управляющих воздействий на экран при их изменении и формирование переменной, отвечающей за подаваемые в ШИМ управляющие воздействия. Внутри данного цикла присутствует мини-цикл, которые отвечает за реализацию ШИМ. Этот мини-цикл работает с частотой, максимально допустимой для работы с LPT-портом. В его первую итерацию все битовые выходные сигналы на движители выставляются в 1, а далее в нем проверяется номер текущей итерации; если он превосходит значение управляющего воздействия для каждого из сигналов, то бит соответствующего сигнала сбрасывается в 0. В конце мини-цикла эта битовая карта выводится в регистр LPT-порта.

Программа имеет два настраиваемых параметра: количество мини-циклов в основном цикле и пауза в конце итераций мини-цикла. Первый параметр отражает точность отработки сигнала, но его большое значение приводит к тому, что на движителях интегрирование ШИМ-сигнала не происходит по причине его затянутости. При последней реализации он был взят равным 4. Второй параметр - отражает частоту работы с LPT-портом. При последней реализации он был выставлен на максимум с обеспечением работоспособности порта - 2 (что соответствует задержке программы на 2 мс).

Выходом программы является битовая карта, которая подается на основной регистр LPT-порта. В этой карте нас интересуют первые 4 бита - те биты, которые подаются на первые 4 ножки вывода LPT-порта. Эти ножки соответствуют двум сигналам вращения двигателей вперед и двум сигналам вращения назад.

Управление с клавиатуры осуществляется нажатием следующих кнопок: Insert - увеличение управляющего воздействия на левом двигателе на 1 (максимум 4), Delete - уменьшение управляющего воздействия на левом двигателе на 1 (минимум -4), PageUp - увеличение управляющего воздействия на правом двигателе на 1 (максимум 4), PageDown - уменьшение управляющего воздействия на правом двигателе на 1 (минимум -4), Esc - выход из программы.

Следует пояснить, что если управляющее воздействие с тем или иным знаком равно значению первого из параметров программы, то и соответствующий двигатель будет постоянно крутиться в соответствующем направлении. Меньше этого параметра по модулю значение означает соответствующую долю сигнала в периоде всего импульса ШИМ-сигнала. При 0 сигнала нет, соответствующий двигатель не вращается.

4.4 Экспериментальное исследование разработанной системы

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

Такая проверка проводилась путем последовательного анализа сигналов в узлах с помощью осциллографа и анализом данных во время трассировки программы.

Тестирование моделей в системе Matlab позволило контролировать все их параметры и быстро локализовывать и находить ошибки.

Первый макет модели был выполнен из конструктора LEGO. Изначально в качестве двигателей использовались ДПТ LEGO. Редукторов при такой реализации не было.

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

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

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

Далее производилась замена ДПТ LEGO на ДПТ со встроенными редукторами, однако к моменту завершения работы эта замена произведена до конца не была.

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

Заключение

В данной работе исследовались мобильные роботы. Их анализ показал, что наиболее перспективным развитием колесных роботов широкого назначения являются шаровые мобильные роботы.

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

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

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

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

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

СПИСОК ИСПОЛЬЗУЕМЫХ ИСТОЧНИКОВ

1. Сайт компании Rotundus: www.rotundus.se

2. Сайт новостей в мире технологий: www.membrana.ru

3. Сайт международной конференции Диалог: www.dialog-21.ru

4. Сайт МГТУ "Станкин": npd.stankin.ru

5. Сайт новых изобретений и новостей: gadgets.compulenta.ru

6. Информационный сайт высоких технологий: www.hizone.info

6. Копылов И.П. Математическое моделирование электрических машин - Москва, 1987 - 248 с.

ПРИЛОЖЕНИЕ А

В данном приложении описываются модели подсистемы Simulink 6.1 системы компьютерного моделирования MATLAB версии 7.0, ссылки на которые приведены в основном тексте диссертации.

Для всех моделей в настройках симуляции в разделе решателя установлен тип шага - фиксированный, размер шага - 0.001 (SIMULATION - CONFIGURATON PARAMETERS - SOLVER - Type: Fixed-step, Fixed-step size: 0.001). Модель mod1, файл модели mod1.mdl.

Упрощенная модель объекта. Входных параметров - 2 параметра: углы управления. Выходных параметров - 9 параметров: параметры вектора скорости, местоположения объекта на плоскости и вспомогательный выход для отладки. Общий вид модели представлен на рис. А1.

Рис. А1 - Модель mod1

Формула блока (название блока не приводится из-за слишком большой длины), который подсоединен к блоку getV:

(u[2]*u[3]*sin(u[5])*cos(u[6])-u[4]*u[7])/(u[1]+u[2])

Блок осуществляет расчет производной скорости.

Формула блока (название блока не приводится из-за слишком большой длины), который подсоединен к блоку getFi:

(u[2]*u[3]*sin(u[5])*sin(u[6]) / u[8] - u[7]*u[4])/ (u[1]+u[2])

Блок осуществляет расчет второй производной угла поворота вектора скорости.

В блоке MATLAB Fcn высчитывается косинус входной величины.

В блоке MATLAB Fcn1 высчитывается синус входной величины.

Константа V!=0 имеет значение: 0.0001

Параметры модели: g=9.8, Mm=0.1, Me=0.1, rk=0.1

Исследование движения модели производится путем запуска ее через скрипт scr_mod1_trace.m, описанный в Приложении 2 к данной работе.

Исследование разгона модели производится путем запуска ее через скрипт scr_mod1_testV.m, описанный в Приложении 2 к данной работе.

Модель mod2, файл модели mod2.mdl.

Полная модель объекта. Входных параметров - 2 параметра: управляющие воздействия на движители. Выходных параметров - 19 параметров: описательные углы модели, указанные выше параметры подмодели (выходы модели mod1), местоположение объекта на плоскости, груза в пространстве и вспомогательные выходы для отладки.

Общий вид модели представлен на рис. А2.



Рис. А2 - Модель mod2

Формула блока 1 - alfa3 2 - Vc 3 - Ref:

u(2)*cos(u(1))/u(3)

Блок осуществляет расчет изменения угла отклонения по вертикали вследствие движения груза.

Формула блока 1 - alfa3 2 - Vc 3 - Ref 4 - alfa1:

u(2)*sin(u(1))/(u(3)*sin(u(4)))

Блок осуществляет расчет изменения угла отклонения по горизонтали вследствие движения груза.

Формула блока X1: 1 - alfa1 2 - alfa2 3 - R:

sin(u(1))*cos(u(2))*u(3)

Блок осуществляет расчет координаты груза по первой горизонтальной оси.

Формула блока X2: 1 - alfa1 2 - alfa2 3 - R:

sin(u(1))*sin(u(2))*u(3)

Блок осуществляет расчет координаты груза по второй горизонтальной оси.

Формула блока X3: 1 - alfa1 2 - alfa2 3 - R:

u(3)-cos(u(1))*u(3)

Блок осуществляет расчет координаты груза по вертикальной оси.

Формула блока Ref:

sqrt(u(3)*u(3)+u(1)*u(1))-u(2)

Блок осуществляет расчет эффективного радиуса вращения груза.

Сравнение в блоке obnul ili neizmen производится с 0.0001.

Сравнение в блоке obnul ili rasch производится с 0.01.

Сравнение в блоке alfa2i производится с 0.01.

Сравнение в блоке alfa3 производится с 0.01.

Константа 1 имеет значение: -100

Параметры модели: g=9.8, Mm=0.1, Me=0.1, rk=0.1, l=0.02, r=0.006, R=0.1

Исследование движения модели производится путем запуска ее через скрипт scr_mod2_trace.m, описанный в Приложении 2 к данной работе.

Модель sys1_1, файл модели sys1_1.mdl.

Модель системы с упрощенным объектом с управлением без корректировок. Входных параметров - нет. Выходных параметров - 15 параметров: описательные параметры модели, заданное и реальное местоположение объекта на плоскости и вспомогательный выход для отладки.

Общий вид модели представлен на рис. А3.



Рис. А3 - Модель sys1_1

Параметры подмодели по умолчанию соответствуют параметрам модели mod2.

Исследование движения модели производится путем запуска ее через скрипт scr_sys1_1.m, описанный в Приложении 2 к данной работе.

Модель sys1_2, файл модели sys1_2.mdl.

Модель системы с упрощенным объектом с управлением с интегрально-дифференциальными корректировками. Входных параметров - нет. Выходных параметров - 17 параметров: описательные параметры модели, заданное и реальное местоположение объекта на плоскости, составляющие управления и вспомогательный выход для отладки.

Общий вид модели представлен на рис. 4.



Рис. А4 - Модель sys1_2

Параметры подмодели по умолчанию соответствуют параметрам модели mod2.

Исследование движения модели производится путем запуска ее через скрипт scr_sys1_2.m, описанный в Приложении 2 к данной работе.

Модель sys2, файл модели - sys2.mdl.

Модель системы с упрощенным объектом с управлением с интегрально-дифференциальными корректировками. Входных параметров - нет. Выходных параметров - 32 параметров: описательные параметры модели, заданное и реальное местоположение объекта на плоскости, составляющие управления, вспомогательные параметры из нелинейного регулятора и вспомогательный выход для отладки.

Общий вид модели представлен на рис. А5.



Рис. А5 - Модель sys2

Содержание блока getvfunc:

function [y1,y2,alfa3zo,mdalfa,mtl,mtvc,mtomega,mtv1,mtv2] = getv(alfa1, alfa2, alfa3, alfa1z, alfa2z, k1, k2, l, r, kdv)

% This block supports an embeddable subset of the MATLAB language.

% See the help menu for details.

if alfa1z==0.501

alfa2;

end;

%privedenie ugla alfa2 k znacheniu v otrezke ot -pi do Pi

while alfa2<-pi

alfa2=alfa2+2*pi;

end;

while alfa2>pi

alfa2=alfa2-2*pi;

end;

%privedenie ugla alfa2z k znacheniu v otrezke ot -pi do pi

while alfa2z<-pi

alfa2z=alfa2z+2*pi;

end;

while alfa2z>pi

alfa2z=alfa2z-2*pi;

end;

%privedenie ugla alfa3 k znacheniu v otrezke ot 0 do 2Pi

while alfa3<0

alfa3=alfa3+2*pi;

end;

while alfa3>2*pi

alfa3=alfa3-2*pi;

end;

%raschet raznosti uglov

dalfa1=(alfa1z-alfa1);

dalfa2=-(alfa2z-alfa2);

%privedenie ugla dalfa2 k znacheniu v otrezke ot -pi do Pi

while dalfa2<-pi

dalfa2=dalfa2+2*pi;

end;

while dalfa2>pi

dalfa2=dalfa2-2*pi;

end;

%raschet uglovogo rasstoyaniya

dalfa=sqrt(dalfa1^2 + dalfa2^2);

%raschet uslovnogo uglovogo rasstoyaniya

tl=l;

%raschet trebuemogo ugla povorota - alfa3z

alfa3z=0;

dalfa3=0;

if alfa1>0.01

if dalfa2>0

alfa3z=asin(dalfa1/dalfa)-pi/2;

end;

if dalfa2<0

alfa3z=pi-asin(dalfa1/dalfa)-pi/2;

end;

%raschet raznosti uglov povorota - alfa3

dalfa3=alfa3-alfa3z;

else

dalfa3=dalfa2;

end;

%privedeniye poluchennogo ugla dalfa3 raznosti k znacheniu v promegutke ot -Pi do Pi

while dalfa3<-pi

dalfa3=dalfa3+2*pi;

end;

while dalfa3>pi

dalfa3=dalfa3-2*pi;

end;

%raschet skorosti dvigenya pryamo

if abs(dalfa1)>0.01

if alfa1>0.01

if abs(pi/2-alfa3)<0.1 && abs(-pi/2-alfa3)<0.1

if abs(dalfa3)<pi/2

tvc=k1*dalfa2;

else

tvc=0;

end;

else

tvc=k1*dalfa1/sin(alfa3+pi/2);

end;

else

tvc=k1*dalfa1;

end;

lse

if abs(dalfa2)>0.01

if alfa1>0.01

if abs(dalfa3)<pi/2

tvc=k1*dalfa2;

else

tvc=0;

end;

else

tvc=0;

end;

else

tvc=0;

end;

end;

%schitaem uglovuyu skorost povorota proporcionalno uglu raznosti

tomega=k2*dalfa3;

%dlya levogo kolesa

tv1=tvc+(tomega*tl)/2;

%dlya pravogo kolesa

tv2=tvc-(tomega*tl)/2;

y1=tv1*kdv/r;

y2=tv2*kdv/r;

alfa3zo=alfa3z;

mdalfa=dalfa;

mtl=tl;

mtvc=tvc;

mtomega=tomega;

mtv1=tv1;

mtv2=tv2;

Параметры модели: g=9.8, Mm=0.1, Me=0.1, rk=0.1, l=0.02, r=0.006, R=0.1, k1=10, k2=500, kdv=1

Исследование движения модели производится путем запуска ее через скрипт scr_sys2.m, описанный в Приложении 2 к данной работе.

ПРИЛОЖЕНИЕ Б

В данном приложении описываются скрипты системы компьютерного моделирования MATLAB версии 7.0, которые используются для запуска моделей и ссылки на которые приведены в основном тексте диссертации.

Методика запуска скриптов: необходимо сделать рабочей папку с файлом запускаемого скрипта и файлом соответствующей модели (запускаемой скриптом, см. описания скриптов далее). Далее в консоли системы MATLAB набирается имя скрипта и нажимается клавиша ВВОД (ENTER). Результат работы скрипта будет отображен на появляющихся фигурах (figure). Перечень результатов работ скриптов указан в описании скриптов далее.

Скрипт scr_mod1_testV, файл скрипта scr_mod1_testV.m

Текст скрипат приведен далее:

clear;

disp('********************************************************************************************************************************************************************************************************');

figure

hold on;

i=1;

for alfa1z=0.1:0.2:1.57

OPT=simset('FinalStateName', 'STF');

[tt,xz,V,alfa1,alfa2,Fi,sFi,x1,x2,ddFi,other]=sim('mod1',15,OPT,[[0 15]',[alfa1z alfa1z]',[0 0]']);

plot(tt, V)

plot([tt(1) tt(length(tt))], [V(length(V)) V(length(V))],'r')

[alfa1z V(length(V)) alfa1z/V(length(V))];

K(i,2)=alfa1z;

K(i,1)=V(length(V));

i=i+1;

end

hold off;

figure

plot(K(:,1),K(:,2))

grid on;

Скрипт входных параметров не имеет. Результат работы скрипта: запуск модели mod1 (см. Приложение 1) с различными углами отклонения груза по вертикали, в результате которых получается соотношение установившейся скорости движения робота к углу отклонения.

Зависимости скорости от времени и скоростей от углов выводятся в виде графиков на двух отдельных фигурах.

Скрипт scr_mod1_trace, файл скрипта scr_mod1_trace.m

Текст скрипат приведен далее:

clear;

clear all;

disp('********************************************************************************************************************************************************************************************************');

%Vz=[0.3 0.3 0.3 0.3];

%Fiz=[0 -1.57 -3.14 1.57];

Vz=[0.4 1];

Fiz=[0 1.57];

OPT=simset('FinalStateName', 'ST');

[tt,xz,V,alfa1,alfa2,Fi,sFi,x1,x2,ddFi,other]=sim('mod1',0,OPT,[[0 0]',[0 0]',[0 0]']);

if length(V)>0

Vr=V(length(V));

Fir=Fi(length(Fi));

end

figure;

hold on;

grid on;

for i=1:length(Vz)

alfa1z(i)=Vz(i);

alfa2z(i)=Fiz(i);

OPT=simset('InitialState', ST, 'FinalStateName','ST');

[tt,xz,V,alfa1,alfa2,Fi,sFi,x1,x2,ddFi,other]=sim('mod1',10,OPT,[[0 10]',[alfa1z(i) alfa1z(i)]',[alfa2z(i) alfa2z(i)]']);

Vrm(i)=V(length(V));

Firm(i)=Fi(length(Fi));

Vr=Vrm(i);

Fir=Firm(i);

for j=1:2:(length(V)-1)

Vx=V(j)*cos(Fi(j));

Vy=V(j)*sin(Fi(j));

plot([x1(j) x1(j)+Vx],[x2(j) x2(j)+Vy],'r');

end;

plot(x1, x2,'b')

plot(x1(length(x1)), x2(length(x2)), 'ko');

end;

Скрипт входных параметров не имеет. Результат работы скрипта: запуск модели mod1 (см. Приложение 1) с заданием нескольких значений вектора скорости во времени для наблюдения за отработкой роботом этих параметров.

Результирующая траектория (синим цветом) и вектора скорости в периодические моменты времени (красным цветом) выводятся на отдельной фигуре.

Скрипт scr_mod2_trace, файл скрипта scr_mod2_trace.m

Текст скрипат приведен далее:

clear;

clear all;

%test1

% mass=[1 1 1 1 1 1 1; 0 1 0 0 1 0 0; 0 1 0 0 0 0 1];

%test2

%mass=[0 3 1 1; 0 0.5 0.5 0; 0 0.5 0.7 0];

%test3

%mass=[1 3 3 3 3 3 3; 0 0.5 0 -0.2 0 0.5 0; 0 0.5 0 0.2 0 0.5 0];

%test4 long

%mass=[1 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3; 0 0.5 0 -0.2 0 0.5 0 -0.2 0 0.5 0 -0.2 0 0.5 0 -0.2 0 0.5 0 -0.2 0 0.5 0 0.2 0 0.5 0; 0 0.5 0 0.2 0 0.5 0 0.2 0 0.5 0 0.2 0 0.5 0 0.2 0 0.5 0 0.2 0 0.5 0 -0.2 0 0.5 0];

%turn left

%mass=[5; -0.2; 0.2];

%accel-stop

%mass=[3 10; 0.7 0; 0.7 0];

%full accel

%mass=[5; 20; 20];

%spiral

%mass=[2 1 10; 0.5 0 0.5; 0.5 1 0.5];

%spiral2

%mass=[2 1 10; 1 1 1; 1 2 1];

%spiral3

mass=[2 1 10; 1 1 1; 1 1.1 1];

mtt=0;

mv=0;

malfa1=0;

malfa2=0;

malfa3=0;

mfi=0;

msfi=0;

mx1=0;

mx2=0;

mddfi=0;

mother=0;

mxc1=0;

mxc2=0;

mxc3=0;

mvc=0;

msv=0;

mv1=0;

mv2=0;

mu1=0;

mu2=0;

mother2=0;

st=0;

[rows,cols]=size(mass);

for i=1:cols

if i==1

OPT=simset('FinalStateName','ST');

else

OPT=simset('InitialState', ST, 'FinalStateName','ST');

end;

t=mass(1, i);

u1=mass(2, i);

u2=mass(3, i);

[tt,xz,v,alfa1,alfa2,alfa3,fi,sfi,x1,x2,ddfi,other,xc1,xc2,xc3,vc,sv,v1,v2,u1,u2,other2]=sim('mod2',t,OPT,[[0 t]' [u1 u1]' [u2 u2]']);

tt=tt+st;

mtt=vertcat(mtt,tt);

mtt(length(mtt))

mv=vertcat(mv,v);

malfa1=vertcat(malfa1,alfa1);

malfa2=vertcat(malfa2,alfa2);

malfa3=vertcat(malfa3,alfa3);

mfi=vertcat(mfi,fi);

msfi=vertcat(msfi,sfi);

mx1=vertcat(mx1,x1);

mx2=vertcat(mx2,x2);

mddfi=vertcat(mddfi,ddfi);

mother=vertcat(mother,other);

mxc1=vertcat(mxc1,xc1);

mxc2=vertcat(mxc2,xc2);

mxc3=vertcat(mxc3,xc3);

mvc=vertcat(mvc,vc);

msv=vertcat(msv,sv);

mv1=vertcat(mv1,v1);

mv2=vertcat(mv2,v2);

mu1=vertcat(mu1,u1);

mu2=vertcat(mu2,u2);

mother2=vertcat(mother2, other2);

st=st+t;

end

figure(1);

plot3(mxc1, mxc2, mxc3);

grid on;

figure(2);

plot3(mx1+mxc1, mx2+mxc2, mxc3);

grid on;

figure(3);

subplot(3,1,1)

plot(mtt, malfa1);

grid on;

title('alfa1');

subplot(3,1,2)

plot(mtt, malfa2);

grid on;

title('alfa2');

subplot(3,1,3)

plot(mtt, malfa3);

grid on;

title('alfa3');

Скрипт входных параметров не имеет, однако в тексте скрипта можно закомментировать текущую входную последовательность (mass) и раскомментировать другую для проведения исследований на другой траектории. Результат работы скрипта: запуск модели mod2 (см. Приложение 1) с заданием нескольких наборов значений углов отклонения и промежутков времени для этих отклонений и наблюдения за результатом такого управления роботом.

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

Скрипт scr_sys1_1, файл скрипта scr_sys1_1.m

Текст скрипат приведен далее:

clear;

OPT=simset('FinalStateName', 'ST');

[tt,xz,v,alfa1,alfa2,fi,sfi,x1,x2,ddfi,other,mustx1,mustx2,mustv,mustfi,vforce,fiforce]=sim('sys1_1',15,OPT);

%figure(1);

subplot(1,3,1);

hold on;

grid on;

plot(mustx1,mustx2,'r');

plot(x1,x2,'b');

%figure(2);

subplot(1,3,2);

hold on;

grid on;

plot(tt,mustv,'r');

plot(tt,v,'b');

plot(tt,vforce,'k');

%figure(3);

subplot(1,3,3);

hold on;

grid on;

plot(tt,mustfi,'r');

plot(tt,fi,'b');

plot(tt,fiforce,'k');

Скрипт входных параметров не имеет. Результат работы скрипта: запуск модели sys1_1 (см. Приложение 1) с последующим выводом результатов движения робота по задаваемой траектории.

На результирующей фигуре представлены три графика:

1) Реальная (синим цветом) и заданная (красным цветом) траектории движения.

2) Заданная (черным цветом) и реальная (синим цветом) зависимости модуля скорости робота от времени.

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

Скрипт scr_sys1_2, файл скрипта scr_sys1_2.m

Текст скрипат приведен далее:

clear;

OPT=simset('FinalStateName', 'ST');

[tt,xz,v,alfa1,alfa2,fi,sfi,x1,x2,ddfi,other,mustx1,mustx2,mustv,mustfi,pvforce,ivforce,pfiforce,dfiforce]=sim('sys1_2',15,OPT);

%figure(1);

subplot(1,3,1);

hold on;

grid on;

plot(mustx1,mustx2,'r');

plot(x1,x2,'b');

%figure(2);

subplot(1,3,2);

hold on;

grid on;

plot(tt,mustv,'r');

plot(tt,v,'b');

plot(tt,pvforce+ivforce,'k');

%figure(3);

subplot(1,3,3);

hold on;

grid on;

plot(tt,mustfi,'r');

plot(tt,fi,'b');

plot(tt,pfiforce+dfiforce,'k');

plot(tt,pfiforce,'g');

plot(tt,dfiforce,'y');

Скрипт входных параметров не имеет. Результат работы скрипта: запуск модели sys1_2 (см. Приложение 1) с последующим выводом результатов движения робота по задаваемой траектории.

На результирующей фигуре представлены три графика:

1) Реальная (синим цветом) и заданная (красным цветом) траектории движения.

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

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

Скрипт scr_sys1_2, файл скрипта scr_sys1_2.m

Текст скрипат приведен далее:

clear;

clear all;

OPT=simset('FinalStateName', 'ST');

[tt,xz,v,alfa1,alfa2,alfa3,fi,sfi,x1,x2,ddfi,other,xc1,xc2,xc3,vc,sv,v1,v2,u1,u2,other2,alfa1z,alfa2z,alfa3z,dalfa1,dalfa2,dalfa3,dalfa,tl,tvc,tomega,tv1,tv2,mustv,mustfi,mustx1,mustx2]=sim('sys2',15,OPT);

%figure(1);

subplot(1,3,1);

hold on;

grid on;

plot(mustx1,mustx2,'r');

plot(x1,x2,'b');

%figure(2);

subplot(1,3,2);

hold on;

grid on;

plot(tt,mustv,'r');

plot(tt,v,'b');

%figure(3);

subplot(1,3,3);

hold on;

grid on;

plot(tt,mustfi,'r');

plot(tt,fi,'b');

Скрипт входных параметров не имеет. Результат работы скрипта: запуск модели sys1_2 (см. Приложение 1) с последующим выводом результатов движения робота по задаваемой траектории.

На результирующей фигуре представлены три графика:

1) Реальная (синим цветом) и заданная (красным цветом) траектории движения.

2) Заданная (красным цветом) и реальная (синим цветом) зависимости модуля скорости робота от времени.

3) Заданная (красным цветом) и реальная (синим цветом) зависимости угла поворота вектора скорости робота от времени.

ПРИЛОЖЕНИЕ В

В данном приложении описывается программа paracont, которая является основной программой ПО системы, исследуемой в диссертации. Текст программы находится в файле paracont.cpp а запускается программа файлом paracont.exe. Также возможен запуск программы из среды программирования TurboC (более поздние компиляторы не поддерживают функций ассемблера прямого обращения к регистрам LPT-порта).

После запуска программы нажатием кнопок Insert, Delete, PageUp, PageDown можно изменять скорость и направление вращения двигателей на объекте, а по нажатии клавиши Esc происходит завершение работы программы.

Текст программы:

#include<stdio.h>

#include<stdlib.h>

#include<dos.h>

#include<conio.h>

void main (void)

{

#define tacts 4

unsigned char port_map = 0;

unsigned char port_map_for_work = 0;

unsigned char key = 0;

char bit_1, bit_2, bit_3, bit_4;// 0 ->16

bit_1= bit_2= bit_3= bit_4= 0;

clrscr();

outportb(0x378, 0);

while(1)

{

while(!kbhit())

{

port_map_for_work = port_map;

for (int i=1; i<=tacts; i++)

{

outportb(0x378, port_map_for_work);

delay (5);

if(bit_1==i)

asm sub port_map_for_work, 00000001b;

if(bit_2==i)

asm sub port_map_for_work, 00000010b;

if(bit_3==i)

asm sub port_map_for_work, 00000100b;

if(bit_4==i)

asm sub port_map_for_work, 00001000b;

}

}

key=getch();

if (key == 27)

{

outportb(0x378, 0);

return;

}

key=getch();

if (key == 82)

{

if (bit_3 > 0)

bit_3--;

else if (bit_1 < tacts)

bit_1++;

}

if (key == 83)

{

if (bit_1 > 0)

bit_1--;

else if (bit_3 < tacts)

bit_3++;

}

if (key == 73)

{

if (bit_4 > 0)

bit_4--;

else if (bit_2 < tacts)

bit_2++;

}

if (key == 81)

{

if (bit_2 > 0)

bit_2--;

else if (bit_4 < tacts)

bit_4++;

}

port_map = 0;

if(bit_1!= 0)

asm or port_map, 00000001b;

if(bit_2!= 0)

asm or port_map, 00000010b;

if(bit_3!=0)

asm or port_map, 00000100b;

if(bit_4!=0)

asm or port_map, 00001000b;

printf("FWD%i, BWD%i,FWD%i, BWD%i\n", bit_1, bit_3, bit_2, bit_4);

}

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

ПРИЛОЖЕНИЕ Г

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

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

Рис. Г1 - Фотография четырехколесного робота Seekur

Далее приведем фотографию трехколесного робота Qwerkbot (см. рис. Г2).

Рис. Г2 - Фотография трехколесного робота Qwerkbot

Также приведем фотографию шестиколесного трехосного робота RouROC (см. рис. Г3).

Рис. Г3 - Фотография шестиколесного робота RobuROC

Далее перейдем к шагающим роботом. Ярким представителем этого направления робототехники является WL-16 (см. рис. 4). Это шагающее кресло-робот, управляемое человеком только в плане направления движения.

Рис. Г4 - Фотография шагающего робота WL-16

Стоит также привести фотографию одного из наиболее сложных человекоподобных роботов - SDR (Sony Dream Robot). Его фото представлено на рис. Г5.

Рис. Г5 - Фотография человекоподобного робота SDR

Далее приведем фотографию шарового мобильного робота фирмы Rotundus, который рассматривался в данной работе.

Кроме этого шарового робота, в том же разделе данной работы рассматривался еще один робот, использующий для передвижения шар - Ballbot.

Размещено на Allbest.ru