Under-actuated ocean robot heuristic path planning method considering dynamic constraint, program, equipment and storage medium

Abstract

The invention belongs to the technical field of underactuated ocean robot path planning, and particularly relates to an underactuated ocean robot heuristic path planning method considering dynamic constraints, a program, equipment and a storage medium. According to the method, the dynamic environment in the planning process of the underactuated ocean robot and the physical constraint of the underactuated ocean robot are fully considered, and heuristic search is carried out by generating a discretized dynamics prediction trajectory and designing an actual cost function and a heuristic evaluation function as a total cost function, so that the search space is greatly reduced; the planning efficiency and adaptability are improved; according to the method, a pruning function is designed to eliminate paths containing obstacles, a final state estimation function is designed to judge whether the path passes or not and accelerate the path search speed, finally, a safe collision-free path conforming to constraints is planned in a dynamic reachable range, and environmental interference and dynamic characteristics of the under-actuated ocean robot are effectively considered. And the autonomous operation capability of the under-actuated ocean robot is effectively improved.

Description

Technical Field

[0001] The present invention belongs to the technical field of path planning for underactuated marine robots, and in particular relates to a heuristic path planning method, program, device and storage medium for an underactuated marine robot considering dynamic constraints. Background Art

[0002] An unmanned marine vehicle (UMV) is a self-powered robot that autonomously completes tasks in marine environments according to pre-programmed procedures. Its applications in engineering and scientific research are becoming increasingly mature. It primarily includes unmanned underwater vehicles (UUVs) and unmanned surface vehicles (USVs). USVs offer advantages such as intelligence, cost-effectiveness, and long-duration operations. UUVs can overcome the shortcomings of human underwater operations and replace divers in deep-water missions. Based on their mechanical design and degrees of freedom, UMVs can be categorized as fully actuated and underactuated. Underactuated robots have fewer independent control inputs than the controlled degrees of freedom, making path planning particularly important. Path planning is a core technology in UMV research. UMV motion planning includes path planning and trajectory planning. Path planning is a global plan that produces a feasible global path. Trajectory planning is typically performed locally based on the global path, targeting complex obstacle avoidance scenarios or specific constraints.

[0003] Chinese patent application number: CN202211377647.7, named "Unmanned Boat Local Obstacle Avoidance Planning Method, System, Equipment and Medium under Boat Kinematic Constraints", provides an unmanned boat local obstacle avoidance planning method, system, equipment and medium under boat kinematic constraints, to achieve a global optimized route that meets the real-time obstacle avoidance requirements of the unmanned boat. However, the dynamic requirement of this method is to check that the turning angle is greater than the maximum steering angle of the unmanned boat at the current speed. The dynamic part is not sufficiently considered and is difficult to achieve in actual obstacle avoidance.

[0004] Chinese patent application number CN201910041363.2, titled "Energy-saving A-star path planning method considering the influence of ocean currents and unmanned vehicle dynamics," provides an energy-saving path planning method based on the A-type algorithm that considers the influence of ocean currents. This method designs an energy consumption heuristic function that considers the influence of ocean currents, but it still uses eight directions of movement with a grid distance as the planned path. The planned path is still a zigzag line, which is difficult to accurately meet the motion and dynamic constraints of the unmanned vehicle. It requires the addition of a path optimization step to meet practical needs.

[0005] Traditional path planning methods (such as the Dijkstra and A* algorithms) typically do not consider the kinematics and dynamics of the planned agent, treating the agent simply as a particle. However, because unmanned marine vehicles (UMVs) have poorer maneuverability and more complex control interference than drones and unmanned vehicles, and because UMV recovery missions require higher tracking accuracy, it is necessary to incorporate the kinematics and dynamics of marine robots into the front-end path planning method. Furthermore, a global path that conforms to kinematics and dynamics can also reduce the computational burden on the trajectory planner during local trajectory planning. Summary of the Invention

[0006] The object of the present invention is to provide a heuristic path planning method, program, device and storage medium for an underactuated marine robot considering dynamic constraints.

[0007] The heuristic path planning method for an underactuated marine robot considering dynamic constraints includes the following steps:

[0008] Obtain a grid map of the sea area where the under-actuated marine robot is located, map the under-actuated marine robot and obstacles in the grid map, expand each obstacle in the grid map by the volume of the under-actuated marine robot, and abstract the under-actuated marine robot into a mass point;

[0009] Obtaining initial state information of the under-actuated marine robot at an initial starting point and desired state information at a desired end point;

[0010] During the path planning process, each time the next path point is planned, the thrust control range and bow torque control range of the under-actuated marine robot are discretized into multiple values to form multiple control input vectors.

[0011] According to the discrete state space equation of the underactuated marine robot, dynamic prediction is performed for each control input vector to obtain the predicted state vector;

[0012] According to the path point corresponding to the predicted state vector, if the line between the path point and the previous path point passes through the expanded obstacle range, the corresponding control input vector is eliminated;

[0013] For the control input vectors remaining after elimination, calculate the total cost function of each group of control input vectors, including the actual cost function and the heuristic evaluation function;

[0014] Take the control input vector with the minimum total cost function value as the control input vector of the next path point, and determine the next path point based on the predicted state vector corresponding to the control input vector;

[0015] Construct a polynomial trajectory from the next path point to the desired destination. If the polynomial trajectory passes through the expanded obstacle range, the polynomial trajectory is determined to be infeasible and the planning of the next path point is continued. If the polynomial trajectory is feasible, all the path points and polynomial trajectories from the initial starting point to the current planned path are output as the path planning result.

[0016] Furthermore, the initial state information of the under-actuated marine robot at the initial starting point is obtained as follows:

[0017] X0=[x0,y0,ψ0,u0,v0,r0] T

[0018] The expected state information of the underactuated marine robot at the expected end point is:

[0019] X end =[x end ,y end ,ψ end ,u end ,v end ,r end ] T

[0020] Among them, n0=(x0,y0) is the initial starting point of the underactuated marine robot, n end =(x end ,y end ) is the desired end point of the under-actuated marine robot; ψ, u, v, and r are the heading angle, longitudinal velocity, lateral velocity, and angular velocity of the under-actuated marine robot, respectively. The subscripts 0 and end represent the initial starting point and the desired end point, respectively.

[0021] Furthermore, the thrust control range and the bow torque control range of the under-actuated marine robot are discretized into multiple values to form multiple control input vectors, specifically:

[0022] Set the discrete density parameter r to change the thrust control range of the underactuated marine robot [-F umax ,F umax ] and bow torque control range [-T rmax ,T rmax ] are discretized into 2r+1 values, forming (2r+1) 2 Control input vector u i =[F ui ,T ri ] T ;

[0023]

[0024] Furthermore, according to the discrete state space equation of the underactuated marine robot, for each control input vector u i Perform dynamic prediction and obtain the predicted state vector X i(t+1) =[x i(t+1) ,y i(t+1) ,ψ i(t+1) ,u i(t+1) ,v i(t+1) ,r i(t+1) ] T , specifically:

[0025] If t = 0, then X i1 =X0+A 0k X0+B k u i ;

[0026] If t≠0, then X i(t+1) =X t +A tk (X t -X t-1 )+B k (u i -u(t-1));

[0027]

[0028]

[0029] Where I is the identity matrix; is the time step; d 11 d 22 d 33 、m 11 、m 22 、m 33 is the hydrodynamic coefficient of the underactuated marine robot; X t =[x t ,y t ,ψ t ,u t ,v t ,r t ] T represents the state vector of the under-actuated marine robot at the t-th path point; u(t-1) represents the control input vector of the under-actuated marine robot at the t-1th path point.

[0030] Furthermore, the actual cost function g(u i ) is calculated as follows:

[0031] If t = 0, then

[0032] If t≠0, then

[0033] Among them, ||·|| 2 represents the L-2 norm; λ is the time weight parameter; ρ is the input weight parameter; α is the maneuverability weight matrix of the underactuated marine robot, γ is the thrust input weight; ζ is the bow moment input weight.

[0034] Furthermore, the heuristic evaluation function h(X i(t+1) ) is calculated as follows:

[0035]

[0036] Satisfaction τ hit , heuristic evaluation function h(X i(t+1) )=H(τ hit ); and is the weight of the heuristic evaluation function.

[0037] Furthermore, take the corresponding total cost function f(u i )=g(u i )+h(X i(t+1) )The minimum control input vector u i As the control input vector u(t) of the next path point, u i The corresponding predicted state vector X i(t+1) As an underactuated marine robot at the next path point n t+1 =(x t+1 ,y t+1 ) at the state vector X t+1 , according to u i The corresponding τ hit Generate from (x t+1 ,y t+1 ) to the desired end point n end =(x end ,y end ) of the cubic polynomial trajectory P μ (t)=[p μx (t),p μy (t)];

[0038] p μx (t) = a 3x t 3 +a 2x t 2 +a 1x t+a 0x

[0039] p μy (t) = a 3y t3 +a 2y t 2 +a 1y t+a 0y

[0040]

[0041]

[0042] If the next path point n t+1 To the desired destination n end The cubic polynomial trajectory P μ (t) After the expanded obstacle range, the polynomial trajectory is infeasible, set t = t + 1, and continue to plan the next path point;

[0043] If the polynomial trajectory is feasible, the output is from the initial starting point n0 = (x0, y0) to the path point n t+1 =(x t+1 ,y t+1 ) and all path points between path point n t+1 To the desired destination n end The cubic polynomial trajectory P μ (t), as the path planning result.

[0044] A computer device / equipment / system comprises a memory, a processor and a computer program stored in the memory, wherein the processor executes the computer program to implement the steps of the above-mentioned heuristic path planning method for an underactuated marine robot considering dynamic constraints.

[0045] A computer-readable storage medium stores a computer program / instruction thereon, which, when executed by a processor, implements the steps of the above-mentioned heuristic path planning method for an underactuated marine robot considering dynamic constraints.

[0046] A computer program product includes a computer program / instruction, which, when executed by a processor, implements the steps of the above-mentioned heuristic path planning method for underactuated marine robots considering dynamic constraints.

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

[0048] The present invention fully considers the dynamic environment and physical constraints of the under-actuated marine robot during the planning process. By generating a discretized dynamic prediction trajectory and using the designed actual cost function and heuristic evaluation function as the total cost function for heuristic search, the search space is greatly reduced, and the planning efficiency and adaptability are improved. The present invention designs a pruning function to eliminate paths containing obstacles, and designs a final state estimation function to determine whether it passes and speed up the path search speed. Finally, a safe and collision-free path that meets the constraints is planned within the dynamically reachable range. It effectively considers environmental interference and the dynamic characteristics of the under-actuated marine robot, and effectively improves the autonomous operation capability of the under-actuated marine robot. BRIEF DESCRIPTION OF THE DRAWINGS

[0049] Figure 1 It is the overall flow chart of the present invention.

[0050] Figure 2 This is a schematic diagram of path planning during the execution of the present invention.

[0051] Figure 3 It is a diagram for establishing the configuration space of the marine robot.

[0052] Figure 4 is a dense graph of dynamic prediction trajectories.

[0053] Figure 5 It is a schematic diagram of the dynamic prediction trajectory. DETAILED DESCRIPTION

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

[0055] The present invention aims to solve the problem that traditional path planning methods for under-actuated marine robots do not consider the kinematics and dynamics of the planned intelligent body, and the path accuracy is difficult to meet the requirements. It provides a heuristic path planning method for under-actuated marine robots that considers dynamic constraints.

[0056] Heuristic path planning method for underactuated marine robots considering dynamic constraints,

[0057] Step 1: Obtain a grid map of the sea area where the under-actuated marine robot is located, and map the under-actuated marine robot and obstacles on the grid map.

[0058] In the grid map, the obstacles are expanded with the volume of the underactuated marine robot as the unit, and then the underactuated marine robot is abstracted as a mass point. The configuration space of the UMV is established as follows: Figure 3 shown.

[0059] Get the initial state information of the underactuated marine robot X0 = [x0, y0, ψ0, u0, v0, r0] T and expected state information Xend =[x end ,y end ,ψ end ,u end ,v end ,r end ] T ; (x0, y0) is the initial position of the underactuated marine robot, ψ0 is the initial heading angle of the underactuated marine robot, u0, v0, r0 are the initial velocities of the underactuated marine robot;

[0060] Initialize t=0; set discrete density parameter r and time step Input weight parameter ρ, time weight parameter λ, thrust input weight γ, bow moment input weight ζ, heuristic evaluation function weight and

[0061] Step 2: According to the discrete density parameter r, the thrust control range of the underactuated marine robot [-F umax ,F umax ] and bow torque control range [-T rmax ,T rmax ] are discretized into 2r+1 values, a total of (2r+1) 2 Control input vector u i =[F ui ,T ri ] T ;

[0062]

[0063] The discrete density parameter r will greatly affect the density of the dynamic prediction trajectory graph, such as Figure 4 Shown is the prediction effect diagram produced by different discrete density parameters r.

[0064] Step 3: According to the discrete state space equation of the underactuated marine robot, for each control input vector u i Perform dynamic prediction and obtain the predicted state vector X i(t+1) ;

[0065] Assume that the current location of the underactuated marine robot is n c , the current discrete moment is T c , set the reference point n r is the location of the underactuated marine robot at the last discrete moment T0, that is, Assume that the predicted position of the underactuated marine robot at the next discrete moment is n i , where the discrete moment is T i , then Generate dynamic prediction trajectories such as Figure 5 As shown;

[0066] If t = 0, then X i1 =X0+A 0k X0+B k u i

[0067] If t≠0, then X i(t+1) =X t +A tk (X t -X t-1 )+B k (u i -u(t-1))

[0068] in, I is the identity matrix;

[0069]

[0070] Among them, d 11 d 22 d 33 、m 11 、m 22 、m 33 is the hydrodynamic coefficient of the underactuated marine robot; m 11 、m 22 、m 33 For additional quality.

[0071] Step 4: Based on each set of predicted state vectors X i(t+1) Corresponding path point n i(t+1) =(x i(t+1) ,y i(t+1) ), judge n i(t+1) and the previous path point n t =(x t ,y t ) passes through the expanded obstacle range; if so, the corresponding input vector u is removed using the pruning function i ;

[0072] Step 5: For the remaining control input vector u after elimination i , calculate each set of control input vector u i The actual cost function g(u i ), according to the control input vector u i The corresponding predicted state vector X i(t+1) Calculate the heuristic evaluation function h(X i(t+1) ), get each set of input vector u i The total cost function f(ui )=g(u i )+h(X i(t+1) );

[0073] The actual cost function g(u i ) is calculated as follows:

[0074] If t = 0, then

[0075] If t≠0, then

[0076] Among them, ||·|| 2 represents the L-2 norm; α is the maneuverability weight matrix of the underactuated marine robot, ρ and λ are the input weight and time weight parameters, respectively. Their purpose is to adjust the input weight and time weight parameters to regulate the consumption of the planned path on the overall input of the control system and the emphasis on the algorithm time consumption.

[0077] Heuristic evaluation function h(X i(t+1) ) is calculated as follows:

[0078]

[0079] Satisfaction τ hit , heuristic evaluation function h(X i(t+1) )=H(τ hit );

[0080] Step 6: Take the corresponding total cost function f(u i )The minimum control input vector u i As the control input vector u(t) of the next path point, u i The corresponding predicted state vector X i(t+1) As an underactuated marine robot at the next path point n t+1 =(x t+1 ,y t+1 ) at the state vector X t+1 ;

[0081] Using the final state estimation function, according to u i The corresponding τ hit Generate from (x t+1 ,y t+1 ) to the desired end point n end =(x end ,y end ) of the cubic polynomial trajectory P μ (t)=[p μx (t),p μy (t)];

[0082] p μx (t) = a 3x t 3 +a 2x t 2 +a 1x t+a 0x

[0083] p μy (t) = a 3y t 3 +a 2y t 2 +a 1y t+a 0y

[0084]

[0085] Step 7: If the next path point n t+1 To the desired destination n end The cubic polynomial trajectory P μ (t) After the expansion of the obstacle range, that is, the trajectory is not feasible, then set t = t + 1 and return to step 2; otherwise, output from the initial starting point n0 = (x0, y0) to the path point n t+1 =(x t+1 ,y t+1 ) and all path points between path point n t+1 To the desired destination n end The cubic polynomial trajectory P μ (t), as the path planning result.

[0086] The foregoing description is merely a preferred embodiment of the present invention and is not intended to limit the present invention. Those skilled in the art will readily appreciate that various modifications and variations of the present invention are possible. Any modifications, equivalent substitutions, or improvements made within the spirit and principles of the present invention are intended to be within the scope of protection of the present invention.

Claims

1. A heuristic path planning method for underactuated marine robots considering dynamic constraints, characterized by: Obtain a grid map of the sea area where the under-actuated marine robot is located, map the under-actuated marine robot and obstacles in the grid map, expand each obstacle in the grid map by the volume of the under-actuated marine robot, and abstract the under-actuated marine robot into a mass point; Obtaining initial state information of the under-actuated marine robot at an initial starting point and desired state information at a desired end point; During the path planning process, each time the next path point is planned, the thrust control range and bow torque control range of the under-actuated marine robot are discretized into multiple values to form multiple control input vectors. According to the discrete state space equation of the underactuated marine robot, dynamic prediction is performed for each control input vector to obtain the predicted state vector; According to the path point corresponding to the predicted state vector, if the line between the path point and the previous path point passes through the expanded obstacle range, the corresponding control input vector is eliminated; For the control input vectors remaining after elimination, calculate the total cost function of each group of control input vectors, including the actual cost function and the heuristic evaluation function; Take the control input vector with the minimum total cost function value as the control input vector of the next path point, and determine the next path point based on the predicted state vector corresponding to the control input vector; Construct a polynomial trajectory from the next path point to the desired destination. If the polynomial trajectory passes through the expanded obstacle range, the polynomial trajectory is determined to be infeasible and the planning of the next path point is continued. If the polynomial trajectory is feasible, all the path points from the initial starting point to the currently planned path and the polynomial trajectory are output as the path planning result.

2. The heuristic path planning method for an underactuated marine robot considering dynamic constraints according to claim 1, characterized in that: The initial state information of the under-actuated marine robot at the initial starting point is obtained as follows: X0=[x0,y0,ψ0,u0,v0,r0] T The expected state information of the underactuated marine robot at the expected end point is: X end =[x end ,y end ,ψ end ,u end ,v end ,r end ] T Among them, n0=(x0,y0) is the initial starting point of the underactuated marine robot, n end =(x end ,y end ) is the desired end point of the under-actuated marine robot; ψ, u, v, and r are the heading angle, longitudinal velocity, lateral velocity, and angular velocity of the under-actuated marine robot, respectively. The subscripts 0 and end represent the initial starting point and the desired end point, respectively.

3. The heuristic path planning method for an underactuated marine robot considering dynamic constraints according to claim 2, characterized in that: The thrust control range and bow torque control range of the underactuated marine robot are discretized into multiple values to form multiple control input vectors, specifically: Set the discrete density parameter r to change the thrust control range of the underactuated marine robot [-F umax ,F umax ] and bow torque control range [-T rmax ,T rmax ] are discretized into 2r+1 values, forming (2r+1) 2 Control input vector u i =[F ui ,T ri ] T ; 4. The heuristic path planning method for an underactuated marine robot considering dynamic constraints according to claim 3, characterized in that: According to the discrete state space equation of the underactuated marine robot, for each control input vector u i Perform dynamic prediction and obtain the predicted state vector X i(t+1) =[x i(t+1) ,y i(t+1) ,ψ i(t+1) ,u i(t+1) ,v i(t+1) ,r i(t+1) ] T , specifically: If t = 0, then X i1 = X0 + A 0k X0 + B k u i ; If t ≠ 0, then X i(t+1) = X t + A tk (X t - X t-1 ) + B k (u i - u(t - 1)); Where I is the identity matrix; is the time step; d 11 d 22 d 33 、m 11 、m 22 、m 33 is the hydrodynamic coefficient of the underactuated marine robot; X t =[x t ,y t ,ψ t ,u t ,v t ,r t ] T represents the state vector of the under-actuated marine robot at the t-th path point; u(t-1) represents the control input vector of the under-actuated marine robot at the t-1th path point.

5. The heuristic path planning method for an underactuated marine robot considering dynamic constraints according to claim 4, characterized in that: The actual cost function g(u i ) is calculated as follows: If t = 0, then If t≠0, then Among them, ||·|| 2 represents the L-2 norm; λ is the time weight parameter; ρ is the input weight parameter; α is the maneuverability weight matrix of the underactuated marine robot, γ is the thrust input weight; ζ is the bow moment input weight.

6. The heuristic path planning method for an underactuated marine robot considering dynamic constraints according to claim 5, characterized in that: The heuristic evaluation function h(X i(t+1) ) is calculated as follows: Satisfaction τ hit , heuristic evaluation function h(X i(t+1) )=H(τ hit ); and is the weight of the heuristic evaluation function.

7. The heuristic path planning method for an underactuated marine robot considering dynamic constraints according to claim 6, characterized in that: Take the corresponding total cost function f(u i )=g(u i )+h(X i(t+1) )The minimum control input vector u i As the control input vector u(t) of the next path point, u i The corresponding predicted state vector X i(t+1) As an underactuated marine robot at the next path point n t+1 =(x t+1 ,y t+1 ) at the state vector X t+1 , according to u i The corresponding τ hit Generate from (x t+1 ,y t+1 ) to the desired end point n end =(x end ,y end ) of the cubic polynomial trajectory P μ (t)=[p μx (t),p μy (t)]; p μx (t)=a 3x t 3 +a 2x t 2 +a 1x t+a 0x p μy (t)=a 3y t 3 +a 2y t 2 +a 1y t+a 0y If the next path point n t+1 To the desired destination n end The cubic polynomial trajectory P μ (t) After the expanded obstacle range, the polynomial trajectory is infeasible, set t = t + 1, and continue to plan the next path point; If the polynomial trajectory is feasible, the output is from the initial starting point n0 = (x0, y0) to the path point n t+1 =(x t+1 ,y t+1 ) and all path points between path point n t+1 To the desired destination n end The cubic polynomial trajectory P μ (t), as the path planning result.

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

9. A computer-readable storage medium having a computer program / instruction stored thereon, characterized in that: When the computer program / instructions are executed by a processor, the steps of the method according to 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 a processor, the steps of the method according to any one of claims 1 to 7 are implemented.

Images