UUV local path planning method based on forward reachable set

Abstract

The invention provides a UUV local path planning method based on a forward reachable set, and the method comprises the steps: estimating an optimal parameter set of an energy function model according to the state of a moving obstacle, and predicting the trajectory of the moving obstacle at a future moment; setting a reference position between two adjacent global path points through a linear interpolation method, and calculating a forward reachable set based on an action interval range of the UUV; generating a constraint condition based on the trajectory of the moving obstacle at the future moment, and searching a plurality of alternative paths which meet the constraint condition and are closest to the reference position in the forward reachable set through a tree path search strategy; and calculating a risk assessment value of the next action of the UUV based on the time parameter and the position information, and selecting an optimal local path from the plurality of alternative paths according to the risk assessment value. According to the method, the path node set meeting the kinematics constraint of the UUV is generated based on the forward reachable set, and the optimal local path for ensuring the safety of the UUV can be generated by sensing the assessment of the obstacle collision risk.

Description

Technical Field

[0001] The present application relates to the field of underwater unmanned vehicle control technology, and in particular to a UUV local path planning method based on a forward reachable set. Background Art

[0002] At present, as a new type of underwater navigation equipment, the popularity of underwater unmanned vehicles (UUVs) in various fields is gradually increasing. With the expansion of UUV practical scenarios, the requirements for its navigation technology are also increasing. As an important part of UUV application technology, the level of UUV path planning will directly affect its ability to perform tasks.

[0003] In the related art, a global path from the starting point to the end point is usually planned for the UUV. Based on this global path, the UUV navigates along the global path to the mission end point according to the control signal output by its own onboard controller.

[0004] However, in practical applications, due to the complexity of the underwater environment, various unexpected moving obstacles are inevitable. The dense global path points in the above-mentioned related technologies not only limit the flexibility of the UUV to avoid obstacles, resulting in a weak ability to avoid moving obstacles, but also the calculation amount of global path planning is large. In addition, this path planning method does not consider the kinematic constraints of the UUV, which affects the path tracking accuracy and cannot select the most reasonable planning path. Summary of the invention

[0005] The present application aims to solve one of the technical problems in the related art at least to some extent.

[0006] To this end, the first purpose of this application is to propose a UUV local path planning method based on a forward reachable set. This method generates a set of path nodes that satisfy the UUV kinematic constraints based on the forward reachable set, and can select an optimal path that ensures safety for the UUV by evaluating the risk of collision with obstacles. This solves the problems of the UUV path planning method's weak collision avoidance ability with moving obstacles, insufficient consideration of the UUV's own dynamic constraints, and the design of the path evaluation function that rarely considers the interaction with obstacles.

[0007] The second object of this application is to propose a UUV local path planning system based on forward reachable sets;

[0008] A third object of the present application is to provide a non-transitory computer-readable storage medium.

[0009] To achieve the above objectives, the first aspect of the present application is to propose a UUV local path planning method based on a forward reachable set, the method comprising the following steps:

[0010] According to the currently observed state of the moving obstacle, an optimal parameter set of the energy function model is estimated, and the optimal parameter set is substituted into the energy function model to predict the trajectory of the moving obstacle at a future time;

[0011] A reference position is set between two adjacent global path points of the existing global planning path by linear interpolation method, and a forward reachable set is calculated based on the preset action interval range of the underwater unmanned vehicle UUV;

[0012] Based on the trajectory generation constraint of the mobile obstacle at a future time, a tree path search strategy is used to search for multiple candidate paths in the forward reachable set that meet the constraint and are closest to the reference position;

[0013] A time parameter representing the threat level of the moving obstacle is set, a risk assessment value of the next action of the UUV is calculated based on the time parameter and position information, and an optimal local path is selected from the multiple candidate paths according to the risk assessment value.

[0014] Optionally, in one embodiment of the present application, the energy function model is represented by the following formula:

[0015]

[0016] in, represents the parameter set of the energy function model, represents the state of mobile obstacle i at time k, is the position of the moving obstacle i at time k, is the speed of moving obstacle i at time k, represents the heading angle of the moving obstacle i at time k and satisfies the following relationship: X l represents the position vector of the obstacle group to which the mobile obstacle i belongs, and are the speed and heading angle of the moving obstacle i at time k+1, The sub-functions E0(·), E1(·), E2(·), E3(·), E4(·) and E5(·) represent the speed variation loss, the expected speed error loss, the global error loss, the group distance error loss, the group speed error loss and the interaction loss, respectively;

[0017] The predicting of the trajectory of the mobile obstacle at a future moment includes: calculating the optimal speed vector of the mobile obstacle at a future moment through the energy function model; and calculating the trajectory of the mobile obstacle at a future moment based on the position of the mobile obstacle at a current moment and the optimal speed vector.

[0018] Optionally, in one embodiment of the present application, the forward reachable set is calculated based on the action range of the underwater unmanned vehicle UUV, including: setting the relative state of the UUV at the current moment according to the current speed and angular velocity of the UUV in the body coordinate system, and calculating the relative state at the future moment according to the action range and the relative state at the current moment; discretizing the action range, and calculating the forward reachable set corresponding to each group of discrete speed values ​​and discrete angular velocity values ​​based on the relative state at the future moment and the action range.

[0019] Optionally, in one embodiment of the present application, the time parameter satisfies the following formula:

[0020]

[0021] Among them, R is the relative distance between the UUV and the specified position, V ≥ 0 is the speed of the UUV, K is the weight parameter, It indicates the deflection angle of the specified position in the UUV body coordinate system, T is the time parameter, R c is the collision distance;

[0022] The calculating the risk assessment value of the next action of the UUV based on the time parameter and the position information includes: calculating the relative distance and the deflection angle according to the position information, and calculating the time parameter according to the calculated relative distance and the deflection angle; substituting the time parameter into the following formula to calculate the risk value of the moving obstacle:

[0023]

[0024] stv i >0

[0025] Among them, p i represents the position of the moving obstacle i, θ i represents the heading angle of the moving obstacle i, ν i represents the speed of the moving obstacle i, p j represents the position of UUV, C P is the penalty value.

[0026] Optionally, in one embodiment of the present application, the calculating the risk assessment value of the next action of the UUV based on the time parameter and the position information further includes:

[0027] Based on the active collision risk of the next action of the UUV and the collision risk of the synchronous action of the moving obstacle, the risk assessment value of the next action of the UUV is calculated by the following formula:

[0028]

[0029] Among them, C r1 (·) and C r2 (·) represents the risk value of the UUV’s own action and the risk value of the moving obstacle’s action, respectively. represents the predicted state of mobile obstacle i at time t, represents the predicted position of the mobile obstacle i at time t, represents the predicted speed of moving obstacle i at time t, Represents the predicted heading angle of moving obstacle i at time t.

[0030] Optionally, in one embodiment of the present application, selecting the optimal local path from the multiple alternative paths based on the risk assessment value includes: calculating the distance error from each of the alternative paths to the global planning path; sorting the multiple alternative paths in order from small to large according to the distance errors to obtain an updated set of alternative paths; selecting the minimum distance error from each of the distance errors, and generating an index set of alternative paths based on the minimum distance error; and calculating the optimal local path based on the index set and the risk assessment value of the next action.

[0031] To achieve the above objectives, the second aspect of the present application further proposes a UUV local path planning system based on a forward reachable set, comprising the following modules:

[0032] A prediction module, used to estimate the optimal parameter set of the energy function model according to the currently observed state of the moving obstacle, and substitute the optimal parameter set into the energy function model to predict the trajectory of the moving obstacle at a future time;

[0033] A calculation module, used for setting a reference position between two adjacent global path points of an existing global planning path by a linear interpolation method, and calculating a forward reachable set based on a preset action interval range of the underwater unmanned vehicle UUV;

[0034] A search module, configured to generate constraint conditions based on the trajectory of the mobile obstacle at a future time, and search for multiple candidate paths that meet the constraint conditions and are closest to the reference position in the forward reachable set through a tree path search strategy;

[0035] A selection module is used to set a time parameter representing the threat level of the moving obstacle, calculate a risk assessment value of the next action of the UUV based on the time parameter and position information, and select an optimal local path from the multiple alternative paths according to the risk assessment value.

[0036] In order to implement the above-mentioned embodiments, the third aspect embodiment of the present application also proposes a non-temporary computer-readable storage medium, on which a computer program is stored. When the computer program is executed by a processor, the UUV local path planning method based on the forward reachable set in the above-mentioned first aspect embodiment is implemented.

[0037] The technical solution provided by the embodiment of the present application brings at least the following beneficial effects: the present application first predicts the trajectory of the mobile obstacle at the future moment through the energy function model, then sets the reference position through the linear interpolation method, and calculates the forward reachable set based on the kinematic constraints of the UUV, then generates constraints according to the predicted trajectory, searches for multiple alternative paths that meet the constraints and are closest to the reference position in the forward reachable set, and finally performs risk assessment on the next action of the UUV, and selects the optimal local path according to the assessment value. Thus, the present application generates a set of path nodes that meet the kinematic constraints of the UUV based on the forward reachable set algorithm, proposes a tree search strategy to take into account the search range and calculation time, and selects an optimal path for the UUV to ensure safety by evaluating the risk of collision with perceived obstacles. Therefore, the present application can provide local path planning for the UUV under the condition of the existing global path node, and the local planning result meets the requirements of mobile obstacle collision avoidance and dynamic constraints, so that the mobile obstacles on the path can be accurately avoided, the path tracking accuracy is ensured, and the rationality of local path selection is improved, which is conducive to ensuring that the UUV reaches the target location safely and quickly.

[0038] Additional aspects and advantages of the present invention will be given in part in the following description and in part will be obvious from the following description, or will be learned through practice of the present invention. BRIEF DESCRIPTION OF THE DRAWINGS

[0039] The above and / or additional aspects and advantages of the present application will become apparent and easily understood from the following description of the embodiments in conjunction with the accompanying drawings, in which:

[0040] Figure 1 A flowchart of a UUV local path planning method based on a forward reachable set proposed in an embodiment of the present application;

[0041] Figure 2 A schematic diagram of a tree path search strategy proposed in an embodiment of the present application;

[0042] Figure 3 A schematic diagram of the threat level of a mobile obstacle to a surrounding area under given parameters proposed in an embodiment of the present application;

[0043] Figure 4 A schematic diagram of a path planning result proposed in an embodiment of the present application;

[0044] Figure 5A schematic diagram of the structure of a UUV local path planning system based on a forward reachable set proposed in an embodiment of the present application. DETAILED DESCRIPTION

[0045] Embodiments of the present invention are described in detail below, examples of which are shown in the accompanying drawings, wherein the same or similar reference numerals throughout represent the same or similar elements or elements having the same or similar functions. The embodiments described below with reference to the accompanying drawings are exemplary and are intended to be used to explain the present invention, and should not be construed as limiting the present invention.

[0046] It should be noted that, based on the global path planning method in the relevant embodiments, the UUV needs to have the ability of local path planning to meet the needs of avoiding underwater obstacles while being able to receive global path points. Because mobile obstacles are more common in underwater environments, introducing mobile obstacle trajectory prediction in path planning problems is beneficial to the safety of planning results. However, the traditional prediction method based on the assumption of constant speed direction in the relevant embodiments is difficult to make a relatively accurate prediction of the trajectory of mobile obstacles in the short term in the future, and thus it is difficult to provide a reliable collision avoidance basis for UUV local path planning. Not only that, UUVs in actual application scenarios usually have kinematic constraints. Most previous studies have not considered the kinematic constraints of UUVs in the path planning framework. The planned path results have some nodes that UUVs cannot reach within the dynamic constraint range, which will affect the path tracking accuracy. In addition, selecting the optimal path from multiple path planning schemes requires relying on the path evaluation function. A commonly used method for evaluating paths is the artificial potential field method, which mainly uses the rate of change of the relative distance between the UUV and the mobile obstacle without considering the direction of the rate of change. Another evaluation method is the asymmetric Gaussian function, which defines the speed on different coordinate axes as the variance. However, when the obstacle stops moving temporarily, the calculation function is invalid.

[0047] To this end, this application proposes a UUV local path planning method and system based on forward reachable sets, which can be applied to UUV local path planning in complex underwater dynamic environments to improve the safety and efficiency of UUV operation.

[0048] A UUV local path planning method and system based on a forward reachable set proposed in an embodiment of the present application is described in detail below with reference to the accompanying drawings.

[0049] Figure 1 A flowchart of a UUV local path planning method based on a forward reachable set proposed in an embodiment of the present application is shown in FIG. Figure 1 As shown, the method comprises the following steps:

[0050] Step S101, based on the currently observed state of the moving obstacle, an optimal parameter set of the energy function model is estimated, and the optimal parameter set is substituted into the energy function model to predict the trajectory of the moving obstacle at a future moment.

[0051] Specifically, this step predicts the trajectory of mobile obstacles during the operation of the UUV. Since the present application provides a local path planning method for the UUV under the condition of existing global path nodes, and the local planning results meet the requirements of mobile obstacle collision avoidance and dynamic constraints. Therefore, under the condition that a continuous section of global path nodes has been selected, the UUV should consider avoiding mobile obstacles when planning the local path to ensure its own safety, and this process requires accurate prediction of the trajectory of mobile obstacles in the short term. To this end, the present application introduces an energy function model to predict the trajectory of mobile obstacles in the future. Among them, the currently observed mobile obstacles are the mobile obstacles that need to be avoided in the planned local path.

[0052] In one embodiment of the present application, the energy function model is represented by the following formula:

[0053]

[0054] in, represents the parameter set of the energy function model, represents the state of mobile obstacle i at time k, is the position of the moving obstacle i at time k, is the speed of moving obstacle i at time k, represents the heading angle of the moving obstacle i at time k and satisfies the following relationship: X l represents the position vector of the obstacle group to which the mobile obstacle i belongs, and are the speed and heading angle of the moving obstacle i at time k+1, The sub-functions E0(·), E1(·), E2(·), E3(·), E4(·) and E5(·) represent the speed variation loss, expected speed error loss, global error loss, group distance error loss, group speed error loss and interaction loss, respectively.

[0055] It can be understood that, with the k moment in the above formula as the current moment, the energy function model can predict the trajectory at the next moment, i.e., the k+1 moment. In specific implementation, predicting the trajectory of the moving obstacle at the future moment includes: calculating the optimal speed vector of the moving obstacle at the next moment through the energy function model; and calculating the trajectory of the moving obstacle at the future moment based on the position and optimal speed vector of the moving obstacle at the current moment.

[0056] Specifically, in this embodiment, the current moving obstacle to be avoided is first observed, and the state of the obstacle is observed, such as the position, speed, size and type of the obstacle. When estimating and predicting the obstacle trajectory based on the observed obstacle state, the optimal parameter set required by the energy function model is According to the formula principle of the above energy function model, the optimal velocity vector of the obstacle at the next moment is calculated by the following formula:

[0057]

[0058] Then, combined with the calculated optimal velocity vector at the next moment, the position of the moving obstacle at the current moment and other parameters, the trajectory of the moving obstacle at the future moment (i.e., moment k+1) is calculated by the following formula:

[0059]

[0060] Among them, Δt p1 is the update step length of the UUV for its own state and external obstacle state. The above prediction update is in [k, k+N c ] No need to update continuously Therefore, this processing method effectively reduces the amount of calculation in the prediction process.

[0061] It should be noted that the meaning of the parameters defined above will not be repeated in the subsequent description of this application, and reference may be made to the description of the related parameters mentioned above.

[0062] Step S102, setting a reference position between two adjacent global path points of the existing global planning path by linear interpolation method, and calculating a forward reachable set based on the preset action interval range of the underwater unmanned vehicle UUV.

[0063] Specifically, this step first designs the reference position. As mentioned above, since the local path planning of the UUV of the present application is based on the global planning path node, it is necessary to construct the reference position of the UUV between two adjacent global path points by linear interpolation.

[0064] As a possible implementation, if the interval between global path points is ΔT b , then ΔT b and the above Δt p1 The following formula is satisfied:

[0065] ΔT b =N bp Δt p1 ,N bp ∈￥ +

[0066] It should be noted that due to the dynamic changes in the underwater environment and the fact that the global planning path is usually a long-term plan, in order to save computing resources, this application does not interpolate all global path points at once to design the UUV reference position. ref global path points and iteratively interpolate the future N ref Global waypoints.

[0067] Based on this, in this embodiment, the reference position of the UUV can be designed by the following formula:

[0068]

[0069] l r =1,…,N ref ,m=1,…,N bp ,

[0070]

[0071] as well as:

[0072]

[0073] in, represents the reference position of the UUV at the corresponding planning time, P T-1 ,P T Represents two adjacent global path points numbered T-1 and T respectively, and the average speed between the nodes is represented by V T-1 =(P T -P T-1 ) / ΔT b , is the number of global path points, Indicates rounding down. r-1 and l r etc. are different subscript indices. Combined with the meaning of specific parameters, it can be inferred that N s 、N ref and N bg Indicates different numbers, and the calculation method or definition can be inferred from the above description.

[0074] Furthermore, this step also performs position sampling based on the forward reachable set. Based on the above-mentioned mobile obstacle trajectory prediction and UUV reference position design results of this application, this application needs to analyze whether the UUV can reach the reference position design results and meet the collision avoidance requirements under the condition of motion constraints. To this end, this application proposes a position sampling method based on the forward reachable set to calculate the position that the UUV can reach in advance.

[0075] In one embodiment of the present application, a forward reachable set is calculated based on the action range of the underwater unmanned vehicle UUV, including: setting the relative state of the UUV at the current moment according to the current speed and angular velocity of the UUV in the body coordinate system, and calculating the relative state at the future moment according to the action range and the relative state at the current moment; discretizing the action range, and calculating the forward reachable set corresponding to each group of discrete speed values ​​and discrete angular velocity values ​​based on the relative state at the future moment and the action range.

[0076] Specifically, in this embodiment, the relative state of the UUV at time k (i.e., the relative state at the current time) can be defined by the following formula through the velocity and angular velocity of the UUV in the body coordinate system:

[0077]

[0078] Among them, ν k and ω k are the velocity and angular velocity of the UUV in the body coordinate system at time k respectively.

[0079] Then, based on the action range given by the UUV (i.e., motion constraints) You can get:

[0080]

[0081] in, is the relative state of the future moment, and are the coordinate position and heading of the UUV in the body coordinate system at time k+1. k =[x k ,y k ,θ k ,ν k ,ω k ] T is the state vector of the UUV in the global coordinate system at time k, x k ,y k ,θ k are the position and heading angle of the UUV in the global coordinate system at time k.

[0082] Among them, X k+1 and The conversion relationship between them can be determined by the following formula:

[0083]

[0084] Furthermore, by discretizing the above action range, we can get the discrete value represented by the following formula:

[0085]

[0086] Among them, ν i and ω j are the discrete values ​​of velocity and angular velocity, N ν and N ω are the discrete numbers of the corresponding quantities; Δν and Δω are the discrete step lengths.

[0087] Furthermore, for every pair of combinations [ν i ,ω j ] The corresponding forward reachable set FRS(·) can be calculated by the following formula:

[0088]

[0089] Therefore, the UUV forward reachable set can be obtained by the above discretization method. Although there may be errors in the forward reachable set, it can be adjusted by adjusting N ν and N ω The value of N can meet the requirements of different systems, and increase ν and N ω The value of does not affect the complexity of the online planning part, but only expands the calculation range.

[0090] Step S103, based on the trajectory of the mobile obstacle at a future time, constraint conditions are generated, and a plurality of candidate paths that meet the constraint conditions and are closest to the reference position are searched in the forward reachable set through a tree path search strategy.

[0091] Specifically, this step implements a real-time path search strategy. Although the reference position of the UUV between two adjacent global path points is given in step S102, since the reference position is obtained by linear interpolation and the influence of obstacle movement and dynamic constraints must be considered, the reference position is not necessarily the optimal local path point. Therefore, in order to obtain a safe and reasonable optimal path and meet real-time requirements, this application uses a tree path search strategy to find multiple paths, thereby providing a selection space for the subsequent selection of the optimal path.

[0092] As a possible implementation, this embodiment sets the length of the local path to be N. F , that is, each local planning path has a total of N F In order to balance the search width and calculation time, based on the state X of the UUV at the current k moment k , select N at k+1 1th state nodes, and select N at each subsequent moment 2nd state nodes, so there are The specific search process is as follows Figure 2 shown.

[0093] And, at each moment, there are N state nodes i , among which At time k+1, select from the reachable set at the corresponding time a point that satisfies the following constraints and is closest to the reference position The nearest N 1th or N 2nd State nodes x k+i,j Among them, the constraints can be determined by the following formula:

[0094]

[0095] in, and is the predicted trajectory of the moving obstacle obtained in the aforementioned step S101, R c Represents the collision distance. The above constraints make full use of the predicted trajectory of the obstacle to avoid possible collision problems in the future.

[0096] Step S104, setting a time parameter representing the threat level of the moving obstacle, calculating a risk assessment value for the next action of the UUV based on the time parameter and the position information, and selecting an optimal local path from multiple candidate paths according to the risk assessment value.

[0097] Specifically, this step first evaluates the UUV movement risk. The purpose of analyzing the UUV movement risk evaluation in this application is to serve as a basis for constructing a path optimization function. The artificial potential field method and the asymmetric Gaussian function in the relevant embodiments are currently commonly used methods. However, the former uses the rate of change of the relative distance between the UUV and the obstacle but does not consider the direction of the rate of change, and the latter defines the speed of different axes as variance. This processing method becomes invalid when the obstacle stops moving. To this end, this application proposes a risk assessment strategy based on social perception to address the shortcomings of the above two commonly used methods. Since the threat level of an obstacle moving towards itself can be intuitively evaluated by estimating the time it takes for the obstacle to reach its position, in addition, the threat of this obstacle to its surrounding area is considered to be symmetric in the direction of speed. Based on this, this application defines a time T to represent the threat level of a moving obstacle.

[0098] In one embodiment of the present application, the time parameter T satisfies the following formula:

[0099]

[0100] Among them, R is the relative distance between the UUV and the specified position, V ≥ 0 is the speed of the UUV, K is the weight parameter, It indicates the deflection angle of the specified position in the UUV body coordinate system, T is the time parameter, R c is the collision distance.

[0101] In this embodiment, the threat degree of the mobile obstacle to the surrounding area represented by the time parameter T is shown in the effect diagram under the given speed V and weight K. Figure 3 Calculating the risk assessment value of the next action of the UUV based on the time parameter and the position information includes the following steps: first, calculating the relative distance and the deflection angle according to the position information, and calculating the time parameter according to the calculated relative distance and deflection angle.

[0102] The position information includes the coordinates of the UUV, the designated position, and the moving obstacle, and the designated position in the above formula can be the position of any point in the evaluation process, for example, Figure 3 P3 and P4 etc. can be used as designated positions.

[0103] For example, first obtain the position p of a given moving obstacle i i , heading angle θ i and speed v i , and the position of the UUV p j Then calculate the relative distance R and deflection angle based on the above position information Then substitute the calculated relative distance and deflection angle into the above calculation formula of the time parameter T to calculate the time parameter T.

[0104] Then, substitute the calculated time parameter into the following formula to calculate the risk value of the moving obstacle:

[0105]

[0106] stv i >0

[0107] Among them, p i represents the position of the moving obstacle i, θ i represents the heading angle of the moving obstacle i, ν i represents the speed of the moving obstacle i, p j represents the position of UUV, C P Indicates the penalty value.

[0108] Among them, C P is a large penalty value, indicating that obstacle i collides with target j. Analyzing the above formula from the perspective of threat level, when the speed of the moving obstacle ν i When =0, the obstacle stops moving, the arrival time T→∞, C(·)→0, and the risk approaches 0.

[0109] Furthermore, the risk of the next action performed by the UUV is evaluated based on the calculated risk value of the moving obstacle.

[0110] In one embodiment of the present application, when evaluating the risk of the next action of the UUV, not only the risk of the UUV actively colliding with the moving obstacle in the next time interval based on the action is considered, but also the collision risk caused by the synchronous action of the moving obstacle in this time period is considered. Based on this, the risk assessment value of the next action of the UUV can be calculated by the following formula:

[0111]

[0112] Among them, C r1 (·) and C r2 (·) represents the risk value of the UUV’s own action and the risk value of the moving obstacle’s action, respectively. represents the predicted state of mobile obstacle i at time t, represents the predicted position of the mobile obstacle i at time t, represents the predicted speed of moving obstacle i at time t, Represents the predicted heading angle of moving obstacle i at time t.

[0113] Then, this step evaluates the optimal path based on the risk assessment value of the UUV's next action.

[0114] Specifically, in step S103, multiple alternative paths are obtained through searching, which can be used as an alternative path set X. The present application selects an optimal path from the alternative path set X so that the UUV can quickly reach the target location and according to the risk assessment value, it can be calculated that the risk value of collision between the optimal path and the moving obstacle is the lowest.

[0115] In one embodiment of the present application, an optimal local path is selected from multiple alternative paths based on a risk assessment value, including: first calculating the distance error from each alternative path to the global planning path; then sorting the multiple alternative paths in order of the distance errors from small to large to obtain an updated set of alternative paths; then selecting the minimum distance error from each distance error, and generating an index set of alternative paths based on the minimum distance error; finally, calculating the optimal local path based on the index set and the risk assessment value of the next action.

[0116] Specifically, in this embodiment, in order to select an optimal path from X so that the UUV can quickly reach the target location, it is necessary to select a set of candidate paths close to the global path. First, the distance error from each candidate path to the global path is calculated by the following formula:

[0117]

[0118] Where D represents the distance error from the alternative path to the global path.

[0119] Then, by calculating the distance error between each candidate path and the global path and reordering all candidate paths in ascending order, a new candidate path set X can be obtained. ι .

[0120] Then take the minimum value of all distance errors as D1, and you can get the following index set:

[0121] I D ={ι|D ι ≤D1+ΔD}

[0122] Here, ΔD is the allowable range.

[0123] According to the index set of the above candidate paths, the optimal local path can be calculated by the following formula:

[0124]

[0125] Among them, W i is the risk value weight coefficient, X k+i-1 ,U k+i-1 They are the state and action of UUV at time k+i-1 respectively.

[0126] Therefore, the UUV can avoid the current moving obstacles and move to the target location at the shortest distance by driving according to the calculated optimal local path.

[0127] In summary, the UUV local path planning method based on the forward reachable set in the embodiment of the present application first predicts the trajectory of the mobile obstacle at the future moment through the energy function model, then sets the reference position through the linear interpolation method, and calculates the forward reachable set based on the kinematic constraints of the UUV, and then generates constraints according to the predicted trajectory, searches for multiple alternative paths that meet the constraints and are closest to the reference position in the forward reachable set, and finally performs risk assessment on the next action of the UUV, and selects the optimal local path according to the assessment value. Thus, the method generates a set of path nodes that meet the kinematic constraints of the UUV based on the forward reachable set algorithm, proposes a tree search strategy to take into account the search range and calculation time, and selects an optimal path for the UUV to ensure safety by evaluating the risk of perceived obstacle collision. Therefore, the method can provide local path planning for the UUV under the condition of the existing global path node, and the local planning result meets the requirements of mobile obstacle collision avoidance and dynamic constraints, so that the mobile obstacles on the path can be accurately avoided, the path tracking accuracy is ensured, and the rationality of local path selection is improved, which is conducive to ensuring that the UUV reaches the target location safely and quickly.

[0128] Based on the above embodiments, in order to more intuitively describe the UUV local path planning method based on forward reachable sets of the present application, the obstacle avoidance effect of the planned optimal local path is described below with a specific embodiment in practical application.

[0129] In this embodiment, the effectiveness of the path planning method of the present application is verified by designing a simulation experiment. Under the condition of the same environmental data set, the path planning method of the present application is compared with the speed space method and the artificial potential field method in the related embodiments, and the results of controlling the movement of UUVs by paths planned by these three methods are compared.

[0130] Among them, the parameter values ​​obtained by the path planning method of this application include: R c =0.66m, N ν =N ω =1681, ν=0.00m / s, ω=-8.05rad / s,Δt p1 =0.20s, N ref =2, N 1th =5,N 2nd =2. The path planning result obtained by the local path planning method of this application is as follows Figure 4 shown.

[0131] In addition, this embodiment also compares the paths planned by the present application and the above two methods to control the result data of whether the UUV can reach the target location when moving. The comparison results are shown in the following Table 1:

[0132] Table 1 Comparison of path planning methods

[0133] Average speed (m / s) Number of arrivals at the destination This method 1.10 36 Speed ​​Space Method 0.67 16 Artificial Potential Field Method 0.35 9

[0134] As shown in Table 1, according to the local path planning method of the present application, the UUV travels to the destination faster, and the success rate of successfully reaching the destination is higher than that of the other two methods. This shows that the local path planned by the present application can reduce the number of collisions of the UUV and can travel along the optimal path with the shortest distance.

[0135] In order to implement the above embodiment, the present application also proposes a UUV local path planning system based on a forward reachable set. Figure 5 A schematic diagram of the structure of a UUV local path planning system based on a forward reachable set proposed in an embodiment of the present application is shown in FIG. Figure 5 As shown, the system includes a prediction module 100, a calculation module 200, a search module 300 and a selection module 400.

[0136] Among them, the prediction module 100 is used to estimate the optimal parameter set of the energy function model according to the state of the currently observed moving obstacle, and substitute the optimal parameter set into the energy function model to predict the trajectory of the moving obstacle at a future moment.

[0137] The calculation module 200 is used to set a reference position between two adjacent global path points of the existing global planning path through a linear interpolation method, and calculate a forward reachable set based on a preset action interval range of the underwater unmanned vehicle UUV.

[0138] The search module 300 is used to generate constraint conditions based on the trajectory of the mobile obstacle at a future time, and to search for multiple candidate paths that meet the constraint conditions and are closest to the reference position in the forward reachable set through a tree path search strategy.

[0139] The selection module 400 is used to set a time parameter representing the threat level of the moving obstacle, calculate the risk assessment value of the next action of the UUV based on the time parameter and the position information, and select the optimal local path from multiple alternative paths according to the risk assessment value.

[0140] Optionally, in one embodiment of the present application, the prediction module 100 is specifically used to: calculate the optimal speed vector of the mobile obstacle at an unknown moment through an energy function model; and calculate the trajectory of the mobile obstacle at a future moment based on the position and optimal speed vector of the mobile obstacle at a current moment.

[0141] Optionally, in one embodiment of the present application, the calculation module 200 is specifically used to: set the relative state of the UUV at the current moment according to the current speed and angular velocity of the UUV in the body coordinate system, and calculate the relative state at the future moment according to the action interval range and the relative state at the current moment; discretize the action interval range, and calculate the forward reachable set corresponding to each group of discrete speed values ​​and discrete angular velocity values ​​based on the relative state at the future moment and the action interval range.

[0142] Optionally, in one embodiment of the present application, the selection module 400 is specifically used to: calculate the relative distance and the deflection angle according to the position information, and calculate the time parameter according to the calculated relative distance and the deflection angle;

[0143] Substitute the time parameter into the following formula to calculate the risk value of the moving obstacle:

[0144]

[0145] stv i >0

[0146] Among them, p i represents the position of the moving obstacle i, θ irepresents the heading angle of the moving obstacle i, ν i represents the speed of the moving obstacle i, p j represents the position of UUV, C P is the penalty value.

[0147] Optionally, in one embodiment of the present application, the selection module 400 is further used to: calculate the risk assessment value of the next action of the UUV based on the active collision risk of the next action of the UUV and the collision risk of the synchronous action of the moving obstacle by the following formula:

[0148]

[0149] Among them, C r1 (·) and C r2 (·) represents the risk value of the UUV’s own action and the risk value of the moving obstacle’s action, respectively. represents the predicted state of mobile obstacle i at time t, represents the predicted position of the mobile obstacle i at time t, represents the predicted speed of moving obstacle i at time t, Represents the predicted heading angle of moving obstacle i at time t.

[0150] Optionally, in one embodiment of the present application, the selection module 400 is specifically used to: calculate the distance error from each alternative path to the global planning path; sort multiple alternative paths in order of distance errors from small to large to obtain an updated set of alternative paths; select the minimum distance error from each distance error, and generate an index set of alternative paths based on the minimum distance error; calculate the optimal local path based on the index set and the risk assessment value of the next action.

[0151] It should be noted that the aforementioned explanation of the embodiment of the UUV local path planning method based on the forward reachable set is also applicable to the system of this embodiment and will not be repeated here.

[0152] In summary, the UUV local path planning system based on the forward reachable set in the embodiment of the present application generates a set of path nodes that meet the kinematic constraints of the UUV based on the forward reachable set algorithm, proposes a tree search strategy to take into account the search range and calculation time, and selects an optimal path for the UUV to ensure safety by evaluating the risk of perceived obstacle collision. Therefore, this method can provide local path planning for the UUV under the condition of existing global path nodes, and the local planning results meet the requirements of mobile obstacle collision avoidance and dynamic constraints, so that mobile obstacles on the path can be accurately avoided, the path tracking accuracy is ensured, and the rationality of local path selection is improved, which is conducive to ensuring that the UUV reaches the target location safely and quickly.

[0153] In order to implement the above embodiments, the present application also proposes a non-temporary computer-readable storage medium on which a computer program is stored. When the computer program is executed by a processor, the UUV local path planning method based on a forward reachable set as described in any of the above embodiments is implemented.

[0154] In the description of this specification, the description with reference to the terms "one embodiment", "some embodiments", "example", "specific example", or "some examples" etc. means that the specific features, structures, materials or characteristics described in conjunction with the embodiment or example are included in at least one embodiment or example of the present application. In this specification, the schematic representations of the above terms do not necessarily refer to the same embodiment or example. Moreover, the specific features, structures, materials or characteristics described may be combined in any one or more embodiments or examples in a suitable manner. In addition, those skilled in the art may combine and combine the different embodiments or examples described in this specification and the features of the different embodiments or examples, without contradiction.

[0155] In addition, the terms "first" and "second" are used for descriptive purposes only and should not be understood as indicating or implying relative importance or implicitly indicating the number of the indicated technical features. Therefore, the features defined as "first" and "second" may explicitly or implicitly include at least one of the features. In the description of this application, the meaning of "plurality" is at least two, such as two, three, etc., unless otherwise clearly and specifically defined.

[0156] Any process or method description in a flowchart or otherwise described herein may be understood to represent a module, fragment or portion of code comprising one or more executable instructions for implementing the steps of a custom logical function or process, and the scope of the preferred embodiments of the present application includes alternative implementations in which functions may not be performed in the order shown or discussed, including performing functions in a substantially simultaneous manner or in the reverse order depending on the functions involved, which should be understood by technicians in the technical field to which the embodiments of the present application belong.

[0157] The logic and / or steps represented in the flowchart or otherwise described herein, for example, can be considered as an ordered list of executable instructions for implementing logical functions, and can be embodied in any computer-readable medium for use by an instruction execution system, device or apparatus (such as a computer-based system, a system including a processor, or other system that can fetch instructions from an instruction execution system, device or apparatus and execute the instructions), or in combination with these instruction execution systems, devices or apparatuses. For the purpose of this specification, "computer-readable medium" can be any device that can contain, store, communicate, propagate or transmit a program for use by an instruction execution system, device or apparatus, or in combination with these instruction execution systems, devices or apparatuses. More specific examples of computer-readable media (a non-exhaustive list) include the following: an electrical connection with one or more wires (electronic device), a portable computer disk box (magnetic device), a random access memory (RAM), a read-only memory (ROM), an erasable and programmable read-only memory (EPROM or flash memory), a fiber optic device, and a portable compact disk read-only memory (CDROM). In addition, the computer-readable medium may even be paper or other suitable medium on which the program is printed, since the program may be obtained electronically, for example, by optically scanning the paper or other medium and then editing, interpreting or processing in other suitable ways if necessary, and then stored in a computer memory.

[0158] It should be understood that the various parts of the present application can be implemented by hardware, software, firmware or a combination thereof. In the above-mentioned embodiments, multiple steps or methods can be implemented by software or firmware stored in a memory and executed by a suitable instruction execution system. For example, if implemented by hardware, as in another embodiment, it can be implemented by any one of the following technologies known in the art or their combination: a discrete logic circuit having a logic gate circuit for implementing a logic function for a data signal, a dedicated integrated circuit having a suitable combination of logic gate circuits, a programmable gate array (PGA), a field programmable gate array (FPGA), etc.

[0159] A person skilled in the art may understand that all or part of the steps in the method for implementing the above-mentioned embodiment may be completed by instructing related hardware through a program, and the program may be stored in a computer-readable storage medium, which, when executed, includes one or a combination of the steps of the method embodiment.

[0160] In addition, each functional unit in each embodiment of the present application may be integrated into a processing module, or each unit may exist physically separately, or two or more units may be integrated into one module. The above-mentioned integrated module may be implemented in the form of hardware or in the form of a software functional module. If the integrated module is implemented in the form of a software functional module and sold or used as an independent product, it may also be stored in a computer-readable storage medium.

[0161] The storage medium mentioned above may be a read-only memory, a magnetic disk or an optical disk, etc. Although the embodiments of the present application have been shown and described above, it can be understood that the above embodiments are exemplary and cannot be understood as limiting the present application. A person of ordinary skill in the art may change, modify, replace and modify the above embodiments within the scope of the present application.

Claims

1. A UUV local path planning method based on forward reachable sets, characterized in that: The following steps are involved: According to the currently observed state of the moving obstacle, an optimal parameter set of the energy function model is estimated, and the optimal parameter set is substituted into the energy function model to predict the trajectory of the moving obstacle at a future time; A reference position is set between two adjacent global path points of the existing global planning path by linear interpolation method, and a forward reachable set is calculated based on the preset action interval range of the underwater unmanned vehicle UUV; Based on the trajectory generation constraint of the mobile obstacle at a future time, a tree path search strategy is used to search for multiple candidate paths in the forward reachable set that meet the constraint and are closest to the reference position; A time parameter representing the threat level of the moving obstacle is set, a risk assessment value of the next action of the UUV is calculated based on the time parameter and position information, and an optimal local path is selected from the multiple candidate paths according to the risk assessment value.

2. The method according to claim 1, characterized in that The energy function model is expressed by the following formula: in, represents the parameter set of the energy function model, represents the state of mobile obstacle i at time k, is the position of the moving obstacle i at time k, is the speed of moving obstacle i at time k, represents the heading angle of the moving obstacle i at time k and satisfies the following relationship: X l represents the position vector of the obstacle group to which the mobile obstacle i belongs, and are the speed and heading angle of the moving obstacle i at time k+1, The sub-functions E0(·), E1(·), E2(·), E3(·), E4(·) and E5(·) represent the speed variation loss, the expected speed error loss, the global error loss, the group distance error loss, the group speed error loss and the interaction loss, respectively; The predicting the trajectory of the moving obstacle at a future time includes: Calculate the optimal velocity vector of the moving obstacle at a certain moment by using the energy function model; Based on the position of the moving obstacle at the current moment and the optimal speed vector, a trajectory of the moving obstacle at a future moment is calculated.

3. The method according to claim 1, characterized in that The forward reachable set is calculated based on the action interval range of the underwater unmanned vehicle UUV, including: According to the current speed and angular velocity of the UUV in the body coordinate system, the relative state of the UUV at the current moment is set, and according to the action interval range and the relative state at the current moment, the relative state at the future moment is calculated; The action interval range is discretized, and based on the relative state at the future moment and the action interval range, a forward reachable set corresponding to each group of discrete velocity values ​​and discrete angular velocity values ​​is calculated.

4. The method according to claim 1, characterized in that: The time parameter satisfies the following formula: Among them, R is the relative distance between the UUV and the specified position, V ≥ 0 is the speed of the UUV, K is the weight parameter, It indicates the deflection angle of the specified position in the UUV body coordinate system, T is the time parameter, R c is the collision distance; The calculating the risk assessment value of the next action of the UUV based on the time parameter and the position information includes: Calculating the relative distance and the deflection angle according to the position information, and calculating the time parameter according to the calculated relative distance and the deflection angle; Substitute the time parameter into the following formula to calculate the risk value of the moving obstacle: stn i >0 Among them, p i represents the position of the moving obstacle i, θ i represents the heading angle of the moving obstacle i, ν i represents the speed of the moving obstacle i, p j represents the position of UUV, C P is the penalty value.

5. The method according to claim 4, characterized in that The calculating of the risk assessment value of the next action of the UUV based on the time parameter and the position information also includes: Based on the active collision risk of the next action of the UUV and the collision risk of the synchronous action of the moving obstacle, the risk assessment value of the next action of the UUV is calculated by the following formula: Among them, C r1 (·) and C r2 (·) represents the risk value of the UUV’s own action and the risk value of the moving obstacle’s action, respectively. represents the predicted state of mobile obstacle i at time t, represents the predicted position of the mobile obstacle i at time t, represents the predicted speed of moving obstacle i at time t, Represents the predicted heading angle of moving obstacle i at time t.

6. The method according to claim 1, characterized in that The selecting the optimal local path from the multiple candidate paths according to the risk assessment value includes: Calculating the distance error from each of the candidate paths to the global planning path; Sort the multiple candidate paths in ascending order of the distance errors to obtain an updated set of candidate paths; Selecting a minimum distance error from each of the distance errors, and generating an index set of candidate paths according to the minimum distance error; The optimal local path is calculated based on the index set and the risk assessment value of the next action.

7. A UUV local path planning system based on forward reachable sets, characterized in that: Includes the following modules: A prediction module, used to estimate the optimal parameter set of the energy function model according to the currently observed state of the moving obstacle, and substitute the optimal parameter set into the energy function model to predict the trajectory of the moving obstacle at a future time; A calculation module, used for setting a reference position between two adjacent global path points of an existing global planning path by a linear interpolation method, and calculating a forward reachable set based on a preset action interval range of the underwater unmanned vehicle UUV; A search module, configured to generate constraint conditions based on the trajectory of the mobile obstacle at a future time, and search for multiple candidate paths that meet the constraint conditions and are closest to the reference position in the forward reachable set through a tree path search strategy; A selection module is used to set a time parameter representing the threat level of the moving obstacle, calculate a risk assessment value of the next action of the UUV based on the time parameter and position information, and select an optimal local path from the multiple alternative paths according to the risk assessment value.

8. The system according to claim 7, characterized in that The prediction module is specifically used for: Calculate the optimal velocity vector of the moving obstacle at a certain moment by using the energy function model; Based on the position of the moving obstacle at the current moment and the optimal speed vector, a trajectory of the moving obstacle at a future moment is calculated.

9. The system according to claim 7, characterized in that The computing module is specifically used for: According to the current speed and angular velocity of the UUV in the body coordinate system, the relative state of the UUV at the current moment is set, and according to the action interval range and the relative state at the current moment, the relative state at the future moment is calculated; The action interval range is discretized, and based on the relative state at the future moment and the action interval range, a forward reachable set corresponding to each group of discrete velocity values ​​and discrete angular velocity values ​​is calculated.

10. A non-transitory computer-readable storage medium having a computer program stored thereon, characterized in that: When the computer program is executed by a processor, a UUV local path planning method based on a forward reachable set is implemented as described in any one of claims 1 to 6.

Images