Xây dựng thuật toán điều khiển quỹ đạo và bám mục tiêu di động mặt đất cho UAV cánh bằng TT TIENG ANH

MINISTRY OF EDUCATION AND TRAINING MINISTRY OF DEFENCE ACADEMY OF MILITARY SCIENCE AND TECHNOLOGY PHAM THI PHUONG ANH ALGORITHM DESIGNING TRAJECTORY CONTROL AND GROUND MOVING TARGET TRACKING FOR FLATWING UAV Major: Engineering control and automation Code: 52 02 16 SUMMARY OF DOCTORAL THESIS IN ENGINEERING Ha Noi – 2021 THE WORK WAS COMPLETED AT ACADEMY OF MILITARY SCIENCE AND TECHNOLOGY Scientific Supervisors: Assoc.Prof, PhD Nguyen Vu PhD Phan Tuong Lai Reviewer 1: Assoc.Prof, PhD Bui Xuan Khoa Reviewer 2: Assoc.Prof, PhD Nguyen Quang Vinh Reviewer 3: PhD Đoan The Tuan The thesis was defensed before the doctoral admission Board of Academy of Military Science and Technology at time date in 2021 The thesis can be fouund at: Library of Academy of Military Science and Technology Việt nam National library Preface Urgency of research Nowaday, UAVs are more popular in social life and defence-security applications The most popular problems in UAV control are path following or ground target tracking At present, the auto pilot systems are very popular For the autopilot control system integrated UAVs, to realize their mission, they must have information about their state vetor (position and attitude) and simultaneously must have control algorithms for outer loop (trajectory control loop) and for inner loop (already integrated autopilot control loop) The mentioned above problems had been studied under different aspect and expressed in various published works However, in the published works there are not real explicit solutions for desired control quality guarantee To overcome this limitation, robust control algorithms must been study continuously in purpose to guarantee that desied control quality are maintained instate of unwaited influences At the same time, the ground target tracking problems must been solved separatly because inspite of path following control the ground target tracking problems are still influented by target speed To satisfy real fly mission, the above mentioned problems must been solve with explicit algorithms for concrete cases Hence, the thesis theme is urgent, scientific and practical Purpose of research The thesis purpose of research: - To design UAV path following algorithms, focus on lateral path following - To design algorithms for UAV ground target tracking using UAV pantilt camera Terms of research Turtorial research on UAV control systems, dynamic model in UAV path control systems, algorithm for UAV lateral path following using UAV roll angle The research on using sliding mode control for trajectory control loop in UAV path control systems The research on design UAV trajectory control algorithm for UAV ground target tracking missions Simulation for thesis results confirm Objects and and sphere of research Objects and of research of thesis is flat-wing UAV, bounded in dynamic of trajectory control loop Sphere of research is trajectory control loop (with supposition that stability control loop has time constant not worth in comparition with time constant of trajectory control loop) in two cases of path following control and groung moving target tracking control Research methodology The thesis uses theorical method in combination with simulation for thesis results confirm, specifically research related publications, then suggest research directions, develop control algorithms by theorical research, conduct simulations to evaluate, verify, and propose suitable solutions Scientific and practical significance The thesis scientific significance: Thesis has theorical designed control algorithms for trajectory control loop in UAV path control systems and ground target tracking systems The thesis practical significance: The thesis results may apply for flatwing UAV in real life mission as environment observation, forest fire prevent, goods transport, agriculture service…or in defence purpose like intelligence, surveilliance, discovery, tracking and protection ground objects Thesis structure With main terms mentioned above, the thesis includes the introduction, chapters and conclusion Chapter UAV and UAV control turtorial 1.1 Structure schema of UAV control system Autonom control system of UAV, which includes fly mission block, control block, sensor systems and sensing computation system, is shown by general structural schema as presented below: Flying mission Control Block Trajectory Stabilized control control sensing comp system UAV dynamic sensor systems Figura 1.1 General structural schema of autonom control system of UAV As shown in Fig 1.1, the fly mission are preloaded on onboard computer On the basis of flying mission and UAV state vector, the trajectory control block (outer loop) makes data processing in real time and its output are trajectory control law as roll angle, pitch angle and power of propulsion system On the basis of UAV state vector, the stabilized control block (inner loop) is got control law from the trajectory control block and will make control law for enforced mechanism of UAV, which control UAV so that real pitch and roll angle of UAV are equal desied angles as output of trajectory control block In practice, inner loop (stabilized control block) has time constant very small in comparision with trajectory control loop (outer loop), so that in same cases, we can ignor the inner loop and then UAV parameters are repeated input law of inner loop (rool and pitch angles) UAV states are determained sensor systems and sensing computation system 1.2 Control loops of UAV control systems: Setting par Outer loop Roll, pitch UAV dynamic Inner loop UAV output UAV states  ststates States mesuaring system Figura 1.2 General structural schema of autonom control system of UAV with state and control vectors In Fig 1.2, Control system of UAV is divided into two control loops On the basis of flying mission and UAV state vector, outer loop makes data processing and its output are control parameters, which are stabilized by inner loop Inner loop is stabilized control loop with integrated in popular autopilot device relatively perfect control algorithms Time constant of inner loop is not worth in comparition with time the constant of outer loop So the thesis is concentrated in outer loop for lateral control 1.3 UAV trajectory control dynamic Firstly, the relationship between values of wind speed vector, airspeed vector and ground speed vector of UAV: Value of ground speed vector will determined by the wind speed vector and airspeed vector as belows: vg  va2  vw2  2va vw cos(     w ) (1.4) Value of airspeed vector vector will determined by the wind speed and ground speed vector as belows: va  vg2  vw2  2vg vw cos(   w ) (1.5) Value of wind speed vector will determined by airspeed vector and ground speed vector as belows: vw  vg2  va2  2vg va cos(    ) (1.6) From (1.6) deviation angle between airspeed vector and ground speed vector are determined as belows:    arc cos  vw2  vg2  va2  /  2vg va  (1.8) In this section, simple UAV mathematical model is applicated for design UAV trajectory control Schema of force components of UAV when UAV in constant heigh fly with roll angle  is shown in Fig 1.6 below:  : roll angle P : gravitational forrce; m: UAV mass; g: gravitational acceleration; Fz :UAV airodynamical force; Fz cos  : lifting force; Fz sin  : centripedal force; R : radius of UAV trajectory Fig 1.6 Components of force in inclined wing flying mode Asumed that UAV airodynamical force is matched condition so that UAV lifting force is balanced its gravitational forrce Then we have: mv u (1.19) F cos  mg ; F sin  z z R Let  e is angle error of trajectory direction, ye is distance error in path following, we get the UAV dynamic equation without influence of wind as below:    y   v sin     e e   g u   u (1.20)         v   e  u with u  tg  (va / g )  p the UAV dynamic equation with influence of wind as belows:  y e  vg sin  e (1.21)    e  ku u In which u is control law, u  u1  u2  u3 with: u1  tg ; u2   p / ku ; u3  N / ku ; ku  g / va and N  arccos'  va2  vg2  vw2  / 2vg va  1.4 UAV trajectory control Structural schema of lateral trajectory control system is shown in Figure 1.8 For simplification of problem, fist at all straight line are considered Fly mission Trajectory Lateral control UAV dynamic Mes system Fig 1.8 Structural schema of lateral trajectory control system The popular path following algorithms are presented below: 1.4 Virtual target following algorithm Nature of this algorithm is to define point S, which is virtual target location, it located at a distance  from point Q, which is the UAC projection on straight trajectory (Fig 1.9a) or located in the circle with center Po so angle S PoQ   (Fig 1.9b) UAV desired heading is line PS Figura 1.9: (a) Virtual target defining schema for straight path (b) Virtual target defining schema for circle path radius r 1.4.2 Nonlinear control algorithms Nonlinear control algorithms also uses the virtual target point as virtual target following algorithm, but the mechanism by which the virtual target position is updated is different, as shown in Fig 1.10a From the current UAV position P draw a circle of radius r The circle will intercept the path at two points S and S S is selected as the VTP because the UAV has to move awards S point 1.4.3 Pure pursuit (P) and light of sight (LOS) control algorithms Figure 10: (a) Nonlinear control algorithms defining schema; (b) Pure pursuit (P) and light of sight (LOS) control algorithms defining schema The pure pursuit law directs the UAV towards the way point Wi 1 , while the LOS law pushes the UAV towards the light of sight or ensures that the angle of line Wi P is the same as the angle of line Wi Wi1 (Fig 10b) The resultant motion of the UAV with P and LOS laws will allow the UAV steee toward the path 1.4.4 Vector field control algorithm The main idea is based on vector field, that determine the direction of flow which the UAV follow to the path This law operates in two modes: When UAV is quite far away from the path, the nature strategy is to move towards the path as quick as possible When the UAV is near the path, the algorithm uses to determine the desired heading angle in dependent of distance to the path 1.4.5 LQR control algorithm LQR based path following algorithm uses optimal control theory to compute control Each of five control algorithms shown above have own advantages and disadvantages The parameters of this control laws have to choose in separate case to ensure control quality For complication decreasing of control law parameter choosing, we need other synthesis method for trajectory control system so the control law parameters have not to choise in separate case, but the control quality allways stayed as desired Hence the thesis will use sliding mode control for trajectory control system synthesis, in which the different sliding surfaces are proposed That is one of problems of the thesis 1.5 Measurement system on UAV Using on board sensors and suitable processing algorithms mesuaring system of UAV gives directly or by estimation UAV translational states (position and speed) and rotational states (attitude and angle speed) Sensors in UAV are presented as belows: 1.5.1 UAV translational state sensing Translational state sensing includes next sensing systems: global positioning system and pressure sensors 1.5.2 UAV rotational state sensing UAV rotational state sensing next sensors: gyroscope, accelerometers and magnetometer 1.5.3 Sensor system integration Structural schema of multisensory mesuaring system is shown in Fig 1.14 below, in which UAV Ouler angles and angle speed are determined by IMU, accelemeters and magnetometer, and translational states are determained by GPS and pressure meter The state estimation as shown in Fig1.12 is made by using Kalman filter and usually integrated in one module, which is called Attitude and Heading reference system, is done through the kalman filter kalman and usually integrated in a separate block and referred to AHRS Fig 1.11 The state estimation module 1.6 Overview of inside our country published works Althouth in present time flying objects are studied by many scientifics, but much more published works are concentrated in flying objects like missiles The research works in flatwing UAV control are not popular and only concentrated in the same sphere of UAV control The problem of desired parameter determaination for stabilized control loop still are not concerned in national published works about UAV control, as one as problem of ground moving target tracking by using pan-tilt camera also is not concerned in published works Conclution of chapter The problems which need to study are: - Desighning UAV trajectory control algorithm by using slide mode control - Desighning UAV ground moving target tracking control algorithm The above problems are research objets of thesis and will solve in next chapters Chapter Designing UAV trajectory control algorithms 2.1 Sketch of sliding mode control At present time, sliding mode control is interested in scientifics and continuously developed and has many applications in real life In thí secsion same basic problem of sliding mode control technique are presented as the basic to synthesis UAV trajectory control Then the problem of UAV path following control using sliding mode control í expressed as belows: Given path with its parameters and direction angle  P and UAV with position, the problem of UAV path following control is determained desired UAV direction angle  d so that with thís direction angle, UAV will move closer to path and follow the given path The path usualy is straight line or circle The purpose of path following control is to make position error ye  and angle error    P  in all time of mission t   , with UAV direction angle  2.2 Path following control algorithm with linear sliding surface 2.2.1 Position and angle error defining algorithm of UAV in comperation with given path 2.2.1.1 Straight line path Position and angle error are determained as below:  y  ( x  x )  ( y  y )2 sin(    ) e u i u i p pw (2.22)    e     p 2.2.1.2 Circle line path 11 2.3 Navigation algorithm based on nonlinear sliding surface in form of trigonometric function Although navigation algorithm with linear sliding surface makes control system robust, but from reason that coeffient of state variable in sliding surface is limited, so when the state variables are near to the origin, the capability of high speed response of system is bounded In this case, when nonlinear sliding surface is applied, the system quality will be improved 2.3.1 Sliding surfasce chosing For satisfying condition that distance error is small and angle error  e   the sliding suface is chosen as bellows: s   e   ac tg   ye  (2.50) In (2.50)  ,  are scalar coefficients    For satisfying condition about distance error,  is used, and  is chosen for    (   ) e 2.3.2 Equivalent control law The equivalent control law is the continuous control law that makes s  If the object dynamic is known, the equivalent control law can be defined From (2.50) we have: or   s    y  e 1  y e e gutd   v sin   e va 1  ye a (2.55) (2.56) From (2.56) we have equivalent control law as bellows:   v   u   a sin e  td  g 2  1  ye   (2.57) In (2.57) utd is equivalent control law The chosen equivalent control law include linear and nonlinear parts: u  utd   sisn  s  ,  (2.59) The simulation results show that control system is stabilized, transient time when the system states lie in the sliding surface (2.50) is shoter than transient time when the system states lie in the sliding surface (2.34), However, the system quality still depends on chosing parameter  ,  2.4 Navigation algorithm with nonlinear sliding surface based on Dubin trajectory Reason of chosing nonlinear sliding surface based on Dubin trajectory is that the Dubin trajectory forces UAV traveling by shoted trajectory based 12 on straight and circle lines with constant radius so when UAV follows this circle the system control with control reservation for sliding mode control With Dubin trajectory, from the starts position and direction, UAV will follow the circle untill speed vector becomes perpendicular to path direction (When the distance from UAV to path biger than circle radius r) and follow the straight line until speed vector becomes tangent to circle radius r , which is tangent to path (When the distance from UAV to path more little than circle radius r) The UAV path following dynamic is:  y e  Vu sin(    p )    ku  With: u  tan r (2.63) , k  g / va In the Dubin trajectory, the relationship betwen d ye is defined as bellow:    d   sgn  ye    p ye  r       (   arcsin(1  ye )) sgn  y  + e p ye  r  d r (2.64) The distance from UAV to path ye is replayed by distance error with symbol e1 , e1 is saturated function:  e1  r ye  r  e1  ye ye  r (2.65)    e From (2.64) we have:    p      arcsin     sgn  e  d  r  2    (2.66)  The equation (2.66) is a database for design nonlinear sliding surface based on Dubin trajectory Then for system the nonlinear sliding surface is applied.:   e sgn e     s     p  sgn e1  arcsin 1  (2.67)   r 2    Let    p  e2 Then sliding surface is:       e sgn  e1    s  e2  sgn  e1    arcsin    r     The control law is defined by next lemma: (2.68) 13 Lema 1: For system:  y e  Vu sin(    p )    ku  (2.69) a) Exist sliding mode on sliding surface:   e sgn e    s     p  sgn e1   arcsin 1  (2.70)   r 2    With control law:            vu   vu u sin    p    sgn  s   (2.71)    p  d r g     e      1      r        b) In sliding mode, system will exponel stabilize The simulation results show that control system is stabilized, transient time when the system states lie in the sliding surface (2.70) is shorter than transient time when the system states lie in the chosen sliding surface The quality of system is not changed, no need to choose parameter of sliding surface 2.5 Curve path following in Serret-Frenet (S-F) frame In the all navigation problem for UAV, the distance from UAV to the path must to be defined, but this problem is relatively complicate when the path is curve with charging radius Besides that, the navigation problem sometime has singular point, when UAV position is the center of curve path To overcome this limitation, the S-F frame is applied Frame origin speed then becomes control law in purpose to maintain UAV coordinates in S_F frame become  xsu , y su    0, y su  Then path direction and distance error of UAV are defined for synthesis path following control system by mention above algorithms The simulation results are explained bellow The curve path in S-F frame is presented bellow:  (t )  sin( /  t ); x(t )  40cos( (t )); y(t )  40sin( (t )) When following the path in S-F frame, system can quickly define the origin of S-F frame so longitudinal distance error become to zero, then the system can easily define direction of path and lateral distance error, In       14 the, when the origin of S-F frame slides on the path, lateral error is changed too and like change of S-F frame origin in navigation frame When the longitudinal distance error become to zero, the lateral distance error monotonuous comes zero Conclution of chapter 2: Main term, which has presented in chapter 2, are synthesis UAV path following control system based on sliding mode The linear sliding surface with distance error, the mixing linear sliding surface, nonlinear sliding surface with arctan function and nonlinear sliding surface based on Dubin trajectory are put forward One of put forward sliding surface, nonlinear sliding surface based on Dubin trajectory will make angular speed of UAV in coming to origin process, or coming to following path greater than another sliding surface Then the transient to path time is smaller than another sliding surface This conclution is proved by simulation results For the curve path with changing radius, the defining distance error by analytical geometry is relatively complicated To solve this problem, the path following in S-F frame with sliding mode are put forward, which will help to define lateral error by making longitudinal error equal to zero The results in chapter may directly apply in planned fly mission, to follow the not changing path or apply in following the changing path, for example in tracking ground moving target, which is presented in chapter bellow Chapter SYNTHESIS OF UAV GROUND MOVING TARGET TRACKING SYSTEM As shown in chapter 2, not only followed the planned path, UAV still has to follow changing path, one of their applications is ground moving target tracking For ability or target observation, searching, detection and tracking, UAV is equipped camera with pan-tilt platform (pan-tilt-zoom camera), its azimuth and elevation angles and field of view is controllable In the tracking mission, the assumption that UAV has stable speed v u and height h is used The problem of ground moving target tracking includes searching, detection, image autotracking, pan-tilt control for target observationby camera with stabilized quality of image In target tracking mode, UAV mission is defining target coordinates and speed for control UAV for best target tracking and for another purpouse 15 3.1 Target coordinates and speed defining 3.1.1 Camera model and Camera frame Image sensor makes the mapping from 3D space to the image plan The coordinates transfer from points in 3D space to image plan has the mode as shown bellow:     f yc    c     x  z c  Where c p   x c y c z c    T , (3.1) : the target position camera coordinates,  ,  : target coordinates in image plan, f ; camera focus length ib ,ib , ib  , bc ,bc , bc  : sequentially are Euler angle of UAV body frame in inertial frame, and Euler angle of camera frame in UAV body frame When camera is located on pantilt platform, we have: bc  0;  bc  900   ; bc   C Let TNED direction cosine matrix from ỉnertial or NED frame to camera frame, Then coordinates of target in camera frame are defined as bellows: C C p T p p T NED  T u  Where: p  R u (3.2) is UAV position in inertial frame, pT R is target C position in inertial frame, p  R is target position in camera frame At T the same time we have: C C B T  T T NED B NED (3.3) Where: TBC direction cosine matrix from UAV body frame to camera frame; B direction cosine matrix from inertia or NED frame to UAV body TNED frame With formulas (3.1), (3.2), (3.3) the problem of target coordinates in image plan has done The concrete algorithm is presented bellows 3.1.2 Determine target coordinates in inertia frame from Target coordinates in camera frame The steps to are listed bellow: Step 1: direction cosine matrix from ỉnertial frame to camera frame definition by formula (3.2) Step2: When the Oxc axis intersected target position, we have y c  z c  , then target coordinates in camera frame are: 16 p C T   x c  0   T C T p p NED  T u  (3.4) In the equations (3.4) there are equations and unknown zT is roughly defined by area digital map To solve this equations, let:  a11 a12 a13  C TNED   a21 a22 a23   a31 a32 a33  x c , xT , yT Parameter (3.5) pT  p u   x y z  After same changes, we have the target coordinates in UAV body frame ( x c , y c , z c ) and in inertial frame ( xt , yt , zt ) Step 3: In the case that the Oxc axis not intersected target position, the target coordinates in image plane are   T and defined by formula (3.1) To resolved this problem, the expanded camera frame is applied The expanded camera frame OxCE , yCE , zCE is a frame, which is receipted from camers frame by turnning  angle in elevation and  angle in azimuth (  and  are coordinates of expanded camera frame) so that the axis OxCE intersected the target position with coordinates in image plane    T   arctg  ,   arctg  cos (3.11) f f After having Euler angles of expanded camera frame, we repeat step to define direction cosine matrix from ỉnertial frame to expanded camera frame: CE CE C (3.12) T T T NED C NED Replacing C T NED in equation (3.4) by CE T NED and executing the transform in the step Target coordinates in UAV frame is defined by formula (3.5): P  P  P   x y z  t u T Step 4: Target coordinates and speed defining P  P  P t ta a (3.19) Let at ti target coordinates is xt  ti  , yt  ti  , zt  ti  , ti 1 target coordinates is xt  ti 1  , yt  ti 1  , zt  ti 1  Then target speed is defined by target 17 coordinates in two time ti and ti 1 as bellow:  ut (ti 1 )    xt (t i 1 )  xt (t i )/(ti 1  ti )      v   vt (ti 1 )     yt (t i 1 )  yt (t i ) /(ti 1  ti )  t    w (t )   t i 1   zT (t i 1 )  zt (t i ) /(t i 1  ti )  (3.20) As shown above, the parameters of target coordinates and speed are fully defined by formula (3.20) That is the input parameters to design target tracking algorithm 3.2 Ground moving target tracking by loitering mode As shown above, ground moving target tracking mode are chosen in relationship target, with target speed For not moving or moving with low speed, loitering mode is the most suitable mode However, for high speed moving target this mode is not still suitable The speed condition for UAV moving target tracking with loitering mode is: vtmax  vu ( R  r ) r (3.31) vu  vtmax (3.32) Circular path Loitering mode for moving target tracking is deferred at circular path following that, because of target moving, the path direction angle is not tangenting to the circle at intersection with projection of line connected the circular center to UAV position in circular plan but a vector, which is depended also at target speed vector and value of UAV speed It is shown in Fig 3.5 bellows: P: UAV position, pu   xu yu zu  ; T: Target position, pt   xt yt zt  ; Q Intersection of PT line with circular with center Y and radius R; v t , vt : target speed vector and its value; v u , vu : UAV speed vector and its value; v ud Desired UAV speed vector when Figura 3.5 Schema for path tracking by Dubin trajectory; v ut , vut : relative speed vector of UAV with direction defining for target Relative speed vector of UAV withand target: v ut  v u  v t (3.33) its value tracking with loitering mode target Triangle QRQ0 has sides: QR : tangent to circular with center T at point Q; length of RQ0  vT ; lenghth of QQ0  vu ; Triangle PSP0 is received by translating triangle QRQ0 so that Q reaches to P;  : angle of line TP;  : angle of line QR;  : desired angle of line QQ0 so that if UAV moves to this direction, relative speed vector of UAV with target has direction angle 18  ; vt 0 : target speed in direction of  : vt 0  vt cos  ; vt : target speed in direction of  ; vt  vt sin  Without losing the general, let moving direction of target is  t  Then  p   : desired path direction at time UAV position is P;  : direction angle of UAV;  d : desired direction angle of UAV in Dunin trajectory mode (2.68) ( v ud direction angle); ye : UAV tracking error at desired distance R from UAV projection in target plane to target Dynamic equation (3.38) with tracking error: e1  vu sin(    p ); e2  ku   p (3.42) With in sequence lateral error and longitudinal error: e1  ye ; e2     P (3.43) The sliding surface is chosen:    s  e2  sgn  e1    arcsin 1  e1 sgn  e1  r   (3.44)   Control law is chosen:       e1   vu vu      u    p  1  1   sin d   p   sgn  s   (3.45)     r   r g           with:  d   p  sgn  e1    arcsin 1  e1 sgn  e1  r   (3.46) This results will be simulated The simulation results show that UAV trajectory is twisted line when AUV trackes moving target by loitering mode 3.3 Moving double streigh path tracking mode The using loitering mode for target tracking is very effective because the distance from UAV to target is kept unchanged But when target speed and UAV speed are not satisfied conditions (3.31), (3.32), the problem is to design suitable path so when UAV trackes to this path, the distance from UAV to target will be kept in allow interval of value, or tracking error is lied on allowed interval Then instead of flying in circus around target by loitering mode, the convoy mode is put forward In target convoyed process, UAV will fly behind the target and trackes same trajectory To design that trajectory, the double straight path tracking mode are put forward as bellow   19 3.3.1 Transition time in path following control based Dubin trajectory using sliding mode In this thesis, the double straight path is defined two parallel lines with changed distance by algorithm, which is defined so when UAV will follow sequentialtly each line by same predefined path following algorithm, the distance from UAV to target after each period is kept in desired value When the system in double straight path tracking mode, the path following algorithm is designed with Dubin trajectory algorithm For this algorithm, time and length of reaching trajectory of UAV from any point in sliding surface to path are absolutely defined This conclusion is a base to define the distance between two straight path When double straight path is designed, the UAV path following algorithm is based on Dubin trajectory, because when this algorithm is used, time and length of reaching trajectory of UAV from any point in sliding surface to path are absolutely defined This is the base to define distance between two path When the based on Dubin trajectory path following algorithm, the relationship between time and length of UAV reaching trajectory to path with lateral error may express by: v t f  arccos(1  ye ) / u (3.64); Lu  vu t f (3.65) r So by using the based on Dubin trajectory path following algorithm with sliding mode control, the time and length of UAV reaching trajectory from any point in sliding surface to path are absolutely definer by (3.64) and (3.65) 3.3.2 Double straight path in target convoying problem Let the distance between two path is 2q , r is smallest radius of UAV circle path,  is chord angle of Dubin trajectory from central line to path (1/4 period), one tracking period has chords, with chord angle  Then real trajectory of UAV in double path following mode is shown in Fig.3.7 as bellows: Figura 3.7 Schema of UAV trajectory in double path following mode 20 In Fig 3.7, when UAV fly from point A to point B, the projection of UAV trajectory to target trajectory is AB, let length of AB is LTu , and real UAV flying trajectory length is Lu and distance between two path l1 and l2 is 2q : Lu  4.r. (3.66) T Lu  4.r.sin  (3.67) 2q  2r.(1  cos ) (3.68) Let va - UAV speed, vt - target speed Then in time interval t UAV fly from point A to point B (one period), target traversing distance is: LT  4.r. vt va (3.69) The differential of distance between UAV and target after one period ia: L  LT  Lu (3.70) When the distance between UAV and target is more than desired,  must be smaller for UAV can race closer to target, that means L  , When the distance between UAV and target is equan to desired, that means L  ,  must be stay unchanging to keep the distance between UAV and target stayed unchanging When the distance between UAV and target is smaller than desired,  must be biger for UAV can race more far from target, that means L  Beginning chord angle is  , then beginning distance between two path is 2qo The approximately of  and qo are:    va  vt  / va (3.74) q0  r 1  cos    r.  3.r  vu  vt  va (3.75) However, in the two path l1 and l2 tracking the error may be exist because of random error and approximation of  o in calculation To overcome this error, the parameter q must be regulated To regulate q value, the adaptive algorithm is proposed behind Let at ti , UAV position is Ai , the parameters of tracking system are Ri , q i Let at ti 1 , UAV position is Ai 1 , the parameters of tracking system are Ri 1 , q i 1 , and R1 , Rd are distance real and desired distance from UAV to target 21 Figura 3.8 Illustrated schema of two path tracking At the time ti , Ri  Rd  Ri , tracking error is Ri , chord angle of ¼ period is  i , of distance from UAV to target is Li  4r  sin i  (vt / va )i  Because of approximation conculation is applied, Ri will have some error In comparition this error, chord angle of next period  i 1   i   i must be chosen so: Li 1  4.r  sin  i 1   vt va   i 1   Li  Ri (3.76) Put  i 1   i   i in (3.76) we have: 4r  sin 1  1    vt vu  1  1    4r  sin 1   vt vu  1   R1 (3.77) After some transformation with using approximate value sin  i   i we have: (3.79) 4r  cosi  vt va  i  Ri  i  Ri 4r  vt va  cos  i  (3.80) Applied Taylor expantion to cos  i , (4.62) is determined as:  Ri  4r  i   i /  (vt / va ) i  (3.81)  i 1   i   i   i  Ri / 4r  vt / va   /  1 Then: i (3.82) In purpose in next period, the distance from UAV to target will Rd , the next condition must be satisfied: L  R1 (3.83) From (3.76) and (3.85) we have: 4.r.(sin   vt  )  R1 va (3.84) Applied Taylor expantion to sin  , from (3.84) we have:     4r       vt va    R1  (3.85) The equation (3.85) may be solved by edge method However, for simplifying in conculation, the adaptive algorithm is propoused for  and q defining After approximation cos , the distance between two moving path is: 2.qi 1  r. i21 (3.86) The differential of distance between two path: 22 qi 1  r. i  i (3.87) And distance between two path is: qi 1  qi  qi 1 (3.88) Formulas (3.74), (3.75), (3.86), (3.87) and (3.88) all are algorithm for designing double moving path for UAV convoy mode based target tracking 3.3.3 Tracking error on UAV convoy mode based target tracking The UAV convoy mode based target tracking system will be kept the distance from UAV to target in the allowed interval and kept UAV behigh the target However, because of circulation of UAV trajectory by tracking to double path, the distance between UAV and target is not constant, but is changing value around some value (nominal value) The maximum difference interval around nominal value when UAV in convoy mode track the target with desired tracking distance relative to minimal radius of UAV circular path and speed of target relative to UAV speed aree shown on table bellow This table is built by simulation Table Rd / r 1,2 1,4 1,6 1,8 2,0 vt / vu 0,5 0,365 0,275 0,225 0,195 0,6 0,32 0,235 0,19 0,16 0,135 0,7 0,185 0,15 0,125 0,10 0,085 0,8 0,085 0,07 0,06 0,05 0,045 So, for the ground moving target, by using based on Dubin trajectory tracking algorithm, UAV can track the target by loitering mode or convoy mode with double path The choice of tracking mode depends on moving parameters of UAV and target, and desired tracking distance, In case when UAV is in autonomy mode, UAV in the reaching period will determine target speed and make dicision about the chosen tracking mode In case, when UAV is controlled by ground operator, the loitering mode will be chosen whrn UAV speed, target speed and tracking distance R suitable for loitering mode The convoy mode will be selected when allowed error is biger than maximun error shown in tab.1 with correlative parameters In the special case, when R  2r , the conditions for loitering mode are always satisfied In the case when target speed is biger than 70% UAV speed, the convoy mode allways makes the tracking error smaller than 20% desired distance This results are proved by simulation The simulation results show that when target speed smaller than 50% UAV speed, the double path convoy mode has big tracking error In this case, the loitering mode is a better choise At last, 23 for ground moving target tracking, the choise of loitering mode or convoy mode is depended on moving parameters of target and UAV as one as desired tracking distance Conclusion of chapter 3: In chapter 3, the main terms are path following system control synthesis and ground moving target tracking, focused on convoy mode The defining position and speed of groung target algorithm will supply the necessary data for tracking task and another real life mission The loitering mode in target tracking system will make the distance from UAV to target allways on stabilized value However, the loitering mode is tied down by target and UAV speed as one as desired distance from UAV to target For the overcome this limitation, the double path tracking control with adaptive algorithm in purpose to change distance between two path makes distance from UAV to target stayed stable on allowed deviation interval of distance error (Tab.1) This results may applied for UAV real mission The simulation about this algorithm is proved to be efficient of proposed algorithm Conclusion The UAV control problem with inner loop (stabilized loop) control algorithm integrated in autopilot equipment is relatively perfectly developed The UAV path following control is concentrated in maintainance the concrete mision of UAV To maintain this mission, UAV trajectory must be designed, and simultaneously, synthesis the path following control loop for UAV That is a necessary problem in plan programming for UAV to execute some mission, in which included, for example, ground moving target tracking For UAV ground moving target tracking mission, it is necessary to define position and speed of target The thesis put the task to solve this problem and has got concrete results as belows: Base on sliding mode UAV path following control algorithms designing explicitly for the case of linear sliding mode, mixing linear sliding mode, nonlinear sliding mode and with arctg function and base on Dubin trajectory nonlinear sliding mode, reaching time conculation for based on Dubin trajectory path following control when system state is on the sliding surface UAV sliding mode path following control algorithms designing for the circut lines with changed circuts radius basrd on Serret - Franet frame Adaptive algorithm for double paths ground moving target following, that allows target tracking by convoy mode when loiterimg mode is not suitable Simulation for proving thesis results 24 New contributions of thesis : Applying the sliding control method with different sliding surfaces proposed for each case to build a navigation algorithm for UAV based on Dubin trajectory and Serret - Franet frame Built an efficient algorithm to control the flight trajectory of the UAV to follow moving targets on the ground Future thesis research direction: Integration outer loop control algorithm with autopilot system for control UAV in maintainning real mission and at the same time, continuously development of the ground moving target tracking algorithm in convoy mode for different cases LIST OF DISCLOSED SCIENTIFIC WORKS Pham Thi Phuong Anh, Nguyen Vu, Phan Tương Lai “Path following algorithm for UAV”, Journal of Military Science and Technology, No 55,June 2018 Pham Thi Phuong Anh, Nguyen Vu, Phan Tương Lai, Nguyen Quang Vinh “Sliding mode based lateral control of Unmanned Aerial Vehicles”,XIIIth International Symposium «Intelligent Systems», INTELS’18, 22-24 October 2018, St Petersburg, Russia Pham Thi Phuong Anh, Nguyen Vu, Phan Tương Lai, “The sliding mode with non linear sliding in path following of uav”, Journal of Military Science and Technology, Special Issue , April 2019 Pham Thi Phuong Anh, Nguyen Vu, Phan Tương Lai, “Algorithm of path following for uav basing on the coordinate system of serret-frenet”, Journal of Military Science and Technology, Special Issue , April 2019 Pham Thi Phuong Anh, Nguyen Vu, “Algorithm follow control mobile target used uav”, Journal of Military Science and Technology, Special Issue , Septemper 2020 ... parameters and direction angle  P and UAV with position, the problem of UAV path following control is determained desired UAV direction angle  d so that with thís direction angle, UAV will move... general, let moving direction of target is  t  Then  p   : desired path direction at time UAV position is P;  : direction angle of UAV;  d : desired direction angle of UAV in Dunin trajectory... becomes perpendicular to path direction (When the distance from UAV to path biger than circle radius r) and follow the straight line until speed vector becomes tangent to circle radius r , which