An adaptive control positioning method for a robot for detecting a sludge deposition tank bottom

Abstract

The application discloses an oil sludge deposition tank bottom detection robot adaptive control positioning method, and belongs to the technical field of oil storage tank operation. The method comprises the following steps: step a, placing the robot (5) into an oil tank (1) through a manhole (4) at the top of the oil tank (1) and arranging a tank top positioning device at the top of the oil tank (1); step b, positioning the robot (5); step c, judging whether the robot (5) is inclined or not by a controller, if the robot (5) is inclined, executing step d, otherwise returning to step b; step d, compensating the positioning of the robot (5); and step e, controlling and positioning a probe (11) on the robot (5). In the oil sludge deposition tank bottom detection robot adaptive control positioning method, the height of a beacon can be adaptively adjusted, the influence of the up-and-down fluctuation of the robot caused by the oil sludge at the bottom of the tank on the positioning precision is overcome, the probe is further controlled and positioned, and the working state of the probe is ensured.

Description

TECHNICAL FIELD

[0001] The oil sludge deposition tank bottom detection robot adaptive control positioning method belongs to the technical field of oil storage tank operation. BACKGROUND

[0002] Storage tanks are extremely common storage devices in the petroleum and petrochemical field. Due to the complexity of the internal medium composition, the tank bottom is prone to corrosion, which leads to equipment failure. To solve this problem, the tank bottom is usually detected and evaluated every certain period. However, due to the fact that the tank bottom is often covered by various types of oil, conventional detection methods are extremely difficult to implement smoothly, and usually require tank opening for shutdown maintenance, which is time-consuming, labor-intensive and increases costs. In the prior art, the development of a storage tank bottom online detection robot using magnetic flux leakage, acoustic emission technology and guided wave detection technology combined with an intelligent robot is on the agenda.

[0003] (1) The application number is 202111314099.9, the application date is November 8, 2021, and the patent name is "a positioning method for an oil-carrying storage tank bottom detection robot". A technical solution is disclosed in this technical solution. In this technical solution, an inertial sensor is installed at the center of the tank bottom operation robot to obtain the direction of travel of the robot. Meanwhile, two groups of four ultrasonic sensors are installed at the front and rear ends of the robot, and the four sensors are located on the central axis of the robot, of which the two sensors at the front and rear ends are outwardly directed, and the two middle sensors are oppositely arranged. When working, the two middle sensors with known distances are used to measure the speed of sound, and the two outer sensors are used for echo ranging to measure the straight-line distance from the front and rear ends of the robot to the tank wall. By combining the sum of the distances and the direction of travel of the robot, the position coordinates of the robot are obtained by calculation.

[0004] However, in this technical solution, four ultrasonic sensors need to be installed at the central axis of the robot. Due to the particularity of the positioning method, the positions of the four sensors used in this method need to be accurately determined, otherwise a small deviation in the installation angle or position will have a great impact on the positioning result. Moreover, as the diameter of the storage tank increases, the positioning error will also gradually increase, so this method is only suitable for use in small oil storage tanks.

[0005] (2) Xu YS. Research on the method of locating sound source in liquid field of closed space[D]. Tianjin University, 2012. A technical solution is provided in which a receiving transducer array is arranged outside the tank to receive the positioning signal emitted by the tank robot, the propagation time of the sound signal is measured, and the time difference between the robot and each transducer is obtained by using the different times at which the sound source emits signals to each transducer. The time difference of the sound source signal reaching two transducers can determine a pair of hyperbolas, and then three transducers can form two pairs of hyperbolas, and the intersection point is the position of the sound source. The positioning of the robot in three-dimensional space requires four receiving transducers to form three pairs of hyperbolas, and the intersection point is the position of the robot.

[0006] In this technical solution, since the positioning receiving transducer is installed outside the storage tank, this method will cause the sound signal to pass through the tank wall and the oil in the tank in two different forms of medium during propagation. Due to the different propagation characteristics of sound signals in different media and the complex effects of sound signals passing through heterogeneous interfaces, the positioning difficulty will increase and the positioning accuracy will be affected. At the same time, the use of signal arrival time difference method to realize robot positioning will appear multi-value phenomenon, that is, ambiguity, and in the case of large noise and error, there may be no solution.

[0007] Not only due to the particularity of the environment, the robot works in a closed liquid environment, it is difficult to observe the position of the robot, and it is also difficult to obtain the specific position of the detection probe. Most of the underwater positioning products on the market are difficult to meet the working environment in the oil storage tank. Moreover, the robot needs to ensure that the distance between the probe and the tank bottom remains unchanged during work, the probe always faces downward, and the angle between the probe and the second mechanical arm remains unchanged. After positioning the robot, it is also difficult to further determine the position of the probe, especially in the storage tank, because the tank bottom is covered with oil sludge, it is difficult to ensure that the robot always runs in a horizontal state. Therefore, designing a technical solution that can accurately position the robot below the liquid level in the tank and further control the second mechanical arm to ensure that the probe is in the required working state has become a technical problem to be solved in the field. SUMMARY

[0008] The technical problem to be solved by the present application is to overcome the shortcomings of the prior art and provide an oil sludge deposition tank bottom detection robot adaptive control positioning method which realizes high self-adaptive adjustment of the beacon, overcomes the influence of the up-and-down movement of the robot caused by the oil sludge on the bottom of the tank on the positioning accuracy, and further controls and positions the probe to ensure that the probe is always in the required working state.

[0009] The technical scheme adopted by the present application to solve its technical problems is: the oil sludge deposition tank bottom detection robot self-adaptive control positioning method, comprising a positioning system, the positioning system comprising a controller connected with the robot, a probe being installed on the front end of the robot through a mechanical arm, and characterized by comprising the following steps:

[0010] Step a, the robot is put in through the manhole at the top of the oil tank, and a tank top positioning device is arranged at the top of the oil tank;

[0011] Step b, the controller controls the positioning mechanism arranged on the surface of the robot to keep the same height with the tank top positioning device, and the robot is positioned through the tank top positioning device, the positioning mechanism comprising a beacon vertically liftable on the surface of the robot;

[0012] Step c, the controller judges whether the robot is tilted, if tilted, step d is executed, if not tilted, step b is returned;

[0013] Step d, the robot is positioned and compensated, and the compensated robot coordinates are obtained;

[0014] Step e, the probe on the robot is controlled and positioned, so that the probe always keeps horizontal and is arranged at an interval from the tank bottom;

[0015] Step f, the control and positioning of the mechanical arm of the robot;

[0016] Step g, the controller judges whether the tank bottom defect is detected, if the tank bottom defect is detected, the coordinates of the tank bottom defect are returned, if the tank bottom defect is not detected, step d is returned.

[0017] Preferably, the tank top positioning device comprises at least three positioning rods put into the oil tank from the manhole at the top of the tank, an ultrasonic sensor being arranged at the bottom of each positioning rod, and all the ultrasonic sensors being at the same distance from the tank top.

[0018] Preferably, the robot comprises a robot body, a containing groove being arranged downward in the middle of the robot body, the positioning mechanism being installed in the containing groove, the positioning mechanism comprising a lifting frame installed in the containing groove, a beacon being arranged at the top of the lifting frame, a fixed rod being horizontally fixed rearward from the top of the lifting frame, a sonar being fixed at the end of the fixed rod, the detection end of the sonar facing directly downward for detecting the vertical distance between the beacon and the tank bottom, and the beacon and the ultrasonic sensor being at the same height.

[0019] Preferably, in step b, the method for keeping the positioning mechanism at the same height with the tank top positioning device comprises the following steps:

[0020] The height of the positioning mechanism is adaptively PID controlled according to the distance of the positioning mechanism from the tank bottom, and the principle formula of the PID control is as follows:

[0021]

[0022] wherein: u(k) is the output of the PID control, representing the change of the system output; K p is the proportional coefficient, representing the ratio of the change of the output signal relative value to the change of the input deviation signal relative value; K i is the integral coefficient; K d is the differential coefficient, e(k) represents the difference between the output value h(k) of the system at k time and the target value , that is:

[0023]

[0024] The expressions of the coefficients in the principle formula are as follows:

[0025]

[0026] wherein: T is the sampling time; T i is the integral time; T d is the differential time.

[0027] Preferably, the step d comprises the following steps:

[0028] Step d-1, obtaining the distance j from the top of the robot to the center point;

[0029] Step d-2, obtaining the following relationship of the depression angle or elevation angle of the robot as α:

[0030]

[0031] Step d-3, obtaining the distance f between the center point of the robot and the projection point of the beacon on the tank bottom:

[0032] f = (i + j) sin α

[0033] Step d-4, according to the vertical distance h' from the center point of the robot to the tank bottom, it is satisfied that:

[0034]

[0035] Obtaining the vertical distance h' from the center point of the robot to the tank bottom:

[0036] h' = h - (i + j) cos α

[0037] Step d-5, the end coordinates of the robot after positioning compensation:

[0038] x1 = x - f sin β, y1 = y - f sin β

[0039] In steps d-1 to d-5, h represents the vertical height of the beacon from the tank bottom, a represents the elevation or depression angle of the robot caused by the oil sludge on the tank bottom, i represents the lifting height of the beacon, j represents the distance from the top of the robot to the center point, f represents the distance between the center point of the robot and the projection point of the beacon on the tank bottom, and h' represents the vertical distance from the center point of the robot to the tank bottom.

[0040] Preferably, the mechanical arm comprises a first mechanical arm connected to the robot and a second mechanical arm having one end connected to the first mechanical arm and the other end connected to the probe.

[0041] Preferably, step f comprises the following steps:

[0042] In step f-1, the arrival position of the probe at the next moment is predicted according to the robot coordinates and the angle of the mechanical arm:

[0043] x d2 = x1 - (x v + kd') sin β

[0044] y d2 = y1 + (y v + kd') cos β

[0045] h d = u

[0046] wherein x d2 is the horizontal coordinate of the detection probe at the next moment, y d2 is the vertical coordinate of the detection probe at the next moment, x v is the positioning error caused by the movement of the robot, x v = vΔt, v is the movement speed of the robot, k is the movement limitation coefficient of the probe, k ≤ 1, and h d is the expected height of the probe.

[0047] In step f-2, the movement angle of the mechanical arm is converted:

[0048]

[0049] wherein l1 represents the length of the first mechanical arm, l2 represents the length of the second mechanical arm, l3 represents the straight line length between the two ends of the first mechanical arm and the second mechanical arm, and a represents the elevation or depression angle of the robot caused by the oil sludge on the tank bottom.

[0050] In step f-3, the driving force τ of the mechanical arm is controlled.

[0051] Compared with the prior art, the present application has the beneficial effects that:

[0052] 1. In the adaptive control positioning method of the robot for detecting the oil sludge deposited at the bottom of a tank, the height of the beacon is adaptively adjusted, the influence of the up-and-down movement of the robot caused by the oil sludge at the bottom of the tank on the positioning accuracy is overcome, the probe is further controlled and positioned, and it is ensured that the probe is always in the required working state.

[0053] 2. In the adaptive control positioning method of the robot for detecting the oil sludge deposited at the bottom of a tank, the structure of the oil storage tank can be adaptively adjusted, the positioning method adopted in the patent can be used for various types of storage tanks with different diameters and heights, the ultrasonic sensor array arrangement and installation method can be combined to realize accurate positioning of the robot under the oil;

[0054] 3. In the adaptive control positioning method of the robot for detecting the oil sludge deposited at the bottom of a tank, the height adaptive adjustment device of the beacon lifting frame based on PID control is designed, the coplanar requirement of the beacon and the sensor array is realized, and the up-and-down influence of the oil sludge at the bottom of the tank on the robot is well adapted; BRIEF DESCRIPTION OF DRAWINGS

[0055] Figure 1 The flow chart of the adaptive control positioning method of the robot for detecting the oil sludge deposited at the bottom of a tank.

[0056] Figure 2 The schematic diagram of the arrangement in the tank of the adaptive positioning method of the robot for detecting the oil sludge deposited at the bottom of a tank.

[0057] Figure 3 The schematic diagram of the robot structure of the adaptive positioning method of the robot for detecting the oil sludge deposited at the bottom of a tank.

[0058] Figure 4 The schematic diagram of the robot positioning principle of the adaptive control positioning method of the robot for detecting the oil sludge deposited at the bottom of a tank.

[0059] Figure 5 The schematic diagram of the robot tilt state of the adaptive control positioning method of the robot for detecting the oil sludge deposited at the bottom of a tank.

[0060] Figure 6 The schematic diagram of the detection probe working of the adaptive control positioning method of the robot for detecting the oil sludge deposited at the bottom of a tank.

[0061] Figure 7 The schematic diagram of the relative position between the probe and the robot plane of the adaptive control positioning method of the robot for detecting the oil sludge deposited at the bottom of a tank.

[0062] Figure 8 The schematic diagram of the robot operation of the adaptive control positioning method of the robot for detecting the oil sludge deposited at the bottom of a tank.

[0063] Figures 9-10 The schematic diagram of the two-degree-of-freedom robot arm relative coordinate system and global coordinate system of the adaptive control positioning method of the robot for detecting the oil sludge deposited at the bottom of a tank.

[0064] Wherein: 1, oil tank 2, positioning rod 3, ultrasonic sensor 4, manhole 5, robot 6, beacon 7, sonar 8, lifting frame 9, first mechanical arm 10, second mechanical arm 11, probe 12, tank bottom sludge. DETAILED DESCRIPTION

[0065] Figures 1-10 is the best embodiment of the present application, the following will be described in detail with reference to the accompanying drawings Figures 1-10 Further illustrate the present application.

[0066] As shown in the figure, a kind of sludge deposition tank bottom detection robot adaptive control positioning method (hereinafter referred to as adaptive positioning method), including the following steps: Figure 1 Step 1001, start.

[0067] Combined with the drawings

[0068] , the robot 5 is put into the oil tank 1 from the manhole 4 of the oil tank 1, and the robot 5 is in contact with the tank bottom after the robot 5 is put into the oil tank 1, and the robot 5 is below the liquid level of the oil tank 1, and the system is initialized. The same as prior art, the robot 5 is connected with the control system outside the tank through the cable (not shown in the drawing) passing through the manhole 4. Figure 2 Three positioning rods 2 are respectively arranged below from the three manholes 4 not collinearly arranged above the oil tank 1, the upper end of the positioning rod 2 is fixed with the oil tank 1, and the lower end is respectively fixed with the ultrasonic sensor 3, the ultrasonic sensor 3 is above the liquid level, and the three ultrasonic sensors 3 are at the same height.

[0069] Combined with the drawings

[0070] , the robot 5 includes a robot body, and bottom wheels are respectively arranged at the four corners of the bottom of the robot body. The first mechanical arm 9 is connected with one end of the second mechanical arm 10 at the upper end of the front end of the robot body, and the other end of the second mechanical arm 10 is provided with the probe 11. The receiving groove is arranged downward in the middle of the robot body 9, the lifting frame 8 is installed in the receiving groove, and the beacon 6 is arranged on the top of the lifting frame 8. A fixed rod is also fixed horizontally from the top of the lifting frame 8, and the sonar 7 is fixed at the end of the fixed rod, and the detection end of the sonar 7 faces directly downward, for detecting the vertical distance between the beacon 6 and the tank bottom. Figure 3 Step 1002, positioning the robot 5;

[0071] The external controller controls the driving mechanism of the lifting frame 8 to act, drives the lifting frame 8 to lift, and makes the sonar 7 rise to the same height as the ultrasonic sensor 3, first defines the origin of the oil tank 1, and determines the coordinate system according to the origin, so as to determine the coordinates of the three ultrasonic sensors 2.

[0072]

[0073] ​Let the distance between the ultrasonic sensor 3 and the top of the oil tank be g, the total height of the tank body of the oil tank 1 be H, and the height of the beacon 6 from the bottom of the oil tank 1 be h, and the plan is to achieve: h = H - g. In order to ensure that the beacon 6 can be at the same height as the three ultrasonic sensors 3, the lifting frame 8 is controlled in the following way:

[0074] The height data measured by the sonar 7 is combined with the driver to achieve adaptive PID control of the beacon height, and the principle formula of the PID control is as follows:

[0075]

[0076] Where: u(k) is the output of the PID control, indicating the change of the system output; K p is the proportional coefficient, indicating the ratio of the relative change of the output signal to the relative change of the input deviation signal; K i is the integral coefficient; K d is the differential coefficient. e(k) represents the difference between the output value h(k) of the system at time k and the target value , that is:

[0077]

[0078] The expressions of the coefficients in the principle formula are as follows:

[0079]

[0080] Where: T is the sampling time; T i is the integral time; T d is the differential time.

[0081] Further, as shown in the positioning principle diagram of Figure 4 , the robot is positioned:

[0082] Figure 4 Ai(i = 1, 2, 3) respectively represents the three ultrasonic sensors 3, and P is the position of the beacon 6. The system uses the "plane three circle intersection" positioning method to determine a point in the plane by three circles to obtain the beacon position. The distance between each sensor and the beacon is calculated by the ultrasonic positioning signal and the propagation speed of the signal in the oil medium:

[0083] r i = v·t i , i = 1, 2, 3

[0084] Where: r i represents the distance between the i(i = 1, 2, 3)th ultrasonic sensor 3 and the beacon 6, v is the propagation speed of the ultrasonic signal in the oil medium, and t i (i = 1, 2, 3) is the time from the emission of the ultrasonic signal to the reception by the i sensor.

[0085] Given the planar coordinates (Xi, Yi) of each ultrasonic sensor, and assuming the beacon's location coordinates are (x, y), the basic mathematical model for underwater acoustic localization based on time delay measurement is as follows:

[0086] (xX i ) 2 +(yY i ) 2 =r i 2 =(vt) i ) 2 i = 1, 2, 3

[0087] set up Subtracting each pair of terms yields the following formula:

[0088]

[0089] The three ultrasonic sensors 3 can generate three sets of equations. By solving these equations, the coordinates of the beacon 6 can be obtained.

[0090] Step 1003: Determine whether robot 5 has tilted;

[0091] The control system outside the oil tank 1 determines whether the robot tilts based on the angle sensor installed inside the robot 5. If it tilts, it executes step 1004; if it does not tilt, it returns to step 1002.

[0092] Step 1004: Perform positioning compensation on robot 5;

[0093] The presence of sludge 12 at the bottom of the tank will cause robot 5 to experience pitch or sag during its movement. Since the lifting frame 8 can only move vertically up and down relative to robot 5, the pitch angle will cause the planar coordinates of beacon 6 to be inconsistent with the planar coordinates of robot 5. Therefore, it is necessary to compensate for the robot's positioning results based on the robot 5's attitude information.

[0094] like Figure 5 As shown, the lifting height i of the lifting frame 8 can be directly obtained from the device's return, and the distance j from the top of the robot 5 to the center point can be directly measured. Based on the geometric relationship shown in the figure and similar triangles, the following formula can be obtained:

[0095]

[0096] And because:

[0097]

[0098] The distance between the two projection points can be obtained from the above two equations:

[0099] f = (i + j)sinα

[0100] The vertical distance h' from the center point of the robot to the bottom of the tank satisfies the following formula:

[0101]

[0102] get:

[0103] h' = h - (i + j)cosα

[0104] In the above formulas: h represents the vertical height of the beacon 6 from the bottom of the tank as measured by the sonar 7; α represents the elevation angle of the robot 5 caused by the sludge 12 at the bottom of the tank; i represents the lifting height of the lifting frame 8; j represents the distance from the top of the robot 5 to the center point; f represents the distance between the center point of the robot 5 and the projection point of the beacon 6 at the bottom of the tank; and h' represents the vertical distance from the center point of the robot 5 to the bottom of the tank.

[0105] At this time, the plan view of robot 5 inside oil tank 1 is as follows: Figure 7 As shown, the pitch angle α and orientation angle β of robot 5 can be measured using inertial navigation. Let β be the angle between the robot 5's orientation and the positive y-axis, with counter-clockwise angles considered positive and clockwise angles negative. Then, the robot's planar coordinates ( Figure 7 Point A in the middle is (x1, y), which can be determined by beacon 6 ( Figure 7 The coordinates (x, y) of point B are calculated. The robot's coordinates are then calculated as follows:

[0106] x1=x-fsinβ, y1=y-fsinβ.

[0107] Step 1005: Control and positioning of the front probe 11 of robot 5.

[0108] To ensure that the front probe 11 of robot 5 always faces the bottom of the tank and maintains a certain distance from it, a two-degree-of-freedom robotic arm is used in this adaptive positioning method to control the detection probe and bring it to a suitable position. To ensure that the robotic arm achieves a stable and effective detection of defects on the tank bottom, it is necessary to control and maintain a suitable distance between the probe and the tank bottom plate, and to return the probe's coordinates after detecting a defect.

[0109] The second robotic arm 10 is equipped with a servo hydraulic cylinder to control its length, and this is coordinated with the rotation of the second robotic arm 10 to achieve the desired effect. Further integration... Figure 3, the mechanical arm is composed of a first mechanical arm 9 with a length of l1 and a second mechanical arm 10 with a length of l2, both ends of the mechanical arm can rotate around the shaft through the shaft coupling, and the angle sensor returns θ1 and θ2 angle size. m is the distance between the robot and the mechanical arm connection and the beacon (the center point of the robot), and n is the vertical length from the center point of the robot to the connection of the robot and the mechanical arm, which can be measured. The probe needs to keep the distance u from the tank bottom unchanged during the operation of the robot to ensure the optimal detection effect, and the working schematic diagram is as shown in Figure 6

[0110] On this basis, the positioning of the detection probe is realized. The distance between the projection of the robot center point and the projection of the detection probe is:

[0111] d=d1+d2+d3

[0112] Among them:

[0113] d1=l2sin(π-θ1-θ2+α)=l2sin(θ1+θ2-α)

[0114] d2=l1sin(π-θ1+α)=l1sin(θ1-α)

[0115] d3=(m-ntanα)cosα

[0116] Combined with Figure 7 , assuming that the plane coordinates of the probe 11 are (x2, y2), then:

[0117] x2=x1-dinβ, y2=y1-dsinβ.

[0118] Step 1006, control and positioning of the mechanical arm;

[0119] Including the following steps: step 1006-1, predicting the arrival position of the probe 11 at the next time according to the robot coordinates and the angles of the mechanical arm;

[0120] When the robot moves forward, the oil sludge on the tank bottom will fluctuate and change, causing the pitch change of the robot posture, which needs to continuously adjust the detection mechanical arm to keep the detection probe at a suitable distance from the tank bottom, while meeting the requirement that the detection mechanical arm does not exceed the maximum controllable range. Therefore, the following tank bottom detection oil sludge self-adaptive control algorithm is designed, which can adjust the detection device posture in real time through the positioning information, and return the corresponding coordinates when the defect point is obtained.

[0121] In order to realize the detection of the planned detection point, the control algorithm of the two-degree-of-freedom mechanical arm is designed, which can control the detection probe to reach the distance u above the detection point under the premise that the robot positioning and the detection probe positioning tasks are completed.

[0122] As​Figure 8 The limit state of the mechanical arm is shown when the robot is working, at this time the two segments of the mechanical arm are in the same straight line, at this time the distance between the projection point of the detection probe and the projection point of the robot center is:

[0123] d' = (l1 + l2) sin (π - θ1 + α) + m' cos α

[0124] Where: m' = m - n tan α.

[0125] Combined with the above probe plane coordinates, at this time the probe plane coordinates are:

[0126] x2 = x1 - d' sin β

[0127] y2 = y1 + d' cos β

[0128] In order to ensure that the mechanical arm always remains within the limit range during the movement of the robot, and the distance between the probe and the tank bottom plate is kept at a distance u, the control target needs to meet the following conditions, that is, the probe reaches the position at the next time:

[0129] x d2 = x1 - (x v + kd') sin β

[0130] y d2 = y1 + (y v + kd') cos β

[0131] h d = u

[0132] Where: x d2 is the expected horizontal coordinate of the detection probe at the next time, y d2 is the expected vertical coordinate of the detection probe at the next time; x v is the positioning error caused by the movement of the robot, x v = vΔt, v is the speed of the robot; k is the movement limitation coefficient of the probe, k ≤ 1; h d is the expected probe height.

[0133] Step 1006-2, convert the mechanical arm movement angle.

[0134] Based on the above, a two-degree-of-freedom mechanical arm controller is designed. The kinematics Lagrange equation is used:

[0135]

[0136] Where, k is the kinetic energy of the mechanical arm; w is the potential energy of the mechanical arm; τ is the driving force at the joint of the mechanical arm; f is the resistance of the mechanical arm in the oil environment on the tank bottom.

[0137] AsFigure 9 As shown in the figure, a relative coordinate system is established with the joint between the mechanical arm and the robot as the coordinate origin.

[0138] Due to the structure and material of the mechanical arm, it is assumed that the mass of each segment of the mechanical arm is uniformly distributed, the bottom plate of the storage tank is set as the zero potential energy surface, and the kinetic potential energy is calculated at the centroid of the mechanical arm, so that:

[0139]

[0140] Where J represents the moment of inertia of the mechanical arm, and here

[0141] In the relative coordinate system, it is assumed that the coordinates of the centroid of the mechanical arm l2 are And the relative velocity is Then And because:

[0142]

[0143] The controller can be designed according to the relative velocity of the mechanical arm, that is:

[0144]

[0145] The dynamic Lagrange equation of the mechanical arm can be obtained from the above formulas:

[0146]

[0147] The above two formulas can be expressed as the following form:

[0148]

[0149] Where M(θ) is the mass matrix; is the matrix composed of centrifugal force and Coriolis force; G(θ) is the gravity matrix. The three matrixes are as shown below:

[0150]

[0151] As shown in the figure, Figure 10 According to the operation process of the robot and the geometric relationship, the following can be obtained:

[0152]

[0153] From this, we can get:

[0154]

[0155] Step 1006-3, control the driving force τ of the mechanical arm;

[0156] According to the dynamic Lagrange equation of the above mechanical arm, the mechanical arm control model can be expressed as follows:

[0157]

[0158] Define the tracking error as:

[0159] e = θ - θ d

[0160] The controller is designed as:

[0161]

[0162] It can be seen that, except for the unknown parameters A1 and A2 of the controller, the remaining parameters are known, so the parameters A1 and A2 are designed in a way based on system stability. Substituting the above controller expression into the mechanical arm control model expression can obtain:

[0163]

[0164] That is:

[0165]

[0166] Among them:

[0167]

[0168] Then it can be ensured that the following formulas are zero:

[0169]

[0170] At the same time:

[0171] g 12 = 0, h 12 = 0, g 21 = 0, h 21 = 0

[0172] In order to ensure that the above formula is zero, γ 2 + g 11 γ + h 11 , γ 2 + g 22 γ + h 22 is Hurwitz, and γ is the defined Lagrange operator.

[0173] It can be taken that (γ + b) 2 = 0, b > 0, then the real part of the characteristic value of the polynomial γ 2 + 2bγ + b 2 = 0 is negative. Correspondingly, g 11 = 2b1, g 22= 2b2,

[0174] At the same time, the above formula:

[0175]

[0176] Guarantee the stability of the system, namely the controller unknown parameters A1 and A2, reach the purpose of mechanical arm control.

[0177] The above is only the preferred embodiment of the present application, not other forms of the present application is limited, any skilled in the art can use the disclosed technical content to change or modification of equivalent embodiments of equivalent changes. But any simple modification, equivalent changes and modification of the above embodiments, which do not deviate from the technical solution of the present application, according to the technical essence of the present application, still belongs to the protection scope of the technical scheme of the present application.

Claims

1. An adaptive control positioning method for a robot for detecting sludge deposition in a tank, comprising a positioning system comprising a controller connected to a robot (5), a probe (11) being mounted at the front end of the robot (5) by means of a mechanical arm, characterized in that: The method comprises the following steps: Step a, placing the robot (5) into the oil tank (1) through the manhole (4) on the top of the oil tank (1) and setting a tank top positioning device on the top of the oil tank (1); Step b, controlling the positioning mechanism arranged on the surface of the robot (5) to keep the same height with the tank top positioning device and positioning the robot (5) through the tank top positioning device, wherein the positioning mechanism comprises a beacon (6) vertically liftable on the surface of the robot (5); Step c, judging whether the robot (5) is tilted or not by the controller, if yes, executing step d, if not, returning to step b; Step d, positioning compensation of the robot (5) to obtain the compensated coordinates of the robot (5); Step e, controlling and positioning the probe (11) on the robot (5) to keep the probe (11) always horizontal and spaced from the tank bottom of the oil tank (1); Step f, controlling and positioning the mechanical arm of the robot (5); Step g, judging whether the tank bottom defect is detected by the controller, if yes, returning the coordinates of the tank bottom defect, if not, returning to step d; The tank top positioning device comprises at least three positioning rods (2) placed into the oil tank (1) through the manhole (4) on the top of the oil tank (1), and an ultrasonic sensor (3) is arranged at the bottom of each positioning rod (2), and all the ultrasonic sensors (3) are spaced from the tank top by the same distance; The robot (5) comprises a robot body, a receiving groove is arranged downward in the middle of the robot body, the positioning mechanism is installed in the receiving groove, the positioning mechanism comprises a lifting frame (8) installed in the receiving groove, the beacon (6) is arranged at the top of the lifting frame (8), a fixed rod is horizontally fixed rearward from the top of the lifting frame (8), the sonar (7) is fixed at the end of the fixed rod, the detection end of the sonar (7) faces directly downward, is used for detecting the vertical distance between the beacon (6) and the tank bottom, and the beacon (6) is at the same height with the ultrasonic sensor (3); In the step b, the method for keeping the positioning mechanism at the same height with the tank top positioning device comprises the following steps: According to the distance between the positioning mechanism and the tank bottom, the height of the positioning mechanism is adaptively controlled by PID, and the principle formula of the PID control is as follows: Where: u(k) is the output of the PID control, representing the change in the system output; K p is the proportional coefficient, representing the ratio of the change in the output signal relative value to the change in the input deviation signal relative value; K i is the integral coefficient; K d is the derivative coefficient, e(k) represents the difference between the output value h(k) of the system at time k and the target value , that is: The expression of each coefficient in the principle formula is as follows: Where: T is the sampling time; T i is the integration time; T d is the differentiation time; The step d comprises the following steps: Step d-1, obtaining the distance j from the top of the robot (5) to the center point; Step d-2, obtaining the following relationship of the depression angle or the elevation angle of the robot (5) as α: Step d-3, obtaining the distance f between the center point of the robot (5) and the projection point of the beacon (6) on the tank bottom: f = (i + j)sinα Step d-4, according to the vertical distance h' of the center point of the robot from the tank bottom satisfying: obtaining the vertical distance h' of the center point of the robot from the tank bottom: h' = h - (i + j)cosα Step d-5, the coordinates of the robot (5) after the positioning compensation: x1 = x - f sinβ, y1 = y + f cosβ In steps d-1 to d-5: h represents the vertical height of the beacon (6) from the tank bottom, a represents the elevation or depression angle of the robot (5) due to the tank bottom sludge (12), i represents the beacon (6) lifting height, j represents the distance from the top of the robot (5) to the center point, f represents the distance between the center point of the robot (5) and the projection point of the beacon (6) on the tank bottom, h' represents the vertical distance from the center point of the robot (5) to the tank bottom, p is the distance between the intersection of the extension line of the connecting line of the beacon (6) and the center of the robot (5) on the tank bottom and the projection of the beacon (6) on the tank bottom, β is the orientation angle of the robot (5), and x and y are the coordinates of the position of the beacon (6).

2. The adaptive control positioning method for the robot of sludge deposition tank bottom detection according to claim 1, characterized in that: The mechanical arm comprises a first mechanical arm (9) connected to the robot (5) and a second mechanical arm (10) connected at one end to the first mechanical arm (9) and at the other end to the probe (11).

Images