Cable type underwater robot path planning method and system for lake underwater plant shooting task

Abstract

The invention discloses a cabled underwater robot path planning method and a cabled underwater robot path planning system for a lake underwater plant shooting task. The method comprises the following steps: acquiring position information of an underwater robot in real time; based on the position information, determining the position of an obstacle in the surrounding environment of the underwater robot; path planning is carried out based on the obstacle position, cost calculation is carried out on all paths after path planning is completed, and the path with the lowest cost is selected as the final path. According to the method, the robot tends to approach the obstacle to avoid the obstacle during obstacle avoidance, so that the obstacle information can be shot in the obstacle avoidance process, and the method is more suitable for an automatic underwater shooting task; meanwhile, compared with other path planning based on sampling, the path planning in the invention not only ensures the path quality, but also ensures the planning speed.

Description

Technical Field

[0001] The present invention relates to the field of underwater positioning, and in particular to a path planning method and system for a cable-type underwater robot used for photographing underwater plants in lakes. Background Art

[0002] The 21st century marks humanity's foray into the underwater realm. Underwater environments, such as oceans and lakes, are valuable resources and high-tech areas that have yet to be fully developed. They have become a key strategic focus for countries around the world and a hotbed of international competition. There are many types of underwater robots, the three most important of which are manned underwater vehicles (HOVs), autonomous underwater vehicles (AUVs), and tethered underwater vehicles (ROVs). Compared to aerial and land-based operations, underwater operations present several challenges: underwater positioning is more challenging, with GPS failing underwater and requiring the use of alternative positioning methods. Due to the harsh underwater environment, poor illumination, and turbidity, conventional lidar and millimeter-wave radars are largely inoperable, while sonar suffers from slower scanning speeds compared to these two types of radar. Due to the complex underwater environment and the abundance of aquatic vegetation, underwater robots cannot be made too large, limiting the accuracy of their inertial sensors. Furthermore, due to the influence of water currents, the measured acceleration information can be noisy, making state estimation difficult for long-distance movements.

[0003] Currently, there are two main types of path planning systems used for underwater robot operations. The first uses large-scale, high-precision sonar to pre-scan the underwater terrain and save the map information. When the underwater robot is given a navigation task, this map is used for global path planning, generating a global path for the robot to refer to. The second method does not pre-scan the underwater terrain, but installs an underwater positioning device on the robot. When the robot is given a task, a string of position data, such as relative coordinates or longitude and latitude information, is sent. The robot then proceeds underwater in an exploratory manner, moving from one coordinate point to another. As it moves toward the coordinate point, it uses its own sensor information for local path planning to avoid obstacles and achieve certain mission requirements. Summary of the Invention

[0004] Most of the current path planning algorithms are more inclined to obstacle avoidance and cannot take into account data collection tasks during the obstacle avoidance process. The present invention installs an underwater positioning device on the robot, issues task points to the robot, and performs a search-based local path planning algorithm during the movement of each point. However, during the local path planning process, the robot tends to move around obstacles, and optimizes the path after generating it to reduce the impact of acceleration sensor noise on robot movement. Multiple paths will be generated and the optimal path will be selected based on various indicators.

[0005] To achieve the above objectives, the present invention provides a path planning method for a tethered underwater robot for photographing underwater plants in lakes, the steps comprising:

[0006] Collect the location information of the underwater robot in real time;

[0007] Determining the locations of obstacles in the environment surrounding the underwater robot based on the location information;

[0008] Based on the obstacle position, path planning is performed, and after the path planning is completed, the cost of all paths is calculated, and the one with the lowest cost is selected as the final path.

[0009] Preferably, the position of the obstacle is acquired using sonar technology, and the sonar coordinates are transformed into robot coordinates:

[0010] P robot =RP sonar +T,

[0011] Among them, P robot represents the robot coordinates; R represents the rotation matrix; P sonar represents the sonar coordinate; T represents the translation amount.

[0012] Preferably, the RRT-connect algorithm is used for path planning, and the steps include:

[0013] Two random trees are generated from the starting point and the end point respectively, and the two random trees are alternately expanded to explore the space; each time a point is randomly sampled from the free space, and the sampled point is connected to the tree node closest to it through the step size limit, and an attempt is made to expand the random tree; if the extension is successful, another random tree is tried to be connected to the new node. When the two trees are successfully connected, a path from the starting point to the end point is found.

[0014] Preferably, after the path planning is completed, the path is optimized and resampling is performed to further optimize the optimized path. After multiple paths are planned, the costs of all paths are calculated and the one with the lowest cost is selected as the final path.

[0015] Preferably, the cost calculation step includes:

[0016]

[0017] Where N is the number of path points; L is the path length; M is the number of beacon points in the path; the subscript max indicates the maximum; w1, w2, and w3 are all project coefficients.

[0018] The present invention also provides a cable-type underwater robot path planning system for the task of photographing underwater plants in lakes. The system is used to implement the above method and includes: an acquisition module, a sonar module and a planning module;

[0019] The acquisition module is used to collect the location information of the underwater robot in real time;

[0020] The sonar module is used to determine the location of obstacles in the environment surrounding the underwater robot based on the position information;

[0021] The planning module is used to perform path planning based on the obstacle position, and after the path planning is completed, calculate the cost of all paths and select the one with the lowest cost as the final path.

[0022] Preferably, the workflow of the sonar module includes:

[0023] Sonar technology is used to obtain the position of the obstacle and transform the sonar coordinates into robot coordinates:

[0024] P robot =RP sonar +T,

[0025] Among them, P robot represents the robot coordinates; R represents the rotation matrix; P sonar represents the sonar coordinate; T represents the translation amount.

[0026] Preferably, the planning module adopts the RRT-connect algorithm for path planning, and the process includes: generating two random trees from the starting point and the end point respectively, and alternately expanding the two random trees to explore the space; randomly sampling a point from the free space each time, connecting the sampling point to the nearest tree node through a step size limit, and trying to expand the random tree; if the extension is successful, try to connect another random tree to the new node. When the two trees are successfully connected, a path from the starting point to the end point is found.

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

[0028] The method of the present invention will make the robot tend to stay close to obstacles when avoiding them, so that obstacle information can be captured during the obstacle avoidance process, making it more suitable for automatic underwater photography tasks; at the same time, compared with other sampling-based path planning, the path planning in the present invention can ensure both path quality and planning speed. BRIEF DESCRIPTION OF THE DRAWINGS

[0029] In order to more clearly illustrate the technical solution of the present invention, the following briefly introduces the drawings required for use in the embodiments. Obviously, the drawings described below are only some embodiments of the present invention. For ordinary technicians in this field, other drawings can be obtained based on these drawings without paying any creative work.

[0030] Figure 1 A diagram of a robot model according to an embodiment of the present invention;

[0031] Figure 2 This is a diagram of the advancement layout of an embodiment of the present invention;

[0032] Figure 3 This is a structural diagram of the control part of an embodiment of the present invention;

[0033] Figure 4 This is an overall framework diagram of the software system according to an embodiment of the present invention;

[0034] Figure 5 This is a flow chart of path planning according to an embodiment of the present invention;

[0035] Figure 6 This is an example of path planning according to an embodiment of the present invention. DETAILED DESCRIPTION

[0036] 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. Based on the embodiments of the present invention, all other embodiments obtained by ordinary technicians in this field without making creative efforts are within the scope of protection of the present invention.

[0037] In order to make the above-mentioned objects, features and advantages of the present invention more obvious and easy to understand, the present invention is further described in detail below with reference to the accompanying drawings and specific embodiments.

[0038] Example 1

[0039] This embodiment provides a path planning method for a tethered underwater robot for a lake underwater plant photography task, comprising the following steps:

[0040] S1. Collect the location information of the underwater robot in real time.

[0041] The underwater robot of this embodiment is as follows Figure 1 As shown in the figure, the hardware part mainly includes the power part and the control part, as well as external sensors not shown in the above figure, such as sonar and underwater positioning.

[0042] like Figure 2As shown, this embodiment uses a six-propeller layout with three positive propellers and three negative propellers. The middle propellers No. 5 and No. 6 are responsible for floating and diving, and the other propellers No. 1-4 are responsible for horizontal movement and steering.

[0043] like Figure 3 As shown, this is the control part of the underwater robot. The control of the robot's propeller motor is mainly completed by the flight control. The flight control used in this embodiment is PIXHAWK4 (PX4). The Raspberry Pi is responsible for data processing and sending instructions to the flight control for motion control, including path planning algorithms, video and robot data uploading, sonar control and data processing, etc. The Raspberry Pi used in the present invention is Raspberry Pi 3B+. The Raspberry Pi network port is connected to the industrial 5G router on the shore through an umbilical cable to achieve network connection.

[0044] The software architecture of the robot is as follows Figure 4 As shown, the software is mainly divided into two parts according to its functions:

[0045] (1) Video upload part: Use the Gstreamer tool to encode the data of the video device and upload it to the server.

[0046] (2) Flight control part: The system used by the flight control is the Ardusub system developed based on the ArduPilot system. This system is developed for underwater robots and can realize thruster control, depth control, and navigation. The flight control is connected to the Raspberry Pi via USB and then processed by the MAVproxy tool. MAVproxy is very powerful and can realize communication between the flight control and the UDP or TCP port through the MavLink protocol to realize the sending of instructions and parameter acquisition to the flight control. In this invention, only the flight control is connected to the local 14550 UDP port of the Raspberry Pi.

[0047] The underwater robot is equipped with an underwater positioning system, which returns the longitude and latitude of the robot's current location (lon1, lat1). Assuming the coordinates of the target point are (lon2, lat2), the two points are converted from the longitude and latitude coordinate system to the Mercator coordinate system. The calculation process is as follows:

[0048] x=R*lon,

[0049] y=R1*log(tan(π / 4+lat / 2)),

[0050] Where x and y represent the coordinates in the Mercator coordinate system of latitude and longitude (lon, lat), and R1 represents the radius of the earth.

[0051] The robot's heading can be calculated as:

[0052]

[0053] Among them, θ is the angle between the robot and the north direction, and the east direction is the positive direction; (x1, y1) and (x2, y2) represent two different position coordinates of the robot.

[0054] S2. Determine the locations of obstacles in the environment surrounding the underwater robot based on the position information.

[0055] The sonar used in this example is a scanning sonar. Its operating principle is to send sound pulses into the water and record the echoes. Each sound pulse returns an echo when it reflects off an object in front of it. Key data returned by the sonar includes the scanning angle, an array of echo strengths, and the time interval between signal strength samples. This data, combined with the speed of sound in water, can be used to calculate the echo strength at a given distance and within a given scanning angle.

[0056] After acquiring the sonar data, it is necessary to transform the sonar data from the sonar coordinate system to the robot coordinate system:

[0057] P robot =RP sonar +T,

[0058] Among them, P robot represents the robot coordinates; R represents the rotation matrix; P sonar represents the sonar coordinate; T represents the translation amount.

[0059] S3. Based on the location of the obstacle, path planning is performed. After the path planning is completed, the cost of all paths is calculated and the path with the lowest cost is selected as the final path.

[0060] like Figure 5 As shown in the figure, after acquiring echo data from a sonar scan, the obstacle edges are expanded based on the robot's radius. This ensures that the robot will not collide with obstacles during subsequent path planning, reduces the complexity of obstacles detected by the sonar scan, and simplifies collision detection during subsequent path planning. Based on the calculated heading, the robot checks whether there is a directly accessible edge point within a 90-degree range of the heading. If there is, the edge point is selected and a command is sent directly to the PX4 to move to it. If there is no edge point, the robot selects the closest edge point to the heading that is not above an obstacle. After edge expansion, multiple obstacles may merge, significantly reducing their number and simplifying their distribution. A breadth-first search can then be used to count the number of obstacles within the scan range. If the number is greater than a certain value, the scene is complex and multiple paths need to be planned. Otherwise, a single path is required.

[0061] This embodiment uses the RRT-connect algorithm for path planning. RRT-connect is a variant of the RRT algorithm and is a bidirectional path planning algorithm. It generates two random trees (the starting tree and the target tree) from the starting point and the end point respectively, and alternately expands the two trees to explore the space. Each time, a point is randomly sampled from the free space (in this embodiment, the position of the last node of the growth of another tree is added as a reference when implementing the random sampling of the algorithm, so that the direction of random sampling is more inclined to this point), and the sampling point is connected to the tree node closest to it through a step size limit, and an attempt is made to expand the tree. If the expansion is successful, an attempt is made to connect another tree to the new node. If the two trees are successfully connected, a path from the starting point to the end point is found. RRT-connect optimizes the efficiency of the RRT algorithm. Its bidirectional generation strategy and the greedy strategy in the search tree generation process make its planning speed faster. Although its greedy strategy tends to generate longer straight paths when there are fewer obstacles, the path generated near obstacles may still be more complicated due to the algorithm's random sampling strategy. In an environment such as water flow and buoyancy that can cause high noise in the acceleration sensor, when the robot executes an overly complex path, the state estimated route will drift significantly compared to the actual motion route.

[0062] Since RRT-connect may be too complex near obstacles, this embodiment will optimize the path after performing RRT-connect path planning, and further optimize the optimized path by resampling. Its path optimization strategy uses the idea of ​​path optimization in RRT*-smart to traverse the path from the target point forward, and according to the principle that the sum of the two sides of a triangle is greater than the third side, it cuts off the redundant target points in the path, straightens the path, and marks the beacon points. The specific process is as follows: Figure 6 Path optimization in .

[0063] Once the initial path is determined, the system starts from the goal and directly connects to the node ahead of it until a collision is detected or the cost becomes too high. Then, starting from the stopped node, it directly connects to the node ahead of it, and repeats this process. If the path cost is lower, the path tree is updated. This directly connected node serves as a beacon node. During resampling, samples are collected within a certain radius of the beacon point and the shortest path optimization strategy used in RRT* is used to further optimize the path.

[0064] After planning multiple paths, the cost of all paths is calculated. The factors considered in the cost calculation are the number of path points (which can reflect the complexity of the path), the length of the path, and the proportion of beacon points in the path points (which can reflect the frequency of the path bypassing obstacles. Since the robot's task is to explore and shoot, the larger the proportion, the better). The path with the lowest cost is selected and sent to the path sending module. The cost calculation formula is as follows:

[0065]

[0066] Where N is the number of path points; L is the path length; M is the number of beacon points in the path; the subscript max indicates the maximum; w1, w2, and w3 are all project coefficients.

[0067] Finally, a connection is established with PX4, and the received path is encapsulated into a planning task message in the Mavlink protocol and sent to PX4 to guide the robot movement.

[0068] Example 2

[0069] This embodiment also provides a cable-type underwater robot path planning system for the task of photographing underwater plants in lakes, including: an acquisition module, a sonar module and a planning module; the acquisition module is used to collect the location information of the underwater robot in real time; the sonar module is used to determine the location of obstacles in the environment around the underwater robot based on the location information; the planning module is used to plan the path based on the obstacle location, and after the path planning is completed, calculate the cost of all paths and select the one with the lowest cost as the final path.

[0070] The following will describe in detail how the present invention solves technical problems in real life in conjunction with this embodiment.

[0071] First, the acquisition module is used to collect the location information of the underwater robot in real time.

[0072] The underwater robot of this embodiment is as follows Figure 1 As shown in the figure, the hardware part mainly includes the power part and the control part, as well as external sensors not shown in the above figure, such as sonar and underwater positioning.

[0073] like Figure 2 As shown, this embodiment uses a six-propeller layout with three positive propellers and three negative propellers. The middle propellers No. 5 and No. 6 are responsible for floating and diving, and the other propellers No. 1-4 are responsible for horizontal movement and steering.

[0074] like Figure 3As shown, this is the control part of the underwater robot. The control of the robot's propeller motor is mainly completed by the flight control. The flight control used in this embodiment is PIXHAWK4 (PX4). The Raspberry Pi is responsible for data processing and sending instructions to the flight control for motion control, including path planning algorithms, video and robot data uploading, sonar control and data processing, etc. The Raspberry Pi used in the present invention is Raspberry Pi 3B+. The Raspberry Pi network port is connected to the industrial 5G router on the shore through an umbilical cable to achieve network connection.

[0075] The software architecture of the robot is as follows Figure 4 As shown, the software is mainly divided into two parts according to its functions:

[0076] (1) Video upload part: Use the Gstreamer tool to encode the data of the video device and upload it to the server.

[0077] (2) Flight control part: The system used by the flight control is the Ardusub system developed based on the ArduPilot system. This system is developed for underwater robots and can realize thruster control, depth control, and navigation. The flight control is connected to the Raspberry Pi via USB and then processed by the MAVproxy tool. MAVproxy is very powerful and can realize communication between the flight control and the UDP or TCP port through the MavLink protocol to realize the sending of instructions and parameter acquisition to the flight control. In this invention, only the flight control is connected to the local 14550 UDP port of the Raspberry Pi.

[0078] The underwater robot is equipped with an underwater positioning system, which returns the longitude and latitude of the robot's current location (lon1, lat1). Assuming the coordinates of the target point are (lon2, lat2), the two points are converted from the longitude and latitude coordinate system to the Mercator coordinate system. The calculation process is as follows:

[0079] x=R*lon,

[0080] y=R1*log(tan(π / 4+lat / 2)),

[0081] Where x and y represent the coordinates in the Mercator coordinate system of latitude and longitude (lon, lat), and R1 represents the radius of the earth.

[0082] The robot's heading can be calculated as:

[0083]

[0084] Among them, θ is the angle between the robot and the north direction, and the east direction is the positive direction; (x1, y1) and (x2, y2) represent two different position coordinates of the robot.

[0085] The sonar module then determines the location of obstacles in the underwater robot's surrounding environment based on the location information.

[0086] The sonar used in this example is a scanning sonar. Its operating principle is to send sound pulses into the water and record the echoes. Each sound pulse returns an echo when it reflects off an object in front of it. Key data returned by the sonar includes the scanning angle, an array of echo strengths, and the time interval between signal strength samples. This data, combined with the speed of sound in water, can be used to calculate the echo strength at a given distance and within a given scanning angle.

[0087] After acquiring the sonar data, it is necessary to transform the sonar data from the sonar coordinate system to the robot coordinate system:

[0088] P robot =RP sonar +T,

[0089] Among them, P robot represents the robot coordinates; R represents the rotation matrix; P sonar represents the sonar coordinate; T represents the translation amount.

[0090] Finally, the planning module performs path planning based on the location of obstacles. After the path planning is completed, the cost of all paths is calculated and the one with the lowest cost is selected as the final path.

[0091] like Figure 5 As shown in the figure, after acquiring echo data from a sonar scan, the obstacle edges are expanded based on the robot's radius. This ensures that the robot will not collide with obstacles during subsequent path planning, reduces the complexity of obstacles detected by the sonar scan, and simplifies collision detection during subsequent path planning. Based on the calculated heading, the robot checks whether there is a directly accessible edge point within a 90-degree range of the heading. If there is, the edge point is selected and a command is sent directly to the PX4 to move to it. If there is no edge point, the robot selects the closest edge point to the heading that is not above an obstacle. After edge expansion, multiple obstacles may merge, significantly reducing their number and simplifying their distribution. A breadth-first search can then be used to count the number of obstacles within the scan range. If the number is greater than a certain value, the scene is complex and multiple paths need to be planned. Otherwise, a single path is required.

[0092] This embodiment uses the RRT-connect algorithm for path planning. RRT-connect is a variant of the RRT algorithm and is a bidirectional path planning algorithm. It generates two random trees (the starting tree and the target tree) from the starting point and the end point respectively, and alternately expands the two trees to explore the space. Each time, a point is randomly sampled from the free space (in this embodiment, the position of the last node of the growth of another tree is added as a reference when implementing the random sampling of the algorithm, so that the direction of random sampling is more inclined to this point), and the sampling point is connected to the tree node closest to it through a step size limit, and an attempt is made to expand the tree. If the expansion is successful, an attempt is made to connect another tree to the new node. If the two trees are successfully connected, a path from the starting point to the end point is found. RRT-connect optimizes the efficiency of the RRT algorithm. Its bidirectional generation strategy and the greedy strategy in the search tree generation process make its planning speed faster. Although its greedy strategy tends to generate longer straight paths when there are fewer obstacles, the path generated near obstacles may still be more complicated due to the algorithm's random sampling strategy. In an environment such as water flow and buoyancy that can cause high noise in the acceleration sensor, when the robot executes an overly complex path, the state estimated route will drift significantly compared to the actual motion route.

[0093] Since RRT-connect may be too complex near obstacles, this embodiment will optimize the path after performing RRT-connect path planning, and further optimize the optimized path by resampling. Its path optimization strategy uses the idea of ​​path optimization in RRT*-smart to traverse the path from the target point forward, and according to the principle that the sum of the two sides of a triangle is greater than the third side, it cuts off the redundant target points in the path, straightens the path, and marks the beacon points. The specific process is as follows: Figure 6 Path optimization in .

[0094] Once the initial path is determined, the system starts from the goal and directly connects to the node ahead of it until a collision is detected or the cost becomes too high. Then, starting from the stopped node, it directly connects to the node ahead of it, and repeats this process. If the path cost is lower, the path tree is updated. This directly connected node serves as a beacon node. During resampling, samples are collected within a certain radius of the beacon point and the shortest path optimization strategy used in RRT* is used to further optimize the path.

[0095] After planning multiple paths, the cost of all paths is calculated. The factors considered in the cost calculation are the number of path points (which can reflect the complexity of the path), the length of the path, and the proportion of beacon points in the path points (which can reflect the frequency of the path bypassing obstacles. Since the robot's task is to explore and shoot, the larger the proportion, the better). The path with the lowest cost is selected and sent to the path sending module. The cost calculation formula is as follows:

[0096]

[0097] Where N is the number of path points; L is the path length; M is the number of beacon points in the path; the subscript max indicates the maximum; w1, w2, and w3 are all project coefficients.

[0098] Finally, a connection is established with PX4, and the received path is encapsulated into a planning task message in the Mavlink protocol and sent to PX4 to guide the robot movement.

[0099] The embodiments described above are merely descriptions of preferred embodiments of the present invention and are not intended to limit the scope of the present invention. Without departing from the spirit of the present invention, various modifications and improvements made to the technical solutions of the present invention by persons skilled in the art should fall within the scope of protection defined by the claims of the present invention.

Claims

1. A path planning method for a cable-type underwater robot used for photographing underwater plants in lakes, characterized in that the steps include: Collect the location information of the underwater robot in real time; Determining the locations of obstacles in the environment surrounding the underwater robot based on the location information; Based on the obstacle position, path planning is performed, and after the path planning is completed, the cost of all paths is calculated, and the one with the lowest cost is selected as the final path.

2. The path planning method for a cable-type underwater robot for photographing underwater plants in lakes according to claim 1, characterized in that: Sonar technology is used to obtain the position of the obstacle and transform the sonar coordinates into robot coordinates: P robot =RP sonar +T, Among them, P robot represents the robot coordinates; R represents the rotation matrix; P sonar represents the sonar coordinate; T represents the translation amount.

3. The path planning method for a cable-type underwater robot for photographing underwater plants in lakes according to claim 1, characterized in that: The RRT-connect algorithm is used for path planning. The steps include: Two random trees are generated from the starting point and the end point respectively, and the two random trees are alternately expanded to explore the space; each time a point is randomly sampled from the free space, and the sampled point is connected to the tree node closest to it through the step size limit, and an attempt is made to expand the random tree; if the extension is successful, another random tree is tried to be connected to the new node. When the two trees are successfully connected, a path from the starting point to the end point is found.

4. The path planning method for a cable-type underwater robot for photographing underwater plants in lakes according to claim 3, characterized in that: After the path planning is completed, the path is optimized and re-sampling is performed to further optimize the optimized path. After multiple path planning is performed, the cost of all paths is calculated and the one with the lowest cost is selected as the final path.

5. The path planning method for a cable-type underwater robot for photographing underwater plants in lakes according to claim 4, characterized in that: The cost calculation steps include: Where N is the number of path points; L is the path length; M is the number of beacon points in the path; the subscript max indicates the maximum; w1, w2, and w3 are all project coefficients.

6. A tethered underwater robot path planning system for photographing underwater plants in lakes, the system being used to implement the method according to any one of claims 1 to 5, characterized in that: include: Acquisition module, sonar module and planning module; The acquisition module is used to collect the location information of the underwater robot in real time; The sonar module is used to determine the location of obstacles in the environment surrounding the underwater robot based on the position information; The planning module is used to perform path planning based on the obstacle position, and after the path planning is completed, calculate the cost of all paths and select the one with the lowest cost as the final path.

7. The cable-type underwater robot path planning system for photographing underwater plants in lakes according to claim 6, characterized in that: The workflow of the sonar module includes: Sonar technology is used to obtain the position of the obstacle and transform the sonar coordinates into robot coordinates: P robot =RP sonar +T, Among them, P robot represents the robot coordinates; R represents the rotation matrix; P sonar represents the sonar coordinate; T represents the translation amount.

8. The cable-type underwater robot path planning system for photographing underwater plants in lakes according to claim 6, characterized in that: The planning module uses the RRT-connect algorithm for path planning. The process includes: generating two random trees from the starting point and the end point respectively, and alternately expanding the two random trees to explore the space; randomly sampling a point from the free space each time, connecting the sampled point to the nearest tree node through a step size limit, and attempting to expand the random tree; if the expansion is successful, attempting to connect another random tree to the new node. When the two trees are successfully connected, a path from the starting point to the end point is found.

Images