如何实现和编码用于四旋翼的内部和外部PD控制器以进行位置跟踪

%%
%######################( Position Controller )%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

clear all;
clc;

dt = 0.001;
 t = 0;

% initial values of the system
 x = 0;
dx = 0;
 y = 0;
dy = 0;
 z = 0;
dz = 0;

   Phi = 0;
  dPhi = 0;
 Theta = 0;
dTheta = 0;
   Psi = pi/3;
  dPsi = 0;


%System Parameters:
m  = 0.75;      % mass (Kg)
L  = 0.25;      % arm length (m)
Jx = 0.019688; % inertia seen at the rotation axis. (Kg.m^2)
Jy = 0.019688; % inertia seen at the rotation axis. (Kg.m^2)
Jz = 0.039380; % inertia seen at the rotation axis. (Kg.m^2)
g  = 9.81;      % acceleration due to gravity m/s^2

errorSumX = 0;
errorSumY = 0;
errorSumZ = 0;

errorSumPhi   = 0;
errorSumTheta = 0;

pose = load('xyTrajectory.txt');

DesiredX = pose(:,1);
DesiredY = pose(:,2);
DesiredZ = pose(:,3);

dDesiredX = 0;
dDesiredY = 0;
dDesiredZ = 0;

DesiredXpre = 0;
DesiredYpre = 0;
DesiredZpre = 0;

dDesiredPhi = 0;
dDesiredTheta = 0;
DesiredPhipre = 0;
DesiredThetapre = 0;



for i = 1:6000

   % torque input
   %&&&&&&&&&&&&( Ux )&&&&&&&&&&&&&&&&&&
   Kpx = 50; Kdx = 8; Kix = 0; 
   Ux = Kpx*( DesiredX(i) - x  ) + Kdx*( dDesiredX - dx ) + Kix*errorSumX;

   errorSumX = errorSumX + ( DesiredX(i) - x );

     dDesiredX = ( DesiredX(i) - DesiredXpre ) / dt;
   DesiredXpre = DesiredX(i);

   %&&&&&&&&&&&&( Uy )&&&&&&&&&&&&&&&&&&
   Kpy = 100; Kdy = 10; Kiy = 0; 
   Uy = Kpy*( DesiredY(i) - y  ) + Kdy*( dDesiredY - dy ) + Kiy*errorSumY;


   errorSumY = errorSumY + ( DesiredY(i) - y );

     dDesiredY = ( DesiredY(i) - DesiredYpre ) / dt;
   DesiredYpre = DesiredY(i);



   %&&&&&&&&&&&&( U1 )&&&&&&&&&&&&&&&&&&
   Kpz = 100; Kdz = 20; Kiz = 0; 
   U1 = Kpz*( DesiredZ(i) - z ) + Kdz*( dDesiredZ - dz ) + Kiz*errorSumZ; 

   errorSumZ = errorSumZ + ( DesiredZ(i) - z );

      dDesiredZ = ( DesiredZ(i) - DesiredZpre ) / dt;
   DesiredZpre = DesiredZ(i);

   %#######################################################################
   %#######################################################################
   %#######################################################################
   % Desired Phi and Theta
   R = [  sin(Psi),cos(Psi); 
         -cos(Psi),sin(Psi)];


   DAngles = R\( (m/U1)*[Ux; Uy]);


   %Wrap angles
   DesiredPhi   = wrapToPi( DAngles(1) ) /2;
   DesiredTheta = wrapToPi( DAngles(2) );


   %&&&&&&&&&&&&( U2 )&&&&&&&&&&&&&&&&&&
   KpP = 100; KdP = 10; KiP = 0;
   U2 = KpP*( DesiredPhi - Phi ) + KdP*( dDesiredPhi - dPhi )  + KiP*errorSumPhi;

   errorSumPhi = errorSumPhi + ( DesiredPhi - Phi );

     dDesiredPhi = ( DesiredPhi - DesiredPhipre ) / dt;
   DesiredPhipre = DesiredPhi;


   %--------------------------------------
   %&&&&&&&&&&&&( U3 )&&&&&&&&&&&&&&&&&&

   KpT = 100; KdT = 10; KiT = 0;
   U3 = KpT*( DesiredTheta - Theta ) + KdP*( dDesiredTheta - dTheta ) + KiT*errorSumTheta;

    errorSumTheta = errorSumTheta + ( DesiredTheta - Theta );

     dDesiredTheta = ( DesiredTheta - DesiredThetapre ) / dt;
   DesiredThetapre = DesiredTheta;



   %--------------------------------------
   %&&&&&&&&&&&&( U4 )&&&&&&&&&&&&&&&&&&
   KpS = 80; KdS = 20.0; KiS = 0.08;
   U4 = KpS*( 0 - Psi ) + KdS*( 0 - dPsi );


   %###################( ODE Equations of Quadrotor )###################
   %===================( X )=====================
   ddx = (U1/m)*( Theta*cos(Psi) + Phi*sin(Psi) );

    dx = dx + ddx*dt;
     x =  x +  dx*dt;
   %===================( Y )=====================
   ddy = (U1/m)*( Theta*sin(Psi) - Phi*cos(Psi) );

    dy = dy + ddy*dt;
     y =  y +  dy*dt;

   %===================( Z )=====================
   ddz = (U1/m) - g;

    dz = dz + ddz*dt;
     z =  z +  dz*dt;

   %===================( Phi )=====================
   ddPhi = ( L/Jx )*U2;

    dPhi = dPhi + ddPhi*dt;
     Phi =  Phi +  dPhi*dt;

   %===================( Theta )=====================
   ddTheta =  ( L/Jy )*U3;

    dTheta =  dTheta + ddTheta*dt;
     Theta =   Theta +  dTheta*dt;

   %===================( Psi )=====================
   ddPsi =  (1/Jz)*U4;

    dPsi = dPsi + ddPsi*dt;
     Psi =  Psi +  dPsi*dt;

   %store the erro
     ErrorX(i)   = ( x - DesiredX(i) );
     ErrorY(i)   = ( y - DesiredY(i) );
     ErrorZ(i)   = ( z - DesiredZ(i) );
%    ErrorPhi(i)   = ( Phi - pi/4 );
%    ErrorTheta(i) = ( Theta - pi/4 );
     ErrorPsi(i)   = ( Psi - 0 );


   X(i) = x;
   Y(i) = y;
   Z(i) = z;

   T(i) = t;

%    drawnow 
%    plot3(DesiredX, DesiredY, DesiredZ, 'r')
%    hold on
%    plot3(X, Y, Z, 'b')
   t = t + dt; 


end


Figure1 = figure(1);
set(Figure1,'defaulttextinterpreter','latex');
%set(Figure1,'units','normalized','outerposition',[0 0 1 1]);

subplot(2,2,1)
plot(T, ErrorX, 'LineWidth', 2)
title('Error in $x$-axis Position (m)')
xlabel('time (sec)')
ylabel('$x_{d}(t) - x(t)$', 'LineWidth', 2)

subplot(2,2,2)
plot(T, ErrorY, 'LineWidth', 2)
title('Error in $y$-axis Position (m)')
xlabel('time (sec)')
ylabel('$y_{d}(t) - y(t)$', 'LineWidth', 2)

subplot(2,2,3)
plot(T, ErrorZ, 'LineWidth', 2)
title('Error in $z$-axis Position (m)')
xlabel('time (sec)')
ylabel('$z_{d} - z(t)$', 'LineWidth', 2)


subplot(2,2,4)
plot(T, ErrorPsi, 'LineWidth', 2)
title('Error in $\psi$ (m)')
xlabel('time (sec)')
ylabel('$\psi_{d} - \psi(t)$','FontSize',12);
grid on 


Figure2 = figure(2);
set(Figure2,'units','normalized','outerposition',[0 0 1 1]);

figure(2)
plot3(X,Y,Z, 'b')
grid on

hold on 
plot3(DesiredX, DesiredY, DesiredZ, 'r')

pos = get(Figure2,'Position');
set(Figure2,'PaperPositionMode','Auto','PaperUnits','Inches','PaperSize',[pos(3),pos(4)]);
print(Figure2,'output2','-dpdf','-r0');

legend('actual', 'desired')

clear all; clc;

fileID = fopen('xyTrajectory.txt','w');

 angle = -pi; radius = 5; z = 0; t = 0;

for i = 1:6000
    if ( z < 2 ) 
        z = z + 0.1;
        x = 0; 
        y = 0;
    end
    if  ( z >= 2 )
        angle = angle + 0.1;
        angle = wrapToPi(angle);
        x = radius * cos(angle);
        y = radius * sin(angle);
        z = 2;
    end

    X(i) = x;
    Y(i) = y;
    Z(i) = z;

    fprintf(fileID,'%f \t %f \t %f\n',x, y, z); end

fclose(fileID); plot3(X,Y,Z) grid on

#1 楼

作为此答案的扩展，我想与社区分享实施上述系统的Simulink模型。它不能代表CroCo报告的代码的解决方案，但可以提供更好的洞察力，因此可能会有所帮助。

系统描述如下：


红色表示的是运行在$ 1 \，\ text {KHz} $上的块，用于处理姿态控制，而绿色表示的是允许跟踪线性位移并以$ 100 \运行的块， \ text {Hz} $。

作为旁注，由于我们必须确保“小角度”假设在整个仿真过程中仍然有效，因此起飞阶段（即达到所需高度$ z_d $）在开始对$ x_d \ left（t \ right）$和$ y_d \ left（t \ right）$随时间变化的线性坐标（半径为$ 5 \，\ text的圆）进行位移跟踪之前，需要基本完成{m} $（在模型中）。此外，出于相同的原因，所需的圆形轨迹必须从四轴飞行器的初始位置$ \ left（x \ left（0 \ right），y \ left（0 \ right）\ right）$开始。

根据控制器的增益，我们可以获得很好的跟踪结果：


前三个图表显示了$ x $，$ y $和$ z $组件（跟随目标（蓝色）。最后一个图报告滚动（蓝色），俯仰（绿色）和偏航（红色）。时间以秒为单位，位移以米为单位，角度以度为单位。

评论

---

$ \ begingroup $
非常感谢。看来我要去某个地方。我的最终目标是用C ++重新实现该项目。在我的Matlab代码中，两个控制器都以1KHz运行，这意味着很难对线性模型进行激进的操纵。对于非线性模型，我使用反推控制器，对于非常小心和平滑的轨迹，似乎四旋翼遵循轨迹，但速度不是很快。
$ \ endgroup $
– Croco
15年5月26日在15:47

---

$ \ begingroup $
续，对于使用操纵杆，很难调整反推控制器以使四旋翼机尽可能快地跟随轨迹，因为此任务需要平台执行激进的操纵，这意味着小角度假设不会等等。
$ \ endgroup $
– Croco
2015年5月26日15:50

---

$ \ begingroup $
在“小角度”假设中（仍然是非线性模型，但仍简化了），最终演习的“攻击性”将取决于使假设本身失效之前可以达到的最大角度，而不取决于我会说外循环。对于完整的非线性模型以及反步控制器，我想您是在引用引用的论文，但我没有阅读。您是否知道，使用Embedded Coder工具箱可以从Simulink模型自动获取生产C ++代码，并准备在您的系统上运行？
$ \ endgroup $
– Ugo Pattacini
15年5月26日在17:09

---

Kp Kd
erroSum=errorSum+(desired-actual);
ddesired=(desired-predesired)/dt;
desiredpre=desired;
PD= Kp*(desired-actual)+Kd*(ddesired-dactual)