Underwater operation robot, control method and control system thereof and storage medium

Abstract

The invention relates to the technical field of robot control, and discloses an underwater operation robot, a control method and system thereof and a storage medium, and the method comprises the steps: obtaining angular velocity, acceleration and attitude information measured by an inertial sensor, and pulse data measured by different flow sensors; according to each path of pulse data, water flow speeds in different directions are obtained, and according to the motor rotating speed of the underwater operation robot and the accelerated speed and the water flow speed in the corresponding direction, the relative movement speed of the underwater operation robot relative to the swimming pool in the corresponding direction is calculated; and according to the attitude information and the angular velocity, the relative movement speeds in different directions are respectively adjusted in real time, so that the underwater operation robot is controlled to operate along the preset operation path in the target direction according to the preset speed, and the actual speed of the underwater operation robot in water is efficiently and accurately measured. And the speed can be adjusted in real time, so that the operation can be performed according to the preset operation path.

Description

Technical Field

[0001] The present application relates to the field of robot control technology, and in particular to an underwater working robot and a control method, a control system and a storage medium thereof. Background Art

[0002] When planning the movement path of underwater operation, existing pool cleaning robots usually directly reflect the relative movement speed of the robot through the wheel speed. However, the wheels may slip severely underwater, so the measured wheel speed and the actual relative movement speed of the robot are very different, resulting in the robot being unable to drive according to the planned route specifications when adjusting the speed with this wheel speed. Summary of the invention

[0003] In view of this, the embodiments of the present application provide a control method and system for an underwater working robot, an underwater working robot and a storage medium, which can effectively solve the problem in the prior art that the underwater working robot has poor driving effect according to the planned route.

[0004] In a first aspect, an embodiment of the present application provides a control method for an underwater working robot, comprising:

[0005] Obtain angular velocity, acceleration and attitude information measured by inertial sensors, as well as pulse data measured by different flow sensors;

[0006] The water flow speed in different directions is obtained according to each pulse data, and the relative moving speed of the underwater working robot relative to the swimming pool in the corresponding direction is calculated according to the motor speed of the underwater working robot, the acceleration in the corresponding direction and the water flow speed;

[0007] According to the posture information and the angular velocity, the relative movement speeds in different directions are respectively adjusted in real time to control the underwater working robot to operate along a preset working path in a target direction at a preset speed.

[0008] In some embodiments, after controlling the underwater working robot to operate along a preset working path at a preset speed in a target direction, the method further includes:

[0009] Mark the road sections where the work has been completed to obtain a marked path;

[0010] After reaching the end point of the preset operation path, judging whether there is a target road section where the operation is missed according to the marked path, and performing additional operation on the target road section if there is a target road section where the operation is missed;

[0011] If the supplementary operation on the current target road section reaches a preset time or a preset number of times, the supplementary operation is terminated and the supplementary operation is performed on the next target road section.

[0012] In some embodiments, the controlling the underwater working robot to operate along a preset working path in a target direction at a preset speed may also include:

[0013] Using an ultrasonic sensor to find a first wall in the swimming pool that has the shortest straight-line distance to the underwater working robot;

[0014] After driving to a preset position close to the first wall, starting from the preset position at the preset speed, circling the wall at the bottom of the swimming pool, and establishing a moving map according to the posture information during the circling process;

[0015] The mobile map is used to plan the preset operation path; during the detour, the location where the obstacle is encountered is marked as the corresponding wall corner point of the swimming pool to obtain multiple wall corner points; any one of the multiple wall corner points is used as the starting point of the next operation.

[0016] In some embodiments, the relative movement speeds in different directions are respectively adjusted in real time according to the posture information and the angular velocity to control the underwater working robot to operate along a preset working path at a preset speed in a target direction, including:

[0017] According to the posture information and angular velocity of the underwater working robot, a proportional-integral-differential control algorithm is used to adjust the relative moving speed in the forward direction to a preset speed in real time, and to adjust the relative moving speed in the lateral direction to zero, so that the underwater working robot can operate along the preset working path.

[0018] In some embodiments, the water flow velocity includes: a vertical flow velocity in a forward direction and a lateral flow velocity in a lateral direction;

[0019] When the directions of the two flow sensors correspond to the same forward direction and the lateral direction, the water flow speeds in different directions are obtained according to the pulse data, including:

[0020] After obtaining first pulse data measured by a first flow sensor installed in the forward direction in real time, the first pulse data is used as the vertical flow velocity;

[0021] After acquiring in real time second pulse data measured by a second flow sensor installed in the lateral direction, the second pulse data is used as the lateral flow velocity;

[0022] When the directions of the two flow sensors are at an angle to both the forward direction and the lateral direction, the water flow velocities in different directions are obtained according to the pulse data, including:

[0023] After acquiring the first pulse data and the second pulse data correspondingly measured by the first flow sensor and the second flow sensor in real time, decomposing the first pulse data into a first vertical flow in a forward direction and a first lateral flow in a lateral direction of the first flow sensor according to a coefficient and an installation angle of the first flow sensor;

[0024] Decomposing the second pulse data into a second vertical flow rate of the second flow sensor in a forward direction and a second lateral flow rate in a lateral direction according to a coefficient and an installation angle of the second flow sensor;

[0025] Calculating the vertical flow rate according to the first vertical flow rate and the second vertical flow rate;

[0026] The lateral flow velocity is calculated according to the first lateral flow rate and the second lateral flow rate.

[0027] In some embodiments, the water flow speed in different directions is obtained according to each pulse data, and the relative moving speed of the underwater working robot relative to the swimming pool in the corresponding direction is calculated according to the motor speed of the underwater working robot and the acceleration and water flow speed in the corresponding direction, including:

[0028] After calculating the product of the acceleration compensation coefficient and the acceleration in the lateral direction to obtain a first acceleration compensation value, and the product of the motor speed and the speed compensation coefficient in the lateral direction to obtain a first speed compensation value, the relative movement speed in the lateral direction is calculated according to the first acceleration compensation value, the first speed compensation value and the lateral flow velocity;

[0029] After calculating the product of the acceleration compensation coefficient and the acceleration in the forward direction to obtain a second acceleration compensation value, and the product of the motor speed and the speed compensation coefficient in the forward direction to obtain a second speed compensation value, the relative moving speed in the forward direction is calculated based on the second acceleration compensation value, the second speed compensation value and the vertical direct speed.

[0030] In a second aspect, an embodiment of the present application provides a control system for an underwater working robot, including:

[0031] An acquisition module is used to obtain the angular velocity, acceleration and attitude information measured by the inertial sensor, as well as the pulse data measured by different flow sensors;

[0032] A calculation module, used to obtain the water flow speed in different directions according to each pulse data, and calculate the relative moving speed of the underwater working robot relative to the swimming pool in the corresponding direction according to the motor speed of the underwater working robot, the acceleration in the corresponding direction and the water flow speed;

[0033] The control module is used to adjust the relative movement speed in different directions in real time according to the posture information and the angular velocity, so as to control the underwater working robot to operate along a preset working path in a target direction at a preset speed.

[0034] In a third aspect, an embodiment of the present application provides an underwater working robot, comprising: a flow sensor, an inertial sensor, a motor, a processor and a memory, wherein the memory stores a computer program, and the processor is used to execute the computer program to implement the control method of the underwater working robot as described above.

[0035] In some embodiments, the number of the flow sensors is two, including a first flow sensor and a second flow sensor that are disposed perpendicular to each other, and the first flow sensor and the second flow sensor are both disposed on the outer shell of the underwater working robot.

[0036] In a fourth aspect, an embodiment of the present application provides a computer-readable storage medium storing a computer program, which, when executed on a processor, implements the control method of the underwater working robot as described above.

[0037] Compared with the prior art, the present application provides an underwater working robot and its control method, control system and storage medium. The control method of the underwater working robot includes: obtaining angular velocity, acceleration and posture information measured by an inertial sensor, and pulse data measured by different flow sensors; obtaining water flow velocities in different directions according to each pulse data, and calculating the relative movement speed of the underwater working robot relative to the swimming pool in the corresponding direction according to the motor speed of the underwater working robot and the acceleration and water flow speed in the corresponding direction; adjusting the relative movement speed in different directions in real time according to the posture information and the angular velocity, so as to control the underwater working robot to operate along a preset working path at a preset speed in a target direction, thereby realizing efficient and accurate measurement of the actual speed of the underwater working robot in water, and realizing real-time adjustment of the speed so that the operation is performed on the preset working path. BRIEF DESCRIPTION OF THE DRAWINGS

[0038] In order to more clearly illustrate the technical solutions of the embodiments of the present application, the drawings required for use in the embodiments will be briefly introduced below. It should be understood that the following drawings only show certain embodiments of the present application and therefore should not be regarded as limiting the scope. For ordinary technicians in this field, other related drawings can be obtained based on these drawings without paying creative work.

[0039] Figure 1 A flow chart showing a control method of an underwater working robot according to an embodiment of the present application is shown;

[0040] Figure 2 A judgment flow chart of the control method of the underwater working robot according to an embodiment of the present application is shown;

[0041] Figure 3 A flow chart of obtaining water flow velocity when two flow sensors are in a first orientation in a control method of an underwater working robot according to an embodiment of the present application is shown;

[0042] Figure 4 A flow chart of obtaining water flow velocity when two flow sensors are in the second orientation in the control method of the underwater working robot according to an embodiment of the present application is shown;

[0043] Figure 5 A flow chart showing the calculation of the relative moving speed in the control method of the underwater working robot according to an embodiment of the present application is shown;

[0044] Figure 6 A flow chart showing the establishment of a moving map in the control method of the underwater working robot according to an embodiment of the present application is shown;

[0045] Fig. 7A A flowchart of the first part of underwater operation of an underwater operation robot in a control method of an underwater operation robot according to an embodiment of the present application is shown;

[0046] Figure 7B A second part of the flow chart of underwater operation of the underwater operation robot in the control method of the underwater operation robot in the embodiment of the present application is shown;

[0047] Figure 7C The third part of the flow chart of underwater operation of the underwater operation robot in the control method of the underwater operation robot in the embodiment of the present application is shown;

[0048] Figure 8 A schematic diagram showing marking of four wall corner points in the control method of the underwater working robot according to an embodiment of the present application is shown;

[0049] Fig. 9 A schematic diagram of planning and cleaning in a control method for an underwater working robot according to an embodiment of the present application is shown;

[0050] Fig.10 A schematic diagram of the process of supplementing operations in the control method of the underwater working robot according to an embodiment of the present application is shown;

[0051] Fig.11 The structural block diagram of the control system of the underwater working robot according to the embodiment of the present application is shown. DETAILED DESCRIPTION

[0052] The technical solutions in the embodiments of the present application will be described clearly and completely below in conjunction with the drawings in the embodiments of the present application. Obviously, the described embodiments are only part of the embodiments of the present application, rather than all of the embodiments.

[0053] The components of the embodiments of the present application generally described and shown in the drawings herein may be arranged and designed in various configurations. Therefore, the following detailed description of the embodiments of the present application provided in the drawings is not intended to limit the scope of the application claimed for protection, but merely represents the selected embodiments of the present application. Based on the embodiments of the present application, all other embodiments obtained by those skilled in the art without making creative work belong to the scope of protection of the present application.

[0054] Hereinafter, the terms "including", "having" and their cognates that can be used in various embodiments of the present application are intended only to indicate specific features, numbers, steps, operations, elements, components or a combination of the foregoing items, and should not be understood as first excluding the existence of one or more other features, numbers, steps, operations, elements, components or a combination of the foregoing items or increasing the possibility of one or more features, numbers, steps, operations, elements, components or a combination of the foregoing items. In addition, the terms "first", "second", "third" and the like are only used to distinguish descriptions and cannot be understood as indicating or implying relative importance.

[0055] Unless otherwise defined, all terms (including technical terms and scientific terms) used herein have the same meanings as those generally understood by those skilled in the art to which the various embodiments of the present application belong. The terms (such as those defined in generally used dictionaries) will be interpreted as having the same meanings as the contextual meanings in the relevant technical field and will not be interpreted as having idealized meanings or overly formal meanings unless clearly defined in the various embodiments of the present application.

[0056] In conjunction with the accompanying drawings, some embodiments of the present application are described in detail below. In the absence of conflict, the following embodiments and features in the embodiments can be combined with each other.

[0057] In the prior art, the actual relative moving speed is calculated based on the wheel speed of the robot, which results in inaccurate mapping and positioning. At the same time, the existing swimming pool robot adjusts the cleaning route by colliding with the edge of the swimming pool wall or ultrasonic recognition, which has the problems of poor planning effect and serious missed cleaning.

[0058] Then, the present application provides an underwater working robot and its control method, control system and storage medium. The control method of the underwater working robot includes: obtaining angular velocity, acceleration and posture information measured by inertial sensors, and pulse data measured by different flow sensors; obtaining water flow velocities in different directions according to each pulse data, and calculating the relative movement speed of the underwater working robot relative to the swimming pool in the corresponding direction according to the motor speed of the underwater working robot and the acceleration and water flow velocity in the corresponding direction; adjusting the relative movement speed in different directions in real time according to the posture information and the angular velocity, so as to control the underwater working robot to operate along a preset working path at a preset speed in the target direction, thereby realizing efficient and accurate measurement of the actual speed of the underwater working robot in the water, and realizing real-time adjustment of the speed so that the operation can be performed on the preset working path.

[0059] After calculating the water flow velocity through the measured pulse data, the relative movement speed is calculated according to the posture information, and then the relative movement speed is adjusted according to the posture information to perform operations on the preset operation path, thereby achieving accurate measurement of the actual underwater speed. By adjusting the relative movement speed in real time, standardized operations can be performed according to the preset operation path, effectively improving the operation efficiency and coverage, and reducing usage consumption.

[0060] The control method of the underwater working robot is described below in conjunction with some specific embodiments.

[0061] Figure 1 A flow chart showing a method for controlling an underwater working robot according to an embodiment of the present application is shown.

[0062] See also Figure 1 The present application embodiment provides a control method for an underwater working robot, comprising steps S100-S300:

[0063] S100, obtaining angular velocity, acceleration and attitude information measured by an inertial sensor, and pulse data measured by different flow sensors.

[0064] Among them, the inertial measurement unit (IMU) is a device used to measure the three-axis attitude angle (or angular rate) and acceleration of an object. Through the built-in accelerometer and gyroscope, the IMU can measure the linear acceleration and rotational angular rate (angular velocity for short) from three directions, and calculate the carrier's attitude, velocity, displacement and other information.

[0065] In this embodiment, the number of flow sensors (or flow meters) is two, including a first flow sensor and a second flow sensor arranged perpendicular to each other, and the first flow sensor and the second flow sensor are both arranged on the shell of the underwater working robot. The two flow sensors can be placed one longitudinally and the other transversely, respectively used to measure the corresponding longitudinal speed and transverse speed; wherein the longitudinal speed is defined as the odometer speed, and the transverse speed is defined as the drift speed; or one is placed obliquely 45° to the left front, and the other is placed obliquely 45° to the right front, and the measured speed is orthogonally decomposed into the body coordinate system to obtain the longitudinal speed and transverse speed; or only one is set obliquely.

[0066] The water flow (i.e. pulse data) measured by the flow sensors placed horizontally and vertically does not need to be decomposed to calculate the speed, while the obliquely placed ones need to be decomposed to calculate the speed. The flow meter is a photoelectric pulse flow meter or a Hall flow meter, which outputs a pulse signal. The flow rate can be calculated by counting the pulses, and then the flow rate can be calculated by the flow rate.

[0067] However, the placement of the flow meter sensor must meet the following requirements at the same time: it must be installed on the machine casing (it can be on the same side, both sides or bottom of the machine) and can be immersed in water; there must be no obstructions in front and behind the water inlet and outlet so that water can flow smoothly.

[0068] Exemplarily, the underwater working robot in this embodiment may be a swimming pool cleaning robot (hereinafter referred to as the robot), and the following takes the swimming pool robot entering the water to clean the bottom of the swimming pool as an example.

[0069] Then, the implementation process of step S100 is as follows:

[0070] See also Figure 2 After the underwater working robot enters the water, the two flow sensors on the underwater working robot measure the pulse data in real time, and the inertial sensor on the underwater working robot measures the angular velocity and acceleration of the underwater working robot in two directions in real time, and calculates the posture information of the underwater working robot.

[0071] For example, in one embodiment, see Figure 3 When the directions of the two flow sensors correspond to the same forward direction and the lateral direction, that is, when the two flow sensors are in the first direction, the water flow velocities in different directions are obtained according to the pulse data, including:

[0072] A110, after obtaining in real time first pulse data measured by a first flow sensor installed in the forward direction, use the first pulse data as the vertical flow velocity;

[0073] A120. After acquiring in real time the second pulse data measured by a second flow sensor installed in the lateral direction, the second pulse data is used as the lateral flow velocity.

[0074] The coordinate origin of the robot is set as the geometric center of the machine, the front direction is the positive direction of the x-axis, that is, the forward direction; the right direction is the positive direction of the y-axis, that is, the lateral direction.

[0075] Exemplarily, when the orientations of the two flow sensors correspond to the same forward direction and transverse direction, that is, the first flow sensor is installed along the longitudinal direction of the x-axis (i.e., the forward direction), and the second flow sensor is installed along the transverse direction of the y-axis (i.e., the transverse direction), then the specific process of obtaining the water flow velocity is as follows:

[0076] At this time, there is no need to decompose the pulse data measured by the two flow sensors. Then, the first pulse data measured by the first flow sensor in the forward direction of the underwater working robot is the vertical flow velocity of the underwater working robot, and the second pulse data measured by the second flow sensor in the lateral direction is the lateral flow velocity of the underwater working robot.

[0077] Set the flow rate measured by the first flow sensor to be L1 (flow rate L1 is the first pulse data) and the speed to be V1, and the flow rate measured by the second flow sensor to be L2 (flow rate L2 is the second pulse data) and the speed to be V2; set the x-axis speed of the robot to be Vx (i.e., vertical flow velocity), and the y-axis speed to be Vy (i.e., lateral flow velocity);

[0078] Then, at this time, Vx=V1(1);

[0079] Vy=V2 (2).

[0080] For further information, see Figure 4 In another embodiment, when the directions of the two flow sensors are at an angle to both the forward direction and the lateral direction, that is, when the two flow sensors are in the second direction, the water flow velocities in different directions are obtained according to the pulse data, including:

[0081] B110, after acquiring the first pulse data and the second pulse data correspondingly measured by the first flow sensor and the second flow sensor in real time, decompose the first pulse data into a first vertical flow of the first flow sensor in the forward direction and a first lateral flow in the lateral direction according to the coefficient and the installation angle of the first flow sensor;

[0082] B120, decomposing the second pulse data into a second vertical flow rate of the second flow sensor in the forward direction and a second lateral flow rate in the lateral direction according to the coefficient and the installation angle of the second flow sensor;

[0083] B130, calculating a vertical flow velocity according to the first vertical flow rate and the second vertical flow rate;

[0084] B140. Calculate the lateral flow velocity based on the first lateral flow rate and the second lateral flow rate.

[0085] Exemplarily, when the directions of the two flow sensors are at an angle to both the forward direction and the lateral direction, the specific process of obtaining the water flow velocity is as follows:

[0086] At this time, the pulse data measured by the flow sensor on each road needs to be decomposed in the forward direction and the lateral direction respectively. Then, firstly, according to the coefficients and installation angles of the first flow sensor and the second flow sensor, the first pulse data measured by the first flow sensor and the second pulse data measured by the second flow sensor are correspondingly decomposed into the first vertical flow in the forward direction and the first lateral flow in the lateral direction of the first flow sensor, and the second vertical flow in the forward direction and the second lateral flow in the lateral direction of the second flow sensor.

[0087] Then, the first vertical flow rate and the second vertical flow rate are added to obtain the vertical flow velocity (ie, Vx), and at the same time, the first lateral flow rate and the second lateral flow rate are added to obtain the lateral flow velocity (ie, Vy).

[0088] For example, the first flow sensor measures a flow rate of L1 and a speed of V1, and the second flow sensor measures a flow rate of L2 and a speed of V2; the corresponding relationship (coefficient) between the speed and the flow rate is determined by the size of the sensor pipe, and the corresponding coefficients after calibration are a1 and a2 (a1 is the coefficient of the first flow sensor, and a2 is the coefficient of the second flow sensor. Theoretically, when the first flow sensor and the second flow sensor are the same, a1 is equal to a2, and the theoretical coefficient a can also be used for the convenience of calculation).

[0089] In the first embodiment of the present application, it is assumed that the installation angle of the first flow sensor is b1 = -45°, and the installation angle of the second flow sensor is b2 = 45°.

[0090] Then, V1=a1*L1(3);

[0091] V2=a2*L2 (4);

[0092] x-axis speed Vx=V1*cos(|b1|)+V2*cos(|b2|)=V1*cos(45)+V2*cos(45) (5);

[0094] Y-axis velocity Vy = V2*sin(|b2|)-V1*sin(|b1|)=V2*sin(45)-V1*sin(45) (6);

[0095] Wherein, V1*cos(45) is the first vertical flow, V1*sin(45) is the first transverse flow; V2*cos(45) is the second vertical flow, V2*sin(45) is the second transverse flow;

[0096] Please continue reading Figure 1 S200, respectively obtain the water flow velocity in different directions according to each pulse data, and calculate the relative moving speed of the underwater working robot relative to the swimming pool in the corresponding direction according to the motor speed of the underwater working robot and the acceleration and water flow velocity in the corresponding direction.

[0097] The water flow velocity includes: the vertical flow velocity in the forward direction and the lateral flow velocity in the lateral direction.

[0098] Exemplarily, after the pulse data is measured, the water flow velocity is calculated based on the pulse data. That is, the flow velocity (corresponding to Figure 2 The speed of the water flowing through the sensor is calculated in order to calculate the speed of the robot relative to the water in order to accurately measure the actual relative movement speed of the robot.

[0099] Then, based on the water flow velocity, the motor operating status (such as information such as motor speed) and the fused acceleration value (the combined acceleration of the underwater working robot), the relative moving speed of the underwater working robot relative to the swimming pool in different directions can be calculated.

[0100] That is, the pulse data measured by the flow sensor on each path is decomposed in the forward direction and the lateral direction respectively, and after the water flow velocity in the corresponding direction is obtained, the relative moving speed of the underwater working robot relative to the swimming pool in the forward direction is calculated according to the motor speed of the underwater working robot, the acceleration in the forward direction and the water flow velocity;

[0101] Similarly, the relative moving speed of the underwater working robot in the lateral direction relative to the swimming pool is calculated based on the motor speed of the underwater working robot, the acceleration in the lateral direction, and the water flow speed.

[0102] It can be understood that in the present application, by calculating the relative movement speed of the underwater working robot based on the motor speed, and the acceleration and water flow speed in the corresponding direction, the real relative movement speed of the underwater working robot relative to the pool is accurately calculated.

[0103] Furthermore, in another embodiment of the present application, before fusing the IMU posture information, it is first determined whether the robot is running (corresponding to Figure 2 (The machine is running): If it is running, the IMU acceleration and other information will be integrated; if it is not running, the pulse data obtained in this measurement will be cleared.

[0104] For example, in one embodiment, see Figure 5 Step S200, obtaining the water flow speed in different directions according to each pulse data, and calculating the relative moving speed of the underwater working robot relative to the swimming pool in the corresponding direction according to the motor speed of the underwater working robot and the acceleration and water flow speed in the corresponding direction, including:

[0105] S210, after calculating the product of the acceleration compensation coefficient and the acceleration in the lateral direction to obtain a first acceleration compensation value, and the product of the motor speed and the speed compensation coefficient in the lateral direction to obtain a first speed compensation value, calculate the relative movement speed in the lateral direction according to the first acceleration compensation value, the first speed compensation value and the lateral flow velocity;

[0106] S220, after calculating the product of the acceleration compensation coefficient and the acceleration in the forward direction to obtain a second acceleration compensation value, and the product of the motor speed and the speed compensation coefficient in the forward direction to obtain a second speed compensation value, the relative moving speed in the forward direction is calculated based on the second acceleration compensation value, the second speed compensation value and the vertical direct speed.

[0107] Demonstratively, the data measured by the flow meter contains various errors caused by the environment. The water flow velocities in two directions measured by the flow meter (including vertical flow velocity and lateral flow velocity) can be compensated by the motor speed and acceleration value, and the relative movement speed can be calculated based on a series of related coefficients.

[0108] Among them, it is set that: the motor speed is Vm; the compensation coefficient of the motor to the x-axis speed is cmx (i.e., the speed compensation coefficient in the forward direction), and the compensation coefficient to the y-axis speed is cmy (i.e., the speed compensation coefficient in the lateral direction); the x-axis acceleration value is ax (i.e., the acceleration in the forward direction, obtained by measuring the accelerator on the underwater working robot by the inertial sensor), and the y-axis acceleration value is ay (i.e., the acceleration in the lateral direction, also obtained by measuring the accelerator by the inertial sensor); the compensation coefficient of the acceleration to the x-axis is cax (i.e., the acceleration compensation coefficient in the forward direction), and the compensation coefficient to the y-axis speed is cay (i.e., the acceleration compensation coefficient in the lateral direction); the x-axis speed calibration coefficient is cx, and the y-axis speed calibration coefficient is cy (the speed calibration coefficient includes cx and cy, and the speed calibration coefficient is obtained by pre-measurement); the fused x-axis speed is Vrx (i.e., the relative moving speed of the underwater working robot relative to the swimming pool in the forward direction), and the fused y-axis speed is Vry (i.e., the relative moving speed of the underwater working robot relative to the swimming pool in the lateral direction).

[0109] Then, the calculation formulas for the x-axis speed Vrx and the y-axis speed Vry are:

[0110] Vrx=((1-cmx-cax)*Vx+cmx*Vm+cax*ax)*cx (7);

[0111] Among them, cmx*Vm is the second velocity compensation value, cax*ax is the second acceleration compensation value; cmx<1; cax<1; cmx+cax<1; cx>0.

[0112] Vry=((1-cmy-cay)Vy+cmy*Vm+cay*ay)*cy (8);

[0113] Among them, cmy*Vm is the first velocity compensation value; cay*ay is the first acceleration compensation value; cmy<1; cay<1; cmy+cay<1; cy>0.

[0114] Please continue reading Figure 1 , S300, according to the posture information and the angular velocity, respectively adjust the relative moving speed in different directions in real time to control the underwater working robot to operate along the preset working path at a preset speed in the target direction.

[0115] The target direction is the forward direction.

[0116] Demonstratively, after calculating the relative movement speed of the underwater working robot in two directions (mainly the forward direction and the lateral direction), the relative movement speed of the underwater working robot in the two directions is adjusted in real time according to the posture information and the angular velocity measured by the inertial sensor, so that the underwater working robot can operate on a preset working path at a preset speed in the forward direction, effectively improving the cleaning efficiency and cleaning coverage.

[0117] It can be understood that in this application, by adjusting the relative moving speed of the underwater working robot in real time according to the posture information and angular velocity, the underwater working robot can only work along the preset working path at a preset speed in the forward direction, so as to achieve precise control of the robot's working speed, thereby enabling it to travel in accordance with the preset working path specifications to improve working efficiency and accuracy. Moreover, it can achieve comprehensive work according to the working path, reduce work omissions, and achieve full work, thereby effectively improving the working effect.

[0118] In this application, the IMU posture can also be fused through the odometer speed (longitudinal speed) and drift speed (lateral speed), and the robot's movement trajectory can be planned and corrected based on the measured swimming pool contour map.

[0119] See also Figure 6 , controlling the underwater operation robot to operate along a preset operation path at a preset speed in a target direction, and previously also including:

[0120] A10. Use the ultrasonic sensor to find the first wall in the swimming pool that has the shortest straight-line distance to the underwater working robot.

[0121] A20. After driving to a preset position close to the first wall, the vehicle starts from the preset position at a preset speed and circles along the wall at the bottom of the swimming pool, and establishes a moving map based on the posture information during the circle.

[0122] Among them, the mobile map is used to plan the preset operation path; during the detour, the location where the obstacle is encountered is marked as the corresponding wall corner point of the swimming pool, and multiple wall corner points are obtained; any one of the multiple wall corner points is used as the starting point of the next operation.

[0123] For illustration, see also Figure 7A-7C In this embodiment, an ultrasonic sensor can be set on the right side of the robot body to measure the distance between the robot and the right wall, and the controller controls the distance between the robot and the wall to go around the wall and establish a swimming pool outline map.

[0124] So, before starting to work at the preset speed, first, establish the robot map coordinates:

[0125] The map coordinate system is a right-handed coordinate system (according to the different directions of the Z axis (depth), it is divided into "right-handed system" and "left-handed system". When the direction of the axis is (in the general two-dimensional rectangular coordinate system: the right is the X axis, the upward is the Y axis) extending from the front to the depth, the coordinate system is a left-handed system, otherwise it is a right-handed system), the robot's forward direction is the positive direction of the coordinate x axis, the horizontal left is the positive direction of the y axis, and the vertical upward is the positive direction of the z axis. The current coordinates of the robot are (x, y, z, theta); among them, x, y, z are the positions of the coordinate axes x, y, z respectively, and theta is the heading angle.

[0126] Then, the process of building a map and planning:

[0127] The first step is to initialize the map parameters after the robot is started, initialize the coordinate origin and the initial posture P0(x0,y0,z0,theta0)=(0,0,0,0), and set different attributes for the map pixels, including: unknown pixels, cleaned pixels, obstacle pixels and uncleaned pixels.

[0128] The robot's working modes include: pool bottom cleaning, water surface cleaning or wall cleaning; then, when the robot moves on the pool bottom and on the water surface, only the P'(x, y, theta) coordinate is updated, and when the robot moves on the wall, the P(x, y, z, theta) coordinate is updated. Then, in another embodiment of the present application, the working principle of the pool cleaning robot operating on the pool surface or on the swimming wall is the same or similar to the working principle of the underwater operation robot in the present embodiment.

[0129] The second step is to determine whether there is a wall on the right side of the robot based on the ultrasonic distance D measured by the ultrasonic sensor (set to determine that there is a wall when the distance D < 1M): if there is a wall, turn right 90°, and then move forward a distance D to find the wall (that is, the first wall at this time); if there is no wall, record the distance d1 and continue to turn right 90°, repeat the above action until d1, d2, d3 and d4 are recorded after three turns. Then, the smallest distance d is determined from d1, d2, d3 and d4, so as to use the ultrasonic sensor to find the first wall with the shortest straight-line distance to the underwater working robot in the swimming pool, that is, to use the ultrasonic sensor to find the wall closest to the underwater working robot. Secondly, turn to the direction corresponding to the minimum distance, that is, turn to the first wall, and move forward d, so as to drive to the preset position closest to the first wall.

[0130] In the third step, after reaching the preset position, the robot first turns left by 90°, resets the coordinate system and map parameters, and marks the starting coordinate of the detour Ps(xs, ys, zs, thetas), which is the preset position coordinate. Then, it performs operations by detouring along the wall at the preset speed, controls the robot to move forward, and always adjusts the distance measured by the ultrasonic wave to the distance from the wall to DW (DW is the safe distance between the robot and the wall, that is, the preset distance, for example, DW = 5 cm) through PID control during the forward movement process, establishes a pool contour map, and marks it as obstacle pixels. Secondly, when it is judged that the detour angle meets the 360° closure (the absolute value of the detour angle AR: 350° < AR < 370°), and the current coordinate P(x, y, z, theta) coincides with the starting coordinate Ps(xs, ys, zs, thetas), that is, |P - Ps| <= L (the expanded calculation formula is: sqrt((x - xs)^2 + (y - ys)^2)) <= L(9) (where L is the preset condition for judging the coincidence of two points) (because there is no displacement in the Z-axis direction, so only the XY plane is considered, and the calculation can be simplified) and L is set to 30 cm), it is considered that one circle of detour is completed.

[0131] Among them, when encountering the first wall corner point that cannot be moved during the detour process, mark the coordinate of the wall corner point P1, and turn left by 90°. Then, mark the coordinates of other wall corner points P2, P3, and P4 in sequence, so as to obtain multiple wall corner points. For the schematic diagram of the marking of the four wall corner points, please refer to Figure 8 .

[0132] Finally, establish a movement map according to the attitude information during the detour process, and the movement map is used to plan the preset operation path.

[0133] In this application, the output of the motor is adjusted according to the data fed back by the ultrasonic wave to control the walking direction of the machine, so as to control the robot to walk at a certain distance DW from the wall.

[0134] For example, in one implementation manner, step S300: According to the attitude information and angular velocity, respectively and real-time adjust the relative movement speed in different directions to control the underwater operation robot to operate along the preset operation path at the preset speed in the target direction, including:

[0135] S310: According to the attitude information and angular velocity of the underwater operation robot, use the proportional integral differential control algorithm to real-time adjust the relative movement speed in the forward direction to the preset speed, and adjust the relative movement speed in the lateral direction to zero, so that the underwater operation robot operates along the preset operation path.

[0136] Exemplarily, after the relative moving speeds in the forward direction and in the lateral direction are calculated, during the entire operation process (including circling around the wall and working along the preset operation path), the proportional integral differential control algorithm (i.e., PID controller / regulator) is always used to adjust the relative moving speed in the forward direction to the preset speed in real time, and the relative moving speed in the lateral direction is adjusted to zero, so that the underwater operation robot can work along the preset operation path at a uniform speed according to the preset speed. At the same time, the points or sections that have been reached are marked as cleaned pixels.

[0137] Then, please refer to the planning cleaning instructions Fig. 9 , the specific process of operating on the preset operating path at the preset speed is as follows:

[0138] Step 1. After marking four wall corner points in one circle, continue walking forward along the wall to the first wall corner point P1 (in this embodiment, the first wall corner point P1 is the starting point of the second operation). If it is judged that |P-P1|<=L(10), it means that P1 has been reached. After reaching P1, turn left 90° and continue to move to P2. If it is judged that |P-P2|<=L(11), it means that the second wall corner point P2 has been reached, and the starting coordinate P0 is recorded.

[0139] Step 2: Start left turn channel cleaning:

[0140] First, turn left 90° to enter the first channel and start cleaning, then go straight and record the straight distance d until the stop condition is triggered.

[0141] Among them, the straight-ahead stopping conditions are: 1. Encountering a wall or obstacle (the robot can be identified as being blocked by an obstacle or wall by judging that the longitudinal odometer speed is much smaller than the preset speed (0 or close to 0 and much smaller than the expected speed V2 obtained by the motor output), and starts to turn to enter the next channel after being blocked by the wall or obstacle); 2. d>=LW, where LW is a preset distance slightly smaller than the width of the fuselage.

[0142] Then, continue to turn left 90° and go straight, and continue to perform PID control adjustment to make the robot's lateral speed (relative moving speed in the lateral direction) 0 and forward speed (relative moving speed in the forward direction) V1. After that, if it encounters a wall or obstacle, it determines whether the machine has cleaned the opposite side of the second operation starting point (corresponding to Fig. 9 The side formed by the third wall corner point P3 and the fourth wall corner point P4): that is, if it is judged that |P-P3|<=L(12) or |P-P4|<=L(13), it means that the opposite side has been cleaned, and the re-sweeping step is entered to re-sweep the missed section, otherwise go to step 3 and enter the right turn channel.

[0143] Step 3: Start cleaning the right turn channel:

[0144] Similarly, turn right 90° first, and then go straight for a distance d until the stop condition is triggered.

[0145] Among them, the straight-ahead stopping conditions are: 1. Encountering a wall or obstacle; 2. d>=LW.

[0146] Then, continue to turn right 90° and go straight, and similarly, keep adjusting the PID to make the robot's lateral speed 0 and forward speed V1. After that, if it encounters a wall or obstacle, determine whether the machine has cleaned the opposite side of the second operation starting point: if it is determined that |P-P3|<=L or |P-P4|<=L, it means that the machine has cleaned the opposite side, and then enter the re-sweeping step, otherwise go to step 2.

[0147] in, Fig. 9 In the figure, according to the turning direction of the robot, the channel can be divided into a left-turn channel and a right-turn channel. Starting from P2, the first preset operation path and the second preset operation path are both left-turn channels, and the next third preset operation path and the fourth preset operation path are both right-turn channels.

[0148] For example, in one embodiment, see Fig.10 , after controlling the underwater operation robot to operate along a preset operation path at a preset speed in a target direction, it also includes:

[0149] A1000, marking the road section where the operation has been completed to obtain a marked path;

[0150] A2000, after reaching the end point of the preset operation path, judging whether there is a target road section for missing operation according to the marked path, and if so, performing additional operation on the target road section;

[0151] A3000: If the supplementary operation on the current target road section reaches a preset time or a preset number of times, the supplementary operation is terminated and the supplementary operation is performed on the next target road section.

[0152] Exemplarily, the specific process of performing the operation on the preset operation path at the preset speed also includes:

[0153] Step 4: Mark the completed sections to obtain a marked path. After reaching the end of the preset operation path, compare the marked path with the target path to determine whether there is a target section of the missed operation. That is, after completing the last route (corresponding to Fig. 9 After the operation of the route from P4 to P3 in the figure, check whether there are reachable uncleared pixels in the map:

[0154] If there is, navigate to the uncleaned pixel point for supplementary scanning; if not, judge that the cleaning is completed. Among them, if the attempt to supplement the current target road section exceeds the set time or the preset number of times, it is judged that the cleaning is completed, the supplementary operation is ended, and the supplementary operation is performed on the next target road section.

[0155] For example, in one embodiment, see Fig.11 , the embodiment of the present application provides a control system of an underwater working robot, including:

[0156] The acquisition module 1 is used to acquire the angular velocity, acceleration and attitude information measured by the inertial sensor, and the pulse data measured by different flow sensors.

[0157] The calculation module 2 is used to obtain the water flow velocity in different directions according to each pulse data, and calculate the relative moving speed of the underwater working robot relative to the swimming pool in the corresponding direction according to the motor speed of the underwater working robot and the acceleration and water flow velocity in the corresponding direction.

[0158] The control module 3 is used to adjust the relative moving speed in different directions in real time according to the posture information and the angular velocity, so as to control the underwater working robot to operate along a preset working path at a preset speed in the target direction.

[0159] Exemplarily, the control method of the underwater working robot is applicable to the control system of the underwater working robot. Then, the implementation process of the control system of the underwater working robot is similar to the implementation process of the control method of the underwater working robot, as shown below:

[0160] First, when the underwater working robot is launched into the water, two flow sensors arranged perpendicular to each other on the underwater working robot measure pulse data in real time (including the first pulse data measured by the first flow sensor and the second pulse data measured by the second flow sensor). Then, the water flow velocities in different directions are calculated based on the pulse data, including the vertical flow velocity and the lateral flow velocity.

[0161] Then, after the inertial sensor measures the angular velocity and posture information of the underwater working robot, the relative movement speed of the robot relative to the swimming pool in different directions is calculated by combining the water flow velocity, the motor operating status (including the motor speed) and the fused acceleration value.

[0162] Finally, the relative moving speed of the underwater working robot in the corresponding direction is adjusted in real time according to the posture information and angular velocity, that is, the relative moving speed in the forward direction is adjusted to the preset speed V1, and the relative moving speed in the lateral direction is adjusted to 0, so that the underwater working robot moves at the preset speed and corrects the movement route so that the robot can work along the preset working path.

[0163] In this application, a flow sensor is used to measure the water flow rate when the robot moves to obtain the flow velocity, thereby calculating the speed of the robot relative to the water. The relative movement speed of the robot relative to the swimming pool is estimated by combining the water flow velocity, the motor operating status and the fused acceleration value. The relative movement speed is then fused with the posture of the IMU to establish a map of the robot's movement trajectory, thereby improving the efficiency of planning and cleaning, and increasing the cleaning coverage rate.

[0164] The present application also provides an underwater working robot. Exemplarily, the underwater working robot includes a processor and a memory, wherein the memory stores a computer program, and the processor runs the computer program to enable the underwater working robot to execute the above-mentioned control method of the underwater working robot or the functions of each module in the control system of the above-mentioned underwater working robot.

[0165] Among them, the processor can be an integrated circuit chip with signal processing capabilities. The processor can be a general-purpose processor, including a central processing unit (CPU), a graphics processing unit (GPU) and a network processor (NP), a digital signal processor (DSP), an application-specific integrated circuit (ASIC), a field programmable gate array (FPGA) or at least one of other programmable logic devices, discrete gates or transistor logic devices, and discrete hardware components. The general-purpose processor can be a microprocessor or the processor can also be any conventional processor, etc., which can implement or execute the disclosed methods, steps and logic block diagrams in the embodiments of the present application.

[0166] The memory may be, but is not limited to, a random access memory (RAM), a read only memory (ROM), a programmable read-only memory (PROM), an erasable programmable read-only memory (EPROM), an electrically erasable programmable read-only memory (EEPROM), etc. The memory is used to store a computer program, and the processor may execute the computer program accordingly after receiving an execution instruction.

[0167] The present application also provides a computer-readable storage medium for storing the computer program used in the underwater operation robot. For example, the computer-readable storage medium may include but is not limited to: a USB flash drive, a mobile hard disk, a read-only memory (ROM), a random access memory (RAM), a magnetic disk or an optical disk, and other media that can store program codes.

[0168] In several embodiments provided in the present application, it should be understood that the disclosed devices and methods can also be implemented in other ways. The device embodiments described above are merely schematic. For example, the flowcharts and structure diagrams in the accompanying drawings show the possible architecture, functions and operations of the devices, methods and computer program products according to multiple embodiments of the present application. In this regard, each box in the flowchart or block diagram can represent a module, a program segment or a part of a code, and the module, a program segment or a part of a code contains one or more executable instructions for implementing the specified logical function. It should also be noted that in an alternative implementation, the functions marked in the box can also occur in a different order from the order marked in the accompanying drawings. For example, two consecutive boxes can actually be executed substantially in parallel, and they can sometimes be executed in the opposite order, depending on the functions involved. It should also be noted that each box in the structure diagram and / or the flow diagram, and the combination of boxes in the structure diagram and / or the flow diagram, can be implemented with a dedicated hardware-based system that performs a specified function or action, or can be implemented with a combination of dedicated hardware and computer instructions.

[0169] In addition, the functional modules or units in the various embodiments of the present application may be integrated together to form an independent part, or each module may exist separately, or two or more modules may be integrated to form an independent part.

[0170] If the functions are implemented in the form of software function modules and sold or used as independent products, they can be stored in a computer-readable storage medium. Based on this understanding, the technical solution of the present application, or the part that contributes to the prior art, or the part of the technical solution, can be embodied in the form of a software product, which is stored in a storage medium and includes several instructions for a computer device (which can be a smart phone, a personal computer, a server, or a network device, etc.) to perform all or part of the steps of the methods described in the various embodiments of the present application.

[0171] The above description is only a specific implementation manner of the present application, but the protection scope of the present application is not limited thereto. Any technician familiar with the technical field can easily think of changes or substitutions within the technical scope disclosed in the present application, which should be included in the protection scope of the present application.

Claims

1. A control method for an underwater working robot, characterized in that: include: Obtain angular velocity, acceleration and attitude information measured by inertial sensors, as well as pulse data measured by different flow sensors; The water flow speed in different directions is obtained according to each pulse data, and the relative moving speed of the underwater working robot relative to the swimming pool in the corresponding direction is calculated according to the motor speed of the underwater working robot, the acceleration in the corresponding direction and the water flow speed; According to the posture information and the angular velocity, the relative movement speeds in different directions are respectively adjusted in real time to control the underwater working robot to operate along a preset working path in a target direction at a preset speed.

2. The control method of the underwater working robot according to claim 1, characterized in that: After controlling the underwater working robot to operate along a preset working path in a target direction at a preset speed, the method further includes: Mark the road sections where the work has been completed to obtain a marked path; After reaching the end point of the preset operation path, judging whether there is a target road section where the operation is missed according to the marked path, and performing additional operation on the target road section if there is a target road section where the operation is missed; If the supplementary operation on the current target road section reaches a preset time or a preset number of times, the supplementary operation is terminated and the supplementary operation is performed on the next target road section.

3. The control method of the underwater working robot according to claim 1, characterized in that: The controlling the underwater working robot to operate along a preset working path in a target direction at a preset speed also includes: Using an ultrasonic sensor to find a first wall in the swimming pool that has the shortest straight-line distance to the underwater working robot; After driving to a preset position close to the first wall, starting from the preset position at the preset speed, circling the wall at the bottom of the swimming pool, and establishing a moving map according to the posture information during the circling process; The mobile map is used to plan the preset operation path; during the detour, the location where the obstacle is encountered is marked as the corresponding wall corner point of the swimming pool to obtain multiple wall corner points; any one of the multiple wall corner points is used as the starting point of the next operation.

4. The control method of the underwater working robot according to claim 3, characterized in that: The step of adjusting the relative movement speeds in different directions in real time according to the posture information and the angular velocity to control the underwater working robot to operate along a preset working path in a target direction at a preset speed includes: According to the posture information and angular velocity of the underwater working robot, a proportional-integral-differential control algorithm is used to adjust the relative moving speed in the forward direction to a preset speed in real time, and to adjust the relative moving speed in the lateral direction to zero, so that the underwater working robot can operate along the preset working path.

5. The control method of the underwater working robot according to claim 1, characterized in that: The water flow velocity includes: a vertical flow velocity in the forward direction and a lateral flow velocity in the lateral direction; When the directions of the two flow sensors correspond to the same forward direction and the lateral direction, the water flow speeds in different directions are obtained according to the pulse data, including: After obtaining first pulse data measured by a first flow sensor installed in the forward direction in real time, the first pulse data is used as the vertical flow velocity; After acquiring in real time second pulse data measured by a second flow sensor installed in the lateral direction, the second pulse data is used as the lateral flow velocity; When the directions of the two flow sensors are at an angle to both the forward direction and the lateral direction, the water flow velocities in different directions are obtained according to the pulse data, including: After acquiring the first pulse data and the second pulse data correspondingly measured by the first flow sensor and the second flow sensor in real time, decomposing the first pulse data into a first vertical flow in a forward direction and a first lateral flow in a lateral direction of the first flow sensor according to a coefficient and an installation angle of the first flow sensor; Decomposing the second pulse data into a second vertical flow rate of the second flow sensor in a forward direction and a second lateral flow rate in a lateral direction according to a coefficient and an installation angle of the second flow sensor; Calculating the vertical flow rate according to the first vertical flow rate and the second vertical flow rate; The lateral flow velocity is calculated according to the first lateral flow rate and the second lateral flow rate.

6. The control method of the underwater working robot according to claim 5, characterized in that: The water flow speed in different directions is obtained according to each pulse data, and the relative moving speed of the underwater working robot relative to the swimming pool in the corresponding direction is calculated according to the motor speed of the underwater working robot, the acceleration in the corresponding direction and the water flow speed, including: After calculating the product of the acceleration compensation coefficient and the acceleration in the lateral direction to obtain a first acceleration compensation value, and the product of the motor speed and the speed compensation coefficient in the lateral direction to obtain a first speed compensation value, the relative movement speed in the lateral direction is calculated according to the first acceleration compensation value, the first speed compensation value and the lateral flow velocity; After calculating the product of the acceleration compensation coefficient and the acceleration in the forward direction to obtain a second acceleration compensation value, and the product of the motor speed and the speed compensation coefficient in the forward direction to obtain a second speed compensation value, the relative moving speed in the forward direction is calculated based on the second acceleration compensation value, the second speed compensation value and the vertical direct speed.

7. A control system for an underwater working robot, characterized in that: include: An acquisition module is used to obtain the angular velocity, acceleration and attitude information measured by the inertial sensor, as well as the pulse data measured by different flow sensors; A calculation module, used to obtain the water flow speed in different directions according to each pulse data, and calculate the relative moving speed of the underwater working robot relative to the swimming pool in the corresponding direction according to the motor speed of the underwater working robot, the acceleration in the corresponding direction and the water flow speed; The control module is used to adjust the relative movement speed in different directions in real time according to the posture information and the angular velocity, so as to control the underwater working robot to operate along a preset working path in a target direction at a preset speed.

8. An underwater working robot, characterized in that: include: A flow sensor, an inertial sensor, a motor, a processor and a memory, wherein the memory stores a computer program, and the processor is used to execute the computer program to implement the control method of the underwater working robot according to any one of claims 1 to 6.

9. The underwater working robot according to claim 8, characterized in that: There are two flow sensors, including a first flow sensor and a second flow sensor; the first flow sensor and the second flow sensor are arranged perpendicular to each other and are both arranged on the shell of the underwater working robot.

10. A computer-readable storage medium, characterized in that: It stores a computer program, which, when executed on a processor, implements the control method of the underwater working robot according to any one of claims 1-6.

Images