Boat navigation control system and method

Abstract

The present invention discloses a boat navigation control system and method, relating to the field of ship control technology. The method comprises the following steps: obtaining first input data and second input data, the first input data including push rod position data and grip force sensor data; the second input data including path planning information and channel obstacle information; setting the current driving mode based on the channel obstacle information and grip force sensor data; and generating ship control instructions based on the first input data, the second input data, and the current driving mode; the ship control instructions including speed control instructions and heading control instructions. This solution improves the adaptability of a ship to different driving modes, achieving precise and real-time intelligent ship position and heading control, satisfying the user's driving experience while ensuring the safe operation of the ship.

Description

Technical Field

[0001] The present invention relates to the technical field of ship control, and in particular to a control system and method for supporting ship navigation. Background Art

[0002] With the rapid development of the global economy and the improvement of people's quality of life, more and more people are choosing ships for entertainment and sightseeing. To enrich the user experience, ships can provide both autonomous and assisted manual driving modes. The introduction of autonomous driving mode not only improves navigation safety but also reduces fatigue during long voyages. At the same time, the existence of assisted manual driving mode provides users with greater autonomy and enjoyment. However, traditional ship control systems often suffer from slow response speeds, insufficient control accuracy, and poor adaptability to external environmental changes during mode switching, making it difficult to effectively ensure the safety and stability of ships in complex waterways.

[0003] In recent years, several technical solutions have attempted to improve ship automation by introducing sensor data fusion and path planning algorithms. These advances have improved ship safety and navigation capabilities to a certain extent. However, existing solutions still suffer from poor adaptability when switching between autonomous and manual driving modes, especially in complex waterway environments, where accurate and real-time ship control remains difficult. Summary of the Invention

[0004] The present invention provides a vessel navigation control system and method to solve at least one of the problems mentioned in the above background technology.

[0005] The specific technical solutions provided in this application are as follows:

[0006] In a first aspect, the present invention provides a method for controlling a navigation of a vessel, comprising the steps of:

[0007] Acquire first input data and second input data, wherein the first input data includes putter position data and grip force sensor data; the second input data includes path planning information and channel obstacle information; the path planning information includes a path node mapping relationship;

[0008] Setting a current driving mode according to the course obstacle information and the grip force sensor data; the current driving mode includes a first driving mode and a second driving mode;

[0009] generating a ship control instruction according to the first input data, the second input data and the current driving mode; the ship control instruction includes a speed control instruction and a heading control instruction;

[0010] When the current driving mode is the first driving mode, generating the ship control instruction includes the steps of:

[0011] Calculate the deviations between the current position, current speed and current heading of the ship and the target position, target speed and target heading respectively according to the path planning information;

[0012] generating a ship control instruction according to the deviation value;

[0013] When the current driving mode is the second driving mode, generating the ship control instruction includes the steps of:

[0014] Determining ideal engine thrust based on the pushrod position data and a pushrod position-engine power mapping relationship;

[0015] A ship control command is generated according to the ideal engine thrust.

[0016] As a preferred solution of the present invention, generating a ship control instruction according to the deviation value is specifically: generating a ship control instruction according to the deviation value and a feasible control input set;

[0017] The generating of the ship control instruction according to the ideal engine thrust is specifically generating the ship control instruction according to the ideal engine thrust and a feasible control input set.

[0018] As a preferred embodiment of the present invention, before generating the ship control instruction according to the first input data, the second input data and the current driving mode, the steps include:

[0019] Construct the discretized dynamic equations of the ship to obtain the current state of the ship and all control input sets;

[0020] Calculating a first state set of the vessel in an obstacle-free situation based on the current state and all control input sets;

[0021] Calculate the current distance between the ship and the obstacle in real time based on the channel obstacle information and current status;

[0022] If the current distance is less than the first safety distance, calculate the second safety distance based on the channel obstacle information and define the constraint conditions based on the second safety distance;

[0023] A state set that meets the constraint condition in the first state set is filtered to obtain a second state set, and a feasible control input set is obtained according to the second state set.

[0024] As a preferred solution of the present invention, the ship discretized dynamic equation is expressed as:

[0025] ;

[0026] in, represents the ship state at the k+1th time step; represents the horizontal coordinate of the ship at the kth time step, represents the ordinate of the ship at the kth time step, represents the ship heading at the kth time step; is the time step length; represents all control input sets, which include several speeds and steering angles; is the ship speed at the kth time step, is the angular velocity of the ship at the kth time step; , , T represents the execution time of the ship control command.

[0027] As a preferred embodiment of the present invention, the constraint condition is expressed as:

[0028]

[0029]

[0030]

[0031] in, represents the horizontal coordinate of the obstacle, Indicates the vertical coordinate of the obstacle; is the preset location distance threshold; is the ship radius; is the obstacle radius.

[0032] As a preferred solution of the present invention, the step of obtaining the first input data and the second input data includes the following steps:

[0033] Acquire perception data and terrain data, obtain a topology map based on the perception data and terrain data, and obtain path planning information based on the topology map.

[0034] As a preferred solution of the present invention, the method of obtaining a topological map based on the sensing data and the terrain data includes the following steps:

[0035] Acquire perception data and terrain data and perform coordinate system transformation;

[0036] The terrain data is divided into undetermined open areas by analyzing the connected components, and the environmental key points are identified by Harris corner detection of the terrain data. The open areas are identified based on the environmental key points and the undetermined open areas, and the center points of the open areas are marked as topological nodes.

[0037] Connect each node to its nearest k nodes through the K-neighbor method to generate edges in the topological graph;

[0038] Set the Euclidean distance between nodes as the initial edge weight; obtain channel obstacle information based on the perception data, and adjust the edge weight according to the channel obstacle information;

[0039] The topology map is updated based on the perception data and terrain data acquired in real time.

[0040] As a preferred solution of the present invention, the path planning information includes a path node mapping relationship, a speed mapping relationship, and a heading mapping relationship; and obtaining the path planning information according to the topological map includes the following steps:

[0041] S121. Select two topological nodes in the topological graph as the starting node and the ending node, respectively, and create a distance vector and a predecessor vector; the distance vector is used to store the shortest distance from the starting point to each node; the predecessor vector is used to record the optimal predecessor node of each node; construct a priority queue, add the starting node to the priority queue, and set the current node as the starting node;

[0042] S122, iterative execution step: check the distance from the current node to each adjacent node, select the adjacent node with the smallest distance and remove it from the priority queue;

[0043] If the node removed from the priority queue is the end node, the iteration ends and proceeds to step S123;

[0044] S123 , tracing back from the end node to the start node through the precursor vector to generate an optimal path; generating a path node mapping relationship, a speed mapping relationship, and a heading mapping relationship based on the optimal path.

[0045] As a preferred embodiment of the present invention, after generating the ship control instruction according to the deviation value, the following steps are further included:

[0046] determining engine thrust based on the vessel control command;

[0047] A pushrod position control instruction is generated according to the engine thrust and pushrod position-engine power mapping relationship.

[0048] As a preferred embodiment of the present invention, after generating the ship control instruction according to the ideal engine thrust, the following steps are further included:

[0049] Obtaining the current state of the ship, and calculating the position deviation value between the current position of the ship and the target position according to the path node mapping relationship;

[0050] Generate push rod damping control instructions according to the current speed, current heading and position deviation value of the ship.

[0051] In a second aspect, the present invention provides a boat navigation control system, comprising a central processing module, a push rod assembly, a push rod control module, a first input module, a second input module and a host computer;

[0052] The push rod assembly is connected to a push rod control module and a first input module respectively; the first input module is used to obtain first input data and transmit it to a central processing module, wherein the first input data includes push rod position data and grip force sensor data; the push rod control module is used to obtain push rod control instructions and control the push rod assembly according to the push rod control instructions;

[0053] The second input module is connected to the host computer and the central processing module; the second input module is used to obtain the second input data transmitted by the host computer and transmit the second input data to the central processing module; the second input data includes path planning information and channel obstacle information;

[0054] The central processing module generates ship control instructions and push rod control instructions based on the first input data, the second input data and the current driving mode; the driving mode includes a first driving mode and a second driving mode; the push rod control instructions include a push rod position control instruction and a push rod damping control instruction; the path planning information includes a path node mapping relationship, a speed mapping relationship and a heading mapping relationship.

[0055] Compared with the prior art, the present invention has the following beneficial effects:

[0056] The boat navigation control method of the present invention can monitor the state of the boat and the external environment in real time by acquiring first input data and second input data, including push rod position data, grip force sensor data, path planning information, and channel obstacle information;

[0057] By calculating the deviation between the ship's current position, current speed, and current heading and the target state based on the path planning information in the first driving mode, the system can dynamically adjust the ship's speed and heading to ensure the accuracy of ship control and the stability of heading control;

[0058] By determining the ideal engine thrust based on the pushrod position data and the pushrod position-engine power mapping relationship in the second driving mode, the system can generate ship control instructions based on the driver's operating intentions, ensuring flexibility and responsiveness of ship control.

[0059] This solution improves the ship's adaptability to different driving modes and realizes intelligent ship position control and heading control with precise real-time response, satisfying the user's driving experience while ensuring the safe operation of the ship. BRIEF DESCRIPTION OF THE DRAWINGS

[0060] The accompanying drawings, which are incorporated in and constitute a part of this specification, illustrate embodiments consistent with the invention and, together with the description, serve to explain the principles of the invention.

[0061] In order to more clearly illustrate the embodiments of the present invention or the technical solutions in the prior art, the following briefly introduces the drawings required for use in the embodiments or the description of the prior art. Obviously, for ordinary technicians in this field, other drawings can be obtained based on these drawings without paying any creative labor.

[0062] Figure 1 A schematic flow chart of a method for controlling a boat's navigation according to an embodiment of the present invention;

[0063] Figure 2 A schematic diagram of a process for obtaining a feasible control input set provided by an embodiment of the present invention;

[0064] Figure 3 A schematic structural diagram of a boat navigation control system provided by an embodiment of the present invention. DETAILED DESCRIPTION

[0065] The following will clearly and completely describe the technical solutions in the embodiments of the present invention in conjunction with the accompanying drawings. Obviously, the described embodiments are only part of the embodiments of the present invention, not all of the embodiments. All other embodiments obtained by ordinary technicians in this field based on the embodiments of the present invention without making any creative efforts shall fall within the scope of protection of the present invention.

[0066] It should be noted that all directional indications (such as up, down, left, right, front, back, etc.) in the embodiments of the present invention are only used to explain the relative position relationship, movement status, etc. between the various components under a certain specific posture (as shown in the accompanying drawings). If the specific posture changes, the directional indication will also change accordingly.

[0067] In addition, the descriptions of "first", "second", etc. in the present invention are for descriptive purposes only and should not be understood as indicating or implying their relative importance or implicitly indicating the number of the indicated technical features. Therefore, the features defined as "first" or "second" may explicitly or implicitly include at least one of such features. In addition, the technical solutions between the various embodiments can be combined with each other, but this must be based on the fact that they can be implemented by ordinary technicians in this field. When the combination of technical solutions is contradictory or cannot be implemented, it should be deemed that such combination of technical solutions does not exist and is not within the scope of protection required by the present invention.

[0068] With the rapid development of the global economy and the improvement of people's quality of life, more and more people are choosing ships for entertainment and sightseeing. To enrich the user experience, ships can provide both autonomous and assisted manual driving modes. The introduction of autonomous driving mode not only improves navigation safety but also reduces fatigue during long voyages. At the same time, the existence of assisted manual driving mode provides users with greater autonomy and enjoyment. However, traditional ship control systems often suffer from slow response speeds, insufficient control accuracy, and poor adaptability to external environmental changes during mode switching, making it difficult to effectively ensure the safety and stability of ships in complex waterways.

[0069] In recent years, several technical solutions have attempted to improve ship automation by introducing sensor data fusion and path planning algorithms. These advances have improved ship safety and navigation capabilities to a certain extent. However, existing solutions still suffer from poor adaptability when switching between autonomous and manual driving modes, especially in complex waterway environments, where accurate and real-time ship control remains difficult.

[0070] In view of the above problems, it is urgent to propose a boat navigation control system and method to ensure the safety, stability and accuracy of the ship in different driving modes.

[0071] The specific embodiments of the present invention will be described in detail below:

[0072] First, see Figure 1 The present invention provides a method for controlling a boat's navigation, comprising the steps of:

[0073] S1. Acquire first input data and second input data, wherein the first input data includes putter position data and grip force sensor data; the second input data includes path planning information and channel obstacle information; the path planning information includes a path node mapping relationship;

[0074] The path node mapping relationship stores the coordinate information of all path nodes from the starting point to the end point and their corresponding time points. This embodiment can accurately represent each position point on the path through the path node mapping relationship, and the mapping relationship uses discrete form to store data.

[0075] S2. Setting a current driving mode according to the channel obstacle information and the grip force sensor data; the current driving mode includes a first driving mode and a second driving mode;

[0076] In this embodiment, setting the current driving mode according to the channel obstacle information and the grip force sensor data specifically involves switching the current driving mode from the first driving mode to the second driving mode, or from the second driving mode to the first driving mode, according to the grip force sensor data and the channel obstacle information.

[0077] Specifically, if the ship is in the first driving mode, the driver's operation data intervenes in the current driving state to achieve acceleration operation, the grip force data detects that the driver suddenly applies a large grip force, and the driver's operation intention can be judged based on the position data. The current driving mode is switched to the second driving mode, and the driver's operation intention is responded to first.

[0078] After the grip strength data remains below the set threshold for a period of time, or in response to an active driving mode switching request, a risk assessment is performed based on the channel obstacle information. If the risk assessment passes, the driving mode can be switched to the first assisted driving mode.

[0079] In this embodiment, when the system detects a low driver grip and normal path planning information, it maintains or enters the first driving mode, generating vessel control and position control commands to achieve autonomous driving or advanced assisted driving with minimal driver intervention. In the second driving mode, if the system detects a significant increase in driver grip or abnormal path planning information, it determines that the driver requires intervention or has a control intention, switches to the second driving mode, prioritizes driver input, and adjusts and generates relevant commands.

[0080] S3. Generate a ship control instruction according to the first input data, the second input data, and the current driving mode; the ship control instruction includes a speed control instruction and a heading control instruction;

[0081] Ship control instructions include speed control instructions and heading control instructions;

[0082] When the current driving mode is the first driving mode, generating the ship control instruction includes the steps of:

[0083] S311, calculating the deviation values ​​between the current position, current speed and current heading of the ship and the target position, target speed and target heading respectively according to the path planning information;

[0084] S312: Generate a ship control instruction according to the deviation value.

[0085] In this embodiment, in the first driving mode, the ship control system is regarded as an automatic driving control system, which autonomously navigates on preset path nodes and performs real-time control and adjustment of the ship based on the path planning information.

[0086] Step S311 uses the path planning information to enable the system to accurately calculate the deviation between the current state (position, speed, heading) and the target state; these deviation values ​​provide the specific differences between the current state of the ship and the target path and navigation parameters, and are the basis for adjusting control instructions, allowing the system to quickly perceive deviations.

[0087] Step S312 generates corresponding ship control instructions, including speed control instructions and heading control instructions, based on the calculated deviation values ​​(position deviation, speed deviation, and heading deviation). Specifically, a PID controller may be used to generate control instructions based on the deviation values. By setting the proportional (P), integral (I), and differential (D) parameters, the controller can effectively respond to deviation changes to dynamically adjust the speed and heading of the ship.

[0088] When the current driving mode is the second driving mode, generating the ship control instruction includes the steps of:

[0089] S321, determining an ideal engine thrust according to the pushrod position data and a pushrod position-engine power mapping relationship;

[0090] S322: Generate a ship control instruction according to the ideal engine thrust.

[0091] In this embodiment, in the second driving mode, the operator controls the vessel through the push rod assembly.

[0092] Step S321 determines the corresponding ideal engine thrust using the pushrod position data and the pushrod position-engine power mapping obtained by the first input module. Similar to the pushrod position-engine power mapping described above for the first steering mode, this mapping can be obtained through experimental analysis. Then, step S322 generates ship control commands based on the ideal engine thrust.

[0093] The boat navigation control method of the present invention can monitor the state of the boat and the external environment in real time by acquiring first input data and second input data, including push rod position data, grip force sensor data, path planning information, and channel obstacle information;

[0094] By setting the current driving mode according to the channel obstacle information and grip force sensor data, the driving mode can be switched intelligently to ensure the safety and responsiveness of the ship in different driving modes; when the driver intervenes in the operation, the system can give priority to responding to the driver's operating intentions to ensure the flexibility and safety of the ship's position control; when the system detects a channel obstacle or the driver's grip force is low, the system can automatically switch to automatic driving mode to ensure the stability and safety of the ship's heading control.

[0095] By calculating the deviation between the ship's current position, current speed, and current heading and the target state based on the path planning information in the first driving mode, the system can dynamically adjust the ship's speed and heading to ensure the accuracy of the ship's position control and the stability of the heading control;

[0096] By determining the ideal engine thrust based on the pushrod position data and the pushrod position-engine power mapping relationship in the second driving mode, the system can generate ship control instructions based on the driver's operating intentions, ensuring the flexibility and responsiveness of the ship's position control.

[0097] Through the above steps, this solution improves the ship's adaptability to different driving modes, realizes intelligent ship position control and heading control with precise real-time response, satisfies the user's driving experience while ensuring the safe operation of the ship.

[0098] As a preferred embodiment of the present invention, step S312, generating a ship control instruction according to the deviation value, specifically comprises: generating a ship control instruction according to the deviation value and a feasible control input set;

[0099] Step S322: generating a ship control instruction according to the ideal engine thrust, specifically generating a ship control instruction according to the ideal engine thrust and a feasible control input set.

[0100] It can be understood that, no matter in the first driving mode or the second driving mode, the ship control instructions need to meet the dynamic obstacle avoidance requirements.

[0101] In the first driving mode, for unexpected obstacles in the waterway, there is no need to regenerate path planning information. Instead, ship control instructions are generated based on the feasible control input set to quickly achieve obstacle avoidance and target tracking. Therefore, combining the deviation value and the feasible control input set to generate control instructions can enable the ship to accurately track the target state while avoiding collisions with obstacles.

[0102] In the second piloting mode, the central processing module prioritizes responding to the pilot's intended actions during normal navigation. However, in situations where obstacle avoidance is necessary, the central processing module can adjust the ship's control instructions generated in response to the ideal engine thrust in conjunction with the feasible control input set to prevent pilot errors and ensure safe navigation. Therefore, generating control instructions based on the ideal engine thrust and the "feasible control input set" allows for rapid adjustments to the ship's state based on the pilot's intended actions, while ensuring safety in complex navigation environments.

[0103] Furthermore, in this embodiment, before generating the ship control instruction according to the first input data, the second input data and the current driving mode, the steps include:

[0104] Get a set of feasible control inputs; see Figure 2 , obtaining a feasible control input set specifically includes the following steps:

[0105] S301, constructing a discretized dynamic equation of the ship to obtain the current state of the ship and all control input sets;

[0106] This step first constructs the ship dynamic model, which is expressed as:

[0107] ;

[0108] in, represents the ship status at time t; represents the control input at time t, , represents the set of all control inputs; represents the rate of change of ship status at time t, , T represents the execution time of the ship control instruction, that is, the time required from the start to the end of the execution of the ship control instruction.

[0109] In order to reduce the amount of calculation, the ship dynamic model is discretized. The ship dynamic model obtained by discretization is expressed as:

[0110] ;

[0111] in, represents the ship state at the k+1th time step, represents the ship state at the kth time step; represents the control input at the kth time step, ; is the time step length;

[0112] Based on the above content, the ship state includes the ship's position, speed and heading, so the ship's discretized dynamic equation is expressed as:

[0113] ;

[0114] in, represents the ship state at the k+1th time step; represents the horizontal coordinate of the ship at the kth time step, represents the ordinate of the ship at the kth time step, represents the ship heading at the kth time step; is the time step length; Represents the set of all control inputs, which includes all possible control inputs, including several speeds and steering angles; is the ship speed at the kth time step, is the angular velocity of the ship at the kth time step; , .

[0115] This step constructs the discretized dynamic equations for the vessel, which more accurately describe how the vessel moves under different control inputs. Obtaining the current state and all control inputs is fundamental to making predictions and subsequent calculations.

[0116] S302, calculating a first state set of the ship in an obstacle-free state based on the current state and all control input sets;

[0117] The first state set is calculated based on the vessel's current state and all possible control inputs, yielding all possible states that can be reached in the next step, assuming no obstacles are present. The first state set is calculated from the current state by considering all control input sets. This first state set is calculated without considering any obstacles in the waterway, meaning it is calculated purely from the perspective of possible motions.

[0118] S303, calculating the current distance between the ship and the obstacle in real time based on the channel obstacle information and the current status;

[0119] This step calculates the distance between the ship and obstacles in real time, ensuring that potential obstacles can be detected and addressed promptly during navigation. This real-time calculation is the foundation for dynamic obstacle avoidance.

[0120] S304: If the current distance is less than the first safety distance, calculate a second safety distance based on the channel obstacle information and define a constraint condition based on the second safety distance;

[0121] In this step, when the current distance is less than the first safety distance, the second safety distance is calculated and the constraint conditions are defined based on this. The first safety distance is greater than the second safety distance; the first safety distance is used to trigger the early warning mechanism, indicating that the ship is facing potential danger. At this time, the second safety distance used to define the constraint conditions needs to be calculated; the second safety distance is used to define the constraint conditions to ensure that there is enough buffer distance between the ship and the obstacle to deal with uncertainty and emergency situations; the channel obstacle information includes the obstacle location and obstacle radius ;

[0122] The constraints are expressed as:

[0123]

[0124]

[0125]

[0126] in, represents the horizontal coordinate of the obstacle, Indicates the vertical coordinate of the obstacle; is the preset location distance threshold; is the ship radius; It should be noted that in this embodiment, for ease of calculation, the ship and the obstacle are both modeled as circles on a two-dimensional plane, and the ship radius and the obstacle radius are the radii of the circular models of the ship and the obstacle, respectively.

[0127] S305: Filter the state set that meets the constraint conditions in the first state set to obtain a second state set, and obtain a feasible control input set based on the second state set;

[0128] The second state set is a subset of the first state set. The second state set is a state set that meets the constraint conditions and is screened out from the first state set. The screening process is achieved by filtering and verifying the ship state at each time step.

[0129] This embodiment constructs a dynamic model, calculates the obstacle-free status, detects obstacles in real time, and defines constraints, and finally screens out a feasible control input set. Based on the control input set, it is possible to ensure that the ship's control instructions take into account safety in emergency situations, thereby enhancing the ship's obstacle avoidance capability.

[0130] As a preferred embodiment of the present invention, the step of obtaining the first input data and the second input data includes the following steps:

[0131] Acquire perception data and terrain data, obtain a topology map based on the perception data and terrain data, and obtain path planning information based on the topology map.

[0132] Among them, perception data includes optical image data, sonar data and inertial measurement data; among them, optical image data captures environmental information in the form of visual images through optical cameras for image recognition, feature extraction and object detection; sonar data transmits sound waves and receives echoes through acoustic wave sensors, which is used for underwater detection of environmental information, such as detecting underwater obstacles, riverbed structures, etc.; inertial measurement data obtains the acceleration, angular velocity and attitude data of the ship through accelerometers and gyroscopes, which is used for attitude estimation and motion prediction.

[0133] Topographic data refers to the geographic and spatial information of the environment, typically obtained through pre-mapping or from map databases. Specifically, topographic data includes nautical chart data, GIS (Geographic Information System) data, and satellite imagery data. Nautical chart data includes detailed information such as water depth, seabed topography, waterways, and port facilities, used for safe navigation and route planning. GIS data includes location information such as coastlines, islands, buoys, and beacons, providing comprehensive environmental geographic information to aid route planning and obstacle avoidance. Satellite imagery data is imagery captured by remote sensing satellites, providing large-scale geographic information for map updates and environmental monitoring.

[0134] Furthermore, the obtaining of a topological map based on the perception data and terrain data comprises the steps of:

[0135] S111, acquiring perception data and terrain data and performing coordinate system transformation processing;

[0136] S112, dividing the undetermined open area by analyzing the terrain data through the connected components, identifying the environmental key points by detecting the terrain data through Harris corner points, identifying the open area based on the environmental key points and the undetermined open area, and marking the center point of the open area as a topological node;

[0137] In this embodiment, terrain data is represented in grid (raster) or vector form. Prior to the connected component analysis, the terrain data undergoes denoising and threshold segmentation. Denoising is used to eliminate noise in the data to ensure analysis accuracy. Threshold segmentation is used to segment the terrain data into open and non-open areas. For example, areas with a height below a set threshold are defined as pending open areas.

[0138] Harris corner detection is used to identify key points in the environment. Harris corner detection is a feature point detection method based on grayscale changes, suitable for finding corners in areas of high change in an image. Specifically, Harris corner detection involves the following steps: grayscale processing of the image; calculating the image gradient to identify areas of significant change; using the Harris response function to calculate the response value of each pixel; and setting a threshold to filter out points with high response values, which are recorded as key points in the environment.

[0139] Furthermore, using environmental key points, we can further accurately demarcate open areas within the unidentified open area. The edge of an open area is formed by a series of key points. Based on environmental key points, we can verify the initially identified unidentified open area to ensure that its boundaries are consistent with the identified environmental key point features. If any inconsistencies or inaccuracies are found, adjustments and corrections can be made using the information from the environmental key points.

[0140] Connected component analysis (CCA) is used to identify interconnected components within an image. In terrain data, CCA can help identify multiple independent open regions. CCA involves traversing the terrain data grid using a depth-first search (DFS) or breadth-first search (BFS) to mark all connected open regions. Each independent open region is assigned a unique label for easy identification and distinction.

[0141] Mark the center point of the open area. Specifically, for each marked open area, calculate its geometric center point and mark it as the center point of the open area. The center point of the open area is expressed as:

[0142] ;

[0143] in, is the horizontal coordinate of the i-th point in the open area, is the ordinate of the i-th point in the open area; N is the total number of points in the open area.

[0144] S113, connecting each node to its nearest k nodes using the K-neighbors method to generate edges of a topological graph;

[0145] Specifically, each node is connected to its k nearest nodes through the K-neighboring node method, including the following steps:

[0146] For each node i, calculate the Euclidean distance between it and all other nodes. For each node, sort the calculated distances from smallest to largest and select the first k nearest neighbors. Connect each node i with its k nearest neighbors to form edges in the topological graph. For example, for each node i, form an adjacency list containing its k neighbors.

[0147] S114. Set the Euclidean distance between nodes as the initial edge weight; obtain channel obstacle information based on the sensing data, and adjust the edge weight based on the channel obstacle information;

[0148] During implementation, if an edge (i.e., path) has an obstacle, the weight of that edge can be increased or set to infinity (indicating an infeasible path). If there are no obstacles on the path, the original Euclidean distance weight remains unchanged. In this step, different degrees of weight adjustment can be set based on the complexity and density of the obstacles. For example, the edge weights in areas with high obstacle density can be increased significantly.

[0149] S115. Update the topology map according to the perception data and terrain data acquired in real time.

[0150] Step S115 updates the topology map based on the perception data and terrain data acquired in real time, i.e., repeats steps S111-S114, compares the current topology map with the latest acquired perception data and terrain data, identifies environmental changes, including newly added obstacles, removed obstacles, and terrain changes; for new scenarios perceived (such as newly discovered traversable areas or newly appeared obstacles), identifies nodes that need to be added; uses the K-nearest neighbor method to find the neighboring nodes of the new nodes, establishes new edge relationships, and adjusts the weights of the newly added edges considering the channel obstacle information; then, locates and determines the topology nodes that are no longer valid in the current topology map (such as nodes covered by newly added obstacles), and is responsible for removing all the edges involved, and reconnecting other nodes contained in these edges to ensure the integrity of the topology map; performs local optimization on the updated topology map to ensure the effectiveness of the topology map and planning efficiency.

[0151] Furthermore, the path planning information includes a path node mapping relationship, a speed mapping relationship, and a heading mapping relationship; obtaining the path planning information according to the topological map includes the following steps:

[0152] S121. Select two topological nodes in the topological graph as the starting node and the ending node, respectively, and create a distance vector and a predecessor vector; the distance vector is used to store the shortest distance from the starting point to each node; the predecessor vector is used to record the optimal predecessor node of each node; construct a priority queue, add the starting node to the priority queue, and set the current node as the starting node;

[0153] Step S121 is the initialization phase, which ensures the initial state definition of each node and the initialization state of the priority queue.

[0154] S122, iterative execution step: check the distance from the current node to each adjacent node, select the adjacent node with the smallest distance and remove it from the priority queue; if the node removed from the priority queue is the end node, end the iteration and enter step S123;

[0155] The check passes the distance from the current node to each adjacent node, specifically:

[0156] Calculate the distance from the current node to the adjacent node;

[0157] If the distance is less than the distance from the starting node to the adjacent node stored in the current distance vector, then update the distance vector, update the optimal predecessor node of the adjacent node in the predecessor vector, and add the adjacent node to the priority queue;

[0158] Step S122 is an iterative processing stage, which realizes the gradual discovery and update of the shortest path by selecting the node with the current shortest distance.

[0159] S123 , tracing back from the end node to the start node through the precursor vector to generate an optimal path; generating a path node mapping relationship, a speed mapping relationship, and a heading mapping relationship based on the optimal path.

[0160] Step S123 is the path reconstruction phase, which involves a reverse tracing process from the endpoint back to the starting node to form an optimal path. Based on the optimal path, the coordinates of all path nodes from the starting point to the endpoint and their corresponding time points are set and stored in the path node mapping relationship. The speed value corresponding to each path node is set and stored in the speed mapping relationship through a node ID-speed structure. The heading angle corresponding to each path node is set and stored in the heading mapping relationship through a node ID-heading angle structure.

[0161] In this embodiment, the path planning information is represented as ;in, Indicates the path node mapping relationship, Represents the velocity mapping relationship, Represents the heading mapping relationship. The path node mapping relationship stores the coordinate information of all path nodes from the starting point to the end point and their corresponding time points; the speed mapping relationship stores the speed value corresponding to each path node through the node ID-speed structure; the heading mapping relationship stores the heading angle corresponding to each path node through the node ID-heading angle structure. This embodiment can accurately represent each position point on the path through the path node mapping relationship, and record the speed and heading of each node through the speed mapping relationship and the heading mapping relationship to ensure the accuracy of path tracking. In addition, since the mapping relationship uses discrete form to store data, the calculation load is smaller than the curve form during path planning and fine-tuning, and it is more intuitive and easy to manage.

[0162] As a preferred embodiment of the present invention, when the current driving mode is the first driving mode, after generating the ship control instruction according to the deviation value, the method further includes the following steps:

[0163] S313, determining engine thrust based on the ship control command;

[0164] S314: Generate a pushrod position control instruction according to the engine thrust and pushrod position-engine power mapping relationship.

[0165] When switching to the second driving mode, if the actual position of the pushrod assembly does not match the current engine output power, the pushrod position does not reflect the actual engine output power, and the operator's operation may lead to misoperation, which in turn may cause unnecessary dangers such as accidental adjustment of engine power. Step S313 determines the required engine thrust based on the ship control command; then step S314 converts the calculated thrust value into the corresponding pushrod position using the pushrod position-engine power mapping relationship. The pushrod position-engine power mapping relationship can be obtained through experimental analysis. After the pushrod position is determined, a control command is generated to adjust the actual position of the pushrod assembly so that the actual position of the pushrod assembly matches the current engine output power.

[0166] In the first driving mode, this embodiment generates a push rod position control instruction based on the engine thrust and push rod position-engine power mapping relationship to ensure that the push rod position matches the engine power when the driving mode is switched, thereby reducing the risk of misoperation, improving overall safety, and ensuring the stability of the ship's operation.

[0167] As a preferred embodiment of the present invention, when the current driving mode is the second driving mode, after generating the ship control instruction according to the ideal engine thrust, the method further includes the following steps:

[0168] S323: Acquire the current state of the ship, and calculate the position deviation value between the current position of the ship and the target position according to the path node mapping relationship.

[0169] S324: Generate a push rod damping control instruction according to the current speed, current heading and position deviation value of the ship.

[0170] In the second steering mode of this embodiment, the operator can obtain accurate control feedback. When the vessel is sailing at high speed or there is a large deviation from the target position, adjusting the push rod damping coefficient can remind the operator to avoid excessive operation or excessive deviation from the planned path, reducing potential risks caused by misoperation and enhancing the safety of vessel operation. For example, increasing the push rod damping can remind the operator not to make excessive speed attempts or remind the operator to steer within the planned range when there is a large position deviation, thereby reducing potential risks caused by misoperation and enhancing the safety of vessel operation.

[0171] Second, see Figure 3 , the present invention provides a boat navigation control system, including a central processing module, a push rod assembly, a push rod control module, a first input module, a second input module and a host computer;

[0172] The push rod assembly is connected to a push rod control module and a first input module respectively; the first input module is used to obtain first input data and transmit it to a central processing module, wherein the first input data includes push rod position data and grip force sensor data; the push rod control module is used to obtain push rod control instructions and control the push rod assembly according to the push rod control instructions;

[0173] The second input module is connected to the host computer and the central processing module; the second input module is used to obtain the second input data transmitted by the host computer and transmit the second input data to the central processing module; the second input data includes path planning information and channel obstacle information;

[0174] The central processing module generates ship control instructions and push rod control instructions based on the first input data, the second input data and the current driving mode; the driving mode includes a first driving mode and a second driving mode; the push rod control instructions include a push rod position control instruction and a push rod damping control instruction; the path planning information includes a path node mapping relationship, a speed mapping relationship and a heading mapping relationship.

[0175] In this embodiment, the first input module continuously monitors the push rod position data and transmits it to the central processing module in real time; the second input module continuously obtains and updates the path planning information and transmits it to the central processing module in real time; the central processing module dynamically adjusts the ship control instructions and the push rod control instructions; the ship engine adjusts the engine output power in response to the ship control instructions, and the push rod control module adjusts the push rod position and adjusts the push rod damping coefficient in response to the push rod control instructions.

[0176] In this embodiment, the path planning information is represented as ;in, Indicates the path node mapping relationship, Represents the velocity mapping relationship, Indicates the heading mapping relationship;

[0177] The path node mapping relationship stores the coordinate information of all path nodes from the starting point to the end point and their corresponding time points; the speed mapping relationship stores the speed value corresponding to each path node through a node ID-speed structure; and the heading mapping relationship stores the heading angle corresponding to each path node through a node ID-heading angle structure. This embodiment uses the path node mapping relationship to accurately represent each location point on the path, and records the speed and heading of each node through the speed mapping relationship and heading mapping relationship, ensuring the accuracy of path tracking. Furthermore, because the mapping relationship stores data in discrete form, the computational load is lower during path planning and fine-tuning compared to the curve form, making it more intuitive and easier to manage.

[0178] The boat navigation control system of the present invention integrates a central processing module, a push rod assembly, a push rod control module, a first input module, a second input module and a host computer. The central processing module generates ship control instructions and push rod control instructions based on the push rod position data provided by the first input module, the path planning information provided by the second input module and the set driving mode, thereby realizing intelligent ship engine control and push rod control, satisfying the user's driving experience while ensuring the safe operation of the ship.

[0179] In this embodiment, grip force sensor data is acquired via a push rod grip force sensor. Mounted on the push rod assembly, the sensor monitors the driver's grip force on the push rod in real time and transmits this data to a first input module via an electrical connection. The first input module synchronizes the grip force sensor data with push rod position data and transmits the data to a central processing module. A second input module transmits channel obstacle information to the central processing module. Based on the channel obstacle information and grip force sensor data, the central processing module sets the current driving mode.

[0180] In the embodiments provided in this application, it should be understood that the disclosed systems and methods can be implemented in other ways. For example, the system embodiments described above are merely illustrative. For example, the division of the modules is merely a logical function division. In actual implementation, there may be other division methods, such as multiple modules or components can be combined or integrated into another system, or some features can be ignored or not executed. Another point is that the mutual coupling or direct coupling or communication connection shown or discussed can be through some interfaces, indirect coupling or communication connection of modules, which can be electrical, mechanical or other forms.

[0181] The modules described as separate components may or may not be physically separate, and the components shown as modules may or may not be physical modules, that is, they may be located in one place or distributed across multiple network modules. Some or all of the modules may be selected to achieve the purpose of the present embodiment according to actual needs.

[0182] In addition, the functional modules in the various embodiments of the present application may be integrated into a processing module, or each module may exist physically separately, or two or more modules may be integrated into a single module. The above-mentioned integrated modules may be implemented in the form of hardware or software functional modules.

[0183] If the integrated module is implemented in the form of a software functional module and sold or used as an independent product, it can be stored in a computer-readable storage medium. Based on this understanding, the technical solution of the present application, or the part that contributes to the prior art, or all or part of the technical solution can be embodied in the form of a software product. The computer software product is stored in a storage medium and includes several instructions for enabling a computer device (which can be a personal computer, server, or network device, etc.) to execute all or part of the steps of the method described in each embodiment of the present application. The aforementioned storage medium includes: U disk, mobile hard disk, read-only memory (ROM, read-only memory), random access memory (RAM, random access memory), disk or optical disk, and other media that can store program code.

[0184] The foregoing description is intended only to provide specific embodiments of the present invention, which will enable those skilled in the art to understand and implement the present invention. Various modifications to these embodiments will be readily apparent to those skilled in the art, and the general principles defined herein may be implemented in other embodiments without departing from the spirit or scope of the present invention. Therefore, the present invention is not intended to be limited to the embodiments shown herein, but is intended to be accorded the widest scope consistent with the principles and novel features claimed herein.

Claims

1. A method for controlling a boat's navigation, characterized in that: Including steps: Acquire first input data and second input data; the first input data includes putter position data and grip force sensor data; the second input data includes path planning information and channel obstacle information; The path planning information includes a path node mapping relationship; Setting a current driving mode according to the course obstacle information and the grip force sensor data; the current driving mode includes a first driving mode and a second driving mode; generating a ship control instruction according to the first input data, the second input data and the current driving mode; the ship control instruction includes a speed control instruction and a heading control instruction; When the current driving mode is the first driving mode, generating the ship control instruction includes the steps of: Calculate the deviations between the current position, current speed and current heading of the ship and the target position, target speed and target heading respectively according to the path planning information; generating a ship control instruction according to the deviation value; When the current driving mode is the second driving mode, generating the ship control instruction includes the steps of: Determining ideal engine thrust based on the pushrod position data and a pushrod position-engine power mapping relationship; generating ship control instructions according to the ideal engine thrust; Generating the ship control instruction according to the deviation value specifically comprises generating the ship control instruction according to the deviation value and a feasible control input set; Generating the ship control instruction according to the ideal engine thrust is specifically generating the ship control instruction according to the ideal engine thrust and a feasible control input set; Before generating a ship control instruction according to the first input data, the second input data and the current driving mode, the method includes the following steps: Construct the discretized dynamic equations of the ship to obtain the current state of the ship and all control input sets; Calculating a first state set of the vessel in an obstacle-free situation based on the current state and all control input sets; Calculate the current distance between the ship and the obstacle in real time based on the channel obstacle information and current status; If the current distance is less than the first safety distance, calculate the second safety distance based on the channel obstacle information and define the constraint conditions based on the second safety distance; Filtering the state set that meets the constraint conditions in the first state set to obtain a second state set, and obtaining a feasible control input set based on the second state set; Acquiring first input data and second input data includes the steps of: Acquire perception data and terrain data, obtain a topology map based on the perception data and terrain data, and obtain path planning information based on the topology map; Obtaining a topological map based on perception data and terrain data includes the following steps: Acquire perception data and terrain data and perform coordinate system transformation; The terrain data is divided into undetermined open areas by analyzing the connected components, and the environmental key points are identified by Harris corner detection of the terrain data. The open areas are identified based on the environmental key points and the undetermined open areas, and the center points of the open areas are marked as topological nodes. Connect each node to its nearest k nodes through the K-neighbor method to generate the edges of the topological graph; Set the Euclidean distance between nodes as the initial edge weight; obtain channel obstacle information based on the perception data, and adjust the edge weight according to the channel obstacle information; The topology map is updated based on the perception data and terrain data acquired in real time.

2. A method for controlling a boat's navigation according to claim 1, characterized in that: The ship discretized dynamic equation is expressed as: ； in, represents the ship state at the k+1th time step; represents the horizontal coordinate of the ship at the kth time step, represents the ordinate of the ship at the kth time step, represents the ship heading at the kth time step; is the time step length; Represents all control input sets, which include several speeds and steering angles; is the ship speed at the kth time step, is the angular velocity of the ship at the kth time step; , , T represents the execution time of the ship control command.

3. A method for controlling a boat's navigation according to claim 2, characterized in that: The constraints are expressed as: in, represents the horizontal coordinate of the obstacle, Indicates the vertical coordinate of the obstacle; is the preset location distance threshold; is the ship radius; is the obstacle radius, Indicates the second safety distance.

4. A method for controlling a boat's navigation according to claim 1, characterized in that: The path planning information includes a path node mapping relationship, a speed mapping relationship, and a heading mapping relationship; and obtaining the path planning information according to the topological map includes the following steps: S121. Select two topological nodes in the topological graph as the starting node and the ending node, respectively, and create a distance vector and a predecessor vector; the distance vector is used to store the shortest distance from the starting point to each node; the predecessor vector is used to record the optimal predecessor node of each node; construct a priority queue, add the starting node to the priority queue, and set the current node as the starting node; S122, iterative execution step: check the distance from the current node to each adjacent node, select the adjacent node with the smallest distance and remove it from the priority queue; If the node removed from the priority queue is the end node, the iteration ends and proceeds to step S123; S123 , tracing back from the end node to the start node through the precursor vector to generate an optimal path; generating a path node mapping relationship, a speed mapping relationship, and a heading mapping relationship based on the optimal path.

5. The method for controlling a boat navigation according to claim 1, wherein: After generating the ship control instruction according to the deviation value, the method further includes the following steps: determining engine thrust based on the vessel control command; A pushrod position control instruction is generated according to the engine thrust and pushrod position-engine power mapping relationship.

6. A method for controlling a boat's navigation according to claim 1, characterized in that: After generating the ship control instruction according to the ideal engine thrust, the method further includes the following steps: Obtaining the current state of the ship, and calculating the position deviation value between the current position of the ship and the target position according to the path node mapping relationship; Generate push rod damping control instructions according to the current speed, current heading and position deviation value of the ship.

Images