Upgrading Linear Proportional-Integral Controller (LPIC) to HPIC
$\textbf{Model of the control system:}$$\dot x={\color{blue}A}x+{\color{blue}B}u, \quad x\in \mathbb{ R } ^n, \quad u\in \mathbb{ R } ^m, \quad {\color{blue}A}\in \mathbb{ R } ^{n\times m}, \quad {\color{blue}B}\in \mathbb{ R } ^{n\times m} $
$u={\color{blue}{K_{lin}}}x+\int^t_{0}{\color{blue}{K_{i}}}x(\tau)d\tau, \quad \text{(already well-tuned LPIC)} $
where the pair $\{A,B\}$ is controllable and ${\color{blue}{K_{lin}}}\in \mathbb{ R } ^{m\times n}$,${\color{blue}{K_{i}}}\in \mathbb{ R } ^{m\times n}$ are such that
$\text{the matrix } \left( \begin{array}{cc} {\color{blue}{ A+BK_{lin}}} & {\color{blue} B}\\ {\color{blue}{ K_{lin}}} & \mathbf{ } \end{array} \right) \text{ is Hurwitz}. $
$\textbf{The aim}$ is to upgrade LPIC to HPIC:
$ u_{\mathrm{hpic}}=u_{\mathrm{hpc}}(x)+\int^{t}_0 u_{\mathrm{int}}(x(\tau)) d \tau $
$ u_{hpc}={\color{magenta}{K_0}}x+\|x\|_{\mathbf{ d } }^{1+\mu} ({\color{blue}{K_{\mathrm{lin}}}}-{\color{magenta}{K_0}})\mathbf{ d } (-\ln \|x\|_{\mathbf{ d } })x $
$ u_{\mathrm{int}}=\frac{\|x\|_{\mathbf{ d } }^{1+2\mu}{\color{magenta}{K_i^{new}}}\mathbf{ d } (-\ln \|x\|_{\mathbf{ d } })x}{x^{\top} \mathbf{ d } ^{\top}(-\ln \|x\|_{\mathbf{ d } }){\color{magenta}P}{G_{\mathbf{ d }} } \mathbf{ d } (-\ln \|x\|_{\mathbf{ d } })x} $
where $\mathbf{ d } (s)\!=\!e^{s(In+{\color{magenta}{\mu G_0}})}$ is a dilation in $\mathbb{ R } ^n$ for any $\mu\in [{\color{magenta}{\mu_1}}, {\color{magenta}{\mu_2}}]$ and $\|x\|_{\mathbf{ d } }$ is induced by $\|x\|\!=\!\sqrt{x^{\top}\!{\color{magenta}P} x}$
Upgrading LPIC to HPIC
The function ${\color{red} { \texttt{lpic2hpic }} } $ computes parameters $ {\color{magenta}{K_0,G_0}}, {\color{magenta} {P, K_i^{new}} }$ and ${\color{magenta} {\mu_1}} < 0 < {\color{magenta}{\mu_2}}$ of HPIC for given $ {\color{blue}A}, {\color{blue}B} $ and $ {\color{blue}{K_{lin}, K_i}}$
- $ \textbf{Input parameters}: {\color{blue}A}, {\color{blue}B}$ and ${\color{blue}{K_{lin} }} $
- $ \textbf{Output parameters}: {\color{magenta}{K_0,G_0}}, {\color{magenta} P}, {\color{magenta}{\mu_1}} < 0 < {\color{magenta}{\mu_2}}$
Implementation of HPIC
See Section Homogeneous Proportional-Integral Control (HPIC) for $G_{\mathbf{d}}=I_n+\mu {\color{magenta}G_0}$ and $K=K_{lin}-K_0$.
Global/Local Upgrading Algorithm
See Section Upgrading Linear Proportional Controller (LPC) to HPC
${\color{red} { \texttt{demo_lpic2hpic.m }} } $
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %% Example of upgrading a linear control to HPC and FHPC %% %% System: dx/dt=A*x+B*u %% %% where %% x - system state vector (n x 1) %% u - control input (m x 1) %% A - system matrix (n x n) %% B - control matrix (m x m) %% %% System for an upgrade is a linearized model of rotary inverted pendulum %% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %% Pendulum model %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% % Motor % Resistance Rm = 8.4; % Current-torque (N-m/A) kt = 0.042; % Back-emf constant (V-s/rad) km = 0.042; % %% Rotary Arm % Mass (kg) mr = 0.095; % Total length (m) r = 0.085; % Moment of inertia about pivot (kg-m^2) Jr = mr*r^2/3; % Equivalent Viscous Damping Coefficient (N-m-s/rad) br = 1e-3; % damping tuned heuristically to match QUBE-Sero 2 response % %% Pendulum Link % Mass (kg) mp = 0.024; % Total length (m) Lp = 0.129; % Pendulum center of mass (m) l = Lp/2; % Moment of inertia about pivot (kg-m^2) Jp = mp*Lp^2/3; % Equivalent Viscous Damping Coefficient (N-m-s/rad) bp = 5e-5; % damping tuned heuristically to match QUBE-Sero 2 response % Gravity Constant g = 9.81; % Total Inertia Jt = Jr*Jp - mp^2*r^2*l^2; % %% Linearized model of the rotary inverted pendulum (in the upper position): %% %% dx/dt=Ax+Bu, x=(x1,x2,x3,x4)' %% %% where x1 - angle of the pentulum arm %% x2 - angle of the rotary arm %% x3 - angular velocity of the pendulum arm %% x4 - angular velocity of the rotary arm A = [0 0 1 0; 0 0 0 1; 0 mp^2*l^2*r*g/Jt -br*Jp/Jt -mp*l*r*bp/Jt 0 mp*g*l*Jr/Jt -mp*l*r*br/Jt -Jr*bp/Jt]; % B = [0; 0; Jp/Jt; mp*l*r/Jt]; % adding a model of actuator dynamics A(3,3) = A(3,3) - km*km/Rm*B(3); A(4,3) = A(4,3) - km*km/Rm*B(4); B = km * B / Rm; %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %% HPC/HFxTC design by upgrading a linear feedback %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% Klin=[2 -35 1.5 -3]; % the linear feedback gain (provided by manufacturer) Klin_int=[0.5 -26.66 1.26 -2.73]; % the integral gain should be provided by manufacturer [K0 G0 P Ki mu1 mu2]=lpic2hpic(A,B,Klin, Klin_int); % upgrade linear PI control to HPIC %selection of the homogeneity degree Gd=eye(4)+mu1*G0; mu=mu1; % for HCP with negative homogeneity degree %Gd=eye(4)+mu2*G0; mu=mu2; % for HCP with positive homogeneity degree K=Klin-K0; %K0 - homogenization feedback gain %K - proportional gain %Ki - integral gain %Gd - generator of dilation %P - shape matrix of the weighted Euclidean norm %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %% Numerical Simulation %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% t=0; Tmax=7; h=0.001; % sampling period x=[1;1;0;0]; tl=[t];xl=[x];ul=[]; %alpha and beta are tuning parameters of the upgrade %alpha=0.01; beta=1; %for HPC with negative degree (upgrade close to zero) %alpha=1;beta=100; %for HPC with positive degree (upgrade close to Inf) alpha=0.01;beta=100; %for HFxTC (global upgrade) noise=0; %magnitude of measurement noises (may be changed for comparison) disp('Run numerical simulation...'); v=0; % for computation of integral term p=2; %contstant perturbation with an unknown bound while t<Tmax xm=x+2*noise*(rand(4,1)-0.5); %u=Klin*xm;ui=Ki*xm; %linear PI control (for comparion) %[u ui]=e_hpic(xm,K0,K,Ki,Gd,P,mu,alpha,beta); %HPIC [u ui]=e_fhpic(xm,K0,K,Ki,G0,P,mu1,mu2,alpha,beta); %FHPIC u=u+v;v=v+h*ui; %u=Klin*xm; %linear control (for comparion) x=x+h*A*x+h*B*(u+p); %explicit Euler method t=t+h; tl=[tl t]; xl=[xl x]; ul=[ul u]; end; ul=[ul u]; disp('Done!'); %%norm of the state at the time instant Tmax disp(['||x(Tmax)||=',num2str(norm(x))]) %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %% Plot simulation results %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% figure; axes1 = subplot(1,2,1); hold(axes1,'on'); plot1 = plot(tl,xl,'LineWidth',2,'Parent',axes1); set(plot1(1),'DisplayName','$x_1$'); set(plot1(2),'DisplayName','$x_2$'); set(plot1(3),'DisplayName','$x_3$'); set(plot1(4),'DisplayName','$x_4$'); ylabel('$x$','Interpreter','latex'); xlabel('$t$','Interpreter','latex'); title({'n=4'}); xlim(axes1,[0 Tmax]); ylim(axes1,[-5 5]); box(axes1,'on'); hold(axes1,'off'); set(axes1,'FontSize',30,'XGrid','on','YGrid','on'); legend1 = legend(axes1,'show'); set(legend1,'Interpreter','latex'); axes2 = subplot(1,2,2); hold(axes2,'on'); plot2 = plot(tl,ul,'LineWidth',2); ylabel('$u$','Interpreter','latex'); xlabel('$t$','Interpreter','latex'); title({'FHPC,m=1'}); xlim(axes2,[0 Tmax]); ylim(axes2,[-5 5]); box(axes2,'on'); hold(axes2,'off'); set(axes2,'FontSize',30,'XGrid','on','YGrid','on');