Geometric path following control of a rigid body based on the stabilization of sets

Transcription

1 Preprints of the 19th World Congress The International Federation of Automatic Control Geometric path following control of a rigid body based on the stabilization of sets uri A. Kapitanyuk Sergey A. Chepinskiy Aleksandr A. Kapitonov ITMO University 9, Kronverkski prospekt, Saint-Petersburg, 19711, Russia yura.kapitanyuk@gmail.com, Chepinsky S@hotmail.com) Abstract: The paper describes an approach to the development of the geometric path following control for a rigid body. Desired path of movement in the space is represented by an intersection of two implicit surfaces. Path following control problem is posed as a problem of maintaining the holonomic relationships between the system outputs. Control is synthesized using the differential geometrical method through nonlinear transformation of initial dynamic model. The main results presented are the model of spatial motion and relevant nonlinear control algorithms. Keywords: Rigid body, path following, coordinates transformation, set stabilisation, nonlinear systems 1. INTRODUCTION The paper considers the development of path following control system for a rigid body, that is, the problem of providing a motion along a given spatial path. With the advent of unmanned vehicles the path following control problem became even more urgent, because path following is an UAV major operating mode. Two approaches to the development of these systems are known [Aguiar et al. ), Nielsen et al. 9)]. In the first one, a tracking system controlled by a reference model is developed [Breivik and Fossen ), Lee et al. 1)]. The path is generally set by a time-dependent function, which leads to practical problems when the object motion is behind or ahead of the program due to parametric uncertainties or external disturbances. To solve this problem, the path should be parametrized by the length instead of time, and dynamics of this parameter should be introduced in the system model [Breivik and Fossen )]. This method rather easily realizes the motion along polynomial curves, which provides better path planning and more accurate path following, however the resultant controller is rather lengthy. An alternative approach is based on stabilization of invariant manifolds in state space based on feedback linearization [Nielsen et al. 9),Hladio et al. 13)] or passivebased control[el-hawwary and Maggiore 11),El-Hawwary and Maggiore 13)]. Simply speaking, a transformation generating an attractor in state space is selected for the initial system. In path following context, the attractor is a desired path set in output coordinates. Then the designer should only stabilize this solution, which is much less demanding than creating a tracking system as in the first approach. As a control object, an autonomous robot is a multichannel nonlinear dynamic system. Control system of a mobile robot should generate control actions providing preset motion of the centre of mass in operating area. The one of the methods for synthesizing the control algorithms was proposed by I.V. Miroshnik. It is based on the second approach and implies nonlinear transformation of robot model to the task-oriented coordinate system, which makes it possible to reduce the complex multichannel control problem to several simple problems of compensation of linear and angular deviations and then to find adequate control laws using nonlinear stabilization [Fradkov et al. 1999), Kapitanyuk and Chepinsky 13)]. Differentially geometric methods of nonlinear control theory [Fradkov et al. 1999), Kapitanyuk and Chepinsky 13), Miroshnik and Nikiforov 199), Miroshnik and Lyamin 199), Miroshnik and Bobtsov ),Miroshnik and Sergeev 1)] are used in the analysis method for these systems and synthesis of control algorithms solving the path following problem as a stabilization problem with respect to implicit space curve. This article deals with further development of task-oriented approach inspired by works[el-hawwary and Maggiore 11),Hua et al. 13)]. The latest results based on feedback linearisation are presented in the first part of article. The further extension of task-oriented framework is described in the second part. Proposed design procedure base on the using of properties of passive systems. This article focuses directly on the synthesis of controllers without restricting the path planning method.. RIGID BOD MODEL AND STATEMENT OF CONTROL PROBLEM This work was partially financially supported by Government of Russian Federation, Grant 7-U1 This work was supported by the Ministry of Education and Science of Russian Federation Project 1.Z.31.31). Consider a fully actuated rigid body model illustrated in Fig. 1 ẋ = v, 1) Copyright 1 IFAC 73

2 Tangential velocity along the curve S is defined as ṡ = φ 1 φ v, ) φ 1 φ Fig. 1. Rigid body and desired trajectory in the space m v = F c, ) Ṙα) = Sω)Rα), 3) J ω + ω Jω = M c, ) where x = [x rb, y rb, z rb ] T R 3 is the Cartesian position vector of the center of mass C in the inertial reference frame Z, v = [ẋ rb, ẏ rb, ż rb ] T R 3 is the velocity vector of the center of mass C in the inertial reference frame, m R is a total mass of the rigid body, F c = [F x, F y, F z ] T R 3 is the vector of the control forces in the inertial reference frame, α = [ϕ, θ, ψ] T R 3 is the vector of Euler angels of the body-fixed frame in the inertial reference frame with yaw, pitch and roll angels respectively, Rα) SO3) is the rotation matrix from the body-fixed frame to the inertial frame, ω = [ω ϕ, ω θ, ω ψ ] T R 3 is the vector of angular velocities in the body-fixed frame, M c = [M ϕ, M θ, M ψ ] T R 3 is the vector of the control moments in the body-fixed frame, Sω) SO3) is the skew symmetric matrix with structure: [ ω3 ] ω Sω) = ω 3 ω 1. ) ω ω 1 The rotation matrix can be expressed through Euler angels as where Rα) = R 3 ψ)r θ)r 1 ϕ) ) [ 1 ] R 1 ϕ) = cos ϕ sin ϕ sin ϕ cos ϕ [ ] cos θ sin θ, R θ) = 1, sin θ cos θ [ ] cos ψ sin ψ R 3 ψ) = sin ψ cos ψ. 1 The desired path is a smooth segment of curve S see Fig. 1) described as an intersection of two implicit surfaces φ 1 x, y, z) = φ x, y, z) =, 7) where φ 1, φ and ψ are smooth functions. where is the vector product and is the vector norm. It should be noted the description of a curve as a smooth geometrical object is not the only one possible, and the selection of functions 7) is ambiguous. Selection of functions φ 1 x, y, z) and φ x, y, z) mostly limited by regularity condition [Fradkov et al. 1999)] implying that Jacobian matrix φ 1 φ φ 1 φ φ 1 Υx, y, z) = 9) φ 1 φ φ is not degenerate for any vector x = [x rb, y rb, z rb ] T belonging to curve S, i.e. detυx, y, z). Path following control problem is posed as a problem of maintaining the holonomic relationships between the system outputs [x rb, y rb, z rb ] T set in 7). It is augmented by the description of desired longitudinal motion of the point of the centre of mass of the rigid body along the desired path S usually set using the reference velocity of longitudinal motion V = ṡ Consider the errors of the path following [Fradkov et al. 1999), Kapitanyuk and Chepinsky 13)]. Violation of condition 7) is characterized by deviations zeroed at manifold S. e 1 = φ 1 x, y, z) 1) e = φ x, y, z) 11) Therefore, the path following control problem for the rigid body consists in determination of inputs F c = [F x, F y, F z ] T in closed loop, which provides: a) stabilization of robot motion with respect to the curve S, which implies asymptotic zeroing of spatial deviation vectors e 1 and e ; b) asymptotic zeroing of velocity error V = V ṡ. 1) c) stabilization of the required robot orientation with respect to the curve S 3. TRANSLATION MOTION CONTROL To synthesize the path following controllers we transform the system model with account for 1)-) to the taskbased path-based) form [Fradkov et al. 1999), Kapitanyuk and Chepinsky 13)] with outputs s, e 1 and e. To do it, we differentiate ), 1) and 11) with respect to time: [ ] ṡ ė 1 ė = Υx, y, z)v. 13) 733

3 Once more differentiate 13) with account for ): [ ] s ë 1 = Υx, y, z)v + Υx, y, z) F c ë m. 1) Now consider the virtual task-based) controls: ] Υx, y, z)v + Υx, y, z) F c m = [ us u e1 u e. 1) Substitute 1) to 1) and obtain: [ ] [ ] s us ë 1 = u e1. 1) ë u e Now select the controllers: u s = K s ṡ, 17) u e1 = K 1e1 ė 1 K e1 e 1, 1) u e = K 1e ė K e e, 19) where K s, K 1e1, K e1, K 1e, K e are positive coefficients which provides the desired dynamics of asymptotic zeroing of s, e 1 and e deviations and solves the problems a) and b). Now we determine actual control actions F c and finally obtain: [ ] ) us F c = mυx, y, z) 1 u e1 Υx, y, z)v. ) u e In this section, we have described the general method for synthesizing the controller of translation motion.. ATTITUDE CONTROL Now focus on the solution of problem c). Introduce the vector of angular errors δ = [δ ϕ, δ θ, δ ψ ] T R 3 and the angular deviations matrix Rδ) = Rα)R T α )R T ), 1) where Rα ) SO3) is the matrix of angular orientation of the body-fixed frame along the curve S in the point [x rb, y rb, z rb ] T, R T ) SO3) is the matrix of the desired angular orientation respect to the curve S. For the stabilisation of the desired attitude and eliminating of the vector δor fulfilment of the identity Rδ) = I) use approach described in work [Lee et al. 1)]. Define the angular error function as e r = 1 Rδ) Rδ) T ), ) where vee map is the transformation SO3) R 3. Now define the angular speed error e ω. Differentiating 1) with respect to time and the expression 3) and obtain the equation d dt Rδ) = S δ)rδ) = e ω Rδ), 3) d dt Rδ) = Sω)Rδ) Rα)RT α )Sω )R T ), ) Use the property of skew symmetric matrix RSω)R T = SRω) obtain the final expression and d dt Rδ) = Sω) SRα)R T α )ω ) ) Rδ), ) e ω = ω Rα)R T α )ω. ) Differentiating 7) with account for 3) where ė ω = 1 J M ω Jω) + a d 7) a d = Sω)Rα)R T α )ω + Rα)R T α ) ω ) Resulting attitude controller has form M c = ω Jω Ja d K R e r K ω e ω. 9) where K R and K ω are positive constants. In a common case for the matrix Rα ) can use the orthogonal matrix based on Jacobian 9) φ 1 φ φ 1 φ Rα φ 1 ) = T x, y, z) =. 3) φ 1 φ 1 φ ) φ 1 φ 1 φ ) φ 1 The vector of desire angular speed can found from the Frenet-like equation [Fradkov et al. 1999)]: T x, y, z) = ṡsξ)t x, y, z), [ ] τ Sξ) = τ κ, κ where ξ = [τ,, κ] T R 3, κ - curvature of curve S in the pointx,y,z), τ - torsion of curve S in the point x, y, z). Then equations 7), ) and 3) transform to e ω = ω ṡrα)a T x, y, z)ξ, a d = srα)t T x, y, z)ξ ṡsω)rα)t T x, y, z)ξ + +ṡ Rα)A T x, y, z)sξ)ξ ṡrα)a T x, y, z) ξ. In this section, we have described the general method for synthesizing the controller of attitude motion which solves the problems c). Now focus on particular cases popular in practice.. PASSIVE BASED PATH FOLLOWING Consider the same model 1)-) and storage function V V = 1 ẋ v d + 1 ω ω d 31) Differentiating 31) with respect to time and obtain the equation 73

4 V = ẋ v d ẍ v d ) + ω ω d ω ω d ) = = ẋ v d 1 m F v d) + +ω ω d J 1 ω Jω + J 1 M c ω d ) Now choose the control inputs F and M c in the forms F = u pf + u F, M c = u pm + u M, where u pf = v d and u pm = ω Jω + J ω d are control inputs, which make system passive. In the result we obtain V = ẋ v d u F + ω ω d u M = y T u. It means that system 1)-) is passive from inputs u = [u T F, u T M ] T to outputs y = [ẋ v d, ω ω d ] T. For the stabilisation of passive system we can use ordinary feedback control law u = Ky where K is diagonal matrix of scalar coefficients k 1,.., k Now we can use variables v d and ω d as new virtual inputs and stabilise the desired manifolds for the translation motion φ 1 x, y, z) = φ x, y, z) =, and desire orientation presented by relations [Hua et al. 13)] 1 n T n d ), where n is a vector of current orientation and n d is a vector of desired orientation Consider Lyapunov function W = 1 φ φ nt n c ) 3) Take the derivative of the function 3) with respect to time Ẇ = φ 1 φ1 φ ẋ + φ ẋ d dt nt n c n T d dt n d = = φ 1 + φ φ v d + +1 n T n c )n T Sω d )n c n T Sω c )n c ) = = φ 1 + φ φ v d + +1 n T n c )n T Sω d ω c )n c = = φ 1 + φ φ v d + +1 n T n c )n T S T n c )ω d ω c ) Then for v d = u τ + u φ and ω d = ω c + u ω obtain Ẇ = φ 1 + φ + φ 1 + φ φ Υ 1 [V,, ] T + u φ + φ +1 n T n c )n T S T n c )u ω Fig.. Motion along space curve Select the control inputs u φ and u ω as ) u φ = K φ φ 1 + φ φ u ω = Sn c )nk ω 1 n T n c ) Then equation of derivative of Lyapunov function becomes Ẇ = φ 1 + φ k ω 1 n T n c ) <, 1 φ K φ φ 1 + φ 1 1 Z ) φ where K φ is the positive defined matrix of constant coefficients and k ω is a strictly positive constant. This result means that our system is asymptotic stable with domain of attraction π, π).. NUMERICAL EAMPLE For the illustration of the rigid body motion we have considered two cases. At first, we was modelling the common control law based on feedback linearisation approach for the rigid body with m = 1, J = 1. The desire path S is an intersection of two implicit surfaces such as elliptic cylinder and parabola φ 1 x, y, z) =.x +y R ) φ x, y, z) = z+.y ) Initial position of the centre C of the rigid body is x = [ 1,, 1] T and initial orientation is α = [3,, 1] T Parameters of the controller is presented below K e11 = 1, K e1 = 1; K e1 = 1, K e = 1; K R =, K ω =. Desire speed along the trajectory ṡ = 1 For the attitude control was used simplified version of control law 3) without feed-forward term a d M c = ω Jω K R e r K ω e ω. The results of modelling is presented on figures )-). In the second case, was modelling the passivity based proposed control law for the rigid body spatial motion for the same desire path S. Desire speed along the trajectory ṡ = 1. Initial orientation describes by vector n = [1,, ]. The desired orientation set by vector n c = [1/ 3, 1/ 3, 1/ 3]. Parameters of controllers is represented below K = diag [ ] K φ = diag [ ] k ω = 73

5 e R t t,time. Fig.. Angular error e r Fig. 3. Projection on plane Z Fig.. Projection on Z plane Fig. 9. Motion along space curve Z 1 e 1 t t,time Fig.. Position error e 1 = φ 1 x, y, z) e t Fig. 1. Projection on plane 1 Z Fig. 11. Projection on Z plane t,time Fig.. Position error e = φ x, y, z) 1. s t 1. 1 e 1 t t,time.. 1 t,time. Fig. 7. Speed error V = ṡ ṡ Fig. 1. Position error e 1 = φ 1 x, y, z) The result of modelling is presented on figures 9)-1) We can t direct compare this two approach yet, for the detailed analysis we should deeper research this two method for identify of possible advantages and disadvantages. This is the main task for further work. 73

6 1 e t t,time Fig. 13. Position error e = φ x, y, z) 1 s t 1 3 Fig. 1. Speed error V = ṡ ṡ 1 n t T n c t t,time 1 3 t,time Fig. 1. Angular error 1 n T t)nt) c ) 1. n t t,time Fig. 1. Orientation of the reference axis nt) 7. CONCLUSIONS The proposed control algorithms can be helpful in development of path following control systems for mobile robots underwater or airborne robots). This method is rather computational complex in common form, but with additional assumptions can find the simple controllers. The primary of the future research is the detailed comprassion of the represented approach. Also it would be interesting to apply this method to adequate robot dynamic models, for example quadrotor UAV. Moreover the performance of proposed controllers in the presence of parametric uncertainties and external disturbances, and development version of this controller without velocity measuring should be the subject to further analysis. REFERENCES Aguiar, A., Hespanha, J., and Kokotovic, P. ). Path-following for nonminimum phase systems removes performance limitations. Automatic Control, IEEE Transactions on, ), doi: 1.119/TAC..19. Breivik, M. and Fossen, T. ). Principles of guidancebased path following in d and 3d. In Decision and Control, and European Control Conference. CDC-ECC. th IEEE Conference on, 7 3. doi: 1.119/CDC..1. El-Hawwary, M.I. and Maggiore, M. 11). Case studies on passivity-based stabilisation of closed sets. International Journal of Control, ), El-Hawwary, M.I. and Maggiore, M. 13). Reduction theorems for stability of closed sets with application to backstepping control design. Automatica, 91), 1. Fradkov, A., Miroshnik, I., and Nikiforov, V. 1999). Nonlinear and Adaptive Control of Complex Systems. Mathematics and Its Applications. Springer. Hladio, A., Nielsen, C., and Wang, D. 13). Path following for a class of mechanical systems. Control Systems Technology, IEEE Transactions on, 1), Hua, M.D., Hamel, T., Morin, P., and Samson, C. 13). Introduction to feedback control of underactuated vtol vehicles: A review of basic control design ideas and principles. IEEE Control Systems Magazine, 331), 1 7. doi:1.119/mcs Kapitanyuk,. and Chepinsky, S. 13). Control of mobile robot following a piecewise-smooth path. Gyroscopy and Navigation, ), doi:1.113/s URL Lee, T., Leoky, M., and McClamroch, N. 1). Geometric tracking control of a quadrotor uav on se3). In Decision and Control CDC), 1 9th IEEE Conference on,. doi:1.119/cdc Miroshnik, I. and Bobtsov, A. ). Stabilization of motions of multi-pendulum systems. In Control of Oscillations and Chaos,. Proceedings. nd International Conference, volume 1, vol.1. doi: 1.119/COC Miroshnik, I. and Lyamin, A. 199). Nonlinear control of multidrive vehicular robots. In Control Applications, 199., Proceedings of the Third IEEE Conference on, 79 vol.1. doi:1.119/cca Miroshnik, I. and Nikiforov, V. 199). Trajectory motion control and coordination of multilink robots. Prepr. 13th IFAC World Congress, A, Miroshnik, I. and Sergeev, K. 1). Nonlinear control of robot spatial motion in the dynamic environments. Proc. Int. IEEE Conf. On Advanced Intel. Mechatronics,, Nielsen, C., Fulford, C., and Maggiore, M. 9). Path following using transverse feedback linearization: Application to a maglev positioning system. In American Control Conference, 9. ACC 9., 3 3. doi: 1.119/ACC