Unmanned vehicle tracking control method and device based on unmanned aerial vehicle recovery

Abstract

The invention provides an unmanned vehicle tracking control method and device based on unmanned aerial vehicle recovery. The method comprises the following steps: predicting a plane coordinate of a projection point of the unmanned aerial vehicle on the ground at the next moment based on a motion track of the unmanned aerial vehicle to obtain a coordinate of a preview point; based on the coordinates of the unmanned vehicle at the current moment, the motion trail of the unmanned aerial vehicle and the coordinates of the preview point, calculating the target speed of the unmanned vehicle by adopting a fuzzy control algorithm, and calculating the steering wheel angle of the unmanned vehicle by adopting a Steiner algorithm; and controlling the unmanned vehicle according to an unmanned vehicle model based on the target speed and the steering wheel rotation angle. According to the invention, the speed of the unmanned vehicle is controlled by adopting the fuzzy control strategy based on the preview point prediction of the unmanned aerial vehicle, the steering of the unmanned vehicle is controlled based on the Steiner algorithm, and the high-precision real-time tracking of the unmanned aerial vehicle can be realized.

Description

Technical Field

[0001] The present invention belongs to the technical field of unmanned vehicle control, and in particular relates to an unmanned vehicle tracking control method and device based on unmanned aerial vehicle recovery. Background Art

[0002] As an autonomous, flexible and highly integrated ground unmanned platform with vehicle control systems and intelligent technologies, ground unmanned vehicles can maneuver according to the target location and can be used as a ground recovery platform for UAVs to complete the ground autonomous recovery mission of cooperative UAVs. For some scenarios where UAVs are easily disturbed when landing near the ground and cannot be adjusted quickly, using unmanned platforms as recovery platforms can achieve high-precision docking and recovery. At present, there are few studies on the dynamic autonomous positioning and planning docking of large UAVs with ground unmanned platforms as the research object in existing literature and public information at home and abroad.

[0003] Since the landing trajectory of large UAVs is easily disturbed by gusts of wind when landing near the ground, it is necessary to design an algorithm for an unmanned vehicle to dynamically track and recover the UAV. Whether the unmanned vehicle can quickly, steadily and accurately track the UAV is related to the success or failure of the UAV recovery control. To this end, the present invention proposes an unmanned vehicle tracking control method and device based on UAV recovery. Summary of the invention

[0004] In order to solve the above problems existing in the prior art, the present invention provides an unmanned vehicle tracking control method and device based on unmanned aerial vehicle recovery.

[0005] In order to achieve the above object, the present invention adopts the following technical solutions.

[0006] In a first aspect, the present invention provides an unmanned vehicle tracking control method based on unmanned aerial vehicle recovery, comprising the following steps:

[0007] Based on the motion trajectory of the UAV, the plane coordinates of the projection point of the UAV on the ground at the next moment are predicted to obtain the coordinates of the preview point;

[0008] Based on the current coordinates of the unmanned vehicle, the motion trajectory of the drone, and the coordinates of the preview point, the fuzzy control algorithm is used to calculate the target speed of the unmanned vehicle, and the Stanley algorithm is used to calculate the steering wheel angle of the unmanned vehicle;

[0009] Based on the target speed and the steering wheel angle, the unmanned vehicle is controlled according to the unmanned vehicle model.

[0010] Furthermore, the prediction method of the preview point coordinates includes: performing Kalman filtering based on the trajectory data of the drone to obtain the position coordinates (x ekf (n+1),y ekf (n+1),z ekf(n+1)), thus obtaining the coordinates of the preview point (x ekf (n+1),y ekf (n+1)); where n represents the current moment and n+1 represents the next moment.

[0011] Furthermore, the method of calculating the target speed of the unmanned vehicle using the fuzzy control algorithm includes:

[0012] Define a fuzzy set {NB, NS, ZO, PB, PS} consisting of five fuzzy quantities, where NB, NS, ZO, PB, and PS represent negative large, negative small, zero, positive small, and positive large, respectively; define five fuzzy level values ​​(-2, -1, 0, 1, 2), and the membership vectors of NB, NS, ZO, PB, and PS relative to the five fuzzy level values ​​are: NB L =(1,0.5,0,0,0), NS L =(0.5,1,0,0,0), ZO L =(0,0.5,1,0.5,0),PB L =(0,0,0,1,0.5), PS L =(0,0,0,0.5,1);

[0013] Determine the fuzzy control rule: If Δk = NB, then v c =NB; if Δk = NS, then v c =NS; if Δk = ZO, then v c =ZO; if Δk = PB, then v c =PB; if Δk = PS, then v c =PS, where Δk is the change in the preview slope, v c is the target speed of the unmanned vehicle;

[0014] According to the fuzzy control rules, v is calculated c The fuzzy relationship matrix with Δk is as follows:

[0015]

[0016] In the formula, R is the fuzzy relation matrix, T represents the matrix transpose, the element multiplication in the matrix operation is equivalent to finding the minimum value, and the element addition is equivalent to finding the maximum value;

[0017] Calculate the change in the preview slope at the current moment. The formula is as follows:

[0018]

[0019] Where Δk(n) is the change in the preview slope at the current moment, (x(n), y(n)) is the ground coordinate of the unmanned vehicle at the current moment, (x ekf (n),yekf (n))、(x ekf (n+1),y ekf (n+1)) are the coordinates of the drone projected on the ground at the current moment and the next moment respectively, (x ekf (n+1),y ekf (n+1)) is the preview point coordinate;

[0020] According to the size of Δk(n), the fuzzy value corresponding to Δk(n) and its corresponding membership vector Δk(n) are determined. L , calculate the current target speed v c The membership vector v of (n) c (n) L , the formula is as follows:

[0021] v c (n) L =Δk(n) L ×R (3)

[0022] Solving for v c (n) L The fuzzy level value corresponding to the largest membership component in is obtained to obtain the fuzzy level value of the target speed, and the fuzzy level value is converted into a speed value to obtain the target speed v c .

[0023] Furthermore, the method of determining the blur amount corresponding to Δk(n) according to the size of Δk(n) includes:

[0024] If Δk(n)≤ΔK1, the blur amount of Δk(n) is negative and large;

[0025] If ΔK1<Δk(n)≤ΔK2, the blur amount of Δk(n) is small and negative;

[0026] If ΔK2<Δk(n)≤ΔK3, the blur value of Δk(n) is zero;

[0027] If ΔK3<Δk(n)≤ΔK4, the blur amount of Δk(n) is positive and small;

[0028] If Δk(n)>ΔK4, the blur amount of Δk(n) is positive;

[0029] Among them, ΔK1, ΔK2, ΔK3, and ΔK4 are four preview slope thresholds, satisfying 0<ΔK1<ΔK2<ΔK3<ΔK4.

[0030] Furthermore, ΔK1=0.25, ΔK2=0.45, ΔK3=0.70, ΔK4=0.85.

[0031] Furthermore, the fuzzy level value of the target speed is converted into a speed value according to the following formula:

[0032] V=G×ΔV+V0 (4)

[0033] Wherein, V is the speed value, G is the fuzzy level value, G = -2, -1, 0, 1, 2, ΔV is the speed change corresponding to a fuzzy level change, ΔV is 1m / s, V0 is the speed value corresponding to the fuzzy level value 0, V0 is 11m / s.

[0034] Furthermore, the method for controlling the unmanned vehicle based on the target speed includes:

[0035] Get the current speed v(n) of the unmanned vehicle;

[0036] Calculate v(n) and target speed v c The deviation e c (n) = v c -v(n);

[0037] E c (n) Input the PID controller, and after amplification, integration and differentiation, control the speed of the unmanned vehicle to make the deviation e c (n) Reduce the direction change until the deviation is 0, so that the speed of the unmanned vehicle is v c ; The adjustment process of the unmanned vehicle speed v(n) is expressed as:

[0038] v(n+1)=v(n)+k p (e c (n)-e c (n-1))+k i e c (n)+k d (e c (n)-2e c (n-1)+e c (n-2)) (5)

[0039] In the formula, k p , k i , k d They are the gain coefficients of the proportional term, integral term and differential term of the PID controller respectively.

[0040] Furthermore, the method of calculating the steering wheel angle of the unmanned vehicle using the Stanley algorithm includes:

[0041] S1. Calculate the steering wheel angle without considering the lateral tracking error

[0042] When the lateral tracking error is not considered, the steering wheel angle is equal to the angle between the tangent direction of the UAV's expected trajectory at the preview point and the UAV's heading. Right now

[0043] S2. Calculate the steering wheel angle δ without considering the heading tracking error y ;

[0044] Without considering the heading tracking error, the larger the lateral tracking error, the larger the steering wheel angle. The steering wheel angle is calculated as follows:

[0045]

[0046] In the formula, e y (t) is the lateral tracking error, d(t) is the distance between the center of the left and right front wheels and the intersection of the tangent of its expected trajectory and the expected trajectory of the UAV at the preview point, d(t) = λv(t), is the speed of the UAV, and λ is the gain parameter;

[0047] S3. Calculate the steering wheel angle using the following formula:

[0048]

[0049] Where δ is the steering wheel angle.

[0050] Furthermore, the unmanned vehicle model includes an unmanned vehicle internal model and an unmanned vehicle external model. The unmanned vehicle internal model includes a body module, a slip rate module, a tire module and a wheel rotation module, and the unmanned vehicle external model includes a driver module, a motor module, a steering module, a braking module and a vertical movement module.

[0051] In a second aspect, the present invention provides an unmanned vehicle tracking control device based on unmanned aerial vehicle recovery, comprising:

[0052] The prediction module is used to predict the plane coordinates of the projection point of the drone on the ground at the next moment based on the motion trajectory of the drone, and obtain the coordinates of the preview point;

[0053] A calculation module is used to calculate the target speed of the unmanned vehicle using a fuzzy control algorithm based on the coordinates of the unmanned vehicle at the current moment, the motion trajectory of the drone, and the coordinates of the preview point, and to calculate the steering wheel angle of the unmanned vehicle using a Stanley algorithm;

[0054] The control module is used to control the unmanned vehicle according to the unmanned vehicle model based on the target speed and the steering wheel angle.

[0055] Compared with the prior art, the present invention has the following beneficial effects.

[0056] The present invention predicts the plane coordinates of the projection point of the drone on the ground at the next moment based on the motion trajectory of the drone, obtains the coordinates of the preview point, uses a fuzzy control algorithm to calculate the target speed of the unmanned vehicle based on the coordinates of the unmanned vehicle at the current moment, the motion trajectory of the drone and the coordinates of the preview point, and uses the Stanley algorithm to calculate the steering wheel angle of the unmanned vehicle. Based on the target speed and the steering wheel angle, the unmanned vehicle is controlled according to the unmanned vehicle model, thereby realizing unmanned vehicle tracking control based on drone recovery. The present invention controls the speed of the unmanned vehicle based on the prediction of the drone preview point, uses a fuzzy control strategy, and controls the steering of the unmanned vehicle based on the Stanley algorithm, thereby realizing high-precision real-time tracking of the drone. BRIEF DESCRIPTION OF THE DRAWINGS

[0057] Figure 1 The present invention is a flowchart of an unmanned vehicle tracking control method based on unmanned aerial vehicle recovery according to an embodiment of the present invention.

[0058] Figure 2 This is the tracking control block diagram of the unmanned vehicle based on vehicle speed fuzzy control and Stanley algorithm.

[0059] Figure 3 Schematic diagram for calculating the steering wheel angle δ.

[0060] FIG4 is a model diagram of an unmanned vehicle, FIG4(a) is an external model diagram of the vehicle, and FIG4(b) is an internal model diagram of the unmanned vehicle.

[0061] Figure 5 The present invention is a block diagram of an unmanned vehicle tracking control device based on unmanned aerial vehicle recovery according to an embodiment of the present invention. DETAILED DESCRIPTION

[0062] In order to make the purpose, technical solution and advantages of the present invention clearer and more understandable, the present invention is further described below in conjunction with the accompanying drawings and specific implementation methods. Obviously, the described embodiments are only part of the embodiments of the present invention, not all of the embodiments. Based on the embodiments of the present invention, all other embodiments obtained by ordinary technicians in this field without making creative work are within the scope of protection of the present invention.

[0063] Figure 1 This is a flow chart of an unmanned vehicle tracking control method for recovering a drone according to an embodiment of the present invention, comprising the following steps:

[0064] Step 101, based on the motion trajectory of the drone, predict the plane coordinates of the projection point of the drone on the ground at the next moment, and obtain the coordinates of the preview point;

[0065] Step 102, based on the coordinates of the unmanned vehicle at the current moment, the motion trajectory of the drone and the coordinates of the preview point, a fuzzy control algorithm is used to calculate the target speed of the unmanned vehicle, and a Stanley algorithm is used to calculate the steering wheel angle of the unmanned vehicle;

[0066] Step 103: Based on the target speed and the steering wheel angle, the unmanned vehicle is controlled according to the unmanned vehicle model.

[0067] In this embodiment, step 101 is mainly used to predict the coordinates of the preview point. This embodiment is applied to the second stage of the ground unmanned vehicle's recovery control of the drone when the drone returns (drone recovery control includes 4 stages, see the background technology section), that is, the stage where the unmanned vehicle tracks the drone. During the tracking process, the ground unmanned vehicle always drives towards the projection point of the drone on the ground. Since the position of the drone is constantly changing during the flight, in order to improve the tracking speed and accuracy, the unmanned vehicle should be aimed at the projection point of the drone on the ground at the next moment, that is, the preview point. Therefore, it is necessary to predict the position of the drone at the next moment based on the motion trajectory of the drone before the current moment, and project it to the ground. There are many methods to predict the position at a future moment based on the motion trajectory, and this embodiment does not limit the specific prediction method.

[0068] In this embodiment, step 102 is mainly used to calculate the target speed and steering wheel angle of the unmanned vehicle. The control of the unmanned vehicle is mainly to control the vehicle speed. Speed ​​is a vector, including size and direction, and the driving direction of the unmanned vehicle is controlled by the steering wheel. Therefore, to control the unmanned vehicle, it is necessary to obtain the target speed and steering wheel angle of the unmanned vehicle. Of course, it can also be understood in this way: the speed of the unmanned vehicle is decomposed into longitudinal speed (body direction) and lateral speed (vertical body direction), that is, the vehicle speed is regarded as the vector sum of longitudinal speed and lateral speed, and the size of lateral speed is closely related to the change of the driving direction of the unmanned vehicle. Therefore, to control the unmanned vehicle, it is necessary to obtain the target speed and steering wheel angle of the unmanned vehicle. Obviously, the target speed and steering wheel angle of the unmanned vehicle are closely related to the current position of the unmanned vehicle and the coordinates of the preview point. For this reason, this embodiment uses a fuzzy control algorithm to calculate the target speed of the unmanned vehicle based on the coordinates of the unmanned vehicle at the current moment and the coordinates of the preview point, and uses the Stanley algorithm to calculate the steering wheel angle of the unmanned vehicle. The following embodiments will respectively provide a technical solution for calculating the target speed and steering wheel angle of the unmanned vehicle.

[0069] In this embodiment, step 103 is mainly used to control the unmanned vehicle based on the target speed and the steering wheel angle. In this embodiment, the unmanned vehicle model (kinematic and dynamic models) is established, and then the unmanned vehicle related parameters are calculated based on the target speed and the steering wheel angle to achieve control of the unmanned vehicle. The unmanned vehicle model is composed of many modules, and the following embodiments will provide a specific modeling method.

[0070] As an optional embodiment, the method for predicting the coordinates of the preview point includes: performing Kalman filtering based on the trajectory data of the drone to obtain the position coordinates (x ekf (n+1),y ekf (n+1),z ekf (n+1)), thus obtaining the coordinates of the preview point (x ekf (n+1),y ekf (n+1)); where n represents the current moment and n+1 represents the next moment.

[0071] This embodiment provides a technical solution for predicting the coordinates of the preview point. As mentioned above, the preview point is the projection point on the ground of the position of the drone at the next moment. Therefore, the key to predicting the coordinates of the preview point is to predict the position coordinates of the drone at the next moment. This embodiment predicts the position coordinates of the drone at the next moment by performing Kalman filtering based on the trajectory data of the drone. The trajectory data of the drone is composed of the position coordinate data before the current moment, and the position coordinates are obtained by transforming the coordinate data output by the GPS positioning module. Kalman filtering is a prediction and filtering technology used in the case of noise in the system. The core of the standard Kalman filtering algorithm consists of 5 formulas, which can be understood as prediction steps and state update steps. Prediction is to estimate the prior estimate of the state vector at the next moment and the covariance of the error of the estimated prior estimate based on the posterior estimate of the state vector; state update is to correct and update the prior estimate using the observed value to obtain the posterior estimate at the current moment. Kalman filtering has the following advantages: it can effectively predict and filter linear systems (extended Kalman filtering can be used for nonlinear systems); the algorithm has low complexity and is easy to implement and apply; it can achieve optimal state estimation under linear and nonlinear dynamics; Kalman filtering is still effective for multidimensional systems and measurements.

[0072] As an optional embodiment, the method of calculating the target speed of the unmanned vehicle using the fuzzy control algorithm includes:

[0073] Define a fuzzy set {NB, NS, ZO, PB, PS} consisting of five fuzzy quantities, where NB, NS, ZO, PB, and PS represent negative large, negative small, zero, positive small, and positive large, respectively; define five fuzzy level values ​​(-2, -1, 0, 1, 2), and the membership vectors of NB, NS, ZO, PB, and PS relative to the five fuzzy level values ​​are: NB L =(1,0.5,0,0,0), NS L =(0.5,1,0,0,0), ZO L =(0,0.5,1,0.5,0),PB L =(0,0,0,1,0.5), PSL =(0,0,0,0.5,1);

[0074] Determine the fuzzy control rule: If Δk = NB, then v c =NB; if Δk = NS, then v c =NS; if Δk = ZO, then v c =ZO; if Δk = PB, then v c =PB; if Δk = PS, then v c =PS, where Δk is the change in the preview slope, v c is the target speed of the unmanned vehicle;

[0075] According to the fuzzy control rules, v is calculated c The fuzzy relationship matrix with Δk is as follows:

[0076]

[0077] In the formula, R is the fuzzy relation matrix, T represents the matrix transpose, the element multiplication in the matrix operation is equivalent to finding the minimum value, and the element addition is equivalent to finding the maximum value;

[0078] Calculate the change in the preview slope at the current moment. The formula is as follows:

[0079]

[0080] Where Δk(n) is the change in the preview slope at the current moment, (x(n), y(n)) is the ground coordinate of the unmanned vehicle at the current moment, (x ekf (n),y ekf (n))、(x ekf (n+1),y ekf (n+1)) are the coordinates of the drone projected on the ground at the current moment and the next moment respectively, (x ekf (n+1),y ekf (n+1)) is the preview point coordinate;

[0081] According to the size of Δk(n), the fuzzy value corresponding to Δk(n) and its corresponding membership vector Δk(n) are determined. L , calculate the current target speed v c The membership vector v of (n) c (n) L , the formula is as follows:

[0082] v c (n) L =Δk(n) L ×R (3)

[0083] Solving for v c (n)L The fuzzy level value corresponding to the largest membership component in is obtained to obtain the fuzzy level value of the target speed, and the fuzzy level value is converted into a speed value to obtain the target speed v c .

[0084] This embodiment provides a technical solution for calculating the target speed of an unmanned vehicle using a fuzzy control algorithm.

[0085] First, define a fuzzy set {NB, NS, ZO, PB, PS} consisting of five fuzzy quantities: negative large NB, negative small NS, zero ZO, positive small PB, and positive large PS. Then define a fuzzy level value consisting of five consecutive integer values ​​from -2 to 2, and a vector consisting of the membership of the five fuzzy quantities relative to the five fuzzy level values, namely the membership vector. For example, the membership vector of negative large is NB L =(1,0.5,0,0,0), which means that the probabilities of negative fuzzy level values ​​of -2, -1, 0, 1, and 2 are 1, 0.5, 0, 0, and 0 respectively.

[0086] Next, the fuzzy control rule is determined. The fuzzy control rule determined in this embodiment is: the larger the change in the preview slope Δk, the higher the target speed v of the unmanned vehicle. c Specifically, if Δk = NB, then v c =NB; if Δk = NS, then v c =NS; if Δk = ZO, then v c =ZO; if Δk = PB, then v c =PB; if Δk = PS, then v c =PS. The technical principle of the fuzzy control rule is introduced below. The preview slope is equal to the slope of the line connecting the unmanned vehicle at the current moment and the projection of the unmanned vehicle on the ground at the next moment, that is, the preview point. When the unmanned vehicle is in a stable flight state, the trajectory of the unmanned vehicle is approximately a straight line, and the preview slope is basically unchanged, that is, the change in the preview slope is very small. If the speed of the unmanned vehicle is too large at this time, it will cause the lateral error to converge quickly, the steering angle amplitude will be too large, and the stability of the unmanned vehicle will be reduced. Therefore, the closer the unmanned vehicle is to a stable state, that is, when the change in the preview slope is smaller, the speed of the unmanned vehicle should also be smaller. When the flight state of the unmanned vehicle fluctuates greatly, the trajectory of the unmanned vehicle is no longer a straight line, but will produce larger fluctuations, and the preview slope will have a larger change, that is, the change in the preview slope is larger. If the speed of the unmanned vehicle is small at this time, a larger tracking error will be generated. Therefore, the more severe the fluctuation of the unmanned vehicle, the greater the change in the preview slope, and the greater the speed of the unmanned vehicle should be. According to the determined fuzzy control rules, the target speed v of the unmanned vehicle can be calculated c The fuzzy relationship matrix R of the preview slope change Δk is specifically shown in formula (1).

[0087] Then, the change of the preview slope Δk(n) at the current moment is calculated, and the calculation formula is shown in formula (2). Since the larger the preview slope is, the easier it is to produce a larger change, formula (2) normalizes the change of the preview slope, as shown in the denominator of formula (2).

[0088] With Δk(n), we can determine the fuzzy value corresponding to Δk(n) and its corresponding membership vector Δk(n) according to the size of Δk(n). L Then based on Δk(n) L And the fuzzy relationship matrix R calculates the current target speed v c The membership vector v of (n) c (n) L , as shown in formula (3). Finally, solve v c (n) L The fuzzy level value corresponding to the largest membership component in is obtained to obtain the fuzzy level value of the target speed, and the fuzzy level value is converted into a speed value to obtain the target speed v c .

[0089] For example, when Δk(n) L =(1,0.5,0,0,0), the controller outputs v c (n) L for:

[0090]

[0091] According to the maximum membership principle, the control quantity v should be selected at this time c (n) is the speed value represented by the -2 level.

[0092] As an optional embodiment, a method for determining the blur amount corresponding to Δk(n) according to the size of Δk(n) includes:

[0093] If Δk(n)≤ΔK1, the blur amount of Δk(n) is negative and large;

[0094] If ΔK1<Δk(n)≤ΔK2, the blur amount of Δk(n) is small and negative;

[0095] If ΔK2<Δk(n)≤ΔK3, the blur value of Δk(n) is zero;

[0096] If ΔK3<Δk(n)≤ΔK4, the blur amount of Δk(n) is positive and small;

[0097] If Δk(n)>ΔK4, the blur amount of Δk(n) is positive;

[0098] Among them, ΔK1, ΔK2, ΔK3, and ΔK4 are four preview slope thresholds, satisfying 0<ΔK1<ΔK2<ΔK3<ΔK4.

[0099] This embodiment provides a technical solution for determining the corresponding blur amount according to the size of the preview slope. This embodiment divides the preview slope value range 0 to ∞ into 5 intervals by setting 4 preview slope thresholds ΔK1, ΔK2, ΔK3, and ΔK4 from small to large, and each interval corresponds to a blur amount. The corresponding blur amount is determined by judging which interval the preview slope belongs to. For example, if ΔK3<Δk(n)≤ΔK4, the blur amount of the preview slope is small.

[0100] As an optional embodiment, ΔK1=0.25, ΔK2=0.45, ΔK3=0.70, and ΔK4=0.85.

[0101] This embodiment provides a set of specific values ​​of the preview slope threshold. In this embodiment, ΔK1, ΔK2, ΔK3, and ΔK4 are 0.25, 0.45, 0.70, and 0.85, respectively. It is worth noting that this embodiment only provides a preferred implementation method and does not deny or exclude other feasible implementation methods, such as other values ​​different from the above specific values.

[0102] As an optional embodiment, the fuzzy level value of the target speed is converted into a speed value according to the following formula:

[0103] V=G×ΔV+V0 (4)

[0104] Wherein, V is the speed value, G is the fuzzy level value, G = -2, -1, 0, 1, 2, ΔV is the speed change corresponding to a fuzzy level change, ΔV is 1m / s, V0 is the speed value corresponding to the fuzzy level value 0, V0 is 11m / s.

[0105] This embodiment provides a technical solution for determining the corresponding speed value according to the fuzzy level value of the target speed. This embodiment regards the speed value as a first-order linear function of the fuzzy level, and is positively correlated (the slope is positive), that is, the larger the fuzzy level value, the higher the speed. The specific function expression is shown in formula (4), V0 is the speed value corresponding to the fuzzy level value of 0, which is the constant term of the first-order linear function, ΔV is the speed change corresponding to a fuzzy level change, which is the linear term coefficient, i.e., the slope. When V0=11m / s, ΔV=1m / s, the speed values ​​corresponding to the fuzzy level values ​​of -2, -1, 0, 1, and 2 are 9, 10, 11, 12, and 13m / s, respectively.

[0106] As an optional embodiment, the method for controlling the unmanned vehicle based on the target speed includes:

[0107] Get the current speed v(n) of the unmanned vehicle;

[0108] Calculate v(n) and target speed v cThe deviation e c (n) = v c -v(n);

[0109] E c (n) Input the PID controller, and after amplification, integration and differentiation, control the speed of the unmanned vehicle to make the deviation e c (n) Reduce the direction change until the deviation is 0, so that the speed of the unmanned vehicle is v c ; The adjustment process of the unmanned vehicle speed v(n) is expressed as:

[0110] v(n+1)=v(n)+k p (e c (n)-e c (n-1))+k i e c (n)+k d (e c (n)-2e c (n-1)+e c (n-2)) (5)

[0111] In the formula, k p , k i , k d They are the gain coefficients of the proportional term, integral term and differential term of the PID controller respectively.

[0112] This embodiment provides a technical solution for controlling an unmanned vehicle based on a target speed. This embodiment uses a PID controller to adjust the motor speed so that the speed of the unmanned vehicle tracks the target speed. The PID controller is a proportional integral differential controller (Proportion Integration Differentiation), which consists of a proportional unit (P), an integral unit (I) and a differential unit (D). Among them, the proportion represents the current information, plays a role in correcting deviations, and makes the process respond quickly; the integral represents the information accumulated in the past, it can eliminate static errors and improve the static characteristics of the system; the differential has a leading control effect when the signal changes, represents future information, forces the process to proceed at the beginning of the process, reduces overshoot at the end of the process, overcomes oscillation, improves the stability of the system, and speeds up the transition process of the system. These three functions are properly coordinated to make the dynamic process fast, smooth, and accurate, and achieve good results. Formula (5) gives the discrete expression of the PID adjustment process.

[0113] As an optional embodiment, the method of calculating the steering wheel angle of the unmanned vehicle using the Stanley algorithm includes:

[0114] S1. Calculate the steering wheel angle without considering the lateral tracking error

[0115] When the lateral tracking error is not considered, the steering wheel angle is equal to the angle between the tangent direction of the UAV's expected trajectory at the preview point and the UAV's heading. Right now

[0116] S2. Calculate the steering wheel angle δ without considering the heading tracking error y ;

[0117] Without considering the heading tracking error, the larger the lateral tracking error, the larger the steering wheel angle. The steering wheel angle is calculated as follows:

[0118]

[0119] In the formula, e y (t) is the lateral tracking error, d(t) is the distance between the center of the left and right front wheels and the intersection of the tangent of its expected trajectory and the expected trajectory of the UAV at the preview point, d(t) = λv(t), is the speed of the UAV, and λ is the gain parameter;

[0120] S3. Calculate the steering wheel angle using the following formula:

[0121]

[0122] Where δ is the steering wheel angle.

[0123] This embodiment provides a technical solution for calculating the steering wheel angle of an unmanned vehicle using the Stanley algorithm. The Stanley algorithm is also known as the front wheel feedback control algorithm. Its core idea is to calculate the steering wheel steering control amount based on the path tracking deviation of the front wheel center. The Stanley algorithm is a nonlinear feedback function based on the lateral tracking error as the distance from the center of the front axle to the nearest path point, and directly obtains the control variable for controlling the steering wheel angle of the vehicle according to the relative geometric relationship between the vehicle posture and the given path point. It can achieve the lateral tracking error convergence to 0 and complete the unmanned vehicle's following control of the heading of the drone. This embodiment first calculates the steering wheel angle without considering the lateral tracking error. Then calculate the steering wheel angle δ without considering the heading tracking error y , and finally through δ y The final steering wheel angle δ is obtained by summing up, as Figure 3 See formula (6) and (7) for details.

[0124] As an optional embodiment, the unmanned vehicle model includes an unmanned vehicle internal model and an unmanned vehicle external model. The unmanned vehicle internal model includes a body module, a slip rate module, a tire module and a wheel rotation module. The unmanned vehicle external model includes a driver module, a motor module, a steering module, a braking module and a vertical movement module.

[0125] This embodiment provides the composition of the unmanned vehicle model. The unmanned vehicle model includes an unmanned vehicle internal model and an unmanned vehicle external model. The unmanned vehicle internal model is a model established for the components of the unmanned vehicle, including a body module, a slip rate module, a tire module, and a wheel rotation module. The unmanned vehicle external model is a model established for external devices associated with the unmanned vehicle, including a driver module, a motor module, a steering module, a brake module, and a vertical movement module.

[0126] The unmanned vehicle of this embodiment adopts a front-wheel drive and rear-wheel drive four-wheel Ackerman model. For the convenience of description, subscripts are used to distinguish the four wheels. Specifically, subscript variables i and j are used to represent the front and rear wheels and the left and right wheels, respectively. i=f, j=l represents the left front wheel, i=f, j=r represents the right front wheel, i=b, j=l represents the left rear wheel, and i=b, j=r represents the right rear wheel.

[0127] The unmanned vehicle kinematic model only considers the vehicle kinematic equations, and does not consider the parts involving forces and moments. Based on the above assumptions, the vehicle kinematic model equations are as follows:

[0128]

[0129] In the formula, (x, y) is the coordinate of the center of the rear axle of the unmanned vehicle, θ and δ are the heading angle and front wheel steering angle of the unmanned vehicle respectively, and v is the speed of the unmanned vehicle (the speed of the rear axle center). Simplified assumptions are made, and the center point of the rear axle of the unmanned vehicle is used as the rotation reference point. The average of the left front wheel steering angle and the right front wheel steering angle of the unmanned vehicle is taken to obtain the average front wheel steering angle δ. From the geometric relationship, it can be obtained that: tanδ=L / R, L is the wheelbase of the unmanned vehicle, and R is the turning radius corresponding to the front wheel steering angle of the unmanned vehicle is δ.

[0130] The models of each module are introduced below.

[0131] Car body model:

[0132] A dynamic model with fourteen degrees of freedom of motion is established in the coordinate system of the unmanned vehicle, specifically the velocity v of the vehicle's center of mass moving longitudinally, transversely, and vertically along the x-axis, y-axis, and z-axis. x 、v y 、v z and acceleration a x 、a y 、a z Three degrees of freedom; roll angle of the vehicle body around the x-axis, y-axis and z-axis and angular velocity ω x , pitch angle θ v and angular velocity ω y , yaw angle Ψ v and angular velocity ω z Three degrees of freedom; the angular velocity of the four wheels of the unmanned vehicle around the center axis of the wheel ω ijand vertical motion z ij Eight degrees of freedom. The dynamic model is expressed as:

[0133]

[0134] Where M x 、M y 、M z are the external moments of the unmanned vehicle around the x, y and z axes respectively; I x ,I y ,I z are the moments of inertia of the unmanned vehicle around the x, y, and z axes, respectively.

[0135] The longitudinal differential equation of the unmanned vehicle is:

[0136]

[0137] Where m is the mass of the unmanned vehicle, F R is the sum of the resistance of the unmanned vehicle, f r is the rolling resistance coefficient, C D is the air resistance coefficient, ρ a is the air density, α g is the climbing coefficient (the maximum slope that can be overcome in first gear on a good road when the vehicle is fully loaded), A f is the frontal area of ​​the unmanned vehicle, F xij 、F yij 、F zij (i=f,b;j=l,r) is the ground longitudinal elastic force, ground lateral elastic force and suspension vertical force on the tire of the unmanned vehicle, δ l ,δ r are the turning angles of the left and right front wheels respectively.

[0138] The differential equation of lateral motion of the unmanned vehicle is:

[0139]

[0140] The differential equation of the yaw motion of the unmanned vehicle is:

[0141]

[0142] In the formula, B i (i=f,b) is the distance between the front left and right wheels and the distance between the rear left and right wheels, L fb is the distance between the center of the front axle and the center of the rear axle, M wij (i=f,b; j=l,r) is the tire self-aligning torque.

[0143] The body of the unmanned vehicle has two main motions: roll motion around its roll axis; vertical motion and pitch motion. Therefore, the vertical motion equation of the unmanned vehicle is:

[0144]

[0145] The differential equation of vehicle body roll motion is:

[0146]

[0147] M φ =m b HkDJ e sinφ v +m b a y H e cosφ v

[0148] In the formula, m b is the body mass of the unmanned vehicle, M φ When the roll angle of the unmanned vehicle is φ v , lateral acceleration a y The rolling moment generated when e It is the height from the center of mass of the unmanned vehicle to the roll axis.

[0149] The differential equation of vehicle body pitch motion is:

[0150]

[0151] M θ =m b HkDJ p sinθ v -m b a x H p cosθ v

[0152] Among them, M θ When the pitch angle of the unmanned vehicle is θ v , the longitudinal acceleration is a x The pitching moment generated when p is the height from the center of mass of the unmanned vehicle to the pitch center.

[0153] Steering module:

[0154] The Ackerman steering angles of the left and right front wheels of the unmanned vehicle are:

[0155]

[0156] In the formula, δ s =δ sw / i sw , δ sw is the steering wheel angle, i swis the transmission ratio of the rotation system. In the unmanned vehicle suspension model, the vehicle is symmetrical about the xoz plane and can be regarded as a two-wheel plane model. Then the unmanned vehicle suspension force F zsij and tire load F zwij It can be expressed as:

[0157]

[0158] In the formula, m w is the wheel mass.

[0159] Vertical motion module:

[0160] Each wheel has two degrees of freedom: vertical motion and rotational motion around the central axis. Assuming that the tire does not leave the ground, the corresponding vertical motion will occur due to elasticity, which can be expressed as:

[0161]

[0162] Wheel rotation module:

[0163] Ignoring the lateral force of the ground, the rotation of the tire is mainly affected by the force P of the unmanned vehicle. zij and P xij , ground longitudinal force F xwij and vertical force F zwij , Braking torque T bi and driving torque T di The distance between the tire’s previous contact point and the ground vertical force action point is Δ ij The wheel center rolling resistance moment T fij for:

[0164] T fij =F zwij Δ ij =F zwij ·f r R wj

[0165] In the formula, R wj is the tire rolling radius. Therefore, the tire rolling equation of the unmanned vehicle is:

[0166]

[0167] In the formula, I wij is the moment of inertia of the tire of the unmanned vehicle around the wheel axis, ω wij is the tire rotation angular velocity, T mij Output torque for the unmanned vehicle motor.

[0168] Tire Model:

[0169] Professor Pacejka and others from the Netherlands proposed a common tire model represented by the magic tire formula, which can express the relationship between the lateral force, longitudinal force and self-aligning torque of the tire. The expression is:

[0170] F v (x ij )=Dsin{Carctan[Bx ij -E(Bx-arctan(Bx ij ))]}

[0171] In the formula, F v is the tire lateral force or longitudinal force, x ij is the tire slip angle α ij Or slip ratio λ ij ; B is the stiffness parameter, C is the shape parameter, D is the peak parameter, and E is the curvature parameter. The four parameters are obtained by fitting the tire model.

[0172] Slip Rate Module:

[0173] In the tire model, the tire slip ratio λ is required ij and the slip angle α ij As the model input, the vehicle slip formula is:

[0174]

[0175] Braking module:

[0176] The speed of the wheel center along the longitudinal axis of the wheel coordinate system is:

[0177] v wxij =v xij cosδ j +v yij sinδ j

[0178] The wheel slip angle is:

[0179]

[0180] From the above formula, we can get the wheel center longitudinal velocity v xij , lateral speed v yij Combined speed v ij for:

[0181]

[0182] Motor module:

[0183] The output torque of the unmanned vehicle motor is:

[0184] T mij =T di-T bi

[0185] The vehicle model is driven by a wheel hub motor. In engineering, a three-dimensional motor lookup model is generally used to determine the motor output. Here, the motor characteristic curve is used to determine the motor output:

[0186] T mij =β ij T max (n ij )

[0187] In the formula, β ij is the torque coefficient of the motor, -1≤β ij ≤1,n ij is the motor input speed, T max (n ij ) is the corresponding motor input speed n ij The torque value of the external characteristic curve.

[0188] Driver Module:

[0189] The driver module model is the steering wheel angle obtained by Stanley algorithm. The steering wheel angle is as shown in equations (5) and (6).

[0190] Figure 5 The following is a schematic diagram of the composition of an unmanned vehicle tracking control device based on drone recovery according to an embodiment of the present invention, wherein the device comprises:

[0191] The prediction module 11 is used to predict the plane coordinates of the projection point of the drone on the ground at the next moment based on the motion trajectory of the drone, and obtain the coordinates of the preview point;

[0192] The calculation module 12 is used to calculate the target speed of the unmanned vehicle using a fuzzy control algorithm based on the coordinates of the unmanned vehicle at the current moment, the motion trajectory of the drone and the coordinates of the preview point, and to calculate the steering wheel angle of the unmanned vehicle using a Stanley algorithm;

[0193] The control module 13 is used to control the unmanned vehicle according to the unmanned vehicle model based on the target speed and the steering wheel angle.

[0194] The device of this embodiment can be used to perform Figure 1 The technical solution of the method embodiment shown has similar implementation principles and technical effects, which will not be repeated here.

[0195] The above is only a specific embodiment of the present invention, but the protection scope of the present invention is not limited thereto. Any changes or substitutions that can be easily thought of by a person skilled in the art within the technical scope disclosed by the present invention should be included in the protection scope of the present invention. Therefore, the protection scope of the present invention shall be subject to the protection scope of the claims.

Claims

1. A method for tracking and controlling an unmanned vehicle based on UAV recovery, characterized in that: The following steps are involved: Based on the motion trajectory of the UAV, the plane coordinates of the projection point of the UAV on the ground at the next moment are predicted to obtain the coordinates of the preview point; Based on the current coordinates of the unmanned vehicle, the motion trajectory of the drone, and the coordinates of the preview point, the fuzzy control algorithm is used to calculate the target speed of the unmanned vehicle, and the Stanley algorithm is used to calculate the steering wheel angle of the unmanned vehicle; Based on the target speed and the steering wheel angle, the unmanned vehicle is controlled according to the unmanned vehicle model.

2. The unmanned vehicle tracking control method based on unmanned aerial vehicle recovery according to claim 1 is characterized in that: The prediction method of the preview point coordinates includes: performing Kalman filtering based on the trajectory data of the UAV to obtain the position coordinates (x ekf (n+1),y ekf (n+1),z ekf (n+1)), thus obtaining the coordinates of the preview point (x ekf (n+1),y ekf (n+1)); where n represents the current moment and n+1 represents the next moment.

3. The unmanned vehicle tracking control method based on unmanned aerial vehicle recovery according to claim 1 is characterized in that: The method of calculating the target speed of the unmanned vehicle using the fuzzy control algorithm includes: Define a fuzzy set {NB, NS, ZO, PB, PS} consisting of five fuzzy quantities, where NB, NS, ZO, PB, and PS represent negative large, negative small, zero, positive small, and positive large, respectively; define five fuzzy level values ​​(-2, -1, 0, 1, 2), and the membership vectors of NB, NS, ZO, PB, and PS relative to the five fuzzy level values ​​are: NB L =(1,0.5,0,0,0), NS L =(0.5,1,0,0,0), ZO L =(0,0.5,1,0.5,0),PB L =(0,0,0,1,0.5), PS L =(0,0,0,0.5,1); Determine the fuzzy control rule: If Δk = NB, then v c =NB; if Δk = NS, then v c =NS; if Δk = ZO, then v c =ZO; if Δk = PB, then v c =PB; if Δk = PS, then v c =PS, where Δk is the change in the preview slope, v c is the target speed of the unmanned vehicle; According to the fuzzy control rule, v is calculated c The fuzzy relationship matrix with Δk is as follows: In the formula, R is the fuzzy relation matrix, T represents the matrix transpose, the element multiplication in the matrix operation is equivalent to finding the minimum value, and the element addition is equivalent to finding the maximum value; Calculate the change in the preview slope at the current moment. The formula is as follows: Where Δk(n) is the change in the preview slope at the current moment, (x(n), y(n)) is the ground coordinate of the unmanned vehicle at the current moment, (x ekf (n),y ekf (n))、(x ekf (n+1),y ekf (n+1)) are the coordinates of the drone projected on the ground at the current moment and the next moment respectively, (x ekf (n+1),y ekf (n+1)) is the preview point coordinate; According to the size of Δk(n), the fuzzy value corresponding to Δk(n) and its corresponding membership vector Δk(n) are determined. L , calculate the current target speed v c The membership vector v of (n) c (n) L , the formula is as follows: v c (n) L =Δk(n) L ×R (3) Solving for v c (n) L The fuzzy level value corresponding to the largest membership component in is obtained to obtain the fuzzy level value of the target speed, and the fuzzy level value is converted into a speed value to obtain the target speed v c .

4. The unmanned vehicle tracking control method based on unmanned aerial vehicle recovery according to claim 3 is characterized in that: The method of determining the blur amount corresponding to Δk(n) according to the size of Δk(n) includes: If Δk(n)≤ΔK1, the blur amount of Δk(n) is negative and large; If ΔK1<Δk(n)≤ΔK2, the blur amount of Δk(n) is small and negative; If ΔK2<Δk(n)≤ΔK3, the blur value of Δk(n) is zero; If ΔK3<Δk(n)≤ΔK4, the blur amount of Δk(n) is positive and small; If Δk(n)>ΔK4, the blur amount of Δk(n) is positive; Among them, ΔK1, ΔK2, ΔK3, and ΔK4 are four preview slope thresholds, satisfying 0<ΔK1<ΔK2<ΔK3<ΔK4.

5. The unmanned vehicle tracking control method based on unmanned aerial vehicle recovery according to claim 4 is characterized in that: ΔK1=0.25, ΔK2=0.45, ΔK3=0.70, ΔK4=0.

85.

6. The unmanned vehicle tracking control method based on unmanned aerial vehicle recovery according to claim 3 is characterized in that: The fuzzy level value of the target speed is converted into a speed value as follows: V=G×ΔV+V0 (4) Wherein, V is the speed value, G is the fuzzy level value, G = -2, -1, 0, 1, 2, ΔV is the speed change corresponding to a fuzzy level change, ΔV is 1m / s, V0 is the speed value corresponding to the fuzzy level value 0, V0 is 11m / s.

7. The unmanned vehicle tracking control method based on unmanned aerial vehicle recovery according to claim 1 is characterized in that: Methods for controlling an unmanned vehicle based on target speed include: Get the current speed v(n) of the unmanned vehicle; Calculate v(n) and target speed v c The deviation e c (n) = v c -v(n); E c (n) Input the PID controller, and after amplification, integration and differentiation, control the speed of the unmanned vehicle to make the deviation e c (n) Reduce the direction change until the deviation is 0, so that the speed of the unmanned vehicle is v c ; The adjustment process of the unmanned vehicle speed v(n) is expressed as: v(n+1)=v(n)+k p (e c (n)-e c (n-1))+k i e c (n)+k d (e c (n)-2e c (n-1)+e c (n-2)) (5) In the formula, k p , k i , k d They are the gain coefficients of the proportional term, integral term and differential term of the PID controller respectively.

8. The unmanned vehicle tracking control method based on unmanned aerial vehicle recovery according to claim 1 is characterized in that: The method of calculating the steering wheel angle of the unmanned vehicle using the Stanley algorithm includes: S1. Calculate the steering wheel angle without considering the lateral tracking error When the lateral tracking error is not considered, the steering wheel angle is equal to the angle between the tangent direction of the UAV's expected trajectory at the preview point and the UAV's heading. Right now S2. Calculate the steering wheel angle δ without considering the heading tracking error y ; Without considering the heading tracking error, the larger the lateral tracking error, the larger the steering wheel angle. The steering wheel angle is calculated as follows: In the formula, e y (t) is the lateral tracking error, d(t) is the distance between the center of the left and right front wheels and the intersection of the tangent of their expected trajectory and the expected trajectory of the UAV at the preview point, d(t) = λv(t), is the speed of the UAV, and λ is the gain parameter; S3. Calculate the steering wheel angle using the following formula: Where δ is the steering wheel angle.

9. The unmanned vehicle tracking control method based on unmanned aerial vehicle recovery according to claim 1 is characterized in that: The unmanned vehicle model includes an internal model of the unmanned vehicle and an external model of the unmanned vehicle. The internal model of the unmanned vehicle includes a body module, a slip rate module, a tire module and a wheel rotation module. The external model of the unmanned vehicle includes a driver module, a motor module, a steering module, a braking module and a vertical movement module.

10. An unmanned vehicle tracking control device based on unmanned aerial vehicle recovery, characterized in that: include: The prediction module is used to predict the plane coordinates of the projection point of the drone on the ground at the next moment based on the motion trajectory of the drone, and obtain the coordinates of the preview point; A calculation module is used to calculate the target speed of the unmanned vehicle using a fuzzy control algorithm based on the coordinates of the unmanned vehicle at the current moment, the motion trajectory of the drone, and the coordinates of the preview point, and to calculate the steering wheel angle of the unmanned vehicle using the Stanley algorithm; The control module is used to control the unmanned vehicle according to the unmanned vehicle model based on the target speed and the steering wheel angle.

Images