A UUV route planning method based on cross-entropy method

Abstract

The application belongs to the technical field of underwater unmanned vehicle route planning, and particularly relates to a UUV route planning method based on a cross-entropy method, which comprises the following steps: step 1), each feasible and optimized path of a UUV is divided into m connected line segments to form a parameter set; step 2), each feasible and optimized path of the UUV corresponds to a parameter set, the path cost corresponding to each feasible path is calculated, and each feasible path is sorted from large to small, 10% to 20% of the superior set is selected as an optimized set; step 3), the mean and variance of each group of parameters in the optimized set are calculated as the mean and variance of the next iteration sampling, and the updated mean and variance of the m groups of parameters are obtained to obtain the optimal solution of this time; and step 4), for the optimal solution of this time, N feasible paths are obtained again, and steps 1) to 3) are repeated, and through continuous iteration, an optimized feasible path of the UUV is obtained, and the optimized feasible path is taken as the final optimal solution.

Description

TECHNICAL FIELD

[0001] The application belongs to the technical field of underwater unmanned vehicle route planning, and particularly relates to a UUV route planning method based on a cross-entropy method. BACKGROUND

[0002] Exploring the cognitive ocean is a prerequisite for the development and utilization and protection of the ocean, and an unmanned underwater vehicle (UUV) provides important support for ocean environment exploration and military reconnaissance. Route planning is one of the key technologies of the UUV and is an important embodiment of the autonomous ability of the UUV. Route planning refers to planning a collision-free path from a starting point to a destination while bypassing all obstacles in a known obstacle environment.

[0003] UUV docking control technology is a key technology for the UUV to select the most energy-saving sailing route, perform autonomous obstacle avoidance, return to the scheduled sea area, successfully dock with the mother ship, and perform energy replenishment, underwater data upload and task download when the task is completed or the energy is about to be depleted. Therefore, UUV docking control should complete path planning under the premise of obstacle avoidance capability to ensure the safe return of the UUV and even successful docking, and ultimately realize a series of recovery actions such as return, approach and docking control.

[0004] The path planning of UUV obstacle avoidance docking technology is an important link of the cruising ability and docking success of the UUV. The UUV calculates the optimal path from the starting point to the docking point according to the obstacles existing in the actual water area, combines the UUV performance, sailing conditions and energy reserves, and usually requires the shortest path distance or the lowest energy consumption.

[0005] I. Path planning method

[0006] Path planning is one of the important directions of intelligent control, which tries to solve the problem of searching for a feasible path from an initial state to a target state under certain constraints and seeking an optimal solution.

[0007] Early path planning methods require the participation of technical personnel to plan the route based on environmental information. Due to limited information acquisition and human factors, it often cannot meet the actual obstacle avoidance recovery task requirements. With the gradual development of computer technology, the intelligence and autonomy of the UUV are continuously improved, and the route planning is changed from manual participation to autonomous planning.

[0008] In recent years, machine learning as a current research hotspot, its use of scenarios continue to expand, in the UUV real-time online autonomous planning navigation has made great achievements. As a path planning difficulties, online path planning has been widely concerned. UUV in the path planning is for the forward direction of the unknown obstacles to take certain collision avoidance strategy to adjust the UUV motion state in time to ensure the safety of navigation. In practical applications, the difficulty of in-flight path planning is to avoid collision with moving obstacles in time. First, the obstacle information collected by the forward-looking sonar needs to be processed, the motion state of the obstacle needs to be fitted and estimated, and the threat level needs to be determined. Then, select the appropriate planning algorithm for collision avoidance planning. The velocity vector synthesis method performs well in real-time online planning and can meet the collision avoidance requirements of obstacles with different motion states.

[0009] In robotics, the in-flight path planning uses a sampling based method, such as randomized potential planer (RPP), Probabilistic Roadmap Method (PRM), Rapidly-exploring Random Trees (RRT), etc.

[0010] Genetic algorithm

[0011] The genetic algorithm has been proposed for the longest time and has been widely used in path optimization problems due to its superior global search ability. Scholars have continuously combined it with other algorithms to achieve better results.

[0012] Artificial potential field method

[0013] In recent years, the potential field method has been widely used in path planning. Although the artificial potential field method is excellent in handling static obstacles, it is not good for planning mobile targets and is prone to oscillation or falling into local minimum, limiting its practical application. Therefore, it is easy to fall into local minimum points and is not effective in relatively narrow areas.

[0014] PRM Probabilistic Roadmap Method

[0015] Probabilistic Roadmap Method (PRM) is a path planning algorithm based on graph structure. The algorithm is divided into two stages: learning stage and query stage. PRM first scatters some points in the map, then connects these points into a graph, and finally finds a path from the starting point to the ending point on the graph. The advantage of PRM lies in its probability completeness and smaller computational complexity. However, this method needs to solve the boundary value problem and is not focused on generating paths during the construction of the graph, so it is not efficient.

[0016] RRT Rapidly-exploring Random Tree

[0017] Similar to PRM algorithm, RRT algorithm is also to build a directed graph on the known map by sampling, and then find the relatively optimal path by search method. The principle of the algorithm is to search and sample from a certain point. The starting point is taken as the root node, and a position point is randomly generated in the workspace. Find the nearest position point of the position point on the position tree, and judge whether they collide with the obstacle. If not, add the position point to the position tree. If the target point is reached, trace back to obtain the path. This algorithm performs well in planning problems with geometric constraints. However, the path obtained by this method is not smooth, and subsequent processing is needed to obtain a smooth path, which cannot be directly used in UUV path planning.

[0018] Virtual potential field based method

[0019] The artificial potential field method establishes a virtual potential field, establishes a repulsive force field between the obstacle and the UUV, and establishes an attractive force field between the target point and the UUV. The resultant force of the repulsive force and the attractive force is regarded as the control force of the UUV, which controls the UUV to avoid obstacles and reach the target position. The advantage of the potential field method is that the algorithm is simple and efficient; however, this method has the disadvantages of local minimum, large path swing, and difficulty in searching the path of similar obstacles.

[0020] Cross Entropy method (CEM)

[0021] Cross Entropy method is a Monte Carlo method, and its core content is a search algorithm based on parameter perturbation. Its process is a parameter optimization process. Some reasonable perturbations are given to the parameter space, and then the better data set is searched and selected among these perturbations, and the cross entropy is used to guide the update of the reference parameter of the importance function. Let the perturbation direction tend to the target optimization direction. Like many supervised learning algorithms, CEM uses cross entropy to guide the update direction, but this method is a gradients free method, and there is an optimization problem from the perspective of evolution.

[0022] II. Path tracking characteristics of underactuated system

[0023] Underactuated system is a typical system in which the number of inputs is less than the number of quantities to be controlled, that is, a class of nonlinear systems in which the number of independent control variables is less than the number of degrees of freedom. From the perspective of control theory, most UUVs in real-world applications are underactuated systems, which means that their degrees of freedom are more than the dimensions of the control input. Only the roll angular velocity, yaw angular velocity, and pitch angular velocity can be used to directly control.

[0024] The autonomous underwater vehicle is an under-actuated system, and in the path tracking process, the expected curve is required to be smooth, that is, the navigation trajectory is a smooth curve. However, the calculation result obtained by using the common algorithm is a series of path point coordinates. Such a reference path is usually "jagged" and does not conform to the navigation trajectory performance of the UUV under-actuated system in reality. Therefore, further smoothing processing is required, which increases the calculation amount and system complexity. SUMMARY

[0025] In order to solve the above-mentioned defects existing in the prior art, the present application provides a UUV route planning method based on a cross-entropy method, which comprises the following steps:

[0026] Step 1) dividing each UUV feasible optimization path into m mutually connected line segments to obtain m sets of parameter sets composed of 2m parameters, and forming a parameter set; wherein each set of parameter sets corresponds to a line segment path;

[0027] Step 2) each UUV feasible optimization path corresponds to a parameter set, and Gaussian sampling is performed according to the mean and variance to obtain N feasible paths of the UUV and N parameter sets, and the path cost corresponding to each feasible path is calculated; the N path costs obtained above are sorted from large to small to obtain a dominant set; 10%-20% of the dominant set is selected as an optimization set;

[0028] Step 3) calculating the mean and variance of each set of parameters in the optimization set as the mean and variance of the next iteration sampling, and obtaining the updated mean and variance of the m sets of parameters, and connecting the m line segment paths corresponding to the updated m sets of parameters to form the optimization path of this time as the optimal solution of this time;

[0029] Step 4) obtaining N feasible paths according to the optimal solution of this time and the updated mean and variance of the m sets of parameters, and repeating steps 1)-3) through continuous iteration until convergence is reached to obtain an optimized UUV feasible path, which is taken as the final optimal solution, thereby realizing optimization of the UUV route path.

[0030] As one of the improvements of the above technical solution, the step 1) specifically comprises:

[0031] dividing a certain feasible path of the UUV into m mutually connected line segment paths;

[0032] assuming that each line segment path is described by a heading angle velocity and a duration as an mth set of parameter row vectors corresponding to an mth line segment path, denoted as an mth set of parameters (w m ,τ m );

[0033] wherein w m is the course angular velocity of the mth line segment path; τ m is the duration of the mth line segment path;

[0034] According to the m line segment paths, each line segment path corresponds to a set of parameters, m sets of parameters composed of 2m parameters are obtained, and the m sets of parameters are taken as a parameter set W = ((w1, τ1), …, (w m , τ m )) ∈ R 2m ;

[0035] wherein R 2m is a 2m-dimensional real space.

[0036] As one of the improvements of the above technical solutions, the step 2) specifically comprises:

[0037] Obtaining N feasible paths of the UUV, calculating a cost function corresponding to each path according to path planning problems in different situations; if the end point is considered in the path planning problem, the following expression is used to calculate the cost function corresponding to each path:

[0038]

[0039] wherein J n is the path cost corresponding to the nth feasible path; t f is the total time required to pass through the nth feasible path, which is a to-be-optimized variable;

[0040] (x f , y f ) is the position of the actual end point of the nth feasible path;

[0041] (x d , y d ) is the position of the expected end point of the nth feasible path;

[0042] λ and γ are both constants;

[0043] If the end point and the course angle are considered in the path planning problem, the following expression is used to calculate the cost function corresponding to each path:

[0044]

[0045] wherein J n is the path cost corresponding to the nth feasible path; t f is the total time required to pass through the nth feasible path;

[0046] θ f is the course angle of the actual end point of the nth feasible path; θ da heading angle of the expected end point of the nth feasible path;

[0047] (x i ,y m ) is the position of the actual end point of the nth feasible path;

[0048] (x m ,y m ) is the position of the expected end point of the nth feasible path;

[0049] λ1, λ2 and γ are all constants;

[0050] Based on the above formula, the path cost corresponding to each feasible path is calculated, N path costs are obtained, the N path costs obtained above are sorted from small to large, and the following is met Wherein, i = 1, 2, …, N; the advantage set is obtained Wherein, Wherein, γ1 is the proportion of 0.1-0.2 in the selected advantage set; 10%-20% in the advantage set is selected as the optimization set.

[0051] As one of the improvements of the above technical solutions, the step 3) specifically comprises:

[0052] The optimization set includes multiple UUV feasible optimization paths, each UUV feasible optimization path is divided into m connected line segment paths; each line segment path corresponds to a set of parameters;

[0053] Suppose the first path cost in the optimization set is corresponding to the first UUV feasible optimization path,

[0054] Can be regarded as the i-th sampling of the first set of parameters (on the i-th path);

[0055] The set of parameters obeys the Gaussian distribution N(μ1, Σ1); wherein, μ1 is the mean of the first set of parameters; Σ1 is the variance of the first set of parameters; μ1∈R 2 , Σ1∈R 2×2 ; i = 1, …, m;

[0056] The calculation method is: the mean and variance of the first set of parameters of the ρ UUV feasible optimization paths are calculated to obtain

[0057]

[0058]

[0059] Repeat the process to obtain the updated mean and variance μ i,Σ i , i = 1, 2, … m, sampling for next iteration, and connecting the m line segment paths corresponding to the updated m groups of parameters to form the optimized path of this time, as the optimal solution of this time, and recording the optimal solution and the corresponding path cost of this time;

[0060]

[0061] The present application has the beneficial effects compared with the prior art:

[0062] 1. The method of the present application adopts the Cross-Entropy method (CEM) commonly used in machine learning to perform optimal path planning on multiple paths of the UUV; the CEM method can conveniently add various constraint conditions, such as terminal constraint, yaw angle constraint, safety constraint, maneuvering ability constraint, etc., and thus can be used for UUV path design, dynamic obstacle avoidance, and UUV docking under complex conditions, etc.;

[0063] 2. In the actual application environment of UUV navigation, if the path planning of robotics adopts a sampling based method, there is path non-smoothness, i.e. the path is composed of straight line segments, and a post-processing is needed to obtain a relatively smooth path, and there is a jagged polyline;

[0064] 3. The path trajectory obtained by the method of the present application is a smooth curve, and there is no jagged polyline, which meets the smooth path requirement of path tracking of the under-actuated system, so that a secondary smoothing processing is not needed, and the method can be directly used for the control strategy of the under-actuated system to complete path tracking control. BRIEF DESCRIPTION OF DRAWINGS

[0065] Figure 1 is a flow chart of a UUV path planning method based on the Cross-Entropy method of the present application;

[0066] Figure 2 is a schematic diagram of calculating the path cost of dynamic obstacle avoidance in the UUV path planning method based on the Cross-Entropy method of the present application;

[0067] Figure 3 is a schematic diagram of calculating the path cost of dynamic obstacle avoidance in the UUV path planning method based on the Cross-Entropy method of the present application;

[0068] Figure 4a is a position schematic diagram of all paths when the CEM method is used for the first iteration under the static obstacle condition in the UUV path planning method based on the Cross-Entropy method of the present application;

[0069] Figure 4bis a position schematic view of all paths in the 10th iteration of a UUV path planning method based on a cross-entropy method of the present application under static obstacle condition using the CEM method;

[0070] Figure 4c is a position schematic view of all paths in the 20th iteration of a UUV path planning method based on a cross-entropy method of the present application under static obstacle condition using the CEM method;

[0071] Figure 5a is a position schematic view of UUV and moving object at the 4th second under dynamic obstacle condition;

[0072] Figure 5b is a position schematic view of UUV and moving object at the 8th second under dynamic obstacle condition;

[0073] Figure 5c is a position schematic view of UUV and moving object at the 18th second under dynamic obstacle condition;

[0074] Figure 5d is a position schematic view of UUV and moving object at the 26th second under dynamic obstacle condition;

[0075] Figure 5e is a schematic view of the relationship between iteration number and cost function J when iteration is performed using the CEM method;

[0076] Figure 6 is a schematic view of the principle of updating sampling mean and variance using an optimization set composed of 10-20% selected from the elite set. DETAILED DESCRIPTION

[0077] The present application will be further described in conjunction with the drawings and examples.

[0078] The present application provides a UUV path planning method based on a cross-entropy method, which is an approximate optimal path planning method involving satisfying terminal constraints and time-varying state constraints. Specifically, the method uses the Cross-Entropy method (CEM) commonly used in machine learning to calculate the optimal path planning problem of UUV. Moreover, relevant constraint conditions such as terminal constraints, safety constraints, and maneuvering ability constraints are added in the method. The calculation results of the method can be used for UUV trajectory design, dynamic obstacle avoidance, and UUV docking control under complex conditions.

[0079] As shown in Figure 1 , the method comprises:

[0080] Step 1) dividing each feasible optimization path of the UUV into m connected line segments to obtain m sets of parameter sets composed of 2m parameters, and forming a parameter set and setting an initial mean and variance for the parameter set; wherein each set of parameter sets corresponds to a line segment path;

[0081] Specifically, a certain feasible path of the UUV is divided into m connected line segment paths;

[0082] Assuming that each line segment path is described by a heading angle velocity and a duration, as the mth set of parameters corresponding to the mth line segment path, denoted as the mth set of parameters (w m ,τ m );

[0083] wherein w m is the heading angle velocity of the mth line segment path; τ m is the duration of the mth line segment path;

[0084] According to the m line segment paths, each line segment path corresponds to a set of parameters, obtaining m sets of parameter sets composed of 2m parameters, and taking it as a parameter set W = ((w1, τ1), …, (w m ,τ m )) ∈ R 2m ;

[0085] wherein R 2m is a 2m-dimensional real number space.

[0086] Step 2) each feasible optimization path of the UUV corresponds to a parameter set, according to the mean and variance, Gaussian sampling is performed to obtain N feasible paths of the UUV and N parameter sets, and the path cost corresponding to each feasible path is calculated; the N path costs obtained above are sorted from large to small to obtain a dominant set; 10%-20% in the dominant set is selected as an optimization set;

[0087] Specifically, N feasible paths of the UUV are obtained, and the cost function corresponding to each path is calculated. The cost function can be selected according to the principle of minimum energy, the principle of minimum time, and the terminal constraint condition. In particular, if the path planning problem is considered, i.e. the starting point and the ending point are given, the path problem can be calculated by using the following cost function: (only one feasible cost function selection principle is listed in this paper, and other cost function selection methods are not excluded).

[0088] If the path planning problem at the end point is considered, the cost function corresponding to each path is calculated by using the following expression:

[0089]

[0090] wherein J nis the path cost corresponding to the nth feasible path; t f is the total time required to pass through the nth feasible path, and is the variable to be optimized;

[0091] (x f ,y f ) is the position of the actual end point of the nth feasible path;

[0092] (x d ,y d ) is the position of the desired end point of the nth feasible path;

[0093] λ and γ are constants;

[0094] If the UUV underwater docking problem is considered, that is, the UUV has not only the start point and end point constraints, but also the terminal heading angle constraint, the cost function can be selected as follows: (This paper only lists one feasible cost function selection principle, and does not exclude other cost function selection methods).

[0095] If the path planning problem of the end point and the heading angle is considered, the following expression is used to calculate the cost function corresponding to each path:

[0096]

[0097] wherein J n is the path cost corresponding to the nth feasible path; t f is the total time required to pass through the nth feasible path, and is the variable to be optimized;

[0098] θ f is the heading angle of the actual end point of the nth feasible path; θ d is the heading angle of the desired end point of the nth feasible path;

[0099] (x f ,y f ) is the position of the actual end point of the nth feasible path;

[0100] (x d ,y d ) is the position of the desired end point of the nth feasible path;

[0101] λ1, λ2 and γ are constants, which can be selected according to the actual situation;

[0102] Based on the above formula, the path cost corresponding to each feasible path is calculated, N path costs are obtained, and the N path costs obtained above are sorted from small to large, satisfying wherein i = 1, 2, …, N; the advantage set is obtained wherein, Wherein, γ1 is the proportion of 0.1-0.2 in the selected dominant set; 10%-20% of the dominant set is selected as the optimization set.

[0103] The position coordinates of the UUV are described using x(t), y(t), θ(t) is the heading angle, u(t) is the heading angle velocity, v is the navigation velocity, and the independent variable t represents time, which is omitted hereinafter without causing ambiguity.

[0104] The dynamic control model of the UUV is calculated as formula (1):

[0105]

[0106] Each feasible path of the UUV is obtained by the above dynamic control model.

[0107] Step 3) For each set of parameters in the optimization set, the mean and variance are calculated as the mean and variance of the next iteration sampling, and the updated mean and variance corresponding to the m sets of parameters are obtained, and the m line segment paths corresponding to the m sets of parameters are connected to form the optimized path of this time as the optimal solution of this time;

[0108] Specifically, as shown in Figure 6 The optimization set includes multiple feasible optimized paths of the UUV, and each feasible optimized path of the UUV is divided into m connected line segment paths; each line segment path corresponds to a set of parameters;

[0109] Suppose the first path cost in the optimization set is corresponding to the first feasible optimized path of the UUV,

[0110] The first set of parameters can be regarded as the i-th sampling (on the i-th path) of the first set of parameters;

[0111] The set of parameters obeys the Gaussian distribution N(μ1,Σ1); wherein, μ1 is the mean of the first set of parameters; Σ1 is the variance of the first set of parameters; μ1∈R 2 , Σ1∈R 2×2 ; i = 1, …, m;

[0112] The calculation method is: the mean and variance of the first set of parameters of the ρ feasible optimized paths of the UUV are calculated to obtain

[0113]

[0114]

[0115] The process is repeated to obtain the updated mean and variance μ i ,Σi , i = 1, 2, … m, the sampling for the next iteration, and connecting the m line segment paths corresponding to the updated m groups of parameters to form the optimized path of this time, as the optimal solution of this time, and recording the optimal solution of this time and the corresponding path cost;

[0116]

[0117] Step 4) For the obtained optimal solution of this time, and the updated mean and variance corresponding to the m groups of parameters, N feasible paths are obtained again, and steps 1)-3) are repeated, and through continuous iteration, an optimized UUV feasible path is obtained, and it is taken as the final optimal solution, and the optimization of the UUV path is realized.

[0118] The following is the principle of the method of the application, which is as follows:

[0119] The method of the application adopts the CEM method commonly used in machine learning to calculate the optimal path planning problem of the UUV, can conveniently add various constraint conditions, for example, terminal constraint, safety constraint, maneuvering ability constraint, etc., and thus can be used for UUV track design, dynamic obstacle avoidance, and UUV docking under complex conditions.

[0120] Among them, the CEM method is a Monte Carlo optimization method based on importance sampling, which can be used to solve nonlinear problems globally, and when certain conditions are met, the convergence of the solution can be guaranteed.

[0121] 1. The specific implementation process of the CEM method is as follows:

[0122] Step 1-1) system modeling;

[0123] Step 1-2) assuming that the parameter W belongs to a Gaussian distribution, an n-dimensional vector μ and an n-dimensional vector σ are randomly set 2 , corresponding to each dimension of W;

[0124] Step 1-3) sampling m groups of parameters [p1, p2, p3... p 2 ] with μ and σ m as the mean and variance;

[0125] Step 1-4) calculating the reward (i.e. path cost) of each group of parameters p i ;

[0126] Step 1-5) selecting the k groups of parameters with the highest reward (k < m), calculating the mean μ and variance σ 2 of the parameters, and updating μ and σ 2 to the just calculated mean and variance.

[0127] Step 1-6) If convergence, return the group p with the maximum reward max ;

[0128] Otherwise, repeat step 1-3) to step 1-6).

[0129] 2. The method of the present application has the following specific features:

[0130] In the actual application environment of UUV navigation, if the path planning of robot learning uses a sampling based method, there is path non-smoothness, i.e. the path is composed of straight line segments, and a post-processing is needed to obtain a smoother path. The CEM algorithm can avoid the above problems, and has the following specific features:

[0131] (1) Structuring of path parameters

[0132] The feasible path is segmented, and the path parameters are structured, which is easy to describe and calculate.

[0133] (2) Few related parameters

[0134] In the calculation process, the fewer the effective parameters, the faster the optimization speed. The algorithm only needs to use a small number of effective parameters to represent each segment of the sub-trajectory. Each path can have at least two parameters, such as time (length) and curvature.

[0135] (3) Numerical solution of calculation results

[0136] Since the calculation result is not an analytical solution, the entire calculation process does not have a specific function form, but only needs to satisfy certain constraint conditions, and the numerical solution is calculated approximately. Therefore, the algorithm can cope with various constraint conditions, such as line of sight angle, collision avoidance constraint, terminal constraint, and navigation time constraint (regression in time order).

[0137] (4) Comply with the smooth path tracking requirement of underactuated system

[0138] The path trajectory calculated by the algorithm is a smooth curve, and there is no jagged polyline, so that secondary smoothing processing is not needed, and the path can be directly used for control strategy of underactuated system to complete path tracking control.

[0139] Problem description

[0140] The position coordinates of the UUV are described by x(t), y(t), θ(t) is the heading angle, u(t) is the heading angle velocity, v is the navigation speed, and the independent variable t represents time. In the case of not causing ambiguity, t is omitted in the following.

[0141] It is assumed that the vehicle obeys the following dynamic control model for movement

[0142]

[0143] 1) take u as the control variable;

[0144] 2) solve the objective function to find the initial position (x0, y0) and the desired terminal position (xT, yT); under the constraint of equation (1) and the constraint of control variable u, use CEM to optimize the UUV path. The feasible path of each UUV in this application is obtained by the above dynamic control model (equation 1). d d The path of the vehicle can be determined by the angular rate at each time and the sailing time. Therefore, we can parameterize the path as a combination of angular rate ω and time τ.

[0145] It can be seen that the angular rate of the vehicle at each time and the sailing time can determine its path. Therefore, we can parameterize the path as a combination of angular rate ω and time τ.

[0146] Algorithm description

[0147] The optimization set includes multiple UUV feasible optimization paths, each UUV feasible optimization path is divided into m connected line segment paths; each line segment path corresponds to a set of parameters;

[0148] The first path cost in the optimization set corresponding to the first UUV feasible optimization path,

[0149] which can be regarded as the i-th sample of the first set of parameters (on the i-th path);

[0150] The set of parameters obeys Gaussian distribution N(μ1,Σ1); where μ1 is the mean of the parameters; Σ1 is the variance; μ1∈R 2 , Σ1∈R 2×2 ; i = 1, …, m; the calculation method is: take the mean and variance of the first set of parameters of ρ paths to obtain

[0151]

[0152]

[0153] Repeat the process to obtain the updated mean and variance μ i , Σ i of the first, second, …, m sets of parameters, i = 1, 2, … m, for the sampling of the next iteration, and record the optimal solution and the corresponding path cost of this time;

[0154]

[0155] The specific steps are as follows:

[0156] ​1) The feasible path of UUV is divided into m segments, which can be circular arc or straight line segment.

[0157] 2) The angular velocity and duration of each segment are represented by w, τ, respectively. Thus, a path can be represented by 2m parameters (w1, τ1, …, w m ,τ m )∈R 2m .

[0158] 3) Let each pair of parameters (w i ,τ i ) T obeys Gaussian distribution N(μ i ,Σ i ), where μ i ∈R 2 , Σ i ∈R 2×2 are its mean and variance, respectively, μ i ~ U(-a, a), Σ i is a positive definite matrix, and can be large.

[0159] 4) Calculate all coordinate points x(t), y(t) and θ(t) of each segment path using formula (1), and calculate the path cost function J according to the path; (for example, see 3. Selection of cost function)

[0160] 5) According to the CEM method, iterative optimization is completed, and the path optimization is completed. (For details, see 1. Specific implementation process of CEM method above)

[0161] 3. Selection principle of cost function J(reward)

[0162] The cost function is selected according to the following principles (only one feasible selection principle of the cost function is listed in this paper, and other selection methods of the cost function are not excluded). The meaning is: considering the length of each path segment, the terminal error and the energy consumed, the path is scored comprehensively.

[0163] (1) Simple path planning problem considering only the coordinates of the starting point and the ending point, as shown in Figure 2 , where static obstacle avoidance means that the obstacle is static; dynamic obstacle avoidance means that the obstacle can move.

[0164] Static obstacle avoidance path planning problem needs to meet the coordinate restriction of the terminal.

[0165] Get N feasible paths of UUV, calculate the cost function corresponding to each path. The cost function definition can be selected according to the energy minimization principle, the time minimization principle, and the terminal constraint condition, etc. In particular, if the path planning problem is considered, i.e. the starting point and the ending point are given, the path calculation problem can adopt the following cost function:

[0166]

[0167] where J n is the cost of the nth path; t f is the time required for the corresponding path, which is the variable to be optimized;

[0168] (x f ,y f ) is the position of the actual end point of the path;

[0169] (x d ,y d ) is the position of the desired end point of the path;

[0170] λ and γ are both constants;

[0171] (2) UUV docking problem in the presence of dynamic obstacles, as shown in Figure 3

[0172] The docking problem not only needs to meet the coordinate constraints of the terminal, but also needs to meet the heading requirements of the terminal because the UUV must autonomously navigate into a conical frame during docking. Considering the presence of moving obstacles in the outside world, the docking difficulty is greatly increased.

[0173] If the underwater docking problem of the UUV is considered, that is, the UUV has not only the constraints of the start point and the end point, but also the constraint of the terminal heading angle, the cost function can be selected as follows:

[0174]

[0175] where J n is the cost of the nth path; t f is the total time required for the path, which is the variable to be optimized;

[0176] θ f is the heading angle of the actual end point of the path; θ d is the heading angle of the desired end point of the path;

[0177] (x f ,y f ) is the position of the actual end point of the path;

[0178] (x d ,y d ) is the position of the desired end point of the path;

[0179] λ1, λ2 and γ are all constants, which can be selected according to the actual situation;

[0180] ​In which, (2) not only dynamic obstacle avoidance is considered, but also underwater docking problem, that is, in addition to coordinate constraints, UUV end point has heading constraints.

[0181] Simulation verification

[0182] (1) UUV path planning in static obstacle case, as shown in Figures 4a-4c ,

[0183] Figure 4a , 4b and 4c respectively describe all paths at the first, tenth and twentieth iterations. It can be seen that at the beginning of iteration, due to the random selection of path parameters (time and angular velocity), the discreteness of the path is large, and the N paths are scattered. With the increase of iteration times, the variance of path parameters gradually decreases, which means that the randomness of the path gradually decreases. Finally, at the 20th iteration, the variance of the path parameters is very small, and the optimal path has certainty. Among them, as shown in Figure 4a , 4b and 4c, the starting point of the UUV path is at the left (-5, 7.5) position "*" A, and the expected end point is at the right (20, 15) position "circle (°)" B;

[0184] (2) UUV docking control problem in the presence of dynamic obstacles, as shown in Figures 5a-5e ,

[0185] The UUV starts from the left starting point C, and the docking point D is at the upper right. The heading when docking is (-10deg, 10deg) to ensure that the UUV can perform the docking frame. At the same time, there are four moving objects in the area, and their boundaries are represented by circles. The size and direction of the arrow represent the speed and direction of the object. The horizontal and vertical coordinates in the figure are in meters. Figure 5a , 5b , 5c, 5d respectively describe the positions of UUV and moving objects at 4, 8, 18 and 26 seconds, in which the trajectory of UUV is specially represented by solid line in order to facilitate analysis. It can be seen that the UUV completes the docking task at the 28th second, and there is no collision with any moving object in the way.

[0186] Figure 5e Describe the change of cost function (the minimum value of N path cost functions) with the number of iterations in the iteration process. At the beginning, due to the random selection of path parameters, the cost function is large (160). At the end of iteration, the cost function converges to about 28. After iteration converges, an optimized path is formed as the final optimal solution.

[0187] Finally, it should be noted that the above examples are merely used to illustrate the technical solutions of the present application but not to limit. Although the present application is explained in detail with reference to the examples, those skilled in the art should understand that the technical solutions of the present application can be modified or replaced equivalently without departing from the spirit and scope of the present application, and all of them should be covered in the scope of the claims of the present application.

Claims

1. A UUV path planning method based on a cross-entropy method, the method comprising: Step 1) dividing each UUV feasible optimization path into m interconnected line segments to obtain m sets of parameter sets composed of 2m parameters, forming a parameter set, and setting initial mean and variance for the parameter set; wherein each set of parameter sets corresponds to a line segment path; Step 2) each UUV feasible optimization path corresponds to a parameter set, Gaussian sampling is performed according to the mean and variance, N feasible paths of the UUV and N parameter sets are obtained, and the path cost corresponding to each feasible path is calculated; the N path costs obtained above are sorted in descending order to obtain a dominant set; 10%-20% of the dominant set is selected as an optimization set; Step 3) calculating the mean and variance of each set of parameters in the optimization set as the mean and variance of the next iteration sampling, and obtaining the updated mean and variance of the m sets of parameters, and connecting the m line segment paths corresponding to the m sets of updated parameters to form the optimization path of this time as the optimal solution of this time; Step 4) obtaining N feasible paths according to the optimal solution of this time and the updated mean and variance of the m sets of parameters, and repeating steps 1)-3) until convergence is reached to obtain an optimized UUV feasible path, which is taken as the final optimal solution to realize optimization of the UUV path; The step 1) specifically comprises: dividing a certain feasible path of the UUV into m interconnected line segment paths; Assume each line segment path is described by a heading angle velocity and a duration, as the mth set of parameters corresponding to the mth line segment path, denoted as the mth set of parameter vectors (w m ,τ m ) wherein w m is the yaw rate of the mth segment path; τ m is the duration of the mth segment path; According to the m line segment paths, each line segment path corresponds to a set of parameter sets, m sets of parameter sets composed of 2m parameters are obtained, and the m sets of parameter sets are taken as a parameter set W = ((w1, τ1), …, (wm, τm) ) ∈ Rm×2m. m , τ m )) ∈ Rm×2m. 2m ; wherein R 2m is a 2m-dimensional real space.

2. The UUV path planning method based on cross-entropy method of claim 1, wherein, The step 2) specifically comprises: obtaining N feasible paths of the UUV, and calculating the cost function corresponding to each path according to different path planning problems; if the end point is considered in the path planning problem, the cost function corresponding to each path is calculated by using the following expression: wherein J n is the path cost corresponding to the nth feasible path; t f is the total time required to pass through the nth feasible path, and is the variable to be optimized. (x f ,y f ) is the position of the actual end point of the nth feasible path. (x d ,y d ) is the position of the expected end point of the nth feasible path; λ and γ are both constants; if the end point and the heading angle are considered in the path planning problem, the cost function corresponding to each path is calculated by using the following expression: wherein J n is the path cost corresponding to the nth feasible path; t f is the total time required to pass through the nth feasible path; θ f heading angle of actual end point of the n-th feasible path; θ d heading angle of desired end point of the n-th feasible path; (x f ,y f ) is the position of the actual end point of the nth feasible path. (x d ,y d ) is the position of the expected end point of the nth feasible path; λ1, λ2 and γ are all constants; Based on the above formula, the path cost corresponding to each feasible path is calculated, N path costs are obtained, the N path costs obtained above are sorted from small to large, and the following is met Wherein, i = 1, 2, …, N; the advantage set is obtained Wherein, Wherein, γ1 is the proportion of 0.1-0.2 in the selected advantage set; 10%-20% in the advantage set is selected as the optimization set.

3. The UUV path planning method based on cross-entropy method of claim 1, wherein, The step 3) specifically comprises: The optimization set includes multiple UUV feasible optimization paths, each UUV feasible optimization path is divided into m interconnected line segment paths, and each line segment path corresponds to a set of parameters; Assuming the first path cost in the optimized set Corresponding to the first UUV feasible optimized path, may be considered the ith sample of the first set of parameters therein; The set of parameters obeys a Gaussian distribution N(μ1,Σ1); where μ1 is the mean of the first set of parameters; Σ1 is the variance of the first set of parameters; μ1 ∈ R 2 , Σ1 ∈ R 2×2 ; i = 1,..., m; The calculation method is: calculating the mean and variance of the first set of parameters of the ρ UUV feasible optimization paths to obtain Repeat the process to obtain the updated mean and variance of the 1st, 2nd, …, mth group of parameters μ i ,Σ i , i = 1, 2, … m, the sampling for the next iteration, and connect the m line segment paths corresponding to the updated m group of parameters to form the optimized path of this time as the optimal solution of this time, and record the optimal solution and the corresponding path cost of this time;

Images