Control method for approaching obstacle based on unmanned aerial vehicle in complex environment

Abstract

The invention belongs to the technical field of unmanned aerial vehicles, and particularly relates to an unmanned aerial vehicle near obstacle control method based on a complex environment, and the method comprises the following steps: 1, obtaining a curve between obstacles through employing a binocular camera; 2, estimating the collision force of the obstacle according to the curve to obtain a {p, v} pair; 3, designing a gradient-based trajectory optimizer to extract information of an obstacle collided with the current trajectory; 4, numerical optimization is carried out; and 5, designing an MPC controller according to the obtained {p, v} pairs, and outputting an optimal control trajectory of the unmanned aerial vehicle near the obstacle based on the MPC controller. According to the invention, the obtained distance field information and height information are used to predict the performance of the unmanned aerial vehicle in a certain time period in the future so as to carry out optimization control, and the distance field, the height and the acceleration are constrained. And safe flight, very small overshoot, very small steady-state error and trajectory prediction of the unmanned aerial vehicle in near obstacle control are ensured.

Description

Technical Field

[0001] The invention belongs to the technical field of unmanned aerial vehicles, and in particular relates to a control method for an unmanned aerial vehicle approaching an obstacle in a complex environment. Background Art

[0002] Research in the field of drones has long been not satisfied with simple human control, but has been moving towards intelligence; so-called intelligence refers to the ability of drones to obtain information about their own movements and surrounding environment, process information, make autonomous decisions and independently execute according to target mission requirements; when drones fly in outdoor areas, if they are in areas where GPS signals are severely blocked, such as signal towers, mines and other environments, in such complex environments, the method of relying on GPS to locate nearby obstacles is no longer reliable or even completely ineffective. Therefore, in order to determine the distance of obstacles, it has brought great challenges to the safe flight of drones and close-range detection of obstacles; in terms of obstacle identification, many methods have been successfully applied based on machine In visual obstacle recognition, such as binocular cameras, laser ranging, ultrasound, millimeter-wave radar, multi-source fusion and recognition methods based on computer vision, the advantages of these visual recognition methods are reflected in their rich information and wide detection range. In the movement close to obstacles, there are traditional gradient-based motion planning algorithms, which need to construct the required Euclidean signed distance field map, and there are two ways to construct the Euclidean signed distance field map, one is global incremental and the other is batch local calculation; but these are not specifically built for near obstacle movement; therefore, designing a near obstacle control method using binocular cameras is of key research significance.

[0003] In the prior art, the traditional gradient-based motion planning algorithm needs to construct the required Euclidean signed distance field map. However, constructing the map takes 70% of the time of the entire planning algorithm, which limits the use of motion planning methods under limited resources. In addition, there are two ways to construct the Euclidean signed distance field: global incremental and batch local calculation. If the global incremental method is used to construct the Euclidean signed distance field, although this method is very effective in the case of dynamic updates, the generated Euclidean signed distance field almost always contains redundant information that may not be used at all in the near obstacle control process. However, for motion planning, the Euclidean signed distance field maps constructed by the two existing methods are redundant and unnecessary, and simply manually reducing the range of the Euclidean signed distance field map lacks theoretical basis and also includes unnecessary calculations. The Euclidean signed distance field has long been used to construct objects from noisy sensor data. One of the envelope algorithms is to convert the Euclidean signed distance field into an envelope. The time complexity of Reed signed distance field construction is reduced to O(n); where n is the number of voxels; the algorithm is not suitable for incremental construction of Euclidean signed distance field, and the field often needs to be dynamically updated during quadcopter flight; incremental Euclidean signed distance field generation methods are also used. Although these methods are very effective in dynamic update situations, the generated Euclidean signed distance field almost always contains distance field information that may not be used at all in the near obstacle control process; resulting in the Euclidean signed distance field requiring a lot of time to construct and extremely low utilization; secondly, if PID is used as the control algorithm for near obstacles, if the proportional link is not adjusted properly and oscillation occurs, the safe flight of the drone cannot be guaranteed; in addition, for nonlinear control systems, the control accuracy will also decrease, and there will be steady-state errors, and the accurate distance between the drone and the obstacle cannot be guaranteed; the PID algorithm generally does not rely on future errors; if it relies on future errors and does not consider the model of the drone, inaccurate control is inevitable.

[0004] In view of the above shortcomings, the present invention proposes a method for controlling a UAV near obstacles in a complex environment, which enables the UAV to be smoothly controlled around obstacles. Summary of the invention

[0005] The purpose of the present invention is to provide a control method for a drone near an obstacle in a complex environment. First, by comparing the collision trajectory and the collision-free trajectory guide path, the collision term in the penalty function is obtained, and then the trajectory optimizer is used to extract the information of the obstacle hit by the current trajectory, and whether the dynamics of the trajectory is feasible is judged, and the time allocated for the trajectory is extended. Finally, the order of the trajectory is reduced while maintaining the original trajectory shape through an anisotropic curve fitting algorithm, thereby obtaining an accurate distance field of the feasible trajectory; in addition, a model predictive control MPC algorithm is used to replace the PID algorithm to process the obtained distance field information and height information, and constraints are added so that the drone has a desired detection trajectory for the near obstacle, and at the same time, there are acceleration and height constraints to prevent the drone from colliding with the obstacle. In addition, there is a similar feedforward function to improve the performance of the controller; the drone can be smoothly controlled around the obstacle.

[0006] The technical solution adopted by the present invention is as follows:

[0007] A control method for a drone near an obstacle in a complex environment comprises the following steps:

[0008] Step 1: Use a binocular camera to obtain the curve between obstacles;

[0009] Step 2: Estimate the collision force of the obstacle based on the curve and obtain the {p,v} pair;

[0010] Step 3: Design a gradient-based trajectory optimizer to extract information about obstacles hit by the current trajectory;

[0011] Step 4: Then perform numerical optimization;

[0012] Step 5: Design an MPC controller for the obtained {p, v} pair, and output the optimal control trajectory of the UAV near obstacles based on the MPC controller.

[0013] Preferably, in step 1, by obtaining the coordinates of the obstacle object in the left and right images of two cameras with a determined positional relationship, the three-dimensional size of the obstacle object and the three-dimensional coordinates of the feature points of the spatial object in the common field of view of the two cameras are output, and the depth distance of the target point from the camera is estimated; then, multiple spatial points P on the obstacle object are calculated, and these spatial points P are connected to obtain a curve about the obstacle.

[0014] Preferably, in step 2, a control point Q of a B-spline curve is defined to generate a B-spline curve Φ that satisfies the terminal constraint but does not consider obstacles; then, for each detected colliding line segment, a collision-free path Γ is generated using an optimization program; for each control point of the colliding line segment, a positioning point on the obstacle surface is generated, corresponding to a repulsive direction vector, where i is the index of the control point and j is the index of the {p, v} pair; each {p, v} pair corresponds to a specific control point; Q i The distance to the jth obstacle is: d ij =(Q i -P ij )V ij ; The unit vector v will not change again after it is generated for the first time, and thus d ij The value of is positive or negative; the {p,v} pair is repeatedly generated during the iteration process. The criterion for judging whether it is a new obstacle is: if the control point Q i When in an obstacle, and for all currently obtained obstacles j, d ij >0, the obstacle is a newly discovered obstacle; thus, the obstacle information affecting the trajectory is calculated.

[0015] Preferably, in step 3, a uniform B-spline curve Φ is used to represent the trajectory, and the variables to be planned are reduced according to the differential flatness characteristics of the drone; the optimization problem is defined as:

[0016]

[0017] Among them J s is the smooth term, J c is the collision term, J d is the dynamic feasible term, λ s ,λ c ,λ d is the weight of the penalty term;

[0018] Minimizing high-order derivatives can make the entire trajectory smooth; so the smooth term is composed of the acceleration and the sum of the squares of the jerk; due to the convex hull property of the B-spline curve, J s for:

[0019]

[0020] The collision term can be penalized by ij f The control point of the safety limit is pushed away from the obstacle; and a second-order continuous penalty function j is constructed c , with d ij The reduction of c The smaller the slope is, the smaller the constructed second-order continuous penalty function j c for:​

[0021]

[0022] where c ij =s f -d ij ;

[0023] The total collision term is obtained by summing the penalties of all control points:

[0024]

[0025] And directly calculate j in closed form c Q i The derivative of :

[0026]

[0027] Where N p is the control point Q i {p,v} j The number of

[0028] The penalty function is constructed with F() as the high-order derivative of each dimension:

[0029]

[0030]

[0031] Among them, ω v ,ω a ,ω J is the weight of each item, c r ∈C∈{V i ,A i ,J i},a1,b1,c1,a2,b2,c2 are used to satisfy the second-order continuity of the function, c m is the derivative restriction, c j is the boundary between the quadratic and cubic functions, λ<1-ε(ε<<1) is the elastic coefficient, so that the final result satisfies the constraints.

[0032] Preferably, in step 4, the objective function is first constructed:

[0033]

[0034] First calculate the proportion that exceeds the limit:

[0035]

[0036] where i∈{1,...,N c -1},j∈{1,...,N c-2},k∈{1,...,N c -3},r∈{x,y,z};r e It shows that relative to Φ s , Φ f How much more time should be allocated? i , A j and J k are inversely proportional to the first, second and third times of Δt respectively; then Φ f The new time interval is:

[0037] Δt'=r e Δt.

[0038] Preferably, in step 4, by solving a closed least squares problem, a trajectory Φ with a time span of Δt' is initially generated under the constraint f , while maintaining the same s The same shape and number of control points; then recalculate the smoothness term and feasible term to get a new objective function:

[0039]

[0040] where λ f is the weight of the fitting term (Φ s and Φ f degree of fit);

[0041] J s is defined as f (αT') to Φ s (αT) is the integral of the anisotropic displacement, where T and T' are the trajectory Φ s and Φ f The length of time, a∈[0,1]; due to the fitting curve Φ s Already collision-free, for both curves, use an axial displacement with a low weight to relax the smoothing restrictions, and a radial displacement with a high weight to prevent collisions.

[0042] Preferably, in step 4, due to Φ f The ellipsoid (αT') is a s (αT) is the center of the ellipse, around its main

[0043] Axis or tangent The axial displacement is obtained by rotating a certain angle; then a and radial displacement d r for:

[0044]

[0045] The fitting term is:

[0046]

[0047] Among them, a and b are the semi-major axis and semi-minor axis of the ellipse respectively. The semi-minor axis b corresponding to the radial displacement increases the penalty weight of the radial displacement to prevent collision. Finally, the data optimization is completed to obtain the feasible anti-collision trajectory and its distance field {p, v} pair.

[0048] Preferably, in step 5, the drone state quantity X is set to [{p, v}, H, V] T , control amount in β and α are mutually coupled;

[0049] The whole system is represented as f(X,U), and the subscript R is used to represent the trajectory reference. Then the reference system is f(X R ,U R ); Taylor expansion of f(X,U) at the reference point, ignoring the higher-order terms, yields:

[0050]

[0051] Subtract X R ,have to:

[0052]

[0053] in is the deviation between the actual state of the UAV and the state of the reference system; X R =[{p,v} R ,H R ,V R ] T ; is the deviation between the actual control quantity of the UAV and the input control quantity of the reference system; A and B are both state matrices;

[0054] Using the approximate discretization method to discretize the above formula, we get:

[0055]

[0056] Where: A k and B k are all discretized linear time-varying matrices; I is the unit matrix; T is the discrete time step control period; k is the number of arbitrary sampling times; are the differences between the actual position and the expected position of the UAV at the kth and k+1th sampling times, respectively; is the difference between the actual pitch angle, roll angle and yaw angle of the UAV and the expected pitch angle, roll angle and yaw angle at the kth sampling; C k is the identity matrix; is the discrete system output, that is Where Y(k) is the actual output trajectory, Y R (k) is the reference trajectory.

[0057] Preferably, in step 5, an incremental state space model is used, that is, As an increment of the control input, it can eliminate the steady-state error caused by external disturbances and inaccurate models; in order to facilitate adding constraints, a new system state variable is defined (Because the kth sampling uses the control quantity calculation result of the previous moment The state quantity is real-time because it uses sensors to collect data. ), so the new state space model is:

[0058]

[0059] in:

[0060]

[0061] Assuming that the wind speed and wind direction do not change within the forecast time domain, to simplify the calculation, the following assumptions are made:

[0062]

[0063] The predicted output is:

[0064] P(k)=ψ k ξ(k)+Θ k H(k)

[0065] in:

[0066]

[0067] Where: P(k) is the k-th sampling in the prediction time domain N P H(k) is the predicted output in the control time domain N at the kth sampling time. C The predicted input increment within, usually N C ≦N P ψ k and θ k All are system matrices; Predict the k+Nth sample at the kth sampling time P The system output value at the time of sampling; ΔU((k+N C -1)|k) is the prediction of k+Nth sampling at the kth sampling c -1 input increment value for sampling, and the same is true for other parameters;

[0068] Preferably, in step 5, the objective function J(k) is constructed using the state deviation of the unmanned sailboat and its control increment:

[0069]

[0070] Where: Q and R are weight matrices, which are changed according to control requirements; This shows the tracking effect of the UAV on the near obstacle under the action of the MPC controller. It reflects whether the UAV motion is stable, that is, to ensure a relatively small control increment as much as possible under the condition of satisfying the tracking task. By sorting and removing the items irrelevant to H(k), the standard quadratic objective function J(H(k)) can be obtained:

[0071]

[0072] Where: M k =2(θ k T Q e Θ k +R e ), G k =2θ k T Q e [ψ k ξ(k)], where And the weight matrix

[0073] The prediction time domain N at the kth sampling P The constraints can be expressed as:

[0074] ΔU min ≦H(k)≦ΔU max

[0075] U min ≦WH(k)+U(k-1)≦U max

[0076] in

[0077]

[0078] Where: ΔU min , ΔU max Both represent input increment constraints; U min , U max are all input constraints; W is the lower triangular matrix; U(k-1) is the control value of the pitch angle, roll angle and yaw angle at the previous moment; the control trajectory of the UAV near the obstacle based on MPC can be described as the optimal value problem:

[0079]

[0080] ΔU min ≦H(k)≦ΔU max

[0081] U min ≦WH(k)+U(k-1)≦U max

[0082] In each control cycle, the quadratic programming (QP) is solved to obtain the control time domain N c The input increment within will be used as the pitch angle, roll angle and yaw angle input of the UAV at the k+1th sampling time. After entering the next control cycle, the above optimization process will be repeated until the near obstacle trajectory tracking process is completed.

[0083] The technical effects achieved by the present invention are:

[0084] 1. In the present invention, the collision term in the penalty function is obtained by first comparing the collision trajectory and the collision-free trajectory guide path, and then the trajectory optimizer is used to extract the information of the obstacle hit by the current trajectory, and whether the dynamics of the trajectory is feasible is judged, and the time allocated for the trajectory is extended. Finally, the order of the trajectory is reduced while maintaining the original trajectory shape through the anisotropic curve fitting algorithm, thereby obtaining an accurate distance field of the feasible trajectory; in addition, the model predictive control MPC algorithm is used to replace the PID algorithm, the distance field information and height information obtained are processed, and constraints are added so that the UAV has the desired trajectory for detecting close obstacles, and at the same time, there are acceleration and height constraints to prevent the UAV from colliding with obstacles. In addition, there is a similar feedforward function to improve the performance of the controller; the UAV can be smoothly controlled around obstacles.

[0085] 2. The present invention does not require a Euclidean signed distance field map. The collision trajectory obtained by the binocular camera is compared with the collision-free trajectory guide path to obtain the collision term in the penalty function. The trajectory optimizer is then used to extract information about obstacles hit by the current trajectory, and whether the dynamics of the trajectory are feasible is determined. The time allocated for the trajectory is extended. Finally, the order of the trajectory is reduced while maintaining the original trajectory shape through an anisotropic curve fitting algorithm, thereby obtaining an accurate distance field of a feasible trajectory. This method enables the distance field obtained to be from a feasible trajectory, and the value is accurate, so that the required distance field information is obtained more efficiently.

[0086] 3. The present invention adopts the MPC algorithm to obtain distance field information, height information, speed information and acceleration information to predict the performance of the UAV in a certain period of time in the future for optimization control, and constrains the information to ensure that the UAV can fly safely and detect obstacles at close range.

[0087] 4. The present invention combines the distance field information obtained from the feasible trajectory with the MPC algorithm to make it a control method for UAVs near obstacles in complex environments. The designed control method has smaller overshoot, smaller steady-state error, trajectory prediction and local optimum compared with the commonly used PID-based control method.

[0088] 5. The present invention proposes a method for obtaining a feasible trajectory distance field without the need for a Euclidean signed distance field map; by comparing collision trajectories, a collision term with a penalty function is obtained, thereby reducing the calculation of redundant information that is not used at all in the near obstacle control process; by using anisotropic curve fitting algorithm, the order of the trajectory is reduced while maintaining the original trajectory shape, thereby reducing the complexity of calculation, thereby obtaining an accurate distance field of the feasible trajectory.

[0089] 6. The present invention proposes to use the obtained distance field information, altitude and other information to predict the performance of the UAV in a certain period of time in the future for optimized control, and to constrain the distance field, altitude and acceleration to ensure that the UAV can fly safely, with very small overshoot, very small steady-state error and trajectory prediction of the UAV in the control of near obstacles. BRIEF DESCRIPTION OF THE DRAWINGS

[0090] Figure 1 This is a flow chart of a control method for a UAV near obstacles in a complex environment according to the present invention;

[0091] Figure 2 This is a binocular camera model diagram in a control method of a drone near obstacles in a complex environment according to the present invention;

[0092] Figure 3 It is a schematic diagram of the generation process of {p, v} pairs in a control method of a UAV near obstacles in a complex environment according to the present invention;

[0093] Figure 4 It is a schematic diagram of spherical measurement in a control method of a UAV near obstacles in a complex environment according to the present invention;

[0094] Figure 5 It is a schematic diagram of the MPC control principle of a UAV based on a distance field in a control method of a UAV near obstacles in a complex environment according to the present invention. DETAILED DESCRIPTION

[0095] In order to make the purpose and advantages of the present invention more clearly understood, the present invention is specifically described below in conjunction with embodiments. It should be understood that the following text is only used to describe one or several specific embodiments of the present invention, and does not strictly limit the scope of protection of the specific claims of the present invention.

[0096] like Figure 1-5As shown, a control method for a drone near an obstacle in a complex environment is characterized by comprising the following steps:

[0097] Step 1: Use a binocular camera to obtain the curve between obstacles;

[0098] Specifically, binocular stereo vision fuses the images obtained by the two eyes and observes the differences between them, so that we can obtain a clear sense of depth, establish a correspondence between features, and correspond the image points of the same spatial physical point in different images; binocular stereo vision obtains three-dimensional information based on the principle of triangulation, that is, a triangle is formed between the image planes of the two cameras and the object being measured; if the positional relationship between the two cameras and the coordinates of the object in the left and right images are known, the three-dimensional size of the object in the common field of view of the two cameras and the three-dimensional coordinates of the feature points of the spatial object can be obtained; so the binocular vision system is generally composed of two cameras, such as Figure 2 As shown;

[0099] Among them, l , O r is the center of the left and right apertures, f is the focal length, Z is the depth, T is the aperture distance difference, x l , x r is the left and right coordinates of the image;

[0100] Consider a space point P. According to the similarity relationship of triangles, we can get:

[0101]

[0102] Arranged:

[0103]

[0104] Let d = x l -x r It is called disparity. When f and T are known, we can estimate the depth distance of the target point from the camera.

[0105] Disparity is inversely proportional to the depth distance. The larger the disparity, the closer the distance. In addition, since the minimum disparity is one pixel, there is a theoretical maximum value for the binocular depth. It can be seen that when the baseline is longer, the maximum distance that the binocular can measure becomes farther. On the contrary, a small binocular device can only measure very close distances. Considering multiple spatial points P on the obstacle, these spatial points P are connected to obtain a curve about the obstacle.

[0106] Step 2: Estimate the collision force of the obstacle based on the curve and obtain the {p,v} pair;

[0107] Define the control point Q of the B-spline curve, and first generate a B-spline curve Φ that satisfies the terminal constraint but does not consider obstacles; then, for each detected collision line segment, use an optimization program, and in this embodiment, use the A* algorithm to generate a collision-free path Γ; for each control point Q of the collision line segment i A positioning point p on the obstacle surface will be generated ij , corresponding to a repulsive direction vector Where i is the index of the control point, j is the index of the {p,v} pair; each {p,v} pair corresponds to a specific control point; the generation process of the {p,v} pair is as follows Figure 3 As shown;

[0108] Q i The distance to the jth obstacle is:

[0109] d ij =(Q i -p ij )v ij

[0110] Note that the unit vector v will not change after it is generated for the first time, so d ij The value of is positive or negative;

[0111] In order to prevent the trajectory from being pulled out of the current obstacle, {p,v} pairs are repeatedly generated during the iteration process. The criterion for judging whether it is a new obstacle is: if the control point Q i When in an obstacle, and for all currently obtained obstacles j, d ij >0, the obstacle is a newly discovered obstacle; thus only the obstacle information that affects the trajectory is calculated, reducing the running time;

[0112] In this method, in order to incorporate the necessary obstacle environment into the planning, a gradient-based trajectory optimizer needs to be explicitly designed to keep the track away from obstacles; the Euclidean signed distance field provides this crucial collision information, but at the cost of a heavy computational burden; in addition, due to the insufficient error information fed back by the Euclidean signed distance field, the planner based on the Euclidean signed distance field can easily fall into a local minimum and cannot escape from obstacles; to avoid this, an additional front end is always required to provide a collision-free initial trajectory; since the explicitly designed repulsion is quite effective for different tasks and environments, the above method is superior to the Euclidean signed distance field in providing important information for avoiding collisions;

[0113] Step 3: Design a gradient-based trajectory optimizer to extract information about obstacles hit by the current trajectory;

[0114] The present invention uses a uniform B-spline curve Φ to represent the trajectory, which is composed of an order pb 、N c control points {Q1,Q2,...,Q Nc} and a node vector {t1,t2,...,t M}determine, where Q i ∈R 3 , t M ∈R,M=N c +p b ; Each node of the uniform B-spline curve has the same time interval Δt=t m+1 -t m ;

[0115] The convex hull property of the B-spline curve shows that a certain section of the curve only has b +1 continuous control points, and the curve is contained in the convex hull formed by these points; the k-th derivative of the B-spline curve is p b -k-order B-spline curve; the control points of the first-order, second-order, and third-order derivatives of the trajectory Φ are:

[0116]

[0117] Then, the variables to be planned are reduced according to the differential flatness characteristics of the UAV; the optimization problem can be defined as

[0118]

[0119] Among them J s is the smooth term, J c is the collision term, J d is the dynamic feasible term, λ s ,λ c ,λ d is the weight of the penalty term;

[0120] Minimizing high-order derivatives can make the entire trajectory smooth; so the smooth term is composed of the sum of the squares of acceleration and jerk; due to the convex hull property of the B-spline curve, as long as the sum of the squares of the second-order and third-order control points of the trajectory Φ is minimized, the sum of the squares of acceleration and jerk can be effectively reduced; then J s for:

[0121]

[0122] The collision term can be penalized by ij f The control point of the safety limit pushes the control point away from the obstacle; for the convenience of optimization, we construct a second-order continuous penalty function j c , with d ij The reduction of c The smaller the slope is, the smaller the constructed second-order continuous penalty function j​c for:

[0123]

[0124] where c ij =s f -d ij ;

[0125] The total collision term is obtained by summing the penalties of all control points:

[0126]

[0127] Compared with the traditional method of using trilinear interpolation to calculate the gradient of the collision term, we directly calculate j in closed form c Q i The derivative of :

[0128]

[0129] Where N p is the control point Q i {p,v} j The number of

[0130] The dynamic feasible term ensures its feasibility by limiting the high-order derivatives in each dimension of the trajectory; thanks to the convex hull property of the B-spline curve, as long as the control points are restricted, it is sufficient to constrain the entire B-spline; the penalty function is constructed with F() as the high-order derivative in each dimension:

[0131]

[0132] Among them, ω v ,ω a ,ω J is the weight of each item, c r ∈C∈{V i ,A i ,J i},a1,b1,c1,a2,b2,c2 are used to satisfy the second-order continuity of the function, c m is the derivative restriction, c j is the boundary between the quadratic and cubic functions, λ<1-ε(ε<<1) is the elastic coefficient, so that the final result satisfies the constraints;

[0133] Step 4: Then perform numerical optimization;

[0134] First, construct the objective function:

[0135]

[0136] It will change with the addition of new obstacles, which requires the solver to be able to restart quickly. The objective function is mainly composed of quadratic terms, so Hessian information can speed up the convergence speed; but obtaining an accurate Hessian consumes a lot of computer resources; so we use the quasi-Newton method to approximate the Hessian from the gradient information;

[0137] Based on the obtained safe trajectory Φ s Generate a uniform B-spline curve trajectory Φ with reasonable time distribution f , and then use the anisotropic curve fitting method to make Φ f is free to optimize its control points to satisfy higher-order derivative constraints while maintaining Φ s Almost identical derivative shapes;

[0138] First calculate the proportion that exceeds the limit:

[0139]

[0140] where i∈{1,...,N c -1},j∈{1,...,N c -2},k∈{1,...,N c -3},r∈{x,y,z};r e It shows that relative to Φ s , Φ f How much more time should be allocated? i , A j and J k are inversely proportional to the first, second and third times of Δt respectively; then Φ f The new time interval is:

[0141] Δt'=r e Δt

[0142] By solving a closed least squares problem, the trajectory Φ with a time span of Δt' is initially generated under the constraint f , while maintaining the same s The same shape and number of control points; then recalculate the smoothness term and feasible term to get a new objective function:

[0143]

[0144] where λ f is the weight of the fitting term (Φ s and Φ f degree of fit);

[0145] J s is defined as f (αT') to Φ s(αT) is the integral of the anisotropic displacement, where T and T' are the trajectory Φ s and Φ f The length of time, a∈[0,1]; due to the fitting curve Φ s There is no collision. For the two curves, we use the axial displacement with low weight to relax the smooth adjustment restriction and use the radial displacement with high weight to prevent collision; Figure 4 As shown, the present invention uses a spherical metric to make the displacement on the same sphere surface produce the same penalty; for Φ f The ellipsoid (αT') is a s (αT) is the center of the ellipse, around its major axis or tangent The axial displacement is obtained by rotating a certain angle; then a and radial displacement d r for:

[0146]

[0147] The fitting term is:

[0148]

[0149] Where a and b are the semi-major and semi-minor axes of the ellipse, respectively. The semi-minor axis b corresponding to the radial displacement increases the penalty weight of the radial displacement to prevent collision.

[0150] Thus, the feasible anti-collision trajectory and its distance field {p, v} pair have been obtained;

[0151] Step 5: Design an MPC controller for the obtained {p, v} pair, and output the optimal control trajectory of the UAV near obstacles based on the MPC controller;

[0152] Specifically, MPC uses a rolling optimization method to achieve optimal control. This method does not rely on the precise model of the system and can effectively deal with the model uncertainty problems caused by changes in the structural parameters of the UAV and complex environments. By adding maneuverability constraints such as distance field, height and speed to the control process, it can be used for the design of the near-obstacle controller of the UAV. Based on the distance field of the feasible trajectory, the controller is designed in combination with the MPC method, such as Figure 5 As shown:

[0153] Set the drone state X = [{p,v},H,V] T , control quantity U=[φ,β,α] T , where φ, β, α are mutually coupled relationships;

[0154] The whole system can be expressed as f(X,U), with the subscript R representing the trajectory reference. The reference system is f(X R ,U R); Taylor expansion of f(X,U) at the reference point, ignoring the higher-order terms, yields:

[0155]

[0156] Subtract X R ,have to:

[0157]

[0158] in is the deviation between the actual state of the UAV and the state of the reference system; X R =[{p,v} R ,H R ,V R ] T ; is the deviation between the actual control quantity of the UAV and the input control quantity of the reference system; A and B are both state matrices;

[0159] Using the approximate discretization method to discretize the above formula, we get:

[0160]

[0161] Where: A k and B k are all discretized linear time-varying matrices; I is the unit matrix; T is the discrete time step (control period); k is the number of arbitrary sampling times; are the differences between the actual position and the expected position of the UAV at the kth and k+1th sampling times, respectively; is the difference between the actual pitch angle, roll angle and yaw angle of the UAV and the expected pitch angle, roll angle and yaw angle at the kth sampling; C k is the identity matrix; is the discrete system output, that is Where Y(k) is the actual output trajectory, Y R (k) is the reference trajectory;

[0162] Using the incremental state space model, As an increment of the control input, it can eliminate the steady-state error caused by external disturbances and inaccurate models; in order to facilitate adding constraints, a new system state variable is defined (Because the kth sampling uses the control quantity calculation result of the previous moment The state quantity is real-time because it uses sensors to collect data. ), so the new state space model is:

[0163]

[0164] in:

[0165]

[0166] Assuming that the wind speed and wind direction do not change within the forecast time domain, to simplify the calculation, the following assumptions are made:

[0167]

[0168] The predicted output is:

[0169] P(k)=ψ k ξ(k)+Θ k H(k)

[0170] in:

[0171]

[0172]

[0173] Where: P(k) is the k-th sampling in the prediction time domain N P H(k) is the predicted output in the control time domain N at the kth sampling time. C The predicted input increment within, usually N C ≦N P ψ k and θ k All are system matrices; Predict the k+Nth sample at the kth sampling time P The system output value at the time of sampling; ΔU((k+N C -1)|k) is the prediction of k+Nth sampling at the kth sampling c -1 input increment value for sampling, and the same is true for other parameters;

[0174] In order to ensure that the unmanned sailboat can accurately track the target trajectory with the smallest possible control increment, the objective function J(k) is constructed using the state deviation of the unmanned sailboat and its control increment:

[0175]

[0176] Where: Q and R are weight matrices, which can be changed according to control requirements; This shows the tracking effect of the UAV on the near obstacle under the action of the MPC controller. It reflects whether the UAV movement is stable, that is, to ensure a relatively small control increment (indicating a small adjustment range for the pitch angle, roll angle and yaw angle) while meeting the tracking task, thereby reducing the system energy consumption to a certain extent;

[0177] By arranging and removing the terms that are not related to H(k), we can obtain the standard quadratic objective function J(H(k)):

[0178]

[0179] Where: M k =2(θ k T Q e Θ k +R e ), G k =2θ k T Q e [ψ k ξ(k)], where And the weight matrix

[0180] The propellers and motors of drones are limited by mechanical properties, and their range of motion and speed are limited to a certain extent. Therefore, the threshold constraints of the control amount and control increment need to be considered. P The constraints can be expressed as:

[0181] ΔU min ≦H(k)≦ΔU max

[0182] U min ≦WH(k)+U(k-1)≦U max

[0183] in

[0184]

[0185] Where: ΔU min , ΔU max Both represent input increment constraints; U min , U max are all input constraints; W is the lower triangular matrix; U(k-1) is the control value of the pitch angle, roll angle and yaw angle at the previous moment;

[0186] In summary, the control trajectory of a UAV near an obstacle based on MPC can be described as an optimal value problem:

[0187]

[0188] ΔU min ≦H(k)≦ΔU max

[0189] U min ≦WH(k)+U(k-1)≦U max

[0190] In each control cycle, the quadratic programming (QP) is solved to obtain the control time domain N c The input increment within will be used as the pitch angle, roll angle and yaw angle input of the UAV at the k+1th sampling time. After entering the next control cycle, the above optimization process will be repeated until the near obstacle trajectory tracking process is completed.

[0191] In the present invention, firstly, the collision term in the penalty function is obtained by comparing the collision trajectory and the collision-free trajectory guide path, then the trajectory optimizer is used to extract the information of the obstacle hit by the current trajectory, and whether the dynamics of the trajectory is feasible is judged, and the time allocated for the trajectory is extended, and finally the order of the trajectory is reduced while maintaining the original trajectory shape through the anisotropic curve fitting algorithm, thereby obtaining the accurate distance field of the feasible trajectory; in addition, the model predictive control MPC algorithm is used to replace the PID algorithm, the obtained distance field information and height information are processed, and constraints are added, so that the UAV has the desired trajectory for detecting close obstacles, and at the same time, there are acceleration and height constraints to prevent the UAV from hitting obstacles, and there is also a similar feedforward function to improve the performance of the controller; so that the UAV can be smoothly controlled around obstacles.

[0192] It is worth mentioning that the present invention does not require a Euclidean signed distance field map, compares the collision trajectory obtained by the binocular camera with the collision-free trajectory guide path, obtains the collision term in the penalty function, and then uses a trajectory optimizer to extract information about obstacles hit by the current trajectory, and determines whether the dynamics of the trajectory are feasible, extends the time allocated for the trajectory, and finally uses an anisotropic curve fitting algorithm to reduce the order of the trajectory while maintaining the original trajectory shape, thereby obtaining an accurate distance field of the feasible trajectory; this method ensures that the distance field obtained is from the feasible trajectory, and the value is accurate, and the required distance field information is obtained more efficiently.

[0193] It is worth mentioning that the present invention adopts the MPC algorithm to obtain distance field information, height information, speed information and acceleration information to predict the performance of the UAV in a certain period of time in the future for optimization control, and constrains the information to ensure that the UAV can fly safely and detect obstacles at close range.

[0194] It must be said that the present invention combines the distance field information obtained from the feasible trajectory with the MPC algorithm, making it a control method based on the UAV near obstacles in complex environments. The designed control method has smaller overshoot, smaller steady-state error, trajectory prediction and local optimum compared with the commonly used PID-based control method.

[0195] Specifically, the present invention proposes a method for obtaining a feasible trajectory distance field without a Euclidean signed distance field map; a collision term with a penalty function is obtained by comparing collision trajectories, thereby reducing the calculation of redundant information that is not used at all in the near obstacle control process; and a heterogeneous curve fitting algorithm is used to reduce the order of the trajectory while maintaining the original trajectory shape, thereby reducing the complexity of the calculation, thereby obtaining an accurate distance field of the feasible trajectory.

[0196] It is worth mentioning that the present invention proposes to use the obtained distance field information, altitude and other information to predict the performance of the UAV in a certain period of time in the future for optimized control, and to constrain the distance field, altitude and acceleration to ensure that the UAV can fly safely, with very small overshoot, very small steady-state error and trajectory prediction of the UAV in the control of near obstacles.

[0197] The above is only a preferred embodiment of the present invention. It should be noted that, for those skilled in the art, several improvements and modifications can be made without departing from the principles of the present invention, and these improvements and modifications should also be considered as the protection scope of the present invention. The structures, devices and operating methods not specifically described and explained in the present invention shall be implemented according to the conventional means in the art unless otherwise specified and limited.

Claims

1. A control method for a drone near obstacles in a complex environment, characterized by: The following steps are involved: Step 1: Use a binocular camera to obtain the curve between obstacles; Step 2: Estimate the collision force of the obstacle based on the curve and obtain the {p,v} pair; Step 3: Design a gradient-based trajectory optimizer to extract information about obstacles hit by the current trajectory; Step 4: Then perform numerical optimization; Step 5: Design an MPC controller for the obtained {p, v} pair, and output the optimal control trajectory of the UAV near obstacles based on the MPC controller.

2. According to claim 1, a method for controlling a drone near obstacles in a complex environment is characterized by: In the step 1, by obtaining the coordinates of the obstacle object in the left and right images of two cameras with a determined position relationship, the three-dimensional size of the obstacle object and the three-dimensional coordinates of the feature points of the spatial object in the common field of view of the two cameras are output, and the depth distance of the target point from the camera is estimated; then, multiple spatial points P on the obstacle object are calculated, and these spatial points P are connected to obtain a curve about the obstacle.

3. The method for controlling a drone near obstacles in a complex environment according to claim 2, characterized in that: In step 2, a control point Q of a B-spline curve is defined, and a B-spline curve Φ that satisfies the terminal constraint but does not consider obstacles is generated; then, for each detected colliding line segment, a collision-free path Γ is generated using an optimization program; for each control point of the colliding line segment, a positioning point on the obstacle surface is generated, corresponding to a repulsive direction vector, where i is the index of the control point and j is the index of the {p, v} pair; each {p, v} pair corresponds to a specific control point; Q i The distance to the jth obstacle is: d ij =(Q i -P ij )V ij ; The unit vector v will not change again after it is generated for the first time, and thus d ij The value of is positive or negative; the {p,v} pair is repeatedly generated during the iteration process. The criterion for judging whether it is a new obstacle is: if the control point Q i When in an obstacle, and for all currently obtained obstacles j, d ij >0, the obstacle is a newly discovered obstacle; thus, the obstacle information affecting the trajectory is calculated.

4. The method for controlling a drone near obstacles in a complex environment according to claim 3 is characterized by: In step 3, a uniform B-spline curve Φ is used to represent the trajectory, and the variables to be planned are reduced according to the differential flatness characteristics of the drone; the optimization problem is defined as: Among them J s is the smooth term, J c is the collision term, J d is the dynamic feasible term, λ s ,λ c ,λ d is the weight of the penalty term; Minimizing high-order derivatives can make the entire trajectory smooth; so the smooth term is composed of the sum of the squares of acceleration and jerk; due to the convex hull property of the B-spline curve, J s for: The collision term can be penalized by ij f The control point of the safety limit is pushed away from the obstacle; and a second-order continuous penalty function j is constructed c , with d ij The reduction of c The smaller the slope is, the smaller the constructed second-order continuous penalty function j c for:​ where c ij =s f -d ij ; The total collision term is obtained by summing the penalties of all control points: And directly calculate j in closed form c Q i The derivative of : Where N p is the control point Q i {p,v} j The number of The penalty function is constructed with F() as the high-order derivative of each dimension: Among them, ω v ,ω a ,ω J is the weight of each item, c r ∈C∈{V i ,A i ,J i },a1,b1,c1,a2,b2,c2 are used to satisfy the second-order continuity of the function, c m is the derivative restriction, c j is the boundary between the quadratic and cubic functions, λ<1-ε(ε<<1) is the elastic coefficient, so that the final result satisfies the constraints.

5. The method for controlling a drone near obstacles in a complex environment according to claim 4, characterized in that: In step 4, the objective function is first constructed: First calculate the proportion that exceeds the limit: where i∈{1,...,N c -1},j∈{1,...,N c -2},k∈{1,...,N c -3},r∈{x,y,z};r e It shows that relative to Φ s , Φ f How much more time should be allocated? i , A j and J k are inversely proportional to the first, second and third times of Δt respectively; then Φ f The new time interval is: Δt′=r e Δt。 6. The method for controlling a drone near obstacles in a complex environment according to claim 5, characterized in that: In step 4, by solving a closed least squares problem, a trajectory Φ with a time span of Δt' is initially generated under the constraint. f , while maintaining the same s The same shape and number of control points; then recalculate the smoothness term and feasible term to get a new objective function: where λ f is the weight of the fitting term (Φ s and Φ f degree of fit); J s is defined as f (αT') to Φ s (αT) is the integral of the anisotropic displacement, where T and T' are the trajectory Φ s and Φ f The length of time, a∈[0,1]; due to the fitting curve Φ s Already collision-free, for both curves, use an axial displacement with a low weight to relax the smoothing restrictions, and a radial displacement with a high weight to prevent collisions.

7. The method for controlling a drone near obstacles in a complex environment according to claim 6, characterized in that: In step 4, since Φ f (αT') is an ellipsoid with Φ s (αT) is the center of the ellipse, around its major axis or tangent The axial displacement is obtained by rotating a certain angle; then a and radial displacement d r for: The fitting term is: Among them, a and b are the semi-major axis and semi-minor axis of the ellipse respectively. The semi-minor axis b corresponding to the radial displacement increases the penalty weight of the radial displacement to prevent collision. Finally, the data optimization is completed to obtain the feasible anti-collision trajectory and its distance field {p, v} pair.

8. The method for controlling a drone near obstacles in a complex environment according to claim 7, characterized in that: In step 5, the drone state quantity X is set to [{p, v}, H, V] T , control amount in β and α are mutually coupled; The whole system is represented as f(X,U), and the subscript R is used to represent the trajectory reference. Then the reference system is f(X R ,U R ); Taylor expansion of f(X,U) at the reference point, ignoring the higher-order terms, yields: Subtract X R ,have to: in is the deviation between the actual state of the UAV and the state of the reference system; X R =[{p,v} R ,H R ,V R ] T ; is the deviation between the actual control quantity of the UAV and the input control quantity of the reference system; A and B are both state matrices; Using the approximate discretization method to discretize the above formula, we get: Where: A k and B k are all discretized linear time-varying matrices; I is the unit matrix; T is the discrete time step control period; k is the number of arbitrary sampling times; are the differences between the actual position and the expected position of the UAV at the kth and k+1th sampling times, respectively; is the difference between the actual pitch angle, roll angle and yaw angle of the UAV and the expected pitch angle, roll angle and yaw angle at the kth sampling; C k is the identity matrix; is the discrete system output, that is Where Y(k) is the actual output trajectory, Y R (k) is the reference trajectory.

9. The method for controlling a drone near obstacles in a complex environment according to claim 8, characterized in that: In step 5, an incremental state space model is used, that is, As an increment of the control input, it can eliminate the steady-state error caused by external disturbances and inaccurate models; define new system state variables (Because the kth sampling uses the control quantity calculation result of the previous moment The state quantity is real-time because it uses sensors to collect data. Therefore, the new state space model is: in: Assuming that the wind speed and wind direction do not change within the forecast time domain, to simplify the calculation, the following assumptions are made: The predicted output is: P(k)=ψ k ξ(k)+Θ k H(k) in: Where: P(k) is the k-th sampling in the prediction time domain N P H(k) is the predicted output in the control time domain N at the kth sampling time. C The predicted input increment within, usually N C ≦N P ψ k and θ k All are system matrices; Predict the k+Nth sample at the kth sampling time P The system output value at the time of sampling; ΔU((k+N C -1)|k) is the prediction of k+Nth sampling at the kth sampling c -1 sampling input increment value, the same applies to other parameters.

10. The method for controlling a drone near obstacles in a complex environment according to claim 9, characterized in that: In step 5, the objective function J(k) is constructed based on the state deviation of the unmanned sailboat and its control increment: Where: Q and R are weight matrices, which are changed according to control requirements; This shows the tracking effect of the UAV on the near obstacle under the action of the MPC controller. It reflects whether the UAV motion is stable, that is, to ensure a relatively small control increment as much as possible under the condition of satisfying the tracking task. By sorting and removing the items irrelevant to H(k), the standard quadratic objective function J(H(k)) can be obtained: Where: in And the weight matrix The prediction time domain N at the kth sampling P The constraints can be expressed as: ΔU min ≦H(k)≦ΔU max IN min ≦WH(k)+U(k-1)≦U max in Where: ΔU min , ΔU max Both represent input increment constraints; U min , U max All are input constraints; W is the lower triangular matrix; U(k-1) is the control value of the pitch angle, roll angle and yaw angle at the previous moment; the control trajectory of the UAV near the obstacle based on MPC can be described as the optimal value problem: ΔU min ≦H(k)≦ΔU max IN min ≦WH(k)+U(k-1)≦U max In each control cycle, the quadratic programming (QP) is solved to obtain the control time domain N c The input increment within will be used as the pitch angle, roll angle and yaw angle input of the UAV at the k+1th sampling time. After entering the next control cycle, the above optimization process will be repeated until the near obstacle trajectory tracking process is completed.

Images