Optimization of triple inverted pendulum control process based on motion vision

Abstract

An inverted pendulum is a typical nonlinear and absolutely unstable system. In order to control the triple inverted pendulum effectively and steadily, an optimization method of inverted pendulum control process based on motion vision was proposed. The real-time motion pictures of the triple inverted pendulum in the swinging-up process were collected through the CCD camera, and the real-time motion pictures of the triple inverted pendulum were recognized, matched and optimized by using Harris algorithm. By using motion vision to control and optimize the triple inverted pendulum, the stabilization control of the triple inverted pendulum was realized.

1 Introduction

The inverted pendulum is the model of the most balanced control and the lack of driving control system, which is widely used in teaching and theoretical research. Therefore, for many years, people have been working on the problem of inverted pendulum control in theory and function. The theory of control operation and the method of control will have a great promotion and positive significance for the industrial production. Automatic control is a highly theoretical subject and it is a very important engineering technology. The engineering technology theory mentioned in the literature [1] was “automatic control theory.” In the scope of the improvement of the control theory, it is not feasible to use the theory in reality without proving the correctness of a theory. It is necessary to design the controller according to its theory in order to achieve the true value of the theory [2, 3]. In literature [4], it was pointed out that the inverted pendulum, as a target of experiment, is a system with multiple variables, rapid transformation, nonlinearity, and poor stability. We can add a control algorithm to increase its stability to prove the processing ability of the control method for complex, unstable, and nonlinear systems. At the same time, how this method embodies the difficult problems in a series of automatic control categories, such as robustness, stabilization, and tracking in the process of controlling the system, could also be reflected in the process of controlling the inverted pendulum movement. Therefore, it was concerned by the scientists. Using different control algorithms, different types of inverted pendulum control have become a worldwide research topic with high research value [5, 6].

The fuzzy control theory was used to control the inverted pendulum in the literature [7], when the fuzzy control method was used to analyze the inverted pendulum problems, the mathematical theory was not used to analyze the nonlinear factors in the inverted pendulum system and the influence of them on the controller, but was used to analyze all kinds of unstable conditions caused by the method in the control of the inverted pendulum, according to the controller’s experience and the previous experience. Finally, the mode of the fuzzy control theory set was used to express the processing methods. However, the fuzzy control theory did not have a concise and practical way to deal with multivariable problems. Therefore, the analysis of the triple inverted pendulum system was not entirely applicable.

The neural network was applied to the inverted pendulum control system and developed rapidly in the early 1990s. It was mentioned in the literature [8] that the neural network could arbitrarily approach the complex nonlinear relationship as much as possible, could learn to train data samples and was suitable for the absolutely unstable inverted pendulum control system, and could use the characteristics that all the characteristic information with a certain quantity or a certain nature could be evenly stored in each layer of neurons in the network, so this method had strong robustness and capacity. The back-propagation neural network could also be combined with the Q learning algorithm, and was used as the non-discrete model to control learning of inverted pendulum. However, due to the sensitivity of the neural network system and slow convergence rate, the computation of objective function was very complex and time consuming.

In view of the incomplete research of the inverted pendulum control system by the above methods, the triple inverted pendulum control system based on visual difference feedback was proposed. The vision sensor camera was used to collect the image information during the inverted pendulum motion, and the image corner matching was applied to monitor the inverted pendulum in real time. Then, the mathematical model of the triple inverted pendulum control system was constructed, and the balance control of the inverted pendulum system was carried out through the linear quadratic optimal control theory [9, 10].

2 Methods—optimization of triple inverted pendulum system control process

2.1 Inverted pendulum control system of motion vision

After collecting the motion image information of triple inverted pendulum by visual sensor, we set up an application network remote control platform. The triple inverted pendulum was mainly composed of car, pendulum 1, pendulum 2, and pendulum 3, and there were free links between them. The car could move horizontally on the horizontal guide rail, and the pendulum bar could move freely in the vertical plane. The motion analysis of the system diagram was shown in Fig. 1.

Fig. 1

Schematic diagram of the three level inverted pendulum

Firstly, we used a camera to collect image information of inverted pendulum and make the image preprocessing. Then, we selected the annotation template and placed it in the same plane, collected the template images from different angles, detected the feature points (corner points) in the image, obtained the internal and external parameters of the camera, and optimized the refinement.

Secondly, we extracted the image feature point information. The corner detection method was based on Harris corner monitoring and matching method. For the pixels (x,y) on the image, the image window W (W(x,y) was the image window function) should be moved on the quantification (u,v). If we wanted to define the energy function E, the formula (1) was shown as follows.

$$ E\left(u,v\right)=\sum \limits_{x,y}W\left(x,y\right){\left[I\left(x+u,y+v\right)-I\left(x,y\right)\right]}^2 $$

(1)

The Harris corner was based on the grayscale images, so I(x,y) was represented the gray value of images. Taylor expansion method was applied to deal with small quantities, so when (u,v) was small, E could be represented by the following formula (2).

$$ E\left(u,v\right)\approx \left[u,v\right]M\left[\begin{array}{c}u\\ {}v\end{array}\right] $$

(2)

Among formula (3),

$$ {\displaystyle \begin{array}{l}M=\sum \limits_{x,y}W\left(x,y\right)\otimes \left[\begin{array}{cc}{I}_x^2& {I}_x{I}_y\\ {}{I}_x{I}_y& {I}_y^2\end{array}\right]=\\ {}\left[\begin{array}{cc}\sum \limits_{x,y}G\otimes {I}_x^2& \sum \limits_{x,y}G\otimes {I}_x{I}_y\\ {}\sum \limits_{x,y}G\otimes {I}_x{I}_y& \sum \limits_{x,y}G\otimes {I}_y^2\end{array}\right]\end{array}} $$

(3)

${I}_x=\frac{\partial I}{\partial x}$ and ${I}_y=\frac{\partial I}{\partial y}$ respectively represented the first-order differential step degree of the image in the direction of x and y. The image had noise when it was obtained and conveyed, so there were many methods to increase the level of noise resistance. Gauss smoothing filter for anti-noise processing was used in this paper, and W was the Gauss template function. The expression was shown in formula (4).

$$ W\left(x,y\right)=G=\exp \left[-\frac{1}{2{\sigma}^2}\left({x}^2+{y}^2\right)\right] $$

(4)

E was close to local autocorrelation function. Assuming that λ1 and λ2 were the eigenvalues of matrix M, we could use λ1 and λ2 to determine the corner. R was defined as the interest value of the corresponding pixel points; the formula was shown as follows in formula (5).

$$ R=\det\ \mathrm{M}-k{\left(\mathrm{traceM}\right)}^2 $$

(5)

Among them, k was the constant, and according to the experience, the value was 0.04–0.06, trace M = λ1 + λ2 represented the direct trace of the matrix, and det M = λ1λ2 was the determinant of the matrix. Then, the basis for obtaining Harris corner was that the R value of a pixel in a frame was the best in other areas, which was greater than the set threshold R₀.

There was a phenomenon of rotation and deviation in the two images. Under the condition of obtaining the corner with the same threshold R₀, if the corner set of the overlapping parts of the image was set to A and A′, the necessary condition for the image matching was given first:

• 1. The number of corners should be the same, A = {a₁,a₂,…,a_n} and A′ = {a₁′, a₂′,…, a_n′}had the same number of corners;

• 2. a₁ and a₁′ were the corresponding corners, then they had the same corner value R(a₁) = r(a₁′);

• 3. If a₁ and a₁′ were the corresponding corner points, their number of adjacent corner points should be equal, that was N_r(a₁) = N_r(a₁′), r represented the neighborhood radius;

• 4. If {a₁,a_i} εA, {a₁′,a_i′} εA were the corresponding corner point, then their distance was equal d(a₁,a_i) = d(a₁′,a_i′);

• 5. If a_i and a_j were corresponding corner points, a_i′ and a_j′ were other corresponding corner points, {a₁,a_i} εA, {a₁′,a_i′} εA. According to a_i, a_j, a_i′, and a_j′, the matching parameter was calculated by the following formula (6).

$$ {P}_{ij}=\left(\Delta {x}_{ij},\Delta {y}_{ij},\Delta {\theta}_{ij}\right) $$

(6)

△x_ij and △y_ij were the translational quantities of the image waiting for matching for the matching image on x and y axis, △Ѳ_ij was the angle of rotation. If the matching parameter P_ij of any matching corner point in the A and A′ set was the constant vector, then the two images matched completely.

2.2 Motion control optimization of triple inverted pendulum system

Firstly, we constructed the mathematical model of triple inverted pendulum system according to Lagrange equation and set r as the motion displacement of horizontal guide rail, and Ѳ₁, Ѳ₂, and Ѳ₃ were respectively the angles of the lower, middle, and upper pendulums relative to the vertical direction. The Lagrange equation for the triple inverted pendulum system with conservative force and dissipative force were shown in the following formula (7).

$$ \frac{d}{dt}\left(\frac{\partial T}{\partial {\dot{q}}_i}\right)-\frac{\partial T}{\partial {\dot{q}}_i}+\frac{\partial V}{\partial {q}_i}+\frac{\partial D}{\partial {\dot{q}}_i}={F}_{q_i} $$

(7)

In the formula, q_i represented the generalized coordinate, that were r, Ѳ₁, Ѳ₂, Ѳ₃; F_qi was the non-potential generalized force, when q_i = r, F_qi = G₀U,U was the controlled quantity, G₀ and was the gain constant. When q_i = Ѳ₁,Ѳ₂, and Ѳ₃, F_qi = 0, T, V, and D were respectively the function, potential energy, and dissipative energy of the system, then $T=\sum \limits_{i=0}^n{T}_i$, $V=\sum \limits_{i=0}^n{V}_i$, and $D=\sum \limits_{i=0}^n{D}_i$. Among them, n was the stage number of the inverted pendulum(n = 3); T_i, V_i, and V_i were kinetic energy, potential energy, and dissipative energy of cars and all levels of pendulum. The mathematical model could be obtained by putting T_i,V_i and D_i(i = 0, 1, 2, 3) into the formula (8).

$$ {\displaystyle \begin{array}{l}M\left({\theta}_1,{\theta}_2,{\theta}_3\right)\left[\begin{array}{c}{r}^n\\ {}{\theta_1}^n\\ {}{\theta_2}^n\\ {}{\theta_3}^n\end{array}\right]+N\left({\theta}_1,{\theta}_2,{\theta}_3,{\dot{\theta}}_1,{\dot{\theta}}_2,{\dot{\theta}}_3\right)\\ {}\kern5.699997em \times \left[\begin{array}{c}{r}^{\prime}\\ {}{\theta_1}^{\prime}\\ {}{\theta_2}^{\prime}\\ {}{\theta_3}^{\prime}\end{array}\right]=G\left(u,{\theta}_1,{\theta}_2,{\theta}_3\right)\end{array}} $$

(8)

Formula (8) was a nonlinear vector differential equation. When the system worked, it moved on the edge of the equilibrium position. In the formula, when u = 0, it was linearized near the equilibrium position $r={\theta}_1={\theta}_2={\theta}_3=\dot{r}={\dot{\theta}}_1={\dot{\theta}}_2={\dot{\theta}}_3=0$ instead of the nonlinear vector differential equation.

Secondly, a linear quadratic optimal control was introduced to improve the triple inverted pendulum system.

• 1. The pendulum bars were linked by hinges without any constraints, so it was difficult to control the attitude in the initial swing. Therefore, in order to control the state balance of the triple inverted pendulum in the initial swing process, we used the finite-time time invariant LQ (liner quadratic) problem to achieve the optimal control.

$$ \left\{\begin{array}{l}-\dot{P}(t)=P(t)A+{A}^TP(t)+Q(t)\\ {}\kern4.099998em -P(t){BR}^{-1}(t){B}^TP(t)\\ {}P\left({t}_f\right)=S,t\in \left[{t}_0,{t}_f\right]\end{array}\right. $$

(9)

In the formula (9), A and B were the coefficient matrices of the corresponding dimensions, S was the weighting matrix, and t was the adjusting time. The solution matrix P(t) was n × n positive semi definite symmetric matrix, then the necessary and sufficient conditions of the optimal control $\dot{u}\left(\bullet \right)$ were shown in formula (10).

$$ \dot{u}(t)=-\dot{K}(t)\dot{x}(t),\dot{K}(t)={R}^{-1} BP(t) $$

(10)

The optimal trajectory $\dot{x}\left(\bullet \right)$ was shown in formula(11).

$$ \dot{x}(t)=A\dot{x}(t)+B\dot{u}(t),\dot{x}\left({t}_0\right)={x}_0 $$

(11)

The optimum performance value $\dot{J}=J\left(\dot{u}\left(\bullet \right)\right)$ was shown in formula (12).

$$ \dot{J}=\frac{1}{2}{x}_0^TP\left({t}_0\right){x}_0,\forall {x}_0\ne 0 $$

(12)

The optimal control system was adjusted by finite-time time invariant LQ, and its control state was observed. The feedback was that the inverted pendulum was stabilized and the jitter time was short in the process from swinging up to steady state.

• 2. The other key link of the triple pendulum automatic swing was the stable state entering the inverted position after swinging up. This area was a balance control problem. If it could not be effectively controlled, it would lead to the failure of the whole control system. Therefore, the control tending to steady state was based on the infinite-time LQ to adjust the optimal control. The sufficient and necessary condition for the given infinite-time time invariant LQ to regulate optimal control was shown as follows in formula (13).

$$ \dot{u}(t)=-\dot{K}(t)\dot{x}(t),\dot{K}={R}^{-1}{B}^TP $$

(13)

The equation of state for the optimal trajectory $\dot{x}(t)$ was shown in formula (14).

$$ \dot{x}(t)=A\dot{x}(t)+ Bu,\dot{x}\left({}_0\right)={x}_0 $$

(14)

The optimal performance value $\dot{J}=J\left(\dot{u}\left(\bullet \right)\right)$ was shown in formula (15).

$$ \dot{J}={x}_0^T{Px}_0,\forall {x}_0\ne 0 $$

(15)

• 3. Two-point boundary value problem: The triple pendulum control system was in the limited time t∈[0,T], and the following boundary value conditions was satisfied to the process from static sag to vertical inverted position.

$$ \mathrm{y}\left(\mathrm{t}\right){\left|{}_{\mathrm{t}=0,\mathrm{T}}=0,\dot{\mathrm{y}}(t)\right|}_{\mathrm{t}=0,\mathrm{T}}=0 $$

(16)

$$ \theta (0){\left|={\left[\pi \kern0.5em \pi \kern0.5em \pi \right]}^{\mathrm{T}},\dot{\theta}(t)\right|}_{\mathrm{t}=0,\mathrm{T}}=0 $$

(17)

Among them, t = 0 was the initial value condition, and t = T was the final value condition. In addition, according to the performance of the hardware, and the high sensitivity and strong instability of the triple pendulum control system, the action of the car must also satisfy the following constraints during the hoisting process.

$$ \left|\mathrm{y}\right|\le 1.5\mathrm{m},\left|\dot{\mathrm{y}}\right|\le 5\mathrm{m}/\mathrm{s},\left|\ddot{\mathrm{y}}\right|\le 20\mathrm{m}/{\mathrm{s}}^2 $$

(18)

${\overline{\theta}}^{\ast }={\left[{\theta}^{\ast}\kern0.5em {\dot{\theta}}^{\ast}\right]}^{\mathrm{T}}=\left[{\theta}_1^{\ast}\kern0.5em {\theta}_2^{\ast}\kern0.5em {\theta}_3^{\ast}\kern0.5em {\dot{\theta}}_1^{\ast}\kern0.5em {\dot{\theta}}_2^{\ast}\kern0.5em {\dot{\theta}}_3^{\ast}\right]$ was appointed, the first-order ordinary differential equations could be obtained was shown in formula (19).

$$ {\dot{\overline{\theta}}}^{\ast }=\overline{\beta}\left({\theta}^{\ast },{\dot{\theta}}^{\ast },{\ddot{y}}^{\ast}\right)=\left[{\theta}_1^{\ast}\kern0.5em {\theta}_2^{\ast}\kern0.5em {\theta}_3^{\ast}\kern0.5em {\dot{\theta}}_1^{\ast}\kern0.5em {\dot{\theta}}_2^{\ast}\kern0.5em {\dot{\theta}}_3^{\ast}\right] $$

(19)

Where the vector function $\overline{\beta}={\left[{\overline{\beta}}_1\kern0.5em {\overline{\beta}}_2\kern0.5em {\overline{\beta}}_3\kern0.5em {\overline{\beta}}_4\kern0.5em {\overline{\beta}}_5\kern0.5em {\overline{\beta}}_6\right]}^{\mathrm{T}}$, the edge value condition were shown in formula (19) and formula (20).

$$ {\theta}^{\ast }(0)={\left[\pi \kern0.5em \pi \kern0.5em \pi \right]}^{\mathrm{T}},{\dot{\theta}}^{\ast }(0)=0 $$

(20)

$$ {\theta}^{\ast}\left(\mathrm{T}\right)=0,{\dot{\theta}}^{\ast }(T)=0 $$

(21)

Obviously, ordinary differential Eq. (19) and boundary value condition (20), (21) were formed a nonlinear two-point boundary value problem.

• 4. Reference ground track: The design method of the nominal trajectory y* = γ(t,p) was not unique, polynomial functions or cosine series could be selected. In general, the input u*(t) was required to be continuous, so γ(t,p) must meet the demands of quadratic differential if α = 2.

We should combine the finite-time and infinite-time invariant LQ regulation to improve the control, so that the system could reduce the attitude adjustment time in the automatic swing-up transition process and could quickly enter the stable stage of the automatic swinging up. After entering the stable stage, the inverted control of the triple pendulum was also the key stage. Through the adjustment of the infinite-time time invariant LQ, the system would maintain a steady and gradual process in a state tending to equilibrium.

3 Analysis of the result of optimization process

The inverted pendulum system was a very unstable system. In order to stabilize the state of the automatic swing-up transition stage and steady swing phase during the control process, the finite-time and infinite-time invariant LQ were used to adjust the optimal control to improve the triple inverted pendulum system in this paper. In order to prove the effectiveness of the improved method in the aspect of stability control, we carried out experiments in MATLAB simulation environment, and waw collected datum and analyzed them. In the process of control, the moving effect of the pendulum and car in the triple inverted pendulum was presented visually and vividly in the form of animation in Fig. 2. And we could observe that many directions control the motion of the rocker and the car in the process of the controller to control the whole process of mobile, and it was a very vivid triple inverted pendulum visual simulation platform.

Fig. 2

3D animation simulation results of the control system

4 Results and discussions

4.1 Experiment 1

In automatic swing-up transition phase, in order to control the accurate swing of cars and pendulum bars, the finite-time time invariant LQ was used to adjust the optimal control, then we could achieve the steady state of the pendulum bars in the initial swing process, after the system was adjusted by the method in the text and literature [7] method, they gradually reached a stable state. The following table was the control time data of automatic swing-up adjustment when the pendulum bars by two methods starts to swing was shown in Table 1.

Table 1 The automatic swing-up adjustment control schedule

From data analysis of Table 1, the two methods adjust the initial swing stage of the inverted pendulum simultaneously. It could be seen from the data that the adjustment time of the literature [7] method was too long, so the stability of the control system was not strong. According to Table 1, we could get the adjustment time curve figure of the inverted pendulum of two methods, as shown in Fig. 3.

Fig. 3

Comparison of adjustment time curves of pendulum bars of two methods.

From the two curves of Fig. 3, it was observed that the time consuming of the inverted pendulum from the initial pendulum to the steady motion of the pendulum bar was about 7–8 s, while the time consuming of the literature [7] was about 25 s. Therefore, the control performance of the initial pendulum stage of the method in this paper was better.

In the process of inverted pendulum adjustment, the swing angle radian would be generated. From Figs. 3 to 4, we would analyze the swing angle radian in the initial swing time of the inverted pendulum controlled by two methods.

Fig. 4

The radian adjustment figure of the inverted pendulum by the method of literature

From Figs. 4 and 5, we could see the difference of the swing radian in the swinging-up stage of the inverted pendulum. After using the method in this paper to control the inverted pendulum, the swing angle radian of initial swing was much less than that of literature [7] method, and it was indicated that the method in this paper was effective for the triple inverted pendulum in the initial swing stage.

Fig. 5

The radian adjustment figure of the inverted pendulum by the method in this paper.

4.2 Experiment 2

Stable swing stage: For the stable swing stage of the triple inverted pendulum from initial swing stage to the steady motion, the swing state was the control of the balance. Figure 6 was the comparison of the optimal control curve given by the system and the curves of the steady swing stage of two methods.

Fig. 6

Comparison chart of the movement velocity of the steady swing stage of the inverted pendulum

The motion velocity of inverted pendulum in steady swing stage could be analyzed from Fig. 6, the motion velocity of the inverted pendulum controlled by the method in this paper was close to the optimal speed of the system, and the goodness of fit was better. The motion speed of literature [7] algorithm was faster, and it was indicated that the inverted pendulum was always in a state of quivering.

4.3 Experiment 3

When we constructed the triple inverted pendulum control model, at first, we set an initial displacement angle for triple inverted pendulum, and the starting position of the three-stage inverted pendulum car was given by the simulation control system to the force F in a uniform direction at the bottom of the car. Within 20 s, we recorded the motion state and the degree of pendulum and kept the car in a set of orbit range (0 to 10 m), then kept the third movement that did not exceed the scope of the above a predetermined angle.

The initial object of the three-stage inverted pendulum system was set in the same range and randomly selected in the above range. Through constant updates, the control system could control the inverted pendulum, so the time of its movement was within the prescribed limits. The time period and sampling frequency were 0.5 s to ensure that the three-stage inverted pendulum could be maintained in a certain angle. In the case of white noise signal interference and shock signal interference, the stability of the system was maintained, and the robustness was strong. The range and control of the upper, middle and lower positions with white noise and impulse were shown in Fig. 7a–c.

Fig. 7

The control curve of the triple inverted pendulum

With the control method of this paper, the amplitude of the three-stage inverted pendulum could be stabilized at a certain time. This method had the advantages of high modeling accuracy and good controllability, which could be used to solve the problems of complex systems which were difficult to solve by traditional methods.

5 Conclusions

In this paper, the triple inverted pendulum control system based on the motion vision was proposed, and motion attitude of the inverted pendulum was monitored by vision sensor. The finite-time and infinite-time time invariant LQ control method were introduced to control the balance of the triple inverted pendulum from the initial swing stage to the steady swing stage. The experiments showed that the inverted pendulum control system designed in this paper solved the problem of changeable and unstable inverted pendulum. The parameters of the controller were further optimized on the basis of optimizing feedback control parameters; thus, we could improve the convergence speed of the system. The design of the controller accords with the goal and principle of triple inverted pendulum control system, which was an effective design scheme, and it could provide some reference value for other fields of the control theory.