Three-dimensional path planning method for underwater vehicle based on space decomposition
By using spatial decomposition and artificial potential field methods in the three-dimensional path planning of underwater vehicles, the problem that DWA algorithm is prone to fall into local optimal solutions in three-dimensional space is solved, and the adaptability and accuracy of path planning is improved, and the Dynamic collision avoidance.
Patent Information
- Application Number
- CN202411151255.8
- Authority / Receiving Office
- CN · China
- Patent Type
- Applications(China)
- Current Assignee / Owner
- Filing Date
- 2024-08-21
- Publication Date
- 2024-11-12
AI Technical Summary
The existing DWA algorithm is prone to fall into local optimal solutions in the three-dimensional path planning of underwater vehicles, with poor applicability and is mainly based on two-dimensional plane expansion, making it difficult to effectively deal with obstacles in three-dimensional space.
Using a spatial decomposition method, the elevation map obstacles and dynamic obstacles in the aircraft mission interval are determined through elevation map data, and the expansion process is performed to determine the safe range, and decompose it into horizontal plane and vertical sections in three-dimensional space, and the DWA algorithm is used respectively. Perform path planning and combine artificial potential field algorithm to assist in breaking away from local optimal solutions.
It improves the adaptability of DWA algorithm in three-dimensional space and the accuracy of path planning, reduces the computational complexity, and realizes dynamic collision avoidance, reducing the frequency of local optimal solutions.
Smart Images

Figure CN118938971A_ABST
Abstract
Description
Technical Field
[0001] The invention relates to a three-dimensional path planning method for an underwater vehicle based on space decomposition, and belongs to the field of underwater vehicle path planning. Background Art
[0002] In recent years, with the development of computer, artificial intelligence and other technologies, underwater vehicles are increasingly used in underwater industry, military and other fields. In the process of performing underwater tasks, the vehicle needs to plan a safe, efficient and feasible path based on the surrounding environment information. The research on underwater three-dimensional path planning methods has become the focus of underwater vehicle autonomous technology.
[0003] Dynamic Window Approaches (DWA) is a local planning algorithm based on predictive control theory, which has the characteristics of small computational complexity, rapid response, and strong operability. The DWA algorithm describes the obstacle avoidance problem as a constrained optimization problem in the velocity space. It can form a dynamic window under the constraints of kinematics and dynamics and obstacle safety distance and sample multiple sets of velocity data in the velocity space, and then simulate the motion trajectory in the future based on these data. Finally, the evaluation function is used to score all simulated trajectories and select the optimal trajectory for movement, thereby achieving dynamic obstacle avoidance.
[0004] However, existing research on DWA algorithm path planning is mostly based on two-dimensional planes. At the same time, the DWA algorithm has problems such as being prone to falling into local optimal solutions. Summary of the invention
[0005] Aiming at the problem that the existing DWA algorithm path planning easily falls into the local optimal solution and has poor applicability to three-dimensional space path planning, the present invention provides a three-dimensional path planning method for underwater vehicles based on space decomposition.
[0006] A three-dimensional path planning method for an underwater vehicle based on space decomposition of the present invention comprises:
[0007] Determine the elevation map obstacles of the aircraft mission interval based on the elevation map data, and determine the environmental boundaries based on the mission interval;
[0008] Use wall obstacles to mark the environment boundary and determine the dynamic obstacles within the environment boundary;
[0009] All wall obstacles, elevation map obstacles and dynamic obstacles are expanded based on obstacle points, and the dynamic obstacle safety range is determined based on the expanded dynamic obstacles; the three-dimensional space of the current path planning of the aircraft is determined by the expanded wall obstacles, expanded elevation map obstacles and dynamic obstacle safety range;
[0010] The DWA algorithm is used to plan the path in the three-dimensional space of the current path planning of the aircraft, including:
[0011] Determine the weight parameters of the DWA algorithm based on the current position of the aircraft, the target position, the obstacle position on the elevation map, and the dynamic obstacle safety range;
[0012] Then, a horizontal plane is constructed according to the depth of the current position point of the aircraft, and a vertical section perpendicular to the horizontal plane is established by a straight line formed by connecting the current position point of the aircraft and the target position point;
[0013] Obtain all obstacle points on the horizontal plane and vertical section respectively; based on the obtained obstacle points, use the DWA algorithm to calculate the one-step planning results of the collision avoidance path from the current position point to the target position point on the horizontal plane and vertical section respectively; couple the two one-step planning results of the collision avoidance path to obtain the coupled one-step planning path; after the vehicle navigates according to the coupled one-step planning path, update the current position point of the vehicle and the dynamic obstacle safety range, and then continue to use the DWA algorithm for path planning until it reaches the target position point;
[0014] In the process of using the DWA algorithm for path planning, when the DWA algorithm falls into a local optimal solution, the artificial potential field algorithm is used for path auxiliary planning until it escapes from the local optimal solution position of the DWA algorithm, and then the DWA algorithm is used to continue path planning.
[0015] According to the underwater vehicle three-dimensional path planning method based on space decomposition of the present invention, determining the dynamic obstacles within the environment boundary includes determining the initial position and movement speed of the dynamic obstacles.
[0016] According to the underwater vehicle three-dimensional path planning method based on space decomposition of the present invention, the puffing processing method is: taking the obstacle point as the center, a spherical obstacle point set is obtained based on a preset radius; and a spherical obstacle boundary is formed by all spherical obstacle point sets.
[0017] According to the method for underwater vehicle three-dimensional path planning based on space decomposition of the present invention, the method for determining the weight parameters of the DWA algorithm is:
[0018] If there is an elevation map obstacle or a dynamic obstacle safety range on the line between the current position point of the aircraft and the target position point, the first flag position is 0; otherwise, it is set to 1;
[0019] If there is an elevation map obstacle within the detection range of the current position of the aircraft, the second flag position is set to 0; otherwise, it is set to 1;
[0020] If both flags are 1, the weight parameter with a high heading weight is selected in the DWA algorithm; otherwise, the weight parameter with a high obstacle distance weight is selected.
[0021] According to the three-dimensional path planning method for underwater vehicles based on space decomposition of the present invention, the profile equation of the vertical profile is expressed as:
[0022] Ax+By+Cz+D=0,
[0023] Where x represents the X-axis coordinate of the vertical section, y represents the Y-axis coordinate of the vertical section, z represents the Z-axis coordinate of the vertical section, A is the X-axis coefficient, B is the Y-axis coefficient, C is the Z-axis coefficient, and D is a constant term;
[0024] Calculate the distance d between the obstacle point and the vertical profile based on the coefficients A, B, C and the constant term D of the profile equation:
[0025]
[0026] Where x ob is the X-axis coordinate of the obstacle point, y ob is the Y-axis coordinate of the obstacle point, z ob is the Z-axis coordinate of the obstacle point;
[0027] The marking result of the second marking position is determined based on the comparison result of the distance between the obstacle and the vertical profile on the elevation map and the detection range of the current position point.
[0028] According to the method for three-dimensional path planning of underwater vehicles based on space decomposition of the present invention, the one-step collision avoidance path planning method includes sampling the current running speed of the vehicle:
[0029] The horizontal speed of the aircraft V h_m The restrictions are:
[0030] V h_m ={(v h ,ω h )|v h ∈[v h_min ,v h_max ],ω h ∈[ω h_min ,ω h_max ]},
[0031] Where v h is the horizontal plane running linear speed, ω h is the angular velocity of the horizontal plane, v h_min is the minimum linear velocity for horizontal operation, v h_max is the maximum linear speed of horizontal plane operation, ω h_min is the minimum angular velocity for horizontal plane operation, ω h_max is the maximum angular velocity of horizontal plane operation;
[0032] Set the sampling period to Δt and get the dynamic rectangular window V h_d :
[0033]
[0034] Where v h_c is the current horizontal plane linear speed of the aircraft, a hv_max is the maximum linear acceleration of the aircraft on the horizontal plane, ω h_c is the current horizontal angular velocity of the aircraft, a hw_max is the maximum angular acceleration of the aircraft in the horizontal plane;
[0035] The horizontal running speed V that satisfies dynamic obstacle avoidance is obtained h_a :
[0036]
[0037] Where dist(v h ,ω h ) is the distance evaluation subfunction, which represents the minimum distance between the planned path and the obstacle point in the horizontal plane at the current horizontal plane running speed;
[0038] Finally, we get the horizontal plane running speed sampling space V of the spacecraft h_r :
[0039] V h_r =V h_m ∩V h_d ∩V h_a .
[0040] According to the method for three-dimensional path planning of underwater vehicles based on space decomposition of the present invention, the one-step collision avoidance path planning method further includes predicting the one-step collision avoidance path planning of the vehicle:
[0041]
[0042] Where x h_k is the X-axis coordinate of the current position of the spacecraft at step k, y h_k is the Y-axis coordinate of the current position of the spacecraft at step k, θ h_k is the azimuth of the current position of the vehicle at the kth step; dt is the step time.
[0043] According to the three-dimensional path planning method for underwater vehicles based on space decomposition of the present invention, an evaluation function is used to calculate the score G(v h ,ω h ):
[0044] G(v h ,ω h)=α·normal_head(v h ,ω h )+β·normal_dist(v h ,ω h )+γ·normal_vel(v h ,ω h ),
[0045] Where α is the azimuth weight, normal_head(v h ,ω h ) is the azimuth evaluation subfunction head(v h ,ω h ), β is the distance weight, normal_dist(v h ,ω h ) is the distance evaluation subfunction dist(v h ,ω h ), γ is the velocity weight, normal_vel(v h ,ω h ) is the speed evaluation subfunction vel(v h ,ω h )’s normalized value;
[0046] According to the score G(v h ,ω h ) respectively determine the optimal one-step planning results of the collision avoidance path from the current position point to the target position point in the horizontal plane and the vertical section.
[0047] According to the three-dimensional path planning method of underwater vehicle based on space decomposition of the present invention,
[0048]
[0049] Where head(i) represents the azimuth evaluation sub-function value of the i-th predicted simulation path, n is the total number of predicted simulation paths under the spatial constraints of the horizontal plane running speed sampling within the period; dist(i) represents the distance evaluation sub-function value of the i-th predicted simulation path, and vel(i) represents the speed evaluation sub-function value of the i-th predicted simulation path.
[0050] According to the three-dimensional path planning method for underwater vehicles based on spatial decomposition of the present invention, a weighted average method is used to couple the optimal results of one-step planning of two collision avoidance paths; the weights of the weighted average method are set according to the difference in magnitude of position coordinates of the horizontal plane and the vertical section.
[0051] Beneficial effects of the present invention: The method of the present invention realizes the application of the DWA algorithm in the three-dimensional path planning of underwater vehicles.
[0052] The method of the present invention realizes environmental boundary restrictions by adding wall obstacles on the basis of elevation map data, adds dynamic obstacles to realize dynamic real-time collision avoidance, and sets different weight values by judging the relationship between the current position of the aircraft, the target point position and the obstacle to realize adaptive change of the weight of the DWA algorithm; decomposes the three-dimensional space into a horizontal plane and a vertical section by the current position of the aircraft and the target point position, respectively adopts the DWA algorithm to perform real-time dynamic collision avoidance on the two planes and couples the results, thereby realizing the DWA algorithm to plan the path in the three-dimensional space, improving the adaptability of the DWA algorithm in the three-dimensional space and reducing the computational complexity; when the DWA algorithm falls into a local optimal solution, the method of the present invention realizes the aircraft to escape from the position where it falls into the local optimal solution by adopting a three-dimensional APF algorithm, and then continues to adopt the DWA algorithm to plan the path, thereby reducing the limitation of the DWA algorithm. BRIEF DESCRIPTION OF THE DRAWINGS
[0053] Figure 1 is a flow chart of the method for three-dimensional path planning of underwater vehicles based on spatial decomposition according to the present invention;
[0054] Figure 2 is the elevation map data obstacle map in the embodiment;
[0055] Figure 3 is a wall obstacle map constructed for the environment boundary in the embodiment;
[0056] Figure 4 It is a schematic diagram of adding dynamic obstacles, current position points and target position points in the environment boundary;
[0057] Figure 5 It is a horizontal plane diagram of the spatial decomposition at a certain moment;
[0058] Figure 6 A vertical cross-section diagram of the spatial decomposition at a certain moment;
[0059] Figure 7 It is a static obstacle map within the detection range of the aircraft in the horizontal plane at a certain moment;
[0060] Figure 8 It is a dynamic obstacle map within the detection range of the aircraft in the horizontal plane at a certain moment;
[0061] Fig. 9 It is a static obstacle map within the detection range of the aircraft in the vertical section at a certain moment;
[0062] Fig.10 It is a dynamic obstacle map within the detection range of the aircraft in the vertical section at a certain moment;
[0063] Fig.11 This is the final post-coupling planning path diagram of the embodiment. DETAILED DESCRIPTION
[0064] The following will be combined with the drawings in the embodiments of the present invention to clearly and completely describe the technical solutions in the embodiments of the present invention. Obviously, the described embodiments are only part of the embodiments of the present invention, not all of the embodiments. Based on the embodiments of the present invention, all other embodiments obtained by ordinary technicians in this field without creative work are within the scope of protection of the present invention.
[0065] It should be noted that, in the absence of conflict, the embodiments of the present invention and the features in the embodiments may be combined with each other.
[0066] The present invention will be further described below in conjunction with the accompanying drawings and specific embodiments, but they are not intended to limit the present invention.
[0067] Specific implementation method 1. Combination Figures 1 to 11 As shown, the present invention provides a three-dimensional path planning method for an underwater vehicle based on space decomposition, comprising:
[0068] According to the elevation map data, obstacle information is extracted to determine the elevation map obstacles in the mission interval of the aircraft, and the environmental boundary is determined according to the mission interval;
[0069] Use wall obstacles to mark the environment boundary and determine the dynamic obstacles within the environment boundary;
[0070] All wall obstacles, elevation map obstacles and dynamic obstacles are expanded based on obstacle points, and the dynamic obstacle safety range is determined based on the expanded dynamic obstacles; the three-dimensional space of the current path planning of the aircraft is determined by the expanded wall obstacles, expanded elevation map obstacles and dynamic obstacle safety range;
[0071] The DWA algorithm is used for path planning in the three-dimensional space of the current path planning of the aircraft, including adaptively changing the DWA weight, decomposing the three-dimensional space, using the DWA algorithm for path planning in the decomposed planes respectively and coupling the results, as follows:
[0072] Determine the weight parameters of the DWA algorithm based on the current position of the aircraft, the target position, the obstacle position on the elevation map, and the dynamic obstacle safety range;
[0073] Then, a horizontal plane is constructed according to the depth of the current position point of the aircraft, and a vertical section perpendicular to the horizontal plane is established by a straight line formed by connecting the current position point of the aircraft and the target position point;
[0074] All obstacle points on the horizontal plane and vertical section are obtained respectively; based on the obtained obstacle points, the DWA algorithm is used to calculate the one-step planning results of the collision avoidance path from the current position point to the target position point of the aircraft in the horizontal plane and vertical section respectively; the two one-step planning results of the collision avoidance path are coupled to obtain the coupled one-step planning path; after the aircraft navigates according to the coupled one-step planning path, the current position point of the aircraft and the dynamic obstacle safety range are updated, and the DWA algorithm is continued to be used for path planning until the target position point is reached; the one-step planning is to perform path planning within the detection range of the horizontal plane where the current position of the aircraft is located;
[0075] In the process of using the DWA algorithm for path planning, when the DWA algorithm falls into a local optimal solution, the artificial potential field algorithm is used for path auxiliary planning until it escapes from the local optimal solution position of the DWA algorithm, and then the DWA algorithm is used to continue path planning.
[0076] In this embodiment, the elevation map data is imported to obtain the longitude, latitude, and depth information of the elevation map, and the location information of the elevation map data is recorded. Then, the environmental space range is obtained based on the obtained longitude, latitude, and depth information, and the wall obstacles are marked at the boundary of the environmental space range so that the path planning is limited to this range, and the location information of the wall obstacles is recorded. Then, the obstacle points of the elevation map data are expanded. The obstacle points of all the original elevation map data are expanded into a spherical obstacle point set with this point as the center and a certain radius, so as to facilitate the subsequent interception of static obstacle data for the horizontal plane of spatial decomposition.
[0077] Expanding dynamic obstacles can facilitate the subsequent interception of dynamic obstacle data for vertical sections of spatial decomposition. After expanding wall obstacles, elevation map obstacles and dynamic obstacles, the spherical obstacle boundary is obtained. In determining the current path planning three-dimensional space and all subsequent calculations, a static obstacle safety range must be set based on the expanded spherical obstacle boundary of wall obstacles and elevation map obstacles to ensure the reliability of path planning, which is similar to the determination of the dynamic obstacle safety range. In actual operation, the static obstacle safety range can be slightly smaller than the extension space of the dynamic obstacle relative to the obstacle itself.
[0078] The DWA algorithm includes adaptively changing the DWA weight and decomposing the three-dimensional space of the current path planning of the aircraft. Then, the DWA algorithm is used for real-time dynamic collision avoidance in the horizontal plane, and then the DWA algorithm is used for real-time dynamic collision avoidance in the vertical section. The difference between real-time dynamic collision avoidance in the vertical section and the horizontal plane is that the use of the DWA algorithm in the vertical section requires all parameters to be changed to vertical plane parameters.
[0079] For the coupling of planning data of horizontal plane and vertical section, due to the difference in unit magnitude of longitude, latitude and depth of elevation map data, the coupling of planning results of two surfaces is realized by setting weights for the coordinate values planned by two surfaces according to the magnitude difference and performing weighted averaging. At the same time, the coordinate values and the position and speed of dynamic obstacles are updated, and the path planning is continued until the target point is reached.
[0080] When DWA falls into a local optimal solution, the three-dimensional APF algorithm (Artificial Potential Field, APF) is called to calculate the next state of the aircraft according to the aircraft motion model under the constraints of aircraft speed, etc. The aircraft state and the position and speed of dynamic obstacles are updated, and the three-dimensional APF algorithm is repeatedly called until the DWA algorithm is out of the position of the local optimal solution, and then the DWA algorithm is continued for path planning to achieve complete path planning.
[0081] In this embodiment, the obstacle points involved in the subsequent path planning are all obstacle points after the expansion process.
[0082] Further, determining the dynamic obstacles within the environment boundary includes determining the initial position and movement speed of the dynamic obstacles.
[0083] The initial parameters of the spacecraft include the initial position of the spacecraft, the position of the target point, the initial state matrix, the detection range, the kinematic model, the algorithm calling APF flag, and the static and dynamic obstacle safety range.
[0084] The puffing method is: taking the obstacle point as the center, obtaining a spherical obstacle point set based on a preset radius; and forming a spherical obstacle boundary from all spherical obstacle point sets.
[0085] Furthermore, the method for determining the weight parameters of the DWA algorithm is:
[0086] If there is an elevation map obstacle or a dynamic obstacle safety range on the line between the current position point of the aircraft and the target position point, the first flag position is 0; otherwise, it is set to 1;
[0087] If there is an elevation map obstacle within the detection range of the current position of the aircraft, the second flag position is set to 0; otherwise, it is set to 1;
[0088] If both flags are 1, the weight parameter with a high heading weight is selected in the DWA algorithm; otherwise, the weight parameter with a high obstacle distance weight is selected.
[0089] In this embodiment, a horizontal plane is constructed based on the two-dimensional grid formed by the row and column information of the elevation map data and the depth of the current position of the aircraft, and a profile equation shown in the following equation perpendicular to the horizontal plane is calculated based on the straight line between the current position of the aircraft and the target point to obtain the coefficients A, B, C, and D of the profile equation. The profile equation of the vertical profile is expressed as:
[0090] Ax+By+Cz+D=0,
[0091] Where x represents the X-axis coordinate of the vertical section, y represents the Y-axis coordinate of the vertical section, z represents the Z-axis coordinate of the vertical section, A is the X-axis coefficient, B is the Y-axis coefficient, C is the Z-axis coefficient, and D is a constant term;
[0092] Then, all the expanded static and dynamic obstacle points contained in the detection range on the horizontal plane where the current position of the spacecraft is located are intercepted, and the obstacle information on the horizontal plane is summarized.
[0093] Then intercept all the expanded static and dynamic obstacle points contained in the detection range on the section perpendicular to the horizontal plane where the current position of the aircraft and the target point are located, and summarize the obstacle information on the vertical section. Among them, the distance d between the obstacle point and the vertical section is calculated based on the coefficients A, B, C and the constant term D of the section equation:
[0094]
[0095] Where x ob is the X-axis coordinate of the obstacle point, y ob is the Y-axis coordinate of the obstacle point, z ob is the Z-axis coordinate of the obstacle point;
[0096] The marking result of the second marking position is determined based on the comparison result of the distance between the obstacle and the vertical profile on the elevation map and the detection range of the current position point.
[0097] Finally, the current state of the vehicle and the target point position in the horizontal and vertical sections are extracted respectively.
[0098] Furthermore, the one-step collision avoidance path planning method includes sampling the current speed of the spacecraft:
[0099] There is a speed limit during the operation of the aircraft. The horizontal speed of the aircraft V h_m The restrictions are:
[0100] V h_m ={(v h ,ω h )|v h ∈[v h_min ,v h_max ],ω h∈[ω h_min ,ω h_max ]},
[0101] Where v h is the horizontal plane running linear speed, ω h is the angular velocity of the horizontal plane, v h_min is the minimum linear velocity for horizontal operation, v h_max is the maximum linear speed of horizontal plane operation, ω h_min is the minimum angular velocity for horizontal plane operation, ω h_max is the maximum angular velocity of horizontal plane operation;
[0102] Set the sampling period to Δt, and a rectangular area can be formed by the current linear velocity and angular velocity, the maximum linear acceleration and angular acceleration, and the sampling period, that is, the dynamic rectangular window V is obtained. h_d :
[0103]
[0104] Where v h_c is the current horizontal plane linear speed of the aircraft, a hv_max is the maximum linear acceleration of the aircraft on the horizontal plane, ω h_c is the current horizontal angular velocity of the aircraft, a hw_max is the maximum angular acceleration of the aircraft in the horizontal plane;
[0105] The horizontal running speed V that satisfies dynamic obstacle avoidance is obtained h_a :
[0106]
[0107] Where dist(v h ,ω h ) is the distance evaluation subfunction, which represents the minimum distance between the planned path and the obstacle point in the horizontal plane at the current horizontal plane running speed;
[0108] Finally, we get the horizontal plane running speed sampling space V of the spacecraft h_r :
[0109] V h_r =V h_m ∩V h_d ∩V h_a .
[0110] The algorithm samples the speed at a certain sampling interval within the dynamic rectangular window, and performs trajectory inference and evaluation score calculation for each group of sampled speeds, and records all qualified simulation trajectories and their corresponding evaluation scores.
[0111] According to the sampled speed, the next trajectory can be predicted by the vehicle motion model and the set step size:
[0112]
[0113] Where x h_k is the X-axis coordinate of the current position of the spacecraft at step k, y h_k is the Y-axis coordinate of the current position of the spacecraft at step k, θ h_k is the azimuth of the current position of the vehicle at the kth step; dt is the step time.
[0114] (x h_k-1 ,y h_k-1 ,θ h_k-1 ) is the horizontal state of the vehicle in the previous step, (x h_k ,y h_k ,θ h_k ) is the predicted next horizontal state of the spacecraft, dt is the step value, and according to the given forward simulation time, a simulation trajectory under the current sampling speed and acceleration can be obtained.
[0115] Evaluation score calculation. Calculate the azimuth evaluation score, distance evaluation score and speed evaluation score of the simulated trajectory respectively, where the distance evaluation sub-function has been given. head(v h ,ω h ) is the azimuth evaluation subfunction, which is used to evaluate the angle Δθ between the direction of the end point of the current simulation trajectory and the line connecting the target point on the horizontal plane and the current position of the vehicle, and Π-Δθ is used in the evaluation; vel(v h ,ω h ) is a speed evaluation sub-function, and the absolute value of the linear speed of the simulated trajectory can be used as the speed score of the trajectory.
[0116] The final score is calculated by the evaluation function, and the optimal solution is selected. The evaluation function is used to calculate the score G(v h ,ω h ):
[0117] G(v h ,ω h )=α·normal_head(v h ,ω h )+β·normal_dist(v h ,ω h )+γ·normal_vel(v h ,ω h ),
[0118] Where α is the azimuth weight, normal_head(v h ,ω h ) is the azimuth evaluation subfunction head(vh ,ω h ), β is the distance weight, normal_dist(v h ,ω h ) is the distance evaluation subfunction dist(v h ,ω h ), γ is the velocity weight, normal_vel(v h ,ω h ) is the speed evaluation subfunction vel(v h ,ω h )’s normalized value;
[0119] According to the score G(v h ,ω h ) respectively determine the optimal one-step planning results of the collision avoidance path from the current position point to the target position point in the horizontal plane and the vertical section.
[0120] Among them, since the evaluation scores vary greatly, they can be normalized. The normalization method is:
[0121]
[0122] Where head(i) represents the azimuth evaluation sub-function value of the i-th predicted simulation path, n is the total number of predicted simulation paths under the spatial constraints of the horizontal plane running speed sampling within the period; dist(i) represents the distance evaluation sub-function value of the i-th predicted simulation path, and vel(i) represents the speed evaluation sub-function value of the i-th predicted simulation path.
[0123] In this embodiment, a weighted average method is used to couple the optimal results of the one-step planning of two collision avoidance paths; the weight of the weighted average method is set according to the difference in magnitude of the position coordinates of the horizontal plane and the vertical section.
[0124] Embodiment 1:
[0125] Step 1:
[0126] Step (1.1): Obtain elevation map information. The elevation map has a longitude range of 119.0°E to 119.4°E, a latitude range of 23.0°N to 23.4°N, and a depth range of 0m to -100m. The selected elevation map data grid range is 96×96×110. The elevation map data is drawn in the form of points in three-dimensional space. Figure 2 shown.
[0127] Step (1.2): Mark the wall obstacles. Obtain the environment space range based on the longitude, latitude, and depth information, mark the wall obstacles at the boundary of the environment space range, record the location information of the wall obstacles, and draw them in the form of points in the three-dimensional space as shown in the following figure: Figure 3 shown.
[0128] Step (1.3): Obstacle expansion processing of elevation map data. All obstacle points of the original elevation map data are expanded into a spherical obstacle point set with the point as the center and a certain radius.
[0129] Step 2: Add dynamic obstacles and set initial parameters. Adding dynamic obstacles includes setting the initial position, speed, and motion boundary of the dynamic obstacles. In this embodiment, 22 dynamic obstacles are set in the three-dimensional space, such as Figure 4 As shown in the green dots in the middle, the randomly generated dynamic obstacle speeds include the three directions of X, Y, and Z. The initial and target grid point positions of the aircraft are set to (80, 23, 70) and (5, 93, 76), respectively. Figure 4 As shown in the single blue and red dots, the aircraft detection range is set to 20.
[0130] Step 3: Use the DWA algorithm to plan the path in three-dimensional space. Specifically, it includes the following steps:
[0131] Step (3.1): Adaptively change the DWA weight. Determine whether there is a point on the straight line between the current position of the aircraft and the target point within the safety range of the elevation map data obstacle and all current dynamic obstacles. If not, its flag is 1, otherwise it is 0; determine whether there is an elevation map obstacle within the detection range of the current position of the aircraft. If not, its flag is 1, otherwise it is 0. If both flags are 1, change the parameter with a higher heading weight, otherwise change the parameter with a higher distance weight from the obstacle.
[0132] Step (3.2): Expand the dynamic obstacles. Expand all current dynamic obstacle points into a spherical obstacle point set with the point as the center and a certain radius, so as to facilitate the subsequent interception of dynamic obstacle data for the vertical section of the spatial decomposition.
[0133] Step (3.3): Decomposition of three-dimensional space. Specifically including:
[0134] Firstly, a horizontal plane is constructed according to the two-dimensional grid formed by the row and column information of the elevation map data and the depth of the current position of the aircraft, and the profile equation perpendicular to the horizontal is calculated according to the straight line between the current position of the aircraft and the target point to obtain the coefficients of the profile equation.
[0135] In this embodiment, the horizontal plane and vertical section of the spatial decomposition at a certain moment are respectively as follows Figure 5 and Figure 6 shown.
[0136] Intercept all the expanded static and dynamic obstacle points contained in the detection range of the horizontal plane where the current position of the spacecraft is at a certain moment, such as Figure 7 and Figure 8 Then, the obstacle information on the horizontal plane is summarized.
[0137] Then intercept all the expanded static and dynamic obstacle points contained in the detection range on the section perpendicular to the horizontal plane where the line connecting the current position of the aircraft and the target point at a certain moment is located, such as Fig. 9 and Fig.10 Then the obstacle information on the vertical section is summarized.
[0138] Finally, the current state of the vehicle and the target point position in the horizontal and vertical sections are extracted respectively.
[0139] Step (3.4): Use the DWA algorithm to perform real-time dynamic collision avoidance on the horizontal plane.
[0140] Step (3.5): Use the DWA algorithm in the vertical section for real-time dynamic collision avoidance. The algorithm collision avoidance process is similar to step (3.4), except that when using the DWA algorithm in the vertical section, all parameters need to be changed to vertical plane parameters.
[0141] Step (3.6): Coupling of horizontal and vertical profile planning data. Since the unit magnitudes of longitude, latitude and depth of the elevation map data are different, the coupling of the two surface planning results is achieved by setting weights for the x-coordinate values planned for the two surfaces according to the magnitude difference and performing weighted averaging. In this embodiment, one grid of the Z axis is 1m, and one grid of the horizontal longitude and latitude is approximately 426m. Therefore, the calculation method for coupling using weighted averaging is x = (426·x OXY +x XOZ ) / 427, y=y XOY , z=z XOZ In this way, the x, y, z coordinates and the position and speed of the dynamic obstacle are updated, and steps (3.1) to (3.6) are repeated until the target point is reached. OXY is the X-axis coordinate in the OXY plane, x XOZ is the X-axis coordinate in the XOZ plane, y XOY is the Y-axis coordinate in the XOY plane, z XOZ is the Z-axis coordinate in the XOZ plane.
[0142] When DWA falls into a local optimal solution, the 3D Artificial Potential Field (APF) algorithm is called to calculate the next state of the aircraft according to the aircraft motion model under the constraints of aircraft speed and other constraints. The aircraft state and the position and speed of dynamic obstacles are updated, and the 3D APF algorithm is called repeatedly until the DWA algorithm is out of the position of the local optimal solution. Then the DWA algorithm is executed for path planning. The final path is as follows Fig.11 shown.
[0143] Although the present invention is described herein with reference to specific embodiments, it should be understood that these embodiments are merely examples of the principles and applications of the present invention. It should therefore be understood that many modifications may be made to the exemplary embodiments and that other arrangements may be devised without departing from the spirit and scope of the present invention as defined by the appended claims. It should be understood that the various dependent claims and features described herein may be combined in a manner different from that described in the original claims. It should also be understood that the features described in conjunction with a single embodiment may be used in other described embodiments.
Claims
1. A three-dimensional path planning method for underwater vehicles based on spatial decomposition, It is characterized in that include: Determine the elevation map obstacles of the aircraft mission interval based on the elevation map data, and determine the environmental boundaries based on the mission interval; Use wall obstacles to mark the environment boundary and determine the dynamic obstacles within the environment boundary; All wall obstacles, elevation map obstacles and dynamic obstacles are expanded based on obstacle points, and the dynamic obstacle safety range is determined based on the expanded dynamic obstacles; the three-dimensional space of the current path planning of the aircraft is determined by the expanded wall obstacles, expanded elevation map obstacles and dynamic obstacle safety range; The DWA algorithm is used to plan the path in the three-dimensional space of the current path planning of the aircraft, including: Determine the weight parameters of the DWA algorithm based on the current position of the aircraft, the target position, the obstacle position on the elevation map, and the dynamic obstacle safety range; Then, a horizontal plane is constructed according to the depth of the current position point of the aircraft, and a vertical section perpendicular to the horizontal plane is established by a straight line formed by connecting the current position point of the aircraft and the target position point; Obtain all obstacle points on the horizontal plane and vertical section respectively; based on the obtained obstacle points, use the DWA algorithm to calculate the one-step planning results of the collision avoidance path from the current position point to the target position point on the horizontal plane and vertical section respectively; couple the two one-step planning results of the collision avoidance path to obtain the coupled one-step planning path; after the vehicle navigates according to the coupled one-step planning path, update the current position point of the vehicle and the dynamic obstacle safety range, and then continue to use the DWA algorithm for path planning until it reaches the target position point; In the process of using the DWA algorithm for path planning, when the DWA algorithm falls into a local optimal solution, the artificial potential field algorithm is used for path auxiliary planning until it escapes from the local optimal solution position of the DWA algorithm, and then the DWA algorithm is used to continue path planning.
2. The method for three-dimensional path planning of underwater vehicles based on spatial decomposition according to claim 1, It is characterized in that Determining the dynamic obstacles within the environment boundary includes determining the initial position and movement speed of the dynamic obstacles.
3. The method for three-dimensional path planning of underwater vehicles based on spatial decomposition according to claim 1, It is characterized in that The puffing method is: taking the obstacle point as the center, obtaining a spherical obstacle point set based on a preset radius; and forming a spherical obstacle boundary from all spherical obstacle point sets.
4. The method for three-dimensional path planning of underwater vehicles based on spatial decomposition according to claim 1, It is characterized in that The method to determine the weight parameters of the DWA algorithm is: If there is an elevation map obstacle or a dynamic obstacle safety range on the line between the current position point of the aircraft and the target position point, the first flag position is 0; otherwise, it is set to 1; If there is an elevation map obstacle within the detection range of the current position of the aircraft, the second flag position is set to 0; otherwise, it is set to 1; If both flags are 1, the weight parameter with a high heading weight is selected in the DWA algorithm; otherwise, the weight parameter with a high obstacle distance weight is selected.
5. The method for three-dimensional path planning of underwater vehicles based on spatial decomposition according to claim 4, It is characterized in that The profile equation of the vertical profile is expressed as: Ax+By+Cz+D=0, Where x represents the X-axis coordinate of the vertical section, y represents the Y-axis coordinate of the vertical section, z represents the Z-axis coordinate of the vertical section, A is the X-axis coefficient, B is the Y-axis coefficient, C is the Z-axis coefficient, and D is a constant term; Calculate the distance d between the obstacle point and the vertical profile based on the coefficients A, B, C and the constant term D of the profile equation: Where x ob is the X-axis coordinate of the obstacle point, y ob is the Y-axis coordinate of the obstacle point, z ob is the Z-axis coordinate of the obstacle point; The marking result of the second marking position is determined based on the comparison result of the distance between the obstacle and the vertical profile on the elevation map and the detection range of the current position point.
6. The method for three-dimensional path planning of underwater vehicles based on spatial decomposition according to claim 5, It is characterized in that The one-step collision avoidance path planning method includes sampling the current speed of the spacecraft: The horizontal speed of the aircraft V h_m The restrictions are: V h_m ={(v h ,ω h )|v h ∈[v h_min ,v h_max ],ω h ∈[ω h_min ,ω h_max ]}, Where v h is the horizontal plane running linear speed, ω h is the angular velocity of the horizontal plane, v h_min is the minimum linear velocity for horizontal operation, v h_max is the maximum linear speed of horizontal plane operation, ω h_min is the minimum angular velocity for horizontal plane operation, ω h_max is the maximum angular velocity of horizontal plane operation; Set the sampling period to Δt and get the dynamic rectangular window V h_d : Where v h_c is the current horizontal plane linear speed of the aircraft, a hv_max is the maximum linear acceleration of the aircraft on the horizontal plane, ω h_c is the current horizontal angular velocity of the aircraft, a hw_max is the maximum angular acceleration of the aircraft in the horizontal plane; The horizontal running speed V that satisfies dynamic obstacle avoidance is obtained h_a : Where dist(v h ,ω h ) is the distance evaluation subfunction, which represents the minimum distance between the planned path and the obstacle point in the horizontal plane at the current horizontal plane running speed; Finally, we get the horizontal plane running speed sampling space V of the spacecraft hr : V h_r =V h_m ∩V h_d ∩V h_a 。 7. The method for three-dimensional path planning of underwater vehicles based on spatial decomposition according to claim 6, It is characterized in that The one-step collision avoidance path planning method also includes predicting the one-step collision avoidance path planning of the spacecraft: Where x h_k is the X-axis coordinate of the current position of the spacecraft at step k, y h_k is the Y-axis coordinate of the current position of the spacecraft at step k, θ h_k is the azimuth of the current position of the vehicle at the kth step; dt is the step time.
8. The method for three-dimensional path planning of underwater vehicles based on spatial decomposition according to claim 7, It is characterized in that The evaluation function is used to calculate the score G(v h ,ω h ): G(v h ,oh h )=α·normal_head(v h ,oh h )+β·normal_dist(v h ,oh h )+γ·normal_vel(v h ,oh h ), Where α is the azimuth weight, normal_head(v h ,ω h ) is the azimuth evaluation subfunction head(v h ,ω h ), β is the distance weight, normal_dist(v h ,ω h ) is the distance evaluation subfunction dist(v h ,ω h ), γ is the velocity weight, normal_vel(v h ,ω h ) is the speed evaluation subfunction vel(v h ,ω h )’s normalized value; According to the score G(v h ,ω h ) respectively determine the optimal one-step planning results of the collision avoidance path from the current position point to the target position point in the horizontal plane and the vertical section.
9. The method for three-dimensional path planning of underwater vehicles based on spatial decomposition according to claim 8, It is characterized in that Where head(i) represents the azimuth evaluation sub-function value of the i-th predicted simulation path, n is the total number of predicted simulation paths under the spatial constraints of the horizontal plane running speed sampling within the period; dist(i) represents the distance evaluation sub-function value of the i-th predicted simulation path, and vel(i) represents the speed evaluation sub-function value of the i-th predicted simulation path.
10. The method for three-dimensional path planning of underwater vehicles based on spatial decomposition according to claim 9, It is characterized in that A weighted average method is used to couple the optimal results of the one-step planning of two collision avoidance paths; the weight of the weighted average method is set according to the difference in magnitude of the position coordinates of the horizontal plane and the vertical section.
Citation Information
Cited By
Unmanned underwater vehicle online path planning method based on APF-DWA fusion algorithm
CN121857719A
