-1
$\begingroup$

I'm trying to simulate a 2DOF planar pendulum with a regresor-passivity control, the thing is I've been having some issues with my simulation, I'm using Simulink, but in theory I know my control law should make the states of the system to converge, however in Simulation this is not happening on the contrary the position variqables diverge. If possible it will be really helpful if someone could guide me here, maybe I'm doing something wrong in simulation or I didnt defined something properly in the theory. This is the link to the simulation file:simulation file (the extension is mdl, should work with any version of matlab, I used R2020). also here is a paper where I found more info bout regressors: "https://link.springer.com/content/pdf/10.1007%2F978-3-642-41610-1_88-1.pdf" .Let me show the theory background first

Mechanical system

enter image description here

\begin{equation}\label{eq:sis2} \mathbf{M}(\mathbf{q})\ddot{\mathbf{q}}+\mathbf{C}(\mathbf{q},\dot{\mathbf{q}})\dot{\mathbf{q}}+\mathbf{g}(\mathbf{q})=\boldsymbol{\tau} \end{equation}

$\mathbf{q}(t)\in \mathbb{R}^{2\times 1}$, $\mathbf{M}(\mathbf{q})\in \mathbb{R}^{2\times 2}$, $\mathbf{C}(\mathbf{q}, \dot{\mathbf{q}})\in \mathbb{R}^{2\times 2}$, $\mathbf{g}(\mathbf{\dot{q}})\in \mathbb{R}^{2\times 1}$, $\boldsymbol{\tau} \in \mathbb{R}^{2\times 1}$.

Inertia Matrix: $\begin{equation*} \mathbf{M}(\mathbf{q})=\begin{bmatrix} m_{11}&m_{12}\\ m_{21}&m_{22}\\ \end{bmatrix} \end{equation*}$, Coriolis:$\begin{equation*} \mathbf{C}(\mathbf{q},\dot{\mathbf{q}})=\begin{bmatrix} -2\phi\dot{q}_{2}&-\phi\dot{q}_{2}\\ \phi\dot{q}_{1}&0\\ \end{bmatrix} \end{equation*} $, gravity vector: $\begin{equation*} \mathbf{g}(\mathbf{q})=\begin{bmatrix} (h_{1}+h_{2})g&h_{2}g \end{bmatrix}^{T} \end{equation*}$.

\begin{align*} &m_{11}=m_{2}L_{1}^2+m_{2}l^2_{c2}+m_{1}l^2_{c1}+I_{1}+I_{2}+2m_{2}L_{1}l_{c2}\cos (q_{2})\\ &m_{12}=m_{21}=m_{2}l_{c2}^2+m_{2}L_{1}l_{c2}\cos (q_{2})+I_{2}\\ &m_{22}=m_{2}l_{c2}^2+I_{2}\\ &\phi=m_{2}L_{1}l_{c2}\sin q_{2}\\ &h_{1}=(m_{1}l_{c1}+m_{2}L_{1})\sin q_{1}\\ &h_{2}=m_{2}l_{c2}\sin (q_{1}+q_{2})\\ \end{align*}

Physical values:

enter image description here

I have previously tested this model with a Calculated torque control for trajectory tracking, it worked just fine. So, I have proven stability in the sense of Lyapunov using an error variable $\mathbf{S}$, such that $\mathbf{S}=\dot{\mathbf{q}}-\dot{\mathbf{q}}_{r}$ y $\dot{\mathbf{q}}_{r}=\dot{\mathbf{q}}_ {d}-\alpha \Delta \mathbf{q}$, thus $\mathbf{S}=\Delta \dot{\mathbf{q}}+\alpha \Delta \mathbf{q}$. $\mathbf{S}$ is the extended error variable which is expected to converge to $(0,0)$.

Let us define the parameterized regressor:

\begin{equation}\label{eq:sis4} \mathbf{Y}_{r}(\ddot{\mathbf{q}},\dot{\mathbf{q}_{r}},\mathbf{q})\mathbf{\theta}=\mathbf{M}(\mathbf{q})\ddot{\mathbf{q}_{r}}+\mathbf{C}(\mathbf{q},\dot{\mathbf{q}})\dot{\mathbf{q}_{r}}+\mathbf{g}(\mathbf{q}) \end{equation}

\begin{equation}\label{eq:yr} \mathbf{Y}_{r}(\ddot{\mathbf{q}}_{r},\dot{\mathbf{q}}_{r},\mathbf{q})=\begin{bmatrix} \ddot{q}_{1r}&\ddot{q}_{1r}&\ddot{q}_{1r}+\ddot{q}_{2r}&y_{1r}&\ddot{q}_{1r}&\ddot{q}_{1r}+\ddot{q}_{2r}&sin(q_{1})&sin(q_{1})&sin(q_{1}+q_{2})\\ 0&0&\ddot{q}_{1r}+\ddot{q}_{2r}&y_{2r}&0&\ddot{q}_{1r}+\ddot{q}_{2r}&0&0&sin(q_{1}+q_{2}) \end{bmatrix} \end{equation}

where,

\begin{align*} y_{1r}&=2cos(q_{2})\ddot{q}_{1r}+cos(q_{2})\ddot{q_{2r}}-2sin(q_{2})\dot{q}_{2}\dot{q}_{1r}-sin(q_{2})\dot{q}_{2}\dot{q}_{2r}\\ y_{2r}&=cos(q_{2})\ddot{q}_{1r}+sin(q_{2})\dot{q}_{1}\dot{q}_{1r} \end{align*}

and the parameters of the system:

\begin{equation} \mathbf{\theta}=\begin{bmatrix} m_{1}l^{2}_{c1}\\ m_{2}L^{2}_{1}\\ m_{2}l^{2}_{c2}\\ m_{2}L_{1}l^{2}_{c2}\\ I_{1}\\ I_{2}\\ m_{1}l_{c1}g\\ m_{2}L_{1}g \\ m_{2}l_{c2}g \end{bmatrix} \end{equation}

I took this regressor from the book "Adaptive control of robot manipulators" by An-Chyau Huang but I made some corrections since the original had typos.

Let us now define our control law:

\begin{equation} \mathbf{\tau}=-\mathbf{K_{d}}\mathbf{S} + \mathbf{Y}_{r}\mathbf{\theta} \end{equation}

The closed-loop system:

\begin{equation}\label{eq:sis5} \mathbf{M}(\mathbf{q})(\ddot{\mathbf{q}}-\ddot{\mathbf{q}_{r}})+\mathbf{C}(\mathbf{q},\dot{\mathbf{q}})(\dot{\mathbf{q}}-\dot{\mathbf{q}_{r}})+\mathbf{g}(\mathbf{q})-\mathbf{g}(\mathbf{q})=\tau - \mathbf{Y}_{r}(\ddot{\mathbf{q}},\dot{\mathbf{q}_{r}},\mathbf{q})\mathbf{\theta} \end{equation}

Reducing the previous expression:

\begin{equation}\label{eq:sis6} \mathbf{M}(\mathbf{q})\dot{S}+\mathbf{C}(\mathbf{q},\dot{\mathbf{q}})S+K_{d}S=0 \end{equation}

Stability in the sense of Lyapunov

Candidate function: $\begin{equation} V=\frac{1}{2}S^{T}M(q)S \end{equation}$

Conditions to be satisfied to guarantee asympotic convergence: a) $\dot{V} < 0$, $S\neq 0$, b) $\lim_{|S| \rightarrow\infty}V=\infty$.

Condition b) is satisfied by $S>0$ if it grows $V$ grows radially. For condition b) we have:

\begin{equation*} \dot{V}=\frac{1}{2}\{\dot{S}^{T}MS+S^{T}\dot{M}S+S^{T}M\dot{S}\} \end{equation*}

since $M$ is symmetric,

\begin{align*} \dot{V}=&\frac{1}{2}\{S^{T}\dot{M}S+2S^{T}M\dot{S}\} \end{align*}

substituting $M\dot{S}$,

\begin{align*} \dot{V}=&\frac{1}{2}\{S^{T}\dot{M}S+2S^{T}(-CS-K_{s}S)\}\\ =&\frac{1}{2}\{S^{T}\dot{M}S-2S^{T}CS-2S^{T}K_{d}S\}\\ =&S^{T}(\frac{1}{2}\dot{M}-C)S-S^{T}K_{d}S \end{align*}

since $\frac{1}{2}\dot{M}-C$ is anti-symmetric, then $S^{T}(\frac{1}{2}\dot{M}-C)S=0$, therefore

\begin{align*} \dot{V}=&-S^{T}K_{d}S\leq-K_{d}||S||^{2} \end{align*}

b) is satisfied.

Now, since I must simulate the "Real plant" I will make use of the control law $\mathbf{\tau}$, variable $\mathbf{S}$ and the parameterized regressor $\mathbf{Y}_{r}\mathbf{\theta}$. So, what I'm doing is to build $\mathbf{\dot{q}}_{r}$ (as shown before:$\dot{\mathbf{q}}_{r}=\dot{\mathbf{q}}_ {d}-\alpha \Delta \mathbf{q}$)from a desired trayectory $\mathbf{\dot{q}}_{d}$ (I chose sine and cosine) and the "actual angular velocity" $\mathbf{\dot{q}}$ so that I can generate $\mathbf{S}=\dot{\mathbf{q}}-\dot{\mathbf{q}}_{r}=\Delta \dot{\mathbf{q}}+\alpha \Delta \mathbf{q}$. In my simulation I'm just changig the contotrol input $\mathbf{\tau}=\mathbf(K)_{s}*\mathbf(S)+\mathbf(Y)_{r}\mathbf(\theta)$. In simulation I'm integrating the following expression:

\begin{equation}\label{eq:sis11} \ddot{\mathbf{q}}=\mathbf{M}^{-1}*\{\tau -\mathbf{C}(\mathbf{q},\dot{\mathbf{q}})*\dot{\mathbf{q}}-\mathbf{g}(\mathbf{q})\} \end{equation}

Simulation information (Matlab/Simulink)

I'm using a fixed integration step (0.0000) with the ODE sover Runge/Kuta-4 in simulink.

Simulation scheme:

enter image description here

Desired trajectories:

enter image description here

Control block:\

enter image description here

Regressor block:\

enter image description here

Position Response:

enter image description here

As you can see the response there is no convergence in the response in opposition to the theory which says it should. To be honest I don't know what I'm doing wrong thats why I need your help guys if possible. Thanks in advance fopr any help.

$\endgroup$
0
$\begingroup$

I solved it. Theory is correct, the problem was $\dot{\mathbf{q}}_{r}$ in simulink, I was being redundant with the variables, so instead of using the derivatives that were already in simulation I was adding more derivative blocks and probably causing issues in simulation. Plus to make sure I had no errors in the regressor, I basically have rewritten it as:

\begin{equation} \mathbf{Y}_r=\mathbf{M}(\mathbf{q})\ddot{\mathbf{q}}_{r}+\mathbf{C}(\dot{\mathbf{q}},\mathbf{q})\dot{\mathbf{q}}_{r}+\mathbf{G}(\mathbf{q}) \end{equation}

inside the function block in simulink. Here is the link to download the mdl file: corrected simulation

$\endgroup$

Your Answer

By clicking “Post Your Answer”, you agree to our terms of service, privacy policy and cookie policy

Not the answer you're looking for? Browse other questions tagged or ask your own question.