Problem solving Differential equation with ODE45: instable behavior
显示 更早的评论
For a Coursera assignment I have to simulate a 2D quadrocopter, i.e. along the z-y-axis using the ODE45 Matlab function. To be correctly I am only supposed to program the controller.
I thought this to be easy after the first assignment. But somehow I get either no behavior or instable behaviour.
These are the control equations which I am supposed to solve numerically and where I have to tune the k-Parameters. They have been provided with the assignment:
I rearranged the formular for highest derivative:
I then created a state vector for the ODE45 as follows: from x1..x12 with x1=y, x2=z, x3=phi, x4=e_y, x5=e_z, x6=e_phi and x7..x12 are the corresponding time derivatives.
This I put into the differential equation function that is called by the ODE45:
function derror = control(time, error)
A=[0 0 0 0 0 0 1 0 0 0 0 0; ...
0 0 0 0 0 0 0 1 0 0 0 0; ...
0 0 0 0 0 0 0 0 1 0 0 0; ...
0 0 0 0 0 0 0 0 0 1 0 0; ...
0 0 0 0 0 0 0 0 0 0 1 0; ...
0 0 0 0 0 0 0 0 0 0 0 1; ...
0 0 -g -K(2) 0 0 0 0 0 -K(1) 0 0; ...
0 0 0 0 -K(4) 0 0 0 0 0 -K(3) 0; ...
0 0 0 0 0 -K(6) 0 0 0 0 0 -K(5); ...
0 0 0 0 0 0 0 0 0 0 0 0; ...
0 0 0 0 0 0 0 0 0 0 0 0; ...
0 0 0 0 0 0 0 0 0 0 0 0];
b=[0 0 0 0 0 0 0 ((u1/m)-g) u2/Ixx 0 0 0]';
derror=A*error+b;
end
And here is the initialisation:
u1 = 0.0;
u2 = 0.0;
tint=0.01;
g=params.gravity;
m=params.mass;
Ixx=params.Ixx;
tinit=0;
K=[0 1000 0 1000 0 1000]'; %[kvy kpy kvz kvz kvphi kpphi]
% FILL IN YOUR CODE HERE
tlim=[tinit, tinit+tint];
iniCond=[state.pos(1), state.pos(2), state.rot, ...
(des_state.pos(1)-state.pos(1)), (des_state.pos(2)-state.pos(2)), -state.rot, ...
state.vel(1), state.vel(2), state.omega, ...
(des_state.vel(1)-state.vel(1)),(des_state.vel(2)-state.vel(2)), -state.omega]; %
[tSol, eSol]=ode45(@control, tlim, iniCond);
v=eSol(end,:)
eSolm=mean(eSol)
u1 = m*(g+des_state.acc(2)+K(3)*eSol(end,11)+K(4)*eSol(end,5))
u2 = Ixx*((eSol(end,9)-state.omega)/tint+K(6)*eSol(end,6) + K(5)*eSol(end,12))
PhiC=-1/g*(des_state.acc(1)+K(1)*eSol(end,10)+K(2)*eSol(end,4));
If I do it this way with u1, u2 = 0 I get nothing. Especially u2 stays zero - see (with the setting as above):
If I set u1, u2 to anything >0 I get instable behavior with the model dropping down.
I am so sure I did it right but it obiously isn't. Can anyone point me the right direction?
Thanks.
回答（0 个）
类别
在 帮助中心 和 File Exchange 中查找有关 Programming 的更多信息
产品
2017-11-8
Community Treasure Hunt
Find the treasures in MATLAB Central and discover how the community can help you!
Start Hunting!
Translated by 
