制御系CAD

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

UV Control

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

●Drone becomes to be very popular tool to take a video from the sky. It is a surprise that we can control its motion with 6 degrees of freedom so smartly. What makes it possible?

In general, a drone has 4 propellers as shown. This means that 4 degrees of freedom can be controlled. They should be the heave $z$ and the roll $\phi$ , the pitch $\theta$ , the yaw $\psi$ . Assume that rotational speeds of 4 propellers can be set to the specified values, $\omega_{1c}$ , $\omega_{2c}$ , $\omega_{3c}$ , $\omega_{4c}$ .

Then we can produce 1 force $F_z$ and 3 torques $\tau_\phi$ , $\tau_\theta$ , $\tau_\psi$ to control the heave $z$ and the roll $\phi$ , the pitch $\theta$ , the yaw $\psi$ respectively by combining the 4 propellers’ thrusts as follows:

$\left[\begin{array}{c} F_z \\ \tau_\phi \\ \tau_\theta \\ \tau_\psi \end{array}\right]= \left[\begin{array}{cccc} k_1 & k_1 & k_1 & k_1 \\ 0 & -\ell k_1 & 0 & \ell k_1 \\ \ell k_1 & 0 & -\ell k_1 & 0 \\ -k_2 & k_2 & -k_2 & k_2 \end{array}\right] \left[\begin{array}{c} \omega_{1c} \\ \omega_{2c} \\ \omega_{3c} \\ \omega_{4c} \end{array}\right]$ .

This means that the MIMO dynamics is decoupled into 4 SISO dynamics. We will employ a PID controller corresponding to each SISO dynamics. Therefore it is the static decoupling to make the control system design easier.

●Many underwater vehicles have been developed for various purposes. For example, we can find them as follows.

Note that 4 to 8 thrustors are equipped to realize the static decoupling. The number depends on motion necessary to achieve the mission of UVs. Most control systems will be designed in the same way as drone.

●Prof. Koterayama and Prof. Nakamura developed the following UV called as DELTA.

The mission of DELTA is for wide-area survey. It works in a towing mode for wade scanning, and in a self-propulsive mode for local investigation. The detail is discussed in the paper:

Masahiko NAKAMURA, Hiroyuki KAJIWARA, Wataru KOTERAYAMA:
Development of an ROV operated both as towed and self-propulsive vehicle
Ocean Engineering, vol.28, no.1, pp.1-43, 2000.

DELTA has only two thrustors. So only the heave $z$ and the yaw $\psi$ are controlled. However it is necessary to control the roll $\phi$ and the pitch $\theta$ to keep the equilibrium state. Such a control system can’t be realized by PID control. The following control system is designed by LQG approach.

For any UV, the collision avoidance is an important function. For example, a deep diving will be necessary to avoid an obstacle as follows.

In order to change the depth in DELTA, there is no way except decreasing the thrust. This means the large speed variation, by which hydrodynamic force will be varied largely. So we need a robust controller or a scheduling controller. Such a control scheme is depicted in the following.

Ship Control

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

●For the directional control of a ship, consider the following NOMOTO model.

$\displaystyle{ \dot{r}(t)=-\frac{1}{T(t)}r(t)+\frac{K(t)}{T(t)}\delta(t-L)+w(t)} }$

where $r(t)=\dot{\psi}(t)$ and

$\displaystyle{ T(t)=\frac{L}{U(t)}T',\ K(t)=\frac{U(t)}{L}K' \quad (U_1\le U(t)\le U_2) }$

We assume the time lag $L$ to take account of the delay to get the rudder effect. We use the same notation for the ship length $L$ assuming no confusion. The time constant $T$ and gain constant $K$ are varied according to the variation of forward speed $U$ . From the following equation, it is clear that the rudder effect is proportional to the square of the forward speed.

$\displaystyle{ \dot{r}(t)=-\underbrace{\left(\frac{U(t)}{L}\right)\frac{1}{T'}}_{a(t)=\frac{1}{T(t)}}r(t) +\underbrace{\left(\frac{U(t)}{L}\right)^2\frac{K'}{T'}}_{b(t)=\frac{K(t)}{T(t)}}\delta(t-L)+w(t) }$

Given a nominal forward speed $U^*$ , the following equation holds.

$\displaystyle{ \dot{r}(t)=-\underbrace{\left(\frac{U(t)}{U^*}\right)\frac{1}{T^*}}_{a(t)=\frac{1}{T(t)}}r(t) +\underbrace{\left(\frac{U(t)}{U^*}\right)^2\frac{K^*}{T^*}}_{b(t)=\frac{K(t)}{T(t)}}\delta(t-L)+w(t) }$

where

$\displaystyle{ T^*=\frac{L}{U^*}T',\ K^*=\frac{U^*}{L}K' \quad (U_1\le U^*\le U_2) }$

●Zig-Zag Test:　Neglecting the time lag $L$ and the forward speed variation, a simulation example of the Zig-Zag test is given as follows.

From this response, we used to obtain both time constant $T$ and gain constant $K$ . However, the response is not always the step response by which a linear system is completely characterized. As a ship has dynamics of an astatic (no-fixed-position) system, the Zig-Zag test is a compromise way for its identification.

We should notice that if we apply the unity feedback as shown in the above we can obtain the step response and identify $(\zeta,\omega_n)$ from $(T_p, 1+p_0)$ by the following formula.

$\displaystyle{ (T_p, 1+p_0)\ \Rightarrow\ (\zeta,\omega_n)= \left( \sqrt{\frac{(\log p_0)^2}{(\log p_0)^2+\pi^2}}, \frac{\sqrt{(\log p_0)^2+\pi^2}}{T_p} \right) }$

●PID control: Under the nominal forward speed $U^*$ , neglecting the time lag $L$ , consider the following NOMOTO model.

$\displaystyle{ \dot{r}(t)=-\frac{1}{T^*}r(t)+\frac{K^*}{T^*}\delta(t)+w(t) }$

Then PID control is presented as follows.

$\displaystyle{ \delta(t)=K_P(\psi_c-\psi(t))+K_D\frac{d}{dt}(\psi_c-\psi(t))+K_I\int_0^t(\psi_c-\psi(\tau))\,d\tau }$

Although we used to tune the P gain $K_P$ , the D gain $K_D$ , the I gain $K_I$ on the site, here we want try to determine them based on the NOMOTO model. The closed-loop system by PID control is given by

Here assuming $\dot{\psi}_c=0$ , we have

$\displaystyle{ \delta(t)=-K_P\psi(t)-K_Dr(t)+K_P\psi_c+K_I\int_0^t(\psi_c-\psi(\tau))\,d\tau }$

Therefore, we have another configuration of PID control system.

Letting $\zeta=\frac{1}{2\sqrt{T^*K^*}},\omega_n=\sqrt{\frac{K^*}{T^*}}$ , the following state equation is obtained.

$\displaystyle{ \underbrace{ \left[\begin{array}{c} \dot{\psi}(t) \\ \dot{r}(t) \end{array}\right] }_{\dot{x}(t)} = \underbrace{ \left[\begin{array}{cc} 0 & 1 \\ 0 & -2\zeta\omega_n \end{array}\right] }_{A} \underbrace{ \left[\begin{array}{c} \psi(t) \\ r(t) \end{array}\right] }_{x(t)} + \underbrace{ \left[\begin{array}{c} 0 \\ \omega_n^2 \end{array}\right] }_{B} \underbrace{\delta(t)}_{u(t)} + \left[\begin{array}{c} 0 \\ w \end{array}\right] }$

Then PID control is a state feed back with an integral action as follows.

$\displaystyle{ \underbrace{\delta(t)}_{u(t)} =- \underbrace{ \left[\begin{array}{cc} K_P & K_D \end{array}\right] }_{F} \underbrace{ \left[\begin{array}{c} \psi(t) \\ r(t) \end{array}\right] }_{x(t)} +\underbrace{K_P}_{G} \underbrace{\psi_c}_{v} +K_I \underbrace{\int_0^t(\psi_c-\psi(\tau))\,d\tau}_{x_I(t)} }$

Therefore, after specifying the desired first overshoot $(T_p',1+p_0')$ , we calculate the corresponding to $(\zeta',\omega_n')$ and then determine PID gains by

$\displaystyle{ K_P=\frac{\omega_n'^2}{\omega_n^2},\ K_D=\frac{2}{\omega_n^2}(\zeta'\omega_n'-\zeta\omega_n),\ K_I=\omega_I\frac{\omega_n'^2}{\omega_n^2} }$

Here we try $\omega_I=\frac{w_n'}{10}$ at first, and then tune it by observing the disturbance rejection.

The CLPS by PID control is presented by

$\displaystyle{ \underbrace{ \left[\begin{array}{c} \dot{\psi}(t) \\ \dot{r}(t) \\ \dot{x}_I(t) \end{array}\right] }_{\dot{x}_{CL}(t)} = \underbrace{ \left[\begin{array}{ccc} 0 & 1 & 0\\ -\omega_n'^2 & -2\zeta'\omega_n' & \omega_I\omega_n'^2 \\ -1 & 0 & 0 \end{array}\right] }_{A_{CL}} \underbrace{ \left[\begin{array}{c} \psi(t) \\ r(t) \\ x_I(t) \end{array}\right] }_{x_{CL}(t)} + \underbrace{ \left[\begin{array}{cc} 0 & 0\\ \omega_n'^2 & 1\\ 1 & 0 \end{array}\right] }_{B_{CL}} \left[\begin{array}{c} \psi_c \\ w \end{array}\right] }$

Here, assuming $\dot{w}=0$ , we differentiate it and obtain

$\displaystyle{ \underbrace{ \left[\begin{array}{c} \ddot{\psi}(t) \\ \ddot{r}(t) \\ \ddot{x}_I(t) \end{array}\right] }_{\ddot{x}_{CL}(t)} = \underbrace{ \left[\begin{array}{ccc} 0 & 1 & 0\\ -\omega_n'^2 & -2\zeta'\omega_n' & \omega_I\omega_n'^2 \\ -1 & 0 & 0 \end{array}\right] }_{A_{CL}} \underbrace{ \left[\begin{array}{c} \dot{\psi}(t) \\ \dot{r}(t) \\ \dot{x}_I(t) \end{array}\right] }_{\dot{x}_{CL}(t)} }$

From the CLPS stability, we have

$\displaystyle{ \underbrace{ \left[\begin{array}{c} \psi(t) \\ r(t) \\ x_I(t) \end{array}\right] }_{x_{CL}} \rightarrow \underbrace{ \left[\begin{array}{ccc} 0 & -1 & 0\\ \omega_n'^2 & 2\zeta'\omega_n' & -\omega_I\omega_n'^2 \\ 1 & 0 & 0 \end{array}\right]^{-1} }_{-A_{CL}^{-1}} \underbrace{ \left[\begin{array}{cc} 0 \\ \omega_n'^2\psi_c +w\\ \psi_c \end{array}\right] }_{B_{CL}} = \left[\begin{array}{cc} \psi_c\\ 0\\ \frac{w}{\omega_I\omega_n'^2} \end{array}\right] }$

This means that the integral action is estimating the unknown disturbance.

The following simulation shows the CLPS responses by PD control. It is observed that the disturbance rejection is not sufficient because of the lack of integral action.

The following simulation shows the CLPS responses by PID control. Here $\omega_I=2\frac{w_n'}{10}$ . It is observed that the disturbance rejection is satisfied but the transient response is fractured a little bit being off expectation.

●LQI control: We will try to determine the PID gains in the framework of LQI control. Consider the following error system:

$\displaystyle{ \left[\begin{array}{c} \dot\psi(t)-\dot{\psi}_c \\ \dot r(t) \\ \dot{\delta}(t) \end{array}\right] = \left[\begin{array}{ccc} 0 & 1 & 0 \\ 0 & -2\zeta\omega_n &\omega_n^2 \\ 0 & 0 & 0 \end{array}\right] \left[\begin{array}{c} \psi(t)-\psi_c \\ r(t) \\ \delta(t) \end{array}\right] + \left[\begin{array}{c} 0 \\ 0 \\ 1 \end{array}\right] \dot{\delta} (t) }$

For the error system, we will minimize the following criterion function:

$\displaystyle{ J=\int_0^\infty(\frac{1}{M_\psi^2}(\psi(t)-\psi_c)^2+\frac{T_\psi^2}{M_\psi^2}r^2(t) +\frac{1}{M_\delta^2}\delta^2(t)+\frac{T_\delta^2}{M_\delta^2}\dot{\delta}^2(t))\,dt }$

Solving this problem, the PID gains are obtained as follows:

$\displaystyle{ \dot{\delta}(t) =- \left[\begin{array}{ccc} f_\psi & f_r & f_\delta \end{array}\right] \left[\begin{array}{c} \psi(t)-\psi_c \\ r(t) \\ \delta(t) \end{array}\right] }$
$\displaystyle{ =- \underbrace{ \left[\begin{array}{ccc} f_\psi & f_r & f_\delta \end{array}\right] \left[\begin{array}{ccc} 0 & 1 & 0 \\ 0 & -2\zeta\omega_n &\omega_n^2 \\ 1 & 0 & 0 \end{array}\right]^{-1} }_{ \left[\begin{array}{ccc} K_P & K_D & K_I \end{array}\right] } \left[\begin{array}{c} \dot\psi(t)-\dot{\psi}_c \\ \dot{r}(t) \\ \psi(t)-\psi_c \end{array}\right] }$

In the case of $M_{\psi}=1, T_{\psi}=0.5T, M_{\delta}=0.5, T_{\delta}=0.1T$ , we have the following simulation result, which seems to be very nice. Note that there is no feedforward term compared with PID control.

However, in the case of having the time lag $L=9$ , the CLPS is unstable as follows:

●LPV control:

Assuming the following speed variation, the responses of CLPS by unity feedback vary largely.

Consider the following NOMOTO model based on a nominal forward speed $U^*$ .

$\displaystyle{ \left\{\begin{array}{l} \dot{\psi}(t)=r(t) \\ \displaystyle{\dot{r}(t)=-\left(\frac{U}{U^*}\right)\frac{1}{T^*}r(t) +\left(\frac{U}{U^*}\right)^2\frac{K^*}{T^*}\delta(t)} \end{array}\right. }$

Adding the rudder dynamics,

$\displaystyle{ \dot{\delta}(t)=-\frac{1}{T_a}\delta(t)+\frac{K_a}{T_a}u(t) }$

we have the following state-space representation.

$\displaystyle{ \underbrace{ \left[\begin{array}{c} \dot{\psi}(t) \\ \dot{r}(t) \\ \dot{\delta}(t) \end{array}\right] }_{\dot{x}(t)} = \underbrace{ \left[\begin{array}{ccc} 0 & 1 & 0\\ 0 & -\left(\frac{U}{U^*}\right)\frac{1}{T^*} & \left(\frac{U}{U^*}\right)^2\frac{K^*}{T^*} \\ 0 & 0 & -\frac{1}{T_a} \end{array}\right] }_{A(U,U^2)} \underbrace{ \left[\begin{array}{c} \psi(t) \\ r(t) \\ \delta(t) \end{array}\right] }_{x(t)} + \underbrace{ \left[\begin{array}{c} 0 \\ 0 \\ \frac{K_a}{T_a} \end{array}\right] }_{B} u(t) }$
$\displaystyle{ \underbrace{ \psi(t) }_{y(t)} = \underbrace{ \left[\begin{array}{ccc} 1 & 0 & 0 \end{array}\right] }_{C} \underbrace{ \left[\begin{array}{c} \psi(t) \\ r(t) \\ \delta(t) \end{array}\right] }_{x(t)} }$

Note that it is possible to have the following polytopic representation.

$\displaystyle{ A(U,U^2)=p_1(U,U^2)A_1+p_2(U,U^2)A_2+p_3(U,U^2)A_3 }$

where

$\displaystyle{A_1=A(U_1,U_1^2)}$
$\displaystyle{A_2=A(U_2,U_2^2)}$
$\displaystyle{A_3=A(\frac{U_1+U_2}{2},U_1U_2)}$ 　

Defining $U_3=\frac{U_1+U_2}{2}$ ,

$\displaystyle{ p_1(U,U^2)=\frac{1}{p_0}\det \left[\begin{array}{cc} U-U_3 & U_2-U_3 \\ U^2-U_1U_2 & U_2^2-U_1U_2 \\ \end{array}\right]}$
$\displaystyle{ p_2(U,U^2)=\frac{1}{p_0}\det \left[\begin{array}{cc} U_1-U_3 & U-U_3 \\ U_1^2-U_1U_2 & U^2-U_1U_2 \\ \end{array}\right]}$
$\displaystyle{ p_3(U,U^2)=\frac{1}{p_0}\det \left[\begin{array}{cc} U_1-U_2 & U_2-U \\ U_1^2-U_2^2 & U_2^2-U^2 \\ \end{array}\right]}$
$\displaystyle{ p_0=\det \left[\begin{array}{cc} U_1-U_2 & U_2-U_3 \\ U_1^2-U_2^2 & U_2^2-U_1U_2 \\ \end{array}\right]}$
$\displaystyle{p_1(U,U^2)+p_2(U,U^2)+p_3(U,U^2)=1}$

Based on this model, the framework of LPV control is proposed. LPV control can cover the variation included in the triangle region with the vertexes $(U_1,U_1^2)$ , $(U_2,U_2^2)$ and temporal vertex $(U_3,U_1U_2)$ .

Constitute the following 2-port system.

$\displaystyle{ P: \left\{\begin{array}{l} \left[\begin{array}{c} \dot{x} \\ \dot{x}_I \end{array}\right]= \underbrace{ \left[\begin{array}{cc} A(U,U^2)& 0 \\ -C & 0 \end{array}\right] }_{\cal{A}(U,U^2)} \left[\begin{array}{c} x \\ x_I \end{array}\right] + \underbrace{ \left[\begin{array}{c} 0 \\ 1 \end{array}\right] }_{B_1} r + \underbrace{\left[\begin{array}{c} B \\ 0 \end{array}\right] }_{B_2} u\\ \underbrace{ \left[\begin{array}{c} y_{11} \\ y_{12} \end{array}\right] }_{y_1} = \underbrace{ \left[\begin{array}{cc} 0 &\omega_I\\ \omega_DCA(U,U^2) & 0 \end{array}\right] }_{C_1} \left[\begin{array}{c} x \\ x_I \end{array}\right] + \underbrace{ \left[\begin{array}{c} 0 \\ 0 \end{array}\right] }_{D_{11}} r + \underbrace{ \left[\begin{array}{c} 0 \\ \omega_DCB \end{array}\right] }_{D_{12}} u\\ \underbrace{ \left[\begin{array}{c} y \\ x_I \end{array}\right] }_{y_2} = \underbrace{ \left[\begin{array}{cc} C & 0\\ 0 & 1 \end{array}\right] }_{C_2} \left[\begin{array}{c} x \\ x_I \end{array}\right] + \underbrace{ \left[\begin{array}{c} 0 \\ 0 \end{array}\right] }_{D_{21}} r \end{array}\right.}$

$A(U,U^2)=p_1(U,U^2)A_1+p_2(U,U^2)A_2+p_3(U,U^2)A_3$

For this LPV model, design the following LPV controller.

$\displaystyle{ K_0: \left\{\begin{array}{l} \dot{x}_K=A_K(U,U^2)x_K+ \underbrace{ \left[\begin{array}{cc} B_K^{(1)}(U,U^2) & B_K^{(2)}(U,U^2) \end{array}\right] }_{B_K(U,U^2)} \left[\begin{array}{c} y \\ x_I \end{array}\right] \\ u=C_K(U,U^2)x_K + \underbrace{ \left[\begin{array}{cc} D_K^{(1)}(U,U^2) & D_K^{(2)}(U,U^2) \end{array}\right] }_{D_K(U,U^2)} \left[\begin{array}{c} y \\ x_I \end{array}\right] \end{array}\right.}$

$A_K(U,U^2)=p_1(U,U^2)A_{K1}+p_2(U,U^2)A_{K2}+p_3(U,U^2)A_{K3}$
$B_K(U,U^2)=p_1(U,U^2)B_{K1}+p_2(U,U^2)B_{K2}+p_3(U,U^2)B_{K3}$
$C_K(U,U^2)=p_1(U,U^2)C_{K1}+p_2(U,U^2)C_{K2}+p_3(U,U^2)C_{K3}$
$D_K(U,U^2)=p_1(U,U^2)D_{K1}+p_2(U,U^2)D_{K2}+p_3(U,U^2)D_{K3}$

Form this, the following output feedback is obtained.

$\displaystyle{ K: \left\{\begin{array}{l} \left[\begin{array}{c} \dot{x}_K \\ \dot{x}_I \end{array}\right]= \underbrace{ \left[\begin{array}{cc} A_K(U,U^2) & B_K^{(2)}(U,U^2) \\ 0 & 0 \end{array}\right] }_{A_K(U,U^2)} \left[\begin{array}{c} x_K \\ x_I \end{array}\right] + \underbrace{ \left[\begin{array}{cc} B_K^{(1)}(U,U^2) & 0\\ -1& 1 \end{array}\right] }_{B_K(U,U^2)} \left[\begin{array}{c} y \\ r \end{array}\right] \\ u= \underbrace{ \left[\begin{array}{cc} C_K(U,U^2) & D_K^{(2)}(U,U^2) \end{array}\right] }_{C_K(U,U^2)} \left[\begin{array}{c} x_K \\ x_I \end{array}\right] + \underbrace{ \left[\begin{array}{cc} D_K^{(1)}(U,U^2) & 0 \end{array}\right] }_{D_K(U,U^2)} \left[\begin{array}{c} y \\ r \end{array}\right] \end{array}\right. }$

We can obtain $H_\infty$ control by fixing as $U=U^*$ . The following simulation shows the CLPS responses by $H_\infty$ control. It is observed that the responses are slightly varied according to the three kinds of speed variation.

On the other hand, the following simulation shows the CLPS responses by the LPV control. It is observed that the responses are almost same in spite of the three kinds of speed variation.

LQG Control

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

1. Optimal Control for MIMO system

$\bullet$ Given an nth-order system (satisfying controllability and observability)

(1) $\begin{eqnarray*} &&\dot{x}(t)=Ax(t)+Bu(t),\ y(t)=Cx(t)\ \\ &&(x(t)\in{\rm\bf R}^n,\ u(t)\in{\rm\bf R}^m,\ y(t)\in{\rm\bf R}^p) \end{eqnarray*}$

and the 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)=\underbrace{(A-BF)}_{A_F}x(t),\ y(t)=Cx(t)\\ &&A_F=A-BF:\ stable\ matrix. \end{eqnarray*}$

The behaviors of the state and the input are given by

(4) $\begin{eqnarray*} x(t)=\exp(A_Ft)x(0) \end{eqnarray*}$

and

(5) $\begin{eqnarray*} u(t)=-F\exp(A_Ft)x(0). \end{eqnarray*}$

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

(6) $\begin{eqnarray*} J=\int_0^\infty(y^T(t)Qy(t)+u^T(t)Ru(t))\,dt. \end{eqnarray*}$

$\bullet$ The criterion function can be written as

(7) $\begin{eqnarray*} J&=&\int_0^\infty x^T(t)(C^TQC+F^TRF)x(t)\,dt\\ &=&x^T(0)\underbrace{\int_0^\infty \exp(A_F^Tt)(C^TQC+F^TRF)\exp(A_Ft)\,dt}_{\Pi}x(0)\\ &=&{\rm tr}\ \Pi x(0)x^T(0). \end{eqnarray*}$

Note that we have the following constraint on $\Pi$ :

(8) $\begin{eqnarray*} \Pi A_F+A_F^T\Pi+C^TQC+F^TRF=0. \end{eqnarray*}$

It is known that $\Pi>0$ holds because of the closed-loop stability. Taking account of all zero-input responses, instead of (7), we consider to minimize

(9) $\begin{eqnarray*} J={\rm tr}\ \Pi. \end{eqnarray*}$

Therefore, we will minimize

(10) $\begin{eqnarray*} J'={\rm tr}\ \Pi+{\rm tr}\ ((\Pi A_F+A_F^T\Pi+C^TQC+F^TRF)\Gamma) \end{eqnarray*}$

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

(11) $\begin{eqnarray*} \frac{\partial J'}{\partial\Pi}=I_n+\Gamma A_F^T+A_F\Gamma=0\Rightarrow \Gamma>0, \end{eqnarray*}$

(12) $\begin{eqnarray*} \frac{\partial J'}{\partial F}=2(RF-B^T\Pi)\Gamma=0\Rightarrow F=R^{-1}B^T\Pi, \end{eqnarray*}$

(13) $\begin{eqnarray*} \frac{\partial J'}{\partial\Gamma}=\Pi A_F+A_F^T\Pi+C^TQC+F^TRF=0. \end{eqnarray*}$

Substituting $F=R^{-1}B^T\Pi$ into (13),

(14) $\begin{eqnarray*} \Pi (A-BR^{-1}B^T\Pi)+(A-BR^{-1}B^T\Pi)^T\Pi\\ +C^TQC+(R^{-1}B^T\Pi)^TRR^{-1}B^T\Pi=0 \end{eqnarray*}$

That is, we have the Algebraic Riccati Equation (ARE) on $\Pi$ :

(15) $\begin{eqnarray*} {\Pi A+A^T\Pi-\Pi BR^{-1}B^T\Pi+C^TQC=0}. \end{eqnarray*}$

By solving this, we can obtain $\Pi>0$ and then $F$ by

(16) $\begin{eqnarray*} {F=R^{-1}B^T\Pi}. \end{eqnarray*}$

The control methodology is called as LQ Control.

$\bullet$ The sufficiency is discussed as follows. The following expression can be derived:

(17) $\begin{eqnarray*} y^TQy+u^TRu=(u+R^{-1}B^T\Pi x)^TR(u+R^{-1}B^T\Pi x) -\underbrace{\frac{d}{dt}x^T\Pi x}_{2x^T\Pi\dot{x}}. \end{eqnarray*}$

In fact, substituting $\dot{x}=Ax+Bu$ into the right hand side and using the Riccati equation,

(18) $\begin{eqnarray*} &&{\rm RHS}=u^TRu+2x^T\Pi Bu+x^T\Pi BR^{-1}B^T\Pi x-2x^T\Pi(Ax+Bu)\\ &&=u^TRu+x^T(\Pi A+A^T\Pi+C^TQC)x-2x^T\Pi Ax={\rm LHS}. \end{eqnarray*}$

Integrating the both side,

(19) $\begin{eqnarray*} J=\int_0^\infty (u+R^{-1}B^T\Pi x)^TR(u+R^{-1}B^T\Pi x)\,dt-\left[x^T\Pi x\right]_0^\infty. \end{eqnarray*}$

As $x(t)=\exp(A_Ft)x(0)\rightarrow 0\ (t\rightarrow\infty)$ ,

(20) $\begin{eqnarray*} J=\int_0^\infty (u+R^{-1}B^T\Pi x)^TR(u+R^{-1}B^T\Pi x)\,dt+x^T(0)\Pi x(0). \end{eqnarray*}$

Therefore, it is shown that $u=-R^{-1}B^T\Pi x$ minimizes the criterion function.

Exercise 1
Given $A=\left[\begin{array}{cc} 0 & 1 \\ 0 & 0 \end{array}\right],\ B=\left[\begin{array}{cc} 0 \\ 1 \end{array}\right],\ C=\left[\begin{array}{cc} 1 & 0 \\ 0 & 1 \end{array}\right], Q=\left[\begin{array}{cc} 1 & 0 \\ 0 & 1 \end{array}\right],R=1$ , obtain the solution $\Pi= \left[\begin{array}{cc} \pi_1 & \pi_3 \\ \pi_3 & \pi_2 \\ \end{array}\right]>0\Leftrightarrow \pi_1>0,\ \pi_1\pi_2-\pi_3^2>0$ of the Riccati equation $\Pi A+A^T\Pi-\Pi BR^{-1}B^T\Pi+C^TQC=0$ and calculate $F=R^{-1}B^T\Pi$ .

Expanding the Ricatti equation

(21) $\begin{eqnarray*} && \left[\begin{array}{cc} \pi_1 & \pi_3 \\ \pi_3 & \pi_2 \end{array}\right] \left[\begin{array}{cc} 0 & 1 \\ 0 & 0 \end{array}\right] + \left[\begin{array}{cc} 0 & 0 \\ 1 & 0 \end{array}\right] \left[\begin{array}{cc} \pi_1 & \pi_3 \\ \pi_3 & \pi_2 \end{array}\right] \nonumber\\ &&-\left[\begin{array}{cc} \pi_1 & \pi_3 \\ \pi_3 & \pi_2 \end{array}\right] \left[\begin{array}{c} 0 \\ 1 \end{array}\right] \left[\begin{array}{cc} 0 & 1 \end{array}\right] \left[\begin{array}{cc} \pi_1 & \pi_3 \\ \pi_3 & \pi_2 \end{array}\right] + \left[\begin{array}{cc} 1 & 0 \\ 0 & 1 \end{array}\right] = \left[\begin{array}{cc} 0 & 0 \\ 0 & 0 \end{array}\right] \nonumber , \end{eqnarray*}$

we have

(22) $\begin{eqnarray*} \left\{ \begin{array}{l} -\pi_3^2+1=0\\ \pi_1-\pi_2\pi_3=0\\ 2\pi_3-\pi_2^2+1=0 \end{array} \right. \nonumber \end{eqnarray*}$

There are the two solutions on $\pi_3$ from $-\pi_3^2+1=0$ , the two solutions on $\pi_2$ from $2\pi_3-\pi_2^2+1=0$ , the one solution $\pi_1$ from $\pi_1-\pi_2\pi_3=0$ . Therefore, we have the four kinds of solution $(\pi_1,\pi_2,\pi_3)$ as follows:

(23) $\begin{eqnarray*} \left\{\begin{array}{llll} \pi_3= 1 & \Rightarrow \left\{\begin{array}{l} \pi_2= \sqrt{3}\\ \pi_2=-\sqrt{3} \end{array}\right. & \left.\begin{array}{l} \Rightarrow\pi_1= \sqrt{3}\\ \Rightarrow\pi_1=-\sqrt{3} \end{array}\right. & \left.\begin{array}{ll} (1) &○\\ (2) &× \end{array}\right. \\ \pi_3=-1 & \Rightarrow \left\{\begin{array}{llll} \pi_2= j \\ \pi_2=-j \end{array}\right. & \hspace{-3mm} \left.\begin{array}{l} \Rightarrow\pi_1=-j\\ \Rightarrow\pi_1=j \end{array}\right. & \left.\begin{array}{ll} (3) &×\\ (4) &× \end{array}\right. \end{array}\right. . \end{eqnarray*}$

The only (1) satisfies $\pi_1,\pi_2>0,\ \pi_1\pi_2-\pi_3^2>0$ . Therefore we have

(24) $\begin{eqnarray*} F= \left[\begin{array}{cc} 0 & 1 \end{array}\right] \left[\begin{array}{cc} \pi_1 & \pi_3 \\ \pi_3 & \pi_2 \end{array}\right]= \left[\begin{array}{cc} \pi_3 & \pi_2 \end{array}\right]= \left[\begin{array}{cc} 1 & \sqrt{3} \end{array}\right] . \end{eqnarray*}$

●How to solve ARE
Given the Riccati equation $\Pi A+A^T\Pi-\Pi BR^{-1}B^T\Pi+C^TQC=0$ , consider the Hamilton matrix

(25) $\begin{eqnarray*} {M=\left[\begin{array}{cc} A & -BR^{-1}B^T \\ C^TQC & -A^T \end{array}\right]}. \end{eqnarray*}$

The eigenvalues of the Hamilton matrix with size $n\times n$ are distributed symmetrically to not only the real axis but also the imaginal axis. So there are $n$ stable eigenvalues and $n$ unstable eigenvalues. The eigenvectors corresponding to $n$ stable eigenvalues $\lambda_1,\cdots,\lambda_n$ are obtained as follows:

(26) $\begin{eqnarray*} \underbrace{ \left[\begin{array}{cc} A & -BR^{-1}B^T \\ -C^TQC & -A^T \end{array}\right]}_{M(2n\times 2n)} \underbrace{ \left[\begin{array}{c} V_1 \\ V_2 \end{array}\right]}_{V^-(2n\times n)} = \underbrace{ \left[\begin{array}{c} V_1 \\ V_2 \end{array}\right]}_{V^-(2n\times n)} \underbrace{ {\rm diag}\{\lambda_1,\cdots,\lambda_n\} }_{\Lambda^-(n\times n)}. \end{eqnarray*}$

Based on this, we can obtain $\Pi$ as

(27) $\begin{eqnarray*} \Pi=V_2V_1^{-1}. \end{eqnarray*}$

A program by SCILAB to solve the Riccati equation is given as follows.

//opt.sce
function [F,p]=opt(A,B,C,Q,R)
W=R\B’; [V,R]=spec([A -B*W;-C’*Q*C -A’]);
p=diag(R); [dummy,index]=gsort(real(p));
n=size(A,1); j=index(n+1:n+n);
p=p(j); V1=V(1:n,j); V2=V(n+1:n+n,j);
X=real(V2/V1); F=W*X;
endfunction
//eof

Solve the Riccati equation in Exercise 1 by using the above program.

A=[0 1;0 0]; B=[0;1]; C=eye(2,2);
Q=diag([1 1]); R=1;
[F,p]=opt(A,B,C,Q,R)
poles=spec(A-B*F)

Exercise 2
Consider the following spring-connected carts as a control object.

The motion is governed by

(28) $\begin{eqnarray*} \left\{\begin{array}{l} m_1\ddot{x}_1(t)=-k(x_1(t)-x_2(t))+f_1(t) \\ m_2\ddot{x}_2(t)=-k(x_2(t)-x_1(t))+f_2(t) \end{array}\right. , \end{eqnarray*}$

where $k$ is a spring constant with the range $0.5<k<2$ . The state equation and output equation are given by

(29) $\begin{eqnarray*} \underbrace{ \left[\begin{array}{c} \dot{x}_1(t) \\ \dot{x}_2(t) \\ \ddot{x}_1(t) \\ \ddot{x}_2(t) \\ \end{array}\right] }_{\dot{x}(t)} &=& \underbrace{ \left[\begin{array}{cccc} 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 1 \\ -\frac{k}{m_1} & -\frac{k}{m_1} & 0 & 0 \\ \frac{k}{m_2} & -\frac{k}{m_2}& 0 & 0 \end{array}\right] }_{A} \underbrace{ \left[\begin{array}{c} {x}_1(t) \\ {x}_2(t) \\ \dot{x}_1(t) \\ \dot{x}_2(t) \\ \end{array}\right] }_{x(t)}\\ &&+ \underbrace{ \left[\begin{array}{cc} 0 & 0 \\ 0 & 0 \\ \frac{k}{m_1} & 0 \\ 0 & \frac{k}{m_2} \end{array}\right] }_{B} \underbrace{ \left[\begin{array}{c} f_1(t) \\ f_2(t) \end{array}\right] }_{u(t)}, \end{eqnarray*}$

(30) $\begin{eqnarray*} \underbrace{ \left[\begin{array}{c} x_1(t) \\ x_2(t) \end{array}\right] }_{y(t)} &=&- \underbrace{ \left[\begin{array}{cccc} 1 & 0 & 0 & 0 \\ 0 & 1 & 0 & 0 \end{array}\right] }_{C} \underbrace{ \left[\begin{array}{c} {x}_1(t) \\ {x}_2(t) \\ \dot{x}_1(t) \\ \dot{x}_2(t) \\ \end{array}\right] }_{x(t)}. \end{eqnarray*}$

The control purpose is to regulate the zero-input response under the initial condition:

(31) $\begin{eqnarray*} \left[\begin{array}{c} {x}_1(0) \\ {x}_2(0) \\ \dot{x}_1(0) \\ \dot{x}_2(0) \\ \end{array}\right] = \left[\begin{array}{c} 0 \\ 0 \\ 0 \\ \frac{k}{m_2} \end{array}\right] \end{eqnarray*}$

by using the state feedback:

(32) $\begin{eqnarray*} \underbrace{ \left[\begin{array}{c} f_1(t) \\ f_2(t) \end{array}\right] }_{u(t)} &=&- \underbrace{ \left[\begin{array}{cccc} g_{11} & g_{12} & g_{13} & g_{14} \\ g_{21} & g_{22} & g_{23} & g_{24} \end{array}\right] }_{F} \underbrace{ \left[\begin{array}{c} {x}_1(t) \\ {x}_2(t) \\ \dot{x}_1(t) \\ \dot{x}_2(t) \\ \end{array}\right] }_{x(t)}. \end{eqnarray*}$

In order to determine the state feedback gain $F$ , for $k$ fixed to an appropriate nominal value $k^*$ , we will minimize the following criterion function:

(33) $\begin{eqnarray*} J=\int_0^\infty (q_1^2x_1^2(t)+q_2^2x_2^2(t)+r_1^2f_1^2(t)+r_2^2f_2^2(t))\,dt, \end{eqnarray*}$

that is

(34) $\begin{eqnarray*} J&=&\int_0^\infty ( \underbrace{ \left[\begin{array}{c} x_1(t) \\ x_2(t) \end{array}\right]^T }_{y^T(t)} \underbrace{ \left[\begin{array}{cc} q_1^2 & 0 \\ 0 & q_2^2 \ \end{array}\right] }_{Q} \underbrace{ \left[\begin{array}{c} x_1(t) \\ x_2(t) \end{array}\right] }_{y(t)}\\ &&+\underbrace{ \left[\begin{array}{c} f_1(t) \\ f_2(t) \end{array}\right]^T }_{u^T(t)} \underbrace{ \left[\begin{array}{cc} r_1^2 & 0 \\ 0 & r_2^2 \ \end{array}\right] }_{R} \underbrace{ \left[\begin{array}{c} f_1(t) \\ f_2(t) \end{array}\right] }_{u(t)} )\,dt. \end{eqnarray*}$

For example, the closed-loop zero-input response is simulated under the following assumptions:

1) $m_1=1$ , $m_2=1$ , $k^*=1$ ,
2) $q_1=\frac{1}{M_{x_1}}=1$ , $q_2=\frac{1}{M_{x_2}}=1$ , $r_1=\frac{\rho}{M_{f_1}}=1$ , $r_2=\frac{\rho}{M_{f_2}}=1$
( $M_{x_1}=M_{x_2}=1, M_{f_1}=M_{f_2}=1,\ \rho=1$ ).

Appendix 1

Check the following properties on matrix trace.

(35) $\begin{eqnarray*} {\rm tr}AB={\rm tr}BA, \end{eqnarray*}$

(36) $\begin{eqnarray*} \frac{\partial}{\partial X}{\rm tr}AXB=A^TB^T, \end{eqnarray*}$

(37) $\begin{eqnarray*} \frac{\partial}{\partial X}{\rm tr}AX^TB=BA, \end{eqnarray*}$

(38) $\begin{eqnarray*} \frac{\partial}{\partial X}{\rm tr}AXBX^T=A^TXB^T+AXB \end{eqnarray*}$

where $\displaystyle{{\rm tr}A=\sum_{i}a_{ii}}$ for $A=[a_{ij}]_{i,j=1,\cdots,n}$ . In fact,

(39) $\begin{eqnarray*} \sum_{i}[AB]_{ii}=\sum_{i}\sum_{j}a_{ij}b_{ji} =\sum_{j}\sum_{i}b_{ji}a_{ij}=\sum_{j}[BA]_{jj}, \end{eqnarray*}$

(40) $\begin{eqnarray*} &&[\frac{\partial}{\partial X}{\rm tr}AXB]_{ij} =\frac{\partial}{\partial x_{ij}}\sum_{k}[AXB]_{kk} =\frac{\partial}{\partial x_{ij}}\sum_{k}\sum_{i,j}a_{ki}x_{ij}b_{jk}\\ &&=\sum_{k}b_{jk}a_{ki}=[BA]_{ji}=[A^TB^T]_{ij}, \end{eqnarray*}$

(41) $\begin{eqnarray*} &&[\frac{\partial}{\partial X}{\rm tr}AX^TB]_{ij} =\frac{\partial}{\partial x_{ij}}\sum_{k}[AX^TB]_{kk} =\frac{\partial}{\partial x_{ij}}\sum_{k}\sum_{i,j}a_{ki}x_{ji}b_{jk}\\ &&=\sum_{k}b_{ik}a_{kj}=[BA]_{ij}, \end{eqnarray*}$

(42) $\begin{eqnarray*} &&\frac{\partial}{\partial X}{\rm tr}AXBX^T =\frac{\partial}{\partial X}{\rm tr}(AXB)X^T +\frac{\partial}{\partial X}{\rm tr}AX(BX^T)\\ &&=AXB+A^TXB^T. \end{eqnarray*}$

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$ の固有値を計算して確かめよ。