A posture recognition method for an automatic cleaning robot for underground water tanks

Abstract

The present invention provides a posture recognition method for an automatic cleaning robot for underground water tanks, comprising the following steps: S1 determines the position of the robot; S2 determines the posture of the robot; S3 determines the posture of the robot. In order to solve the problems of image texture loss caused by depth camera positioning due to uneven illumination in the well, uneven detection of laser radar sensors caused by large dust and noise in the well, poor filtering effect due to large noise, large calculation amount, and inaccurate measurement caused by cumulative errors of inertial measurement units, the present invention adopts a laser ranging sensor and an inertial measurement device, and adopts a mathematical calculation model to realize the posture recognition of the automatic cleaning robot for water tanks. This recognition method is not limited by the underground environment, and makes full use of the special environment in the well to realize the posture recognition of the automatic cleaning robot for water tanks.

Description

Technical Field

[0001] The invention belongs to the field of posture recognition, and in particular relates to a posture recognition method for an automatic cleaning robot for an underground water tank. Background Art

[0002] In the process of coal mining, the cleaning of underground water tanks plays an important role, but the current methods of cleaning are mainly: manual cleaning, vacuum pump extraction and manual driving of engineering machinery cleaning; manual cleaning is the method currently adopted by most companies. Workers carry cleaning tools such as shovels to load the coal sludge in the water tank into the car and then transport it out of the water tank. The workers have high labor intensity, low cleaning efficiency and are very easy to get injured and cause safety accidents; vacuum pump extraction mainly uses the pressure difference between the mud storage tank and the mud suction pipe to suck the coal sludge into the mud storage tank. Due to the sedimentation and separation of coal sludge and water inside the water tank, the pressure difference cleaning efficiency is low, and the service life of various pipes and valves is short, which is not conducive to long-term work; although manual driving of engineering machinery cleaning can reduce people's labor intensity and improve production efficiency to a certain extent, it cannot clean the water tank cleanly and thoroughly. Therefore, designing an automatic water tank cleaning robot is an urgent problem to be solved.

[0003] Affected by the special environment underground, the posture determination of the automatic water tank cleaning robot has become one of the key technologies to solve this problem. Common positioning methods include the following: (1) geomagnetic positioning, which is to lay metal wires or magnetic tracks on the driving track of the AGV and load the guidance frequency on the metal wire to form a magnetic field. This positioning method is costly and difficult to operate; (2) visual positioning, which uses cameras to collect and process images, and feed back the processed images in real time, and compare them with the real-time position to achieve robot positioning. However, due to the limitations of underground lighting conditions, the features of the collected images are not obvious, resulting in low positioning accuracy and other problems; (3) GPS positioning, whose principle is to determine the position of the receiver by measuring the distance between the satellite and the receiver at different positions. Due to the limitations of the special environment underground, GPS signals cannot be received; (4) inertial positioning, which uses an inertial measurement unit to measure the motion parameters such as the acceleration and linear velocity of the relative inertial space of the moving carrier to obtain the position parameters of the moving carrier. Due to the influence of cumulative errors, the error will be infinitely expanded and the operation is unreliable; (5) lidar positioning, whose working principle is to use lidar to continuously transmit detection signals to the target, and then compare the received reflected signals with the transmitted signals to obtain the target's relevant position information, but it will be affected by dust, resulting in low positioning accuracy. Summary of the invention

[0004] The purpose of the implementation of the present invention is to propose a method for position recognition of a cleaning robot based on a laser radar ranging sensor in order to solve the problem of inaccurate positioning of a mine environment water tank cleaning robot in the prior art.

[0005] Since the above positioning methods have poor adaptability to the special working environment underground, the present invention proposes a method of using an IMU module to measure the posture angle and a laser ranging sensor to measure the distance. The IMU measures the posture angle, which is not affected by the motion state and is only related to the posture of the robot at this time, greatly reducing the cumulative error of the IMU. The laser ranging sensor is not affected by light and dust and has high measurement accuracy. The organic combination of the two can accurately identify the posture of the sump cleaning robot.

[0006] A posture recognition method for an underground water tank automatic cleaning robot comprises the following steps:

[0007] S1 determines the position of the robot;

[0008] S2 determines the posture of the robot;

[0009] S3 determines the position and posture of the robot.

[0010] Wherein, the S1 specifically includes:

[0011] S1.1 measures the distance between the robot and the two sides of the underground coal slime tunnel through the laser ranging sensors 1 to 4 installed on both sides of the robot. The laser ranging sensors 1 to 4 are symmetrically arranged on both sides of the water tank automatic cleaning robot. The sensor arrangement plane is in the same plane as the center of gravity of the robot. The laser ranging sensors 1 and 2 are installed on the left side from front to back, and the laser ranging sensors 3 and 4 are installed on the right side from back to front. The measuring direction of the laser ranging sensor is perpendicular to the installation direction of the sensor and does not change with the movement of the robot. The laser pulse is emitted by the laser diode and then received by the receiving device on the sensor. The distance is determined by processing the time from the light pulse being emitted to returning. When the robot moves, the distance between the robot and the underground coal slime tunnel is measured, and the measured value is recorded as d1 to d4.

[0012] The horizontal rangefinder and laser rangefinder sensor combination module is installed at the center position 5 on the upper surface of the robot. When the robot is on the horizontal plane, the center of gravity and the center of gravity of the horizontal rangefinder and laser rangefinder sensor combination module should be in the vertical direction; the laser rangefinder is used to measure the vertical distance from the upper surface of the robot to the underground coal slime tunnel, recorded as d5, and the horizontal rangefinder is used to measure the horizontal distance Iy' from the center point of the upper surface to the entrance of the underground coal slime tunnel;

[0013] The inertial measurement unit IMU is installed at the robot's center of gravity position 6, and is used to measure the straight-line distance Iy of the robot's center of gravity position along the direction of the underground coal slime tunnel and record the angular velocity and angular acceleration of the robot during movement; the angular velocity and angular acceleration measured by the inertial measurement unit IMU are processed by the extended Kalman filter method to preliminarily estimate the robot's posture; the posture angle is directly obtained from the laser observation data, which effectively avoids the influence of the IMU cumulative error, and can be used to correct the estimated posture and improve the measurement accuracy;

[0014] S1.2 Establish an underground coal slime tunnel model. The width of the underground coal slime tunnel is 2D. Establish a spatial rectangular coordinate system o-xyz. The origin o is the center position of the underground coal slime tunnel entrance. The bottom surface of the underground coal slime tunnel is the xoy plane. The y axis is the center line of the bottom of the underground coal slime tunnel. The height direction of the underground coal slime tunnel is the z axis. The function equation of the height arch of the underground coal slime tunnel is z=H(x).

[0015] The coordinate axis o'-x'y'z' at the center of gravity M of the robot is determined by the right-hand rule, and the coordinate axis z' is consistent with the direction of the laser ranging sensor of the horizontal rangefinder and laser ranging sensor combination module;

[0016] The robot can be regarded as a rectangular parallelepiped model, whose length, width and height are a, b and h respectively;

[0017] S1.3 Since the laser distance measuring sensors are perpendicular to the installation plane and the two are located on the same straight line, the robot can be vertically projected onto the xoy plane. The position of the robot's center of gravity point M0 is located at the center of the projection plane. The x coordinate of point M0 can be determined, and its value is:

[0018]

[0019] S1.4 The vertical distance from point M to the starting position is calculated by the inertial measurement unit IMU, that is, the y coordinate of point M0 is Iy;

[0020] S1.5 The vertical distance from the upper surface of the robot to the coal slime tunnel in the mine is measured by the combination module of the horizontal rangefinder and the laser rangefinder sensor, which is recorded as d5. The horizontal rangefinder is used to measure the horizontal distance from the center point M' of the upper surface to the tunnel entrance, which is recorded as Iy'.

[0021] Project the robot parallel to the YOZ plane. The distance from its center of gravity M0 to the y-axis is the z-coordinate of M0, whose value is:

[0022]

[0023] in, represents the x-coordinate of the position at that moment, δ=(I y -I y′ ) 2Indicates the deviation between the projected position of point M and its actual position.

[0024] Among them, in S1.1 of said S1,

[0025] For laser ranging sensors, horizontal rangefinders, and inertial measurement units (IMUs), the collected data needs to be filtered. First, an amplitude limiter filter is performed, and then a recursive average filter is performed to obtain the measurement value and enter the next step of data processing.

[0026] Wherein, the S2 specifically includes:

[0027] S2.1 The posture of the robot is determined by three angles, namely, the heading angle α is the angle between the projection of the y' axis on the bottom surface and the y axis, calculated in the xoy plane, the pitch angle β is the angle between the y' axis and the xoy plane, calculated in the yoz plane, and the roll angle γ is the angle between the plane where the robot is located and the horizontal plane, calculated in the xoz plane;

[0028] S2.2 Calculation of heading angle α: The robot can be projected onto the xoy plane and its cosine value can be calculated: but

[0029] S2.3 Calculation of pitch angle β: The robot can be projected onto the yoz plane and its cosine value can be calculated:

[0030]

[0031] S2.4 Calculation of the rolling angle γ. The robot can be projected onto the xoz plane and its cosine value can be calculated:

[0032]

[0033]

[0034] S2.5 The above attitude angle is obtained by observing the laser ranging sensor observation value through plane projection method and using inverse trigonometric function. There is a certain gap between the value directly observed by IMU, so the two need to be fused;

[0035] The inertial measurement module IMU has cumulative errors, so its confidence will be correspondingly lower, while the observation data error is small, but there is a problem of calculation accuracy. Weights are assigned separately and their linear combination is used as the output value;

[0036] The above-mentioned posture angle, hereinafter referred to as the calculated value a, should be fused with the posture angle of the above-mentioned inertial measurement device, hereinafter referred to as the observed value b, to obtain the posture angle of the underground water tank cleaning robot;

[0037] The fusion adopts the method of First Order Complementary Filter, that is, the weights are assigned to the two to obtain a new angle as the output value to represent the actual posture of the underground water tank cleaning robot;

[0038] When the calculated value is equal to the observed value, the calculated value is output; when the calculated value is not equal to the observed value, the following formula is used for calculation:

[0039] c=(1-η)a+ηb

[0040] Error rate The larger the error value, the greater the proportion of the observed value.

[0041] Wherein, the S3 is specifically:

[0042] In the robot coordinate system o'-x'y'z', it can be known from the rigid body assumption that the position of each point of the robot relative to the center of gravity remains unchanged. Through the transformation matrix J, the coordinates of the robot relative to the underground coal slime tunnel coordinate system o-xyz, that is, the position coordinates of the robot, can be obtained;

[0043] The conversion formula is: M = M0J, which is the coordinate conversion formula of the center of gravity point. The conversion formula of any point can be easily obtained;

[0044] The expression of the transformation matrix J is as follows:

[0045]

[0046] A set of quaternions is used to represent the robot's position and posture, that is, C = [α, β, γ, r] T ,

[0047] Among them, x M is the x coordinate of the center position, y M is the y coordinate of the center position, z M is the z coordinate of the center position.

[0048] The position and posture of the robot in S3 can be transmitted to a receiving device such as a display, a mobile phone, or a computer through a signal transmitting device and a wireless local area network.

[0049] Among them, the size deformation rate of the underground water tank automatic cleaning robot is less than 1%, so the robot can be assumed to be a rigid body.

[0050] The signal acquisition devices used include but are not limited to sensors, which can be connected to the control mainboard via the RS485 bus.

[0051] Among them, the control main board can process the data through the CPU to obtain the position information of the underground water tank automatic cleaning robot.

[0052] Technical effects of the present invention:

[0053] In order to solve the problems of image texture loss caused by depth camera positioning due to uneven illumination underground, uneven detection of lidar sensors caused by large dust and noise underground, poor filtering effect due to large noise, large calculation amount, and inaccurate measurement caused by cumulative errors of inertial measurement units, the present invention adopts laser ranging sensors and inertial measurement devices, and adopts mathematical calculation models to realize the position and posture recognition of the automatic water tank cleaning robot. This recognition method is not limited by the underground environment and makes full use of the special environment underground to realize the position and posture recognition of the automatic water tank cleaning robot. BRIEF DESCRIPTION OF THE DRAWINGS

[0054] The accompanying drawings generally illustrate various embodiments by way of example and not limitation, and together with the description and claims, serve to illustrate the embodiments of the invention. Where appropriate, the same reference numerals are used throughout the drawings to refer to the same or similar parts. Such embodiments are illustrative and are not intended to be exhaustive or exclusive embodiments of the present apparatus or method.

[0055] Figure 1 Calculate the process flow of the overall solution of the present invention;

[0056] Figure 2 It is the xoy plane projection diagram of the present invention;

[0057] Figure 3 It is the yoz plane projection diagram of the present invention;

[0058] Figure 4 It is the xoz surface projection diagram of the present invention;

[0059] Figure 5 It is a cross-sectional view of the tunnel of the present invention;

[0060] Figure 6 The IMU module installation position of the present invention;

[0061] Figure 7 This is a cross-sectional view of the lane cleaning robot of the present invention;

[0062] Figure 8 A top view of the lane cleaning robot of the present invention;

[0063] Fig. 9 It is a schematic diagram of data transmission according to the present invention. DETAILED DESCRIPTION

[0064] In order to more clearly understand the above-mentioned purpose and features of the present invention, the present invention is further described in detail below in conjunction with the accompanying drawings and specific embodiments. It should be noted that the embodiments of the present application and the features in the embodiments can be combined with each other without conflict.

[0065] In the following description, many details are set forth to facilitate a full understanding of the present invention. However, the present invention may also be implemented in other ways different from those described herein. Therefore, the protection scope of the present invention is not limited to the specific embodiments disclosed below.

[0066] The laser ranging sensor adopts the TOF laser ranging module SK-Z series, with a measurement range of 0.06-30m, CM-level measurement accuracy, and a maximum measurement frequency of 100Hz.

[0067] The horizontal distance meter adopts PLS-T laser distance sensor with a maximum measurement range of 70 meters, CM-level measurement accuracy, and a measurement frequency of 305KHz which can be adjusted by itself.

[0068] The inertial measurement unit uses the FDISYSTEMS industrial waterproof-grade attitude sensor, which features a built-in Kalman filter and can directly output the attitude angle, but there is a large deviation.

[0069] Laser rangefinders, horizontal rangefinders, inertial measurement devices, etc. are mainly used to collect data and are connected to the control mainboard through the SR485 communication interface on the above devices (including but not limited to sensors);

[0070] The collected data is transmitted to the microprocessor of the control mainboard through the communication interface, and the collected data is processed and calculated;

[0071] The first step is to read the sensor data. In the Python environment, import the GPIO module, sys module, time module, panda module, serial module and sensor-related data module, define the main function and related parameters to read the data;

[0072] Since the sensor reads hexadecimal data, it needs to be converted into decimal data for processing;

[0073] Secondly, filtering is performed to reduce noise and enhance the required features. The filtering algorithm uses the Amplitude Limiter Filter algorithm combined with the Recursive Average Filtering algorithm.

[0074] The characteristic of limiting filtering is that it can effectively avoid the interference of pulse signals caused by accidental factors, but its disadvantage is that it cannot effectively filter periodic signals;

[0075] Recursive average filtering can effectively avoid the noise caused by periodic signals, but its disadvantage is that it cannot filter the noise caused by accidental factors.

[0076] Here, limiting filtering is performed first, and then recursive averaging filtering is performed. The two are effectively combined, which can not only reduce the noise caused by accidental factors, but also effectively avoid the errors caused by periodic signals.

[0077] Finally, the filtered data is processed to obtain the x, y, and z coordinate information of the robot's center point. The coordinate values ​​are in, represents the x-coordinate of the position at that moment, δ=(I y -I y′ ) 2 Indicates the deviation between the projection position of point M and the actual position;

[0078] Its attitude angle is the heading angle Pitch Angle Roll Angle

[0079] The above-mentioned posture angle (hereinafter referred to as calculated value a) should be integrated with the posture angle of the above-mentioned inertial measurement device (hereinafter referred to as observed value b) to obtain the posture angle of the underground water tank cleaning robot.

[0080] The fusion adopts the method of first order complementary filter, that is, the weights are assigned to the two to obtain a new angle as the output value to represent the actual posture of the underground water tank cleaning robot;

[0081] When the calculated value is equal to the observed value, the calculated value is output; when the calculated value is not equal to the observed value, the following formula is used for calculation:

[0082] c=(1-η)a+ηb

[0083] Error rate The larger the error value, the greater the proportion of the observed value.

[0084] A coordinate system is established at the center point, and the position of any point relative to the origin coordinate system can be obtained through the transformation matrix. The transformation matrix is ​​as follows:

[0085]

[0086] At this point, the robot's position and posture are determined, which can be represented by a 4x1 matrix, namely C = [α, β, γ, r] T , (x M is the x coordinate of the center position, y M is the y coordinate of the center position, zM is the z coordinate of the center position)

[0087] The above-mentioned posture data can be sent to mobile phones, displays, control centers and other terminals through the local area network for viewing, and the posture of the underground water tank cleaning robot can also be visualized;

[0088] By constructing a three-dimensional point cloud map of the tunnel and inputting the robot's posture information, the actual working status of the robot at this time can be obtained.

[0089] The above description is only a preferred embodiment of the present invention. It should be pointed out that for ordinary technicians in this field, without departing from the creative concept of the present invention, they are not limited to the embodiments, and all of them belong to the protection scope of the present invention.

Claims

1. A posture recognition method for an automatic cleaning robot for underground water tanks, characterized in that: The following steps are involved: S1 determines the robot’s position; S2 determines the robot’s posture; S3 determines the position and posture of the robot; The S1 specifically includes: S1.1 The distance between the robot and the two sides of the underground coal slime tunnel is measured by the laser ranging sensors 1~4 installed on both sides of the robot. The laser ranging sensors 1~4 are symmetrically arranged on both sides of the water tank automatic cleaning robot. The sensor layout plane is in the same plane as the center of gravity of the robot. The laser ranging sensors 1 and 2 are installed on the left side from front to back, and the laser ranging sensors 3 and 4 are installed on the right side from back to front. The measurement direction of the laser ranging sensor is perpendicular to the installation direction of the sensor and does not change with the movement of the robot. The laser pulse is emitted by the laser diode and then received by the receiving device on the sensor. The distance is determined by processing the time from the light pulse to the return. When the robot moves, the distance between the robot and the underground coal slime tunnel is measured, and the measured values ​​are recorded as d1~d4, where d1 means the measurement value of the laser ranging sensor 1, d2 means the measurement value of the laser ranging sensor 2, d3 means the measurement value of the laser ranging sensor 3, and d4 means the measurement value of the laser ranging sensor 4. The horizontal rangefinder and laser rangefinder sensor combination module are installed at the center of the upper surface of the robot. When the robot is on the horizontal plane, the center of gravity and the center of gravity of the horizontal rangefinder and laser rangefinder sensor combination module should be in the vertical direction; the laser rangefinder is used to measure the vertical distance from the upper surface of the robot to the underground coal slime tunnel, recorded as d5, and the horizontal rangefinder is used to measure the horizontal distance Iy' from the center point of the upper surface to the entrance of the underground coal slime tunnel; The inertial measurement unit (IMU) is installed at the center of gravity of the robot to measure the straight-line distance Iy of the robot's center of gravity along the direction of the coal slime tunnel in the mine and record the angular velocity and angular acceleration of the robot during movement. The angular velocity and angular acceleration measured by the inertial measurement unit (IMU) are processed by the extended Kalman filter method to preliminarily estimate the robot's posture. The posture angle is directly obtained from the laser observation data, which effectively avoids the influence of the IMU cumulative error and can be used to correct the estimated posture and improve the measurement accuracy. S1.2 Establish an underground coal slime tunnel model. The width of the underground coal slime tunnel is 2D. Establish a spatial rectangular coordinate system o-xyz. The origin o is the center position of the underground coal slime tunnel entrance. The bottom surface of the underground coal slime tunnel is the xoy plane. The y axis is the center line of the bottom of the underground coal slime tunnel. The height direction of the underground coal slime tunnel is the z axis. The function equation of the height arch of the underground coal slime tunnel is z=H(x). The coordinate axis o'-x'y'z' at the center of gravity M of the robot is determined by the right-hand rule, and the coordinate axis z' is consistent with the direction of the laser ranging sensor of the horizontal rangefinder and laser ranging sensor combination module; The robot can be regarded as a rectangular parallelepiped model, whose length, width and height are a, b and h respectively; S1.3 Since the laser distance measuring sensors are perpendicular to the installation plane and the two are located on the same straight line, the robot can be vertically projected onto the xoy plane. The position of the robot's center of gravity point M0 is located at the center of the projection plane. The x coordinate of point M0 can be determined, and its value is: ； S1.4 The y coordinate of point M0 is calculated by the inertial measurement unit IMU as Iy; S1.5 The vertical distance from the upper surface of the robot to the coal slime tunnel in the mine is measured by the combination module of the horizontal rangefinder and the laser rangefinder sensor, which is recorded as d5. The horizontal rangefinder is used to measure the horizontal distance from the center point M' of the upper surface to the tunnel entrance, which is recorded as Iy'. Project the robot parallel to the YOZ plane. The distance from its center of gravity M0 to the y-axis is the z-coordinate of M0, whose value is: ； in, , represents the x coordinate at the current moment, Indicates the deviation between the projected position of point M and its actual position.

2. The method according to claim 1, characterized in that In S1.1 of S1, For laser ranging sensors, horizontal rangefinders, and inertial measurement units (IMUs), the collected data needs to be filtered. First, an amplitude limiter filter is performed, and then a recursive average filter is performed to obtain the measurement value and enter the next step of data processing.

3. The method according to claim 1, characterized in that The S2 specifically includes: S2.1 The posture of the robot is determined by three angles, namely, the heading angle α is the angle between the projection of the y' axis on the bottom surface and the y axis, calculated in the xoy plane, the pitch angle β is the angle between the y' axis and the xoy plane, calculated in the yoz plane, and the roll angle γ is the angle between the plane where the robot is located and the horizontal plane, calculated in the xoz plane; S2.2 Calculation of heading angle α: The robot can be projected onto the xoy plane and its cosine value can be calculated: ,but ; S2.3 Calculation of the pitch angle β: The robot can be projected onto the yoz plane and its cosine value can be calculated: , ; S2.4 Calculation of the rolling angle γ. The robot can be projected onto the xoz plane and its cosine value can be calculated: ， ； The S2.5 attitude angle is obtained by observing the laser ranging sensor observation value through plane projection method and using inverse trigonometric functions. There is a certain gap between the value directly observed by IMU, so the two need to be fused; The inertial measurement module IMU has cumulative errors, so its confidence will be correspondingly lower, while the observation data error is small, but there is a problem of calculation accuracy. Weights are assigned separately and their linear combination is used as the output value; The pose angle, hereinafter referred to as the calculated value a, should be fused with the pose angle of the inertial measurement device, hereinafter referred to as the observed value b, to obtain the pose angle of the underground water tank cleaning robot; The fusion adopts the method of First Order Complementary Filter, that is, the weights are assigned to the two to obtain a new angle as the output value to represent the actual posture of the underground water tank cleaning robot; When the calculated value is equal to the observed value, the calculated value is output; when the calculated value is not equal to the observed value, the following formula is used for calculation: ； Error rate , the larger the error value, the greater the proportion of the observed value.

4. The method according to claim 3, characterized in that The S3 is specifically: In the robot coordinate system o'-x'y'z', it can be known from the rigid body assumption that the position of each point of the robot relative to the center of gravity remains unchanged. Through the transformation matrix J, the coordinates of the robot relative to the underground coal slime tunnel coordinate system o-xyz, that is, the position coordinates of the robot, can be obtained; The coordinate transformation formula of the center of gravity is: M=M0J, from which it is easy to derive the transformation formula of any point; The expression of the transformation matrix J is as follows: ； A set of quaternions is used to represent the robot's position and posture, namely , ; in, is the x-coordinate of the center position of the robot's upper surface, is the y coordinate of the center position of the robot's upper surface, is the z coordinate of the center position of the robot's upper surface.

5. The method according to claim 3, characterized in that: The position and posture of the robot in S3 can be transmitted to a receiving device such as a display, a mobile phone, or a computer through a signal transmitting device and a wireless local area network.

6. The method according to claim 1, characterized in that The size deformation rate of the underground water tank automatic cleaning robot is less than 1%, so a rigid body assumption is made for the robot.

Images