Nonholonomic Motion Planning for a Free-Falling Cat Using Quasi-Newton Method

Transcription

1 ECHNISCHE MECHANIK, 31, 1, (2011), submitted: April 14, 2008 Nonholonomi Motion Planning for a Free-Falling Cat Using Quasi-Newton Method Xin-Sheng Ge, Wei-Jia Zhao, Yan-Zhu Liu he motion planning problem of a free-falling at is investigated. Nonholonomiity arises in a free-falling at subjet to nonintegrable veloity onstraints or nonintegrable onservation laws. When the total angular momentum is zero, the rotational motion of the at subjets to nonholonomi onstraints. he equation of dynamis of a free-falling at is obtained by using the model of two symmetri rigid bodies. he ontrol of system an be onverted to the motion planning problem for a driftless system. Based on the input parameterization, the ontinuous optimal ontrol problem is transformed into the disrete one. he quasi- Newton method of motion planning for nonholonomi multibody system is proposed. he effetiveness of the numerial algorithm is demonstrated by numerial simulation. 1 Introdution It is well nown that a at, when released from an upside down onfiguration starting from rest, is able to land on her feet. At the end of 19th entury, people began to try to explain this interesting phenomenon. Guyou and Marey (Liu, 1982) first explained from lassial mehanis that the angular momentum of a falling at is onserved. MDonald (1955) also represents this problem from a point of view of physiology. He believed a at firstly ontrats its front feet, then protrats the front feet while rotating its front body. Meanwhile, its rear body also experienes a rotation. Aording to the onservation law of angular momentum, the rotation angle of the front body is larger than that of the rear body in the opposite diretion. his theory satisfied the priniple of mehanis, however, in free-falling at experiments, we hardly find any obvious protrat-ontrat motion of the at s feet. Лойцянский (1954) present another explanation that the rapid rotation of the at s tail maes its body turn over in the opposite diretion. But his onjeture an not stand either. Experiments show that a at without tail an also finish the rotating motion. Kane and Sher (1969) proposed the dynamial explanation of the phenomenon that a free-falling at usually lands on its feet. hey assumed the at s turning motion with its waist as the top point using the model of two symmetri rigid bodies. Based on this model, a set of governing equation was established and the general harateristi of the turning motion was obtained. Further numerial analysis showed that this model mathes the experiment result very well. For the more general ondition of two unsymmetrial rigid body s turning motion, a set of dynamis equation was set up by Yanzhu Liu (1982). Reently, with the development of manned spaerafts and exploratory researhes of human turning motion under zero-gravity onditions, the researh on a free-falling at beomes a signifiant topi. Due to the nonintegrable angle veloity, the first integration of the equation of at s rotation is an equation with nonholonomi onstrains, and it is a speial nonlinear system. In this equation, the dimension of generalized oordinates is larger than that of the ontrol input. Broett (1993) first finished a systemati researh on the optimal ontrol problem of driftless nonholonmi system. Using ontrol objetive funtions to onstrut Lagrangian funtions, they reahed onlusions under optimal input of sinusoidal funtion and elliptial funtion respetively. Murray and Sastry (1993) extended Broett s onlusion to the ontrol of nonholonomi hain system under sinusoidal input. A similar motion planning method was also given by Reyhanoglu and Muherjee (1994), whih used Stoes theorem and aylor series expansion to analyze the dynami model of nonholonomi system. For motion planning problems of nonholonomi ontrol systems, various numerial methods were ahieved by some researhes. Fernandes et al (1995) formulated the nonholonomi motion planning problem as an optimal ontrol problem, and developed a simple algorithm for a oupled rigid body system using ideas from Ritz approximation 42

2 theory. Yih and Ro (1996, 1997) proposed the algorithms of near optimal motion planning using multipoint shooting and quasi Newton method for nonholonomi systems. Duleba and Sasiade (2003) disuss a modifiation of the Newton algorithm applied to nonholonomi motion planning with energy optimization. he Lyapunov ontrol method for solving motion planning was proposed by suhiya et al. (2002). In this method, the ontrol input is obtained by multiplying the gradient vetor of the Lyapunov funtion by a tensor. Ge et al (2007, 2006) studied an optimal algorithm to find feasible trajetories for motion planning of a free-falling at. Based on the Ritz approximation theory in funtional analysis, they approximated a solution of an infinitedimensional optimization problem by a family of finite-dimensional Fourier basis funtion expansion. In this paper, the motion of a free-falling at is formulated through a double rigid body model whih an represent the front and rear half of its body. he motion equation of a free-falling at is established based on multibody dynamis and onservation of angular momentum. When the total angular momentum is zero, the attitude motion of a free-falling at subjets to nonholonomi onstraints. he ontrol of a free-falling at an be onverted to the motion planning problem without drift. o find a globally onvergene strategy, the unit step funtions are introdued to form the ontrol inputs, and a quasi-newton method is designed to solve the nonholonomi motion planning problem. Finally, the algorithm is tested through simulation, and the simulation results indiate that the algorithm is an effetive approah to deal with a free-falling at. 2 Kinematis in Mixture heories o simplify the free-falling motion model, the body of a at is taen as two symmetri rigid bodies B 1 and B 2 whih are joined at O. Assume the rigid bodies are torsion free. Only bending exists when the at bends its spine. he oordinate systems O - XiYi Zi ( i = 1, 2) are presribed as follows: OXi is entroid axis of the rigid bodies pointing from O to the head of the at ( i = 1) or the tail of the at ( i = 2 ), OZi points to the abdomen of the at. he oordinate system O - X 2Y2 Z 2 is obtained by first rotating about axis OX1 through angle ψ to obtain # # # O - X1Y1 Z 1, then rotating about axis OY1 through angleϑ to obtain O - X1 Y1 Z 1, and finally rotating about axis # OX 1 through angle ϕ to obtain O - X 2Y2 Z 2. After getting O - X 2Y2 Z 2, we onstrut a new oordinate system # # O - X Y Z, in whih OX and OZ are along the bisetor of X1 OX1 and Z1 OZ1 separately, and OY is # oinident to OY1 and OY 1. he angleγ, whih equals to ϑ 2, is the angularity between the front half (or rear half) spine and OX. X OZ is the spine-urving plane. ψ denotes the position of the plane in the at s body. Figure 1: A free-falling at model Figure 2 : Attitude angle transform he angular veloityω of B2 with respet to B 1 is obtained by projetion on the O- X Y Z oordinate system as ω = ( ψɺ + ɺ ϕ)osγ i + 2 ɺ γ j + ( ψɺ ɺ ϕ)sinγ (1) 43

3 Aording to the torsion free ondition, the veloity omponent along axis OX must be zero, and then we obtain the relationshipɺ ϕ = ψɺ. Sine the initial ondition is also torsion free, we getϕ = ψ by employing integration. Considering the relation between angles ϕ and ψ, the equation (1) an be simplify as ω = 2( ɺ γ j + ψɺ sin γ ). (2) After bending the spine, the enter of mass of the at O loates on axis OZ. If we move the origin from O to O, the axis O X0 eeps a steady horizontal diretion during the observation of a free-falling at. A new oordinate system O -XYZ is built, in whih O X is oinident to O X 0, the axis O Z goes upward and vertially to the ground. During the proess of free falling, the inertia fores in O -XYZ are balaned with gravity. When we onsider rotating about the enter of mass and oordinates O -XYZ an be taing as the inertial referene frame. Set vertial plane XO Z as Π 0, and letφ be the lowise angle from plane Π 0 to Π 1. he purpose of rotational motion of the at is to mae its abdomen from faing upward to downward, namely, the angleφ from 0 toπ. he angular veloities of Bi ( i = 1, 2) with respet to O-X Y Z referene frame are ω1 = ψɺ osγ i ɺ γ j ψɺ sinγ (3) ω = ω + ω = ψɺ osγ i + ɺ γ j + ψɺ sinγ 2 1 Letting A B C m and a be the entral inertia moments, mass and distane between entroid and O of Bi ( i = 1, 2) respetively. he moment of momentum H i of Bi with respet to O ould be omputed. he vetor H i an be deomposed into omponents with respet to in the O - XiYi Zi ( i = 1, 2) oordinate systems; we have (Liu, 1982) H 1x H 2x A F E p H1y H 2 y F B D = = q (4) H E 1z H 2z D C r where and A = A + ma os γ sin γ, B = B + ma os γ (os γ + sin γ os ψ ) C = C + ma os γ (os γ + sin γ sin ψ ), D = ma os γ sin γ os ψ sinψ E = ma os γ sin γ os ψ, F = ma os γ sin γ sinψ 2 2 p = [( ψɺ os γ + ɺ γ sin γ os ψ sin ψ ) / (1 sin γ os ψ ) ɺ φ]osγ ψɺ 2 2 q = [( ψɺ os γ + ɺ γ sin γ os ψ sin ψ ) / (1 sin γ os ψ ) ɺ φ]sin γ sin ψ ɺ γ osψ ɺ 2 2 ɺ ɺ ɺ r = [ φ ( ψ os γ + γ sin γ os ψ sin ψ ) / (1 sin γ os ψ )]sin γ os ψ γ sinψ Adding H1 and H 2, the sum is the total moment of momentum of the at respet to O. After transformation to the O - X Y Z oordinate system, the omponent of the sum along axis OX is H = 2{[ Aos γ + ( Bsin ψ + Cos ψ )sin γ ][ ɺ φ ( ψɺ os γ + ɺ γ sin γ os ψ sin ψ ) / (1 sin γ os ψ )] (7) + Aψɺ os γ + ( B C) ɺ γ sin γ os ψ sin ψ} i During the proess of falling of at, the moment with respet to entroid is zero. Sine the angular momentum H is onservative, the assumption of invariane of diretion of axis O X or O X is proved to be true. Considering H 0, we an obtain the motion equation from equation (7) given by 2 2 ɺ { ψɺ os γ sin γ [ λ + (1 ε )os ψ ] + ɺ γ os ψ sin ψ (1 ε + λ sin γ )}sinγ φ = (8) (1 sin γ os ψ )[1 + ( λ ε os ψ )sin γ ] where λ = ( B A) / A, ε = ( B C) / A are parameters assoiated with the mass of at. Equation (8) is the nonholonomi attitude motion equation of free-falling at. 3 he quasi-newton method for nonholonomi motion planning he motion planning is to find ontrol input to steer a nonholonomi system from an initial onfiguration to final onfiguration along a feasible trajetory in time. We an formulate the motion planning problem as a nonlinear (5) (6) 44

4 optimal ontrol problem using a performane funtional. Without loss of generality we assume that a free-falling at has been formulated in the form 3 xɺ = G ( x ) u, x, x R (9) 0 f 1 0 G ( x ) = os γ sin γ [ λ + (1 ε )os ψ ] os ψ sin ψ sin γ (1 ε + λ sin γ ) (1 sin γ os ψ )[1 + ( λ ε os ψ )sin γ ] (1 sin γ os ψ )[1 + ( λ ε os ψ )sin γ ] 3 2 where x = ( ψ, γ, φ) R is the onfiguration variable, u R is the ontrol input noted as u 1 = ψɺ and 3 2 u =ɺ γ, G ( x ) R is a regular 2-dimensional distribution and the ost funtion to be minimized is 1 = 0 J ( u) < u, u>dt We assume that the system is ontrollable (Fernandes et al. 1995) and thus there exists a solution u L 2 ([0, ]) for the problem. Here, L2 ([0, ]) denotes the Hilbert spae of measurable vetor valued funtions of the form u( t) = [ u1 ( t), u2 ( t)], t [0, ]. By Ritz s approximation theory, the problem of nonholonomi motion planning is equivalent to an infinite dimensional optimization problem. One an use the alulus of variations to find the neessary optimality onditions for the nonlinear optimal ontrol problem. Sine the system is nonlinear, the optimal ontrol depends on the solution of the nonlinear multi-point boundary value problem (Yih 1996, Murray 1994). Numerial shemes suh as multi-point shooting method and quasi-linearization method an be used to solve the boundary value problem. However, one needs to find an initial solution lose to the optimum point in order to ensure the onvergene of these numerial algorithms. Also, sine only neessary onditions are satisfied, the minimum point is not guaranteed to be the solution of the optimization problem. For ompliated underatuated systems suh as the rigid spaeraft with two wheels, it is a very diffiult tas to test the optimality onditions. he problems mentioned above an be overome by using ontrol parameterization method. Using the parameterization of the ontrol variables, one an transform the infinite dimensional optimization problem to a finite dimensional one. In this paper, eah ontrol input is approximated by a pieewise onstant ontrol,whih maes it easier to implement the algorithm. he ontrol inputs an be expressed as follows N 1 j,i + 1 i i+ 1 i= 0 (10) u( t ) = h [1( t t ) 1( t t )] (11) where1( t) is the unit step funtion, h j, i + 1 is the ontrol input in[ ti, t i + 1], j = 1,2,, m. hen, one an reast the optimal motion planning as an optimal parameter searhing problem. he advantages of this indiret approah are: First, we do not need a very lose starting guess to the optimal solution beause a globally onvergent method an be utilized to find the minimum solution for the motion planning problem. Seond, the parameterization of the ontrol variables maes the algorithm easier to implement. By using penalty funtions to desribe the ost funtion, the ost funtion an be approximated by m N -1 j= 1 i= J ( h, ξ )= ( hj, i ) + ξ x( ) x f (12) where parameter ξ is the penalty fator, x( ) is the solution of Equation (9) at time t = with initial ondition x 0. Atually, restriting the ontrol input at the initial and final time leads to another onstraint, whih introdues a new penalty fator ζ..one N,ξ and ζ are hosen, the ost funtion given by (10) is a funtion of h whih an be expressed as 2 x f ζ t =0 t =0 t = t = J ( h ) =< h, h > + ξ f ( h ) + ( h h + h h ) (13) 45

5 where hi = 0 = [ h1, 1 h2, 1], hi = = [ h1, N h2, N ]. herefore the problem now is to find h suh that the ost funtion in (10) is minimized. A robust and globally onvergent quasi-newton method is used to find the solution for the optimal motion planning problem. Quasi-Newton methods use an iterative proess to approximate the inverse Hessian matrix so that no alulation for the seond derivatives is needed for arrying out the searh of the optimal parameters. Using line searh, the quasi-newton method also ombines a globally onvergene strategy with a fast loal onvergene rate of Newton s method. Define h to be the minimum of the ost funtional J ( h ). Let h be the urrent parameter and h be the differene between h and h as follows where h = h + h (14) h = ( J ( h ) - J ( h ) (15) 2 1 h h he above update algorithm is nown as Newton s method whih is quadratily onvergent in the neighborhood of the minimum. However, there are problems assoiated with Newton s method. First, the omputation of the inverse of the Hessian matrix is a very diffiult tas. Here we use the BFGS (Broyden-Felether-Goldfarb-Shanno) algorithm to update the inverse of a Hessian matrix. BFGS method has been nown to have a global onvergene. 2 1 Using the BFGS algorithm to approximate the inverse of the Hessian matrix, ( h J ( h ) - ) an be replaed by B whih is given by (Joshi, 2004) where B γ B γ δδ δγ B + B γ δ +1 B δ γ δ γ δ γ = + (1 + )( ) ( ) (16) δ = h h +1 γ = J ( h ) J ( h ) +1 We an use a line searh to ensure the global onvergene of the quasi-newton method. he following algorithm an be used to update vetor h suh that a minimization of J will be reahed h + 1 = h + λ p (18) where p = B J ( h ) (19) o minimize J ( h + λ p ) with respet to λ by the line searh, Define J ˆ ( λ ) = J ( h + λ p ) (20) With the following onditions J ˆ (0) = J ( h ), J ˆ (0) = J ( h ) p, J ˆ (1) = J ( h + p ) (21) o guarantee that { h } onverge to a minimize of the ost J ( h) and to avoid very small dereases in J ( h) relative to the lengths of the steps, we an write the step-aeptane riteria as follows (Yih, 1997) Jˆ (1) Jˆ (0)+ ξ Jˆ (0), ξ (0, 1) (22) If Ĵ (1) dose not satisfy Equation (22), we an approximate Ĵ ( λ) by the following quadrati model whih satisfies onditions (21) 2 J ˆ ( λ) = [ J ˆ (1) J ˆ (0) J ˆ (0)] λ + J ˆ (0) λ + J ˆ (0) (23) By setting Ĵ ( ˆ λ) = 0, we obtain ˆ ˆ J (0) λ = 2( Jˆ (1) Jˆ (0) Jˆ (24) (0)) It is easy to verify that Ĵ ( ˆ λ) > 0, thus ˆλ minimizes Ĵ ( λ ). Now we an replae λ in Equation (18) by ˆλ to update vetor h. Hene, the quasi-newton iteration proedure is desribed as follows: Step 1: Setting up initial and final onfigurations x0, x f R and G( q) R. Step 2: Assign initial parameters:ξ ζ h 0 δ B0 and the ontrol error r e. Step 3: Solve the differential equations given by (9), and ompute J ( h0 ) using (13). (17) 46

6 Step 4: Compute J ( h0 ) using (15), substituting h 0 and J ( h0 ) into equation (19) to ompute p 0, and substituting J ( h0 ), h0 and p 0 into equations (21)~(22) and (24) to ompute λ 0. Step 5: Compute h (=1) using (18), and he the ondition h < re. If the ondition is not satisfied, ompute B (=1) using (16), and repeat Step 2, otherwise exit. 4 Numerial simulation Assume that during the proess of a at s free falling, only its spine bends, there is no rotation between the front and rear body. he at bends its spine forward to all the diretions in turn and eeps angleγ onstant. When the front body of the at moves a whole irle, the whole body of the at turns radianπ in the reverse diretion, i.e. when angleψ hanges from 0 to 2π, the angleφ hanges from 0 to 2π. From the experiment data, λ 3, ε 1. 6 In the simulation experiment, λ = 3, ε = 0. 01, N = 20, ξ = 120 diag[ ], ζ = , e = 10, the time interval of falling is t =1s. he presribed time spae in simulation omputation is 0.05s. We denote the initial position and the end position of the free-falling at as x 0 = (0 π 6 0), x = (2π π 6 π ) f he simulation results are shown in Figure 4~7, where Figure 4 shows plots of the optimal ontrol inputs for the middle joint of the double rigid body. Figure 5-7 shows the attitude optimal trajetory of the at during its falling. he two ends of the urves are separately the initial point and the landing point. We an see from Figure 4 that the ontrol input urve is not as smooth as that in some other papers suh as (Ge, 2006) when we hoose the stepwise shape funtion (11), whih means that smooth onditions of it have to be onsidered if smooth ontrol input is required. From Figure 5 and Figure 7, it is obvious the at experienes a steady rotation. here s no detour behavior in the turndown motion. From Figure 6, we an see the bending angle has a small amplitude variation. hese simulation results are very inosulate to the experiment reord. Figure 4: he optimal ontrol input for free-falling at Figure 5: he optimal trajetory of angleψ Figure 6: he optimal trajetory of angle γ 47

7 Figure 7: he optimal trajetory of angleφ 5 Conlusion From the modeling of free-falling at and numerial analysis, we get the following onlusion: 1) he nonlinear ontrol problem of free-falling at an be transformed to a nonholonmi motion planning problem of a driftless system. 2) he nonholonomi motion planning problem an be solved effetively by quasi-newton method, whih implements the attitude planning of free-falling at and the optimal of ontrol input. During the simulation omputation, the quasi-newton method shows fast onvergene speed and good auray. 3) Using the stepwise shape funtion as the ontrol input is a new attempt. It has some advantages suh as onvenient in use and high onvergent speed. However, it brings about some diffiulties in designing atuators in the engineering praties. Anowledgments his wor was supported by the National Natural Siene Foundation of P. R. China (No ). Referenes Liu Y. Z., On the turning motion of a free-falling at, Ata Mehania Sinia, 149, (1982), (in Chinese). MDonald D. A., How dose a falling at turn over. Amer. J. Physiol. 129, (1955), Лойцянский Л.Г., Лурье А. И., Курс теореmuческая механuка, Гостехиздат, Москва, (1954). Kane. R., Sher M. P., A dynamial explanation of the falling at phenomenon. Int. J. Solids Strutures, 5, (1969), Broett R. W., Dai L., Nonholonomi inematis and the role of ellipti funtions in onstrutive ontrollability. In Li Z. and Canny J. F. editors, Nonholonomi Motion Planning, Kluwer, (1993), 1-22 Murray R. M., Sastry S. S., Nonholonomi Motion Planning: steering using sinusoids. IEEE ransations on Automati Control. 38 (5), (1993), Reyhanoglu M., A general nonholonomi motion planning strategy for Chaplygin systems. Proeedings of the 33 rd IEEE Conferene on Deision and Control , (1994). Muherjee R., Anderson D. P., A Surfae integral approah to the motion planning of nonholonomi systems. ASME Journal of Dynami Systems, Measurement and Control, 116 (9), (1994),

8 Fernandes C., Gurvits L., Li Z., Near-optimal nonholonomi motion planning for a system of oupled rigid bodies, IEEE ransation Automation Control, 39 (3), ( 1995), Yih C. C., Ro P. I., Near-optimal motion planning for nonholonomi systems using multi- point shooting method, in: Pro. IEEE Conf. Robotis and Automation, Minneapolis, M N, (1996). Yih C. C., Ro P. I., Near-optimal motion planning for nonholonomi systems with state/input onstraints via quasi-newton method, in: Pro. IEEE Conf. Robotis and Automation, Albuquerque, New Mexio, (1997). Duleba I., Sasiade J. Z., Nonholonomi Motion Planning Based on Newton Algorithm with Energy Optimization, IEEE ransations on Control Systems ehnology, 11 (3), (2003), suhiya K., Uraubo., sujita K., Motion Control of a Nonholonomi Systems Based on the Lyapunov Control Method, Journal of Guidane, Control and Dynamis, 25(2), (2002), Xin-Sheng Ge, Li-Qun Chen. Optimal Control of Nonholonomi Motion Planning for a Free-Falling Cat, Applied Mathematis and Mehanis 28 (5), (2007), Xinsheng Ge, Qi-Zhi Zhang. Optimal Control of Nonholonomi Motion Planning for a Free-Falling Cat. In: First International Conferene on Innovative Computing, Information and Control, Beijing, China, (2006) Murray R. M., Li Z., Sastry S. S., A Mathematial Introdution to Roboti Manipulation. CRC Press, (1994). Joshi M. C., Moudgalya K. N., Optimization heory and Praties, ASI Ltd. Harrow, U. K., (2004). Addresses: Prof. Xin-Sheng Ge, College of Mehanial & Eletrial Engineering, Beijing Information Siene & ehnology University, Beijing, , China. Prof. Wei-Jia Zhao, Siene College of Qingdao University, Qingdao , China. Prof. Yan-Zhu Liu, Department of Engineering Mehanis, Shanghai Jiaotong University, Shanghai, , China. gebim@vip.sina.om: zhweijia@vip.soho.om: liuyzhu@online.sh.n 49