A target prediction type underwater robot trajectory planning system for dynamic target tracking

Abstract

The application discloses a target prediction type underwater vehicle trajectory planning system for dynamic target tracking, and belongs to the field of underwater vehicles.The system is independently encapsulated and comprises a communication interface, a mission parameter analysis module, a state information management module, a UUV motion constraint modeling module, a target prediction module, a front-end path generation module, a safety corridor construction module, a back-end trajectory optimization module and a control instruction output module.The target prediction module generates a target future short-time prediction trajectory by using a time sequence confidence weighted Bessel regression method.The front-end path generation module combines a target current state and a prediction state to form a dynamic expected tracking point to generate a rough path.The back-end trajectory optimization module jointly optimizes a trajectory shape and a time distribution within a safety corridor to generate a continuous executable trajectory, thereby improving the integration and engineering adaptability of a UUV dynamic target tracking task, and enhancing the continuous tracking capability, obstacle avoidance safety and trajectory execution stability in a complex underwater environment.

Description

Technical Field

[0001] This invention relates to the field of autonomous underwater robots, and more specifically to an underwater robot trajectory planning system for dynamic target tracking. Background Technology

[0002] With the increasing demands for tasks such as marine resource development, underwater security surveillance, seabed facility inspection, and underwater rescue, the autonomous operation capabilities of Unmanned Underwater Vehicles (UUVs) in complex underwater environments have attracted widespread attention. Among these capabilities, dynamic non-cooperative target tracking is one of the key abilities of UUVs in performing underwater detection, surveillance, and accompanying tracking tasks.

[0003] In underwater dynamic target tracking missions, UUVs typically rely on sensors such as sonar to acquire target observation information and combine it with their own navigation status and environmental obstacle information to plan their trajectory. However, the underwater environment is characterized by strong observation noise, limited communication, significant obstacle obstruction, and uncertain target motion, which leads to problems such as unstable target state estimation, lagging trajectory planning, difficulty in local obstacle avoidance, and unsmooth control commands when UUVs are tracking dynamic targets.

[0004] Existing UUV target tracking systems typically consist of a sensing module, an estimation module, a planning module, and a control module. The planning module usually generates a tracking path based on the current target position or filtered estimation results. However, when the target is maneuvering rapidly or the observation information is interfered with by noise, relying solely on the current target state can easily lead to UUV tracking lag, or even target loss. Furthermore, some trajectory planning systems do not adequately consider UUV underactuated motion constraints, turning radius constraints, velocity and acceleration constraints, and hull collision envelope, making it difficult for the generated trajectory to be stably executed by the underlying control system.

[0005] Currently, some patents have been developed focusing on UUV target tracking, underwater maneuvering target estimation, and cooperative encirclement.

[0006] CN120703682A An Interactive Multi-Model Underwater Maneuvering Target Tracking Method and System Based on Pure Azimuth Second-Order EKF

[0007] This invention discloses an interactive multi-model underwater maneuvering target tracking method and system based on a pure azimuth second-order EKF filter, primarily targeting applications where UUVs use passive sonar to detect and track underwater maneuvering targets. The method establishes a discrete state-space model of the pure azimuth target tracking system and employs an interactive multi-model algorithm and a second-order EKF filter to estimate the target state, which can, to some extent, address the problems of strong nonlinearity in pure azimuth measurements and decreased estimation accuracy due to changes in the target's maneuvering state. However, this invention mainly focuses on target state estimation and filtering, and does not further encapsulate the UUV trajectory planning process into a complete trajectory planning system that includes target prediction, motion constraint modeling, path generation, safe corridor construction, and control command output.

[0008] CN116166034B Cross-domain collaborative capture method, apparatus and system

[0009] This invention discloses a cross-domain cooperative encirclement method, apparatus, and system, involving cross-domain cooperation among unmanned aerial vehicles (UAVs), unmanned surface vessels (USVs), and multiple underwater vehicles (UVs). It acquires the position information of each platform through surface and underwater communication and generates corresponding encirclement paths based on the encirclement task, achieving cooperative encirclement of the target. This scheme embodies the concept of cross-domain unmanned system cooperative encirclement, leveraging complementary information from multiple platforms to enhance task execution capabilities. However, this invention primarily focuses on cross-domain multi-platform cooperative relationships and encirclement task organization, without addressing key issues such as target motion prediction, underactuated motion constraints, obstacle avoidance safety, and continuous executable trajectory generation when a single UUV performs real-time trajectory planning for dynamic targets in complex underwater environments. While existing technologies cover pure azimuth underwater target estimation, cross-domain cooperative encirclement, and UUV non-cooperative target prediction and tracking, they still have the following shortcomings: First, they focus primarily on target estimation, cooperative encirclement, or intelligent prediction, lacking an independent trajectory planning system for underwater dynamic target tracking; second, there is insufficient coupling between target prediction results and the trajectory planning process; third, they do not adequately consider UUV underactuated motion constraints, hull collision envelopes, and the generation of continuously executable trajectories in complex obstacle environments; and fourth, there is a lack of modular engineering implementation schemes that can be directly deployed on UUV platforms and connected to sensing, mapping, and underlying control systems. These technical deficiencies severely restrict the mission execution efficiency and practical application value of UUV target tracking systems in complex underwater environments.

[0010] CN118688807A A Non-cooperative Target Tracking Method for UUVs Based on Axial Attention

[0011] This invention proposes a UUV non-cooperative target tracking method based on axial attention. It primarily addresses the unreliability of forward-looking sonar observations and the unpredictability of target motion by constructing a first-order Markov state-space model with memory and a multi-step prediction network based on axial attention Transformer. While this approach improves the UUV's ability to predict and track the motion state of non-cooperative targets, it mainly focuses on target state modeling and intelligent prediction algorithms. It does not further encapsulate this into a modular trajectory planning system that can be directly connected to the UUV's perception system, environmental mapping system, and underlying control system. Furthermore, it does not fully consider engineering planning aspects such as UUV motion constraints, collision envelopes, safety corridors, and back-end trajectory optimization.

[0012] While existing technologies cover pure azimuth underwater target estimation, cross-domain cooperative encirclement, and UUV non-cooperative target prediction and tracking, they still have the following shortcomings: First, they focus primarily on target estimation, cooperative encirclement, or intelligent prediction, lacking an independent trajectory planning system for underwater dynamic target tracking; second, the coupling between target prediction results and the trajectory planning process is insufficient; third, they do not adequately consider the underactuated motion constraints of UUVs, hull collision envelopes, and the generation of continuously executable trajectories in complex obstacle environments; and fourth, they lack a modular engineering implementation scheme that can be directly deployed on a UUV platform and connected to the sensing, mapping, and underlying control systems. These technical deficiencies severely restrict the mission execution efficiency and practical application value of UUV target tracking systems in complex underwater environments. Therefore, this solution proposes a lightweight underwater trajectory planning architecture that integrates a prediction-planning-execution closed loop. Summary of the Invention

[0013] To address the aforementioned problems, this invention discloses a target-predictive underwater robot trajectory planning system for dynamic target tracking.

[0014] The specific technical solution is as follows:

[0015] A target-predictive underwater robot trajectory planning system for dynamic target tracking is provided. The system is independently encapsulated as a trajectory planning calculation unit, serving as an intermediate planning unit between the UUV perception system, environmental mapping system, and underlying control system. It includes a communication interface subsystem, a mission parameter parsing subsystem, a state information management subsystem, a UUV motion constraint modeling subsystem, a target prediction subsystem, a front-end path generation subsystem, a safety corridor construction subsystem, a back-end trajectory optimization subsystem, and a control command output subsystem.

[0016] The communication interface subsystem is used to provide a unified standard input / output interface protocol, receive mission information, submarine status information, target observation information and environmental obstacle information, and output planning status information, target prediction information, expected trajectory point sequence, expected speed, expected heading angle, expected acceleration and fault handling command information.

[0017] The mission parameter parsing subsystem is used to parse the mission task information, obtain planning parameters, and output them.

[0018] The status information management subsystem is used to perform time synchronization, format conversion, caching, and validity judgment on data from different sources, and to maintain UUV status cache, target historical observation queue, environmental map cache, and task parameter cache.

[0019] The UUV motion constraint modeling subsystem is used to establish trajectory planning constraints based on the UUV platform structural parameters and motion performance parameters.

[0020] The target prediction subsystem is used to receive the target historical observation queue and the target observation validity flag, and use the target historical observation information to generate the target's future short-term predicted trajectory.

[0021] The front-end path generation subsystem is used to generate a coarse front-end path that meets obstacle avoidance requirements and motion constraints by combining the target predicted trajectory, UUV motion constraints and environmental obstacle information.

[0022] The safe corridor construction subsystem is used to construct a continuous safe passage area around the coarse path to form a safe corridor;

[0023] The back-end trajectory optimization subsystem is used to jointly optimize the trajectory shape and time allocation within the safety corridor by combining the target prediction results and UUV motion constraints, so as to generate a continuous, smooth and dynamically feasible UUV target tracking trajectory.

[0024] The control command output subsystem is used to convert the optimized trajectory into control commands that can be recognized by the UUV underlying control system and output them.

[0025] Furthermore, the UUV motion constraint modeling subsystem includes an underactuated motion constraint modeling unit and a collision envelope modeling unit. The underactuated motion constraint modeling unit restricts the executability of candidate paths and optimized trajectories based on constraints such as the UUV's maximum speed, maximum acceleration, rate of change of heading angle, range of turning angle, and minimum turning radius. The collision envelope modeling unit uses a dual virtual circle model to describe the safe occupancy area of ​​the UUV hull, approximating the UUV's slender hull as a bow virtual circle and a stern virtual circle, and determines the center position and radius of the two virtual circles based on the hull length, width, and safety margin. It then determines whether the candidate path meets obstacle avoidance safety requirements by detecting the distance relationship between the dual virtual circles and obstacles.

[0026] Furthermore, the target prediction subsystem employs a time-series confidence-weighted Bezier regression method to predict the target trajectory, using an nth-order Bezier curve to describe the predicted trajectory. Time-series confidence weights are assigned to observation information at different timestamps, with higher weights for observations closer to the current time and lower weights for observations farther from the current time. The target prediction subsystem uses a hyperbolic tangent function to calculate the time-series confidence weights; as the time difference between the observation time and the current time increases, the corresponding weight decreases. The target prediction trajectory fitting is transformed into a constrained quadratic programming problem. The optimized objective function consists of an observation fitting residual term and a second-order regularization term. The time-series confidence weights are introduced into the observation fitting residual term to distinguish the confidence levels of observations at different timestamps.

[0027] Furthermore, when new target observation information arrives, the status information management subsystem writes the target observation location, observation timestamp, and observation confidence level into the target historical observation queue; when the queue length exceeds the preset window length, the earliest observation data is deleted; if the target observation information is continuously missing within a preset time, a target observation failure flag is output, and the target prediction subsystem is notified to enter the conservative processing flow.

[0028] Furthermore, when generating a coarse path, the front-end path generation subsystem uses the target's current position and the target's predicted position as planning references, and forms a dynamic expected tracking point by weighting the target's current state and the target's predicted state; it generates candidate motion primitives based on UUV motion constraints, and uses a double virtual circle collision envelope to perform safety checks on the candidate paths, eliminating paths that conflict with obstacles or do not meet motion constraints.

[0029] Furthermore, the safety corridor construction subsystem generates multiple local safety regions along the front-end coarse path and connects adjacent safety regions to form a safety corridor, serving as a spatial constraint for back-end trajectory optimization, ensuring that the optimized continuous trajectory always lies within collision-free free space. The back-end trajectory optimization subsystem transforms the back-end trajectory generation process into a constrained optimization problem, simplifying the decision variables of the optimization problem into intermediate waypoints and segmented times of a piecewise polynomial trajectory. It describes the trajectory quality, spatial safety, and dynamic feasibility through smoothness cost, logarithmic obstacle cost, and aggressive penalty cost. Among these, the smoothness cost minimizes the jerk energy for each segment of the polynomial, the logarithmic obstacle cost ensures that each intermediate waypoint is constrained to the intersection of adjacent corridors, and the aggressive penalty cost encourages shorter total duration and applies soft penalties to portions exceeding velocity and acceleration thresholds.

[0030] Furthermore, the system supports target search mode, conservative predictive tracking mode, and target predictive tracking mode. When target observation information is valid but the confidence level of the target's motion trend is low, the conservative predictive tracking mode is used. When historical target observation information is continuously valid and the prediction confidence level meets the threshold, the target predictive tracking mode is used. If trajectory planning is normal, the control command output subsystem outputs a target tracking control command. If target observation fails but environmental information is normal, it outputs a conservative tracking or re-search control command. If environmental information is abnormal or trajectory optimization fails, it outputs a deceleration, obstacle avoidance, or heading maintenance safety control command.

[0031] The advantages of this invention are as follows: This invention provides an independent trajectory planning system for dynamic target tracking tasks, which can be deployed inside a UUV carrier or in a main control computing platform to connect with sensing, mapping, and low-level control devices, realizing a complete planning function from target observation to control command output. It also provides a unified standard input / output interface and task parameter parsing method, simplifying multi-source data interaction and reducing the difficulty of system development, debugging, and platform adaptation. Furthermore, by using the target prediction subsystem to generate future short-term motion trends using historical target observation information, the UUV no longer relies solely on the target's current position for tracking planning, effectively reducing tracking lag and target loss risks under conditions of rapid target maneuvering, observation noise interference, or short-term occlusion. Moreover, the modular encapsulation of state management, target prediction, path generation, trajectory optimization, and control output facilitates deployment and expansion on different UUV platforms, and also facilitates subsequent upgrades to multi-UUV collaborative tracking, multi-target tracking, and complex task planning scenarios. Attached Figure Description

[0032] Figure 1 This is a hardware connection diagram of the trajectory planning system of the present invention;

[0033] Figure 2 This is the overall flowchart of the system task execution of the present invention;

[0034] Figure 3 This is a topology diagram of the software subsystems of the trajectory planning system of the present invention;

[0035] Figure 4 This is a flowchart illustrating the execution process of the target predictive tracking task of this invention.

[0036] Figure 5 This is a diagram of the target prediction algorithm of the present invention;

[0037] Figure 6 This is a flowchart of the front-end path generation process of the present invention;

[0038] Figure 7 This is a schematic diagram of the back-end trajectory planning of the present invention. Detailed Implementation

[0039] The technical solutions of the embodiments of the present invention will be clearly and completely described below with reference to the accompanying drawings. Obviously, the described embodiments are only some embodiments of the present invention, and not all embodiments. Based on the embodiments of the present invention, all other embodiments obtained by those skilled in the art without creative effort are within the scope of protection of the present invention.

[0040] This invention provides a target-predictive underwater robot trajectory planning system for dynamic target tracking. The hardware connection diagram of the trajectory planning system is shown below. Figure 1 As shown, the system can be independently packaged as a trajectory planning and calculation unit, powered by the UUV carrier power supply module, and connected to the UUV main control system, sonar detection system, environmental mapping system, and underlying motion control system via a communication interface. The trajectory planning system can be equipped with a network port, serial port, power supply port, and debugging and maintenance port. The network port or serial port is used to receive UUV status information, target observation information, and environmental obstacle information, and to output control commands such as the desired trajectory, desired speed, and desired heading angle to the underlying control system.

[0041] Figure 2 This is the overall flowchart of the system's mission execution. When a UUV performs a mission, the mission task is first issued by the surface mother ship, the shore-based command and control center, or the UUV's upper-level mission system. The mission task includes information such as the mission area, target tracking mode, expected tracking distance, safe distance, maximum speed, maximum acceleration, planning period, and return conditions. After the UUV completes deployment and preparation, it sails to the mission area and enters the target search or target tracking phase. During the target search phase, if the sonar detection system has not yet obtained effective target observation information, the system of this invention receives the search path or search heading given by the upper-level mission system and outputs a safe navigation trajectory in conjunction with environmental obstacle information. Once the sonar detection system obtains target observation information, the system automatically enters the dynamic target tracking phase. During the dynamic target tracking phase, the system operates in two modes based on the validity of target observation information: First, when the target observation information is valid but the confidence level of the target's movement trend is low, the system adopts a conservative predictive tracking mode, generating a tracking trajectory based on the target's current position and short-term historical observations. Second, when the target's historical observation information is continuously valid and the prediction confidence level meets the threshold, the system adopts a target predictive tracking mode, generating a forward-looking tracking trajectory based on the target's future short-term predicted trajectory. If target observation information is missing for an extended period, the system outputs a target loss flag and switches to either a re-search or safe waiting state.

[0042] Figure 3This is a topology diagram of the software subsystems of the trajectory planning system of the present invention. The system includes a communication interface subsystem, a mission parameter parsing subsystem, a status information management subsystem, a UUV motion constraint modeling subsystem, a target prediction subsystem, a front-end path generation subsystem, a safety corridor construction subsystem, a back-end trajectory optimization subsystem, and a control command output subsystem.

[0043] The communication interface subsystem provides a unified standard input / output interface protocol to enable data interaction between this trajectory planning system and external UUV systems. Its input information includes mission information, UUV status information, UUV sensor target observation information, environmental obstacle information, and low-level control feedback information; its output information includes planning status information, target prediction information, desired trajectory point sequence, desired velocity, desired heading angle, desired acceleration, and fault handling command information.

[0044] The input mission information consists of mission parameters issued by the upper-level mission system during the mission preparation phase or mission execution. These parameters include information such as mission area, tracking mode, expected tracking distance, maximum speed, maximum acceleration, minimum turning radius, safe distance, prediction time domain, planning period, and mission termination conditions. The information is then output to the mission parameter parsing subsystem.

[0045] The input information about the UUV's own navigation status includes key information used for trajectory planning calculations, such as UTC timestamp, current position, speed, acceleration, heading angle, heading angular velocity, depth, and navigation status.

[0046] The target observation information input to the UUV's sensors is target detection data output by the sonar and inertial navigation fusion sensing system, including key information for target prediction and tracking planning such as target azimuth, estimated target position, estimated target velocity, signal-to-noise ratio, observation timestamp, and observation confidence level.

[0047] The input environmental obstacle information is local environmental information output by the environmental mapping system or obstacle avoidance perception system, including occupied grid map, obstacle boundary or obstacle distance information, etc., which is used for front-end path generation, safe corridor construction and back-end trajectory optimization.

[0048] The output planning status information includes the current planning mode, target prediction validity, path generation status, trajectory optimization status, and control output validity flag, which are used by the upper-level task system to determine the current task execution status of the UUV.

[0049] The output target prediction information includes the target's future predicted trajectory, predicted velocity, predicted confidence level, and predicted timestamp, which can be called by the upper-level task system or other collaborative modules.

[0050] The output control command information is control information generated by this trajectory planning system and sent to the UUV's underlying control system, including the desired trajectory point sequence, desired velocity, desired heading angle, desired acceleration, and trajectory timestamp. This command uses a unified standard format and can be parsed and executed by the underlying control systems of different UUV platforms.

[0051] The mission parameter parsing subsystem parses the mission information input from the communication interface subsystem based on a unified standard mission parameter format. This subsystem supports target search mode, conservative predictive tracking mode, and target predictive tracking mode. The parsing results include planning period, tracking distance, velocity boundary, acceleration boundary, safety distance, prediction time domain, and optimization weights, and are output to the status information management subsystem, target prediction subsystem, front-end path generation subsystem, and back-end trajectory optimization subsystem.

[0052] The status information management subsystem receives the vessel's status information, target observation information, environmental obstacle information, and mission parameter information output by the communication interface subsystem. It performs time synchronization, format conversion, caching, and validity checks on data from different sources. This subsystem maintains a UUV status cache, a target historical observation queue, an environmental map cache, and a mission parameter cache. When new target observation information arrives, the system writes the target observation location, observation timestamp, and observation confidence level into the target historical observation queue. When the queue length exceeds a preset window length, the oldest observation data is deleted. If target observation information is continuously missing within a preset time, a target observation failure flag is output, and the target prediction subsystem is notified to enter a conservative processing flow.

[0053] The UUV motion constraint modeling subsystem is used to establish trajectory planning constraints based on the UUV platform's structural parameters and motion performance parameters. This subsystem includes an underactuated motion constraint modeling unit and a collision envelope modeling unit. The underactuated motion constraint modeling unit restricts the feasibility of candidate paths and optimized trajectories based on constraints such as the UUV's maximum speed, maximum acceleration, rate of change of heading angle, steering angle range, and minimum turning radius. The collision envelope modeling unit uses a dual virtual circle model to describe the safe occupancy area of ​​the UUV hull, approximating the UUV's slender hull as a bow virtual circle and a stern virtual circle, and determining the center positions and radii of the two virtual circles based on the hull length, width, and safety margin. During trajectory planning, the distance relationship between the dual virtual circles and obstacles is detected to determine whether the candidate path meets obstacle avoidance safety requirements.

[0054] The target prediction subsystem employs a time-series confidence-weighted Bezier regression method to predict the target trajectory. It receives the target historical observation queue and target observation validity flag from the state information management subsystem, and generates the target's short-term future movement trend. This subsystem does not rely solely on a single target observation result but utilizes historical target observation information within a certain time window for prediction. To reduce the impact of early observation noise on the prediction results, the subsystem assigns time-series confidence weights to observation information at different timestamps, with observation information closer to the current time having higher weights and observation information farther from the current time having lower weights. The target prediction subsystem outputs the target predicted trajectory, predicted velocity, prediction confidence, and prediction validity flag, and sends them to the front-end path generation subsystem and the back-end trajectory optimization subsystem.

[0055] The front-end path generation subsystem takes into account the current state of the UUV, the current observed state of the target, the predicted trajectory of the target, environmental obstacle information, and UUV motion constraint information to generate a coarse front-end path that meets basic obstacle avoidance requirements and motion constraints. During path generation, this subsystem dynamically constructs desired tracking points, which are jointly determined by the current state and the predicted state of the target. This ensures that the UUV no longer simply moves towards the target's current position but considers the target's short-term future movement trends in advance. Based on the UUV's motion constraints, the front-end path generation subsystem generates candidate motion primitives and uses a double virtual circle collision envelope to perform safety checks on the candidate paths, eliminating paths that conflict with obstacles or do not meet motion constraints, and finally outputs the coarse front-end path.

[0056] The safety corridor construction subsystem receives coarse path and environmental obstacle information output by the front-end path generation subsystem and constructs a continuous safe passage area around the coarse path. This subsystem generates multiple local safe areas along the front-end coarse path and connects adjacent safe areas to form a safety corridor. The safety corridor serves as a spatial constraint for back-end trajectory optimization, ensuring that the optimized continuous trajectory always remains within collision-free free space, preventing collision risks due to deviation from the safe area during trajectory smoothing.

[0057] The back-end trajectory optimization subsystem receives the safety corridor, the front-end coarse path, the target prediction results, and UUV motion constraint information, and generates a continuous, smooth, and dynamically feasible UUV target tracking trajectory. This subsystem transforms the front-end coarse path into a continuous trajectory representation and jointly optimizes the trajectory shape and time allocation. The optimization process comprehensively considers trajectory smoothness, flight time, velocity constraints, acceleration constraints, safety corridor constraints, and target tracking requirements. The trajectory output by the back-end trajectory optimization subsystem features continuous differentiability, smooth heading changes, reasonable velocity changes, and obstacle avoidance safety, reducing the execution complexity of the underlying control system.

[0058] The control command output subsystem receives the optimized trajectory output by the backend trajectory optimization subsystem and converts it into control commands recognizable by the UUV's underlying control system. These control commands include the desired trajectory point sequence, desired velocity, desired heading angle, desired acceleration, trajectory timestamp, and control validity flag. If trajectory planning is normal, a target tracking control command is output; if target observation fails but environmental information is normal, a conservative tracking or re-search control command is output; if environmental information is abnormal or trajectory optimization fails, safety control commands such as deceleration, obstacle avoidance, or maintaining heading are output.

[0059] Figure 4 The flowchart illustrates the execution process of the target predictive tracking task. In this invention, when performing the target predictive tracking task, the communication interface subsystem first receives the current UUV status, target observation information, and environmental obstacle information. Then, the status information management subsystem completes time synchronization and updates the target historical observation queue. Subsequently, the target prediction subsystem generates a short-term predicted trajectory for the target based on the historical observation queue. The front-end path generation subsystem combines the target predicted trajectory and UUV motion constraints to generate a front-end coarse path. The safety corridor construction subsystem generates a safety corridor based on the coarse path and obstacle information. The back-end trajectory optimization subsystem generates a continuous executable trajectory within the safety corridor. Finally, the control command output subsystem converts the optimized trajectory into a desired speed, desired heading angle, and desired trajectory point sequence, which is then sent to the UUV's underlying control system.

[0060] Figure 5 The target prediction algorithm uses a time-series confidence-weighted Bézier regression method to fit the target trajectory. The historical observation queue includes multiple timestamped target observation points. The system assigns different confidence weights based on the observation time's proximity to the current time; newer observation points have a greater impact on the prediction process, while the influence of earlier observation points gradually weakens. This approach reduces the interference of observation noise and abrupt changes in historical motion on the prediction results, making the predicted trajectory closer to the target's current motion trend. An n-order Bézier curve is used. To describe the predicted trajectory of the target:

[0061] in, For time variables, , representing the parameterized time along the predicted trajectory, It is an nth-order Bernstein polynomial. It is the set of control points for the Bézier curve. Meanwhile, it will... The coordinates of the target observed at any given time in the world coordinate system are represented as follows: Then, a first-in, first-out queue of length L is maintained to store past observation data and their corresponding timestamps. This queue is represented as... , . The included time range is ,in It equals the current time.

[0062] Due to the integral of Bézier curve The norm is a quadratic form, and the target trajectory prediction fitting can be transformed into a constrained quadratic programming problem. In the cost function of this problem, different confidence levels are assigned to observations at different timestamps. The further away the observation is from the current time, the smaller its influence should be in the fitting process, and the smaller its corresponding weight should be in the cost function. Add a weighting term. To distinguish the confidence levels of observations with different timestamps, the hyperbolic tangent function was selected. To calculate :

[0063] in It is a proportional parameter. When With the current moment When the time difference increases, The confidence level of different observations is reduced accordingly, thereby effectively distinguishing the confidence level of different observations, reducing the influence of earlier observations on the regression results, highlighting the role of the latest measurement information in trajectory prediction, and achieving confidence weighting in time series.

[0064] Based on the aforementioned time-weighted mechanism, the objective function to be optimized is... Composed of observation-fit residuals and regularization terms, the design is as follows, including measures of the residuals between the predicted trajectory and the observed positions. , To determine the actual target location, and to avoid overfitting, a second-order regularization term is added. , This represents the weight of the regularization term. The weights represent the residual terms, L is the number of historical observation samples involved in trajectory fitting, and the objective function is... The definition is as follows.

[0065] Figure 6 A flowchart for generating the front-end path is provided. The front-end path generation subsystem uses both the target's current position and predicted position as planning references when generating the tracking path. The current UUV state is the path starting point, and the target's current state and predicted state are weighted to form a dynamic desired tracking point. The system expands candidate paths based on UUV motion constraints and performs safety screening based on environmental obstacle information and the double virtual circle collision envelope, ultimately obtaining a coarse front-end path oriented towards the target's future movement trend.

[0066] Figure 7This is a schematic diagram of the back-end trajectory planning. The safety corridor construction subsystem and the back-end trajectory optimization subsystem are used to further transform the front-end coarse path into a continuous, smooth, and executable tracking trajectory. Specifically, the safety corridor construction subsystem generates a continuous safe passage area based on the front-end coarse path and environmental obstacle information, providing spatial safety constraints for the back-end trajectory optimization. The back-end trajectory optimization subsystem performs joint optimization of trajectory shape and time allocation within these safety constraints, ensuring that the generated trajectory simultaneously meets smoothness, velocity constraints, acceleration constraints, and obstacle avoidance safety requirements. In particular, this invention transforms the back-end trajectory generation process into a constrained optimization problem, and uses smoothness cost, logarithmic obstacle cost, and aggression penalty cost to jointly describe trajectory quality, spatial safety, and dynamic feasibility. Within a given safety corridor, the system generates a spatiotemporally optimal piecewise polynomial trajectory. The decision variables of the optimization problem are simplified to intermediate waypoints of a piecewise polynomial trajectory. The piecewise time T is used to reduce the complexity of the optimization solution. The optimized trajectory is converted by the control command output subsystem into the desired trajectory, desired velocity, and desired heading angle that can be executed by the underlying control system. The safe corridor F is defined as: , of which each It is a finite plane. , and For the first The constraint matrix and constraint vector of each corridor define the planar boundary. The smoothness cost... Logarithmic barrier cost Aggressive punishment costs As shown below:

[0067] 1) Smoothing cost

[0068] To ensure the executability of tracking control, for each polynomial segment... Minimizing the jerk energy, i.e., the one-dimensional smoothness cost, is described as: As a cost of smoothness, For the first Segment polynomial locus, For the first The segment duration is M, where M is the number of segments in the piecewise polynomial trajectory.

[0069] 2) Logarithmic barrier cost

[0070] Ensure each intermediate waypoint Constrained in the intersection of adjacent corridors In the context of , it is defined as:

[0071] Among them is , where is the obstacle strength coefficient, and 1 is a completely one vector. It is the input component-wise natural logarithm.

[0072] 3) The cost of aggressive punishment

[0073] The last cost item It is an aggressive punishment that adjusts the entire trajectory, defined as:

[0074] in , For maximum speed, For maximum acceleration, The weights are: The first term encourages a shorter total duration, while the latter two terms impose soft penalties on the portion of the differential velocity and differential acceleration patterns between segments that exceed the threshold, making the trajectory more active in time but still constrained by the dynamic boundary.

[0075] Command and control personnel can define target tracking tasks on the surface mother ship, at the shore-based command and control center, or in the UUV's host computer software, and issue them to the UUV via standard mission files or online communication commands. The mission file includes the mission area, target tracking mode, desired tracking distance, safe distance, speed constraints, acceleration constraints, planning period, target prediction time domain, and mission termination conditions. Upon receiving the mission file, the system automatically completes parameter parsing, status management, target prediction, path generation, trajectory optimization, and control command output.

[0076] The above description is only a preferred embodiment of the present invention and is not intended to limit the ideas of the present invention. Any modifications, equivalent substitutions, improvements, etc., made within the spirit and principles of the present invention should be included within the protection scope of the present invention.

[0077] The key technical point of this invention is:

[0078] 1. A target prediction trajectory planning system for underwater robots oriented towards dynamic target tracking is provided. As an independent planning unit between the UUV perception system, environmental mapping system and underlying control system, it realizes unified processing and connection between target observation information, environmental obstacle information, the vessel's status information and underlying control commands, thereby reducing the integration difficulty of the UUV target tracking system.

[0079] 2. Provide a standardized input / output interface system that supports receiving UUV's own status information, target observation information, environmental obstacle information, and mission parameters, and outputs expected trajectory, expected speed, expected heading angle, planning status, and fault handling information, enabling the system to be adapted to navigation, sonar, mapping, and control equipment of different UUV platforms.

[0080] 3. Systematically encapsulate the target historical observation management, time-series confidence target prediction, UUV motion constraint modeling, front-end path generation, safety corridor construction, back-end trajectory optimization, and control command output to form a complete closed-loop planning system from target observation input to executable trajectory output.

Claims

1. A target-predictive underwater robot trajectory planning system for dynamic target tracking, characterized in that, This system is independently packaged as a trajectory planning and calculation unit, serving as an intermediate planning unit between the UUV sensing system, the environmental mapping system, and the underlying control system. It includes: The communication interface subsystem is used to provide a unified standard input and output interface protocol, receive mission information, submarine status information, target observation information and environmental obstacle information, and output planning status information, target prediction information, desired trajectory point sequence, desired speed, desired heading angle, desired acceleration and fault handling command information. The mission parameter parsing subsystem is used to parse the mission task information, obtain planning parameters, and output them. The status information management subsystem is used to perform time synchronization, format conversion, caching, and validity judgment on data from different sources, and to maintain UUV status cache, target historical observation queue, environmental map cache, and task parameter cache; The UUV motion constraint modeling subsystem is used to establish trajectory planning constraints based on the UUV platform's structural parameters and motion performance parameters. The target prediction subsystem is used to receive the target historical observation queue and the target observation validity flag, and use the target historical observation information to generate the target's future short-term predicted trajectory. The front-end path generation subsystem is used to generate a coarse front-end path that meets obstacle avoidance requirements and motion constraints by combining the target predicted trajectory, UUV motion constraints and environmental obstacle information. The safety corridor construction subsystem is used to create a continuous safe passage zone around the coarse path to form a safety corridor; The back-end trajectory optimization subsystem is used to jointly optimize the trajectory shape and time allocation within the safety corridor by combining the target prediction results and UUV motion constraints, so as to generate a continuous, smooth and dynamically feasible UUV target tracking trajectory. The control command output subsystem is used to convert the optimized trajectory into control commands that can be recognized by the UUV underlying control system and then output them.

2. The system according to claim 1, characterized in that, The UUV motion constraint modeling subsystem includes an underactuated motion constraint modeling unit and a collision envelope modeling unit. The underactuated motion constraint modeling unit restricts the executability of candidate paths and optimized trajectories based on constraints such as the maximum speed, maximum acceleration, rate of change of heading angle, range of steering angle, and minimum turning radius of the UUV. The collision envelope modeling unit uses a dual virtual circle model to describe the safe occupancy area of ​​the UUV hull. The slender hull of the UUV is approximated as a virtual circle at the bow and a virtual circle at the stern. The center position and radius of the two virtual circles are determined according to the hull length, width and safety margin. The distance relationship between the dual virtual circles and obstacles is detected to determine whether the candidate path meets the obstacle avoidance safety requirements.

3. The system according to claim 1, characterized in that, The target prediction subsystem uses the time-series confidence-weighted Bezier regression method to predict the target trajectory. It uses an n-order Bezier curve to describe the predicted trajectory of the target and assigns time-series confidence weights to observation information at different timestamps. The closer the observation information is to the current time, the higher the weight, and the farther the observation information is from the current time, the lower the weight. The target prediction subsystem selects the hyperbolic tangent function to calculate the time series confidence weight. When the time difference between the observation time and the current time increases, the corresponding weight decreases. The target prediction trajectory fitting is transformed into a constrained quadratic programming problem. The optimized objective function is composed of the observation fitting residual term and the second-order regularization term. The time series confidence weight is introduced into the observation fitting residual term to distinguish the confidence of observations at different timestamps.

4. The system according to claim 1, characterized in that, When new target observation information arrives, the status information management subsystem writes the target observation location, observation timestamp, and observation confidence level into the target historical observation queue; when the queue length exceeds the preset window length, it deletes the earliest observation data. If target observation information is continuously missing within a preset time, a target observation failure flag will be output, and the target prediction subsystem will be notified to enter a conservative processing flow.

5. The system according to claim 1, characterized in that, When generating a front-end coarse path, the front-end path generation subsystem uses the target's current position and the target's predicted position as planning references, and forms a dynamic expected tracking point by weighting the target's current state and the target's predicted state. Candidate motion primitives are generated based on UUV motion constraints, and the candidate paths are subjected to safety checks using a double virtual circle collision envelope, eliminating paths that conflict with obstacles or do not meet motion constraints.

6. The system according to claim 1, characterized in that, The safety corridor construction subsystem generates multiple local safety areas along the front-end coarse path and connects adjacent safety areas to form a safety corridor, which serves as a spatial constraint for back-end trajectory optimization, restricting the optimized continuous trajectory to always be located in collision-free free space.

7. The system according to claim 1, characterized in that, The back-end trajectory optimization subsystem transforms the back-end trajectory generation process into a constrained optimization problem. The decision variables of the optimization problem are simplified to intermediate waypoints and segment time of the piecewise polynomial trajectory. The trajectory quality, spatial safety, and dynamic feasibility are described by smoothness cost, logarithmic obstacle cost, and aggressive penalty cost. Among them, the smoothness cost minimizes the jerk energy for each segment polynomial, the logarithmic obstacle cost ensures that each intermediate waypoint is constrained to the intersection of adjacent corridors, and the aggressive penalty cost encourages shorter total duration and applies soft penalties to the portion that exceeds the velocity and acceleration thresholds.

8. The system according to claim 1, characterized in that, The system supports target search mode, conservative prediction tracking mode, and target prediction tracking mode. When the target observation information is valid but the confidence level of the target movement trend is low, the conservative prediction tracking mode is adopted. When the historical observation information of the target is continuously valid and the prediction confidence level meets the threshold, the target prediction tracking mode is adopted. If the trajectory planning is normal, the control command output subsystem outputs a target tracking control command. If target observation fails but environmental information is normal, output conservative tracking or re-search control commands; if environmental information is abnormal or trajectory optimization fails, output deceleration, obstacle avoidance, or course maintenance safety control commands.

Images