四旋翼窄缝路径规划

路径规划"/>

四旋翼窄缝路径规划

本篇讲的是四旋翼飞到窄缝附近时，如何利用类平抛运动斜着穿过窄缝。
并给出了Matlab代码。

Traverse Trajectory

From the above optimal theory we can obtain the trajectory of quadcopter if we know the initial state and final state of the quadcopter. So now we can make the quadcopter to any location at any posture. It’s the basis for us to make the quadcopter pass through the narrow gap. Now let’s consider how to pass through the narrow gap.

Look at these two pictures and note plane II is perpendicular to the gap and pass through the center of the gap. So our task is simple: make the quadcopter always fly in the plane II and the center of quadcopter overlap the center of gap which is p_G in the picture when pass through the gap. It can reduce the possibility of collision as much as possible.

If we want the quadcopter always fly in the plane it means the force normal to the plane should be zero when passing. So the thrust produced by four rotors must be same with the gravity’s component normal to this plane. But don’t forget the gravity’s component in this plane which is shown as g II in two pictures. As a result the motion of quadcopter in this plane is an uniformly accelerated motion. To simplify the calculation, we can decompose the motion into two directions and in each direction the motion is still uniformly accelerated motion. So we have these two equations if the motion is uniformly accelerated motion:

Where i can be 1 or 2 which represent the direction of the motion. But how do we confirm the initial position? Theoretically it is arbitrary if the quadcopter can fly exactly in the path we plan before. However it’s impossible and there must be some error between the ideal path and true path. So we have to minimize the error as much as possible and less the time to pass, less error. So how can we reduce the time to pass?

If the position and posture of gap is stationary, |a_1 | can’t be changed so the only way to reduce time is reduce l. But l can’t be too small because the velocity in the beginning will be too fast. But you might say if we reduce d the velocity will be reduced too so why don’t you just reduce d. It is reasonable but d cannot be too small either. Because the d is too small means when the quadcopter fly into plane II it’s very close to the gap and it’s dangerous. So we have following optimization problem:

The specific parameters are depended on the specific problem.

Simulation of Route Planning

A. Input is Jerk

So now we can divide the task into two stages: fly from the origin point to the initial point p_0 in the plane II and fly through the gap from p_0 to p_f. We call the former one Approach Trajectory and the latter on Traverse Trajectory. Steps of simulation in MATLAB are these:
1. Confirm the location and posture of gap, then choose proper l and d to confirm the initial point of traverse trajectory p_0. In this simulation, we choose l=0.45m and d=0.6m considering the size of quadcopter. Position of gap center is (5,5,5) and all units is meter in this simulation. Posture of gap is ϕ=45°,θ=30°,ψ=10°. So the position of p_0 and equations of plane II in the world coordinates can be calculated.
2. Use the equations of uniformly accelerated motion in plane II to calculate the velocity in p_0. Use the postures of p_0 to calculate the acceleration in p_0. So we get all states of p_0 including position, velocity and acceleration in world coordinates.
3. Use the optimal control to generate the approach trajectory while the initial state is all zero and final state is state in p_0 which have been calculated before.
4. Till now we get the approach trajectory and traverse trajectory. So we can draw the 3D path in MATLAB and see if out result is right. And plot position, velocity, acceleration and Euler angle to analyze the simulation.

So the quadcopter at beginning is at the original point. Then it flies to the gap and arrives p_0. We can see that when quadcopter approaching the gap it’s not in the plane II but eventually when it reaches p_0 the quadcopter is in the plane II. Then it follows the traverse trajectory when it always flies in the plane II and pass the gap. Note that when it is passing the gap the quadcopter center overlap the gap center as planned. So it proves our simulation is successful.

The following five pictures are Euler angles, error of angles, thrust, position in three directions and angular velocity respectively. Although we have identify that XYZ sequence is better for calculating anti-trigonometric function, we’d better check the error here again. The result is satisfied.

The final graphic is the traverse path in the plan II. The initial position is (-0.6,-0.45) and it reaches the (0,0) which is the gap center. The traverse path is parabola indeed as it’s an uniformly accelerated motion.

However, if we look at the graphic of angular velocity, we will find that at the initial time the angular velocity is not zero which dose not satisfy the reality. But it cannot be avoid and the reason we already discussed before. This is the actual reason that we must use jounce as the input.

B. Input is Jounce

The simulation steps is similar with when input is jerk. But I don’t justify if our trajectory meet the boundary conditions such as if the frame of quadcopter hit the ground when flying or if the angular velocity of motors can meet the requirement of torque. Unfortunately the simulation we do when input is jerk is not possible in reality. If we draw the velocity of four motors we will find out why.

We can easily see that one of the curve reaches zero when time is about 1.7s which means the angular velocity of this motor is zero at that moment. After that moment the angular velocity does not exit actually because the torque requires the force produced by this propeller is negative which is impossible. But in the MATLAB program it can still calculate sqrt() function even the number is negative but it’s actually meaningless. This is one kind of boundary condition which is the angular velocity of propeller is negative. Another boundary condition is the frame of quadcopter hit the ground. Let’s see if the ultimate state in position is still (5,5,5) and Euler angle is ϕ=45°,θ=10°,ψ=10° and time is 5s. Graphics of angular velocity of propellers and positions in three directions are shown as these:

Angular velocity of four motors are definitely fine as no one reaches zero. But the problem is the position in two directions become negative when flying which is not possible because our quadcopter cannot fly under the ground. So this trajectory should also be given up.

So when we generate a trajectory we should justify if the trajectory meets the boundary conditions. If the ultimate state has been determined just like our simulation, the only parameter we can change is the time to reach this state. So we will choose numbers of different times to reach the goal and calculate the trajectory respectively. Then we give up some of them if they beyond boundary conditions and finally draw the remaining trajectories. The following is the example when the ultimate state in position is still (5,5,5) and Euler angle is ϕ=45°,θ=20°,ψ=-20° and time chosen from 2s to 4s when interval is 0.1s. Those grey trajectories with quadcopters are valid and others are not because of boundary conditions.

Matlab code

alpha=45/180*pi;
beta=20/180*pi;
gama=-20/180*pi;
g=9.8;
p=[5;5;5];
l=0.45;
d=0.6;
des_b=[-d;-l;0];
phi=alpha;
theta=beta;
psi=gama;
Rx=[1 0 0;0 cos(phi) -sin(phi);0 sin(phi) cos(phi)];
Ry=[cos(theta) 0 sin(theta);0 1 0;-sin(theta) 0 cos(theta)];
Rz=[cos(psi) -sin(psi) 0;sin(psi) cos(psi) 0;0 0 1];
R=Rx*Ry*Rz;
des_w=R*des_b+p;
Acceleration_b=R'*[0;0;-g];
t=sqrt(2*l/abs(Acceleration_b(2)));
Velocity_b=[d/t-1/2*Acceleration_b(1)*t;l/t-1/2*Acceleration_b(2)*t;0];
Velocity_w=R*Velocity_b;
Acceleration_w=R*[Acceleration_b(1);Acceleration_b(2);0];frame1_b=[0;0.4;0.15];
frame1_w=R*frame1_b;
frame2_b=[0;-0.4;0.15];
frame2_w=R*frame2_b;
frame3_b=[0;-0.4;-0.15];
frame3_w=R*frame3_b;
frame4_b=[0;0.4;-0.15];
frame4_w=R*frame4_b;
X_frame=[frame1_w(1) frame2_w(1) frame3_w(1) frame4_w(1) frame1_w(1)]+[p(1) p(1) p(1) p(1) p(1)];
Y_frame=[frame1_w(2) frame2_w(2) frame3_w(2) frame4_w(2) frame1_w(2)]+[p(2) p(2) p(2) p(2) p(2)];
Z_frame=[frame1_w(3) frame2_w(3) frame3_w(3) frame4_w(3) frame1_w(3)]+[p(3) p(3) p(3) p(3) p(3)];
plot3(X_frame,Y_frame,Z_frame,'r','LineWidth',3);
grid on;
axis([0 7 0 7 0 7]);
hold on;Units_w=R*[1 0 0;0 1 0;0 0 1]+5*ones(3);
Unitx=Units_w(:,1);
Unity=Units_w(:,2);
Unitz=Units_w(:,3);
plot3([p(1) Unitx(1)],[p(2) Unitx(2)],[p(3) Unitx(3)]);
hold on;
plot3([p(1) Unity(1)],[p(2) Unity(2)],[p(3) Unity(3)]);
hold on;
plot3([p(1) Unitz(1)],[p(2) Unitz(2)],[p(3) Unitz(3)]);
hold on;x_plane=linspace(p(1)-2,p(1)+2,100);
y_plane=linspace(p(2)-2,p(2)+2,100);
[x_mesh y_mesh]=meshgrid(x_plane,y_plane);
z_mesh=((Unitz(1)-p(1))*(p(1)-x_mesh)+(Unitz(2)-p(2))*(p(2)-y_mesh))/(Unitz(3)-p(3))+p(3);
surf(x_mesh,y_mesh,z_mesh,'FaceAlpha',0.8);
shading interpt_2=linspace(0,2*sqrt(2*l/abs(Acceleration_b(2))),100);
x_e1=-d+Velocity_b(1)*t_2+1/2*Acceleration_b(1)*t_2.^2;
x_e2=-l+Velocity_b(2)*t_2+1/2*Acceleration_b(2)*t_2.^2;
x_e3=linspace(0,0,100);
x_w=R*[x_e1;x_e2;x_e3]+p;
plot3(x_w(1,:),x_w(2,:),x_w(3,:),'color',[0.8 0.8 0.8],'LineWidth',1.5);
hold on;
% figure(2)
% plot(x_e1,x_e2)T=3;
pf_x=des_w(1),pf_y=des_w(2),pf_z=des_w(3);
p0_x=0,p0_y=0,p0_z=0.1;
vf_x=Velocity_w(1),vf_y=Velocity_w(2),vf_z=Velocity_w(3);
v0_x=0,v0_y=0,v0_z=0;
af_x=Acceleration_w(1),af_y=Acceleration_w(2),af_z=Acceleration_w(3);
a0_x=0,a0_y=0,a0_z=0;
jf_x=0,jf_y=0,jf_z=0;
j0_x=0,j0_y=0,j0_z=0;% pf_x=0,pf_y=0,pf_z=5;
% p0_x=0,p0_y=0,p0_z=0;
% vf_x=0,vf_y=0,vf_z=0;
% v0_x=0,v0_y=0,v0_z=0;
% af_x=0,af_y=0,af_z=0;
% a0_x=0,a0_y=0,a0_z=0;delta_x=[pf_x-p0_x-v0_x*T-1/2*a0_x*T^2-1/6*j0_x*T^3;vf_x-v0_x-a0_x*T-1/2*j0_x*T^2;af_x-a0_x-j0_x*T;jf_x-j0_x];
delta_y=[pf_y-p0_y-v0_y*T-1/2*a0_y*T^2-1/6*j0_y*T^3;vf_y-v0_y-a0_y*T-1/2*j0_y*T^2;af_y-a0_y-j0_y*T;jf_y-j0_y];
delta_z=[pf_z-p0_z-v0_z*T-1/2*a0_z*T^2-1/6*j0_z*T^3;vf_z-v0_z-a0_z*T-1/2*j0_z*T^2;af_z-a0_z-j0_z*T;jf_z-j0_z];
Smatrix_co=1/T^7*[-16800 8400*T -1680*T^2 140*T^3;-25200*T 12240*T^2 -2340*T^3 180*T^4;-10080*T^2 4680*T^3 -840*T^4 60*T^5;-840*T^3 360*T^4 -60*T^5 4*T^6];coefficient_x=Smatrix_co*delta_x;coefficient_y=Smatrix_co*delta_y;coefficient_z=Smatrix_co*delta_z;t=0:T/1000:T;s_x_p=coefficient_x(1)/840*t.^7-coefficient_x(2)/360*t.^6+coefficient_x(3)/120*t.^5-coefficient_x(4)/24*t.^4+j0_x/6*t.^3+a0_x/2*t.^2+v0_x*t+p0_x;s_x_v=coefficient_x(1)/120*t.^6-coefficient_x(2)/60*t.^5+coefficient_x(3)/24*t.^4-coefficient_x(4)/6*t.^3+j0_x/2*t.^2+a0_x*t+v0_x;s_x_a=coefficient_x(1)/20*t.^5-coefficient_x(2)/12*t.^4+coefficient_x(3)/6*t.^3-coefficient_x(4)/2*t.^2+j0_x*t+a0_x;s_x_j=coefficient_x(1)/4*t.^4-coefficient_x(2)/3*t.^3+coefficient_x(3)/2*t.^2-coefficient_x(4)*t+j0_x;s_y_p=coefficient_y(1)/840*t.^7-coefficient_y(2)/360*t.^6+coefficient_y(3)/120*t.^5-coefficient_y(4)/24*t.^4+j0_y/6*t.^3+a0_y/2*t.^2+v0_y*t+p0_y;s_y_v=coefficient_y(1)/120*t.^6-coefficient_y(2)/60*t.^5+coefficient_y(3)/24*t.^4-coefficient_y(4)/6*t.^3+j0_y/2*t.^2+a0_y*t+v0_y;s_y_a=coefficient_y(1)/20*t.^5-coefficient_y(2)/12*t.^4+coefficient_y(3)/6*t.^3-coefficient_y(4)/2*t.^2+j0_y*t+a0_y;s_y_j=coefficient_y(1)/4*t.^4-coefficient_y(2)/3*t.^3+coefficient_y(3)/2*t.^2-coefficient_y(4)*t+j0_y;s_z_p=coefficient_z(1)/840*t.^7-coefficient_z(2)/360*t.^6+coefficient_z(3)/120*t.^5-coefficient_z(4)/24*t.^4+j0_z/6*t.^3+a0_z/2*t.^2+v0_z*t+p0_z;s_z_v=coefficient_z(1)/120*t.^6-coefficient_z(2)/60*t.^5+coefficient_z(3)/24*t.^4-coefficient_z(4)/6*t.^3+j0_z/2*t.^2+a0_z*t+v0_z;s_z_a=coefficient_z(1)/20*t.^5-coefficient_z(2)/12*t.^4+coefficient_z(3)/6*t.^3-coefficient_z(4)/2*t.^2+j0_z*t+a0_z;s_z_j=coefficient_z(1)/4*t.^4-coefficient_z(2)/3*t.^3+coefficient_z(3)/2*t.^2-coefficient_z(4)*t+j0_z;%�����������������λ�ơ��ٶȡ����ٶȡ��Ӽ��ٶ�Cdm=0.009012;Cd=0.06579;m=1.5;f=sqrt((s_x_a+Cd*s_x_v.^2/m).^2+(s_y_a+Cd*s_y_v.^2).^2+(s_z_a+g+Cd*s_z_v.^2).^2);for i=1:1:1001X(i,2)=asin((s_x_a(i)+Cd*s_x_v(i)^2/m)/f(i));X(i,1)=atan(-(s_y_a(i)+Cd*s_y_v(i)^2/m)/(s_z_a(i)+g+Cd*s_z_v(i)^2));X(i,3)=0;endwrong_num=0;
for i=1:1:1001 
phi=X(i,1);
theta=X(i,2);
psi=X(i,3);
Rx=[1 0 0;0 cos(phi) -sin(phi);0 sin(phi) cos(phi)];
Ry=[cos(theta) 0 sin(theta);0 1 0;-sin(theta) 0 cos(theta)];
Rz=[cos(psi) -sin(psi) 0;sin(psi) cos(psi) 0;0 0 1];
R=Rx*Ry*Rz;
flight_w=R*[0.225 -0.225 0 0;0 0 0.225 -0.225;0 0 0 0];
flight1=flight_w(:,1)+[s_x_p(i);s_y_p(i);s_z_p(i)];
flight2=flight_w(:,2)+[s_x_p(i);s_y_p(i);s_z_p(i)];
flight3=flight_w(:,3)+[s_x_p(i);s_y_p(i);s_z_p(i)];
flight4=flight_w(:,4)+[s_x_p(i);s_y_p(i);s_z_p(i)];if flight1(3)<0 || flight2(3)<0 || flight3(3)<0 || flight4(3)<0disp('wrong') disp(i)wrong_num=wrong_num+1end
if mod(i,100)==1
plot3([flight1(1) flight2(1)],[flight1(2) flight2(2)],[flight1(3) flight2(3)],'color',[0.2 0.2 0.2],'LineWidth',3);
hold on;
plot3([flight3(1) flight4(1)],[flight3(2) flight4(2)],[flight3(3) flight4(3)],'color',[0.2 0.2 0.2],'LineWidth',3);
hold on;
end
endplot3(s_x_p,s_y_p,s_z_p,'color',[0.8 0.8 0.8],'LineWidth',1.5);
hold on;for j=linspace(1,100,5)
i=floor(j);
flight_w=R*[0.225 -0.225 0 0;0 0 0.225 -0.225;0 0 0 0];
flight1=flight_w(:,1)+[x_w(1,i);x_w(2,i);x_w(3,i)];
flight2=flight_w(:,2)+[x_w(1,i);x_w(2,i);x_w(3,i)];
flight3=flight_w(:,3)+[x_w(1,i);x_w(2,i);x_w(3,i)];
flight4=flight_w(:,4)+[x_w(1,i);x_w(2,i);x_w(3,i)];
plot3([flight1(1) flight2(1)],[flight1(2) flight2(2)],[flight1(3) flight2(3)],'color',[0.2 0.2 0.2],'LineWidth',3);
hold on;
plot3([flight3(1) flight4(1)],[flight3(2) flight4(2)],[flight3(3) flight4(3)],'color',[0.2 0.2 0.2],'LineWidth',3);
hold on;
endfigure(2);plot(t,s_x_p,t,s_y_p,t,s_z_p)  set(gcf,'color','w');legend('boxoff');set(gcf,'unit','centimeters','position',[10 5 10 10]);
%  
% figure(3)
% plot(x_e1,x_e2)
% set(gcf,'color','w');
%  legend('boxoff');
%  set(gcf,'unit','centimeters','position',[10 5 10 10]);
%  
%  figure(4);
%  plot(t,f);
%  set(gcf,'unit','centimeters','position',[10 5 10 10]);
%  set(gcf,'color','w');
%  
%  figure(5)
%  plot(t,X(:,1),t,X(:,2),t,X(:,3));
%  set(gcf,'unit','centimeters','position',[10 5 10 10]);
%  set(gcf,'color','w');for i=1:1:1001phi=X(i,1);
theta=X(i,2);
psi=X(i,3); 
Rx=[1 0 0;0 cos(phi) -sin(phi);0 sin(phi) cos(phi)];
Ry=[cos(theta) 0 sin(theta);0 1 0;-sin(theta) 0 cos(theta)];
Rz=[cos(psi) -sin(psi) 0;sin(psi) cos(psi) 0;0 0 1];w=[1 0 0;0 1 0;0 0 0]*Rz'*Ry'*Rx'*[s_x_j(i)+2*Cd*s_x_v(i)*s_x_a(i)/m;s_y_j(i)+2*Cd*s_y_v(i)*s_y_a(i)/m;s_z_j(i)+2*Cd*s_z_v(i)*s_z_a(i)/m]/f(i);w_v2(i)=w(1);w_v1(i)=-w(2);w_final(i)=sqrt(w_v1(i)^2+w_v2(i)^2);end%   figure(6)
%  plot(t,w_final);
%  set(gcf,'unit','centimeters','position',[10 5 10 10]);
%  set(gcf,'color','w');for i=1:1:1000diff_x=(X(i+1,1)-X(i,1))/(t(i+1)-t(i));diff_y=(X(i+1,2)-X(i,2))/(t(i+1)-t(i));diff_z=(X(i+1,3)-X(i,3))/(t(i+1)-t(i));w_another=[cos(X(i,2))*cos(X(i,3)) sin(X(i,3)) 0;-cos(X(i,2))*sin(X(i,3)) cos(X(i,3)) 0;sin(X(i,2)) 0 1]*[diff_x;diff_y;diff_z];p(i)=w_another(1);q(i)=w_another(2);r(i)=w_another(3);w_anotherall(i)=sqrt(w_another(1)^2+w_another(2)^2+w_another(3)^2);endfor i=1:1:999p_ac(i)=(p(i+1)-p(i))/(t(i+1)-t(i));q_ac(i)=(q(i+1)-q(i))/(t(i+1)-t(i));r_ac(i)=(r(i+1)-r(i))/(t(i+1)-t(i));endIx=0.01745;
Iy=0.01745;
Iz=0.03175;
l=0.225;
Irotor=0.000099;
Cd=0.009012;
CT=0.00001105;
CQ=0.0000001489;
% w(1)=w(2)=w(3)=w(4)=0;
U1=f*1.5;
wrong_numangle=0;
for i=1:1:1
U2(i)=Ix*p_ac(i)-(Iy-Iz)*q(i)*r(i)+Cd*p(i)^2;
U3(i)=Iy*q_ac(i)-(Iz-Ix)*p(i)*r(i)+Cd*q(i)^2;
U4(i)=Iz*r_ac(i)-(Ix-Iy)*p(i)*q(i)+Cd*r(i)^2;
w1(i)=sqrt((1/(4*CT)*U1(i)-1/(2*l*CT)*U3(i)+1/(4*CQ)*U4(i)));
w2(i)=sqrt((1/(4*CT)*U1(i)+1/(2*l*CT)*U2(i)-1/(4*CQ)*U4(i)));
w3(i)=sqrt((1/(4*CT)*U1(i)+1/(2*l*CT)*U3(i)+1/(4*CQ)*U4(i)));
w4(i)=sqrt((1/(4*CT)*U1(i)-1/(2*l*CT)*U2(i)-1/(4*CQ)*U4(i)));
end  for i=2:1:999
U2(i)=Ix*p_ac(i)-(Iy-Iz)*q(i)*r(i)-Irotor*(w1(i-1)-w2(i-1)+w3(i-1)-w4(i-1))*q(i)+Cd*p(i)^2;
U3(i)=Iy*q_ac(i)-(Iz-Ix)*p(i)*r(i)-Irotor*(w1(i-1)-w2(i-1)+w3(i-1)-w4(i-1))*p(i)+Cd*q(i)^2;
U4(i)=Iz*r_ac(i)-(Ix-Iy)*p(i)*q(i)+Cd*r(i)^2;
w1sq=1/(4*CT)*U1(i)-1/(2*l*CT)*U3(i)+1/(4*CQ)*U4(i);
w2sq=1/(4*CT)*U1(i)+1/(2*l*CT)*U2(i)-1/(4*CQ)*U4(i);
w3sq=1/(4*CT)*U1(i)+1/(2*l*CT)*U3(i)+1/(4*CQ)*U4(i);
w4sq=1/(4*CT)*U1(i)-1/(2*l*CT)*U2(i)-1/(4*CQ)*U4(i);
if w1sq<0 || w2sq<0 ||w3sq<0 ||w4sq<0wrong_numangle=wrong_numangle+1
end
w1(i)=sqrt((1/(4*CT)*U1(i)-1/(2*l*CT)*U3(i)+1/(4*CQ)*U4(i)));
w2(i)=sqrt((1/(4*CT)*U1(i)+1/(2*l*CT)*U2(i)-1/(4*CQ)*U4(i)));
w3(i)=sqrt((1/(4*CT)*U1(i)+1/(2*l*CT)*U3(i)+1/(4*CQ)*U4(i)));
w4(i)=sqrt((1/(4*CT)*U1(i)-1/(2*l*CT)*U2(i)-1/(4*CQ)*U4(i)));
end  %  figure(8)
%  plot(t(1:999),p_ac,t(1:999),q_ac,t(1:999),r_ac);
%  set(gcf,'unit','centimeters','position',[10 5 10 10]);
%  set(gcf,'color','w');%  figure(9)
%  plot(t(1:999),w1,t(1:999),w2,t(1:999),w3,t(1:999),w4);
%  set(gcf,'unit','centimeters','position',[10 5 10 10]);
%  set(gcf,'color','w');%  figure(10)
%  plot(t(1:999),U2,t(1:999),U3,t(1:999),U4);
%  set(gcf,'unit','centimeters','position',[10 5 10 10]);
%  set(gcf,'color','w');
%  figure(7)
%  plot(t(1:1000),w_anotherall);
%  set(gcf,'unit','centimeters','position',[10 5 10 10]);
%  set(gcf,'color','w');figure(11)plot(t(1:1000),p,t(1:1000),q,t(1:1000),r);set(gcf,'unit','centimeters','position',[10 5 10 10]);set(gcf,'color','w');