CN-121995919-A - Unmanned ship safe high-speed route planning method under complex unknown environment

CN121995919ACN 121995919 ACN121995919 ACN 121995919ACN-121995919-A

Abstract

The invention discloses a safe high-speed route planning method of an unmanned ship under a complex unknown environment, which relates to the technical field of unmanned ship motion planning and comprises the steps of constructing and solving a track optimization problem aiming at maximization of the speed and track smoothing, generating a high-speed route from a current position to a target point or a projection point of the target point, selecting at least one safety point in a known safety area in a local environment map, generating a safe route from a preset switching point to the safety point on the high-speed route, leading the unmanned ship to navigate along the high-speed route, detecting a front obstacle in real time in the navigation process, controlling the unmanned ship to transfer into the safe route from the preset switching point for traveling when judging that the obstacle cannot be safely avoided along the current high-speed route, continuously attempting to re-plan the high-speed route during the traveling of the safe route, and controlling the unmanned ship to switch back to the high-speed route when conditions are met. The invention can provide an emergency path for local obstacle avoidance while ensuring the optimal global navigational speed.

Inventors

FANG ZHE
HUANG JICHONG
REN YONG
LU CUI

Assignees

中国舰船研究设计中心

Dates

Publication Date: 20260508
Application Date: 20260204

Claims (10)

1. The unmanned ship safe high-speed route planning method under the complex unknown environment is characterized by comprising the following steps of: Constructing and solving a track optimization problem aiming at maximization of the speed and track smoothing according to the current state of the unmanned ship, a local environment map and target point information, generating an optimal route from the current position to the target point or a projection point thereof, and recording the optimal route as a high-speed route; Selecting at least one safety point in a known safety area in the local environment map, constructing and solving a track optimization problem aiming at safely reaching the safety point, and generating a collision-free route from a preset switching point on the high-speed route to the safety point, and recording the collision-free route as a safety route; And enabling the unmanned ship to navigate along the high-speed navigation path, detecting a front obstacle in real time in the navigation process, controlling the unmanned ship to transfer into the safe navigation path from the preset switching point to travel when the unmanned ship is judged to be incapable of safely avoiding the obstacle along the current high-speed navigation path, continuously attempting to re-plan the high-speed navigation path during the travel of the safe navigation path, and controlling the unmanned ship to switch back to the high-speed navigation path when the condition is met.
2. The method of claim 1, wherein said constructing and solving a trajectory optimization problem targeting safe arrival at the safe point comprises: constructing an objective function, wherein the objective function at least comprises a deviation term for minimizing the speed of the unmanned ship along the target direction and the maximum allowable speed, and a smooth term for minimizing the acceleration and the higher-order change rate thereof; And setting constraint conditions, wherein the constraint conditions at least comprise initial state constraint, kinematics and dynamics constraint and obstacle collision prevention constraint based on a local environment map of the unmanned ship.
3. The method according to claim 2, characterized by that, by Obtaining the objective function, wherein T represents the total time step number after the course track is discretized, T represents the discrete time index, For the acceleration of the unmanned ship at the time t, the current position P of the unmanned ship is connected with the target point position P d , and the speed of the unmanned ship is decomposed into the acceleration and the target point position P d Parallel speed At a velocity perpendicular to , Is the speed of the unmanned ship at the time t, The maximum speed of the unmanned ship during navigation is limited according to the thrust of the unmanned ship.
4. The method of claim 1, wherein when the target point is outside the current detection zone of the drone, the method further comprises calculating a target projection point on the boundary of the current detection zone of the drone as a temporary target point for the current planning phase, the target projection point being selected based on a criteria that minimizes an estimated total voyage time from the current location through the temporary target point to the final target point.
5. A method according to claim 3, wherein if the target point is outside the current detection area of the unmanned boat, the target projection point P dr is selected on the detection boundary, and the constraints include: n (0) is a state vector of the unmanned ship at a planning starting time t=0, P is a position, Acceleration, v is speed, H is heading, Representing the time at the last discrete point in the route planning, i.e. t=t 1, Unmanned ship position The target projection points P dr must be reached, B (t) are used to construct a route in the form of a cubic bezier curve, representing the corresponding route point coordinates at t, Represents a control point, K (t) represents a track curvature, describes a degree of curvature of the course curve at t, K 0 represents a maximum allowable curvature, The other is that the distance between the current position P of the unmanned ship and the nearest obstacle O is larger than the minimum safety distance L.
6. A method according to claim 3, wherein if the target point is within the detection region, the constraints include: , The state N (T-1) indicating that the unmanned ship is at the end of the course should be the same as the target point end condition N d , N (0) is a state vector of the unmanned ship at the planning start time t=0, P is a position, Acceleration, v is speed, H is heading, B (t) is constructing a route in the form of a cubic Bezier curve, representing corresponding route point coordinates at t, Represents a control point, K (t) represents a track curvature, describes a degree of curvature of the course curve at t, K 0 represents a maximum allowable curvature, Indicating that the distance between the current position P of the unmanned ship and the nearest obstacle O is greater than the minimum safety distance L.
7. The method of claim 1, wherein the selection of the safety point comprises the point being located within a currently known safety zone and the unmanned boat remaining stationary upon reaching the point.
8. The method of claim 7, wherein the constraints in constructing the trajectory optimization problem for the safe route include: , wherein, Indicating that the velocity at the safety point is zero, Indicating that at the safety point the acceleration is zero, Representing the last discrete point in time t=t in the planned safe route 1, Unmanned ship position A preset safety point S must be reached.
9. The method of claim 8, wherein the location of the predetermined switching point on the highway satisfies that a time interval δt from when the unmanned aerial vehicle passes the point to when the predetermined switching point is reached is greater than a calculation time required to solve the highway trajectory optimization problem.
10. The method of claim 9, wherein the condition for determining that the obstacle cannot be safely avoided along the current highway is that the trajectory optimization problem of the highway fails or is not solved again based on the latest perceived environmental information; During the running of the safe route, the step of re-planning the high-speed route refers to re-executing the global high-speed route planning by taking the real-time position of the unmanned ship on the safe route as a starting point and taking the original target point as an ending point.

Description

Unmanned ship safe high-speed route planning method under complex unknown environment Technical Field The invention relates to the technical field of unmanned ship motion planning, in particular to a unmanned ship safe high-speed route planning method under a complex unknown environment. Background Unmanned boats are an important branch of intelligent unmanned systems, and one of the core manifestations of the level of intelligence is autonomous path planning capability. However, in unknown environments such as dense barriers and complex electromagnetic environments, the unmanned boat can only rely on local sensors to sense the environment, and the following problems exist: global planning limitation is that global algorithms such as A, dijkstra and the like rely on a complete map, and an optimal path is difficult to find under local information; Local planning is insufficient, although local algorithms such as RRT and the like can adapt to a local map, the generated path is often not smooth, and the kinematic and dynamic constraints of the unmanned ship are difficult to meet; the existing method focuses on a single target, for example, the safety track planning only ensures local obstacle avoidance, and the optimal speed planning possibly ignores the risk of unknown obstacles. Therefore, how to realize the collaborative optimization of the unmanned ship high-speed navigation and the dynamic obstacle avoidance when only having the condition of carrying out the route planning by the local map under the complex unknown environment becomes a technical problem to be solved. Disclosure of Invention The embodiment of the invention provides a safe high-speed route planning method for an unmanned ship under a complex unknown environment, which can provide an emergency path for local obstacle avoidance while guaranteeing the optimal global speed. The invention provides a safe high-speed route planning method for an unmanned ship in a complex unknown environment, which comprises the following steps: Constructing and solving a track optimization problem aiming at maximization of the speed and track smoothing according to the current state of the unmanned ship, a local environment map and target point information, generating an optimal route from the current position to the target point or a projection point thereof, and recording the optimal route as a high-speed route; Selecting at least one safety point in a known safety area in the local environment map, constructing and solving a track optimization problem aiming at safely reaching the safety point, and generating a collision-free route from a preset switching point on the high-speed route to the safety point, and recording the collision-free route as a safety route; The unmanned aerial vehicle is made to navigate along the high-speed course, front obstacles are detected in real time in the navigation process, when the unmanned aerial vehicle is judged that the obstacles cannot be safely avoided along the current high-speed course, the unmanned aerial vehicle is controlled to transfer into the safe course from the preset switching point to travel, during the safe course to travel, the high-speed course is continuously tried to be planned again, and when the conditions are met, the unmanned aerial vehicle is controlled to switch back to the high-speed course. In some examples, the constructing and solving a trajectory optimization problem targeting safe arrival at the safe point includes: constructing an objective function, wherein the objective function at least comprises a deviation term for minimizing the speed of the unmanned ship along the target direction and the maximum allowable speed, and a smooth term for minimizing the acceleration and the higher-order change rate thereof; And setting constraint conditions, wherein the constraint conditions at least comprise initial state constraint, kinematics and dynamics constraint and obstacle collision prevention constraint based on a local environment map of the unmanned ship. In some examples, byThe objective function is obtained, wherein,For the acceleration of the unmanned ship at the time t, the current position P of the unmanned ship is connected with the target point position P d, and the speed of the unmanned ship is decomposed into the acceleration and the target point position P dParallel speedAt a velocity perpendicular to,Is the speed of the unmanned ship at the time t,The maximum speed of the unmanned ship during navigation is limited according to the thrust of the unmanned ship. In some examples, when the target point is located outside the current detection area of the unmanned ship, the method further comprises calculating a target projection point on the boundary of the current detection area of the unmanned ship as a temporary target point of the current planning stage, wherein the target projection point is selected according to the selection standard that the estimated total navigation time from