Unmanned air cushion boat docking trajectory planning method and system based on mixed A star and safety corridor, medium and program product

Abstract

The invention discloses an unmanned air cushion boat docking trajectory planning method and system based on a mixed A star and a safety corridor, a medium and a program product, and belongs to the field of path planning. According to the method, a mixed A star algorithm is used for searching a path, sampling is carried out in a control space of the unmanned hovercraft, searching is carried out in a state space, it can be guaranteed that the planned path is safe and conforms to the kinematics constraint of the unmanned hovercraft, then the obstacle constraint is converted into geometric space representation by constructing a safe corridor, and therefore the path planning accuracy is improved. On the basis, trajectory optimization conforming to an unmanned hovercraft dynamics model is carried out, a full-size collision-free path for docking of the unmanned hovercraft can be planned, and the full-size collision-free path is optimized into an optimal trajectory conforming to the kinematics and dynamics characteristics of the unmanned hovercraft. According to the method, the kinematics feasibility and obstacle avoidance reliability of the docking track are effectively improved, and the problem can be effectively solved.

Description

Technical Field

[0001] The present invention belongs to the field of path planning, and particularly relates to a method, a system, a medium and a program product for the docking trajectory planning of an unmanned hovercraft based on hybrid A-star and a safety corridor. Background Art

[0002] The development and application of unmanned systems can achieve safer, more energy-efficient and more efficient operations. As an important member of the marine unmanned systems, the unmanned hovercraft plays a crucial role in the development and utilization of the marine field due to its advantages such as intelligence, small size and low cost. After the unmanned hovercraft finishes its operation, it needs to move safely and autonomously into the recovery device of the mother ship or the dock, and trajectory planning is a key technology for its autonomous recovery.

[0003] The trajectory planning of an unmanned hovercraft usually needs to meet the following requirements: the planned trajectory conforms to the kinematic and dynamic models of the unmanned hovercraft; there is no collision between the unmanned hovercraft and obstacles. At the same time, the following key problems exist in the process of the unmanned hovercraft autonomously docking: the recovery device belongs to a narrow and enclosed obstacle environment relative to the sea surface; the distance between the unmanned hovercraft and the recovery device during the autonomous recovery process is small, and full-size collision needs to be considered.

[0004] In the field of path planning, current methods mainly improve or integrate algorithms for specific problems based on classical algorithms, including graph search-based methods such as Dijkstra's algorithm, A* algorithm, JPS algorithm, etc.; sampling-based search methods such as Probabilistic Road Map (PRM), Rapidly-Exploring Random Trees (RRT), etc.; bionics-based intelligent methods such as Ant Colony Optimization (ACO), Particle Swarm Optimization (PSO), Genetic Algorithm (GA), etc.; potential field-based methods such as Artificial Potential Field (APF); methods considering dynamic constraints such as Time Elastic Band (TEB), Dynamic Window Approach (DWA), etc. Among the above classical algorithms, graph search-based methods and sampling-based methods discretize the environment, and it is difficult to directly apply them to meet the kinematic and dynamic requirements of unmanned air-cushion vehicles; bionics-based intelligent methods have a large amount of iterative calculation and relatively poor adaptability to the environment; potential field-based methods and methods considering dynamic constraints are prone to falling into local optimal solutions when facing complex obstacles and cannot handle semi-enclosed obstacles such as dock boundaries, making it difficult to perform well in the problem of unmanned air-cushion vehicle docking.

[0005] In the problem of unmanned air-cushion vehicle trajectory planning, in order to ensure the safety of the feasible path planned by the algorithm, one method is to inflate the obstacles or the unmanned air-cushion vehicle, and the other method is to introduce a safety corridor. The inflation method plays a very effective role in most application scenarios. However, in the problem of unmanned air-cushion vehicle docking, whether the inflation object is an obstacle or an unmanned air-cushion vehicle, there will be a situation where the dock does not meet the collision avoidance constraints, resulting in the inability to complete the trajectory planning of the unmanned air-cushion vehicle docking.

[0006] Based on the above situation, there is a current need for a method that can plan a trajectory that meets the kinematic and dynamic requirements of an unmanned air-cushion vehicle and enables the full-size collision-free docking of the unmanned air-cushion vehicle. Summary of the Invention

[0007] The purpose of the present invention is to provide a method, system, medium and program product for unmanned air-cushion vehicle docking trajectory planning based on hybrid A* and safety corridor, which can effectively improve the kinematic feasibility and obstacle avoidance reliability of the docking trajectory.

[0008] The purpose of the present invention is achieved through the following technical solutions:

[0009] Trajectory planning method for an unmanned hovercraft to enter a dock based on hybrid A-star and safety corridor, the specific steps are as follows:

[0010] Step 1: Determine the information of the unmanned hovercraft during the docking process and convert it into a grid map to complete the initialization operation;

[0011] Step 2: Use the hybrid A-star algorithm to search for a path and obtain 5 uniform forward expansion nodes according to the kinematic equation of the unmanned hovercraft;

[0012] Step 3: Use the size information of the unmanned hovercraft to perform collision detection on the expansion nodes in Step 2; according to the pose information [x, y, ψ] of each expansion node T Calculate the four vertex coordinates of the unmanned hovercraft, and then obtain the contour line of the unmanned hovercraft. Uniformly sample points on the contour line and judge whether all sampled points collide with obstacles. If there is no collision, retain the corresponding expansion node as a feasible node; otherwise, discard the corresponding expansion node;

[0013] Step 4: Directly connect the feasible nodes retained in Step 3 to the target point with a kinematic curve. If the target point cannot be directly reached, add the node to the OPENLIST and calculate the f value corresponding to the feasible node; Pop up the node with the smallest f value in the OPENLIST, and repeat Step 2 and Step 3 until a sub-optimal path without time parameterization is obtained;

[0014] Step 5: Build a convex polygon safety corridor around the path generated by the front-end hybrid A-star, and convert the collision constraint of the obstacle into a spatial constraint of the contour vertices within the safety corridor;

[0015] Step 6: According to the spatial constraint in Step 5, and combined with the speed and acceleration limitations of the unmanned hovercraft, optimize to obtain a trajectory that conforms to the kinematic and dynamic characteristics of the unmanned hovercraft;

[0016] Step 7: Fit the trajectory points of the optimized discrete path, use a piecewise fifth-order polynomial to fit the trajectory points, and obtain the optimal trajectory for the control system to use. The unmanned hovercraft runs along the optimal trajectory.

[0017] Furthermore, the information of the docking process in Step 1 includes the size of the unmanned hovercraft, the starting and ending points of the unmanned hovercraft, the shape and position information of the obstacles and the dock; the initialization operation is to establish the OPENLIST, and at the same time add the starting point of the unmanned hovercraft to the OPENLIST.

[0018] Furthermore, the obtaining of the 5 forward expansion nodes in Step 2 is as follows:

[0019]

[0020] Among them, u is the constant longitudinal velocity, and r is five angular velocities evenly divided according to the minimum turning radius. Thus, the obtained extended nodes are connected to the parent nodes through curves that conform to the kinematics of the unmanned boat.

[0021] Furthermore, the coordinates of the four vertices in step 3 are calculated by the following formula:

[0022]

[0023] Among them, [x A , y A T , [x B , y B T , [x C , y C T , [x D , y D T are the vertex coordinates of the upper left, upper right, lower right, and lower left of the unmanned hovercraft, respectively.

[0024] Furthermore, for the feasible nodes in step 4, the calculation formula for the f value is as follows:

[0025] f(n) = g(n) + h(n)

[0026] Among them, the calculation of the cost function f value includes the trajectory cost g and the heuristic function cost h. The path length from the starting point multiplied by the penalty term is used as the trajectory cost g, and the Dubins curve length from the current node to the target point is used as the heuristic function cost h, so as to obtain the f value considering kinematics for each feasible node.

[0027] Furthermore, in step 5, let the position of the unmanned hovercraft at time k be p k , then the position of the i-th vertex in the hull coordinate system at time k is Then the position of the i-th vertex at time k in the inertial system is:

[0028]

[0029] Among them, R(ψ) is the rotation matrix; thus, the safety corridor constraint of the unmanned hovercraft vertex is expressed as:

[0030]

[0031] Among them, A is the transformation matrix from the hull coordinate system to the inertial system, and b is the spatial constraint of the safety corridor.

[0032] Furthermore, the kinematic constraints of the unmanned hovercraft in discrete form in step 6:

[0033] x k+1 -L(x k ,u k ) = 0, k = 0, ..., N - 1,

[0034] u min ≤ u k ≤ u max , k = 0, 1, ..., N

[0035] a min ≤ a k ≤ a max , k = 0, 1, ..., N - 1

[0036] where L is the kinematic mapping and a is the longitudinal acceleration; the trajectory optimization aims to minimize time and control variation, and constructs the following optimal control problem:

[0037]

[0038] x k+1 -L(x k , u k ) = 0, k = 0, ..., N - 1,

[0039]

[0040] u min ≤ u k ≤ u max , k = 0, 1, ..., N,

[0041] a min ≤ a k ≤ a max , k = 0, 1, ..., N - 1,

[0042] T f > 0

[0043] where J input (u k ) is the control variation cost and J t is the time - term cost.

[0044] A computer device / system, including a memory, a processor, and a computer program stored on the memory, where the processor executes the computer program to implement the steps of the unmanned hovercraft docking trajectory planning method based on hybrid A* and safety corridors.

[0045] A computer - readable storage medium, on which a computer program / instructions are stored, and when the computer program / instructions are executed by a processor, the steps of the unmanned hovercraft docking trajectory planning method based on hybrid A* and safety corridors are implemented.

[0046] A computer program product includes a computer program / instructions, and when the computer program / instructions are executed by a processor, the steps of an unmanned hovercraft docking trajectory planning method based on hybrid A* and safety corridor are implemented.

[0047] The beneficial effects of the present invention are as follows:

[0048] This method uses the hybrid A* algorithm to search for paths, samples in the control space of the unmanned hovercraft, and searches in the state space, which can ensure that the planned path is safe and conforms to the kinematic constraints of the unmanned hovercraft. Subsequently, by constructing a safety corridor, the obstacle constraints are transformed into geometric space representations, and on this basis, trajectory optimization that conforms to the dynamic model of the unmanned hovercraft is carried out, which can plan a full-size collision-free path for the unmanned hovercraft to dock and optimize it into an optimal trajectory that conforms to the kinematic and dynamic characteristics of the unmanned hovercraft. The present invention effectively improves the kinematic feasibility and obstacle avoidance reliability of the docking trajectory and can effectively solve the above problems. Description of the Drawings

[0049] Figure 1 It is a flow chart of the unmanned hovercraft docking trajectory planning method;

[0050] Figure 2 It is a schematic diagram of full-size collision detection of the unmanned hovercraft;

[0051] Figure 3 It is a discrete path diagram of hybrid A* search;

[0052] Figure 4 It is a safety corridor space constraint diagram;

[0053] Figure 5 It is a comparison diagram before and after path optimization (the cyan continuous trajectory is the optimized optimal docking path);

[0054] Figure 6 It is a trend diagram of the longitudinal speed, longitudinal acceleration, and heading angle of the optimized trajectory;

[0055] Figure 7 It is an actual motion path diagram (purple curve) of the unmanned hovercraft in the simulation environment. Specific Embodiments

[0056] The present invention will be further described below with reference to the drawings.

[0057] Embodiment 1:

[0058] Combined with the attached Figures 1 to 4 , the technical solutions in the embodiments of the present invention are described in detail.

[0059] Step 1: Determine the relevant information for the process of the unmanned hovercraft entering the dock, including the size of the unmanned hovercraft, the starting and ending points of the unmanned hovercraft, the shapes and positions of obstacles and the dock. Convert the determined relevant information into a grid map, establish an OPENLIST, and at the same time add the starting point of the unmanned hovercraft to the OPENLIST to complete the initialization operation.

[0060] Step 2: Use the hybrid A* algorithm to search for the docking path. Considering the dynamic constraints during subsequent trajectory optimization, according to the following kinematic equation of the unmanned hovercraft, evenly divide the angular velocity based on the minimum turning radius to obtain 5 different angular velocities, so as to obtain 5 evenly distributed forward expansion nodes.

[0061]

[0062] Among them, u is the constant longitudinal velocity, ψ is the heading angle, r is the 5 angular velocities evenly divided according to the minimum turning radius, and a is the longitudinal acceleration. Thus, the obtained expansion nodes can be connected to the parent node through a curve that conforms to the kinematics of the unmanned hovercraft.

[0063] Step 3: Use the size information of the unmanned hovercraft to perform collision detection on the expansion nodes in Step 2. According to the pose information [x, y, ψ] of each expansion node T , calculate the coordinates of the four vertices of the unmanned hovercraft through coordinate rotation and translation, and then obtain the contour line of the unmanned hovercraft. The coordinates of the four vertices can be calculated by the following formula:

[0064]

[0065] Among them, [x A , y A T , [x B , y B T , [x C , y C T , [x D , y D T are the vertex coordinates of the upper, upper right, lower right, and lower left of the unmanned hovercraft respectively. Then evenly take out several sampling points on the contour line. The selection of the sampling points is as shown in the appendix Figure 2 . Take whether the sampling point is within the obstacle grid as the judgment basis to judge whether the sampling point collides with the obstacle. If there is no collision, retain the corresponding expansion node as a feasible node; otherwise, discard the corresponding expansion node.

[0066] ​​​​Step 4: Try to connect the feasible nodes and the target point retained in Step 3 with a curve that conforms to the kinematics of the unmanned hovercraft. If the connection is successful, it indicates that a feasible docking path has been searched; otherwise, continue the search. When the feasible nodes and the target point cannot be successfully connected, the feasible nodes need to be added to the OPENLIST, and the f value corresponding to the feasible nodes is calculated. The calculation formula is as follows:

[0067] f(n) = g(n) + h(n)

[0068] Among them, the calculation of the cost function f value includes the trajectory cost g and the heuristic function cost h. In the present invention, the path length from the starting point multiplied by the penalty term is used as the trajectory cost g, and the Dubins curve length from the current node to the target point is used as the heuristic function cost h, so as to obtain the f value considering the kinematics of each feasible node. Pop out the node with the smallest f value in the OPENLIST, and repeat Step 2 and Step 3 until a sub-optimal path without time parameterization is obtained. The path given by the actual search of the hybrid A* is shown in the attached Figure 3 figure

[0069] Step 5: Based on the path generated by the front-end hybrid A*, construct a convex polygon safety corridor, and convert the obstacle avoidance constraint into a constraint that the contour vertices are within the safety corridor. Assume that the position of the unmanned hovercraft at time k is p k , then the position of the i-th vertex in the hull coordinate system at time k is Then the position of the i-th vertex at time k in the inertial system is:

[0070]

[0071] In the above formula, R(ψ) is the rotation matrix. Thus, the safety corridor constraint of the unmanned hovercraft vertex can be expressed as:

[0072]

[0073] Among them, A is the transformation matrix from the hull coordinate system to the inertial system, and b is the spatial constraint of the safety corridor. The visualized safety corridor constraint is shown in the attached Figure 4 figure

[0074] Step 6: Under the safety corridor constraint in Step 5, combined with the boundary limitations of the speed and acceleration of the unmanned hovercraft, construct an optimization problem and solve for a trajectory that conforms to the kinematic and dynamic characteristics of the unmanned hovercraft. Since the path obtained by the hybrid A* search is in discrete form, the following gives the kinematic constraints of the unmanned hovercraft in discrete form:

[0075] x k+1 - L(x k , u k ) = 0, k = 0,..., N - 1,

[0076] u min ≤ u k ≤ u max , k = 0, 1, ..., N

[0077] a min ≤ a k ≤ a max , k = 0, 1, ..., N - 1

[0078] Among them, L is the kinematic mapping. The trajectory optimization problem of the present invention aims to minimize time and control variation, and constructs the following optimal control problem:

[0079]

[0080] x k+1 -L(x k , u k ) = 0, k = 0, ..., N - 1,

[0081]

[0082] u min ≤ u k ≤ u max , k = 0, 1, ..., N,

[0083] a min ≤ a k ≤ a max , k = 0, 1, ..., N - 1,

[0084] T f > 0

[0085] Among them, J input (u k ) is the control variation cost, and J t is the time - term cost. The solution of the optimal problem can be quickly calculated through existing solvers, and the solution time of the docking trajectory can be controlled within 500 ms.

[0086] Step 7: The discrete trajectory points solved in Step 6 need to be fitted with trajectory points for the discrete path so that the unmanned hovercraft control module can adjust the control period according to the continuous trajectory. In the present invention, the minimum control input of the unmanned hovercraft can reach the jerk level, so a piece - wise fifth - order polynomial is used to fit the trajectory points to obtain a continuous optimal trajectory.

[0087] Example 2:

[0088] The present invention uses hybrid A-star for front-end path search. The obtained sub-optimal trajectory conforms to the kinematic characteristics of the unmanned hovercraft while meeting the requirement of full-size collision-free docking of the unmanned hovercraft. In the back-end trajectory optimization, based on the spatial constraint of the safety corridor, the construction of the optimal problem is simplified, so that while improving the problem-solving efficiency, the trajectory conforms to the dynamic characteristics of the unmanned hovercraft. Finally, a smooth and continuous autonomous docking trajectory of the unmanned hovercraft is fitted by a piecewise fifth-order polynomial.

[0089] From the comparison of the paths before and after optimization in the appendix Figure 5 it can be seen that the planning method proposed by the present invention can effectively reduce the length of the docking path during the back-end optimization process; from the longitudinal velocity, acceleration and heading angle change curves in the appendix Figure 6 it can be seen that the path optimized by the present invention realizes the smooth transition of the control parameters, avoids the sudden change of the input quantity, and meets the dynamic requirements of the actual docking operation; in the appendix Figure 7 the purple curve represents the actual running trajectory of the unmanned hovercraft in the simulation test. Compared with the trajectory searched by the front-end hybrid A-star, the actual trajectory is smoother and always remains within the safety corridor range, meeting the dynamic requirements of the actual docking operation.

[0090] Embodiment 3:

[0091] The function of the unmanned hovercraft docking trajectory planning system based on hybrid A-star and safety corridor of the present invention can be illustrated by the aforementioned unmanned hovercraft docking trajectory planning method based on hybrid A-star and safety corridor. The system includes a central processing unit (CPU) 301, which can perform various appropriate actions and processes according to the program stored in the read-only memory (ROM) 302 or the program loaded from the storage part 308 into the random access memory (RAM) 303, such as executing the method described in the above embodiments. In the RAM 303, various programs and data required for the operation of the rescue response system are also stored. The CPU 301, ROM 302 and RAM 303 are connected to each other through a bus 304. The input / output (I / O) interface 305 is also connected to the bus 304.

[0092] The following components are connected to the I / O interface 305: an input section 306 including an audio input device, a button switch, etc.; an output section 307 including a liquid crystal display (LCD), an audio output device, an indicator light, etc.; a storage section 308 including a hard disk, etc.; and a communication section 309 including a network interface card such as a LAN (Local Area Network) card, a modem, etc. The communication section 309 performs communication processing via a network such as the Internet. The drive 310 is also connected to the I / O interface 305 as needed. A removable medium 311, such as a magnetic disk, an optical disk, a magneto-optical disk, a semiconductor memory, etc., is mounted on the drive 310 as needed so that a computer program read from it can be installed into the storage section 308 as needed.

[0093] Specifically, according to an embodiment of the present invention, the processes described above with reference to the flowcharts can be implemented as computer software programs. For example, an embodiment of the present invention includes a computer program product that includes a computer program carried on a computer-readable medium, and the computer program includes a computer program for performing the method shown in the flowchart. In such an embodiment, the computer program can be downloaded and installed from a network through the communication section 309, and / or installed from the removable medium 311. When the computer program is executed by a central processing unit (CPU) 301, various functions defined in the present invention are executed.

[0094] It should be noted that specific examples of computer-readable storage media may include, but are not limited to: electrical connections with one or more wires, portable computer disks, hard disks, random access memories (RAMs), read-only memories (ROMs), erasable programmable read-only memories (EPROMs), flash memories, optical fibers, portable compact disc read-only memories (CD-ROMs), optical storage devices, magnetic storage devices, or any suitable combination of the above. In the present invention, a computer-readable storage medium can be any tangible medium that contains or stores a program, and the program can be used by or in combination with an instruction execution system, device, or apparatus.

[0095] Specifically, the unmanned hovercraft docking trajectory planning system based on hybrid A* and safety corridor in this embodiment includes a processor and a memory, and a computer program is stored on the memory. When the computer program is executed by the processor, the unmanned hovercraft docking trajectory planning method provided in the above embodiment is implemented.

[0096] As another aspect, the present invention also provides a computer-readable storage medium, which may be included in the unmanned hovercraft docking trajectory planning system based on hybrid A* and safety corridor described in the above embodiments; or it may exist alone without being assembled into the unmanned hovercraft docking trajectory planning system based on hybrid A* and safety corridor. The above storage medium carries one or more computer programs. When the above one or more computer programs are executed by a processor of the unmanned hovercraft docking trajectory planning system based on hybrid A* and safety corridor, the unmanned hovercraft docking trajectory planning system based on hybrid A* and safety corridor realizes the unmanned hovercraft docking trajectory planning method provided in the above embodiments.

[0097] As described above, the above embodiments are only used to illustrate the technical solutions of the present application, rather than to limit them; although the present application has been described in detail with reference to the foregoing embodiments, those of ordinary skill in the art should understand that they can still modify the technical solutions described in the foregoing embodiments, or perform equivalent replacements on some of the technical features; and these modifications or replacements do not cause the essence of the corresponding technical solutions to deviate from the scope of the technical solutions of the embodiments of the present application.

Claims

1. An undocked hovercraft docking trajectory planning method based on hybrid A-star and safety corridor, characterized in that: The specific steps are as follows: Step 1: Determine the information of the unmanned hovercraft's docking process and convert it into a grid map to complete the initialization operation; Step 2: Use the hybrid A* algorithm to search for a path and obtain 5 evenly spaced forward expansion nodes according to the kinematic equation of the unmanned hovercraft; Step 3: Perform collision detection on the expanded nodes in Step 2 using the size information of the unmanned hovercraft; according to the pose information [x, y, ψ] of each expanded node T Calculate the coordinates of the four vertices of the unmanned hovercraft, and then obtain the contour line of the unmanned hovercraft. Uniformly sample points on the contour line, and determine whether all sampled points collide with obstacles. If there is no collision, retain the corresponding expanded node as a feasible node; otherwise, discard the corresponding expanded node. Step 4: Connect the feasible nodes retained in Step 3 directly to the target point with a kinematic curve. If the target point cannot be reached directly, add the node to the OPENLIST and calculate the f value corresponding to the feasible node; Pop the node with the smallest f value in the OPENLIST, and repeat Steps 2 and 3 until a sub-optimal path without time parameterization is obtained; Step 5: Construct a convex polygon safety corridor around the path generated by the front-end hybrid A*, and convert the collision constraints of the obstacles into spatial constraints where the contour vertices are within the safety corridor; Step 6: According to the spatial constraints in Step 5 and combined with the speed and acceleration limitations of the unmanned hovercraft, optimize to obtain a trajectory that conforms to the kinematic and dynamic characteristics of the unmanned hovercraft; Step 7: Fit the trajectory points of the optimized discrete path, use a piecewise fifth-order polynomial to fit the trajectory points, obtain the optimal trajectory for the control system to use, and the unmanned hovercraft runs along the optimal trajectory.

2. The method for unmanned hovercraft docking trajectory planning based on hybrid A-star and safety corridor according to claim 1, wherein: The information of the docking process in Step 1 includes the size of the unmanned hovercraft, the starting and ending points of the unmanned hovercraft, the shapes and positions of the obstacles and the dock; The initialization operation is to establish the OPENLIST and add the starting point of the unmanned hovercraft to the OPENLIST at the same time.

3. The method for unmanned hovercraft docking trajectory planning based on hybrid A-star and safety corridor according to claim 1, characterized in that: The obtaining of the 5 forward expansion nodes in Step 2 is as follows: Among them, u is the constant longitudinal speed, and r is the 5 angular velocities evenly divided according to the minimum turning radius. Thus, the obtained expansion nodes are connected to the parent node through a curve that conforms to the kinematics of the unmanned boat.

4. The method for unmanned hovercraft docking trajectory planning based on hybrid A-star and safety corridor according to claim 1, wherein: The coordinates of the four vertices in Step 3 are calculated by the following formula: Among them, [x A , y A T , [x B , y B T , [x C , y C T , [x D , y D T are the vertex coordinates of the upper left, upper right, lower right, and lower left of the unmanned hovercraft respectively.​​​​ 5. The method for planning the docking trajectory of an unmanned air-cushion vehicle based on hybrid A-star and safety corridors according to claim 1, characterized in that: The f value corresponding to the feasible node in Step 4 is calculated by the following formula: f(n) = g(n) + h(n) Among them, the calculation of the cost function f value includes the trajectory cost g and the heuristic function cost h. Multiply the path length from the starting point by the penalty term as the trajectory cost g, and use the Dubins curve length from the current node to the target point as the heuristic function cost h, so as to obtain the f value considering kinematics of each feasible node.

6. The method for unmanned hovercraft docking trajectory planning based on hybrid A-star and safety corridor according to claim 1, characterized in that: Step 5: Let the position of the unmanned hovercraft at time k be p k , then the position of the i-th vertex in the hull coordinate system at time k is Then the position of the i-th vertex at time k in the inertial coordinate system is: Among them, R(ψ) is the rotation matrix; Thus, the safety corridor constraint of the unmanned hovercraft vertex is expressed as: Among them, A is the transformation matrix from the hull coordinate system to the inertial system, and b is the spatial constraint of the safety corridor.

7. The method for unmanned hovercraft docking trajectory planning based on hybrid A-star and safety corridor according to claim 1, wherein: The kinematic constraint in discrete form of the unmanned hovercraft in Step 6: x k+1 -L(x k ,u k ) = 0, k = 0, ..., N-1, u min ≤ u k ≤ u max , k = 0, 1, ..., N, a min ≤a k ≤a max , k = 0, 1, ..., N - 1 Among them, L is the kinematic mapping, and a is the longitudinal acceleration; The trajectory optimization takes the minimum time and the minimum control change as the optimization objectives, and constructs the following optimal control problem: s.t. x k+1 -L(x k ,u k ) = 0, k = 0, ..., N-1, u min ≤ u k ≤ u max , k = 0, 1, ..., N, a min ≤a k ≤a max , k = 0, 1, ..., N - 1, T f ＞0 Among them, J input (u k ) is to control the change cost, and J t is the time-term cost.

8. A computer device / apparatus / system, comprising a memory, a processor, and a computer program stored on the memory, characterized in that: The processor executes the computer program to implement the steps of the method described in any one of claims 1 to 7.

9. A computer-readable storage medium having computer programs / instructions stored thereon, characterized in that: When the computer program / instructions are executed by the processor, the steps of the method described in any one of claims 1 to 7 are implemented.

10. A computer program product, comprising a computer program / instructions, characterized in that: When the computer program / instructions are executed by the processor, the steps of the method described in any one of claims 1 to 7 are implemented.

Images