A path planning method and device, computer equipment and storage medium

Abstract

The application discloses a path planning method, which comprises the following steps: firstly, an improved motion dynamics A* algorithm is used to perform path planning on an established grid map to generate an initial path, and the speed and acceleration of an amphibious equipment in an air mode and a ground mode are distinguished; then, path points are obtained by sampling the initial path through uniform discrete time, and the mode of the amphibious equipment is switched according to the height of the path points; finally, the initial path is optimized by using a segmented B-spline curve in combination with the gradient information of B-spline control point jump, an Euclidean distance field, speed and acceleration constraints and curvature constraints. The speed and acceleration of the amphibious equipment in the air mode and the ground mode are distinguished by using the improved motion dynamics A* algorithm, so that the amphibious equipment has stronger maneuverability, and the path planning success rate is improved.

Description

Technical Field

[0001] This application relates to the field of path planning technology, and more specifically, to a path planning method, apparatus, computer device, and storage medium. Background Technology

[0002] With the increasing maturity of robotics, vehicle, and drone technologies, more advanced robotic vehicles and drones will be equipped with various weapon systems to perform military missions, thereby reducing personnel casualties on the battlefield. Amphibious equipment, possessing both ground-based walking and air-based flight modes, offers high reconnaissance flexibility, obstacle-crossing capabilities, real-time reconnaissance and surveillance, and operational reconnaissance in dangerous areas. The high mobility of its air mode and the low energy consumption of its ground mode make its applications increasingly widespread, establishing it as a new force in combat reconnaissance. In the research of amphibious equipment, unmanned operation and intelligence are key research directions. Path planning algorithms, as a crucial element of intelligence, generate a safe, collision-free path from the starting point to the destination. Currently, the intelligence level of amphibious equipment still needs improvement; mature autonomous navigation solutions for amphibious equipment on the market are all designed for specific robot configurations.

[0003] A grid map is established as the path planning map. The Kinodynamic A* algorithm, also known as the Kinodynamic A* algorithm, is based on the traditional A* algorithm. It uses motion primitives instead of the four- or eight-connected nodes of A* to connect node states, while ensuring that each grid retains only one node to speed up the search. A motion primitive refers to the trajectory the robot will travel given a control variable (three-dimensional acceleration) and control time, based on a given state (three-dimensional position and three-dimensional velocity information). The motion primitives in three-dimensional space are generated from the control inputs in three dimensions and the current state. To reflect the energy consumption difference between land and air amphibious equipment, an additional penalty term is added to the air motion primitives. The algorithm tends to find a ground-based path, only choosing to cross obstacles in the air when necessary.

[0004] Existing technologies, when generating motion primitives for both the air and ground segments, may fail to reflect the differences in maneuverability between the land and air modes if the same control inputs are used. This leads to several problems, such as: the generation method of air motion primitives cannot fully utilize the high maneuverability characteristics of the air mode; and the underutilization of maneuverability results in suboptimal planned paths. These are among the problems that those skilled in the art need to solve. Summary of the Invention

[0005] The purpose of this application is to overcome the shortcomings of existing technologies and provide a path planning method and device for amphibious equipment. By adopting an improved motion dynamics A* algorithm, it can solve the problem that the air path cannot fully utilize its maneuverability when generating land and air modal paths in the process of amphibious equipment path planning, due to the use of a consistent motion primitive generation method.

[0006] The objective of this application is achieved through the following technical solution:

[0007] Firstly, this application proposes a path planning method for amphibious equipment, including:

[0008] An improved motion dynamics A* algorithm is used to generate an initial path on the established grid map, and the speed and acceleration of the amphibious equipment in air mode and ground mode are distinguished.

[0009] The initial path is sampled using uniform discrete time to obtain path points, and the mode of the amphibious equipment is switched according to the height of the path points.

[0010] By combining the gradient information of B-spline control point jump, Euclidean distance field, velocity acceleration constraint, and curvature constraint, the initial path is optimized using piecewise B-spline curves.

[0011] In one alternative implementation, the step of generating an initial path using an improved motion dynamics A* algorithm on the established grid map includes:

[0012] S1. Obtain the initial start state and the end state;

[0013] S2. Add the initial starting state to the open list;

[0014] S3. Determine whether the number of nodes in the open list is zero. If it is zero, execute S4; otherwise, execute S5.

[0015] S4. Display that no feasible path was found and end the process;

[0016] S5. Select the node with the minimum cost among multiple nodes in the open list as the current node;

[0017] S6. Determine whether the distance from the current node to the endpoint is less than the threshold based on the endpoint status. If it is not less than the threshold, execute S7; otherwise, execute S9.

[0018] S7. Divide multiple discrete accelerations equally according to their directions, and combine them with discrete time to generate multiple motion primitives;

[0019] S8. If the endpoint velocity of a motion primitive exceeds the upper limit of motion capability, it is discarded, and a motion primitive whose endpoint state is located within a grid point is retained in each grid point of the grid map.

[0020] S9. Construct boundary transition cost and heuristic function to obtain amphibious motion control, time and energy costs, process the motion primitives, and return to S3;

[0021] S10. Backtrack the nodes and generate the initial path.

[0022] In one optional implementation, the discrete acceleration is divided into air acceleration and ground acceleration, wherein the air acceleration is greater than the ground acceleration. The steps of dividing multiple discrete accelerations equally according to their directions and generating multiple motion primitives by combining discrete time parameters include:

[0023] Determine whether the current altitude of the amphibious equipment is less than the altitude threshold;

[0024] If it is less than, it is the ground mode. The ground acceleration is divided into equal parts, and the motion primitives of multiple ground modes are obtained by combining discrete time.

[0025] If it is greater than, it is an aerial mode. The aerial acceleration is divided equally, and the motion primitives of multiple aerial modes are obtained by combining discrete time.

[0026] In an alternative implementation, the steps following S7 further include:

[0027] Determine whether the current altitude of the amphibious equipment is less than the altitude threshold;

[0028] If the value is less than the ground mode, it will operate at ground speed.

[0029] If the value is greater than the specified value, it is in aerial mode and operates at aerial speed.

[0030] In one optional implementation, the steps of constructing the boundary transition cost and heuristic function to obtain the control, time, and energy costs of amphibious movement include:

[0031] Multiplying the motion primitive in the air by the flight energy consumption coefficient yields the flight energy loss multiple.

[0032] For the motion primitives of climbing, the additional cost represents the extra energy loss during the mode switching process.

[0033] In one alternative implementation, the amphibious equipment has modes including ground mode, takeoff mode, flight mode, and landing mode.

[0034] Secondly, this application also proposes a path planning device, the device comprising:

[0035] The path search module is used to generate an initial path by performing path planning on the established grid map using an improved motion dynamics A* algorithm, and to distinguish the speed and acceleration of the amphibious equipment in air mode and ground mode.

[0036] The mode switching module is used to sample the initial path through uniform discrete time to obtain path points, and switch the mode of the land and air amphibious equipment according to the height of the path points.

[0037] The trajectory optimization module is used to combine the gradient information of B-spline control point jump, Euclidean distance field, velocity acceleration constraint and curvature constraint, and optimize the initial path using piecewise B-spline curves.

[0038] In an optional implementation, the path search module is further configured to:

[0039] S1. Obtain the initial start state and the end state;

[0040] S2. Add the initial starting state to the open list;

[0041] S3. Determine whether the number of nodes in the open list is zero. If it is zero, execute S4; otherwise, execute S5.

[0042] S4. Display that no feasible path was found and end the process;

[0043] S5. Select the node with the minimum cost among multiple nodes in the open list as the current node;

[0044] S6. Determine whether the distance from the current node to the endpoint is less than the threshold based on the endpoint status. If it is not less than the threshold, execute S7; otherwise, execute S9.

[0045] S7. Divide multiple discrete accelerations equally according to their directions, and combine them with discrete time to generate multiple motion primitives;

[0046] S8. Remove motion primitives whose endpoint speed exceeds the upper limit of motion capability, and retain one motion primitive whose endpoint state is located within a grid point in the grid map;

[0047] S9. Construct boundary transition cost and heuristic function to obtain amphibious motion control, time and energy costs, process the motion primitives, and return to S3;

[0048] S10. Backtrack the nodes and generate the initial path.

[0049] Thirdly, this application also proposes a computer device comprising a processor and a memory, wherein the memory stores a computer program that is loaded and executed by the processor to implement the path planning method as described in any of the first aspects.

[0050] Fourthly, this application also proposes a computer-readable storage medium storing a computer program that is loaded and executed by a processor to implement the path planning method as described in any of the first aspects.

[0051] The main solution and its various further alternatives described above can be freely combined to form multiple solutions, all of which are solutions that can be adopted and are claimed in this application; furthermore, the (non-conflicting alternatives) can also be freely combined with each other and with other alternatives. Those skilled in the art, after understanding the solution of this application, will realize from the prior art and common general knowledge that there are many combinations, all of which are technical solutions to be protected by this application, and will not be exhaustively listed here.

[0052] This application discloses a path planning method, which includes: firstly, generating an initial path on an established grid map using an improved kinematics A* algorithm, distinguishing the velocity and acceleration of amphibious equipment in air and ground modes; then, sampling the initial path using uniform discrete time to obtain path points, and switching the amphibious equipment's mode according to the height of the path points; finally, optimizing the initial path using piecewise B-spline curves by combining gradient information from B-spline control point judder, Euclidean distance field, velocity-acceleration constraints, and curvature constraints. By using the improved kinematics A* algorithm to distinguish the velocity and acceleration of amphibious equipment in air and ground modes, the amphibious equipment gains greater maneuverability, and the success rate of path planning is improved.

[0053] The beneficial effects of the path planning method proposed in this application are as follows: by improving the motion dynamics A* algorithm, the upper limits of speed and acceleration of amphibious equipment in air mode are increased, thereby improving the success rate of path planning. At the same time, considering the distance to obstacles, the mode of amphibious equipment is switched. A larger obstacle distance safety threshold is used in the air segment path to ensure safety performance, and a larger speed and acceleration threshold is given in the air mode to ensure maneuverability. Curvature constraints are added to the ground segment path to ensure tracking performance. In addition, optimizing the initial path can improve the smoothness, safety and maneuverability of the initial path. Attached Figure Description

[0054] Figure 1 A flowchart illustrating the path planning method proposed in an embodiment of this application is shown.

[0055] Figure 2A flowchart of step S100 in an embodiment of this application is shown.

[0056] Figure 3 A comparison diagram of the motion primitive generation results proposed in the embodiments of this application is shown.

[0057] Figure 4 A schematic diagram of a finite state machine during motion, as proposed in an embodiment of this application, is shown.

[0058] Figure 5 A schematic diagram of the mode switching decision proposed in an embodiment of this application is shown. Detailed Implementation

[0059] The following specific examples illustrate the implementation of this application. Those skilled in the art can easily understand other advantages and effects of this application from the content disclosed in this specification. This application can also be implemented or applied through other different specific embodiments, and various details in this specification can also be modified or changed based on different viewpoints and applications without departing from the spirit of this application. It should be noted that, unless otherwise specified, the following embodiments and features in the embodiments can be combined with each other.

[0060] Based on the embodiments in this application, all other embodiments obtained by those skilled in the art without creative effort are within the scope of protection of this application.

[0061] Motion primitives refer to the process of generating a robot's trajectory by providing control variables (three-dimensional acceleration) and control time, given a given state (three-dimensional position information and three-dimensional velocity information).

[0062] In existing technologies, if the same control input is used when generating motion primitives for the air and ground segments, the differences in maneuverability between the land and air modes cannot be reflected, resulting in some problems, such as: the generation method of air motion primitives cannot fully utilize the high maneuverability characteristics of the air mode, and the planned path is not actually optimal due to the lack of full utilization of maneuverability.

[0063] To address the aforementioned issues, this application provides a path planning method. By employing an improved motion dynamics A* algorithm, it can resolve the problem that the use of a consistent motion primitive generation method during the path planning process for amphibious equipment, which prevents the air path from fully utilizing its maneuverability, is addressed in detail below.

[0064] Please refer to Figure 1 , Figure 1The diagram illustrates a flowchart of the path planning method proposed in this application. This method is applied to amphibious equipment, which can be an amphibious robot or a manned aircraft, without limitation. It possesses vertical takeoff and landing capabilities as well as ground mobility. The aerial flight capability is achieved through the equipment's multi-rotor flight system, while the ground mobility is achieved through a tracked drive system. This amphibious equipment can switch between flight and driving modes according to the application scenario, specifically including the following steps:

[0065] Step S100: On the established grid map, the improved motion dynamics A* algorithm is used to perform path planning to generate an initial path, and the speed and acceleration of the amphibious equipment in air mode and ground mode are distinguished.

[0066] Step S200: Sample the initial path using uniform discrete time to obtain path points, and switch the mode of the amphibious equipment based on the height of the path points.

[0067] Step S300: Combining the gradient information of B-spline control point jump, Euclidean distance field, velocity acceleration constraint and curvature constraint, the initial path is optimized using piecewise B-spline curves.

[0068] First, to characterize the trajectory of amphibious equipment in three independent dimensions, a time-parameterized fifth-order polynomial function is used: p dim (t)=a5t 5 +a4t 4 +a3t 3 +a2t 2 +a1t+a0 dim∈[x,y,z], where a0 represents the initial position, a5 to a1 all represent unknown parameters to be solved, and t represents the time parameter.

[0069] Then set the state variables of the amphibious equipment. and control quantity u = [a x a y a z Using state-space equations To describe the system, where The control quantity u can control the state quantity x of the amphibious equipment. The control quantity u includes the control quantity acceleration a, and the state quantity x includes the state quantity position p and velocity v. Therefore, the state quantity position p and velocity v can be controlled by the input control quantity acceleration a.

[0070] If the current state of the amphibious equipment is x(t0) and the control input is u(τ), then to consider the integral over time t, it is represented by a state equation, which is:

[0071]

[0072] Where t0 is the initial time and τ is the control variable. To obtain the position of the next node, since the state equation cannot be used for discrete-time states, the state equation is changed to:

[0073]

[0074] Where p x_next p y_next p z_next These are the positions of the next node on the x-axis, y-axis, and z-axis, respectively. x_next v y_next v z_next p z_next These are the velocities of the next node along the x-axis, y-axis, and z-axis, respectively. 'a' represents acceleration, and 't' represents time. From this, we can obtain the discrete-time position of the next node, and thus the motion state of that node, generating motion primitives.

[0075] based on Figure 1 Please refer to Figure 2 , Figure 2 The flowchart of step S100 in an embodiment of this application is shown. The improved motion dynamics A* algorithm specifically includes the following steps:

[0076] S1. Obtain the initial start state and the end state.

[0077] S2. Add the initial starting state to the open list.

[0078] S3. Determine if the number of nodes in the open list is zero. If it is zero, execute S4; otherwise, execute S5.

[0079] S4. Display "No feasible path found" and end the process.

[0080] S5. Select the node with the minimum cost among multiple nodes in the open list as the current node.

[0081] S6. Determine whether the distance from the current node to the endpoint is less than the threshold based on the endpoint status. If it is not less than the threshold, execute S7; otherwise, execute S9.

[0082] S7. Divide multiple discrete accelerations equally according to their directions, and combine them with discrete time to generate multiple motion primitives.

[0083] S8. Remove motion primitives whose endpoint speed exceeds the upper limit of motion capability, and retain one motion primitive whose endpoint state is located within a grid point in each grid point of the grid map.

[0084] S9. Construct the boundary transition cost and heuristic function to obtain the amphibious motion control, time and energy costs, process the motion primitives, and return to S3.

[0085] S10. Backtrack the nodes and generate the initial path.

[0086] First, the amphibious equipment acquires information about the starting and ending points (initial starting and ending states), such as location or altitude. It then sends the initial starting state to the open list. Next, it checks if the number of nodes in the open list is zero. If zero, it indicates no nodes are being processed or all nodes have been processed, displaying "No feasible path found" and ending the process. If the number of nodes is not zero, it indicates there are still nodes to process. The node with the lowest cost is selected from the open list as the current node, expanded, and then removed from the open list. Finally, based on the ending state, it checks if the distance from the current node to the ending point is less than a threshold. The threshold can be set according to the actual situation. If it is less, it means the current node is close to the ending point. The system then backtracks through all previous nodes, ultimately generating an initial path.

[0087] If the acceleration is greater than the ground acceleration, a motion primitive is generated using the improved A* algorithm. Specifically, multiple discrete accelerations are divided equally according to their directions, and multiple motion primitives are generated by combining discrete time. The discrete accelerations are divided into air acceleration and ground acceleration, and the air acceleration is greater than the ground acceleration. The generation of multiple motion primitives in step S7 includes the following steps:

[0088] Determine whether the current altitude of the amphibious equipment is below the altitude threshold.

[0089] If it is less than, it is the ground mode. The ground acceleration is divided equally, and the motion primitives of multiple ground modes are obtained by combining discrete time.

[0090] If it is greater than, it is an aerial mode. The aerial acceleration is divided equally, and the motion primitives of multiple aerial modes are obtained by combining discrete time.

[0091] When multiple discrete accelerations are input, due to the different motion primitives, each discrete acceleration a will be... max Divide into equal parts in each direction to obtain... (k is a constant). If (2k+1) is generated in discrete time τ... 3 In this context, amphibious equipment operates in two modes: air mode and ground mode. To simultaneously demonstrate the high maneuverability of the air mode and the low energy consumption of the ground mode, the discrete acceleration used in the air mode is higher than that used in the ground mode. Where p z p is the height value of the current node.thr To set a preset height threshold, a g_max For discrete acceleration in the air mode, a f_max This represents the discrete acceleration in the ground mode.

[0092] Please refer to Figure 3 , Figure 3 The diagram shows a comparison of the motion primitive generation results proposed in this application. By controlling the acceleration input of the state space equation, land and air mode motion primitives are generated differently. The smooth black lines indicate the motion primitives generated by using the same acceleration in the air and land modes before the improvement, while the black lines with asterisks indicate the motion primitives generated by using different accelerations in the air and land modes after the improvement. It can be seen from the diagram that the motion primitives generated in the air mode can explore a wider range and produce more aggressive velocity and acceleration, enabling amphibious equipment to fully utilize its superior maneuverability in the air mode.

[0093] Since there are multiple motion primitives, an improved motion dynamics A* algorithm is needed to prune them to ensure the efficiency of the search algorithm. The pruning steps are as follows: if the endpoint velocity of a motion primitive exceeds the upper limit of its motion capability, it is discarded, and one motion primitive whose endpoint state is located within each grid point in the grid map is retained. By applying a consistent upper limit of the endpoint velocity to the current node for pruning, the problem of overly conservative planning paths can be solved while ensuring the safety of the planned paths for amphibious equipment.

[0094] The steps to eliminate motion primitives where the finish speed exceeds the upper limit of athletic ability include:

[0095] Determine whether the current altitude of the amphibious equipment is below the altitude threshold;

[0096] If the value is less than the ground mode, it will operate at ground speed.

[0097] If the speed is greater than the ground speed, it is in air mode and operates at air speed, which is greater than the ground speed.

[0098] Remove motion primitives that exceed the upper limit of motion capability, whether the ground speed or the air speed.

[0099] If the ground mode and the air mode use the same speed, it will greatly reduce the robot's maneuverability. In order to make the improved pruning method use different upper limits of the endpoint speed according to the robot's motion mode, the following is adopted:

[0100]

[0101] Among them, v g_thr For the speed in aerial mode, v f_thrThe speed is for ground mode. The robot's motion mode is distinguished by the height of the final state of the motion primitives. Motion primitives that conform to the speed upper limit of the final state are retained. Since the aerial mode has a larger speed upper limit, it can characterize its strong maneuverability.

[0102] In step S9 above, the control, time, and energy costs of amphibious movement can be obtained by constructing the boundary transition cost and heuristic function. The implementation steps are as follows:

[0103] The boundary transition cost is calculated by multiplying the motion primitive with the endpoint in the air by the flight energy consumption coefficient, representing the energy loss of flight compared to ground travel. An additional cost is added to the motion primitive for altitude climb, representing the additional energy loss during mode switching. Since the costs of time and control are considered, the actual cost function can be defined as: Where u(t) represents the control input, ρ is used as the weight for control time, T is a variable, and g(u(t)) represents the control cost, defined as:

[0104]

[0105] g energy_cost For the energy consumption cost of ground driving, k f f is the flight energy consumption coefficient. switch_cost For the cost of mode switching flight, p z (t) represents the final state height, v z (t) represents the final state velocity.

[0106] If the discretized control quantity u(t) = u d Then the motion state transition is complete, the final state is p, the duration is t, and we can calculate g(u(t)) = g. The cost function of the motion primitive is expressed as: e cost = (g+ρ)t. And from the starting point to the ending point, multiple motion primitives are passed, resulting in...

[0107] Energy optimization is achieved through the execution of heuristic functions, which are based on the Pontryagin minimum principle. These functions solve for the current state x under constraints on the control vector u(t). current To the final state x goal The optimal control is achieved so that the objective function takes the minimum value, where the objective function J(T) is: Solving for it yields the minimum value.

[0108] in

[0109] p dimg p represents the position of the final state. dimcv represents the current position. dimc The velocity at the final state, v dimg Let p* be the velocity of the current state. dim (t) is the position function.

[0110] The objective function J(T) differs between air mode and ground mode, depending on the current state x during the switch. current state variable p z Determine the current flight status for the final target x goal Let be the objective function for the ground-based model. The heuristic term is:

[0111]

[0112] Amphibious equipment needs to overcome gravity to operate in air mode and overcome ground friction to operate on the ground. The energy consumption in air mode is significantly higher than in ground mode, with the energy consumption ratio under different loads corresponding to a flight energy consumption coefficient of approximately 15 to 20. The algorithm will prioritize a ground-based path, only choosing an air path when the cost of detouring through obstacles far outweighs the cost of flying over obstacles after switching modes.

[0113] Existing path search methods do not consider the distance between the aircraft and obstacles, but the heuristic in this application takes this problem into account. Since the motion constraints of amphibious equipment differ between land and air modes, when constructing the optimization cost function, a larger obstacle distance safety threshold is used in the air segment path to ensure safety performance, a larger speed and acceleration threshold is given in the air mode to ensure maneuverability performance, and curvature constraints are added to the ground segment path to ensure tracking performance.

[0114] In step S200, because trajectory optimization is needed to smooth the path, keep the amphibious equipment away from obstacles, and fully utilize its maneuverability, path points need to be obtained by sampling the initial path using uniform discrete time. The parameterized path is a sequence of control points S = {Q0, Q1, ..., Q...} total The third-order uniform B-spline curve is used to divide the initial path into ground driving path, land-air switching-takeoff path, air flight path and land-air switching-landing path according to the height of the path points, and correspondingly switch the mode of the land-air amphibious equipment to ground mode, takeoff mode, flight mode and landing mode.

[0115] Set the control point height to p z The set of control points for ground driving is S. g The set of air traffic control points is S. f The mode switching height threshold is p thr Based on path continuity, the set of control points for each segment of the path is divided as follows:

[0116]

[0117] Let S g There are m segments of ground driving paths. S is a set of continuous ground path control points for the i-th segment, containing gi_count control points. Its first and last control points connect to the start and end states or aerial path control points. f There are n ground driving paths, Let be the set of consecutive air path control points for the i-th segment, with a total of fi_count control points, connected to ground path control points before and after it, resulting in... and

[0118] Please refer to Figure 4 , Figure 4 This diagram illustrates a finite state machine during motion, as proposed in an embodiment of this application. When the ground mode switches to takeoff mode, the current control point Q... i When a point is a ground control point and the next n consecutive points are air control points, this point is the point where the ground mode switches to takeoff mode, resulting in:

[0119] based on Figure 4 Please refer to Figure 5 , Figure 5 This illustration shows a mode switching decision proposed in an embodiment of this application, which automatically switches to air mode after the takeoff mode is completed: When switching from air mode to landing mode, if the current sequence control point is an air control point and the next n consecutive sequence points are ground control points, this point is the switch point from air mode to landing mode, resulting in:

[0120]

[0121] After completing the landing mode, the propellers will automatically stop and the vehicle will switch to ground mode to continue driving. For a trajectory or For ease of description in the subsequent trajectory optimization, the control points are:

[0122] In step S300, the gradient information of B-spline control point jump, Euclidean distance field, velocity acceleration constraint, and curvature constraint is combined to optimize the initial path using piecewise B-spline curves, improving the smoothness, safety, and maneuverability of the initial path. The cost function is first set as: f = λ s f s +λ d f d +λ f (f v +fa )+λ c f c , where f s For the cost of smoothness, f d For the obstacle distance cost, f v f a For the cost of speed and acceleration, f c For the curvature cost, λ s , λ d , λ f , λ c The weights of each cost.

[0123] Smoothness can be quantified using the B-spline control point judder (the derivative of acceleration with respect to time), which is expressed as: in Let Q be the position of node j in the i-th segment of the trajectory. sij+1 Let j be the position of node j+1 in the i-th segment of the trajectory, and so on.

[0124] The obstacle distance cost is the cost of the control point's distance from the nearest obstacle. This cost is used to discourage control points from moving away from obstacles. Obstacle distance cost: The distance from the control point to the nearest obstacle. The obstacle distance cost function for a single control point. This represents the obstacle distance safety threshold. For aerial trajectory segments, considering the wall effect, we assign a larger obstacle distance safety threshold compared to ground trajectory segments.

[0125] Velocity and acceleration costs are used to penalize velocities and accelerations that exceed constraints: where the velocity cost is: Acceleration cost:

[0126] in and To control the point velocity and acceleration, The velocity cost and acceleration cost functions for a single control point. We set the control point speed and acceleration thresholds. Considering the robot's enhanced mobility in flight mode, we assign a higher speed threshold compared to ground mode to better leverage its strong maneuverability.

[0127] When a tracked vehicle moves on the ground, its velocity direction is in the yaw direction, and its forward and inverse kinematics models constrain the magnitudes of velocity and angular velocity. If the trajectory curvature is too large, the tracked vehicle will experience significant tracking errors when turning. A curvature cost term f is used. c To limit the curvature of the ground trajectory: in Control points The curvature is defined as:

[0128]

[0129] Give λ in aerial mode c =0 indicates that in aerial mode, the aircraft can plan large-angle turns and successfully complete tracking without considering additional costs. λ is assigned to ground mode. c >0 indicates that tracked vehicles in ground mode need to consider curvature limitations to reduce turning tracking errors during control.

[0130] The beneficial effects of the path planning method proposed in this application are as follows:

[0131] 1. During the generation of motion primitives, the upper limit of the control input for aerial motion primitives has been increased, which makes aerial motion primitives more maneuverable and improves the success rate of path planning.

[0132] 2. Increase the upper limits of speed and acceleration in aerial mode, so that the generated path has greater maneuverability in the air segment, thereby reducing the algorithm running time, total path time and total path cost.

[0133] The following describes an implementation of a path planning device, which executes the various path planning steps and corresponding technical effects shown in the above embodiments and possible implementations. The path planning device is applied to the aforementioned amphibious equipment and includes:

[0134] The path search module is used to generate an initial path on the established grid map using an improved motion dynamics A* algorithm, and to distinguish the speed and acceleration of amphibious equipment in air mode and ground mode.

[0135] The mode switching module is used to sample the initial path using uniform discrete time to obtain path points, and to switch the mode of the amphibious equipment based on the altitude of the path points.

[0136] The trajectory optimization module is used to combine gradient information from B-spline control point jumps, Euclidean distance fields, velocity-acceleration constraints, and curvature constraints to optimize the initial path using piecewise B-spline curves.

[0137] The path search module is also used for:

[0138] S1. Obtain the initial start state and the end state;

[0139] S2. Add the initial starting state to the open list;

[0140] S3. Determine whether the number of nodes in the open list is zero. If it is zero, execute S4; otherwise, execute S5.

[0141] S4. Display that no feasible path was found and end the process;

[0142] S5. Select the node with the minimum cost among multiple nodes in the open list as the current node;

[0143] S6. Determine whether the distance from the current node to the endpoint is less than the threshold based on the endpoint status. If it is not less than the threshold, execute S7; otherwise, execute S9.

[0144] S7. Divide multiple discrete accelerations equally according to their directions, and combine them with discrete time to generate multiple motion primitives;

[0145] S8. Remove motion primitives whose endpoint speed exceeds the upper limit of motion capability, and retain one motion primitive whose endpoint state is located within a grid point in the grid map;

[0146] S9. Construct boundary transition cost and heuristic function to obtain amphibious motion control, time and energy costs, process the motion primitives, and return to S3;

[0147] S10. Backtrack the nodes and generate the initial path.

[0148] This preferred embodiment provides a computer device that can implement the steps of any embodiment of the path planning method provided in this application. Therefore, it can achieve the beneficial effects of the path planning method provided in this application. For details, please refer to the previous embodiments, which will not be repeated here.

[0149] Those skilled in the art will understand that all or part of the steps in the various methods of the above embodiments can be implemented by instructions, or by instructions controlling related hardware. These instructions can be stored in a computer-readable storage medium and loaded and executed by a processor. Therefore, embodiments of this application provide a storage medium storing multiple instructions that can be loaded by a processor to execute the steps of any embodiment of the path planning method provided in this application.

[0150] The storage medium may include: read-only memory (ROM), random access memory (RAM), disk or optical disk, etc.

[0151] Since the instructions stored in the storage medium can execute the steps in any of the path planning method embodiments provided in this application, the beneficial effects that any of the path planning methods provided in this application can achieve can be realized. For details, please refer to the previous embodiments, which will not be repeated here.

[0152] The above description is merely a preferred embodiment of this application and is not intended to limit this application. Any modifications, equivalent substitutions, and improvements made within the spirit and principles of this application should be included within the protection scope of this application.

Claims

1. A path planning method, characterized in that, Applications in amphibious equipment, including: An improved motion dynamics A* algorithm is used to generate an initial path on the established grid map, and the speed and acceleration of the amphibious equipment in air mode and ground mode are distinguished. The steps for generating initial paths using the improved motion dynamics A* algorithm on the established grid map include: S1. Obtain the initial start state and the end state; S2. Add the initial starting state to the open list; S3. Determine whether the number of nodes in the open list is zero. If it is zero, execute S4; otherwise, execute S5. S4. Display that no feasible path was found and end the process; S5. Select the node with the minimum cost among multiple nodes in the open list as the current node; S6. Determine whether the distance from the current node to the endpoint is less than the threshold based on the endpoint status. If it is not less than the threshold, execute S7; if it is less than the threshold, execute S9. S7. Divide multiple discrete accelerations equally according to their directions and generate multiple motion primitives by combining them with discrete time; the discrete accelerations are divided into air accelerations and ground accelerations, with the air acceleration being greater than the ground acceleration. The steps of dividing multiple discrete accelerations equally according to their directions and generating multiple motion primitives by combining them with discrete time include: Determine whether the current altitude of the amphibious equipment is less than the altitude threshold; If it is less than, it is the ground mode. The ground acceleration is divided into equal parts, and the motion primitives of multiple ground modes are obtained by combining discrete time. If it is greater than, it is an aerial mode. The aerial acceleration is divided into equal parts, and the motion primitives of multiple aerial modes are obtained by combining discrete time. S8. Remove motion primitives whose endpoint speed exceeds the upper limit of motion capability, and retain one motion primitive whose endpoint state is located within each grid point in the grid map; the step of removing motion primitives whose endpoint speed exceeds the upper limit of motion capability further includes: Determine whether the current altitude of the amphibious equipment is less than the altitude threshold; If the value is less than the ground mode, it will operate at ground speed. If the speed is greater than the ground speed, it is in air mode and operates at air speed, which is greater than the ground speed. Remove motion primitives that exceed the upper limit of motion capability, whether the ground speed or the air speed. S9. Construct boundary transition cost and heuristic function to obtain amphibious motion control, time and energy costs, process the motion primitives, and return to S3; S10. Backtrack the nodes and generate the initial path; The initial path is sampled using uniform discrete time to obtain path points, and the mode of the amphibious equipment is switched according to the height of the path points. By combining the gradient information of B-spline control point jump, Euclidean distance field, velocity acceleration constraint, and curvature constraint, the initial path is optimized using piecewise B-spline curves.

2. The path planning method as described in claim 1, characterized in that, The steps for constructing the boundary transition cost and heuristic function to obtain the control, time, and energy costs of amphibious movement on land and in the air include: Multiplying the motion primitive in the air by the flight energy consumption coefficient yields the flight energy loss multiple. For the motion primitives of climbing, the additional cost represents the extra energy loss during the mode switching process.

3. The path planning method as described in claim 1, characterized in that, The elevation of the waypoints divides the initial path into a ground driving path, a land-air transition-takeoff path, an air flight path, and a land-air transition-landing path. The amphibious equipment is available in several modes, including ground mode, takeoff mode, flight mode, and landing mode.

4. A path planning device, characterized in that, The device includes: The path search module is used to generate an initial path on the established grid map using an improved A* motion dynamics algorithm, and to distinguish the speed and acceleration of amphibious equipment in air and ground modes. The steps for generating the initial path on the established grid map using the improved A* motion dynamics algorithm include: S1. Obtain the initial start state and the end state; S2. Add the initial starting state to the open list; S3. Determine whether the number of nodes in the open list is zero. If it is zero, execute S4; otherwise, execute S5. S4. Display that no feasible path was found and end the process; S5. Select the node with the minimum cost among multiple nodes in the open list as the current node; S6. Determine whether the distance from the current node to the endpoint is less than the threshold based on the endpoint status. If it is not less than the threshold, execute S7; if it is less than the threshold, execute S9. S7. Divide multiple discrete accelerations equally according to their directions and generate multiple motion primitives by combining them with discrete time; the discrete accelerations are divided into air accelerations and ground accelerations, with the air acceleration being greater than the ground acceleration. The steps of dividing multiple discrete accelerations equally according to their directions and generating multiple motion primitives by combining them with discrete time include: Determine whether the current altitude of the amphibious equipment is less than the altitude threshold; If it is less than, it is the ground mode. The ground acceleration is divided into equal parts, and the motion primitives of multiple ground modes are obtained by combining discrete time. If it is greater than, it is an aerial mode. The aerial acceleration is divided into equal parts, and the motion primitives of multiple aerial modes are obtained by combining discrete time. S8. Remove motion primitives whose endpoint speed exceeds the upper limit of motion capability, and retain one motion primitive whose endpoint state is located within each grid point in the grid map; the step of removing motion primitives whose endpoint speed exceeds the upper limit of motion capability further includes: Determine whether the current altitude of the amphibious equipment is less than the altitude threshold; If the value is less than the ground mode, it will operate at ground speed. If the speed is greater than the ground speed, it is in air mode and operates at air speed, which is greater than the ground speed. Remove motion primitives that exceed the upper limit of motion capability, whether the ground speed or the air speed. S9. Construct boundary transition cost and heuristic function to obtain amphibious motion control, time and energy costs, process the motion primitives, and return to S3; S10. Backtrack the nodes and generate the initial path; The mode switching module is used to sample the initial path through uniform discrete time to obtain path points, and switch the mode of the land and air amphibious equipment according to the height of the path points. The trajectory optimization module is used to combine the gradient information of B-spline control point jump, Euclidean distance field, velocity acceleration constraint and curvature constraint, and optimize the initial path using piecewise B-spline curves.

5. A computer device, characterized in that, The computer device includes a processor and a memory, the memory storing a computer program that is loaded and executed by the processor to implement the path planning method as described in any one of claims 1-3.

6. A computer-readable storage medium, characterized in that, The storage medium stores a computer program, which is loaded and executed by a processor to implement the path planning method as described in any one of claims 1-3.

Images