Solution of the Kalman filtering problem in control and modeling of a double inverted pendulum with rolling friction

Pages 8
Views 7

Please download to get full document.

View again

of 8
All materials on our website are shared by users. If you have any questions about copyright issues, please report us to resolve them. We are always happy to assist you.
Solution of the Kalman filtering problem in control and modeling of a double inverted pendulum with rolling friction Paweł Olejnik* Jan Awrejcewicz* Michał Niełaczny** *Department of Automation and Biomechanics
Solution of the Kalman filtering problem in control and modeling of a double inverted pendulum with rolling friction Paweł Olejnik* Jan Awrejcewicz* Michał Niełaczny** *Department of Automation and Biomechanics Lodz University of Technology **Faculty of Mechanical Engineering Lodz University of Technology Abstract: The problem of control and dynamical modeling of a unicycle-cyclist system treated as a double inverted pendulum with rolling friction and vibrating in the plane that is perpendicular to the direction of movement is studied. The object of analysis consists of two basic parts: the wheel and the double pendulum. The equations of motion have been derived using the Lagrange equation of the second kind. The kinematic excitation has been applied to the cyclist. The aim of control is to maintain the unicyclecyclist system in an unstable equilibrium around given angular position. Control moment of force has been applied to the wheel in a numerical procedure performed in Simulink. Kalman filtering problem has been solved basing on the feedback control system and functions included in Control System Toolbox of MATLAB. The proposed approach could be extended in future to solve some dynamical problem of transverse vibrations. Keywords: double inverted pendulum linearization dynamic modeling rolling friction Kalman filter LQG control. Introduction This work is devoted to a problem of modeling and control of a dynamical system consisting of a single wheel vehicle (a single-track unicycle) and a cyclist s body. The unicycle is a specific kind of bicycle because it consists only from one wheel and a seat on which a driver operates to keep balance and to drive forward or backward. The unicycle is a child of an original bicycle having the driving big wheel and a small wheel that was only the rolling one helping the driver in keeping the vehicle s direction of movement. Basic feature of the monocycle s construction is that it looks like a bicycle wheel with a hub designed so the axle is the fixed part of the hub. Therefore the rotation of the cranks directly controls the rotation of the wheel (it is called direct drive). The cranks are attached to the ends of the axle so pedals always rotate during riding the unicycle. The direct connection between the axle and the crack is not a rule and ratio between their rotations may exist. Nomenclature a amplitude of cyclist s inclination c i coefficient of damping e i distance between the center of gravity and the axis of rotation of the i-th link E kinetic energy f resistance of rolling friction g gravitational constant I i moment of inertia of the i-th link l i length of the i-th link m i mass of the i-th link M i moment of friction in the i-th bearing M N driving moment N c normal load Q i i-th generalized force r wheel radius v m experimentally estimated mean linear velocity of the unicycle-cyclist system V potential energy t m experimentally estimated mean time of ride t time of simulation j i angle of rotation of the i-th link w frequency of cyclist inclinations r i radius of inertia of the i-th link y kinematic excitation. Fig.. Physical model of a simplified unicycle-cyclist system with rolling friction Rys.. Model fizyczny uproszczonego układu monocykl-rowerzysta z tarciem tocznym Pomiary Automatyka Robotyka nr /03 63 Nauka Riding the unicycle is not easy. It is caused by single tracking of the vehicle that requires to keep balance of the system simultaneously in two planes. Moreover to ride comfortably the distance between the saddle and the lowest pedal position has to be smaller than length of the cyclist s leg. In a consequence center of gravity of the cyclist s body lies a bit upper than his normal upright position. To keep balance in plane that is parallel to the direction of riding (forward or backward) the cyclist has to accelerate or slow the driving wheel to maintain his center of gravity perpendicularly above the axle of rotation of the wheel. To keep balance in plane that is transversal to the direction of riding the cyclist has to balance from left to right side with the use of his loins. Construction and mechanics of a unicycle allows to consider it as an inverted double pendulum (see fig. ). The first link is created by the cyclist s body and the second link by the fork frame stiffly joined with the seat post (a link between the frame and saddle). Considering that the wheel states the third link so a kind of triple pendulum could be even assumed. The system visible in fig. has three degrees of freedom and to control it one would involve a control moment of force applied to the driving wheel or in the joint created by the rotational connection between the second and third link. Fig.. Follow-up control system Rys.. Schemat kontroli nadążnej If a human loses his balance while riding the unicycle his membranous labyrinth senses it and an error of regulation of the unstable equilibrium appears. Brain functions here as regulator that receives the error and accordingly to its learned neural network produces appropriate control signals that through the nervous system cause desired action of muscles (actuators). Muscles action enables the human to correct his body s position. Following this description a cyclist driving a unicycle could be approximately analyzed as a follow-up control system visible in fig... Modelling of the unicycle-cyclist system The above analysis of various pendulums behavior usually attains stabilization so later attempts have produced many problems regarding the field of control and optimization of linear and nonlinear discontinuous (with impact and friction) and continuous multi degrees of freedom realizations of inverted pendulum systems. Balancing of inverted pendulums of any kinds is a classic control problem of some 30 years. A new fuzzy controller for stabilizing series-type double inverted pendulum systems is proposed in [] based on the SIRMs (Single Input Rule Modules) dynamically connected fuzzy inference model. The proposed controller deals with six input items. Each input item is provided with a SIRM and a dynamic importance degree (DID). The SIRM and the DID are set up such that the angular control of the upper pendulum takes the highest priority order over the angular control of the lower pendulum and the position control of the cart when the relative angle of the upper pendulum is big. By using the SIRMs and the DIDs the control priority orders are automatically adjusted according to control situations. Simulation results show that the controller stabilizes series-type double inverted pendulum systems of different parameter values in about 0 seconds for a wide range of the initial angles. In [6] an adaptive fuzzy logic control of dynamic balance and motion is investigated for wheeled inverted pendulums with parametric and functional uncertainties. The proposed adaptive fuzzy logic control based on physical properties of wheeled inverted pendulums makes use of a fuzzy logic engine and a systematic online adaptation mechanism to approximate the unknown dynamics. Based on Lyapunov synthesis the fuzzy control ensures that the system outputs track the given bounded reference signals to within a small neighborhood of zero and guarantees semi-global uniform boundedness of all closed-loop signals. The effectiveness of the proposed control is verified through extensive simulations. Contribution [9] deals with the application of energy based control methods for a model of inverted pendulum on a cart. A swing-up controller as well as a nonlinear balancing controller with the focus on the implementation on a laboratory model is presented. The well-known control concepts has been adapted such that they work on a concrete experiment with all the undesirable effects like friction and quantization. In [7] a linear state feedback design technique for balancing an inverted pendulum is provided. The pivot of the investigated pendulum is mounted on a carriage that has limited horizontal travel. For any given (arbitrarily small) allowable travel of the carriage a linear state feedback controller that balances the pendulum with an infinite amount of gain margin has been adopted in the sense that if the feedback gain is perturbed by any multiplying factor greater than one the controller balances the pendulum without requiring greater traveling distance than the maximum allowable. A mathematical model of a planar double inverted pendulum was established in [3] by means of analytical dynamics method. Based on the linear quadratic optimal theory a LQR self-adjusting controller was derived. Further the output of LQR controller was refined through optimize factor which was the function of the states of planar pendulum and on account of that control action exerted on the pendulum was improved. Simulation results together with pilot scale experiment verify the efficacy of the suggested scheme. 64 Work [0] uses the symbolic manipulation toolbox available in MATLAB to investigate polezero cancellation of the uncontrollable double inverted pendulum algebraically following exploratory numerical computation. The ability of the software to factorize complicated multivariable polynomials is exploited to identify in algebraic form the anticipated pole-zero term cancelling throughout the transfer functions of the uncontrollable pendulum system. The investigated system has been considered with respect to the force on the trolley for which it is a conditionally uncontrollable problem and with respect to each of the torques on the arms which are unconditionally uncontrollable problems. A methodology of Lyapunov stability control is presented in [] to achieve the upright balance of a baseexcited inverted pendulum with two degrees of rotational freedom. The inclusion of the base point movement led to the dynamic system of such a pendulum which is non-autonomous and is under persistent disturbance. An idealized piecewise continuous control strategy was designed and for the obtained controller the solution trajectories to be arbitrarily close to the upright position have been guaranteed. The continuous control law guarantees that the solution trajectories are kept in a controlled region around the upright position. The stability has been traded off with a weaker stability to prevent chattering. The robustness of the controllers with respect to certain class of uncertainties was also examined. A passive fault tolerant control scheme has been suggested in [8]. A nominal controller is augmented with an additional block which guarantees stability and performance after the occurrence of a fault. The method is based on parameterization which requires the nominal controller to be implemented in observer based form. The proposed method is applied to a double inverted pendulum system for which the H controller has been designed and verified in a lab setup. The literature overview shows that the problem is still valid states a good field for practicing in control of multi degrees of freedom systems as well as opens new perspectives for application of interesting structures of controllers. One of such simple structures that are based on the standard LQG control has been studied in this work. Application of the Kalman filtering problem to solve such kinds of continuous systems is in general not examined in literature. An exemplary contribution [5] that uses a Kalman filter to help the estimation of a gyro angle presents some study devoted to balancing and navigation of a MIPS robot. It is a mobile inverted pendulum system whose structure is a combination of a wheeled mobile robot and an inverted pendulum system. Low cost gyro and tilt sensors are used and fused to detect balancing angle. Digital filters are selectively designed for sensors to measure an inclined angle accurately with respect to different frequencies. Performances of balancing and navigation of the MIPS are tested by experimental studies through remote control. Let us take into analysis a simplified model of unicycle-cyclist system in fig.. Assuming that the most upper link is the cyclist s body and the remaining two are the unicycle s links it creates a physical model placed in Cartesian coordinates. Physical model of the unicycle-cyclist system consists of three solid bodies of which masses are focused in points of their centers of gravity: ) the driving wheel ) fork frame with the seat post 3) cyclist s body. Equations of motion have been derived by means of Lagrange equation of second kind [] d E E V Q n n... N dt - + = = q q q n n n () where: N is the number of generalized coordinates q n is the n-th coordinate. In the analysis we will assume that j j j 3 are the generalized coordinates but if to regard that a cyclist riding the unicycle can incline forward and backward with a frequency w in the direction of movement ϕ3 = ϕ - ψ ( t) ψ ( t) = a sin ( ωt) () where: y(t) angle of cyclist s body around the saddle. Having the above one rewrites eq. () as follows d E E V Q n n. dt - + = = ϕ ϕ ϕ n n n Moments of inertia I i of i-th link with respect to axes perpendicular to centre of gravity of i-th mass are given by the formula (3) I = m ρ i = 3 (4) i i i where corresponding radii of inertia are as follows e 3 e3 3 ρ = r ρ = ρ3 =. (5) 3 3 Distances between centers of gravity and the corresponding axes of rotation e i are as follows e = 0.5 l i = 3. (6) i i Kinetic energy E of the unicycle-cyclist system is a sum of kinetic energies of the linear and angular displacement of each mass m E = ( xi yi ) ρi ϕ + + i. (7) 3 i i = Gravitational forces are conservative so potential energy with respect to each mass of the system reads V = mgy + mgy + m3gy3. (8) Assuming that the direct driving wheel is stiff and rolls without slips the integrable geometrical and kinematic constraints are superposed Pomiary Automatyka Robotyka nr /03 65 Nauka x = rϕ y = r. (9) In accordance to the above assumptions the following relations between Cartesian and generalized coordinates are found x = x + e sin ϕ y = y + e cos ϕ x3 = x + e3 sin ( ϕ - a sin ( ωt) ) + l sin ϕ y = y + e cos( ϕ - a sin ( ωt) ) + l cos ϕ. 3 3 (0) For estimation of generalized forces we need to assume some non-conservative forces: normal force N c and any resistances in joints. Generalized coordinates describe absolute angular displacements therefore moments of forces acting on appropriate links are taken as the generalized forces Q = M - M + M Q = M. () N 0 Dynamics of the system will be investigated while riding with constant velocity. At this condition moments acting on the wheel with respect to z axis have to balance themselves. If links and 3 move forward with constant velocity keeping their upright positions in directions x and x 3 respectively then the driving moment M N is equal to the moment of rolling resistance M = ( m + m + m ) fg. () N 3 /6( l3m3 ( 3 ( r ϕ sin ( a sin ( ωt) - ϕ) + g sin ( a sin ( ωt) - ϕ) ) + l 3 ( ϕ + aω sin ( ωt) ) + + 3l ( m3 ( l3 ( aω (- ϕ cos( ωt) sin ( a sin ( ωt) - ϕ) + aω cos ( ωt) sin ( a sin ( ωt) ) + ( )) ( ( ))) ) ( ϕ cosϕ sinϕ )) ( 3 ) ϕ ) ( ϕ ϕ ) = 0. ω sin ( ωt) cos a sin ( ωt) + ϕ cos a sin ωt + r ϕ cosϕ - g sinϕ + + m r - g + l m + m + c - 3 (7a) /r ( l3m ( 3 ϕ sin ( a sin ( ωt) - ϕ )( ϕ - aω cos( ωt) ) + ϕ cos( a sin ( ωt) - ϕ) + + aω a cos ( ωt) sin ( a sin ( ωt) - ϕ) + sin ( ωt) cos( a sin ( ωt) - ϕ) + ( )) ( )( ϕ sinϕ ϕ cosϕ ) ( ) ϕ ) ( ϕ ϕ ) - l m + m - + m + m + m r - c sgn( ϕ) m3 ( l3 sin ( a sin ( ωt) - ϕ) + aω sin ( ωt) ) + -l3 cos( a sin ( ωt) - ϕ )( ϕ - aω cos( ωt) ) - l ϕ cosϕ + -l ϕ sinϕ - lm ( ϕ cosϕ + ϕ sin ϕ) + g ( m + m + m3 ) = MN 3. Numerical Solution of dynamics (7b) Numerical solution of the unicycle-cyclist system has been preceded by experimental estimation of some parameters. It was found that at a normal use of the unicycle that rides on a concrete road the mean linear velocity of driving v m» 3 m/s and the corresponding mean angular velocity of the wheel dj /dt» 0 rad/s. Other parameters of an exemplary unicycle read: m = 5 kg m = 30 kg r = 0.3 m l = m l 3 = 0.8 m e = 0.5 m e 3 = 0.4 m c = 0.0 Nm/s a = 0.5 m w = 4.7 rad/s f = 0.0 m t = 0 s. Additionally moment of rolling resistance is given by M = f ϕ N (3) 0 sgn i c where normal force N c reads 3 3 N = g m + m y. (4) c i i i i = i = Moment generated by the viscous damping of frame ( ϕ ϕ ) M = -c - = -M. (5) Generalized forces are finally found in the form (6) By substitution of eqs. (7) (8) and (5) in () we found in Mathematica the two ordinary differential equations of second order describing the reduced dynamical model of the unicycle-cyclist system Fig. 3. A stroboscopic view on motion of the unicycle-cyclist system Rys. 3. Stroboskopowy widok ruchu układu monocykl-rowerzysta 66 Fig. 4. Time histories of state variables φ i for i =... 3 Rys. 4. Wykresy czasowe zmiennych stanu φ i przy i =... 3 Fig. 5. Time histories of dependencies between state variables x i and y i for i =... 3 Rys. 5. Wykresy czasowe zależności między zmiennymi stanu x i i y i przy i =... 3 Figure 3 presents 5 subsequent views of numerical solution of differential equations (7) without any control torque input at initial conditions: dj /dt(0) = 0 rad/s j (0) = j (0) = j 3 (0) = 0. Comparison of results could be done on the basis of state variables' time histories shown in figs. 5 and 6 of the analyzed not controlled inverted double pendulum. cos coswt fi s fi s fi Out w*t sin sinwt wt M c pfi Out a*sin(wt) sin cos sinasinwt cosasinwt M P O Out sin sinasinwt_fi a*sin(wt fi) coswt fi cos cos sin cosasinwt_fi coswt_ cosfi sinfi M fi s fi O fi s fi pfi Out sgnpfi mian M N fi mian P MN Fig. 6. Input-output Simulink model of the unicycle-cyclist system ( simulink_unicycle.mdl ) Rys. 6. Model w Simulinku wejście-wyjście układu monocykl-rowerzysta ( simulink_unicycle.mdl ) Pomiary Automatyka Robotyka nr /03 67 Nauka 4. Control of the unicycle-cyclist system The aim of control is to force the pendulum to maintain its upright equilibrium position. It has been done in MATLAB/Simulink by application of a control moment of force applied to the driving wheel. The procedure of control is prepared accordingly to instructions provided by the MATLAB s Control System Toolbox [3] which supports: gain selection from root locus pole placement and linear-quadratic-gaussian (LQG) regulation. Let us rewrite eq. (7) in the matrix form []: ( ) ( ) ( ) ( ) M ϕ ϕ + N ϕ ϕ + O ϕ ϕ + P ϕ + C ϕ = F (8) which is prepared for solution in Simulink as below - ( ) ( ) ( ) ( ) ϕ = M ϕ F - N ϕ ϕ - O ϕ ϕ - P ϕ -C ϕ where: ϕ ϕ = c -c C = c c - ϕ = ϕ ϕ ϕ ϕ ϕ = ϕ ϕ ϕ = ϕ M N F = 0 (9) ϕ (0) = ϕ (0) = ϕ (0) = ϕ (0) = 0. Corresponding Simulink model has been presented in fig. 6. The unicycle-cyclist dynamical system is strongly nonlinear but to control it closely around the desired zero angle position of second link one needs to assume the following simplifications. (0) State-space representation of the single input (the control moment) single output (angle of rotation of the second link) system derived from eq. () is given as follows ζ ( t) = Aζ ( t) + Bu( t) ζ ( t ) = ζ υ( t) = Cζ ( t) + Du( t) 0 0 where z is the state variable u is the output variable A = 0 0 B = () D [ 0]. (3) C = - = One can check stability of the system by finding some function V(x) called the Lyapunov function which for the time invariant system takes the form V dζ V( ζ) 0 V(0) = 0 V ( ζ) = 0. (4) ζ dt For linear time invariant systems the procedure for finding the Lyapunov function comes down to the problem of solving the Lyapunov algebraic equation. The linear system () is stable if one is able to find a scalar function V(z) such that when this function is associated with the system conditions (4) are satisfied []. The Lyapunov function can be chosen to be quadratic V(z) = z T P z for P = P T
Related Documents
View more...
We Need Your Support
Thank you for visiting our website and your interest in our free products and services. We are nonprofit website to share and download documents. To the running of this website, we need your help to support us.

Thanks to everyone for your continued support.

No, Thanks

We need your sign to support Project to invent "SMART AND CONTROLLABLE REFLECTIVE BALLOONS" to cover the Sun and Save Our Earth.

More details...

Sign Now!

We are very appreciated for your Prompt Action!