Two-Degree-of-Freedom Motion Control Method for a Cable Detection Robot

Abstract

The present invention discloses a two-degree-of-freedom motion control method for a cable detection robot, specifically including the following steps: Step 1, establish a coordinate system; Step 2, establish a rotational speed model; Step 3, solve for the rotational speed; Step 4, establish a control efficiency model; Step 5, establish an attitude model; Step 6, solve for the attitude angular velocity; Step 7, perform coordinate transformation; Step 8, perform two-degree-of-freedom control. The present invention can achieve arbitrary control of two degrees of freedom in the axial and circumferential directions of the cable, can effectively detect cables with different inclination angles and the same cable with a uniformly changing cable body diameter, and the detection range covers smooth long straight cables and spiral cable types, that is, it can detect the cable in a posture with a constant direction along the axial direction of the cable and a spiral ascending posture.

Description

Technical Field

[0001] The present invention relates to the technical field of cable detection, in particular to a two-degree-of-freedom motion control method for a cable detection robot. Background Art

[0002] Due to the current requirements of industrial building design for aesthetics, practicability and economy, cables are used more and more widely. For example, cable-stayed bridges, suspension bridges, cable cars, etc. all need to use cables to achieve the stability of the overall structure. During the use of cables, they bear a large force and are affected by various external forces during use. Moreover, cable-stayed bridges, suspension bridges, cable cars, etc. have very strict requirements for safety performance. Once a fracture occurs, very serious consequences will occur, even endangering people's lives. Therefore, the quality acceptance, maintenance and fault detection of cables are particularly important.

[0003] A cable-stayed bridge is also called a cable-stayed bridge or a cable-stayed suspension bridge. It is mainly a force-bearing system bridge that uses cables to pull the main girder on the bridge tower. It is a new type of bridge in recent decades and has excellent economic performance and seismic performance.

[0004] Cables are the key load-bearing components of cable-stayed bridges. Their designed service life is generally 25 - 30 years. However, several cable-stayed bridges have had their cables replaced in advance due to corrosion and damage of the cables. For example, the P-K Bridge in the United States is a modern cable-stayed bridge. The original estimated service life of the bridge was 25 years, but in fact, it only took 5 years to replace all the cables; the Jinan Yellow River Highway Bridge in China was in use for 13 years, and the Guangzhou Haiyin Bridge was only in use for 6.5 years before all the cables were replaced. According to the statistics of domestic cable-replaced bridges, the actual average service life of cable-stayed bridge cables is less than 15 years. Among them, the cables of the Qi'ao Bridge in Guangdong were only used for 6 years from 2001 to 2007 before being replaced, far from reaching the designed service life. To ensure the operation safety of large-scale infrastructure structures and the safety of people's lives and property, the detection of cables is very important.

[0005] At present, the detection of cable-stayed bridge cables mainly focuses on appearance detection and cable force measurement, and mainly uses the manual detection method as the main measurement method. A winch is used to drag a hanging basket trolley carrying workers and detection sensors for measurement. This method is highly dangerous, and the workers and detection equipment weighing hundreds of kilograms will cause damage to the cables during the measurement process.

[0006] As the span of cable-stayed bridges increases, the influence of wind-induced vibration and rain-induced vibration on the cables becomes more and more significant. Since spiral cables and indented pit cables can effectively suppress the wind-rain induced vibration of cables, they have been widely adopted in newly built bridges. At the same time, a series of new problems will arise. Because the surface of spiral cables has circular protrusions with a diameter of 6 - 10 mm, and the protruding spiral line is made of the same material as the PE protective layer of the cable, it cannot withstand large external forces. Therefore, the winch-driven hanging basket trolley and the currently studied wheeled inspection robot scheme cannot effectively detect the cable while protecting the spiral line on the cable surface. Therefore, it is of great significance to detect the cable in a spiral ascending state. Summary of the Invention

[0007] The technical problem to be solved by the present invention is to provide a two-degree-of-freedom motion control method for a cable inspection robot in view of the above-mentioned deficiencies of the prior art. This two-degree-of-freedom motion control method for a cable inspection robot can effectively detect cables with different diameters and the same cable with a uniformly changing cable body diameter. The detection range covers smooth long straight cables and spiral cable types, that is, it can detect the cable in a posture with an unchanged axial direction along the cable or in a spiral ascending posture.

[0008] To solve the above technical problems, the technical solution adopted by the present invention is as follows:

[0009] A two-degree-of-freedom motion control method for a cable inspection robot includes the following steps.

[0010] Step 1: Establish a coordinate system: The cable inspection robot is a four-rotor inspection robot with four rotors; the coordinate system includes the earth coordinate system E(X, Y, Z) and the inspection robot coordinate system e(x, y, z); where the origin O of the earth coordinate system E(X, Y, Z) is the center of the earth, the X-axis is the horizontal direction passing through the center of the earth, the Y-axis is the horizontal direction passing through the center of the earth and perpendicular to the X-axis, and the Z-axis is the vertical direction passing through the center of the earth; the origin o in the inspection robot coordinate system e(x, y, z) is the symmetric center of the cable inspection robot, the z-axis is the axis passing through the origin o and parallel to the axial direction of the cable, and the x-y plane is parallel to the plane where the four rotors are located.

[0011] Step 2: Establish a rotational speed model: In the inspection robot coordinate system, a rotational speed model is established for each rotor motor. The rotational speed model is a functional equation between the rotational speed of the rotor motor and the input current, and the specific expression is:

[0012]

[0013]

[0014] In the formula, T m is the motor response time constant; is the rotational speed of the rotor motor at the current time t; σ(t) is the magnitude of the input current at the current time t; is the steady-state speed; σ c is the input current value at the steady-state speed; C R is the ratio of c to σ; is the offset coefficient, reflecting the offset speed of the motor under the condition of zero input current.

[0015] Step 3, Solve for the rotational speed: Substitute the input currents of the motors corresponding to the four rotors into Equation (1) respectively, so as to obtain the rotational speeds of the four rotors, which are and

[0016] Step 4, Establish a control efficiency model: In the detection robot coordinate system, according to the rotational speed model established in Step 2, establish a control efficiency model for the cable detection robot; the control efficiency model includes a lift model and moment models on the x, y, and z axes, and is specifically expressed as:

[0017]

[0018] In the formula, f is the lift of the rotor; c T , c M characterize the relationship between the motor rotational speed and the moments on each axis, and are measured in a specific experimental environment; T x , T y and T z are the rotational moments of the cable detection robot on the x, y, and z axes respectively; d is the distance between the rotor motor and the central axis of the cable.

[0019] Step 5, Establish an attitude model: In the detection robot coordinate system, according to Euler's equation, establish an attitude model of the moment and angular velocity for the cable detection robot as:

[0020]

[0021] In Equation (3), J xx , J yy and J zz are the moments of inertia of the cable detection robot on the x, y, and z axes respectively; ω x , ω y and ω z are the attitude angular velocities of the cable detection robot on the x, y, and z axes respectively; is the first derivative of ω x ; is the first derivative of ω y ; is the first derivative of ω z ;

[0022] Step 6, Solve the attitude angular velocity: Substitute the rotational speeds of the four rotors solved in Step 3 into Equation (2) to solve for T x , T y and T z , and then substitute the obtained T x , T y and T z into Equation (3) for solution, so as to obtain ω x , ω y and ω z .

[0023] Step 7, Coordinate transformation: Transform the ω x , ω y and ω z solved in Step 4 to the Earth coordinate system respectively to obtain the corresponding attitude angular velocities ω x ′, ω y ′and ω z ′in the Earth coordinate system.

[0024] Step 8, Two-degree-of-freedom control: By controlling the rotational speeds of the four rotor motors, adjust the attitude angular velocities ω x ′, ω y ′and ω z ′of the cable inspection robot in the Earth coordinate system, and further realize the two-degree-of-freedom control of the cable inspection robot in the axial and circumferential directions of the cable.

[0025] The cable is a smooth cable or a spiral cable.

[0026] In Step 7, the coordinate transformation formula is as follows:

[0027]

[0028] where θ is the pitch angle of the cable inspection robot in the inspection robot coordinate system; φ is the roll angle of the cable inspection robot in the inspection robot coordinate system.

[0029] The calculation formulas for θ and φ are respectively: θ = 90° - α; φ = 90°, where α is the cable inclination angle.

[0030] In Step 8, during two-degree-of-freedom control, for cables with different inclination angles α, by only controlling the rotational speeds of the four rotor motors, the cable inspection robot can rise steadily around the cable with the same Z-axis rotational angular velocity ω z ′.

[0031] In step 8, during two-degree-of-freedom control, for a cable with a uniformly variable diameter, by setting a pressure sensor on the flexible wheel of the cable detection robot and adjusting the pressure value between the flexible wheel and the cable, the pressure between the flexible wheel and the cable is maintained within the set pressure range, thereby realizing the stable climbing of the cable detection robot.

[0032] The number and position of the flexible wheels need to be set according to the pitch of the cable helix and the cable diameter.

[0033] c T =1.201×10 -5 N / (rad / s) 2 ,c M =1.574×10 -7 N·m / (rad / s) 2 。

[0034] In step 8, during two-degree-of-freedom control, when the rotor rotates, the air will exert a resistance on the rotor, and the resistance forms a counter-torque applied to the cable detection robot; when the speeds of the four rotors are equal, and the rotors No. 1 and 3 rotate counterclockwise, and the rotors No. 2 and 4 rotate clockwise, the counter-torques acting on the rotors will cancel each other out, and the cable detection robot reaches a balanced state; then, by changing the speeds of each rotor, the control of the motion state of the cable detection robot is realized.

[0035] When the total lift force is greater than or less than the component of the weight of the cable detection robot on the Z-axis, the cable detection robot will rise or fall along the cable; when the speeds of the rotors No. 1 and 3 are the same, the speeds of the rotors No. 2 and 4 are the same, and the speeds of the rotors No. 1 and 3 are greater than or less than the speeds of the rotors No. 2 and 4, the cable detection robot will rotate clockwise or counterclockwise, and the cable detection robot shows a helical rising motion state; when the total lift force is equal to the component of the weight of the cable detection robot on the Z-axis, the cable detection robot will hover in the air.

[0036] The present invention has the following beneficial effects: The robot of the present invention can realize arbitrary control of two degrees of freedom in the axial and circumferential directions of the cable by controlling the motor speeds of the four rotors, and can detect spiral cables with different inclinations and diameters in a controllable helical rising motion posture. BRIEF DESCRIPTION OF THE DRAWINGS

[0037] Figure 1 It is a schematic diagram of the detection robot applied to the detection of a smooth long straight cable.

[0038] Figure 2 It is a schematic diagram of the detection robot applied to the detection of a spiral cable.

[0039] Figure 3 It is a top view of the detection robot applied to a smooth long straight cable.

[0040] Figure 4 Schematic diagram of the quadrotor climbing mechanism of the inspection robot.

[0041] Figure 5 Schematic diagram of the suspension mechanism of the inspection robot.

[0042] Figure 6 Schematic diagram of the coordinate system established in the present invention.

[0043] Figure 7 For the attitude angular velocity ω x ′, ω y ′ and ω z ′ variation law with time.

[0044] Figure 8 For the variation laws of the pitch angle θ′, roll angle φ′, and yaw angle ψ′ with time in the earth coordinate system.

[0045] Figure 9 Graph of the relationship between the resultant upward force of the robot and the cable inclination angle.

[0046] Among them:

[0047] 1 - Climbing mechanism.

[0048] 11 - Battery.

[0049] 2 - Suspension mechanism.

[0050] 21 - Arm mounting plate; 22 - Fixed plate; 23 - Link.

[0051] 3 - Data box.

[0052] 4 - Cable guiding and clamping assembly.

[0053] 41 - Steering structure; 42 - Flexible wheel; 43 - Pressure sensor.

[0054] 5 - Connector.

[0055] 7 - Smooth long straight cable.

[0056] 8 - Helical cable. Specific embodiments

[0057] The present invention will be further described in detail below in conjunction with the accompanying drawings and specific preferred embodiments.

[0058] In the description of the present invention, it should be understood that the orientation or positional relationships indicated by terms such as "left side", "right side", "upper part", "lower part", etc. are based on the orientation or positional relationships shown in the drawings. These are only for the convenience of describing the present invention and simplifying the description, rather than indicating or implying that the device or element referred to must have a specific orientation, be constructed and operate in a specific orientation. "First", "second", etc. do not represent the importance of the components, so they should not be construed as limitations on the present invention. The specific dimensions adopted in this embodiment are only for illustrating the technical solution by way of example and do not limit the protection scope of the present invention.

[0059] As Figure 1 shown, a robot for cable detection includes a climbing mechanism 1, a suspension mechanism 2, and a cable guiding and clamping assembly 4.

[0060] The climbing mechanism 1 is preferably a quadcopter climbing mechanism. The climbing mechanism 1 includes four rotors, four rotor motors, four arms 11, and two arm mounting plates 12.

[0061] One end of each arm 11 is used to mount a rotor, and the other end of the arm 11 is mounted between the two arm mounting plates 12, connecting the two arm mounting plates 12 to form a whole.

[0062] The rotor is driven to rotate by the corresponding rotor motor. The rotor motor can adjust the attitude of the detection robot, so that the main central axis of the climbing mechanism 1 flies in a posture parallel to the central axis of the cable to provide the maximum upward power.

[0063] The number of the suspension mechanisms 2 is two groups, namely the upper suspension and the lower suspension, which are symmetrically connected to the upper and lower sides of the climbing mechanism 1. Each group of suspension mechanisms 2 includes a fixing plate 21 and a connecting rod 22. The fixing plate 21 is fixed to the arm mounting plate 12 through the connecting rod 22.

[0064] Furthermore, each arm mounting plate is a detachable assembled structure, and preferably includes a left semi-circular mounting plate and a right semi-circular mounting plate symmetrically distributed on both sides of the cable. The left semi-circular mounting plate and the right semi-circular mounting plate are fixedly connected through a connecting member.

[0065] Each fixing plate 21 preferably includes a right semi-fixing plate and a left semi-fixing plate symmetrically distributed on both sides of the cable; the right semi-fixing plate and the left semi-fixing plate are fixedly connected through a connecting member.

[0066] The detachable structure settings of the arm mounting plate and the fixing plate make disassembly and installation convenient, with a modular design. Enough space is left between the fixing plate 21 and the arm mounting plate 12 for wiring and adding other cable detection equipment and repair equipment.

[0067] Several sets of cable guiding and clamping assemblies 4 are circumferentially installed on the upper part of the fixed plate 21; each set of cable guiding and clamping assemblies 4 includes a steering structure 41, a flexible wheel 42 and a pressure sensor 43. The flexible wheel 42 is connected to the driving motor through the steering mechanism 41, and the flexible wheel 42 is rollingly arranged outside the cable, and the pressure sensor 43 is arranged on the flexible wheel 42. Preferably, the steering mechanism adopts a 360° steering mechanism, and the pressure sensor 43 is arranged on the flexible wheel 42 to measure the pressure between the flexible wheel 12 and the cable surface, and then feedback it to the driving motor to adjust the pressure between the flexible wheel 42 and the cable. The driving motor, the flexible wheel and the pressure sensor work together to press the suspension device on the cable surface.

[0068] A data box 3 is installed at the lower part of the climbing mechanism 1, and a battery 13 is arranged on the climbing mechanism 1 for power supply. The data box 3 is used to store the collected data and receive work instructions.

[0069] During implementation, due to the modular structure of the present invention, for cables with different diameters and types, appropriate climbing mechanisms and suspension mechanisms are selected, and the number and positions of flexible wheels are arranged according to the pitch of the cable helix and the cable diameter. The main central axis of the quadrotor climbing mechanism flies in a horizontal or even coincident posture with the central axis of the cable body to provide the maximum climbing power, greatly improving the detection efficiency. The climbing mechanism can detect the cable in a posture with an unchanged axial direction along the cable and a helical rising posture. The flexible wheel connected by the 360° steering mechanism can rotate 360° on the cable surface.

[0070] Example 1: When detecting the smooth long straight cable 7, when the diameter of the detected smooth long straight cable changes uniformly with the length, the pressure sensor on the flexible wheel measures the pressure between the flexible wheel and the cable surface, and feeds it back to the driving motor to adjust the pressure between the flexible wheel and the cable. They work together to press the suspension device on the cable surface.

[0071] Example 2: When detecting the spiral cable 8, the flexible wheel will perform a helical rising movement on the cable surface between the helix and the line, avoiding the flexible wheel from making a cross-line movement. Compared with other wheeled and magnetic adsorption robots that need to continuously perform obstacle-crossing movements for the helix on the spiral cable, the overall stability of the detection robot is greatly increased.

[0072] A two-degree-of-freedom motion control method for a cable detection robot includes the following steps.

[0073] Step 1: Establish a coordinate system: The cable detection robot is a quadrotor detection robot with four rotors; the coordinate system includes the earth coordinate system E(X, Y, Z) and the detection robot coordinate system e(x, y, z). The established coordinate system is as Figure 1 shown.

[0074] The origin O of the ascending Earth coordinate system E(X, Y, Z) is the center of the Earth. The X-axis is the horizontal direction passing through the center of the Earth. The Y-axis is the horizontal direction passing through the center of the Earth and perpendicular to the X-axis. The Z-axis is the vertical direction passing through the center of the Earth. The origin o of the inspection robot coordinate system e(x, y, z) is the symmetry center of the cable inspection robot. The z-axis is the axis passing through the origin o and parallel to the cable axis. The x-y plane is parallel to the plane where the four rotors are located.

[0075] Step 2: Establish a rotational speed model.

[0076] After a current is input to the motor, it cannot immediately reach a stable rotational speed. It is a gradually increasing process. Through mechanism analysis, the motor can be approximately regarded as a first-order system. Assuming the battery power remains unchanged, in the inspection robot coordinate system, a rotational speed model is established for each rotor motor. The rotational speed model is a functional equation between the rotational speed of the rotor motor and the input current. The specific expression is:

[0077]

[0078]

[0079] In the formula, T m is the motor response time constant, generally in the range of dozens to hundreds of milliseconds. The smaller it is, the faster the motor can reach a stable rotational speed. is the rotational speed of the rotor motor at the current time t; σ(t) is the magnitude of the input current at the current time t, which can be used as a step signal. In simulation modeling, it is simplified to an input step signal, so its value ranges between [0, 1]. is the steady-state rotational speed; σ c is the input current value at the steady-state rotational speed; C R is the ratio of c to σ is the offset coefficient, which reflects the offset rotational speed of the motor under the condition of zero input current.

[0080] Step 3: Solve the rotational speed: Substitute the input currents corresponding to the four rotors into formula (1) respectively, so as to obtain the rotational speeds of the four rotors, which are and

[0081] Step 4: Establish a control efficiency model: In the inspection robot coordinate system, according to the rotational speed model established in Step 2, a control efficiency model is established for the cable inspection robot; the control efficiency model includes a lift model and moment models on the x, y, and z axes, and is specifically expressed as:

[0082]

[0083] In the formula, f is the lift of the rotor; c T, c M Characterizes the relationship between the motor speed and the torques on each axis, measured in a specific experimental environment, and the preferred value is c T = 1.201×10 -5 N / (rad / s) 2 ; The selected value is c M = 1.574×10 -7 N·m / (rad / s) 2 .

[0084] T x , T y and T z are the rotational torques of the cable inspection robot on the x, y, and z axes respectively; d is the distance between the rotor motor and the central axis of the cable, and the preferred value is d = 0.225m.

[0085] Step 5: Establish an attitude model: In the coordinate system of the inspection robot, according to Euler's equation, establish an attitude model of torque and angular velocity for the cable inspection robot as follows:

[0086]

[0087] In Equation (3), J xx , J yy and J zz are the moments of inertia of the cable inspection robot on the x, y, and z axes respectively; ω x , ω y and ω z are the attitude angular velocities of the cable inspection robot on the x, y, and z axes respectively; is the first derivative of ω x ; is the first derivative of ω y ; is the first derivative of ω z .

[0088] Step 6: Solve for the attitude angular velocity: Substitute the rotational speeds of the four rotors solved in Step 3 into Equation (2) to solve for T x , T y and T z , and then substitute the obtained T x , T y and T z into Equation (3) for solution, so as to obtain ω x , ω y and ω z .

[0089] Step 7: Coordinate transformation: The ω x , ω y and ω z, they are respectively transformed into the Earth coordinate system to obtain the corresponding attitude angular velocities ω x ′, ω y ′ and ω z ′.

[0090] The upward coordinate transformation formula is preferably:

[0091]

[0092] In the formula, θ is the pitch angle of the cable inspection robot in the inspection robot coordinate system, that is, the angle between the X direction in the inspection robot coordinate system and the horizontal plane; φ is the roll angle of the cable inspection robot in the inspection robot coordinate system, that is, the angle of rotation of the body Y axis around the X axis; α is the cable inclination angle.

[0093] In addition, after the pitch angle θ, roll angle φ and yaw angle ψ in the inspection robot coordinate system are transformed into the Earth coordinate system, they are respectively the pitch angle θ′, roll angle φ′ and yaw angle ψ′.

[0094] Step 8, two-degree-of-freedom control: By controlling the rotational speeds of the four rotor motors, the attitude angular velocities ω x ′, ω y ′ and ω z ′ of the cable inspection robot in the Earth coordinate system are adjusted, and then the two-degree-of-freedom control of the cable inspection robot in the cable axial and circumferential directions is realized.

[0095] During the two-degree-of-freedom control, for cables with different inclination angles α, by only controlling the rotational speeds of the four rotor motors, the cable inspection robot can rise steadily around the cable with the same Z-axis rotational angular velocity ω z ′.

[0096] During the two-degree-of-freedom control, for cables with a uniform variable diameter, by setting pressure sensors on the flexible wheels of the cable inspection robot and adjusting the pressure value between the flexible wheels and the cable, the pressure between the flexible wheels and the cable is kept within the set pressure range, thereby realizing the stable climbing of the cable inspection robot.

[0097] During the two-degree-of-freedom control, when the rotor rotates, the air will exert a resistance on the rotor, and the resistance forms a reaction torque applied to the cable inspection robot; when the rotational speeds of the four rotors are equal, and the No. 1 and No. 3 rotors rotate counterclockwise, and the No. 2 and No. 4 rotors rotate clockwise, the reaction torques acting on the rotors will cancel each other out, and the cable inspection robot reaches a balanced state; then, by changing the rotational speeds of each rotor, the control of the motion state of the cable inspection robot is realized.

[0098] When the total lift force (i.e., the upward resultant force F = f - mgsinα) is greater than or less than the component of the cable inspection robot's weight along the Z-axis, the cable inspection robot will ascend or descend along the cable; when the rotational speeds of the No. 1 and No. 3 rotors are the same, the rotational speeds of the No. 2 and No. 4 rotors are the same, and the rotational speeds of the No. 1 and No. 3 rotors are greater than or less than those of the No. 2 and No. 4 rotors, the cable inspection robot will rotate clockwise or counterclockwise, and the cable inspection robot will be in a helical ascending motion state; when the total lift force is equal to the component of the cable inspection robot's weight along the Z-axis, the cable inspection robot will hover in the air.

[0099] Helical Climbing Simulation Experiment Analysis

[0100] A robot dynamics simulation model was built in the MATLAB simulation environment. The robot can finally ascend stably at a rotational angular velocity of ω z ′ = 0.72 rad / s within the cable inclination angle range of α = 30° - 90°. During the simulation process, the motor constant T m = 17.3×10 -3 , the motor speed change ratio C R = 706.1, the robot mass m = 1.4 kg, the gravitational acceleration g = 9.8 N / kg, and the moments of inertia about the x-axis, y-axis, and z-axis are J xx = 1.563×10 -2 kg·m 2 , J yy = 1.563×10 -2 kg·m 2 , J zz = 2.636×10 -2 kg·m 2 respectively. The simulation time is 30 s, and factors such as air resistance are ignored. During the simulation process, the initial input current of the robot is [0.53; 0.51; 0.53; 0.51]. After the robot starts, the lift force gradually increases and finally overcomes gravity to climb upward. During the climbing process, the attitude change rate of the robot along the z-axis gradually increases, and finally it rotates around the cable at a speed of ω z ′ = 0.72 rad / s.

[0101] Figure 7 and Figure 8 reflect the change laws of the attitude angular velocity and attitude angle of the robot when the initial input current of the robot is [0.53; 0.51; 0.53; 0.51]. The attitude angular velocity is mainly provided by the torques of the four propellers. The time for it to reach the stable rotation speed is related to the motor constant, and the robot attitude angular velocity is related to the final rotation speed of the motor. Figure 9It is a relationship diagram of the resultant upward force of the robot and the inclination angle of the cable. The total lifting force is provided by the lifting forces of four propellers. At the same initial throttle, as the inclination angle of the cable gradually increases, the upward force of the robot gradually decreases. However, as the inclination angle of the cable gradually increases, its influence on the upward speed of the robot becomes smaller and smaller. Synthesizing Figure 7 , Figure 8 and Figure 9 , the stable spiral ascent of the robot can be achieved by controlling the rotational speed of the motors.

[0102] According to the force analysis, the supporting force of the cable is balanced with mgcosα. When the total lifting force is greater than mgsinα, it is detected that the robot slowly ascends along the cable. Keeping the rotational speeds of the No. 1 and No. 3 rotors the same, and the rotational speeds of the No. 2 and No. 4 rotors the same. When the rotational speeds of the No. 1 and No. 3 rotors are greater than (less than) the rotational speeds of the No. 2 and No. 4 rotors, it is detected that the robot rotates clockwise (counterclockwise), and it is detected that the robot is in a spiral ascending motion state. We found that when the 4 rotors start at their respective fixed rotational speeds, the inclination angle only affects the ascending speed of the UAV along the climbing pole and does not affect the spiral self-rotation speed of the UAV.

[0103] In summary, the robot of the present invention can achieve arbitrary control of two degrees of freedom in the axial and circumferential directions of the cable by controlling the rotational speeds of the four rotors, and detect spiral cables with different inclination angles and diameters in a controllable spiral ascending motion posture.

[0104] The preferred embodiments of the present invention have been described in detail above. However, the present invention is not limited to the specific details in the above embodiments. Within the scope of the technical concept of the present invention, various equivalent transformations can be made to the technical solutions of the present invention, and these equivalent transformations all belong to the protection scope of the present invention.

Claims

1. A two-degree-of-freedom motion control method for a cable inspection robot, characterized in that: It includes the following steps: Step 1, establish a coordinate system: The cable detection robot is a quadrotor detection robot with four rotors; the coordinate system includes the Earth coordinate system E(X, Y, Z) and the detection robot coordinate system e(x, y, z); among them, the origin O of the Earth coordinate system E(X, Y, Z) is the center of the Earth, the X-axis is the horizontal direction passing through the center of the Earth, the Y-axis is the horizontal direction passing through the center of the Earth and perpendicular to the X-axis, and the Z-axis is the vertical direction passing through the center of the Earth; the origin o in the detection robot coordinate system e(x, y, z) is the symmetry center of the cable detection robot, the z-axis is the axis passing through the origin o and parallel to the cable axis, and the x-y plane is parallel to the plane where the four rotors are located. Step 2, establish a rotational speed model: In the detection robot coordinate system, a rotational speed model is established for each rotor motor. The rotational speed model is a functional equation between the rotational speed of the rotor motor and the input current, and the specific expression is: Where, T m is the motor response time constant; is the rotational speed of the rotor motor at the current time t; σ(t) is the magnitude of the input current at the current time t; is the steady-state rotational speed; σ c is the input current value at the steady-state rotational speed; C R is the ratio of c to σ; is the offset coefficient, reflecting the offset rotational speed of the motor under the condition of zero input current; Step 3, Solve for the rotational speed: Substitute the motor input currents corresponding to the four rotors into Equation (1) respectively, so as to obtain the rotational speeds of the four rotors, which are respectively and Step 4, establish a control efficiency model: In the detection robot coordinate system, according to the rotational speed model established in Step 2, a control efficiency model is established for the cable detection robot; the control efficiency model includes a lift model and torque models on the x, y, and z axes, and is specifically expressed as: Where f is the rotor lift force; c T and c M characterize the relationship between the motor speed and the torques on each axis, and are measured under specific experimental conditions; T x and T y and T z are the rotational torques of the cable inspection robot on the x, y, and z axes respectively; d is the distance between the rotor motor and the central axis of the cable; Step 5, establish an attitude model: In the detection robot coordinate system, according to Euler's equation, an attitude model of torque and angular velocity is established for the cable detection robot as: In Equation (3), J xx , J yy and J zz are the moments of inertia of the cable inspection robot about the x, y, and z axes, respectively; ω x , ω y and ω z are the angular velocities of the attitude of the cable inspection robot about the x, y, and z axes, respectively; is the first derivative of ω x ; is the first derivative of ω y ; is the first derivative of ω z ; Step 6, solve the attitude angular velocity: Substitute the rotational speeds of the four rotors solved in Step 3 into Equation (2) to solve for T x , T y and T z , and then substitute the obtained T x , T y and T z into Equation (3) for solution, so as to obtain ω x , ω y and ω z ; Step 7, Coordinate Transformation: Transform the ω x , ω y and ω z obtained in Step 4 to the Earth coordinate system respectively to obtain the corresponding attitude angular velocities ω x ′, ω y ′ and ω z ′ in the Earth coordinate system; Step 8, two-degree-of-freedom control: By controlling the rotational speeds of the four rotor motors, the attitude angular velocities ω x ′, ω y ′ and ω z ′ of the cable inspection robot are adjusted, thereby achieving two-degree-of-freedom control of the cable inspection robot in the axial and circumferential directions of the cable.

2. The two-degree-of-freedom motion control method of the cable detection robot according to claim 1, characterized in that: The cable is a smooth cable or a spiral cable.

3. The two-degree-of-freedom motion control method of the cable detection robot according to claim 1, characterized in that: In Step 7, the coordinate transformation formula is: In the formula, θ is the pitch angle of the cable detection robot in the detection robot coordinate system; φ is the roll angle of the cable detection robot in the detection robot coordinate system.

4. The two-degree-of-freedom motion control method of the cable detection robot according to claim 3, characterized in that: The calculation formulas for θ and φ are respectively: θ = 90° - α; φ = 90°, where α is the cable inclination angle.

5. The two-degree-of-freedom motion control method of the cable detection robot according to claim 1, characterized in that: In step 8, during the two-degree-of-freedom control, for cables with different inclination angles α, by only controlling the rotational speeds of the four rotor motors, the cable detection robot can rise steadily along the cable at the same angular velocity ω z ' around the cable.

6. The two-degree-of-freedom motion control method of the cable detection robot according to claim 1, characterized in that: In Step 8, during two-degree-of-freedom control, for a cable with a uniformly variable diameter, by setting a pressure sensor on the flexible wheel of the cable detection robot and adjusting the pressure value between the flexible wheel and the cable, the pressure between the flexible wheel and the cable is kept within the set pressure range, so as to realize the stable climbing of the cable detection robot.

7. The two-degree-of-freedom motion control method of the cable detection robot according to claim 6, characterized in that: The number and position of the flexible wheels need to be set according to the pitch of the cable helix and the cable diameter.

8. The two-degree-of-freedom motion control method of the cable detection robot according to claim 1, characterized in that: c T = 1.201×10 -5 N / (rad / s) 2 , c M = 1.574×10 -7 N·m / (rad / s) 2 。 9. The two-degree-of-freedom motion control method of the cable detection robot according to claim 1, wherein: In Step 8, during two-degree-of-freedom control, when the rotors rotate, the air will exert a resistance on the rotors, and the resistance forms a counter-torque applied to the cable detection robot; when the rotational speeds of the four rotors are equal, and the No. 1 and No. 3 rotors rotate counterclockwise, and the No. 2 and No. 4 rotors rotate clockwise, the counter-torques acting on the rotors will cancel each other out, and the cable detection robot reaches a balanced state. Then, by changing the rotational speeds of each rotor, the control of the motion state of the cable detection robot is realized.

10. The two-degree-of-freedom motion control method of the cable detection robot according to claim 9, characterized in that: When the total lift is greater than or less than the weight component of the cable detection robot on the Z-axis, the cable detection robot will rise or fall along the cable; when the rotational speeds of the No. 1 and No. 3 rotors are the same, the rotational speeds of the No. 2 and No. 4 rotors are the same, and the rotational speeds of the No. 1 and No. 3 rotors are greater than or less than the rotational speeds of the No. 2 and No. 4 rotors, the cable detection robot will rotate clockwise or counterclockwise, and the cable detection robot shows a spiral upward motion state; when the total lift is equal to the weight component of the cable detection robot on the Z-axis, the cable detection robot will hover in the air.

Images