Using saturated inputs in ode45

2 次查看（过去 30 天）
C
C 2011-8-22
编辑： yosra welhazi 2015-4-9
Hi everyone, I'm kind of new to matlab and I'm having trouble saturating a control input by simulating a nonlinear system in ode45. My code basically takes an arbitrary set of initial conditions of a ground vehicle and uses predictive control theory to track a reference trajectory. Anyways, there are constraints on the control inputs which I impose using an "if" statement. When I go to plot my control inputs I see that my inputs aren't saturating. Can someone help me out on this? Here is my code. Thanks.
function tracking
global Izz m C1 C2 Rc wr vr h
Izz = 271091;
m = 52540;
C1 = 144;
C2 = 13550;
Rc = 300;
wr = 0.04;
vr = 12;
h = 6;
t = [0:0.1:170];
x0 = [0 0 -pi/2 0 0];
[t, x] = ode45(@track, [0 170], x0);
close all;
figure(1)
plot(x(:,1),x(:,2))
axis('square')
title('Tracked vehicle trajectory in xy-plane')
xlabel('x(m)')
ylabel('y(m)')
hold on;
xr = -Rc*sin(wr*t);
yr = Rc*cos(wr*t);
plot(xr,yr,'--black')
legend('reference','actual')
fr = -C1*x(:,4).*abs(x(:,4)); % ground resistant force
tr = -C2*x(5); % ground resistant torque
fv = fr/m;
gv = 1/m;
fw = tr/Izz;
gw = 1/Izz;
xr = -Rc*sin(wr*t); % reference trajectory - circle
yr = Rc*cos(wr*t);
thetar = pi/2 + wr*t;
xrdot = -Rc*wr*cos(wr*t);
yrdot = -Rc*wr*sin(wr*t);
thetardot = wr;
wrdot = 0;
vrdot = 0;
xrddot = Rc*(wr)^2*sin(wr*t);
yrddot = -Rc*(wr)^2*cos(wr*t);
thetarddot = 0;
u1 = -((4)/(5*gv))*(((x(:,2) - yr).*cos(x(:,3))-(x(:,1) - xr).*sin(x(:,3)))/(2*h^2)...
+((x(:,4).*cos(x(:,3)) - yrdot).*cos(x(:,3))-(-x(:,4).*sin(x(:,3)) - xrdot).*sin(x(:,3)) +...
2*(x(:,4) - vr))/(2*h) + 5*fv/4 - vrdot - (yrddot.*cos(x(:,3)) -...
xrddot.*sin(x(:,3)))/4);
u2 = -((4)/(5*gw))*((x(:,3)-thetar)/(2*h^2) + 3*(x(:,5)-wr)/(2*h) -...
5*(-fw + wrdot)/4);
figure(2)
plot(t,u1)
title('Control history of u1(t)')
xlabel('Time(sec)')
ylabel('Force(N)')
figure(3)
plot(t,u2)
title('Control history of u2(t)')
xlabel('Time(sec)')
ylabel('Torque(N-m)')
function xprime = track(t, x)
global Izz m C1 C2 Rc wr vr h
Limit1 = 56400;
Limit2 = 2710;
fr = -C1*x(4)*abs(x(4)); % ground resistant force
tr = -C2*x(5); % ground resistant torque
fv = fr/m;
gv = 1/m;
fw = tr/Izz;
gw = 1/Izz;
xr = -Rc*sin(wr*t); % reference trajectory - circle
yr = Rc*cos(wr*t);
thetar = pi/2 + wr*t;
xrdot = -Rc*wr*cos(wr*t);
yrdot = -Rc*wr*sin(wr*t);
thetardot = wr;
wrdot = 0;
vrdot = 0;
xrddot = Rc*(wr)^2*sin(wr*t);
yrddot = -Rc*(wr)^2*cos(wr*t);
thetarddot = 0;
u1 = -((4)/(5*gv))*(((x(2) - yr)*cos(x(3))-(x(1) - xr)*sin(x(3)))/(2*h^2)...
+((x(4)*cos(x(3)) - yrdot)*cos(x(3))-(-x(4)*sin(x(3)) - xrdot)*sin(x(3)) +...
2*(x(4) - vr))/(2*h) + 5*fv/4 - vrdot - (yrddot*cos(x(3)) -...
xrddot*sin(x(3)))/4);
if( abs(u1) > Limit1 ) %implementing saturation on control input 1
u1 = sign(u1)*Limit1;
end
u2 = -((4)/(5*gw))*((x(3)-thetar)/(2*h^2) + 3*(x(5)-wr)/(2*h) -...
5*(-fw + wrdot)/4);
if( abs(u2) > 2000 ) % implementing saturation on control input 2
u2 = sign(u2)*2000;
end
xprime = zeros(5,1);
xprime(1) = -x(4)*sin(x(3)); % equations of motion
xprime(2) = x(4)*cos(x(3));
xprime(3) = x(5);
xprime(4) = fv + gv*u1;
xprime(5) = fw + gw*u2;
  2 个评论
shravankumar
shravankumar 2011-8-22
what do you mean by saturating a control input..
yosra welhazi
yosra welhazi 2015-4-9
编辑：yosra welhazi 2015-4-9
Dear friend;
I have the same problem as you and I have cheked the limits inside function track, but when I simulate and plot my control inputs, they are not saturated , what can I do please ??? please help me

请先登录，再进行评论。

请先登录，再回答此问题。

回答（1 个）

Fangjun Jiang
Fangjun Jiang 2011-8-22
The check for limit is inside the function track(). The u1 and u2 you plotted are inside the function tracking(). They have nothing to do with each other except having the same variable name.
Are you supposed to do the check inside function tracking()?
  2 个评论
C
C 2011-8-22
Well I thought if I checked for the limits inside function track(), when ode45 solves for the equations of motion, i.e, x(:,1),x(:,2)...,x(:,5), these solutions would correspond to a saturated control input. Therefore, since u1 and u2 are functions of
x(:,1),x(:,2)...,x(:,5) I thought that would give me what I expected when I plotted u1 and u2 in function tracking().
Fangjun Jiang
Fangjun Jiang 2011-8-22
I don't know about that. If you don't have that check for limit in track(), will you be able to re-construct the same u1 and u2 in tracking()?
The usual way would be calculating u1 and u2 in tracking() and then passing them to track(), right? track() is defining an ODE function. In state space notation, xprime=Ax+Bu, the input u1 and u2 should be passed in.

请先登录，再进行评论。

类别

在 Help Center 和 File Exchange 中查找有关 State Estimation 的更多信息

标签

Community Treasure Hunt

Find the treasures in MATLAB Central and discover how the community can help you!

Start Hunting!

Translated by