Đang tải... (xem toàn văn)
We present an application of our constructive method for design cooperative controllers in (KhacDuc Do et al., 2018) to solve the problem of forcing a group of N ocean vehicles under environmental disturbances to track desired paths in a horizontal plane. The reader is referred to (Khac-Duc Do et al., 2018) for a survey of the formation control field.
ISSN: 1859-2171 TNU Journal of Science and Technology 192(16): 87 - 96 FORMATION TRACKING CONTROL OF OCEAN VEHICLES Dang-Binh Nguyen1,*, Khac-Duc Do2, Van-Vi Nguyen1, Van-Hung Nguyen1 Viet Bac University, 1B street, Dongbam ward; Curtin University, Autralia; ABSTRACT We present an application of our constructive method for design cooperative controllers in (KhacDuc Do et al., 2018) to solve the problem of forcing a group of N ocean vehicles under environmental disturbances to track desired paths in a horizontal plane The reader is referred to (Khac-Duc Do et al., 2018) for a survey of the formation control field Keywords: Formation tracking control, ocean vehicles Received: 12/11/2018;Revised: 21/11/2018; Approved: 28/12/2018 ĐIỀU KHIỂN BÁM NHĨM CÁC PHƯƠNG TIỆN GIAO THƠNG ĐƯỜNG BIỂN Nguyễn Đăng Bình1, Đỗ Khắc Đức2, Nguyễn Văn Vị1, Nguyễn Văn Hùng1 Trường Đại học Việt Bắc, Thành phố Thái Ngun Đại học Curtin, Úc TĨM TẮT Trình bày ứng dụng lý thuyết điều khiển nhóm (Khac-Duc Do cộng sự, 2019) vào việc thiết kế điều khiển nhóm bám mặt phẳng nằm ngang cho nhóm N phương tiện đường biển chịu tác động môi trường biển Người đọc tham khảo (Khac-Du Do cộng sự, 2019) để khảo sát lý thuyết thiết kế điều khiển hợp tác Từ khóa: Điều khiển nhóm, phương tiện giao thơng đường biển Ngày nhận bài: 12/11/2018; Hoàn thiện: 21/11/2018; Duyệt dăng: 28/12/2018 (*) Corresponding author: Tel:0913 286661, Email: nguyendangbinh@vietbac.edu.vn http://jst.tnu.edu.vn; Email: jst@tnu.edu.vn 87 Nguyen Dang Binh et al TNU Journal of Science and Technology 192(16): 87 - 96 MATHEMATICAL MODEL AND CONTROL OBJECTIVE The equations of motion of the i th ocean vehicle such as surface ships and underwater vehicles moving in a horizontal plane (for clarity roll, pitch and heave motions are ignored) can be written as 0: i = J ( i )i M ii = -Ci (i )i - ( Di + Din (i ))i + i + J T ( i )bi (1) with i [ xi yi i ]T , i [ui vi ri ]T , i [ ui vi ri ]T , bi [bui bvi bri ]T , cos( i ) sin( i ) J ( i ) sin( i ) cos( i ) 0 0 mi11 0 Ci13 Di11 , M i mi 22 mi 23 , Ci (i ) 0 Ci 23 , Di Din (i ) Di 22 Di 23 m m C C D D 1 i 32 i 33 i 32 i 33 i 31 i 32 (2) where mi11 mi X iu , mi 22 mi Yiv , mi 23 mi xig Yir , mi 32 mi xig N iv , mi 33 I iz N ir , Ci13 Ci 31 mi 22vi 0.5(mi 23 mi 32 )ri , Ci 23 Ci 32 mi11ui , Di11 ( X iu X i u u ui ), (3) Di 22 (Yiv Yi v v vi Yi r v ri ), Di 23 (Yir Yi v r vi ), Di 32 ( N iv N i v v vi N i r v ri ), Di 33 ( N ir N i v r vi N i r r ri ) where xi , yi are the surge and sway displacements, i is the yaw angle with in the earth fixed frame; ui , vi and ri denote surge, sway and yaw Yib X ib Y v coordinates Oib yi Oic velocities with coordinates in the body-fixed frame; mi is the mass of the ship; I iz is the i xig ship’s inertia about the Z ib -axis of the bodyfixed frame; xig is the X ib -coordinate of the O xi X ship center of gravity, O ic , in the body-fixed Figure Vessel coordinates frame (see Figure 1); the controls iu , iv and ir are the surge and sway forces Since collision is related to the position ( xi , yi ) of the vessel, we decouple the model (1) into the “position” and “orientation” models as follows and yaw moment in the body-fixed frame; biu , biv and bir are the constant disturbance forces and moment acting on surge, sway and yaw axes The other symbols are referred to as hydrodynamic derivatives For example, the hydrodynamic added mass force Yi along the y i -axis due to an acceleration ui in the xi -direction is written as Yi Yiuui with Yiu : Yi / ui We assume that all the ship parameters and disturbances are unknown but constant 88 qi i ui ui b i vi vi i i ri ri ri ri bi (4) where x cos( i ) sin( i ) ui qi i , i yi sin( i ) cos( i ) vi and we have chosen the control i as (5) i Ci (i )i ( Di + Din (i )i M i J 1 ( i ) J ( i )i [ ui vi ri ]T (6) http://jst.tnu.edu.vn; Email: jst@tnu.edu.vn Nguyen Dang Binh et al TNU Journal of Science and Technology where ui , vi and ri are new controls to be designed; ui , vi and ri are the first, second and third rows of J ( i ) M i-1 J T ( i ) , i.e ui vi ri J ( i ) M i-1 J T ( i ) T (7) In this section, we consider the problem of designing the control input i or ( ui , vi , ri ) for each vehicle i that forces the group of N vehicles whose dynamics are given in (1) or (4) to track a moving changeable desired formation graph in the sense that the desired formation graph is allowed to move on a desired trajectory Γod , and is allowed to change its shape including rotation, contraction and expansion, see Figure The group of N vessels needs N individual reference trajectories The desired formation is achieved by forcing each vessel to track its reference trajectory We consider the formation graph whose center O moves along a reference trajectory Γ od ( s ) with s being the path parameter We assume that Γ od ( s ) is regular in the sense that it is single valued and its first and second derivatives exist and are bounded Since the formation graph under consideration is only representative, the center does not have to be the center of the graph but can be any convenient point The shape of the graph can be varied by specifying the coordinates as a function of m called the formation shape parameter vector, from each vertex i to the center of the graph The parameter vector is used to specify rotation, expansion and contraction of the formation such that when converges to its desired value f , the desired shape of the formation is achieved When the graph moves along the trajectory Γ od ( s ) , the vertex i generates the reference trajectory qid (s,) for the agent i Designing the control input i or ( ui , vi , ri ) for each agent i that directly forces the vessel i to track its reference trajectory qid (s,) is difficult except for the case where the http://jst.tnu.edu.vn; Email: jst@tnu.edu.vn 192(16): 87 - 96 trajectory qid (s,) is a straight line due to collision avoidance taken into account Therefore we consider the dynamics of the vessels in the moving coordinate frame attached to the graph and its origin coincides with the center of the graph Moving frame Y Vessel i Y vi X yi yi xi O yod qid Γod O Formation graph xod X xi Figure Formation coordinates in 2D The control objective is formally stated as follows: Control objective: Assume that at the initial time t0 each vessel starts from a different location, and that each vessel has a different desired location on its reference trajectory qid (s,) , i.e there exists a strictly positive constant dij , which is referred to as the minimum safe distance between the vessel i and the vessel j , such that || qi (t0 ) q j (t0 ) || d ij (8) || qid q jd || d ij , i, j {1,2, N } Design the control input ( ui , vi , ri ) for each vessel i and an update law for the unknown disturbance vector bi , and the formation vector such that position and yaw angle of each vessel (almost) globally asymptotically tracks its reference trajectory qid (s,) and id , while avoids collisions with all other vessels in the group, i.e limt ( qi (t ) qid ) limt ( i (t ) id ) || qi (t ) q j (t ) || d ij , i, j {1,2, N }, t t0 (9) limt ( (t ) f ) 89 Nguyen Dang Binh et al TNU Journal of Science and Technology 192(16): 87 - 96 CONTROL DESIGN As mentioned before, we now consider the dynamics of the agents in the moving coordinate frame, OXY attached to the formation graph, see Figure The origin O of this frame coincides with the center of the graph, and is on the reference trajectory Γod ( xod ( s), yod ( s)) The OX and OY axes of this frame are tangential and perpendicular to the reference trajectory Γod ( xod ( s), yod ( s)) Therefore the angle between the OX and OX is calculated as arctan( yd' / xd' ) , where ' / s Let the coordinates and desired coordinates of the agent i assigned to the vertex i of the formation graph in the moving frame OXY be qi ( xi , yi ) and qid () ( xid (), yid ()) Therefore, if we are able to design the control input ( ui , vi , ri ) for the lim t ( qi (t ) qid ( )) 0, lim t ( (t ) f ) 0, agent i such that (10) || qi (t ) qi (t ) || d ij , lim t ( i (t ) id ) 0, t t0 where id is the desired yaw angle of the vessel i , and let the moving frame OXY moves along the trajectory Γod ( xod ( s), yod ( s)) , then the control objective is solved A simple choice of the desired angle is id This choice implies that we want the yaw angle i of all vessels to approach the same value arctan( yd' / xd' ) From Figure 6, we have qi R( )(qi qod ) (11) where qod [ xod yod ] and R ( ) is the rotation matrix given by T cos( ) R ( ) sin( ) sin( ) cos( ) It is noted that R ( ) is indeed invertible for all the solutions of (4) gives (12) Differentiating both sides of (11) along qi i ui ui R ( )( q q ) R ( )( q ) R ( ) b q i i od i od i od vi vi i ri ri ri ri bi (13) where i R( )( i qod ) R( )(qi qod ) Since the system (13) is of a strict feedback form, we will use the backstepping technique and the technique developed in the previous section to design the control ( ui , vi , ri ) to achieve the control objective The control design consists of two steps as follows Step At this step, we consider i and ri as controls Define i i ri ri r i (14) i where i and ri are virtual controls of i and ri , respectively In order to design i and ri , we consider the following potential function: i i i 0.5 ie2 0.5 || f ||2 90 (15) http://jst.tnu.edu.vn; Email: jst@tnu.edu.vn Nguyen Dang Binh et al TNU Journal of Science and Technology 192(16): 87 - 96 where is a positive tuning constant, and ie i id The functions i and i are the goal and related collision avoidance functions specified as follows (see Subsection 3.2 for motivation): ijk 1 i || qi qid ||2 , i (16) k k ij jN i ijd where Ni is the set of the agents which are adjacent to the agent i and 1 (17) ij (|| qi q j ||2 dij2 ), ijd (|| qid q jd ||2 dij2 ) 2 It is noted the yaw angle is not included in the collision avoidance function ij since it does not contribute to collisions Differentiating both sides of (15) along the solutions of (13) with the use of (14), (16) and (17) gives (18) i Ti ( i i ) Tij ( j j ) ie ( ri ri id ) Ti ( f )T jNi 1 ij k k k ijk 1 (qi q j ) ijd ij where i qi qif ij jNi ijk q jd q T qid i (qi qid ) 2 k (qid q jd )T id k 1 jNi ijd (19) T The equation (18) suggests that we choose the controls i and ri and the update as i C i ri i ie id ( f ) (20) where C 22 and m m are symmetric positive definite matrices, and i is a positive constant Substituting (20) into (18) results in T i Ti Ci Tij ( j j ) ie ri Ti ( f ) ( f )T ( f ) (21) i ie i i jNi qi Ci i (22) ie i ie ri Step At this step, we design the control i and an update law for the disturbance vector bi Consider the following function (23) i i 0.5 iT i ri biT b1bi Substituting (20) into the first two equations of (13) yields i where bi bi bˆi with bˆi an estimate of bi Differentiating both sides of (23) the solutions of (21) and the last equation of (13) results in i Ti C i i ie2 Tij ( j j ) Ti ( f ) ( f )T ( f ) jNi ui bˆ q R( )(qi qod ) R( )( i qod ) R( ) ui od i i i vi vi ri ri ri bˆi ie ri iT R ( ) ui ri ri bˆiT bi1 bi vi T i http://jst.tnu.edu.vn; Email: jst@tnu.edu.vn (24) 91 Nguyen Dang Binh et al TNU Journal of Science and Technology 192(16): 87 - 96 From (24), we choose the control i and an update law for the unknown parameter vector i as follows: ui ui bˆ q R 1 ( ) R ( )( q q ) R ( )( q ) H od i od i od i i i i vi vi i ri ri bˆi ie ri wi ri (25) T bˆi bi iT R ( ) ui ri ri vi where Hi is a symmetric positive definite matrix, and wi is a positive constant Substituting (25) into (24) results in T i Ti Ci Tij ( j j ) Ti ( f ) ( f )T ( f ) (26) i ie i H i i wi ri jNi Indeed, substituting the control ( ui , vi , ri ) into the derivative of i and ri results in ui b i H i i i vi i (27) ri wi ri ie ri bi STABILITY ANALYSIS We only show that with the control ( ui , vi , ri ) and the update law for the disturbance vector given in (25), and the update law for formation parameter (20), there are no collisions between agents, the solutions of the closed loop system consisting of (22), the third equations of (20) and (25), and (27) exist, and limt i Proof of the critical point q qd with q [q1T , , qNT ] T and qd [q1Td , , qNd ] being the asymptotically stable, and other equilibrium points being unstable or saddle follows the same lines as in Section We consider the following function (28) tot log(1 ) 0.5( f )T ( f ) N (i 0.5 i 0.5 ie2 0.5 iT i 0.5ri 0.5biT b1bi 0.5( f )T ( f )) where i i 1 N ( i 0.5 i 0.5 0.5 i 0.5ri 0.5b b 0.5( f ) ( f )) ie i 1 T i T i 1 bi i (29) T which is proper using the same arguments as in Subsection 3.2 Differentiating both sides of (28) along the solutions of (26) and the second equation of (20) satisfies 2 ( f )T ( f ) i 1 i 1 (30) From the expressions of and i , see (15), (16), (19) and (29), it can be checked that there tot 1 N (Ti C i i ie2 iT H i i wi ri ) exists a positive constant max such that 1 N || i 1 N || max f ) T i ( (31) i 1 Using (31), we can write (30) as max () N || i || max () || f ||2 min () || f ||2 (32) 4 (1 ) i 1 where is a positive constant, max () and min () denote the maximum and minimum eigenvalues of Picking min () / max () , we can write (32) as tot 92 http://jst.tnu.edu.vn; Email: jst@tnu.edu.vn Nguyen Dang Binh et al TNU Journal of Science and Technology 192(16): 87 - 96 max ( ) max max 4min Integrating both sides of (33) from t0 to t results in tot (t ) tot (t0 ) max (t t0 ) From definition of tot we can write (34) as tot N ( (t ) 0.5 (t ) 0.5 i i 1 i ie (33) (34) (t ) 0.5 iT (t ) i (t ) 0.5ri ( t ) 0.5biT ( t ) bi1bi ( t ) 0.5( ( t ) f ) T ( (t ) f )) N ( i (t0 ) 0.5 i (t0 ) 0.5 ie2 (t0 ) 0.5 iT (t0 ) i (t0 ) 0.5ri (t0 ) 0.5biT ( t0 ) bi1bi ( t0 ) i 1 0.5( (t0 ) f )T ( (t0 ) f )) max (t t0 ) (35) where ijk (t ) k k , ij ( t ) || qi ( t ) q j ( t ) || , ( t ) ijd ij jN i (36) k ij (t0 ) 1 2 i (t0 ) || qi (t0 ) qid || , i (t0 ) k k , ij ( t0 ) || qi ( t0 ) q j (t0 ) || 2 ij (t0 ) jN i ijd i (t ) || qi (t ) qid ||2 , i (t ) From (8) and (10) we have ij (t0 ) and ijd are strictly larger than some positive constants Therefore the right hand side of (35) cannot escape to infinity unless at the time t Therefore, the left hand side of (35) cannot escape to infinity for all t [t0 , ) This implies that ij (t ) cannot be equal to zero for all t [t0 , ) , i.e no collisions can occur for all t [t0 , ) Since the left hand side of (35) cannot be escape to infinity in a finite time, qi (t ) cannot escape to infinity in a finite time This means that the solutions of the closed loop system consisting of (22), the second equations of (20) and (25), and (27) exist On the other hand, it is true from the second equation of (20) that || (t ) f ) |||| (t0 ) f ) || e min ( )( t t0 ) (37) which implies that the desired formation shape is exponentially achieved Substituting (37) into (30) yields tot max ( ) max || (t0 ) f || e min ( t t0 ) (38) Integrating both sides of (38) from t0 to t gives N ( (t ) 0.5 (t ) 0.5 i i 1 i ie (t ) 0.5 iT (t ) i (t ) 0.5ri ( t ) 0.5biT ( t ) bi1bi ( t ) 0.5( ( t ) f ) T ( (t ) f )) N ( i (t0 ) 0.5 i (t0 ) 0.5 ie2 (t0 ) 0.5 iT (t0 ) i (t0 ) 0.5ri (t0 ) 0.5biT ( t0 ) bi1bi ( t0 ) i 1 0.5( (t0 ) f )T ( (t0 ) f )) max ( ) max || (t0 ) f || / min (39) The right hand side of (39) is bounded Therefore the left hand side of (39) must also be bounded This implies that the third inequality of (10) holds Since limt ( (t ) f ) , applying Barbalat’s lemma to (30) gives N lim t (Ti (t )Ci (t ) i ie2 ( t ) 0.5 iT ( t ) i ( t ) 0.5ri ( t )) (t ) i 1 which implies that T limt Ti (t )Ci (t ) i ie (t ) 0.5 i (t ) i (t ) 0.5ri (t ) limt (t ) 1 http://jst.tnu.edu.vn; Email: jst@tnu.edu.vn (40) (41) 93 Nguyen Dang Binh et al or TNU Journal of Science and Technology 192(16): 87 - 96 T limt Ti (t )Ci (t ) i ie (t ) 0.5 i (t ) i (t ) 0.5ri (t ) limt (t ) where and are some constants From definitions of i and , the limit set (42) cannot be true Therefore, the limit set (41) implies that limt || (i (t ), ie (t ), i (t ), ri (t )) || Therefore, carrying out the same analysis as in Section yields the critical point q qd is the asymptotically stable, and other equilibrium points are unstable or saddle Furthermore, we can let the moving frame OXY move along the trajectory Γod ( xod ( s), yod ( s)) by letting qod move, i.e by giving s some desired value since ' ' qod [ xod ( s) yod ( s)]T s Finally, we note that convergence of q to qd implies that of qi to R( )1 qid qod qid , i.e what we wanted to achieve We summarize the results of this subsection in the following theorem Theorem Under the assumptions stated in the control objective (see (9)), the control ˆ for unknown i and the update law i parameters given in (25), and the update law for formation parameter (20) each agent i solves the control objective, i.e (9) is achieved (42) (0,0) ODIN ODIN ODIN (r,-r) (-r,-r) ODIN (0,-2r) Figure Desired formation graph SIMULATION RESULTS We now illustrate the results stated in Theorem by a simulation on formation tracking control for a fleet of omnidirectional intelligent navigators (ODINs) moving in a horizontal plane, see Figure The parameters of each ODIN are taken as follows mi11 mi 22 300, mi 33 16, X iu Yiv 168, Nir 10 All other parameters defined in (3) are equal to zero The disturbance vector is taken as bi 0.5[mi11 mi 22 mi 33 ]T The safe distance between any two ODINs is dij 0.8 , (i, j ) {1,2,3,4} The control gains and tuning constants are chose as C diag(3,3), 0.1, k 1, wi 1, i 1, bi diag(1,1,1) and the trajectory parameter 0.1 ||qi qid || For clarity, s is updated with s e we not include the formation change in simulations, i.e the formation parameter is not used The desired formation shape is specified as q1d [0, 0]T , q2d [r, r]T , q3d [0, 2r]T , q4d [r , r]T with r 15 , see Figure We carry out two simulations For the first simulation, the initial conditions are chosen as q1 (0) (0.7r,0), q2 (0) (0, 0.7r), q3 (0) (0.7r,0), q4 (0) (0,0.7r) , and the i 1 Figure An outside view of an ODIN Courtesy http://www.eng.hawaii.edu/~asl/odinpics 94 http://jst.tnu.edu.vn; Email: jst@tnu.edu.vn Nguyen Dang Binh et al TNU Journal of Science and Technology 192(16): 87 - 96 motion of ODINs in the (x,y) plane is plotted in Figure The controls 1 [ u1 v1 r1 ]T , qod [r sin( s) r cos( s)]T A snap shot of and the distances from ODIN to all other motion of ODINs in the (x,y) plane is plotted ODINs || q1 qi ||, i 2,3,4 are plotted in in Figure For clarity, we only plot the T Figure It is seen from these figures that all controls 1 [ u1 v1 r1 ] , and the distances ODINs nicely form the desired formation and from ODIN to all other ODINs the desired formation graph moves on the || q1 qi ||, i 2,3,4 in Figure For the desired reference trajectory It is also seen second simulation, the initial conditions are that these distances are greater than chosen as 2d , (i, j ) {1,2,3,4} , i.e no collisions q1 (0) (0, 1.7r), q2 (0) (0.7r, r), q3 (0) (0, 0.3r), qij4 (0) (0.7r, r) between the ODINs occur , and the reference trajectory qod is a straight reference trajectory qod is a circle given by line given by qod [s 0]T A snap shot of Figure Circular reference trajectory: a snap shot of motion of ODINs in (x,y) plane Figure Circular reference trajectory: Controls and distances from ODIN to other ODINs http://jst.tnu.edu.vn; Email: jst@tnu.edu.vn 95 Nguyen Dang Binh et al TNU Journal of Science and Technology 192(16): 87 - 96 Figure Linear reference trajectory: a snap shot of motion of ODINs in (x,y) plane Figure Linear reference trajectory: Controls and distances from ODIN to other ODINs REFERENCES Fossen T.I (2002) Marine control systems Marine Cybernetics, Trondheim, Norway Khac-Duc Do, Dang-Binh Nguyen, Van-Vi Nguyen and Van-Hung Nguyen (2019) Formation stabilization of mobile agents using local potential functions, TNU Journal of Science and Technology, 192(16), pp 73-86 96 http://jst.tnu.edu.vn; Email: jst@tnu.edu.vn ... TNU Journal of Science and Technology 192(16): 87 - 96 MATHEMATICAL MODEL AND CONTROL OBJECTIVE The equations of motion of the i th ocean vehicle such as surface ships and underwater vehicles moving... function of m called the formation shape parameter vector, from each vertex i to the center of the graph The parameter vector is used to specify rotation, expansion and contraction of the formation. .. The control design consists of two steps as follows Step At this step, we consider i and ri as controls Define i i ri ri r i (14) i where i and ri are virtual controls of