Centering navigation control method and device of underwater robot and underwater robot

Abstract

The invention discloses a central navigation control method and device of an underwater robot, the underwater robot and a computer readable storage medium, and the method comprises the steps: obtaining distance data collected by a distance sensor cluster of the underwater robot when the underwater robot navigates in a target channel, and calculating the central position of the target channel according to the distance data, the distance sensor cluster comprises distance sensors arranged in at least three directions; the distance deviation of the underwater robot relative to the center position of the target channel is determined; and controlling the underwater robot to move to the central position of the target channel according to the distance deviation, and controlling the navigation direction of the underwater robot to deflect towards the central position. According to the central navigation control method and device of the underwater robot, the underwater robot and the computer readable storage medium, the underwater robot can keep central navigation in the target channel, and the navigation safety of the underwater robot is improved.

Description

Technical Field

[0001] The embodiments of the present application relate to the field of system control technology, and in particular to a method and device for controlling the centering navigation of an underwater robot, and an underwater robot. Background Art

[0002] Under the background of the rapid development of global economy and science and technology, the development of marine resources and marine scientific research activities are booming. In this series of activities, underwater robots have become the core equipment, opening up a new way for human beings to explore and utilize marine resources. At present, underwater robots have been applied in deep-sea exploration, marine life research, seabed cleaning and other fields.

[0003] The inventors found that in the prior art, the navigation of underwater robots is mostly controlled in real time by operators through remote controls. However, this control method has some problems when facing special underwater claustrophobic tunnel environments such as water storage area tunnels and water transfer channels of water plants. For example, in such environments, the light conditions are poor, and traditional visual sensors such as optical cameras are difficult to play an effective role, and the visual range and clarity will be greatly limited. Operators are prone to misjudgment of the underwater environment, and may issue wrong navigation instructions, resulting in damage to the underwater robot. Summary of the invention

[0004] The embodiments of the present application provide a method and device for controlling the centering navigation of an underwater robot, an underwater robot, and a computer-readable storage medium, which can enable the underwater robot to maintain centering navigation in a target channel and improve the safety of the underwater robot during navigation.

[0005] An embodiment of the present application provides a method for controlling the centering navigation of an underwater robot, including:

[0006] When the underwater robot navigates in the target channel, distance data collected by a distance sensor cluster of the underwater robot is acquired, and the center position of the target channel is calculated according to the distance data, wherein the distance sensor cluster includes distance sensors arranged in at least three directions;

[0007] Determining a distance deviation of the underwater robot relative to a center position of the target channel;

[0008] The underwater robot is controlled to move to the center position of the target channel according to the distance deviation, and the navigation direction of the underwater robot is controlled to deflect toward the center position.

[0009] Optionally, the distance sensor cluster includes distance sensors arranged in four directions: up, down, left, and right. The calculating the center position of the target channel according to the distance data includes:

[0010] Calculating the lateral center point of the target channel according to the left obstacle distance and the right obstacle distance in the distance data;

[0011] Calculating the longitudinal center point of the target channel according to the upper obstacle distance and the lower obstacle distance in the distance data;

[0012] Determine the central position of the target channel composed of the transverse center point and the longitudinal center point.

[0013] Optionally, before acquiring the distance data collected by the distance sensor cluster of the underwater robot, the method further includes:

[0014] Obtaining the traveled distance of the underwater robot and the navigation speed of the underwater robot;

[0015] If the navigation speed is greater than a preset first threshold, determining the alarm interval time period as a ratio of a preset fixed value to the navigation speed;

[0016] If the navigation speed is less than a preset second threshold, determining the alarm interval time period to be the preset fixed value;

[0017] Real-time monitoring of the real-time change distance of the underwater robot within the alarm interval time period;

[0018] When the real-time change distance reaches the preset alarm distance, an obstacle distance measurement instruction is generated to execute the operation of acquiring the distance data collected by the distance sensor cluster of the underwater robot, and reset the timing starting point of the alarm interval time period;

[0019] When the real-time changing distance does not reach the preset alarm distance, but the alarm interval time period expires, an obstacle distance detection instruction is generated to execute the operation of obtaining the distance data collected by the distance sensor cluster of the underwater robot and reset the timing starting point of the alarm interval time period.

[0020] Optionally, after acquiring the distance data collected by the distance sensor cluster of the underwater robot, the method further includes:

[0021] Obtaining a channel diameter of the target channel, and determining whether the distance of each obstacle in the distance data is greater than the channel diameter;

[0022] The obstacle distance whose distance is greater than the channel diameter is determined as the channel lateral gap vector, and the channel lateral gap vector is deleted from the distance data to obtain updated distance data.

[0023] Optionally, calculating the center position of the target channel according to the distance data includes:

[0024] Obtaining a route traveled by the underwater robot in adjacent unit times;

[0025] Calculating a cross-sectional plane perpendicular to the traveled route based on a center point of the traveled route;

[0026] Mapping the distance data to the cross-sectional plane to obtain a plane range of the navigation environment;

[0027] Detecting each edge irregular area in the plane range of the navigation environment;

[0028] Acquire the maximum circumscribed boundary of the underwater robot, and use the maximum circumscribed boundary to perform traversal tangency operations with each of the irregular edge regions;

[0029] According to the traversal tangency result, the portion of each of the irregular edge regions outside the maximum circumscribed boundary is identified to obtain an inaccessible area, and the inaccessible area in the plane range of the navigation environment is deleted to obtain a navigable area;

[0030] The center of the navigable area is calculated using a clustering algorithm to obtain the center position of the target channel.

[0031] Optionally, after controlling the underwater robot to move to the center position of the target channel according to the distance deviation, and controlling the navigation direction of the underwater robot to deflect toward the center position, the method further includes:

[0032] Obtaining the angular velocity, acceleration and geomagnetic field direction collected by the inertial measurement unit cluster of the underwater robot;

[0033] According to the direction of the geomagnetic field, the coordinate system of the underwater robot is corrected to obtain an initial position and posture;

[0034] Integrating the angular velocity to obtain an angle variation, and subtracting a pre-constructed gravity acceleration from the angular velocity to obtain a linear acceleration, wherein the angle variation includes roll, pitch and yaw;

[0035] The position change of the initial position posture is calculated according to the angle change and the linear acceleration to obtain the real-time moving posture.

[0036] Optionally, controlling the underwater robot to move to the center position of the target channel according to the distance deviation includes:

[0037] Acquire the pressure matrix of water flow in all directions of the underwater robot in real time, and calculate the stable dynamic coordination relationship when the underwater robot is stable according to the pressure matrix;

[0038] According to the stable power coordination relationship, power is output to each power device in the power cluster of the underwater robot to maintain the balance of the underwater robot;

[0039] Using an error PD controller, according to the distance deviation, a power enhancement operation based on the distance deviation direction is performed on the stable power coordination relationship to obtain a motion power coordination relationship;

[0040] The propulsion device of the underwater robot is controlled to move the position of the underwater robot to the center position of the target channel according to the motion dynamic coordination relationship.

[0041] The present application also provides a centering navigation control device for an underwater robot, including:

[0042] A collection module, used for acquiring distance data collected by a distance sensor cluster of the underwater robot when the underwater robot navigates in a target channel, and calculating the center position of the target channel according to the distance data, wherein the distance sensor cluster includes distance sensors arranged in at least three directions;

[0043] A determination module, used to determine a distance deviation of the underwater robot relative to a center position of the target channel;

[0044] The control module is used to control the underwater robot to move to the center position of the target channel according to the distance deviation, and control the navigation direction of the underwater robot to deflect toward the center position.

[0045] On the one hand, an embodiment of the present application also provides an underwater robot, including: a memory and a processor; the memory stores executable program code; the processor coupled to the memory calls the executable program code stored in the memory to execute the centering navigation control method of the underwater robot provided in the above embodiment.

[0046] On one hand, an embodiment of the present application further provides a computer-readable storage medium on which a computer program is stored. When the computer program is executed by a processor, the centering navigation control method of the underwater robot provided in the above embodiment is implemented.

[0047] It can be seen from the above embodiments of the present application that when the underwater robot is navigating in the target channel, the distance data collected by the distance sensor cluster of the underwater robot is obtained, and the center position of the target channel is calculated based on the distance data, wherein the distance sensor cluster includes distance sensors arranged in at least three directions; the distance deviation of the underwater robot relative to the center position of the target channel is determined; the underwater robot is controlled to move to the center position of the target channel according to the distance deviation, and the navigation direction of the underwater robot is controlled to deflect toward the center position. By controlling the position of the underwater robot to move to the center position of the target channel and controlling the navigation direction of the underwater robot to deflect toward the center position, the underwater robot can continue to navigate in the center of the target channel, reduce the risk of the underwater robot touching the wall, and improve the safety of the underwater robot during navigation. BRIEF DESCRIPTION OF THE DRAWINGS

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

[0049] Figure 1 A flowchart of a method for controlling the centering navigation of an underwater robot provided in one embodiment of the present application;

[0050] Figure 2 A schematic diagram of the structure of a centering navigation control device for an underwater robot provided in one embodiment of the present application;

[0051] Figure 3 A schematic diagram of the hardware structure of an underwater robot provided in one embodiment of the present application. DETAILED DESCRIPTION

[0052] In order to make the purpose, technical solution and advantages of the embodiments of the present application clearer, the technical solution in the embodiments of the present application will be clearly and completely described below in conjunction with the drawings in the embodiments of the present application. Obviously, the described embodiments are 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.

[0053] See also Figure 1 , Figure 1 This is a flowchart of an implementation method of a navigation control method of an underwater robot provided in an embodiment of the present application. The method described in this embodiment can be applied to an underwater robot. Figure 1 As shown, the method specifically includes:

[0054] Step S11, when the underwater robot navigates in the target channel, obtain distance data collected by the distance sensor cluster of the underwater robot, and calculate the center position of the target channel according to the distance data, wherein the distance sensor cluster includes distance sensors arranged in at least three directions.

[0055] In this embodiment, the underwater robot includes a distance sensor cluster, a DVL sensor and an inertial measurement unit cluster.

[0056] In an optional embodiment, the distance sensor cluster includes 3 or more distance sensors. Each distance sensor is respectively arranged directly above or directly below the underwater robot, and at the lower left, lower right, or upper left, upper right of the underwater robot. Alternatively, the three distance sensors of the underwater robot are arranged perpendicular to each other, and are used to detect obstacle distances in three perpendicular directions.

[0057] In another optional embodiment, the distance sensor cluster includes 4 or more distance sensors. If the number of the distance sensor cluster is 4, each distance sensor is respectively arranged in the upper, lower, left and right directions of the underwater robot to detect the distance between the underwater robot and the target channel in the upper, lower, left and right directions.

[0058] Preferably, the distance sensors on the underwater robot are arranged at intervals of 90 degrees in the four directions of up, down, left and right. Therefore, the acquired obstacle distance vector set may include distance data in the four directions of up, down, left and right.

[0059] In another preferred embodiment, the distance sensor cluster includes a distance sensor group arranged front and back, that is, at least two groups of distance sensors, one group of probes facing the front of the underwater robot, used to detect the distance of the object in front of the underwater robot; the other group of probes facing the rear of the underwater robot, used to detect the distance of the object behind the underwater robot. In addition, each group of distance sensors includes at least three distance sensors. For example, each group of distance sensors includes 3 sonar probes, and these 3 probes are set at different angles in each group to cover a wider area. Assume that one of the probes is set in the front (or rear) to detect the distance in the direction of the central axis of the underwater robot; the other two probes are respectively tilted at a certain angle to the left front (or left rear) and the right front (or right rear), so that the distance of objects in multiple directions can be detected at the same time, thereby constructing a fan-shaped (if it is considered in plane angle) or similar three-dimensional space perception range, and enhancing the underwater robot's perception of the surrounding environment. Therefore, the acquired obstacle distance vector set can include distance values ​​in each direction.

[0060] Preferably, in this embodiment, each distance sensor in the distance sensor cluster is a ranging sonar.

[0061] In this embodiment, the DVL sensor is a sensor for measuring the speed of the underwater robot relative to the seabed or water body.

[0062] The inertial measurement unit includes a gyroscope, an accelerometer, and a magnetometer, which can be used to estimate the real-time posture of the underwater robot.

[0063] Specifically, in an embodiment of the present invention, the target channel may be a water pipeline of a water plant. In other embodiments, the target channel may also be a pipeline of a certain length that can be navigated by other underwater underwater robots. The pipeline may be linear or non-linear, for example, the pipeline may be a pipeline with multiple turns.

[0064] In an optional embodiment of the present invention, when receiving a traveling remote control instruction, the underwater robot starts to navigate in the target channel, and then performs the operation of acquiring the distance data collected by the distance sensor cluster of the underwater robot in step S11. The traveling remote control instruction is used to enable the underwater robot to start automatic navigation, or the traveling remote control instruction is used to enable the underwater robot to navigate in the direction indicated by the traveling remote control instruction.

[0065] Specifically, the travel remote control command can be a control signal sent by RC (Remote Control) or a communication interface (for example, the travel remote control command includes the control expectation of centering navigation), and then the corresponding control command is obtained through the mavlink protocol analysis to obtain the control expectation value, and then the state machine of the underwater robot determines the current state of the underwater robot and the next state to be converted according to the control expectation value. Further, the controller of the underwater robot calculates the specific control quantity according to the state and control strategy determined by the state machine. The control quantity is finally realized through the electronic speed controller (ESC) of the underwater robot to control the motor, so that the motor of the underwater robot runs at the required speed, thereby driving the underwater robot to move in the desired manner.

[0066] Further, in an optional embodiment of the present invention, when the distance sensor cluster includes distance sensors arranged in three directions, the distance data between the underwater robot and the side walls of the target channel in three different directions can be obtained, and then a triangular plane can be determined, and the center of the triangle can be determined according to the triangular plane, which is the center position of the target channel. For example, the incenter or circumcenter of the triangle is determined as the center position of the target channel.

[0067] Further, in an optional embodiment of the present invention, the distance sensor cluster includes distance sensors arranged in four directions: up, down, left, and right. The calculating the center position of the target channel according to the distance data includes:

[0068] Calculating the lateral center point of the target channel according to the left obstacle distance and the right obstacle distance in the distance data;

[0069] Calculating the longitudinal center point of the target channel according to the upper obstacle distance and the lower obstacle distance in the distance data;

[0070] Determine the central position of the target channel composed of the transverse center point and the longitudinal center point.

[0071] In this embodiment, the left obstacle distance and the right obstacle distance are the distances of obstacles in the direction parallel to the underwater robot. The left obstacle distance is the distance between the left side of the underwater robot and the inner wall of the target channel, and the right obstacle distance is the distance between the right side of the underwater robot and the inner wall of the target channel. Specifically, the left obstacle distance and the right obstacle distance can be added and divided by two to obtain the center point in the direction parallel to the underwater robot, which is the lateral center point of the target channel.

[0072] In this embodiment, the upper obstacle distance and the lower obstacle distance are the distances of obstacles in the direction perpendicular to the underwater robot. The upper obstacle distance is the distance between the upper side of the underwater robot and the inner wall of the target channel, and the lower obstacle distance is the distance between the lower side of the underwater robot and the inner wall of the target channel. Specifically, the upper obstacle distance and the lower obstacle distance can be added and divided by two to obtain the center point in the direction perpendicular to the underwater robot, which is the longitudinal center point of the target channel.

[0073] Further, in an optional embodiment of the present invention, the calculating the center position of the target channel according to the distance data includes:

[0074] Obtaining a route traveled by the underwater robot in adjacent unit times;

[0075] Calculating a cross-sectional plane perpendicular to the traveled route based on a center point of the traveled route;

[0076] Mapping the distance data to the cross-sectional plane to obtain a plane range of the navigation environment;

[0077] Detecting each edge irregular area in the plane range of the navigation environment;

[0078] Acquire the maximum circumscribed boundary of the underwater robot, and use the maximum circumscribed boundary to perform traversal tangency operations with each of the irregular edge regions;

[0079] According to the traversal tangency result, the portion of each of the irregular edge regions outside the maximum circumscribed boundary is identified to obtain an inaccessible area, and the inaccessible area in the plane range of the navigation environment is deleted to obtain a navigable area;

[0080] The center of the navigable area is calculated using a clustering algorithm to obtain the center position of the target channel.

[0081] Specifically, in this embodiment, the unit time may be 1 second, and the traveled route of the underwater robot 1 second ago is obtained.

[0082] Specifically, the center point of the traveled route is the position forward from the current position of the underwater robot, that is, the position of the middle point of the traveled route. Specifically, the position of the point where the coordinates of the starting point of the traveled route are divided by 2 is the center point of the traveled route. Then, the tangent at the center point of the traveled route is obtained, and the plane perpendicular to the tangent direction is determined as the cross-sectional plane.

[0083] In the embodiment of the present invention, the distance data acquired by the specific sensor cluster can at least identify objects within a preset distance on a complete cross section of the underwater robot. The complete cross section can be a scanning plane that is perpendicular or intersecting (cannot overlap) the navigation route of the underwater robot. Since the scanning plane does not overlap with the driving route, when the cross-sectional plane is perpendicular to the driving route, the distance data collected on the scanning plane can be mapped to the cross-sectional plane to form a plane range of the navigation environment.

[0084] Specifically, in the embodiment of the present invention, detecting each irregular edge region in the plane range of the navigation environment includes: obtaining the edge portion in the plane range of the navigation environment. Then, the structure diagram of the underwater robot is constructed with a minimum circumscribed circle to obtain the maximum circumscribed boundary of the underwater robot, and then the maximum circumscribed boundary is tangent to the edge portion, and some places can be tangent to some places but not to others. The places that cannot be tangent indicate that the robot is unlikely to collide, and these places that cannot be tangent are not considered when calculating the center position.

[0085] The environment of the underwater robot in the embodiment of the present invention may be relatively complex. For example, sometimes a rectangular gap is detected. Although the area is large, the short side of the rectangular gap is smaller than the radius of the underwater robot. Therefore, no matter how large the rectangular area is, the underwater robot cannot pass through it. Therefore, in the embodiment of the present invention, the edge part of the plane range of the navigation environment is inscribed with the maximum circumscribed boundary of the underwater robot to summarize the passable area, and then the center is calculated to obtain the center position.

[0086] Specifically, when calculating the center, the relative coordinates of each point in the navigable area can be established by measuring the distance value, and then the cluster center point, that is, the center position of the target channel, is found through the clustering algorithm.

[0087] Furthermore, in other optional embodiments of the present invention, the above two methods for obtaining the center position of the target channel can also be used in combination. When in different areas of the target channel, different methods are used.

[0088] Furthermore, in other optional embodiments of the present invention, before obtaining the distance data collected by the distance sensor cluster of the underwater robot, the method further includes:

[0089] Obtaining the traveled distance of the underwater robot and the navigation speed of the underwater robot;

[0090] If the navigation speed is greater than a preset first threshold, determining the alarm interval time period as a ratio of a preset fixed value to the navigation speed;

[0091] If the navigation speed is less than a preset second threshold, determining the alarm interval time period to be the preset fixed value;

[0092] Real-time monitoring of the real-time change distance of the underwater robot within the alarm interval time period;

[0093] When the real-time change distance reaches the preset alarm distance, an obstacle distance measurement instruction is generated to execute the operation of acquiring the distance data collected by the distance sensor cluster of the underwater robot, and reset the timing starting point of the alarm interval time period;

[0094] When the real-time changing distance does not reach the preset alarm distance, but the alarm interval time period expires, an obstacle distance detection instruction is generated to execute the operation of obtaining the distance data collected by the distance sensor cluster of the underwater robot and reset the timing starting point of the alarm interval time period.

[0095] In this embodiment, the traveled distance of the underwater robot may be the distance the underwater robot has traveled up to the current time, or the traveled distance of the underwater robot within a period of time (such as 2 minutes).

[0096] Specifically, the specific values ​​of the preset first threshold and the second threshold can be pre-set, and the first threshold is greater than or equal to the second threshold. The first threshold and the second threshold are used to determine the navigation speed of the underwater robot. Specifically, when the navigation speed of the underwater robot is greater than the preset first threshold, it indicates that the underwater robot is in a fast-moving state; when the navigation speed of the underwater robot is less than the preset second threshold, it indicates that the underwater robot is in a hovering or slow-moving state.

[0097] In this embodiment, the size of the preset fixed value can be preset. For example, if the navigation speed of the underwater robot is 4 meters per second, which is greater than the preset first threshold (such as 3 meters per second), the data collection frequency of the preset fixed value (such as 2 seconds) is too slow and cannot adapt to the changing environment in time. Therefore, the ratio of the preset fixed value to the navigation speed, that is, 2 / 4s, is used to collect distance data.

[0098] For example, when the navigation speed of the underwater robot is 0.5 meters per second, which is less than the second threshold (such as 2 meters per second), it means that the navigation speed is slow, and the distance data can be collected through a fixed value (2 seconds).

[0099] Through the above-mentioned different data collection frequencies, it can be ensured that when the underwater robot is traveling at a low speed, the collection frequency can be reduced to save computing resources while ensuring the safety of the surrounding environment. When traveling at a high speed, the collection frequency can be increased to improve the speed of position correction during driving.

[0100] In this embodiment, the real-time changing distance refers to the driving distance within the alarm time interval.

[0101] For example, the preset fixed value is 10 seconds. Then the real-time change distance of the underwater robot within 10 seconds is monitored. If the real-time change distance reaches the preset alarm distance at the 5th second, an obstacle distance measurement instruction is generated, and the navigation position and direction of the underwater robot are adjusted. That is, if the distance changes greatly within a certain period of time, the distance data collected by the sensor cluster of the underwater robot is obtained again to continuously control the navigation position. If the real-time change distance has not reached the preset alarm distance when 10 seconds are reached, an obstacle distance measurement instruction is also generated to adjust the navigation position and direction of the underwater robot.

[0102] Through this embodiment, the navigation position and direction of the underwater robot can be adjusted in time regardless of whether the navigation speed is fast or slow, thereby improving the safety and stability of the underwater robot during navigation.

[0103] In an optional embodiment of the present invention, the above-mentioned monitoring operation can be performed by pre-setting the buried point. The buried point refers to a code segment pre-set in the code for collecting and tracking the navigation data of the underwater robot. In this embodiment, the scanning frequency of the underwater robot during its driving process is controlled by pre-configuring the code for collecting the traveled distance, driving time and navigation speed.

[0104] Step S12: determining a distance deviation of the underwater robot relative to the center position of the target channel.

[0105] In this embodiment, the relative position of the underwater robot in the channel can be determined based on the size of the underwater robot, the position and angle of the distance sensor installed on the robot, and the collected distance data.

[0106] Specifically, the radius of the target channel can be subtracted from the distance data in each different direction to obtain the relative position of the underwater robot in the channel. Alternatively, the deviation between the distance data in each different direction and the center position of the target channel can be calculated to determine the relative position of the underwater robot in the channel.

[0107] Furthermore, in an optional embodiment of the present invention, after obtaining the distance data collected by the distance sensor cluster of the underwater robot, the method further includes:

[0108] Obtaining a channel diameter of the target channel, and determining whether the distance of each obstacle in the distance data is greater than the channel diameter;

[0109] The obstacle distance whose distance is greater than the channel diameter is determined as the channel lateral gap vector, and the channel lateral gap vector is deleted from the distance data to obtain updated obstacle distance vector set distance data.

[0110] In this embodiment, since there may be openings, branches or holes on the side of the channel, the value of the obstacle distance vector may be extremely large. If these values ​​are retained, the center position calculation may be inaccurate when calculating the center position of the target channel, and then erroneous control may occur, resulting in dangers such as collision of the underwater robot. Therefore, in this embodiment, by filtering part of the distance data by the channel diameter, the above-mentioned misoperation can be avoided, the safety of the underwater robot during driving can be improved, and at the same time, the underwater robot can be prevented from deviating from the main channel and the correctness of navigation can be ensured.

[0111] Step S13, controlling the underwater robot to move to the center position of the target channel according to the distance deviation, and controlling the navigation direction of the underwater robot to deflect toward the center position.

[0112] In other optional embodiments of the present invention, the distance deviation may be determined first, and then the navigation state of the underwater robot may be adjusted or maintained. Specifically, if the distance deviation is less than or equal to a preset offset threshold, the navigation position of the underwater robot is determined to be the center position of the target channel, and the current navigation state of the underwater robot is maintained.

[0113] In this embodiment, the travel direction of the underwater robot can be adjusted by the automatic feedback controller according to the distance deviation so that the underwater robot remains centered. The thrust of the underwater robot back to the center can also be adjusted by the automatic feedback controller so that the underwater robot remains centered. For example, a thrust in the opposite direction of the deviation is output, and the thrust is controlled by the automatic feedback controller (such as an error PD controller) so that the underwater robot moves to the center of the target channel, for example, the underwater robot is controlled to move horizontally to the center of the target channel.

[0114] Specifically, the distance deviation between the underwater robot and the center position of the target channel is a vector with both direction and length. That is, the distance deviation includes the distance between the underwater robot and the center position of the target channel, and also includes the angle change between the two positions in the absolute coordinate system. Therefore, during operation, the PD controller can also be used to calculate the desired direction vector through the centering deviation, so that the forward direction of the underwater robot changes accordingly.

[0115] Specifically, the propulsion device can be used to adjust the underwater robot in the lateral direction, pitch direction, yaw direction and other directions to adjust the running direction of the underwater robot.

[0116] Further, in an optional embodiment of the present invention, controlling the underwater robot to move to the center position of the target channel according to the distance deviation includes:

[0117] Acquire the pressure matrix of water flow in all directions of the underwater robot in real time, and calculate the stable dynamic coordination relationship when the underwater robot is stable according to the pressure matrix;

[0118] According to the stable power coordination relationship, power is output to each power device in the power cluster of the underwater robot to maintain the balance of the underwater robot;

[0119] Using an error PD controller, according to the distance deviation, a power enhancement operation based on the distance deviation direction is performed on the stable power coordination relationship to obtain a motion power coordination relationship;

[0120] The propulsion device of the underwater robot is controlled to move the position of the underwater robot to the center position of the target channel according to the motion dynamic coordination relationship.

[0121] Specifically, changes in water flow have an impact on the pressure distribution around the underwater robot. The volume of the underwater robot and the degree of completion of the underwater channel and tunnel environment will interfere with the stability of the underwater robot. The directions of water flow include six directions: front, back, left, right, up and down.

[0122] In this embodiment, when the underwater robot is stable, it means that it is hovering in a certain channel area, or moving forward stably, wherein stable means that the underwater robot is not pushed and shaken by the water flow.

[0123] In this embodiment, the directions of the various power devices in the power cluster are arranged according to the sports power coordination relationship, and then the power distribution and output are unified.

[0124] In this embodiment, an enhanced PD controller can be used to adjust the stable power coordination relationship. The enhanced PD controller adds an extra item on the basis of the traditional PD controller to process the power enhancement operation in the direction of the relative position difference.

[0125] In this embodiment, the error PD controller method can be used to control each power device according to the motion power coordination relationship, so as to achieve stable movement of the underwater robot.

[0126] Specifically, the power in the opposite direction of the distance deviation can be enhanced according to the stable power coordination relationship and the distance deviation.

[0127] Through this embodiment, it can be ensured in real time that the underwater robot can stably maintain the center of the target channel when it is in a navigation or hovering state.

[0128] Furthermore, in an optional embodiment of the present invention, the method further includes: determining whether there is an obstacle within a preset travel distance in front of the underwater robot, and if there is no obstacle, executing the operation of step S13.

[0129] In this embodiment, the value of the preset travel distance can be preset. When there is no obstacle, the operation of step S13 is performed to adjust the position and direction of the underwater robot to avoid misoperation and further improve the safety of the underwater robot during navigation.

[0130] Furthermore, in an optional embodiment of the present invention, a DVL sensor can be used to monitor the real-time relative speed of the underwater robot to the target channel, and the underwater robot can be controlled to travel at a preset target speed, for example, to control the underwater robot to travel at a speed of 0.5 m / s.

[0131] Furthermore, in an embodiment of the present invention, after controlling the underwater robot to move to the center position of the target channel according to the distance deviation, and controlling the navigation direction of the underwater robot to deflect toward the center position, the method further includes:

[0132] Obtaining the angular velocity, acceleration and geomagnetic field direction collected by the inertial measurement unit cluster of the underwater robot;

[0133] According to the direction of the geomagnetic field, the coordinate system of the underwater robot is corrected to obtain an initial position and posture;

[0134] Integrating the angular velocity to obtain an angle variation, and subtracting a pre-constructed gravity acceleration from the angular velocity to obtain a linear acceleration, wherein the angle variation includes roll, pitch and yaw;

[0135] The position change of the initial position posture is calculated according to the angle change and the linear acceleration to obtain the real-time moving posture.

[0136] In this embodiment, the gyroscope is used to measure the angular velocity (roll, pitch, yaw) of the underwater robot around three axes to describe the rotation state of the underwater robot. The accelerometer is used to measure the linear acceleration of the underwater robot on three axes and can provide basic information about the movement of the underwater robot. The magnetometer is used to measure the direction of the geomagnetic field to provide heading information.

[0137] Specifically, in this embodiment, the direction of the geomagnetic field is first obtained by a magnetometer to obtain the initial position and posture of the underwater robot, and then the position movement direction and angle of the underwater robot are determined according to the angular velocity and acceleration to obtain the real-time moving posture.

[0138] Through this embodiment, the real-time moving posture of the underwater robot can be controlled to remain stable when the underwater robot is navigating.

[0139] In an embodiment of the present application, when the underwater robot is navigating in a target channel, the distance data collected by the distance sensor cluster of the underwater robot is obtained, and the center position of the target channel is calculated based on the distance data, wherein the distance sensor cluster includes distance sensors arranged in at least three directions; the distance deviation of the underwater robot relative to the center position of the target channel is determined; the underwater robot is controlled to move to the center position of the target channel based on the distance deviation, and the navigation direction of the underwater robot is controlled to deflect toward the center position. By controlling the position of the underwater robot to move to the center position of the target channel and controlling the navigation direction of the underwater robot to deflect toward the center position, the underwater robot can continue to navigate in the center of the target channel, reduce the risk of the underwater robot hitting the wall, and improve the safety of the underwater robot during navigation.

[0140] like Figure 2 , which is a functional module diagram of a centering navigation control device based on an underwater robot provided in one embodiment of the present invention.

[0141] The centering navigation control device 100 of the underwater robot of the present invention can be installed in the underwater robot. According to the functions to be implemented, the centering navigation control device 100 of the underwater robot includes a collection module 101, a determination module 102 and a control module 103. The module of the present invention can also be called a unit, which refers to a series of computer program segments that can be executed by the processor of the underwater robot and can complete fixed functions, which are stored in the memory of the underwater robot.

[0142] In this embodiment, the functions of each module / unit are as follows:

[0143] The acquisition module 101 is used to acquire distance data collected by a distance sensor cluster of the underwater robot when the underwater robot navigates in the target channel, and calculate the center position of the target channel according to the distance data, wherein the distance sensor cluster includes distance sensors arranged in at least three directions;

[0144] A determination module 102 is used to determine a distance deviation of the underwater robot relative to the center position of the target channel;

[0145] The control module 103 is used to control the underwater robot to move to the center position of the target channel according to the distance deviation, and control the navigation direction of the underwater robot to deflect toward the center position.

[0146] In detail, each module described in the centering navigation control device 100 of the underwater robot described in the embodiment of the present application is used in the same manner as described above. Figure 1 The same technical means are used as the centering navigation control method of an underwater robot described in , and can produce the same technical effects, which will not be repeated here.

[0147] See also Figure 3 , a schematic diagram of the hardware structure of an underwater robot provided in one embodiment of the present application.

[0148] The underwater robot 10 may include a control circuit, which may include a storage and processing circuit 30. The storage and processing circuit 30 may include a memory, for example, a non-volatile memory (such as a flash memory or other electronically programmable memory with limited deletion for forming a solid-state drive, etc.), a volatile memory (such as a static or dynamic random access memory, etc.), etc., which is not limited by the embodiment of the present application. The processing circuit in the storage and processing circuit 30 can be used to control the operation of the underwater robot 10. The processing circuit can be implemented based on one or more microprocessors, microcontrollers, digital signal processors, baseband processors, power management units, audio codec chips, application-specific integrated circuits, display driver integrated circuits, etc.

[0149] The storage and processing circuit 30 can be used to run software in the underwater robot 10, such as operating system functions, etc. These software can be used to perform some control operations, such as image acquisition based on a camera, ambient light measurement based on an ambient light sensor, proximity sensor measurement based on a proximity sensor, information display functions based on status indicators such as status indicators of light-emitting diodes, touch event detection based on a touch sensor, functions associated with displaying information on multiple (e.g., layered) displays, operations associated with performing wireless communication functions, operations associated with collecting and generating audio signals, control operations associated with collecting and processing button press event data, and other functions in the underwater robot 10, etc., which are not limited by the embodiments of the present application.

[0150] Furthermore, the memory stores an executable program code, and the processor coupled to the memory calls the executable program code stored in the memory to execute the above Figure 1 The centered navigation control method of the underwater robot is described in the illustrated embodiment.

[0151] The executable program code includes the above Figure 2 The various modules in the control device for the central navigation of the underwater robot described in the illustrated embodiment, for example: a collection module, a determination module and a control module.

[0152] The underwater robot 10 may further include an input-output circuit 42. The input-output circuit 42 may be used to enable the underwater robot 10 to input and output data, that is, to allow the underwater robot 10 to receive data from an external device and also to allow the underwater robot 10 to output data from the underwater robot 10 to an external device. The input-output circuit 42 may further include a sensor 32. The sensor 32 may include an ambient light sensor, a proximity sensor based on light and capacitance, a touch sensor (e.g., based on a light touch sensor and / or a capacitive touch sensor, wherein the touch sensor may be part of a touch display screen or may be used independently as a touch sensor structure), an acceleration sensor, and other sensors.

[0153] The input-output circuit 42 may also include a data transmission interface, a display, such as the display 14. The display 14 may include an organic light-emitting diode display, an electronic ink display, a plasma display, a display using other display technologies, or a combination of several of them. The display 14 may include a touch sensor array (i.e., the display 14 may be a touch display screen). The touch sensor may be a capacitive touch sensor formed by an array of transparent touch sensor electrodes (e.g., indium tin oxide (ITO) electrodes), or may be a touch sensor formed using other touch technologies, such as sonic touch, pressure-sensitive touch, resistive touch, optical touch, etc., which is not limited in the embodiments of the present application.

[0154] The underwater robot 10 may also include an audio component 36. The audio component 36 may be used to provide audio input and output functions for the underwater robot 10. The audio component 36 in the underwater robot 10 may include a speaker, a microphone, a buzzer, a tone generator, and other components for generating and detecting sound.

[0155] The communication circuit 38 can be used to provide the underwater robot 10 with the ability to communicate with external devices. The communication circuit 38 may include analog and digital input / output interface circuits, and wireless communication circuits based on radio frequency signals and / or optical signals. The wireless communication circuit in the communication circuit 38 may include a radio frequency transceiver circuit, a power amplifier circuit, a low noise amplifier, a switch, a filter, and an antenna. For example, the wireless communication circuit in the communication circuit 38 may include a circuit for supporting near field communication (NFC) by transmitting and receiving near field coupled electromagnetic signals. For example, the communication circuit 38 may include a near field communication antenna and a near field communication transceiver. The communication circuit 38 may also include a cellular phone transceiver and antenna, a wireless local area network transceiver circuit and antenna, etc.

[0156] The underwater robot 10 may further include a battery, a power management circuit and other input / output units 40. The input / output units 40 may include buttons, joysticks, click wheels, scroll wheels, touch pads, cameras, light emitting diodes and other status indicators.

[0157] The user can input commands through the input-output circuit 42 to control the operation of the underwater robot 10 , and can use the output data of the input-output circuit 42 to receive status information and other outputs from the underwater robot 10 .

[0158] Furthermore, an embodiment of the present invention further provides a computer-readable storage medium, which may be provided in the underwater robot in each of the above embodiments. Figure 3 The memory in the storage and processing circuit 30 in the illustrated embodiment. The computer readable storage medium stores a computer program, which, when executed by the processor, implements the aforementioned Figure 1 The centering navigation control method of the underwater robot described in the illustrated embodiment. Further, the computer storage medium can also be a U disk, a mobile hard disk, a read-only memory (ROM), a RAM, a magnetic disk or an optical disk, and other media that can store program codes.

[0159] It should be noted that, for the convenience of description, the aforementioned method embodiments are all described as a series of action combinations, but those skilled in the art should be aware that the present invention is not limited by the described action sequence, because according to the present invention, certain steps can be performed in other sequences or simultaneously. Secondly, those skilled in the art should also be aware that the embodiments described in the specification are all preferred embodiments, and the actions and modules involved are not necessarily required by the present invention.

[0160] In the above embodiments, the description of each embodiment has its own emphasis. For parts that are not described in detail in a certain embodiment, reference can be made to the relevant descriptions of other embodiments.

[0161] The above is a description of the underwater robot's centered navigation control method, device, underwater robot and computer-readable storage medium provided by the present invention. For a person skilled in the art, according to the ideas of the embodiments of the present invention, there may be changes in the specific implementation methods and application scopes. In summary, the content of this specification should not be understood as a limitation on the present invention.

Claims

1. A method for controlling the centering navigation of an underwater robot, characterized in that: The method comprises: When the underwater robot navigates in the target channel, distance data collected by a distance sensor cluster of the underwater robot is acquired, and the center position of the target channel is calculated according to the distance data, wherein the distance sensor cluster includes distance sensors arranged in at least three directions; Determining a distance deviation of the underwater robot relative to a center position of the target channel; The underwater robot is controlled to move to the center position of the target channel according to the distance deviation, and the navigation direction of the underwater robot is controlled to deflect toward the center position.

2. The method according to claim 1, characterized in that The distance sensor cluster includes distance sensors arranged in four directions: up, down, left, and right. The calculating the center position of the target channel according to the distance data includes: Calculating the lateral center point of the target channel according to the left obstacle distance and the right obstacle distance in the distance data; Calculating the longitudinal center point of the target channel according to the upper obstacle distance and the lower obstacle distance in the distance data; Determine the central position of the target channel composed of the transverse center point and the longitudinal center point.

3. The method according to claim 1, characterized in that Before acquiring the distance data collected by the distance sensor cluster of the underwater robot, the method further includes: Obtaining the traveled distance of the underwater robot and the navigation speed of the underwater robot; If the navigation speed is greater than a preset first threshold, determining the alarm interval time period as a ratio of a preset fixed value to the navigation speed; If the navigation speed is less than a preset second threshold, determining the alarm interval time period to be the preset fixed value; Real-time monitoring of the real-time change distance of the underwater robot within the alarm interval time period; When the real-time change distance reaches the preset alarm distance, an obstacle distance measurement instruction is generated to execute the operation of acquiring the distance data collected by the distance sensor cluster of the underwater robot, and reset the timing starting point of the alarm interval time period; When the real-time changing distance does not reach the preset alarm distance, but the alarm interval time period expires, an obstacle distance detection instruction is generated to execute the operation of obtaining the distance data collected by the distance sensor cluster of the underwater robot and reset the timing starting point of the alarm interval time period.

4. The method according to claim 1, characterized in that: After obtaining the distance data collected by the distance sensor cluster of the underwater robot, the method further includes: Obtaining a channel diameter of the target channel, and determining whether the distance of each obstacle in the distance data is greater than the channel diameter; The obstacle distance whose distance is greater than the channel diameter is determined as the channel lateral gap vector, and the channel lateral gap vector is deleted from the distance data to obtain updated distance data.

5. The method according to claim 1, characterized in that The calculating the center position of the target channel according to the distance data comprises: Obtaining a route traveled by the underwater robot in adjacent unit times; Calculating a cross-sectional plane perpendicular to the traveled route based on a center point of the traveled route; Mapping the distance data to the cross-sectional plane to obtain a plane range of the navigation environment; Detecting each edge irregular area in the plane range of the navigation environment; Acquire the maximum circumscribed boundary of the underwater robot, and use the maximum circumscribed boundary to perform traversal tangency operations with each of the irregular edge regions; According to the traversal tangency result, the portion of each of the irregular edge regions outside the maximum circumscribed boundary is identified to obtain an inaccessible area, and the inaccessible area in the plane range of the navigation environment is deleted to obtain a navigable area; The center of the navigable area is calculated using a clustering algorithm to obtain the center position of the target channel.

6. The method according to any one of claims 1 to 5, characterized in that After controlling the underwater robot to move to the center position of the target channel according to the distance deviation, and controlling the navigation direction of the underwater robot to deflect toward the center position, the method further includes: Obtaining the angular velocity, acceleration and geomagnetic field direction collected by the inertial measurement unit cluster of the underwater robot; According to the direction of the geomagnetic field, the coordinate system of the underwater robot is corrected to obtain an initial position and posture; Integrating the angular velocity to obtain an angle variation, and subtracting a pre-constructed gravity acceleration from the angular velocity to obtain a linear acceleration, wherein the angle variation includes roll, pitch and yaw; The position change of the initial position posture is calculated according to the angle change and the linear acceleration to obtain the real-time moving posture.

7. The method according to any one of claims 1 to 5, characterized in that The step of controlling the underwater robot to move to the center position of the target channel according to the distance deviation includes: Acquire the pressure matrix of water flow in all directions of the underwater robot in real time, and calculate the stable dynamic coordination relationship when the underwater robot is stable according to the pressure matrix; According to the stable power coordination relationship, power is output to each power device in the power cluster of the underwater robot to maintain the balance of the underwater robot; Using an error PD controller, according to the distance deviation, a power enhancement operation based on the distance deviation direction is performed on the stable power coordination relationship to obtain a motion power coordination relationship; The propulsion device of the underwater robot is controlled to move the position of the underwater robot to the center position of the target channel according to the motion dynamic coordination relationship.

8. A centering navigation control device for an underwater robot, characterized in that: The device comprises: A collection module, used for acquiring distance data collected by a distance sensor cluster of the underwater robot when the underwater robot navigates in a target channel, and calculating the center position of the target channel according to the distance data, wherein the distance sensor cluster includes distance sensors arranged in at least three directions; A determination module, used to determine a distance deviation of the underwater robot relative to a center position of the target channel; The control module is used to control the underwater robot to move to the center position of the target channel according to the distance deviation, and control the navigation direction of the underwater robot to deflect toward the center position.

9. An underwater robot, characterized in that: The underwater robot comprises: Memory and processor; The memory stores executable program code; The processor coupled to the memory calls the executable program code stored in the memory to execute the control method for the central navigation of the underwater robot according to any one of claims 1 to 7.

10. A computer-readable storage medium having a computer program stored thereon, characterized in that: When the computer program is executed by a processor, the control method for central navigation of an underwater robot according to any one of claims 1 to 7 is implemented.

Images