Automatic leveling control method for snakelike flexible mechanical arm

Abstract

The automatic leveling control method for the snakelike flexible mechanical arm comprises the steps that all steel wire ropes are tensioned, and it is guaranteed that all the steel wire ropes are in the tensioned state; when it is judged that each joint is not at a leveling angle according to angle feedback of an angle encoder, the current rope length of each steel wire rope is calculated by obtaining an included angle between every two adjacent execution joints; calculating the rope length when the pitch angle and the yaw angle of each execution joint are zero, and comparing the rope length with the current rope length to obtain the rope length increment to be tensioned or loosened when each execution joint is moved from the current position to the leveling position; then the driving module acts, the steel wire rope with the rope length needing to be loosened is loosened in place, and then the steel wire rope with the rope length needing to be tensioned is acted to a designated position; and after all the steel wire ropes act in place, whether all the joints are leveled or not is judged continuously till automatic leveling is finished. According to the method, the motor can be controlled to automatically loosen or tension the steel wire rope to achieve automatic leveling of multiple joints, operation is easy, efficiency is high, and precision is high.

Description

Technical Field

[0001] The invention relates to the field of mechanical arm control, and in particular to an automatic leveling control method for a snake-shaped flexible mechanical arm. Background Art

[0002] The serpentine flexible robot arm mostly adopts a parallel rope drive structure with more than ten degrees of freedom, which is suitable for use in a small environment. Compared with the traditional articulated industrial robot, it can achieve flexible obstacle avoidance, hole crossing and other operations. After the serpentine robot arm performs the action, the wire ropes on each joint may be in a random loose and tight state, and the force is concentrated on only a few wire ropes. If left for a long time, the stressed wire ropes will cause a large deformation, affecting the subsequent control performance. Therefore, the joints of the robot arm are generally leveled to make the wire ropes evenly stressed. In addition, the robot arm can also save space after leveling, especially in a small environment where there is a need for leveling.

[0003] When the serpentine flexible manipulator is automatically controlled, the initial reference point is usually the leveling position of each joint of the manipulator, and each wire rope is required to be in a tensioned state to ensure that the tension can be immediately transmitted to the joint when the wire rope is pulled backward to avoid the generation of virtual position. Therefore, it is of great significance to study the automatic leveling control method of the manipulator.

[0004] Since most snake-like robotic arms are driven by more than ten steel ropes, manual leveling requires adjusting each steel rope in turn, and there is mutual influence between the steel ropes. The angle of the front joint will also affect the rope length on the rear joint. The manual leveling process is cumbersome, time-consuming, and has poor leveling accuracy. Therefore, an automatic and efficient leveling control method is needed. Summary of the invention

[0005] In view of the shortcomings of the prior art, the present invention provides an automatic leveling control method for a serpentine flexible robotic arm. By installing an angle encoder on the universal joint, the rope length increment from the current position of each wire rope to the leveling position is calculated, the motor is controlled to pull the wire rope to the corresponding position, and the wire rope is automatically tensioned. The method has the characteristics of automatic adjustment, high efficiency and high precision.

[0006] The technical solution adopted by the present invention is as follows:

[0007] An automatic leveling control method for a serpentine flexible robotic arm, the serpentine flexible robotic arm comprising an actuator joint, a universal joint, a pitch angle encoder, a yaw angle encoder, a steel wire rope and a drive module;

[0008] There are multiple execution joints, and two adjacent execution joints are connected by a universal joint; the pitch angle encoder is installed on the horizontal rotation axis of each universal joint to feedback the pitch angle between two adjacent execution joints, and the yaw angle encoder is installed on the vertical rotation axis of each universal joint to feedback the yaw angle between two adjacent execution joints;

[0009] Each of the actuating joints is driven by three steel wires with circumferential angles differing by 120°, and the steel wires of all the actuating joints are tightened and relaxed by the driving module;

[0010] The automatic leveling control method comprises the following steps:

[0011] Step 1: Tension each wire rope to ensure that each wire rope is in a taut state;

[0012] Step 2: judging whether all joints of the robot arm are at the leveling angle according to the angle feedback of the pitch angle encoder and the yaw angle encoder; when they are at the leveling angle, the automatic leveling ends; when they are not at the leveling angle, executing step 3;

[0013] Step 3: Obtain the angle between two adjacent actuator joints through the pitch angle encoder and the yaw angle encoder, and calculate the current rope length of each wire rope according to the angle;

[0014] Step 4: Calculate the rope length when the pitch angle and yaw angle of each actuator joint are both zero, and compare it with the current rope length calculated in step 3 to obtain the rope length increment that needs to be tightened or loosened when each actuator joint moves from the current position to the leveling position;

[0015] Step 5: The driving module takes action to first loosen the wire rope that needs to be loosened, and then tighten the wire rope that needs to be tightened to the specified position; when all the wire ropes are in place, return to step 2 and continue to judge until the automatic leveling is completed.

[0016] Furthermore, in the step 2, the joints of the robot arm are at a leveling angle, specifically referring to that all pitch angles α and all yaw angles β are within the range of -ε to +ε, where ε is a preset angle deviation threshold.

[0017] Furthermore, the driving module includes a motor for driving the wire rope.

[0018] Furthermore, when the steel wire rope of each execution joint passes through other execution joints, it is parallel to the axis of other execution joints and has a fixed length. The length of the steel wire rope at the joint point varies with the pitch angle and the yaw angle.

[0019] Furthermore, in the step 1, whether the tensioning is completed is determined by the current value of the driving motor corresponding to each steel wire rope in the driving module.

[0020] Furthermore, in the step 4, after obtaining the displacement increments of each steel wire rope to be tightened or loosened, the displacement increments are converted into position increments of the drive motors corresponding to each steel wire rope.

[0021] Furthermore, in step five, when loosening or tightening the steel wire ropes, the operations are performed sequentially according to the order of the steel wire rope numbers to ensure that the tightened steel wire ropes during the operation do not affect the tension of the subsequent steel wire ropes.

[0022] The beneficial effects of the present invention are as follows:

[0023] 1. The present invention adopts an angle encoder to perform closed-loop feedback on the pitch angle and yaw angle of each joint, and the leveling accuracy is high.

[0024] 2. The present invention can realize automatic leveling of the robot arm with high leveling efficiency.

[0025] 3. The present invention can realize automatic tensioning of each steel wire rope, and can still maintain a high level after the mechanical arm is placed for a long time. BRIEF DESCRIPTION OF THE DRAWINGS

[0026] Figure 1 It is a schematic diagram of the structure of the robotic arm after leveling disclosed in an embodiment of the present invention.

[0027] Figure 2 It is a schematic diagram of the joints and joint points of the mechanical arm of the present invention.

[0028] Figure 3 It is a schematic diagram of the installation of the angle encoder of the present invention.

[0029] Figure 4 It is a flow chart of the automatic leveling control method of the robot arm of the present invention.

[0030] Figure 5 It is a flow chart of automatic tensioning control of the steel wire rope on the mechanical arm of the present invention.

[0031] In the figure, 1 is the execution joint, 2 is the universal joint, 3 is the pitch angle encoder, 4 is the yaw angle encoder, 5 is the wire rope, 6 is the drive module, 101 is the first execution joint, 102 is the second joint, 103 is the third joint, 104 is the fourth joint, 105 is the fifth joint, 106 is the terminal joint, 201 is the first joint point, 202 is the second joint point, 203 is the third joint point, 204 is the fourth joint point, 205 is the fifth joint point, and 206 is the terminal joint point. DETAILED DESCRIPTION

[0032] The present invention will be described in detail below based on the accompanying drawings and preferred embodiments, and the purpose and effects of the present invention will become more clear. It should be understood that the specific embodiments described herein are only used to explain the present invention and are not used to limit the present invention.

[0033] like Figure 1 As shown, the serpentine flexible mechanical arm of the present invention comprises an actuator joint 1, a universal joint 2, a pitch angle encoder 3, a yaw angle encoder 4, a steel wire rope 5 and a driving module 6. There are multiple actuator joints, and two adjacent actuator joints are connected by a universal joint 2. The pitch angle encoder 3 is installed on the horizontal rotation axis of the universal joint to feedback the pitch angle between the two actuator joints. The yaw angle encoder 4 is installed on the vertical rotation axis of the universal joint to feedback the yaw angle between the two actuator joints. Each actuator joint is driven by three steel wire ropes 5 with an angle difference of 120°. The steel wire ropes 5 are tightened and relaxed by the motor, reducer, lead screw and slider combination on the driving module 6.

[0034] Specifically, Figure 2 As shown, the execution joint 1 is respectively called the first joint 101, the second joint 102, the third joint 103, the fourth joint 104, the fifth joint 105, and the terminal joint 106 according to the distance between the execution joint 1 and the driving module 6; the universal joint 2 is respectively called the first joint point 201, the second joint point 202, the third joint point 203, the fourth joint point 204, the fifth joint point 205, and the terminal joint point 206 according to the distance between the execution joint 1 and the driving module 6.

[0035] Specifically, Figure 3 As shown, a pitch angle encoder 3 and a yaw angle encoder 4 are installed on the universal joint of each joint point, the pitch angle α represents the angle of the adjacent joints in the up-down direction, and the yaw angle β represents the angle of the adjacent joints in the left-right direction, wherein the pitch angle at the first joint point 201 is represented by α1, and the yaw angle is represented by β1, the pitch angle at the second joint point 202 is represented by α2, and the yaw angle is represented by β2, ..., the pitch angle at the end joint point 206 is represented by α6, and the yaw angle is represented by β6; after the robot arm is in the absolute leveling position, all pitch angles α1~α6 are 0°, and all yaw angles β1~β6 are 0°

[0036] Specifically, three steel wires are installed on each joint for driving. Each steel wire is arranged 120° apart in the circumferential direction. When the steel wire passes through the previous execution joint, it is parallel to the joint axis and has a fixed length. The length of the steel wire at the joint point varies with the pitch angle and the yaw angle. The length of the steel wire is defined as the length from the end face of the driving module to the fixed installation point on the joint. The lengths of the three steel wires driving the first joint 101 are l1, l2, and l3, the lengths of the three steel wires driving the second joint 102 are l4, l5, and l6, ..., and the lengths of the three steel wires driving the terminal joint 106 are l 16 , l 17 , l 18 ; The lengths of 1#~18# steel wire ropes are adjusted by 1#~18# servo motors inside the drive module 6 respectively.

[0037] Figure 4 The figure is a flow chart of an automatic leveling control method for a serpentine flexible robotic arm, and the automatic leveling control method comprises the following steps:

[0038] Step 1: Automatically tension 1#~18# steel wire ropes; Figure 5 This is the flow chart of the automatic tensioning control of the wire rope. First, set the motor tensioning current i s When the feedback current of the motor is greater than this value, the wire rope is considered to be tensioned. The initial motor number j = 1, and the tensioning speed of motor 1# is set to V s , control the 1# motor to rotate in the direction of the wire rope tightening, and collect the feedback current i in real time during the motor rotation process f1 , when i f1 Greater than the tension current i s After that, the 1# motor stops, the motor number j=j+1, and the tensioning speed of the j# motor is set to V s , control the j# motor to rotate in the direction of the wire rope tightening, and collect the feedback current i in real time during the motor rotation process fj , when i fj Greater than the tension current i s After that, the j# motor stops rotating; when j<18, the motor number j=j+1, and the above tensioning process is repeated. After j=18, the automatic tensioning step is completed.

[0039] Step 2: Determine whether the vehicle is currently in the leveling position; based on the values ​​fed back by the pitch angle encoder and the yaw angle encoder, if the absolute values ​​of the pitch angles α1~α6 and β1~β6 are all less than the preset angle deviation threshold ε, it is considered that the automatic leveling is completed, otherwise continue to step 3.

[0040] Step 3: Calculate the current rope length l1~l 18The included angle between two adjacent execution joints is obtained through the pitch angles α1~α6 and the yaw angles β1~β6. When the size of the robot arm is known, the current rope length of each wire rope can be calculated based on the included angle using the DH coordinate transformation.

[0041] Step 4: Calculate the motor position increment; calculate the length l of each wire rope when α1~α6, β1~β6 are all equal to 0° 01 ~l 018 , the rope length increment from the current position to the leveling position is △l1=l1-l 01 , △l2=l2-l 02 ,……,△l 18 =l6-l 018 , under the premise of knowing the lead screw δ, motor reduction ratio R, and motor encoder single-turn resolution κ, convert the rope length increment into the motor encoder position increment △C1=△l 1* R*κ / δ、△C2=△l 2* R*κ / δ,……,△C 18 =△l 18* R*κ / δ, △Cj (j=1~18) is a positive value, which means that the wire rope needs to be tightened, and a negative value, which means that the wire rope needs to be loosened.

[0042] Step 5: The motor executes the action according to the position increment; the servo motor adopts the position control mode, first moves the motor with △Cj (j=1~18)<0 to the corresponding position increment, and then moves the motor with △Cj>0 to the corresponding position increment. The action sequence is from small to large according to the number j to ensure that the tightened wire rope during the action does not affect the tension of the subsequent wire rope.

[0043] The above five steps are executed repeatedly. When the pitch angle and yaw angle values ​​fed back by each joint point in the second step are both within ±ε (allowable deviation), the automatic leveling is completed.

[0044] After the automatic leveling is completed, all the steel wire ropes of the robotic arm are in a tensioned state, and the robotic arm can still maintain a good horizontal angle after being placed for a long time.

[0045] The present invention is mainly used in the field of robotic arm control. It is an automatic leveling control method for a serpentine flexible robotic arm. It can realize automatic leveling of multiple joints of the serpentine robotic arm and automatic tensioning of wire ropes. It has the advantages of simple control, convenient operation, high efficiency and high leveling accuracy. The control method can be transplanted to flexible serpentine robotic arms with more joints for application.

[0046] Those skilled in the art can understand that the above are only preferred examples of the invention and are not intended to limit the invention. Although the invention is described in detail with reference to the above examples, those skilled in the art can still modify the technical solutions recorded in the above examples or replace some of the technical features therein with equivalents. Any modification, equivalent replacement, etc. made within the spirit and principle of the invention shall be included in the protection scope of the invention.

Claims

1. An automatic leveling control method for a snake-shaped flexible robotic arm, characterized in that: The serpentine flexible mechanical arm comprises an actuator joint, a universal joint, a pitch angle encoder, a yaw angle encoder, a steel wire rope and a driving module; There are multiple execution joints, and two adjacent execution joints are connected by a universal joint; the pitch angle encoder is installed on the horizontal rotation axis of each universal joint to feedback the pitch angle between two adjacent execution joints, and the yaw angle encoder is installed on the vertical rotation axis of each universal joint to feedback the yaw angle between two adjacent execution joints; Each of the actuating joints is driven by three steel wires with circumferential angles differing by 120°, and the steel wires of all the actuating joints are tightened and relaxed by the driving module; The automatic leveling control method comprises the following steps: Step 1: Tension each wire rope to ensure that each wire rope is in a taut state; Step 2: judging whether all joints of the robot arm are at the leveling angle according to the angle feedback of the pitch angle encoder and the yaw angle encoder; when they are at the leveling angle, the automatic leveling ends; when they are not at the leveling angle, executing step 3; Step 3: Obtain the angle between two adjacent actuator joints through the pitch angle encoder and the yaw angle encoder, and calculate the current rope length of each wire rope according to the angle; Step 4: Calculate the rope length when the pitch angle and yaw angle of each actuator joint are both zero, and compare it with the current rope length calculated in step 3 to obtain the rope length increment that needs to be tightened or loosened when each actuator joint moves from the current position to the leveling position; Step 5: The driving module takes action to first loosen the wire rope that needs to be loosened, and then tighten the wire rope that needs to be tightened to the specified position; when all the wire ropes are in place, return to step 2 and continue to judge until the automatic leveling is completed.

2. The automatic leveling control method for a serpentine flexible robotic arm according to claim 1, characterized in that: In the step 2, the joints of the robot arm are at the leveling angle, which specifically means that all pitch angles α and all yaw angles β are within the range of -ε to +ε, where ε is a preset angle deviation threshold.

3. The automatic leveling control method for a serpentine flexible robotic arm according to claim 1, characterized in that: The driving module includes a motor for driving the wire rope.

4. The automatic leveling control method for a serpentine flexible robotic arm according to claim 1, characterized in that: When the steel wire rope of each execution joint passes through other execution joints, it is parallel to the axis of other execution joints and has a fixed length. The length of the steel wire rope at the joint point varies with the pitch angle and yaw angle.

5. The automatic leveling control method for a serpentine flexible robotic arm according to claim 1, characterized in that: In the step 1, whether the tensioning is completed is determined by the current value of the driving motor corresponding to each steel wire rope in the driving module.

6. The automatic leveling control method for a serpentine flexible robotic arm according to claim 1, characterized in that: In the step 4, after the displacement increments of each steel wire rope to be tightened or loosened are obtained, the displacement increments are converted into position increments of the drive motors corresponding to each steel wire rope.

7. The automatic leveling control method for a serpentine flexible robotic arm according to claim 1, characterized in that: In the step 5, when loosening or tightening the steel wire ropes, the operations are performed in sequence according to the numbering sequence of the steel wire ropes to ensure that the tightened steel wire ropes do not affect the tension of the subsequent steel wire ropes during the operation.

Images