2014 dxdy logo

Научный форум dxdy

Математика, Физика, Computer Science, Machine Learning, LaTeX, Механика и Техника, Химия,
Биология и Медицина, Экономика и Финансовая Математика, Гуманитарные науки




На страницу 1, 2  След.
 
 Балансирование тела на 2 точках опоры
Сообщение01.09.2013, 12:47 
Возникла мысль сделать такую игрушку. Ботом эта мысль быстро трансформировалась в балансирование на одной точке опоры, но я себя сдержал и сначала решил сделать хотя бы на двух :-) Конструкция - прямоугольник из листового материала ставится вертикально на свою сторону, на нем закреплен датчик угла отклонения от вертикали (пока не будем об особенностях реализации, считаем что он есть), ПИД контроллер и двигатель с редуктором, ось которого горизонтальна и находится в плоскости нашего прямоугольника. На оси двигателя жестко закреплен маятник качения, который, работая как противовес, балансирует всю конструкцию и не дает ей упасть. Я создал такую тему на форумах робототехники, определяюсь с моделями двигателей и уже хочу собрать рабочий макет для экспериментов. Да, сразу макет, вместо нормального подхода - теоретических расчетов, моделирования системы в матлабе/симулинке, выбора параметров и т.п. Понимаю, что такой подход дилетантский и не гарантирует никакого результата, но я не обладаю достаточными знаниями теории и симулинка для подобных расчетов. Хотя и практические такое конструкции я тоже еще не строил, с другой стороны :-)
Собственно, вопросы, на которые хотелось бы получить хотя бы качественные ответы:
1) центр тяжести всей конструкции мне следует стремиться делать как можно ниже или как можно выше?
2) маятник-балансир лучше вниз или вверх?
3) длина маятника-балансира - максимальная (чуть меньше высоты прямоугольника) или какая лучше?
4) масса маятника-балансира относительно массы конструкции и взаимного расположения ее центра тяжести, точки закрепления и уровня груза маятника-балансира?
5) что-то еще, что забыл спросить. но может быть важным?

ЗЫ мне дали ссылку на интересный аппнот микрочипа http://www.microchip.com/stellent/idcpl ... e=en021807 , там есть формула зависимости углового ускорения обратного маятника в зависимости от его длины и угла отклонения, из нее следует, что чем длиннее маятник, тем проще его балансировать, для модели они взяли длину пол метра. Если у меня такая же ситуация, то мне надо ориентироваться на примерно такую длину и делать центр тяжести конструкции как можно выше. С другой стороны, чем ниже центр тяжести, тем меньше усилий надо для балансировки (?). Я запутался даже в таком простом вопросе. Выскажите пожалуйста свои соображения.

 
 
 
 Re: Балансирование тела на 2 точках опоры
Сообщение01.09.2013, 13:12 
Аватара пользователя
_Ivana в сообщении #759493 писал(а):
двигатель с редуктором, ось которого горизонтальна и находится в плоскости нашего прямоугольника.

Достаточно двигателя с управлением направлением вращения. Если мал момент инерции, нацепить маховичок :wink:

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

 
 
 
 Re: Балансирование тела на 2 точках опоры
Сообщение01.09.2013, 13:46 
Так и знал, что забуду упомянуть что-то из того, что уже было упомянуто в темах на других форумах :-) Я не хочу собрать электрическую юлу, не хочу применять и использовать вращение маховиков и моменты инерции, не хочу такого, а хочу скорее такое или как в приведенном аппноте что описано. А вообще, я постарался написать подробно, как хочу балансировать - "статикой", просто маятником-противовесом. Если вы хотите предложить "как проще", то идите до конца и предложите третью точку опоры безо всяких электронных штучек :-)

 
 
 
 Re: Балансирование тела на 2 точках опоры
Сообщение01.09.2013, 13:59 
Аватара пользователя
_Ivana в сообщении #759508 писал(а):
А вообще, я постарался написать подробно, как хочу балансировать - "статикой", просто маятником-противовесом.

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

 
 
 
 Re: Балансирование тела на 2 точках опоры
Сообщение01.09.2013, 14:18 
Пытался нарисовать рисунок в паинте мышкой - вышло не очень, выкладывать не решился :-) Но если нужно для понимания - нарисую и выложу. Собственно, одна ось вращения у меня есть и никуда от нее не деться - это ось, проходящая через 2 точки установки тела на горизонтальную опору. А больше вращений можно избежать, если они вас так смущают - балансир выполнить в виде тяжелого железного цилиндра, перемещающегося вдоль оси перпендикулярной плоскости балансируемого прямоугольника, например, внутри длинного соленоида с несколькими переключающимися катушками для управления положением цилиндра (если так понятно описание конструкции).

 
 
 
 Re: Балансирование тела на 2 точках опоры
Сообщение01.09.2013, 15:18 
Аватара пользователя
_Ivana в сообщении #759517 писал(а):
балансир выполнить в виде тяжелого железного цилиндра, перемещающегося вдоль оси перпендикулярной плоскости балансируемого прямоугольника, например, внутри длинного соленоида с несколькими переключающимися катушками для управления положением цилиндра (если так понятно описание конструкции).

Достаточно.
Понадобится бороться с трением, что естественно реализовать как движение по окружности :wink:

 
 
 
 Re: Балансирование тела на 2 точках опоры
Сообщение01.09.2013, 17:37 
Поэтому я и остановился на балансире в виде маятника.
Однако, простите, я не понимаю многих вещей, но в числе всего прочего еще и:
1) что вы пытаетесь мне сказать
2) что вы пытаетесь у меня спросить
3) как это поможет мне собрать мою конструкцию, например, ответит на пронумерованные вопросы из первого поста темы

 
 
 
 Re: Балансирование тела на 2 точках опоры
Сообщение22.02.2014, 10:15 
Интересная задачка. Вот ссылка на теорию двойного маятника: http://www.math24.ru/double-pendulum.html
Хотелось бы уточнить, правильно ли я понимаю: задача состоит в том, чтобы управлять углом между двумя звеньями маятника так, чтобы маятник колебался выше точки опоры?

 
 
 
 Re: Балансирование тела на 2 точках опоры
Сообщение23.02.2014, 12:56 
За ссылку на теорию спасибо, как раз сейчас пишу программу моделирования поведения системы в зависимости от управляющего воздействия, нужны диффуры систем.

А по задаче - у нас двухзвенный маятник, 2 шарнира: $O$ в точке опоры и $A$ в точке соединения звеньев, в т. $A$ расположен двигатель с редуктором, который управляет маятником второго звена так, чтобы угол первого звена относительно вертикали был заданным (то есть, чтобы конструкция устойчиво неподвижно стояла под любым задаваемым углом в диапазоне от +-45 градусов от вертикали). Если наш двигатель - сервопривод, то мы управляем через угол между звеньями, если коллекторный двигатель постоянного тока - то через момент силы в т. $A$ - если я правильно понимаю физику процесса. Сейчас я склоняюсь ко второму варианту, уже подобрал мотор, надо заказать. Плата с датчиками и МК Кортексом уже есть, можно писать программу получения абсолютного угла через Калмана или альфа-бета фильтр.

 
 
 
 Re: Балансирование тела на 2 точках опоры
Сообщение25.02.2014, 01:12 
Вести с полей - навскидку накидал программку анимации двойного маятника в Матлабе, со своим доморощенным кустарным методом решения системы линейных/нелинейных диффуров (придуман на коленке, допускаю, что далеко не оптимальный), код в теге оффтопа, желающие могут поиграться длинами/массами, временем/интервалом дискретизации и начальными условиями (с ними есть одна тонкость).
ЗЫ правда, мне придется вывести свою систему диффуров - у меня центр масс первого звена не в точке соединения звеньев а где-то между ней и точкой подвеса и надо заложить функцию внешнего воздействия в точке соединения звеньев. А потом и к симуляции ПИДа можно переходить.

(Оффтоп)

Код:
function Main()
global l1 m1 l2 m2 g dt y
l1 = 1.5;   m1 = 1;
l2 = 1;     m2 = 1.7;
dt = 0.01;
t_max = 50;
g = 9.8;

function r = system_1_point(v)
    r = [(m1+m2)*l1*v(3) + m2*l2*v(6)*cos(v(1)-v(4)) + m2*l2*v(5)^2*sin(v(1)-v(4)) + (m1+m2)*g*sin(v(1));
        -v(1) + dt*v(2) + y(ii-1, 1);
        -v(2) + dt*v(3) + y(ii-1, 2);
        l2*v(6) + l1*v(3)*cos(v(1)-v(4)) - l1*v(2)^2*sin(v(1)-v(4)) + g*sin(v(4));
        -v(4) + dt*v(5) + y(ii-1, 4);
        -v(5) + dt*v(6) + y(ii-1, 5);
        ];
end
function r = system_2_point(v)
    r = [(m1+m2)*l1*v(3) + m2*l2*v(6)*cos(v(1)-v(4)) + m2*l2*v(5)^2*sin(v(1)-v(4)) + (m1+m2)*g*sin(v(1));
        -1.5*v(1) + dt*v(2) - 0.5*y(ii-2, 1) + 2*y(ii-1, 1);
        -1.5*v(2) + dt*v(3) - 0.5*y(ii-2, 2) + 2*y(ii-1, 2);
        l2*v(6) + l1*v(3)*cos(v(1)-v(4)) - l1*v(2)^2*sin(v(1)-v(4)) + g*sin(v(4));
        -1.5*v(4) + dt*v(5) - 0.5*y(ii-2, 4) + 2*y(ii-1, 4);
        -1.5*v(5) + dt*v(6) - 0.5*y(ii-2, 5) + 2*y(ii-1, 5)];
end

t = 0:dt:t_max;
N = numel(t);
y = zeros(N, 6); % 1,2,3 - угол первого звена с производными, 4,5,6 - второго

% начальные условия - задание примерных
% и потом пересчет их же для удовлетворения системе
y(1, 1) = 2;        y(1, 2) = 0;    y(1, 3) = 1;
y(1, 4) = y(1, 1);  y(1, 5) = 0;    y(1, 6) = 1;
ii = 2;
options = optimoptions('fsolve', 'Display','off');
v = fsolve(@system_1_point, y(ii - 1, :), options);
y(1, :) = v;

hf = figure(1);
clf reset
set(hf,'color', 'black');
axis off, axis equal
xlim([-(l1+l2) (l1+l2)]); ylim([-(l1+l2) (l1+l2)]);
title(['See Dowble Pendulum Run! ( l1=', num2str(l1), ', m1=', num2str(m1),....
    ', l2=', num2str(l2), ', m2=', num2str(m2), ' )'],'Color','white');
hold on
plot(0, 0, 'o', 'LineWidth', 3, 'color', [160/255 160/255 160/255]);
ht = plot(0, 0, ':w');
hl = plot([0, 0, 0], [0, 0, 0], '-', 'LineWidth', 4, 'color', [43/255 84/255 126/255]);
ha = plot(0, 0, 'o', 'LineWidth', 1 + round((m1*50)^(1/2)), 'color', [160/255 160/255 160/255]);
hb = plot(0, 0, 'o', 'LineWidth', 1 + round((m2*50)^(1/2)), 'color', [160/255 160/255 160/255]);
for ii = 2:N
    options = optimoptions('fsolve', 'Display','off');
    if ii == 2
        v = fsolve(@system_1_point, y(ii - 1, :), options);
    else
        v = fsolve(@system_2_point, y(ii - 1, :), options);
    end
    y(ii, :) = v;
   
    xa =  l1.*sin(y(ii, 1));
    ya = -l1.*cos(y(ii, 1));
    xb = xa + l2.*sin(y(ii, 4));
    yb = ya - l2.*cos(y(ii, 4));
    set(hl,'XData',[0, xa, xb]);    set(hl,'YData',[0, ya, yb]);
    set(ha,'XData',xa);             set(ha,'YData',ya);
    set(hb,'XData',xb);             set(hb,'YData',yb);
    set(ht,'XData',  l1.*sin(y(1:ii, 1)) + l2.*sin(y(1:ii, 4)))
    set(ht,'YData', -l1.*cos(y(1:ii, 1)) - l2.*cos(y(1:ii, 4)))
    drawnow
end
end

 
 
 
 Re: Балансирование тела на 2 точках опоры
Сообщение26.02.2014, 00:07 
Еще по теории, но ближе к управлению и реальному устройству.
Из русскоязычных: http://books.google.com.ua/books?id=oCs ... &q&f=false
Из англоязычных:
http://www.clemson.edu/ces/crb/ece496/s ... n_time.pdf
http://www.clemson.edu/ces/crb/ece496/s ... wingup.pdf
http://citeseerx.ist.psu.edu/viewdoc/do ... 1&type=pdf
http://homes.cs.washington.edu/~todorov ... _notes.pdf
По первой из англоязычных статей попробовала набрать код, прорисовку оставила вашу. Управление задала нулевое. Результат какой-то странный. Может я в чем-то ошиблась. Код прилагаю в оффтопе.

(Оффтоп)

Код:
function Main()
global l1 l2 lc1 lc2 m1 m2 I1 I2 g tau
l1 = 1; l2=1;
lc1=0.5; lc2=0.5;
m1 = 1; m2=1;
I1=1; I2=1;
g = 9.8;

tau=0; %управление

dt = 0.01;
t_max = 50;

    function dX=right(t,X)
        q1=X(1); %угол первого звена
        q2=X(2); %угол второго звена
        dq1=X(3); %производная угла первого звена
        dq2=X(4); %производная угла второго звена
       
        d11=m1*lc1^2+m2*(l1^2+lc2^2+2*l1*lc2*cos(q2))+I1+I2;
        d22=m2*lc2^2+I2;
        d12=m2*(lc2^2+l1*lc2*cos(q2))+I2;
        d21=d12;
        c1=-m2*l1*lc2*sin(q2)*dq2^2-2*m2*l1*lc2*sin(q2)*dq2*dq1;
        c2=m2*l1*lc2*sin(q2)*dq1^2;
        f1=(m1*lc1+m2*l1)*g*cos(q1)+m2*lc2*g*cos(q1+q2);
        f2=m2*lc2*g*cos(q1+q2);
       
        den=d11*d22-d12^2;
       
        dX(1)=dq1;
        dX(2)=dq2;
        dX(3)=(-d12*(tau-c2-f2)+d22*(c1+f1))/den;
        dX(4)=( d11*(tau-c2-f2)+d12*(c1+f1))/den;
        dX=dX';
    end

X0=[1 1 0 0]';
t = 0:dt:t_max;
[t,X] = ode45(@right,t,X0);
figure; plot(t,X(:,1),'k.');grid; title('first angle');
figure; plot(t,X(:,2),'k.');grid; title('second angle');
%-------------------------------------------------------------------------
N = numel(t);

hf = figure(1);
clf reset
set(hf,'color', 'black');
axis off, axis equal
xlim([-(l1+l2) (l1+l2)]); ylim([-(l1+l2) (l1+l2)]);
title(['See Dowble Pendulum Run! ( l1=', num2str(l1), ', m1=', num2str(m1),....
    ', l2=', num2str(l2), ', m2=', num2str(m2), ' )'],'Color','white');
hold on
plot(0, 0, 'o', 'LineWidth', 3, 'color', [160/255 160/255 160/255]);
ht = plot(0, 0, ':w');
hl = plot([0, 0, 0], [0, 0, 0], '-', 'LineWidth', 4, 'color', [43/255 84/255 126/255]);
ha = plot(0, 0, 'o', 'LineWidth', 1 + round((m1*50)^(1/2)), 'color', [160/255 160/255 160/255]);
hb = plot(0, 0, 'o', 'LineWidth', 1 + round((m2*50)^(1/2)), 'color', [160/255 160/255 160/255]);
for ii = 1:N 
    xa =  l1.*sin(X(ii, 1));
    ya = -l1.*cos(X(ii, 1));
    xb = xa + l2.*sin(X(ii, 2));
    yb = ya - l2.*cos(X(ii, 2));
    set(hl,'XData',[0, xa, xb]);    set(hl,'YData',[0, ya, yb]);
    set(ha,'XData',xa);             set(ha,'YData',ya);
    set(hb,'XData',xb);             set(hb,'YData',yb);
    set(ht,'XData',  l1.*sin(X(1:ii, 1)) + l2.*sin(X(1:ii, 2)))
    set(ht,'YData', -l1.*cos(X(1:ii, 1)) - l2.*cos(X(1:ii, 2)))
    drawnow
end
end

 
 
 
 Re: Балансирование тела на 2 точках опоры
Сообщение26.02.2014, 00:55 
Еще раз большое спасибо Вам за ссылки на теорию. Я ее обязательно посмотрю и попытаюсь подробно разобраться.
А насчет управления - в моем коде заложен понятный мне алгоритм решения диффура (или системы), я протестировал его на одинарном маятнике - гармоническом и физическом, прямом (вниз) и обратном (вверх), заложил управляющее воздействие и с помощью ПИД регулирования этим воздействием осуществляю управление. Если я пойму (или прочитаю в ваших ссылках) как заложить управляющее воздействие на двухзвенный маятник моментом силы в точке соединения звеньев - то есть, модифицировать систему диффуров свободных колебаний с учетом этого воздействия, то думаю, также не должно быть сложностей добавить ПИД по углу первого звена в мой приведенный код.

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

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

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

 
 
 
 Re: Балансирование тела на 2 точках опоры
Сообщение26.02.2014, 09:05 
Нашла одну ошибочку в статейке (в знаке) :) плюс там надо было учесть, что первый угол отсчитывается от горизонтальной оси, а второй - относительно первого. Вобщем, исправленный вариант в оффтопе. Выглядит вполне прилично:)
Насчет управления - в данном случае оно подразумевает вращающий момент (torque), как я поняла.

(Оффтоп)

Код:
function Main()
clear all; close all;
global l1 l2 lc1 lc2 m1 m2 I1 I2 g tau
l1 = 1; l2=1;
lc1=0.5; lc2=0.5;
m1 = 1; m2=1;
I1=1; I2=1;
g = 9.8;

tau=0; %управление - вращающий момент

dt = 0.01;
t_max = 50;

    function dX=right(t,X)
        q1=X(1); %угол первого звена относительно горизонтальной оси
        q2=X(2); %угол второго звена относительно первого
        dq1=X(3); %производная угла первого звена
        dq2=X(4); %производная угла второго звена относительно первого
       
        d11=m1*lc1^2+m2*(l1^2+lc2^2+2*l1*lc2*cos(q2))+I1+I2;
        d22=m2*lc2^2+I2;
        d12=m2*(lc2^2+l1*lc2*cos(q2))+I2;
        d21=d12;
        c1=-m2*l1*lc2*sin(q2)*dq2^2-2*m2*l1*lc2*sin(q2)*dq2*dq1;
        c2=m2*l1*lc2*sin(q2)*dq1^2;
        f1=(m1*lc1+m2*l1)*g*cos(q1)+m2*lc2*g*cos(q1+q2);
        f2=m2*lc2*g*cos(q1+q2);
       
        den=d11*d22-d12^2;
       
        dX(1)=dq1;
        dX(2)=dq2;
        dX(3)=(-d12*(tau-c2-f2)-d22*(c1+f1))/den;%!!!!
        dX(4)=( d11*(tau-c2-f2)+d12*(c1+f1))/den;
        dX=dX';
    end

X0=[1 0 0 0]'; %Начальные углы
t = 0:dt:t_max;
[t,X] = ode45(@right,t,X0);
figure;
figure; plot(t,X(:,1),'k.');grid; title('first angle');
figure; plot(t,X(:,2),'k.');grid; title('second angle');
%-------------------------------------------------------------------------
N = numel(t);

hf = figure(1);
clf reset
set(hf,'color', 'black');
axis off, axis equal
xlim([-(l1+l2) (l1+l2)]); ylim([-(l1+l2) (l1+l2)]);
title(['See Dowble Pendulum Run! ( l1=', num2str(l1), ', m1=', num2str(m1),....
    ', l2=', num2str(l2), ', m2=', num2str(m2), ' )'],'Color','white');
hold on
plot(0, 0, 'o', 'LineWidth', 3, 'color', [160/255 160/255 160/255]);
ht = plot(0, 0, ':w');
hl = plot([0, 0, 0], [0, 0, 0], '-', 'LineWidth', 4, 'color', [43/255 84/255 126/255]);
ha = plot(0, 0, 'o', 'LineWidth', 1 + round((m1*50)^(1/2)), 'color', [160/255 160/255 160/255]);
hb = plot(0, 0, 'o', 'LineWidth', 1 + round((m2*50)^(1/2)), 'color', [160/255 160/255 160/255]);
for ii = 1:N
    xa =  l1.*cos(X(ii, 1));
    ya =  l1.*sin(X(ii, 1));
    xb = xa + l2.*cos(X(ii, 1)+X(ii, 2));
    yb = ya + l2.*sin(X(ii, 1)+X(ii, 2));
    set(hl,'XData',[0, xa, xb]);    set(hl,'YData',[0, ya, yb]);
    set(ha,'XData',xa);             set(ha,'YData',ya);
    set(hb,'XData',xb);             set(hb,'YData',yb);
    set(ht,'XData',  l1.*cos(X(1:ii, 1)) + l2.*cos(X(1:ii, 1)+X(1:ii, 2)))
    set(ht,'YData',  l1.*sin(X(1:ii, 1)) + l2.*sin(X(1:ii, 1)+X(1:ii, 2)))
    drawnow
end
end

 
 
 
 Re: Балансирование тела на 2 точках опоры
Сообщение26.02.2014, 22:39 
Вот навскидку ПД управление, далеко не оптимальное, но как-то работает, хотя на точную уставку не выходит, но стоит, не падает. Дальше можно подбирать коэффициенты и хорошо бы добавить интегральные компоненты. Кстати, у меня есть модель именно такой системы в симулинке, я добился что она выходит на уставку, но мне же в железе реализовывать в конце концов. А без хорошего знания теории это будет блуждание вслепую, и в лучшем случае будет работать кое-как, вместо изящного балансирования на колеблющемся основании. Буду постигать ваши ссылки.

(Оффтоп)

Код:
function Main()
clear all; close all;
global l1 l2 lc1 lc2 m1 m2 I1 I2 g tau al_ust be_ust
l1 = 1; l2=1;
lc1=0.5; lc2=0.8;
m1 = 1; m2=2;
I1=1; I2=1;
g = 9.8;

tau = 0; %управление - вращающий момент

al_ust = 5*pi/180; % угол уставки первого звена от вертикали
%угол второго звена относительно первого в положении равновесия
be_ust = pi - (asin((m2*l1 + m1*lc1)/(m2*lc2)*sin(al_ust)) - al_ust);
al_ust = pi/2 - al_ust;

dt = 0.01;
t_max = 50;

    function dX=right(t,X)
        q1=X(1); %угол первого звена относительно горизонтальной оси
        q2=X(2); %угол второго звена относительно первого
        dq1=X(3); %производная угла первого звена
        dq2=X(4); %производная угла второго звена относительно первого
       
        kp1 = 60;
        kd1 = 50;
        kp2 = -30;
        kd2 = -30;
        tau = kp1*(q1 - al_ust) + kd1*dq1 + kp2*(q2 - be_ust) + kd2*dq2;
       
        d11=m1*lc1^2+m2*(l1^2+lc2^2+2*l1*lc2*cos(q2))+I1+I2;
        d22=m2*lc2^2+I2;
        d12=m2*(lc2^2+l1*lc2*cos(q2))+I2;
        d21=d12;
        c1=-m2*l1*lc2*sin(q2)*dq2^2-2*m2*l1*lc2*sin(q2)*dq2*dq1;
        c2=m2*l1*lc2*sin(q2)*dq1^2;
        f1=(m1*lc1+m2*l1)*g*cos(q1)+m2*lc2*g*cos(q1+q2);
        f2=m2*lc2*g*cos(q1+q2);
       
        den=d11*d22-d12^2;
       
        dX(1)=dq1;
        dX(2)=dq2;
        dX(3)=(-d12*(tau-c2-f2)-d22*(c1+f1))/den;%!!!!
        dX(4)=( d11*(tau-c2-f2)+d12*(c1+f1))/den;
        dX=dX';
    end

X0=[1.5 pi+0.1 0 0]'; %Начальные углы
t = 0:dt:t_max;
[t,X] = ode45(@right,t,X0);
figure(1); plot(t, (180/pi).*(pi/2 - X(:,1)), 'b-', t, (180/pi).*(pi/2 - al_ust.*ones(numel(t), 1)), 'g-');
grid; title('first angle');
% figure; plot(t,X(:,2),'b-');grid; title('second angle');
%return
%-------------------------------------------------------------------------
N = numel(t);

hf = figure(2);
clf reset
set(hf,'color', 'black');
axis off, axis equal
%xlim([-(l1+l2) (l1+l2)]); ylim([-(l1+l2) (l1+l2)]);
title(['See Dowble Pendulum Run! ( lc1=', num2str(lc1), ', m1=', num2str(m1),....
    ', lc2=', num2str(lc2), ', m2=', num2str(m2), ' )'],'Color','white');
hold on
plot(0, 0, 'o', 'LineWidth', 3, 'color', [160/255 160/255 160/255]);
ht = plot(0, 0, ':w');
hl = plot([0, 0, 0], [0, 0, 0], '-', 'LineWidth', 4, 'color', [43/255 84/255 126/255]);
hc1 = plot(0, 0, 'o', 'LineWidth', 1 + round((m1*50)^(1/2)), 'color', [160/255 160/255 160/255]);
hc2 = plot(0, 0, 'o', 'LineWidth', 1 + round((m2*50)^(1/2)), 'color', [160/255 160/255 160/255]);
ha  = plot(0, 0, 'o', 'LineWidth', 3,                        'color', [160/255 160/255 160/255]);
for ii = 1:N
    xa =  l1.*cos(X(ii, 1));
    ya =  l1.*sin(X(ii, 1));
    xb = xa + l2.*cos(X(ii, 1)+X(ii, 2));
    yb = ya + l2.*sin(X(ii, 1)+X(ii, 2));
    xc1 = xa*lc1/l1;
    yc1 = ya*lc1/l1;
    xc2 = xa + lc2.*cos(X(ii, 1)+X(ii, 2));
    yc2 = ya + lc2.*sin(X(ii, 1)+X(ii, 2));
    set(hl,'XData',[0, xa, xb]);    set(hl,'YData',[0, ya, yb]);
    set(ha,'XData',xa);             set(ha,'YData',ya);
    set(hc1,'XData',xc1);           set(hc1,'YData',yc1);
    set(hc2,'XData',xc2);           set(hc2,'YData',yc2);
    set(ht,'XData',  l1.*cos(X(1:ii, 1)) + l2.*cos(X(1:ii, 1)+X(1:ii, 2)))
    set(ht,'YData',  l1.*sin(X(1:ii, 1)) + l2.*sin(X(1:ii, 1)+X(1:ii, 2)))
    drawnow
end
end

 
 
 
 Re: Балансирование тела на 2 точках опоры
Сообщение09.03.2014, 22:30 
Вести с полей - почитал Формальского, переделал диффур для моего случая, линеаризовал его в окрестности нужного мне неустойчивого равновесия, вычислил собственные значения, перешел к нормальным координатам, потом от них к жордановой форме, выделил одну неустойчивую моду (собственное значение больше нуля), построил управление, подавляющее эту неустойчивую моду. Насколько я понимаю (как пишет Формальский) такое управление максимизирует область притяжения при ограниченных ресурсах управления. Еще одно собственное значение меньше нуля, еще два - чисто мнимые, их пока не подавляю - результат виден в анимации в виде колебаний. Наверное, надо отщепить процентов десять от ресурсов управления на подавление двух устойчивых мод, чтобы колебания затухали - чуть пожертвовать областью притяжения ради уменьшения колебательного режима.

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

(Оффтоп)

Код:
function Main()
clear all; close all;

    function r = system_1_point(v)       
        A = [a11, -a12*cos(v(4)-v(1)); -a12*cos(v(4)-v(1)), a22];
        F = [0, a12*sin(v(4)-v(1)); -a12*sin(v(4)-v(1)), 0];
        B = [-b1, 0; 0, b2];
        C = [-1; 1];
        r = A*[v(3); v(6)] + F*[v(2)^2; v(5)^2] + B*[sin(v(1)); sin(v(4))] - C*L;       
        r = [r;
            -v(1) + dt*v(2) + y(ii-1, 1);
            -v(2) + dt*v(3) + y(ii-1, 2);
            -v(4) + dt*v(5) + y(ii-1, 4);
            -v(5) + dt*v(6) + y(ii-1, 5)];
    end
    function r = system_2_point(v)
        A = [a11, -a12*cos(v(4)-v(1)); -a12*cos(v(4)-v(1)), a22];
        F = [0, a12*sin(v(4)-v(1)); -a12*sin(v(4)-v(1)), 0];
        B = [-b1, 0; 0, b2];
        C = [-1; 1];
        r = A*[v(3); v(6)] + F*[v(2)^2; v(5)^2] + B*[sin(v(1)); sin(v(4))] - C*L;       
        r = [r;
            -1.5*v(1) + dt*v(2) - 0.5*y(ii-2, 1) + 2*y(ii-1, 1);
            -1.5*v(2) + dt*v(3) - 0.5*y(ii-2, 2) + 2*y(ii-1, 2);
            -1.5*v(4) + dt*v(5) - 0.5*y(ii-2, 4) + 2*y(ii-1, 4);
            -1.5*v(5) + dt*v(6) - 0.5*y(ii-2, 5) + 2*y(ii-1, 5)];
    end
    function r = system_initial_conditions(v)       
        A = [a11, -a12*cos(v(4)-v(1)); -a12*cos(v(4)-v(1)), a22];
        F = [0, a12*sin(v(4)-v(1)); -a12*sin(v(4)-v(1)), 0];
        B = [-b1, 0; 0, b2];
        C = [-1; 1];
        r = A*[v(3); v(6)] + F*[v(2)^2; v(5)^2] + B*[sin(v(1)); sin(v(4))] - C*L;
        r = [r;
            v(1) - y(1, 1);
            v(4) - y(1, 4);
            v(2);
            v(5)];
    end

m1 = 1; r1 = 0.5; l = 1;
m2 = 2; r2 = 0.9;
t_max = 10;
dt = 5e-3;
% начальные условия - градусы отклонения звеньев от вертикали
fi1_0 = 5; fi2_0 = 3;

L = 0;
I1 = m1*r1^2;       I2 = m2*r2^2;   g = 9.8;
a11 = I1 + m2*l^2;   a22 = I2;       a12 = m2*r2*l;
b1 = (m1*r1 + m2*l)*g;              b2 = m2*r2*g;
t = 0:dt:t_max;
N = numel(t);
y = zeros(N, 6);    % 1,2,3 - угол первого звена с производными
                    % 4,5,6 - угол второго звена с производными
% начальные условия
y(1, 1) = fi1_0/180*pi;   y(1, 2) = 0;    y(1, 3) = 0;
y(1, 4) = fi2_0/180*pi;   y(1, 5) = 0;    y(1, 6) = 0;
solver_options = optimoptions('fsolve', 'Display','off');
v = fsolve(@system_initial_conditions, y(1, :), solver_options);
y(1, :) = v;

hf = figure(1);
clf reset
set(hf,'color', 'black');
axis off, grid off, axis equal
xlim([-(l+r2) (l+r2)]); ylim([-(l+r2) (l+r2)]);
xlim([-l/2 l/2]); ylim([0 l]);
title(['See Double Pendulum Run! ( l=', num2str(l), ', m1=', num2str(m1),....
    ', r2=', num2str(r2), ', m2=', num2str(m2), ' )'],'Color','white');
hold on
plot(0, 0, 'o', 'LineWidth', 3, 'color', [160/255 160/255 160/255]);
ht = plot(0, 0, ':w');
hl = plot([0, 0, 0], [0, 0, 0], '-', 'LineWidth', 4, 'color', [43/255 84/255 126/255]);
hm1 = plot(0, 0, 'o', 'LineWidth', 1 + round((m1*50)^(1/2)), 'color', [160/255 160/255 160/255]);
hm2 = plot(0, 0, 'o', 'LineWidth', 1 + round((m2*50)^(1/2)), 'color', [160/255 160/255 160/255]);
kfi = -1;

aeq = a12^2 - a11*a22;
beq = a22*b1 - a11*b2;
ceq = b1*b2;
Deq = beq^2 - 4*aeq*ceq;
la1 = (-beq + Deq^0.5)/(2*aeq);
la2 = (-beq - Deq^0.5)/(2*aeq);
% A0 = [a11, -a12; -a12, a22];
K_inv = inv([(b2 + a22*la1)/(a12*la1), (b2 + a22*la2)/(a12*la2); 1, 1]);
% d = K_inv*inv(A0)*[-1; 1];
mu2 = la2^0.5;

kg = 300;
fm = zeros(N, 1);
for ii = 2:N
     fi = [y(ii - 1, 1); y(ii - 1, 4)];
    dfi = [y(ii - 1, 2); y(ii - 1, 5)];
     x = K_inv*fi;
    dx = K_inv*dfi;
    L = kg*(x(2) + dx(2)/mu2);
    fm(ii) = L;
   
    if ii == 2
        v = fsolve(@system_1_point, y(ii - 1, :), solver_options);
    else
        v = fsolve(@system_2_point, y(ii - 1, :), solver_options);
    end
    y(ii, :) = v;
   
    xa = -l*sin(y(ii, 1));
    ya =  l*cos(y(ii, 1));
    xb = xa - kfi*r2*sin(y(ii, 4));
    yb = ya + kfi*r2*cos(y(ii, 4));
    set(hl,'XData',[0, xa, xb]);            set(hl,'YData',[0, ya, yb]);
    set(hm1,'XData', -r1*sin(y(ii, 1)));    set(hm1,'YData', r1*cos(y(ii, 1)));
    set(hm2,'XData',xb);                    set(hm2,'YData',yb);
    set(ht,'XData', -l.*sin(y(1:ii, 1)) - kfi*r2.*sin(y(1:ii, 4)))
    set(ht,'YData',  l.*cos(y(1:ii, 1)) + kfi*r2.*cos(y(1:ii, 4)))
    drawnow
end

figure(2);
plot(t, (180/pi).*y(:, 1), '-b', t, (180/pi).*y(:, 4), '-g', t, fm(), '-r')
legend('первое звено', 'второе звено', 'управление')
axis on, grid on
end

 
 
 [ Сообщений: 16 ]  На страницу 1, 2  След.


Powered by phpBB © 2000, 2002, 2005, 2007 phpBB Group