An unmanned ship adaptive navigation control method and device

Abstract

This invention discloses an adaptive navigation control method and apparatus for unmanned surface vessels (USVs), belonging to the field of adaptive navigation control for USVs. The method comprises: obtaining a current global state estimate of the USV based on historical USV control commands, historical USV global state estimates, and raw observation data; obtaining a set of current USV self-state estimates and current target state estimates based on the current USV global state estimate; for each current target state estimate in the current target state estimate set, obtaining a threat score based on the current USV self-state estimate and the current target state estimate; obtaining a threat assessment result based on all threat scores corresponding to the target state estimate set; obtaining a current USV control command based on the current USV global state estimate, the threat assessment result, and a preset top-level mission objective; and controlling the USV to perform USV navigation actions based on the current USV control command, thereby improving the reliability of adaptive navigation for USVs.

Description

Technical Field

[0001] This invention relates to the field of adaptive navigation control for unmanned surface vessels (USVs), and more particularly to an adaptive navigation control method and apparatus for USVs. Background Technology

[0002] Current unmanned surface vessel (USV) navigation technology primarily relies on traditional path planning and target recognition methods. Path planning often employs algorithms based on static maps for route design. This approach is only suitable for pre-defined tasks and low-dynamic environments, failing to handle dynamic obstacles and complex sea conditions, and struggling to adapt to unexpected situations such as floating objects and ocean currents. Target recognition typically uses a single type of sensor (such as sonar or radar) for target detection and identification, resulting in limited recognition capabilities in complex environments, high false detection rates, and insufficient multi-target tracking capabilities. During system operation, manual intervention for target determination is often required, leading to slow response times and, obviously, problems such as collisions and entry into complex sea conditions during USV navigation and control. Summary of the Invention

[0003] This invention provides an adaptive navigation control method and apparatus for unmanned surface vessels (USVs), which can solve the above-mentioned problems in the prior art and improve the reliability of adaptive navigation for USVs.

[0004] This invention provides an adaptive navigation control method for unmanned surface vessels, comprising: Acquire raw observation data, historical unmanned surface vessel (USV) control commands, and historical USV global state estimates; Based on the historical unmanned surface vessel control commands, the historical unmanned surface vessel global state estimates, and the original observation data, the current unmanned surface vessel global state estimate is obtained; Based on the current global state estimation of the unmanned surface vessel, a set of current unmanned surface vessel state estimation and current target state estimation is obtained; For each current target state estimate in the current target state estimate set, a threat score is obtained based on the current unmanned surface vessel's own state estimate and the current target state estimate. The threat assessment result is obtained based on all the threat scores corresponding to the target state estimation set; Based on the current global state estimation of the unmanned surface vessel, the threat assessment results, and the preset top-level mission objective, the current control command for the unmanned surface vessel is obtained; Based on the current unmanned surface vessel (USV) control commands, the USV is controlled to perform corresponding unmanned surface vessel navigation actions.

[0005] By updating the original observation data using historical UAV control commands and historical UAV global state estimates, an accurate current UAV global state estimate is obtained. This enables the UAV to perceive and update its state in real time in the dynamic marine environment. Based on this, a set of current UAV self-state estimates and current target state estimates is extracted. Threat assessment results are formed by calculating the threat score of each current target state estimate. Finally, the threat assessment results are combined with the preset top-level mission objectives to generate current control commands to execute navigation actions. This significantly improves the estimation accuracy of the UAV's own position and surrounding targets, enabling the UAV to proactively identify high-threat targets and optimize its navigation strategy accordingly, effectively avoiding potential risks. This greatly enhances the UAV's adaptive navigation capability in complex sea conditions, thereby improving the reliability of UAV adaptive navigation.

[0006] Furthermore, the historical unmanned surface vessel (USV) global state estimation includes a set of historical USV self-state estimations, historical environment map estimations, and historical target state estimations. The step of obtaining the current USV global state estimation based on the historical USV control commands, the historical USV global state estimations, and the original observation data includes: The predicted state is obtained based on the historical unmanned surface vessel control commands and the historical unmanned surface vessel's own state estimation. The original observation data is fused with the predicted state to obtain the current state estimate of the unmanned surface vessel. The historical environmental map estimate is updated based on the original observation data to obtain the current environmental map estimate. The historical target state estimation set is updated based on the original observation data to obtain the current target state estimation set; Based on the current unmanned surface vessel's own state estimate, the current environment map estimate, and the current target state estimate set, the current unmanned surface vessel's global state estimate is obtained.

[0007] In the above scheme, the global state estimation of the historical unmanned surface vessel (USV) is decomposed into a set of historical USV self-state estimation, historical environment map estimation, and historical target state estimation. The scheme then uses historical control commands and the self-state estimation to perform state prediction, fuses the original observation data with the predicted state to obtain the current self-state estimation, and updates the environment map and target state estimation sets using the original observation data. Finally, the scheme synthesizes the current USV global state estimation, realizing the joint recursive estimation of the USV's self-state, environmental obstacles, and the states of multiple targets. This improves the accuracy and stability of the global state estimation and provides complete and accurate information for subsequent threat assessment and navigation decision-making.

[0008] Further, updating the historical environmental map estimate based on the original observation data to obtain the current environmental map estimate includes: Get the grid region; A likelihood ratio is constructed based on the original observation data and the grid region; The historical environment map estimate is updated based on the likelihood ratio to obtain the current environment map estimate.

[0009] In the above scheme, by acquiring the grid area and constructing the likelihood ratio based on the original observation data and the grid area, and then using the likelihood ratio to update the historical environmental map estimate to obtain the current environmental map estimate, Bayesian recursive update of the grid map is realized. This enables the unmanned surface vessel to efficiently update the distribution of environmental obstacles by relying only on the current observation and the map accumulated information of the previous moment, reducing data storage and computing overhead, while ensuring the map's real-time response capability to dynamic obstacles.

[0010] Furthermore, the step of obtaining the current unmanned surface vessel control command based on the current global state estimation, the threat assessment result, and the preset top-level mission objective includes: Generate a reference path sequence based on the preset top-level task objective; Based on the threat assessment results, the current state estimate of the unmanned surface vessel, and the current environmental map estimate, the obstacle avoidance cost and the threat avoidance cost are obtained. Based on the reference path sequence, the obstacle avoidance cost, the threat avoidance cost, and the current unmanned surface vessel's own state estimate, the current unmanned surface vessel control command is obtained.

[0011] In the above scheme, a reference path sequence is generated based on a preset top-level mission objective. Obstacle avoidance cost and threat avoidance cost are calculated based on threat assessment results, current unmanned surface vessel state estimation, and current environment map estimation, respectively. Then, the reference path sequence and the unmanned surface vessel state estimation are combined to generate the current unmanned surface vessel control command. This enables the unmanned surface vessel to simultaneously take into account mission orientation, obstacle avoidance, and high-threat target avoidance during navigation, achieving path optimization under multiple constraints and improving navigation safety and mission completion efficiency.

[0012] Further, the step of obtaining the current unmanned surface vessel (USV) control command based on the reference path sequence, the obstacle avoidance cost, the threat avoidance cost, and the current USV state estimate includes: Based on the reference path sequence, the obstacle avoidance cost, the threat avoidance cost, and the current unmanned surface vessel's own state estimate, an unmanned surface vessel target cost function is constructed; The target cost function of the unmanned surface vessel is solved based on preset constraints to obtain the optimal control sequence of the unmanned surface vessel; The first control variable in the optimal control sequence of the unmanned surface vessel is taken as the current control command of the unmanned surface vessel.

[0013] In the above scheme, an objective cost function for the unmanned surface vessel (USV) is constructed based on a reference path sequence, obstacle avoidance cost, threat avoidance cost, and the current state estimate of the USV itself. The optimal control sequence of the USV is obtained by solving the cost function under preset constraints. The first control variable in the sequence is then used as the current control command of the USV. A model predictive control framework is introduced to achieve optimal decision-making in the rolling time domain. This enables the USV to anticipate state changes within a finite number of steps and adjust the control variable in real time, effectively coping with system dynamic constraints and environmental uncertainties, and improving the reliability of control.

[0014] Furthermore, controlling the unmanned surface vessel (USV) to perform corresponding navigation actions based on the current USV control command includes: The desired speed and desired rudder angle are obtained based on the current unmanned surface vessel control commands; The unmanned surface vessel (USV) is controlled to perform a corresponding navigation speed based on the desired speed, and the USV is controlled to perform a corresponding navigation deflection based on the desired rudder angle.

[0015] In the above scheme, the desired speed and desired rudder angle are parsed from the current unmanned surface vessel (USV) control commands, and the sailing speed is controlled based on the desired speed and the sailing deflection is controlled based on the desired rudder angle, respectively. The control commands planned by the upper layer are directly mapped into the execution actions of the propeller and servo motor, realizing dual-channel precise control of the speed and direction of the USV's movement. This ensures that the USV can accurately track the planned path and thus reliably complete the navigation maneuver.

[0016] Another embodiment of the present invention also provides an adaptive navigation control device for unmanned surface vessels, comprising: The basic data acquisition module is used to acquire raw observation data, historical unmanned surface vessel control commands, and historical unmanned surface vessel global state estimates. The current unmanned surface vessel global state estimation acquisition module is used to obtain the current unmanned surface vessel global state estimate based on the historical unmanned surface vessel control commands, the historical unmanned surface vessel global state estimates, and the original observation data; The module for obtaining the self-state and target state is used to obtain a set of current unmanned surface vessel self-state estimates and current target state estimates based on the current global state estimate of the unmanned surface vessel. The threat score acquisition module is used to obtain a threat score for each current target state estimate in the current target state estimate set, based on the current unmanned surface vessel's own state estimate and the current target state estimate. The threat assessment result acquisition module is used to obtain the threat assessment result based on all the threat scores corresponding to the target state estimation set; The current unmanned surface vessel (USV) control command acquisition module is used to obtain the current USV control command based on the current USV global state estimation, the threat assessment result, and the preset top-level mission objective. The execution module is used to control the unmanned surface vessel to perform corresponding unmanned surface vessel navigation actions based on the current unmanned surface vessel control command.

[0017] Furthermore, the historical unmanned surface vessel (USV) global state estimation includes a set of historical USV self-state estimations, historical environment map estimations, and historical target state estimations. The current USV global state estimation acquisition module is used to obtain the current USV global state estimation based on the historical USV control commands, the historical USV global state estimations, and the original observation data, including: The predicted state is obtained based on the historical unmanned surface vessel control commands and the historical unmanned surface vessel's own state estimation. The original observation data is fused with the predicted state to obtain the current state estimate of the unmanned surface vessel. The historical environmental map estimate is updated based on the original observation data to obtain the current environmental map estimate. The historical target state estimation set is updated based on the original observation data to obtain the current target state estimation set; Based on the current unmanned surface vessel's own state estimate, the current environment map estimate, and the current target state estimate set, the current unmanned surface vessel's global state estimate is obtained.

[0018] Further, updating the historical environmental map estimate based on the original observation data to obtain the current environmental map estimate includes: Get the grid region; A likelihood ratio is constructed based on the original observation data and the grid region; The historical environment map estimate is updated based on the likelihood ratio to obtain the current environment map estimate.

[0019] Furthermore, the step of obtaining the current unmanned surface vessel control command based on the current global state estimation, the threat assessment result, and the preset top-level mission objective includes: Generate a reference path sequence based on the preset top-level task objective; Based on the threat assessment results, the current state estimate of the unmanned surface vessel, and the current environmental map estimate, the obstacle avoidance cost and the threat avoidance cost are obtained. Based on the reference path sequence, the obstacle avoidance cost, the threat avoidance cost, and the current unmanned surface vessel's own state estimate, the current unmanned surface vessel control command is obtained. Attached Figure Description

[0020] To more clearly illustrate the technical solution of this application, the drawings used in the embodiments will be briefly introduced below. Obviously, the drawings described below are only some embodiments of this application. For those skilled in the art, other drawings can be obtained from these drawings without creative effort.

[0021] Figure 1 This is a flowchart illustrating an adaptive navigation control method for unmanned surface vessels according to an embodiment of the present invention. Figure 2 This is a schematic diagram of the structure of an adaptive navigation control device for an unmanned surface vessel provided in an embodiment of the present invention. Detailed Implementation

[0022] To make the objectives, technical solutions, and advantages of this application clearer, the technical solutions of this application will be clearly and completely described below with reference to the accompanying drawings of the embodiments. Obviously, the described embodiments are only some embodiments of this application, not all embodiments. Based on the embodiments of this application, all other embodiments obtained by those skilled in the art without creative effort are within the scope of protection of this application.

[0023] 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 application pertains; the terminology used herein is for the purpose of describing particular embodiments only and is not intended to limit the application; the terms “comprising” and “having”, and any variations thereof, in the specification, claims, and foregoing description of the drawings are intended to cover non-exclusive inclusion.

[0024] In the description of the embodiments of this application, technical terms such as "first" and "second" are used only to distinguish different objects and should not be construed as indicating or implying relative importance or implicitly specifying the number, specific order, or primary and secondary relationship of the indicated technical features. In the description of the embodiments of this application, "multiple" means two or more, unless otherwise explicitly defined.

[0025] In this document, the term "embodiment" means that a particular feature, structure, or characteristic described in connection with an embodiment may be included in at least one embodiment of this application. The appearance of this phrase in various places throughout the specification does not necessarily refer to the same embodiment, nor is it a separate or alternative embodiment mutually exclusive with other embodiments. It will be explicitly and implicitly understood by those skilled in the art that the embodiments described herein can be combined with other embodiments.

[0026] In the description of the embodiments in this application, the term "and / or" is merely a description of the relationship between related objects, indicating that three relationships can exist. For example, A and / or B can represent: A existing alone, A and B existing simultaneously, and B existing alone. Additionally, the character " / " in this document generally indicates that the preceding and following related objects have an "or" relationship.

[0027] In the description of the embodiments of this application, the term "multiple" refers to two or more (including two), similarly, "multiple sets" refers to two or more (including two sets), and "multiple pieces" refers to two or more (including two pieces).

[0028] In the description of the embodiments of this application, unless otherwise expressly specified and limited, technical terms such as "installation," "connection," "joining," and "fixing" should be interpreted broadly. For example, they can refer to a fixed connection, a detachable connection, or an integral part; they can refer to a mechanical connection or an electrical connection; they can refer to a direct connection or an indirect connection through an intermediate medium; they can refer to the internal communication of two components or the interaction between two components. For those skilled in the art, the specific meaning of the above terms in the embodiments of this application can be understood according to the specific circumstances.

[0029] See Figure 1 To address the aforementioned problems in the prior art and improve the reliability of adaptive navigation for unmanned surface vessels (USVs), an embodiment of the present invention provides an adaptive navigation control method for USVs, comprising: Step S1: Acquire raw observation data, historical unmanned surface vessel control commands, and historical unmanned surface vessel global state estimates; Step S2: Based on the historical unmanned surface vessel control commands, the historical unmanned surface vessel global state estimates, and the original observation data, obtain the current unmanned surface vessel global state estimate; Step S3: Based on the current global state estimation of the unmanned surface vessel, obtain the current state estimation of the unmanned surface vessel itself and the current state estimation of the target; Step S4: For each current target state estimate in the current target state estimate set, based on the current unmanned surface vessel's own state estimate and the current target state estimate, obtain the threat score of the current target state estimate; Step S5: Based on all the threat scores corresponding to the target state estimation set, obtain the threat assessment result; Step S6: Based on the current global state estimation of the unmanned surface vessel, the threat assessment results, and the preset top-level mission objective, obtain the current control command for the unmanned surface vessel; Step S7: Control the unmanned surface vessel to perform the corresponding unmanned surface vessel navigation actions based on the current unmanned surface vessel control command.

[0030] In the above scheme, the original observation data is updated by utilizing historical unmanned surface vessel (USV) control commands and the historical USV global state estimates, thereby obtaining an accurate current USV global state estimate. This enables the USV to perceive and update the dynamic marine environment in real time. Based on this, the current USV's own state estimate and the current target state estimate set are extracted. A threat assessment result is formed by calculating the threat score of each current target state estimate. Finally, the threat assessment result is combined with the preset top-level mission objective to generate the current control command to execute navigation actions. This significantly improves the estimation accuracy of the USV's own position and surrounding targets, enabling the USV to actively identify high-threat targets and optimize its navigation strategy accordingly, effectively avoiding potential risks. This greatly enhances the adaptive navigation capability in complex sea conditions, thereby improving the reliability of the USV's adaptive navigation.

[0031] It should be noted that the acquisition of raw observation data, historical unmanned surface vessel (USV) control commands, and historical USV global state estimates specifically involves: Global state estimation The general expression is: ; It should be noted that, For the global estimated state at time t, the corresponding state is... For time t-1, that is, the historical global estimated state corresponding to the time before time t, where... , and Similarly, , and Represent , and Historical parameters corresponding to the previous moment.

[0032] The current state estimate of the unmanned surface vessel is derived from the fusion estimate of the data processing module: ; , Here, x represents the x-axis coordinate, y represents the y-axis coordinate, and z represents the z-axis coordinate; , Let u be the linear velocity component along the x-axis, v be the linear velocity component along the y-axis, and w be the linear velocity component along the z-axis. , As a gesture, Let be the rotation angle about the x-axis. Let be the rotation angle about the y-axis. Let be the rotation angle about the z-axis.

[0033] M(t) is the environmental map estimate, which originates from the mapping algorithm of the data processing module and is a probabilistic raster map.

[0034] The target state set originates from the target tracking algorithm in the data processing module. = { },in It includes the position, velocity, and attributes of the k-th target state.

[0035] Furthermore, the raw observation data can be acquired through sensor modules, which perceive the physical world to provide noisy observations of a portion of the unmanned surface vessel's global state estimation. Specifically, the sensor modules perceive the real-world state. (t), thus outputting the original observation vector set. The original observation vector set is the original observation data.

[0036] The sensor module is the starting point of the data stream, and its output Z(t) is the sole data source for all subsequent processing. Specifically, for the original observation vector of the i-th sensor: For sensor readings, such as distance, angle, pixel matrix, etc.; (·) represents the sensor physical model, describing how the real state is mapped to the ideal observation; The real-world state at time t (real boat state, real target position, etc.); These are inherent parameters (constants) of the sensor, such as mounting pose, focal length, intrinsic parameters, etc. The observed noise follows a zero-mean Gaussian distribution, and its covariance matrix is... .

[0037] Each sensor has its own corresponding physical model, specifically, (·) The sensor physical model specifically includes the following: (1) GNSS (such as GPS) sensor: ; in, This is the GNSS sensor observation vector, used to characterize the position measurement value output by the GNSS receiver at time t; is the actual position of the unmanned surface vessel (USV), used to characterize the USV's true geographical location at time t; GNSS systematic error / bias is used to characterize systematic biases and errors in GNSS measurements; This is GNSS random observation noise, used to characterize the random noise component in GNSS measurements.

[0038] (2) Radar sensor: ; in, Let be the radar sensor observation vector, representing the output of the radar sensor at time t; ‖·‖ is the Euclidean norm used to calculate the distance; actual vessel position. True target location and real submarine arctan2( ,) is the arctangent function in the four quadrants, returning angles in the range [-π, π]. This represents the position of time on the x-axis. The position of the velocity on the x-axis. This represents the position of time on the y-axis. The position of the velocity in the y-coordinate axis. Radial velocity represents the velocity component of the target relative to the radar along the line-of-sight (radar-target line). This is the radar observation noise vector, representing the random errors superimposed during radar measurements.

[0039] (3) IMU sensor: ; in, The IMU observation vector represents the acceleration and angular velocity measurements output by the IMU at time t. Specific force (specific force vector) characterizes the non-gravitational acceleration acting on the IMU, i.e., specific force; This is the projection of the gravity vector in the volume coordinate system, representing the component of gravitational acceleration under the current hull attitude; Angular velocity in body coordinates represents the rotational angular velocity of the hull relative to the inertial coordinate system in body coordinates. This is IMU observation noise, typically of the type Gaussian white noise.

[0040] Furthermore, the historical unmanned surface vessel control commands are obtained, specifically referring to the control command u(t-1) from the previous moment, which is used for state prediction.

[0041] In another embodiment, the historical unmanned surface vessel (USV) global state estimation includes a set of historical USV self-state estimations, historical environment map estimations, and historical target state estimations. The step of obtaining the current USV global state estimation based on the historical USV control commands, the historical USV global state estimations, and the original observation data includes: The predicted state is obtained based on the historical unmanned surface vessel control commands and the historical unmanned surface vessel's own state estimation. The original observation data is fused with the predicted state to obtain the current state estimate of the unmanned surface vessel. The historical environmental map estimate is updated based on the original observation data to obtain the current environmental map estimate. The historical target state estimation set is updated based on the original observation data to obtain the current target state estimation set; Based on the current unmanned surface vessel's own state estimate, the current environment map estimate, and the current target state estimate set, the current unmanned surface vessel's global state estimate is obtained.

[0042] It should be noted that step S2 can be executed through the data processing module. This module fuses the multi-source raw observation vector set (i.e., raw observation data) Z(t), optimally estimates and updates the historical UAV self-state estimates, historical environment map estimates, and historical target state estimates from the historical global estimation state. It then outputs the updated current UAV self-state estimates, current environment map estimates, and current target state estimates. The data processing module transforms the raw observation data Z(t) into statistically significant situational awareness. This provides a unique and unified situational understanding for all subsequent decisions and control.

[0043] The following details the process of updating and obtaining the current global state estimate of the unmanned surface vessel: Based on the previous output of the historical unmanned surface vessel control command u(t-1) and the previous output of the historical unmanned surface vessel's own state estimate Obtain the predicted state , specifically: ; Where f() is the existing kinematic / dynamic model of the hull.

[0044] Then the original observation data Z(t) is compared with the predicted state. By fusion, the current state estimate of the unmanned surface vessel is obtained. , specifically: ; Among them, in obtaining Then, the aforementioned As K(t) is the Kalman gain matrix, which is the weight matrix and determines the degree of influence of the observation residuals on the state correction, balancing the uncertainty of the state prediction with the uncertainty of the observation noise; The prior state covariance matrix represents the quantification of uncertainty in state prediction. H(t) is the observation Jacobian matrix, which represents the linearization matrix of the observation function at the current state estimation point and indicates the rate of change of the observation value when the state changes slightly; where the superscript T is the transpose matrix.

[0045] N(t) is the observation noise covariance matrix, which characterizes the statistical properties of sensor measurement noise; Z(t) represents the raw observation data, characterizing the actual measurements of all sensors at time t, such as GPS position, radar distance / angle, IMU acceleration, etc. h(·) is the sensor physical model The set of (·) is used to predict the state. and the previous time step historical target state estimation set To calculate the predicted current state estimate of the unmanned surface vessel. .

[0046] In another embodiment, updating the historical environmental map estimate based on the original observation data to obtain the current environmental map estimate includes: Get the grid region; A likelihood ratio is constructed based on the original observation data and the grid region; The historical environment map estimate is updated based on the likelihood ratio to obtain the current environment map estimate.

[0047] It should be noted that the following details the process of updating and obtaining the current environment map estimate: Based on the original observation data and the raster region, a likelihood ratio is constructed, which is then expressed as: ; in, Let be the raster cell in the i-th row and j-th column of the map (i.e., the raster region), and be the map element in the Bayesian function; p() represents the raster cell in the given map element. The probability of observing Z(t) under the given conditions; Assuming that the grid is indeed occupied, the sensor measures the likelihood (probability) of the raw observation data Z(t). It is the likelihood (probability) of the sensor measuring the raw observation data Z(t) when the grid is idle.

[0048] The historical environment map estimate is then updated based on the likelihood ratio to obtain the current environment map estimate: ; Perform dynamic map maintenance: ; Where Z(1:t) represents all sensor observation data from time 1 to the current time t. Given all observation data from time 1 to the current time t Given the given conditions, the probability (conditional probability) that this grid cell is occupied. This represents the probability that the grid is idle, given all observation data from time 1 to the current time t. The sum of the two is 1. The historical environment map estimation itself already contains all the information of Z(1:t-1).

[0049] M(t) is the current environment map estimate at time t; This is the occupancy probability threshold; To construct a notation for a set means to include all grid cells that satisfy the condition... Collected together, they form M(t).

[0050] Furthermore, the historical target state estimation set is updated based on the original observation data to obtain the current target state estimation set, specifically: The historical target state estimation set based on the original observation data Z(t) Update the current target state estimate set. Taking a PHD filter as an example: Historical target state estimation set Mapped to the posterior strength of the previous time step (For example, in the implementation of Gaussian mixture PHD,) Each target state estimate in the model corresponds to a Gaussian component mean. First, the target motion model (such as a uniform linear motion CV model) is used to... Transmitted to the current moment, the predicted intensity is obtained. This process essentially transforms historical states The statistical information was projected forward along the time axis. Subsequently, based on the current raw observation data Z(t), the following PHD update formula was used to update the data. Make corrections to obtain the posterior strength at the current time. : ; in, To predict intensity, For at any time Based on until The observation at time step, the updated PHD intensity function, i.e. the posterior intensity at the current time step; For at any time A state is The probability that the target will be detected; For at any time (here correspond The detected set of observations refers to the subset of target-related observations extracted from the original observation data. Note that in the update formula, the summation is performed on each observation. The observation was performed, where z represents a single target observation.

[0051] Let be the likelihood function for a single objective, representing the likelihood given a state as . Target generation observation The probability density. Here, the sensor physical model is used directly. and its noise characteristics (For example, include Furthermore, the observation noise follows a Gaussian distribution, i.e. ~ N(0, ),So = N(z;h(x), This means that the observation model h(x) is an ideal function with mean , Let z be the probability density value of the Gaussian distribution of the covariance matrix at point z.

[0052] Clutter intensity (the intensity of the Poisson clutter process). Describes the density of clutter (false observations) in the observation space.

[0053] It is an inner product representation, which is actually: ; That is, integrating over the state space to calculate the observations. The total intensity generated by all possible targets. Then for... State extraction is performed to achieve peak separation. Common methods include finding local maxima or the Expectation-Maximization (EM) algorithm, etc. .

[0054] Furthermore, what can be understood is that... The target state set represents the theoretical state variables used in this invention to characterize the real physical properties (position, velocity, type) of all potential targets in the environment. In contrast, It refers to the estimated values ​​used for interactive data in actual operation, module communication, and decision calculation, which are ultimately the basis for control and decision-making.

[0055] Further, for steps S4-S5, "For each current target state estimate in the current target state estimation set, based on the current unmanned surface vessel's own state estimate and the current target state estimate, a threat score for the current target state estimate is obtained; based on all the threat scores corresponding to the target state estimation set, the threat assessment result is obtained," specifically: Threat assessment results are : ; in, This is the current target state estimation set; Estimate the state (position, velocity, type) of the k-th target. These are all derived from the input. .

[0056] T() is the threat function for a single target, and its calculation explicitly depends on . These are preset weighting coefficients. It is understood that the threat score for each estimated current target state is... .

[0057] In another embodiment, obtaining the current unmanned surface vessel control command based on the current unmanned surface vessel global state estimation, the threat assessment result, and the preset top-level mission objective includes: Generate a reference path sequence based on the preset top-level task objective; Based on the threat assessment results, the current state estimate of the unmanned surface vessel, and the current environmental map estimate, the obstacle avoidance cost and the threat avoidance cost are obtained. Based on the reference path sequence, the obstacle avoidance cost, the threat avoidance cost, and the current unmanned surface vessel's own state estimate, the current unmanned surface vessel control command is obtained.

[0058] It should be noted that generating the reference path sequence based on the preset top-level task objective specifically involves: obtaining... The reference path sequence is determined by the preset top-level task objective G.

[0059] Based on the threat assessment results The current unmanned surface vessel's own state estimation and the current environment map estimation Obtaining the cost of obstacle avoidance and the cost of threat avoidance .in, To estimate based on the current state of the unmanned surface vessel The state is estimated at the k-th step in the prediction time domain.

[0060] In another embodiment, the step of obtaining the current unmanned surface vessel (USV) control command based on the reference path sequence, the obstacle avoidance cost, the threat avoidance cost, and the current USV state estimate includes: Based on the reference path sequence, the obstacle avoidance cost, the threat avoidance cost, and the current unmanned surface vessel's own state estimate, an unmanned surface vessel target cost function is constructed; The target cost function of the unmanned surface vessel is solved based on preset constraints to obtain the optimal control sequence of the unmanned surface vessel; The first control variable in the optimal control sequence of the unmanned surface vessel is taken as the current control command of the unmanned surface vessel.

[0061] It should be noted that this step is performed by the adaptive control module. Based on the reference path sequence, the obstacle avoidance cost, the threat avoidance cost, and the current state estimate of the unmanned surface vessel (USV), a target cost function for the USV is constructed as follows: ; Among them, among them, To estimate based on the current state of the unmanned surface vessel The state is estimated at the k-th step in the prediction time domain. To solve the objective cost function; The second-order norm, weighted by the weight matrix Q, is used to quantify the magnitude of the unmanned surface vessel's state error. Let R be the quadratic norm weighted by the weight matrix R, used to quantify the magnitude of the control input of the unmanned surface vessel.

[0062] The target cost function of the unmanned surface vessel is solved based on preset constraints to obtain the optimal control sequence of the unmanned surface vessel. The preset constraints are: ; ; ; in, To be subject to...; This is the lower bound constraint vector for unmanned surface vessel (USV) control commands, defining the minimum allowable values ​​for USV control commands (such as speed and rudder angle). Let be the upper limit constraint vector for unmanned surface vessel control commands. This represents the minimum permissible position of the unmanned surface vessel. To predict the predicted position of the unmanned surface vessel at the k-th step in the time domain, This represents the maximum permissible position of the unmanned surface vessel. For predictive control commands.

[0063] Then, the first control variable in the optimal control sequence of the unmanned surface vessel is used as the current control command of the unmanned surface vessel, specifically: By solving the above objective cost function, the optimal control sequence is obtained. : ; Among them, subscript Corresponding to the current moment ; Corresponding to the next moment ; correspond What I can understand is... The elements in the array are arranged in chronological order.

[0064] A rolling time-domain strategy is adopted, using only the first element of the sequence as the current unmanned surface vessel control command. Because what follows is just a plan and will not be implemented for the time being.

[0065] ; What is understandable is that in the next moment Estimate based on the new state Using the initial conditions, we re-solve the MPC optimization problem to obtain a new optimal sequence of Confucius holes. And execute the first element corresponding to the new current unmanned surface vessel control command. This process continues, forming a closed-loop feedback control.

[0066] In another embodiment, controlling the unmanned surface vessel (USV) to perform corresponding USV navigation actions based on the current USV control command includes: The desired speed and desired rudder angle are obtained based on the current unmanned surface vessel control commands; The unmanned surface vessel (USV) is controlled to perform a corresponding navigation speed based on the desired speed, and the USV is controlled to perform a corresponding navigation deflection based on the desired rudder angle.

[0067] It should be noted that the desired speed and desired rudder angle are extracted from the current unmanned surface vessel (USV) control commands. These commands are typically given in the form of thrust / torque or direct speed and rudder angle commands, which include the desired USV speed and the desired rudder deflection angle.

[0068] The desired speed is converted into a propeller rotation speed command and sent to the unmanned surface vessel's (USV) propulsion motor via a high-precision servo control system. This drives the propeller to rotate, generating corresponding thrust, so that the USV's actual speed approaches the desired speed. Simultaneously, the desired rudder angle is converted into a servo deflection command and sent to the USV's servo motor. This drives the rudder surfaces to deflect by the corresponding angle, so that the USV's actual heading (i.e., travel direction) approaches the direction corresponding to the desired rudder angle.

[0069] Through the coordinated action of the thrusters and servo motors, the unmanned surface vessel (USV) performs navigation maneuvers such as acceleration, deceleration, and turning, thereby achieving tracking and response to the current USV control commands. This process demonstrates the function of the high-precision servo control system in the execution module, translating upper-level control commands into physical actions of the lower-level actuators, enabling the USV to navigate safely and stably according to the planned control commands.

[0070] like Figure 2 As shown, based on the above method embodiments, corresponding apparatus embodiments are provided; An embodiment of the present invention provides an adaptive navigation control device for unmanned surface vessels, comprising: The basic data acquisition module is used to acquire raw observation data, historical unmanned surface vessel control commands, and historical unmanned surface vessel global state estimates. The current unmanned surface vessel global state estimation acquisition module is used to obtain the current unmanned surface vessel global state estimate based on the historical unmanned surface vessel control commands, the historical unmanned surface vessel global state estimates, and the original observation data; The module for obtaining the self-state and target state is used to obtain a set of current unmanned surface vessel self-state estimates and current target state estimates based on the current global state estimate of the unmanned surface vessel. The threat score acquisition module is used to obtain a threat score for each current target state estimate in the current target state estimate set, based on the current unmanned surface vessel's own state estimate and the current target state estimate. The threat assessment result acquisition module is used to obtain the threat assessment result based on all the threat scores corresponding to the target state estimation set; The current unmanned surface vessel (USV) control command acquisition module is used to obtain the current USV control command based on the current USV global state estimation, the threat assessment result, and the preset top-level mission objective. The execution module is used to control the unmanned surface vessel to perform corresponding unmanned surface vessel navigation actions based on the current unmanned surface vessel control command.

[0071] The historical unmanned surface vessel (USV) global state estimation includes a set of historical USV self-state estimations, historical environment map estimations, and historical target state estimations. The current USV global state estimation acquisition module is used to obtain the current USV global state estimation based on the historical USV control commands, the historical USV global state estimations, and the original observation data, including: The predicted state is obtained based on the historical unmanned surface vessel control commands and the historical unmanned surface vessel's own state estimation. The original observation data is fused with the predicted state to obtain the current state estimate of the unmanned surface vessel. The historical environmental map estimate is updated based on the original observation data to obtain the current environmental map estimate. The historical target state estimation set is updated based on the original observation data to obtain the current target state estimation set; Based on the current unmanned surface vessel's own state estimate, the current environment map estimate, and the current target state estimate set, the current unmanned surface vessel's global state estimate is obtained.

[0072] The process of updating the historical environmental map estimate based on the original observation data to obtain the current environmental map estimate includes: Get the grid region; A likelihood ratio is constructed based on the original observation data and the grid region; The historical environment map estimate is updated based on the likelihood ratio to obtain the current environment map estimate.

[0073] The process of obtaining current unmanned surface vessel control commands based on the current global state estimation, the threat assessment results, and the preset top-level mission objective includes: Generate a reference path sequence based on the preset top-level task objective; Based on the threat assessment results, the current state estimate of the unmanned surface vessel, and the current environmental map estimate, the obstacle avoidance cost and the threat avoidance cost are obtained. Based on the reference path sequence, the obstacle avoidance cost, the threat avoidance cost, and the current unmanned surface vessel's own state estimate, the current unmanned surface vessel control command is obtained.

[0074] It is understood that the above-described device embodiments correspond to the method embodiments of the present invention, and can implement the adaptive navigation control method for unmanned surface vessels provided by any of the above-described method embodiments of the present invention.

[0075] It should be noted that the device embodiments described above are merely illustrative, and some or all of the modules can be selected to achieve the purpose of this embodiment according to actual needs. Furthermore, in the accompanying drawings of the device embodiments provided by this invention, the connection relationships between modules indicate that they have communication connections, which can specifically be implemented as one or more communication buses or signal lines. Those skilled in the art can understand and implement this without any creative effort.

[0076] Based on the above-described embodiment of an adaptive navigation control method for unmanned surface vessels, another embodiment of the present invention provides a terminal device, which includes a processor, a memory, and a computer program stored in the memory and configured to be executed by the processor. When the processor executes the computer program, it implements an adaptive navigation control method for unmanned surface vessels according to any embodiment of the present invention.

[0077] For example, in this embodiment, the computer program can be divided into one or more modules, which are stored in the memory and executed by the processor to complete the present invention. The one or more modules may be a series of computer program instruction segments capable of performing a specific function, which describe the execution process of the computer program in the terminal device.

[0078] The terminal device may be a desktop computer, laptop, handheld computer, or cloud server, etc. The terminal device may include, but is not limited to, a processor and a memory.

[0079] The processor can be a Central Processing Unit (CPU), or other general-purpose processors, digital signal processors (DSPs), application-specific integrated circuits (ASICs), field-programmable gate arrays (FPGAs), or other programmable logic devices, discrete gate or transistor logic devices, discrete hardware components, etc. A general-purpose processor can be a microprocessor or any conventional processor. The processor is the control center of the terminal device, connecting all parts of the terminal device via various interfaces and lines.

[0080] Based on the above-described method embodiments, another embodiment of the present invention provides a computer-readable storage medium including a stored computer program, wherein, when the computer program is executed, it controls the device where the computer-readable storage medium is located to execute an unmanned surface vessel adaptive navigation control method as described in any of the above-described method embodiments of the present invention.

[0081] The modules / units integrated in the device / terminal equipment, if implemented as software functional units and sold or used as independent products, can be stored in a computer-readable storage medium. Based on this understanding, all or part of the processes in the above embodiments of the present invention can also be implemented by a computer program instructing related hardware. The computer program can be stored in a computer-readable storage medium, and when executed by a processor, it can implement the steps of the various method embodiments described above. The computer program includes computer program code, which can be in the form of source code, object code, executable files, or certain intermediate forms. The computer-readable medium can include: any entity or device capable of carrying the computer program code, a recording medium, a USB flash drive, a portable hard drive, a magnetic disk, an optical disk, a computer memory, a read-only memory (ROM), a random access memory (RAM), an electrical carrier signal, a telecommunication signal, and a software distribution medium, etc.

[0082] The above description represents the preferred embodiments of the present invention. It should be noted that those skilled in the art can make various improvements and modifications without departing from the principles of the present invention, and these improvements and modifications are also considered to be within the scope of protection of the present invention.

Claims

1. An adaptive navigation control method for unmanned surface vessels, characterized in that, include: Acquire raw observation data, historical unmanned surface vessel (USV) control commands, and historical USV global state estimates; Based on the historical unmanned surface vessel control commands, the historical unmanned surface vessel global state estimates, and the original observation data, the current unmanned surface vessel global state estimate is obtained; Based on the current global state estimation of the unmanned surface vessel, a set of current unmanned surface vessel state estimation and current target state estimation is obtained; For each current target state estimate in the current target state estimate set, a threat score is obtained based on the current unmanned surface vessel's own state estimate and the current target state estimate. The threat assessment result is obtained based on all the threat scores corresponding to the target state estimation set; Based on the current global state estimation of the unmanned surface vessel, the threat assessment results, and the preset top-level mission objective, the current control command for the unmanned surface vessel is obtained; Based on the current unmanned surface vessel (USV) control commands, the USV is controlled to perform corresponding unmanned surface vessel navigation actions.

2. The adaptive navigation control method for unmanned surface vessels according to claim 1, characterized in that, The historical unmanned surface vessel (USV) global state estimation includes a set of historical USV self-state estimations, historical environment map estimations, and historical target state estimations. The process of obtaining the current USV global state estimation based on the historical USV control commands, the historical USV global state estimations, and the original observation data includes: The predicted state is obtained based on the historical unmanned surface vessel control commands and the historical unmanned surface vessel's own state estimation. The original observation data is fused with the predicted state to obtain the current state estimate of the unmanned surface vessel. The historical environmental map estimate is updated based on the original observation data to obtain the current environmental map estimate. The historical target state estimation set is updated based on the original observation data to obtain the current target state estimation set; Based on the current unmanned surface vessel's own state estimate, the current environment map estimate, and the current target state estimate set, the current unmanned surface vessel's global state estimate is obtained.

3. The adaptive navigation control method for unmanned surface vessels according to claim 2, characterized in that, The process of updating the historical environmental map estimate based on the original observation data to obtain the current environmental map estimate includes: Get the grid region; A likelihood ratio is constructed based on the original observation data and the grid region; The historical environment map estimate is updated based on the likelihood ratio to obtain the current environment map estimate.

4. The adaptive navigation control method for unmanned surface vessels according to claim 2, characterized in that, The process of obtaining current unmanned surface vessel control commands based on the current global state estimation, the threat assessment results, and the preset top-level mission objective includes: Generate a reference path sequence based on the preset top-level task objective; Based on the threat assessment results, the current state estimate of the unmanned surface vessel, and the current environmental map estimate, the obstacle avoidance cost and the threat avoidance cost are obtained. Based on the reference path sequence, the obstacle avoidance cost, the threat avoidance cost, and the current unmanned surface vessel's own state estimate, the current unmanned surface vessel control command is obtained.

5. The adaptive navigation control method for unmanned surface vessels according to claim 4, characterized in that, The current unmanned surface vessel (USV) control command is obtained based on the reference path sequence, the obstacle avoidance cost, the threat avoidance cost, and the current USV state estimate, including: Based on the reference path sequence, the obstacle avoidance cost, the threat avoidance cost, and the current unmanned surface vessel's own state estimate, an unmanned surface vessel target cost function is constructed; The target cost function of the unmanned surface vessel is solved based on preset constraints to obtain the optimal control sequence of the unmanned surface vessel; The first control variable in the optimal control sequence of the unmanned surface vessel is taken as the current control command of the unmanned surface vessel.

6. The adaptive navigation control method for unmanned surface vessels according to claim 2, characterized in that, The step of controlling the unmanned surface vessel (USV) to perform corresponding navigation actions based on the current USV control commands includes: The desired speed and desired rudder angle are obtained based on the current unmanned surface vessel control commands; The unmanned surface vessel (USV) is controlled to perform a corresponding navigation speed based on the desired speed, and the USV is controlled to perform a corresponding navigation deflection based on the desired rudder angle.

7. An adaptive navigation control device for unmanned surface vessels, characterized in that, include: The basic data acquisition module is used to acquire raw observation data, historical unmanned surface vessel control commands, and historical unmanned surface vessel global state estimates. The current unmanned surface vessel global state estimation acquisition module is used to obtain the current unmanned surface vessel global state estimate based on the historical unmanned surface vessel control commands, the historical unmanned surface vessel global state estimates, and the original observation data; The module for obtaining the self-state and target state is used to obtain a set of current unmanned surface vessel self-state estimates and current target state estimates based on the current global state estimate of the unmanned surface vessel. The threat score acquisition module is used to obtain a threat score for each current target state estimate in the current target state estimate set, based on the current unmanned surface vessel's own state estimate and the current target state estimate. The threat assessment result acquisition module is used to obtain the threat assessment result based on all the threat scores corresponding to the target state estimation set; The current unmanned surface vessel (USV) control command acquisition module is used to obtain the current USV control command based on the current USV global state estimation, the threat assessment result, and the preset top-level mission objective. The execution module is used to control the unmanned surface vessel to perform corresponding unmanned surface vessel navigation actions based on the current unmanned surface vessel control command.

8. The unmanned surface vessel adaptive navigation control device according to claim 7, characterized in that, The historical unmanned surface vessel (USV) global state estimation includes a set of historical USV self-state estimations, historical environment map estimations, and historical target state estimations. The current USV global state estimation acquisition module is used to obtain the current USV global state estimation based on the historical USV control commands, the historical USV global state estimations, and the original observation data, including: The predicted state is obtained based on the historical unmanned surface vessel control commands and the historical unmanned surface vessel's own state estimation. The original observation data is fused with the predicted state to obtain the current state estimate of the unmanned surface vessel. The historical environmental map estimate is updated based on the original observation data to obtain the current environmental map estimate. The historical target state estimation set is updated based on the original observation data to obtain the current target state estimation set; Based on the current unmanned surface vessel's own state estimate, the current environment map estimate, and the current target state estimate set, the current unmanned surface vessel's global state estimate is obtained.

9. The unmanned surface vessel adaptive navigation control device according to claim 8, characterized in that, The process of updating the historical environmental map estimate based on the original observation data to obtain the current environmental map estimate includes: Get the grid region; A likelihood ratio is constructed based on the original observation data and the grid region; The historical environment map estimate is updated based on the likelihood ratio to obtain the current environment map estimate.

10. An adaptive navigation control device for an unmanned surface vessel according to claim 8, characterized in that, The process of obtaining current unmanned surface vessel control commands based on the current global state estimation, the threat assessment results, and the preset top-level mission objective includes: Generate a reference path sequence based on the preset top-level task objective; Based on the threat assessment results, the current state estimate of the unmanned surface vessel, and the current environmental map estimate, the obstacle avoidance cost and the threat avoidance cost are obtained. Based on the reference path sequence, the obstacle avoidance cost, the threat avoidance cost, and the current unmanned surface vessel's own state estimate, the current unmanned surface vessel control command is obtained.

Images