Mathematical modeling
This part strongly recommends to read a Zhihu article: zinghd's answer
The final model result:
For the definition of some of the symbols, please refer to the Zhihu link given above. The definition of the angle and the appearance of the aircraft are given below.
For the quad-rotor aircraft shown in the figure above, the x-axis is the vector M3M1, the y-axis is M4M2, and the z-axis points upward.
Observing the mathematical model of the quad-rotor aircraft given before, we find that the first three formulas are the calculation of the position of the aircraft, and three Euler angles are used, and the last three formulas are the calculation of the three Euler angles, and the position of the aircraft. It is irrelevant, so the quadrotor is a coupled system, with position and angle coupled.
Start simulation
In fact, the simulation of the quadrotor is to build the above six differential equations in Simulink. In fact, building the differential equation is a very intuitive and simple thing, and the main thing is to make good use of the integrator.
Euler angle model building
Overall appearance:
fcn mainly takes 2 π 2\pi for the three Euler anglesThe remainder of 2 π is convenient for the realization of the subsequent control algorithm (because there is a comparison with PD control later, it is necessary to specify the angle at2 π 2\pi2 π ) After
clicking into the module, it will look like the following:
click into the first red module (calculateϕ \phiϕ ): The
following two Euler angles can be constructed in the same way (according to the fifth and sixth formulas). Each corner here is composed of two integrators (the system is a second-order differential equation)
Location model building
Overall appearance:
Take x as an example, build according to the first formula, click into the sub-module:
if x is successfully built, y and z will be easy to build. Building xyz is a bit simpler than angle, but the calculation of xyz requires three ohms. Angled.
Control algorithm construction
It is strongly recommended to read a Zhihu article: Cascade PD control quadrotor.
The control block diagram is shown in the figure below:
The calculation formula for each step of the Zhihu article has been written in great detail, and the next article will be based on the article. The formula is used to build the controller.
Desired point
Here psai (yaw angle) needs to be given first, so the entire controller actually needs to be given 4 reference values first.
The outer loop adopts PD control: the
input here is xd, yd, zd, and the output is Ux, Uy, Uz. They will be used in the next calculation. It is recommended to look at the formula of the article above. See it clearly.
Next is a function calculation module:
function [U1,phid,thetad] = fcn(Ux,Uy,Uz,psaid,m)
U1 = m*sqrt(Ux^2+Uy^2+(Uz+9.81)^2);
phid = asin((Ux*sin(psaid)-Uy*cos(psaid))*m/U1);
thetad = asin((Ux*m-U1*sin(psaid)*sin(phid))/(U1*cos(psaid)*cos(phid)));
end
Please also move to know the article about the formula here.
The inner ring is angle control, and PD control is still used:
now we have got U1, U2, U3, U4, U1 of these four quantities corresponds to the sum of the four propeller forces, and the last three quantities have a varying effect on the Euler angle The force of, which is derived from the CT (......) C_T(......) in formulas 4, 5, and 6CT( . . . . . . ) , So that the amount of these four solutions can be an anti-rotation angular velocity of the four propellerswi w_iwiUp.
function [w1,w2,w3,w4] = fcn(U1,U2,U3,U4)
CT = 1.116e-5; dCT = 0.225*CT;
matrixA = [CT,CT,CT,CT;0,CT,0,-CT;-CT,0,CT,0;-dCT,dCT,-dCT,dCT];
w_2 = inv(matrixA)*[U1,U2,U3,U4]';
w1 = sqrt(abs(w_2(1))); w2 = sqrt(abs(w_2(2)));
w3 = sqrt(abs(w_2(3))); w4 = sqrt(abs(w_2(4)));
end
Got wi w_iwiInput it into the previously built position-angle model to form a closed-loop control.
Script to draw 3D tracking graph
Drawing of quadrotor
Here, two lines are used to form the aircraft. One of the difficulties is how to draw the posture of the aircraft after Euler angle transformation. It is actually very simple. It is to multiply the body coordinates by the rotation matrix to get the earth coordinates, and then add each component of the earth coordinates. The actual position x, y, z of the upper aircraft can draw the appearance of the aircraft under any Euler angle and position in three-dimensional space.
The function of drawing the quadrotor:
function [point1_trans,point2_trans,point3_trans,point4_trans] = drone(phi,theta,psai)
Cbn = [cos(psai)*cos(theta),cos(psai)*sin(theta)*sin(phi)-sin(psai)*cos(phi),...
sin(theta)*cos(phi)*cos(psai)+sin(phi)*sin(psai);...
sin(psai)*cos(theta),cos(phi)*cos(psai)+sin(phi)*sin(theta)*sin(psai),...
sin(theta)*cos(phi)*sin(psai)-sin(phi)*cos(psai);...
-sin(theta),cos(theta)*sin(phi),cos(theta)*cos(phi)
];
x0 = 0; y0 = 0; z0 = 0;
point1 = [x0-1,y0,z0]; point2 = [x0+1,y0,z0];
point3 = [x0,y0-1,z0]; point4 = [x0,y0+1,z0];
point1_trans = Cbn*point1';point2_trans = Cbn*point2';
point3_trans = Cbn*point3';point4_trans = Cbn*point4';
end
Cbn is the rotation matrix.
3D trajectory simulation
This part is relatively simple. After the position information and angle information Simulink is transmitted to the workspace, these data can be used to draw. In addition, the simulation step of Simulink is set to a fixed step and a smaller setting, so that the actual elapse of time can be simulated.
%仿真轨迹与绘制
clf
len = length(tout);
xmax = 0; ymax = 0; zmax = 0;
xmin = 0; ymin = 0; zmin = 0;
for i = 1:len
figure(1);
if(x(i)>=xmax)
xmax = x(i);
end
if(y(i)>=ymax)
ymax = y(i);
end
if(z(i)>=zmax)
zmax = z(i);
end
if(x(i)<=xmin)
xmin = x(i);
end
if(y(i)<=ymin)
ymin = y(i);
end
if(z(i)<=zmin)
zmin = z(i);
end
limitmin = min(xmin,ymin); limitmax = max(xmax,ymax);
xlim([limitmin-2,limitmax+2]),ylim([limitmin-2,limitmax+2]),zlim([-1,zmax+5])
grid on;
[point1_trans,point2_trans,point3_trans,point4_trans]=drone(phi(i),theta(i),psai(i));%绘制四旋翼
try
delete(h1);delete(h2);delete(point);
plot3([x(i-1),x(i)],[y(i-1),y(i)],[z(i-1),z(i)],"LineWidth",2)
catch
end
h1 = plot3([x(i)+point1_trans(1),x(i)+point2_trans(1)],[y(i)+point1_trans(2),y(i)+point2_trans(2)],...
[z(i)+point1_trans(3),z(i)+point2_trans(3)],"LineWidth",3,"Color","r");
hold on;
h2 = plot3([x(i)+point3_trans(1),x(i)+point4_trans(1)],[y(i)+point3_trans(2),y(i)+point4_trans(2)],...
[z(i)+point3_trans(3),z(i)+point4_trans(3)],"LineWidth",3,"Color","b");
point = scatter3(xd(i),yd(i),zd(i),100,"filled","g");
set(gca,'ztick',0:20:z(i)+5)
% pause(0.1)
end
Trace curve drawing
%绘制位置图像
figure(2);clf;
subplot(1,3,1)
h1 = plot(tout,xd,"r","LineWidth",2);hold on;
h2 = plot(tout,x,"g","LineWidth",2);
legend([h1,h2],["参考位置xd","实际位置x"])
grid on;
subplot(1,3,2)
h3 = plot(tout,yd,"b","LineWidth",2);hold on;
h4 = plot(tout,y,"m","LineWidth",2);
legend([h3,h4],["参考位置yd","实际位置y"])
grid on;
subplot(1,3,3)
h5 = plot(tout,zd,"y","LineWidth",2);hold on;
h6 = plot(tout,z,"k","LineWidth",2);
legend([h5,h6],["参考位置zd","实际位置z"])
grid on;
%绘制角度图像
figure(3);clf;
h7 = plot(tout,phi,"LineWidth",2); hold on;
h8 = plot(tout,psai,"LineWidth",2);
h9 = plot(tout,theta,"LineWidth",2);hold off;
legend([h7,h8,h9],["phi","psai","theta"]),grid on;
Simulation results
The quadcopter tracks the green marked target point:
Attitude angle curve, here is set psai = 0.2 rad, we
can see that the entire control system is stable, trajectory tracking can also be achieved, and the PD controller is successfully built.
Model download
Simulink model: https://download.csdn.net/download/weixin_43145941/13712261
Tutorial writing
I have written the detailed tutorial of this article and uploaded the pdf to the following website. Students in need can do it step by step according to the content of the article to build their own control system:
Simulink simulation of quadrotor aircraft and PD cascade control trajectory tracking simulation pdf Tutorial download address
