制御系CAD

制御を意識したモノつくりを目指して

LQG Control

投稿日時: 2022年8月17日投稿者: cacsd

1. Optimal Control for a SISO system

Given a 1st-order system with an input and an output:

(1) $\begin{eqnarray*} &&\dot{x}(t)=ax(t)+bu(t),\ y(t)=x(t)\ \\ &&(x(t)\in{\rm\bf R},\ u(t)\in{\rm\bf R},\ y(t)\in{\rm\bf R}) \end{eqnarray*}$

and its stabilizing state feedback:

(2) $\begin{eqnarray*} u(t)=-fx(t), \end{eqnarray*}$

the closed-loop system is represented by

(3) $\begin{eqnarray*} \dot{x}(t)=(a-bf)x(t),\ a-bf<0. \end{eqnarray*}$

The state behavior $x(t)$ and the input behavior $u(t)$ are given by

(4) $\begin{eqnarray*} x(t)=e^{(a-bf)t}x(0) \end{eqnarray*}$

and

(5) $\begin{eqnarray*} u(t)=-fe^{(a-bf)t}x(0). \end{eqnarray*}$

respectively. Then consider a problem to determine $f$ to minimize a criterion function

(6) $\begin{eqnarray*} J=\int_0^\infty(q^2x^2(t)+r^2u^2(t))\,dt. \end{eqnarray*}$

The first term of the criterion function is calculated as

(7) $\begin{eqnarray*} J_x&=&\int_0^\infty q^2x^2(t)\,dt \nonumber\\ &=&\int_0^\infty q^2e^{2(a-bf)t}x^2(0)\,dt \nonumber\\ &=&q^2x^2(0)\left[\frac{1}{2(a-bf)}e^{2(a-bf)t}\right]_0^\infty \nonumber\\ &=&\frac{q^2x^2(0)}{2(a-bf)}\left[\underbrace{e^{2(a-bf)\infty}}_{0}-\underbrace{e^{2(a-bf)0}}_{1}\right] \nonumber\\ &=&-\frac{q^2}{2(a-bf)}x^2(0)>0\quad (a-bf<0), \end{eqnarray*}$

and the second term of the criterion function is calculated as

(8) $\begin{eqnarray*} J_u&=&\int_0^\infty r^2u^2(t)\,dt \nonumber\\ &=&\int_0^\infty r^2f^2e^{2(a-bf)t}x^2(0)\,dt \nonumber\\ &=&r^2f^2x^2(0)\left[\frac{1}{2(a-bf)}e^{2(a-bf)t}\right]_0^\infty \nonumber\\ &=&-\frac{r^2f^2}{2(a-bf)}x^2(0)>0\quad (a-bf<0). \end{eqnarray*}$

Therefore, the criterion function can be written as

(9) $\begin{eqnarray*} J=\underbrace{\frac{-q^2}{2(a-bf)}x^2(0)}_{J_x}+\underbrace{\frac{-r^2f^2}{2(a-bf)}x^2(0)}_{J_u} =\underbrace{-\frac{q^2+r^2f^2}{2(a-bf)}}_{\Pi}x^2(0). \end{eqnarray*}$

Minimizing $J$ is equivalent to minimizing

(10) $\begin{eqnarray*} \Pi=-\frac{q^2+r^2f^2}{2(a-bf)}. \end{eqnarray*}$

Differentiating by $f$ brings

(11) $\begin{eqnarray*} \frac{d\Pi}{df} &=&-\frac{2r^2f}{2(a-bf)}-(-1)\frac{q^2+r^2f^2}{2(a-bf)^2}(-b)\nonumber\\ &=&-\frac{2r^2(a-bf)f+(q^2+r^2f^2)b}{2(a-bf)^2}\nonumber\\ &=&\frac{br^2f^2-2ar^2f-q^2b}{2(a-bf)^2}. \end{eqnarray*}$

Therefore

(12) $\begin{eqnarray*} \frac{d\Pi}{df}=0 & \Rightarrow & bf^2-2af-\left(\frac{q}{r}\right)^2b=0. \end{eqnarray*}$

As $f$ must satisfy $a-bf<0$ , we have

(13) $\begin{eqnarray*} {f=\frac{1}{b}\left(a+\sqrt{a^2+\left(\frac{q}{r}\right)^2b^2}\right)}\nonumber \end{eqnarray*}$

This is uniquely determined because $\Pi$ is downward convex as follows:

(14) $\begin{eqnarray*} \frac{d^2\Pi}{df^2}&=&\frac{2br^2f-2ar^2}{2(a-bf)^2} +(-2)\frac{br^2f^2-2ar^2f-q^2b}{2(a-bf)^3}(-b)\nonumber\\ &=&\frac{-r^2}{(a-bf)} +b\frac{-2r^2(a-bf)f-br^2f^2-q^2b}{(a-bf)^3}\nonumber\\ &=&\frac{-r^2(a-bf)^2-2r^2(a-bf)bf-r^2b^2f^2-q^2b^2}{(a-bf)^3}\nonumber\\ &=&\frac{-r^2(a-bf+bf)^2-q^2b^2}{2(a-bf)^3}\nonumber\\ &=&\frac{-r^2a^2-q^2b^2}{2(a-bf)^3}>0. \end{eqnarray*}$

The closed-loop system by the $f$ is given by

(15) $\begin{eqnarray*} \dot{x}(t)=(a-bf)x(t)=-\sqrt{a^2+\left(\frac{q}{r}\right)^2b^2}x(t). \end{eqnarray*}$

In the case of $a=\frac{1}{T},b=\frac{K}{T}$ ,

(16) $\begin{eqnarray*} \dot{x}(t)&=&-\frac{1}{T}\sqrt{1+\left(\frac{q}{r}\right)^2K^2}x(t)\nonumber\\ &=&-\frac{1}{\frac{T}{\sqrt{1+\left(\frac{q}{r}\right)^2K^2}}}x(t) \end{eqnarray*}$

which means that the new time constant is shorter than the original time constant $T$ .

Exercise 1
Letting $a=-1$ , $b=1$ , $x(0)=1$ , consider the following cases:

Case#1: $\displaystyle{\frac{q}{r}=2}$
Case#2: $\displaystyle{\frac{q}{r}=4}$
Case#3: $\displaystyle{\frac{q}{r}=8}$

Then simulate the behaviors of $x(t)$ and $u(t)$ as follows.

Question:
Why do we use not only $J_x$ but also $J_u$ in the criterion function.
Answer:
Check that $J_u$ is downward convex, and takes the minimum value at $f=\frac{2b}{a}$ as follows:

(17) $\begin{eqnarray*} \frac{dJ_u}{df}&=&\frac{d}{df}\left(-\frac{r^2f^2}{2(a-bf)}x^2(0)\right)\nonumber\\ &=&\frac{-r^2x^2(0)}{2}\left( \frac{2f}{(a-bf)}+(-1)\frac{f^2}{(a-bf)^2}(-b)\right)\nonumber\\ &=&\frac{-r^2x^2(0)}{2}\frac{2(a-bf)f+f^2b}{(a-bf)^2}\nonumber\\ &=&\frac{-r^2x^2(0)}{2}\frac{(2a-bf)f}{(a-bf)^2}, \end{eqnarray*}$

(18) $\begin{eqnarray*} \frac{d^2J_u}{df^2}&=&\frac{-r^2x^2(0)}{2}\left(\frac{2a-2bf}{(a-bf)^2} +(-2)\frac{(2a-bf)f}{2(a-bf)^3}(-b)\right)\nonumber\\ &=&-r^2x^2(0)\left(\frac{1}{a-bf}+b\frac{2(a-bf)f+bf^2}{(a-bf)^3}\right)\nonumber\\ &=&-r^2x^2(0)\frac{(a-bf)^2+2(a-bf)bf+b^2f^2}{(a-bf)^3}\nonumber\\ &=&-r^2x^2(0)\frac{(a-bf+bf)^2}{2(a-bf)^3}\nonumber\\ &=&-r^2x^2(0)\frac{a^2}{2(a-bf)^3}>0. \end{eqnarray*}$

For example, letting $b=1,q=1,r=1$ , for $a=-1$ and $a=1$ , the overview of $J_x$ , $J_u$ , $J=J_x+J_u$ are drown as follows.

Here the symbol “o” shows the minimum of $J$ . Note that $J_u$ is necessary to make $J$ downward convex.

Appendix
In order to extend the above discussion to MIMO systems, we should be familiar with Lagrange’s method of undetermined multipliers. We will rewrite the above discussion by using this method as follows.

From (10), note that the constraint on $\Pi$ is given by the following Lyapnov’s equation

(19) $\begin{eqnarray*} {2(a-bf)\Pi+q^2+r^2f^2=0} \end{eqnarray*}$

Here $\Pi>0$ holds because of $a-bf<0$ . Therefore, instead of minimizing $\Pi$ , we will minimize

(20) $\begin{eqnarray*} J'=\Pi+\Gamma(2(a-bf)\Pi+q^2+r^2f^2). \end{eqnarray*}$

using undetermined multiplier $\Gamma$ and the stability constraint (19). As the necessary conditions, we have

(21) $\begin{eqnarray*} \frac{\partial J'}{\partial \Pi}=1+2(a-bf)\Gamma=0\Rightarrow\Gamma>0, \end{eqnarray*}$

(22) $\begin{eqnarray*} \frac{\partial J'}{\partial f}=(-2b\Pi+2r^2f)\Gamma=0\Rightarrow f=r^{-2}b\Pi, \end{eqnarray*}$

(23) $\begin{eqnarray*} \frac{\partial J'}{\partial \Gamma}=2(a-bf)\Pi+q^2+r^2f^2=0. \end{eqnarray*}$

Substituting $f=r^{-2}b\Pi$ into (23),

(24) $\begin{eqnarray*} 2(a-br^{-2}b\Pi)\Pi+q^2+r^2r^{-4}b^2\Pi^2=0 \end{eqnarray*}$

That is, we have the second-order equation on $\Pi$

(25) $\begin{eqnarray*} {2a\Pi-r^{-2}b^2\Pi^2+q^2=0} \end{eqnarray*}$

which is called as Ricatti equation. By solving this, $\Pi>0$ is obtained by

(26) $\begin{eqnarray*} \Pi=\frac{a+\sqrt{a^2+r^{-2}q^2b^2}}{r^{-2}b^2}, \end{eqnarray*}$

and $f$ is given by

(27) $\begin{eqnarray*} f=r^{-2}b\Pi=\frac{1}{b}\left(a+\sqrt{a^2+\left(\frac{q}{r}\right)^2b^2}\right). \end{eqnarray*}$

Lastly consider the following matrix:

(28) $\begin{eqnarray*} {M=\left[\begin{array}{cc} a & -r^{-2}b^2 \\ -q^2 & -a \end{array}\right]}. \end{eqnarray*}$

which is called as Hamilton matrix. The stable eigenvalue is given by

(29) $\begin{eqnarray*} \lambda=-\sqrt{a^2+r^{-2}b^2q^2} \end{eqnarray*}$

from

(30) $\begin{eqnarray*} {\rm det}(\lambda I_2-M)=\lambda^2-a^2-r^{-2}b^2q^2=0. \end{eqnarray*}$

The corresponding eigenvector is obtained as

(31) $\begin{eqnarray*} \left[\begin{array}{cc} v_1 \\ v_2 \end{array}\right] =\left[\begin{array}{cc} 1 \\ \frac{-a-\sqrt{a^2+r^{-2}b^2q^2}}{-r^{-2}b^2} \end{array}\right] \end{eqnarray*}$

Note that

(32) $\begin{eqnarray*} \Pi=v_2v_1^{-1}=\frac{a+\sqrt{a^2+r^{-2}q^2b^2}}{r^{-2}b^2} \end{eqnarray*}$

and

(33) $\begin{eqnarray*} f=r^{-2}b\Pi=\frac{a+\sqrt{a^2+r^{-2}b^2q^2}}{b}. \end{eqnarray*}$

PID Control

投稿日時: 2022年8月17日投稿者: cacsd

1. Speed Control under Disturbance

Consider the above situation we we are driving a car under a unknown constant wind disturbance. Then assume that we are required to keep a specified speed $v^*$ . In order to go forward against the wind disturbance, we will step down the acceleration pedal compared with the case of no wind disturbance. But usually we will not able to measure the disturbance force $w^*$ . How do we manipulate the driving force in an automatic way?

Let $m$ , $v(t)$ and $f(t)$ be the mass, the velocity and the driving force at time $t$ of the car respectively. Its motion is governed by the following differential equation:

(1) $\begin{eqnarray*} m\dot{v}(t)=f(t)+w^*. \end{eqnarray*}$

The usual speed control is given by

(2) $\begin{eqnarray*} f(t)=k_ve_v(t)\quad(e_v(t)=v^*-v(t)). \end{eqnarray*}$

In addition to this proportional control, we need an extra term $f'(t)$ to cancel the disturbance as follows:

(3) $\begin{eqnarray*} f(t)=k_ve_v(t)+f'(t)\quad(e_v(t)=v^*-v(t)). \end{eqnarray*}$

Then substituting (2) into (1), the closed-loop system is described by

(4) $\begin{eqnarray*} m\dot{v}(t)=k_ve_v(t)+f'(t)+w^*. \end{eqnarray*}$

Noting $\dot{e}_v(t)=-\dot{v}(t)$ , this can be rewritten as

(5) $\begin{eqnarray*} \dot{e}_v(t)=-\frac{k_v}{m}e_v(t)-\frac{1}{m}(f'(t)+w^*). \end{eqnarray*}$

In general, the solution of $\dot{x}(t)=ax(t)+bu(t)$ is derived as

(6) $\begin{eqnarray*} x(t)=e^{at}x(0)+\int_0^t e^{a(t-\tau)}bu(\tau)d\tau. \end{eqnarray*}$

Therefore the solution of (4) is given by

(7) $\begin{eqnarray*} e_v(t)=e^{-\frac{k_v}{m}t}e_v(0)+\int_0^t e^{-\frac{k_v}{m}(t-\tau)}\frac{-1}{m}(f'(t)+w^*)d\tau. \end{eqnarray*}$

Based on this expression, it is trivial that if $f'(t)=-w^*$ , $e_v(t)\rightarrow0$ $(t\rightarrow\infty)$ . But this strategy can’t be used because of unknown $w^*$ . We need some mechanism to estimate the the value of $w^*$ .

Consider the following integral action:

(8) $\begin{eqnarray*} \dot{x}_{I}(t)=e_v(t)\Leftrightarrow x_{I}(t)=\int_0^te_v(\tau)d\tau. \end{eqnarray*}$

Instead of (3), we will use the following proportional control with integral action (PI Control):

(9) $\begin{eqnarray*} \underline{f(t)=k_ve_v(t)+k_I\overbrace{\int_0^te_v(\tau)d\tau}^{x_{I}(t)}}. \end{eqnarray*}$

Substituting (9) into (1) and using $\dot{e}_v(t)=-\dot{v}(t)$ , the closed-loop system is described by

(10) $\begin{eqnarray*} \dot{e}_v(t)=-\frac{k_v}{m}e_v(t)-\frac{k_I}{m}x_I(t)-\frac{1}{m}w^*. \end{eqnarray*}$

Furthermore, combining with (8), we have

(11) $\begin{eqnarray*} \left[\begin{array}{c} \dot{e}_v(t) \\ \dot{x}_I(t) \end{array}\right] = \underbrace{ \left[\begin{array}{cc} -\frac{k_v}{m} & -\frac{k_I}{m}\\ 1 & 0 \\ \end{array}\right] }_{A} \left[\begin{array}{c} e_v(t) \\ x_I(t) \end{array}\right] + \left[\begin{array}{c} -\frac{1}{m}w^* \\ 0 \end{array}\right]. \end{eqnarray*}$

Therefore if $A$ is a stable matrix, when $t\rightarrow\infty$ , the following holds:

(12) $\begin{eqnarray*} &&\left[\begin{array}{c} e_v(t) \\ x_I(t) \end{array}\right] \rightarrow -\left[\begin{array}{cc} -\frac{k_v}{m} & -\frac{k_I}{m}\\ 1 & 0 \\ \end{array}\right]^{-1} \left[\begin{array}{c} -\frac{1}{m}w^* \\ 0 \end{array}\right]\\ &&=\frac{m}{k_I} \left[\begin{array}{cc} 0 & \frac{k_I}{m}\\ -1 & -\frac{k_v}{m} \\ \end{array}\right] \left[\begin{array}{c} \frac{1}{m}w^* \\ 0 \end{array}\right] =\left[\begin{array}{c} 0\\ -\frac{1}{k_I}w^* \end{array}\right]. \end{eqnarray*}$

Therefore

(13) $\begin{eqnarray*} \left\{\begin{array}{l} v(t)\rightarrow v^* \\ x_I(t)\rightarrow -\frac{1}{k_I}w^* \end{array}\right.\quad (t\rightarrow\infty). \end{eqnarray*}$

This means that $-k_Ix_I$ can estimate $w^*$ which is unknown.

The above control strategy is depicted as follows:

Exercise 1
Execute the following program under SCILAB. Determine appropriate the D gain $k_v$ (kv) and the I gain $k_I$ (kI), observing the corresponding constant distance $w^*$ estimated.
——————————————————————————————————————–
//cart3.sce
function dx=f(t,x),dx=A*x+w, endfunction
m=1; kv=3; kI=1; A=[-kv/m -kI/m;1 0];
ws=-1; w=[-1/m*ws;0]
v0=1; vs=0.5; x0=[vs-v0;0];
t0=0; t=0:0.1:20;
x=ode(x0,t0,t,f);
v=vs-x(1,:); d=-kI*x(2,:);
scf(1);
subplot(211), plot(t,v), mtlb_grid, title(‘v(t)’)
subplot(212), plot(t,d), mtlb_grid, title(‘w*’)
——————————————————————————————————————–

2. Position Control under Disturbance

Consider the above situation we we are driving a car under a unknown constant wind disturbance. Then assume that we are required to stop in front of the obstacle. How do we manipulate the driving force in an automatic way?

Let $m$ , $r(t)$ , and $f(t)$ be the mass, the position and the driving force at time $t$ of the car respectively. The velocity is $v(t)=\dot{r}(t)$ . Its motion is governed by the following differential equation:

(14) $\begin{eqnarray*} m\ddot{r}(t)=m\dot{v}(t)=f(t)+w^*. \end{eqnarray*}$

Consider the following integral action:

(15) $\begin{eqnarray*} \dot{x}_{I}(t)=e_r(t)\Leftrightarrow x_{I}(t)=\int_0^te_r(\tau)d\tau. \end{eqnarray*}$

The position control under disturbance should be given by the following proportional and derivative control with integral action (PID Control, PD-I Control):

(16) $\begin{eqnarray*} \underline{f(t)=k_re_r(t)+k_v\overbrace{\frac{d}{dt}e_r(t)}^{e_v(t)}+k_I\overbrace{\int_0^te_r(\tau)d\tau}^{x_I(t)}}, \end{eqnarray*}$

where $e_r(t)=r^*-r(t)$ , $e_v(t)=-v(t)$ . Substituting (16) into (14) and using $\dot{e}_v(t)=-\dot{v}(t)$ , the closed-loop system is described by

(17) $\begin{eqnarray*} \dot{e}_v(t)=-\frac{k_r}{m}e_r(t)-\frac{k_v}{m}e_v(t)-\frac{k_I}{m}x_I(t)-\frac{1}{m}w^*. \end{eqnarray*}$

Combining with $\dot{x}_I(t)=e_r(t)$ , we have

(18) $\begin{eqnarray*} \left[\begin{array}{c} \dot{e}_r(t) \\ \dot{e}_v(t) \\ \dot{x}_I(t) \end{array}\right] = \underbrace{ \left[\begin{array}{ccc} 0 & 1 & 0\\ -\frac{k_r}{m} & -\frac{k_v}{m} & -\frac{k_I}{m}\\ 1 & 0 & 0\\ \end{array}\right] }_{A} \left[\begin{array}{c} e_r(t) \\ e_v(t) \\ x_I(t) \end{array}\right] + \left[\begin{array}{c} 0\\ -\frac{1}{m}w^* \\ 0 \end{array}\right]. \end{eqnarray*}$

Therefore if $A$ is a stable matrix, when $t\rightarrow\infty$ , the following holds:

(19) $\begin{eqnarray*} &&\left[\begin{array}{c} e_r(t) \\ e_v(t) \\ x_I(t) \end{array}\right] \rightarrow -\left[\begin{array}{ccc} 0 & 1 & 0\\ -\frac{k_r}{m} & -\frac{k_v}{m} & -\frac{k_I}{m}\\ 1 & 0 & 0\\ \end{array}\right]^{-1} \left[\begin{array}{c} 0 \\ -\frac{1}{m}w^* \\ 0 \end{array}\right]\\ &&= \left[\begin{array}{ccc} 0 & 0 & 1\\ 1 & 0 & 0\\ -\frac{k_v}{k_I} & -\frac{m}{k_I} & -\frac{k_r}{k_I} \end{array}\right] \left[\begin{array}{c} 0 \\ \frac{1}{m}w^* \\ 0 \end{array}\right] =\left[\begin{array}{c} 0\\ 0\\ -\frac{1}{k_I}w^* \end{array}\right]. \end{eqnarray*}$

Therefore

(20) $\begin{eqnarray*} \left\{\begin{array}{l} r(t)\rightarrow r^* \\ v(t)\rightarrow 0 \\ x_I(t)\rightarrow -\frac{1}{k_I}w^* \end{array}\right.\quad (t\rightarrow\infty). \end{eqnarray*}$

This means that $-k_Ix_I$ can estimate $w^*$ which is unknown.

The above control strategy is depicted as follows:

This is equivalent to the following structure:

Exercise 2
Execute the following program under SCILAB. Determine appropriate PID gains $k_r,\ k_v,\ k_I$ (kr, kv, kI), observing the corresponding constant distance $w^*$ (ws) estimated.
——————————————————————————————————————–
//cart4.sce
function dx=f(t,x),dx=A*x+w,endfunction
m=1; kr=5; kv=5; kI=1; A=[0 1 0;-kr/m -kv/m -kI/m;1 0 0];
ws=-1; w=[0;-1/m*ws;0]
r0=-1; rs=0; v0=0; vs=0; x0=[rs-r0;vs-v0;0];
t0=0; t=0:0.1:20;
x=ode(x0,t0,t,f);
r=rs-x(1,:); d=-kI*x(3,:);
scf(1);
subplot(211), plot(t,r), mtlb_grid, title(‘r(t)’)
subplot(212), plot(t,d), mtlb_grid, title(‘w*’)
——————————————————————————————————————–

PID Control

投稿日時: 2022年8月17日投稿者: cacsd

1. Speed Control

We are driving a car with a constant speed. Consider the situation that we are necessary to reduce the speed to stop somewhere. How do we manipulate the driving force?

Let $m$ , $v(t)$ and $f(t)$ be the mass, the velocity and the driving force at time $t$ of the car respectively. Its motion is governed by the following differential equation:

(1) $\begin{eqnarray*} m\dot{v}(t)=f(t). \end{eqnarray*}$

The problem is how to manipulate the driving force $f(t)$ . Consider the following proportional control (P Control):

(2) $\begin{eqnarray*} \underline{f(t)=-k_vv(t)}. \end{eqnarray*}$

Substituting (2) into (1), we have the closed-loop system:

(3) $\begin{eqnarray*} \dot{v}(t)=-\frac{k_v}{m}v(t). \end{eqnarray*}$

That is, the driving force is manipulated to be proportional to the velocity with negative sign. This means that for a high speed the large regulating force is produced in the reverse direction, and for a low speed the small regulating force is applied. This control strategy is called as feedback mechanism and depicted as follows:

In general, the solution of a differential equation $\dot{x}(t)=ax(t)$ is given by

(4) $\begin{eqnarray*} x(t)=e^{at}x(0). \end{eqnarray*}$

Therefore the solution of (3) is given by

(5) $\begin{eqnarray*} v(t)=e^{-\frac{k_v}{m}t}v(0). \end{eqnarray*}$

This means

(6) $\begin{eqnarray*} v(t)\rightarrow 0\quad (t\rightarrow\infty). \end{eqnarray*}$

That is, we can reduce the car speed gradually and stop it.

By the way, assume that we are required to change the current speed to the new speed $v^*$ . How do we manipulate the driving force for the speed up or speed down?

Let the velocity error from the current velocity $v(t)$ to the required constant speed $v^*$ be

(7) $\begin{eqnarray*} e_v(t)=v^*-v(t). \end{eqnarray*}$

Then consider the following proportional control:

(8) $\begin{eqnarray*} \underline{f(t)=k_ve_v(t)}, \end{eqnarray*}$

i.e.

(9) $\begin{eqnarray*} f(t)=-k_vv(t)+k_vv^*. \end{eqnarray*}$

Substituting (8) into (1), we have

(10) $\begin{eqnarray*} m\dot{v}(t)=k_ve_v(t). \end{eqnarray*}$

Noting $\dot{e}_v(t)=-\dot{v}(t)$ , this can be rewritten as

(11) $\begin{eqnarray*} \dot{e}_v(t)=-\frac{k_v}{m}e_v(t). \end{eqnarray*}$

Therefore, we have

(12) $\begin{eqnarray*} e_v(t)=e^{-\frac{k_v}{m}t}e_v(0)\ \rightarrow 0\quad (t\rightarrow\infty). \end{eqnarray*}$

This means

(13) $\begin{eqnarray*} v(t)\rightarrow v^*\quad (t\rightarrow\infty). \end{eqnarray*}$

The above control strategy is depicted as follows:

Exercise 1
Execute the following program under SCILAB. By changing the P gain $k_v$ (kv) and the target speed $v^*$ (vs), investigate how the velocity $v(t)$ approaches to $v^*$ .
——————————————————————————————————————–
//cart1.sce
function dx=f(t,x),dx=A*x,endfunction
m=1; kv=1; A=-kv/m;
v0=1; vs=0.5; x0=vs-v0;
t0=0; t=0:0.1:10;
x=ode(x0,t0,t,f);
v=vs-x;
scf(1);
plot(t,v), mtlb_grid, title(‘v(t)’)
——————————————————————————————————————–

2. Position Control
Under the speed control described in the above, we can’t specify the stop position. For example, in the case of encountering an obstacle at the distance $r^*$ from the current position as shown in the following, we will be required to stop in front of the obstacle. How do we manipulate the driving force in order to stop within the running distance of $r^*$ ?

(14) $\begin{eqnarray*} m\ddot{r}(t)=m\dot{v}(t)=f(t). \end{eqnarray*}$

Let the position error from the current position $r(t)$ to the required target position $r^*$ be

(15) $\begin{eqnarray*} e_r(t)=r^*-r(t). \end{eqnarray*}$

Then consider the following proportional and derivative control (PD Control):

(16) $\begin{eqnarray*} \underline{f(t)=k_re_r(t)+k_v\overbrace{\frac{d}{dt}e_r(t)}^{e_v(t)}}, \end{eqnarray*}$

i.e.

(17) $\begin{eqnarray*} f(t)=-k_rr(t)-k_vv(t)+k_rr^*. \end{eqnarray*}$

Here $e_v(t)$ is the speed error given by (7), in which $v^*$ must be 0. Note $e_v(t)=-v(t)$ .

Substituting (16) into (1), we have

(18) $\begin{eqnarray*} m\dot{v}(t)=k_re_r(t)+k_ve_v(t). \end{eqnarray*}$

Noting $\dot{e}_v(t)=-\dot{v}(t)$ , this can be rewritten as

(19) $\begin{eqnarray*} \dot{e}_v(t)=-\frac{k_r}{m}e_r(t)-\frac{k_v}{m}e_v(t). \end{eqnarray*}$

Furthermore, taking account of $\dot{e}_r(t)=e_v(t)$ , we have

(20) $\begin{eqnarray*} \underbrace{ \left[\begin{array}{c} \dot{e}_r(t) \\ \dot{e}_v(t) \end{array}\right] }_{\dot{e}(t)} = \underbrace{ \left[\begin{array}{cc} 0 & 1 \\ -\frac{k_r}{m} & -\frac{k_v}{m} \end{array}\right] }_{A} \underbrace{ \left[\begin{array}{c} e_r(t) \\ e_v(t) \end{array}\right] }_{e(t)}. \end{eqnarray*}$

As shown here, the solution is given by

(21) $\begin{eqnarray*} e(t)=\exp(At)e(0). \end{eqnarray*}$

As $A$ is a stable matrix, the following holds.

(22) $\begin{eqnarray*} \left\{\begin{array}{l} e_r(t)\rightarrow 0 \\ e_v(t)\rightarrow 0 \end{array}\right.\quad (t\rightarrow\infty). \end{eqnarray*}$

This means

(23) $\begin{eqnarray*} \left\{\begin{array}{l} r(t)\rightarrow r^* \\ v(t)\rightarrow 0 \end{array}\right.\quad (t\rightarrow\infty). \end{eqnarray*}$

The above control strategy is depicted as follows:

Based on (17), this is equivalent to the following structure:

Exercise 2
Execute the following program under SCILAB. Determine the D gain $k_v$ (kv) for collision avoidance, such that no undershoot occurs in the position $r(t)$ .
——————————————————————————————————————–
//cart2.sce
function dx=f(t,x),dx=A*x,endfunction
m=1; kr=1; kv=2; A=[0 1;-kr/m -kv/m];
r0=-1; rs=0; v0=0; vs=0; x0=[rs-r0;vs-v0];
t0=0; t=0:0.1:10;
x=ode(x0,t0,t,f);
v=vs-x(2,:); r=rs-x(1,:);
scf(1);
subplot(211), plot(t,v), mtlb_grid, title(‘v(t)’)
subplot(212), plot(t,r), mtlb_grid, title(‘r(t)’)
——————————————————————————————————————–

Question:
Why the control law (2) is called as not D control but P control?
Answer:
In the situation of the speed control, we are assumed to measure the velocity, that is, we don’t have to differentiate the position. So we need to feed back the value proportional to the measured velocity. On the other hand, in the situation of the position control, we are assumed to measure the position, that is, we have to differentiate the position to get the velocity.

基礎事項

投稿日時: 2022年8月17日投稿者: cacsd

[0] 表計算ソフトEXCELの基礎事項を、初歩的な統計計算を通して説明します。

[1] 次の集計表を作成しましょう。

各列の集計は、まず、セルA4に「=sum(A1:A3)」を入れ、これをセルB4,C4に、Copy & Paste （以下、C&P）します。このとき、セルB4,C4はそれぞれ、「=sum(B1:B3)」、「=sum(C1:C3)」
となっており、和をとる列が自動的に変更されていることに注意してください。同様に、各行の集計は、まず、セルD1に「=sum(A1:C1)」を入れ、これをセルD2,D3,D4にC&Pします。表計算の利点はデータ更新時の再計算にあります。適当に、データを変更して、再計算の便利さを確かめてみましょう。

[2] 次の成績データの偏差値を計算してください。

{(i,x_i)|i=1,…,10}={(1,89),(2,70),(3,63),(4,100),(5,81),(6,30),(7,56),(8,77),(9,90),(10,63)}

まず、留意すべきは、列Aの入力方法です。セルA2に初期値「1」を入力し、セルA3に「=A2+1」を入力し、これを領域A4:A11にC&Pします。

つぎに、和、個数、平均値、分散、標準偏差をそれぞれ次のように入力して求めておきます。

セルB12　⇒　=sum(B2:B11)
セルB13　⇒　=count(B2:B11)
セルB14　⇒　=average(B2:B11)
セルB15　⇒　=varp(B2:B11)
セルB16　⇒　=stdevp(B2:B11)

ここで、平均値と分散は、次式で与えられるのでした。

$\displaystyle{m_x=\frac{1}{n}\sum_{i=1}^{n}x_i}$
$\displaystyle{\sigma_x^2=\frac{1}{n}\sum_{i=1}^{n}(x_i-m)^2}$

偏差値とは，平均値50，標準偏差10となるように線形変換した値のことで、次式で計算されます。

$\displaystyle{y_i=10\frac{x_i-m_x}{\sigma_x}+50}$

列C,列D,列Eの作成法は、次の通りです。セルC2,D2,D2に、次のように入力します。

セルC2　⇒　=B2-$B$14
セルD2　⇒　=C2^2
セルE2　⇒　=10*C2/$B$16+50

ここで、セルB14とB16の参照は、それぞれ絶対アドレス$B$14と$B$16を用いることに留意してください。これらを下方にC&Pします。ちなみに、分布の形を変えないで60点以上が合格となるように、新しい平均値，標準偏差の値を調整して、ゲタをはかせることができます。

参考までに、平均値と分散の漸化式（オンライン計算式）を与えておきます。

$\begin{array}{lll} m_n&=&\frac{1}{n}\sum_{i=1}^{n}x_i \nonumber\\ &=&\frac{n-1}{n}\left(\frac{1}{n-1}\sum_{i=1}^{n-1}x_i\right)+\frac{1}{n}x_n \nonumber\\ &=&\frac{n-1}{n}m_{n-1}+\frac{1}{n}x_n\nonumber \end{array}$

$\begin{array}{lll} \sigma_n^2&=&\frac{1}{n}\sum_{i=1}^{n}(x_i-m_n)^2\nonumber\\ &=&\frac{1}{n}\sum_{i=1}^{n}x_i^2-m_n^2\nonumber\\ &=&\frac{n-1}{n}\left(\frac{1}{n-1}\sum_{i=1}^{n-1}x_i^2\right)+\frac{1}{n}x_n^2 -\left(\frac{n-1}{n}m_{n-1}+\frac{1}{n}x_n\right)^2\nonumber\\ &=&\frac{n-1}{n}(\sigma_{n-1}^2+m_{n-1}^2)+\frac{1}{n}x_n^2- \left(\frac{(n-1)^2}{n^2}m_{n-1}^2+2\frac{n-1}{n^2}m_{n-1}x_n+\frac{1}{n^2}x_n^2\right)\nonumber\\ &=&\frac{n-1}{n}\sigma_{n-1}^2+\frac{n-1}{n^2}m_{n-1}^2 -2\frac{n-1}{n^2}m_{n-1}x_n+\frac{n-1}{n^2}x_n^2\nonumber\\ &=&\frac{n-1}{n}\sigma_{n-1}^2+\frac{n-1}{n^2}(x_n-m_{n-1})^2\nonumber \end{array}$

それでは、与えられたデータの棒グラフを描きましょう。「挿入」、「グラフ」と進んで、「縦棒」、「2D-縦棒」を選びます。

[4] 次の計測データの回帰直線を計算してください。

{(x_i,y_i)|i=1,…,6}={(1,5.7),(3,10.4),(4,11.1),(6,19.5),(7,21.8),(10,26.2)}

ここで、行列（領域C10:D11）の逆行列を領域G10:H11に計算しています。そのためには、

セルG10　⇒　=MINVERSE(C10:D11)

を入力し、領域C10:D11を選択し、窓fxにカーソルを合わせ、”[Ctrl] + [Shift] + [Enter]” を入力します。

次に、この逆行列とベクトル（領域C12:C13）の掛け算を領域C15:C16に計算しています。そのためには、

セルC15　⇒　=MMULT(G10:H11,C12:C13)

を入力し、領域C15:C16を選択し、窓fxにカーソルを合わせ、”[Ctrl] + [Shift] + [Enter]” を入力します。

それでは、計算式の詳細を説明します。回帰直線 $ax+b$ は、次の最小化問題を解いて得られます。

$\displaystyle{\min_{a,b} \frac{1}{n}\sum_{i=1}^{n}(y_i-ax_i-b)^2}$

この解は、次式で与えられます。

$\displaystyle{a=\frac{1}{d}(c_1q_2-c_2q_3)},\quad \displaystyle{b=\frac{1}{d}(-c_1q_3+c_2q_1)}$

ただし、 $d=q_1q_2-q_3^2$ 、また

$\displaystyle{q_1=\frac{1}{n}\sum_{i=1}^{n}x_i^2},\quad \displaystyle{q_2=1},\quad \displaystyle{q_3=\frac{1}{n}\sum_{i=1}^{n}x_i}$
$\displaystyle{c_1=\frac{1}{n}\sum_{i=1}^{n}x_iy_i},\quad \displaystyle{c_2=\frac{1}{n}\sum_{i=1}^{n}y_i}$

実際、目的関数を

$\displaystyle{J(a,b)=\frac{1}{n}\sum_{i=1}^{n}(y_i-ax_i-b)^2}$

とおくと、これが極値をとる条件は

$\displaystyle{\left\{\begin{array}{l} \frac{\partial J}{\partial a}=\frac{2}{n}\sum_{i=1}^{n}(y_i-ax_i-b)(-x_i)=0\\ \frac{\partial J}{\partial b}=\frac{2}{n}\sum_{i=1}^{n}(y_i-ax_i-b)(-1)=0 \end{array}\right.}$

となります。これは、上記の記号を用いて

$\displaystyle{\left\{\begin{array}{l} -c_1+q_1a+q_3b=0\\ -c_2+q_3a+q_2b=0 \end{array}\right.}$

のように書けます。これを行列表示すると

$\left[\begin{array}{cc} q_1 & q_3 \\ q_3 & q_2 \end{array}\right] \left[\begin{array}{cc} a \\ b \end{array}\right] = \left[\begin{array}{cc} c_1 \\ c_2 \end{array}\right]$

となって、aとbは、次式で与えられるからです。

$\left[\begin{array}{cc} a \\ b \end{array}\right] = \frac{1}{d} \left[\begin{array}{cc} q_2 & -q_3 \\ -q_3 & q_1 \end{array}\right] \left[\begin{array}{cc} c_1 \\ c_2 \end{array}\right]$

ちなみに、SCILABでは、次式で計算されます。

–> A=[[1 3 4 6 7 10]’ ones(6,1)];
–> b=[5.7 10.4 11.1 19.5 21.6 26.2]’;
–> A\b

それでは、与えられたデータの折れ線グラフを描きましょう。「挿入」、「グラフ」と進んで、そのグラフをと使えばよいでしょうか？実は「散布図」を選びます。グラフの背景はデフォールトが灰色の場合、白色に変更しましょう。

台車駆動型倒立振子の特性解析

投稿日時: 2022年8月17日投稿者: cacsd

[1] 台車駆動型倒立振子CIP

上図のように、倒立振子を軸支した台車を、傾斜角 $\alpha$ を持つレール上に載せた状況を考えます。このとき、まず運動方程式を導出し，これを平衡状態まわりで線形化し、状態方程式と出力方程式からなる状態空間表現を得ましょう。

以下では、倒立振子となる棒は長さ $2\ell$ 、質量 $m$ の一様な剛体で、台車の質量は $M$ とします。棒の鉛直線からの傾きを $\theta$ 、台車のレールに沿う変位を $r$ 、台車を駆動する力 $F$ とします。また，簡単のため、軸は、棒の端（棒の重心から $-\ell$ の位置）に取り付けられ、台車の重心と一致し，駆動力の作用点でもあるとします。なお、この制御対象に対して、計測可能な物理変数は、 $r$ と $\theta$ であるとします。

まず、軸の位置のx座標を $x_0$ 、y座標を $y_0$ とすると

$x_0=r\cos\alpha$
$y_0=r\sin\alpha$

となり、台車の運動エネルギー $T_0$ と位置エネルギー $V_0$ は

$T_0=\frac{1}{2}M(\dot{x}^2_0+\dot{y}^2_0)=\frac{1}{2}M\dot{r}^2_0$
$V_0=Mgy_0$

のように表されます。また、棒の重心位置のx座標を $x_1$ 、y座標を $y_1$ とすると

$x_1=x_0+\ell\sin\theta$
$y_1=y_0+\ell\cos\theta$

となり、棒の運動エネルギー $T_1$ と位置エネルギー $V_1$ は

$T_1=\frac{1}{2}m(\dot{x}^2_1+\dot{y}^2_1)+\frac{1}{2}J_1\dot{\theta}^2_0$
$V_1=mgy_1$

のように表されます。ここで、 $J_1$ は重心周りの慣性モーメントを表し

$J_1=\frac{1}{3}m\ell^2$

です。以上の準備の下で、ラグランジュの運動方程式は，ラグランジアンを

$L=T_0+T_1-V_0-V_1$

として

$\frac{d}{dt}(\frac{\partial L}{\partial \dot{r}})-\frac{\partial L}{\partial r}=F$
$\frac{d}{dt}(\frac{\partial L}{\partial \dot{\theta}})-\frac{\partial L}{\partial \theta}=0$

のように与えられます。

それでは、MAXIMAを用いて、制御対象の運動方程式を導出してみましょう。

/*cip.wxm*/
dr:’diff(r(t),t); ddr:’diff(r(t),t,2);
dth:’diff(th(t),t); ddth:’diff(th(t),t,2);
x0:r(t)*cos(alpha); y0:r(t)*sin(alpha);
T0:(1/2)*M*(diff(x0,t)^2+diff(y0,t)^2);
V0:M*g*y0;
x1:x0+ell*sin(th(t)); y1:y0+ell*cos(th(t));
J1:(1/3)*m*ell^2;
T1:(1/2)*m*(diff(x1,t)^2+diff(y1,t)^2)+(1/2)*J1*dth^2;
V1:m*g*y1;
L:T0+T1-V0-V1;
LE1:diff(diff(L,dr),t)-diff(L,r(t))=F,trigreduce,ratsimp;
LE2:diff(diff(L,dth),t)-diff(L,th(t))=0,trigreduce,ratsimp;

これを実行すると、LE1として、次の結果を得ます。

また、LE2として、次の結果を得ます。

これらを行列表示して、制御対象の運動方程式として

$\displaystyle{ \left[\begin{array}{cc} M+m & m\ell\cos(\theta+\alpha) \\ m\ell\cos(\theta+\alpha) & \frac{4}{3}m\ell^2 \end{array}\right] \left[\begin{array}{c} \ddot{r} \\ \ddot{\theta} \end{array}\right] + \left[\begin{array}{cc} 0 & -m\ell\sin(\theta+\alpha) \\ 0 & 0 \end{array}\right] \left[\begin{array}{c} \dot{r} \\ \dot{\theta} \end{array}\right] + \left[\begin{array}{c} (M+m)g\sin\alpha\\ -m\ell g\sin\theta \end{array}\right] = \left[\begin{array}{c} 1 \\ 0 \end{array}\right] F }$

を得ます。これを行列表示すると

$M(\xi_1)\dot{\xi}_2+C(\xi_1){\xi}_2+G(\xi_1)=e_1\zeta$

ただし

$\xi_1= \left[\begin{array}{c} {r} \\ {\theta} \end{array}\right],\ \xi_2= \left[\begin{array}{c} \dot{r} \\ \dot{\theta} \end{array}\right],\ \zeta=F$

$\displaystyle{ M(\xi_1)= \left[\begin{array}{cc} M+m & m\ell\cos(\theta+\alpha) \\ m\ell\cos(\theta+\alpha) & \frac{4}{3}m\ell^2 \end{array}\right],\ C(\xi_1)= \left[\begin{array}{cc} 0 & -m\ell\sin(\theta+\alpha) \\ 0 & 0 \end{array}\right],\ e_1= \left[\begin{array}{c} 1 \\ 0 \end{array}\right] }$

となります。これから、制御対象の非線形状態方程式

$\left[\begin{array}{c} \dot{\xi}_1 \\ \dot{\xi}_2 \end{array}\right] = \left[\begin{array}{c} {\xi}_2 \\ M^{-1}(\xi_1)(e_1\zeta-C(\xi_1){\xi}_2-G(\xi_1)) \end{array}\right]$

すなわち、状態変数ベクトルと入力変数を

$\xi= \left[\begin{array}{c} \xi_1 \\ \xi_2 \end{array}\right] = \left[\begin{array}{c} r \\ \theta \\ \dot{r} \\ \dot{\theta} \end{array}\right],\ \zeta=F$

と定義するとき、制御対象の非線形状態方程式

$\dot{\xi}=f(\xi,\zeta)$

ただし

$\displaystyle{ f(\xi,\zeta)= \left[\begin{array}{c} f_1(\xi,\zeta) \\ f_2(\xi,\zeta) \\ f_3(\xi,\zeta) \\ f_4(\xi,\zeta) \end{array}\right]= \left[\begin{array}{c} f_1(r,\theta,\dot{r},\dot{\theta},F) \\ f_2(r,\theta,\dot{r},\dot{\theta},F) \\ f_3(r,\theta,\dot{r},\dot{\theta},F) \\ f_4(r,\theta,\dot{r},\dot{\theta},F) \end{array}\right]= \left[\begin{array}{c} {\xi}_2 \\ M^{-1}(\xi_1)(e_1\zeta-C(\xi_1){\xi}_2-G(\xi_1)) \end{array}\right] }$

を得ます。

さて、制御対象の平衡状態 $\xi^*$ と、これを実現する平衡入力 $\zeta^*$ を考えます。これらは、運動方程式において、加速度＝０（ $\dot{\xi}_2=0$ ）とおいた、

$C(\xi_1)\dot{\xi}_1+G(\xi_1)=e_1\zeta$

すなわち

$\displaystyle{ \left[\begin{array}{cc} 0 & -m\ell\sin(\theta+\alpha) \\ 0 & 0 \end{array}\right] \left[\begin{array}{c} \dot{r} \\ \dot{\theta} \end{array}\right] + \left[\begin{array}{c} (M+m)g\sin\alpha\\ -m\ell g\sin\theta \end{array}\right] = \left[\begin{array}{c} 1 \\ 0 \end{array}\right] F }$

を解いて得られます。これより、平衡状態 $\xi^*$ と、これを実現する平衡入力 $\zeta^*$ として

$\xi^*= \left[\begin{array}{c} \xi^*_1 \\ \xi^*_2 \end{array}\right]= \left[\begin{array}{c} r^* \\ \theta^* \\ \dot{r}^* \\ \dot{\theta}^* \end{array}\right]= \left[\begin{array}{c} 0 \\ 0 \\ 0 \\ 0 \end{array}\right]\ or\ \left[\begin{array}{c} 0 \\ \pi \\ 0 \\ 0 \end{array}\right]$
$\zeta^*=F^*=(M+m)g\sin\alpha$

を得ます。このとき、明らかに、次式が満足されています。

$\dot{\xi}^*=f(\xi^*,\zeta^*)= \left[\begin{array}{c} {\xi}^*_2 \\ M^{-1}(\xi^*_1)(e_1\zeta^*-C(\xi^*_1){\xi^*}_2-G(\xi^*_1)) \end{array}\right]= \left[\begin{array}{c} 0 \\ 0 \end{array}\right]$

この平衡状態（と平衡入力）まわりで、非線形状態方程式を、線形化（１次近似）します。

$\dot{\xi}=f(\xi,\zeta)\simeq f(\xi^*,\zeta^*) +\frac{\partial f(\xi^*,\zeta^*)}{\partial\xi}(\xi-\xi^*) +\frac{\partial f(\xi^*,\zeta^*)}{\partial\zeta}(\zeta-\zeta^*)$

ただし

$\displaystyle{ \frac{\partial f(\xi^*,\zeta^*)}{\partial\xi} =\left.\left[\begin{array}{cccc} \frac{\partial f_1}{\partial r} & \frac{\partial f_1}{\partial\theta} &\frac{\partial f_1}{\partial\dot{r}} & \frac{\partial f_1}{\partial\dot{\theta}} \\ \frac{\partial f_2}{\partial r} & \frac{\partial f_2}{\partial\theta} &\frac{\partial f_2}{\partial\dot{r}} & \frac{\partial f_2}{\partial\dot{\theta}} \\ \frac{\partial f_3}{\partial r} & \frac{\partial f_3}{\partial\theta} &\frac{\partial f_3}{\partial\dot{r}} & \frac{\partial f_3}{\partial\dot{\theta}} \\ \frac{\partial f_4}{\partial r} & \frac{\partial f_4}{\partial\theta} &\frac{\partial f_4}{\partial\dot{r}} & \frac{\partial f_4}{\partial\dot{\theta}} \end{array}\right] \right|_{r=0,\theta=\theta^*,\dot{r}=0,\dot{\theta}=0,F=F^*}$
$\displaystyle{ \frac{\partial f(\xi^*,\zeta^*)}{\partial\zeta} =\left.\left[\begin{array}{cccc} \frac{\partial f_1}{\partial F} \\ \frac{\partial f_2}{\partial F} \\ \frac{\partial f_3}{\partial F} \\ \frac{\partial f_4}{\partial F} \end{array}\right] \right|_{r=0,\theta=\theta^*,\dot{r}=0,\dot{\theta}=0,F=F^*}$

これから、 $f(\xi^*,\zeta^*)=0$ に注意して、制御対象の線形状態方程式

$\underbrace{\dot{\xi}-\dot{\xi}^*}_{\dot{x}}= \underbrace{\frac{\partial f(\xi^*,\zeta^*)}{\partial\xi}}_A \underbrace{(\xi-\xi^*)}_{x} + \underbrace{\frac{\partial f(\xi^*,\zeta^*)}{\partial\zeta}}_B \underbrace{(\zeta-\zeta^*)}_{u}$

すなわち

$\underbrace{ \frac{d}{dt} \left[\begin{array}{c} r-r^* \\ \theta-\theta^* \\ \dot{r}-\dot{r}^* \\ \dot{\theta}-\dot{\theta}^* \end{array}\right] }_{\dot{x}}= \underbrace{ \left[\begin{array}{cccc} 0 & 0 & 1 & 0\\ 0 & 0 & 0 & 1\\ 0 & a_{32} & 0 & 0\\ 0 & a_{42} & 0 & 0 \end{array}\right] }_{A} \underbrace{ \left[\begin{array}{c} r-r^* \\ \theta-\theta^* \\ \dot{r}-\dot{r}^* \\ \dot{\theta}-\dot{\theta}^* \end{array}\right] }_{x} +\underbrace{ \left[\begin{array}{c} 0 \\ 0 \\ b_{32} \\ b_{42} \end{array}\right] }_{B} \underbrace{ (\dot{\zeta}-\dot{\zeta}^*) }_{u}$

を得ます。

以上の計算をMAXIMAを用いて実行するには、次のプログラムを作成します。

/*cip.wxm*/
:
sol:solve([LE1,LE2],[ddr,ddth]);
f:matrix([dr],[dth],[rhs(sol[1][1])],[rhs(sol[1][2])]);
a31:diff(f[3,1],r(t)); a32:diff(f[3,1],th(t));
a33:diff(f[3,1],dr); a34:diff(f[3,1],dth);
a41:diff(f[4,1],r(t)); a42:diff(f[4,1],th(t));
a43:diff(f[4,1],dr); a44:diff(f[4,1],dth);
b3:diff(f[3,1],F); b4:diff(f[4,1],F);
Fs:(M+m)*g*sin(alpha);
A:matrix([0,0,1,0],[0,0,0,1],[a31,a32,a33,a34],[a41,a42,a43,a44]);
A1:A,th(t)=0,F=Fs,trigreduce,ratsimp;
A2:A,th(t)=%pi,F=Fs,trigreduce,ratsimp;
B:matrix([0],[0],[b3],[b4]);
B1:B,th(t)=0,F=Fs,trigreduce,ratsimp;
B2:B,th(t)=%pi,F=Fs,trigreduce,ratsimp;

ここで、 $\theta^*=0$ に対応する平衡状態周りの線形状態方程式のA行列がA1、B行列がB1で、それぞれ次のような結果となります。

また、 $\theta^*=\pi$ に対応するA行列がA2、B行列がB2で、それぞれ次のような結果となります。

計算工学演習第一レポート（１）

振り子の特性解析

投稿日時: 2022年8月17日投稿者: cacsd

[0]　MAXIMAを用いて、倒立振子の運動方程式を導出し、非線形状態方程式を求め、これを平衡状態まわりで線形化（１次近似）し、線形状態方程式を求めます。

そのために必要となるのが、ベクトル値多変数関数

$\displaystyle{y=f(x)\ (x\in{\rm\bf R}^n,y\in{\rm\bf R}^m)}$

の $x=a(\in{\rm\bf R}^n)$ まわりの１次近似式

$\displaystyle{f(x)\simeq f(a)+\frac{\partial f(a)}{\partial x}(x-a)$

です。これは、たとえば、 $n=m=2$ のとき、次のように計算されます。

$\begin{array}{lll} \underbrace{ \left[\begin{array}{cc} f_1(x_1,x_2)\\ f_2(x_1,x_2) \end{array}\right] }_{f(x)}\nonumber\\ \simeq \left[\begin{array}{cc} f_1(a_1,a_2)+\frac{\partial\,f_1(a_1,a_2)}{\partial\,x_1}(x_1-a_1)+\frac{\partial\,f_1(a_1,a_2)}{\partial\,x_2}(x_2-a_2)\\ f_2(a_1,a_2)+\frac{\partial\,f_2(a_1,a_2)}{\partial\,x_1}(x_1-a_1)+\frac{\partial\,f_2(a_1,a_2)}{\partial\,x_2}(x_2-a_2) \end{array}\right]\nonumber\\ = \underbrace{ \left[\begin{array}{cc} f_1(a_1,a_2)\\ f_2(a_1,a_2) \end{array}\right] }_{f(a)} + \underbrace{ \left[\begin{array}{cc} \frac{\partial f_1(a_1,a_2)}{\partial x_1}&\frac{\partial f_1(a_1,a_2)}{\partial x_2}\\ \frac{\partial f_2(a_1,a_2)}{\partial x_1}&\frac{\partial f_2(a_1,a_2)}{\partial x_2} \end{array}\right] }_{\frac{\partial\,f(a)}{\partial\,x}} \underbrace{ \left[\begin{array}{cc} x_1-a_1\\ x_2-a_2\nonumber \end{array}\right] }_{x-a}\nonumber \end{array}$

[1] 振り子PEN

上図の振り子PENを見てください。長さ、 $L=2\ell$ の振り子の重心周りの慣性モーメントは

$\displaystyle{J_0=\int_{-\ell}^{\ell}r^2\,dm=\int_{-\ell}^{\ell}r^2\,\frac{m}{2\ell}dr=\frac{m}{2\ell}\left[\frac{r^3}{3}\right]_{-\ell}^{\ell}=\frac{1}{3}m\ell^2}$

のように計算できます。これから，振り子の軸まわりの慣性モーメントは、平行軸の定理を使って

$\displaystyle{J=\frac{1}{3}m\ell^2+m\ell^2=\frac{4}{3}m\ell^2}$

のように与えられます。また、軸まわりのトルクは

$\displaystyle{\tau=mg\ell\sin\theta}$

のように表わされます。したがって、振り子の運動方程式は、次式となります。

$\displaystyle{J\ddot{\theta}=\tau\ \Rightarrow\ \ddot{\theta}=\frac{3g}{4\ell}\sin\theta}$

それでは、この運動方程式を、ラグランジュの方法で導出してみます。その手順は次のようになります。

まず、振り子の重心のx、y座標と、軸周りの慣性モーメントは

$x=\ell\sin\theta$
$y=\ell\cos\theta$
$J_0=\frac{1}{3}m\ell^2$

ですから、運動エネルギーと位置エネルギーは

$T=\frac{1}{2}m(\dot{x}^2+\dot{y}^2)+\frac{1}{2}J_0\dot{\theta}^2$
$V=mg\ell\cos\theta$

となり、ラクランジアンを

$L=T-V$

として、ラグランジュの運動方程式は

$\frac{d}{dt}\left(\frac{\partial L}{\partial \dot{\theta}}\right)-\frac{\partial L}{\partial\theta}=0$

となります。これから振り子の運動方程式が得られます。

それでは、MAXIMAを用いて、振り子の運動方程式を導出してみましょう。

/*pen.wxm*/
dth:’diff(th(t),t);
ddth:’diff(th(t),t,2);
x:ell*sin(th(t));
y:ell*cos(th(t));
J:(1/3)*m*ell^2;
T:(1/2)*m*(diff(x,t)^2+diff(y,t)^2)+(1/2)*J*dth^2;
V:m*g*ell*cos(th(t));
L:T-V;
LE:diff(diff(L,dth),t)-diff(L,th(t))=0,trigreduce;
sol:solve(LE,ddth);

次に、２階微分を含む運動方程式を１階微分だけの連立微分方程式に変換し、これをベクトル表示します。

$\displaystyle{\ddot{\theta}=\frac{3g}{4\ell}\sin\theta \quad\Leftrightarrow \left\{\begin{array}{l} \dot{\theta}=\omega\\ \dot{\omega}=\frac{3g}{4\ell}\sin\theta \end{array}\right. \quad\Leftrightarrow \left[\begin{array}{c} \dot{\theta} \\ \dot{\omega} \end{array}\right] = \left[\begin{array}{c} \omega \\ \frac{3g}{4\ell}\sin\theta \end{array}\right]}$

すなわち、状態変数ベクトル

$\xi= \left[\begin{array}{c} {\theta} \\ {\omega} \end{array}\right]$

を定義するとき、振り子の非線形状態方程式

$\dot\xi=f(\xi)$

ただし

$f(\xi)= \left[\begin{array}{c} f_1(\xi) \\ f_2(\xi) \end{array}\right] = \left[\begin{array}{c} f_1(\theta,\omega) \\ f_2(\theta,\omega) \end{array}\right] = \left[\begin{array}{c} \omega \\ \frac{3g}{4\ell}\sin\theta \end{array}\right]$

が得られました。ところで、振り子は２つの平衡状態（物理的な釣り合いの状態）を持ちます。

これらは次のように特徴付けられます。

$\dot\xi^*=f(\xi^*)=0$

すなわち

$\underbrace{ \left[\begin{array}{c} \dot{\theta}^* \\ \dot{\omega}^* \end{array}\right] }_{\dot{\xi}^*} = \underbrace{ \left[\begin{array}{c} \omega^* \\ \frac{3g}{4\ell}\sin\theta^* \end{array}\right] }_{f(\xi^*)} = \underbrace{ \left[\begin{array}{c} 0 \\ 0 \end{array}\right] }_{0} \ \Rightarrow\ \xi^* = \left[\begin{array}{c} \theta^* \\ \omega^* \end{array}\right] = \left[\begin{array}{c} 0 \\ 0 \end{array}\right] \,or\, \left[\begin{array}{c} \pi \\ 0 \end{array}\right]$

そこで、振り子の非線形状態方程式を、平衡状態まわりで線形化（１次近似）します。

$\dot{\xi}=f(\xi)\simeq \underbrace{f(\xi^*)}_{\dot{\xi}^*=0} +\frac{\partial f(\xi^*)}{\partial\xi} (\xi-\xi^*)$

ただし

$\displaystyle{ \frac{\partial f(\xi^*)}{\partial\xi}=\left.\left[\begin{array}{cc} \frac{\partial f_1}{\partial\theta} & \frac{\partial f_1}{\partial\omega} \\ \frac{\partial f_2}{\partial\theta} & \frac{\partial f_2}{\partial\omega} \end{array}\right] \right|_{\theta=\theta^*,\omega=0}}$

これから、振り子の線形状態方程式

$\underbrace{\dot{\xi}-\dot{\xi}^*}_{\dot{x}}= \underbrace{\left.\frac{\partial f(\xi^*)}{\partial\xi}\right.}_A \underbrace{(\xi-\xi^*)}_{x}$

すなわち

$\underbrace{ \frac{d}{dt} \left[\begin{array}{c} \theta-\theta^* \\ \omega-\omega^* \end{array}\right] }_{\dot{x}}= \underbrace{ \left[\begin{array}{cc} 0 & 1 \\ \frac{3g}{4\ell}\cos\theta^* & 0 \end{array}\right] }_{A} \underbrace{ \left[\begin{array}{c} \theta-\theta^* \\ \omega-\omega^* \end{array}\right] }_{x}$

を得ます。以上の計算をMAXIMAを用いて実行するには、次のコマンドを与えます。

f:matrix([dth],[rhs(sol[1])]);
A:matrix([diff(f[1,1],th(t)),diff(f[1,1],dth)],[diff(f[2,1],th(t)),diff(f[2,1],dth)]);
A1:A,th(t)=0;
A2:A,th(t)=%pi;

演習

投稿日時: 2022年8月17日投稿者: cacsd

[1] 次の値を計算せよ。

$1^\circ\ 1-\frac{1}{e}$
$2^\circ\ \cos^{-1}(-1)$

[2] 次の関数のグラフを描け。

$p(x)=\frac{1}{\sqrt{2\pi}}e^{-\frac{x^2}{2}}\quad(-3\le x\le 3)$

[3] つぎの行列 $M$ の逆行列 $M^{-1}$ を計算し、 $MM^{-1}=I_2$ を確認せよ。

$M=\left[\begin{array}{cc}a&b\\c&d\end{array}\right]$

[4] 次のA行列の固有値を計算せよ。 ${\rm det}(sI_2-A)=0$ を解け。

$A=\left[\begin{array}{cc}0&1\\-\omega_n^2&-2\zeta\omega_n\end{array}\right]$

[5] $\dot{x}=Ax+Bu$ において、 $A=\left[\begin{array}{cc}1&2\\3&4\end{array}\right]$ 、 $B=\left[\begin{array}{cc}5\\6\end{array}\right]$ のとき、 $A-BF$ の固有値が{-1,-2}となるように状態フィードバック $u=-Fx$ を、次式によって定めよ。また、その妥当性を $A-BF$ の固有値を計算して確かめよ。

$F=[a_2'-a_2,a_1'-a_1]\left[\begin{array}{cc}a_1&1\\1&0\end{array}\right]^{-1}\left[\begin{array}{cc}B&AB\end{array}\right]^{-1}$

ヒント：多項式 $p(s)=s^2+a_1s+a_2$ の係数 $a_1$ と $a_2$ はそれぞれcoeff(p,s,1)とcoeff(p,s,0)で与えられる。

[6] 次の倒立振子の運動方程式を導き、状態空間表現を導け。

基礎事項

投稿日時: 2022年8月17日投稿者: cacsd

[0] はじめに
対話型数式処理プログラムMAXIMAは、次からフリーで入手できます。
http://sourceforge.net/projects/maxima/files/
次のフォントが必要な場合があります。
TeX-fonts-25

[1] 基礎
１）次のコマンドを与えます。最後にshift+enterを押してください。
–> a:1;
こうすると変数aに値1を与えたことになります。MAXIMAでは，「:」は「定義によって等しい」を意味します。確認のため，次のコマンドを与えてみます。
–> a;
また，最後の「;」はコマンド行の区切りを示し（つけない場合は自動的に付加されます），MAXIMAは対応する処理を実行後，その結果を返します。もし，処理結果の表示が不要な場合は，最後に「$」を置きます。次を確かめてください。
–> a:1$
もし，変数aやすべての変数を消去したい場合は，それぞれ次のコマンドを与えます。
–> kill(a);
–> kill(all);

２）さて，四則演算の記法については，通常の言語処理系と同じように，次が用いられます。
–> x+y;
–> x-y;
–> x*y;
–> x/y;
–> x^y;

３）関数 $y=ax^2$ を定義するには，次のコマンドを与えます。
–> y: a*x^2;
yの値を，x=2について計算するときは，次のコマンドを与えます。
–> ev(y,x:2);
または
–> y, x:2

４）一方， $f(x)=ax^2$ のように関数を明示的に定義するには，次のコマンドを与えます。
–> f(x):=a*x^2;
ここで、x=2のときの値は，次のように参照できます。
–> f(2);

[2] 関数のグラフ
１）関数のグラフを描くときは，たとえば次のコマンドを与えます。
–> y1: x^2;
–> plot2d(y1,[x,-2,2]);

２）２つのグラフを重ねて描くときは，続けて次のコマンドを与えます。
–> y2: 2*x;
–> plot2d([y1,y2],[x,-2,2]);

[3] リスト
１）MAXIMAの基本的なデータ構造は，「リスト」と呼ばれるものです。たとえば，次のコマンドを与えます。
–> a1:[a11,a12];
これにより，a1は，２つの要素a11とa12からなるリストとして定義されます。また，要素の区切りは「,」であることに注意してください。

２）ある変数がリストであるかどうかを調べるためには，次のコマンドを与えます。
–> listp(a1);
リストの長さを調べるには，次のコマンドを与えます。
–> length(a1);
リストの要素へのアクセス，たとえば，第１番目の要素を調べるには，次のコマンドを与えます。
–> a1[1];

３）さて，リスト自身を要素にもつリストも定義できます。
–> a2:[a21,a22];
を定義して，次のコマンドを与えてください。
–> A:[a1,a2];
これが，サイズ２×２の行列Ａを定義したことに相当します（行列データについては，あとで詳しく説明します）。たとえば，A[2]はa2を意味しますので，Aの（２，１）要素へのアクセスは次のようにします。
–> A[2][1];

[4] 関数
１）絶対値と平方根については，たとえば次のように得られます。
–> abs(-2);
–> sqrt(2);

２）円周率は，%piで参照することができます。
–> %pi;
–> %pi, numer;
三角関数の計算は，たとえば次の通り実行できます。
–> sin(%pi/4);
–> cos(%pi/4);
–> tan(%pi/4);

３）三角関数の逆関数の計算は，たとえば次の通り実行できます。
–> asin(1);
–> acos(-1);
–> atan(1);
–> atan2(1,-1);
ここで，x>0のときatan2(y,x)=atan(y/x)です。

４）次は，加法定理を示しています。
–> trigexpand(sin(x+y));
–> trigreduce(%)
–> trigexpand(cos(x+y));
–> trigreduce(%)
–> trigexpand(tan(x+y));
–> trigreduce(%)

５）指数関数の計算は，たとえば次の通り実行できます。
–> exp(0);

６）この逆関数(対数関数)の計算は，たとえば次の通り実行できます。
–> log(1);
この底はネイピア数eですが，10の場合は次の関数を定義して用います。
–> log10(x):=log(x)/log(10);
–> log10(1);

７）双曲線関数は，次式で定義されます。
$\sinh=\frac{e^x-e^{-x}}{2},\,\cosh=\frac{e^x+e^{-x}}{2},\,\tanh=\frac{\sinh\,x}{\cosh\,x}$
これらの計算は，たとえば次の通り実行できます。
–> sinh(0);
–> cosh(0);
–> tanh(0);

８）双曲線関数の逆関数の計算は，たとえば次の通り実行できます。
–> asinh(0);
–> acosh(1);
–> atanh(0);

[5] 多項式
１）因数分解
$x^2-1=(x+1)(x-1)$
を行うために，次のコマンドを与えてください。
–> factor(x^2-1);

２）また，式の展開
$(a+1)(x-1)=ax-a+x-1$
を行うために，次のコマンドを与えてください。
–> expand((a+1)*(x-1));
ここで，xだけについて整理して
$(a-1)x-a+1$
としたいときには，次のコマンドを与えまず。
–> ratsimp(%,x);
ここで，「%」は直前の結果を参照しています。

３）さらに，部分分数展開
$\frac{1}{x^2+3x+2}=\frac{1}{x+1}-\frac{1}{x+1}$
を行うために，次のコマンドを与えてみてください。
–> partfrac(1/(x^2+3*x+2),x);

[6] 方程式
１）２次方程式
$x^2+1=0$
を解くために，次のコマンドを与えてください。
–> solve(x^2+1=0,x);
ここで，出力結果における「%i」は虚数単位を表しています。解は2つの要素をもつリストとして得られていることに注意してください。第１番目の解は%[1]，第２番目の解は%[2]でアクセスできます。

２）それでは，２次方程式
$ax^2+bx+c=0$
の解の公式を求めてみましょう。
–> sol: solve(a*x^2+b*x+c=0,x)

３）次に，連立１次方程式
$\left\{\begin{array}{l} x+y=1\\ x+2y=1 \end{array}\right.$
を解くために，次のコマンドを与えてください。
–> solve([x+y=1,x+2*y=1],[x,y])

４）それでは，連立１次方程式
$\left\{\begin{array}{l} ax+by=e\\ cx+dy=f \end{array}\right.$
の解を求めてください。
–> sol: solve([a*x+b*y=e,c*x+d*y=f],[x,y])
解はリストのリストとして得られています。したがって，第１番目の解はsol[1][1],第２番目の解はsol[1][2]でアクセスできることに注意してください。

５）ちなみに，複素数
$1-j$
の実部と虚部は，次のように得られます。
–> realpart(1-%i)
–> imagpart(1-%i)

[7] 論理演算
１）論理和，論理積，否定はそれぞれ
$a\&b$
$a|b$
$\sim\,a$
のように表されます。たとえば
–> true and false;
–> true or false;
–> not true;
これらは，論理式と呼ばれ，trueかfalseのどちらかの値をとります。

２）真理値表を作ってみましょう。
–> [true and true, true and false, false and true, false and false];
–> [true or true, true or false, false or true, false or false];
–> [not true, not false];

３）次の等価な関係は，さまざまな論理展開に有用です。
$\sim (A\Rightarrow\,B)\leftrightarrow\,A\&\sim B$
右辺の真理値表を作ってみましょう。
–> [true and not true, true and not false, false and not true, false and not false];

４）さて，次のように入力してみてください。
–> is(1=1);
–> is(1#1);
これらも論理式です。次も同様です。
–> is(1>1);
–> is(1>=1);
–> is(1<1);
–> is(1<=1);

[8] 微分積分
１）まず極限の例を，いくつか挙げます。
–> limit(exp(-x),x,inf);
–> limit(exp(x),x,inf);
–> limit((1+1/x)^x,x,0);
–> limit(sin(x)/x,x,0);

２）微分の例を，いくつか挙げます。
–> diff(x^2,x);
–> diff(1/x,x);
–> diff(exp(x),x);
–> diff(log(x),x);
–> diff(sin(x),x);
–> diff(atan(x),x)

２）２階微分は，たとえば次のように行います。
–> diff(x^2,x,2);

３）積分の例を，いくつか挙げます。
–> integrate(2*x,x);
–> integrate(-1/x^2,x);
–> integrate(exp(x),x);
–> integrate(1/x,x);
–> integrate(cos(x),x);
–> integrate(1/(x^2+1),x);

４）次のようにtaylor展開を行うことができます。
–> taylor(exp(x),x,0,10);
–> taylor(sin(x),x,0,10);
–> taylor(cos(x),x,0,10);

５）微分方程式
$\frac{dx(t)}{dt}=-x(t)$
を解くために，次のコマンドを与えてください。
–> sol:desolve(’diff(x(t),t)=-x(t),x(t));
ここで，「’diff(x(t),t)」は，
$\frac{dx(t)}{dt}$
を表しています。また，注意しなければないのは，solがリストとなっていないことです。たとえば，x(0)=1のとき，この解のグラフを描くには，次のように，コマンドを与えます。
–> s1:rhs(sol), x(0)=1;
–> plot2d(s1,[t,0,5]);

６）さらに，微分方程式
$\frac{dx(t)}{dt}=-x(t)+\sin(10t)$
の解の構成要素を調べるために，次のコマンドを与えてください。
–> sol:desolve(’diff(x(t),t)=-x(t)+sin(10*t),x(t));
–> s2:rhs(sol), x(0)=1;
–> s3:rhs(sol), x(0)=0;
–> plot2d([s1,s2,s3],[t,0,5]);

[9] 行列計算
１）次のコマンドを与えてみましょう。
–> A:matrix([a11,a12],[a21,a22]);
–> B:ident(2);
–> C:zeromatrix(2,2);
–> D:apply(matrix,makelist(ident(2)[i]*[d1,d2][i],i,1,2));
行列Aの（2,1）要素へのアクセスは,A[2][1]またはA[2,1]として行います。
–> A[2,1];

２）行列の結合は次のように行います。
–> addcol(A,B);
–> addrow(A,C);

３）まず，要素ごとの四則演算は，次のように行えます。
–> A+B;
–> A-B;
–> A*B;
–> B/A;
–> A^2;

４）次に，行列としての演算は，次のように行います。
–> A.B;
–> A.invert(A);
–> A^^2;
–> transpose(A);

５）行列の階数，行列式，特性多項式、余因子行列は，次のように求められます。
–> rank(A);
–> determinant(A);
–> charpoly(A,s);
–> adjoint(A);

６）固有値と固有ベクトルは，次のように求められます。
–> Q:matrix([q1,q3],[q3,q2]);
–> eigenvalues(Q);
–> V:eigenvectors(Q);
この固有ベクトルが直交することは、次のように確かめられます。
–> V[2][1][1].V[2][2][1],ratsimp;

倒立振子の安定化制御

投稿日時: 2022年8月17日投稿者: cacsd

[0] このページは、「SCILABによる倒立振子の特性解析」の続きです。以下では、物理定数を次のように仮定します。

$M=0.94$ [kg], $m=0.23$ [m], $\ell=0.6413/2$ [m], $\alpha=5$ [deg]

[1] まず，安定性を調べます。そのために，SCILABの関数specを用いて， $A$ 行列の固有値を求めると，次のように得られます。

$\lambda_1=0,\ \lambda_2=0,\ \lambda_3=+5.181,\ \lambda_4=-5.181$

したがって，３つの固有値の実部が負ではなく，漸近安定性の条件（すべての固有値の実部が負）を満足していません。よって，倒立振子は不安定な制御対象であると判定します。
次に，可制御性の判定を行ないます。そのために，可制御性行列の階数が，制御対象の次数４に等しいか，すなわち

${\rm rank}[B\ A-\lambda_i I_4]=4\quad(i=1,2,3,4)$

が成り立つかどうかを調べます。行列 $[B\ A-\lambda_iI_4]$ の最小特異値を求めると，次のように得られます。

$\begin{tabular}{l|l}\hline eigenvalues of A & minimum singular values of $[B\ A-\lambda_iI_4]$\\\hline \lambda_1=0 & \sigma_{min}=0.849\\ \lambda_2=0 & \sigma_{min}=0.849\\ \lambda_3=+5.181 & \sigma_{min}=0.434\\ \lambda_4=-5.181 & \sigma_{min}=0.434\\\hline \end{tabular}$

すべての最小特異値は正なので，倒立振子の可制御性は成り立つと判定します。

次に，可観測性の判定を行ないます。そのために，可観測性行列の階数が，制御対象の次数４に等しいか，すなわち

${\rm rank}\left[\begin{array}{c}C\\ A-\lambda_i I_4\end{array}\right]=4\quad(i=1,2,3,4)$

が成り立つかどうかを調べます。行列 $\left[\begin{array}{c}C\\ A-\lambda_i I_4\end{array}\right]$ の最小特異値を求めると，次のように得られます。

$\begin{tabular}{l|l}\hline eigenvalues of A & minimum singular values of $\left[\begin{array}{c}C\\ A-\lambda_i I_4\end{array}\right]$\\\hline \lambda_1=0 & \sigma_{min}=1\\ \lambda_2=0 & \sigma_{min}=1\\ \lambda_3=+5.181 & \sigma_{min}=0.189\\ \lambda_4=-5.181 & \sigma_{min}=0.189\\\hline \end{tabular}$

すべての最小特異値は正なので，倒立振子の可観測性は成り立つと判定します。

以上の計算を，SCILABで行うためのプログラムを，次に示します。

//cip.sce
M=0.94; m=0.23; ell=0.6413/2; g=9.8;
alpha=5/180*%pi
a32=-6*m*g/(8*M+(5-3*cos(2*alpha))*m);
b3=8/(8*M+(5-3*cos(2*alpha))*m);
a42=6*(M+m)*g/(8*M+(5-3*cos(2*alpha))*m)/ell;
b4=-6*cos(alpha)/(8*M+(5-3*cos(2*alpha))*m)/ell;
A=[0 0 1 0;0 0 0 1;0 a32 0 0;0 a42 0 0];
B=[0;0;b3;b4];
C=eye(2,4);
r=spec(A)
for i=1:4, min(svd([B A-r(i)*eye(4,4)])), end
for i=1:4, min(svd([C; A-r(i)*eye(4,4)])), end

[2] 不安定な倒立振子を安定化するための状態フィードバック

$u=-Fx$

を決定することを考えます。そのために安定な閉ループ系

$\dot{x}=(A-BF)x$

の応答に対して，２次形式評価関数

$\displaystyle{J=\int_0^\infty (q_1^2r^2(t)+q_2^2\dot{r}^2(t)+q_3^2\theta^2(t)+q_4^2\dot{\theta}^2(t)+\rho^2r_1^2u^2(t))dt}$

ただし、

$\displaystyle{q_1=\frac{1}{\max |r|},\ q_2=\frac{1}{\max |\dot{r}|},\ q_3=\frac{1}{\max |\theta|},\ q_4=\frac{1}{\max |\dot{\theta}|},\ r_1=\frac{1}{\max |u|}}$

を最小化するものを選ぶことを考えます。そのような安定化状態フードバックのゲイン行列 $F$ は，リッカチ方程式CARE

$\Pi A+A^T\Pi -\Pi B R^{-1} B^T\Pi+Q=0$

ただし、

$Q={\rm diag}\{q_1^2,q_2^2,q_3^2.q_4^2\},\ R=\rho^2r_1^2$

の解 $\Pi>0$ を用いて，次のように与えられます。

$F=R^{-1}B\Pi$

以上の計算を，SCILABで行うためのプログラムを，次に示します。

Mr=1; Tr=0.1; Mth=5/180*%pi;
Tth=(1/4)*(2*%pi*sqrt(4/3*ell/g)); Mu=5;
q1=1/Mr; q2=1/Mth; q3=Tr/Mr; q4=Tth/Mth; r1=1/Mu; rho=1;
Q=diag([q1^2,q2^2,q3^2,q4^2]); R=rho^2*r1^2;
//—–
function [F,p]=opt(A,B,C,Q,R)
w2=R\B’;
[v,p]=spec([A -B*w2;-C’*Q*C -A’]); p=diag(p);
[dummy,index]=sort(real(p));
n=size(A,1); j=index(n+1:n+n);
p=p(j); x=v(1:n,j); y=v(n+1:n+n,j);
X=real(y/x); F=w2*X;
endfunction
//—–
[F,p]=opt(A,B,eye(4,4),Q,R);
function dz=clps1(t,z),dz=(A-B*F)*z,endfunction
t0=0; t=0:0.01:10; x0=[0;3/180*%pi;0;0];
z=ode(x0,t0,t,clps1);
scf(0);
subplot(211),plot(t,diag([100 180/%pi])*C*z); mtlb_grid
subplot(212),plot(t,-F*z); mtlb_grid

計算工学演習第一レポート（２）

単振り子のアニメーション

投稿日時: 2022年8月17日投稿者: cacsd

次の単振り子を考えます。

この運動方程式は

$\begin{array}{ll} \displaystyle{m\ddot{r}=\underbrace{-mg\sin\theta}_f\quad(r=\ell\theta)} \end{array}$

すなわち

$\begin{array}{ll} \displaystyle{\ddot{\theta}=-\frac{g}{\ell}\sin\theta} \end{array}$

となります。ここで、 $\theta\simeq0$ 　と近似すると

$\begin{array}{ll} \displaystyle{\ddot{\theta}=-\frac{g}{\ell}\theta} \end{array}$

となります。この解は、 $\dot{\theta}(0)=0$ 　を仮定すると

$\theta(t)=\theta(0)\cos\left(\sqrt{\frac{g}{\ell}}t\right)$

のように表されます。ここで、単振り子の周期は次式で与えられます。

$\frac{2\pi}{T}=\sqrt{\frac{g}{\ell}} \Rightarrow T=2\pi\sqrt{\frac{\ell}{g}}$

この線形シミュレーションを行うために、次のプログラムを実行してみてください。

function dx=f(t,x),
dx=A*x,
endfunction
g=9.8; L=1;
A=[0 1;-g/L 0];
t0=0; t1=10; nt=100; td=(t1-t0)/nt; t=t0:td:t1;
x0=[-30/180*%pi;0];
x1=ode(x0,t0,t,f);
x0=[-177/180*%pi;0];
x2=ode(x0,t0,t,f);
clf(0),scf(0);
plot(t,180/%pi*x1(1,:),’b’,t,180/%pi*x2(1,:),’r’),
mtlb_grid

次のようなグラフが現れるでしょう。初期値は２通り設定しています。

次に、非線形シミュレーションを行うために、次のプログラムを実行してみてください。

function dx=f1(t,x)
th=x(1);
dth=x(2);
dx(1)=dth;
dx(2)=-g/ell*sin(th);
endfunction
g=9.8; ell=1; T=2*%pi*1/sqrt(g/ell)
t0=0; t1=10; nt=100; td=(t1-t0)/nt; t=t0:td:t1;
x=[-30/180*%pi;0];
for i=1:nt, x=[x ode(x(:,i),t(i),t(i+1),f1)]; end
x1=x;
x=[-177/180*%pi;0];
for i=1:nt, x=[x ode(x(:,i),t(i),t(i+1),f1)]; end
x2=x;
clf(0),scf(0);
plot(t,180/%pi*x1(1,:),’b’,t,180/%pi*x2(1,:),’r’),
mtlb_grid

次のようなグラフが現れるでしょう。初期値は２通り設定しています。

線形と非線形では、シミュレーション結果が全く違います！

それでは、これらをアニメーションで比較してみましょう。まず、線形シミュレーションの場合です。

function dx=f1(t,x),
dx=A*x,
endfunction
g=9.8; L=1;
A=[0 1;-g/L 0];
t0=0; t1=10; nt=2000; td=(t1-t0)/nt; t=t0:td:t1;
x0=[-30/180*%pi;0];
x1=ode(x0,t0,t,f1);
x0=[-177/180*%pi;0];
x2=ode(x0,t0,t,f1);
x=L*sin(x2(1,:)+%pi);
y=L*cos(x2(1,:)+%pi);
figure(1);
plot([0;x(1)],[0;y(1)],’o-‘);
h_compound = gce();
h_compound.children.thickness = 2
h_compound.children.mark_size = 10;
h_compound.children.mark_background = 2;
h_axes = gca();
h_axes.data_bounds = [-1.5,-1.5;1.5,1.5];
i = 1;
while i<=length(t)
drawlater();
h_compound.children.data = [0 0;x(i),y(i)];
drawnow();
i = i+1;
end

次に、非線形シミュレーションの場合です。

function dx=f2(t,x)
th=x(1);
dth=x(2);
dx(1)=dth;
dx(2)=-g/ell*sin(th);
endfunction
g=9.8; ell=0.5; T=2*%pi*1/sqrt(g/ell);
t0=0; t1=10; nt=2000; td=(t1-t0)/nt; t=t0:td:t1;
x0=[-30/180*%pi;0];
x1=ode(x0,t0,t,f2);
x0=[-177/180*%pi;0];
x2=ode(x0,t0,t,f2);
x=2*ell*sin(x2(1,:)+%pi);
y=2*ell*cos(x2(1,:)+%pi);
figure(1);
plot([0;x(1)],[0;y(1)],’o-‘);
h_compound = gce();
h_compound.children.thickness = 2
h_compound.children.mark_size = 10;
h_compound.children.mark_background = 2;
h_axes = gca();
h_axes.data_bounds = [-1.5,-1.5;1.5,1.5];
i = 1;
while i<=length(t)
drawlater();
h_compound.children.data = [0 0;x(i),y(i)];
drawnow();
i = i+1;
end

このアニメーションの作成には、次のサイトを参考にさせていただきました。

Scilabでアニメーション作成

【参考】　バネで拘束されている台車のアニメーションを以下に示します。

clear; xdel(winsid());
function dx=f(t,x),
dx=A*x,
endfunction
A=[0 1;-1 0];
t0=0; t1=10; nt=2000; td=(t1-t0)/nt; t=t0:td:t1;
x0=[0;1];
x1=ode(x0,t0,t,f);
x=0.2*[0 1 1 0 0]’;
y=0.2*[0 0 1 1 0]’;
figure(1);
plot(x,y);
mtlb_grid
h_compound = gce();
h_compound.children.thickness = 2
h_compound.children.mark_size = 10;
h_compound.children.mark_background = 2;
h_axes = gca();
h_axes.data_bounds = [-1.5,-1;1.5,1];
i = 1;
while i<=length(t)
drawlater();
h_compound.children.data = [x+x1(1,i) y];
drawnow();
i = i+1;
end