A method for local path planning of UUV based on forward reachable set

Abstract

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

Description

TECHNICAL FIELD

[0001] The present application relates to the technical field of underwater unmanned vehicle control, and particularly relates to a UUV local path planning method based on a forward reachable set. BACKGROUND

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

[0003] In the related art, a global path from a starting point to an ending point is usually planned for a UUV. On the basis of the global path, the UUV navigates to the task ending point according to a control signal output by a controller carried by the UUV.

[0004] However, in actual applications, due to the complexity of the underwater environment, various unexpected moving obstacles inevitably exist. The dense global path points in the above related technology not only limit the flexibility of the UUV in avoiding obstacles, but also cause the ability of the UUV to avoid moving obstacles to be weak. Moreover, the calculation amount of the global path planning is large. Furthermore, the path planning manner does not consider the kinematic constraints of the UUV, which affects the path tracking accuracy and cannot select the most reasonable planning path. SUMMARY

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

[0006] To this end, a first object of the present application is to provide a UUV local path planning method based on a forward reachable set. The method generates a path node set satisfying the kinematic constraints of the UUV based on the forward reachable set, can select an optimal path ensuring safety for the UUV through the assessment of the collision risk of the perceived obstacle, and solves the problems that the UUV path planning method has weak obstacle avoidance ability for moving obstacles, does not consider the kinematic constraints of the UUV itself, and the design of the path evaluation function does not consider the interaction with the obstacle.

[0007] A second object of the present application is to provide a UUV local path planning system based on a forward reachable set.

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

[0009] To achieve the above objects, a first aspect of the present application provides a UUV local path planning method based on a forward reachable set, which comprises the following steps:

[0010] Based on the currently observed state of the moving obstacle, the 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 future times.

[0011] Reference positions are set between two adjacent global path points in the existing global planning path using linear interpolation, and the forward reachable set is calculated based on the preset action range of the underwater unmanned vehicle (UUV).

[0012] Based on the trajectory generation constraints of the moving obstacle at future time, a tree-shaped path search strategy is used to search the forward reachability set for multiple alternative paths that satisfy the constraints 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 for the next action of the UUV is calculated based on the time parameter and location information, and the optimal local path is selected from the multiple alternative paths according to the risk assessment value.

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

[0015]

[0016] in, The parameter set representing the energy function model. Indicates moving obstacles exist The state at any given moment, It is a moving obstacle. exist Location at any given moment It is a moving obstacle. exist The speed of time Indicates moving obstacles exist The heading angle at time t and satisfying the following relationship: , Indicates moving obstacles The position vector of the obstacle group to which it belongs. and These are moving obstacles exist Speed ​​and heading angle at any given moment sub-function These represent the velocity change loss, expected velocity error loss, global error loss, group distance error loss, group velocity error loss, and interaction loss, respectively.

[0017] The method of predicting the trajectory of the moving obstacle in the future includes: calculating the optimal velocity vector of the moving obstacle in the future using the energy function model; and calculating the trajectory of the moving obstacle in the future based on the position of the moving obstacle at the current moment and the optimal velocity vector.

[0018] Optionally, in one embodiment of this application, the calculation of the forward reachable set based on the action range of the underwater unmanned vehicle (UUV) includes: setting the relative state of the UUV at the current moment according to the current velocity and angular velocity of the UUV in the body coordinate system, and calculating the relative state at future moments 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 set of velocity discrete values ​​and angular velocity discrete values ​​based on the relative state at future moments and the action range.

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

[0020]

[0021] in, R It is the relative distance between the UUV and the specified location. It's the speed of UUV. These are weight parameters. This indicates the deflection angle of the specified position in the UUV body coordinate system. T It is a time parameter. R c It is the collision distance;

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

[0023]

[0024] in, Indicates moving obstacles Location, Indicates moving obstacles The heading angle, Indicates moving obstacles speed, Indicates the location of the UUV. It is a penalty value.

[0025] Optionally, in one embodiment of this application, the step of calculating the risk assessment value of the next action of the UUV based on the time parameter and location information further includes:

[0026] Based on the active collision risk of the UUV's next action and the collision risk of the moving obstacle moving synchronously, the risk assessment value of the UUV's next action is calculated using the following formula:

[0027]

[0028] in, These represent the risk values ​​of the UUV's own actions and the risk values ​​of the moving obstacle's actions, respectively. , Indicates moving obstacles exist Predicted state at any given time Indicates moving obstacles exist Predicted location at time Indicates moving obstacles exist Predicting speed at any given moment Indicates moving obstacles exist Predicted heading angle at any given time.

[0029] Optionally, in one embodiment of this application, selecting the optimal local path from the plurality of candidate paths based on the risk assessment value includes: calculating the distance error from each candidate path to the globally planned path; sorting the plurality of candidate paths in ascending order of the distance errors to obtain an updated set of candidate paths; selecting the minimum distance error from the distance errors and generating an index set of candidate 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.

[0030] To achieve the above objectives, a second aspect of this application also proposes a UUV local path planning system based on forward reachability sets, comprising the following modules:

[0031] The prediction module is used to estimate the optimal parameter set of the energy function model based on 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 future times.

[0032] The calculation module is used to set reference positions between two adjacent global path points of the existing global planning path using linear interpolation, and to calculate the forward reachable set based on the preset action range of the underwater unmanned vehicle (UUV).

[0033] The search module is used to generate constraints based on the trajectory of the moving obstacle at a future time, and to search for multiple alternative paths that meet the constraints and are closest to the reference position in the forward reachability set through a tree path search strategy.

[0034] The selection module 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 location information, and select the optimal local path from the multiple alternative paths according to the risk assessment value.

[0035] To implement the above embodiments, a third aspect of this application also proposes a non-transitory computer-readable storage medium storing a computer program thereon, which, when executed by a processor, implements the UUV local path planning method based on forward reachable set in the first aspect embodiment.

[0036] The technical solution provided by the embodiments of this application brings at least the following beneficial effects: This application first predicts the trajectory of a moving obstacle at a future time using an energy function model, then sets a reference position using linear interpolation, and calculates the forward reachability set based on the kinematic constraints of the UUV. Then, it generates constraint conditions based on the predicted trajectory, searches the forward reachability set for multiple candidate paths that satisfy the constraint conditions and are closest to the reference position, and finally assesses the risk of the UUV's next action, selecting the optimal local path based on the assessment value. Thus, this application generates a set of path nodes that satisfy the kinematic constraints of the UUV based on the forward reachability set algorithm, proposes a tree-based search strategy to balance search range and computation time, and selects a safe optimal path for the UUV by assessing the collision risk of perceived obstacles. Therefore, this application can provide local path planning for the UUV under the condition of existing global path nodes, and the local planning result satisfies the requirements of moving obstacle collision avoidance and dynamic constraints, thereby accurately avoiding moving obstacles on the path, ensuring path tracking accuracy, improving the rationality of local path selection, and helping to ensure the safe and rapid arrival of the UUV at the target location.

[0037] Additional aspects and advantages of the invention will be set forth in part in the description which follows, and in part will be obvious from the description, or may be learned by practice of the invention. Attached Figure Description

[0038] The above and / or additional aspects and advantages of this application will become apparent and readily understood from the following description of the embodiments taken in conjunction with the accompanying drawings, wherein:

[0039] Figure 1 A flowchart illustrating a UUV local path planning method based on forward reachable sets proposed in an embodiment of this application;

[0040] Figure 2 This is a schematic diagram of a tree-structured path search strategy proposed in an embodiment of this application;

[0041] Figure 3 This is a schematic diagram illustrating the threat level of a moving obstacle to the surrounding area under given parameters, as proposed in an embodiment of this application.

[0042] Figure 4 This is a schematic diagram of a path planning result proposed in an embodiment of this application;

[0043] Figure 5 This is a schematic diagram of the structure of a UUV local path planning system based on forward reachable sets proposed in an embodiment of this application. Detailed Implementation

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

[0045] It should be noted that, based on the global path planning method in the relevant embodiments, UUVs, while receiving global path points, need to possess local path planning capabilities to meet the requirements of avoiding underwater obstacles. Since moving obstacles are common in underwater environments, introducing moving obstacle trajectory prediction into the path planning problem is beneficial to the safety of the planning results. However, the traditional prediction method based on the assumption of constant velocity direction in the relevant embodiments is difficult to accurately predict the trajectory of moving obstacles in the short term, thus making it difficult to provide a reliable collision avoidance basis for UUV local path planning. Furthermore, UUVs in real-world applications typically have kinematic constraints. Previous studies have mostly failed to consider these kinematic constraints within the path planning framework, resulting in some nodes in the planned path that are inaccessible to the UUV within the dynamic constraints, which affects path tracking accuracy. Moreover, selecting the optimal path from multiple path planning schemes requires a path evaluation function. A commonly used method for evaluating paths is the artificial potential field method, which mainly utilizes the rate of change of the relative distance between the UUV and the moving obstacle, without considering the direction of the rate of change. Another evaluation method is the asymmetric Gaussian function, which defines the velocity on different coordinate axes as variance. However, the calculation function is invalid when the obstacle temporarily stops moving.

[0046] Therefore, 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, thereby improving the safety and efficiency of UUV operation.

[0047] The following describes in detail, with reference to the accompanying drawings, a UUV local path planning method and system based on forward reachable sets proposed in this application.

[0048] Figure 1 A flowchart of a UUV local path planning method based on forward reachable sets proposed in this application is shown below. Figure 1 As shown, the method includes the following steps:

[0049] Step S101: Based on the currently observed state of the moving obstacle, estimate the optimal parameter set of the energy function model, and substitute the optimal parameter set into the energy function model to predict the trajectory of the moving obstacle at future moments.

[0050] Specifically, this step predicts the trajectory of moving obstacles during UUV operation. Since this application provides a local path planning method for UUVs under the condition of existing global path nodes, and the local planning results meet the requirements of moving obstacle collision avoidance and dynamic constraints, given a selected continuous global path node segment, the UUV should consider avoiding moving obstacles to ensure its own safety when planning its local path. This process requires accurate prediction of the trajectory of moving obstacles in the short term. Therefore, this application introduces an energy function model to predict the trajectory of moving obstacles in future moments. Here, the currently observed moving obstacles are the moving obstacles that need to be avoided in the planned local path.

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

[0052]

[0053] in, The parameter set representing the energy function model. Indicates moving obstacles exist The state at any given moment, It is a moving obstacle. exist Location at any given moment It is a moving obstacle. exist The speed of time Indicates moving obstacles exist The heading angle at time t and satisfying the following relationship: , Indicates moving obstacles The position vector of the obstacle group to which it belongs. and These are moving obstacles exist Speed ​​and heading angle at any given moment sub-function These represent the velocity change loss, expected velocity error loss, global error loss, group distance error loss, group velocity error loss, and interaction loss, respectively.

[0054] It is understandable that, in the formula above If the current moment is taken as the reference point, then the trajectory at the next moment, i.e., moment k+1, can be predicted using this energy function model. In specific implementation, predicting the trajectory of a moving obstacle at a future moment includes: calculating the optimal velocity vector of the moving obstacle at the future moment using the energy function model; and calculating the trajectory of the moving obstacle at the future moment based on its current position and optimal velocity vector.

[0055] Specifically, in this embodiment, the moving obstacle to be avoided is first observed, including its state such as position, speed, size, and type. Based on the observed obstacle state, the optimal parameter set required by the energy function model for predicting the obstacle's trajectory is estimated. Based on the formula principle of the energy function model mentioned above, the optimal velocity vector of the obstacle at the next moment can be calculated using the following formula:

[0056]

[0057] Furthermore, combining the calculated optimal velocity vector for the next moment, the position of the moving obstacle at the current moment, and other parameters, the trajectory of the moving obstacle at a future moment (i.e., moment k+1) is calculated using the following formula:

[0058]

[0059] in, This is the update step size of the UUV regarding its own state and the state of external obstacles. The above prediction update is in... No need for continuous updates within a given timeframe This approach effectively reduces the computational load of the prediction process.

[0060] It should be noted that the meaning of the parameters that have been defined above will not be repeated in the following description, but can be referred to the description of the relevant parameters above.

[0061] Step S102: Set reference positions between two adjacent global path points of the existing global planning path using linear interpolation, and calculate the forward reachable set based on the preset action range of the underwater unmanned vehicle (UUV).

[0062] Specifically, this step begins with the design of the reference position. As mentioned above, since the local path planning of the UUV in this application is based on the global path planning nodes, the reference position of the UUV needs to be constructed between two adjacent global path points through linear interpolation.

[0063] As one possible implementation, if the time interval between global pathpoints is ,but and the above The following formula applies between them:

[0064]

[0065] It should be noted that, due to the dynamic changes in the underwater environment and the fact that global path planning is usually a long-term process, this application does not interpolate all global path points at once to design the UUV reference position in order to save computational resources. Instead, it selects twenty... A global path point is identified, and future interpolation is performed iteratively. A global path point.

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

[0067]

[0068] as well as:

[0069]

[0070] in, This indicates the reference position of the UUV at the corresponding planning time. Indicated by the numbering and The average velocity between two adjacent global path points is expressed as: , This represents the number of global path points. This indicates rounding down to the nearest integer. l r-1 and l r These are different subscript indices, which can be inferred from the meaning of the specific parameters. N s , N ref and N bg The different numbers can be inferred from the above description, including the calculation method or definition.

[0071] Furthermore, this step also involves position sampling based on the forward reachability set. Based on the moving obstacle trajectory prediction and UUV reference position design results described above in 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 reachability set to calculate the positions that the UUV can reach in advance.

[0072] In one embodiment of this application, calculating the forward reachable set based on the action range of an underwater unmanned vehicle (UUV) includes: setting the relative state of the UUV at the current moment based on its current velocity and angular velocity in the body coordinate system, and calculating the relative state at future moments based on the action range and the relative state at the current moment; discretizing the action range, and calculating the forward reachable set corresponding to each set of velocity and angular velocity discrete values ​​based on the relative state at future moments and the action range.

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

[0074]

[0075] in, They are respectively The velocity and angular velocity of the UUV in the body coordinate system at any given time.

[0076] Then, based on the given motion range (i.e., motion constraints) of the UUV. We can obtain:

[0077]

[0078] in, It is a relative state at a future moment. , and They are The coordinates and heading of the UUV in the body coordinate system at any given time. yes The state vector of the UUV in the global coordinate system at time t. They are The position and heading angle of the UUV in the global coordinate system at any given time.

[0079] in, and The conversion relationship between them can be determined by the following formula:

[0080]

[0081] Furthermore, discretizing the above action range yields the discrete values ​​represented by the following formula:

[0082]

[0083] in, These are the discrete values ​​of velocity and angular velocity, respectively. These are the discrete numbers of the corresponding quantities; It's a long walk.

[0084] Furthermore, for each pair of combinations The corresponding forward reachable set can be calculated using the following formula. :

[0085]

[0086] Therefore, the forward reachable set of UUVs obtained through the above discretization method may contain errors, but these errors can be corrected by adjusting... The value is used to meet the requirements of different systems, and to increase... The value of does not affect the complexity of the online planning part, but only expands the calculation range.

[0087] Step S103: Based on the trajectory of the moving obstacle at a future time, generate constraints and use a tree-based path search strategy to search for multiple alternative paths in the forward reachability set that meet the constraints and are closest to the reference position.

[0088] Specifically, this step executes a real-time path search strategy. Although a reference position of the UUV between two adjacent global path points is given in step S102, this reference position is not necessarily the optimal local path point because it is obtained by linear interpolation and the effects of obstacle movement and dynamic constraints must be considered. Therefore, in order to obtain a safe and reasonable optimal path that meets real-time requirements, this application adopts a tree-based path search strategy to find multiple paths, thereby providing a selection space for subsequently selecting the optimal path.

[0089] As one possible implementation, this embodiment sets the length of the local path to be... That is, each local planning path has a total of To balance search breadth and computation time, based on the current number of nodes... The state of UUV at all times ,exist choose Each state node is selected at each subsequent time step. There are state nodes, therefore, there are a total of The path, the specific search process is as followsFigure 2 As shown.

[0090] Furthermore, at each time step, the state node has N i One, of which .exist k At time +1, select from the reachability set at the corresponding time that satisfies the following constraints and is consistent with the reference position. The closest or Status nodes The constraints can be determined using the following formula:

[0091]

[0092] in, The predicted trajectory of the moving obstacle obtained in step S101 above. The above constraints, which represent the collision distance, fully utilize the predicted trajectory of obstacles to avoid potential collisions in the future.

[0093] Step S104: 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 location information, and select the optimal local path from multiple alternative paths according to the risk assessment value.

[0094] Specifically, this step first assesses the movement risk of the UUV. The purpose of this application's analysis of UUV movement risk assessment is to lay the foundation for constructing a path optimization function. The artificial potential field method and asymmetric Gaussian function used in related embodiments are commonly used methods. However, the former utilizes the rate of change of the relative distance between the UUV and the obstacle without considering the direction of the rate of change, while the latter defines the velocity along different axes as variance. This approach fails when the obstacle stops moving. Therefore, this application proposes a socially perceptual risk assessment strategy to address the shortcomings of the two commonly used methods. Since the threat level of an obstacle moving towards the user can be intuitively assessed by estimating the time it takes for the obstacle to reach its current position, and furthermore, the threat of such an obstacle to its surrounding area is considered symmetrical in the velocity direction, this application defines a time... T This indicates the level of threat posed by a moving obstacle.

[0095] In one embodiment of this application, the time parameter T Satisfy the following formula:

[0096]

[0097] in, R It is the relative distance between the UUV and the specified location. It's the speed of UUV. These are weight parameters. This indicates the deflection angle of the specified position in the UUV body coordinate system. T It is a time parameter. R c It is the collision distance.

[0098] In this embodiment, a time parameter is used. T The indicated threat level of a moving obstacle to the surrounding area at a given speed. and weight The following is the effect image Figure 3 As shown. Calculating the risk assessment value for the next action of the UUV based on time parameters and location information includes the following steps: First, calculating the relative distance and the deflection angle based on the location information, and then calculating the time parameters based on the calculated relative distance and deflection angle.

[0099] The location information includes the coordinates of the UUV, the specified location, and the moving obstacle. The specified location in the above formula can be the location of any point during the evaluation process, for example... Figure 3 P3 and P4 can both be used as designated locations.

[0100] For example, first obtain the given moving obstacles. i Location Heading angle and speed v i and the location of UUVs Then, based on the obtained location information, calculate the relative distance R and the deflection angle. Then substitute the calculated relative distance and deflection angle into the above time parameters. T The calculation formula and calculation time parameters T .

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

[0102]

[0103] in, Indicates moving obstacles Location, Indicates moving obstacles The heading angle, Indicates moving obstacles speed, Indicates the location of the UUV. This represents the penalty value.

[0104] in, It is a large penalty value, representing the obstacle. With the goal A collision occurs. Analyzing the above formula from the perspective of threat level, when the speed of the moving obstacle... When the obstacle stops moving, the time is reached. , The risk approaches zero.

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

[0106] In one embodiment of this application, when assessing the risk of the UUV's next action, not only is the risk of the UUV actively colliding with a moving obstacle in the next time interval based on that action considered, but also the collision risk caused by the synchronous movement of the moving obstacle during this time interval. Based on this, the risk assessment value of the UUV's next action can be calculated using the following formula:

[0107]

[0108] in, These represent the risk values ​​of the UUV's own actions and the risk values ​​of the moving obstacle's actions, respectively. , Indicates moving obstacles exist Predicted state at any given time Indicates moving obstacles exist Predicted location at time Indicates moving obstacles exist Predicting speed at any given moment Indicates moving obstacles exist Predicted heading angle at any given time.

[0109] Then, based on the risk assessment value obtained for the next action of the UUV, this step evaluates the optimal path.

[0110] Specifically, in step S103, multiple alternative paths are obtained through searching, which can be used as a set of alternative paths, X. This application selects an optimal path from the set of alternative paths X so that the UUV can quickly reach the target location, and the risk value of the optimal path colliding with the moving obstacle can be calculated based on the risk assessment value.

[0111] In one embodiment of this application, selecting the optimal local path from multiple candidate paths based on a risk assessment value includes: firstly, calculating the distance error from each candidate path to the globally planned path; then, sorting the multiple candidate paths in ascending order of the distance errors to obtain an updated set of candidate paths; then, selecting the minimum distance error from the distance errors and generating an index set of candidate paths based on the minimum distance error; and finally, calculating the optimal local path based on the index set and the risk assessment value of the next action.

[0112] 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 alternative paths that are close to the global path. First, the distance error from each alternative path to the global path is calculated using the following formula:

[0113]

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

[0115] Furthermore, by calculating the distance error between each candidate path and the global path and reordering all candidate paths in ascending order, a new set of candidate paths can be obtained. .

[0116] Then take the minimum value among all distance errors. The following set of indices can be obtained:

[0117]

[0118] in, It is within the permissible range.

[0119] Based on the index set of the above candidate paths, the optimal local path can be calculated using the following formula:

[0120]

[0121] in, The weighting coefficient for risk value. They are UUVs in The state and actions at any given moment.

[0122] Therefore, by traveling along the calculated optimal local path, the UUV can avoid current moving obstacles and move to the target location in the shortest distance.

[0123] In summary, the UUV local path planning method based on forward reachability sets in this application first predicts the trajectory of moving obstacles at future moments using an energy function model. Then, it sets a reference position using linear interpolation and calculates the forward reachability set based on the UUV's kinematic constraints. Next, it generates constraint conditions based on the predicted trajectory and searches the forward reachability set for multiple candidate paths that satisfy the constraints and are closest to the reference position. Finally, it assesses the risk of the UUV's next action and selects the optimal local path based on the assessment value. Thus, this method generates a set of path nodes that satisfy the UUV's kinematic constraints based on the forward reachability set algorithm, proposes a tree-based search strategy to balance search range and computation time, and selects a safe optimal path for the UUV by assessing the perceived obstacle collision risk. 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 moving obstacle collision avoidance and dynamic constraints. This allows for accurate avoidance of moving obstacles on the path, ensuring path tracking accuracy, improving the rationality of local path selection, and facilitating the safe and rapid arrival of the UUV at the target location.

[0124] Based on the above embodiments, in order to more intuitively describe the obstacle avoidance effect of the UUV local path planning method based on forward reachability set of this application, the following is an explanation of a specific embodiment in a practical application.

[0125] In this embodiment, the effectiveness of the path planning method of this application is verified by designing a simulation experiment. Under the same environmental dataset, the path planning method of this application is compared with the velocity space method and artificial potential field method in related embodiments, and the results of controlling UUV movement by planning the path according to these three methods are compared.

[0126] The parameter values ​​obtained by the path planning method in this application include: , , , , , , , , , The path planning results obtained according to the local path planning method of this application are as follows: Figure 4 As shown.

[0127] Furthermore, this embodiment also compares the path planned according to this application and the two methods described above, and compares the results of controlling the UUV's movement to reach the target location. The comparison results are shown in Table 1 below:

[0128] Table 1 Comparison of Path Planning Methods

[0129]

[0130] As shown in Table 1, the local path planning method of this application results in a faster speed for the UUV to reach the destination, and a higher success rate than the other two methods. This indicates that the local path planned in this application can reduce the number of collisions for the UUV and enable it to travel along the optimal path with the shortest distance.

[0131] To implement the above embodiments, this application also proposes a UUV local path planning system based on forward reachability sets. Figure 5 This is a schematic diagram of the structure of a UUV local path planning system based on a forward reachable set proposed in an embodiment of this application, as shown below. Figure 5 As shown, the system includes a prediction module 100, a calculation module 200, a search module 300, and a selection module 400.

[0132] The prediction module 100 is used to estimate the optimal parameter set of the energy function model based on 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 future times.

[0133] The calculation module 200 is used to set reference positions between two adjacent global path points of the existing global planning path using linear interpolation, and to calculate the forward reachable set based on the preset action range of the underwater unmanned vehicle (UUV).

[0134] The search module 300 is used to generate constraints based on the trajectory of the moving obstacle at a future time, and to search for multiple alternative paths that meet the constraints and are closest to the reference position in the forward reachability set through a tree path search strategy.

[0135] Module 400 is selected to set a time parameter representing the threat level of a moving obstacle, calculate the risk assessment value of the UUV's next action based on the time parameter and location information, and select the optimal local path from multiple alternative paths according to the risk assessment value.

[0136] Optionally, in one embodiment of this application, the prediction module 100 is specifically used to: calculate the optimal velocity vector of the moving obstacle at a future time using an energy function model; and calculate the trajectory of the moving obstacle at a future time based on the position and optimal velocity vector of the moving obstacle at the current time.

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

[0138] Optionally, in one embodiment of this application, module 400 is selected and is specifically used to: calculate the relative distance and deflection angle based on the location information, and calculate the time parameter based on the calculated relative distance and deflection angle;

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

[0140]

[0141] in, Indicates moving obstacles Location, Indicates moving obstacles The heading angle, Indicates moving obstacles speed, Indicates the location of the UUV. It is a penalty value.

[0142] Optionally, in one embodiment of this application, module 400 is further configured to: calculate the risk assessment value of the UUV's next action based on the active collision risk of the UUV's next action and the collision risk of the moving obstacle's synchronous action, using the following formula:

[0143]

[0144] in, These represent the risk values ​​of the UUV's own actions and the risk values ​​of the moving obstacle's actions, respectively. , Indicates moving obstacles exist Predicted state at any given time Indicates moving obstacles exist Predicted location at time Indicates moving obstacles exist Predicting speed at any given moment Indicates moving obstacles exist Predicted heading angle at any given time.

[0145] Optionally, in one embodiment of this application, module 400 is specifically used for: calculating the distance error from each candidate path to the globally planned path; sorting multiple candidate paths in ascending order of distance errors to obtain an updated set of candidate paths; selecting the minimum distance error from each distance error and generating an index set of candidate 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.

[0146] It should be noted that the foregoing explanation of the embodiment of the UUV local path planning method based on forward reachable set also applies to the system of this embodiment, and will not be repeated here.

[0147] In summary, the UUV local path planning system based on forward reachable sets in this application generates a set of path nodes that satisfy the kinematic constraints of the UUV based on the forward reachable set algorithm. It proposes a tree-based search strategy to balance search range and computation time, and selects an optimal path for the UUV to ensure safety by assessing the collision risk of perceived obstacles. Therefore, this method can provide local path planning for the UUV given existing global path nodes, and the local planning results meet the requirements of moving obstacle avoidance and dynamic constraints. This allows for precise avoidance of moving obstacles on the path, ensuring path tracking accuracy, improving the rationality of local path selection, and facilitating the safe and rapid arrival of the UUV at the target location.

[0148] To implement the above embodiments, this application also proposes a non-transitory computer-readable storage medium storing a computer program thereon, which, when executed by a processor, implements the UUV local path planning method based on forward reachable sets as described in any of the above embodiments.

[0149] In the description of this specification, the references to terms such as "one embodiment," "some embodiments," "example," "specific example," or "some examples," etc., indicate that a specific feature, structure, material, or characteristic described in connection with that embodiment or example is included in at least one embodiment or example of this application. In this specification, the illustrative expressions of the above terms do not necessarily refer to the same embodiment or example. Furthermore, the specific features, structures, materials, or characteristics described may be combined in any suitable manner in one or more embodiments or examples. Moreover, without contradiction, those skilled in the art can combine and integrate the different embodiments or examples described in this specification, as well as the features of different embodiments or examples.

[0150] Furthermore, the terms "first" and "second" are used for descriptive purposes only and should not be construed as indicating or implying relative importance or implicitly specifying the number of technical features indicated. Thus, a feature defined as "first" or "second" may explicitly or implicitly include at least one of that feature. In the description of this application, "multiple" means at least two, such as two, three, etc., unless otherwise explicitly specified.

[0151] Any process or method description in the flowchart or otherwise herein can be understood as representing a module, segment, or portion of code comprising one or more executable instructions for implementing custom logic functions or processes, and the scope of the preferred embodiments of this application includes additional implementations in which functions may be performed not in the order shown or discussed, including substantially simultaneously or in reverse order depending on the functions involved, as should be understood by those skilled in the art to which embodiments of this application pertain.

[0152] The logic and / or steps represented in the flowchart or otherwise described herein, for example, can be considered as a sequenced list of executable instructions for implementing logical functions, and can be embodied in any computer-readable medium for use by, or in conjunction with, an instruction execution system, apparatus, or device (such as a computer-based system, a processor-included system, or other system that can fetch and execute instructions from, an instruction execution system, apparatus, or device). For the purposes of this specification, "computer-readable medium" can be any means that can contain, store, communicate, propagate, or transmit programs for use by, or in conjunction with, an instruction execution system, apparatus, or device. More specific examples (a non-exhaustive list) of computer-readable media include: an electrical connection having one or more wires (electronic device), a portable computer disk drive (magnetic device), random access memory (RAM), read-only memory (ROM), erasable and editable read-only memory (EPROM or flash memory), fiber optic devices, and portable optical disc read-only memory (CDROM). Alternatively, the computer-readable medium may be paper or other suitable media on which the program can be printed, since the program can be obtained electronically, for example, by optically scanning the paper or other medium, followed by editing, interpreting, or otherwise processing as necessary, and then stored in a computer memory.

[0153] It should be understood that various parts of this application can be implemented using hardware, software, firmware, or a combination thereof. In the above embodiments, multiple steps or methods can be implemented using software or firmware stored in memory and executed by a suitable instruction execution system. For example, if implemented in hardware as in another embodiment, it can be implemented using any one or a combination of the following techniques known in the art: discrete logic circuits having logic gates for implementing logical functions on data signals, application-specific integrated circuits (ASICs) having suitable combinational logic gates, programmable gate arrays (PGAs), field-programmable gate arrays (FPGAs), etc.

[0154] Those skilled in the art will understand that all or part of the steps of the methods in the above embodiments can be implemented by a program instructing related hardware. The program can be stored in a computer-readable storage medium, and when executed, the program includes one or a combination of the steps of the method embodiments.

[0155] Furthermore, the functional units in the various embodiments of this application can be integrated into a processing module, or each unit can exist physically separately, or two or more units can be integrated into a module. The integrated module can be implemented in hardware or as a software functional module. If the integrated module is implemented as a software functional module and sold or used as an independent product, it can also be stored in a computer-readable storage medium.

[0156] The storage medium mentioned above can be a read-only memory, a disk, or an optical disk, etc. Although embodiments of this application have been shown and described above, it is understood that the above embodiments are exemplary and should not be construed as limiting this application. Those skilled in the art can make changes, modifications, substitutions, and variations to the above embodiments within the scope of this application.

Claims

1. A local path planning method for UUVs based on forward reachable sets, characterized in that, Includes the following steps: Based on the currently observed state of the moving obstacle, the 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 future times. Reference positions are set between two adjacent global path points in the existing global planning path using linear interpolation. A forward reachable set is calculated based on the preset action range of the underwater unmanned vehicle (UUV). Specifically, the relative state of the UUV at the current moment is set according to its current velocity and angular velocity in the body coordinate system. The relative state at future moments is calculated based on the action range and the current relative state. The action range is then discretized, and a forward reachable set corresponding to each set of velocity and angular velocity discretization values ​​is calculated based on the relative state at future moments and the action range. Based on the trajectory generation constraints of the moving obstacle at future time, a tree-shaped path search strategy is used to search the forward reachability set for multiple alternative paths that satisfy the constraints and are closest to the reference position. A time parameter representing the threat level of the moving obstacle is set, a risk assessment value for the next action of the UUV is calculated based on the time parameter and location information, and the optimal local path is selected from the multiple alternative 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, The parameter set representing the energy function model. Indicates moving obstacles exist The state at any given moment, It is a moving obstacle. exist Location at any given moment It is a moving obstacle. exist The speed of time Indicates moving obstacles exist The heading angle at time t and satisfying the following relationship: , Indicates moving obstacles The position vector of the obstacle group to which it belongs. and These are moving obstacles exist Speed ​​and heading angle at any given moment sub-function These represent the velocity change loss, expected velocity error loss, global error loss, group distance error loss, group velocity error loss, and interaction loss, respectively. The prediction of the trajectory of the moving obstacle at a future time includes: The optimal velocity vector of the moving obstacle at future moments is calculated using the energy function model. Based on the current position of the moving obstacle and the optimal velocity vector, calculate the trajectory of the moving obstacle at future moments.

3. The method according to claim 1, characterized in that, The time parameter satisfies the following formula: in, R It is the relative distance between the UUV and the specified location. It's the speed of UUV. These are weight parameters. This indicates the deflection angle of the specified position in the UUV body coordinate system. T It is a time parameter. R c It is the collision distance; The calculation of the risk assessment value for the next action of the UUV based on the time parameters and location information includes: The relative distance and the deflection angle are calculated based on the location information, and the time parameter is calculated based on 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: in, Indicates moving obstacles Location, Indicates moving obstacles The heading angle, Indicates moving obstacles speed, Indicates the location of the UUV. It is a penalty value.

4. The method according to claim 3, characterized in that, The calculation of the risk assessment value for the next action of the UUV based on the time parameters and location information also includes: Based on the active collision risk of the UUV's next action and the collision risk of the moving obstacle moving synchronously, the risk assessment value of the UUV's next action is calculated using the following formula: in, These represent the risk values ​​of the UUV's own actions and the risk values ​​of the moving obstacle's actions, respectively. , Indicates moving obstacles exist Predicted state at any given time Indicates moving obstacles exist Predicted location at time Indicates moving obstacles exist Predicting speed at any given moment Indicates moving obstacles exist Predicted heading angle at any given time.

5. The method according to claim 1, characterized in that, The step of selecting the optimal local path from the plurality of alternative paths based on the risk assessment value includes: Calculate the distance error from each of the candidate paths to the globally planned path; The multiple candidate paths are sorted in ascending order of their respective distance errors to obtain an updated set of candidate paths. Select the minimum distance error from all the distance errors, and generate an index set of candidate paths based on the minimum distance error; The optimal local path is calculated based on the index set and the risk assessment value of the next action.

6. A UUV local path planning system based on forward reachable sets, characterized in that, Includes the following modules: The prediction module is used to estimate the optimal parameter set of the energy function model based on 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 future times. The calculation module is used to set reference positions between two adjacent global path points of an existing global planning path using linear interpolation, and to calculate the forward reachable set based on the preset action range of the underwater unmanned vehicle (UUV). Specifically, based on the UUV's current velocity and angular velocity in the body coordinate system, the relative state of the UUV at the current moment is set, and based on the action range and the relative state at the current moment, the relative state at future moments is calculated. The action range is discretized, and based on the relative state at future moments and the action range, the forward reachable set corresponding to each set of velocity and angular velocity discrete values ​​is calculated. The search module is used to generate constraints based on the trajectory of the moving obstacle at a future time, and to search for multiple alternative paths that meet the constraints and are closest to the reference position in the forward reachability set through a tree path search strategy. The selection module 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 location information, and select the optimal local path from the multiple alternative paths according to the risk assessment value.

7. The system according to claim 6, characterized in that, The prediction module is specifically used for: The optimal velocity vector of the moving obstacle at future moments is calculated using the energy function model. Based on the current position of the moving obstacle and the optimal velocity vector, calculate the trajectory of the moving obstacle at future moments.

8. The system according to claim 6, characterized in that, The computing module is specifically used for: Based on the current velocity and angular velocity of the UUV in the body coordinate system, the relative state of the UUV at the current moment is set, and based on the action range and the relative state at the current moment, the relative state at future moments is calculated. The action range is discretized, and based on the relative state at the future time and the action range, the forward reachable set corresponding to each set of discrete velocity values ​​and discrete angular velocity values ​​is calculated.

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

Images