✅作者简介：热爱科研的Matlab仿真开发者，修心和技术同步精进，matlab项目合作可私信。
🍎个人主页：Matlab科研工作室
🍊个人信条：格物致知。
更多Matlab仿真内容点击👇
⛄ 内容介绍
⛄ 部分代码
% /****************************************************************************% * modification, are permitted provided that the following conditions% * are met:% *% * 1. Redistributions of source code must retain the above copyright% * notice, this list of conditions and the following disclaimer.% * 2. Redistributions in binary form must reproduce the above copyright% * notice, this list of conditions and the following disclaimer in% * the documentation and/or other materials provided with the% * distribution.% * 3. Neither the name of this repo nor the names of its contributors may be% * used to endorse or promote products derived from this software% * without specific prior written permission.% *% * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS% * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT% * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS% * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE% * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,% * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,% * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS% * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED% * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT% * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN% * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE% * POSSIBILITY OF SUCH DAMAGE.% *% ****************************************************************************/clc;close all;clear all;set(0,'defaulttextInterpreter','latex');set(groot, 'defaultAxesTickLabelInterpreter','latex'); set(groot, 'defaultLegendInterpreter','latex');set(groot, 'DefaultAxesFontWeight', 'bold');%set(0,'DefaultFigureWindowStyle','docked')set(0,'DefaultFigureWindowStyle','normal')global my_sigmoid_ a a_orign label torque_vector amp1 amp2 tt input_pd input_teb input_cg input_bgf input_cf lambda0 lambdaf lambdac T get_qd1 get_qd2 get_qd3 get_qd4 k0 A Kd P0 K0; my_sigmoid_ = @my_sigmoid;label='1'; %1 or time_var. If time_var, the parameters will change as a sigmoidprint_plots=0;if strcmp(label,'time_var') T=3;else T=1.5;endm=0.5;Ixx=2*4.8e-02;Iyy=2*4.8e-02;Izz=2*8.8e-02;a=[m;Ixx;Iyy;Izz]; %Real parametersa_orign=[m;Ixx;Iyy;Izz]; %Real parameters%Parameters only of CF:K0=50*eye(4); %Parameters of CF and BGF:lambda0=4800; k0=200; %%Parameters of CG and BGF and CF:lambdaf=10; %Parameters of TEB and CG:P0=5*eye(4); %Parameters of all the algorithms (used for the control law and the definition of s):lambdac=5; % A=20*eye(4);% Kd=50*eye(4);thrust.teb=[]; thrust.cg=[]; thrust.bgf=[];tau_yaw.teb=[]; tau_yaw.cg=[]; tau_yaw.bgf=[];tau_pitch.teb=[]; tau_pitch.cg=[]; tau_pitch.bgf=[];tau_roll.teb=[]; tau_roll.cg=[]; tau_roll.bgf=[];tau4_vector=[];syms tt%opts = odeset('RelTol',1e-2); % [t,Y] = ode45(@(t,y) myfunc(t,y,....), tspan, ic, opts);[t_pd,y_pd] = adapt_control("PD"); %"TEB", "CG" or "BGF" [t_teb,y_teb] = adapt_control("TEB"); %"TEB", "CG" or "BGF"[t_cg,y_cg] = adapt_control("CG"); %"TEB", "CG" or "BGF"[t_bgf,y_bgf] = adapt_control("BGF"); %"TEB", "CG" or "BGF"[t_cf,y_cf] = adapt_control("CF"); %"TEB", "CG" or "BGF" %% Plotsclose allset(gcf,'renderer','Painters')close allwidth=1500; height=300;%%%%%% Estimated Parametersinit_par=13;figure; set(gcf, 'Position', [100, 100, width, height])subplot(1,4,1); hold onplot(t_teb,y_teb(:,init_par))plot(t_cg,y_cg(:,init_par))plot(t_bgf,y_bgf(:,init_par))plot(t_cf,y_cf(:,init_par))if strcmp(label,'time_var') plot(t_cf,my_sigmoid_(t_cf)*a_orign(1),'Color','blue','LineStyle','--')else line([0 T],[a_orign(1) a_orign(1)],'Color','blue','LineStyle','--');end%yline(a(1),'-.b')title('$\hat{m}\;\;(kg)$'); xlabel('$t(s)$')legend('TEB','CG','BGF','CF','Real','Location','southeast')subplot(1,4,2); hold onplot(t_teb,y_teb(:,init_par+1))plot(t_cg,y_cg(:,init_par+1))plot(t_bgf,y_bgf(:,init_par+1))plot(t_cf,y_cf(:,init_par+1))if strcmp(label,'time_var') plot(t_cf,my_sigmoid_(t_cf)*a_orign(2),'Color','blue','LineStyle','--')else line([0 T],[a_orign(2) a_orign(2)],'Color','blue','LineStyle','--');endtitle('$\hat{I}_{xx}\;\;(kg \; m^2)$'); xlabel('$t(s)$')legend('TEB','CG','BGF','CF','Real','Location','southeast')subplot(1,4,3); hold onplot(t_teb,y_teb(:,init_par+2))plot(t_cg,y_cg(:,init_par+2))plot(t_bgf,y_bgf(:,init_par+2))plot(t_cf,y_cf(:,init_par+2))if strcmp(label,'time_var') plot(t_cf,my_sigmoid_(t_cf)*a_orign(3),'Color','blue','LineStyle','--')else line([0 T],[a_orign(3) a_orign(3)],'Color','blue','LineStyle','--');endtitle('$\hat{I}_{yy}\;\;(kg \; m^2)$'); xlabel('$t(s)$')legend('TEB','CG','BGF','CF','Real','Location','southeast')subplot(1,4,4); hold onplot(t_teb,y_teb(:,init_par+3))plot(t_cg,y_cg(:,init_par+3))plot(t_bgf,y_bgf(:,init_par+3))plot(t_cf,y_cf(:,init_par+3))if strcmp(label,'time_var') plot(t_cf,my_sigmoid_(t_cf)*a_orign(4),'Color','blue','LineStyle','--')else line([0 T],[a_orign(4) a_orign(4)],'Color','blue','LineStyle','--');endtitle('$\hat{I}_{zz}\;\;(kg \; m^2)$'); xlabel('$t(s)$')legend('TEB','CG','BGF','CF','Real','Location','southeast')set(gcf,'renderer','Painters')%suptitle('Estimated Parameters')if(print_plots==1) printeps(1,strcat('./figs/estimated_parameters',label))end%%%%%% Errors in qfigureinit_q=3;subplot(1,4,1); hold onplot(t_teb,y_teb(:,init_q)-get_qd1(t_teb))plot(t_cg,(y_cg(:,init_q)-get_qd1(t_cg)))plot(t_bgf,(y_bgf(:,init_q)-get_qd1(t_bgf)))plot(t_cf,(y_cf(:,init_q)-get_qd1(t_cf)))plot(t_pd,y_pd(:,init_q)-get_qd1(t_pd))title('$\tilde{z}$ (m)'); xlabel('$t(s)$')legend({'TEB','CG','BGF','CF','PD'},'Location','southeast')%ylim([-0.45 0.1])subplot(1,4,2); hold onplot(t_teb,(180/pi)*(y_teb(:,init_q+1)-get_qd2(t_teb)))plot(t_cg,(180/pi)*(y_cg(:,init_q+1)-get_qd2(t_cg)))plot(t_bgf,(180/pi)*(y_bgf(:,init_q+1)-get_qd2(t_bgf)))plot(t_cf,(180/pi)*(y_cf(:,init_q+1)-get_qd2(t_cf)))plot(t_pd,(180/pi)*(y_pd(:,init_q+1)-get_qd2(t_pd)))title('$\widetilde{roll}$ (deg)'); xlabel('$t(s)$')legend('TEB','CG','BGF','CF','PD','Location','southeast')%ylim([-2 1])subplot(1,4,3); hold onplot(t_teb,(180/pi)*(y_teb(:,init_q+2)-get_qd3(t_teb)))plot(t_cg,(180/pi)*(y_cg(:,init_q+2)-get_qd3(t_cg)))plot(t_bgf,(180/pi)*(y_bgf(:,init_q+2)-get_qd3(t_bgf)))plot(t_cf,(180/pi)*(y_cf(:,init_q+2)-get_qd3(t_cf)))plot(t_pd,(180/pi)*(y_pd(:,init_q+2)-get_qd3(t_pd)))title('$\widetilde{pitch}$ (deg)'); xlabel('$t(s)$')legend('TEB','CG','BGF','CF','PD','Location','southeast')%ylim([-2 1])subplot(1,4,4); hold onplot(t_teb,(180/pi)*(y_teb(:,init_q+3)-get_qd4(t_teb)))plot(t_cg,(180/pi)*(y_cg(:,init_q+3)-get_qd4(t_cg)))plot(t_bgf,(180/pi)*(y_bgf(:,init_q+3)-get_qd4(t_bgf)))plot(t_cf,(180/pi)*(y_cf(:,init_q+3)-get_qd4(t_cf)))plot(t_pd,(180/pi)*(y_pd(:,init_q+3)-get_qd4(t_pd)))title('$\widetilde{yaw} (deg)$'); xlabel('$t(s)$')legend('TEB','CG','BGF','CF','PD','Location','southeast')%ylim([-2 1])%suptitle('Position Errors')set(gcf, 'Position', [100, 100, width, height])set(gcf,'renderer','Painters')if(print_plots==1) printeps(2,strcat('./figs/position_errors',label))end%%%%%%%%%%%%%%%%% INPUTSfigure; set(gcf, 'Position', [100, 100, width, height])subplot(1,4,1); hold onplot(t_teb,input_teb(:,1))plot(t_cg,input_cg(:,1))plot(t_bgf,input_bgf(:,1))plot(t_cf,input_cf(:,1))plot(t_pd,input_pd(:,1))%yline(a(1),'-.b')title('$T\;\;(N)$'); xlabel('$t(s)$'); xlim([0 T])legend('TEB','CG','BGF','CF','PD','Location','southeast')subplot(1,4,2); hold onplot(t_teb,input_teb(:,2))plot(t_cg,input_cg(:,2))plot(t_bgf,input_bgf(:,2))plot(t_cf,input_cf(:,2))plot(t_pd,input_pd(:,2))%yline(a(1),'-.b')title('$\tau_{roll}\;\;(Nm)$'); xlabel('$t(s)$'); xlim([0 T])legend('TEB','CG','BGF','CF','PD','Location','southeast')subplot(1,4,3); hold onplot(t_teb,input_teb(:,3))plot(t_cg,input_cg(:,3))plot(t_bgf,input_bgf(:,3))plot(t_cf,input_cf(:,3))plot(t_pd,input_pd(:,3))%yline(a(1),'-.b')title('$\tau_{pitch}\;\;(Nm)$'); xlabel('$t(s)$'); xlim([0 T])legend('TEB','CG','BGF','CF','PD','Location','southeast')subplot(1,4,4); hold onplot(t_teb,input_teb(:,4))plot(t_cg,input_cg(:,4))plot(t_bgf,input_bgf(:,4))plot(t_cf,input_cf(:,4))plot(t_pd,input_pd(:,4))%yline(a(1),'-.b')title('$\tau_{yaw}\;\;(Nm)$'); xlabel('$t(s)$'); xlim([0 T])legend('TEB','CG','BGF','CF','PD','Location','southeast')%suptitle('Inputs')set(gcf,'renderer','Painters')if(print_plots==1) printeps(3,strcat('./figs/inputs',label))end%%%%%%%%%%%%%%%%%%% TRAJECTORIES% figure% subplot(3,1,1); hold on% plot(t_teb,get_qd1(t_teb));% plot(t_teb,y_teb(:,init_q));% legend('z_d (m)','z (m)')% % subplot(3,1,2)% hold on% plot(t_teb,(180/pi)*get_qd2(t_teb));% plot(t_teb,(180/pi)*get_qd3(t_teb));% plot(t_teb,(180/pi)*get_qd4(t_teb));% legend('roll_d (deg)','pitch_d (deg)','yaw_d(deg)')% % subplot(3,1,3); hold on% plot3(y_teb(:,1),y_teb(:,2),y_teb(:,3));% plot3(y_cg(:,1),y_cg(:,2),y_cg(:,3));% plot3(y_bgf(:,1),y_bgf(:,2),y_bgf(:,3));% title('3D trajectory'); xlabel('x'); ylabel('y'); zlabel('z'); grid on% legend('TEB','CG','BGF')%%function y=my_sigmoid(t) global T y=(1+0.5*sigmoid(t,T/2,80));endfunction [yaw pitch roll]=obtainEulerFromAccelAndYaw(accel,yaw) %See Mellinger paper (Minimum Snap Trajectory Generation...) tmp=[accel(1);accel(2);accel(3) + 9.81]; zb=tmp/norm(tmp); xc=[cos(yaw);sin(yaw);0]; yb=cross(zb,xc)/norm(cross(zb,xc)); xb=cross(yb,zb); rot_matrix = [xb,yb,zb]; [yaw pitch roll] = rotm2eul(rot_matrix,'ZYX');end
⛄ 运行结果
⛄ 参考文献
[1] Weiping Li and Jean-Jacques E Slotine. An indirect adaptive robot controller. Systems & control letters, 12(3):259–266,1989.
[2] Jean-Jacques E Slotine and Weiping Li. On the adaptive control of robot manipulators. The international journal of robotics research, 6(3):49–59, 1987.
[3] Zachary T Dydek, Anuradha M Annaswamy, and Eugene Lavretsky. Adaptive control of quadrotor uavs: A design trade study with flight evaluations. IEEE Transactions on control systems technology, 21(4):1400–1406, 2013.
