А филиппов Робототехника для детей и родителей Под редакцией д ра техн наук, проф а л фрадкова Издание 3- е, дополненное и исправленное Санкт Петербург наука 2013

149
Hello, world!
Традиционно первая программа на языке Си должна вывести на экран приветствие миру: «Hello, world!». Воспользовавшись возможно- стями NXT, исполним традицию:
task main()
{
nxtDisplayTextLine(0, "Hello, world!");
wait1Msec(5000);
}
Команда nxtDisplayTextLine(0, "Hello world!"); выведет в строку экрана NXT с номером ноль сообщение «Hello world!». По прошествии
5 с программа закончит выполнение и текст исчезнет. Сохраните про- грамму на жесткий диск с именем "hello.c".
Для загрузки в микроконтроллер подключите его к компьютеру, включите NXT и нажмите на клавиатуре компьютера клавишу F5. Если программа загрузилась успешно, на экране компьютера появится окно отладки Program Debug (рис. 6.2).
Рис. 6.2. Окно отладки программы.
Если нажать в нем кнопку Start, программа запустится на NXT.
Можно игнорировать это окно, найти в меню микроконтроллера файл hello и запустить его оранжевой кнопкой.
Усовершенствуем программу. То же сообщение можно вывести по центру экрана:
task main()
{
nxtDisplayCenteredTextLine(3, "Hello world!");
wait1Msec(5000);
}
На этот раз сообщение будет выведено в третьей строке дисплея
NXT с выравниванием посередине.

150
Используя набор команд по работе с экраном NXT, в том числе и графических, можно написать собственные миниигры для микрокон- троллера. Но это тема отдельной главы или даже книжки.
Структура программы
Теперь обратим внимание на структуру исходного кода. Главная задача, с которой начинается выполнение программы в RobotC, называ- ется task main(). Тело задачи располагается между двумя фигурными скобками: открывающей и закрывающей. Все команды должны быть размещены между ними.
Более подробно о задачах написано в разделе «Параллельные задачи».
Управление
моторами
Состояние моторов
Простейшая задача: вращать мотор C вперед 3 с:
task main()
{
motor[motorC] = 100;
wait1Msec(3000);
}
Команда motor[] представляет собой массив из трех элементов, ка- ждый из которых определяет текущее состояние вращения моторов A,
B и C. Действие «полный вперед» задается числом 100, «полный назад» — числом –100, остановка — числом 0. Действие этой команды можно считать мгновенным. После ее выполнения мотор включается и продолжает работать до тех пор, пока не будет остановлен другой ана- логичной командой.
Команда wait1Msec() задает время ожидания в миллисекундах
(1 мс = 1/1000 с). Таким образом, wait1Msec(3000) означает «ждать
3 секунды».
Теперь сформулируем задачу для робота более четко: проехать вперед 2 секунды со средней скоростью и остановиться. В программе последовательно включаются моторы C и B с мощностью 50, работают в течение 2 с, после чего последовательно выключаются. По сути вклю-

151 чение обоих моторов произойдет практически одновременно, как и их выключение:
task main()
{
motor[motorC] = 50;
motor[motorB] = 50;
wait1Msec(2000);
motor[motorC] = 0;
motor[motorB] = 0;
}
Следующий пример демонстрирует возожность бесконечного вра- щения мотора с помощью цикла while. Остановить его можно, прервав выполнение программы на NXT:
task main()
{
while (true)
motor[motorA]=100;
}
Следующий пример работает аналогично:
task main()
{
motor[motorA]=100;
while (true)
wait1Msec(1);
}
Миллисекундная задержка полезна для разгрузки контроллера при работе бесконечного цикла.
Встроенный датчик оборотов
Команда nMotorEncoder[] — это обращение к датчику оборотов мотора, который возвращает значение в градусах. Каждый из трех эле- ментов этого массива имеет 16-битное значение, что дает возможность хранить число в диапазоне –32768...32767, это соответствует 95 полным
(360 градусов) оборотам моторов. В длительно работающих програм- мах следует время от времени обнулять значения массива во избежание переполнения.
nMotorEncoder[motorA] = 0;
Этим же действием датчик оборотов инициализируется для начала измерений.

152
Следующая программа обеспечит вращение мотора A на 1000 гра- дусов.
task main()
{
motor[motorA]=50;
while (nMotorEncoder[motorA] < 1000)
{
//
никаких
действий
можно
не
производить
}
}
Команда nMotorEncoderTarget[] обеспечивает остановку мотора при повороте на заданное число градусов. Команда обладает собствен- ным «интеллектом» и, учитывая инерцию движения, может откоррек- тировать окончательную позицию мотора.
task main()
{
nMotorEncoderTarget[motorA] = 1000;
motor[motorA] = 50;
wait1Msec(10000);
}
Синхронизация моторов
На практике часто можно заметить, что движение колесного робота непрямолинейно. Это может быть связано с различным трением приво- дов, разницей нагрузок на колеса и т. п. Команда nSyncedMotors позво- ляет синхронизовать моторы, объявив один из них главным, второй — подчиненным (один ведущим, второй — ведомым):
nSyncedMotors = synchNone; //
Отключить
синхронизацию
nSyncedMotors = synchAC; //
Мотор
C
подчинен
мотору
A
В режиме синхронизации достаточно управлять только одним мо- тором: второй будет в точности повторять его действия.
Переменная nSyncedTurnRatio позволяет изменить соотношение движения синхронизированных моторов. Ее значения изменяются от –
100 до 100 %. Отрицательный знак указывает на противоположное на- правление движения подчиненного мотора. Абсолютное значение пока- зывает отношение скорости ведомого мотора к скорости ведущего. Ес- ли оно меньше 100, робот поворачивает.
В следующем фрагменте программы робот проедет по прямой, а затем повернется:

153
nSyncedMotors = synchAC; //
Мотор
C
подчинен
мотору
A
//
Движение
по
прямой
nSyncedTurnRatio = +100; //
Режим
движения
прямо
nMotorEncoder[motorA] = 0;
//
остановиться
через
1000
градусов
nMotorEncoderTarget[motorA] = 1000;
motor[motorA] = 100;
while (nMotorEncoder[motorA] < 1000) //
дождаться
остановки
{}
//
Повернуться
на
месте
nSyncedTurnRatio = -100; //
Режим
поворота
//
остановиться
через
200
градусов
nMotorEncoderTarget[motorA] = 200;
motor[motorA] = 50;
wait1Msec(3000);
Режим импульсной модуляции
Скорость моторов контролируется технологией широтно- импульсной модуляции (ШИМ, pulse width modulation — PWM). Им- пульсы тока подаются на моторы тысячи раз в секунду. Каждый им- пульс представляет из себя волну, содержащую период наличия напря- жения (on-time) и период отсутствия напряжения (off-time). Отношение между этими двумя периодами определяет мощность, подаваемую на мотор. В периоды on-time используется полный ресурс батареи, и эта технология более эффективна, чем регуляция скорости с помощью из- менения напряжения.
В периоды off-time, когда напряжение на моторы не подается, цепь может находиться в двух состояниях: разомкнутом и замкнутом. Ра- зомкнутая цепь получается в режиме плавающего напряжения, замкну- тая — в режиме торможения. RobotC позволяет выбрать режим как в установках среды, так и с помощью специальной переменной bFloatDuringInactiveMotorPWM. При компиляции программы будет ус- тановлен выбранный режим:
bFloatDuringInactiveMotorPWM = false; //
режим
торможения
bFloatDuringInactiveMotorPWM = true; //
плавающий
режим
Для моторов NXT предпочтительным является режим торможения, он и выставляется по умолчанию.

170
Глава
7.
Алгоритмы
управления
Релейный
регулятор
Одной из главных задач теории автоматического управления явля- ется управление с помощью обратной связи. В таких задачах можно выделить четыре основных компонента [1]:
¾
управляемую систему (или как говорят специалисты, объект управления) — то, чем мы хотим управлять;
¾
цель управления — то, чего мы хотим достичь при помощи управления, т.е. желаемое поведение объекта управления;
¾
список измеряемых переменных (или выходов) — то, что мы мо- жем измерять;
¾
список управляющих переменных (или входов) — то, что мы мо- жем менять для того, чтобы воздействовать на объект управления.
Еще один важный компонент — регулятор — устройство, выраба- тывающее входные величины, необходимые для достижения заданной цели. Этот пятый элемент обычно появляется после того, как теорети- ческое решение задачи найдено. Под решением проблемы управления будем понимать нахождение закона управления (алгоритма управле- ния), обеспечивающего достижение цели. Как только искомый закон найден, он может быть использован для вычисления управляющих вхо- дов по измеренным значениям выходов объекта управления. Получен- ные значения входов в виде некоторых сигналов подаются на исполни- тельные устройства.
В формировании этих сигналов может принимать участие микро- процессор, производящий достаточно сложные вычисления в соответ- ствии с заданным алгоритмом.
Будем рассматривать простейший случай, когда задача состоит в поддержании системы в определенном состоянии. Тогда можно гово- рить о задаче регулирования как частном случае задачи автоматическо- го управления. Для осуществления автоматического регулирования к объекту подключается регулятор. Под регулятором будем понимать устройство, которое с помощью чувствительного элемента (датчика) измеряет регулируемую величину и в соответствии с законом регулиро- вания вырабатывает воздействие на регулирующий орган объекта. Сис- тема, состоящая из объекта и регулятора, называется системой управле- ния [1].
Исполнительное устройство, осуществляющее механическое пере- мещение регулирующего органа, обычно называется сервоприводом.

171
Релейным двухпозиционным регулятором называется регулятор, у которого регулирующий орган под действием сигнала от датчика может принимать одно из двух крайних положений: «открыт» или «закрыт».
При этом управляющее воздействие на регулируемый объект может быть только максимальным или минимальным.
Управление мотором
«Робот поднимает меч и бросается на противника!» Здорово. Как в сказке. А что если меч наткнется на что-нибудь и потеряет свое задан- ное положение? Нехорошо бросаться на противника с опущенным мечом.
Для примера прикрепите к мотору длинную балку с помощью сдвоенного трехмодульного штифта. Это будет меч. По сценарию, в момент запуска его надо поднять на 45 градусов и удерживать в этом положении, что бы ни случилось (рис. 7.1).
Рис. 7.1. Стабилизация мотора в положении 45 градусов.
Регулируемой величиной будет угол поворота мотора, определяе- мый через показания энкодера, регулирующим органом — сам мотор.
Алгоритм таков: обнулить показания датчика оборотов, задать же- лаемое положение в 45 градусов и в цикле поддерживать это положе- ние, учитывая возможные отклонения. Если показания энкодера пре- вышают 45, мотор вращается назад. В противном случае вращается вперед. Задержка в 1 мс предназначена для разгрузки контроллера.
Рис. 7.2. Релейное управление одним мотором.

172
task main()
{
int alpha=45;
nMotorEncoder[motorA]=0;
while(true)
{
if(nMotorEncoder[motorA]>alpha)
motor[motorA]=-100;
else
motor[motorA]=100;
wait1Msec(1);
}
}
Что же мы увидим? Робот ведет себя как новобранец с мечом. Мо- тор удерживает балку, но как-то неуверенно: происходят постоянные колебания. Уменьшить их можно, разве что понизив мощность мотора.
Попробуйте сделать это.
Особенность релейного регулятора в том, что он в принципе не может стабилизироваться в нужном положении и вызывает колебания с той или иной амплитудой. Как этого избежать, описано чуть дальше.
Движение с одним датчиком освещенности
Рассмотрим пример трехколесного Lego-робота с одним датчиком освещенности, который должен двигаться по плоской поверхности вдоль границы черного и белого (рис. 7.3).
Рис. 7.3. Движение вдоль границы черного и белого.

173
Исполнительным органом объекта в дан- ном случае будут два колеса, подключенные к сервоприводам. Регулируемая величина — положение датчика света на границе черного и белого, зависящее от уровня освещенности под ним. Возмущающее воздействие — это движение робота вперед, которое приводит к отклонению от границы. Поскольку линия может быть кривой, а также вследствие дру- гих факторов при отсутствии регулирующего воздействия робот непременно съедет с ли- нии.
Подключим левый мотор на порт B, пра- вый — на порт C (рис. 7.4). Стартовая позиция робота — датчик на белом. Построим про- граммный регулятор, который обеспечит дви- жение по дуге в сторону черного, пока робот на белом, и движение по дуге в сторону белого, пока робот на черном.
Для этого выключается или резко понижается мощность одного из мо- торов. Вот реализация на RobotC:
task main()
{
int grey=45;
while (true)
{ // grey -
значение
серого
if (SensorValue[S1]>grey)
{
motor[motorB]=100;
motor[motorC]=0;
}
else
{
motor[motorB]=0;
motor[motorC]=100;
}
wait1Msec(1);
}
}
Значение «серого» (grey) может быть константой для данной сис- темы или вырабатываться в результате предварительной калибровки как среднее арифметическое черного и белого.
Под управлением такого регулятора робот движется вдоль линии границы по ломаной кривой, периодически наезжая то на черное, то на белое. Скорость его невысока, стабильность тоже, а траектория движе-
Рис. 7.4. Расположение
моторов и датчика ос-
вещенности.

174 ния оставляет желать лучшего. Все это издержки использования двух- позиционного релейного регулятора.
Программа для движения по линии с помощью релейного регуля- тора в Robolab показана на рис. 7.5.
Рис. 7.5. Алгоритм движения вдоль границы черного и белого на релейном
регуляторе.
Движение с двумя датчиками освещенности
Правильно проехать перекресток с одним датчиком освещенности довольно сложно. Если требуется сделать это с достаточно высокой скоростью, нужны хотя бы два дат- чика, поставленные на расстоянии в две-три ширины линии (или шире).
Для начала используем релейный регулятор, с помощью которого можно обработать четыре возмож- ных состояния датчиков (рис. 7.6):
¾
оба на белом — движение прямо;
¾
левый (S1) на черном, пра- вый (S2) на белом — движение нале- во;
¾
левый на белом, правый на черном — движение направо;
¾
оба на черном — движение прямо.
Этот регулятор реализуется программно с помощью вложенных ветвлений:
task main()
{
int grey1=45, grey2=45;
while(true) {
if(SensorValue[S1]>grey1) {
if(SensorValue[S2]>grey2) { //
Оба
на
белом
motor[motorB]=100;
motor[motorC]=100;
Рис. 7.6. Варианты расположения
двух датчиков освещенности над
черной линией.

175
}
else { //
Правый
на
черном
motor[motorB]=100;
motor[motorC]=-100;
}
}
else {
if(SensorValue[S2]>grey2) { //
Левый
на
черном
motor[motorB]=-100;
motor[motorC]=100;
}
else { //
Оба
на
черном
motor[motorB]=100;
motor[motorC]=100;
}
}
wait1Msec(1);
}
}
То же самое в Robolab (рис. 7.7):
Рис. 7.7. Алгоритм движения вдоль черной линии с двумя датчиками на ре-
лейном регуляторе.
Вот такой, довольно длинный и малоэффективный алгоритм автор и многие его коллеги по состязаниям Lego-роботов использовали для решения классической задачи движения по траектории. Чтобы не сбиться с линии, приходилось значительно усложнять его, вводить до- полнительные проверки и, конечно, калибровку датчиков под конкрет- ную освещенность. Однако достаточно было в окнах появиться сол- нышку, как робот терял линию, поскольку значения были жестко фик- сированными. Так продолжалось до тех пор, пока мы не познакомились с базовыми принципами автоматического регулирования.

176
Пропорциональный
регулятор
Описание
При автоматическом регулировании управляющее воздействие u(t) обычно является функцией динамической ошибки — отклонения e(t) регулируемой величины x(t) от ее заданного значения x0(t):
e(t) = x0(t) – x(t).
Это принцип Ползунова-Уатта регулирования по отклонению, или принцип обратной связи. Математическое выражение функциональной зависимости желаемого управляющего воздействия u0(t) от измеряемых регулятором величин называется законом или алгоритмом регулирова- ния, о котором говорилось выше.
Пропорциональный регулятор — это устройство, оказывающее управляющее воздействие на объект пропорционально его отклонению от заданного состояния:
u0(t) = ke.
Здесь k — это коэффициент усиления регулятора.
Заданное состояние x0 принято называть уставкой, а отклонение от него e — невязкой. Далее для определенности будем обозначать невязку сокращением err (от английского слова «error» — ошибка).
Управление мотором
Опытный воин не станет размахивать мечом, как это делает робот на релейном регуляторе. Надо придумать алгоритм, который задержит мотор, удерживающий меч, в строго фиксированном положении (рис.
7.1). В этом поможет П-регулятор.
Пусть e1 — показания датчика оборотов
1
на моторе A — является регулируемой величиной. Уставка x0 = 45, а невязка e = 45 – e1. Тогда управляющее воздействие на мотор задается формулой
u = k ∙ (45 – e1).
Здесь k — коэффициент усиления, например 5, который позволит усилить реакцию мотора даже при небольших отклонениях от уставки.
1
Не стоит путать математическое обозначение невязки e (от error) с показа- ниями энкодера e1 (от encoder), предопределенной переменной среды Robolab.

177
При отклонении в положительную сторону на мотор будет пода- ваться отрицательное управляющее воздействие, и наоборот. Это управление можно применять к мотору в цикле с небольшой задержкой в 1-10 мс, чтобы разгрузить контроллер (рис. 7.8).
Рис. 7.8. Алгоритм управления мотором на пропорциональном регуляторе.
Если усиливающий коэффициент увеличить с 5 до 100, наш про- порциональный регулятор начнет работать как релейный, вызывая сильные колебания из-за возникновения эффекта перерегулирования.
В языке RobotC нет такого удобного обозначения показаний энко- дера как в Robolab, поэтому программа выглядит немного длиннее:
task main()
{
int k=5, u;
nMotorEncoder[motorA]=0;
while(true)
{
u=k*(45-nMotorEncoder[motorA]);
motor[motorA]=u;
wait1Msec(1);
}
}
Далее, чтобы нанести «удар мечом», достаточно, имея вместо чис- ла 45 переменную, изменить ее значение извне, например, из парал- лельной задачи. Об этом написано в разделе, посвященном роботам- барабанщикам в главе 8.
А сейчас построим регулятор, управляющий не только статическим положением мотора, но и скоростью его движения. Следуя логике алго- ритма, уставка, которая до сих пор была константой и не менялась, должна начать движение в сторону увеличения или уменьшения. Пови- нуясь регулятору, мотор неизбежно будет следовать за ней. Самый про- стой инструмент для постоянного приращения значения уставки — это таймер.
В контроллере NXT есть четыре встроенных таймера, каждый из которых может отмерять время в десятых, сотых и тысячных долях се- кунды. Освоим первый таймер, который за секунду совершает 10 «ти- ков». В Robolab он обозначается T1 или Timer100ms1, а в RobotC — timer100[T1].

178
Угол alpha отклонения мотора, заданный в предыдущем примере значением 45, поставим в зависимость от показаний таймера с уско- ряющим коэффициентом k2:
alpha = k2 ∙ T1.
Управляющее воздействие останется прежним с усиливающим ко- эффициентом k1:
u= k1 ∙ (alpha – e1).
Кратко в программе на языке Robolab управляющее воздействие подадим сразу на мотор, предварительно инициализировав таймер
(рис. 7.9).
Рис. 7.9. Управление скоростью мотора — один оборот в секунду.
Коэффициент k2 = 36 определяет, что за секунду значение alpha набегает до 360, что соответствует одному полному обороту двигателя:
task main()
{
int k1=2, k2=36, u, alpha;
nMotorEncoder[motorA]=0;
ClearTimer(T1);
while(true)
{
alpha=timer100[T1]*k2;
u=k1*(alpha-nMotorEncoder[motorA]);
motor[motorA]=u;
wait1Msec(1);
}
}
Используя целочисленное деление, принятое в языке C (и в Robo- lab) для переменных целого типа, можно достичь дискретного измене- ния угла, т.е. приращения его раз в секунду:
alpha = T1 / 10 ∙ k2.
При коэффициенте k2 = 60 перемещения балки будут соответство- вать движению секундной стрелки на циферблате часов. Но это мало

179 заметно. Для наглядности можно задать k2 = 30, тогда стрелка сделает полный оборот за 12 «тиков» по 30 градусов каждый. Будьте внима- тельны с последовательностью операций целочисленного деления и умножения, при изменении их порядка или «сокращении» непременно изменится результат (рис. 7.10).
Рис. 7.10. Ускоренная имитация движения стрелки часов.
И, наконец, пример математического барабанщика. Вместо посто- янного движения вперед стрелка будет совершать колебания вперед- назад под управлением П-регулятора. В этом поможет операция деле- ния с остатком, которая в языке C обозначается знаком %. Остаток от деления неотрицательного целого числа на 2 всегда будет 0 или 1:
alpha = T1 % 2 ∙ k2.
Усилив отклонение в k2=15 раз, получим колеблющуюся уставку
alpha, что вынудит регулятор 5 раз в секунду перемещать мотор то в 0º, то в 15 градусов. Изменения в программе невелики. Рассмотрим пример на RobotC:
task main()
{
int k1=3, k2=15, u, alpha;
nMotorEncoder[motorA]=0;
ClearTimer(T1);
while(true)
{
alpha=timer100[T1]%2*k2;
u=k1*(alpha-nMotorEncoder[motorA]);
motor[motorA]=u;
wait1Msec(1);
}
}
Этот прототип барабанщика наносит удары по столу через одина- ковые промежутки времени. Главное, стартовать в нужной позиции.
Используя целочисленную математику, можно задать и более сложный ритмический рисунок, например (табл. 7.1):
alpha = T1 % 5 % 2 ∙ k2.

196
center = S3.
Коэффициент определяется в цикле:
k1 = c + (S3 - center) / k2.
Рис. 7.36. Движение по линии на пропорциональном регуляторе с плавающим
коэффициентом.
Полученный закон управления коэффициентами усиления можно применить не только к пропорциональной составляющей, но и к любой другой, а также к управляющему воздействию в целом (рис. 7.36).
ПИД
-
регулятор
Пропорционально-интегрально-дифференциальный (ПИД) регуля- тор является одним из наиболее популярных и используется в огромном количестве устройств самых разных типов, в которых требуются быст- рота реакции и точность позиционирования системы. Как следует из названия, этот регулятор состоит из суммы трех компонент и графиче- ски изображен на рис. 7.37.
Рис. 7.37. Схема ПИД-регулятора.

197
Это упрощенная схема. На вход регулятора подается значение ди- намической ошибки e(t), а на выходе вырабатывается управляющее воздействие u(t):
u(t) = p + i + d = k
p ∙
e(t) + k
i
∙
ò
t
e
0
(τ)dτ + k
d
∙
dt
de
Пропорциональная составляющая, изображенная на схеме тре- угольником, отвечает за позиционирование системы в заданном состоя- нии. В некоторых случаях может вызвать перерегулирование с после- дующими автоколебаниями. То есть П-регулятор может «перестарать- ся» и робота начнет заносить из стороны в сторону.
Интегральная составляющая накапливает отрицательный опыт
(суммирует ошибки) и вырабатывает компенсирующее воздействие.
При минимальных отклонениях пропорциональная составляющая «сла- беет» и интегральная, за счет своего быстрого увеличения суммирова- нием, помогает «дотянуть» регулируемую величину до уставки.
Дифференциальная составляющая (Д-составляющая) следит за скоростью изменения состояния системы и препятствует возможному перерегулированию. В некоторых случаях Д-составляющая противопо- ложна пропорциональной по знаку, а в некоторых совпадает.
С пропорциональной составляющей мы уже знакомы, дифферен- циальная описана в предыдущей главе 6. Возьмемся за интегральную.
Эта составляющая определяется динамически, суммируясь с предыду- щим значением:
dt
t
e
k
i
i
i
×
×
+
=
)
(
Физический смысл величины
dt
t
e
×
)
(
состоит в том, что она про- порциональная длительности нахождения системы в состоянии ошибки.
Поскольку коэффициент k
i
выносится за скобки, можно говорить о ве- личине i как сумме длительностей ошибок. Таким образом, мы находим интеграл путем суммирования.
Рассмотрим применение ПИД-регулятора на примере робота, ба- лансирующего на двух колесах. Эта классическая задача может быть решена с помощью различных датчиков множеством способов. В пред- ложенном примере использован датчик освещенности и простейшая форма ПИД-регулятора. Однако для достижения стабилизации робота потребуется использовать более точные показания датчика.

198
Формат RAW
Данные с датчиков поступают в контроллер NXT в необработан- ном «сыром» виде. Все датчики передают операционной системе циф- ровое значение от 0 до 1023, которое затем обрабатывается соответст- вующим драйвером и приводится к более понятному виду (расстояние
0...255, освещенность 0...100, касание 0 или 1 и т. д.). Но данные можно получать и, минуя драйвер, напрямую. Такой необработанный формат принято называть RAW (от англ. «сырой»). В некоторых случаях с по- мощью него можно получить бóльшую точность. Так, например, диапа- зон значений датчика освещенности может увеличиться примерно в 10 раз. Именно эта возможность использована далее.
Получать данные в формате RAW можно и в Robolab, и в RobotC.
Для этого датчик инициализируется соответствующим образом, а дан- ные с него считываются с использованием специальной предопределен- ной переменной.
Балансирующий робот
Конструкция робота-сигвея изображена на рис. 7.38: вертикально расположенный контроллер, близко поставленные колеса и датчик ос- вещенности, направленный вниз. Алгоритм будет несколько сложнее.
Принцип стабилизации сигвея в положении равновесия состоит в следующем. Если робот наклоняется вперед, показания на датчике освещенности повышаются за счет отраженного света. В ответ на это вырабатывается управляющее воздействие, заставляющее робота ехать вперед и тем самым снова принимать вертикальное положение.
При отклонении назад показания датчика понижаются и робот начнает движение назад. За все это отвечает пропорциональная состав- ляющая. На роль интегральной и дифференциальной составляющих от- водится страховка от перерегулирования.
Рис. 7.38. Балансирующий робот-сигвей.

199
На рис. 7.39 представлен алгоритм в Robolab. Бóльшую его часть занимает инициализация переменных. Для повышения точности не только данные c датчика считываются в формате RAW, но большинство переменных объявляется в вещественном формате float. Собственно
ПИД-алгоритм находится в цикле.
Рис. 7.39. Алгоритм балансировщика основан на ПИД-регуляторе.
Следуя традиции движения по линии, в качестве уставки использу- ем переменную grey — средние показания датчика освещенности в по- ложении равновесия. Новый параметр scale задает масштабирование управляющего воздействия. По сути, это ослабляющий коэффициент, поскольку вырабатываемое регулятором значение слишком высоко для моторов NXT. Можно было бы внести его внутрь уже имеющихся ко- эффициентов, но для RobotC этот параметр будет другой, а коэффици- енты те же.
С приведенными коэффициентами робот хорошо стабилизируется на однотонном светлом линолеуме или парте. То есть ему не требуется белый цвет поверхности. Для запуска нужно достаточно точно устано- вить сигвея в положение равновесия. Если робот стартует при некото- ром наклоне вперед или назад, то сразу начнет движение в направлении наклона.
Аналогичный пример на RobotC несколько отличается в силу ряда причин. Во-первых, быстродействие NXT с прошивкой этой среды вы- ше примерно в 1.4 раза, чем у Robolab, поэтому коэффициент scale сле- дует увеличить. Во-вторых, RAW-значения передаются в правильном порядке и потребуется установить реверс моторов или просто подавать отрицательное управляющее воздействие:

200
task main()
{
int grey=SensorRaw[S3];
int err, errold=0;
float kp=25, ki=350, kd=0.3;
float scale=14;
float dt=0.001;
float p, i=0, d, u;
while (true)
{
err= grey-SensorRaw[S3]; //
Отклонение
с
обратным
знаком
p=kp*err;
i=i+ki*err*dt;
d=kd*(err-errold)/dt;
errold=err;
u=(p+i+d)/scale;
motor[motorB]=u;
motor[motorC]=u;
wait1Msec(1);
}
}
Элементы
теории
автоматического
управления
в
школе
1
Важной и интересной методической задачей является «переброска мостика» между областями знаний специалиста и школьника, помо- гающая учащимся школы увидеть перспективу будущей специальности, т.е. осуществить профориентацию, а студентам увидеть практическую применимость своих профессиональных знаний. Для достижения по- добного эффекта были разработаны приемы расчета регуляторов, ис- пользующие математический аппарат, не выходящий за рамки школь- ных программ по математике и физике. В частности, вместо дифферен- циальных уравнений использованы разностные, хорошо соответствующие дискретному характеру взаимодействия объекта и регулятора при компьютерном управлении.
Рассмотрим, например, задачу построения пропорциональных (П) и пропорционально-дифференциальных (ПД) регуляторов в задаче управления движением мобильного робота вдоль стены. Обозначим че- рез x
t
расстояние между роботом и стеной, через θ
t
— курсовой угол робота, а через u
t
— управляющее воздействие в момент с порядковым номером t, соответственно, где t = 0, 1, 2, … — номера моментов изме-
1
Автор статьи – д. техн. наук, профессор А. Л. Фрадков.

201 рений. Считается, что опрос датчиков и изменения величины управ- ляющего воздействия производится через равные промежутки времени
h. Для задач управления Lego NXT роботами естественно считать, что управляющим воздействием является разность угловых скоростей вра- щения колес, пропорциональная скорости изменения курсового угла:
θ
t +
1
= θ
t
+ u
t
hr / b,
(1) где r — радиус колес, b — база транспортного средства (расстояние между колесами). Легко также видеть, что расстояние робота от стены за время h изменится на величину h sin θ
t
, т.е. имеет место соотношение
x
t + 1
= x
t
+ v
t
h sin θ
t
(2)
Считая отклонения курса от номинального θt =0 малыми, а сред- нюю скорость робота постоянной: vt=v, динамику изменения перемен- ных состояния робота в первом приближении можно описать линейны- ми уравнениями состояния:
x
t + 1
= x
t
+ vhθ
t
, θ
t + 1
= θ
t
+ u
t
hr / b.
(3)
Исключая переменную θ
t
, приходим к разностному уравнению 2-го порядка, непосредственно связывающему управляющую и регулируе- мую переменные:
x
t + 2
– 2 x
t +1
+ x
t
= gu
t
,
(4) где g = h2vr / b.
Зададим желаемое расстояние до стены x*>0 и определим цель управления (ЦУ) соотношением
x
t
→ x* при t→∞.
(5)
Теперь естественным образом введем на содержательном уровне понятие асимптотической устойчивости, как свойства решений системы
(4), обеспечивающего достижение ЦУ (5) при любых начальных усло- виях, достаточно мало отличающихся от целевых. Легко видеть, что при u
t
= 0решением уравнения (4) является любое постоянное значение
x
t
= x*. Но поскольку уравнение (4), соответствующее модели двойного интегратора (двойного сумматора), не обладает свойством асимптоти- ческой устойчивости, ЦУ (5) при постоянном управлении не достигает- ся. Это легко демонстрируется как аналитически — суммированием ря-

202 да натуральных чисел, так и экспериментально, выставляя робот в раз- личные начальные положения.
После такой демонстрации весьма естественно ввести понятия ре- гулятора и обратной связи по результатам измерений и они легко ос- ваиваются школьниками. Сначала рассматривается простейший про- порциональный регулятор (П-регулятор):
u
t
= K
0
(x* – x
t
),
(6) где K
0
— коэффициент усиления регулятора. Подстановкой (6) в (4) по- лучаем уравнение замкнутой системы
x
t +2
– 2 x
t
+
1
+ (1 + gK
0
)x
t
= gK
0
x*.
(7)
Переходя к уравнениям для отклонений yt =x* – xt , получим одно- родное уравнение
y
t+2
– 2 y
t+1
+ (1+ gK
0
)y
t
= 0.
(7a)
Возникающий вопрос об устойчивости системы (7) теперь можно решить в случае общей однородной системы 2-го порядка:
x
t+2
+ a
1
x
t+1
+ a
0
x
t
= 0,
(8) записав ее общее решение в виде суммы двух геометрических прогрес- сий со знаменателями λ
1
и λ
2
(возможно, комплексными) равными кор- ням квадратного трехчлена λ
2
+ a
1
λ + a
0
. Условием асимптотической устойчивости оказывается пара неравенств |λ
1
| < 1, |λ
2
| < 1, означаю- щая, что обе геометрические прогрессии являются бесконечно убы- вающими. Теперь стандартное условие устойчивости в терминах коэф- фициентов
a
0
< 1, 1+ a
0
> | a
1
|
(9) школьники могут вывести самостоятельно.
Применяя правило (9) к уравнению системы с П-регулятором (7), убеждаемся в том, что неравенства (9) несовместны, т.е. что не сущест- вует числа K
0
, делающего систему (7) асимптотически устойчивой. Та- ким образом, П-регулятор оказывается неработоспособным, и для ре- шения задачи поиски нужно продолжить.
Следующим этапом является построение ПД-регулятора, который можно описать соотношением

203
u
t
= K
0
(x* – x
t
) + K
1
(x
t+1
– x
t
),
(10) где K
1
— коэффициент усиления по разности (дифференциальный). По- скольку в момент времени t измерить x
t+1
невозможно, для реализации регулятора (10) можно воспользоваться соотношением
x
t+1
– x
t
= vhθ
t
= vhθ
t–1
+ u
t–1
vh2r / b.
Подставляя в (10), получим ПД-закон управления в виде
u
t
= [K
0
(x* – x
t
) + K
1
vh(θ
t–1
+ u
t–1
hr / b)],
(11) т. е. для применения (11) требуется помнить значения курсового угла и управления на предыдущем шаге. Для исследования устойчивости и выбора коэффициентов ПД-регулятора подставим (10) в (4). Получим уравнение замкнутой системы:
x
t+2
– (2 + gK
1
)x
t+1
+ (1 + gK
0
+ gK
1
)x
t
= gK
0
x*.
(12)
Проверяя его на устойчивость с помощью критерия (9), убеждаем- ся в том, что достаточными условиями устойчивости служат два нера- венства
K
0
> 0, K
0
+ K
1
< 0.
(13)
Таким образом, для построения работоспособного регулятора сле- дует выбирать дифференциальный коэффициент, превосходящий про- порциональный по абсолютному значению и противоположный по зна- ку. В справедливости этого правила также следует убедиться в экспе- риментах.
Предложенные методики исследования и реализации мобильных роботов на основе элементарной теории управления способствуют рос- ту интереса учащихся к робототехнике и, более широко, к инженерным наукам.

204

перейти в каталог файлов