Decision-making and planning method for unmanned ships based on spatiotemporal synchronous planning

Abstract

The present invention relates to an unmanned ship decision-making and planning method based on spatiotemporal synchronous planning, comprising the following steps: S1, a task survey line or route is issued by the centralized control background according to user application requirements, and the task survey line or route is used as a guide line for decision-making and planning; S2, a list of static obstacles and a list of trajectories of dynamic obstacles over a period of time are subscribed to in real time from the perception unit; S3, an optimal control problem is constructed: a cost function, kinematic constraints, boundary conditions, and collision constraints are defined, and the optimal control problem is solved; S4, a control unit subscribes to the planning results, and the trajectory is divided into a path-time curve and a speed-time curve for closed-loop regulation and tracking. The present invention effectively and simultaneously processes the kinematic constraints of the unmanned ship itself, the static obstacle constraints, and the highly dynamic object constraints, and outputs a safe, efficient, and comfortable planning result. This has good applications for unmanned mapping operations in the environment of a large number of dynamic cargo ships on the Yangtze River and water quality testing operations in dirty lakes.

Description

Technical Field

[0001] The present invention relates to the technical field of unmanned boats, and more particularly to an unmanned boat decision-making and planning method based on spatiotemporal synchronous planning. Background Art

[0002] As unmanned boats continue to gain increasing intelligence, they are poised for widespread application in river and lake mapping, water quality monitoring, marine engineering, and maritime military applications. Decision-making and planning remain key challenges in the development of unmanned boats, particularly intelligent navigation in highly dynamic and static congested environments. Finding a method that can adapt to unknown environments and plan optimal trajectories could significantly improve the safety, efficiency, and economic benefits of unmanned boats.

[0003] Published on May 14, 2021, under the publication number CN 112799405A, this patent application, titled "Path Planning Method for Unmanned Vessels in Dynamic Obstacle Environments," addresses path planning in dynamic environments by improving upon the D*Lite algorithm. However, this method cannot effectively address the planning of high-speed moving objects, particularly dynamic velocity planning while ensuring minimal deviation from the measured line. It also fails to simultaneously account for kinematic constraints to achieve a comfortable and efficient trajectory output.

[0004] The patent application, published on October 14, 2022, with publication number CN 115185262A, is titled "A Method for Rapid Dynamic Obstacle Avoidance Path Planning Based on Minimum Safe Encounter." This method calculates the shortest encounter distance and minimum encounter time for multiple vessels and applies a priority sequence to calculate the next-moment planned speed and direction. However, this method is limited to greedy planning, ignoring the prediction of highly dynamic objects and the kinematic constraints of the vessel. This results in an unsmooth planning output and fails to meet the requirements of close-to-line travel in surveying and water quality operations. Furthermore, this method cannot handle trajectory planning in crowded environments.

[0005] In summary, the difficulties in decision-making and planning for unmanned boats are: (1) the need to consider the motion prediction of objects in a highly dynamic environment so that the planning will not change overnight; (2) the need to consider the kinematic and even dynamic constraints of the unmanned boat itself so that the planning results can be controlled and truly executed; (3) the need to consider efficiency and comfort issues, which can better reflect the intelligent navigation capabilities of the unmanned boat. Summary of the Invention

[0006] The technical problem to be solved by the present invention is to provide an unmanned boat decision-making and planning method based on spatiotemporal synchronous planning. Through a general decision-making and planning method, the unmanned boat simultaneously considers platform motion constraints, obstacle constraints, comfort requirements, efficiency requirements, and safety requirements, and gives a spatiotemporal trajectory that meets the requirements.

[0007] The technical solution adopted by the present invention to solve the technical problem is to construct an unmanned ship decision-making planning method based on spatiotemporal synchronous planning, which includes the following steps:

[0008] S1. The centralized control backend issues the mission survey line or flight route according to the user's application requirements, and uses the mission survey line or flight route as a guideline for decision-making and planning;

[0009] S2, subscribe to the static obstacle list and the trajectory list of dynamic obstacles over a period of time in real time from the perception unit;

[0010] S3. Construct the optimal control problem: define the cost function, kinematic constraints, boundary conditions, and collision constraints, and solve the optimal control problem;

[0011] S4. The control unit subscribes to the planning results and divides the trajectory into a path-time curve and a speed-time curve for closed-loop adjustment and tracking.

[0012] According to the above scheme, step S3 includes the following steps:

[0013] S301, constructing the optimal control problem OCP;

[0014] S302, dynamic programming decision, while opening up convex space to obtain rough solution:

[0015] S303, iterative planning optimization algorithm.

[0016] According to the above solution, in step S301, the optimal control problem OCP is in the following form:

[0017] MinimizeJ(z(t),u(t))

[0018]

[0019] z(t)∈[z min ,z max ],u(t)∈[u min ,u max ],t∈[0,T]

[0020] z(0)=z0,u(0)=u0,z(T)=z T ,u(T)=u T

[0021] h collision (z(t))≤0

[0022] Where z(t) represents the system state vector, z(t) represents [x(t), y(t), θ(t), v(t), a(t)], [x(t), y(t)] expresses the position of the unmanned ship, θ(t) represents the current heading of the unmanned ship, v(t) represents the current longitudinal velocity of the unmanned ship, and a(t) represents the acceleration; u(t) = [jerk(t), ω(t)], where

[0023] According to the above solution, in step S301, the kinematic constraint f kinematics The differential equation relationship describing the state of the unmanned ship is as follows:

[0024]

[0025]

[0026]

[0027]

[0028]

[0029] Collision constraint h collision (z(t)) is the time position of the UAV in the planned trajectory without collision with static and dynamic objects, and T is the planning time.

[0030] According to the above solution, in step S301, the nonlinear collision constraint is simplified to a box constraint, which is specifically in the following form:

[0031]

[0032] J(z(t),u(t)) is the sum of the cost functions required by the planning, and its specific form is as follows:

[0033]

[0034] According to the above solution, in step S302, T represents the planning time, and the time [0, T] is divided into N T Each segment represents the set of positions of the unmanned ship at that time. The time between two adjacent time segments is Δt=T / (N T -1), S represents the mileage of Δt time along the survey line or route direction, L represents the position deviating from the survey line or route, and N is sampled in the S direction. s Segment, L direction sampling N L end, then each layer has nodes N L ×N S The maximum mileage between layers is v max Δt.

[0035] According to the above solution, in step S303, the temporary OCP problem of the iterative planning optimization algorithm is as follows:

[0036]

[0037] z(t)∈[z min ,z max ],u(t)∈[u min ,u max ],t∈[0,T]

[0038] z(0)=z0,u(0)=u0,z(T)=z T ,u(T)=u T

[0039]

[0040] According to the above scheme, the steps of the iterative planning optimization algorithm are as follows:

[0041] S303-a: Initialize χ←traj coarse ,iter←0, that is, input the initial solution and set the number of iterations to 0;

[0042] S303-b: while loop, if iter<iter max , then execute step S303-c, otherwise execute step S303-e;

[0043] S303-c: Construct and solve the temporary OCP problem, obtain the solution χ, and calculate the kinematic constraint violation metric If f penalty If it is less than the set margin, then execute step S303-e; otherwise, execute step S303-d;

[0044] S303-d: Use the current solution χ as the initial solution for the next iteration, recalculate the navigation tunnel, and update the variables: iter←iter+1,w penalty ←αw penalty (α>1), then execute step S303-b;

[0045] S303-e: Extract the trajectory part from the current solution as the final solution of the decision planning.

[0046] The implementation of the unmanned ship decision-making and planning method based on spatiotemporal synchronization planning of the present invention has the following beneficial effects:

[0047] 1. The present invention can meet the intelligent navigation requirements of river surveying and mapping in highly dynamic environments, minimize the number of re-surveys, and be safe, efficient, and comfortable.

[0048] 2. The present invention can meet the navigation requirements of lake water quality monitoring in congested environments and can operate efficiently in a large number of static or dynamic environments;

[0049] 3. Since the present invention takes into account both environmental collision constraints and the platform's own kinematic constraints, intelligent navigation becomes more efficient, energy-saving, safe and comfortable. BRIEF DESCRIPTION OF THE DRAWINGS

[0050] The present invention will be further described below with reference to the accompanying drawings and embodiments, in which:

[0051] Figure 1 It is a flow chart of an iterative optimization algorithm of an unmanned ship decision-making and planning method based on spatiotemporal synchronous planning of the present invention;

[0052] Figure 2 It is the SLT state space of the unmanned ship decision planning method based on spatiotemporal synchronous planning of the present invention. DETAILED DESCRIPTION

[0053] In order to have a clearer understanding of the technical features, purposes and effects of the present invention, specific embodiments of the present invention are now described in detail with reference to the accompanying drawings.

[0054] like Figure 1 As shown, the unmanned ship decision-making and planning method based on spatiotemporal synchronous planning of the present invention includes the following steps:

[0055] S1: The centralized control backend issues the mission measurement line or flight route according to the user's application requirements, which serves as a guideline for decision-making and planning.

[0056] S2: Subscribe to the static obstacle list and the trajectory list of dynamic obstacles over a period of time from the perception unit in real time.

[0057] S3: Construct an optimal control problem, define the cost function, kinematic constraints, boundary conditions, and collision constraints, solve the problem, and output a safe, comfortable, and efficient planning result.

[0058] The specific steps include:

[0059] S301. Construct an optimal control problem (OCP) in the following form:

[0060] MinimizeJ(z(t),u(t))

[0061]

[0062] z(t)∈[z min ,z max ],u(t)∈[u min ,u max],t∈[0,T]

[0063] z(0)=z0,u(0)=u0,z(T)=z T ,u(T)=u T

[0064] h collision (z(t))≤0

[0065] Where z(t) represents the system state vector, specifically [x(t), y(t), θ(t), v(t), a(t)], where [x(t), y(t)] represents the position of the UAV, θ(t) represents the current heading of the UAV, v(t) represents the current longitudinal velocity of the UAV, and a(t) represents the acceleration. u(t) = [jerk(t), ω(t)], where

[0066] Kinematic constraint f kinematics The differential equation relationship describing the state of the unmanned ship is as follows:

[0067]

[0068]

[0069]

[0070]

[0071]

[0072] Collision constraint h collision (z(t)) requires that the unmanned vessel cannot collide with static or dynamic objects at any time during the planned trajectory. T is the planning time. To handle various collision constraint forms and reduce the nonlinear and nonconvex nature of collision constraints, a navigation tunnel is constructed along a reference line. This naturally separates environmental obstacles from the feasible region, unifying various constraint issues and simplifying nonlinear collision constraints into box constraints. The box constraint form is as follows:

[0073]

[0074] J(z(t),u(t)) is the sum of the cost functions required by the plan. Generally speaking, the cost function should include efficiency, that is, driving as close to the reference line as possible; comfort, requiring the longitudinal and lateral accelerations to be as small as possible; and safety, requiring that the planned path cannot collide with static and dynamic objects. The specific form of J(z(t),u(t)) is as follows:

[0075]

[0076] S302: Dynamic programming decision-making, while opening up convex space to obtain a rough solution.

[0077] like Figure 2 As shown, the SLT state space, T represents the planning time, is divided into N T Each fragment represents the set of positions of the unmanned ship at that time. The time between two adjacent time slices is Δt=T / (N T -1). S represents the distance traveled along the survey line or route in Δt time, L represents the position deviating from the survey line or route, and then N is sampled in the S direction. s Segment, L direction sampling N L end, so each layer has nodes N L ×N S The maximum mileage between layers is v max Δt. Dynamic programming DP is suitable for dealing with this kind of sequential decision problem. It can effectively reduce the original search complexity to O((N S ·N T ) NT ) is reduced to O(N T ·(N S ·N T ) 2 ). Therefore, a time-space decision trajectory can be calculated quickly, and then a navigation tunnel can be laid according to the decision path, which simplifies the environmental constraints into convex box constraints. The basic operation of laying a navigation tunnel is to take the points on the decision path as the center of the rectangle, and then expand each side clockwise or counterclockwise from the center of the rectangle until the current rectangle collides with the obstacle, thus forming a tunnel box. Finally, a feasible tunnel is formed.

[0078] S303, iterative planning optimization algorithm

[0079] According to the rough solution {(x i ,y i ,t i )|i=0...N T The interpolated encrypted calculation is used as the initial solution to the OCP problem. Generally speaking, trajectory planning needs to resolve the conflict between intrinsic kinematic constraints and environmental constraints. However, the navigation tunnel provided by the rough solution may not be able to obtain a solution that satisfies the kinematic constraints, resulting in solution failure. To this end, an iterative planning optimization algorithm is designed to improve the rough solution and ensure successful solution. First, the temporary OCP problem is designed as follows:

[0080]

[0081] z(t)∈[z min ,zmax ],u(t)∈[u min ,u max ],t∈[0,T]

[0082] z(0)=z0,u(0)=u0,z(T)=z T ,u(T)=u T

[0083]

[0084] The algorithm flow for solving the temporary OCP problem is as follows:

[0085] S303-a: Initialize χ←traj coarse ,iter←0, that is, input the initial solution and set the number of iterations to 0;

[0086] S303-b: while loop, if iter<iter max , then execute step S303-c, otherwise execute step S303-e;

[0087] S303-c: Construct and solve the temporary OCP problem, obtain the solution χ, and calculate the kinematic constraint violation metric If f penalty If it is less than the set margin, then execute step S303-e; otherwise, execute step S303-d;

[0088] S303-d: Use the current solution χ as the initial solution for the next iteration, recalculate the navigation tunnel, and update the variables: iter←iter+1,w penalty ←αw penalty (α>1), then execute step S303-b;

[0089] S303-e: Extract the trajectory part from the current solution as the final solution of the decision planning.

[0090] S4: The control unit subscribes to the planning results and divides the trajectory into a path-time curve and a speed-time curve for closed-loop adjustment and tracking.

[0091] The present invention's unmanned vessel decision-making and planning method, based on spatiotemporal synchronous planning, is suitable for river mapping tasks in highly dynamic environments, as well as water quality sampling tasks in lakes with dense obstacles. First, a task survey line or route is obtained based on the application requirements, serving as a guideline for decision-making and planning. A list of static obstacles and a list of dynamic obstacle trajectories over a period of time are then subscribed to in real time from the perception unit. An optimal control problem (OCP) for decision-making and planning is constructed, defining a cost function, kinematic constraints, boundary conditions, and collision constraints. Dynamic programming (DP) is performed directly in the three-dimensional space-time domain to obtain real-time decision results, creating a convex space for the OCP and providing an initial solution for planning. A navigation tunnel is constructed based on this, and the nonlinear environmental collision constraints are converted into convex box constraints. An iterative optimal solution (ITOM) algorithm is then used to continuously improve the conflict between environmental constraints and kinematic constraints, outputting a safe, efficient, and comfortable trajectory. Finally, the control unit separates the planned trajectory into a path-time curve and a velocity-time curve for closed-loop tracking. This method effectively and simultaneously addresses the unmanned vessel's own kinematic constraints, static obstacle constraints, and highly dynamic object constraints, and outputs a safe, efficient, and comfortable planning result. This has great applications for unmanned mapping operations in the Yangtze River environment with a large number of dynamic cargo ships and water quality testing operations in dirty lakes.

[0092] The embodiments of the present invention are described above in conjunction with the accompanying drawings, but the present invention is not limited to the above-mentioned specific implementation methods. The above-mentioned specific implementation methods are merely illustrative and not restrictive. Under the guidance of the present invention, ordinary technicians in this field can also make many forms without departing from the scope of protection of the present invention and the claims, all of which are protected by the present invention.

Claims

1. A decision-making and planning method for an unmanned ship based on spatiotemporal synchronous planning, characterized in that: The following steps are involved: S1. The centralized control backend issues the mission survey line or flight route according to the user's application requirements, and uses the mission survey line or flight route as a guideline for decision-making and planning; S2, subscribe to the static obstacle list and the trajectory list of dynamic obstacles over a period of time in real time from the perception unit; S3. Construct the optimal control problem: define the cost function, kinematic constraints, boundary conditions, and collision constraints, and solve the optimal control problem; S4, the control unit subscribes to the planning results, divides the trajectory into a path-time curve and a speed-time curve, and performs closed-loop adjustment and tracking respectively; The step S3 includes the following steps: S301, constructing the optimal control problem OCP; S302, dynamic programming decision, while opening up convex space to obtain rough solution: S303, iterative planning optimization algorithm; In step S301, the optimal control problem OCP is in the following form: MinimizeJ(z(t),u(t)) z(t)∈[z min ,z max ],u(t)∈[u min ,u max ],t∈[0,T] z(0)=z0,u(0)=u0,z(T)=z T ,u(T)=u T h collision (z(t))≤0 Where z(t) represents the system state vector, z(t) represents [x(t), y(t), θ(t), v(t), a(t)], [x(t), y(t)] expresses the position of the unmanned ship, θ(t) represents the current heading of the unmanned ship, v(t) represents the current longitudinal velocity of the unmanned ship, and a(t) represents the acceleration; u(t) = [jerk(t), ω(t)], where In step S301, the kinematic constraint f kinematics The differential equation relationship describing the state of the unmanned ship is as follows: Collision constraint h collision (z(t)) is the time position of the UAV in the planned trajectory without collision with static and dynamic objects, and T is the planning time; In step S301, the nonlinear collision constraint is simplified to a box constraint, which is specifically in the following form: J(z(t),u(t)) is the sum of the cost functions required by the planning, and its specific form is as follows: In step S302, T represents the planning time, and the time [0, T] is divided into N T Each segment represents the set of positions of the unmanned ship at that time. The time between two adjacent time segments is Δt=T / (N T -1), S represents the mileage of Δt time along the survey line or route direction, L represents the position deviating from the survey line or route, and N is sampled in the S direction. s Segment, L direction sampling N L end, then each layer has nodes N L ×N S The maximum mileage between layers is v max Δt; In step S303, the temporary OCP problem of the iterative planning optimization algorithm is as follows: z(t)∈[z min ,z max ],u(t)∈[u min ,u max ],t∈[0,T] z(0)=z0,u(0)=u0,z(T)=z T ,u(T)=u T The steps of the iterative planning optimization algorithm are as follows: S303-a: Initialize χ←traj coarse ,iter←0, that is, input the initial solution and set the number of iterations to 0; S303-b: while loop, if iter<iter max , then execute step S303-c, otherwise execute step S303-e; S303-c: Construct and solve the temporary OCP problem, obtain the solution χ, and calculate the kinematic constraint violation metric If f penalty If it is less than the set margin, then execute step S303-e; otherwise, execute step S303-d; S303-d: Use the current solution χ as the initial solution for the next iteration, recalculate the navigation tunnel, and update the variables: iter←iter+1,w penalty ←αw penalty (α>1), then execute step S303-b; S303-e: Extract the trajectory part from the current solution as the final solution of the decision planning.

Images