Система дистанционного управления роботом «Феникс 2

Система дистанционного управления роботом «Феникс 3»
А.В. Бурдуков, студент-стажер СКБ ГУАП
Введение.
Данная работа была проведена в рамках студенческого исследовательского проекта
«Феникс -3», проводимого СКБ ГУП.
Целью этого проекта является изучение методов синтеза нейронных регуляторов
применительно к автономным мобильным роботам. Традиционная терминология
использует термин “Обучение нейронной сети ”, под которым понимается процедура
выбора весов искусственных нейронов, образующих управляющую сеть [1].[2]. В
англоязычной литературе применяют
термины: “Teaching by showing”, “Supervised
learning ” и т.д.
В рамках студенческих
исследовательских проектов “Феникс” проводиться
изучение систем управления мобильными объектами на базе нейронных сетей [3],
обучаемых в соответствии с методикой “Обучение через показ “ с использованием
одношаговой процедуры обучения. Общая концепция предлагаемого подхода изложена в
работе [4] .
Одним из ключевых элементов при использовании этой методики является
использование экспериментальной информации о показаниях датчиков и управляющих
воздействиях “учителя” (оператора), получаемых в процессе обучающих заездов.
В данной работе представлено описание
системы управления оператора,
реализованной в рамках проекта “Феникс-3.
Автономный робот Феникс-3
предназначен для автономного патрулирования в
заданном районе с целью обнаружения очагов возгорания. В случае обнаружения
источника робот должен приблизиться к очагу возгорания и используя бортовой
огнетушитель погасить огонь. Для ориентации на местности предполагается использовать
купольную видеокамера с поворотным механизмом и трансфокатором в вандалозащищенном исполнении. Внешний вид робота Феникс-3 приведен на рис. В.1 – В.4
Рис. В.1.
Рис. В.2.
Рис. В.3.
Рис.В.4.
1. Структурная схема системы дистанционного управления.
В соответствии с постановленной задачей требовалось обеспечить дистанционное
управление скоростью и направлением вращения ходовых двигателей двухколёсного
робота, что обеспечивает возможность оператору управлять перемещением робота вперёдназад, а также поворотами влево-вправо.
На основании накопленного в проекте “Феникс-2” опыта было принято решение
заменить цифровой канал управления на аналоговый. Целью этой модификации являлась
желание уменьшить искажения вносимые джойстиком управления и задержек цифрового
тракта при передаче команд управления. В качестве пульта оператора был использован
пульт радиоуправления, используемый в модельном спорте.
Структурная схема системы дистанционного управления приведена на рис. 1.1.
К электродвигателям
1
2
3
4
1
5
К электродвигателям
Рис.1.1 Структурная схема системы дистанционного управления
робота “Феникс-3”
1 – выходной мост; 2 – плата контроллера; 3 – приёмник
радиосигнала; 4 – пульт управления; 5 – источники питания.
Пульт управления формирует радиосигнал, промодулированный в соответствии с
текущим положением ручек управления. Приёмник принимает излучённый радиосигнал и
выделяет из него сигнал модуляции. Плата контроллера производит извлечение из сигнала
модуляции информации о положении ручек управления пульта, и в соответствии с ней
формирует сигналы ШИМ и направления для левого и правого электродвигателя робота.
Выходные мосты 1 усиливают сигнал ШИМ по мощности, а также переключают его
полярность в соответствии с сигналом направления вращения.
Источник питания представляет собой комплект кислотно-свинцовых аккумуляторов
напряжением 12 В. Он обеспечивает электропитание узлов 1 – 3. Пульт управления 4
имеет автономный источник питания.
В качестве узлов для работы по радиоканалу использован промышленный комплект
пропорционального дистанционного управления по аналоговому радиоканалу Hitec
FOCUS 6. Он включает в себя пульт управления и приёмник-детектор радиосигнала.
Пульт управления в соответствии с положением ручек управления формирует следующий
модулирующий сигнал, структура которого представлена на рис.1.2.
20 мс
±0,6 мс
1,6
мс
Рис.1.2. Структура сигнала пульта FOCUS 6
Сигнал управления представляет собой пачку из 6 импульсов. Первые 4 импульса
содержат информацию о положении по осям X и Y первой и второй ручек управления. 5
импульс содержит информацию о положении переключателя «gear», а шестой – о
положении ручки «CH6». В среднем положении ручек длительность соответствующего
импульса составляет 1,6 мс. При изменении положения ручек соответственно
увеличивается или уменьшается длительность импульсов. В крайних положениях
изменение длительность изменяется примерно на 0,6 мс. Длительность паузы между
импульсами составляет примерно 1,5 мс. После завершения передачи пачки, через 20 мс
начинается формирование новой пачки в соответствии с текущим положением ручек
управления.
Далее производится частотная модуляция радиосигнала в соответствии с
сформированным сигналом. Центральная частота несущей – 40,675 МГц. Приёмник
радиосигнала выделяет модулирующую пачку из принятого радиосигнала.
2. Контроллер робота “Феникс-3”.
Плата контроллера представляет собой модернизированную макетную плату,
использованную для других разработок. Эта решение было продиктовано временными
ограничениями, отведенными на реализацию проекта. Она содержит микроконтроллер
PIC18F2320, а также пассивные элементы и элементы индикации.
Плата закреплена на специальном несущем основании, которое устанавливается в
отсек робота. К ней с помощью разъёмных соединителей подключается передняя панель,
на которой размещены разъёмы питания, соединения с выходными мостами, элементы
внешней индикации, и управления. На макетной плате также закрепляется
радиоприёмный модуль.
Контролер вырабатывает управляющие сигналы, поступающие на мосты управления
двигателями. Мосты обеспечивают усиление по мощности сигналов ШИМ,
сформированных платой контроллера, а также переключение их полярности в
соответствии с сигналом направления вращения. Эти мосты были заимствованы из
проекта робота “Феникс-2”.
Схема электрическая принципиальная платы контроллера и мостов управления
двигателями робота приведена в Приложении 1 и Приложении 2 соответственно.
Контроллер устанавливается в доработанную корзину стандарта “Евромеханика”,
образующую приборный отсек робота. Внешний вид контроллера, установленного в
приборный отсек, приведён на рис. 2.1. Как видно из рисунка, коммуникации мостов и
датчиков робота оказалось целесообразным вынести на переднюю панель контроллера.
Рис. 2.1
Контроллер робота выполнен на микроконтроллер фирмы Microchip PIC18F2320.
Базовыми функциями разработанной системы дистанционного управления являются :
a) декодирование сигналов, принимаемых с пульта управления
б) формирование сигналов управления двигателям.
При декодировании сигнал модуляции, выделенный приёмником радиосигнала,
поступает на вход прерывания INT0 МК. При каждом изменении состояния этого сигнала
формируется аппаратное прерывание. Программный обработчик этого прерывания
фиксирует моменты изменения состояния. Для измерения времени между изменениями
состояния используется встроенный таймер TMR0 МК. Таким образом, это позволяет
измерять длительность каждого импульса в пачке. В нашем случае анализируется
длительность только 3 и 4 импульсов в пачке, т.е. информация о положении по осям X и Y
одной ручки пульта. На основании полученной информации программа МК рассчитывает
2 16-битных числа, определяющие скорость и направление вращения двигателей робота.
Предусмотрена компенсация положения ручки управления с сохранением значения
калибровки в энергонезависимой памяти контроллера.
В программе обработки сигналов пульта также предусмотрен алгоритм обнаружения
пропадания сигнала с пульта управления. При пропадании сигнала программа
обеспечивает формирование признака необходимости экстренной остановки двигателей.
Программное обеспечение обеспечивает управление аппаратными формирователями
ШИМ МК и сигналом направления вращения. Для управления скоростью вращения
коллекторных двигателей постоянного тока используется импульсное питание. При
увеличении длительности импульсов происходит увеличение скорости вращения
двигателей. Соответственно, можно менять длительность импульсов от 0 до значения
периода импульсов (модуляция ШИМ от 0 до 100%). Период ШИМ – примерно 20 мкс.
При формировании сигналов управления оказалось удобным иметь два режима
работы: сервисный и рабочий.
В сервисном режиме проводится стендовая проверка работоспособности системы
управления. В этом режиме обеспечивается возможность пошагового приращение
скорости вращения левого или правого двигателя соответственно с кнопок автономного
управления, реализованных на контроллере.
В рабочем режиме программа управляет генераторами ШИМ сигнала и сигналом
направления вращения в соответствии со значениями желаемой скорости вращения,
рассчитанными декодером сигналов пульта управления. Предусмотрено плавное
изменение длительности ШИМ при резком измерении значений скорости (т.е. при резком
изменении положения ручки управления). Это позволяет избежать возникновения
импульсных перегрузок в работе ходовой части робота. Также программа обеспечивает
экстренную остановку двигателей при обнаружении признака необходимости экстренной
остановки.
Программа для контроллера написана на языке С18, откомпилирована в среде
MPLAB 8.2 . В качестве отладчика и программатора использовался Microchip MPLAB
ICD2. Листинг программы приведен в Приложении 3.
3. Испытания системы.
Для проверки работоспособности системы и оценки ее эксплуатационных качеств
были проведены ее испытания в натурных условиях. Проверка включала в себя езду
робота в полевых условиях с дистанционным управлением скоростью и направлением
движения оператором, а также езду с полезной нагрузкой. Система дистанционного
управления выдержала испытания, сбоев в работе замечено не было.
Видеоролик с результатами испытаний системы управления робота Феникс-3
представлены на сайте http://guap.ru.
Проведенные испытания показали
необходимость доработать алгоритма
вычисления скорости вращения двигателей в соответствии с особенностями ходовой
части робота. Соответственно, в качестве первоочередной задачи для следующей фазы
проекта в части совершенствования системы управления является оснащение робота
датчиками оборотов.
Также представляется целесообразным разделение приёмной и ходовой части платы
контроллера на отдельные функционально законченные блоки.
Заключение
В заключение автор выражает благодарность коллективу СКБ ГУАП и OOO “ASK
Lab” за всестороннюю поддержку в реализации данного проекта.
Литература
1. Саймон Хайкин. Нейронные сети. Полный курс. Издание второе, исправленное.
Издательский дом “Вильямс”. Москва-Санкт-Петербург-Киев. 2006. с. 1104.
2. Industrial application of neural networks. Edited by Lakhmi C.Jain, V.Rao Vemuri. CRC Press
LLС, Boca Raton, London, New York, Washington, D.C ,1999.
3. Д.А.Астапкович, А.А.Гончаров, А.С.Дмитриев, А.В.Михеев. Проект “Робот Феникс-1”. Сборник
докладов пятьдесят девятой международной студенческой научно-технической конференции ГУАП.,
Часть I, Технические науки, 18-22 апреля 2006, г. Санкт-Петербург, с.211-216.
4. Astapkovitch A.M. Оne step learning procedure for neural net control system. Proc. International
forum “Information systems. Problems, perspectives , innovation approaches” , Vol.2, p.3-9, SUAI, SaintPetersburg, 2007.
Приложение 1.
Схема электрическая принципиальна контроллера робота “Феникс-3”
Приложение 2.
Схема электрическая принципиальная моста управления двигателем робота
«Феникс 3».
Приложение 3.
Листинг программы контроллера робота “Феникс-3”
#include "p18F2320.h"
#include <eep.h>
void InterruptHandlerRControl(void);
void InterruptHandlerTimerZ(void);
void tunePWM(unsigned char period, short duty, char chan);
char DAV=1; //данные достоверны?
unsigned char countDEC=0; //сколько импульсов 1 на вход пришло
char IsService=0; //1 - сервисный режим
short LeftPWM=0; //желаемые значения ШИМа
short RightPWM=0;
short currLeftPWM=0; //текущие значения ШИМа
short currRightPWM=0;
short ZeroUDPWM; //калибровочные числа
unsigned char ZeroUDPWMh; //побайтовое представление
unsigned char ZeroUDPWMl;
short ZeroLRPWM;
unsigned char ZeroLRPWMh;
unsigned char ZeroLRPWMl;
short UDPWM; //временные переменные положения ручек
short LRPWM;
#pragma code InterruptVectorRControl=0x08 //это выполнится на высокоприоритетном
прерывании
void InterruptVectorRControl(void) {
_asm
goto InterruptHandlerRControl //там основной обработчик
_endasm
}
#pragma code InterruptVectorTimerZ=0x18 //это выполнится на низкоприоритетном прерывании
void InterruptVectorTimerZ(void) {
_asm
goto InterruptHandlerTimerZ //там основной обработчик
_endasm
}
#pragma code
#pragma interrupt InterruptHandlerRControl //основной обработчик
void InterruptHandlerRControl() {
unsigned char pred;
if (!INTCON2bits.INTEDG0) { //если ноль на вх пришёл
T0CONbits.TMR0ON=0; //стоп
pred=TMR0L; //обновить значение TMR0H
if (TMR0H>9) { //если большая длительность импульса
countDEC=0;
DAV=1; //восстанавливаем флаг достоверности
}
if (countDEC>4) {DAV=0; //более 4 импульсов не анализ.
}
if (countDEC>6) { //более 6 импульсов – пульт по видимому выключен!
countDEC--;
LeftPWM=0;
RightPWM=0;
}
if ((countDEC==3) & DAV) {
ZeroUDPWMh=TMR0H;
ZeroUDPWMl=pred;
UDPWM=(TMR0H*256+TMR0L-ZeroUDPWM)*4; //вычисляем промежуточные числа
}
if ((countDEC==4) & DAV) {
ZeroLRPWMh=TMR0H;
ZeroLRPWMl=pred;
LRPWM=(TMR0H*256+TMR0L-ZeroLRPWM)*2;
}
if (DAV) {
PORTBbits.RB3=1;
LeftPWM=UDPWM-LRPWM; //теперь вычисляем реальные скорости вращения колёс
RightPWM=UDPWM+LRPWM;
if (LeftPWM>1023) {LeftPWM=1023;}
if (LeftPWM<-1023) {LeftPWM=-1023;}
if (RightPWM>1023) {RightPWM=1023;}
if (RightPWM<-1023) {RightPWM=-1023;}
}
INTCON2bits.INTEDG0=1; //сменим на прерывание по фронту
} else { //а ежели 1 то
countDEC++;
TMR0H=0; TMR0L=0; //сброс таймера
INTCON2bits.INTEDG0=0; //сменим на прерывание по срезу
T0CONbits.TMR0ON=1; //запуск счёта
}
INTCONbits.INT0IF=0; //сбрасываем флаг прерывания
}
#pragma code
#pragma interrupt InterruptHandlerTimerZ //основной обработчик
void InterruptHandlerTimerZ() {
if (IsService) { //если сервисный режим
if (INTCON3bits.INT2IF) { //обрабатываем нажатия кнопок
PORTBbits.RB3=1;
if (!PORTAbits.RA0) {RightPWM+=32;}
else {RightPWM-=32;}
INTCON3bits.INT2IF=0;
}
if (INTCON3bits.INT1IF) {
PORTBbits.RB3=1;
if (!PORTAbits.RA0) {LeftPWM+=32;}
else {LeftPWM-=32;}
INTCON3bits.INT1IF=0;
}
} else { //иначе нажатие кнопки "2" только обрабатываем
if (INTCON3bits.INT2IF) {
ZeroUDPWM=ZeroUDPWMh*256+ZeroUDPWMl; //настроим калибровки
ZeroLRPWM=ZeroLRPWMh*256+ZeroLRPWMl;
Write_b_eep (0,ZeroUDPWMh); //сдампим калибровку в NVRAM
Busy_eep ();
Write_b_eep (1,ZeroUDPWMl);
Busy_eep ();
Write_b_eep (2,ZeroLRPWMh);
Busy_eep ();
Write_b_eep (3,ZeroLRPWMl);
PORTBbits.RB3=1;
INTCON3bits.INT2IF=0;
}
}
if (PIR1bits.TMR1IF) {
PORTBbits.RB3=0;
if (LeftPWM>currLeftPWM) {currLeftPWM++;}
if (LeftPWM<currLeftPWM) {currLeftPWM--;}
if (RightPWM>currRightPWM) {currRightPWM++;}
if (RightPWM<currRightPWM) {currRightPWM--;}
if ((!PORTAbits.RA0) | (IsService)) {
tunePWM(0xff,currLeftPWM,0);
tunePWM(0xff,currRightPWM,1);
} else {
tunePWM(0xff,0,0);
tunePWM(0xff,0,1);
}
TMR1H=0xff; //чтобы сократить время счёта таймера
PIR1bits.TMR1IF=0; //сбросим флаг прерывания
}
}
void setup(void){
OSCCON=0x73; //0x33; //ставим 8 МГц внутр.
ADCON0bits.ADON=0; //выкл АЦП
ADCON1bits.PCFG3=1; //все ноги - цифровые I/O
TRISA=1; //порт А на вывод кроме RA0
TRISB=7; //все ноги порта В на выход кроме INT-ов
TRISC=0x80; //все ноги на выход кроме последней (которая RX)
PORTA=0;
PORTB=0;
if (!PORTAbits.RA0) {IsService=1;}
T2CON=7; //настраиваем и вкл таймер 2 - предделитель 1:16
//настройка блоков ШИМ
CCP1CON=0x0f;
CCP2CON=0x0f;
//настройка таймера 0
T0CON=1; //предделить 1:4
//настройка таймера 1
T1CON=5; // fosc/16
INTCON2bits.RBPU=0; //вкл. пулапы
//Настраиваем прерывания
RCONbits.IPEN=1; //Разрешить систему приоритетов
IPR1bits.TMR1IP=0; //низкий приоритет прерывания таймеру 1
PIE1bits.TMR1IE=1; //разрешить прерывания от таймера 1
//Настройка ноги INT2
INTCON2bits.INTEDG2=0; //Прерывание по срезу
INTCON3bits.INT2IP=0; //Низкий приоритет
INTCON3bits.INT2IE=1; //Разрешаем прерывание
if (IsService) {
//Настройка ноги INT1
INTCON2bits.INTEDG1=0; //Прерывание по срезу
INTCON3bits.INT1IP=0; //Низкий приоритет
INTCON3bits.INT1IE=1; //Разрешаем прерывание
} else {
//Настройка ноги INT0
INTCON2bits.INTEDG0=0; //Чтоб прерывание было по срезу вначале
INTCONbits.INT0IE=1; //Разрешаем прерывания от ноги
ZeroUDPWM=Read_b_eep(0)*256+Read_b_eep(1); //читаем значения калибровки
ZeroLRPWM=Read_b_eep(2)*256+Read_b_eep(3);
}
INTCONbits.GIEH=1; //Разрешаем высокоуровневые прерывания
INTCONbits.GIEL=1; //Разрешаем низкоуровневые прерывания
}
void tunePWM(unsigned char period, short duty, char chan){
//это настраивает параметры ШИМа. Передавать надо период, длительность и
//канал. 0 - левый, 1 - правый
char Lduty; //временные переменные
if (duty<0) {
duty=(-duty);
if (chan) {
PORTBbits.RB5=0;
} else {
PORTBbits.RB4=0;
}
} else {
if (chan) {
PORTBbits.RB5=1;
} else {
PORTBbits.RB4=1;
}
}
PR2=period; //Значение периода
if (chan) {
CCPR2L=duty >> 2; //пишем 8 старших битов
} else {CCPR1L=duty >> 2;}
Lduty=(duty&3) << 4;
if (chan) {
CCP2CON=(CCP2CON&0xcf)|Lduty; //подсовываем биты 5 и 4 в регистр
} else {
CCP1CON=(CCP1CON&0xcf)|Lduty;
}
}
void main(void) { //запуск программы
setup(); //настроить всё
tunePWM(0xff,0,0); //вначале полный стоп
tunePWM(0xff,0,1);
while (1) {} //далее пустой цикл
}