CN117369507A - Unmanned aerial vehicle dynamic path planning method of self-adaptive particle swarm algorithm - Google Patents
Unmanned aerial vehicle dynamic path planning method of self-adaptive particle swarm algorithm Download PDFInfo
- Publication number
- CN117369507A CN117369507A CN202311442280.7A CN202311442280A CN117369507A CN 117369507 A CN117369507 A CN 117369507A CN 202311442280 A CN202311442280 A CN 202311442280A CN 117369507 A CN117369507 A CN 117369507A
- Authority
- CN
- China
- Prior art keywords
- unmanned aerial
- aerial vehicle
- obstacle
- equation
- state
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Pending
Links
- 238000000034 method Methods 0.000 title claims abstract description 38
- 239000002245 particle Substances 0.000 title claims abstract description 35
- 230000001133 acceleration Effects 0.000 claims abstract description 19
- 230000004888 barrier function Effects 0.000 claims abstract description 6
- 239000011159 matrix material Substances 0.000 claims description 35
- 230000006870 function Effects 0.000 claims description 29
- 238000005259 measurement Methods 0.000 claims description 13
- 230000008569 process Effects 0.000 claims description 11
- 230000002776 aggregation Effects 0.000 claims description 9
- 238000004220 aggregation Methods 0.000 claims description 9
- 230000007704 transition Effects 0.000 claims description 9
- 238000009826 distribution Methods 0.000 claims description 6
- 230000003044 adaptive effect Effects 0.000 claims description 5
- 238000001914 filtration Methods 0.000 claims description 5
- 238000005070 sampling Methods 0.000 claims description 4
- 108010015046 cell aggregation factors Proteins 0.000 claims description 3
- 238000005457 optimization Methods 0.000 claims description 2
- 230000009286 beneficial effect Effects 0.000 description 2
- 238000004364 calculation method Methods 0.000 description 2
- 230000003068 static effect Effects 0.000 description 2
- 241001077391 Cynea sp. Burns04 Species 0.000 description 1
- 230000008859 change Effects 0.000 description 1
- 238000010276 construction Methods 0.000 description 1
- 238000012986 modification Methods 0.000 description 1
- 230000004048 modification Effects 0.000 description 1
- 238000005096 rolling process Methods 0.000 description 1
Landscapes
- Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)
Abstract
The invention discloses a unmanned aerial vehicle dynamic path planning method of a self-adaptive particle swarm algorithm, which belongs to the field of unmanned aerial vehicle path planning methods and comprises the following steps: s1: establishing a state equation of the unmanned aerial vehicle, and setting an objective function of the unmanned aerial vehicle; s2: establishing an optimal state estimation equation of the dynamic barrier; s21: establishing a state equation of the obstacle; s22: establishing an observation equation; s23: establishing an optimal state estimation equation of the dynamic barrier; s3: acquiring position coordinates, acceleration and speed of the unmanned aerial vehicle; obtaining position coordinates and speed of the obstacle; and taking the position coordinate, acceleration and speed of the unmanned aerial vehicle and the position coordinate and speed of the obstacle as inputs of a self-adaptive particle swarm algorithm, taking the objective function as a fitness function of the self-adaptive particle swarm algorithm, and iteratively solving the position coordinate of the unmanned aerial vehicle at the next moment. According to the method, the position of the unmanned aerial vehicle at the next moment can be iterated out through the self-adaptive particle swarm according to the optimal position of the dynamic obstacle.
Description
Technical Field
The invention belongs to the field of unmanned aerial vehicle path planning, and particularly relates to an unmanned aerial vehicle dynamic path planning method of a self-adaptive particle swarm algorithm.
Background
In complex multi-obstacle environments, not only static obstacles are present, but also unknown moving obstacles are included. Therefore, the unmanned aerial vehicle flight safety is ensured by adopting a local path planning method with high timeliness in the face of a dynamic complex environment. Therefore, accurate obstacle avoidance for moving obstacles is a key to path planning.
The hierarchical particle swarm algorithm can plan a better path avoiding the obstacle under the complex condition of multiple obstacles, but can only be used for a static scene of the obstacle, and the path planning can not be adjusted in time for dynamic obstacles so as to cause accidents. The name of the application publication number CN 115357050A is an unmanned aerial vehicle path planning method based on a hierarchical particle swarm algorithm and application of the unmanned aerial vehicle path planning method in the prior art;
the multi-unmanned aerial vehicle collaborative path planning algorithm can carry out dynamic planning, so that accidents caused by internal collision among groups when unmanned aerial vehicles execute tasks can be prevented, but external dynamic obstacles cannot be avoided. The name of the application publication number CN 106873628A is a prior art of a collaborative path planning method for tracking multiple moving targets by multiple unmanned aerial vehicles.
Disclosure of Invention
The unmanned aerial vehicle dynamic path planning method of the self-adaptive particle swarm algorithm can be used for solving the problem of dynamic obstacle for the path planned by the unmanned aerial vehicle.
In order to achieve the above object, the invention 1. An unmanned aerial vehicle dynamic path planning method of the adaptive particle swarm algorithm is characterized by comprising the following steps:
s1: establishing a state equation of the unmanned aerial vehicle, and setting an objective function of the unmanned aerial vehicle;
s2: establishing an optimal state estimation equation of the dynamic barrier;
s21: establishing a state equation of the obstacle;
s22: establishing an observation equation;
s23: establishing an optimal state estimation equation of the dynamic barrier through a Kalman filtering gain equation;
s3: acquiring the position coordinates, acceleration and speed of the unmanned aerial vehicle from a state equation of the unmanned aerial vehicle; obtaining the position coordinates and the speed of the obstacle from the optimal state estimation equation; and taking the position coordinate, acceleration and speed of the unmanned aerial vehicle and the position coordinate and speed of the obstacle as inputs of a self-adaptive particle swarm algorithm, taking an objective function of the unmanned aerial vehicle as an fitness function of the self-adaptive particle swarm algorithm, and iteratively solving the position coordinate of the unmanned aerial vehicle at the next moment.
Further, the state equation of the unmanned aerial vehicle is:
x(k+1)＝Ax(k)+Bu(k)
y(k)＝Cx(k)+Du(k)∈γ(k)
wherein x (k) is a state vector of the unmanned aerial vehicle; x (k) is a state vector of the drone; y (k) =cx (k) +du (k) ∈γ (k) is a specific form of the state space model of the unmanned aerial vehicle; y (k) is the system output vector; c= [10 00], D is the spatial dimension; setting initial position coordinates and target point coordinates of the unmanned aerial vehicle, initializing a path, discretizing the path,
the position P (t) of the unmanned plane at the time t and the position of the next route point are expressed as [ P (t+ 1|t) |, P (t+
2|t)，...P(t+j|t)]，P(t)＝[p 1 (t)，p 2 (t)，...，p N (t)] T ；
Δt is the sampling period of the system;
the objective function includes:
path cost function F L The expression is as follows:
wherein m is the number of current path subsequences, L i For the length of the ith path sequence, P m And P f Respectively representing the position of the current last subsequence and the position of the target point;
threat cost function F T The expression is as follows:
wherein m is the number of current subsequences, n is the number of obstacles detected by the unmanned aerial vehicle,for the position coordinates of the ith sub-sequence, +.>And O is the position set of all the obstacles detected by the unmanned aerial vehicle, and is the position information of the jth obstacle.
Further, in step S21, a state equation is built based on the moving obstacle in the uniform linear motion, the uniform turning motion and the oblique uniform simple harmonic motion mode;
the state equation of the dynamic obstacle is:
X k ＝φX k-1 +ΓW k-1
phi and Γ represent the system state transition matrix and the process noise distribution matrix, W, respectively k-1 Is a process noise matrix;
when the obstacle is in uniform linear motion in the three-dimensional task space:
when the obstacle is in uniform turning motion:
omega is the angular velocity of movement of the obstacle;
when the obstacle moves obliquely at uniform speed in a simple harmonic manner:
D 1 ＝A 0 (2πf 0 )cos(2πf 0 ·(k-1)),D 2 ＝A 0 (2πf 0 )sin(2πf 0 ·(k-1))；A o for simple harmonic motion amplitude, f o Is the simple harmonic motion frequency.
In step S22, the system observation equation is:
Z k ＝HX k +V k
Z k in order to observe the equation,the linear state transition matrix is used for converting the variable of the state matrix into a measurement matrix; vk is measurement noise.
Further, in step S23, the best estimated value of the obstacle at time k-1 is usedIn combination with the equation of state X k ＝φX k-1 +ΓW k-1 The position information of the next time is predicted as follows:
wherein the method comprises the steps ofThe current time state estimated value is obtained according to the optimal estimated value of the last time;
state estimation valueAnd system measurement Z k As system input, constructing a dynamic obstacle optimal state estimation equation by a filter gain equation and the like>
Wherein,the estimated state value of the obstacle at the time k is also the state value before prediction at the time k+1; k (K) k [Z k -HX k/k-1 ]Referred to as the system residual, reflects the difference between the predicted estimated value and the actual measured value, meaning that the two agree if the residual is equal to 0.
Further, in step S3, the weights of the adaptive particle swarm algorithm are:
ω＝ω 0 -hω h +sω s
ω 0 the termination and initial values are of size ω f And omega i Representing; omega h And omega s Proportional weights for the evolution rate factor and the aggregation factor; g and G max Respectively representing the current iteration times and the maximum cycle times;
acceleration coefficient:
evolution speed:
the aggregation degree of the particles is determined by the average value F of the optimal value F (gbest (G)) and the fitness function mean Is expressed by the ratio of (2);
degree of aggregation:
n is the iteration number, F (x i ) The x-coordinate of the position point for each drone.
The beneficial effects are that:
according to the method, an optimal state equation is designed for the dynamic obstacle, so that the position coordinate and the speed of the obstacle at each moment can be obtained, the position coordinate and the speed of the unmanned aerial vehicle are combined and are brought into the self-adaptive particle swarm, and the objective function is used as a result to perform iterative computation, so that the position of the unmanned aerial vehicle at the next moment is obtained. Because the optimal state equation of the obstacle is introduced, the dynamic obstacle can be predicted; and moreover, the optimal state equation of the obstacle obtained after the state equation of the obstacle is optimized is more accurate for the prediction result.
Drawings
Fig. 1 is a flow chart of the present method.
Detailed Description
For the purpose of making the objects, technical solutions and advantages of the embodiments of the present invention more apparent, the technical solutions of the present invention will be clearly and completely described below with reference to the accompanying drawings, and it is apparent that the described embodiments are some embodiments of the present invention, but not all embodiments. All other embodiments, which can be made by those skilled in the art based on the embodiments of the invention without making any inventive effort, are intended to be within the scope of the invention.
A unmanned aerial vehicle dynamic path planning method of a self-adaptive particle swarm algorithm comprises the following steps:
s1: and (3) constructing a state equation of the unmanned aerial vehicle, setting limiting conditions of the unmanned aerial vehicle, and setting an objective function set of the unmanned aerial vehicle.
Specifically, a linear approximation model is adopted to build an unmanned aerial vehicle state equation, and the unmanned aerial vehicle state equation is as follows:
x(k+1)＝Ax(k)+Bu(k)
y(k)＝Cx(k)+Du(k)∈γ(k)
wherein: x (k) is a state vector of the drone; u (k) is the control input vector; y (k) =cx (k) +du (k) ∈γ (k) is a specific form of the state space model of the unmanned aerial vehicle; y (k) is the system output vector. C= [10 00], D is the spatial dimension.
Gamma (k) is the constraint that the drone needs to meet at time k,
the representation includes the x-axis coordinates, y-axis coordinates, and the flight speed of the unmanned aerial vehicle. u (k) =a k The input vector of the unmanned aerial vehicle at the moment k is represented, and the acceleration of the unmanned aerial vehicle is represented; Δt is the sampling period of the system.
Setting initial position coordinates of the unmanned aerial vehicle, setting target position coordinates of the unmanned aerial vehicle, and forming an initial path of the unmanned aerial vehicle between the initial position coordinates of the unmanned aerial vehicle and the target position coordinates through initialization of the unmanned aerial vehicle. Discretizing the initial path of the unmanned aerial vehicle to obtain the position P (t) of the unmanned aerial vehicle at the time t and the position of the next path point to be expressed as [ P (t+
1|t)|,P(t+2|t),...P(t+j|t)]，P(t)＝[p 1 (t),p 2 (t),...,p N (t)] T Wherein p is i (t) represents the position of the ith unmanned aerial vehicle at the moment t,n is the number of unmanned aerial vehicles, j is the size of the rolling window.
The objective function of the unmanned aerial vehicle comprises a path cost function F L ：
Not only the shortest flight distance at the current moment, but also the closest distance between the last subsequence and the target point is required to ensure overall optimization. Path cost function F L The expression of (2) is as follows:
wherein m is the number of current path subsequences, L i For the length of the ith path sequence, P m And P f The position of the current last sub-sequence and the position of the target point are represented, respectively.
Threat cost function F T ：
The threat cost function is to guide the drone to fly to a safer area. It is ensured that all the waypoints are outside the threat zone, and that the distance between each waypoint and the obstacle is as minimal as possible. The specific form is as follows:
wherein m is the number of current subsequences, n is the number of obstacles detected by the unmanned aerial vehicle,for the position coordinates of the ith sub-sequence, +.>And O is the position set of all the obstacles detected by the unmanned aerial vehicle, and is the position information of the jth obstacle.
S2: and predicting the position of the moving obstacle.
(1) Construction of state equation
It is assumed that the drone sensor device is able to directly detect the coordinate position of the dynamic obstacle and the speed of the two axes. The unmanned aerial vehicle encounters a sudden obstacle at the moment k, and the position information of the detected obstacle is (x) k ，y k ) The speed information is (v xk ，v yk ) Information of acceleration (a xk ，a yk ). And constructing a state equation based on the moving obstacle in the mode of uniform linear motion, uniform turning motion and oblique uniform simple harmonic motion according to the position, the speed and the acceleration information of the obstacle.
A. Uniform linear motion
If the obstacle does uniform linear motion in the task scene, acceleration is introduced into the obstacle with constant speed in consideration of the fact that an accurate model of the obstacle cannot be obtained and uncertainty factors exist in the environment, and a process error is reflected by adding small random fluctuation, so that a state equation is constructed.
The motion model of the obstacle in the x-axis direction can be expressed by the following formula:
where T is the sampling period. Likewise, the motion model of the obstacle in the y-axis direction is as follows:
according to the obstacle motion model of the x-axis direction and the y-axis direction, a state vector is constructed by integrating variables such as obstacle position, speed and the like, and the state vector is expressed as follows:
finally according to the state vector X of the last moment k-1 And combining the prediction errors to form a state equation of the dynamic obstacle. The specific form is as follows:
X k ＝φX k-1 +ΓW k-1
wherein Φ and Γ represent a system state transition matrix and a process noise distribution matrix, respectively, in a specific form as shown in the following formula, wherein W k-1 Is a process noise matrix.
If the obstacle does uniform linear motion in the three-dimensional task space, the three-dimensional space position, speed and other information of the obstacle are selected to form a state vector X k ＝[x k ,y k ,z k ,v xk ，v yk ，v zk ] T The state equation thereof can still be expressed as formula X k ＝φX k-1 +ΓW k-1 . Updating state transition matrix and process noise distribution matrix to be publicThe formula:
B. constant speed turning motion
Assuming that the obstacle makes uniform turning motion in the task scene, the motion model of the obstacle in the discrete time system can be expressed as:
where ω is the angular velocity of movement of the obstacle.
Constructing a state vector of an obstacle from the above as X k ＝[x k ,y k ,v xk ，v yk ] T Its obstacle state equation can still be expressed as X k ＝φX k-1 +ΓW k-1 . The system state transition matrix and the process noise distribution matrix are updated as follows:
C. oblique uniform velocity simple harmonic motion
Assuming that the obstacle makes oblique uniform-speed simple harmonic motion in the task scene, a motion model of the obstacle in the discrete-time system can be expressed as:
wherein A is o For simple harmonic motion amplitude, f o Is the simple harmonic motion frequency.
Constructing a state vector of the obstacle as X by integrating two-axis coordinates of the obstacle and speed information k ＝[x k ,y k ,v xk ，v yk ] T The obstacle state equation thereof can still be expressed as formula X k ＝φX k-1 +ΓW k-1 . The system state transition matrix and the process noise distribution matrix are updated as follows:
wherein D is 1 ＝A 0 (2πf 0 )cos(2πf 0 ·(k-1)),D 2 ＝A 0 (2πf 0 )sin(2πf 0 ·(k-1))。
In addition to the state equation, a system observation equation needs to be established, and the system observation equation together form a prediction model of the dynamic obstacle.
The system observation equation is mainly constructed by information detected by the sensor equipment, namely, an obstacle state equation and a measurement error. The specific form is as follows:
Z k ＝HX k +V k
wherein Z is k In order to observe the equation,the linear state transition matrix is used for converting the variable of the state matrix into a measurement matrix. Vk is measurement noise.
(2) Kalman filtering prediction of dynamic obstacles
Specifically, the obstacle position prediction is to obtain a state estimation value of k moment by using an obstacle state vector of k-1 moment, take the state estimation value and a system measurement value as system input, and obtain an optimal estimation value of the obstacle at k moment by Kalman filtering calculation and updating.
A. Position prediction
To obtain the obstacle state information at time k, the optimal estimated value of the obstacle at time k-1 is usedIn combination with the equation of state X k ＝φX k-1 +ΓW k-1 The position information of the next time is predicted as follows:
wherein the method comprises the steps ofIs the estimated value of the state of the current moment, which is obtained according to the optimal estimated value of the last moment, and is also called the prior estimated value of the obstacle state of the k moment,>is the best estimate at time k-1 (also known as a posterior estimate at time k-1).
B. Error analysis
However, uncertainty in the actual environment causes state estimation values to be inconsistent with the actual state estimation values, and covariance matrix and Kalman gain are adopted in Kalman filtering to define noise. The Kalman gain and covariance matrix are updated in each cycle to minimize the error. The covariance matrix is the correlation between measured data, and can be constructed to extract more useful information from the large amount of uncertain information, and M is used for the covariance matrix k The Kalman gain representation means the weight factors of the observed value and the estimated value in the system, and the larger the value is, the larger the weight of the observed value is represented, and the larger the weight of the estimated value of the system is in contrast.
The Kalman filter gain equation may be represented by a covariance matrix M k The specific form is as follows:
M k ＝ΦP k-1 Φ T +ΓQ k-1 Γ T
K k ＝M k H T [HM k H T +R k ] -1
P k ＝M k -K k HM k
wherein M is k Representing covariance matrix, Q k-1 A noise matrix representing a discrete process. K (K) k Kalman gain, H is unit measurement matrix, H at time k T Is the transposed matrix of H. R is R k Representing measurement noise for different systems. P (P) k Is a representation ofCovariance matrix of error over time. From the above equations, three equations are cyclically executable over time. From initial covariance error P 0 Can obtain variance matrix M k Then applied to equation K k ＝M k H T [HM k H T +R k ] -1 Obtaining a Kalman gain, and finally substituting the Kalman gain into a formula P k ＝M k -K k HM k Obtaining covariance error P of the next period k 。
C. Optimal estimation
State estimation valueAnd system measurement Z k As system input, constructing a dynamic obstacle optimal state estimation equation by a filter gain equation and the like>
Wherein,the estimated state value of the obstacle at time k is also the state value before prediction at time k+1. K (K) k [Z k -HX k/k-1 ]Referred to as the system residual, reflects the difference between the predicted estimated value and the actual measured value, meaning that the two agree if the residual is equal to 0.
The step A, B, C is continuously repeated to obtain the position information of the obstacle at each momentAnd transmitting the optimal estimated value to the unmanned aerial vehicle.
S3: taking the position coordinates, the speed and the acceleration of the unmanned aerial vehicle in the S1 as input; the optimal position coordinates of the obstacle in S2 and the obstacle are combinedThe speed of the obstacle is also taken as an input; meanwhile, a self-adaptive particle swarm algorithm is used for calculating a path cost function F L And threat cost function F T The method comprises the steps of carrying out a first treatment on the surface of the And iterating out the coordinates of the next position point of the unmanned aerial vehicle.
Will path cost function F L And threat cost function F T All serve as fitness functions of the adaptive particle swarm algorithm;
the position coordinates, the speed and the acceleration of the unmanned aerial vehicle are obtained by a state equation of the unmanned aerial vehicle: i.e.The unmanned aerial vehicle comprises an x-axis coordinate, a y-axis coordinate and a flying speed under the two coordinates. Acceleration u (k) =a k 。
Optimal position and speed of obstacle, estimated equation from optimal stateObtaining; the method comprises the following steps of: obstacle positionSpeed (v) xk ，v yk )；
Setting the iteration times of the self-adaptive particle swarm algorithm, wherein the iteration times are 200 times;
the weight of the self-adaptive particle swarm algorithm is set as follows:
ω＝ω 0 -hω h +sω s
ω 0 the termination and initial values are of size ω f And omega i The representation is performed. Omega h And omega s Proportional weights for the evolution rate factor and the aggregation factor; g and G max Respectively representing the current iteration number and the maximum cycle number. It can be seen that the ω value is large in the early iteration stage, making the algorithm focus on a large-scale search. And the particle aggregation degree becomes higher as the particle evolution speed becomes slower at the later stage of iterationOmega also gets smaller, which will make the algorithm more focused on local searches.
Setting an acceleration coefficient:
the acceleration coefficient is the ability of the particle to learn excellent individuals, and is also called a learning factor. The acceleration coefficient is composed of an individual learning factor c1 and a group learning factor c2, and the acceleration coefficient is used for enabling particles to quickly approach to the individual optimum and the global optimum. Most of the prior art documents set c1 and c2 to a fixed value of 2, and cannot continuously match the dynamic change of the moving obstacle, thereby reducing the optimizing performance.
Where T is the total iteration time, T is the current iteration time, c min And c max Is a constant, c max >c min >0. As is apparent from the formula, as the number of iterations increases, c is satisfied in the first half 1 >c 2 Thus search diversity is enhanced, which will facilitate quick acquisition of globally optimal solutions and avoid local minima. Satisfy c in the last half stage 1 <c 2 The method is beneficial to the algorithm to quickly converge to the same global optimal solution.
Setting an evolution speed:
apart from the evolutionary speed of the particles, their degree of aggregation will also influence the convergence speed of the algorithm. The aggregation degree of the particles is determined by the average value F of the optimal value F (gbest (G)) and the fitness function mean Is expressed as a ratio of (2). When the optimal value is closer to the average value, that is, s is closer to 1, it means that the aggregation degree of particles is higher; when the optimum value differs greatly from the average value, i.e. the closer s is to 0, theIndicating that the more dispersed the particles. Therefore, the algorithm can dynamically adjust the optimizing range of the algorithm through the value of s. Local searches should be focused when s is larger; when s is smaller, the algorithm should increase the optimizing range to improve the global searching power of the algorithm.
Setting the aggregation degree:
n is the iteration number, F (x i ) The x-coordinate of the position point for each drone.
Obtaining a path cost function F through iterative calculation L And threat cost function F T And the coordinate of the unmanned aerial vehicle at the minimum is the next position of the unmanned aerial vehicle.
With the above-described preferred embodiments according to the present invention as an illustration, the above-described descriptions can be used by persons skilled in the relevant art to make various changes and modifications without departing from the scope of the technical idea of the present invention. The technical scope of the present invention is not limited to the description, but must be determined according to the scope of claims.
Claims (6)
1. The unmanned aerial vehicle dynamic path planning method of the self-adaptive particle swarm algorithm is characterized by comprising the following steps:
s1: establishing a state equation of the unmanned aerial vehicle, and setting an objective function of the unmanned aerial vehicle;
s2: establishing an optimal state estimation equation of the dynamic barrier;
s21: establishing a state equation of the obstacle;
s22: establishing an observation equation;
s23: establishing an optimal state estimation equation of the dynamic barrier through a Kalman filtering gain equation;
s3: acquiring the position coordinates, acceleration and speed of the unmanned aerial vehicle from a state equation of the unmanned aerial vehicle; obtaining the position coordinates and the speed of the obstacle from the optimal state estimation equation; and taking the position coordinate, acceleration and speed of the unmanned aerial vehicle and the position coordinate and speed of the obstacle as inputs of a self-adaptive particle swarm algorithm, taking an objective function of the unmanned aerial vehicle as an fitness function of the self-adaptive particle swarm algorithm, and iteratively solving the position coordinate of the unmanned aerial vehicle at the next moment.
2. The unmanned aerial vehicle dynamic path planning method of claim 1, wherein in step S1, the state equation of the unmanned aerial vehicle is:
x(k+1)＝Ax(k)+Bu(k)
y(k)＝Cx(k)+Du(k)∈γ(k)
wherein x (k) is a state vector of the unmanned aerial vehicle; x (k) is a state vector of the drone; y (k) =cx (k) +du (k) ∈γ (k) is a specific form of the state space model of the unmanned aerial vehicle; y (k) is the system output vector; c= [10 00], D is the spatial dimension; setting initial position coordinates and target point coordinates of the unmanned aerial vehicle, initializing a path, discretizing the path,
the position P (t) of the unmanned plane at the time t and the position of the next route point are expressed as [ P (t+ 1|t) |, P (t+
2|t),...P(t+j|t)]，P(t)＝[p 1 (t),p 2 (t),...,p N (t)] T ；
Δt is the sampling period of the system;
the objective function includes:
path cost function F L The expression is as follows:
wherein m is the number of current path subsequences, L i Is the ithLength of the path sequence, P m And P f Respectively representing the position of the current last subsequence and the position of the target point;
threat cost function F T The expression is as follows:
wherein m is the number of current subsequences, n is the number of obstacles detected by the unmanned aerial vehicle,for the position coordinates of the ith sub-sequence, +.>And O is the position set of all the obstacles detected by the unmanned aerial vehicle, and is the position information of the jth obstacle.
3. The unmanned aerial vehicle dynamic path planning method of the adaptive particle swarm optimization according to claim 1, wherein in step S21, a state equation is built based on the moving obstacle in the uniform linear motion, the uniform turning motion and the oblique uniform simple harmonic motion mode;
the state equation of the dynamic obstacle is:
X k ＝φX k-1 +ΓW k-1
phi and Γ represent the system state transition matrix and the process noise distribution matrix, W, respectively k-1 Is a process noise matrix;
when the obstacle is in uniform linear motion in the three-dimensional task space:
when the obstacle is in uniform turning motion:
omega is the angular velocity of movement of the obstacle;
when the obstacle moves obliquely at uniform speed in a simple harmonic manner:
D 1 ＝A 0 (2πf 0 )cos(2πf 0 ·(k-1)),D 2 ＝A 0 (2πf 0 )sin(2πf 0 ·(k-1))；A o for simple harmonic motion amplitude, f o Is the simple harmonic motion frequency.
4. The unmanned aerial vehicle dynamic path planning method of claim 3, wherein in step S22, the system observation equation is:
Z k ＝HX k +V k
Z k in order to observe the equation,the linear state transition matrix is used for converting the variable of the state matrix into a measurement matrix; vk is measurement noise.
5. The unmanned aerial vehicle dynamic path planning method of claim 4, wherein in step S23, the best estimated value of the obstacle at time k-1 is used as the basisIn combination with the equation of state X k ＝φX k-1 +ΓW k-1 The position information of the next time is predicted as follows:
wherein the method comprises the steps ofThe current time state estimated value is obtained according to the optimal estimated value of the last time;
state estimation valueAnd system measurement Z k As system input, constructing a dynamic obstacle optimal state estimation equation by a filter gain equation and the like>
Wherein,the estimated state value of the obstacle at the time k is also the state value before prediction at the time k+1; k (K) k [Z k -HX k/k-1 ]Referred to as the system residual, reflects the difference between the predicted estimated value and the actual measured value, meaning that the two agree if the residual is equal to 0.
6. The unmanned aerial vehicle dynamic path planning method of claim 4, wherein in step S3, the weights of the adaptive particle swarm algorithm are:
ω＝ω 0 -hω h +sω s
ω 0 the termination and initial values are of size ω f And omega i Representing; omega h And omega s Proportional weights for the evolution rate factor and the aggregation factor; g and G max Respectively representing the current iteration times and the maximum cycle times;
acceleration coefficient:
evolution speed:
the aggregation degree of the particles is determined by the average value F of the optimal value F (gbest (G)) and the fitness function mean Is expressed by the ratio of (2);
degree of aggregation:
n is the iteration number, F (x i ) The x-coordinate of the position point for each drone.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202311442280.7A CN117369507A (en) | 2023-10-31 | 2023-10-31 | Unmanned aerial vehicle dynamic path planning method of self-adaptive particle swarm algorithm |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202311442280.7A CN117369507A (en) | 2023-10-31 | 2023-10-31 | Unmanned aerial vehicle dynamic path planning method of self-adaptive particle swarm algorithm |
Publications (1)
| Publication Number | Publication Date |
|---|---|
| CN117369507A true CN117369507A (en) | 2024-01-09 |
Family
ID=89396417
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202311442280.7A Pending CN117369507A (en) | 2023-10-31 | 2023-10-31 | Unmanned aerial vehicle dynamic path planning method of self-adaptive particle swarm algorithm |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN117369507A (en) |
Cited By (1)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN117631689A (en) * | 2024-01-25 | 2024-03-01 | 成都航空职业技术学院 | Unmanned aerial vehicle obstacle avoidance flight method |
Citations (5)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| WO2017041730A1 (en) * | 2015-09-09 | 2017-03-16 | 北京进化者机器人科技有限公司 | Method and system for navigating mobile robot to bypass obstacle |
| CN107168309A (en) * | 2017-05-02 | 2017-09-15 | 哈尔滨工程大学 | A kind of underwater multi-robot paths planning method of Behavior-based control |
| CN111505623A (en) * | 2020-04-24 | 2020-08-07 | 中南大学 | Method and system for detecting obstacle in driving process of unmanned vehicle and vehicle |
| CN116859979A (en) * | 2023-08-09 | 2023-10-10 | 深圳市华赛睿飞智能科技有限公司 | Unmanned aerial vehicle motion path planning method, unmanned aerial vehicle and storage medium |
| CN116880561A (en) * | 2023-07-31 | 2023-10-13 | 中科方寸知微(南京)科技有限公司 | Optimization method and system based on improved particle swarm unmanned aerial vehicle path planning safety enhancement |
-
2023
- 2023-10-31 CN CN202311442280.7A patent/CN117369507A/en active Pending
Patent Citations (5)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| WO2017041730A1 (en) * | 2015-09-09 | 2017-03-16 | 北京进化者机器人科技有限公司 | Method and system for navigating mobile robot to bypass obstacle |
| CN107168309A (en) * | 2017-05-02 | 2017-09-15 | 哈尔滨工程大学 | A kind of underwater multi-robot paths planning method of Behavior-based control |
| CN111505623A (en) * | 2020-04-24 | 2020-08-07 | 中南大学 | Method and system for detecting obstacle in driving process of unmanned vehicle and vehicle |
| CN116880561A (en) * | 2023-07-31 | 2023-10-13 | 中科方寸知微(南京)科技有限公司 | Optimization method and system based on improved particle swarm unmanned aerial vehicle path planning safety enhancement |
| CN116859979A (en) * | 2023-08-09 | 2023-10-10 | 深圳市华赛睿飞智能科技有限公司 | Unmanned aerial vehicle motion path planning method, unmanned aerial vehicle and storage medium |
Non-Patent Citations (3)
| Title |
|---|
| 杨承雨: "基于改进RRT算法的AUV路径规划研究", 中国优秀硕士学位论文全文数据库 (工程科技Ⅱ辑), no. 03, 15 March 2022 (2022-03-15), pages 13 - 22 * |
| 王喜宾 等: "基于优化支持向量机的个性化推荐研究", 30 April 2017, pages: 66 - 72 * |
| 王小璐 等: "基于适应度值优劣粒子群算法的无人机路径规划", 电子制作, 30 April 2022 (2022-04-30), pages 16 - 19 * |
Cited By (2)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN117631689A (en) * | 2024-01-25 | 2024-03-01 | 成都航空职业技术学院 | Unmanned aerial vehicle obstacle avoidance flight method |
| CN117631689B (en) * | 2024-01-25 | 2024-04-16 | 成都航空职业技术学院 | Unmanned aerial vehicle obstacle avoidance flight method |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| Scherer et al. | River mapping from a flying robot: state estimation, river detection, and obstacle mapping | |
| Li et al. | Deep sensor fusion between 2D laser scanner and IMU for mobile robot localization | |
| Yan et al. | Consensus formation tracking for multiple AUV systems using distributed bioinspired sliding mode control | |
| Penin et al. | Minimum-time trajectory planning under intermittent measurements | |
| CN112762957B (en) | Multi-sensor fusion-based environment modeling and path planning method | |
| Takleh et al. | A brief survey on SLAM methods in autonomous vehicle | |
| Raja et al. | PFIN: An efficient particle filter-based indoor navigation framework for UAVs | |
| CN117369507A (en) | Unmanned aerial vehicle dynamic path planning method of self-adaptive particle swarm algorithm | |
| Qadir et al. | Vision based neuro-fuzzy controller for a two axes gimbal system with small UAV | |
| Sun et al. | Aggressive quadrotor flight using curiosity-driven reinforcement learning | |
| Chen et al. | Cooperative tracking of vessel trajectories based on curved dynamic coordinates | |
| Cristofalo et al. | Vision-based control for fast 3-d reconstruction with an aerial robot | |
| Hewawasam et al. | Comparative study on object tracking algorithms for mobile robot navigation in gps-denied environment | |
| Yang et al. | DRL-based Path Planner and its Application in Real Quadrotor with LIDAR | |
| Hu et al. | Reinforcement learning for orientation estimation using inertial sensors with performance guarantee | |
| Deilamsalehy et al. | Fuzzy adaptive extended Kalman filter for robot 3D pose estimation | |
| de Moura Magalhães et al. | EKF on Lie groups for radar tracking using polar and Doppler measurements | |
| Guan et al. | A new integrated navigation system for the indoor unmanned aerial vehicles (UAVs) based on the neural network predictive compensation | |
| Wang et al. | Research on SLAM road sign observation based on particle filter | |
| Yoshimura et al. | Highlighted map for mobile robot localization and its generation based on reinforcement learning | |
| Taghizadeh et al. | Low-cost integrated INS/GNSS using adaptive H∞ Cubature Kalman Filter | |
| Lin et al. | Monocular vision-based localization and pose estimation with a nudged particle filter and ellipsoidal confidence tubes | |
| Hasanzade et al. | Deep reinforcement learning based aggressive collision avoidance with limited fov for unmanned aerial vehicles | |
| EP0580140B1 (en) | Covert ranging method and system | |
| González-Rodríguez et al. | Uncertainty-Aware autonomous mobile robot navigation with deep reinforcement learning |
Legal Events
| Date | Code | Title | Description |
|---|---|---|---|
| PB01 | Publication | ||
| PB01 | Publication | ||
| SE01 | Entry into force of request for substantive examination | ||
| SE01 | Entry into force of request for substantive examination |