CN111552296B

Movatterモバイル変換

Info

Publication number: CN111552296B
Application number: CN202010410092.6A
Authority: CN
Inventors: 高会军; 李湛; 宋罘林; 于兴虎
Original assignee: Ningbo Intelligent Equipment Research Institute Co ltd
Current assignee: Ningbo Intelligent Equipment Research Institute Co ltd
Priority date: 2020-05-14
Filing date: 2020-05-14
Publication date: 2021-03-26
Anticipated expiration: 2040-05-14
Also published as: CN111552296A

Abstract

A local smooth track planning method based on a curved column coordinate system belongs to the technical field of unmanned vehicle track planning. The invention solves the problems of poor track continuity and smoothness and difficult track constraint of the existing unmanned vehicle local path planning method in the three-dimensional space. According to the invention, a smooth global path is planned according to map information and the target task requirement of the unmanned vehicle; taking the global path as a base line, and creating a curved column coordinate system; converting the motion state information of the unmanned vehicle from a global coordinate system to a curved column coordinate system; planning a local motion track of the unmanned vehicle in the future time T by using a cost function and time space discretization according to the global path; and deleting the local motion trail which is collided with the barrier and exceeds the power constraint range of the unmanned vehicle, and taking the local motion trail with the lowest cost function value as the optimal motion trail of the local planning. The method is suitable for planning the local motion trail of the unmanned vehicle.

Description

Local smooth track planning method based on curved cylindrical coordinate system

Technical Field

The invention belongs to the technical field of unmanned vehicle trajectory planning.

Background

In recent years, unmanned vehicles in three-dimensional space, such as small unmanned submarines and unmanned planes, have attracted attention of a large number of researchers at home and abroad due to the advantages of simple models, convenience in manufacturing, rapidness in response, good maneuverability, low cost and the like. However, in a complex three-dimensional space, planning a local path in real time, which can successfully avoid an obstacle, has a smooth track and does not change a motion state suddenly, has been one of the main research subjects for realizing high dynamic operation of an unmanned vehicle. Ensuring that a long and straight area of an unknown obstacle passes through quickly and stably in a three-dimensional space and successfully and accurately avoiding the obstacle to reach a target point is still very challenging work. This work focuses on methods that obtain optimal local paths given the knowledge of global paths, which is essentially an optimization problem.

Planning and controlling task level of unmanned vehicle in three-dimensional space can be divided into three levels: global path planning, local path following. Three levels are from high to low, abstract to concrete: in the highest layer of the whole task planning, a smooth path (path) from a starting point to an end point needs to be acquired according to the existing task, a map and other constraint information before the unmanned vehicle runs; local track planning (Local track planning) is arranged on a second layer of the whole mission planning, and the Local track planning is based on an obtained global path and combines the information of the surrounding environment observed in real time by a sensor carried on the unmanned vehicle in the three-dimensional space to plan a track (track) which accords with the current environmental constraint and the unmanned vehicle dynamic constraint in real time; local Path following (Path tracking) is performed on the lowest layer, namely the most specific layer, of task planning and control of the unmanned vehicle, and the unmanned vehicle uses a model-based or model-free control rate according to the current state of the unmanned vehicle and a local track obtained by the previous layer to realize that the local Path is followed with lower deviation. In the whole task planning and controlling process, any one of the above layers is the research focus at present, and the local path planning is located at the position where the local path planning starts and ends, and the difficulty is that the planned local path is guaranteed to meet the optimization of a certain degree (speed, curvature, smoothness and the like) and the feasibility of the path (obstacle avoidance, dynamics and the like).

The current method for local path planning can be divided into autonomous learning-based and non-autonomous learning-based types, wherein an end-to-end planning and control method based on deep learning and an algorithm based on reinforcement learning are greatly developed in theory in recent years, but the method has great problems in safety, reliability and stability and cannot be used in unknown environments at all; the non-autonomous learning algorithm can be classified into a search-based planning algorithm and a sampling-based planning algorithm. The search-based planning algorithm mainly represents an A-algorithm and a Dijkstra algorithm, and the two algorithms are well applied to many occasions, particularly in the fields of games, automobile navigation and the like, but have long planning time and large consumption and calculation power, and cannot be well applied to unmanned vehicles which run in high dynamic state in areas with unknown obstacle positions; the RRT algorithm and the PRM algorithm are mainly represented in the planning algorithm based on sampling, but the randomness of sampling in a three-dimensional space is higher, and the obtained path cannot be optimal, and a lot of burrs and jitters can appear and exceed the model constraint and the dynamic range of the unmanned vehicle.

In order to solve the problems, a special curve splicing method which is more suitable for unmanned vehicle path planning in a three-dimensional space appears: the path planning algorithm based on the smooth connection straight line and the circular arc can easily obtain a smooth path according to the initial state and the target configuration of the unmanned carrier, but the obtained path has poor flexibility, the centripetal acceleration of the unmanned carrier in a three-dimensional space at the connection part of the straight line and the circular arc has sudden change, and the requirement on following the path of the unmanned carrier behind is higher; the path obtained by the path planning algorithm based on the Bezier curve is more intelligent, four control points of the path can be edited, but the Bezier curve is difficult to meet the geometric continuity condition when spliced, and the Bezier curve is difficult to further optimize; the path planning algorithm based on the B-spline curve can obtain a smoothly spliced path, but the curvature of the B-spline curve is difficult to represent, and the curvature constraint condition of the unmanned carrier in the three-dimensional space is not easy to guarantee.

Disclosure of Invention

The invention provides a local smooth track planning method based on a curved cylindrical coordinate system, which aims to solve the problems that the existing method for planning the local path of an unmanned vehicle in a three-dimensional space is poor in track continuity and smoothness and difficult in track constraint.

The invention relates to a local smooth track planning method based on a curved column coordinate system, which comprises the following specific steps:

the method comprises the following steps: planning a smooth global path according to the map information and the target task requirement of the unmanned vehicle;

step two: taking the global path as a base line, and creating a curved column coordinate system;

step three: carrying out environment construction on the visual space of the unmanned vehicle, and extracting barrier information in a coordinate system of the visual space of the unmanned vehicle;

step four: converting the motion state information of the unmanned vehicle from a global coordinate system to a curved column coordinate system; converting the obstacle information in the visual space of the unmanned vehicle into obstacle information of a global coordinate system;

planning the local motion trail of the unmanned vehicle in the future time T by using a cost function and a time-space discretization algorithm according to the global path; acquiring a plurality of possible local motion tracks;

step six, deleting invalid local motion tracks from multiple possible local motion tracks according to the global coordinate system obstacle information obtained by conversion in the step four, and obtaining multiple effective local motion tracks; and step seven, calculating a cost function value of each effective local motion track, and taking the local motion track with the lowest cost function value as the optimal motion track of the local planning.

Further, in the present invention, the specific method for creating the curved cylindrical coordinate system with the global path as the baseline in the second step is as follows:

taking the global path as a z-axis, expressing the normal plane of each point on the path by using polar coordinates (r, theta), wherein r is the distance between the coordinate point and the global path, and theta is the spiral angle of the coordinate point around the global path, and obtaining a curved cylindrical coordinate system (r, theta, z).

Further, in the present invention, the invalid local motion trajectory in step six includes: the local motion trail of the collision with the obstacle and the local motion trail beyond the power constraint range of the unmanned vehicle.

Further, in the present invention, the cost function in the step five is:

C＝k_rC_r+k_zC_z+k_θC_θ (1)

where C is the total cost function, k_rIs a proportionality coefficient of a radial cost function, C_rAs a radial cost function, k_zIs a proportionality coefficient of a course cost function, C_zAs a course cost function, k_θIs the proportionality coefficient of the spiral cost function, C_θIs a spiral cost function.

Radial cost function C_rComprises the following steps:

wherein, C_t(r (t)) is an optimization index of the radial acceleration change rate, r (T) is the distance from the position of the local planning end to the global path at the time of the local planning, r (t) is the distance from the position of the unmanned vehicle to the global path at the time of t,

is the radial weight coefficient, k, of the local planning time index of this time_jrThe radial weight coefficient of the current local planning acceleration change rate index,

is the radial weight coefficient of the terminal state index of the local planning,

is the square of the third derivative of r (t) over time.

The course cost function is:

wherein, C_t(z (t)) is an optimization index of the change rate of the heading acceleration, z (t) is the course of the heading motion of the unmanned vehicle at the time t,

is the cost function of the local planning terminal moment in the z-axis direction; z (T) is a value in the z-axis direction at the time when the local planning is finished,

is the first derivative of z (T),

is the second derivative of z (T),

is the course weight coefficient of the local planning acceleration change rate index,

the course weight coefficient of the local planning time index is as follows:

wherein, C_t(theta (t)) is an optimization index of the rate of change of the helical acceleration, theta (0) is the helix angle at the initial time, theta (t) is the helix angle at time t,

is the spiral direction weight coefficient of the current local planning acceleration rate index,

is the spiral direction weight coefficient of the terminal state index of the local planning,

is the spiral direction weight coefficient of the current local planning time index.

Further, the specific method for acquiring multiple possible local motion trajectories in step five is as follows:

calculating an expression of a distance r (t) between the position of the unmanned vehicle and a global path at the time t, an expression of a displacement z (t) of course motion of the unmanned vehicle at the time t and an expression of a spiral angle theta (t) at the time t by using a time-space discretization algorithm and an optimization index of a radial acceleration change rate, an optimization index of a course acceleration change rate and an optimization index of a spiral acceleration change rate;

the specific method for solving the expression of the distance r (t) between the position of the unmanned vehicle and the global path is as follows:

order:

requirement C_t(r (t)) minimum, r (t) is required to satisfy

Therefore, the variable r (t) which is indexed by the accumulation of the acceleration rate must satisfy the fifth order polynomial relationship:

r(t)＝a₀+a₁t+a₂t²+a₃t³+a₄t⁴+a₅t⁵ (6)

a₀、a₁、a₂、a₃、a₄、a₅coefficients, which are all fifth order polynomials, are substituted for the initial and final values:

r(0)＝a₀ (7)

r(T)＝a₀+a₁T+a₂T²+a₃T³+a₄T⁴+a₅T⁵ (10)

obtaining a₀、a₁And a₂A value of (d); wherein r (0) is the distance of the unmanned vehicle from the global path at the initial time,

is the radial velocity of the unmanned vehicle at the initial moment,

is the radial acceleration of the unmanned vehicle at the initial moment, r (T) is the target radial position of the unmanned vehicle when the local planning is finished,

when the local planning is finished, the target radial speed of the unmanned vehicle,

the target radial acceleration of the unmanned vehicle is obtained when the local planning is finished;

bringing (7), (8) and (9) into (10), (11) and (12) to obtain:

solve to a₃、a₄And a₅Obtaining an expression of r (t); obtaining an expression of z (t) and an expression of theta (t) in the same way, and further obtaining a possible local motion trajectory as follows:

wherein, b₀、b₁、b₂、b₃、b₄、b₅、c₀、c₁、c₂、c₃、c₄、c₅The coefficients of expression z (t) and the coefficients of expression θ (t), respectively. The invention adopts a cost function and a time-space decomposition algorithm to findThe method solves the balance between the path optimality and the problem solvability, balances the calculation efficiency and the cost function complexity, and is very suitable for the local trajectory planning of unmanned vehicles such as high-dynamic unmanned submarines, unmanned aerial vehicles and the like in the three-dimensional space. The smoothness of the local motion trail is effectively improved. And a new coordinate system is provided, and under the coordinate system, the description of task planning problems of unmanned vehicles such as unmanned submarines, unmanned aerial vehicles and the like and the design of cost functions can be greatly simplified, the continuity and smoothness of the motion trail are improved, and the constraint of the motion trail is effectively realized.

Drawings

FIG. 1 is a flow chart of a local smooth trajectory planning method based on a curved cylindrical coordinate system according to the present invention;

FIG. 2 is a schematic representation of a curved cylindrical coordinate system;

FIG. 3 is a schematic diagram of a curved cylinder coordinate system and a global coordinate system;

FIG. 4 is the result of radial planning in an example;

FIG. 5 is the result of the spiral direction planning in the example;

FIG. 6 is a result of the course planning in an example;

fig. 7 is a partial planned trajectory of a drone in a particular embodiment.

Detailed Description

In a first embodiment, the present embodiment is described with reference to fig. 1 to 3, and the method for planning a local smooth trajectory based on a curved cylindrical coordinate system in the present embodiment includes the following specific steps:

step six, deleting invalid local motion tracks from multiple possible local motion tracks according to the global coordinate system obstacle information obtained by conversion in the step four, and obtaining multiple effective local motion tracks;

the invalid local motion profile comprises: the local motion trail of the collision with the obstacle and the local motion trail beyond the power constraint range of the unmanned vehicle.

And step seven, calculating a cost function value of each effective local motion track, and taking the local motion track with the lowest cost function value as the optimal motion track of the local planning.

In the embodiment, the specific implementation steps of the local planning algorithm can be seen, collision avoidance and unmanned aerial vehicle dynamics constraint are considered independently and are not designed in the cost function, because the introduction of the collision penalty term brings a large number of parameters which need to be adjusted manually, so that the design of the cost function becomes extremely complex, and the calculation amount and the complexity of track optimization can be greatly increased.

The optimization index only considers the running stability of the unmanned vehicle and the smoothness of the track, namely the acceleration continuity of the unmanned vehicle is ensured, and jerk (namely the change rate of the acceleration) is selected as one of the optimization indexes; because global path planning is related to task layers and static map information, but does not necessarily adapt to slight details and temporary dynamic changes in the environment, local path planning mainly exists for coping with temporary conditions of the environment, so that local paths are slightly adjusted on the basis of global paths, and the deviation is not too large relative to the global paths, and the difference between the local paths and the global paths is selected as one of optimization indexes; the local path planning can also be regarded as that of the unmanned aerial vehicle for the self track in a future periodThe prediction is that as the unmanned vehicle moves, the information of the environment acquired by the onboard sensor is more and more abundant, the path planned by the unmanned vehicle in the future time T may have a better path along with the movement of the unmanned vehicle, even sometimes, the original path is directly changed into invalid due to new change of the environment, so that the reliability of the actually planned path may be worse if the planned time T is longer, but if the T is too short, the power requirement on the unmanned vehicle is higher, most of the paths may exceed the dynamics constraint of the unmanned vehicle, so the planned time T should be valued in a reasonable range according to the actual task, and the planned time T is selected as one of the cost functions. The indexes can be easily designed under a curved cylindrical coordinate system. The cost function is thus decomposed into three terms along the coordinate axes (r, θ, z) of the curved cylindrical coordinate system: radial cost C_rHeading cost C_zAnd helical cost C_θ。

The unmanned vehicle comprises an unmanned aerial vehicle, and is also suitable for unmanned submarines and robots moving in three-dimensional space, such as magnetic robots, medical nano robots and the like.

Further, in this embodiment, the specific method for creating the curved cylindrical coordinate system with the global path as the baseline in the second step is as follows:

In the present embodiment, the curved cylindrical coordinate system is established with the global path as a base line, and for a smooth global path, the path is taken as a z-axis, and the normal plane of each point on the path is described using polar coordinates (r, θ) (as shown in fig. 2). Assuming the global path is a straight line, the coordinate system is degraded to a cylindrical coordinate system, where r is the distance of the coordinate point from the global path and θ is the spiral angle of the coordinate point around the global path. When the cylindrical coordinate system is generalized to an arbitrary smooth global path, a so-called "curved cylindrical coordinate system" (r, θ, z) is obtained. By utilizing the idea of Newton-Lei-Netzian infinitesimal, each small segment in the curved cylindrical coordinate system can be approximately regarded as a cylindrical coordinate system, so that the curved cylindrical coordinate system can also be considered to be formed by splicing countless cylindrical coordinate systems. For the problem of planning the local track of the unmanned vehicle, in each transient state, z is the flying distance of the unmanned vehicle along the global path direction, r is the distance between the unmanned vehicle and the global path in the current normal plane, r is 0 to represent that the unmanned vehicle is on the global path, and θ is 0, and the orientation is horizontal to the right relative to the direction time of the global path. Because the curved cylindrical coordinate system is established on the basis of the global path, the actual physical meaning of the cost function can be conveniently expressed on the basis of the coordinate system.

In this embodiment, the cost function in the step five is:

C＝k_rC_r+k_zC_z+k_θC_θ (1)

k_r、k_z、k_θThe scale factor of the cost in the three directions can be understood as the total cost obtained by weighted average of the costs in the three directions. In different actual planning tasks, different influence degrees of different costs on the path may be emphasized, for example, in a certain task, it is generally desirable that the path is not spiral as much as possible, and the change of distance from the global path is less important, so that k can be determined_θThe adjustment is larger, when the distance between the local path and the global path is important, k is set_rAdjust for big, etc. These three parameters are adjusted by the user depending on the emphasis of the actual problem. k is a radical of_r、k_z、k_θThe three parameters have another effect because the design of each cost function is different, and the cost functionThere are also respective scaling parameters, and the cost values in the last three directions are not necessarily in the same order of magnitude, and the order of magnitude needs to be adjusted by the three parameters.

Radial cost function C_rComprises the following steps:

is the radial weight coefficient of the local planning time index,

the radial weight coefficient of the current local planning acceleration change rate index,

is the square of the third derivative of r (t) over time.

According to the practical problems solved by the invention, the method is selected

As an optimization index of Jerk (Jerk); the deviation between the local path and the global path can be represented by the radial distance r (t) at the last time of the local planning.

The course cost function is:

is the first derivative of z (T),

is the second derivative of z (T),

is the course weight coefficient of the local planning time index,

is the course weight coefficient of the local planning terminal state index of the current time.

Rate of change of course cost function with respect to acceleration C_tThe same principle of the design of (z (T)) and the planning time length T part and the radial cost function is that the deviation of a local path and a global path does not exist in the navigation direction, so that no definite cost item related to the terminal position exists, and the cost item can be replaced by other expressions related to the terminal state according to the actual task situation, for example, the cost related to the terminal speed state can be set by requiring constant speed (tasks such as cruising and racing)

Requiring fixed points (tasks such as follow or frame-through) to set the cost (z-z) for the end position state_d)²And the like. In summary, regarding the end shapeThe state cost design is uniformly applied to h (z (T),

to indicate.

The spiral cost function is:

Further, in this embodiment, the specific method for acquiring multiple possible local motion trajectories in step five is as follows:

order:

requirement C_t(r (t)) minimum, r (t) is required to satisfy

r(t)＝a₀+a₁t+a₂t²+a₃t³+a₄t⁴+a₅t⁵ (6)

r(0)＝a₀ (7)

r(T)＝a₀+a₁T+a₂T²+a₃T³+a₄T⁴+a₅T⁵ (10)

is at the beginningThe radial speed of the unmanned carrier is measured,

bringing (7), (8) and (9) into (10), (11) and (12) to obtain:

wherein, b₀、b₁、b₂、b₃、b₄、b₅、c₀、c₁、c₂、c₃、c₄、c₅The coefficients of expression z (t) and the coefficients of expression θ (t), respectively.

The plurality of possible local motion trajectories described in this embodiment are finite local paths that are uniformly distributed around the global path.

In this embodiment, we can let us have

And

at zero, i.e. at the end of the local planning, the radial velocity and radial acceleration are 0, r (T) is reasonably discretized, where (r)_min,r_max) In between, a finite number of end states are obtained by sampling at intervals of Δ r, and T is similarly at (T)_min,T_max) In between, sampling at intervals of DeltaT, and substituting into the above linear equation system to obtain a series of a₀、a₁、a₂、a₃、a₃、a₄、a₅A series of r (t) is obtained. The other two directions are the same, and z (t) and theta (t) are obtained, so that a parameter equation which is equivalent to a curve is obtained, and a series of paths are obtained.

The time-space discretization algorithm described in this embodiment is a discretization of a target state. The specific process is that in the r direction, the setting is carried out_min,r_max) And (T)_min,T_max) The distance of the unmanned aerial vehicle deviating from the global path and the planning time length are limited, and a group of radial target states are obtained by setting delta r and delta T. Similarly, in the theta direction is set (theta)_min,θ_max) Limiting the range of helix angles, setting Δ θ to obtain a set of target states for the helical direction, which is the z direction (z direction)_min,z_max) And Δ z to obtain a set of target states for the heading. Δ r, Δ θ, Δ z, and Δ T represent discretization.

The specific embodiment is as follows:

the present embodiment is described with reference to fig. 4 to 7, which illustrate a method for planning a local trajectory of an unmanned aerial vehicle based on a curved cylindrical coordinate system.

The method specifically comprises the following steps: in a long corridor with a width and height of 3m, the unmanned aerial vehicle is made to fly at a constant speed of 5m/s, and occasionally pedestrians pass through the corridor, occasionally doors are opened and the like (various unmodeled obstacles). Assuming that the unmanned vehicle is a four-rotor aircraft with a wheelbase of 200, the upper speed limit is 10m/s, and the upper acceleration limit is 5m/s²The curvature is not restricted, and the positioning system is positioned by UWB. Assuming that the initial position of the quad-rotor aircraft is located at the global coordinates (1,0,0) the initial velocity and the initial acceleration are both 0.

The method comprises the following steps: a smooth global path is planned according to the map information, in the example, the global path can be set along the central line of the corridor, the smoothing processing at the corner of the corridor can be forgotten, and the cubic spline interpolation smoothing is assumed here. After the global path is acquired, the coordinates of the global path in the global coordinate system, the tangential direction vector of each point, and the distance from the starting point to each point on the path along the global path (i.e., the z-coordinate value in the curved cylindrical coordinate system) are known.

Step two: based on the global path, a "curved cylindrical coordinate system" is created.

Step three: the corridor space is constructed through airborne sensors such as TOF cameras on the four-rotor aircraft, and information of all obstacles in the visual space is extracted.

Step four: and converting the state information of the four-rotor aircraft from a global coordinate system to a 'curved cylindrical coordinate system'. The global coordinate system is aligned with the origin of the curved cylinder coordinate system, and the initial z-axis direction is the same. As is well known, the cylindrical coordinate system and the rectangular coordinate system are transformed as follows:

however, the z-axis of the curved cylindrical coordinate system is curved, so there is a small tick used to obtain the z-value of the quad-rotor vehicle in the curved cylindrical coordinate system: selecting the z-coordinate value of the closest point in the global path to the current position of the quadrotor aircraft in the curved cylindrical coordinate system as the z-coordinate value of the quadrotor aircraft in the curved cylindrical coordinate system, as shown in fig. 3, where the unmanned vehicle is located at point a, and point B on the global path is closest to the unmanned vehicle, because point B is located on the global path and the z-coordinate value of point B in the curved cylindrical coordinate system is known, we can consider that the z-coordinate value of point a in the curved cylindrical coordinate system is the same as the z-coordinate value of point B in the curved cylindrical coordinate system. And then, the coordinate values of the quadrotor aircraft under the 'curved cylindrical coordinate system' can be obtained only by using the conversion of the rectangular coordinate system and the polar coordinate system in a normal plane.

Step five: designing a cost function;

radial cost function:

wherein

Course cost function:

wherein the cruising speed

Helical cost function:

the total cost is C ═ k_rC_r+k_zC_z+k_θC_θRespectively adjusting the parameters according to the physical meanings of the parameters;

step six: discretizing the target configuration, namely discretizing a time space; according to the practical situation, the width and the height of the corridor are all 3m, the global path is in the middle of the corridor, and d can be set_min＝-1.2m，d_maxSelecting space sampling interval of 1.2m, and selecting space sampling interval of 0.2m to obtain the same distance between axes; the cylinder is equally divided into 8 parts of theta_min＝0，θ_max＝π，

Planning time T_min＝4s，T_max5s, 0.2 s; since it is a constant velocity problem, v is also discretized for the target velocity_min＝3m/s，v_max＝7m/s，△v＝1m/s。

The relationship between the coordinates (r, θ, z) of all the waypoints in the target configuration set and the time t is solved by using the theory of the theoretical basis part, as shown in fig. 4, 5 and 6, which are a relationship diagram between the radial position and the time, a relationship diagram between the spiral direction position and the time, and a relationship diagram between the heading position and the time in the embodiment. Since the obtained path is not an explicit analytical expression but in a parametric equation form, the path is saved in a waypoint list form. Fig. 7 shows the three-dimensional local trajectory drawn by some waypoints.

Step seven: and D, traversing all paths by combining the cost functions in the step five, replacing integration by accumulated summation, and solving the cost function values of all paths in the set.

Step eight: by judging the distance between the path points and the obstacle, setting a threshold value d to be 0.2m, and when the distance between any one of the path points and the obstacle is smaller than the threshold value d, considering that the local path has certain danger, such path is directly deleted from the waypoint list. The speed, acceleration and instantaneous curvature of each path point are calculated, and paths exceeding the set upper limit are also deleted. And selecting the path with the minimum cost in the remaining paths, namely the path of the local planning.

The unmanned aerial vehicle obtained by the invention has smooth local track path, no sudden change of acceleration and speed, no sudden change of acceleration, no sudden stop of sudden rotation of a motor, improved service life of the aircraft, improved energy efficiency of a battery and the like, and the provided coordinate system can provide clearer physical description for the local planning process, thereby facilitating the design of various cost functions and greatly simplifying the description of the local path planning problem of the unmanned aerial vehicle. By reasonable dispersion in a new coordinate system, the suboptimal solution of the optimization problem is skillfully solved, the balance between solvability and optimality is found, and the method is more suitable for really solving the actual problem.

The present invention is described herein with reference to particular embodiments, but it is to be understood that these embodiments are merely illustrative of the principles and applications of the present invention. It is therefore to be understood that numerous modifications may be made to the illustrative embodiments and that other arrangements may be devised without departing from the spirit and scope of the present invention as defined by the appended claims. It should be understood that features described in different dependent claims and herein may be combined in ways different from those described in the original claims. It is also to be understood that features described in connection with individual embodiments may be used in other described embodiments.

Claims

1. A local smooth track planning method based on a curved cylindrical coordinate system is characterized by comprising the following specific steps:

wherein the cost function is:

C＝k_rC_r+k_zC_z+k_θC_θ (1)

where C is the total cost function, k_rIs a proportionality coefficient of a radial cost function, C_rAs a radial cost function, k_zIs a proportionality coefficient of a course cost function, C_zAs a course cost function, k_θIs the proportionality coefficient of the spiral cost function, C_θIs a spiral cost function;

radial cost function C_rComprises the following steps:

wherein, C_t(r (t)) is an index for optimizing the radial acceleration rate, and r (T) isThe distance between the position of the local planning terminal and the global path at the time, r (t), is the distance between the position of the unmanned carrier and the global path at the time t,

is the radial weight coefficient of the local planning time index,

is the square of the third derivative of r (t) over time;

the course cost function is:

wherein, C_t(z (t)) is an optimization index of the change rate of the heading acceleration, z (t) is the course of the heading motion of the unmanned vehicle at the time t, h (z (T),

is the first derivative of z (T),

is the second derivative of z (T),

is the course weight coefficient of the local planning time index,

is the course weight coefficient of the terminal state index of the local planning;

the spiral cost function is:

is the spiral direction weight coefficient of the current local planning time index;

the specific method for acquiring a plurality of possible local motion trajectories is as follows:

order:

requirement C_t(r (t)) minimum, r (t) is required to satisfy

r(t)＝a₀+a₁t+a₂t²+a₃t³+a₄t⁴+a₅t⁵ (6)

r(0)＝a₀ (7)

r(T)＝a₀+a₁T+a₂T²+a₃T³+a₄T⁴+a₅T⁵ (10)

is the radial velocity of the unmanned vehicle at the initial moment,

is the radial acceleration of the unmanned vehicle at the initial time, r (T) is the target radial position of the unmanned vehicle at the end time of the local planning,

the target radial speed of the unmanned carrier at the end moment is locally planned,

the target radial acceleration of the unmanned carrier at the end moment of the local planning is obtained;

bringing (7), (8) and (9) into (10), (11) and (12) to obtain:

wherein, b₀、b₁、b₂、b₃、b₄、b₅、c₀、c₁、c₂、c₃、c₄、c₅Coefficients of expression z (t) and coefficients of expression θ (t), respectively;

2. The method for planning a local smooth trajectory based on a curved cylindrical coordinate system according to claim 1, wherein the second step takes the global path as a baseline, and the specific method for creating the curved cylindrical coordinate system is as follows:

3. The method according to claim 1, wherein the invalid local motion trajectory in the sixth step comprises: the local motion trail of the collision with the obstacle and the local motion trail beyond the power constraint range of the unmanned vehicle.