Tracking methods, devices, media, and products based on variable forward look-ahead distance

Abstract

This invention discloses a trajectory tracking method, device, medium, and product based on variable forward look-ahead distance, comprising: a surface unmanned platform acquiring real-time operational status information through sensors and obtaining ideal position information based on a reference path trajectory; dynamically calculating the current target forward look-ahead distance to obtain a reference heading angle; using a pre-built nonlinear disturbance observer to acquire real-time unknown time-varying disturbance information of the ocean; calculating the target path curvature to obtain the dynamic prediction step size used for trajectory tracking; and using a pre-built model prediction controller to obtain the predicted motion state information of the surface unmanned platform; using a constructed path tracking deviation optimization function to solve for the optimal command rudder angle at the current moment for motion control of the surface unmanned platform, and iteratively updating the optimal command rudder angle during motion. This technical solution can suppress the cumulative effect of uncertainty, possesses strong robustness, adaptability, and fast convergence, and improves trajectory tracking accuracy.

Description

Technical Field

[0001] This invention relates to the field of control technology for unmanned surface platforms, and in particular to a method, device, medium, and product for track tracking based on variable forward sight distance. Background Technology

[0002] As a new generation of intelligent surface platforms, unmanned surface platforms (USPs) have become an important component of marine mission systems due to their advantages such as compact size, low cost, maneuverability, high speed, high level of intelligence, low radar detectability, and zero crew risk. USPs are becoming increasingly indispensable in marine surveying and mapping scenarios such as marine environmental monitoring, seabed topography mapping, and resource exploration, and their application scope and depth are continuously expanding. Track tracking and control is the core technology for achieving precise line-following navigation and high-fidelity mapping operations for USPs; its accuracy directly determines the reliability and resolution of topographic measurement results.

[0003] Traditional unmanned surface platforms employ PID (Proportional-Integral-Derivative) control, which relies on precise mathematical models and adjusts proportional, integral, and derivative parameters to correct errors. While simple in structure and easy to debug, it performs stably in inland rivers and lakes with relatively calm waters. However, in complex sea conditions, strong currents or sudden obstacles can render the fixed-parameter PID controller inadequate, potentially leading to heading oscillations or trajectory deviations. Geometric tracking methods, such as pure line-of-sight navigation or pure tracking algorithms, convert path tracking into heading control, offering simple calculations but poor disturbance rejection. These methods are only suitable for open waters with minimal environmental interference; in complex sea conditions, the cumulative effects of uncertainties from wind, waves, and currents cannot effectively guarantee tracking accuracy. Summary of the Invention

[0004] This invention provides a method, device, medium, and product for track tracking based on variable forward look-ahead distance, which can effectively reduce the cumulative impact of uncertainties and improve the track tracking accuracy of unmanned surface vessels.

[0005] According to one aspect of the present invention, a trajectory tracking method based on variable forward look-ahead distance is provided, executed by a main controller in an unmanned surface platform, the method comprising:

[0006] During the autonomous navigation of the unmanned surface platform according to the reference path trajectory, the real-time operating status information of the unmanned surface platform at the current moment is obtained through at least one sensor carried on the platform, and the ideal position information of the unmanned surface platform at the current moment is obtained from the reference path trajectory.

[0007] Based on the ideal position information, as well as the real-time position and speed of the unmanned surface platform in the real-time operation status information, the target forward-looking distance at the current moment is dynamically calculated, and the reference heading angle of the unmanned surface platform is calculated based on the target forward-looking distance.

[0008] Based on real-time operational status information and a pre-built nonlinear disturbance observer, real-time unknown time-varying disturbance information of the ocean at the current moment is obtained;

[0009] Based on real-time operating status information and reference path trajectory, the curvature trajectory and the target path curvature of the unmanned surface platform under the preset sliding window are calculated. Based on the target path curvature and curvature trajectory, the dynamic prediction step size used for this round of trajectory tracking is calculated.

[0010] Based on real-time operational status information, real-time unknown time-varying disturbance information of the ocean, dynamic prediction step size, reference heading angle, and pre-built model prediction controller, the predicted motion status information of the surface unmanned platform at future moments is obtained by the number of dynamic prediction step sizes.

[0011] The optimal command rudder angle at the current moment is solved by using the path tracking deviation optimization function constructed based on the predicted motion state information and the reference path trajectory, and the motion control of the unmanned surface platform is performed based on the optimal command rudder angle.

[0012] Upon reaching the new current moment, return to execute the operation of acquiring the real-time operating status information of the unmanned surface platform at the current moment through at least one sensor carried on the platform, until the entire trajectory tracking process is completed.

[0013] According to another aspect of the present invention, a trajectory tracking device based on variable forward look-ahead distance is provided, configured in the main controller of an unmanned surface platform, the device comprising:

[0014] The status information acquisition module is used to acquire the real-time operating status information of the unmanned surface platform at the current moment through at least one sensor carried on the platform during the autonomous navigation of the unmanned surface platform according to the reference path trajectory, and to acquire the ideal position information of the unmanned surface platform at the current moment in the reference path trajectory.

[0015] The reference heading angle calculation module is used to dynamically calculate the target forward distance at the current moment based on the ideal position information and the real-time position and speed of the unmanned surface platform in the real-time operation status information, and to calculate the reference heading angle of the unmanned surface platform based on the target forward distance.

[0016] The interference information acquisition module is used to acquire real-time unknown time-varying interference information of the ocean at the current moment based on real-time operating status information and a pre-built nonlinear interference observer.

[0017] The prediction step length calculation module is used to calculate the curvature trajectory and the target path curvature of the unmanned surface platform under a preset sliding window based on real-time operating status information and reference path trajectory, and to calculate the dynamic prediction step length used for this round of trajectory tracking based on the target path curvature and curvature trajectory.

[0018] The motion state prediction module is used to obtain the predicted motion state information of the surface unmanned platform at future moments based on real-time operating status information, real-time unknown time-varying disturbance information of the ocean, dynamic prediction step size, reference heading angle and pre-built model prediction controller.

[0019] The optimal command rudder angle calculation module is used to solve for the optimal command rudder angle at the current moment using the path tracking deviation optimization function constructed based on each predicted motion state information and reference path trajectory, and to perform motion control on the unmanned surface platform based on the optimal command rudder angle.

[0020] The cyclic update module is used to return to the operation of obtaining the real-time operating status information of the unmanned surface platform at the current moment through at least one sensor carried on the platform after reaching a new current moment, until the entire trajectory tracking process is completed.

[0021] According to another aspect of the present invention, an electronic device is provided, the electronic device comprising:

[0022] At least one processor; and

[0023] A memory communicatively connected to the at least one processor; wherein,

[0024] The memory stores a computer program that can be executed by the at least one processor, which enables the at least one processor to perform the track tracking method based on variable forward look-ahead distance according to any embodiment of the present invention.

[0025] According to another aspect of the present invention, a computer-readable storage medium is provided, the computer-readable storage medium storing computer instructions for causing a processor to execute and implement the track tracking method based on variable forward look-ahead distance as described in any embodiment of the present invention.

[0026] According to another aspect of the present invention, a computer program product is also provided, including a computer program that, when executed by a processor, implements the steps of the method as described in any embodiment of the present invention.

[0027] The technical solution of this invention involves an unmanned surface platform (USP) acquiring real-time operational status information through onboard sensors and obtaining its ideal position information at the current moment based on a reference path trajectory. Based on the ideal position information, the USP's real-time position information, and real-time velocity, the forward-looking distance to the target at the current moment is dynamically calculated, and the reference heading angle of the USP is further calculated. Real-time unknown time-varying disturbance information of the ocean is acquired based on the real-time operational status information and a pre-built nonlinear disturbance observer. Based on the real-time operational status information and the reference path trajectory, the curvature trajectory and the target path curvature of the USP within a preset sliding window are calculated, and the dynamic prediction step size used for this round of trajectory tracking is calculated based on the target path curvature and the curvature trajectory. Based on the real-time operational status information, real-time unknown time-varying disturbance information of the ocean, the dynamic prediction step size, the reference heading angle, and a pre-built model prediction controller, the predicted motion state information of the USP at future moments is obtained. Using a path tracking deviation optimization function constructed based on each predicted motion state information and the reference path trajectory, the optimal command rudder angle at the current moment is solved, and the motion control of the USP is performed based on the optimal command rudder angle. Upon reaching the new current moment, the optimal command rudder angle is calculated again according to the above logic to control the motion of the unmanned surface platform until the entire trajectory tracking process is completed. This technical solution can effectively suppress the cumulative effect of uncertainty, and has strong robustness, adaptability and fast convergence, thus improving the trajectory tracking accuracy of the unmanned surface platform.

[0028] It should be understood that the description in this section is not intended to identify key or essential features of the embodiments of the present invention, nor is it intended to limit the scope of the invention. Other features of the invention will become readily apparent from the following description. Attached Figure Description

[0029] To more clearly illustrate the technical solutions in the embodiments of the present invention, the accompanying drawings used in the description of the embodiments will be briefly introduced below. Obviously, the accompanying drawings described below are only some embodiments of the present invention. For those skilled in the art, other drawings can be obtained based on these drawings without creative effort.

[0030] Figure 1 This is a flowchart of a trajectory tracking method based on variable forward look-ahead distance according to Embodiment 1 of the present invention;

[0031] Figure 2 This is a flowchart of another trajectory tracking method based on variable forward look-ahead distance provided according to Embodiment 2 of the present invention;

[0032] Figure 3 This is a schematic diagram of a line-of-sight guidance algorithm for trajectory tracking according to Embodiment 2 of the present invention;

[0033] Figure 4 This is a schematic diagram of a trajectory tracking controller structure applicable to an embodiment of the present invention;

[0034] Figure 5 This is a diagram illustrating the effect of trajectory tracking in a specific scenario applicable to an embodiment of the present invention.

[0035] Figure 6 This is a diagram illustrating the longitudinal deviation curve of trajectory tracking in a specific scenario applicable to an embodiment of the present invention.

[0036] Figure 7 This is a diagram illustrating the lateral deviation curve of trajectory tracking in a specific scenario applicable to an embodiment of the present invention.

[0037] Figure 8 This is a schematic diagram of the structure of a trajectory tracking device based on variable forward look-ahead distance according to Embodiment 3 of the present invention;

[0038] Figure 9 This is a schematic diagram of the structure of an electronic device that implements the trajectory tracking method based on variable forward look distance according to an embodiment of the present invention. Detailed Implementation

[0039] To enable those skilled in the art to better understand the present invention, the technical solutions of the present invention 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 the present invention, and not all embodiments. Based on the embodiments of the present invention, all other embodiments obtained by those skilled in the art without creative effort should fall within the scope of protection of the present invention.

[0040] It should be noted that the terms "first," "second," etc., in the specification, claims, and accompanying drawings of this invention are used to distinguish similar objects and are not necessarily used to describe a specific order or sequence. It should be understood that such data can be interchanged where appropriate so that the embodiments of the invention described herein can be implemented in orders other than those illustrated or described herein. Furthermore, the terms "comprising" and "having," and any variations thereof, are intended to cover non-exclusive inclusion; for example, a process, method, system, product, or apparatus that comprises a series of steps or units is not necessarily limited to those steps or units explicitly listed, but may include other steps or units not explicitly listed or inherent to such processes, methods, products, or apparatus.

[0041] Example 1

[0042] Figure 1This is a flowchart of a trajectory tracking method based on variable forward look-ahead distance provided in Embodiment 1 of the present invention. This embodiment is applicable to situations where an unmanned surface platform autonomously tracks its navigation trajectory according to a reference path. This method can be executed by a trajectory tracking device with variable forward look-ahead distance, which can be implemented in hardware and / or software and is generally configured in the main controller of the unmanned surface platform. Figure 1 As shown, the method includes:

[0043] S110. During the autonomous navigation of the unmanned surface platform according to the reference path trajectory, the platform acquires the real-time operating status information of the unmanned surface platform at the current moment through at least one sensor carried on the platform, and acquires the ideal position information of the unmanned surface platform at the current moment in the reference path trajectory.

[0044] The reference path trajectory refers to the desired navigation trajectory pre-set before the unmanned surface platform (USP) performs its autonomous navigation mission. It consists of a series of continuous geographic coordinates and is used to identify the position coordinates that the USP tracks when navigating along the desired path. Ideal position information refers to the position coordinates within the reference path trajectory that the USP should currently track when its actual navigation trajectory deviates from the reference path trajectory during autonomous navigation.

[0045] In this embodiment, the sensor equipment carried by the unmanned surface platform may include the BeiDou-2 navigation system, integrated navigation equipment, and AIS (Automatic Identification System). The unmanned surface platform can acquire real-time information on its current operating status, including its position coordinates in the world coordinate system, heading angle, and speed. Based on the acquired motion status information and reference trajectory, the unmanned surface platform can determine the ideal position it should follow.

[0046] S120. Based on the ideal position information and the real-time position and speed of the unmanned surface platform in the real-time operation status information, dynamically calculate the target forward-looking distance at the current moment, and calculate the reference heading angle of the unmanned surface platform based on the target forward-looking distance.

[0047] In this context, the target forward-looking distance refers to the distance between a forward reference point selected for tracking a reference path trajectory and the current position of the unmanned surface platform (USP). This is crucial data in path tracking algorithms. The reference heading angle refers to the heading angle that the USP should adjust to when navigating from its current position to its ideal position on the reference path trajectory.

[0048] Understandably, there will be some discrepancy between the actual trajectory of the unmanned surface platform (USP) and the reference path during navigation. The platform needs to adjust its course based on the reference heading angle to reach the ideal position. The reference heading angle can be calculated from the forward-looking distance of the target. During its movement, the USP dynamically calculates the forward-looking distance of the target at the current moment based on the real-time position and speed information from its real-time operational status data, combined with the ideal position information, and updates the calculated reference heading angle in real time.

[0049] S130. Based on real-time operating status information and a pre-built nonlinear disturbance observer, obtain real-time unknown time-varying disturbance information of the ocean at the current moment.

[0050] Among them, unknown time-varying interference information can refer to information that dynamically changes over time during the navigation of unmanned surface platforms and has an interfering effect. This information can be interference information generated by wind and waves in the navigation environment, which is difficult to measure accurately through sensors. It can manifest as additional forces and torques that directly affect the tracking accuracy of unmanned surface platforms.

[0051] In this embodiment, based on the pre-constructed nonlinear disturbance observer and combined with the real-time acquired current motion state information, the time-varying additional forces and torques generated in the navigation environment of the unmanned surface platform are calculated in real time to obtain the real-time unknown time-varying disturbance information of the ocean at the current moment. This information can be used as a basis for dynamic compensation of the motion control of the unmanned surface platform.

[0052] Optionally, the real-time lateral velocity v of the unmanned surface platform can be extracted from the real-time operating status information and calculated according to the formula. Calculate the nonlinear function in the nonlinear disturbance observer. ,in, This is a preset positive definite gain matrix;

[0053] According to the formula: Calculate the real-time unknown time-varying disturbance information of the ocean at the current moment. Where z represents the internal state of the nonlinear disturbance observer at the current moment. ,in This is an estimate of the roll disturbance of the unmanned surface platform. This is an estimate of the pitch disturbance of the unmanned surface platform. The estimated value of heave disturbance for unmanned surface platforms;

[0054] According to the formula: The internal state of the nonlinear disturbance observer is updated to obtain the observer's state at the next time step. For use in the next round of track tracking;

[0055] in, Let M be the observer gain matrix, and M be a preset inertia matrix, which is a positive definite pairwise matrix. For Coriolis and, Here is the damping matrix. To control the input force.

[0056] S140. Based on real-time operating status information and reference path trajectory, calculate the curvature trajectory and the target path curvature of the unmanned surface platform under the preset sliding window, and calculate the dynamic prediction step size used for this round of trajectory tracking based on the target path curvature and curvature trajectory.

[0057] Here, curvature trajectory refers to the navigation trajectory of an unmanned surface platform described by a path curvature parameter. Target path curvature refers to the smoothed curvature value of the reference path trajectory. The original curvature data of the reference path trajectory may contain noise, which can be filtered using a preset sliding window to obtain stable and reliable curvature characteristics. Dynamic prediction step size refers to the prediction time-domain parameter that is adaptively adjusted according to the path curvature. For example, a longer prediction step size can be used on high-curvature path segments (such as sharp curves) to improve control foresight, while a shorter step size can be used on low-curvature path segments (such as straight lines) to reduce computational load.

[0058] In this embodiment, the instantaneous curvature can be calculated using differential geometry methods based on the rate of change of the tangential angle between the real-time motion state and the reference path. The target path curvature is then obtained by weighted averaging the instantaneous curvature using a preset sliding window function. The window length is adaptively adjusted according to the speed of the unmanned surface platform; that is, the window is shortened at high speeds to improve response speed, and lengthened at low speeds to enhance smoothing. The dynamic prediction step size adjustment algorithm uses the radius of curvature as an input variable and generates a prediction step size mapping table using a piecewise linear interpolation method. This ensures that shorter step sizes are used on straight sections to save computational resources, while longer step sizes are used on curved sections to maintain tracking accuracy.

[0059] Optionally, based on real-time operating status information and reference path trajectory, the curvature trajectory and the target path curvature of the unmanned surface platform within a preset sliding window can be calculated, which may include:

[0060] Extract real-time location information and real-time speed from the real-time operation status information, and determine the sub-path to be navigated based on the real-time location information and the reference path trajectory;

[0061] Determine the curvature trajectory based on the sub-path to be navigated. Based on real-time speed information and the sub-path to be navigated, the curvature-time function is dynamically calculated. ;

[0062] According to the formula: The calculation of the unmanned surface platform within the preset sliding window Target path curvature ,in, The preset control cycle is denoted by , and i is the index variable of the summation loop.

[0063] Optional, according to the formula: The dynamic prediction step size used for calculating wheel trajectory tracking .

[0064] in, For the target path curvature The absolute value, and This is an adjustment factor used to balance the path tracking accuracy and control stability of unmanned surface platforms. The preset maximum step size threshold, The preset minimum step size threshold, For curvature trajectory The maximum absolute value among all trajectory points.

[0065] S150. Based on real-time operating status information, real-time unknown time-varying disturbance information of the ocean, dynamic prediction step size, reference heading angle, and pre-built model prediction controller, the predicted motion status information of the surface unmanned platform at future moments is obtained by dynamically predicting step size number of steps.

[0066] Among them, the model predictive controller can refer to a control algorithm based on a dynamic predictive model for rolling optimization. That is, by using the current system state and the predictive model, it can predict the state information of the system in the future for a period of time. It can comprehensively consider the constraints, generate the optimal control sequence by solving the optimization problem online, and implement the control command at the current moment.

[0067] Specifically, the real-time motion state information of the unmanned surface platform, the time-varying disturbance estimate output by the disturbance observer, and the dynamic prediction step size can be input into a pre-established model prediction controller. By solving the discretized prediction model that includes the dynamics of the unmanned surface platform, the servo response, and environmental disturbance terms, the predicted state sequence of the unmanned surface platform's position, velocity, and heading angle at multiple future moments can be obtained.

[0068] S160. Using the path tracking deviation optimization function constructed based on each predicted motion state information and reference path trajectory, solve for the optimal command rudder angle at the current moment, and perform motion control on the unmanned surface platform based on the optimal command rudder angle.

[0069] The path tracking deviation optimization function can be a mathematical function describing the error relationship between the actual trajectory of the unmanned surface platform and the reference trajectory. This optimization function comprehensively considers multiple dimensions of error, such as lateral position deviation, heading angle deviation, and velocity deviation, between the unmanned surface platform's trajectory and the reference trajectory, constructing a calculation rule to minimize the error. The optimal command rudder angle can be the optimal control output calculated by the path tracking deviation optimization function, used to control the unmanned surface platform to move to the ideal position.

[0070] In this embodiment, based on the motion state sequence of the unmanned surface platform obtained by the pre-built model predictive controller and the deviation of the reference path, a path tracking optimization function can be constructed. An improved artificial bee colony algorithm can be used to solve this function to obtain the optimal command rudder angle at which the tracking deviation of the unmanned surface platform converges at the current moment. The optimal command rudder angle is then output to the servo actuator to control the motion of the unmanned surface platform.

[0071] S170. After reaching the new current moment, return to execute the operation of obtaining the real-time operating status information of the unmanned surface platform at the current moment through at least one sensor carried on the platform, until the entire trajectory tracking process is completed.

[0072] In this embodiment, the tracking of the reference path trajectory by the unmanned surface platform is a dynamic iterative process. After the optimal command rudder angle is calculated to control the movement of the unmanned surface platform, the latest real-time motion state data collected by the sensors is re-executed, and the above-mentioned processes of calculating the reference heading angle, acquiring real-time interference, calculating the dynamic prediction step size, state prediction and optimization control are executed in sequence to achieve the goal of continuously converging the actual motion trajectory of the unmanned surface platform to the reference path trajectory.

[0073] The technical solution of this invention involves an unmanned surface platform (USP) acquiring real-time operational status information through onboard sensors and obtaining its ideal position information at the current moment based on a reference path trajectory. Based on the ideal position information, the USP's real-time position information, and real-time velocity, the forward-looking distance to the target at the current moment is dynamically calculated, and the reference heading angle of the USP is further calculated. Real-time unknown time-varying disturbance information of the ocean is acquired based on the real-time operational status information and a pre-built nonlinear disturbance observer. Based on the real-time operational status information and the reference path trajectory, the curvature trajectory and the target path curvature of the USP within a preset sliding window are calculated, and the dynamic prediction step size used for this round of trajectory tracking is calculated based on the target path curvature and the curvature trajectory. Based on the real-time operational status information, real-time unknown time-varying disturbance information of the ocean, the dynamic prediction step size, the reference heading angle, and a pre-built model prediction controller, the predicted motion state information of the USP at future moments is obtained. Using a path tracking deviation optimization function constructed based on each predicted motion state information and the reference path trajectory, the optimal command rudder angle at the current moment is solved, and the motion control of the USP is performed based on the optimal command rudder angle. Upon reaching the new current moment, the optimal command rudder angle is calculated again according to the above logic to control the motion of the unmanned surface platform until the entire trajectory tracking process is completed. This technical solution can effectively suppress the cumulative effect of uncertainty, and has strong robustness, adaptability and fast convergence, thus improving the trajectory tracking accuracy of the unmanned surface platform.

[0074] Example 2

[0075] Figure 2 This is a flowchart of another trajectory tracking method based on variable forward look-ahead distance provided in Embodiment 2 of the present invention. This embodiment is based on and optimized from the above embodiments. Specifically, the operation of "calculating the reference heading angle of the unmanned surface platform according to the target forward look-ahead distance, and solving for the optimal command rudder angle at the current moment using the path tracking deviation optimization function constructed based on each predicted motion state information and the reference path trajectory" has been refined. Figure 2 As shown, the method includes:

[0076] S210. During the autonomous navigation of the unmanned surface platform according to the reference path trajectory, the platform acquires the real-time operating status information of the unmanned surface platform at the current moment through at least one sensor carried on the platform, and acquires the ideal position information of the unmanned surface platform at the current moment in the reference path trajectory.

[0077] S220. Based on the ideal position information and the real-time position and speed of the unmanned surface platform in the real-time operation status information, dynamically calculate the target forward-looking distance at the current moment, and calculate the reference heading angle of the unmanned surface platform based on the target forward-looking distance.

[0078] Optionally, the longitudinal error of the unmanned surface platform can be calculated based on two X-axis coordinate values ​​from the real-time location information and the ideal location information. Based on the two Y-axis coordinate values ​​from the real-time location information and the ideal location information, the lateral error of the unmanned surface platform is calculated. .

[0079] According to the formula: Calculate the target's forward-looking distance at the current time t. .

[0080] in, U represents the preset minimum forward-looking distance, and U is the real-time speed of the unmanned surface platform. , These are preset adaptive parameters;

[0081] According to the formula: Calculate the reference heading angle of the unmanned surface platform. ;

[0082] in, atan2(.) is the arctangent function in the fourth quadrant. The Y-axis coordinate value in the ideal position information at the current time t. The derivative of The X-axis coordinate value in the ideal position information at the current time t. The derivative of The sideslip angle of the unmanned surface platform.

[0083] In this embodiment, Figure 3 This is a schematic diagram of the trajectory tracking principle of the line-of-sight guidance algorithm. For real-time position information located at ( ( ) unmanned surface platform, longitudinal error and lateral error It can be represented as follows:

[0084]

[0085] Using the sliding membrane algorithm, the virtual input can be designed as follows:

[0086]

[0087] Among them, in the aforementioned virtual input In the design formula, This is a virtual input. To measure the current heading angle of an unmanned surface vessel (USV) at sea, sign() is the sign function. , Both are sliding membrane control gain.

[0088] The trajectory tracking principle diagram of the adaptive robust line-of-sight guidance algorithm in this embodiment is shown in the following formula:

[0089]

[0090] in, This refers to the longitudinal direction error of the unmanned surface vessel in the direction error. This refers to the lateral direction error of the unmanned surface vessel (USV) within the direction error. The derivative of the tangent angle of the path. For kinematic uncertainties, the parameter is denoted as .

[0091] To analyze the stability of line-of-sight guidance algorithms, a Lyapunov function can be defined:

[0092]

[0093] Differentiating the Lyapunov function and substituting the above formula into the resulting equation, we get:

[0094]

[0095] Among them, because The value is in the range [-1, 1], and when When smaller, It can be approximated as A linear function. Therefore, for Scaling:

[0096]

[0097] Using inequalities (in Further compression:

[0098]

[0099] Choose the appropriate Make Then the above formula can be further written as:

[0100]

[0101]

[0102]

[0103] in, , and yes The complement. In In addition, due to There is a lower realm, therefore The system is stable. Inside, The system status will eventually be received. Within a subset, and within that subset Therefore, the system is asymptotically stable.

[0104] S230. Based on real-time operational status information and a pre-built nonlinear disturbance observer, obtain real-time unknown time-varying disturbance information of the ocean at the current moment.

[0105] S240. Based on real-time operating status information and reference path trajectory, calculate the curvature trajectory and the target path curvature of the unmanned surface platform under the preset sliding window, and calculate the dynamic prediction step size used for this round of trajectory tracking based on the target path curvature and curvature trajectory.

[0106] S250: Based on real-time operating status information, real-time unknown time-varying disturbance information of the ocean, dynamic prediction step size, reference heading angle, and pre-built model prediction controller, the predicted motion status information of the surface unmanned platform at future moments is obtained by dynamically predicting step size number of steps.

[0107] In one optional implementation of this embodiment, the schematic diagram of the trajectory tracking controller is as follows: Figure 4 As shown, the pre-built model prediction controller can be:

[0108]

[0109] In the relevant formulas involved in the model predictive controller, in the motion coordinates of the unmanned surface platform, the x-axis is defined to point due north, and the angle between the bow and stern centerlines and the x-axis is the bow angle. express. and These represent the flow direction and velocity, respectively. For the speed of water movement, For the lateral velocity of water, for the combined velocity of water The components of the surface unmanned platform's velocity along the x-axis and y-axis in the oxyz coordinate system are respectively... and , For ground-based forward speed, Let r be the lateral velocity relative to the ground, r be the angular velocity of the bow rotating about the z-axis, and r be the resultant velocity relative to the ground. drift angle , For rudder angle. To command the rudder angle, For servo control gain, Let m be the servo motor time constant, and m be the mass of the unmanned surface platform. and For added mass, , and For viscous hydrodynamics acting on the hull, , and For propeller force, , and For wind power, , and For the force of the waves, Let be the moment of inertia of the unmanned surface platform about its vertical axis. To add a moment of inertia, , and As rudder force, It is the reduction in rudder drag. It is the ratio of the additional lateral force on the hull caused by steering to the lateral force of the rudder. It is the distance from the center of the lateral force acting on the steering-guided hull to the center of gravity of the unmanned surface platform. This is the positive rudder pressure. The symbol with a dot at the top represents the model predictor controller's predicted value.

[0110] In the trajectory tracking and control process, real-time operational status information, real-time unknown time-varying disturbance information of the ocean, and dynamic prediction step size are input as input parameters to a pre-built model predictive controller. The specific implementation method for predicting the motion state of the unmanned surface platform at future moments is as follows:

[0111]

[0112] in, , , , , , , For the future The dynamic status of the unmanned surface platform at any given time. , , , , , , for The movement status of the unmanned platform on the water surface at any given time. It refers to the prediction sampling time, which is the time interval between two consecutive predicted values. , , , , , and These are the discretizations of the differential equations in the model predictive controller.

[0113] Combination , , , , , , , , , , , , , , as well as The values ​​of parameters at time k. , , , , , and The calculation method is as follows:

[0114]

[0115] in, , , This is an estimate of the unknown time-varying disturbance at time k, which is estimated and compensated using the nonlinear disturbance observer proposed in this embodiment of the invention. Track tracking is performed considering only lateral displacement, from the current time k to the future... At time points (k+1, k+2, ..., k+...), the time intervals between each k+1, k+2, ..., k+... The output of ) is used for prediction.

[0116]

[0117] S260. Construct a path tracking deviation optimization function based on the predicted motion state information and the reference path trajectory.

[0118] In this embodiment, a path tracking deviation optimization function is constructed based on the multi-step predicted motion state sequence output by the model predictive controller and combined with the expected value of the reference path trajectory. This function adopts a weighted quadratic form, focusing on evaluating multi-dimensional errors such as lateral position deviation, heading angle deviation, and velocity deviation. Lateral displacement deviation is given a high weight to ensure path tracking accuracy, heading angle deviation is given a medium weight to maintain motion stability, and velocity deviation is given a low weight to avoid over-adjustment. The optimization function also introduces a penalty term for the rate of change of control variables to prevent drastic changes in rudder angle commands and ensure smooth control.

[0119] In an optional implementation of this embodiment, based on the prediction results and the reference lateral displacement... Calculate path prediction error :

[0120]

[0121] in, =1, 2, ..., Based on the prediction error, construct an optimization function:

[0122]

[0123]

[0124] Here, Q is the weight matrix. This is achieved by solving the quadratic form of QP or under constraints. Solve the equation below The optimal control rudder angle can be calculated. :

[0125]

[0126] in, This represents the output of the entire optimization problem. In this embodiment, it represents the optimal control rudder angle, and argmin represents finding a control variable that minimizes the objective function. This is represented as the minimum control rudder angle. This represents the maximum control rudder angle. It is represented as the objective function that needs to be minimized. Figure 5 This is a diagram illustrating the trajectory tracking path effect in a specific scenario applicable to an embodiment of the present invention. Figure 6 This is a graph showing the longitudinal deviation curve of the trajectory tracking in the corresponding scenario. Figure 7 The effect diagram of the lateral deviation curve of the trajectory tracking in the corresponding scenario.

[0127] S270. Under the constraints of state variables and control inputs, the optimal solution of the path tracking deviation optimization function is obtained by using the improved artificial bee colony algorithm to obtain the optimal command rudder angle at the current moment.

[0128] The initial population used in the improved artificial bee colony algorithm is obtained by searching within a search space defined by a preset upper bound and a preset lower bound, based on a preset chaotic variable.

[0129] Artificial bee colony optimization (AQC) is a swarm intelligence optimization algorithm that simulates the foraging behavior of bee colonies. It solves complex optimization problems by mimicking the division of labor, cooperation, and information sharing mechanisms bees use in searching for nectar sources. Its core idea is to map the solution space of the optimization problem to the locations of nectar sources. Through the collaborative search of three roles—hired bees (developing known nectar sources), follower bees (selecting high-quality nectar sources), and scout bees (exploring new nectar sources)—the algorithm gradually approaches the global optimum. The initial population refers to a set of initial feasible solutions constructed in a specific way before the AQC begins iterative optimization. Chaotic variables refer to variables generated using chaotic systems in chaotic algorithms, exhibiting sensitivity to initial conditions, ergodicity, and pseudo-randomness. Their core function is to provide the algorithm with efficient global search capabilities, helping it escape local optima. The search space refers to the space consisting of the set of all possible feasible solutions to the problem to be optimized. The algorithm searches within this space using the bee colony to find the optimal solution.

[0130] It is understandable that model predictive control algorithms are prone to local optima. The multi-strategy cooperative adaptive bee colony optimization algorithm provided in this embodiment can effectively solve this problem by introducing chaotic initialization, dynamic neighborhood search and elite retention strategies.

[0131] Chaotic initialization generates an initial population through chaotic mapping, calculates the fitness value of each honey source, and identifies the current optimal solution. Chaotic sequences have better traversal properties and can more effectively cover the search space compared to random initialization. The formulas for generating and mapping chaotic variables are shown below:

[0132]

[0133] Let p be a chaotic variable of dimension p, with initial values ​​taken as non-periodic points between (0,1). As the lower bound of the search space, The upper bound of the search space, The set of feasible solutions for the generated initial population. Let be the chaotic variable of the (p+1)th dimension.

[0134] To achieve extensive initial exploration followed by refined development, the algorithm updates positions according to the following dynamic weight formula during the bee-hiring phase, applying dynamic neighborhood search for local development.

[0135]

[0136] Let be the search radius at time t. The attenuation coefficient is... Let N(0,1) be the maximum number of iterations, and N(0,1) be a standard normally distributed random number. In each iteration of the artificial bee colony algorithm, the worst honey source is identified as the location. The source location for the new honey has also been set. The global optimal new position can be determined through iteration. Vie , The optimal position obtained at time t-1. This is the optimal position obtained by searching at time t in the current iteration.

[0137] This embodiment employs the following dynamic adaptive location update strategy, which guides inferior solutions to migrate to elite regions while maintaining population diversity:

[0138]

[0139] in, This refers to the old position in the p-dimensional dimension from the previous iteration. This refers to the new position of the current iteration in the p-dimensional dimension. This represents the optimal position obtained at time t in the current iteration. The dynamic weighting coefficients are uniformly distributed random numbers within the range [0,1]. The expression is as follows:

[0140]

[0141] in, The curvature coefficient, and These are two coefficients that can be optimally set through algorithm simulation.

[0142] Adaptive scaling factor for:

[0143]

[0144] in, The minimum scaling factor. The maximum scaling factor is t, where t is the current iteration number. This represents the maximum number of iterations.

[0145] When the artificial bee colony solves a constrained quadratic programming problem for the first time, it selects the globally optimal initial solution from the randomly generated solutions. When this is not the first time, it selects the globally optimal initial solution. The last control input sequence The remaining control input components are used as the globally optimal initial values ​​for the artificial bee colony algorithm, i.e. This will greatly improve the solution efficiency of the artificial bee colony algorithm.

[0146] Based on the current optimal solution To improve the location update using perturbation information, this embodiment employs the following location update algorithm:

[0147]

[0148] When a high-quality honey source is found in a new location, replace the original honey source in the artificial bee colony algorithm.

[0149] in, Let p be the position of the individual bee in the hive that needs to be updated in the p-th dimension. The position of the best individual in the current population on the p-th dimension. The remaining coefficients can be adaptively adjusted according to the following formula: (This is a Gaussian perturbation term.) , .

[0150] The elite preservation strategy used in this embodiment can accelerate convergence while maintaining population diversity through an adaptive scaling factor F, as shown below:

[0151]

[0152] in, and For two randomly selected distinct solutions... This is the current optimal solution. This is the elite solution. F is an adaptive scaling factor (non-linearly decreasing). The optimal elite solution is determined... Then, the principle of model predictive control can be applied for optimization.

[0153] In this embodiment, the constrained model prediction closed-loop control is finally adopted. The optimal solution that satisfies the constraints is solved using the following formula in each iteration cycle.

[0154]

[0155] in, For constraint parameters, and These represent the minimum and maximum values ​​of the constraint parameters, respectively. Cost function. The expression J is shown below:

[0156]

[0157] in, This is the weight matrix. for Timing deviation, The prediction step size for the output error. To control the incremental optimization step size, typically Not greater than , for The control increment vector at each time step, Let be the square of the 2-norm of the data over the real number field R.

[0158] The designed Lyapunov function is shown below:

[0159]

[0160] in, It is the system's error state vector (representing the deviation between the actual and desired states of the system). It is the error state vector The transpose of converts a column vector into a row vector, which is used for quadratic form operations with matrix P, where P is a symmetric positive definite matrix. It is a quadratic function that satisfies the positive definiteness requirement of the Lyapunov function. The difference equation is as follows:

[0161]

[0162] in, It is a Lyapunov function The difference, that is, the change in V2 between time k+1 and time k. It is the Lyapunov function value at time k. It is a negative definite quadratic form (Q is a symmetric positive definite matrix).

[0163] When Q>0 and P>0, it can be ensured that the derivative of the Lyapunov function is negative definite. That is, in each iteration of the model predictive control, the error asymptotically converges to zero.

[0164] This allows us to define the Lyapunov function for the entire control system:

[0165]

[0166] according to and Then we can get:

[0167]

[0168] Therefore, it can be concluded that the trajectory tracking control framework is globally asymptotically stable.

[0169] S280. After reaching the new current moment, return to execute the operation of obtaining the real-time operating status information of the unmanned surface platform at the current moment through at least one sensor carried on the platform, until the entire trajectory tracking process is completed.

[0170] In the technical solution of this invention embodiment, the unmanned surface platform is an unmanned surface vessel, and the trajectory tracking method based on variable forward sight distance is applied in an automatic measurement and mapping scenario based on island and reef sea areas.

[0171] The technical solution of this invention involves an unmanned surface platform acquiring real-time operational status information through onboard sensors and obtaining its ideal position information at the current moment based on a reference path trajectory. Based on the ideal position information, the real-time position information, and the real-time velocity of the unmanned surface platform, the forward-looking distance to the target at the current moment is dynamically calculated, and the reference heading angle of the unmanned surface platform is further calculated. Real-time unknown time-varying disturbance information of the ocean is acquired based on the real-time operational status information and a pre-built nonlinear disturbance observer. Based on the real-time operational status information and the reference path trajectory, the curvature trajectory and the target path curvature of the unmanned surface platform under a preset sliding window are calculated, and the dynamic prediction step size used for this round of trajectory tracking is calculated based on the target path curvature and the curvature trajectory. The predicted future motion state information of the unmanned surface platform is obtained based on a pre-built model predictive controller and various input parameters. Under constraints of state variables and control inputs, the optimal solution for the path tracking deviation optimization function is obtained using an improved artificial bee colony algorithm, yielding the optimal command rudder angle at the current moment. Upon reaching the new current moment, the optimal command rudder angle is calculated again according to the above logic to control the motion of the unmanned surface platform until the entire trajectory tracking process is completed. This technical solution significantly improves the algorithm's global search capability and convergence speed by introducing chaotic initialization, dynamic neighborhood search, and elite preservation strategies into the artificial bee colony algorithm.

[0172] Example 3

[0173] Figure 8 This is a schematic diagram of a trajectory tracking device based on variable forward look-ahead distance, provided in Embodiment 3 of the present invention. Figure 8 As shown, the device includes: a status information acquisition module 310, a reference heading angle calculation module 320, an interference information acquisition module 330, a prediction step length calculation module 340, a motion state prediction module 350, an optimal command rudder angle calculation module 360, and a cyclic update module 370.

[0174] The status information acquisition module 310 is used to acquire the real-time operating status information of the unmanned surface platform at the current moment through at least one sensor carried on the platform during the autonomous navigation of the unmanned surface platform according to the reference path trajectory, and to acquire the ideal position information of the unmanned surface platform at the current moment in the reference path trajectory.

[0175] The reference heading angle calculation module 320 is used to dynamically calculate the target forward distance at the current moment based on the ideal position information and the real-time position and speed of the unmanned surface platform in the real-time operation status information, and to calculate the reference heading angle of the unmanned surface platform based on the target forward distance.

[0176] The interference information acquisition module 330 is used to acquire real-time unknown time-varying interference information of the ocean at the current moment based on real-time operating status information and a pre-built nonlinear interference observer.

[0177] The prediction step length calculation module 340 is used to calculate the curvature trajectory and the target path curvature of the unmanned surface platform under a preset sliding window based on real-time operating status information and reference path trajectory, and to calculate the dynamic prediction step length used for this round of trajectory tracking based on the target path curvature and curvature trajectory.

[0178] The motion state prediction module 350 is used to obtain the predicted motion state information of the surface unmanned platform at future times based on real-time operating status information, real-time unknown time-varying disturbance information of the ocean, dynamic prediction step size, reference heading angle and pre-built model prediction controller.

[0179] The optimal command rudder angle calculation module 360 ​​is used to solve for the optimal command rudder angle at the current moment using the path tracking deviation optimization function constructed based on the predicted motion state information and the reference path trajectory, and to perform motion control on the unmanned surface platform based on the optimal command rudder angle.

[0180] The cyclic update module 370 is used to return to the operation of obtaining the real-time operating status information of the unmanned surface platform at the current moment through at least one sensor carried on the platform after reaching a new current moment, until the entire trajectory tracking process is completed.

[0181] The technical solution of this invention involves an unmanned surface platform acquiring real-time operational status information through onboard sensors and obtaining its ideal position information at the current moment based on a reference path trajectory. Based on the ideal position information, the real-time position information, and the real-time velocity of the unmanned surface platform, the forward-looking distance to the target at the current moment is dynamically calculated, and the reference heading angle of the unmanned surface platform is further calculated. Real-time unknown time-varying disturbance information of the ocean is acquired based on the real-time operational status information and a pre-built nonlinear disturbance observer. Based on the real-time operational status information and the reference path trajectory, the curvature trajectory and the target path curvature of the unmanned surface platform within a preset sliding window are calculated, and the dynamic prediction step size used for this round of trajectory tracking is calculated based on the target path curvature and the curvature trajectory. Based on the real-time operational status information, the real-time unknown time-varying disturbance information of the ocean, the dynamic prediction step size, the reference heading angle, and the pre-built model prediction controller, the predicted motion state information of the unmanned surface platform at future moments is obtained. Using a path tracking deviation optimization function constructed based on each predicted motion state information and the reference path trajectory, the optimal command rudder angle at the current moment is solved, and the motion control of the unmanned surface platform is performed based on the optimal command rudder angle. Upon reaching the new current moment, the optimal command rudder angle is calculated again according to the above logic to control the motion of the unmanned surface platform until the entire trajectory tracking process is completed. This technical solution can effectively suppress the cumulative effect of uncertainty, and has strong robustness, adaptability and fast convergence, thus improving the trajectory tracking accuracy of the unmanned surface platform.

[0182] Optionally, referring to the heading angle calculation module 320, it can be specifically used to: calculate the longitudinal error of the unmanned surface platform based on two X-axis coordinate values ​​from the real-time position information and the ideal position information. Based on the two Y-axis coordinate values ​​from the real-time location information and the ideal location information, the lateral error of the unmanned surface platform is calculated. .

[0183] According to the formula: Calculate the target's forward-looking distance at the current time t. .

[0184] in, U represents the preset minimum forward-looking distance, and U is the real-time speed of the unmanned surface platform. , These are preset adaptive parameters.

[0185] According to the formula: Calculate the reference heading angle of the unmanned surface platform. .

[0186] in, atan2(.) is the arctangent function in the fourth quadrant. The Y-axis coordinate value in the ideal position information at the current time t. The derivative of The X-axis coordinate value in the ideal position information at the current time t. The derivative of The sideslip angle of the unmanned surface platform.

[0187] Optionally, the interference information acquisition module 330 can be specifically used to: extract the real-time lateral velocity v of the unmanned surface platform from the real-time operating status information, and calculate it according to the formula... Calculate the nonlinear function in the nonlinear disturbance observer. ,in, This is a preset positive definite gain matrix.

[0188] According to the formula: Calculate the real-time unknown time-varying disturbance information of the ocean at the current moment. Where z represents the internal state of the nonlinear disturbance observer at the current moment. ,in This is an estimate of the roll disturbance of the unmanned surface platform. This is an estimate of the pitch disturbance of the unmanned surface platform. This is an estimate of the heave disturbance of the unmanned surface platform.

[0189] According to the formula: The internal state of the nonlinear disturbance observer is updated to obtain the observer's state at the next time step. This will be used for the next round of track tracking.

[0190] in, Let M be the observer gain matrix, and M be a preset inertia matrix, which is a positive definite pairwise matrix. For Coriolis and, Here is the damping matrix. To control the input force.

[0191] Optionally, the prediction step length calculation module 340 can be specifically used to: extract real-time position information and real-time speed from the real-time operating status information, and determine the sub-path to be navigated based on the real-time position information and the reference path trajectory.

[0192] Determine the curvature trajectory based on the sub-path to be navigated. Based on real-time speed information and the sub-path to be navigated, the curvature-time function is dynamically calculated. .

[0193] According to the formula: The calculation of the unmanned surface platform within the preset sliding window Target path curvature ,in, The preset control cycle is denoted by , and i is the index variable of the summation loop.

[0194] Optionally, the prediction step size calculation module 340 can also be used specifically for: based on the formula: The dynamic prediction step size used for calculating wheel trajectory tracking ;

[0195] in, For the target path curvature The absolute value, and This is an adjustment factor used to balance the path tracking accuracy and control stability of unmanned surface platforms. The preset maximum step size threshold, The preset minimum step size threshold, For curvature trajectory The maximum absolute value among all trajectory points.

[0196] Optionally, the optimal command rudder angle calculation module 360 ​​can be specifically used for: based on the formula: The dynamic prediction step size used for calculating wheel trajectory tracking ;

[0197] in, For the target path curvature The absolute value, and This is an adjustment factor used to balance the path tracking accuracy and control stability of unmanned surface platforms. The preset maximum step size threshold, The preset minimum step size threshold, For curvature trajectory The maximum absolute value among all trajectory points.

[0198] The track tracking device based on variable forward look-ahead distance provided in the embodiments of the present invention can execute the track tracking method based on variable forward look-ahead distance provided in any embodiment of the present invention, and has the corresponding functional modules and beneficial effects of the execution method.

[0199] The collection, storage, use, processing, transmission, provision, and disclosure of user personal information involved in the technical solution disclosed herein comply with the provisions of relevant laws and regulations and do not violate public order and good morals.

[0200] Example 4

[0201] Figure 9A schematic diagram of an electronic device 10, which can be used to implement embodiments of the present invention, is shown. The electronic device is intended to represent various forms of digital computers, such as laptop computers, desktop computers, workstations, personal digital assistants, servers, blade servers, mainframe computers, and other suitable computers. The electronic device can also represent various forms of mobile devices, such as personal digital processors, cellular phones, smartphones, wearable devices (e.g., helmets, glasses, watches, etc.), and other similar computing devices. The components shown herein, their connections and relationships, and their functions are merely illustrative and are not intended to limit the implementation of the invention described and / or claimed herein.

[0202] like Figure 9 As shown, the electronic device 10 includes at least one processor 11 and a memory, such as a read-only memory (ROM) 12 or a random access memory (RAM) 13, communicatively connected to the at least one processor 11. The memory stores computer programs executable by the at least one processor. The processor 11 can perform various appropriate actions and processes based on the computer program stored in the ROM 12 or loaded from storage unit 18 into the RAM 13. The RAM 13 can also store various programs and data required for the operation of the electronic device 10. The processor 11, ROM 12, and RAM 13 are interconnected via a bus 14. An input / output (I / O) interface 15 is also connected to the bus 14.

[0203] Multiple components in electronic device 10 are connected to I / O interface 15, including: input unit 16, such as keyboard, mouse, etc.; output unit 17, such as various types of displays, speakers, etc.; storage unit 18, such as disk, optical disk, etc.; and communication unit 19, such as network card, modem, wireless transceiver, etc. Communication unit 19 allows electronic device 10 to exchange information / data with other devices through computer networks such as the Internet and / or various telecommunications networks.

[0204] Processor 11 can be a variety of general-purpose and / or special-purpose processing components with processing and computing capabilities. Some examples of processor 11 include, but are not limited to, a central processing unit (CPU), a graphics processing unit (GPU), various special-purpose artificial intelligence (AI) computing chips, various processors running machine learning model algorithms, a digital signal processor (DSP), and any suitable processor, controller, microcontroller, etc. Processor 11 performs the various methods and processes described above, such as a trajectory tracking method based on variable forward look-ahead distance.

[0205] That is, during the autonomous navigation of the unmanned surface platform according to the reference path trajectory, the platform acquires the real-time operating status information of the unmanned surface platform at the current moment through at least one sensor carried on the platform, and acquires the ideal position information of the unmanned surface platform at the current moment in the reference path trajectory.

[0206] Based on the ideal position information, as well as the real-time position and speed of the unmanned surface platform in the real-time operation status information, the forward sight distance of the target at the current moment is dynamically calculated, and the reference heading angle of the unmanned surface platform is calculated based on the forward sight distance of the target.

[0207] Based on real-time operational status information and a pre-built nonlinear disturbance observer, real-time unknown time-varying disturbance information of the ocean at the current moment is obtained.

[0208] Based on real-time operational status information and reference path trajectory, the curvature trajectory and the target path curvature of the unmanned surface platform under a preset sliding window are calculated. Based on the target path curvature and curvature trajectory, the dynamic prediction step size used for this round of trajectory tracking is calculated.

[0209] Based on real-time operational status information, real-time unknown time-varying disturbance information in the ocean, dynamic prediction step size, reference heading angle, and pre-built model prediction controller, the predicted motion status information of the surface unmanned platform at future moments is obtained by dynamically predicting step size.

[0210] The optimal command rudder angle at the current moment is solved by using the path tracking deviation optimization function constructed based on the predicted motion state information and the reference path trajectory, and the motion control of the unmanned surface platform is performed based on the optimal command rudder angle.

[0211] Upon reaching the new current moment, return to execute the operation of acquiring the real-time operating status information of the unmanned surface platform at the current moment through at least one sensor carried on the platform, until the entire trajectory tracking process is completed.

[0212] In some embodiments, the variable look-ahead distance-based trajectory tracking method can be implemented as a computer program tangibly contained in a computer-readable storage medium, such as storage unit 18. In some embodiments, part or all of the computer program can be loaded and / or installed on electronic device 10 via ROM 12 and / or communication unit 19. When the computer program is loaded into RAM 13 and executed by processor 11, one or more steps of the variable look-ahead distance-based trajectory tracking method described above can be performed. Alternatively, in other embodiments, processor 11 can be configured to perform the variable look-ahead distance-based trajectory tracking method by any other suitable means (e.g., by means of firmware).

[0213] Various embodiments of the systems and techniques described above herein can be implemented in digital electronic circuit systems, integrated circuit systems, field-programmable gate arrays (FPGAs), application-specific integrated circuits (ASICs), application-specific standard products (ASSPs), systems-on-a-chip (SoCs), payload-programmable logic devices (CPLDs), computer hardware, firmware, software, and / or combinations thereof. These various embodiments may include implementations in one or more computer programs that can be executed and / or interpreted on a programmable system including at least one programmable processor, which may be a dedicated or general-purpose programmable processor, capable of receiving data and instructions from a storage system, at least one input device, and at least one output device, and transmitting data and instructions to the storage system, the at least one input device, and the at least one output device.

[0214] Computer programs used to implement the methods of the present invention may be written in any combination of one or more programming languages. These computer programs may be provided to a processor of a general-purpose computer, a special-purpose computer, or other programmable data processing device, such that when executed by the processor, the computer programs cause the functions / operations specified in the flowcharts and / or block diagrams to be performed. The computer programs may be executed entirely on a machine, partially on a machine, or as a standalone software package, partially on a machine and partially on a remote machine, or entirely on a remote machine or server.

[0215] In the context of this invention, a computer-readable storage medium can be a tangible medium that may contain or store a computer program for use by or in conjunction with an instruction execution system, apparatus, or device. A computer-readable storage medium may include, but is not limited to, electronic, magnetic, optical, electromagnetic, infrared, or semiconductor systems, apparatus, or devices, or any suitable combination thereof. Alternatively, a computer-readable storage medium may be a machine-readable signal medium. More specific examples of machine-readable storage media include electrical connections based on one or more wires, portable computer disks, hard disks, random access memory (RAM), read-only memory (ROM), erasable programmable read-only memory (EPROM or flash memory), optical fibers, portable compact disk read-only memory (CD-ROM), optical storage devices, magnetic storage devices, or any suitable combination thereof.

[0216] To provide interaction with a user, the systems and techniques described herein can be implemented on an electronic device having: a display device (e.g., a CRT (cathode ray tube) or LCD (liquid crystal display) monitor) for displaying information to the user; and a keyboard and pointing device (e.g., a mouse or trackball) through which the user provides input to the electronic device. Other types of devices can also be used to provide interaction with the user; for example, feedback provided to the user can be any form of sensory feedback (e.g., visual feedback, auditory feedback, or tactile feedback); and input from the user can be received in any form (including sound input, voice input, or tactile input).

[0217] The systems and technologies described herein can be implemented in computing systems that include backend components (e.g., as data servers), or middleware components (e.g., application servers), or frontend components (e.g., user computers with graphical user interfaces or web browsers through which users can interact with implementations of the systems and technologies described herein), or any combination of such backend, middleware, or frontend components. The components of the system can be interconnected via digital data communication of any form or medium (e.g., communication networks). Examples of communication networks include local area networks (LANs), wide area networks (WANs), blockchain networks, and the Internet.

[0218] A computing system can include clients and servers. Clients and servers are generally located far apart and typically interact through communication networks. The client-server relationship is created by computer programs running on the respective computers and having a client-server relationship with each other. The server can be a cloud server, also known as a cloud computing server or cloud host, which is a hosting product within the cloud computing service system to address the shortcomings of traditional physical hosts and VPS services, such as high management difficulty and weak business scalability.

[0219] It should be understood that the various forms of processes shown above can be used, with steps reordered, added, or deleted. For example, the steps described in this invention can be executed in parallel, sequentially, or in different orders, as long as the desired result of the technical solution of this invention can be achieved, and this is not limited herein.

[0220] The specific embodiments described above do not constitute a limitation on the scope of protection of this invention. Those skilled in the art should understand that various modifications, combinations, sub-combinations, and substitutions can be made according to design requirements and other factors. Any modifications, equivalent substitutions, and improvements made within the spirit and principles of this invention should be included within the scope of protection of this invention.

Claims

1. A variable look-ahead distance based track-while-scan method, characterized by, The method is executed by a main controller in a surface unmanned platform, and comprises the following steps: In the process of autonomous navigation of the surface unmanned platform according to the reference path trajectory, real-time running state information of the surface unmanned platform at the current time is obtained through at least one sensor carried on the platform, and ideal position information of the surface unmanned platform at the current time is obtained in the reference path trajectory; According to the ideal position information, real-time position information and real-time speed of the surface unmanned platform in the real-time running state information, a target forward-looking distance at the current time is dynamically calculated, and a reference heading angle of the surface unmanned platform is calculated according to the target forward-looking distance; Real-time ocean unknown time-varying disturbance information at the current time is obtained according to the real-time running state information and a pre-constructed nonlinear disturbance observer; According to the real-time running state information and the reference path trajectory, a curvature trajectory and a target path curvature of the surface unmanned platform in a preset sliding window are calculated, and a dynamic prediction step used in this round of track tracking is calculated according to the target path curvature and the curvature trajectory; According to the real-time running state information, the real-time ocean unknown time-varying disturbance information, the dynamic prediction step, the reference heading angle and a pre-constructed model prediction controller, predicted motion state information of the surface unmanned platform at a plurality of future times is obtained; An optimal command rudder angle at the current time is solved by using a path tracking deviation optimization function constructed according to each predicted motion state information and the reference path trajectory, and the motion of the surface unmanned platform is controlled according to the optimal command rudder angle; After reaching a new current time, the operation of obtaining the real-time running state information of the surface unmanned platform at the current time through the at least one sensor carried on the platform is returned to be executed until the entire track tracking process is completed.

2. The method of claim 1, wherein, According to the ideal position information, real-time position information and real-time speed of the surface unmanned platform in the real-time running state information, a target forward-looking distance at the current time is dynamically calculated, and a reference heading angle of the surface unmanned platform is calculated according to the target forward-looking distance, which comprises the following steps: According to two X-axis coordinate values in the real-time position information and the ideal position information, a longitudinal error of the water surface unmanned platform is calculated , and according to two Y-axis coordinate values in the real-time position information and the ideal position information, a lateral error of the water surface unmanned platform is calculated ; According to the formula: , the target forward-looking distance at the current time t is calculated ; wherein, is a preset minimum look-ahead distance, U is the real-time speed of the water surface unmanned platform, , is a preset adaptive parameter; The reference course angle of the water surface unmanned platform is calculated according to the formula: , wherein, the reference course angle of the water surface unmanned platform is calculated according to the formula: ; wherein, atan2(.) is a four-quadrant arctangent function, is a derivative of a Y-axis coordinate value in ideal position information at a current time t is a derivative of an X-axis coordinate value in ideal position information at a current time t is a sideslip angle of the surface unmanned platform.​​ 3. The method of claim 1, wherein, Real-time ocean unknown time-varying disturbance information at the current time is obtained according to the real-time running state information and a pre-constructed nonlinear disturbance observer, which comprises the following steps: In the real-time running state information, the real-time lateral velocity v of the water surface unmanned platform is extracted, and the nonlinear function in the nonlinear disturbance observer is calculated according to the formula , , wherein, is a preset positive definite gain matrix; According to the formula: , the real-time unknown time-varying interference information of the ocean at the current moment is calculated ; wherein z is the internal state of the nonlinear interference observer at the current moment, , wherein is the roll interference estimation value of the surface unmanned platform, is the pitch interference estimation value of the surface unmanned platform, is the heave interference estimation value of the surface unmanned platform; According to the formula: , the observer internal state of the nonlinear disturbance observer at the next moment is updated for the next round of track tracking; wherein, is an observer gain matrix, M is a predetermined inertia matrix, and the inertia matrix is positive definite, is a Coriolis and, is a damping matrix, is a control input force.

4. The method of claim 1, wherein, According to the real-time running state information and the reference path trajectory, a curvature trajectory and a target path curvature of the surface unmanned platform in a preset sliding window are calculated, which comprises the following steps: Real-time position information and real-time speed in the real-time running state information are extracted, and a to-be-navigated sub-path is determined according to the real-time position information and the reference path trajectory; According to the to-be-traveled sub-path, a curvature trajectory is determined , and a curvature time function is dynamically calculated according to real-time speed information and the to-be-traveled sub-path ; The target path curvature of the water surface unmanned platform under a preset sliding window is calculated according to the formula: , wherein, is a preset control period, and i is an index variable of the summation loop.

5. The method of claim 4, wherein, A dynamic prediction step used in this round of track tracking is calculated according to the target path curvature and the curvature trajectory, which comprises the following steps: The dynamic prediction step size used for the wheel track following is calculated according to the formula: ; and ; wherein, the absolute value of the curvature of the target path, and is an adjustment coefficient for balancing the path tracking accuracy and control stability of the water surface unmanned platform, is a preset maximum step threshold value, is a preset minimum step threshold value, is the maximum value in the absolute value of each trajectory point in the curvature trajectory .​ 6. The method according to any one of claims 1 to 5, characterized in that, An optimal command rudder angle at the current time is solved by using a path tracking deviation optimization function constructed according to each predicted motion state information and the reference path trajectory, which comprises the following steps: The path tracking deviation optimization function is constructed according to each predicted motion state information and the reference path trajectory; Under the constraint conditions of state variables and control inputs, an optimal solution of the path tracking deviation optimization function is solved by using an improved artificial bee colony algorithm, so as to obtain the optimal command rudder angle at the current time. The initial population used in the improved artificial bee colony algorithm is searched in a search space defined by a preset upper search space boundary and a preset lower search space boundary based on a preset chaotic variable.

7. The method according to any one of claims 1 to 5, characterized in that, The water surface unmanned platform is a water surface unmanned ship, and the track tracking method based on the variable forward-looking distance is applied to an automatic measurement surveying and mapping scene based on an island reef sea area.

8. An electronic device, comprising: The electronic device includes: at least one processor; and a memory connected with the at least one processor; wherein The memory stores a computer program executable by the at least one processor, and the computer program is executed by the at least one processor to enable the at least one processor to execute the track tracking method based on the variable forward-looking distance according to any one of claims 1-7.

9. A computer-readable storage medium, characterized in that, The computer readable storage medium stores computer instructions for causing the processor to execute the track tracking method based on the variable forward-looking distance according to any one of claims 1-7.

10. A computer program product, characterised in that, The computer program product includes a computer program that, when executed by a processor, implements the track tracking method based on the variable forward-looking distance according to any one of claims 1-7.

Images