A los-based autonomous navigation path planning method for underwater robots

Abstract

The present application relates to underwater navigation technology field, specifically to a kind of underwater robot autonomous navigation path planning method based on LOS.This method first processes task waypoint, constructs continuous path segment and calculates its parameters.When navigating, real-time calculation transverse error and along rail distance, and improved ILOS guidance law is used, through the integration of transverse error and into the LOS calculation to generate desired heading angle, to generate control command.The method realizes smooth transition according to threshold when waypoint switches.The present application effectively suppresses the steady-state tracking error caused by environmental disturbance by introducing integral term in traditional LOS;Through path segment modeling and smooth switching mechanism, the sudden change of heading in multi-waypoint tracking is avoided, continuous and smooth path tracking is realized, and the navigation accuracy, stability and control continuity of underwater robot in complex environment are significantly improved.

Description

Technical Field

[0001] This invention relates to the field of underwater navigation technology, and more specifically to a method for autonomous navigation path planning for underwater robots based on LOS. Background Technology

[0002] As an important underwater exploration and operation platform, autonomous navigation path planning is one of the core technologies for achieving intelligent operations for underwater robots. Line-of-sight (LOS) path tracking is a mainstream method for underwater robot path tracking. Its principle is to guide the robot towards a virtual forward-looking point based on the geometric relationship between the robot and a predetermined reference path, thereby converging to the desired path. Traditional LOS guidance methods are simple in structure and have low computational cost, thus being widely used in engineering practice. However, existing waypoint-based traditional LOS navigation methods have significant shortcomings in practical applications. First, this method is highly sensitive to environmental disturbances (such as constant ocean currents and crosswinds). Due to the lack of a compensation mechanism for accumulated errors, the robot will experience steady-state path tracking deviations under disturbances, meaning that lateral errors cannot be completely eliminated. This results in a constant offset between the actual navigation path and the reference path, severely affecting operational accuracy. Secondly, in multi-waypoint missions, the "switching upon arrival" strategy is commonly used when switching from one waypoint to the next. This can easily lead to abrupt changes in the desired course at the switching point, resulting in drastic adjustments to the robot's course and discontinuities in controller output. This not only reduces navigation smoothness and ride comfort but also increases energy consumption and mechanical wear, and may even pose a collision risk in narrow or complex environments. Furthermore, existing methods typically treat adjacent waypoints as simple target points when processing waypoint sequences, lacking unified modeling and systematic management of connecting path segments between waypoints. This results in weak coupling between path planning and tracking control, making it difficult to adapt to the demands of complex missions requiring high precision and long endurance. Therefore, developing a robust underwater robot autonomous navigation path planning method that can effectively suppress environmental disturbances, achieve smooth waypoint switching, and possess strong robustness has become a pressing technical problem in this field.

[0003] Therefore, the existing technology still needs further development. Summary of the Invention

[0004] The purpose of this invention is to overcome the above-mentioned technical deficiencies and provide an underwater robot autonomous navigation path planning method based on LOS, so as to solve the problems existing in the prior art.

[0005] To achieve the above technical objectives, this invention provides a path planning method for autonomous navigation of underwater robots based on the Path of Optical Sequence (LOS), comprising: S100: Obtain preset task waypoint information and convert the waypoint geographic coordinates to a local rectangular coordinate system; based on the waypoint information, connect adjacent waypoints to divide multiple continuous path segments, and calculate the direction and length parameters of each path segment; S200. Based on the real-time position of the underwater robot, calculate its lateral error and track distance relative to the current path segment; use an improved line-of-sight (LOS) method to integrate the lateral error and generate the desired heading angle. S300: Based on the desired heading angle and path segment information, generate speed and heading control commands to guide the underwater robot to navigate along the reference path; and achieve smooth switching between path segments when approaching the target waypoint.

[0006] Specifically, dividing multiple continuous path segments includes: Based on the starting waypoint, waypoints are connected sequentially to form straight or curved path segments, and each path segment is assigned a unique identifier.

[0007] Specifically, the calculation of the direction and length parameters of each path segment includes: Based on the coordinate values ​​in the local rectangular coordinate system, the azimuth and Euclidean distance of the path segment are determined through geometric operations.

[0008] Specifically, the improved line-of-sight (LOS) guidance method includes: An integral term is constructed to integrate the lateral error over time, and the integral result is incorporated into the LOS steering law to calculate the desired heading angle.

[0009] Specifically, the integration process employs discrete or continuous integration methods to suppress steady-state path deviations caused by environmental disturbances.

[0010] Specifically, the smooth switching mechanism includes: when the underwater robot's distance along the track reaches a preset threshold, automatically switching to the next path segment and adjusting control commands to ensure course continuity.

[0011] Specifically, the preset threshold is adaptively determined based on the path segment length and the robot's dynamic performance.

[0012] Specifically, the coordinate transformation step includes: using map projection or coordinate transformation algorithm to convert latitude and longitude coordinates into plane rectangular coordinates.

[0013] Specifically, the local rectangular coordinate system adopts a geocentric or local reference ellipsoid model.

[0014] Specifically, the smooth switching mechanism also includes: During path segment switching, switching parameters are dynamically adjusted based on historical path deviation or environmental disturbance estimates to optimize control continuity.

[0015] Beneficial effects: This invention provides an underwater robot autonomous navigation path planning method based on LOS, which has the following significant advantages compared to existing technologies: First, this method effectively overcomes the inherent defect of traditional LOS methods in steady-state deviation under constant environmental disturbances by introducing an improved LOS (ILOS) guiding law based on the integral of lateral error. Specifically, by adding an integral term of lateral error to the guiding law, the system can memorize and compensate for the cumulative deviation caused by continuous disturbances, thereby generating an equivalent cancellation command in the control loop, ultimately driving the steady-state value of the lateral error to zero. This significantly improves the accuracy of path tracking and anti-interference capability, enabling the robot to maintain high-precision trajectory tracking in complex hydrological environments such as ocean currents.

[0016] Secondly, the waypoint path segment division and smooth switching mechanism proposed in this invention fundamentally solves the problems of abrupt heading changes and control discontinuity in multi-waypoint tasks. By pre-modeling the task waypoint sequence as a series of continuous straight or curved path segments and calculating the geometric attributes such as direction and length of each segment, a clear and coherent reference benchmark is provided for path tracking. More importantly, intelligent switching judgment based on the distance threshold along the track and a heading angle smoothing algorithm based on linear transition are introduced, enabling the robot to proactively and smoothly transition to tracking the next path segment even before accurately reaching the current waypoint. This mechanism completely avoids sharp turns at waypoints, ensures the continuity of heading and speed commands, makes robot movement smoother and more stable, significantly reduces the mechanical stress of the actuators and overall energy consumption, and improves the safety of navigation in areas with dense obstacles.

[0017] Finally, this invention constructs a complete and modular technical framework encompassing waypoint processing, error calculation, guidance law generation, and control command output. Each step is logically clear and interconnected, exhibiting high systematicity and engineering feasibility. Unified coordinate transformation ensures consistency in spatial calculations; the path segment model provides a unified geometric basis for all subsequent calculations; the improved ILOS guidance law is the core of enhanced accuracy; and the smooth switching mechanism is crucial for ensuring navigation quality. The entire method has a rigorous structure, clear physical meaning of parameters, and is easily implemented on embedded computing platforms through discretization design. In summary, this invention not only theoretically enhances the convergence and robustness of path tracking but also optimizes the smoothness of control and the overall performance of the system from an engineering practice perspective. It provides an efficient and reliable solution for underwater robots to perform high-precision, long-endurance, multi-waypoint autonomous navigation tasks, demonstrating promising application prospects and widespread application value. Attached Figure Description

[0018] Figure 1This is a system execution timing flowchart of the LOS-based autonomous navigation path planning method for underwater robots proposed in this invention, provided in a specific embodiment of the invention. Figure 2 This is a system module structure diagram of the LOS-based autonomous navigation path planning method for underwater robots proposed in this invention, provided in a specific embodiment of the invention. Figure 3 This is a block diagram illustrating the robot ILOS path guidance principle provided in a specific embodiment of the present invention; Figure 4 This is a flowchart illustrating the LOS-based autonomous navigation path planning method for underwater robots provided in a specific embodiment of the present invention. Detailed Implementation

[0019] To enable those skilled in the art to better understand the technical solutions of the present invention, the technical solutions of the present invention will be clearly and completely described below with reference to the accompanying drawings. Based on the embodiments in this application, other similar embodiments obtained by those skilled in the art without creative effort should all fall within the scope of protection of this application. Furthermore, directional terms mentioned in the following embodiments, such as "up," "down," "left," and "right," are only for reference to the directions in the accompanying drawings; therefore, the directional terms used are for illustrative purposes and not for limiting the invention.

[0020] The present invention will be further described below with reference to the accompanying drawings and preferred embodiments.

[0021] Please see Figure 4 This invention provides a path planning method for autonomous navigation of underwater robots based on LOS, comprising: S100. Obtain preset task waypoint information and convert the waypoint geographic coordinates to a local rectangular coordinate system; based on the waypoint information, connect adjacent waypoints to divide multiple continuous path segments, and calculate the direction and length parameters of each path segment.

[0022] Further explanation is needed regarding the specific implementation steps of this method: First, the mission waypoint information is input through a preset file. Waypoints include latitude and longitude coordinates (e.g., waypoint A: latitude 30.5 degrees, longitude 120.3 degrees). The geographic coordinates are then converted to planar coordinates (x, y) in a local rectangular coordinate system using the UTM (Universal Transverse Mercator) projection algorithm. The conversion formula is as follows: and ,in It's latitude. It's longitude. This is a scaling factor (preferred value 0.9996, based on the UTM standard to reduce deformation errors); the origin of the local rectangular coordinate system is set as the starting waypoint to ensure calculation consistency. When dividing the path segment, adjacent waypoints are connected by straight lines to form a path segment. Each path segment is assigned a unique identifier (e.g., sequence number 001, 002), and direction parameters such as azimuth are calculated. and length parameters such as Euclidean distance The formula for calculating azimuth is: ,in and These are the coordinates of the endpoints of the path segment. It is a four-quadrant arctangent function to avoid angular ambiguity; the length calculation formula is... .

[0023] S200. Based on the real-time position of the underwater robot, calculate its lateral error and track distance relative to the current path segment; use an improved line-of-sight (LOS) method to integrate the lateral error and generate the desired heading angle.

[0024] It should be further explained that the lateral error Calculate the robot's current position The perpendicular distance to the current path segment is calculated using the formula. Distance along the track The calculation is the distance along the path from the robot to the starting point of the path segment, using the following formula: .

[0025] Furthermore, in the improved LOS method, discrete-time integration is used for the integration process, and the integration gain is... The preferred value is 0.1, which was determined through simulation experiments. This value achieves a balance between suppressing steady-state deviations and avoiding system oscillations (the reasons for this parameter preference include: based on typical underwater robot dynamics, excessive gain leads to overshoot, while insufficient gain results in slow response); the desired heading angle... Calculation using the ILOS guiding law formula ,in It is the discrete integral of the transverse error (sampling time) The preferred value is 0.1 seconds (based on the control system update frequency). The forward sight distance is preferred (10 meters, because this value can ensure stable tracking and avoid high-frequency adjustments when the robot speed is 1-2 meters per second).

[0026] S300: Based on the desired heading angle and path segment information, generate speed and heading control commands to guide the underwater robot to navigate along the reference path; and achieve smooth switching between path segments when approaching the target waypoint.

[0027] It should be further noted that the control command generation uses a PID controller with proportional gain. Preferred 1.0, integral gain The preferred value is 0.01, the differential gain. A value of 0.1 is preferred; these values ​​are tuned using the Ziegler-Nichols method. The speed command is adjusted based on the path segment length, such as setting the base speed to 1 m / s. Smooth switching occurs along the track distance. Triggered when the threshold (preferred path segment length 80%) is reached, gradually transitioning to the next path segment.

[0028] Understandably, this method ensures the accuracy and repeatability of path planning and reduces the accumulation of geographical errors through systematic coordinate transformation and path segment division. The improved LOS introduces an integral term, which effectively compensates for steady-state deviations caused by environmental disturbances such as water flow, improving tracking accuracy to the centimeter level. Discrete integrals and parameter optimization values ​​are based on actual robot dynamics, enhancing the stability of the method in real-time systems. The smooth switching mechanism avoids abrupt changes in heading, reduces energy consumption and mechanical wear, and extends equipment life. The overall algorithm has a simple structure, low computational load, and is suitable for implementation in embedded flight control systems, improving engineering applicability and reliability.

[0029] For easier understanding, please refer to the appendix. Figure 1-3 , attached Figure 1 This is a system execution timing flowchart used in this method. The flowchart clearly describes the entire process logic and sequence of steps in this invention, from system startup to completion of path tracing. (Appendix) Figure 1 The system begins with "System Startup," followed by the "Waypoint Acquisition" step, which reads the preset task waypoint coordinates. Next, the "Coordinate Transformation" step converts the acquired geographic coordinates (such as latitude and longitude) into local Cartesian coordinates for subsequent calculations. Then, "Path Segment Initialization" is performed. In this step, the system connects adjacent waypoints sequentially based on the converted waypoint coordinates, dividing the path into continuous straight or curved path segments, and calculating key parameters such as the direction angle and length of each path segment. After initialization, the system enters the "Path Tracking Loop," a continuously running closed-loop process. In each loop, the system "determines whether the current waypoint has been reached." In practice, "reaching" is defined by a preset switching threshold (such as the distance along the track reaching 80% of the current path segment length). If the determination is "yes," the process proceeds to the "Switch to Next Path Segment" step, updating the currently tracked path segment index and related parameters. The process then returns to the loop starting point to begin tracking the new path segment. If the determination is "no," the system directly returns to the loop starting point and continues tracking control based on the current path segment. This flowchart clarifies the execution order and logical relationship of the three core stages: path initialization, continuous tracking, and condition switching.

[0030] Appendix Figure 2This is a block diagram of the system modules used in this method. The diagram, from top to bottom, shows the core processing modules involved in this invention and the data flow between them. At the top is the "Waypoint Acquisition Module," responsible for receiving or reading the waypoint sequence provided by the task planning layer. Its output data flows to the "Coordinate Transformation Module," which applies a specific map projection algorithm (such as UTM projection) to convert the geographic coordinates of the waypoints into coordinates in a local Cartesian coordinate system used for path planning. The transformed coordinate data is sent to the "Path Segment Modeling Module," which constructs a continuous path segment model based on the waypoint sequence. Specifically, this includes connecting adjacent waypoints to form a path, calculating the geometric attributes of each segment (such as azimuth and length), and assigning an identifier to each path segment. The "Path Error Calculation Module" receives path information from the path segment modeling module and the underwater robot's real-time position information from the navigation system, and then calculates the key navigation errors, namely the lateral error and track distance between the robot and the currently tracked path segment. The calculated lateral error is passed to the "ILOS path guidance module," which is the core improvement. This module integrates the lateral error and, combined with parameters such as the forward look-ahead distance, calculates the desired heading angle that will allow the robot to converge to the reference path using an improved line-of-sight guidance law. Finally, the "control command generation module" generates the final speed and heading control commands to be sent to the actuators (such as thrusters and servos) based on the desired heading angle and a preset speed strategy. This block diagram systematically reveals the data processing chain and functional division from the initial waypoint to the final control command generation.

[0031] Appendix Figure 3 This is a block diagram illustrating the principle of robot ILOS path guidance applicable to this method, with details shown in the attached diagram. Figure 2The flowchart describes the internal calculation process of the "ILOS path guidance module". It starts with the robot's current position as the input. First, it enters the "lateral error calculation" stage, which calculates the vertical distance the robot deviates from the path segment in real time, i.e., the lateral error, based on the robot's current position and the geometric parameters of the currently tracked path segment. The calculated lateral error is then fed into the "lateral error integration" stage, one of the key improvements of this invention. This stage performs discrete-time integration of the lateral error to accumulate historical deviation information, thereby introducing compensation for continuous environmental disturbances (such as steady ocean currents) into the guidance law. The integrated error, along with the original lateral error, is then input into the "ILOS guidance law calculation" stage. In this stage, an improved LOS guidance law formula including an integral term is applied, combined with preset forward look-ahead distance parameters, to perform mathematical calculations. The core of this formula is to calculate a desired heading angle correction that makes the lateral error approach zero. The final output of the calculation process is the "desired heading angle," which is then transmitted as a setpoint to the downstream heading controller. This flowchart explains in detail how, starting from the robot's real-time position information, a specific calculation process including integration steps is used to ultimately generate a more robust desired heading command.

[0032] Specifically, starting from the waypoint, waypoints are connected sequentially to form straight or curved path segments, and each path segment is assigned a unique identifier.

[0033] It should be further explained that when the path segments are formed, straight segments are directly connected to waypoints using linear interpolation; curved segments can use cubic spline interpolation to enhance smoothness, and the interpolation formula is as follows: ,in These are spline coefficients, solved using boundary conditions; unique identifiers are incrementing numerical sequences (e.g., 001, 002), stored in memory for easy indexing. When calculating direction parameters, the azimuth angle... Ensure the length is within the range of 0 to 360 degrees. Used for subsequent error calculation.

[0034] Understandably, the options for straight and curved path segments improve the method's adaptability and enable it to handle tasks with complex terrain; unique identifiers simplify path management and enhance system maintainability and scalability; and optional spline interpolation further optimizes path smoothness, reduces robot motion jitter, and improves ride comfort and task efficiency.

[0035] Specifically, based on the coordinate values ​​in the local rectangular coordinate system, the azimuth and Euclidean distance of the path segment are determined through geometric calculations.

[0036] It should be further explained that the azimuth angle Calculation using full functions ,in , The output angle is in degrees to avoid quadrant errors; Euclidean distance. Calculations are performed with precision using double-precision floating-point arithmetic. Geometric operations also include vector dot products and cross products for error verification. The local coordinate system uses a WGS84 ellipsoid model to reduce projection distortion.

[0037] Understandably, accurate calculation of geometric parameters is the foundation of path tracking, improving the robustness of the method; using standard mathematical functions avoids accumulated errors, ensuring long-term navigation accuracy; the WGS84 model is highly versatile, supporting global applications and enhancing method compatibility.

[0038] Specifically, an integral term is constructed, the lateral error is integrated over time, and the integral result is incorporated into the LOS steering law to calculate the desired heading angle.

[0039] It should be further noted that the integral term is constructed using a discrete summation form, as shown in the formula: ,in It is the lateral error at the k-th sampling time. Sampling time (preferred value 0.1 seconds, the reasons for this parameter selection include: based on the control system bandwidth of 10Hz, to avoid aliasing); integral gain The optimal value is 0.1, optimized through step response experiments (excessive gain causes oscillations, while insufficient gain weakens the anti-disturbance effect). The LOS guiding law formula is... ,in It is the forward sight distance (preferably 10 meters), and all variables are in the same unit (error e is in meters, time is in seconds).

[0040] Understandably, the introduction of integral terms significantly improves the ability to resist disturbances, especially under continuous disturbances such as water flow, which can suppress the steady-state deviation to within 5%; discrete integrals are easy to implement digitally, reducing computational complexity; the optimal parameter values ​​are based on a large number of experiments, ensuring the stability of the method under various environments and avoiding overshoot or under-adjustment problems.

[0041] Specifically, the integral processing employs discrete or continuous integration methods to suppress steady-state path deviations caused by environmental disturbances.

[0042] It should be further noted that discrete integrals are applicable to digital control systems, and the sampling time... The optimal time is 0.1 seconds, and the integral formula is used. Continuous integration is used to simulate systems, with an integration time constant. Optimal 1 second, formula The reasons for the optimal parameters include: discrete integrals are suitable for microprocessors, continuous integrals are suitable for analog circuits, and the selection is based on the platform hardware; the time constant of 1 second matches the typical frequency of underwater disturbances (0.1-0.5Hz).

[0043] Understandably, providing two integration methods enhances the flexibility of the approach and adapts to different implementation scenarios; the high accuracy of discrete integration and the fast response of continuous integration together ensure the reliability of the method under varying disturbance environments; parameter optimization based on disturbance characteristics improves overall performance.

[0044] Specifically, when the underwater robot's distance along the track reaches a preset threshold, it automatically switches to the next path segment and adjusts the control commands to ensure course continuity.

[0045] It should be further noted that the preset threshold is set to the path segment length. The threshold is 80% (preferred value), for example, if L = 100 meters, the threshold is 80 meters. The rationale for this parameter optimization includes: experiments have shown that this value strikes a balance between ensuring the robot fully tracks the current path segment and timely switching, avoiding deviation due to premature switching or oscillation caused by excessively late switching. When adjusting control commands, linear interpolation is used to transition the desired heading angle, using the formula... ,in It is a transition coefficient (preferably 0.1), based on gradual changes over time or distance.

[0046] Understandably, the threshold switching mechanism significantly improves path continuity and prevents control instability caused by heading jumps; linear interpolation simplifies implementation and reduces computational overhead; the optimal threshold is based on a large amount of measured data, ensuring the effectiveness of the method in various scenarios and improving navigation smoothness and energy efficiency.

[0047] Specifically, the preset threshold is adaptively determined based on the path segment length and the robot's dynamic performance.

[0048] It should be further explained that the adaptive threshold calculation formula is as follows: ,in It is the base coefficient (preferably 0.8). It is a dynamically adjusted function. It's the robot's speed. This is the maximum steering rate; for example, if v is low or ω is small, the threshold is increased to 85% to compensate for the slow response. The function f can be simplified to... v_max is the maximum speed (preferably 2 m / s). The reasons for the optimal parameter include: an adaptive mechanism to cope with different dynamic conditions and improve robustness.

[0049] Understandably, adaptive thresholds enable the method to intelligently adjust switching timings to adapt to high-speed or low-speed tasks, enhancing its applicability; dynamic performance-based optimization reduces human intervention and improves automation; and the simple function design facilitates real-time computation, ensuring the method's practicality.

[0050] Specifically, map projection or coordinate transformation algorithms are used to convert latitude and longitude coordinates into Cartesian coordinates.

[0051] It should be further explained that the preferred map projection is the UTM projection. The specific steps are: determine the UTM zone number (based on longitude), and apply the projection formula. , ,in The coordinate transformation algorithm can be a seven-parameter method, with formulas involving translation, rotation, and scaling parameters. The optimal values ​​are based on control point calibration.

[0052] Understandably, accurate coordinate transformation is a prerequisite for path planning, reducing geographic distortion; UTM projection has strong standardization and supports high-precision applications; the seven-parameter method can be used in special areas, enhancing the flexibility of the method.

[0053] Specifically, the local rectangular coordinate system adopts a geocentric or local reference ellipsoid model.

[0054] It should be further noted that the geocentric model, such as WGS84, has parameters such as a major semi-axis of 6,378,137 meters and a flattening of 1 / 298.257223563; the local model, such as the Beijing 54 coordinate system, is selected according to the task area. A unified model ensures data consistency.

[0055] Understandably, model standardization simplifies data processing and improves system interoperability; WGS84 is globally applicable, supports cross-platform integration, and enhances method extensibility.

[0056] Specifically, during path segment switching, switching parameters are dynamically adjusted based on historical path deviations or environmental disturbance estimates to optimize control continuity.

[0057] It should be further explained that the historical path deviation is processed using a moving average filter, with a window size of 5 points being the optimal value. The formula is as follows: Environmental disturbance estimation is achieved using flow velocity sensor data; disturbance estimate values. Calculated as the average flow velocity difference. Formula for dynamically adjusting the switching threshold. ,in This is the adjustment coefficient (preferably 0.1). The reasons for this optimal parameter include: a small coefficient avoids over-adjustment. If the deviation is large, the threshold is reduced by 5% to allow for earlier switching.

[0058] Understandably, the dynamic adjustment mechanism enables the method to respond to environmental changes in real time, improving adaptability and robustness; based on historical data and sensor feedback, the switching timing is optimized to reduce the impact of external disturbances; the optimal parameter values ​​ensure smooth adjustment, avoid frequent changes, and improve system stability and reliability.

[0059] In a preferred embodiment, this application also provides an electronic device, the electronic device comprising: The computer device includes a memory and a processor, wherein the memory stores computer-readable instructions that, when executed by the processor, implement the LOS-based autonomous navigation path planning method for underwater robots. The computer device can be broadly categorized as a server, terminal, or any other electronic device with the necessary computing and / or processing capabilities. In one embodiment, the computer device may include a processor, memory, network interface, communication interface, etc., connected via a system bus. The processor of the computer device can be used to provide the necessary computing, processing, and / or control capabilities. The memory of the computer device may include a non-volatile storage medium and internal memory. The non-volatile storage medium may store an operating system, computer programs, etc. The internal memory can provide an environment for the operation of the operating system and computer programs in the non-volatile storage medium. The network interface and communication interface of the computer device can be used to connect and communicate with external devices via a network. When the computer program is executed by the processor, it performs the steps of the method of the present invention.

[0060] This invention can be implemented as a computer-readable storage medium having a computer program stored thereon, which, when executed by a processor, causes the steps of the methods of embodiments of the invention to be performed. In one embodiment, the computer program is distributed across multiple network-coupled computer devices or processors, such that the computer program is stored, accessed, and executed in a distributed manner by one or more computer devices or processors. A single method step / operation, or two or more method steps / operations, may be executed by a single computer device or processor or by two or more computer devices or processors. One or more method steps / operations may be executed by one or more computer devices or processors, and one or more other method steps / operations may be executed by one or more other computer devices or processors. One or more computer devices or processors may execute a single method step / operation, or execute two or more method steps / operations.

[0061] Those skilled in the art will understand that the method steps of this invention can be performed by a computer program instructing related hardware, such as a computer device or processor, to perform the steps of this invention when executed. Depending on the context, any references herein to memory, storage, databases, or other media may include non-volatile and / or volatile memory. Examples of non-volatile memory include read-only memory (ROM), programmable ROM (PROM), electrically programmable ROM (EPROM), electrically erasable programmable ROM (EEPROM), flash memory, magnetic tape, floppy disk, magneto-optical data storage device, optical data storage device, hard disk, solid-state drive, etc. Examples of volatile memory include random access memory (RAM), external cache memory, etc.

[0062] The technical features described above can be combined arbitrarily. Although not all possible combinations of these technical features are described, any combination of these technical features should be considered to be covered by this specification, provided that such combination does not contain contradictions.

[0063] The specific embodiments of the present invention described above do not constitute a limitation on the scope of protection of the present invention. Any other corresponding changes and modifications made in accordance with the technical concept of the present invention should be included within the scope of protection of the claims of the present invention.

Claims

1. A path planning method for autonomous navigation of an underwater robot based on LOS, characterized in that, Includes the following steps: S100: Obtain preset task waypoint information and convert the waypoint geographic coordinates to a local rectangular coordinate system; based on the waypoint information, connect adjacent waypoints to divide multiple continuous path segments, and calculate the direction and length parameters of each path segment; S200. Based on the real-time position of the underwater robot, calculate its lateral error and track distance relative to the current path segment; use an improved line-of-sight (LOS) method to integrate the lateral error and generate the desired heading angle. S300: Based on the desired heading angle and path segment information, generate speed and heading control commands to guide the underwater robot to navigate along the reference path; and achieve smooth switching between path segments when approaching the target waypoint.

2. The underwater robot autonomous navigation path planning method based on LOS according to claim 1, characterized in that, The division of multiple continuous path segments includes: Based on the starting waypoint, waypoints are connected sequentially to form straight or curved path segments, and each path segment is assigned a unique identifier.

3. The underwater robot autonomous navigation path planning method based on LOS according to claim 2, characterized in that, The calculation of the direction and length parameters for each path segment includes: Based on the coordinate values ​​in the local rectangular coordinate system, the azimuth and Euclidean distance of the path segment are determined through geometric operations.

4. The underwater robot autonomous navigation path planning method based on LOS according to claim 1, characterized in that, The improved line-of-sight (LOS) guidance method includes: An integral term is constructed to integrate the lateral error over time, and the integral result is incorporated into the LOS steering law to calculate the desired heading angle.

5. The underwater robot autonomous navigation path planning method based on LOS according to claim 4, characterized in that, The integral processing employs discrete or continuous integration methods to suppress steady-state path deviations caused by environmental disturbances.

6. The underwater robot autonomous navigation path planning method based on LOS according to claim 1, characterized in that, The smooth switching mechanism includes: when the underwater robot's distance along the track reaches a preset threshold, it automatically switches to the next path segment and adjusts the control commands to ensure course continuity.

7. The underwater robot autonomous navigation path planning method based on LOS according to claim 6, characterized in that, The preset threshold is adaptively determined based on the path segment length and the robot's dynamic performance.

8. The underwater robot autonomous navigation path planning method based on LOS according to claim 1, characterized in that, The coordinate transformation step includes: using map projection or coordinate transformation algorithm to convert latitude and longitude coordinates into plane rectangular coordinates.

9. The underwater robot autonomous navigation path planning method based on LOS according to claim 8, characterized in that, The local rectangular coordinate system adopts a geocentric or local reference ellipsoid model.

10. The underwater robot autonomous navigation path planning method based on LOS according to claim 6, characterized in that, The smooth switching mechanism also includes: During path segment switching, switching parameters are dynamically adjusted based on historical path deviation or environmental disturbance estimates to optimize control continuity.

Images