Cable type underwater robot path planning system and method

Abstract

The invention discloses a cable type underwater robot path planning system and method, and belongs to the technical field of underwater robot path planning, and the method comprises the following steps: obtaining target position information, and obtaining the course of a robot through a path planning algorithm based on the target position information; starting along the obtained course, scanning a target moving range of the robot based on sonar, and obtaining obstacle position information; in combination with the obstacle position information acquired in the current course, performing narrowing processing on the target moving range; and continuing to perform path planning in the narrowed target moving range, and obtaining new heading forward motion again until the target moves out of the range where the obstacle is located and reaches the target position. According to the invention, the path planning tends to approach the obstacle to avoid the obstacle during obstacle avoidance, so that the obstacle information can be shot during the obstacle avoidance process, the method is more suitable for an automatic underwater shooting task, and the path quality and the planning speed are ensured.

Description

Technical Field

[0001] The present invention belongs to the technical field of underwater robot path planning, and in particular relates to a cable-type underwater robot path planning system and method. Background Art

[0002] There are many types of underwater robots, among which the three most important ones are manned underwater vehicles (HOVs), autonomous underwater robots (AUVs) and tethered underwater vehicles (ROVs). Compared with aerial and land operations, underwater operations have the following difficulties: underwater positioning is more difficult, GPS does not work underwater, and other positioning methods are required; due to the harsh underwater environment, poor lighting, and turbid environment, ordinary laser radars and millimeter-wave radars basically do not work underwater, and sonar has the disadvantage of slow scanning speed compared to the above two radars; due to the complex underwater environment and the large number of aquatic plants, the size of underwater robots should not be too large, and the accuracy of inertial sensors of underwater robots will not be too high. In addition, due to the influence of water flow, the noise of the measured acceleration information will be large, which will make it difficult to estimate the state of long-distance movement.

[0003] At present, there are two main types of path planning systems used for underwater robot operations. The first type uses a large-scale, high-precision sonar to scan the underwater terrain in advance and save the map information. When the underwater robot is given a navigation task, the map is used for global path planning to generate a global path for the robot to refer to. The method of using sonar to scan the terrain in advance and then performing global path planning has the following disadvantages: 1. The requirements for sonar accuracy and scanning range are very high, which makes this solution very expensive; 2. The real-time performance is poor and it may take a long time; 3. For dynamic obstacles and changes in terrain caused by water flow, the global map of the robot during operation will not match the actual environment.

[0004] The second method is to install an underwater positioning device for the robot without scanning the underwater terrain in advance. 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 works at one coordinate point after another underwater in an exploratory manner. In the process of going to the coordinate point, it uses its own sensor information for local path planning to achieve obstacle avoidance and achieve certain task requirements. The vision-based obstacle avoidance method has the following disadvantages: 1. Since the video and image processing is required, the computing power of the robot processor is relatively high; 2. In the underwater environment, especially the underwater environment, due to the influence of light and impurities, the image quality will be poor or even impossible to process.

[0005] In addition, general path planning algorithms using sonar have the following disadvantages: they tend to avoid obstacles too much, which will cause the target to be bypassed during shooting; when the environment is complex, more path curves are generated, but under the high-noise acceleration sensor data caused by the underwater environment, the robot's movement will deviate from the target path.

[0006] In summary, the autonomous cruising of underwater robots is underdeveloped in ROVs, and most of the current path planning algorithms are more inclined to obstacle avoidance rather than taking into account data collection tasks during the obstacle avoidance process. In view of the above problems, it is urgent to propose a tethered underwater robot path planning method. Summary of the invention

[0007] In order to solve the above technical problems, the present invention proposes a path planning method for a cable-type underwater robot, installs an underwater positioning device for the robot, issues task points to the robot, performs a search-based local path planning algorithm during the movement of each point, and optimizes the path after generating it to reduce the impact of acceleration sensor noise on robot movement. It also generates multiple paths and selects the optimal path based on various indicators to solve the problems existing in the above-mentioned prior art.

[0008] To achieve the above-mentioned purpose, the present invention provides a cable-type underwater robot path planning system, comprising: a data acquisition module, an obstacle scanning module, a path planning module, a motion control module and a real-time motion module;

[0009] The data acquisition module is used to collect underwater video data;

[0010] The obstacle scanning module is used to scan the target moving range of the robot based on sonar to obtain obstacle position information;

[0011] The path planning module is used to combine underwater video data, obstacle location information, robot location information and target location information to perform local path planning;

[0012] The motion control module is used to send the planned path to the flight controller of the robot, and control the robot to move along the planned path;

[0013] The real-time motion module is used to control the robot's ascent and descent in the vertical direction, as well as its movement and steering in the horizontal direction, using a six-thruster layout.

[0014] Optionally, the six propellers of the real-time motion module include three forward propellers and three reverse propellers.

[0015] Optionally, the path planning module is used to obtain the robot's heading based on the target position information using a path planning algorithm, start along the acquired heading, and reduce the target moving range in combination with the obstacle position information obtained within the current heading. Then, path planning is continued within the reduced target moving range, and a new heading is re-acquired to move forward until the robot moves out of the range of the obstacle and reaches the target position.

[0016] The present invention also provides a cable-type underwater robot path planning method, based on the system, comprising the following steps:

[0017] Acquire target position information, and acquire the robot heading using a path planning algorithm based on the target position information;

[0018] Starting from the acquired heading, the robot scans the target moving range based on sonar to obtain obstacle location information;

[0019] Combined with the obstacle position information obtained within the current heading, the target moving range is reduced;

[0020] Continue to plan the path within the reduced target moving range, reacquire the new heading and move forward until you move out of the range where the obstacle is located and reach the target position.

[0021] Optionally, the process of obtaining target position information and obtaining the robot heading by using a path planning algorithm based on the target position information includes:

[0022] Based on the radius of the earth, the robot's current position coordinates and target position coordinates are converted from the longitude and latitude coordinate system to the Mercator coordinate system, and the robot's heading is obtained in the Mercator coordinate system.

[0023] Optionally, the process of reducing the target moving range in combination with the obstacle position information obtained within the current heading includes:

[0024] The obtained obstacle edge is expanded to determine whether there is a directly accessible edge point within 90 degrees of the current heading. If there is a directly accessible edge point, the robot moves in the direction of the edge point as the new heading. If there is no directly accessible edge point, the edge point closest to the current heading that is not on the obstacle is selected as the new heading for movement. The target movement range of the robot is reduced based on the selected new heading.

[0025] Optionally, before performing the local path planning algorithm, the complexity of the scene is also judged;

[0026] Based on the breadth-first search algorithm, the number of obstacles within the target moving range is counted. When the number of obstacles is greater than the preset threshold, the scene is determined to be complex and several travel paths are planned; when the number of obstacles is less than or equal to the preset threshold, one travel path is planned.

[0027] Optionally, the process of performing path optimization on the planned plurality of travel paths includes:

[0028] Several planned paths are traversed. Based on the path optimization and resampling method in RRT*-Smart and the principle that the sum of two sides of a triangle is greater than the third side, the redundant target points in each path are trimmed, the path is straightened, and resampling is performed in the range near the beacon point to further optimize the path. Finally, the optimized path is obtained.

[0029] Optionally, the process of performing path selection on the optimized path includes:

[0030] Calculate the cost of all optimized paths, obtain the length of each path and sort them, and select the shortest path as the final robot travel path.

[0031] Optionally, the path planning algorithm adopts an RRT-connect algorithm.

[0032] Compared with the prior art, the present invention has the following advantages and technical effects:

[0033] 1. The cost is relatively low;

[0034] 2. Compared with other obstacle avoidance strategies, the path planning in the present invention expands the acquired obstacle edge, determines whether there is a directly accessible edge point within 90 degrees of the current heading, and tends to avoid obstacles close to them, so that obstacle information can be captured during the obstacle avoidance process, which is more suitable for automatic underwater shooting tasks;

[0035] 3. Compared with other sampling-based path planning, the path planning in the present invention can ensure both path quality and planning speed;

[0036] 4. Compared with the artificial potential field method, the path planning in the present invention generates more straight paths and reduces the path complexity, thereby reducing the impact of high noise of the sensor caused by water flow and buoyancy on state estimation when executing the path, and the generated path is more suitable for robots using the AHRS reference system;

[0037] 5. Strong autonomy, no need for excessive human intervention during autonomous cruising;

[0038] 6. Good adaptability, able to carry out underwater operations in the underwater environment of various lakes. BRIEF DESCRIPTION OF THE DRAWINGS

[0039] The drawings constituting a part of the present application are used to provide a further understanding of the present application. The illustrative embodiments and descriptions of the present application are used to explain the present application and do not constitute an improper limitation on the present application. In the drawings:

[0040] Figure 1 is a propeller layout diagram of an embodiment of the present invention;

[0041] Figure 2 is a structural diagram of a control part of an embodiment of the present invention;

[0042] Figure 3 It is the overall framework diagram of the software system of the embodiment of the present invention;

[0043] Figure 4 A flow chart of a path planning algorithm according to an embodiment of the present invention;

[0044] Figure 5 Schematic diagram of path optimization according to an embodiment of the present invention, wherein (a) is the path planning process, (b) is the initial path optimization process, and (c) is the secondary path optimization process. DETAILED DESCRIPTION

[0045] It should be noted that, in the absence of conflict, the embodiments and features in the embodiments of the present application can be combined with each other. The present application will be described in detail below with reference to the accompanying drawings and in combination with the embodiments.

[0046] It should be noted that the steps shown in the flowcharts of the accompanying drawings can be executed in a computer system such as a set of computer executable instructions, and that, although a logical order is shown in the flowcharts, in some cases, the steps shown or described can be executed in an order different from that shown here.

[0047] Embodiment 1.

[0048] In this embodiment, a cable-type underwater robot path planning system is provided, including: a data acquisition module, an obstacle scanning module, a path planning module, a motion control module and a real-time motion module;

[0049] The data acquisition module is used to collect underwater video data;

[0050] The obstacle scanning module is used to scan the robot's target moving range based on sonar to obtain obstacle location information;

[0051] The path planning module is used to combine underwater video data, obstacle location information, robot location information, and target location information to perform local path planning;

[0052] The motion control module is used to send the planned path to the robot's flight controller and control the robot to move along the planned path;

[0053] The real-time motion module is used to control the robot's ascent and descent in the vertical direction, as well as movement and steering in the horizontal direction using a six-thruster layout.

[0054] As a specific implementation method, this embodiment encodes the data of the video device through the tool Gstreamer and uploads it to the server.

[0055] As a specific implementation, the six propellers of the real-time motion module include three positive propellers and three reverse propellers, such as Figure 1 As shown, propellers No. 1, 2, and 5 are reverse propellers, propellers No. 3, 4, and 6 are forward propellers, the middle propellers No. 5 and 6 are responsible for buoyancy and diving, and the other four propellers are responsible for horizontal movement and steering.

[0056] As a specific implementation method, the motion control module is mainly completed by the flight controller (hereinafter referred to as "flight control"). The flight control used in this embodiment is PIXHAWK4. 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 upload, sonar control and data processing, etc. The Raspberry Pi used in this embodiment 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. The structure diagram of the motion control is as follows: Figure 2 As shown, the structure diagram of software control is as follows Figure 3 shown.

[0057] Furthermore, the system used by the flight control is the Ardusub system developed based on the ArduPilot system, which 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, and realize the sending of instructions and parameter acquisition to the flight control. In this embodiment, the flight control is only connected to the local 14550 udp port of the Raspberry Pi.

[0058] As a specific embodiment, the path planning module is used to obtain the robot's heading based on the target position information using a path planning algorithm, start along the obtained heading, and reduce the target moving range in combination with the obstacle position information obtained within the current heading. Then, path planning is continued within the reduced target moving range, and a new heading is re-acquired to move forward until the robot moves out of the range where the obstacle is located and reaches the target position.

[0059] Furthermore, the path planning module connects and communicates with the server, further encapsulates the flight control parameters, and adds some other data such as underwater positioning data, uploads it to the server, and reads the instructions issued by the server and performs corresponding operations. Basic locking and unlocking as well as basic moving operations are completed by this module. When an autonomous cruise or recovery instruction is received, the path planning module is called, and the issued cruise point information is sent to the path planning module.

[0060] Furthermore, the path planning module moves towards the cruise point by acquiring the cruise point information and combining it with the information of the underwater positioning device. The movement process is exploratory movement. When obstacles are found in the front or within a certain radius around it, local path planning is performed based on the sonar information until the target cruise point is reached.

[0061] like Figure 2 As shown, the present embodiment also provides a cable-type underwater robot path planning method, based on the system, comprising the following steps: obtaining target position information, and obtaining the robot heading based on the target position information by using a path planning algorithm; starting along the obtained heading, scanning the target moving range of the robot based on sonar to obtain obstacle position information; reducing the target moving range in combination with the obstacle position information obtained within the current heading; continuing path planning within the reduced target moving range, re-obtaining a new heading and moving forward until moving out of the range where the obstacle is located and reaching the target position.

[0062] Specifically, when receiving the longitude and latitude sequence of the task point from the server, the information processing program will send the first target point to the path planning algorithm in sequence. The path planning algorithm will first calculate the heading of the robot and set off towards the target point. If an obstacle is found within the range, that is, the echo intensity information of the sonar reaches a certain threshold, the sonar accuracy will be improved, the terrain within a small range will be scanned, and the path will be planned within this small range until it moves out of the range, and the heading will continue to be calculated and move forward until the target point is reached. A success message will be sent to the information processing program. The information processing program will check whether there are any task points in the task point sequence. If there are, it will continue to send them to the path planning algorithm. If not, a completion message will be sent to the upper server, and the robot will be asked to turn on the depth-fixing mode and wait in place. The algorithm flow chart is as follows Figure 4 shown.

[0063] As a specific implementation method, the process of obtaining target position information and obtaining the robot's heading using a path planning algorithm based on the target position information includes: based on the radius of the earth, converting the robot's current position coordinates and target position coordinates from a latitude and longitude coordinate system to a Mercator coordinate system, and obtaining the robot's heading in the Mercator coordinate system.

[0064] Specifically, the latitude and longitude of the robot's current location is , assuming the coordinates of the target point , convert the two points from the latitude and longitude coordinate system to the Mercator coordinate system. The calculation process is as follows:

[0065] .

[0066] .

[0067] Where R is the radius of the earth, convert the latitude and longitude points to , , then the robot's heading can be calculated as:

[0068] .

[0069] It is the angle between the robot and the north direction, with the east direction being the positive direction.

[0070] As a specific implementation, the sonar used in this embodiment is a scanning sonar, which works by sending sound pulses into the water and recording echoes. When each sound pulse is reflected from an object in front of it, an echo will return. The important data returned by the sonar include the scanning angle, the echo intensity array, and the time interval of the signal intensity samples. The above data combined with the propagation speed of sound in water can be used to calculate the echo intensity within a certain distance under a scanning angle.

[0071] After obtaining the sonar data, it is necessary to transform the sonar data from the sonar coordinate system to the robot coordinate system. The rotation matrix is , the translation amount is , the conversion formula is as follows:

[0072] .

[0073] As a specific implementation method, the process of reducing the target moving range in combination with the obstacle position information obtained within the current heading includes: expanding the obtained obstacle edge, determining whether there is an edge point that can be directly reached within 90 degrees of the current heading, and when there is an edge point that can be directly reached, moving in the direction of the edge point as the new heading; when there is no edge point that can be directly reached, selecting the edge point that is closest to the current heading and not above the obstacle as the new heading for movement; and reducing the target moving range of the robot based on the selected new heading.

[0074] Furthermore, edge expansion: after obtaining the echo data of a sonar scan, the edge of the obstacle is expanded according to the robot radius. This will ensure that the robot will not touch the obstacle in the subsequent planned path, reduce the complexity of the obstacles scanned by the sonar, and simplify the collision judgment method during subsequent path planning.

[0075] Furthermore, target point selection: check whether there is an edge point that can be reached directly within 90 degrees of the calculated heading. If there is, select the edge point and send a command to PX4 directly to move to the point. If not, select an edge point that is closest to the heading and not above an obstacle.

[0076] As a specific implementation method, before the obtained obstacle edge is expanded, the scene complexity is also judged: Scene complexity judgment: After edge expansion, multiple obstacles may be merged, the number of obstacles will be significantly reduced, and the distribution will become simpler. At this time, the breadth-first search can be used to count the number of obstacles in the scanning range. If it is greater than the preset threshold, it means that the scene is relatively complex and multiple plans are performed during path planning. Otherwise, only one path needs to be planned.

[0077] As a specific implementation method, the RRT-connect algorithm is used for path planning, such as Figure 5 As shown in (a) in the figure, RRT-connect is a variant of the RRT algorithm. It is a bidirectional path planning that 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 very fast. Although its greedy strategy tends to generate a relatively long straight path when there are fewer obstacles, the path generated by the algorithm's random sampling strategy near obstacles may still be relatively complex. In an environment such as water flow and buoyancy that will 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.

[0078] As a specific implementation method, the process of performing path optimization on a plurality of planned travel paths includes:

[0079] Since RRT-connect may be too complex near obstacles, this algorithm will optimize the path after 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 and, according to the principle that the sum of the two sides of a triangle is greater than the third side, cut the redundant target points in the path, straighten the path, and mark the beacon points. The specific process is as follows: Figure 5As shown in (b) in the figure: After the initial path is obtained, it will start from the goal and directly connect to the node in front of it until a collision is detected or the cost becomes high and stops. Then it will start from the stopped node and directly connect to the node in front of it, and repeat this process. If the path cost is lower, the path tree will be updated. This directly connected node will be used as a beacon node. When sampling again, it will be sampled within a certain radius of the beacon point, and the shortest path optimization strategy in RRT* will be used to optimize the path again, as shown in the figure. Figure 5 As shown in (c) in .

[0080] As a specific implementation method, the process of selecting the optimized path includes:

[0081] Calculate the cost of all optimized paths, obtain the length of each path and sort them, and select the shortest path as the final robot travel path. Establish a connection with PX4, encapsulate the received path into a WAYPOINT message in the Mavlink protocol, and send it to PX4 to guide the robot movement.

[0082] The above are only preferred specific implementations of the present application, but the protection scope of the present application is not limited thereto. Any changes or substitutions that can be easily thought of by a person skilled in the art within the technical scope disclosed in the present application should be included in the protection scope of the present application. Therefore, the protection scope of the present application should be based on the protection scope of the claims.

Claims

1. A cable-type underwater robot path planning system, characterized in that: include: Data acquisition module, obstacle scanning module, path planning module, motion control module and real-time motion module; The data acquisition module is used to collect underwater video data; The obstacle scanning module is used to scan the target moving range of the robot based on sonar to obtain obstacle position information; The path planning module is used to combine underwater video data, obstacle location information, robot location information and target location information to perform local path planning; The motion control module is used to send the planned path to the flight controller of the robot, and control the robot to move along the planned path; The real-time motion module is used to control the robot's ascent and descent in the vertical direction, as well as its movement and steering in the horizontal direction, using a six-thruster layout.

2. The system according to claim 1, characterized in that The six propellers of the real-time motion module include three forward propellers and three reverse propellers.

3. The system according to claim 1, characterized in that The path planning module is used to obtain the robot's heading based on the target position information using a path planning algorithm, start along the obtained heading, and reduce the target movement range in combination with the obstacle position information obtained within the current heading, and then continue path planning within the reduced target movement range, re-acquire a new heading and move forward until it moves out of the range where the obstacle is located and reaches the target position.

4. A path planning method for a cable-type underwater robot, characterized in that: The system according to any one of claims 1 to 3 comprises the following steps: Acquire target position information, and acquire the robot heading using a path planning algorithm based on the target position information; Starting from the acquired heading, the robot scans the target moving range based on sonar to obtain obstacle location information; Combined with the obstacle position information obtained within the current heading, the target moving range is reduced; Continue to plan the path within the reduced target moving range, reacquire the new heading and move forward until you move out of the range where the obstacle is located and reach the target position.

5. The method according to claim 4, characterized in that The process of obtaining target position information and obtaining the robot heading by using a path planning algorithm based on the target position information includes: Based on the radius of the earth, the robot's current position coordinates and target position coordinates are converted from the longitude and latitude coordinate system to the Mercator coordinate system, and the robot's heading is obtained in the Mercator coordinate system.

6. The method according to claim 4, characterized in that The process of reducing the target moving range by combining the obstacle position information obtained within the current heading includes: The obtained obstacle edge is expanded to determine whether there is a directly accessible edge point within 90 degrees of the current heading. If there is a directly accessible edge point, the robot moves in the direction of the edge point as the new heading. If there is no directly accessible edge point, the edge point closest to the current heading that is not on the obstacle is selected as the new heading for movement. The target movement range of the robot is reduced based on the selected new heading.

7. The method according to claim 6, characterized in that Before the local path planning algorithm is implemented, the complexity of the scene is also judged; Based on the breadth-first search algorithm, the number of obstacles within the target moving range is counted. When the number of obstacles is greater than the preset threshold, the scene is determined to be complex and several travel paths are planned; when the number of obstacles is less than or equal to the preset threshold, one travel path is planned.

8. The method according to claim 7, characterized in that The process of optimizing several planned travel paths includes: Several planned paths are traversed. Based on the path optimization and resampling method in RRT*-Smart and the principle that the sum of two sides of a triangle is greater than the third side, the redundant target points in each path are trimmed, the path is straightened, and resampling is performed in the range near the beacon point to further optimize the path. Finally, the optimized path is obtained.

9. The method according to claim 8, characterized in that The process of selecting the optimized path includes: Calculate the cost of all optimized paths, obtain the length of each path and sort them, and select the shortest path as the final robot travel path.

10. The method according to claim 4, characterized in that The path planning algorithm adopts the RRT-connect algorithm.

Images