Boat navigation control system and method

Abstract

The invention discloses a ship navigation control system and method, and relates to the technical field of ship control, and the method comprises the steps: obtaining first input data and second input data, the first input data comprising push rod position data and grip sensing data; the second input data comprises path planning information and channel obstacle information; setting a current driving mode according to the channel obstacle information and the grip sensing data; generating a ship control instruction according to the first input data, the second input data and the current driving mode; the ship control instruction comprises a speed control instruction and a course control instruction. According to the scheme, the adaptability of the ship to different driving modes is improved, intelligent ship position control and course control with accurate and real-time response are realized, and safe operation of the ship is ensured while the driving experience of a user is met.

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 choose ships as a form of entertainment and sightseeing. In order to enrich the user's activity experience, ships can provide automatic driving and assisted manual driving modes for users to use. The introduction of the automatic driving mode not only improves the safety of navigation, but also prevents users from feeling tired during long-term navigation; at the same time, the existence of the assisted manual driving mode provides users with more autonomy and fun. However, traditional ship control systems often have problems such as slow response speed, insufficient control accuracy, and poor adaptability to changes in the external environment during mode switching, which makes it difficult to effectively guarantee the navigation safety and stability of ships in complex waterways.

[0003] In response to the above problems, in recent years, some technical solutions have tried to improve the automation level of ships by introducing sensor data fusion, path planning algorithms, etc. These technological advances have improved the safety and navigation capabilities of ships to a certain extent. However, in the scenario of switching between automatic driving and manual driving modes, the existing solutions still have the problem of poor adaptability, especially in complex waterway environments, it is still difficult for ship control to achieve accurate and real-time response. Summary of the invention

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

[0005] The specific technical solutions provided by this application are as follows: In a first aspect, the present invention provides a method for controlling a navigation of a boat, comprising the steps of: 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; 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; 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; When the current driving mode is the first driving mode, generating the ship control instruction comprises 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 comprises the steps of: Determining an ideal engine thrust according to the pushrod position data and a pushrod position-engine power mapping relationship; A ship control command is generated according to the ideal engine thrust.

[0006] As a preferred solution of the present invention, the generating of the ship control instruction according to the deviation value is specifically: generating the ship control instruction according to the deviation value and the feasible control input set; 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.

[0007] 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: Construct the discretized dynamic equations of the ship to obtain the current state of the ship and all control input sets; Calculate 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, the second safety distance is calculated according to the channel obstacle information and the constraint condition is defined according to the second safety distance; A state set satisfying the constraint condition in the first state set is screened to obtain a second state set, and a feasible control input set is obtained according to the second state set.

[0008] As a preferred solution of the present invention, 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.

[0009] As a preferred solution of the present invention, the constraint condition is expressed as: , , ; in, represents the horizontal coordinate of the obstacle, Indicates the vertical coordinate of the obstacle; is the preset position distance threshold; is the ship radius; is the obstacle radius.

[0010] As a preferred solution of the present invention, the step of obtaining the first input data and the second input data comprises the following steps: Acquire perception data and terrain data, acquire a topology map based on the perception data and terrain data, and acquire path planning information based on the topology map.

[0011] As a preferred solution of the present invention, the method of obtaining a topological map based on the perception data and terrain data comprises the following steps: Acquire perception data and terrain data and perform coordinate system transformation; The terrain data is divided into undetermined open areas by connecting components, the environmental key points are identified by Harris corner detection of terrain data, the open areas are identified based on the environmental key points and 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-neighboring node method to generate the edges of the topological graph; Set the Euclidean distance between nodes as the initial edge weight; obtain the waterway obstacle information according to the perception data, and adjust the edge weight according to the waterway obstacle information; Update the topology map based on the perception data and terrain data acquired in real time.

[0012] 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; the path planning information is obtained according to the topological map, including the steps of: S121, select two topological nodes in the topological graph as the starting node and the end node respectively, 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 the process goes to step S123; S123, tracing back from the end node to the start node through the predecessor vector to generate an optimal path; generating a path node mapping relationship, a speed mapping relationship, and a heading mapping relationship according to the optimal path.

[0013] As a preferred solution of the present invention, after generating the ship control instruction according to the deviation value, the step of: determining engine thrust based on the vessel control command; A push rod position control instruction is generated according to the engine thrust and push rod position-engine power mapping relationship.

[0014] As a preferred solution of the present invention, after generating the ship control instruction according to the ideal engine thrust, the step of: 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.

[0015] In a second aspect, 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; The push rod assembly is connected to the push rod control module and the first input module respectively; the first input module is used to obtain first input data and transmit it to the central processing module, the first input data including 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; 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; The central processing module generates ship control instructions and push rod control instructions according to the first input data, the second input data and the current driving mode; the driving mode includes the first driving mode and the second driving mode; the push rod control instructions include the push rod position control instructions and the push rod damping control instructions; the path planning information includes the path node mapping relationship, the speed mapping relationship and the heading mapping relationship.

[0016] Compared with the prior art, the present invention has the following beneficial effects: 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 the first input data and the second input data, including the push rod position data, the grip force sensor data, the path planning information and the waterway obstacle information; By calculating the deviation between the current position, current speed and current heading of the ship and the target state according to the path planning information in the first driving mode, the system can dynamically adjust the speed and heading of the ship to ensure the accuracy of ship control and the stability of heading control; 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.

[0017] 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. BRIEF DESCRIPTION OF THE DRAWINGS

[0018] 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.

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

[0020] Figure 1 A schematic diagram of a process flow of a boat navigation control method provided by an embodiment of the present invention; Figure 2 A schematic diagram of a process for obtaining a feasible control input set provided by an embodiment of the present invention; Figure 3 A schematic structural diagram of a boat navigation control system provided by an embodiment of the present invention. DETAILED DESCRIPTION

[0021] The following will be combined with the drawings in the embodiments of the present invention to clearly and completely describe the technical solutions in the embodiments of the present invention. Obviously, the described embodiments are only part of the embodiments of the present invention, not all of the embodiments. Based on the embodiments of the present invention, all other embodiments obtained by ordinary technicians in this field without creative work are within the scope of protection of the present invention.

[0022] 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 components under a certain specific posture (as shown in the accompanying drawings). If the specific posture changes, the directional indication will also change accordingly.

[0023] In addition, the descriptions of "first", "second", etc. in the present invention are only used for descriptive purposes and cannot 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" and "second" may explicitly or implicitly include at least one of the features. In addition, the technical solutions between the various embodiments can be combined with each other, but they must be based on the ability of ordinary technicians in the field to implement them. When the combination of technical solutions is contradictory or cannot be implemented, it should be deemed that such a combination of technical solutions does not exist and is not within the scope of protection required by the present invention.

[0024] With the rapid development of the global economy and the improvement of people's quality of life, more and more people choose ships as a form of entertainment and sightseeing. In order to enrich the user's activity experience, ships can provide automatic driving and assisted manual driving modes for users to use. The introduction of the automatic driving mode not only improves the safety of navigation, but also prevents users from feeling tired during long-term navigation; at the same time, the existence of the assisted manual driving mode provides users with more autonomy and fun. However, traditional ship control systems often have problems such as slow response speed, insufficient control accuracy, and poor adaptability to changes in the external environment during mode switching, which makes it difficult to effectively guarantee the navigation safety and stability of ships in complex waterways.

[0025] In response to the above problems, in recent years, some technical solutions have tried to improve the automation level of ships by introducing sensor data fusion, path planning algorithms, etc. These technological advances have improved the safety and navigation capabilities of ships to a certain extent. However, in the scenario of switching between automatic driving and manual driving modes, the existing solutions still have the problem of poor adaptability, especially in complex waterway environments, it is still difficult for ship control to achieve accurate and real-time response.

[0026] 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.

[0027] The specific embodiments of the present invention will be described in detail below: First, see Figure 1 The present invention provides a method for controlling a boat navigation, comprising the steps of: S1, obtaining 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; 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 stores data in a discrete form.

[0028] 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; In this embodiment, setting the current driving mode according to the channel obstacle information and the grip force sensor data specifically means 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.

[0029] 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, then the current driving mode is switched to the second driving mode, giving priority to responding to the driver's operation intention.

[0030] After the grip strength data is lower than 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.

[0031] In this embodiment, when the system recognizes that the driver's grip strength is low and the path planning information is normal, the system maintains or enters the first driving mode, generates ship control instructions and position control instructions to achieve unmanned driving or high-level assisted driving with less driver intervention. In the second driving mode, when the system recognizes that the driver's grip strength has increased significantly or the path planning information is abnormal, it is determined that the driver needs to intervene or has control intentions, switches to the second driving mode, responds to the driver's operations first, and adjusts and generates relevant instructions.

[0032] S3, 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; Ship control instructions include speed control instructions and heading control instructions; When the current driving mode is the first driving mode, generating the ship control instruction comprises the steps of: S311, calculating the deviation values ​​of the current position, current speed and current heading of the ship from the target position, target speed and target heading respectively according to the path planning information; S312: Generate a ship control instruction according to the deviation value.

[0033] 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.

[0034] Step S311 uses the path planning information, and the system can 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, which are the basis for adjusting the control instructions, allowing the system to quickly perceive the deviation.

[0035] 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, heading deviation); specifically, to generate ship control instructions, a PID controller can be used to generate control instructions based on the deviation values, and by setting the proportional (P), integral (I), and differential (D) parameters, it can effectively respond to deviation changes to dynamically adjust the speed and heading of the ship.

[0036] When the current driving mode is the second driving mode, generating the ship control instruction comprises the steps of: S321, determining an ideal engine thrust according to the push rod position data and a push rod position-engine power mapping relationship; S322. Generate a ship control instruction according to the ideal engine thrust.

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

[0038] Step S321 determines the corresponding ideal engine thrust through the push rod position data and the push rod position-engine power mapping relationship obtained by the first input module; the push rod position-engine power mapping relationship in the first driving mode described above can be obtained through experimental analysis. Then, step S322 generates a ship control instruction based on the ideal engine thrust.

[0039] 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 the first input data and the second input data, including the push rod position data, the grip force sensor data, the path planning information and the waterway obstacle information; By setting the current driving mode according to the channel obstacle information and grip 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 strength is low, the system can automatically switch to the automatic driving mode to ensure the stability and safety of the ship's heading control.

[0040] By calculating the deviation between the current position, current speed and current heading of the ship and the target state according to the path planning information in the first driving mode, the system can dynamically adjust the speed and heading of the ship to ensure the accuracy of the ship's position control and the stability of the heading control; 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 position control.

[0041] Through the above steps, this solution improves the adaptability of the ship 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.

[0042] 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; 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.

[0043] 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.

[0044] In the first driving mode, for unexpected obstacles in the waterway, there is no need to regenerate the path planning information. Instead, the ship control instructions are generated according to 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 collision with obstacles.

[0045] In the second driving mode, the central processing module needs to respond to the driver's operating intentions first in normal navigation, but when obstacle avoidance is required, the ship control instructions generated in response to the ideal engine thrust can be adjusted in combination with the feasible control input set to avoid driver operation errors and ensure navigation safety. Therefore, the control instructions generated in combination with the ideal engine thrust and the "feasible control input set" can quickly adjust the ship's state according to the driver's operating intentions, while ensuring the safety of the ship in a complex waterway environment.

[0046] 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: Get a set of feasible control inputs; see Figure 2 , obtaining a feasible control input set specifically includes the following steps: S301, constructing a discretized dynamic equation of the ship to obtain the current state of the ship and all control input sets; In this step, the ship dynamic model is first constructed, which is expressed as: ; 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.

[0047] In order to reduce the amount of calculation, the ship dynamic model is discretized; the ship dynamic model obtained by discretization is expressed as: ; 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; Based on the above, the ship state includes the position, speed and heading of the ship, so the discretized dynamic equation of the ship 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 contain 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; , .

[0048] This step constructs the discretized dynamic equations of the ship to more accurately describe how the ship moves under different control inputs. Obtaining the current state and all control input sets is the basis for prediction and subsequent calculations.

[0049] S302, calculating a first state set of the ship in the absence of obstacles based on the current state and all control input sets; The first state set is all possible states that can be reached in the next step without obstacles, calculated based on the current state of the ship and all possible control inputs. From the current state, the first state set is calculated by considering all control input sets. The first state set is carried out without taking into account the obstacles in the channel, that is, it is calculated purely from the perspective of possible movements.

[0050] S303, calculating the current distance between the ship and the obstacle in real time according to the channel obstacle information and the current state; This step calculates the distance between the ship and the obstacle in real time to ensure that potential obstacles can be discovered and dealt with in a timely manner during navigation. This real-time calculation is the basis for dynamic obstacle avoidance.

[0051] S304: If the current distance is less than the first safety distance, calculate the second safety distance according to the channel obstacle information and define a constraint condition according to the second safety distance; In this step, when the current distance is less than the first safety distance, the second safety distance is calculated and the constraint condition is defined based on it. 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 condition needs to be calculated; the second safety distance is used to define the constraint condition 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 ; The constraints are expressed as: , , ; in, represents the horizontal coordinate of the obstacle, Indicates the vertical coordinate of the obstacle; is the preset position distance threshold; is the ship radius; It should be noted that in this embodiment, for the convenience 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.

[0052] S305, filtering a state set that satisfies a constraint condition in the first state set to obtain a second state set, and obtaining a feasible control input set according to the second state set; The second state set is a subset of the first state set. The second state set is a set of states that meet the constraint conditions selected from the first state set. The selection process is achieved by filtering and verifying the ship state at each time step.

[0053] 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 the safety in emergency situations, thereby enhancing the ship's obstacle avoidance capability.

[0054] As a preferred embodiment of the present invention, the step of obtaining the first input data and the second input data comprises the following steps: Acquire perception data and terrain data, acquire a topology map based on the perception data and terrain data, and acquire path planning information based on the topology map.

[0055] 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 emits 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.

[0056] Terrain data refers to the geographic and spatial information of the environment, which is usually obtained through pre-mapping or from a map database. Specifically, terrain data includes chart data, GIS (Geographic Information System) data and satellite image data; the chart data includes detailed information such as water depth, seabed topography, waterways, port facilities, etc., which are used for safe navigation and route planning; GIS data includes location information such as coastlines, islands, buoys, and beacons, providing comprehensive environmental geographic information, which is helpful for route planning and obstacle avoidance; satellite image data is images taken by remote sensing satellites to provide a wide range of geographic information for map updates and environmental monitoring.

[0057] Furthermore, the obtaining of a topological map according to the perception data and the terrain data comprises the steps of: S111, acquiring perception data and terrain data and performing coordinate system transformation processing; 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 according to the environmental key points and the undetermined open area, and marking the center point of the open area as a topological node; In this embodiment, the terrain data is represented in the form of a grid (raster) or vector. Before the interconnected component analysis, the terrain data is subjected to denoising and threshold segmentation; denoising is used to eliminate noise in the data to ensure the accuracy of the analysis; threshold segmentation is used to segment the terrain data into open areas and non-open areas. For example, an area with a height below a set threshold is defined as a pending open area.

[0058] 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, which is suitable for finding corner points in high-change areas in images. Specifically, Harris corner detection includes the following steps: grayscale processing of the image; calculating the gradient of the image to find areas with obvious changes; using the Harris response function to calculate the response value of each pixel; setting a threshold to filter out points with higher response values, which are recorded as key points in the environment.

[0059] Furthermore, through the environmental key points, the open area can be further accurately demarcated in the pending open area. The edge of an open area is formed by continuous key points. Based on the environmental key points, the initially identified pending open area can be verified to ensure that its boundary is consistent with the identified environmental key point features. If a conflicting or inaccurate boundary is found, it can be adjusted and corrected through the information of the environmental key points.

[0060] Connected component analysis is used to identify interconnected parts of an image. In terrain data, connected component analysis can help identify multiple independent open areas. Connected component analysis is specifically: traversing the grid of terrain data through depth-first search (DFS) or breadth-first search (BFS), marking all connected open areas; assigning a unique label to each independent open area for easy identification and distinction.

[0061] 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: ; 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.

[0062] S113, connecting each node to its nearest k nodes by using the K-neighboring node method to generate edges of a topological graph; Specifically, each node is connected to its nearest k nodes through the K-neighboring node method, including the following steps: For each node i, calculate the Euclidean distance between it and all other nodes; for each node, sort the calculated distances from small to large and select the first k nearest neighbors; connect each node i with its k nearest neighbor nodes to form the edges of the topological graph. For example, for each node i, form an adjacency list containing its k neighbors.

[0063] S114, setting the Euclidean distance between nodes as the initial edge weight; obtaining channel obstacle information according to the sensing data, and adjusting the edge weight according to the channel obstacle information; During the implementation, if there is an obstacle on an edge (i.e., path), the weight of the edge can be increased or set to infinity (indicating an infeasible path); if there is no obstacle on the path, the original Euclidean distance weight remains unchanged; in this step, different degrees of weight adjustment can be set according to the complexity and density of the obstacles. For example, the edge weight of the high-density obstacle interval can be increased more.

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

[0065] Step S115 updates the topology map according to 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 data based on the latest perception data and terrain data, identifies environmental changes, including newly added obstacles, removed obstacles, and terrain changes; identifies nodes that need to be added for new scenarios (such as newly discovered traversable areas or newly appeared obstacles); uses the K-neighboring node method to find the neighboring nodes of the new nodes, establishes new edge relationships, and considers the channel obstacle information to adjust the weights of the newly added edges; then, locates and determines the topological 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.

[0066] 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 steps of: S121, select two topological nodes in the topological graph as the starting node and the end node respectively, 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; Step S121 is the initialization phase, which ensures the initial state definition of each node and the initialization state of the priority queue.

[0067] 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; The check passes through the distance from the current node to each adjacent node, specifically: Calculate the distance from the current node to the adjacent node; If the distance is less than the distance from the starting node to the adjacent node stored in the current distance vector, the distance vector is updated, and the optimal predecessor node of the adjacent node in the predecessor vector is updated, and the adjacent node is added to the priority queue; 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.

[0068] S123, tracing back from the end node to the start node through the predecessor vector to generate an optimal path; generating a path node mapping relationship, a speed mapping relationship, and a heading mapping relationship according to the optimal path.

[0069] Step S123 is the path reconstruction stage, which is a reverse tracing process from the end node back to the start node to form an optimal path. Based on the optimal path, the coordinate information of all path nodes from the start point to the end point 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 the node ID-speed structure, and the heading angle corresponding to each path node is set and stored in the heading mapping relationship through the node ID-heading angle structure.

[0070] In this embodiment, the path planning information is represented as ;in, Represents 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.

[0071] 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 step further includes: S313, determining engine thrust based on the ship control instruction; S314, generating a push rod position control instruction according to the engine thrust and push rod position-engine power mapping relationship.

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

[0073] In the first driving mode, this embodiment generates a push rod position control instruction according to 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.

[0074] 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 step further includes: S323, 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.

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

[0076] In the second driving mode of this embodiment, the operator can obtain accurate control feedback. When the ship is sailing at high speed or there is a large deviation from the target position, adjusting the damping coefficient of the push rod can remind the driver to avoid excessive operation or excessive deviation from the planned path, reduce the potential risks caused by misoperation, and enhance the safety of ship operation. For example, increasing the damping of the push rod can remind the operator not to make a posture of excessive speed, or remind the operator to drive within the planned range when the position deviation is large, so as to reduce the potential risks caused by misoperation and enhance the safety of ship operation.

[0077] 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; The push rod assembly is connected to the push rod control module and the first input module respectively; the first input module is used to obtain first input data and transmit it to the central processing module, the first input data including 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; 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; The central processing module generates ship control instructions and push rod control instructions according to the first input data, the second input data and the current driving mode; the driving mode includes the first driving mode and the second driving mode; the push rod control instructions include the push rod position control instructions and the push rod damping control instructions; the path planning information includes the path node mapping relationship, the speed mapping relationship and the heading mapping relationship.

[0078] 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.

[0079] In this embodiment, the path planning information is represented as ;in, Represents the path node mapping relationship, Represents the velocity mapping relationship, Indicates the heading mapping relationship; Among them, 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, which is more intuitive and easy to manage.

[0080] 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 according to 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.

[0081] In this embodiment, the grip force sensing data is obtained by a push rod grip force sensor. The push rod grip force sensor is installed on the push rod assembly, and is used to monitor the driver's grip force sensing data on the push rod in real time, and transmit the data to the first input module through an electrical connection. The first input module synchronizes the grip force sensing data with the push rod position data and transmits them to the central processing module, and the second input module transmits the channel obstacle information to the central processing module; in response to the channel obstacle information and the grip force sensing data, the central processing module sets the current driving mode.

[0082] In the embodiments provided in the present 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 only schematic. For example, the division of the modules is only a logical function division. There may be other division methods in actual implementation, 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, and the indirect coupling or communication connection of the modules can be electrical, mechanical or other forms.

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

[0084] In addition, each functional module in each embodiment of the present application can be integrated into a processing module, or each module can exist physically separately, or two or more modules can be integrated into one module. The above integrated modules can be implemented in the form of hardware or software functional modules.

[0085] If the integrated module is implemented in the form of a software function 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 is essentially 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, including several instructions to enable 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.

[0086] The foregoing is merely a specific embodiment of the present invention, which enables those skilled in the art to understand or implement the present invention. Various modifications to these embodiments will be 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 will not be limited to the embodiments shown herein, but rather to 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: Includes 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 channel obstacle information and the grip force sensor data; the current driving mode includes a first driving mode and a second driving mode; 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; When the current driving mode is the first driving mode, generating the ship control instruction comprises 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 comprises the steps of: Determining an ideal engine thrust according to the pushrod position data and a pushrod position-engine power mapping relationship; A ship control command is generated according to the ideal engine thrust.

2. A method for controlling boat navigation according to claim 1, characterized in that: 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; 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.

3. A method for controlling boat navigation according to claim 2, characterized in that: Before generating the ship control instruction according to the first input data, the second input data and the current driving mode, the method comprises the following steps: Construct the discretized dynamic equations of the ship to obtain the current state of the ship and all control input sets; Calculate 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, the second safety distance is calculated according to the channel obstacle information and the constraint condition is defined according to the second safety distance; A state set satisfying the constraint condition in the first state set is screened to obtain a second state set, and a feasible control input set is obtained according to the second state set.

4. A method for controlling boat navigation according to claim 3, 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.

5. A method for controlling boat navigation according to claim 4, 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 position distance threshold; is the ship radius; is the obstacle radius.

6. A method for controlling boat navigation according to claim 1, characterized in that: The step of obtaining the first input data and the second input data comprises the following steps: Acquire perception data and terrain data, acquire a topology map based on the perception data and terrain data, and acquire path planning information based on the topology map.

7. A method for controlling boat navigation according to claim 6, characterized in that: The method of obtaining a topological map according to the perception data and the terrain data comprises the following steps: Acquire perception data and terrain data and perform coordinate system transformation; The terrain data is divided into undetermined open areas by connecting components, the environmental key points are identified by Harris corner detection of terrain data, the open areas are identified based on the environmental key points and 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-neighboring node method to generate the edges of the topological graph; Set the Euclidean distance between nodes as the initial edge weight; obtain the waterway obstacle information according to the perception data, and adjust the edge weight according to the waterway obstacle information; Update the topology map based on the perception data and terrain data acquired in real time.

8. A method for controlling boat navigation according to claim 7, characterized in that: The path planning information includes a path node mapping relationship, a speed mapping relationship, and a heading mapping relationship; the path planning information is obtained according to the topological map, including the steps of: S121, select two topological nodes in the topological graph as the starting node and the end node respectively, 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 the process goes to step S123; S123, tracing back from the end node to the start node through the predecessor vector to generate an optimal path; generating a path node mapping relationship, a speed mapping relationship, and a heading mapping relationship according to the optimal path.

9. A method for controlling boat navigation according to claim 1, characterized in that: After generating the ship control instruction according to the deviation value, the step further includes: determining engine thrust based on the vessel control command; A push rod position control instruction is generated according to the engine thrust and push rod position-engine power mapping relationship.

10. A method for controlling boat 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