CN115097857A - Real-time trajectory planning method considering the shape of rotor UAV in complex environment - Google Patents

Abstract

The invention discloses a real-time trajectory planning method considering the appearance of a rotor unmanned aerial vehicle in a complex environment, which comprises the steps of collecting the current local map information and real-time positioning information of the unmanned aerial vehicle, establishing an occupancy probability grid map, and constructing an ESDF (electronic stability and data distribution function) map based on the occupancy probability grid map; by considering a front-end path planning algorithm of a yaw angle, abstracting the appearance of the unmanned aerial vehicle into a combination of a plurality of balls, and inquiring whether the distance between the circle center of each ball and the nearest barrier is smaller than the radius of the ball through an ESDF map to perform collision detection so as to obtain a path which accords with the kinematic constraint of the unmanned aerial vehicle and has no collision on the whole machine; and abstracting the appearance of the unmanned aerial vehicle into a combination of a plurality of balls in a back-end trajectory optimization algorithm, inquiring distance information between all ball centers and the obstacle and gradient direction information far away from the obstacle from an ESDF map, and optimizing a path by using a gradient descent method. According to the invention, safety constraint models of the unmanned aerial vehicle body are established at the front end and the rear end, and the safe and feasible track is ensured in a complex environment.

Description

A real-time trajectory planning method considering the shape of rotor UAV in complex environment

technical field

The invention relates to the field of trajectory planning of a multi-rotor unmanned aerial vehicle, in particular to a real-time trajectory planning method considering the shape of the rotor unmanned aerial vehicle in a complex environment.

Background technique

The real-time autonomous trajectory planning algorithm of rotary-wing UAV refers to generating a trajectory that satisfies the robot dynamics/kinematics constraints, smoothness constraints, and collision-free constraints in a complex environment. The algorithm includes a front-end path planning algorithm and a back-end trajectory optimization algorithm. The front-end path planning algorithm of the rotor UAV obtains a rough passable path, and its methods mainly include the path planning algorithm based on search and the path planning algorithm based on sampling. The search-based path planning algorithm is represented by the A* algorithm, which combines the Dijkstra algorithm and the breadth-first algorithm to search for the optimal path faster by using the heuristic function. The sampling-based path planning algorithm is dominated by RRT*, which randomly samples in space, connects the nearest sampling points into the path tree and considers whether it has a better parent node, until the area near the end point is explored. The front-end path planning algorithm provides the optimal initial value for the back-end trajectory optimization algorithm, so that the trajectory can quickly converge to the optimal solution when the constraints are met. In order to save computing resources, the current rotor UAV trajectory planning algorithm treats the UAV as a mass point, and only considers the planning of the three axes of x, y, and z when planning the path, and does not consider the influence of the shape of the UAV on the trajectory. If the UAV is really large, it will lead to the result that the searched path is not feasible.

For the trajectory planning of the rotor UAV, the research in recent years mainly focuses on the trajectory planning of the particle model of the UAV, and there is a lack of an easy-to-implement and general trajectory planning method considering the shape of the rotor UAV.

In the prior art, the shape of the rotor UAV is represented by a convex polyhedron, and the flight corridor is used to represent the safety constraints to avoid collision with obstacles. When planning, the convex polyhedron of the rotor drone is always included in the flight corridor, then The trajectory must be collision-free. According to the path searched by the front end, the flight corridor generation algorithm finds the farthest unobstructed point along the path from the first point, generates a convex polyhedron according to the connection between them, and loops many times until the end point is contained in the convex polyhedron. The algorithm is used for rotor UAV racing competitions, and is suitable for small rotor UAVs with strong maneuverability. If it is used for large UAVs, there are the following two problems: (1) The front-end path planning algorithm does not consider the For the actual shape of the human-machine, adding the back-end optimization considering the shape of the drone to the front-end of the particle model planning may lead to inability to solve; (2) When the back-end is optimized in the flight corridor, it must be ensured that every point of the convex polyhedron of the aircraft is within the The same flight corridor convex hull, but this is difficult to guarantee for large UAVs. And the algorithm is based on the known situation of the global map, and the planning of each vertex on the UAV convex polyhedron is very computationally intensive.

Therefore, it is urgent to propose a real-time trajectory planning method for large rotary-wing UAVs in narrow and complex environments.

SUMMARY OF THE INVENTION

In view of the deficiencies of the prior art, the present invention proposes a real-time trajectory planning method considering the shape of the rotor UAV in a complex environment. The shape constraints of the rotor UAV are added to the back end for effective and robust real-time local trajectory generation.

The technical solution of the present invention is as follows: the embodiment of the present invention provides a real-time trajectory planning method considering the shape of a rotor UAV in a complex environment, and the method includes the following steps:

(1) Collect the current local map information and real-time positioning information of the UAV, establish an occupancy probability grid map, and build an ESDF map based on the occupancy probability grid map;

(2) Through the front-end path planning algorithm considering the yaw angle, and abstracting the shape of the UAV as a combination of several spheres, check whether the distance between the center of the sphere and the nearest obstacle is less than the distance of the sphere through the ESDF map established in step (1). The radius of the UAV is used for collision detection, and the path that conforms to the kinematic constraints of the UAV and has no collision of the whole machine is obtained;

(3) In the back-end trajectory optimization algorithm, the shape of the UAV is abstracted as a combination of several balls, and the distance information between the center of the ball and the obstacle and the gradient direction information away from the obstacle are queried from the ESDF map, and then the gradient descent method is used. Optimize the path obtained in step (2).

The beneficial effects of the present invention are as follows: the present invention mainly designs a real-time trajectory planning method considering the shape of a large-scale rotor UAV in a complex environment, improves the trajectory planning algorithm based on the particle model of the UAV, and does not perform expansion of obstacles, The algorithm can ensure that a safe and feasible trajectory can be quickly obtained under the condition of considering the shape constraints of the large rotor UAV. By comparing with the trajectory planning method based on the particle model, it can be seen that if the UAV is regarded as a particle and the obstacle is expanded, it is likely to search for a path with collision, and the present invention searches roughly the same In the case of time, a safe and collision-free trajectory can be obtained. The path searched by the front end of the present invention is more suitable for the result of the back end optimization, and a collision-free trajectory can still be obtained when the obstacle is a corner. The present invention proposes a general real-time trajectory planning scheme considering the shape of a large-scale rotary-wing UAV, and establishes a UAV fuselage safety constraint model at the front and rear ends, which is feasible to ensure the trajectory safety in a complex environment. The method of the invention analytically expresses the geometric shape of the rotor UAV as a combined form of convex geometric bodies. The front-end path planning algorithm considers the planning of the yaw angle yaw and performs the collision check of the whole machine shape. The back-end trajectory optimization algorithm uses the chain rule to transfer the gradient at the center of the circle to the trajectory points according to the rotation and translation relationship between the center of the circle and the center of mass of the drone.

Description of drawings

Figure 1 shows the planning effect of the particle model of the large rotor UAV;

Fig. 2 is a schematic diagram of the analytical expression method of the geometric shape of the rotor UAV;

Figure 3 is an effect diagram of front-end path planning;

Figure 4 shows the effect of back-end trajectory optimization.

Detailed ways

Exemplary embodiments will be described in detail herein, examples of which are illustrated in the accompanying drawings. Where the following description refers to the drawings, the same numerals in different drawings refer to the same or similar elements unless otherwise indicated. The implementations described in the illustrative examples below are not intended to represent all implementations consistent with the present invention. Rather, they are merely examples of apparatus and methods consistent with some aspects of the invention as recited in the appended claims.

The following describes the real-time trajectory planning method considering the shape of the rotor UAV under the complex environment proposed by the present invention in detail with reference to the accompanying drawings. The features of the embodiments and implementations described below may be combined with each other without conflict.

The embodiment of the present invention proposes a real-time trajectory planning method that considers the shape of the rotor UAV in a complex environment. The method of the present invention can be used for the exploration of large rotor UAVs in unknown and complex environments, and can be used as a planning method in the autonomous navigation system of the UAV. device module. Based on the current local map information and the real-time positioning information of the UAV, a grid map and an ESDF map are established, and the collision with the surrounding environment can be quickly detected by abstracting the shape of the UAV as a combination of finite spheres. The front-end path search algorithm obtains a rough but safe and feasible local path, and the back-end optimizes the trajectory obtained by the front-end to satisfy the overall safety constraints of the fuselage. Slicker and workable.

The method of the present invention mainly involves the processing of the constraints on the overall shape of the UAV at the front and back ends of the algorithm. For the convenience of introduction, the present invention takes a rotor UAV with a rectangular shape (length 1.2m, width 0.8m) and negligible z-axis as an example . Specifically include the following steps:

(1) Collect the current local map information and real-time positioning information of the UAV, and establish the occupancy probability grid map and ESDF map.

The embodiment of the present invention uses an occupancy probability grid map to describe environmental information, and generates a map from measurement data of noisy sensors, such as lidar and binocular cameras, when the robot positioning information is known. The grid map uses the binarization method to indicate whether a grid is occupied by obstacles. For grid m _i , the probability of being occupied is p(m _i |z _1:t ,x _1:t ), which indicates that for the given grid The probability z _1:t of observing with a given grid m _i and 1 to t times of positioning information x ₁ :t. The larger the probability value, the higher the probability that the grid is occupied.

In the embodiment of the present invention, an ESDF (Euclidean Symbolic Distance Field) map is used to represent the potential field information of environmental obstacles. The map is established based on the occupancy probability grid map, and the distance of each grid from its nearest obstacle is obtained. For the three axes of x, y, and z, the distance functions between the obstacle grid and the surrounding grids are respectively constructed. The lower envelope of these functions is the distance function between the grid and its nearest obstacle, which can be expressed as:

为占据概率栅格地图中的所有栅格，f(q)为q的采样函数。为得到空间中某点与最近障碍物的距离与梯度，需要对该点周围的八个栅格进行三线性插值。Among them, p represents the query grid, q is the grid where the obstacle is located,

For all grids in the occupancy probability grid map, f(q) is a sampling function of q. In order to obtain the distance and gradient between a point in space and the nearest obstacle, it is necessary to perform trilinear interpolation on the eight grids around the point.

The occupancy probability grid map and the real-time data of the ESDF map are updated, and the map information is stored in the form of a vector. During path planning, the obstacle information can be obtained by querying the index value of the corresponding grid.

(2) Through the front-end path planning algorithm considering the yaw angle, a path that conforms to the kinematic constraints of the UAV and has no collision of the whole machine is obtained; the shape of the UAV is abstracted as a combination of several balls (taking two balls as an example), Collision detection is performed by checking whether the distance between the center of the two circles and the nearest obstacle is less than the radius of the circle through the established ESDF map.

The invention is divided into two parts: front-end path planning and back-end trajectory optimization. According to the differential flat dynamic characteristics of the rotor UAV model, the flat output space of the rotor UAV can be represented by x, y, z and the yaw angle yaw. For general rotor UAV, the use of particle model can meet its planning requirements, but for the front-end path planning algorithm of large rotor UAV, if the particle model is used for planning, a path that the UAV cannot actually pass may be obtained. As shown in Figure 1, the overall shape of the UAV needs to be considered when planning the path. The kinematics of large rotary-wing UAVs make it impossible to fly with a large attitude, that is, to rotate the pitch (pitch angle) and roll (roll angle) greatly, so only the kinematics of its yaw (yaw angle) are considered in planning. constraint.

In the example of the present invention, the front-end path planning algorithm utilizes the kinematic-based A* algorithm considering the yaw angle yaw to obtain the initial path, specifically:

(2.1) Expand the nodes from the starting point, obtain several motion primitives based on different control inputs, and retain the motion primitives that meet the constraints by checking the safety and dynamic feasibility of the motion primitives;

The general A* algorithm uses the Euclidean distance from the starting point to the current node as the cost function, and the Euclidean distance from the current node to the end point as the heuristic function. Every time when searching to the end point, the cost function and the heuristic function in the node to be expanded are selected with the smallest sum of the cost function and the heuristic function. The node is used as the next waypoint until the search reaches the end point. For the A* algorithm based on dynamics, the kinematic characteristics of the UAV need to be considered when expanding nodes, and the trajectory of its motion can be expressed as a polynomial with respect to time t. Considering the real-time trajectory planning of the shape of the rotor UAV, it is necessary to add a one-dimensional The planning for the yaw angle is expressed using four independent one-dimensional time-parameterized polynomial equations:

p(t): = [p _x (t), p _y (t), p _z (t), P _yaw (t)] ^T

为状态向量，使

为控制输入，则其状态空间模型可以表示为：Among them, p _μ (t) is the time-parameterized polynomial equation of each dimension, μ∈{x, y, z, yaw}, a _k is the polynomial coefficient, and k is the polynomial order. Make

is the state vector, so that

is the control input, its state space model can be expressed as:

The complete solution to the equation of state is expressed as:

It indicates that the initial state is x(0), the control input is u(t), and the trajectory of the rotor UAV system is x(t).

本发明实施例中给定n＝2，表示以各轴的加速度为输入量，则偏航角yaw的输入量为角加速度。各轴的[-u_max，u_max]被均匀离散为

则每次扩展节点可以得到(2r+1)⁴条运动基元。When expanding the node, given the current state of the rotor UAV, input a set of discrete control quantities for the duration τ

In the embodiment of the present invention, n=2 is given, which means that the acceleration of each axis is used as the input quantity, and the input quantity of the yaw angle yaw is the angular acceleration. [-u _max , u _max ] of each axis is uniformly discretized as

Then each time a node is expanded, (2r+1) ⁴ motion primitives can be obtained.

In the present invention, in order to balance the calculation consumption of front-end and back-end planning, let r=2, and each motion primitive needs to pass feasibility and security checks before its corresponding node can be added to the scalable node set. The feasibility check requires that the velocity and acceleration of the trajectory points on the motion primitives do not exceed the maximum limit of the UAV's motion capability, and the safety check requires that the entire motion primitives have no collision, and the distance between the center of each spherical combination and the nearest obstacle is inquired. Distance and calculate, the distance cannot be less than the radius of the ball.

Rapid collision detection is performed by abstracting the shape of the drone as a combination of a limited number of balls (two balls are used as an example in the embodiment of the present invention).

In order to meet the real-time performance of trajectory generation, the present invention abstracts the drone body as a combination of two balls for safety inspection. , and the distance to the nearest obstacle is obtained by querying the ESDF map information. Since the computational complexity of the query operation is 0(n), the present invention can achieve a real-time effect for the collision detection method considering the shape of the drone.

(2.2) According to the motion primitives obtained in step (2.1), the nodes corresponding to each motion primitive are calculated, the cost function and the heuristic function are evaluated for each node, and the node is added to the list of nodes to be expanded. The node with the smallest sum of the cost function and the heuristic function in the node list to be expanded is used as the next node; the above steps are repeated until the searched node is the end point, that is, a path that conforms to the kinematic constraints of the UAV and has no collision for the whole machine is obtained.

The cost function J(T) of the node is designed as a function of the control input u(t) and time t, namely:

Among them, ρ is the weight of time t.

The heuristic function J*(Th) of the node is designed as the optimization problem from the current state of the node to the end state without considering the collision, aiming to find the optimal transition time t, namely:

where p _μc , v _μc , p _μg , v _μg are the position and speed of the current node and end point UAV, by using

得到最优时间t下，最小的J*(Th)。

Get the minimum J*(Th) under the optimal time t.

If the node to be expanded is empty and has not reached the end point, the input control amount of yaw is more finely discretized, let r _yaw =4, and regenerate (2r+1) ³ *(2r _yaw +1) motions Primitive, if there is still no scalable node, return to the previous node and search for a path with a different topology.

The rough trajectory generated by the kinematics-based A* algorithm that considers the yaw angle at the front end provides a better initial value for the back-end trajectory optimization, which speeds up the back-end optimization speed and avoids the general A* algorithm to search for infeasible areas. Conditions that cause optimization to fail.

(3) Through the back-end trajectory optimization algorithm, the non-particle model of the UAV is constructed, the geometric shape of the UAV is abstracted into an analytical expression method, and the local path obtained in step (2) is optimized.

The back end of the present invention performs trajectory optimization based on B-splines or MINCO trajectory classes. In the embodiment of the present invention, taking a B-spline as an example, the front-end path can be converted into a pb-order B-spline with control points Q={Q_0, Q_1, . . Trajectory optimization is based on the method of gradient descent. By derivation of the objective function, the gradient direction that reduces the value of the objective function is obtained, thereby optimizing the position of the control point to minimize the objective function, and the objective function is a function of the control point. Its objective function can be expressed as:

f=λ _s f _s +λ _c f _c +λ(f _v +f _a )

Among them, f _s is the smooth cost function, λ _s is the coefficient corresponding to the user-defined smooth cost function, and the trajectory is made smoother by obtaining the geometric information of the trajectory. f _c is the collision cost function, and λ _c is the coefficient corresponding to the user-defined collision cost function to keep the trajectory away from obstacles. f _v and f _a are the dynamic feasibility cost function, λ is the coefficient corresponding to the self-defined dynamic feasibility cost function, and the speed and acceleration of the robot are limited to not exceed the limit.

In order to generate a safe trajectory considering the shape of the whole machine, the present invention uses the shape of the rotor UAV as a combination of two balls, as shown in Figure 2 (the five-pointed star is P _A and P _B , the cross star is P _O ), P _O is the center of mass of the rotor UAV, P _A and P _B are the centers of the two spheres, respectively, the modulus of P _A and P _B to P _O in space is a certain value, and the coordinates of P _A and P _B to P _O The amount of translation is a function of the yaw angle yaw. In order to make the constraint function (collision cost function) a convex function, when calculating the collision constraint, the distance information between the two sphere centers and the obstacles in the environment and the gradient direction information away from the obstacles are obtained from the ESDF map, and then according to P _A The rotation and translation relationships of , P _B , and _PO transfer distance and gradient information to _PO point, thereby pushing the trajectory away from obstacles in the direction of gradient descent to avoid collisions. Taking the center of the sphere PA as an example, _PA can be expressed as _a function of the center of mass _PO and the heading angle:

为机器人当前的航向角，

为P_O对P_A关于航向角

的平移量。根据链式法则，可求出约束函数J(即碰撞成本函数f_c)关于机器人位置和航向角的梯度：

is the current heading angle of the robot,

for P _O to P _A with respect to the heading angle

amount of translation. According to the chain rule, the gradient of the constraint function J (ie, the collision cost function f _c ) with respect to the robot position and heading angle can be found:

为从环境距离场得到的P_A点关于障碍物的梯度。in,

is the gradient of point P _A with respect to the obstacle obtained from the environmental distance field.

The method for abstracting the geometric shape of the rotor UAV into an analytical expression in the present invention includes, but is not limited to, using a limited combination of circles to represent, and any other combination of convex polyhedrons can be applied.

The rendering of the front-end path planning is shown in Figure 3. The effect diagram of back-end trajectory optimization is shown in Figure 4. By comparison, it can be seen that the path searched by the front end of the present invention is more suitable for the result of the back end optimization, and a safe and collision-free trajectory can still be obtained when the obstacle is a corner.

The above embodiments are only used to illustrate the design ideas and features of the present invention, and the purpose is to enable those skilled in the art to understand the contents of the present invention and implement them accordingly, and the protection scope of the present invention is not limited to the above embodiments. Therefore, all equivalent changes or modifications made according to the principles and design ideas disclosed in the present invention fall within the protection scope of the present invention.

Claims (10)

1. A real-time trajectory planning method considering the appearance of a rotor unmanned aerial vehicle in a complex environment is characterized by comprising the following steps:

(1) acquiring current local map information and real-time positioning information of the unmanned aerial vehicle, establishing an occupation probability grid map, and establishing an ESDF (extended service distribution function) map based on the occupation probability grid map;

(2) by considering a front-end path planning algorithm of a yaw angle, abstracting the appearance of the unmanned aerial vehicle into a combination of a plurality of balls, and checking whether the distance between the circle centers of all the balls and the nearest barrier is smaller than the radius of the ball through the ESDF map established in the step (1) to perform collision detection, so that a path which is in accordance with the kinematic constraint of the unmanned aerial vehicle and has no collision on the whole machine is obtained;

(3) and (3) abstracting the appearance of the unmanned aerial vehicle into a combination of a plurality of balls in a back-end trajectory optimization algorithm, inquiring distance information between all ball centers and the obstacle and gradient direction information far away from the obstacle from an ESDF map, and optimizing the path obtained in the step (2) by using a gradient descent method.

2. The real-time trajectory planning method considering the appearance of a rotary-wing unmanned aerial vehicle in a complex environment according to claim 1, wherein the process of constructing the ESDF map based on the occupancy probability grid map specifically comprises:

the ESDF map represents potential field information of the environmental obstacles, distance functions of obstacle grids and surrounding grids are respectively constructed for three axes of x, y and z, and the lower envelope lines of the functions are the distance functions of the grids and the nearest obstacles

Can be expressed as:

wherein p represents the grid of the query, q is the grid where the obstacle is located,

to occupy all the grids in the probability grid map, f (q) is a sampling function of q.

3. The real-time trajectory planning method taking into account the external shape of the unmanned rotary wing aircraft in a complex environment according to claim 1, wherein the yaw-angle-taking-into-account front-end path planning algorithm is a kinematics-based a-x algorithm taking into account a yaw angle yaw.

4. The real-time trajectory planning method taking into account the profile of a rotary-wing drone in a complex environment according to claim 1, characterized in that said step (2) comprises the following sub-steps:

(2.1) expanding nodes from a starting point, obtaining a plurality of motion primitives based on different control input quantities, and reserving the motion primitives meeting constraint conditions by carrying out safety and dynamics feasibility inspection on the motion primitives;

(2.2) calculating a node corresponding to each motion element according to the motion elements obtained in the step (2.1), evaluating a cost function and a heuristic function of each node, and adding the node into a node list to be expanded; taking the node with the minimum sum of the cost function and the heuristic function in the node list to be expanded as a next node; and repeating the steps until the searched node is the end point, and obtaining a path which accords with the kinematics constraint of the unmanned aerial vehicle and has no collision on the whole machine.

5. The real-time trajectory planning method considering the appearance of a rotary-wing unmanned aerial vehicle in a complex environment according to claim 4, wherein the step (2.1) is specifically as follows:

the kinematics of the unmanned aerial vehicle is considered during node expansion, the motion track of the unmanned aerial vehicle is expressed as a polynomial about time t, the real-time track planning considering the appearance of the rotor unmanned aerial vehicle is added into the one-dimensional yaw angle planning, and four independent one-dimensional time parameterized polynomial equations are used for expression:

p(t):＝[p _x (t)，P _y (t)，p _z (t)，p _yaw (t)] ^T

wherein p is _μ (t) is the time parameterized polynomial equation for each dimension, μ ∈ { x, y, z, yaw }, a _k Is a polynomial coefficient, k is a polynomial order

As a state vector, make

Is a control input;

its state space model can be expressed as:

the complete solution to the equation of state is expressed as:

it represents that the initial state is x (0), the control input quantity is u (t), and the terminal state is x (t);

in the expansion node, given the current state of the rotorcraft, a set of discrete control quantities is input for the duration τ

The input quantity of the yaw angle yaw is angular acceleration; of each shaft [ -u [ ] _max ,u _max ]Is uniformly dispersed into

The extension node can get (2r +1) each time ⁴ A bar motion primitive.

6. The real-time trajectory planning method considering the appearance of the unmanned rotorcraft in the complex environment according to claim 4, wherein the step (2.1) further includes performing feasibility and security check on each motion primitive so as to enable a corresponding node to be added into an extensible node set, specifically: each motion element can enable a corresponding node to be added into an expandable node set only through feasibility and safety check, the feasibility check requires that the speed and the acceleration of track points on the motion elements do not exceed the maximum limit of the motion capability of the unmanned aerial vehicle, the safety check requires that the whole motion element is free of collision, the distance from each sphere center of the spherical combination to the nearest obstacle is inquired and calculated, and the distance cannot be smaller than the radius of a sphere; the shape of the unmanned aerial vehicle is abstracted into a combination of a limited number of balls for rapid collision detection; and checking whether the distance between the circle centers of every two balls and the nearest barrier is smaller than the radius of the ball to judge whether collision occurs or not, wherein the distance between the circle centers of every two balls and the nearest barrier is obtained by inquiring ESDF map information.

7. The real-time trajectory planning method taking into account the external shape of the unmanned rotorcraft in a complex environment according to claim 4, wherein the node cost function in step (2.2) is a function of the control input u (t) and time t, and the formula is as follows:

wherein ρ is the weight of time t;

the heuristic function of a node is:

wherein p is _μc ，v _μc ,，p _μg ，v _μg For the current node and destination drone position and speed, by

And obtaining the minimum heuristic function J (Th) under the optimal time t.

8. The real-time trajectory planning method taking into account the appearance of a rotorcraft in a complex environment according to claim 1, wherein the back-end trajectory optimization algorithm is a B-spline or mioco trajectory-based trajectory optimization.

9. The real-time trajectory planning method considering the appearance of a rotary-wing unmanned aerial vehicle in a complex environment according to claim 1, wherein the step (3) is implemented by using a gradient descent method, and optimizing a gradient direction for reducing an objective function value by deriving an objective function, wherein the objective function has the following formula:

f＝λ _s f _s +λ _c f _c +λ(f _v +f _a )

wherein f is _s As a smooth cost function, λ _s A coefficient corresponding to the self-defined smooth cost function; f. of _c As a function of collision cost, λ _c A coefficient corresponding to the self-defined collision cost function; f. of _v And f _a And lambda is a coefficient corresponding to the self-defined kinetic feasibility cost function.

10. The real-time trajectory planning method considering the profile of a rotorcraft in a complex environment according to claim 9, wherein the process of calculating the collision cost function is specifically:

equivalent rotor unmanned aerial vehicle appearance into two ballsCombination of (1), P _O For rotor unmanned plane centroid, P _A And P _B Respectively the centers of the two spheres, respectively obtaining the distance information between the two centers and the obstacle in the environment and the gradient direction information far away from the obstacle from the ESDF map, and then according to the P _A 、P _B 、P _O The rotational and translational relationships of (A) convey distance and gradient information to P _O Points to push the trajectory away from the obstacle in the direction of gradient descent to avoid collision; p _A Expressed as the center of mass P _O And heading angle:

is the current course angle of the robot,

is P _O To P _A About the angle of course

The amount of translation of; from the chain rule, the constraint function J, i.e. the collision cost function f, is determined _c Gradient with respect to robot position and heading angle:

wherein,

for P derived from an ambient distance field _A Turning off the lightGradient over obstacles.

Images