Control method and device based on unmanned ship, equipment and storage medium

Abstract

The invention relates to a control method and device based on an unmanned ship, equipment and a storage medium, and the method comprises the steps: carrying out the fusion and filtering of multi-source sensor data, and generating the real-time state estimation and error covariance of a dynamic obstacle; on the basis, rolling prediction is carried out in combination with a process noise covariance matrix, and a trajectory prediction result is generated; performing adaptive correction on noise parameters of the prediction model by calculating a mixed error scalar between an actual state and a historical prediction state; and constructing a time-varying security constraint by using prediction information generated by the corrected model, and optimally solving and executing an optimal obstacle avoidance instruction through model prediction control. According to the method, a closed loop of perception-prediction-error estimation-model correction is established, so that the problem of control optimization failure caused by dynamic obstacle trajectory prediction errors is effectively solved, the robustness and safety of obstacle avoidance decision making of the unmanned ship in a crowded water area are improved, and meanwhile, unnecessary path re-planning is reduced.

Description

Technical Field

[0001] This invention relates to the field of autonomous navigation control technology for unmanned vessels, specifically to a control method, device, equipment, and storage medium based on unmanned vessels. Background Technology

[0002] In the field of autonomous navigation for unmanned surface vessels (USVs), ensuring their safe and efficient obstacle avoidance in complex and dynamic waters is one of the core technological challenges. To address this challenge, obstacle avoidance methods based on model predictive control (MPC) have become a mainstream research direction. The core of this type of method lies in its ability to utilize predictions of the future trajectories of obstacles in the environment (especially dynamic obstacles, such as other vessels) to pre-plan and optimize the motion control sequence of the USV within a finite time window, thereby theoretically achieving proactive, smooth obstacle avoidance maneuvers that comply with various constraints.

[0003] However, the actual effectiveness of such methods heavily relies on a crucial premise: the prediction of the future trajectory of dynamic obstacles must possess sufficient accuracy. In real-world marine environments, trajectory predictions based on fixed motion models (such as constant speed or constant rotation rate models) often produce significant errors due to time-varying environmental disturbances such as wind, waves, and currents, as well as the uncertainty of the target vessel's own maneuvering intentions. Existing technologies typically employ static or conservatively estimated error models to handle this uncertainty, but fail to effectively integrate the obstacle state information perceived in real-time by the unmanned surface vessel with the error feedback from historical predictions in a closed loop, thereby dynamically correcting the prediction model itself online. This leads to the optimization process of the model predictive controller being based on potentially distorted prediction information, resulting in increased conservatism in obstacle avoidance decisions or decreased safety, and even affecting the consistency and economy of navigation due to frequent path replanning. Summary of the Invention

[0004] Based on this, the purpose of the present invention is to provide a control method, device, equipment, and storage medium based on unmanned vessels that can adaptively correct prediction models online to improve dynamic obstacle avoidance robustness and decision-making efficiency.

[0005] The objective of this invention is achieved through the following solution:

[0006] In a first aspect, the present invention provides a control method based on an unmanned vessel, comprising the following steps:

[0007] S1: The raw observation data collected by the multi-source sensors of the unmanned vessel is fused and filtered. The data from millimeter-wave radar, lidar, automatic identification system and visual sensor are fused through spatiotemporal alignment and capacitive Kalman filtering algorithm to generate the fused state estimate of all dynamic obstacles at the current moment and the corresponding estimation error covariance.

[0008] S2: Perform rolling prediction on the fusion state estimate, estimation error covariance and preset process noise covariance matrix. Use a discrete linear Kalman predictor to recursively calculate the motion state and uncertainty of dynamic obstacles at multiple future times, and generate trajectory prediction results containing the predicted state sequence and predicted covariance sequence of all dynamic obstacles at multiple future times.

[0009] S3: Estimate the error between the fusion state estimate and the trajectory prediction result obtained from the previous cycle, extract the actual state at the current moment and the state predicted for the current moment in the previous cycle, calculate the Mahalanobis distance and the Euclidean distance between the two based on the prediction uncertainty, and sum the two distances according to the preset weights to generate a hybrid error scalar.

[0010] S4: Perform model parameter correction on the mixed error scalar, and use the nonlinear mapping relationship based on the hyperbolic tangent function to convert the smoothed error value into the adjustment coefficient of the process noise covariance matrix, and generate the process noise covariance matrix for trajectory prediction in the next cycle.

[0011] S5: Perform predictive control optimization on the predicted state sequence and predicted covariance sequence, construct and solve the optimization problem constrained by the time-varying safety distance, and generate and execute the optimal control command to drive the unmanned vessel at the current moment.

[0012] In one embodiment, S1 of the control method based on an unmanned vessel provided by the present invention specifically includes the following steps:

[0013] S11: Perform spatiotemporal alignment preprocessing on the raw observation data collected by the multi-source sensors of the unmanned vessel, unify the timestamps and spatial coordinate systems of the data from each sensor, and generate synchronous observation data with consistent spatiotemporal references.

[0014] S12: Perform multi-source information fusion processing on synchronous observation data, and perform weighted fusion of the state observation values ​​of the same obstacle based on the confidence levels of different sensors to generate a fusion observation vector with higher accuracy and robustness.

[0015] S13: Perform nonlinear filtering estimation on the fused observation vector to establish an obstacle kinematic model that includes position, velocity and random acceleration perturbations. Then, use the capacitive Kalman filter algorithm to perform state recursion and update, generating the fused state estimate of all dynamic obstacles at the current moment and the corresponding estimation error covariance.

[0016] In one embodiment, S2 of the control method based on an unmanned vessel provided by the present invention specifically includes the following steps:

[0017] S21: Adaptively initialize the preset process noise covariance matrix, dynamically set the amplitude of its diagonal elements according to the current sea state level and sensor noise characteristics, and generate an initial process noise covariance matrix that matches the environment.

[0018] S22: Based on the state transition matrix recursive algorithm, uncertainty propagation is performed on the initial process noise covariance matrix. Under the discrete linear Kalman prediction framework, the evolution process of the fusion state estimate and the estimation error covariance is recursively calculated over time to generate intermediate predicted states and intermediate predicted covariance sequences.

[0019] S23: Perform prediction time domain truncation and formatting on the intermediate predicted state and intermediate predicted covariance sequences. According to the prediction time domain length of the model prediction control, extract all state vectors within the corresponding time window from the intermediate predicted state sequence, and extract all covariance matrices within the same time window from the intermediate predicted covariance sequence. Generate prediction state sequences and prediction covariance sequences for all dynamic obstacles at multiple future times, and integrate them to generate trajectory prediction results.

[0020] In one embodiment, S3 of the control method based on an unmanned vessel provided by the present invention specifically includes the following steps:

[0021] S31: Extract and process data from the previous cycle trajectory prediction results in the cache, obtain the prediction state vector for the current moment and its corresponding prediction error covariance matrix, and generate historical prediction state and historical prediction covariance.

[0022] S32: Generate a state deviation vector based on the difference between the fused state estimate and the historical predicted state. Combine the historical prediction covariance to perform a weighted sum of squares operation on the state deviation vector. Use the inverse of the historical prediction covariance matrix to weight the state deviation vector and calculate its inner product to generate a standardized statistical deviation.

[0023] S33: Calculate the difference between the position components in the actual state and the historical predicted state of the fused state estimate to generate a position deviation vector. Combine the standardized statistical deviation to calculate the Euclidean geometric distance between the actual position and the predicted position. Then, weight and sum the Euclidean geometric distance and the standardized statistical deviation according to a preset ratio to generate a mixed error scalar.

[0024] In one embodiment, the present invention provides a formula for calculating the hybrid error scalar of a control method based on an unmanned vessel as follows:

[0025]

[0026] in, This is the fusion state estimation vector at the current moment. This is the historical predicted state vector for the current moment, as predicted in the previous cycle. This is the corresponding historical prediction covariance matrix. , They represent from and The position vector extracted from it To balance the mixing coefficient of the two weights, This is a preset constant used to normalize the Euclidean distance.

[0027] In one embodiment, S4 of the control method based on an unmanned vessel provided by the present invention specifically includes the following steps:

[0028] S41: Perform smoothing and noise reduction on the mixed error scalar. Suppress the instantaneous high-frequency fluctuations of the error signal through a first-order low-pass filter, extract the slowly changing trend components in the mixed error scalar, and generate a smoothed error trend value.

[0029] S42: Calculate nonlinear adjustment coefficients based on error trend values. Input the error trend values ​​into a nonlinear mapper with the hyperbolic tangent function as its core. Smoothly compress the input values ​​and map them into a preset positive coefficient range to generate dynamic adjustment coefficients for directly adjusting the process noise intensity.

[0030] S43: Based on the dynamic adjustment coefficient, the preset process noise covariance matrix is ​​adaptively scaled element by element. Each element in the preset process noise covariance matrix is ​​combined with the dynamic adjustment coefficient to form a process noise covariance matrix that matches the current prediction error level and is used for trajectory prediction in the next cycle. Each element value of the process noise covariance matrix is ​​the result of scaling the corresponding element value of the original matrix by the same proportion.

[0031] In one embodiment, S5 of the control method based on an unmanned vessel provided by the present invention specifically includes the following steps:

[0032] S51: Perform time-varying safety boundary construction processing on the predicted covariance sequence. Based on the uncertainty ellipsoid represented by the covariance matrix at each prediction time, generate a series of cylindrical safety boundaries surrounding the predicted position of each dynamic obstacle, with the radius varying with time, as spatial constraints.

[0033] S52: Based on the predicted state sequence and spatial constraints, the rolling optimization problem is constructed and processed. Under the model predictive control framework, the geometric relationship that the future state trajectory of the unmanned vessel must not intrude into the safety boundary of a series of cylinders is transformed into a set of inequality constraints in the nonlinear programming problem, generating a complete optimization problem that includes dynamic constraints, control constraints and obstacle avoidance constraints.

[0034] S53: The complete optimization problem is solved online and the optimal control sequence is extracted. The numerical optimization algorithm is used to solve the control input sequence that satisfies all constraints and optimizes the tracking performance index. The control quantity corresponding to the current moment is extracted from the control input sequence to generate the optimal control command to drive the unmanned vessel.

[0035] Secondly, the present invention provides a control system based on an unmanned vessel, which is configured with the following modules:

[0036] The multi-source data fusion and filtering module is used to fuse and filter the raw observation data collected by the multi-source sensors of the unmanned vessel. It fuses the data from millimeter-wave radar, lidar, automatic identification system and visual sensor through spatiotemporal alignment and capacitive Kalman filtering algorithm to generate the fused state estimate of all dynamic obstacles at the current moment and the corresponding estimation error covariance.

[0037] The obstacle trajectory rolling prediction module is used to perform rolling prediction on the fused state estimate, estimation error covariance and preset process noise covariance matrix. It uses a discrete linear Kalman predictor to recursively calculate the motion state and uncertainty of dynamic obstacles at multiple future times, and generates trajectory prediction results containing the predicted state sequence and predicted covariance sequence of all dynamic obstacles at multiple future times.

[0038] The trajectory prediction error calculation module is used to estimate the error between the fused state estimation and the trajectory prediction results obtained from the previous cycle. It extracts the actual state at the current moment and the state predicted for the current moment in the previous cycle, calculates the Mahalanobis distance and the Euclidean distance between the two based on the prediction uncertainty, and weights the two distances according to preset weights to generate a hybrid error scalar.

[0039] The process noise parameter correction module is used to correct the model parameters of the mixed error scalar. It uses a nonlinear mapping relationship based on the hyperbolic tangent function to convert the smoothed error value into the adjustment coefficient of the process noise covariance matrix, and generates the process noise covariance matrix for trajectory prediction in the next cycle.

[0040] The predictive control command optimization module is used to perform predictive control optimization on the predicted state sequence and the predicted covariance sequence, construct and solve the optimization problem with time-varying safety distance as constraint, and generate and execute the optimal control command to drive the unmanned vessel at the current moment.

[0041] Thirdly, this application provides a computer device, including a memory and a processor, wherein the memory stores a computer program, and the processor executes the computer program to implement any of the above-mentioned unmanned vessel-based control methods.

[0042] Fourthly, this application provides a computer-readable storage medium storing a computer program thereon, which, when executed by a processor, implements any of the above-mentioned unmanned vessel-based control methods.

[0043] In summary, the unmanned surface vessel (USV) control method provided in this application effectively addresses the control failure problem caused by inaccurate prediction of dynamic obstacle trajectories in congested waters by constructing an adaptive closed loop of "perception-prediction-error assessment-model correction-control optimization." This method, through the fusion of multi-source perception data, enables accurate estimation of obstacle motion states, providing a reliable basis for prediction. Furthermore, by calculating historical prediction errors in real time and adaptively adjusting noise parameters in the prediction model, the model continuously approximates the real dynamic disturbance environment, achieving accurate quantification of future trajectory uncertainties. The time-varying safety distance constraint constructed based on this quantification result guides the model predictive controller to make dynamic balance decisions between safety and efficiency, thereby significantly reducing frequent and unnecessary path replanning triggered by prediction deviations while ensuring absolute obstacle avoidance safety. Overall, this method significantly improves the autonomy, smoothness, and overall mission reliability of USVs navigating in dynamic, dense waters.

[0044] To better understand and implement this invention, the following detailed description is provided in conjunction with the accompanying drawings. Attached Figure Description

[0045] Figure 1 A flowchart illustrating a control method based on an unmanned vessel provided in this application embodiment;

[0046] Figure 2 This is a schematic diagram of the process for generating a hybrid error scalar provided in an embodiment of this application;

[0047] Figure 3 This is a schematic diagram of a control device based on an unmanned vessel, provided as another embodiment of this application. Detailed Implementation

[0048] To facilitate understanding of the present invention, a more complete description will be given below with reference to the accompanying drawings. Preferred embodiments of the invention are shown in the drawings. However, the invention can be implemented in many different forms and is not limited to the embodiments described herein. Rather, these embodiments are provided to provide a thorough and complete understanding of the disclosure of the invention.

[0049] Unless otherwise defined, all technical and scientific terms used herein have the same meaning as commonly understood by one of ordinary skill in the art to which this invention pertains. The terminology used herein in the description of the invention is for the purpose of describing particular embodiments only and is not intended to be limiting of the invention. The term "and / or" as used herein includes any and all combinations of one or more of the associated listed items.

[0050] In one embodiment, such as Figure 1 As shown, a control method based on an unmanned surface vessel is provided. This embodiment illustrates the method by applying it to a terminal. It is understood that this method can also be applied to a server, and further to a system including both a terminal and a server, and is implemented through interaction between the terminal and the server. In this embodiment, the method includes the following steps:

[0051] S1: The raw observation data collected by the multi-source sensors of the unmanned vessel is fused and filtered. The data from millimeter-wave radar, lidar, automatic identification system and vision sensor are fused through spatiotemporal alignment and capacitive Kalman filtering algorithm to generate the fused state estimate of all dynamic obstacles at the current moment and the corresponding estimation error covariance.

[0052] Specifically, the system performs multi-source sensor data fusion and filtering. Through complementary fusion and noise suppression of multi-source sensor data, it obtains the current state estimate and estimation uncertainty quantification results of dynamic obstacles. It also integrates raw observation data collected by various sensors. Different sensors provide different dimensional information about the obstacles. Some sensors provide distance, azimuth and radial velocity information, some provide three-dimensional spatial coordinates and contour features, some provide dynamic information such as latitude and longitude, speed, heading and turning rate, and some provide visual recognition and contour matching related data. The various types of data complement each other to cover the observation needs under different distance ranges and environmental conditions.

[0053] Furthermore, the system performs spatiotemporal alignment. Since the sampling frequencies, timestamp references, and coordinate systems of the multi-source sensors differ, spatiotemporal alignment is a prerequisite for data fusion. During time synchronization, the system unifies the timestamps of all sensors to the unmanned surface vessel's (USV) hull clock reference. For data with inconsistent sampling frequencies, the system uses interpolation to supplement missing observations, ensuring the consistency of the data time series. During spatial registration, the system defines a unified hull coordinate system. The origin of this coordinate system is set at the USV's center of mass, and the coordinate axes extend forward along the bow, to the starboard side, and vertically upward. The system obtains the extrinsic parameter matrices of each sensor relative to the hull coordinate system through pre-calibration. These extrinsic parameter matrices include rotation matrices and translation vectors. The system uses these extrinsic parameter matrices to convert the lidar point cloud and visual sensor pixel coordinates into three-dimensional coordinates in the hull coordinate system. Simultaneously, the latitude and longitude data are converted into unified coordinates using the coordinate transformation formula between the geodetic coordinate system and the hull coordinate system, ensuring that all sensor data are on the same spatial reference.

[0054] Preferably, the system employs a capacitive Kalman filter algorithm to achieve nonlinear fusion of multi-source data. This algorithm avoids linearization errors and reduces computational complexity by approximating Gaussian distribution through integral operations using a volume rule. The system defines a state vector for the dynamic obstacle, containing parameters such as position, velocity, heading angle, and turning rate. It also defines an observation vector, composed of observation data from each sensor after coordinate transformation. During the filter initialization phase, the system sets the initial state estimate and the initial estimate error covariance. The velocity and turning rate parameters in the initial state estimate are set to initial values, while the position and heading angle parameters are taken from the first fused observation values. During the time update phase, the system calculates the state prediction based on the dynamic obstacle's constant turning rate-constant velocity motion model, combined with the filter cycle. A specified number of volume points are generated using a volume point generation rule; the number of volume points is related to the state dimension. The system calculates the time update results of the volume points to obtain the prediction covariance. During the observation update phase, the system substitutes the time-updated volume points into the observation model to obtain the observation prediction value, calculates the observation covariance matrix and the state-observation cross-covariance matrix, solves the Kalman gain, and finally obtains the fused state estimate and the corresponding estimation error covariance through Kalman gain update, thus completing the fusion and filtering of multi-source sensor data.

[0055] S2: Perform rolling prediction on the fusion state estimate, estimation error covariance and preset process noise covariance matrix. Use a discrete linear Kalman predictor to recursively calculate the motion state and uncertainty of dynamic obstacles at multiple future times, and generate trajectory prediction results containing the predicted state sequence and predicted covariance sequence of all dynamic obstacles at multiple future times.

[0056] Specifically, the system, based on a discrete linear Kalman predictor, recursively predicts the future motion state of dynamic obstacles. The system first determines preset parameters. The initial value of the process noise covariance matrix is ​​obtained through statistical analysis of a large amount of ship navigation data, reflecting the inherent uncertainty of ship motion. The prediction time domain is set to multiple future moments, the number matching the control cycle to ensure that the prediction results can cover the obstacle avoidance decision requirements within the subsequent control cycle. After the parameters are determined, the system constructs a state transition matrix based on the ship's linear motion model, assuming that the obstacle's speed and heading remain stable within the prediction time domain. After the state transition matrix is ​​constructed, the system performs recursive prediction calculations. Using the fused state estimate and estimation error covariance obtained in the previous step as initial inputs, and following the recursive rules of discrete linear Kalman prediction, the system sequentially calculates the predicted state value and covariance value for each future prediction moment.

[0057] Furthermore, state prediction is obtained by operating on the state transition matrix and the predicted value at the previous time step, while covariance prediction is obtained by operating on the state transition matrix, the predicted covariance value at the previous time step, the transpose of the state transition matrix, and the process noise covariance matrix. The state prediction values ​​at all prediction times are combined to form a predicted state sequence, and the corresponding covariance prediction values ​​are combined to form a predicted covariance sequence. Together, they constitute the trajectory prediction result.

[0058] S3: Estimate the error between the fusion state estimate and the trajectory prediction result obtained from the previous cycle, extract the actual state at the current moment and the state predicted for the current moment in the previous cycle, calculate the Mahalanobis distance and the Euclidean distance between the two based on the prediction uncertainty, and sum the two distances according to the preset weights to generate a hybrid error scalar.

[0059] Specifically, the system generates a hybrid error scalar reflecting prediction accuracy by comparing the current actual state with historical predicted states. After the process begins, the system reads the trajectory prediction results generated in the previous control cycle from the data cache unit, extracting the predicted state and corresponding prediction covariance for the current moment. Simultaneously, the system obtains the fused state estimate for the current moment, using it as a reference value for the actual state to ensure complete consistency between the extracted historical predicted state and the current actual state in terms of state dimension and coordinate system. Further, the system calculates two distance metrics: the Mahalanobis distance, calculated based on the difference between the current actual state and the historical predicted state, and the inverse matrix of the historical prediction covariance, obtained through matrix transposition, multiplication, and square root operations; this distance characterizes the severity of the prediction error relative to uncertainty; and the Euclidean distance, calculated only for the position dimension, obtained by performing a square root operation on the sum of the squares of the differences between the current actual position and the historical predicted position.

[0060] Preferably, the system smooths the Euclidean distance between positions and uses a moving average method to eliminate instantaneous error interference, avoiding the impact of abnormal errors at a single moment on subsequent calculations. After smoothing, the system performs a weighted summation of the two distances. The weights are calibrated through actual ship tests, taking into account both the impact of uncertainty and the actual position deviation, and finally generates a hybrid error scalar. This scalar quantifies the degree of deviation of the prediction model in the previous cycle, and its numerical change directly reflects the fluctuation of prediction accuracy.

[0061] S4: Perform model parameter correction on the mixed error scalar, and use the nonlinear mapping relationship based on the hyperbolic tangent function to convert the smoothed error value into the adjustment coefficient of the process noise covariance matrix, generating the process noise covariance matrix for trajectory prediction in the next cycle.

[0062] Specifically, the system uses a mixed error scalar as the basis for correcting the prediction model parameters. The mixed error scalar is further smoothed using an exponential moving average method, combined with the smoothing results from the previous period, to balance error response speed and stability, avoiding parameter fluctuations caused by instantaneous errors. After smoothing, the system constructs a nonlinear mapping relationship between the error value and the process noise covariance adjustment coefficient using a hyperbolic tangent function. This function has saturation characteristics, limiting the range of the adjustment coefficient and preventing model instability caused by excessively large adjustment coefficients. During the mapping process, the error value is converted into the corresponding adjustment coefficient through function calculation. The change in the adjustment coefficient is positively correlated with the change in the error value; the adjustment coefficient increases when the error value increases and decreases when the error value decreases.

[0063] After the adjustment coefficients are generated, the system uses them to correct the current process noise covariance matrix. The correction process employs a weighted fusion operation of the current process noise covariance matrix, the adjustment coefficients, and the initial process noise covariance matrix, where the initial process noise covariance matrix is ​​a preset value from the trajectory prediction stage. This operation generates the process noise covariance matrix for trajectory prediction in the next cycle, achieving smooth parameter updates, avoiding abrupt changes in model parameters, and ensuring the stability and adaptability of the prediction model.

[0064] S5: Perform predictive control optimization on the predicted state sequence and predicted covariance sequence, construct and solve the optimization problem constrained by the time-varying safety distance, and generate and execute the optimal control command to drive the unmanned vessel at the current moment.

[0065] Specifically, the system performs predictive control optimization and optimal control command generation. Using the trajectory prediction results obtained from previous steps as input, it constructs an optimization problem with time-varying safety distance constraints. Solving this problem yields the optimal control command for the unmanned surface vessel (USV), achieving safe and efficient dynamic obstacle avoidance. The system defines a time-varying safety distance, which is dynamically adjusted based on the relative motion state between the USV and dynamic obstacles, avoiding the conservatism or insufficient safety of obstacle avoidance caused by a fixed safety distance. The time-varying safety distance consists of a basic safety distance, a relative velocity correlation term, and a prediction uncertainty correlation term. The basic safety distance is determined based on the USV's hull dimensions. The relative velocity correlation term is obtained by multiplying the relative velocity between the USV and the obstacle by the USV's maneuvering response time. The prediction uncertainty correlation term is obtained by taking the square root of the trace of the prediction covariance matrix. The greater the prediction uncertainty, the larger the time-varying safety distance, thus improving obstacle avoidance robustness.

[0066] Furthermore, the system constructs a quadratic programming optimization problem within a finite time domain, with the optimization variables being the control inputs of the unmanned surface vessel (USV), including rudder angle and thruster speed. The objective function of the optimization problem adopts a multi-objective weighted summation form, balancing obstacle avoidance safety, navigation economy, and trajectory smoothness. The objective function includes a trajectory tracking error term, a control input smoothing term, and an obstacle avoidance penalty term. The trajectory tracking error term is the sum of squared L2 norms of the USV's predicted state and the reference route state, with the reference route state output by the path planning module. The control input smoothing term is the sum of squared L2 norms of the control input vector, used to avoid hull oscillations caused by sudden changes in control commands. The obstacle avoidance penalty term is the penalty value for violating the safety distance constraint; when the minimum distance between the USV and an obstacle is less than the time-varying safety distance, a penalty value is generated to force the optimization process to satisfy the obstacle avoidance constraint. The weight coefficients of each component in the objective function are dynamically adjusted according to navigation priority to adapt to different navigation scenario requirements.

[0067] The constraints of the optimization problem include state constraints, control input constraints, and obstacle avoidance constraints. State constraints limit the range of values ​​for the unmanned surface vessel's (USV) state parameters such as position, velocity, and heading angle. Position constraints are determined based on the water boundary, velocity constraints are determined based on the USV's dynamic performance, and heading angle constraints are determined based on the physical limits of ship maneuvering. Control input constraints limit the range of values ​​for the rudder angle and propeller speed, determined based on the equipment limits of the actuators. Obstacle avoidance constraints require that the minimum distance between the USV and obstacles in the prediction time domain is not less than the time-varying safe distance, ensuring obstacle avoidance safety. Preferably, the system can use a numerical optimization algorithm to solve the quadratic programming problem. This algorithm can obtain the optimal solution within a preset iteration accuracy and maximum number of iterations, meeting real-time control requirements. After obtaining the control input sequence in the prediction time domain, the system extracts the control input at the current moment as the optimal control command based on the rolling time domain principle of model predictive control. The system sends the optimal control command to the USV's actuators via the communication bus. After receiving the control command, the actuators drive the USV to move. In the next control cycle, the system repeats all the above steps to achieve closed-loop rolling optimization, ensuring that the USV continuously adaptively avoids obstacles in dynamic waters.

[0068] In summary, the unmanned surface vessel (USV) control method provided in this application effectively addresses the control failure problem caused by inaccurate prediction of dynamic obstacle trajectories in congested waters by constructing an adaptive closed loop of "perception-prediction-error assessment-model correction-control optimization." This method, through the fusion of multi-source perception data, enables accurate estimation of obstacle motion states, providing a reliable basis for prediction. Furthermore, by calculating historical prediction errors in real time and adaptively adjusting noise parameters in the prediction model, the model continuously approximates the real dynamic disturbance environment, achieving accurate quantification of future trajectory uncertainties. The time-varying safety distance constraint constructed based on this quantification result guides the model predictive controller to make dynamic balance decisions between safety and efficiency, thereby significantly reducing frequent and unnecessary path replanning triggered by prediction deviations while ensuring absolute obstacle avoidance safety. Overall, this method significantly improves the autonomy, smoothness, and overall mission reliability of USVs navigating in dynamic, dense waters.

[0069] In one embodiment, S1 of the control method based on an unmanned vessel provided by the present invention specifically includes the following steps:

[0070] S11: Perform spatiotemporal alignment preprocessing on the raw observation data collected by the multi-source sensors of the unmanned vessel, unify the timestamps and spatial coordinate systems of the data from each sensor, and generate synchronous observation data with consistent spatiotemporal reference.

[0071] Specifically, the system receives raw observation data from various sensors installed on the unmanned surface vessel (USV). Millimeter-wave radar outputs distance, radial velocity, and azimuth data of target obstacles; lidar outputs point cloud data containing obstacle contours and location information; the automatic identification system outputs navigation status data of surrounding vessels; and visual sensors output visual data containing obstacle image features. After raw data collection, the system performs timestamp unification processing, using the standard timestamp output by the positioning module as a benchmark to time-calibrate the observation data from all sensors. For sensors with different output frequencies, the system uses interpolation to supplement missing time points, obtaining the missing time values ​​through calculations using adjacent valid observation data. This ensures that the observation data from all sensors are aligned to the discrete time points of the control cycle in the time dimension, eliminating time reference deviations caused by data acquisition delays from different sensors.

[0072] Preferably, the system performs spatial coordinate system processing, transforming the observation data from all sensors to a preset unified ship coordinate system. This ship coordinate system uses the unmanned vessel's center of mass as its origin, clearly defining the coordinate axis directions to form a unified spatial reference benchmark. Considering the installation characteristics of different sensors, the system calls preset installation calibration parameters to transform the observation data from millimeter-wave radar, lidar, and vision sensors from their respective local coordinate systems to the unified ship coordinate system. The system automatically identifies the latitude and longitude data output by the system, converts it to planar coordinates using a projection transformation method, and then combines this with the unmanned vessel's own positioning data and heading information to complete the conversion from planar coordinates to the unified ship coordinate system. Through the synergy of timestamp unification and spatial coordinate system transformation, the system eliminates the spatiotemporal reference differences between multi-source sensor data.

[0073] S12: Perform multi-source information fusion processing on synchronous observation data, and perform weighted fusion of the state observation values ​​of the same obstacle based on the confidence levels of different sensors to generate a fusion observation vector with higher accuracy and robustness.

[0074] Specifically, the system performs multi-source information fusion processing on the spatiotemporally aligned synchronous observation data. Based on the confidence levels of different sensors, it weights and fuses the state observations of the same obstacle to generate a fused observation vector. After the synchronous observation data enters the fusion process, the system first performs obstacle association matching. By comparing the feature information of obstacles in the observation data from different sensors, it identifies the same obstacle corresponding to the observation data from different sensors, establishing a correlation between the sensor observation data and the obstacle to avoid confusion between observation data of different obstacles. After the association matching is completed, the system determines the confidence level of each sensor's observation data for the same obstacle.

[0075] The confidence level is determined based on the inherent characteristics of the sensors, the influence of the observation environment on the sensors, and the stability of the sensors' historical observation data. By comprehensively analyzing the performance of the sensors in the current observation scenario, the reliability of each sensor's observation data is quantified, forming a confidence coefficient for each sensor. After determining the confidence coefficients, the system constructs a weighted fusion model, using the confidence coefficients of each sensor as weights to perform weighted calculations on the state observations of the same obstacle. For different observations in the same state dimension, the fusion result for that state dimension is obtained by summing the products of each observation and its corresponding confidence coefficient. To address the differences in state dimensions between different sensor observation data, the system performs state dimension completion processing, expanding the observation data of each sensor to a unified state dimension space before performing weighted fusion calculations. During the fusion process, the system considers the error characteristics of each sensor's observation data, suppressing the observation errors of a single sensor through multi-source data complementarity, and generating a fused observation vector covering key state information of the obstacle.

[0076] S13: Perform nonlinear filtering estimation on the fused observation vector to establish an obstacle kinematic model that includes position, velocity and random acceleration perturbations. Then, use the capacitive Kalman filter algorithm to perform state recursion and update, generating the fused state estimate of all dynamic obstacles at the current moment and the corresponding estimation error covariance.

[0077] Specifically, the system performs nonlinear filtering estimation on the fused observation vector generated by multi-source information fusion. By constructing an obstacle kinematic model and using a capacitive Kalman filter algorithm for state recursion and updating, it generates the fused state estimate and corresponding estimation error covariance of all dynamic obstacles at the current moment. The system first establishes an obstacle kinematic model, which includes the obstacle's position, velocity, and random acceleration perturbation-related states. A random acceleration perturbation term is introduced to describe the influence of various disturbance factors in the marine environment on the obstacle's motion, accurately characterizing the dynamic characteristics and uncertainties of the obstacle's motion. The kinematic model is presented in the form of state equations, clearly defining the dynamic relationships between the state variables in the state vector, providing a model foundation for filtering estimation.

[0078] Furthermore, the system constructs observation equations and establishes a mapping relationship between the fused observation vectors and the kinematic model state vectors based on the state dimension and observation characteristics of the fused observation vectors. The state vectors are then transformed into the observation space through the observation equations, realizing the correlation between observation data and model states. Subsequently, the system initiates a capacitive Kalman filter algorithm for state recursion and updating. First, a time update is performed. Based on the established state equations and the state estimation results from the previous time step, the obstacle state at the current time step is predicted to obtain the predicted state value. Simultaneously, the state prediction error covariance is calculated using the error propagation characteristics of the state equations.

[0079] After the time update is completed, the system substitutes the fused observation vector into the observation equation to obtain the observation prediction value, compares the difference between the observation prediction value and the actual fused observation vector to calculate the observation residual, and combines the observation error covariance to correct the state prediction value obtained from the time update, thus obtaining the fused state estimate at the current time. Simultaneously, the system calculates the estimation error covariance corresponding to the fused state estimate using the covariance update rule of the filtering algorithm; this covariance quantifies the uncertainty of the fused state estimate.

[0080] In one embodiment, S2 of the control method based on an unmanned vessel provided by the present invention specifically includes the following steps:

[0081] S21: Adaptively initialize the preset process noise covariance matrix, dynamically set the amplitude of its diagonal elements according to the current sea state level and sensor noise characteristics, and generate an initial process noise covariance matrix that matches the environment.

[0082] Specifically, the system performs adaptive initialization of a preset process noise covariance matrix. Based on the current sea state level and sensor noise characteristics, it dynamically sets the amplitude of the matrix's diagonal elements to generate an initial process noise covariance matrix that matches the environment. The system first collects relevant marine environmental data, acquiring environmental parameters affecting ship motion, such as ocean currents and waves, through environmental sensing devices installed on the unmanned vessel. Combined with preset sea state level classification rules, it analyzes and determines the current sea state level. Simultaneously, the system statistically analyzes the observation error characteristics of each sensor. By continuously collecting sensor output data under the current observation scenario, it compares the deviation of sensor output values ​​with standard reference values ​​to extract sensor noise characteristics and clarify the noise distribution pattern and amplitude range.

[0083] After acquiring the sea state level and sensor noise characteristics, the system establishes a correlation between these two levels and the amplitudes of the diagonal elements of the process noise covariance matrix. Higher sea state levels indicate greater environmental interference affecting ship motion, resulting in larger process noise amplitudes. Similarly, larger error amplitudes in sensor noise characteristics lead to corresponding adjustments in process noise amplitudes. Based on this correlation, the system dynamically calculates the amplitudes of each diagonal element of the process noise covariance matrix, constructing an initial process noise covariance matrix. This matrix is ​​a symmetric positive definite matrix, with diagonal elements corresponding to the process noise intensity of each dimension of the obstacle's motion state, and off-diagonal elements set to zero. This ensures the matrix meets the computational requirements of a discrete linear Kalman predictor, providing an initial uncertainty model that matches the environmental and sensor characteristics for subsequent trajectory prediction.

[0084] S22: Based on the state transition matrix recursive algorithm, uncertainty propagation is performed on the initial process noise covariance matrix. Under the discrete linear Kalman prediction framework, the evolution process of the fusion state estimate and the estimation error covariance is recursively calculated over time to generate intermediate predicted states and intermediate predicted covariance sequences.

[0085] Specifically, the system, based on a recursive algorithm of the state transition matrix, performs uncertainty propagation of the initial process noise covariance matrix within the discrete linear Kalman prediction framework. It recursively calculates the evolution of the fused state estimate and estimation error covariance over time, generating a sequence of intermediate predicted states and intermediate predicted covariances. The system constructs a state transition matrix based on a linear motion model of the obstacle, clearly defining the linear transformation relationship between the obstacle state vectors at adjacent time points, with the matrix dimension consistent with the obstacle state vector dimension. After the state transition matrix is ​​constructed, the system uses the fused state estimate and estimation error covariance as initial inputs into the discrete linear Kalman prediction framework. During the recursive calculation, the system recurses according to a preset time step. Within each time step, the intermediate predicted state for the current time step is obtained by matrix multiplication of the state transition matrix with the fused state estimate from the previous time step; simultaneously, the intermediate predicted covariance for the current time step is obtained through matrix operations on the state transition matrix, the estimation error covariance from the previous time step, the transpose of the state transition matrix, and the initial process noise covariance matrix. The system repeats this recursive process, continuously calculating the intermediate predicted state and intermediate predicted covariance for each subsequent time step, until the number of recursions covers the preset maximum prediction time, ultimately forming a continuous sequence of intermediate predicted states and intermediate predicted covariance, which fully characterizes the evolution trend of obstacle motion state and uncertainty over time.

[0086] S23: Perform prediction time domain truncation and formatting on the intermediate predicted state and intermediate predicted covariance sequences. According to the prediction time domain length of the model prediction control, extract all state vectors within the corresponding time window from the intermediate predicted state sequence, and extract all covariance matrices within the same time window from the intermediate predicted covariance sequence. Generate prediction state sequences and prediction covariance sequences for all dynamic obstacles at multiple future times, and integrate them to generate trajectory prediction results.

[0087] Specifically, the system performs prediction time-domain truncation and formatting on the intermediate predicted states and intermediate predicted covariance sequences. Based on the prediction time-domain length of the model predictive control, it extracts data within the corresponding time window and organizes it into a standard format to generate trajectory prediction results. The system first obtains the prediction time-domain length required for model predictive control. This length is determined based on the unmanned vessel's control cycle, maneuver response characteristics, and obstacle avoidance decision requirements, ensuring that the prediction results cover the time range required for subsequent control decisions. After determining the prediction time-domain length, the system truncates all state vectors within the corresponding time window from the intermediate predicted state sequence. The starting time of the truncation is the next moment after the current time, and the ending time is the current time plus the last moment corresponding to the prediction time-domain length, ensuring that the truncated state vectors can completely reflect the trajectory of obstacles within the preset future timeframe.

[0088] Furthermore, the system extracts all covariance matrices within the same time window from the intermediate predicted covariance sequence. Each covariance matrix corresponds one-to-one with a corresponding predicted state vector, quantifying the uncertainty of the predicted state at that moment. After extraction, the system formats the extracted state vectors and covariance matrices, arranging them according to a preset data structure specification, unifying the data storage format and indexing method, and ensuring that the data structure meets the input requirements of subsequent predictive control optimization. The system combines the formatted predicted state vectors into a predicted state sequence and the formatted covariance matrices into a predicted covariance sequence. The two are integrated to form the trajectory prediction result, which is stored in the data cache unit, providing structured predictive data support for subsequent error estimation and control optimization.

[0089] In one embodiment, S3 of the control method based on an unmanned vessel provided by the present invention specifically includes the following steps:

[0090] S31: Extract and process data from the cached trajectory prediction results of the previous cycle, obtain the predicted state vector for the current moment and its corresponding prediction error covariance matrix, and generate historical prediction states and historical prediction covariance.

[0091] Specifically, the system performs data extraction processing of the trajectory prediction results from the previous cycle, retrieving the predicted state vector and corresponding prediction error covariance matrix for the current moment from the cache, and generating historical predicted states and historical prediction covariances. The system first accesses the data cache unit, which stores trajectory prediction results in partitions according to the control cycle. The prediction results for each cycle are associated with a timestamp and a data body in key-value pairs. The data body contains the set of predicted state vectors and the corresponding set of prediction error covariance matrices for all moments within the prediction time domain. The system reads the timestamp of the current moment, converts it to a format consistent with the cached data timestamp, and locates the data storage partition for the prediction data of the previous cycle by traversing the cache's cycle index directory.

[0092] Furthermore, the system uses a timestamp matching algorithm to search for the vector entry corresponding to the current time stamp in the predicted state vector set for that period, and simultaneously obtains the prediction error covariance matrix associated with that entry. In the validity verification phase, the system verifies whether the dimension of the predicted state vector is consistent with the preset obstacle state dimension, and verifies whether the prediction error covariance matrix is ​​a symmetric positive definite matrix. The system ensures matrix validity through checks on the non-negativity of diagonal elements and symmetry verification. After confirming that the data is anomaly-free, the system records the predicted state vector as a historical predicted state. The corresponding prediction error covariance matrix is ​​denoted as the historical prediction covariance. ,in This represents the state prediction result of the previous period for the current moment. A quantitative indicator characterizing the uncertainty of the prediction result.

[0093] S32: Generate a state deviation vector based on the difference between the fused state estimate and the historical predicted state. Combine the historical prediction covariance to perform a weighted sum of squares operation on the state deviation vector. Use the inverse of the historical prediction covariance matrix to weight the state deviation vector and calculate its inner product to generate a standardized statistical deviation.

[0094] Specifically, the system generates a state bias vector based on the difference between the fused state estimate and the historical predicted state, and performs a weighted sum of squares operation on the historical predicted covariance to generate a standardized statistical bias. The system then calls the fused state estimate at the current time. This vector contains complete state components of the obstacle at the current moment, including its position, velocity, and heading, along with its historical predicted state. The dimensions are completely identical. The system performs interpolation calculations according to the corresponding state dimensions to generate a state deviation vector. ,in Each element corresponds to a deviation value in a single state dimension, accurately reflecting the difference between the current actual state and the historical predicted state.

[0095] Furthermore, the system obtains historical prediction covariance. The inverse matrix is ​​calculated using a matrix inversion algorithm. The matrix invertibility is ensured by checking the non-zero determinant during the inversion process. The system performs a weighted sum of squares operation, calculating according to the formula:

[0096]

[0097] in, The transpose of the state deviation vector is used to weight the deviations of different state dimensions by multiplying the transpose vector with the inverse matrix. This weighted sum is then multiplied by the original state deviation vector to obtain a scalar result. This result is the standardized statistical deviation. Its operational logic uses the inverse matrix of historical prediction covariance to weight the state deviation, so that the deviation quantification result can reflect the impact of prediction uncertainty and achieve standardized characterization of error.

[0098] S33: Calculate the difference between the position components in the actual state and the historical predicted state of the fused state estimate to generate a position deviation vector. Combine the standardized statistical deviation to calculate the Euclidean geometric distance between the actual position and the predicted position. Then, weight and sum the Euclidean geometric distance and the standardized statistical deviation according to a preset ratio to generate a mixed error scalar.

[0099] Specifically, the system generates a hybrid error scalar through position deviation calculation, Euclidean distance operation, and weighted fusion. The system first estimates the fused state from the current time step. Extract the position components to form a position vector. At the same time, based on historical predictions Extract the corresponding positional components to form a position vector. The positional dimensions of the two sets of vectors are consistent. The system calculates the positional deviation vector. And calculate the Euclidean geometric distance based on this vector:

[0100]

[0101] in, The actual position is obtained by transposing the position deviation vector and then performing square root operations on the sum of squares to get the straight-line distance between the actual and predicted positions. The system calls preset constants. Normalize the Euclidean geometric distance. A preset distance normalization benchmark is used to eliminate the impact of differences in distance dimensions under different scenarios on the fusion results. Subsequently, the system obtains preset mixing coefficients. This coefficient is used to balance the weights of the standardized statistical bias and the normalized Euclidean distance. Preferably, the formula for calculating the mixed error scalar is:

[0102]

[0103] in, This is the fusion state estimation vector at the current moment. This is the historical predicted state vector for the current moment, as predicted in the previous cycle. This is the corresponding historical prediction covariance matrix. , They represent from and The position vector extracted from it To balance the mixing coefficient of the two weights, This is a preset constant used to normalize the Euclidean distance.

[0104] In one embodiment, S4 of the control method based on an unmanned vessel provided by the present invention specifically includes the following steps:

[0105] S41: Perform smoothing and noise reduction on the mixed error scalar. Suppress the instantaneous high-frequency fluctuations of the error signal through a first-order low-pass filter, extract the slowly changing trend components in the mixed error scalar, and generate a smoothed error trend value.

[0106] Specifically, the system performs smoothing and noise reduction on the mixed error scalar. It suppresses instantaneous high-frequency fluctuations using a first-order low-pass filter, extracts slowly changing trend components, and generates a smoothed error trend value. The system acquires the mixed error scalar. This scalar contains comprehensive error information across both the full-state and position dimensions, and may exhibit high-frequency fluctuations due to instantaneous observation interference. The system invokes a preset first-order low-pass filter model to construct the filtering calculation formula:

[0107]

[0108] in, This represents the smoothed error trend value at the current moment. This represents the smoothed error trend value from the previous time step. These are the filter coefficients. This formula is used to balance the response speed of the current error scalar with the stability of historical smoothing results, and its value ranges from 0 to 1. The system performs iterative calculations according to this formula, weighting and fusing the current mixed error scalar with the smoothing result from the previous time step. By adjusting the filter coefficients, the impact of instantaneous high-frequency fluctuations on the results is weakened, highlighting the slow trend component of the error scalar that changes with prediction accuracy. During the calculation, the system initializes the smoothing results at the initial time step to ensure the validity of the starting conditions for the iterative calculations and to generate stable error trend values. This provides a stable input signal for subsequent nonlinear mapping.

[0109] S42: Calculate the nonlinear adjustment coefficient based on the error trend value. Input the error trend value into a nonlinear mapper with the hyperbolic tangent function as the core, smoothly compress the input value and map it into a preset positive coefficient range, and generate a dynamic adjustment coefficient for directly adjusting the process noise intensity.

[0110] Specifically, the system calculates nonlinear adjustment coefficients based on the error trend value. A nonlinear mapper constructed using the hyperbolic tangent function compresses and maps the error trend value to a preset positive coefficient range, generating dynamic adjustment coefficients. The system then acquires the error trend value generated in step S41. This signal is used as the input signal to a nonlinear mapper, which constructs the mapping relationship based on the hyperbolic tangent function. Preferably, the system uses a formula for calculation:

[0111]

[0112] in, This is the dynamic adjustment coefficient at the current moment. The mapping sensitivity coefficient, To determine the maximum value of the adjustment coefficient, This represents the minimum value of the adjustment coefficient. The output range of the hyperbolic tangent function is between -1 and 1. Through the linear transformation in the formula, it is mapped to... to The preset positive coefficient range ensures that the adjustment coefficient is always positive and within a reasonable range. This mapping process smoothly compresses the error trend value, avoiding large fluctuations in the adjustment coefficient due to sudden error changes, and generating a dynamic adjustment coefficient. It is positively correlated with the error trend value. When the error trend value increases, the adjustment coefficient moves closer to the maximum value, and when the error trend value decreases, the adjustment coefficient moves closer to the minimum value.

[0113] S43: Based on the dynamic adjustment coefficient, the preset process noise covariance matrix is ​​adaptively scaled element by element. Each element in the preset process noise covariance matrix is ​​combined with the dynamic adjustment coefficient to form a process noise covariance matrix that matches the current prediction error level and is used for trajectory prediction in the next cycle. Each element value of the process noise covariance matrix is ​​the result of scaling the corresponding element value of the original matrix by the same proportion.

[0114] Specifically, based on dynamic adjustment coefficients, the system adaptively scales the preset process noise covariance matrix element-wise to generate a new matrix that matches the current prediction error level, which is then used for trajectory prediction in the next cycle. The system obtains the dynamic adjustment coefficients. Simultaneously, retrieve the process noise covariance matrix used in the current cycle. This matrix is ​​a symmetric positive definite matrix, and the diagonal elements correspond to the process noise intensity of each state dimension. The system performs element-wise scaling operations, calculating according to the formula:

[0115]

[0116] in, Let be the element in the i-th row and j-th column of the process noise covariance matrix for the next cycle. The current periodic matrix is ​​the first... Line 1 Column elements, and These are the row and column indices of the matrix, respectively. During the operation, the system scales each element of the matrix by the same proportion to ensure that the matrix's symmetric positive definite properties remain unchanged and the noise intensity ratios of each state dimension are maintained as originally set. Through element-wise scaling, the overall strength of the process noise covariance matrix changes with the dynamic adjustment coefficient. When the prediction error is large, the matrix element values ​​increase accordingly, improving the prediction model's adaptability to uncertainty; when the prediction error is small, the matrix element values ​​decrease accordingly, ensuring the accuracy of the prediction model. The generated process noise covariance matrix... It dynamically matches the current prediction error level, providing adaptive uncertainty model support for trajectory prediction in the next cycle.

[0117] In one embodiment, S5 of the control method based on an unmanned vessel provided by the present invention specifically includes the following steps:

[0118] S51: Perform time-varying safety boundary construction processing on the predicted covariance sequence. Based on the uncertainty ellipsoid represented by the covariance matrix at each prediction time, generate a series of cylindrical safety boundaries surrounding the predicted position of each dynamic obstacle, with the radius changing with time, as spatial constraints.

[0119] Specifically, the system performs time-varying safety boundary construction on the predicted covariance sequence. Based on the uncertainty ellipsoid represented by the covariance matrix at each prediction time, it generates a series of cylindrical safety boundaries surrounding the predicted position of the dynamic obstacle, with radii varying over time, as spatial constraints. The system obtains the predicted covariance sequence. and the corresponding predicted state sequence N is the prediction time domain length. For each prediction time... The system uses the prediction covariance matrix at that moment. Extracting the xy-plane projection submatrix The extraction method involves retaining the rows and columns corresponding to the positional components in the matrix while removing elements from other dimensions. The system constructs an uncertainty ellipsoid based on this submatrix, the mathematical expression of which is:

[0120]

[0121] in, Let be the vector of the actual possible states of the obstacle. The chi-square distribution quantile, whose value is related to the degrees of freedom corresponding to the dimension of the state vector, is used to define the confidence coverage of the ellipsoid. The system calculates this using an eigenvalue decomposition algorithm. The eigenvalues ​​are selected, and the largest eigenvalue is chosen. Its square root corresponds to the length of the semi-major axis of the ellipse projected onto the xy plane by the uncertain ellipsoid.

[0122] Furthermore, the system introduces a safety margin factor. Through formula Calculate the radius of the base of the safety boundary of the cylinder. This is used to compensate for residual errors in uncertainty estimation. The cylinder height is set along the z-axis of the ship's coordinate system. The system acquires the unmanned vessel's draft parameters and the vertical dimension statistics of the obstacles, ensuring that the height covers the intersection of their vertical ranges. The system constructs the cylinder safety boundaries sequentially according to the prediction time sequence, forming a set of spatial constraints that dynamically changes over time, achieving dynamic matching between the safety boundaries and the uncertainty of obstacle motion.

[0123] S52: Based on the predicted state sequence and spatial constraints, a rolling optimization problem is constructed and processed. Under the model predictive control framework, the geometric relationship that the future trajectory of the unmanned vessel must not intrude into the safety boundary of a series of cylinders is transformed into a set of inequality constraints in a nonlinear programming problem, generating a complete optimization problem that includes dynamic constraints, control constraints and obstacle avoidance constraints.

[0124] Specifically, the system constructs a rolling optimization problem based on the predicted state sequence and spatial constraints. Under the model predictive control framework, the geometric relationship that the future trajectory of the unmanned vessel must not intrude into the safety boundaries of a series of cylinders is transformed into a set of inequality constraints in a nonlinear programming problem, generating a complete optimization problem including dynamic constraints, control quantity constraints, and obstacle avoidance constraints. Preferably, the system retrieves the nonlinear dynamic model of the unmanned vessel, which is established through ship maneuverability theory, and the state equations are in the form of:

[0125]

[0126] in, The inertia matrix, Here is the damping matrix. The Coriolis centripetal force matrix, Let the unmanned surface vessel's velocity vector be... To control the thrust and torque vectors of the input conversion, To control the step size, the system constructs a tracking performance objective function:

[0127]

[0128] in, For the desired reference trajectory state, This is a diagonal state weight matrix, where each diagonal element corresponds to the tracking priority of a different state. This is a diagonal control weight matrix used to suppress excessive control input. In the constraint transformation stage, the system calculates the Euclidean distance between the unmanned vessel and obstacles in the xy-plane and transforms the obstacle avoidance constraints into... The formula for calculating the Euclidean distance of the predicted location is:

[0129]

[0130] The system introduces dynamic constraints to ensure that the unmanned vessel's state changes conform to the laws of the dynamic model; it also introduces control quantity constraints. ,in and The objective function is determined by the physical characteristics of the actuator. The system integrates the objective function and three types of constraints to form a complete nonlinear programming optimization problem.

[0131] S53: The complete optimization problem is solved online and the optimal control sequence is extracted. The numerical optimization algorithm is used to solve the control input sequence that satisfies all constraints and optimizes the tracking performance index. The control quantity corresponding to the current moment is extracted from the control input sequence to generate the optimal control command to drive the unmanned vessel.

[0132] Specifically, the system performs online solution and optimal control sequence extraction for the complete optimization problem. It employs a numerical optimization algorithm to find the control input sequence that optimizes the tracking performance under all constraints, and extracts the control quantity at the current moment to generate the optimal control command. Based on the nonlinearity and constraint characteristics of the optimization problem, the system selects the interior-point method as the numerical optimization algorithm. This algorithm achieves unconstrained solution by transforming constraints into penalty terms in the objective function, thus meeting the requirements for fast online computation. The system initializes the control input sequence. Based on the optimal control sequence of the previous period, the terminal elements are supplemented by linear extrapolation to ensure that the sequence length meets the prediction time domain requirements.

[0133] During the solution process, the system updates the control input sequence according to the interior-point method iterative rules. After each iteration, the objective function value is calculated, and the satisfaction of dynamic constraints, control constraints, and obstacle avoidance constraints is verified. Constraint violations are quantified using preset criteria. Iterative convergence is determined using a dual condition: when the difference between the objective functions of two adjacent iterations reaches a certain threshold... ( The convergence threshold (determined by the control accuracy requirements) is used to stop the iteration and output the optimal control input sequence when all constraint violations are less than the constraint tolerance. The system extracts the first element of the sequence as the control variable for the current time step. The control parameters include propeller speed and rudder angle. The system verifies the validity of the control parameters to ensure they are within acceptable limits. and Within the defined interval, the system simultaneously verifies whether the rate of change of the control quantity meets the response capability of the actuator. After the verification is passed, the system encapsulates the control quantity into an optimal control command according to the communication protocol format and sends it to the control execution unit via the bus to drive the unmanned vessel's movement. This optimal control sequence is also stored in the cache unit to provide initialization input for the next cycle of optimization.

[0134] It should be understood that although the steps in the flowcharts of the embodiments described above are shown sequentially according to the arrows, these steps are not necessarily executed in the order indicated by the arrows. Unless explicitly stated herein, there is no strict order restriction on the execution of these steps, and they can be executed in other orders. Moreover, at least some steps in the flowcharts of the embodiments described above may include multiple steps or multiple stages. These steps or stages are not necessarily completed at the same time, but can be executed at different times. The execution order of these steps or stages is not necessarily sequential, but can be performed alternately or in turn with other steps or at least some of the steps or stages of other steps.

[0135] Based on the same inventive concept, this application also provides an unmanned vessel-based control device for implementing the aforementioned unmanned vessel-based control method. The solution provided by this device is similar to the implementation described in the above method; therefore, the specific limitations in one or more embodiments of the unmanned vessel-based control device provided below can be found in the limitations of the unmanned vessel-based control method described above, and will not be repeated here.

[0136] Preferably, such as Figure 3 As shown, the present invention provides a control system 600 based on an unmanned vessel, which is configured with the following modules:

[0137] The multi-source data fusion and filtering module 610 is used to fuse and filter the raw observation data collected by the multi-source sensors of the unmanned vessel. It fuses the data from millimeter-wave radar, lidar, automatic identification system and visual sensor through spatiotemporal alignment and capacitive Kalman filtering algorithm to generate the fused state estimate of all dynamic obstacles at the current moment and the corresponding estimation error covariance.

[0138] The obstacle trajectory rolling prediction module 620 is used to perform rolling prediction on the fusion state estimate, estimation error covariance and preset process noise covariance matrix. It uses a discrete linear Kalman predictor to recursively calculate the motion state and uncertainty of dynamic obstacles at multiple future times, and generates trajectory prediction results containing the predicted state sequence and predicted covariance sequence of all dynamic obstacles at multiple future times.

[0139] The trajectory prediction error calculation module 630 is used to estimate the error between the fused state estimation and the trajectory prediction result obtained from the previous cycle prediction, extract the actual state at the current moment and the state predicted for the current moment in the previous cycle, calculate the Mahalanobis distance and the Euclidean distance between the two based on the prediction uncertainty, and sum the two distances according to preset weights to generate a hybrid error scalar.

[0140] The process noise parameter correction module 640 is used to correct the model parameters of the mixed error scalar. It uses a nonlinear mapping relationship based on the hyperbolic tangent function to convert the smoothed error value into the adjustment coefficient of the process noise covariance matrix, and generates the process noise covariance matrix for trajectory prediction in the next cycle.

[0141] The predictive control command optimization module 650 is used to perform predictive control optimization on the predicted state sequence and the predicted covariance sequence, construct and solve the optimization problem constrained by the time-varying safety distance, and generate and execute the optimal control command to drive the unmanned vessel at the current moment.

[0142] Preferably, the multi-source data fusion filtering module 610 provided in this application is configured with the following units:

[0143] The multi-source data spatiotemporal alignment unit is used to perform spatiotemporal alignment preprocessing on the raw observation data collected by the multi-source sensors of the unmanned vessel, unify the timestamps and spatial coordinate systems of the data from each sensor, and generate synchronized observation data with consistent spatiotemporal references.

[0144] The observation information weighted fusion unit is used to perform multi-source information fusion processing on synchronous observation data. It performs weighted fusion of the state observation values ​​of the same obstacle based on the confidence levels of different sensors to generate a fused observation vector with higher accuracy and robustness.

[0145] The fusion state filtering estimation unit is used to perform nonlinear filtering estimation on the fusion observation vector, establish an obstacle kinematic model including position, velocity and random acceleration perturbations, and perform state recursion and update through the capacitive Kalman filter algorithm to generate the fusion state estimate of all dynamic obstacles at the current time and the corresponding estimation error covariance.

[0146] Preferably, the obstacle trajectory rolling prediction module 620 provided in this application is configured with the following units:

[0147] The process noise initialization unit is used to adaptively initialize the preset process noise covariance matrix. It dynamically sets the amplitude of its diagonal elements according to the current sea state level and sensor noise characteristics to generate an initial process noise covariance matrix that matches the environment.

[0148] The uncertainty propagation unit is used to propagate uncertainty in the initial process noise covariance matrix based on the state transition matrix recursive algorithm. Under the discrete linear Kalman prediction framework, it recursively calculates the evolution process of the fused state estimate and the estimation error covariance over time, and generates intermediate predicted states and intermediate predicted covariance sequences.

[0149] The prediction sequence formatting unit is used to perform prediction time domain truncation and formatting processing on the intermediate prediction state and intermediate prediction covariance sequences. According to the prediction time domain length of the model prediction control, it extracts all state vectors within the corresponding time window from the intermediate prediction state sequence and extracts all covariance matrices within the same time window from the intermediate prediction covariance sequence. It generates prediction state sequences and prediction covariance sequences for all dynamic obstacles at multiple future times, and integrates them to generate trajectory prediction results.

[0150] Preferably, the trajectory prediction error calculation module 630 provided in this application is configured with the following units:

[0151] The historical prediction extraction unit is used to extract and process data from the cached trajectory prediction results of the previous cycle, obtain the prediction state vector for the current moment and its corresponding prediction error covariance matrix, and generate historical prediction state and historical prediction covariance.

[0152] The statistical bias calculation unit is used to generate a state bias vector based on the difference between the fused state estimate and the historical predicted state. It performs a weighted sum of squares operation on the state bias vector in combination with the historical prediction covariance. It then uses the inverse of the historical prediction covariance matrix to weight the state bias vector and calculates its inner product to generate a standardized statistical bias.

[0153] The hybrid error generation unit is used to calculate the difference between the position components in the actual state and the historical predicted state of the fused state estimate, generate a position deviation vector, and calculate the Euclidean geometric distance between the actual position and the predicted position by combining the standardized statistical deviation. The Euclidean geometric distance and the standardized statistical deviation are weighted and summed according to a preset ratio to generate a hybrid error scalar.

[0154] Preferably, the process noise parameter correction module 640 provided in this application is configured with the following units:

[0155] The error smoothing filter unit is used to smooth and reduce noise in the mixed error scalar. It suppresses the instantaneous high-frequency fluctuations of the error signal through a first-order low-pass filter, extracts the slowly changing trend components in the mixed error scalar, and generates a smoothed error trend value.

[0156] The nonlinear coefficient calculation unit is used to calculate the nonlinear adjustment coefficient based on the error trend value. The error trend value is input into a nonlinear mapper with the hyperbolic tangent function as its core, which smoothly compresses and maps the input value to a preset positive coefficient range, generating a dynamic adjustment coefficient for directly adjusting the process noise intensity.

[0157] The noise matrix adjustment unit is used to adaptively scale the preset process noise covariance matrix element by element based on the dynamic adjustment coefficient. It combines each element in the preset process noise covariance matrix with the dynamic adjustment coefficient to generate a process noise covariance matrix that matches the current prediction error level and is used for trajectory prediction in the next cycle. Each element value of the process noise covariance matrix is ​​the result of scaling the corresponding element value of the original matrix by the same proportion.

[0158] Preferably, the predictive control command optimization module 650 provided in this application is configured with the following units:

[0159] The time-varying safety boundary construction unit is used to construct time-varying safety boundaries for the predicted covariance sequence. Based on the uncertainty ellipsoid represented by the covariance matrix at each prediction time, a series of cylindrical safety boundaries surrounding the predicted position of each dynamic obstacle and whose radius changes with time are generated as spatial constraints.

[0160] The rolling optimization problem construction unit is used to construct rolling optimization problems based on predicted state sequences and spatial constraints. Under the model predictive control framework, the geometric relationship that the future trajectory of the unmanned vessel must not intrude into the safety boundary of a series of cylinders is transformed into a set of inequality constraints in a nonlinear programming problem, generating a complete optimization problem that includes dynamic constraints, control constraints and obstacle avoidance constraints.

[0161] The optimal control command solving unit is used to solve the complete optimization problem online and extract the optimal control sequence. It uses numerical optimization algorithms to solve the control input sequence that satisfies all constraints and optimizes the tracking performance index. It also extracts the control quantity corresponding to the current moment from the control input sequence and generates the optimal control command to drive the unmanned vessel.

[0162] In one embodiment, this application also provides a computer device, including a memory and a processor, wherein the memory stores a computer program, and the processor executes the computer program to implement the above-described unmanned vessel-based control method.

[0163] In one embodiment, this application also provides a computer-readable storage medium storing a computer program thereon, which, when executed by a processor, implements the above-described unmanned vessel-based control method.

[0164] In the description of this specification, references to terms such as "one embodiment," "some embodiments," "example," "specific example," or "some examples," etc., indicate that a specific feature, structure, material, or characteristic described in connection with that embodiment or example is included in at least one embodiment or example of this application. Furthermore, the specific features, structures, materials, or characteristics described may be combined in any suitable manner in one or more embodiments or examples. Moreover, without contradiction, those skilled in the art can combine and integrate the different embodiments or examples described in this specification, as well as the features of those different embodiments or examples.

[0165] For the device embodiments, since they basically correspond to the method embodiments, the relevant parts can be referred to in the description of the method embodiments. The device embodiments described above are merely illustrative. The components described as separate parts may or may not be physically separate, and the components shown as units may or may not be physical units, that is, they may be located in one place or distributed across multiple network units. Some or all of the modules can be selected to achieve the purpose of this disclosure according to actual needs. Those skilled in the art can understand and implement this without creative effort.

[0166] The above description is merely a specific embodiment of this application, but the scope of protection of this application is not limited thereto. Any person skilled in the art can easily conceive of various variations or substitutions within the technical scope disclosed in this application, and these should all be included within the scope of protection of this application. Therefore, the scope of protection of this application should be determined by the scope of the claims.

Claims

1. A control method based on an unmanned surface vessel, characterized in that, Includes the following steps: S1: The raw observation data collected by the multi-source sensors of the unmanned vessel is fused and filtered. The data from millimeter-wave radar, lidar, automatic identification system and visual sensor are fused through spatiotemporal alignment and capacitive Kalman filtering algorithm to generate the fused state estimate of all dynamic obstacles at the current moment and the corresponding estimation error covariance. S2: Perform rolling prediction on the fusion state estimate, the estimation error covariance and the preset process noise covariance matrix, and use a discrete linear Kalman predictor to recursively calculate the motion state and uncertainty of the dynamic obstacle at multiple future times, and generate a trajectory prediction result containing the predicted state sequence and predicted covariance sequence of all dynamic obstacles at multiple future times. S3: Perform error estimation on the fusion state estimation and the trajectory prediction result obtained from the previous cycle prediction, extract the actual state at the current moment and the state predicted for the current moment in the previous cycle, calculate the Mahalanobis distance and the Euclidean distance between the two based on the prediction uncertainty, and sum the two distances according to preset weights to generate a hybrid error scalar. S4: Perform model parameter correction on the hybrid error scalar, and use the nonlinear mapping relationship based on the hyperbolic tangent function to convert the smoothed error value into the adjustment coefficient of the process noise covariance matrix, and generate the process noise covariance matrix for trajectory prediction in the next cycle. S5: Perform predictive control optimization on the predicted state sequence and the predicted covariance sequence, construct and solve an optimization problem constrained by time-varying safety distance, and generate and execute the optimal control command to drive the unmanned vessel at the current moment.

2. The method according to claim 1, characterized in that, S1 includes: S11: Perform spatiotemporal alignment preprocessing on the raw observation data collected by the multi-source sensors of the unmanned vessel, unify the timestamps and spatial coordinate systems of the data from each sensor, and generate synchronous observation data with consistent spatiotemporal references. S12: Perform multi-source information fusion processing on the synchronous observation data, and perform weighted fusion of the state observation values ​​of the same obstacle based on the confidence levels of different sensors to generate a fusion observation vector with higher accuracy and robustness. S13: Perform nonlinear filtering estimation on the fused observation vector to establish an obstacle kinematic model that includes position, velocity and random acceleration perturbations, and perform state recursion and update through the capacitive Kalman filter algorithm to generate the fused state estimate of all dynamic obstacles at the current time and the corresponding estimation error covariance.

3. The method according to claim 1, characterized in that, S2 includes: S21: Adaptively initialize the preset process noise covariance matrix, dynamically set the amplitude of its diagonal elements according to the current sea state level and sensor noise characteristics, and generate an initial process noise covariance matrix that matches the environment. S22: Based on the state transition matrix recursive algorithm, uncertainty propagation is performed on the initial process noise covariance matrix. Under the discrete linear Kalman prediction framework, the evolution process of the fused state estimate and the estimation error covariance is recursively calculated over time to generate intermediate prediction states and intermediate prediction covariance sequences. S23: Perform prediction time-domain truncation and formatting processing on the intermediate predicted state and the intermediate predicted covariance sequence. According to the prediction time-domain length of the model prediction control, extract all state vectors within the corresponding time window from the intermediate predicted state sequence, and extract all covariance matrices within the same time window from the intermediate predicted covariance sequence. Generate the predicted state sequence and the predicted covariance sequence for all dynamic obstacles at multiple future times, and integrate them to generate trajectory prediction results.

4. The method according to claim 1, characterized in that, S3 includes: S31: Extract data from the trajectory prediction results of the previous cycle in the cache, obtain the prediction state vector for the current moment and its corresponding prediction error covariance matrix, and generate historical prediction state and historical prediction covariance. S32: Generate a state deviation vector based on the difference between the fused state estimate and the historical predicted state. Perform a weighted sum of squares operation on the state deviation vector in combination with the historical predicted covariance. Calculate the inner product of the state deviation vector by weighting it with the inverse of the historical predicted covariance matrix to generate a standardized statistical deviation. S33: Calculate the difference between the position components in the actual state estimated by the fusion state and the position components in the historical predicted state to generate a position deviation vector. Simultaneously calculate the Euclidean geometric distance between the actual position and the predicted position by combining the standardized statistical deviation. Then, weight and sum the Euclidean geometric distance and the standardized statistical deviation according to a preset ratio to generate a mixed error scalar.

5. The method according to claim 4, characterized in that, The formula for calculating the hybrid error scalar is: in, This is the fusion state estimation vector at the current moment. This is the historical predicted state vector for the current moment, as predicted in the previous cycle. This is the corresponding historical prediction covariance matrix. , They represent from and The position vector extracted from it. To balance the mixing coefficient of the two weights, This is a preset constant used to normalize the Euclidean distance.

6. The method according to claim 1, characterized in that, S4 includes: S41: The mixed error scalar is smoothed and denoised by using a first-order low-pass filter to suppress the instantaneous high-frequency fluctuations of the error signal, extracting the slowly changing trend components in the mixed error scalar, and generating a smoothed error trend value. S42: Calculate the nonlinear adjustment coefficient based on the error trend value, input the error trend value into a nonlinear mapper with the hyperbolic tangent function as the core, smoothly compress the input value and map it into a preset positive coefficient range, and generate a dynamic adjustment coefficient for directly adjusting the process noise intensity; S43: Based on the dynamic adjustment coefficient, the preset process noise covariance matrix is ​​adaptively scaled element by element. Each element in the preset process noise covariance matrix is ​​combined with the dynamic adjustment coefficient to form a process noise covariance matrix that matches the current prediction error level and is used for trajectory prediction in the next cycle. Each element value of the process noise covariance matrix is ​​the result of scaling the corresponding element value of the original matrix by the same proportion.

7. The method according to any one of claims 1-6, characterized in that, S5 includes: S51: Perform time-varying safety boundary construction processing on the predicted covariance sequence. Based on the uncertainty ellipsoid represented by the covariance matrix at each prediction time, generate a series of cylindrical safety boundaries surrounding the predicted position of each dynamic obstacle, with the radius varying with time, as spatial constraints. S52: Based on the predicted state sequence and the spatial constraints, a rolling optimization problem is constructed and processed. Under the model predictive control framework, the geometric relationship that the future trajectory of the unmanned vessel must not intrude into the safety boundary of the series of cylinders is transformed into a set of inequality constraints in the nonlinear programming problem, generating a complete optimization problem that includes dynamic constraints, control constraints and obstacle avoidance constraints. S53: Solve the complete optimization problem online and extract the optimal control sequence. Use numerical optimization algorithm to solve the control input sequence that satisfies all constraints and optimizes the tracking performance index. Extract the control quantity corresponding to the current moment from the control input sequence and generate the optimal control command to drive the unmanned vessel.

8. A control system based on an unmanned vessel, characterized in that, The system includes: The multi-source data fusion and filtering module is used to fuse and filter the raw observation data collected by the multi-source sensors of the unmanned vessel. It fuses the data from millimeter-wave radar, lidar, automatic identification system and visual sensor through spatiotemporal alignment and capacitive Kalman filtering algorithm to generate the fused state estimate of all dynamic obstacles at the current moment and the corresponding estimation error covariance. The obstacle trajectory rolling prediction module is used to perform rolling prediction on the fused state estimate, the estimation error covariance and the preset process noise covariance matrix. It uses a discrete linear Kalman predictor to recursively calculate the motion state and uncertainty of the dynamic obstacle at multiple future moments, and generates a trajectory prediction result containing the predicted state sequence and predicted covariance sequence of all dynamic obstacles at multiple future moments. The trajectory prediction error calculation module is used to estimate the error between the fused state estimate and the trajectory prediction result obtained from the previous cycle prediction, extract the actual state at the current moment and the state predicted for the current moment in the previous cycle, calculate the Mahalanobis distance and the Euclidean distance between the two based on the prediction uncertainty, and weight the two distances according to preset weights to generate a hybrid error scalar. The process noise parameter correction module is used to correct the model parameters of the mixed error scalar. It uses a nonlinear mapping relationship based on the hyperbolic tangent function to convert the smoothed error value into the adjustment coefficient of the process noise covariance matrix, and generates the process noise covariance matrix for trajectory prediction in the next cycle. The predictive control command optimization module is used to perform predictive control optimization on the predicted state sequence and the predicted covariance sequence, construct and solve an optimization problem constrained by a time-varying safety distance, and generate and execute the optimal control command to drive the unmanned vessel at the current moment.

9. A computer device comprising a memory and a processor, wherein the memory stores a computer program, characterized in that, When the processor executes the computer program, it implements the method of any one of claims 1 to 7.

10. A computer-readable storage medium having a computer program stored thereon, characterized in that, When the computer program is executed by a processor, it implements the method of any one of claims 1 to 7.

Images