roboforum.ru

[PANYTA]

Здравствуйте.
Сделал я балансирующего робота:

Вернее, делаю.
В сравнении с моим первым механизмом он, конечно, живчик.
Но в сравнении с этим

Мой - просто увалень.
Аппаратная часть взята почти полностью здесь. Моторы только поменьше (дешевле в два раза ), да контроллер из того, что было (ATmega8A).
Программу пытаюсь сбацать сам. Взял из оригинала то, что позволили знания С+ (почти нулевые).

На этом форуме были аналогичные темы, но там я не нашёл ответа на свои вопросы.
Да и вообще нигде не нашёл. В рунете практически тишина. За бугром - языками не владеем.
Собственно вопрос: какой должен быть алгоритм управления?
Не в смысле удержания в вертикальном положении, а в смысле управления движением.
Сейчас алгоритм того, что на первом видео такой:

Зелёным - то, что держит его в вертикальном положении. Тут всё достаточно просто, понятно, предсказуемо. Просто ПИД регулятор.
Все реакции на изменения коэффициентов соответствуют ожидаемым. Стоит себе, покачиваясь, на горизонтальной (примерно) поверхности.
И катится, разгоняясь, по наклонной. Разогнавшись до максимума (своего) падает. Всё как положено. В точке А подключается собственно управление.

А вот с нижней, красной петлёй обратной связи всё сложнее.
Понимаю, что нужно управлять "нужным углом". Т.е. "наклонять" его, скажем так, внутреннюю вертикаль, чтобы он ехал в нужном направлении.
Но вот как управлять? Алгоритм? Свой крутил/вертел... Коэффициенты менял... Составляющие ПИД2 подключал/отключал... Хуже делается. Лучшее - на первом видео.
Ужель всё дело в тонких настройках коэффициентов? Или таки алгоритм не совсем "правильный"?

Подглядел кое что тут.
Тут ребята сами с собой поговорили. Высшая математика.

Мне бы "на пальцах"... Понять бы физику/математику процесса. А реализовать - дело техники, что ASM что C, да что угодно.

Склоняюсь уже к тому, что в моём варианте недостаточная "энерговооружённость".
Моторы поменьше (послабее), да и токи на моторах я ограничил до 150ма на каждом, чтобы батареек мал мал хватало.

[Madf]

не дешевые, точные степеры в качестве привода, неудивительно, что у них так всё плавно/красиво и с большой отдачей

[vvk]

Собственно вопрос: какой должен быть алгоритм управления?
Не в смысле удержания в вертикальном положении, а в смысле управления движением.

Равновесие держит он у вас для таких двигателей нормально. Подстроить ПИ-регулятор можно, но не сильно. В реальном времени сначала увеличивайте П - составляющую до тех пор, пока не начнется вибрация. А затем аналогично И - составляющую. Д - составляющая в данном случае не нужна, она только мешать будет. Ее лучше задать равной 0.

А вот с нижней, красной петлёй обратной связи всё сложнее.

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



Для смещения центра тяжести я ему на плату специально положил свинцовый аккумулятор. Соответственно для движения я его наклоняю в нужную сторону. А для торможения в обратную.

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

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


Собственно говоря и вся теория

[PANYTA]

Да с теорией я знаком...
Алгоритм?
Исходниками не поделитесь?

[PANYTA]

Ммдяяя...
Волшебное слово "исходники" отпугнуло единственного собеседника...

[vvk]

Ммдяяя...
Волшебное слово "исходники" отпугнуло единственного собеседника...

Вы вряд-ли будете разбираться в моем проекте, т.к. он на 99% состоит из TCP/IP стека от микрочип.

Но если есть желание, вот ссылка на проект: https://yadi.sk/d/TYaAohK034mRLP Не удивляйтесь - его размер около 100mB. Проект делался с целью изучить TCP/IP стек, а робот был добавлен скорее для развлечения. Комментариев там, конечно, никаких нет.

Аппаратная часть робота состоит из такого контроллера: forum4/topic11707.html#p242504 , к которому подключен WiFi-модуль MRF24WB0MA. Данные о наклоне получает с этого датчика: forum4/topic11675.html#p241703 .

Проект открывается в MPLAB.X : ...\BalancingMplabX\TCPIP\WiFi Console\BalancingRobot.X

Файлы самого робота находятся в каталоге проекта "Source Files\Balancing". Вам будут нужны файлы "mainBalancing.c" и "Tilt.c" . В первом находится весь алгоритм движения робота, во втором - протокол обмена с датчиком наклона.

Все остальное вам не нужно. Если будет сильно интересно, можете посмотреть файл "Dynamixel.c". Там реализован протокол обмена с сервоприводами Dynamixel.


Основная функция робота - void mainBalancing(void). Работает, когда пришел пакет с данными от датчика наклона (при наличии нового пакета в файле "Tilt.c" устанавливается флаг "HvTiltPacket").


К этому роботу еще где-то была управляющая программа для ПК. Если будет нужна, поищу.

[PANYTA]

вот ссылка

Спасибо.
Почитал...
Вопросы можно?

[PANYTA]

Расшифровал я void mainBalancing ка смог:

Код: Выделить всё • Развернуть

void mainBalancing(void)
    {


        if (!SB2)
        {
            /*
    KpPos=5;
    KdPos=70;
    KiPos=3;
    CntImpR=0;
    CntImpL=0;
    EPosSumm=0;
    */
        }


        if (HvTiltPacket)
        {
        HvTiltPacket=0;

        ZeroSPD();

        CntPos--;


        if (!CntPos)//(EPos>50)||(EPos<-50))
        {
//POSITION
            CntPos=CntPosConst;


 //       TestOut_IO=1;

long int  Temp=(CntImpR+CntImpL)>>1;                слаживем и делим пополам импульсы от левого и правого энкодеров
                                                    получаем "пробег" (за 1 цикл)

        EPos=PosZad-Temp;//((CntImpR+CntImpL)>>1);  вычисляем разницу (ошибку) между измеренным (Temp) и "нужным" (PosZad) пробегами

        EpPos=EPos*(float)KpPos/100; ---------------
                                                    \
        Ediff=EPos-EPosPrev;                         \
                                                      \
        EdPos=(float)Ediff*KdPos/100;                  \ прогоняем ошибку через ПИД
        EPosPrev=EPos;                                 /
                                                      /
                                                     /
        EPosSumm=EPosSumm+EPos;                     /
        EiPos=EPosSumm*KiPos/1000;-----------------/
                         
        Angle=EpPos+EdPos+EiPos;                       получаем поправку к углу наклона
        

        if (Angle>AngleLimit)
            Angle=AngleLimit;
                                                       ограничиваем...
        if (Angle<-AngleLimit)
            Angle=-AngleLimit;
//POSITION END


                                                      ТЕПЕРЬ ВТОРОЙ РЕГУЛЯТОР (ПИ)                                    
                                                      От Tilt.c никаких кватернионов. Просто данные с гироскопа и акселерометра.


        }
        TiltYRes=TiltY-TiltCorr;                            корректируем (на константу TitlCorr) угол наклона от акселерометра

        DbgPack[CntUDPTrmConst-CntUDPTrm].TiltY=TiltYRes; \
        if (!TS)                                           \ 
            TS=250;                                         \  ????????  не понял...    Что за Дебаг?  Готовим отправку текущего состояния на комп?
        else                                                /
            TS=0;                                          /

        E=TiltZad-(int)Angle-TiltYRes;                      Вычисляем ошибку. TitlZad - заданный угол (показания акселя в вертикальном положении корпуса)
        Ep=E*Kp;                                            Минус поправка (результат первого ПИДа), минус текущее положение корпуса (измеренное акселем).               
        DbgPack[CntUDPTrmConst-CntUDPTrm].Ep=Ep;             ?????????

        Ed=-Gyro4Y;                                         получаем ИЗМЕНЕНИЕ угла от гироскопа (за 1 цикл)
        DbgPack[CntUDPTrmConst-CntUDPTrm].Gyro4X=Gyro4Y;      ?????????
        
        Ed=Ed*Kd+Kdt*EdPrev;                                 Комплиментарный фильтр.
        EdPrev=Ed;

        DbgPack[CntUDPTrmConst-CntUDPTrm].Ed=Ed;             ???????????????


        EPrev=E;                                              Комплиментарный фильтр.

        ESumm=ESumm+E;                                      Интегральная составляющая...
   Ei=(float)Ki*ESumm/100;

        DbgPack[CntUDPTrmConst-CntUDPTrm].Ei=Ei;            ??????????????

        if (Ei>30000)
            Ei=30000;
                                                           ограничили И
        if (Ei<-30000)
            Ei=-30000;

                                                          
        TempR=Ep+Ed+(int)Ei;                               Комплиментарный фильтр. Пропорц. +  интегр.               
        TempL=TempR;

        
        if (TempR>3000)
            TempR=3000;
        if (TempR<-3000)
            TempR=-3000;
                                                           ограничили
        if (TempL>3000)
            TempL=3000;
        if (TempL<-3000)
            TempL=-3000;


       PWMResR=(int)TempR+PWMRotR;                           добавили
       PWMResL=(int)TempL+PWMRotL;                           повороты вл. впр.

        DbgPack[CntUDPTrmConst-CntUDPTrm].PWM=PWMResR;
                                                              Далее устанавливаем ШИМ...
//       PWMResR=PWMResR+PosR;

       if ((TiltYRes<400)&&(TiltYRes>-400))
       {
        SetRightPWM(PWMResR);//PWMRes);
        SetLeftPWM(PWMResL);//PWMRes);
       }
       else
       {
        SetRightPWM(0);//PWMRes);
        SetLeftPWM(0);//PWMRes);
        while (1);
       }

                    CntSamples++;
        DbgPack[CntUDPTrmConst-CntUDPTrm].CntPackets=CntSamples;
        MustTrm=1;

        if (CntUDPTrm==1)
        {
int k;
                    for (k=0;k<CntUDPTrmConst;k++)
                    {

                        DbgPackSend[k].CntPackets=DbgPack[k].CntPackets;
                        DbgPackSend[k].TiltY=DbgPack[k].TiltY;
                        DbgPackSend[k].Gyro4X=DbgPack[k].Gyro4X;
                        DbgPackSend[k].Ep=DbgPack[k].Ep;
                        DbgPackSend[k].Ed=DbgPack[k].Ed;
                        DbgPackSend[k].Ei=DbgPack[k].Ei;
                        DbgPackSend[k].PWM=DbgPack[k].PWM;
                    }
                    CntPackets++;

        }
        }
    
}

vvk, почитайте, пожалуйста, мои комментарии.
Ткните носом, если где ошибка...

И главное. Я таки не понял - где собственно управление движением?
Растолкуйте чайнику, если можно.

[vvk]

Расшифровал я void mainBalancing ка смог:

vvk, почитайте, пожалуйста, мои комментарии.
Ткните носом, если где ошибка...

И главное. Я таки не понял - где собственно управление движением?
Растолкуйте чайнику, если можно.

Вы в принципе, разобрались правильно. Только чуть поправлю:

Управление движением.
Все входящие команды от сети обрабатываются в функции "void ReceiveUDP(void)" . В ней выполняется получение настроек (например, коэффициентов ПИ - регуляторов, калибровка и т.д.) и получение команд на движение. Для поворота задаются значения переменных PWMRotR и PWMRotL, которые затем прибавляются к рассчитанному ранее значению ШИМ. Для движения вперед/назад задается добавочный угол наклона TiltZad. Изменением этого угла регулируется скорость движения робота. В переменной PosZad задается точка, в которую должен приехать робот (в значениях импульсов энкодеров). По-моему, в этой программе PosZad всегда равна 0.

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

[PANYTA]

Вопросов стало только больше...

При всём уважении к Автору. VVK, похоже вы подзабыли свой проект.
Ещё раз извините за такие предположения, но:
-"Угол с датчика наклона берется уже готовый"
А что тогда за строчки в "void mainBalancing" с гироскопом (Ed=-Gyro4Y)? Ниже она складывается с акселерометром.

Движение.
Если задать TiltZad ненулевую, то робот будет разгоняться. Именно разгоняться, а не ехать с постоянной скоростью.
Мы ведь тем самым просто изменим "нужный угол" на константу. То же самое будет если просто сместить ему центр тяжести. Или наклонить под ним поверхность.

Как PosZad задаёт дистанцию тоже непонятно.
Если Temp=(CntImpR+CntImpL)>>1 это "пробег" за один цикл - то это получается скорость...

Ниччё не понял...

[vvk]

Вопросов стало только больше...

-"Угол с датчика наклона берется уже готовый"
А что тогда за строчки в "void mainBalancing" с гироскопом (Ed=-Gyro4Y)?

Код: Выделить всё • Развернуть

Ed=-Gyro4Y;   //Ошибка для дифф. составляющей, которую можно считать либо как разницу между текущим и предыдущим 
//отсчетами, либо просто взять угловую скорость с гироскопа по оси колес. В данном случае она берется с гироскопа, 
//так получается точнее. НО, КАК Я ПИСАЛ РАНЕЕ, ДИФФ. СОСТАВЛЯЮЩУЮ Я НЕ ИСПОЛЬЗУЮ, т.к. она в такой конструкции 
//приводит к вибрации. Чтобы дифф. составляющую можно было применить, нужно уменьшить дискретизацию, время реакции 
//системы, убрать все люфты. Поэтому у меня Kd=Kdt=0; 
       
        
        Ed=Ed*Kd+Kdt*EdPrev;   //Да, это когда-то был комплиментарный фильтр, но потом я стал задавать Kdt=0
        EdPrev=Ed;             

Ниже она складывается с акселерометром

Нет, ниже она складывается с П-составляющей и И-составляющей. И в результате получается значение ШИМ для двигателей:

Код: Выделить всё • Развернуть

TempR=Ep+Ed+(int)Ei;//Правое колесо
TempL=TempR; //Левое колесо

DbgPack - Это просто отладочная структура с текущими данными, которая передается в ПК. Не обращайте на нее внимания.

Если задать TiltZad ненулевую, то робот будет разгоняться. Именно разгоняться, а не ехать с постоянной скоростью.
Мы ведь тем самым просто изменим "нужный угол" на константу. То же самое будет если просто сместить ему центр тяжести. Или наклонить под ним поверхность.

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

Как PosZad задаёт дистанцию тоже непонятно.
Если Temp=(CntImpR+CntImpL)>>1 это "пробег" за один цикл - то это получается скорость...

Temp - это расстояние в импульсах, поступающих с энкодеров (среднее значение двух колес). Зная диаметр колес и число импульсов на оборот, можно пересчитать их в метры.

[PANYTA]

Когда достигнута требуемая скорость, наклон робота опять становится нулевым

Вот это меня и интересует.
Где в mainBalancing "требуемая" скорость?
Или. Кто (какая переменная) делает угол нулевым после набора скорости?

Мне как юзеру, просто пользователю, управляющему машинкой, фиолетово какой там угол наклона, в какую сторону,
в какой момент его нужно убрать...
Управляя, я задаю скорость, а он (алгоритм) сам управляет углом, регулируя (стабилизируя) заданную скорость.

[vvk]

Вот это меня и интересует.
Где в mainBalancing "требуемая" скорость?

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

[PANYTA]

отпускаю клавишу

ЁПРСТ...
А у меня было подозрение на такой ответ.
Ну да ладно. Отрицательный результат - тоже результат.
Будем искать (с перламутровыми пуговицами ). Будем сами чёнить пробовать.

Спасибо за немолчание.
С прошедшими!