diff --git a/MRAC.md b/MRAC.md new file mode 100644 index 0000000..e5318c7 --- /dev/null +++ b/MRAC.md @@ -0,0 +1,124 @@ +# MRAC (Model Reference Adaptive Control) + Considering the LTI system + +$$ +\dot{x}=Ax+Bu\\ +y=Cx +$$ + +where: +* $x\in \mathbb{R}^n$: state vector +* $u\in \mathbb{R}^m$: control input vector +* $y\in \mathbb{R}^p$: output vector +* $A\in\mathbb{R}^{n\times n}$: state matrix (Unkonwn) +* $B\in\mathbb{R}^{n\times m}$: input matrix (Partially known) +* $C\in\mathbb{R}^{p\times n}$: output matrix (Known) + + + +Now, we define a reference model such that in open loop the interesting signal follows the reference: + +$$ +\dot{x}_m=A_mx_m+B_mr\\ +y=Cx_m +$$ + +* $r\in\mathbb{R}^p$: reference vector + +This model complies that: + +* $A_m=A+BK_x^*$ +* $B_m=BK_r^*$ + +### Observer +Since not all states are accessible, a Luenberger observer is created as: +$$ +\dot{\hat{x}}=A\hat{x}+Bu+L(y-\hat{y})\\ +\hat{y}=C\hat{x} +$$ +$$ +\dot{\hat{x}}=A\hat{x}+Bu+L(y-C\hat{x})\\ +\dot{\hat{x}}=A\hat{x}+Bu+Ly-LC\hat{x}\\ +\dot{\hat{x}}=(A-LC)\hat{x}+Bu+Ly +$$ + +* $BK_y^*=-L$ +Observer error: +$$ +e_l=x-\hat{x}\\ +\dot{e}_l=\dot{x}-\dot{\hat{x}}\\ +\dot{e}_l=Ax+Bu-(A-LC)\hat{x}-Bu-Ly\\ +\dot{e}_l=Ax-(A-LC)\hat{x}-LCx\\ +\dot{e}_l=(A-LC)x-(A-LC)\hat{x}\\ +\dot{e}_l=(A-LC)e_l +$$ + +### Controller +Using the next control law: +$$ +u=K_x^*\hat{x}+K_r^*r+K_y^*(y-\hat{y}) +$$ + +The state estimate is: +$$ +\dot{\hat{x}}=(A_m-BK_x^*)\hat{x}+B(K_x\hat{x}+K_rr+K_y(y-\hat{y}))+L(y-\hat{y}) +$$ + + +### Error +Defining the error as: +$$ +e=x_m-\hat{x}\\ +\dot{e}=\dot{x}_m-\dot{\hat{x}}\\ +\dot{e}=A_mx_m+B_mr-A_m\hat{x}+BK_x^*\hat{x}-BK_x\hat{x}-BK_rr-BK_y(y-\hat{y})-L(y-\hat{y})\\ +\dot{e}=A_me+BK_r^*r+BK_x^*\hat{x}-BK_x\hat{x}-BK_rr-BK_y(y-\hat{y})+BK_y^*(y-\hat{y}) +$$ + +Defining: +* $\tilde{K}_x = K_x-K_x^*$ +* $\tilde{K}_r = K_r-K_r^*$ +* $\tilde{K}_y = K_y-K_y^*$ + +$$ +\dot{e}=A_me-B\tilde{K}_rr-B\tilde{K}_x\hat{x}-B\tilde{K}_y(y-\hat{y})\\ +\dot{e}=A_me-B\tilde{K}_rr-B\tilde{K}_x\hat{x}-B\tilde{K}_yCe_l\\ +\dot{e}=A_me-B\left(\tilde{K}_rr+\tilde{K}_x\hat{x}+\tilde{K}_yCe_l\right) +$$ + + +To find the $K$, we choice the following Lyapunov candidate: +$$ +V = e^TPe + \text{trace}(\tilde{K}_x\Gamma_x^{-1}\tilde{K}^T_x) + \text{trace}(\tilde{K}_r\Gamma_r^{-1}\tilde{K}^T_r) + \text{trace}(\tilde{K}_y\Gamma_y^{-1}\tilde{K}^T_y) \\ +\dot{V} = e^TP\dot{e} + \dot{e}^TPe + 2\text{trace}(\tilde{K}_x\Gamma_x^{-1}\dot{\tilde{K}}^T_x)+2\text{trace}(\tilde{K}_r\Gamma_r^{-1}\dot{\tilde{K}}^T_r)+2\text{trace}(\tilde{K}_y\Gamma_y^{-1}\dot{\tilde{K}}^T_y) \\ +\dot{V} = e^TP(A_me-BK)+(A_me-BK)^TPe+ 2\text{trace}(\tilde{K}_x\Gamma_x^{-1}\dot{\tilde{K}}^T_x)+2\text{trace}(\tilde{K}_r\Gamma_r^{-1}\dot{\tilde{K}}^T_r)+2\text{trace}(\tilde{K}_y\Gamma_y^{-1}\dot{\tilde{K}}^T_y)\\ +\dot{V} = e^TP(A_me-BK)+(e^TA_m^T-K^TB^T)Pe+ 2\text{trace}(\tilde{K}_x\Gamma_x^{-1}\dot{\tilde{K}}^T_x)+2\text{trace}(\tilde{K}_r\Gamma_r^{-1}\dot{\tilde{K}}^T_r)+2\text{trace}(\tilde{K}_y\Gamma_y^{-1}\dot{\tilde{K}}^T_y)\\ +\dot{V} = e^TPA_me-e^TPBK+e^TA_m^TPe-K^TB^TPe+ 2\text{trace}(\tilde{K}_x\Gamma_x^{-1}\dot{\tilde{K}}^T_x)+2\text{trace}(\tilde{K}_r\Gamma_r^{-1}\dot{\tilde{K}}^T_r)+2\text{trace}(\tilde{K}_y\Gamma_y^{-1}\dot{\tilde{K}}^T_y)\\ +\dot{V} = e^T[PA_me+e^TA_m^TP]e-e^TPBK-K^TB^TPe+ 2\text{trace}(\tilde{K}_x\Gamma_x^{-1}\dot{\tilde{K}}^T_x)+2\text{trace}(\tilde{K}_r\Gamma_r^{-1}\dot{\tilde{K}}^T_r)+2\text{trace}(\tilde{K}_y\Gamma_y^{-1}\dot{\tilde{K}}^T_y)\\ +\dot{V} = e^T[PA_me+e^TA_m^TP]e-2e^TPB\left(\tilde{K}_rr+\tilde{K}_x\hat{x}+\tilde{K}_yCe_l\right)+ 2\text{trace}(\tilde{K}_x\Gamma_x^{-1}\dot{\tilde{K}}^T_x)+2\text{trace}(\tilde{K}_r\Gamma_r^{-1}\dot{\tilde{K}}^T_r)+2\text{trace}(\tilde{K}_y\Gamma_y^{-1}\dot{\tilde{K}}^T_y) +$$ + +* $e^TPB\tilde{K}_rr = \text{trace}(\tilde{K}_rre^TPB)$ +* $e^TPB\tilde{K}_x\hat{x} = \text{trace}(\tilde{K}_x\hat{x}e^TPB)$ +* $e^TPB\tilde{K}_yCe_l = \text{trace}(\tilde{K}_yCe_le^TPB)$ + +$$ +\dot{V} = e^T[PA_me+e^TA_m^TP]e ++ 2\text{trace}\left(\tilde{K}_x\left[-\hat{x}e^TPB+\Gamma_x^{-1}\dot{\tilde{K}}^T_x\right]\right) ++ 2\text{trace}\left(\tilde{K}_r\left[-re^TPB+\Gamma_r^{-1}\dot{\tilde{K}}^T_r\right]\right) ++ 2\text{trace}\left(\tilde{K}_y\left[-Ce_le^TPB+\Gamma_y^{-1}\dot{\tilde{K}}^T_y\right]\right) +$$ + +$$ +\tilde{K}_i = K_i-K_i^*\\ +\dot{\tilde{K}_i}=\dot{K}_i-\dot{K}_i^*\\ +\dot{\tilde{K}_i}=\dot{K}_i +$$ + +* $\dot{K}^T_x = \Gamma_x\hat{x}e^TPB$ +* $\dot{K}^T_r = \Gamma_rre^TPB$ +* $\dot{K}^T_y = \Gamma_yCe_le^TPB$ + + +$$ + +$$ \ No newline at end of file diff --git a/PID.md b/PID.md new file mode 100644 index 0000000..e69de29 diff --git a/README.md b/README.md index 6228659..3be1229 100644 --- a/README.md +++ b/README.md @@ -8,40 +8,33 @@ This repository contains the development of an aeropendulum and the performance $$ \begin{gather*} - I\ddot{\theta}=W_cl_c\cos{\theta}+F_hl_m-W_bl_b\cos{\theta}-W_nl_n\cos{\theta}-B\dot{\theta}\\ - \ddot{\theta}=\frac{1}{I}(W\cos{\theta}-B\dot{\theta}+F_hl_m) + I\ddot{\theta}=W_cl_c\cos{\theta}+F_hl_m-W_bl_b\cos{\theta}-W_nl_n\cos{\theta}-\beta\dot{\theta}\\ + \ddot{\theta}=\frac{1}{I}(W\cos{\theta}-\beta\dot{\theta}+F_hl_m) \end{gather*} $$ Where - $$ W=W_cl_c-W_bl_b-W_nl_n $$ To simplify the inertia, we define the term $I_0$ as the system inertia without counterweight and the term $I$ as the system inertia with counterweight. - $$ -I=I_0+\frac{W_c}{g}l_c^2\\ -I_0 = I_b+I_n +I=I_0+m_cl_c^2 $$ -To compute the bar inertia ($I_b$), we use the parallel axes theorem: - +Where $I_0 = I_b+I_n$, to compute the bar inertia ($I_b$), we use the parallel axes theorem: $$ I_b = I_{b_{cm}} + m_bl_b^2\\ -I_b = \frac{1}{12}m_b(l_1+l_m)^2 + m_bl_b^2\\ -I_b = \frac{1}{12} \frac{W_b}{g} (l_1 + l_m)^2 + \frac{W_b}{g}l_b^2 +I_b = \frac{1}{12}m_b(l_1+l_m)^2 + m_bl_b^2 $$ And the motor+nuts inertia ($I_n$) is considered as a puntual load, i.e. - $$ -I_n = m_nl_n^2\\ -I_n = \frac{W_n}{g} l_n^2 +I_n = m_nl_n^2 $$ -#### State-space model +## State-space model To represent the state-space model of the system, we define the next state equations: $$ @@ -53,7 +46,7 @@ Therefore: $$ \dot{x}_1 = x_2\\ -\dot{x}_2 = \frac{1}{I}(W\cos{x_1}-Bx_2+F_hl_m) +\dot{x}_2 = \frac{1}{I}(W\cos{x_1}-\beta x_2+F_hl_m) $$ ### Linearization @@ -77,7 +70,7 @@ $$ $$ \dot{Z}=\left. \begin{bmatrix} - \frac{\partial f_1}{\partial x_1} & \frac{\partial f_1}{\partial x_2}\\ +\frac{\partial f_1}{\partial x_1} & \frac{\partial f_1}{\partial x_2}\\ \frac{\partial f_2}{\partial x_1} & \frac{\partial f_1}{\partial x_2} \end{bmatrix}\right|_{X^*,F^*} Z+\left. @@ -99,26 +92,53 @@ $$ To $x_1$: $$ f_2(x_1^*,x_2^*,F^*)=\dot{x}_2=0\\ -\frac{1}{I}(W\cos{x_1^*}-Bx_2^*+F^*l_m)=0\\ -W\cos{x_1^*}-Bx_2^*+F^*l_m=0\\ -F^*=\frac{Bx_2^*-W\cos{x_1^*}}{l_m}\\ +\frac{1}{I}(W\cos{x_1^*}-\beta x_2^*+F^*l_m)=0\\ +W\cos{x_1^*}-\beta x_2^*+F^*l_m=0\\ +F^*=\frac{\beta x_2^*-W\cos{x_1^*}}{l_m}\\ \mathbf{F^*=-\frac{W\cos{x_1^*}}{l_m}} $$ -If you want $x_1^*=0$, the thrust force is $F^*=-\frac{W}{l_m}$. + Finally: $$ \dot{Z}=\left. \begin{bmatrix} 0 & 1\\ - -\frac{W}{I}\sin{x_1} & -\frac{B}{I} + -\frac{W}{I}\sin{x_1} & -\frac{\beta}{I} \end{bmatrix}\right|_{X^*,F^*} Z+\left. \begin{bmatrix} 0\\ \frac{l_m}{I} \end{bmatrix}\right|_{X^*,F^*} +u\\ +\dot{Z}= +\begin{bmatrix} + 0 & 1\\ + -\frac{W}{I}\sin{x_1^*} & -\frac{\beta}{I} +\end{bmatrix} +Z+ +\begin{bmatrix} + 0\\ + \frac{l_m}{I} +\end{bmatrix} +u +$$ + +If you want $x_1^*=0$, the thrust force is $F^*=-\frac{W}{l_m}$ and: + +$$ +\dot{Z}= +\begin{bmatrix} + 0 & 1\\ + 0 & -\frac{\beta}{I} +\end{bmatrix} +Z+ +\begin{bmatrix} + 0\\ + \frac{l_m}{I} +\end{bmatrix} u $$ @@ -140,7 +160,15 @@ To find the $B$ and $F_h$ parameters, we use the following methods: 1. **Thrust force ($F_h$)**
The objective of this identification is to find the thrust force $F_h$ that depends of voltage. Therefore, we take measurements of the voltage locating the system in an angle $0$ with different counterweights. With this, we can calculate the thrust force $F_h$ that corresponds to the voltage $V$ in the system making a torque summation. + If we have: + $$ + B= + \begin{bmatrix} + 0\\ + K_f\frac{l_m}{I} + \end{bmatrix} + $$ + -## MRAC(Model Reference Adaptive Control) diff --git a/SMC.md b/SMC.md new file mode 100644 index 0000000..a03eb4a --- /dev/null +++ b/SMC.md @@ -0,0 +1,51 @@ +# SMC +DEfine the next states: +$$ +\dot{x}_1 = x_2\\ +\dot{x}_2 = \frac{1}{I}(W\cos{x_1}-\beta x_2+F_hl_m) +$$ + +Can be written as: +$$ +\dot{x}_1 = x_2\\ +\dot{x}_2 = \frac{1}{I}[W\cos{x_1}-\beta x_2+(K_f\cdot v+b_f)l_m] +$$ + +* $h(x) = \frac{1}{I}(W\cos{x_1}-\beta x_2+b_fl_m)$ +* $f(x) = \frac{1}{I}(K_fl_m)$ + +Now, we define an error: + +$$ +x_e=x_1-x_r\\ +\dot{x}_e=\dot{x}_1 +$$ + +and the next sliding manifold: +$$ +S = ax_e+x_2 +$$ + +where its derivative is: +$$ +\dot{S} = a\dot{x}_e+\dot{x}_2\\ +\dot{S} = a\dot{x}_1+h(x)+f(x)v +$$ + +and the signal control law is: +$$ +v=-\rho\ \text{sign}(S) +$$ + +To the system stability, we need to define the following: +$$ +\rho \geq \left|\frac{ax_2+h(x)}{f(x)}\right|\\ +\rho \geq \left|\frac{ax_2+\frac{1}{I}(W\cos{x_1}-\beta x_2+b_fl_m)}{\frac{1}{I}(K_fl_m)}\right| +$$ + +Taken as limits $x_2\leq\pi$ and, we get: + +$$ +\rho \geq \frac{a\max{x_2}+\frac{1}{\min{I}}(\max{W}+\max{\beta}\max{x_2}+\max{b_fl_m})}{\frac{1}{\max{I}}\min{K_fl_m}}\\ +\rho \geq \frac{a\pi+\frac{1}{I_0}(\max{W}-\max{\beta}\pi+\max{b_fl_m})}{\frac{1}{I}\min{K_fl_m}} +$$ \ No newline at end of file diff --git a/esp32-20220618-v1.19.1.bin b/esp32-20220618-v1.19.1.bin new file mode 100644 index 0000000..72d5846 Binary files /dev/null and b/esp32-20220618-v1.19.1.bin differ diff --git a/test.py b/test.py new file mode 100644 index 0000000..dcb3572 --- /dev/null +++ b/test.py @@ -0,0 +1,15 @@ +from machine import Pin +import time +import numpy as np + +p2 = Pin(2, Pin.OUT) +count = 0 + +while True: + count+= 1 + p2.on() + time.sleep(1) + p2.off() + time.sleep(1) + + print(count) \ No newline at end of file