Unmanned boat motion path planning method, device, terminal equipment and storage medium

Abstract

The present invention discloses a motion path planning method, device, terminal equipment and storage medium for an unmanned boat. The method comprises: obtaining a target posture state and a starting posture state of the unmanned boat; the target posture state comprises: target coordinates and a target heading angle; the starting posture state comprises: starting coordinates and a starting heading angle; constructing a grid map of the navigation area of ​​the unmanned boat; performing a reverse search from the target posture state to determine the cost of the unmanned boat from the target posture state to each grid in the grid map, using the cost of the unmanned boat from the target posture state to each grid in the grid map as the heuristic value of each grid, and generating a heuristic value grid map; generating a motion path of the unmanned boat from the starting posture state to the target posture state according to the heuristic value grid map, the starting posture state, the target posture state and the trajectory units of each posture state; the trajectory unit corresponding to each posture state is generated according to each posture state and a preset unmanned boat dynamics model.

Description

Technical Field

[0001] The present invention relates to the technical field of motion path planning, and in particular to a motion path planning method, device, terminal equipment and storage medium for an unmanned boat. Background Art

[0002] The ocean, rich in biological and mineral resources, is a vital venue for human economic, cultural, and scientific research. To better manage marine resources, protect the marine environment, and safeguard maritime rights, unmanned aerial vehicles (UAVs), as autonomous surface mobile platforms, offer advantages such as long-term operation, ability to operate in hazardous environments, low cost, and high efficiency. They have attracted widespread attention from countries, economies, and research institutions worldwide. Path planning, as a core technology for UAVs, has become a research hotspot. Path planning can be divided into three stages, depending on whether the UAV's kinematic and dynamic constraints are considered during the planning process: route planning, trajectory planning, and motion planning. In the route planning stage, the UAV is treated as a point mass model, ignoring its shape, dynamics, and kinematic constraints. In the trajectory planning stage, some constraints, such as the UAV's shape and minimum turning radius, are considered. In the motion planning stage, the UAV's dynamic model is considered to plan a path suitable for tracking.

[0003] Currently, the main motion planning algorithms for mobile robots include the A* algorithm, the D* algorithm, and the Dijkstra algorithm. For unmanned surface vehicles (UAVs), the path planning algorithm currently primarily uses the A* algorithm. However, traditional A* algorithms treat the UAV as a point mass model and employ a two-dimensional search space. This lacks the constraints of the UAV's dynamic characteristics, making the planned paths difficult to follow and limiting them to route or trajectory planning. Summary of the Invention

[0004] The embodiments of the present invention provide a motion path planning method, apparatus, terminal device and storage medium for an unmanned boat, which can make the planned path more consistent with the actual navigation conditions of the unmanned boat and achieve the scope of motion planning.

[0005] An embodiment of the present invention provides a motion path planning method for an unmanned boat, comprising:

[0006] Acquire the target posture state and the starting posture state of the unmanned boat; wherein the target posture state includes: target coordinates and target heading angle; the starting posture state includes: starting coordinates and starting heading angle;

[0007] Construct a grid map of the unmanned boat navigation area;

[0008] Performing a reverse search from the target posture state to determine the cost of the unmanned boat from the target posture state to each grid in the grid map, and then using the cost of the unmanned boat from the target posture state to each grid in the grid map as the heuristic value of each grid to generate a heuristic value grid map;

[0009] According to the heuristic value grid map, the starting posture state, the target posture state and the trajectory units corresponding to each posture state, a motion path of the unmanned boat from the starting posture state to the target posture state is generated; wherein the trajectory unit corresponding to each posture state is generated according to each posture state and a preset unmanned boat dynamics model.

[0010] Furthermore, the reverse search is performed from the target posture state to determine the cost of the unmanned boat from the target posture state to each grid in the grid map, and then the cost of the unmanned boat from the target posture state to each grid in the grid map is used as the heuristic value of each grid to generate the heuristic value grid map, which specifically includes:

[0011] Step A: setting the heuristic value of each grid in the grid map to a first preset value; setting a first pose state set for storing pose states to be searched and a second pose state set for storing pose states that have been searched, and initializing the first pose state set and the second pose state set to empty sets; extracting all pose states located in inaccessible grids to generate a third pose state set;

[0012] Step B: Adding the target posture state to the first posture state set;

[0013] Step C: taking the posture state stored in the first posture state set as the first posture state; calculating the cost from the target posture state to each first posture state, and taking the first posture state with the smallest cost as the selected first posture state; wherein, by calculating the Euclidean distance between the target posture state and each first posture state, the cost from the target posture state to each first posture state is obtained; adding the selected first posture state to the second posture state set, and deleting the selected first posture state from the first posture state set, and then taking the cost from the target posture state to the selected first posture state as the cost from the unmanned boat from the target posture state to the grid where the selected first posture state is located, and obtaining the heuristic value of the grid where the selected first posture state is located;

[0014] Step D: extracting the pose states of all neighboring grids adjacent to the grid where the selected first pose state is located to generate a fourth pose state set, and deleting the pose states that are simultaneously in the fourth pose state set and the second pose state set, or simultaneously in the fourth pose state set and the third pose state set, from the fourth pose state set;

[0015] Step E: taking the remaining posture states in the fourth posture state set as the fourth posture state, determining whether each fourth posture state is in the first posture state set one by one; if not, adding the fourth posture state to the first posture state set, and updating the cost from the target posture state to the fourth posture state to the combined cost; wherein the combined cost is: the sum of the cost from the target posture state to the selected first posture state and the cost from the selected first posture state to the fourth posture state; if so, when it is determined that the cost from the target posture state to the fourth posture state is greater than the combined cost, updating the cost from the target posture state to the fourth posture state to the combined cost;

[0016] Step F: Determine whether the first posture state set is an empty set. If not, jump to step C to continue execution. If so, generate an inspiration value grid map based on the inspiration value of each grid.

[0017] Furthermore, generating a motion path of the unmanned boat from the starting posture state to the target posture state according to the heuristic value grid map, the starting posture state, the target posture state, and the trajectory units corresponding to each posture state specifically includes:

[0018] Step 1: Clear the first posture state set and the second posture state set, add the starting posture state to the first posture state set, and extract the cost from the starting posture state to the target position state according to the heuristic value grid map; wherein the cost from the starting posture state to the target position state is the heuristic value of the grid where the starting posture state is located;

[0019] Step 2: Calculate the cost of reaching the target posture from the starting posture state through each first posture state, and select the first posture state with the least cost as the selected posture state; the cost of reaching the target posture from the starting posture state through the first posture state is: the sum of the cost from the starting posture state to the first posture state and the cost from the first posture state to the target posture state;

[0020] Add the selected posture state to the second posture state set and delete the selected posture state from the first posture state set; determine whether the selected posture state is in the same neighborhood as the target posture state, if so, execute step 9, if not, execute step 3;

[0021] Step 3: Generate a set of trajectory units based on the selected attitude state and the preset unmanned vehicle dynamics model;

[0022] Step 4: superimposing the selected pose state with each trajectory unit in the trajectory unit set to generate a reachable pose state set, and executing steps 5, 6, and 7 according to each reachable pose state in the reachable state set;

[0023] Step 5: Extract the cost from the reachable posture state to the target posture state according to the heuristic value grid map, and update the cost from the starting posture state to the reachable posture state to the second combined cost; wherein the second combined cost is: the sum of the cost from the starting posture state to the selected posture state and the trajectory unit cost corresponding to the reachable posture state;

[0024] Step 6: Using the posture state stored in the second posture state set as the second posture state; if there is a second posture state that meets the first preset condition in the second posture state set, deleting the second posture state that meets the first preset condition from the second posture state set; wherein the first preset condition is: being in the same neighborhood as the reachable posture state, and the cost from the starting posture state to the second posture state is greater than the cost from the starting posture state to the reachable posture state;

[0025] If there is a first posture state that meets the second preset condition in the first posture state set, the first posture state that meets the second preset condition is deleted from the first posture state set; wherein the second preset condition is: being in the same neighborhood as the reachable posture state, and the cost from the starting posture state to the first posture state is greater than the cost from the starting posture state to the reachable posture state;

[0026] If there is a first posture state that meets the third preset condition in the first posture state set, the operation of step 7 is not performed on the reachable posture state; wherein the third preset condition is: in the same neighborhood as the reachable posture state, but the cost from the starting posture state to the first posture state is not greater than the cost from the starting posture state to the reachable posture state; step 7: add the reachable posture state to the first posture state set; and set the cost from the starting posture state to the reachable posture state to the target posture as: the sum of the cost from the starting posture state to the reachable posture state and the cost from the reachable posture state to the target posture state;

[0027] Step 8: When it is determined that the first posture state set is not an empty set, jump to step 2;

[0028] Step 9: Extracting a second posture state in the same neighborhood as the target posture state from the second posture state set as the second target posture state; setting a fourth set for storing the unmanned boat path, and initializing the fourth set to an empty set;

[0029] Step 10: Taking the second target posture state as the initial reference posture state, repeatedly performing the trajectory unit adding operation according to the reference posture state until the reference posture state is the starting posture state; wherein the trajectory unit adding operation includes: searching for the parent posture state of the reference posture state; using the trajectory unit from the parent posture state to the reference posture state as the found trajectory unit; adding the found trajectory unit to the fourth set, and using the parent posture state as the updated reference posture state;

[0030] Step 11: Generate a motion path of the unmanned boat from the starting posture state to the target posture state according to each trajectory unit in the fourth set.

[0031] Furthermore, the corresponding trajectory unit is generated according to the posture state and the preset unmanned vehicle dynamics model, specifically including:

[0032] Obtaining a preset unmanned watercraft dynamics model, a preset constant forward speed, and a preset steering torque sequence; wherein the preset steering torque sequence includes: a plurality of steering torques;

[0033] According to the preset unmanned boat dynamics model and the preset steering torque sequence, the heading angular velocity of the unmanned boat at each moment under different steering torques is calculated;

[0034] Based on the posture state, the heading angular velocity of the unmanned boat at each moment under different steering torques and the preset constant forward speed are integrated to generate trajectory units corresponding to the posture state under different steering torques.

[0035] Furthermore, it also includes: controlling the unmanned boat to move according to the motion path, and when encountering an obstacle, updating the grid map according to the location of the obstacle; updating the subsequent motion path according to the updated heuristic value grid map, the current posture state of the unmanned boat and the target posture state.

[0036] Based on the above method embodiment, the present invention provides a corresponding device embodiment;

[0037] An embodiment of the present invention provides a motion path planning device for an unmanned boat, comprising: a posture acquisition module, a grid map construction module, a heuristic value grid map construction module, and a path planning module;

[0038] The posture acquisition module acquires the target posture state and the starting posture state of the unmanned boat; wherein the target posture state includes: target coordinates and target heading angle; the starting posture state includes: starting coordinates and starting heading angle;

[0039] The grid map construction module is used to construct a grid map of the unmanned boat navigation area;

[0040] The heuristic value grid map construction module is used to perform a reverse search from the target posture state to determine the cost of the unmanned boat from the target posture state to each grid in the grid map, and then use the cost of the unmanned boat from the target posture state to each grid in the grid map as the heuristic value of each grid to generate a heuristic value grid map;

[0041] The path planning module is used to generate a motion path of the unmanned vehicle from the starting posture state to the target posture state based on the heuristic value grid map, the starting posture state, the target posture state and the trajectory units corresponding to each posture state; wherein the trajectory unit corresponding to each posture state is generated according to each posture state and a preset unmanned vehicle dynamics model.

[0042] Based on the above method embodiment, the present invention provides a corresponding terminal device embodiment;

[0043] An embodiment of the present invention provides a terminal device, comprising a processor, a memory, and a computer program stored in the memory and configured to be executed by the processor, wherein when the processor executes the computer program, the motion path planning method of the unmanned boat described in any one of the present inventions is implemented.

[0044] Based on the above method embodiment, the present invention provides a storage medium embodiment;

[0045] An embodiment of the present invention provides a storage medium, which includes a stored computer program, wherein when the computer program is running, the device where the storage medium is located is controlled to execute the motion path planning method of the unmanned boat according to any one of the present inventions.

[0046] The following beneficial effects are achieved by implementing the embodiments of the present invention:

[0047] An embodiment of the present invention provides a motion path planning method, apparatus, terminal device and storage medium for an unmanned boat. The method generates a trajectory unit for each posture state according to each posture state and a preset unmanned boat dynamics model, and searches for a path from a starting posture state to a target position state in three-dimensional space (including coordinates and heading angles) based on the trajectory units corresponding to each posture state. The path finally generated is closer to the actual navigation conditions of the unmanned boat, achieving the scope of motion planning. BRIEF DESCRIPTION OF THE DRAWINGS

[0048] Figure 1 The figure is a flow chart of a motion path planning method for an unmanned boat provided by one embodiment of the present invention.

[0049] Figure 2 It is a schematic diagram showing the status of an unmanned boat on a grid map in the prior art.

[0050] Figure 3 This is another schematic diagram showing the state of an unmanned boat on a grid map in the prior art.

[0051] Figure 4 This is a schematic diagram showing the status of an unmanned vehicle on a grid map provided by one embodiment of the present invention.

[0052] Figure 5 Schematic diagram of a trajectory unit provided in one embodiment of the present invention.

[0053] Figure 6 The figure is a schematic structural diagram of a motion path planning device for an unmanned boat provided in one embodiment of the present invention. DETAILED DESCRIPTION

[0054] The following will clearly and completely describe the technical solutions in the embodiments of the present invention in conjunction with the accompanying drawings. Obviously, the described embodiments are only part of the embodiments of the present invention, not all of the embodiments. All other embodiments obtained by ordinary technicians in this field based on the embodiments of the present invention without making any creative efforts shall fall within the scope of protection of the present invention.

[0055] like Figure 1 As shown, an embodiment of the present invention provides a motion path planning method for an unmanned boat, comprising:

[0056] Step S101: Acquire the target posture state and the starting posture state of the unmanned boat; wherein the target posture state includes: target coordinates and target heading angle; the starting posture state includes: starting coordinates and starting heading angle.

[0057] Step S102: Construct a grid map of the unmanned boat navigation area.

[0058] Step S103: Perform a reverse search from the target posture state to determine the cost of the unmanned boat from the target posture state to each grid in the grid map, and then use the cost of the unmanned boat from the target posture state to each grid in the grid map as the heuristic value of each grid to generate a heuristic value grid map.

[0059] Step S104: Generate a motion path of the unmanned vehicle from the starting posture state to the target posture state according to the heuristic value grid map, the starting posture state, the target posture state, and the trajectory units corresponding to each posture state; wherein the trajectory unit corresponding to each posture state is generated according to each posture state and a preset unmanned vehicle dynamics model.

[0060] For step S101: schematically, it is assumed that the target posture state is: Set the starting posture state to: X1 and Y1 are the starting coordinates mentioned above, is the starting heading angle; X2 and Y2 are the target coordinates mentioned above, is the target heading angle.

[0061] Regarding step S102: in the present invention, a grid map is constructed using an existing method based on global environmental information.

[0062] The search space of the A* algorithm used in traditional unmanned boat path planning is two-dimensional C(x,y), which only searches for the position coordinates x, y of the unmanned boat, and they are specific position coordinates. The state of the unmanned boat on the grid map is as follows Figure 2 and Figure 3 As shown, only the center of each grid in the grid map is used (such as Figure 2 ) or grid junctions (such as Figure 3 ) represents the state of the UAV. This method cannot make the UAV appear at any position on the map. It can represent fewer UAV states and has poor optimality (increasing the length of the path). In addition, the two-dimensional search space does not consider the dynamic constraints of the UAV, and the planned path is difficult to follow.

[0063] In the present invention, an improved A* algorithm (the specific steps and logic of the algorithm will be described later) is used to search for paths, and its search space is three-dimensional. Therefore, it is necessary to represent the three-dimensional status information of the unmanned boat (coordinates (x, y) and heading angle) on the grid map. ), while the traditional method can only represent two-dimensional state information. Different from the traditional representation method, the state of the unmanned boat in the present invention is a state with steering information. The position point (x, y) can be at any position of the grid (such as Figure 4 (As shown) O is the starting point, B, A, D, and C are the subsequent state points respectively, and the adjacent state points are connected by trajectory units (the generation method of trajectory units will be described later). The traditional method position points can only be at the center of the grid or the intersection of the grids, and the adjacent state points are connected by straight lines, which allows the state searched by the improved A* algorithm to appear anywhere on the map. In order to reduce the complexity of the algorithm, the improved A* algorithm regards the state points in the same neighborhood as the same point when searching, that is, in each grid, there can be only one state point with the same turning direction. The domain means that for a posture state P of the unmanned boat, the neighborhood of the posture state P refers to the set of all posture states that are in the same grid and have the same heading angle as the posture state P.

[0064] Regarding step S103: in a preferred embodiment, performing a reverse search from the target posture state to determine the cost of the unmanned boat from the target posture state to each grid in the grid map, and then using the cost of the unmanned boat from the target posture state to each grid in the grid map as the heuristic value of each grid to generate the heuristic value grid map, specifically includes:

[0065] Step A: setting the heuristic value of each grid in the grid map to a first preset value; setting a first pose state set for storing pose states to be searched and a second pose state set for storing pose states that have been searched, and initializing the first pose state set and the second pose state set to empty sets; extracting all pose states located in inaccessible grids to generate a third pose state set;

[0066] Step B: Adding the target posture state to the first posture state set;

[0067] Step C: taking the posture state stored in the first posture state set as the first posture state; calculating the cost from the target posture state to each first posture state, and taking the first posture state with the smallest cost as the selected first posture state; wherein, by calculating the Euclidean distance between the target posture state and each first posture state, the cost from the target posture state to each first posture state is obtained; adding the selected first posture state to the second posture state set, and deleting the selected first posture state from the first posture state set, and then taking the cost from the target posture state to the selected first posture state as the cost from the unmanned boat from the target posture state to the grid where the selected first posture state is located, and obtaining the heuristic value of the grid where the selected first posture state is located;

[0068] Step D: extracting the pose states of all neighboring grids adjacent to the grid where the selected first pose state is located to generate a fourth pose state set, and deleting the pose states that are simultaneously in the fourth pose state set and the second pose state set, or simultaneously in the fourth pose state set and the third pose state set, from the fourth pose state set;

[0069] Step E: taking the remaining posture states in the fourth posture state set as the fourth posture state, determining whether each fourth posture state is in the first posture state set one by one; if not, adding the fourth posture state to the first posture state set, and updating the cost from the target posture state to the fourth posture state to the combined cost; wherein the combined cost is: the sum of the cost from the target posture state to the selected first posture state and the cost from the selected first posture state to the fourth posture state; if so, when it is determined that the cost from the target posture state to the fourth posture state is greater than the combined cost, updating the cost from the target posture state to the fourth posture state to the combined cost;

[0070] Step F: Determine whether the first posture state set is an empty set. If not, jump to step C to continue execution. If so, generate an inspiration value grid map based on the inspiration value of each grid.

[0071] For a better understanding, the following describes the generation of the heuristic value grid map in a more popular way: First, the steps involved in the creation of the heuristic value grid map are explained:

[0072] P: The position of the unmanned boat in a certain grid Take the center position of the grid, Any value can be set here (When establishing the heuristic value grid map, only one state point is taken for each grid); M: grid map;

[0073] HM: heuristic value grid map;

[0074] Target posture state;

[0075] OPEN: A set that stores the pose state to be searched, that is, the first pose state set mentioned above.

[0076] CLOSE: A set that stores the searched pose state, that is, the second pose state set mentioned above.

[0077] UNREACHABLE: A set of inaccessible grids (including grids with obstacles and grids at the map boundary), i.e., the third pose state set mentioned above.

[0078] C(P,P'): The cost of reaching state P' from state P, where Euclidean distance is used. but

[0079] The steps for establishing the heuristic value grid map are as follows:

[0080] 1. Copy the grid map and fill in the heuristic value ∞ (i.e., the first preset threshold) in each grid as the initial heuristic value grid map HM; clear the sets OPEN and CLOSE.

[0081] 2. Set the target posture state Add to the set OPEN, C(T,T)=0.

[0082] 3. Calculate the cost of the target pose state T and each pose state in OPEN, take out the state P with the smallest C(T,P) value from OPEN (that is, the first pose state selected above), delete P from OPEN, and add it to CLOSE, and update the heuristic value in the grid where P is located to C(T,P).

[0083] 4. Get the state set G(P) (i.e. the fourth pose state set mentioned above) in all neighboring grids of the grid where P is located, and delete the pose states in the CLOSE or UNREACHABLE set.

[0084] 5. For all remaining pose states in G(P), if the pose state (assumed to be P') is not in OPEN, add it to OPEN and set C(T,P') = C(T,P) + C(P,P'); if the pose state (assumed to be P') is in OPEN and C(T,P') > C(T,P) + C(P,P'), set C(T,P') = C(T,P) + C(P,P'); do not perform any operation in other cases.

[0085] 6. If the OPEN set is not empty, jump to step 3. Otherwise, the heuristic grid map is created. Each grid stores the minimum cost to reach the grid from the target position state. This value will be used as the heuristic value for the next path search. A grid with a value of ∞ indicates an obstacle or the map boundary. Setting the heuristic value to ∞ will help avoid obstacles in the next path search.

[0086] Regarding step S104; in a preferred embodiment, generating a motion path of the unmanned vehicle from the starting posture state to the target posture state based on the heuristic value grid map, the starting posture state, the target posture state, and the trajectory units corresponding to each posture state specifically includes:

[0087] Step 1: Clear the first posture state set and the second posture state set, add the starting posture state to the first posture state set, and extract the cost from the starting posture state to the target position state according to the heuristic value grid map; wherein the cost from the starting posture state to the target position state is the heuristic value of the grid where the starting posture state is located;

[0088] Step 2: Calculate the cost of reaching the target posture from the starting posture state through each first posture state respectively, and take the first posture state with the least cost as the selected posture state; wherein, the cost of reaching the target posture from the starting posture state through the first posture state is: the sum of the cost from the starting posture state to the first posture state and the cost from the first posture state to the target posture state; add the selected posture state to the second posture state set and delete the selected posture state from the first posture state set; determine whether the selected posture state is in the same neighborhood as the target posture state, if so, execute step 9, if not, execute step 3;

[0089] Step 3: Generate a set of trajectory units based on the selected attitude state and the preset unmanned vehicle dynamics model;

[0090] Step 4: superimposing the selected pose state with each trajectory unit in the trajectory unit set to generate a reachable pose state set, and executing steps 5, 6, and 7 according to each reachable pose state in the reachable state set;

[0091] Step 5: Extract the cost from the reachable posture state to the target posture state according to the heuristic value grid map, and update the cost from the starting posture state to the reachable posture state to the second combined cost; wherein the second combined cost is: the sum of the cost from the starting posture state to the selected posture state and the trajectory unit cost corresponding to the reachable posture state;

[0092] Step 6: Using the posture state stored in the second posture state set as the second posture state; if there is a second posture state that meets the first preset condition in the second posture state set, deleting the second posture state that meets the first preset condition from the second posture state set; wherein the first preset condition is: being in the same neighborhood as the reachable posture state, and the cost from the starting posture state to the second posture state is greater than the cost from the starting posture state to the reachable posture state;

[0093] If there is a first posture state that meets the second preset condition in the first posture state set, the first posture state that meets the second preset condition is deleted from the first posture state set; wherein the second preset condition is: being in the same neighborhood as the reachable posture state, and the cost from the starting posture state to the first posture state is greater than the cost from the starting posture state to the reachable posture state;

[0094] If there is a first pose state that meets the third preset condition in the first pose state set, then the operation of step 7 is not performed on the reachable pose state; wherein the third preset condition is: it is in the same neighborhood as the reachable pose state, but the cost from the starting pose state to the first pose state is not greater than the cost from the starting pose state to the reachable pose state;

[0095] Step 7: Add the reachable pose state to the first pose state set; and set the cost of reaching the target pose from the starting pose state through the reachable pose state to be the sum of the cost from the starting pose state to the reachable pose state and the cost from the reachable pose state to the target pose state;

[0096] Step 8: When it is determined that the first posture state set is not an empty set, jump to step 2;

[0097] Step 9: Extracting a second posture state in the same neighborhood as the target posture state from the second posture state set as the second target posture state; setting a fourth set for storing the unmanned boat path, and initializing the fourth set to an empty set;

[0098] Step 10: Taking the second target posture state as the initial reference posture state, repeatedly performing the trajectory unit adding operation according to the reference posture state until the reference posture state is the starting posture state; wherein the trajectory unit adding operation includes: searching for the parent posture state of the reference posture state; using the trajectory unit from the parent posture state to the reference posture state as the found trajectory unit; adding the found trajectory unit to the fourth set, and using the parent posture state as the updated reference posture state;

[0099] Step 11: Generate a motion path of the unmanned boat from the starting posture state to the target posture state according to each trajectory unit in the fourth set.

[0100] In a preferred embodiment, generating corresponding trajectory units according to the posture state and the preset unmanned vehicle dynamics model specifically includes:

[0101] Obtaining a preset unmanned watercraft dynamics model, a preset constant forward speed, and a preset steering torque sequence; wherein the preset steering torque sequence includes: a plurality of steering torques;

[0102] According to the preset unmanned boat dynamics model and the preset steering torque sequence, the heading angular velocity of the unmanned boat at each moment under different steering torques is calculated;

[0103] Based on the posture state, the heading angular velocity of the unmanned boat at each moment under different steering torques and the preset constant forward speed are integrated to generate trajectory units corresponding to the posture state under different steering torques.

[0104] First, the generation of trajectory units is described in detail:

[0105] The present invention generates the minimum trajectory unit based on the following model.

[0106]

[0107] Where u represents the forward speed of the unmanned boat, represents the forward acceleration of the unmanned boat, T represents the thrust of the unmanned boat, r represents the angular velocity of the unmanned boat, represents the steering acceleration of the unmanned boat, δ represents the steering torque of the unmanned boat, and a u 、b u 、c r d r is the preset coefficient.

[0108] In the motion model, δ and T are the control variables of the unmanned boat. In the present invention, when performing motion planning, the given thrust T is controlled to make the forward speed u constant (that is, the above-mentioned preset constant forward speed), and a certain discrete sampling of δ is performed to obtain {δ1, δ2, ..., δn} (i.e. the above-mentioned preset steering torque sequence) It can be seen from the motion model that when different δ is applied to the unmanned boat, different heading accelerations can be obtained according to the current heading angular velocity, and the corresponding stable heading angular velocity can be obtained after a certain time interval. A specified time interval Δt is specified, and the above-mentioned discrete δ and a given thrust T that keeps the forward velocity of the unmanned boat constant are applied within this time interval. The heading angular velocity is adjusted based on the current posture state of the unmanned boat. r By integrating with the constant forward velocity, we can obtain the corresponding trajectories under different steering torques and given thrust T, and obtain the set of trajectory units. The set of trajectory units obtained by different steering torques is as follows: Figure 4 As shown in the figure, taking one trajectory unit as an example, the two endpoints of the trajectory unit represent the two posture states of the unmanned vehicle. The trajectory unit consists of three parts: OA, AB, and BC. The OA and AB segments apply a steering torque δ, while the BC segment cancels the steering torque δ. The heading angular velocity r at the initial state point O is zero. The OA segment is the heading angular velocity acceleration phase, the AB segment is the heading angular velocity constant phase, and the BC segment is the heading angular velocity deceleration phase. After these three phases, the heading angular velocity r returns to zero, meaning that the heading angular velocity at point C is also zero. When generating the trajectory unit, a given thrust T is applied throughout the entire process to maintain a constant forward velocity. Therefore, each trajectory unit has the same forward velocity and heading angular velocity at both ends. The velocity along the path formed by the trajectory units is continuous and differentiable, which better reflects the actual motion of the unmanned vehicle.

[0109] The following is a detailed description of how to generate the motion path of the unmanned boat from the starting posture state to the target posture state based on the heuristic value grid map, the starting posture state, the target posture state, and the trajectory units corresponding to each posture state. In this invention, the improved A* algorithm is used to generate the motion path. The specific algorithm logic is as follows:

[0110] First, some symbols involved in the algorithm are explained:

[0111] P: The position of the unmanned boat in a certain grid This pose state can be at any position in the grid;

[0112] U P : Trajectory unit set U P ={u P1 ,u P2 ,…u Pn} (generated by the unmanned boat dynamics model according to the current posture state of the unmanned boat, the specific method will be introduced above);

[0113] L: motion path;

[0114] M: grid map;

[0115] HM: heuristic value grid map;

[0116] Starting posture state;

[0117] Target posture state;

[0118] OPEN: A set that stores the pose state to be searched, that is, the first pose state set;

[0119] CLOSE: A set that stores the searched pose state, that is, the second pose state set;

[0120] T 邻域 : Target pose state Neighborhood;

[0121] C(P,P'): The cost of reaching the posture state P' from the posture state P, calculated using the Euclidean distance, with two state points but

[0122] C(S,P'): the cost of reaching the pose state P' from the pose state S, calculated using the Euclidean distance;

[0123] C(S,P): The cost of reaching the pose state P from the pose state S, calculated using the Euclidean distance;

[0124] C(S,P i ”): From posture state S to posture state P i The cost of " is calculated using Euclidean distance;

[0125] C(u Pi ): trajectory unit u Pi The cost includes two parts: distance cost ∫udt and power cost a u T+b u δ, C(u Pi )=∫udt+a u T+b u δ;

[0126] H(S): The cost of reaching the target pose state T from the starting pose state S, obtained by querying the heuristic value map of the grid where the S pose state is located (pose states in the same grid have the same heuristic value)

[0127] H(P): The cost of reaching the target pose state T from the pose state P, which is obtained by querying the heuristic value map of the grid where the pose state P is located (pose states in the same grid have the same heuristic value).

[0128] H(P i '): From the pose state P i'The cost of reaching the target posture state T is determined by P i 'The grid where the posture state is located queries the heuristic value map (the posture states in the same grid have the same heuristic value).

[0129] F(P): The cost of reaching the target pose state from the starting pose state through the P pose state, F(P) = C(S,P) + H(P);

[0130] F(P i '): From the starting position through P i 'The cost of the pose state to reach the target pose state.

[0131] F(S): The cost of reaching the target pose from the starting pose state through the S pose state. F(S) = C(S, S) + H(S);

[0132] Par(P'): represents the parent pose state (previous state) of pose state P' and the minimum trajectory unit from the parent pose state to pose state P'. Par(P') = P,u Pi ;

[0133] The specific algorithm steps are as follows:

[0134] 1. Clear the collection OPEN and CLOSE, Add to OPEN, query the heuristic value grid map HM to get H(S), set F(S) = C(S,S) + H(S) = H(S);

[0135] 2. For each first pose state in OPEN, calculate the cost of reaching the target pose from the starting pose state through each first pose state, take out the pose state P with the smallest F(P) value from OPEN (that is, the above-mentioned selected pose state), delete P from OPEN, and add P to CLOSE. If P is in the target pose state In the neighborhood of , find the path to the target posture state and jump to step 9;

[0136] 3. Given the posture state P, the unmanned boat dynamics model, the given thrust T and the discrete steering torque {δ1,δ2,……,δ n Generate trajectory unit set U P ={u P1 ,u P2 ,…u Pn}

[0137] 4. The trajectory unit set U is superimposed by the posture state P P The set of reachable posture states P'={P1',P2',...P n '}. i' represents a reachable posture state in the reachable posture state set P', for all P in P' i 'Execute steps 5, 6, and 7.

[0138] 5. Query the heuristic value grid map HM to obtain P i 'H(P i '), the original C(S,P i ') is updated to: C(S,P i ')=C(S,P)+C(u Pi );.

[0139] 6. If there is a certain posture state P in CLOSE i "(i.e. a second posture state) is in posture state P i 'neighborhood and C(S,P i ”)>C(S,P i '), the posture state P i ” removed from CLOSE;

[0140] If there is a pose state P in OPEN i ”' (i.e. a certain first posture state) is in state P i 'neighborhood and C(S,P i ”')>C(S,P i '), the posture state P i ”'delete from OPEN;

[0141] If there is a pose state P in OPEN i ”' (i.e. a certain first posture state) is in state P i 'neighborhood and C(S,P i ”')≤C(S,P i '), not the posture state P i 'Go to step 7.

[0142] 7. Set the posture state P i 'Add to OPEN, set F(P i ')=C(S,P i ')+H(P i '),Par(P i ')=P,u Pi .

[0143] 8. If Jump to step 2. Otherwise, there is no safe and feasible path from the current pose state to the target pose state, and end.

[0144] 9. Take the state P"" (i.e. the second target posture state) of the target posture state T neighborhood from CLOSE and set the optimal path of the unmanned boat

[0145] 10. Update P””,u Pi =Par(P””), u Pi Add to L.

[0146] 11. If P””≠S, jump to step 10. Otherwise, reverse the order of the trajectory units in L, and L is from the initial state Reaching the target posture state The optimal path.

[0147] In a preferred embodiment, the method further includes controlling the unmanned boat to travel along the motion path, and when encountering an obstacle, updating the grid map based on the location of the obstacle; and updating the subsequent motion path based on the updated heuristic value grid map, the current position state of the unmanned boat, and the target position state. This embodiment allows the planned path to be modified based on actual navigation conditions.

[0148] Based on the above method embodiment, the present invention provides a corresponding device embodiment;

[0149] like Figure 6 As shown, an embodiment of the present invention provides a motion path planning device for an unmanned boat, comprising: a posture acquisition module, a grid map construction module, a heuristic value grid map construction module, and a path planning module;

[0150] The posture acquisition module acquires the target posture state and the starting posture state of the unmanned boat; wherein the target posture state includes: target coordinates and target heading angle; the starting posture state includes: starting coordinates and starting heading angle;

[0151] The grid map construction module is used to construct a grid map of the unmanned boat navigation area;

[0152] The heuristic value grid map construction module is used to perform a reverse search from the target posture state to determine the cost of the unmanned boat from the target posture state to each grid in the grid map, and then use the cost of the unmanned boat from the target posture state to each grid in the grid map as the heuristic value of each grid to generate a heuristic value grid map;

[0153] The path planning module is used to generate a motion path of the unmanned vehicle from the starting posture state to the target posture state based on the heuristic value grid map, the starting posture state, the target posture state and the trajectory units corresponding to each posture state; wherein the trajectory unit corresponding to each posture state is generated according to each posture state and a preset unmanned vehicle dynamics model.

[0154] Based on the above method embodiment, the present invention provides a corresponding terminal device embodiment:

[0155] An embodiment of the present invention provides a terminal device, comprising a processor, a memory, and a computer program stored in the memory and configured to be executed by the processor, wherein when the processor executes the computer program, the motion path planning method of the unmanned boat described in any one of the present inventions is implemented.

[0156] Based on the above method embodiment, the present invention provides a corresponding storage medium embodiment:

[0157] An embodiment of the present invention provides a storage medium, which includes a stored computer program, wherein when the computer program is running, the device where the storage medium is located is controlled to execute the motion path planning method of the unmanned boat according to any one of the present inventions.

[0158] It should be noted that the device embodiments described above are merely illustrative, wherein the units described as separate components may or may not be physically separated, and the components displayed as units may or may not be physical units, that is, they may be located in one place, or they may be distributed across multiple network units. Some or all of the modules may be selected according to actual needs to achieve the purpose of the present embodiment. In addition, in the drawings of the device embodiments provided by the present invention, the connection relationship between the modules indicates that there is a communication connection between them, which may be specifically implemented as one or more communication buses or signal lines. A person of ordinary skill in the art can understand and implement the present invention without inventive effort.

[0159] The above is a preferred embodiment of the present invention. It should be pointed out that for ordinary technicians in this technical field, several improvements and modifications can be made without departing from the principles of the present invention. These improvements and modifications are also considered to be within the scope of protection of the present invention.

Claims

1. A motion path planning method for an unmanned boat, characterized in that: include: Acquire the target posture state and the starting posture state of the unmanned boat; wherein the target posture state includes: target coordinates and target heading angle; the starting posture state includes: starting coordinates and starting heading angle; Construct a grid map of the unmanned boat navigation area; Performing a reverse search from the target posture state to determine the cost of the unmanned boat from the target posture state to each grid in the grid map, and then using the cost of the unmanned boat from the target posture state to each grid in the grid map as the heuristic value of each grid to generate a heuristic value grid map; Generate a motion path of the unmanned vehicle from the starting posture state to the target posture state according to the heuristic value grid map, the starting posture state, the target posture state, and the trajectory units corresponding to each posture state; wherein the trajectory unit corresponding to each posture state is generated according to each posture state and a preset unmanned vehicle dynamics model; The reverse search is performed from the target posture state to determine the cost of the unmanned boat from the target posture state to each grid in the grid map, and then the cost of the unmanned boat from the target posture state to each grid in the grid map is used as the heuristic value of each grid to generate the heuristic value grid map, which specifically includes: Step A: setting the heuristic value of each grid in the grid map to a first preset value; setting a first pose state set for storing pose states to be searched and a second pose state set for storing pose states that have been searched, and initializing the first pose state set and the second pose state set to empty sets; extracting all pose states located in inaccessible grids to generate a third pose state set; Step B: Adding the target posture state to the first posture state set; Step C: taking the posture state stored in the first posture state set as the first posture state; calculating the cost from the target posture state to each first posture state, and taking the first posture state with the smallest cost as the selected first posture state; wherein, by calculating the Euclidean distance between the target posture state and each first posture state, the cost from the target posture state to each first posture state is obtained; adding the selected first posture state to the second posture state set, and deleting the selected first posture state from the first posture state set, and then taking the cost from the target posture state to the selected first posture state as the cost from the unmanned boat from the target posture state to the grid where the selected first posture state is located, and obtaining the heuristic value of the grid where the selected first posture state is located; wherein, the first posture state includes the initial target posture state and the posture states added subsequently; Step D: extracting the pose states of all neighboring grids adjacent to the grid where the selected first pose state is located to generate a fourth pose state set, and deleting the pose states that are simultaneously in the fourth pose state set and the second pose state set, or simultaneously in the fourth pose state set and the third pose state set, from the fourth pose state set; Step E: taking the remaining posture states in the fourth posture state set as the fourth posture state, determining whether each fourth posture state is in the first posture state set one by one; if not, adding the fourth posture state to the first posture state set, and updating the cost from the target posture state to the fourth posture state to the combined cost; wherein the combined cost is: the sum of the cost from the target posture state to the selected first posture state and the cost from the selected first posture state to the fourth posture state; if so, when it is determined that the cost from the target posture state to the fourth posture state is greater than the combined cost, updating the cost from the target posture state to the fourth posture state to the combined cost; Step F: Determine whether the first posture state set is an empty set. If not, jump to step C to continue execution. If so, generate an inspiration value grid map based on the inspiration value of each grid.

2. The motion path planning method of the unmanned boat according to claim 1, characterized in that: The step of generating a motion path of the unmanned vehicle from the starting posture state to the target posture state according to the heuristic value grid map, the starting posture state, the target posture state, and the trajectory units corresponding to each posture state specifically includes: Step 1: Clear the first posture state set and the second posture state set, add the starting posture state to the first posture state set, and extract the cost from the starting posture state to the target position state according to the heuristic value grid map; wherein the cost from the starting posture state to the target position state is the heuristic value of the grid where the starting posture state is located; Step 2: Calculate the cost of reaching the target posture from the starting posture state through each first posture state, and select the first posture state with the least cost as the selected posture state; the cost of reaching the target posture from the starting posture state through the first posture state is: the sum of the cost from the starting posture state to the first posture state and the cost from the first posture state to the target posture state; Add the selected posture state to the second posture state set and delete the selected posture state from the first posture state set; determine whether the selected posture state is in the same neighborhood as the target posture state, if so, execute step 9, if not, execute step 3; Step 3: Generate a set of trajectory units based on the selected attitude state and the preset unmanned vehicle dynamics model; Step 4: superimposing the selected pose state with each trajectory unit in the trajectory unit set to generate a reachable pose state set, and executing steps 5, 6, and 7 according to each reachable pose state in the reachable state set; Step 5: Extract the cost from the reachable posture state to the target posture state according to the heuristic value grid map, and update the cost from the starting posture state to the reachable posture state to the second combined cost; wherein the second combined cost is: the sum of the cost from the starting posture state to the selected posture state and the trajectory unit cost corresponding to the reachable posture state; Step 6: Using the posture state stored in the second posture state set as the second posture state; if there is a second posture state that meets the first preset condition in the second posture state set, deleting the second posture state that meets the first preset condition from the second posture state set; wherein the first preset condition is: being in the same neighborhood as the reachable posture state, and the cost from the starting posture state to the second posture state is greater than the cost from the starting posture state to the reachable posture state; If there is a first posture state that meets the second preset condition in the first posture state set, the first posture state that meets the second preset condition is deleted from the first posture state set; wherein the second preset condition is: being in the same neighborhood as the reachable posture state, and the cost from the starting posture state to the first posture state is greater than the cost from the starting posture state to the reachable posture state; If there is a first pose state that meets the third preset condition in the first pose state set, then the operation of step 7 is not performed on the reachable pose state; wherein the third preset condition is: it is in the same neighborhood as the reachable pose state, but the cost from the starting pose state to the first pose state is not greater than the cost from the starting pose state to the reachable pose state; Step 7: Add the reachable pose state to the first pose state set; and set the cost of reaching the target pose from the starting pose state through the reachable pose state to be the sum of the cost from the starting pose state to the reachable pose state and the cost from the reachable pose state to the target pose state; Step 8: When it is determined that the first posture state set is not an empty set, jump to step 2; Step 9: Extracting a second posture state in the same neighborhood as the target posture state from the second posture state set as the second target posture state; setting a fourth set for storing the unmanned boat path, and initializing the fourth set to an empty set; Step 10: Taking the second target posture state as the initial reference posture state, repeatedly performing the trajectory unit adding operation according to the reference posture state until the reference posture state is the starting posture state; wherein the trajectory unit adding operation includes: searching for the parent posture state of the reference posture state; using the trajectory unit from the parent posture state to the reference posture state as the found trajectory unit; adding the found trajectory unit to the fourth set, and using the parent posture state as the updated reference posture state; Step 11: Generate a motion path of the unmanned boat from the starting posture state to the target posture state according to each trajectory unit in the fourth set.

3. The motion path planning method of the unmanned boat according to claim 2, characterized in that: Generate corresponding trajectory units according to the posture state and the preset unmanned vehicle dynamics model, including: Obtaining a preset unmanned watercraft dynamics model, a preset constant forward speed, and a preset steering torque sequence; wherein the preset steering torque sequence includes: a plurality of steering torques; According to the preset unmanned boat dynamics model and the preset steering torque sequence, the heading angular velocity of the unmanned boat at each moment under different steering torques is calculated; Based on the posture state, the heading angular velocity of the unmanned boat at each moment under different steering torques and the preset constant forward speed are integrated to generate trajectory units corresponding to the posture state under different steering torques.

4. The motion path planning method of the unmanned boat according to claim 3, characterized in that: Also includes: The unmanned boat is controlled to move according to the motion path, and when encountering an obstacle, the grid map is updated according to the location of the obstacle; the subsequent motion path is updated according to the updated heuristic value grid map, the current posture state of the unmanned boat and the target posture state.

5. An unmanned boat motion path planning device, characterized in that: include: Pose acquisition module, grid map construction module, heuristic value grid map construction module and path planning module; The posture acquisition module acquires the target posture state and the starting posture state of the unmanned boat; wherein the target posture state includes: target coordinates and target heading angle; the starting posture state includes: starting coordinates and starting heading angle; The grid map construction module is used to construct a grid map of the unmanned boat navigation area; The heuristic value grid map construction module is used to perform a reverse search from the target posture state to determine the cost of the unmanned boat from the target posture state to each grid in the grid map, and then use the cost of the unmanned boat from the target posture state to each grid in the grid map as the heuristic value of each grid to generate a heuristic value grid map; a path planning module, configured to generate a motion path of the unmanned vehicle from the starting posture state to the target posture state based on the heuristic value grid map, the starting posture state, the target posture state, and the trajectory units corresponding to each posture state; wherein the trajectory units corresponding to each posture state are generated based on each posture state and a preset unmanned vehicle dynamics model; The reverse search is performed from the target posture state to determine the cost of the unmanned boat from the target posture state to each grid in the grid map, and then the cost of the unmanned boat from the target posture state to each grid in the grid map is used as the heuristic value of each grid to generate the heuristic value grid map, which specifically includes: Step A: setting the heuristic value of each grid in the grid map to a first preset value; setting a first pose state set for storing pose states to be searched and a second pose state set for storing pose states that have been searched, and initializing the first pose state set and the second pose state set to empty sets; extracting all pose states located in inaccessible grids to generate a third pose state set; Step B: Adding the target posture state to the first posture state set; Step C: taking the posture state stored in the first posture state set as the first posture state; calculating the cost from the target posture state to each first posture state, and taking the first posture state with the smallest cost as the selected first posture state; wherein, by calculating the Euclidean distance between the target posture state and each first posture state, the cost from the target posture state to each first posture state is obtained; adding the selected first posture state to the second posture state set, and deleting the selected first posture state from the first posture state set, and then taking the cost from the target posture state to the selected first posture state as the cost from the unmanned boat from the target posture state to the grid where the selected first posture state is located, and obtaining the heuristic value of the grid where the selected first posture state is located; wherein, the first posture state includes the initial target posture state and the posture states added subsequently; Step D: extracting the pose states of all neighboring grids adjacent to the grid where the selected first pose state is located to generate a fourth pose state set, and deleting the pose states that are simultaneously in the fourth pose state set and the second pose state set, or simultaneously in the fourth pose state set and the third pose state set, from the fourth pose state set; Step E: taking the remaining posture states in the fourth posture state set as the fourth posture state, determining whether each fourth posture state is in the first posture state set one by one; if not, adding the fourth posture state to the first posture state set, and updating the cost from the target posture state to the fourth posture state to the combined cost; wherein the combined cost is: the sum of the cost from the target posture state to the selected first posture state and the cost from the selected first posture state to the fourth posture state; if so, when it is determined that the cost from the target posture state to the fourth posture state is greater than the combined cost, updating the cost from the target posture state to the fourth posture state to the combined cost; Step F: Determine whether the first posture state set is an empty set. If not, jump to step C to continue execution. If so, generate an inspiration value grid map based on the inspiration value of each grid.

6. A terminal device, characterized in that: The method comprises a processor, a memory, and a computer program stored in the memory and configured to be executed by the processor, wherein when the processor executes the computer program, the motion path planning method of the unmanned boat according to any one of claims 1 to 4 is implemented.

7. A storage medium, characterized in that: The storage medium includes a stored computer program, wherein when the computer program is running, the device where the storage medium is located is controlled to execute the motion path planning method of the unmanned boat according to any one of claims 1 to 4.

Images