Design and control of spatial inverted pendulum with two degrees of freedom

The inverted pendulum systems have inherently unstable dynamics. In order to stabilize the inverted pendulum at upright position, an actuation mechanism should generate fast-reactive motions at the pivot point of the system. This paper addressed the design and control of a spatial inverted pendulum with two degrees of freedom (DOF). The first part of the study consists of designing a novel planar two-DOF (PRRRR) actuation mechanism in order to balance the spatial inverted pendulum. The system is underactuated and has inherently extreme nonlinearity and also the restrictions on the actuators. Then, in the second part, a second-order sliding-mode and a linear quadratic Gaussian (LQG) controller have been proposed to control the pendulum within the equilibrium position. Finally the simulation results evaluated in terms of the robustness, time response and stability show that the second-order sliding-mode controller is more robust and has fast response performances in re-stabilizing the spatial inverted pendulum, while LQG controller is better in terms of keeping the system in equilibrium during the long period of time.


Introduction
In order to balance a stick on the fingertip or palm of the hand, the pose of the pivoting point needs to be rapidly moved along its falling direction. The inverted pendulum problem is based on the same principle. Just like the stick, the inverted pendulum system has the same inherently unstable dynamics. Due to their nonlinear models, need of quick response, and unstable behaviors, the inverted pendulumbased systems have served the researchers in the control field as a subtle control problem for a long time. To realize experimental models, to verify new control techniques, to improve existing control methods, and especially to represent real engineering applications, the inverted pendulum systems have become one of the main focuses for the researchers. Nonlinear inverted pendulum dynamics are utilized in some specific subjects such as modeling of human posture and gait [1], control of robotic arms used in pick and place operations [2], flight controls of rocket and missiles [3], balancing the landing modules of airplanes and helicopter and air vehicles in turbulence [4], etc.
The literature contains a very large family of inverted pendulum systems with many different configurations. Most of them are a single inverted pendulum actuated with a linear car or skidder moving in one direction [5]. Also, there are some studies based on double inverted pendulum balanced using again a single actuator [6,7]. The common point of these inverted pendulum systems mentioned above is that the number of actuators is less than the degrees of freedom of the system. In other words, they are deficient or underactuated [8].
The other inverted pendulum group, which is also the subject of this paper, is the spatial inverted pendulums with two degrees of freedom. Unlike many studies [9,10] in which the rotational axes are parallel to each other and the rotational motion occurs on a single plane, the spatial inverted pendulum is designed so that two rotational axes are orthogonal to each other and the pendulum is prevented from rotating around its own axis and can easily tilt over any direction in 3D space. Spatial inverted pendulum systems are classified in three basic groups depending on the mechanism used to stabilize the pendulum [11]. The studies of Soto [12] and Ishii [11] can also be added in these three categories as a fourth type. The first type among this classification is a system where a spatial inverted pendulum connected to the X-Y table is controlled by applying linear motions in x and y directions in horizontal plane parallel to the ground [13]. The spatial inverted pendulum systems mounted on a multidirectional vehicle can be mentioned as the second type. In these systems, unlike the X-Y table, the same planar motion is obtained by using wheels that can move in more than one direction. Another difference from the X-Y table systems is that the pendulum can turn around its own axis [14,15]. In the third type, robotic arms with serial configurations are used to balance a spatial pendulum [16,17]. In the fourth type, a spatial inverted pendulum is mounted on a five-bar planar mechanism highly studied in the literature. In these systems, the pendulum is controlled by the motion generated through a mechanism with two rotational inputs [11,12].
As known, control methods are generally divided into model-dependent and model-free methods. In cases, where the dynamic model represents the system strongly, the methods such as sliding-mode control, model predictive control and computational torque control are used, whereas the model-free methods like artificial neural networks, fuzzy logic and standard PID methods are used to control the system where the model cannot be obtained precisely. Besides, in the literature, the PID methods are also used on model-based controllers such as the model based PID controller with Ziegler-Nichols tuning. In the studies carried out on four different types of spatial inverted pendulum mentioned previously, the controllers have been designed based on the model-free or linearized model. A PID controller was successfully applied on the X-Y table pendulum system [13]. In addition, on a very similar system, there are also some studies using sliding-mode control [18], artificial neural networks, fuzzy neural networks [19] and adaptive sliding-mode control [20]. In another study, the spatial inverted pendulum was stabilized at the unstable upright position by using the adaptive feedback controller designed with the help of the Lyapunov function [12]. Furthermore, other studies using similar setups have accomplished the stabilization via the controllers of fuzzy logic [21] and linear quadratic regulators [22]. In third type of the inverted pendulum system, the control operation was executed through a three-DOF robot arm. In this study, the controller involved two linear and one nonlinear controller based on energy and kinetic momentum [16]. In another energy based method, a feedback control method using Tereshko [23] by altering the energy of the chaotic system was presented to control an inverted pendulum with parametric errors [24]. This method converts the chaotic behavior of the pendulum into a periodic oscillation.
In control of spatial case, a spatial inverse pendulum was compensated by a serial robot arm with three DOF but planar motion. The dynamic equations were linearized to design the linear quadratic regulator (LQR) controller. There are only a few studies in literature on the spatial inverted pendulum actuated five bar mechanism. The researchers designed the controllers for the stabilization of the pendulum based on H ∞ robust control method and the linear quadratic regulator over the linearized model respectively [11,12].
In this paper, a spatial inverted pendulum was stabilized by using a novel two-DOF PRRRR (P: Prismatic joint, R: Revolute joint) planar mechanism driven with one rotational and one linear actuators. The tangible contribution of the study is the balancing an inverted pendulum by means of a novel actuation mechanism. Therefore, this mechanism presented in this paper can be regarded as the fifth type of mechanism in balancing of spatial pendulums. The reason why this mechanism is preferred is that first it can create higher accelerations at the pivot point where the spatial pendulum is mounted to the mechanism. Secondly, the nonlinear model of the pendulum system with the joint limitation has revealed an opportunity with a very complex structure to assess the performance of control methods. This study involves design of a planar PRRRR mechanism to generate the required forces to keep the spatial pendulum at equilibrium position. This design procedure is to determine the dimensions of the mechanism maximizing the workspace by using the kinematic analysis and the genetic algorithm (GA). In the second stage, the studies were implemented on the control of the inverted pendulum. Finally, the spatial inverted pendulum system was simulated by using a nonlinear method of the second-order sliding-mode control (SOSMC) and also the linear quadratic Gaussian (LQG) method through the linearization of the dynamic model. At the end of this study, the two control methods were evaluated comparatively according to performance criteria such as fast response, stability and robustness for the disturbances.

The modeling of inverted spatial pendulum system
This section addresses the construct and kinematic analysis of the PRRRR actuation mechanism and the dimensional optimization with genetic algorithm. Then, the dynamic analysis of the whole spatial inverted pendulum system and the controller design which holds the pendulum in equilibrium points will be emphasized. Here, all the joints, links and pendulum were assumed to be rigid. Figure 1 shows the CAD model of the spatial inverted pendulum and its actuation mechanism presented in this paper. This system possesses a total of 4 DOFs, two from the PRRRR mechanism and two from the spatial inverted pendulum. The pendulum has two rotational DOFs whereas the actuation mechanism has two positional DOFs for the associated pivot point. This system does not allow the self-swing control since the space which is pendulum works is limited.
The actuation mechanism has a planar structure and its four links are connected with one prismatic (P) and four revolute (RRRR) joints (Fig. 2). The first input applied to the system is a linear actuator represented by the prismatic joint, which can be driven by means of a linear gantry. For the second input, the revolute joint at the other end of the mechanism was selected as a rotational actuator. In this case, the P and R actuated joints represented by s 5 and 2 Fig. 1 The CAD model of the spatial inverted pendulum system Fig. 2 The schematic diagram of the PRRRR mechanism and spatial inverted pendulum system form as follows.
Using (1) and (2), 3 and 4 , which are passive joint variables, are carried out analytically in terms of the active joint variables, 2 and s 5 . After defining the passive joint variables, the x and y coordinates of the pivot point O j based on the active joint variables will be as follows. (1) This pivot point will be used in the workspace analysis of the mechanism.

The dimension synthesis via genetic algorithm
As previously mentioned, in order for the spatial inverted pendulum to be kept in the upright state properly, the pendulum platform point ( O j ) must have the ability to move continuously in a homogeneous workspace without singular positions. The workspace of the planar mechanism (Cartesian workspace) and the mobility of the joints on the mechanism (joint workspace) are directly related to the relative dimensions of the links of the mechanism. For this reason, an optimization study has been carried out in order to make this workspace relatively larger and to find suitable link dimensions for higher mobility. In this study, genetic algorithm has been chosen as the optimization technique in a simple way without the need for a deep search space. The genetic algorithm can be used for difficult problems that cannot be easily solved by traditional optimization methods.
The optimization was initiated primarily by the representation of dimensional parameters. A total of four-dimensional parameters, L 1 , L 2 , L 3 and L 4 were represented by a 1 × 32 population consisting of 8-bit (binary code) chromosomes. Due to the nature of the genetic algorithm, the results have been searched over the certain boundary values like For a finite number of the inputs of the mechanism, a fitness function given in (5) was established on the existence of the inverse kinematics of the mechanism and Jacobian-based condition index.
T is the number of the platform points, O j falling into a prescribed region within the reachable workspace of the mechanism. W is the weighting term and is the sum of the Local Condition Index (LCI) over the entire workspace. The local condition index is the reciprocal of the condition number. The term in the denominator has been added to ensure the workspace to be isotropic. where the Euclidean norm or 2-norm of a matrix and ‖J‖ = � the maximum eigenvalue of � J T J � .
In this searching, for a finite number of inputs ( M × N ), the valid inverse kinematics solutions for the point, O j are placed inside the specified region, while the points with no solution are excluded from this region. Thus, the elite populations with the highest cost values were chosen at each generation and then the best solutions were reached at the end of the optimization cycle. As a result of the optimization performed on the range of the actuator inputs of −300 mm ≤ s 5 ≤ 300 mm and 0 • ≤ 2 ≤ 360 • with one increment, the dimension of the links shown in Fig. 2 were obtained approximately as L 1 = 300 mm , L 2 = 100 mm , L 3 = 350 mm , L 4 = 200 mm . Figure 3 depicts the dexterous workspace of the mechanism, which is constructed by the density of the platform points on the workspace. Meanwhile, the joint space shown in Fig. 4 is complete with these dimensions. This figure indicates that for each value of 2 between 0 • and 360 • , the mechanism can reach all values of s 5 in the range of −300 mm ≤ s 5 ≤ 300 mm so that the inverted pendulum can move without blocking in all directions in the Cartesian workspace. If the dimensions of the mechanism were not chosen properly, serious gaps would occur within this workspace. In other words, there is no solution for the joint variables corresponding to these empty regions, i.e., the mechanism with blocking.

Dynamic analysis
Dynamic equations must be obtained for the design of model dependent controllers that are divided into two linear and nonlinear models. The equations obtained for the design of a nonlinear controller are used directly; on the other hand, the dynamic equations for linear controller design need to be linearized in the unstable upright position of the inverted pendulum. There are many methods to study the dynamic analysis of the systems in the literature, one of which is the virtual work method that eliminates the reaction forces and does not need the constraint equations too.
In this section, the dynamic equations of the spatial inverted pendulum system shown in Fig. 2 were firstly obtained by the virtual work method, and the verification of these equations was proved by the Simscape-Multibody software. The dynamic equations of the system are given as follows.
where the terms of r i = J V,i q i and i = J ,i q i are the projections from the joint space to the task space, J V,i and J ,i represents the translational and rotational Jacobians respectively. In this equation, F i T i involves the frictional, external, and actuation forces (torques). The statements of a c i and ̇i denote the linear and angular accelerations of the center of gravity of each link. Since these acceleration terms include the passive joint variables, they can be written in terms of the independent joint variables as follows.
u i and w i contain the centrifugal and Coriolis accelerations while q T = [ 2 s 5 ] represents the independent joint variables. Let's substitute (7) into (6) and rearrange these equations, the dynamic equations of the spatial inverted pendulum system can be obtained in terms of the independent joint variables in a matrix form.
M(q) is a positively defined mass matrix containing the masses and inertias of the links. C(q,q) is the matrix consisting of the gravity, centrifugal, Coriolis and external forces, and finally is the input matrix to control the system. The Fig. 3 The dexterous workspace of PRRRR mechanism Fig. 4 The joint space of PRRRR mechanism equations will be used in the following sections of the study to design the controllers.

Controller design
In this paper, the sliding-mode control (SMC), linear quadratic regulator (LQR), and linear quadratic Gaussian (LQG) controllers will be used as controllers. They will not only try to keep the spatial inverted pendulum in an unstable equilibrium position and but also to move the pivot point through to the region where the workspace of the mechanism is highly dexterous. Thus, the mechanism will not encounter any joint restriction and will not be in the singular position during a long period of operation.

The second-order sliding-mode control (SOSMC)
Sliding-mode control (SMC) is a nonlinear control method that forces a nonlinear system to "slide" on a defined error surface by means of a discontinuous control signal. In this controller, the control law with feedback is not a continuous function of time. Instead, it is switched from a continuous structure to another one depending on the current position in the state space. In this study, the second-order sliding-mode control is proposed for the control of the spatial inverted pendulum system since it eliminates the chattering effects.
The dynamic equations must be written in the desired form so that the SMC can be designed easily. (8) must first be rearranged in the semi-open form by separated into active and passive independent variables, and then only the feedback output should be partially linearized. The linearization process is done according to the independent variables to be controlled, by arranging the equations in the different form.
If the desired outputs are also used as active independent variables (actuated independent joint variables) then the equations should be collocated and if the outputs to be controlled are passive independent variables (pendulum angles) then the equations should be written in non-collocated form [25]. In this study, since the control of active and passive variables will be performed alternately, the equations should be arranged in both types.
Let's rearrange (8) as the passive and active forms as follows.
where q a = [ 2 s 5 ] T and q p = [ ] T are active and passive joint variables respectively. Convert this matrix equation into scalar equations.The details of these coefficient matrices are given in "Appendix".
Collocated form: When q p is taken from (11) and then substituted into (10), the following equation involves the active joint variables, q a .
Substituting all these equations into (12), the control actions to be applied on the mechanism can be obtained as follows.
Non-Collocated form: In this case, the equations are rearranged in terms of the passive joint variables of q p like the previous operations.
Substituting all these equations into (14), the control actions to be applied on the mechanism depending on the passive joint variables can be obtained as follows.
Twisting sliding-mode controller (higher-order sliding-mode controller) (TSMC) design: The controller design should be started by describing the sliding surfaces to control the pendulum angles. q rp = r r T : the reference pendulum angles q p = [ ] T : the instantaneous measured pendulum angles The sliding surfaces can be written as follows. s = (q rp − q p ) and ̇s = (q rp −q p ) Since it is desired that the pendulum is stabilized at the unstable upright position by means of the twisting slidingmode control [26], the reference values were taken as q rp = 0 and ̇q rp = 0 . The hyperbolic tangent function was also used as the sign function. In this case, the inputs for the SMC can be calculated with TSMC in terms of the passive variables.
with the given references, If we want to control the active independent variables of the system, the same procedure is repeated for the active variables.
q ra = 2r s 5r T : the reference inputs q a = [ 2 s 5 ] T : the instantaneous measured input values. In that case, the sliding surfaces can be defined as. s = (q ra − q a ) and ̇s = (q ra −̇q a ) In order to control the inputs of the mechanism (active independent variables), the references are considered as q rp and ̇q rp .
As a result, (15) was used to control the pendulum angles while (13) was applied for the control law of the actuation mechanism. In the simulations, the coefficients used in the control input of the SMC for both passive case and active case are selected by trial and error as follows.

Linear quadratic Gaussian control
LQG control is one of the most fundamental optimal control problems of control theory. The problem is related to the control of uncertain linear systems subjected to a secondorder cost function. These systems have a missing state information and additional disturbance of white Gaussian noise. At the end of the solution of this problem, a unique dynamic feedback control law which can be easily calculated and implemented is obtained. The LQG controller is a combination of the linear quadratic regulator (LQR) and the Kalman filter that is a linear quadratic estimator (LQE). The method also holds good for linear time-invariant systems as well as linear time-variant ones. In order to use this method, the system must first be linearized on a specified point. Linearization of the model and obtaining the state space: Let's x = q̇q T be the state variables of the system. So, the nonlinear equation of (8) can be written as follows.
When the system is at the equilibrium position, the inputs and state variables can be specified as in matrix form like e = 0 0 T and x e = 0 2 0 0 0 0 0 0 . The nonlinear system can be represented in state space form. C(q,̇q) where A, B and C matrices can be calculated at the given equilibrium position by using (8) . w d is the system noises represented by a white noise with a mean of zero on actuators and an impulsive input at specified times to the pendulum, w n is the observation noise represented with a mean of zero. The following matrices are obtained numerically for the equilibrium points and constant parameters given in Table 1.   LQG Design: In order to be able to apply LQG control to a linear system which generally appears as in the diagram given in Fig. 5, LQR controller design should be preliminarily done. At first, the linear system seen in (17) has been subjected to a quadratic cost function given in (18) and then the states that optimize this cost function have been carried out by solving the Riccati differential equation.
In this cost function, x represents the states, u inputs, Q and R the weight matrices. These weight matrices can be written  depending on the desired behavior of the system. The Riccati matrix equation and its solution were defined in (19)(20)(21).
The solution of (19) is L and it makes the system stable. (20) gives the static gain constant, K r , while (21) generates the inputs that will optimize the states. Meanwhile, Q and R matrices which are positive semi-definite and positive definite, respectively, were arbitrarily chosen as follows for the inverted pendulum system.
These values were substituted into (20) to obtain the static gain constant K r .

State estimator design:
The equations belonging to Kalman filter to estimate the other states by means of the measured states are given below. Here, ̇x represents the estimated states, ŷ the estimated outputs, the error between the measured and estimated outputs and K f the Kalman gain considered as an updater between the measured and estimated outputs.
In a similar manner to the LQR controller design with Kalman filter, the error ( ) is optimized by subjecting to a scalar cost function defined in (25). So as to do this, it is necessary to solve again a Riccati differential equation.
In the cost function given in (25), y represents the real output values while ŷ stands for the estimated output values. In order to solve the Riccati equation, we need again the weight matrices. In our system, these matrices were defined as Q se and R se the same as the LQR weight matrices. Kalman filter gain ( K f ) was obtained by getting P from the above equations.

Combination of LQR and state estimator (LQE):
If (17) and (22) are combined and rearranged, then the LQG controller will be achieved via the combination of LQR and LQE (Kalman filter).
Linear system Arranging these two equations for the whole system, it can be said that the controller represents a dynamic system by itself. The expression in matrix form is given as follows.
This equation was used to control the inverted pendulum system with the LQG on the different conditions.

Results
In this section, the previously designed SOSMC and LQG controllers were simulated by using MATLAB over the dynamic model and the results were comparatively evaluated according to the controller performances which are robustness, response time and stability. An external disturbance of white Gaussian noise was added on the system in all simulations. One of the most important studies to assess the designed controllers was to examine the behavior of the system for the impulse disturbances applied to the spatial inverted pendulum. Also, these simulations are important to show the success of the actuation mechanism designed for the control of the pendulum in this study. The simulations started from the equilibrium position and were lasting for 1 second. The impulsive input was applied for 0.001 seconds at 0.2 second of the simulation. The value of the disturbing impulse was gradually increased until the controllers failed to bring the pendulum to the equilibrium position. The responses to these inputs for both controllers are given in Figs. 6 and 7 for two angles of the pendulum. As shown in the figure, the SOSMC could be imposed to an impulsive torque up to 7.5 mNm (mN:millli-Newton) and the LQG controller up to 10 mNm as a disturbance. In addition, the maximum disturbance of 7.5 mNm brought about approximately the deviation angle of 4 • on while a maximum deviation of 5.5 • was observed for LQG controller. In Fig. 6, a similar aspect is seen on the angle. When using SOSMC, a deviation of 2.1 • on the angle is observed at the impulsive torque of 3.5 mNm, while the LQG controller possesses an angle of 6 • . The first outcome here is that the LQG controller is more robust than the SOSMC for the disturbing impulses. However, when stabilizing time is evaluated, the SOSMC performs the stabilization process of the pendulum faster than the LQG controller. In the meanwhile, these results with the absorbed deviations of up to 6 • on both angles seem to be closely consistent with the results of an experimental study which is balancing of a spatial inverted pendulum actuated via a 5R mechanism [12]. In this study, some disturbances were applied to the spatial pendulum by knocking and both deflection angles of about 5 • were absorbed in a short time. Figure 8 shows the reactions of the SOSMC and LQG controller to the different initial angels and . Examining the graph, it can be seen that the SOSMC can balance the initial angle of up to a maximum of 8 • and the initial angle of up to a minimum of −8 • . In the LQG controller, it is seen that these angles is 11 • for and −11 • for . Thus, the LQG controller is more robust for the responses to the initial angles.
Another important aspect is that stabilizing the inverted pendulum and keeping it in equilibrium for a long period of time. The study on this subject is given in Fig. 9. During the long working period, the disturbance impulsive inputs to the pendulum were applied at the 15th and 30th seconds of the simulation. The outcomes show that the LQG controller can keep the pendulum in equilibrium for long period of time because it was linearized about the specified point on which the dexterity of the workspace is very high. In addition, in this study, as previously expressed, a white noise  with specified amplitudes was applied to the inputs. With this study, the SOSMC can compensate the noise band of ±0.05 Nm to the rotational actuator and ±0.05 N to the linear actuator, while the LQG controller can tolerate the noise bands of ±1.0 Nm and ±1.0 N respectively. By using the LQG method, a sample animation on the balancing of the spatial inverted pendulum is given by using in Online Resource 1 [27].
Another study on the spatial inverted pendulum was the investigation of the system response to the change of pendulum length. All parameters were kept constant and simulations were performed by taking the length of L/2, L, 2L, and 3L. In this study, using only the LQG controller, the maximum impulsive disturbing values were investigated for the different lengths of the pendulum. The results are given in Fig. 10.
It is observed that the tolerable value of the impulsive input increases as the length of the pendulum increases. For example, the impulsive torque in the direction of alpha is 0.108 mNm for L / 2, 0.146 mNm for L, 0.196 mNm for 2L and 0.295 mNm for 3L. On the other hand, the maximum angle corresponding to these tolerable impulse torques are 16 • , 9 • , 5 • and 3.5 • for L / 2, L, 2L and 3L, respectively. This shows that the pendulum with the shorter lengths is stabilized even for larger deflection angles. This indicates physically that the pendulum with smaller inertia can be brought to equilibrium with less effort.

Discussion and conclusion
In this paper, the kinematic and dynamic modeling of a spatial inverted pendulum system was presented. This system is nonlinear and requires a fast response for the stabilizing control of the pendulum in upright position. An actuation mechanism with PRRRR planar structure in this pendulum system has not been used in literature so far. The dimensional design of the mechanism has been achieved successfully based on an optimization with genetic algorithm. According to these design parameters, the dexterity workspace which is determined via the pivot point of the mechanism was presented in this paper to move the platform to the suitable region without blocking over all the operations. In simulations, SOSMC and LQG controller have been used to keep the pendulum in equilibrium. In balancing, the SOSMC is more robust and faster than LQG controller for disturbing effects. However, the LQG controller shows better performance for the initial positions of the pendulum and keeping it in the stable position for a long period of time. For the future work, another paper will be assigned on the setup and real-time controller design of the spatial inverted pendulum. m 34 = m 43 = 0 m 44 = I py + L 2 p m 6 ∕4 The Coriolis, centrifugal and gravity acceleration terms: