CN109048961B - Climbing truss robot capable of swinging and grasping long-distance truss rods and its control method - Google Patents

Abstract

A climbing truss robot capable of swinging and grasping a long-distance truss rod and a control method thereof relate to a robot and a control method thereof. The invention solves the problems that the existing truss climbing robot cannot reliably grasp the distant truss rod and cannot move continuously in the truss structure. The friction material is added inside the gripper of the robot to provide greater damping torque; the gyro sensor is added on the gripper to eliminate the influence of friction wheel slippage during high-speed swing; and a comprehensive consideration of the friction wheel reversal feedback mechanism and gyroscope is given. The feedback data of the instrument is used to generate the feedback method of the motion state of the underactuated joint. The excitation control method based on phase difference is proposed, and a swing grab bar control method initiated from the natural suspension state and a continuous movement control method are proposed. The swing grab bar experiment and continuous movement initiated from the natural suspension state are carried out. Simulations and experiments verify the effectiveness of the proposed method. The invention is applied to the field of climbing truss robots.

Description

Truss climbing robot capable of swinging and grabbing remote truss rod and control method thereof

Technical Field

The invention relates to a truss climbing robot and a control method thereof.

Background

The climbing truss robot is a typical mobile robot in a discontinuous medium, and the application environment of the climbing truss robot comprises a rigid truss structure in buildings such as bridges and buildings, a flexible truss (network) structure formed by lapping cables, chains and the like, and even an unstructured complex environment such as naturally-growing tree branches. The truss climbing robot does not comprise an under-actuated joint, but is influenced by gravity, and a paw of the robot and a truss rod with a circular section can rotate relatively to form the under-actuated joint, so that an under-actuated system is formed. The moving mode of the climbing truss robot is simulated in swinging and branch gripping motions of primates (apes) among branches, one paw is used for holding a truss rod as a supporting hand, and the other moving paw swings to and grips the other truss rod under the swinging motion effect of coupling of an under-actuated joint and an active joint of the robot, so that the robot repeatedly realizes rapid and continuous movement in a discontinuous truss structure.

The two-rod bionic monkey robot with one active joint is developed by professor of Futian Ministry of Japan famous and ancient House university in 1996, and successfully simulates the moving mode of swinging branches, but the developed bionic monkey robot does not have a wrist joint, so that the robot has insufficient active adjusting capability on the position of a moving paw in the moving process, and a hook-shaped paw has no driving capability, so that the movement uncertainty of an underactuated joint at a supporting hand is difficult to eliminate, the robot is difficult to ensure the stability of grabbing rods, and the grabbing rods often fail and need to swing again to excite vibration in the branch swinging movement experiment.

The invention patent with publication number CN101434268 and patent number ZL200810209775.4, published as 2009, 5/20, provides a dual-purpose double-arm hand-moving robot for ground moving and space analysis and climbing. The robot has 10 degrees of freedom, two first vision sensors symmetrically arranged on a middle bedplate and two second vision sensors respectively arranged on a left paw and a right paw, and a gripping mechanism and a wheel type moving mechanism are respectively designed on the left paw and the right paw, so that the robot can flexibly move in truss rod spaces with different section shapes and sizes and can move on the ground with smaller energy consumption. No method of controlling swinging grab bars when grabbing a circular cross-section truss bar is given.

The invention patent with publication number of CN103332233A and patent number of ZL201310288965.0 is patent of invention with publication number of 10 and 2 in 2013, and provides a three-degree-of-freedom truss climbing robot and a large-damping under-actuated control method thereof. The robot gripper is provided with a friction wheel back-rotation feedback mechanism, and can measure the rotation angle of an under-actuated joint formed when the robot gripper grips a circular truss rod, and the rotation angle is used for calculating target tracks of other active joints during swinging and grabbing the rod in the control method, so that the robot can complete swinging grabbing rod movement in a truss structure formed by truss rods with circular sections. However, the three-degree-of-freedom truss climbing robot and the large-damping under-actuated control method thereof still stay at the theoretical and simulation research stage, testing and verification in an experimental environment are not performed, and a control method corresponding to continuous movement is not given in the control method.

In addition, other scholars at home and abroad also carry out intensive research on the excitation and grab bar movement of the primate bionic robot in published periodicals and meeting articles, but the prior robot and the control method thereof have the following problems: the motion state of the under-actuated joint is fed back by using a single vision or inertia measuring device, and the problem that a single sensor cannot adapt to all complex working conditions in an actual environment is not considered; the influence of motion uncertainty of an under-actuated joint on the reliability of the grab bar is not considered, and multiple times of excitation is needed when the grab bar fails in the experimental process; the influence of the damping of the underactuated joint is not considered during vibration excitation, so that the vibration excitation speed is slower or cannot reach a larger swing range.

In summary, the problems and deficiencies in the prior patents and academic papers can be summarized as follows: the existing feedback measurement method for the motion state of the paw of the truss climbing robot is not complete, and most paw designs cannot meet the requirements for reliably gripping truss rods with different sizes and cross-sectional shapes; the existing truss climbing robot and the control method thereof lack effective experimental data on the aspect of the motion reliability of the swinging grab bar, so the effectiveness of the designed robot and the given control method cannot be completely determined; the existing excitation control method of the under-actuated truss climbing robot is greatly influenced by the damping of an under-actuated joint, the swing amplitude rises slowly under the condition of bad working conditions (the damping is large during free rotation), and the problem of low final swing amplitude possibly occurs.

Disclosure of Invention

The invention provides a truss climbing robot capable of swinging and grabbing remote truss rods and a control method thereof, aiming at solving the problems that the existing truss climbing robot cannot reliably grab the remote truss rods and cannot continuously move in a truss structure.

The invention aims to provide an opening and closing paw with larger friction damping and more accurate self-motion state feedback with truss rods on the basis of a three-degree-of-freedom large-damping underslung truss climbing robot and a control method thereof (publication No. CN103332233A, patent No. ZL201310288965.0), provide an excitation control method which enables the robot to overcome the friction of an underactuated joint and swing to reach the swing amplitude required by grasping a long-distance (more than 50% of the extension length of a robot mechanism) target truss rod, and provide a staged large-damping underslung control method for a swing grasping rod which can reliably grasp the long-distance target truss rod, so as to solve the problem that the existing climbing truss robot and the swing grasping rod motion control method thereof cannot realize the grasping of the long-distance circular cross-section truss rod or have low grasping success rate.

The technical scheme adopted by the invention for solving the technical problems is as follows:

a truss climbing robot capable of swinging and grabbing a remote truss rod comprises a first claw, a first wrist joint, a first connecting rod, an elbow joint, a second connecting rod, a second wrist joint and a second claw; the first claw is rotatably connected with one end of a first connecting rod through a first wrist joint, the other end of the first connecting rod is rotatably connected with one end of a second connecting rod through an elbow joint, and the second claw is rotatably connected with the other end of the second connecting rod through a second wrist joint; the first paw and the second paw have the same structure; the first gripper consists of a fixed half gripper, a friction wheel reverse feedback mechanism, a moving gripper slide rail, a ball screw, a gripper rack, a connecting flange, a servo motor encoder, a direct current servo motor, a servo motor reducer, a driving gear, a driven gear, a moving gripper slide block, a gyroscope, a moving half gripper and a nut; the servo motor encoder, the direct current servo motor and the servo motor reducer are sequentially installed together, a driving gear is fixed on an output shaft of the servo motor reducer, a driven gear is meshed with the driving gear and fixed at the shaft end of a ball screw, the ball screw is positioned on a gripper frame through a bearing, a nut sleeved on the ball screw is fixedly connected with a movable half claw through a movable claw sliding block, gyroscopes and friction wheel reverse feedback mechanisms are correspondingly installed on the outer side surfaces of the movable half claw and a fixed half claw one by one, friction wheels of the friction wheel reverse feedback mechanisms are tightly pressed on a truss rod through springs on two sides, the fixed half claw, a movable claw sliding rail and the servo motor reducer are all fixed on the gripper frame, a connecting flange is further arranged on the gripper frame, and the gripper can be connected with a tail end flange of a first wrist joint or a tail end flange of a second wrist joint through the connecting flange.

Furthermore, the first claw further comprises a fixed claw bush and a movable claw bush, and the movable claw bush and the fixed claw bush are correspondingly arranged in the rectangular grooves on the inner sides of the movable half claw and the fixed half claw one by one.

Furthermore, the first claw also comprises a friction material layer, and the friction material layer is pasted in the semicircular grooves on the inner sides of the movable claw lining and the fixed claw lining.

When the truss rods to be grasped are truss rods with circular sections, the designed opening and closing claw can be respectively provided with a fixed claw bushing and a movable claw bushing in the fixed half claw and the movable half claw, and different friction materials can be selected according to the materials and the surface roughness of the truss rods; for truss rods with 90-degree edges and cross-sectional shapes such as angle steel, square steel, channel steel, I-shaped steel and the like, the fixed claw bushing and the movable claw bushing can be removed, and rectangular grooves on the inner sides of the fixed half claw and the movable half claw are used for grasping; the rectangular grooves on the inner sides of the fixed half claw and the movable half claw and the semicircular grooves on the inner sides of the fixed claw bushing and the movable claw bushing are designed in series, so that truss rods with different sizes can be gripped.

Furthermore, the truss climbing robot further comprises a control system consisting of an upper computer, a first claw, a first wrist joint, an elbow joint, a second wrist joint and a servo control/driver of the second claw, wherein five servo control/drivers are in one-to-one correspondence with the direct current servo motor of the first claw, the direct current servo motor of the first wrist joint, the direct current servo motor of the elbow joint, the direct current servo motor of the second wrist joint and the direct current servo motor of the second claw; the upper computer is communicated with a main node servo control/driver which drives and controls a direct current servo motor in the first paw through a USB interface, and the main node servo control/driver is communicated with the other four servo control/drivers through a CAN bus; the friction wheel reverse feedback mechanism and the gyroscope are communicated with an upper computer through a serial port, the upper computer sends a control instruction of a target position containing the active joint to a main node servo control/driver through a USB interface, and the main node servo control/driver sends the received control instruction to a corresponding servo control/driver through a CAN bus.

In the control system, an upper computer CAN add a CAN-PCI board card on a PCI slot, and directly communicates with all servo control/drivers through the board card; under the condition that the upper computer uses the CAN-PCI board card, the upper computer sequentially sends motion instructions according to the node numbers of the servo control/drivers, and directly exchanges data with each servo control/driver; and after receiving the control instruction, all servo control/drivers synchronize according to the synchronous signal in the CAN bus and control the position or the moment of the respective DC servo motor.

A control method of the truss climbing robot comprises the following implementation processes:

the generation process of the under-actuated joint motion state comprises the following steps: according to feedback data of a friction wheel reverse feedback mechanism and a gyroscope obtained by a serial port, an upper computer generates motion state feedback of an under-actuated joint

The steps are as follows:

step one, according to a fixed sampling period T_SData reading is carried out on the friction wheel reverse feedback mechanism and the gyroscope through a serial port of the upper computer, the obtained binary number is converted into a decimal number, the rotation angle of the photoelectric encoder obtained by the friction wheel reverse feedback mechanism is theta, and the rotation angular speed of the paw obtained by the gyroscope is omega;

step two, calculating the under-actuated joint rotation angle theta only by considering the feedback data theta of the friction wheel reverse rotation feedback mechanism_1FAnd angular velocity ω_1F(ii) a By means of I_FTheta is calculated from the rotation angle theta_1FAs shown in formula (1);

θ_1F＝θ/I_F (1)

at theta_1F ⁽ⁿ⁾Theta calculated for the nth sampling period_1FIn the nth sampling period, only considering the feedback data of the friction wheel reverse feedback mechanism to obtain the angular velocity omega of the under-actuated joint_1F ⁽ⁿ⁾Can be calculated according to the formula (2);

step three, calculating the under-actuated joint rotation angle theta only by considering the feedback data omega of the gyroscope_1GAnd angular velocity ω_1G(ii) a Since the gyroscope is mounted on the robot gripper forming the under-actuated joint, the gyroscope directly measures the rotational speed of the under-actuated joint, i.e. omega_1Gω; at omega_1G ⁽ⁿ⁾Denotes ω obtained at the nth sampling period_1GWhen only the feedback data of the gyroscope is considered, the under-actuated joint angle theta of the nth sampling period_1G ⁽ⁿ⁾Can be calculated according to the formula (3);

in the formula (3) < theta >_1G ⁽⁰⁾When the truss climbing robot is powered on, the corner of the under-actuated joint is in an initial state, the robot grips the truss rod by one hand, and the main actuated joint is straightened and naturally droops to be motionless, namely theta_1G ⁽⁰⁾＝0°；

Step four, comprehensively considering theta calculated by feedback data of the friction wheel reverse rotation feedback mechanism_1F、ω_1FAnd theta calculated from feedback data of the gyroscope_1G、ω_1GDetermining motion state feedback for under-actuated joints

Line number N of photoelectric encoder according to friction wheel back rotation feedback mechanism_FThe orthogonal pulse signal of the photoelectric encoder is subdivided by four times when the reading is considered, and the signal is reversed by the reversing of the friction wheelTheta calculated from feedback data of the feed mechanism_1FMaximum truncation error theta of_1ECan be calculated according to the formula (4);

from the equation (2), the truncation error θ_1EAt an angular velocity ω_1FError omega introduced in_1EIs composed of

Under the condition of low-speed rotation of the under-actuated joint, the angular velocity omega is calculated by using feedback data of the gyroscope_1GAngular velocity as an under-actuated joint

Angular velocity omega obtained by friction wheel back-rotating feedback mechanism when under-actuated joint rotates at high speed_1FAt angular velocity of under-actuated joints

At omega_1GAnd ω_1FThe threshold value selected between is determined to be 20 omega_1ETo ensure

The relative error of (A) is not more than +/-5%;

at high speed of rotation of the under-actuated joint, in omega_1GAnd ω_1FJudging whether the friction wheel of the friction wheel back-rotating feedback mechanism slips or not by the absolute value of the deviation, and when the absolute value of the deviation is in the range of omega_1G-ω_1F|>If epsilon is equal to theta, the friction wheel is considered to be slipping (epsilon is a threshold value of angular velocity deviation), and theta should be set₁＝θ_1GAnd updates θ of the current (nth) sampling period by equation (6)_1F ⁽ⁿ⁾To eliminate the effect of slippage;

when | ω_1G-ω_1FWhen | < epsilon, the friction wheel is considered not to slip, and theta is ensured at the moment₁＝θ_1G(ii) a Feedback of motion state of under-actuated joint by combining the above rules

It should be determined as in equation (7):

further, the implementation process of the method further comprises:

the control process of swinging the grab bar started by the natural suspension state is as follows:

step one, modeling an inverse kinematics of a truss climbing robot: defining an active joint angle vector of the robot as theta_S＝[θ₂,θ₃,θ₄]^T，x＝[x_A,y_A,θ_A]^TIs the pose vector of the rider, where θ₂、θ₃、θ₄A wrist joint corner, an elbow joint corner, a wrist joint corner, x, adjacent to the supporting hand, respectively_A、y_A、θ_AThe x-axis coordinate and the y-axis coordinate of the central point of the free hand and the attitude angle of the free hand are respectively, the first paw is taken as a supporting hand, and the second paw is taken as the free hand; the inverse kinematics equation of the robot is shown in equation (8):

in the formula (8) < i >₁The distance from the axis of the truss rod to the axis of the wrist joint adjacent to the supporting hand,/₂Distance from the axis of the wrist joint adjacent to the supporting hand to the axis of the elbow joint,/₃The distance from the elbow joint axis to the wrist joint axis adjacent to the free hand, l₄Is the distance from the wrist joint axis adjacent to the free hand to the center of the free hand, a₁Is the distance from the first wrist joint axis to the second wrist joint axis, a₂Distance from wrist axis adjacent to free hand to truss rod, a₃The distance from the axis of the wrist joint adjacent to the supporting hand to the center of the free hand; a is₁、a₂、a₃Calculated according to the formulas (9), (10) and (11) respectively,

considering the need to align the gripper openings with the target truss bar when grabbing the bar with the hands, a constraint condition is added as shown in formula (12), where x^d、y^dRespectively the x-axis position coordinate and the y-axis position coordinate of the target truss rod, and the constraint condition determines the attitude angle theta when the center of the rider is not coincident with the center of the target truss rod_A；

θ_A＝π/2+arctan[(y^d-y_A)/(x^d-x_A)] (12)

When the calculated center of the free hand coincides with the center of the target truss rod, the constraint condition shown in equation (12) is invalid, and then the wrist joint angle adjacent to the free hand is set to a fixed value, namely theta, in order to make the robot avoid the singular configuration₄＝θ₀(θ₀>0) At this time, the attitude angle θ of the rider's hand_ACalculated according to equation (13):

in the formula (13), a₄Calculating the distance from the central point of the free hand to the axis of the elbow joint according to the formula (14); combined type (8), (12) and (13) according to double-arm handRobot hand position coordinate x_A、y_AAnd under-actuated joint angle theta₁Determining a joint angle θ of an active joint₂、θ₃、θ₄；

Step two, self-starting stage control: firstly, a supporting hand of the truss climbing robot is held on a truss rod, other joints of the robot are straightened, and the whole robot keeps a suspension static state; then powering on a robot control system, after self-checking the communication states of the friction wheel backward rotation feedback mechanism, the gyroscope and each servo control/driver, enabling the elbow joint of the robot to move according to the track shown in the formula (15), and simultaneously keeping the first wrist joint and the second wrist joint still to support the target position x of the opening and closing of the hand₁ ^dSet to the release grip position x_sTarget position x for opening and closing of rider₂ ^dSet to the open position x₀；

Theta in the formula (15)₃ ^dIs the elbow joint target angle; a is the motion amplitude of the elbow joint of the robot; t is t₁The system time obtained by timing from the self-starting stage; t is₁Is the movement time of the self-starting phase; the moment of finishing the self-starting stage is selected from the zero crossing point of the phase of the swing motion of the under-actuated joint, and the judgment condition is shown as the formula (16):

t₁≥T₁&β＝0 (16)

in the formula (16), beta is a phase angle of the swing motion of the under-actuated joint, beta is defined as an included angle between a connecting line from any point on a swing phase curve of the under-actuated joint to a central point of the phase curve and a longitudinal axis of a phase space, and as the central point of the phase curve possibly deviates from a theoretical (0,0) point and drifts along with time, an included angle between a tangent line of the phase curve and a transverse axis of the phase space is adopted

The approximation is carried out on the obtained data,

according to angular acceleration of under-actuated joints

And angular velocity

Calculated according to equation (17):

step three, controlling the excitation stage: starting to control the vibration exciting stage after the self-starting stage is finished, keeping the first wrist joint, the second wrist joint, the first paw and the second paw of the robot still, and moving the elbow joint according to the target position given by the formula (18), namely, the elbow joint rotating angle theta₃Angle of rotation theta of under-actuated joint₁The phase difference of (a) is always kept at 90 degrees;

the judgment condition for the end of the excitation phase is as shown in equation (19), and is selected to be theta₁Maximum value of (max) ([ theta ])₁) To a theta₁ ^fWhen the swing phase angle β of the under-actuated joint is 180 °, the velocity of the elbow joint of the robot is zero, and the impact when the robot starts to perform subsequent motion can be reduced;

in the judgment condition (19), a is a safety factor larger than 1, and the function of the safety factor is to consider and compensate energy loss possibly generated in subsequent movement in the excitation stage in advance so that the robot starts to moveThe under-actuated joint angle can still reach the target angle theta required by the gripping movement when the target truss rod is gripped₁ ^f，θ₁ ^fThe angle of the under-actuated joint when the first wrist joint and the second wrist joint of the robot are both straightened and the second claw grips the target truss rod is calculated according to the formula (20):

step four, control of an adjusting stage: at the moment when the tuning phase begins to end, the robot is tuned from the configuration at the end of the excitation phase to the configuration suitable for gripping the target truss rod in the tuning phase, and the specific tuning motion trajectory is as shown in formula (21), that is: at T₂Angle of rotation theta of elbow joint in time₃Smooth transition from backward curved-A position to forward curved theta₃ ^fPosition while rotating the wrist joint adjacent to the free hand by an angle theta₄Transition from 0 to theta₄ ^f(ii) a In the adjusting stage, a first claw, a wrist joint adjacent to the supporting hand and a second claw of the robot are kept still;

theta in the formula (21)₃ ^f、θ₄ ^fTo be theta₁ ^fAnd target truss rod position coordinates (x)^d,y^d) Substituting the robot joint angle into an inverse kinematics equation (8); b₁Is the transition coefficient, calculated as equation (22):

t in formula (22)₂The system time is counted from the adjustment phase; in theory, the moment when the adjustment phase ends should be chosen at the moment when the under-actuated joint reaches the maximum pivot angle and the speed is 0, i.e. when β is 90 °, but with a view to supportingThe time is needed for the hand to move from the loose-grip position to the tight-grip position, and the moment when the adjustment stage is finished needs to be selected before the under-actuated joint reaches the highest swing angle so as to prevent the support hand from rotating too much in the process of gripping; the point in time at which the adjustment phase ends is therefore selected to be β 90 ° - β₀At a time of (b), wherein₀Is a constant greater than 0, thus at β<The adjustment phase is ended in advance at 90 DEG, and the target position x for opening and closing the supporting hand is modified at the end₁ ^dSet it to a grip position x_tSo that the supporting hand can be tightly held;

step five, controlling in a large damping stage: immediately after the adjustment phase, the large damping phase is started, and the target positions θ of the wrist joint adjacent to the supporting hand, the elbow joint, and the wrist joint adjacent to the free hand are set to the target positions θ₂ ^f、θ₃ ^f、θ₄ ^fIs represented by equation (23):

in the formula (23) < theta >₂ ^*、θ₃ ^*、θ₄ ^*Is theta to be determined by equation (7)₁And target truss rod position coordinates (x)^d,y^d) Substituting the robot joint angle calculated in the inverse kinematics equation (8); b₂To smooth the transition coefficients for robot motion, the following is calculated as equation (24):

t in formula (24)₃The system time is counted from the beginning of the large damping stage; t is₃A grip transition time; when t is₃>T₃When the user wants to open or close the game hand, the game hand is closed to open or close the target position x₂ ^dFrom an open position x₀Switching to (loosely holding) closed position x_sCompleting the grasping of the target truss rod; if the motion of the grab bar fails, the under-actuated joint recovers the free rotation and is controlled again from the excitation stage until the motion is successfulGrasping the target rod.

Further, the implementation process of the method further comprises: the truss climbing robot continues to perform a continuous moving control process after finishing primary target truss rod gripping:

step one, the transfer of joint variables: the initial state of the continuous movement is a state that the first claw and the second claw of the robot both grasp the truss rods, the supporting hand positioned at the rear side in the moving direction of the robot loosens the truss rods and becomes a free hand in the subsequent continuous movement, and the free hand at the front side becomes the supporting hand in the subsequent continuous movement and forms an under-actuated joint with the grasped truss rods; therefore, the joint variables need to be transcribed, namely the corner of the first wrist joint is exchanged with the corner of the second wrist joint, the opening and closing distance of the first paw is exchanged with the opening and closing distance of the second paw, and the corner of the elbow joint is not changed; after the variable is transcribed, the following variables are always: x is the number of₁Indicating the opening and closing distance, x, of the supporting hand₂Indicating the opening and closing distance, theta, of the rider's hand₁Indicates the angle of rotation of the under-actuated joint (measured by a gyroscope mounted on the supporting hand and a friction wheel back-off feedback mechanism), θ₂Representing the angle of rotation of the wrist joint, theta, adjacent to the supporting hand₃Representing the angle of rotation of the elbow joint, theta₄Representing the wrist joint rotation angle adjacent to the free hand;

step two, controlling the configuration adjusting stage: angle of rotation theta in this stage₂、θ₄The two corresponding wrist joints move according to the planned track, and the position of the elbow joint rotation angle is servo to a given value theta₃ ^dAccording to theta₂、θ₄Kinematically determining, and adjusting the configuration of the robot by using the track shown in the formula (25); the open and close positions of the supporting hand and the free hand are kept in a loose holding state, i.e. x₁ ^d＝x_sAnd x₂ ^d＝x_s；

In the formula (25), θ₂ ^h、θ₄ ^hAt initial states of configuration adjustment stages respectivelyθ₂、θ₄A value of (d); theta₂ ^pAnd theta₄ ^pRespectively representing theta in the target configuration at this stage₂、θ₄Value of (a), theta₂ ^p＝0，θ₄ ^pCalculated according to equation (26); b₃For the configuration adjustment of the transition coefficients used in the movement, the calculation is performed according to equation (27):

t in formula (28)₄System time timed from the start of the configuration adjustment phase; t is₄Adjusting the time of the movement for the configuration; the moment at which the adjustment phase ends is chosen at t₄＝T₄I.e. the moment at which the configuration adjustment movement is completed;

step three, rod loosening stage control: this phase begins immediately after the configuration adjustment phase is over, theta₂ ^d、θ₃ ^d、θ₄ ^d、x₁ ^dKeeping the position unchanged and releasing the rider to the open position (x)₂ ^d＝x₀) (ii) a The discrimination condition for the end of the rod releasing stage is shown in equation (28):

in the formula (28)

To identify the minimum speed of the under-actuated joint, t, at which the release of the rod is completed₅The system time is counted from the rod loosening stage; t is₅Is the minimum rod loosening time;

step four, controlling in a swing stage: the stage starts immediately after the pole loosening stage is finished, and the opening and closing of the supporting hand and the free hand are carried out in the swinging stageThe degree of freedom and the wrist joint adjacent to the supporting hand are kept still, and the elbow joint and the wrist joint adjacent to the free hand move from the position of rod loosening to the position ready for grasping within one swinging time of the under-actuated joint from back to front; the rotation angle of the wrist joint adjacent to the free hand at the end of the rod loosening stage is the terminal theta of the trajectory planning in the formula (25)₄ ^pAnd the rotation angle of the elbow joint is recorded as theta₃ ^p(ii) a The target rotation angles of the elbow joint and the wrist joint adjacent to the free hand in the swinging stage are the same as the target rotation angles in the adjusting stage and are respectively theta₃ ^f、θ₄ ^f(ii) a The motion trajectories of the elbow joint and the wrist joint adjacent to the free hand are shown as formula (29):

b in formula (29)₁Is the excessive coefficient of the swing motion, and is calculated according to the formula (30):

in the formula (30), t₅The system time is counted from the swing stage; t is₅The time of the swinging motion is determined according to the average time of swinging the under-actuated joint from back to front once in a plurality of experiments; the selection of the end time of the swing phase is the same as the selection of the end time of the adjustment phase, and the selection is carried out at the swing phase angle beta of the under-actuated joint of 90 DEG-beta₀The time of day; modifying the target opening and closing position x of the supporting hand at the end of the swinging stage₁ ^dSet it to a grip position x_t；

Step five, controlling in a large damping stage: the large damping stage during continuous movement is completely the same as the large damping stage for grabbing the rod starting from the natural suspension state, please refer to step five in the control process of swinging the grabbing rod starting from the natural suspension state; and after the grabbing rod is finished, if the next target truss rod needs to be grabbed forwards continuously, restarting from the first step, and repeating the steps to realize continuous movement in the truss structure.

The invention has the beneficial effects that:

the invention improves the paw design of the truss climbing robot, and designs and develops the truss climbing robot which can reliably grasp the long-distance truss rods (the distance between the adjacent truss rods is more than 50 percent of the arm span of the robot) and a control system thereof; the invention provides an under-actuated joint motion state generation method comprehensively considering feedback data of a friction wheel back-rotating feedback mechanism and a gyroscope, and provides a swinging grab bar method capable of reliably grasping a long-distance truss bar and a motion control method for continuously moving in a truss structure in a swinging mode by using a phase difference-based excitation vibration control method and a large-damping under-actuated grab bar control method.

Friction materials are arranged on the inner sides of the claws of the designed truss climbing robot, so that the robot can obtain enough damping when extending to a far target; because the paw is simultaneously provided with the friction wheel back-rotating feedback mechanism and the gyroscope, the influence of the friction wheel slip on the motion state detection of the under-actuated joint is eliminated, and the motion state feedback of the under-actuated joint is more accurate; the provided excitation vibration control method can exert all driving capability of the robot, is compared with the existing excitation vibration control method, and has the advantages of high excitation vibration speed, easy control of amplitude and small influence by under-actuated joint friction; under the condition that the robot is continuously retreated under the action of gravity, the proposed swinging grab bar control method started from a natural suspension state can still realize stable grabbing on the long-distance truss bar, and the success rate of the experiment can reach 100%; through simulation and experimental verification, the continuous movement control method can complete the continuous movement control task of the truss climbing robot, and the success rate of the continuous movement experiment of the half period is 100%, so that the method has practical value.

In summary, the truss climbing robot and the control system thereof and the swing grab bar control method thereof provided by the invention advance a great step in the aspects of theory and technology for the truss climbing robot to be practical.

The invention patent with publication number of CN103332233A and patent number of ZL201310288965.0 is patent of invention with publication number of 10 and 2 in 2013, and provides a three-degree-of-freedom truss climbing robot and a large-damping under-actuated control method thereof. The invention is different from the following: a gyroscope for measuring the rotation angular velocity and a friction material for increasing the friction force between the robot and a truss rod are added to the opening and closing paw of the robot; the control method of the invention considers the influence of under-actuated joint damping on the excitation stage in the free rotation state, and provides the excitation control method based on the oscillation phase difference; the invention not only provides a swinging grab bar control method for grabbing a target truss bar after starting excitation from a natural suspension state, but also provides a control method for continuously moving in a truss structure after finishing swinging the grab bar once; the invention completes the swinging grab bar motion experiment of the truss climbing robot under the experimental condition, completes the multi-period continuous movement simulation under the simulation condition, and is easy to carry out technical conversion to form a product.

The friction material is added on the inner side of the paw of the robot to provide larger damping torque; the gyroscope sensor is added on the paw, so that the influence of the slipping of the friction wheel during high-speed swinging can be eliminated; and a method for generating feedback of the motion state of the under-actuated joint by comprehensively considering feedback data of the friction wheel reverse rotation feedback mechanism and the gyroscope is provided. A phase difference-based excitation vibration control method is provided, a swing grab bar control method started from a natural suspension state and a continuous movement control method are provided, a swing grab bar experiment started from the natural suspension state and continuous movement simulation and experiments are performed, and the effectiveness of the provided method is verified. The truss climbing robot is applied to the field of truss climbing robots.

Drawings

Fig. 1 is a photograph of an object (three-degree-of-freedom climbing truss robot with patent number ZL201310288965.0) improved and controlled by the present invention; FIG. 2 is a simplified view of the improved gripper mechanism of the present invention; fig. 3 is a schematic view of the open-close claws of the truss climbing robot designed in the invention gripping truss rods with different cross-sectional shapes; FIG. 4 is a diagram of the definition of the mechanism parameters and the physical parameters of the controlled object (climbing truss robot) according to the present invention; FIG. 5 is a hardware block diagram of a control system of the truss climbing robot designed by the invention; FIG. 6 is a schematic diagram of the present invention illustrating the phase division and the motion of each phase of the oscillating grab bar initiated by the natural suspension state; FIG. 7 is a schematic diagram of the staging and movement of each stage of the invention as the robot continues to move continuously between the truss arms after having gripped the target truss arms; FIG. 8 is a schematic phase space diagram of under-actuated joint oscillation; FIG. 9 is a functional block diagram of a swing grab bar controller initiated by a natural hang condition according to the present invention; FIG. 10 is a functional block diagram of a continuous motion controller designed in accordance with the present invention; FIG. 11 is a graph of the amplitude of the under-actuated joint for two control methods (energy pumping and phase difference based control proposed by the present invention) obtained from the excitation control simulation; fig. 12 is a photograph of an experimental scene when the improved truss climbing robot of the present invention performs a swinging grab bar experiment; fig. 13 is a graph showing a robot joint angle curve when a target truss rod at a distance of 0.4m and 1.0m is grasped, wherein (a) is a joint angle curve when the target rod distance is 400mm, and (b) is a joint angle curve when the target rod distance is 1000 mm; FIG. 14 is a screenshot of an experimental video when grasping a target truss rod at distances of 0.4m and 1.0 m; FIG. 15 is a simulated video screenshot of the continuous movement of one cycle completed after initiating the complete swing grab bar from a natural hang position; fig. 16 is a half-period continuous moving experimental video screenshot in which target truss rods at distances of 0.5m and 0.8m are respectively grasped (truss rods at a distance of 1.0m have been grasped in an initial state); fig. 17 is a graph of the joint angles of the robot for the two sets of experiments in fig. 16.

Detailed Description

The first embodiment is as follows: as shown in fig. 1 to 4, the improved truss climbing robot in the present embodiment includes a first claw 1, a first wrist joint 2, a first connecting rod 3, an elbow joint 4, a second connecting rod 5, a second wrist joint 6, and a second claw 7, where the first claw 1 is rotatably connected to one end of the first connecting rod 3 through the first wrist joint 2, the other end of the first connecting rod 3 is rotatably connected to one end of the second connecting rod 5 through the elbow joint 4, and the second claw 7 is rotatably connected to the other end of the second connecting rod 5 through the second wrist joint 6; the first paw 1 and the second paw 7 are the same component, the first wrist joint 2 and the second wrist joint 6 are the same component, and the first connecting rod 3 and the second connecting rod 5 are the same part. The first wrist joint 2, the elbow joint 4 and the second wrist joint 6 are all composed of a direct-current servo motor, a synchronous toothed belt transmission and a harmonic gear speed reducer, and the direct-current servo motor drives the joints to rotate through the synchronous toothed belt transmission and the harmonic gear transmission.

The first gripper 1 or the second gripper 7 is composed of a fixed half gripper 1-1, a fixed gripper bushing 1-2, a friction wheel reverse feedback mechanism 1-3, a moving gripper slide rail 1-4, a ball screw 1-5, a gripper frame 1-6, a connecting flange 1-7, a servo motor encoder 1-8, a direct current servo motor 1-9, a servo motor reducer 1-10, a driving gear 1-11, a driven gear 1-12, a moving gripper slide block 1-13, a gyroscope 1-14, a moving half gripper 1-15, a moving gripper bushing 1-16, a friction material 1-17 and a nut 1-19. A servo motor encoder 1-8, a direct current servo motor 1-9 and a servo motor reducer 1-10 are sequentially installed together, a driving gear 1-11 is fixed on an output shaft of the servo motor reducer 1-10, a driven gear 1-12 is meshed with the driving gear 1-11 and fixed at the shaft end of a ball screw 1-5, the ball screw 1-5 is positioned on a paw rack 1-6 by a bearing, a nut 1-19 sleeved on the ball screw 1-5 is fixedly connected with a movable paw slider 1-13 and a movable half paw 1-15, a movable paw bush 1-16 and a fixed paw bush 1-2 are respectively installed in a rectangular groove at the inner side of the movable half paw 1-15 and the fixed half paw 1-1, a gyroscope 1-14 and a friction wheel reverse feedback mechanism 1-3 are respectively installed on the outer side surfaces of the movable half paw 1-15 and the fixed half paw 1-1, the friction wheel 1-3-2 of the friction wheel reverse feedback mechanism 1-3 is tightly pressed on the truss rod 1-18 through the springs 1-3-1 on the two sides, friction materials 1-17 are pasted in semicircular grooves on the inner sides of the movable claw bush 1-16 and the fixed claw bush 1-2, the fixed half claw 1-1, the movable claw slide rail 1-4 and the servo motor speed reducer 1-10 are all fixed on the claw rack 1-6, the claw rack 1-6 is further provided with connecting flanges 1-7, and the claw can be connected with a tail end flange of the first wrist joint 2 or the second wrist joint 6 through the connecting flanges 1-7. The DC servo motor 1-9 of the paw drives the movable half-paw 1-15 to carry out the closing movement towards the fixed half-paw 1-1 and the opening movement away from the fixed half-paw 1-1 through the driving gear 1-11 → the driven gear 1-12 → the ball screw 1-5 → the nut 1-19 → the movable paw slide block 1-13.

The second embodiment is as follows: as shown in fig. 2 and 3, when the truss rods to be gripped are truss rods 1-18 with circular cross sections, the open-close claws of the robot can be respectively provided with fixed claw bushings 1-2 and movable claw bushings 1-16 in the fixed half claws 1-1 and the movable half claws 1-15, and different friction materials 1-17 can be selected according to the materials and the surface roughness of the truss rods 1-18, for example, a rubber material with a larger friction coefficient can be used for gripping a hard smooth surface, a silicon material with a better wear resistance can be used for gripping a hard rough surface, and a asbestos layer with a better tabling property can be used for gripping a soft uneven surface such as a cable. For truss rods with 90-degree edges and angles in cross sections, such as angle steels 1-20, square steels 1-21, channel steels 1-22 and I-shaped steels 1-23, fixed claw bushings 1-2 and movable claw bushings 1-16 can be removed, and rectangular grooves on the inner sides of fixed half claws 1-1 and movable half claws 1-15 are used for grasping. The rectangular grooves on the inner sides of the fixed half claw 1-1, the movable half claw 1-15, the fixed claw bush 1-2 and the movable claw bush 1-16 are designed in series, so that truss rods with different sizes can be grasped.

The third concrete implementation mode: as shown in fig. 5, the control system of the truss climbing robot comprises an upper computer and servo motor controls/drivers of a first gripper 1, a first wrist joint 2, an elbow joint 4, a second wrist joint 6 and a second gripper 7, wherein the upper computer is communicated with a master node servo control/driver for driving and controlling direct current servo motors 1-9 in the first gripper 1 through a USB interface, the master node servo control/driver is communicated with other servo control/drivers through a CAN bus, and each servo control/driver corresponds to one direct current servo motor. The upper computer CAN also be additionally provided with a CAN-PCI board card on the PCI slot, and the CAN-PCI board card is directly communicated with all servo control/drivers. The rotation angle theta of a photoelectric encoder shaft in the robot claw is fed back by a friction wheel reverse feedback mechanism 1-3 on the robot claw, the rotation angular speed omega of the gyroscope 1-14 is fed back by the gyroscope 1-14, the friction wheel reverse feedback mechanism 1-3 and the gyroscope 1-14 are communicated with an upper computer through a serial port, and the upper computer generates the motion state feedback of an under-actuated joint according to the theta and the omega

And according to

And other mastersAnd (4) calculating the target position of the active joint by the rotation angle feedback of the movable joint. The host computer sends a control instruction of a target position containing the active joint to the master node servo control/driver through the USB interface, and the master node servo control/driver sends the received control instruction to the corresponding servo control/driver through the CAN bus; under the condition that the upper computer uses the CAN-PCI board card, the upper computer sequentially sends motion instructions according to the node numbers of the servo control/drivers and directly exchanges data with each servo control/driver, the communication time required under the condition is shorter than that required by a USB interface, and online real-time control with the same number of axes in a shorter control period or a same number of axes in a control period CAN be realized. And after receiving the control instruction, all servo control/drivers synchronize according to the synchronous signal in the CAN bus and control the position or the moment of the respective DC servo motor.

The fourth concrete implementation mode: according to feedback data of the friction wheel reverse feedback mechanism 1-3 and the gyroscope 1-14 obtained by the serial port, the upper computer generates the motion state feedback of the under-actuated joint

The steps are as follows:

step one, according to a fixed sampling period T_SAnd data reading is carried out on the friction wheel reverse feedback mechanism 1-3 and the gyroscope 1-14 through a serial port of an upper computer, an obtained binary number is converted into a decimal number, the rotation angle of the photoelectric encoder obtained by the friction wheel reverse feedback mechanism 1-3 is theta, and the rotation angular speed of the paw obtained by the gyroscope 1-14 is omega.

Step two, calculating the under-actuated joint rotation angle theta only by considering the feedback data theta of the friction wheel reverse rotation feedback mechanism 1-3_1FAnd angular velocity ω_1F. By means of I_FThe reduction ratio of the feedback mechanism 1-3 representing the reverse rotation of the friction wheel is calculated according to the rotation angle theta_1FAs shown in formula (1).

θ_1F＝θ/I_F (1)

At theta_1F ⁽ⁿ⁾Theta calculated for the nth sampling period_1FIn the nth sampling period, only the reverse rotation of the friction wheel is consideredUnder-actuated joint angular velocity omega derived from feedback data of feed mechanism 1-3_1F ⁽ⁿ⁾Can be calculated according to equation (2).

Step three, calculating the under-actuated joint rotation angle theta only by considering the feedback data omega of the gyroscopes 1 to 14_1GAnd angular velocity ω_1G. Since the gyroscopes are mounted on robot paws forming under-actuated joints, the gyroscopes 1-14 directly measure the rotation speed of the under-actuated joints, i.e. omega_1Gω. At omega_1G ⁽ⁿ⁾Denotes ω obtained at the nth sampling period_1GConsidering only the feedback data of the gyroscopes 1 to 14, the under-actuated joint angle theta of the nth sampling period_1G ⁽ⁿ⁾Can be calculated according to equation (3).

In the formula (3) < theta >_1G ⁽⁰⁾The truss climbing robot is a corner of an underactuated joint in an initial state when the truss climbing robot is powered on, the initial state is a state that the robot grips a truss rod by one hand and a main drive joint is straightened and naturally droops and does not stand, namely theta_1G ⁽⁰⁾＝0°。

Step four, comprehensively considering theta calculated by feedback data of the friction wheel reverse rotation feedback mechanisms 1-3_1F、ω_1FAnd theta calculated from feedback data of gyroscopes 1 to 14_1G、ω_1GDetermining motion state feedback for under-actuated joints

The number N of lines of the photoelectric encoder according to the return feedback mechanism 1-3 of the friction wheel_FWhen the quadrature pulse signal of the photoelectric encoder is subdivided by four times in consideration of the reading, theta is calculated from the feedback data of the friction wheel reverse feedback mechanism 1-3_1FMaximum truncation error theta of_1ECan be counted according to the formula (4)And (4) calculating.

From the equation (2), the truncation error θ_1EAt an angular velocity ω_1FError omega introduced in_1EIs composed of

Due to the sampling period T of the sensor_SVery short (typically several milliseconds to ten-odd milliseconds), and therefore the angular velocity ω is calculated according to equation (5)_1FTruncation error ω in (d)_1ENot negligible, the angular velocity ω should be calculated using feedback data from gyroscopes 1-14 under conditions of low-speed rotation of the under-actuated joint_1GAngular velocity as an under-actuated joint

However, the mechanical body of the friction wheel reverse feedback mechanism 1-3 has a certain filtering function, and can filter out high-frequency disturbance signals which can be fed back and transmitted through the gyroscope 1-14 due to the vibration of the truss, so that the angular velocity omega obtained by the friction wheel reverse feedback mechanism 1-3 is used when the under-actuated joint rotates at a high speed_1FAt angular velocity of under-actuated joints

At omega_1GAnd ω_1FThe threshold value selected between is determined to be 20 omega_1EThus can ensure

The relative error of (A) does not exceed +/-5%.

In addition, at high rotation speeds of the under-actuated joint, the angle is ω_1GAnd ω_1FThe absolute value of the deviation between the two friction wheels judges whether the friction wheels of the friction wheel reverse feedback mechanism 1-3 slip or not, and when the absolute value of the deviation is | omega |, the absolute value of the deviation is_1G-ω_1F|>E is considered to slip (e is the threshold value of the angular velocity deviation),should be such that theta₁＝θ_1GAnd updates θ of the current (nth) sampling period by equation (6)_1F ⁽ⁿ⁾To eliminate the effect of slippage.

When | ω_1G-ω_1FWhen | < epsilon, the friction wheel is considered not to slip, and theta is ensured at the moment₁＝θ_1G. Feedback of motion state of under-actuated joint by combining the above rules

It should be determined as in equation (7).

The fifth concrete implementation mode: as shown in fig. 6 and 9, the control method of the swinging grab bar of the truss climbing robot, which is started from a natural suspension state, comprises the following steps:

step one, modeling an inverse kinematics of the truss climbing robot. Defining an active joint angle vector of the robot as theta_S＝[θ₂,θ₃,θ₄]^T，x＝[x_A,y_A,θ_A]^TIs the pose vector of the rider, where θ₂、θ₃、θ₄A wrist joint corner, an elbow joint corner, a wrist joint corner, x, adjacent to the supporting hand, respectively_A、y_A、θ_AThe x-axis coordinate and the y-axis coordinate of the center point of the free hand and the attitude angle of the free hand are respectively, and in the embodiment, the first claw 1 is used as a supporting hand and the second claw 7 is used as the free hand. The inverse kinematics equation of the robot is shown in formula (8).

In the formula (8) < i >₁For the axis of the truss rod (1-18) to the supporting handDistance of adjacent wrist joint axes,/₂Is the distance from the axis of the wrist joint adjacent to the supporting hand to the axis of the elbow joint (4) |₃The distance between the elbow joint (4) axis and the wrist joint axis adjacent to the free hand, l₄Is the distance from the wrist joint axis adjacent to the free hand to the center of the free hand, a₁Is the distance from the axis of the first wrist joint 2 to the axis of the second wrist joint 6, a₂Distance from wrist axis adjacent to free hand to truss arms 1-18, a₃Is the distance from the wrist joint axis adjacent the supporting hand to the free hand center. a is₁、a₂、a₃Calculated according to equations (9), (10) and (11), respectively.

Considering the need to align the gripper openings with the target truss bar when grabbing the bar with the hands, a constraint condition is added as shown in formula (12), where x^d、y^dRespectively the x-axis position coordinate and the y-axis position coordinate of the target truss rod, and the constraint condition determines the attitude angle theta when the centre of the rider is not coincident with the centre of the target truss rod_A。

θ_A＝π/2+arctan[(y^d-y_A)/(x^d-x_A)] (12)

When the calculated free hand center coincides with the target truss rod center, the constraint condition shown in equation (12) is invalid, and then the wrist joint angle adjacent to the free hand is set to a fixed value, namely theta, in order to make the robot avoid the singular configuration₄＝θ₀(θ₀>0) At this time, the hand attitude angle θ_ACalculated according to equation (13).

In the formula (13), a₄The distance from the center point of the free hand to the axis of the elbow joint 4 is calculated according to the formula (14). The comprehensive formulas (8), (12) and (13) can be used for determining the position coordinates x of the free hand of the double-arm robot_A、y_AAnd under-actuated joint angle theta₁Determining a joint angle θ of an active joint₂、θ₃、θ₄。

Step two, self-starting stage control. Firstly, the supporting hands of the truss climbing robot are held on the truss rods 1-18, other joints of the robot are straightened, and the whole robot is kept in a suspended static state. Then powering on a robot control system, carrying out self-checking on communication states of the friction wheel reverse feedback mechanisms 1-3, the gyroscopes 1-14 and each servo control/driver, enabling the elbow joint 4 of the robot to move according to a track shown in a formula (15), keeping the first wrist joint 2 and the second wrist joint 6 still, and supporting a target position x of hand opening and closing₁ ^dSet to the release grip position x_sTarget position x for opening and closing of rider₂ ^dSet to the open position x₀。

Theta in the formula (15)₃ ^dIs the elbow joint target angle; a is the motion amplitude of the elbow joint of the robot; t is t₁The system time obtained by timing from the self-starting stage; t is₁Is the movement time of the self-starting phase. The moment of finishing the self-starting stage is selected from the zero crossing point of the phase of the swing motion of the under-actuated joint, and the judgment condition is shown as the formula (16).

t₁≥T₁&β＝0 (16)

Beta in the formula (16) is under-actuated joint swinging motionAs shown in fig. 8, β is defined as an angle between a connecting line from any point on the swing phase curve of the under-actuated joint to the center point of the phase curve and the longitudinal axis of the phase space, and since the center point of the phase curve may deviate from the theoretical (0,0) point and drift with time, the present invention uses the angle between the tangent line of the phase curve and the horizontal axis of the phase space

The approximation is carried out on the obtained data,

according to angular acceleration of under-actuated joints

And angular velocity

Calculated as equation (17).

Step three, controlling the vibration stage. Starting to control the vibration exciting stage after the starting stage is finished, keeping the first wrist joint 2, the second wrist joint 6, the first paw 1 and the second paw 7 of the robot still, and moving the elbow joint 4 according to the target position given by the formula (18), namely, rotating the elbow joint 4 at the angle theta₃Angle of rotation theta of under-actuated joint₁The phase difference of (a) is always maintained at 90 °.

The judgment condition for the end of the excitation phase is as shown in equation (19), and is selected to be theta₁Maximum value of (max) ([ theta ])₁) To a theta₁ ^fWhen the swing phase angle β of the under-actuated joint is 180 °, the velocity of the robot elbow joint 4 becomes zero, and the impact at the start of the follow-up motion can be reduced.

The safety factor a in the judgment condition (19) is more than 1, and the function of the safety factor a is to consider and compensate energy loss possibly generated in subsequent movement in the excitation stage in advance, so that the underactuated joint angle can still reach the target angle theta required by the gripping movement when the robot starts to grip the target truss rod₁ ^f，θ₁ ^fThe angle of the under-actuated joint when the first wrist joint 2 and the second wrist joint 7 of the robot are both straightened and the second claw 7 grips the target truss rod is calculated according to the formula (20).

And step four, adjusting stage control. At the moment when the tuning phase begins to end, the robot is tuned from the configuration at the end of the excitation phase to the configuration suitable for gripping the target truss rod in the tuning phase, and the specific tuning motion trajectory is as shown in formula (21), that is: at T₂Angle of rotation theta of elbow joint 4₃Smooth transition from backward curved-A position to forward curved theta₃ ^fPosition while rotating the wrist joint adjacent to the free hand by an angle theta₄Transition from 0 to theta₄ ^f. During the adjustment phase, the first gripper 1, the wrist joint adjacent to the supporting hand, and the second gripper 7 of the robot remain stationary.

Theta in the formula (21)₃ ^f、θ₄ ^fTo be theta₁ ^fAnd target truss rod position coordinates (x)^d,y^d) Substituting the robot joint angle into an inverse kinematics equation (8); b₁The transition coefficient is calculated by equation (22).

T in formula (22)₂Is the system time counted from the start of the tuning phase. The time when the adjusting phase is finished is selected to be 90-beta of swing phase angle beta of the under-actuated joint₀At a time of (b), wherein₀Is a constant greater than 0. The time at which the under-actuated joint reaches the maximum pivot angle and the velocity is 0 (theoretically, the best time at which the supporting hand grips the truss arms 1 to 18) corresponds to 90 °, but the supporting hand is considered to be in the loosely gripped position x_sMove to the grip position x_tThe invention provides a method for adjusting the under-actuated joint, which requires time, namely the under-actuated joint cannot be switched from a loose small damping state to a large damping state immediately under the actual working condition, and the damping has a rising process, so that the invention provides that the adjusting stage is ended before the phase angle of the under-actuated joint reaches 90 degrees, and the target position x for opening and closing the supporting hand is modified when the phase angle reaches to the end₁ ^dSet it to a grip position x_t。

And step five, controlling in a large damping stage. Immediately after the adjustment stage, the large damping stage is started, and the target positions θ of the wrist joint adjacent to the supporting hand, the elbow joint 4, and the wrist joint adjacent to the free hand are set to the target positions θ₂ ^f、θ₃ ^f、θ₄ ^fThe calculation formula of (2) is shown in formula (23).

In the formula (23) < theta >₂ ^*、θ₃ ^*、θ₄ ^*Is theta to be determined by equation (7)₁And target truss rod position coordinates (x)^d,y^d) Substituting the robot joint angle calculated in the inverse kinematics equation (8); b₂The transition coefficients for smoothing the robot motion are calculated as equation (24).

T in formula (24)₃Is from large to largeStarting the system time of timing in the damping stage; t is₃To grip the transition time. When t is₃>T₃When the user wants to open or close the game hand, the game hand is closed to open or close the target position x₂ ^dFrom an open position x₀Switching to (loosely holding) closed position x_sAnd completing the grasping of the target truss rod. And if the motion of the grabbing rod fails, the under-actuated joint recovers free rotation and is controlled again from the excitation stage until the target rod is successfully grabbed.

Controlling according to the control method of the swinging grab bar of the truss climbing robot, wherein the schematic diagram of the motion process of the robot is shown as 6, and the supporting hands of the robot loose and hold truss bars 1-18 in an initial state and keep a naturally drooping static state; in the self-starting stage, the robot starts to deviate from the natural drooping initial state by the motion of the elbow joint 4, and is switched to the vibration exciting stage when the swing phase angle of the under-actuated joint reaches 0 degree; energy is input into the system through the motion of the elbow joint 4 in the excitation stage, and the swing amplitude of the under-actuated joint is switched to the adjustment stage after meeting the requirement; in the adjusting stage, the robot is quickly transited from the configuration when the vibration exciting motion is finished to the configuration of a ready-to-grasp target truss rod, and the support hand is closed; and after entering a large damping stage, the robot calculates the target positions of three main driving joints of the robot according to the position of the target truss rod, and when the free hand reaches the position of the target truss rod, the free hand is closed to finish the gripping.

The sixth specific implementation mode: as shown in fig. 7 and 10, the control method for the truss climbing robot to continue to move continuously after completing one grabbing of the target truss rod includes the following steps:

step one, the transfer of joint variables. The initial state of the continuous movement is a state that the first gripper 1 and the second gripper 7 of the robot both grip the truss rods, the supporting hand located at the rear side in the moving direction of the robot will release the truss rods and become a free hand in the subsequent continuous movement, and the free hand at the front side will become a supporting hand in the subsequent continuous movement and form an under-actuated joint with the gripped truss rods. Therefore, it is necessary to transfer joint variables, that is, the rotation angle of the first wrist joint 1 and the rotation angle of the second wrist joint 7 are exchanged, and the opening/closing distance of the first hand 1 and the opening/closing distance of the second hand are exchangedThe opening and closing distance of the second paw 7 is changed, and the rotation angle of the elbow joint 4 is unchanged. After the variable is transcribed, the following variables are always: x is the number of₁Indicating the opening and closing distance, x, of the supporting hand₂Indicating the opening and closing distance, theta, of the rider's hand₁Indicates the rotation angle of the under-actuated joint (measured by the gyroscope 1-14 and the friction wheel back-off feedback mechanism 1-3 mounted on the supporting hand), theta₂Representing the angle of rotation of the wrist joint, theta, adjacent to the supporting hand₃Representing the angle of rotation, theta, of the elbow joint 4₄Indicating the wrist joint rotation angle adjacent to the free hand.

And step two, controlling the configuration adjusting stage. The aim of this phase is to adjust the robot from the configuration at the end of the last grab bar to a configuration suitable for releasing the rider and swinging forward again, this target configuration should have the following characteristics: the wrist joint adjacent to the under-actuated joint is straightened to facilitate the next swing, and the swimmer is in a vertical posture (theta)_AThe robot can smoothly swing down after the paw is loosened by-180 degrees. The truss climbing robot forms a single closed loop 5-bar mechanism with two truss bars gripped at the beginning of the configuration adjustment phase, the mechanism has 2 degrees of freedom but the robot has 3 main driving joints, so the rotation angle theta in the configuration adjustment phase₂、θ₄The two corresponding wrist joints move according to the planned track, and the position of the elbow joint rotation angle is servo to a given value theta₃ ^dAccording to theta₂、θ₄The robot configuration is adjusted using the trajectory shown in equation (25) as determined kinematically. The open and close positions of the supporting hand and the free hand are kept in a loose holding state, i.e. x₁ ^d＝x_sAnd x₂ ^d＝x_s。

In the formula (25), θ₂ ^h、θ₄ ^hTheta at initial states of the configuration adjustment stages respectively₂、θ₄A value of (d); theta₂ ^pAnd theta₄ ^pRespectively representing theta in the target configuration at this stage₂、θ₄Value of (a), theta₂ ^p＝0，θ₄ ^pCalculated according to equation (26); b₃For the configuration adjustment of the transition coefficients used in the movement, the calculation is performed according to equation (27).

T in formula (28)₄System time timed from the start of the configuration adjustment phase; t is₄The time of the movement is adjusted for the configuration. The moment at which the adjustment phase ends is chosen at t₄＝T₄I.e. the moment at which the configuration adjustment movement is completed.

And step three, rod loosening stage control. This phase begins immediately after the configuration adjustment phase is over, theta₂ ^d、θ₃ ^d、θ₄ ^d、x₁ ^dKeeping the position unchanged and releasing the rider to the open position (x)₂ ^d＝x₀). The judgment of the rod loosening completion can be carried out by judging whether the speed of the under-actuated joint reaches a certain threshold value, and a delay condition is added in order to prevent the motion of the next stage from being started too early when the free hand is not completely separated from the truss rod, so the judgment condition of the rod loosening stage completion is shown as a formula (28).

In the formula (28)

To identify the minimum speed of the under-actuated joint, t, at which the release of the rod is completed₅The system time is counted from the rod loosening stage; t is₅Is the minimum rod loosening time.

And step four, controlling in a swing stage. The stage starts immediately after the pole loosening stage is finished, and the hands are supported and swim in the swinging stageThe opening and closing freedom and the wrist joint adjacent to the supporting hand are kept still, and the elbow joint 4 and the wrist joint adjacent to the free hand move from the position of rod loosening to the position ready for grasping within one swinging time of the under-actuated joint from back to front. The active joint of the robot is kept still in the rod releasing stage, so that the position of the active joint of the robot at the end of the rod releasing stage is also kept at the position at the end of the configuration adjusting stage, and the corner of the wrist joint adjacent to the free hand is the end point theta planned by the orbit in the formula (25)₄ ^pAnd the rotation angle of the elbow joint is recorded as theta₃ ^p. The target rotation angles of the elbow joint 4 and the wrist joint adjacent to the free hand in the swinging stage are the same as the target rotation angles in the adjustment stage in the fifth embodiment, and are respectively theta₃ ^f、θ₄ ^f. The motion trajectories of the elbow joint 4 and the wrist joint adjacent to the free hand are shown in formula (29).

B in formula (29)₁The coefficient of the swing motion is calculated by equation (30).

In the formula (30), t₅The system time is counted from the swing stage; t is₅The time of the swinging motion is determined according to the average time of the under-actuated joint swinging backwards and forwards once in a plurality of experiments. The selection of the oscillation phase ending time is the same as the selection of the adjustment phase ending time in the fifth embodiment, and the oscillation phase angles beta of the under-actuated joints are all selected to be 90-beta₀To let the supporting hand out of the release position x in advance_sMove to the grip position x_tIt takes time. Modifying the target opening and closing position x of the supporting hand at the end of the swinging stage₁ ^dSet it to a grip position x_t。

And step five, controlling in a large damping stage. The large damping phase during continuous movement is exactly the same as the large damping phase for starting the grab bar from a natural overhang, see step five of the fifth embodiment. And after the grabbing rod is finished, if the next target truss rod needs to be grabbed forwards continuously, restarting from the first step, and repeating the steps to realize continuous movement in the truss structure.

The seventh embodiment: the control method of the vibration stage in the grab bar control method is subjected to simulation test and compared with the vibration simulation result of directly applying the energy pumping method. The simulation environment is jointly established by Matlab/Simulink software and Adams software, wherein the Matlab/Simulink software is responsible for the control of the robot, and the Adams software is responsible for the entity modeling and the mechanical simulation of the robot. Excitation control simulations were performed for five cases where the damping coefficient c of the under-actuated joint was 0, 29, 58, 87, and 106(Nms/°), and the amplitude curves of the under-actuated joint obtained by applying the phase difference-based control method and the energy pumping method proposed by the present invention are shown in fig. 11.

In fig. 11, no matter how the damping coefficient c changes, the excitation control method proposed by the present invention can always obtain an under-actuated joint amplitude curve higher than that of the energy pumping method; all the amplitude curves obtained by the excitation control method provided by the invention show the characteristic of increasing speed and decreasing the speed; friction in under-actuated joints (c)>0) In the case of (2), the amplitude curve of the under-actuated joint obtained by the vibration excitation control method according to the present invention is decreased to a small extent with respect to the case of no friction (c is 0). The above phenomena are illustrated: at-90 deg. of simulation test<θ₁<Within the range of 90 degrees, the phase difference-based excitation control method provided by the invention has the advantages that the excitation speed is higher, the amplitude of the under-actuated joint is easier to control, and the influence caused by friction of the under-actuated joint can be better overcome.

The specific implementation mode is eight: as shown in fig. 12, the climbing truss robot and the swinging grab bar control method are applied to carry out grab bar experiments on target truss bars with different distances, the range of the target truss bar distance is 0.4m to 1m (28.5 percent to 69.4 percent of the extension length of the robot mechanism), and 5 times of repeated experiments are respectively carried out on one target bar distance (7 different target bar distances) selected at intervals of 0.1m in the range. Fig. 13 shows robot articulation curves for a set of gripping experiments for target truss rods at distances of 0.4m and 1m, respectively, and fig. 14 shows video screenshots corresponding to the two experiments in fig. 13. All 35 experiments can be successful within a complete swing grab bar period, namely the target truss bar can be grasped at a success rate of 100% without an additional excitation stage, and the climbing truss robot and the swing grab bar control method thereof can still achieve the success rate of 100% for three target truss bars with distances of 0.8m, 0.9m and 1.0m which exceed the extension length of the robot mechanism by 50%.

The specific implementation method nine: in order to verify the effectiveness of the proposed continuous movement motion control method, a continuous movement simulation environment with 4 truss rods is established in Adams software, a virtual prototype of the climbing truss robot naturally hangs below a first truss rod in an initial state, and the distance between every two truss rods is 0.6 m. As shown in fig. 15, according to the sequence of (a), (b) …, and (l), the climbing truss robot firstly uses the swing grab bar control method started from the natural suspension state provided in the present invention to make the free hand grasp the second truss bar, then loosens the first truss bar to grasp the third truss bar and loosens the second truss bar to grasp the fourth truss bar in sequence according to the proposed continuous movement control method, a complete continuous movement cycle is completed, and finally the robot loosens the third truss bar and returns to the natural suspension state. The whole movement process takes 90s, the grabbing rod started from the suspension state once and the grabbing rod continuously moved twice are completed, and the robot moves 1.8m in total. According to the simulation process, the following steps are carried out: the swinging grab bar method and the continuous moving method which are started from the suspension state can be smoothly combined together, and the moving task of the truss climbing robot in the truss structure can be completed.

As shown in fig. 16, experimental verification is performed on the control method for the truss climbing robot to perform continuous movement after both hands of the truss climbing robot have gripped the truss rods, and since the robot motions of each half of the continuous movement period after the exchange of the supporting hand and the free hand are the same, only half-period continuous movement experiments are performed in the invention. The initial state of the experiment is the state after the robot grips the truss rods with the distance of 1.0m and has performed configuration adjustment movement, so that the robot directly starts to move from the rod loosening stage in the experiment, the distances of the target truss rods to be gripped are respectively 0.5m, 0.6m, 0.7m and 0.8m, three times of experiments are repeated at each distance, a video screenshot of the continuous movement experiment of the grab rod for the target truss rods with the distances of 0.5m and 0.8m is given in fig. 16, and fig. 17 is a joint angle curve of the two experiments. All 12 continuous movement experiments only need one swinging and large damping switching to complete the grabbing rod, namely the power is 100%, and the designed truss climbing robot and the proposed continuous movement control method have the capability of completing operation tasks with high reliability in an actual working environment.

For the meaning of all parameters or variables in the present invention, see table one.

Table-parameter, variable definition table

Claims (8)

1. A truss climbing robot capable of swinging and grabbing a remote truss rod is characterized by comprising a first paw (1), a first wrist joint (2), a first connecting rod (3), an elbow joint (4), a second connecting rod (5), a second wrist joint (6) and a second paw (7); the first paw (1) is rotatably connected with one end of a first connecting rod (3) through a first wrist joint (2), the other end of the first connecting rod (3) is rotatably connected with one end of a second connecting rod (5) through an elbow joint (4), and the second paw (7) is rotatably connected with the other end of the second connecting rod (5) through a second wrist joint (6); the first paw (1) and the second paw (7) have the same structure;

the first gripper (1) consists of a fixed half gripper (1-1), a friction wheel reverse feedback mechanism (1-3), a moving gripper slide rail (1-4), a ball screw (1-5), a gripper rack (1-6), a connecting flange (1-7), a servo motor encoder (1-8), a direct current servo motor (1-9), a servo motor reducer (1-10), a driving gear (1-11), a driven gear (1-12), a moving gripper slide block (1-13), a gyroscope (1-14), a moving half gripper (1-15) and a nut (1-19);

a servo motor encoder (1-8), a direct current servo motor (1-9) and a servo motor reducer (1-10) are sequentially installed together, a driving gear (1-11) is fixed on an output shaft of the servo motor reducer (1-10), a driven gear (1-12) is meshed with the driving gear (1-11) and fixed at the shaft end of a ball screw (1-5), the ball screw (1-5) is positioned on a gripper frame (1-6) through a bearing, a nut (1-19) sleeved on the ball screw (1-5) is fixedly connected with a movable half gripper (1-15) through a movable gripper sliding block (1-13), gyroscopes (1-14) and a friction wheel reverse feedback mechanism (1-3) are correspondingly installed on the outer side surfaces of the movable half gripper (1-15) and the fixed half gripper (1-1), the friction wheel (1-3-2) of the friction wheel reverse feedback mechanism (1-3) is tightly pressed on the truss rod (1-18) through springs (1-3-1) on two sides, the fixed half claw (1-1), the movable claw slide rail (1-4) and the servo motor speed reducer (1-10) are all fixed on the claw rack (1-6), a connecting flange (1-7) is further arranged on the claw rack (1-6), and the claw can be connected with the tail end flange of the first wrist joint (2) or the second wrist joint (6) through the connecting flange (1-7).

2. The climbing truss robot of claim 1, wherein: the first paw (1) further comprises a fixed paw lining (1-2) and a movable paw lining (1-16), and the movable paw lining (1-16) and the fixed paw lining (1-2) are correspondingly arranged in the rectangular grooves on the inner sides of the movable half paw (1-15) and the fixed half paw (1-1) one by one.

3. The climbing truss robot of claim 2, wherein: the first paw (1) further comprises a friction material layer (1-17), and the friction material layer (1-17) is pasted in the semicircular grooves on the inner sides of the moving paw lining (1-16) and the fixed paw lining (1-2).

4. The climbing truss robot of claim 1, 2, or 3, wherein: the truss climbing robot further comprises a control system consisting of an upper computer, a first claw (1), a first wrist joint (2), an elbow joint (4), a second wrist joint (6) and servo control/drivers of a second claw (7), wherein the five servo control/drivers correspond to a direct-current servo motor of the first claw (1), a direct-current servo motor of the first wrist joint (2), a direct-current servo motor of the elbow joint (4), a direct-current servo motor of the second wrist joint (6) and a direct-current servo motor of the second claw (7) one by one;

the upper computer is communicated with a master node servo control/driver which drives and controls a direct current servo motor (1-9) in the first gripper (1) through a USB interface, and the master node servo control/driver is communicated with the other four servo control/drivers through a CAN bus; the friction wheel reverse feedback mechanism (1-3) and the gyroscope (1-14) are communicated with an upper computer through serial ports, the upper computer sends a control instruction of a target position containing the active joint to a main node servo control/driver through a USB interface, and the main node servo control/driver sends the received control instruction to a corresponding servo control/driver through a CAN bus.

5. The truss climbing robot as claimed in claim 4, wherein in the control system, the upper computer CAN add a CAN-PCI board card to the PCI slot, and directly communicate with all servo control/drivers through the CAN-PCI board card; under the condition that the upper computer uses the CAN-PCI board card, the upper computer sequentially sends motion instructions according to the node numbers of the servo control/drivers, and directly exchanges data with each servo control/driver; and after receiving the control instruction, all servo control/drivers synchronize according to the synchronous signal in the CAN bus and control the position or the moment of the respective DC servo motor.

6. A control method of the climbing truss robot as claimed in claim 1, 2, 3, 4 or 5, wherein the method is implemented by the following steps:

the generation process of the under-actuated joint motion state comprises the following steps: according to feedback data of a friction wheel reverse feedback mechanism (1-3) and a gyroscope (1-14) obtained by a serial port, an upper computer generates motion state feedback of the under-actuated joint

The steps are as follows:

step one, according to a fixed sampling period T_SData reading is carried out on the friction wheel reverse feedback mechanism (1-3) and the gyroscope (1-14) through a serial port of an upper computer, an obtained binary number is converted into a decimal number, a photoelectric encoder rotation angle is obtained through the friction wheel reverse feedback mechanism (1-3), the photoelectric encoder rotation angle is theta, and the paw rotation angular speed obtained through the gyroscope (1-14) is omega;

step two, calculating the under-actuated joint rotation angle theta by only considering the feedback data theta of the friction wheel reverse rotation feedback mechanism (1-3)_1FAnd angular velocity ω_1F(ii) a By means of I_FA reduction ratio of the friction wheel reverse feedback mechanism (1-3) is represented, and theta is calculated according to the rotation angle theta_1FAs shown in formula (1);

θ_1F＝θ/I_F (1)

at theta_1F ⁽ⁿ⁾Theta calculated for the nth sampling period_1FIn the nth sampling period, only considering the feedback data of the friction wheel reverse feedback mechanism (1-3) to obtain the angular velocity omega of the under-actuated joint_1F ⁽ⁿ⁾Can be calculated according to the formula (2);

step three, calculating the under-actuated joint rotation angle theta only by considering the feedback data omega of the gyroscopes (1-14)_1GAnd angular velocity ω_1G(ii) a Since the gyroscope is mounted on a robot gripper forming an under-actuated joint, the gyroscope (1-14) directly measures the rotational speed of the under-actuated joint, i.e. omega_1Gω; at omega_1G ⁽ⁿ⁾Indicating that the nth sampling period is obtainedOmega of_1GConsidering only the feedback data of the gyroscopes (1-14), the under-actuated joint angle theta of the nth sampling period_1G ⁽ⁿ⁾Can be calculated according to the formula (3);

in the formula (3) < theta >_1G ⁽⁰⁾When the truss climbing robot is powered on, the corner of the under-actuated joint is in an initial state, the robot grips the truss rod by one hand, and the main actuated joint is straightened and naturally droops to be motionless, namely theta_1G ⁽⁰⁾＝0°；

Step four, comprehensively considering theta calculated by feedback data of the friction wheel back rotation feedback mechanism (1-3)_1F、ω_1FAnd theta calculated from feedback data of the gyroscopes (1 to 14)_1G、ω_1GDetermining motion state feedback for under-actuated joints

The number N of lines of the photoelectric encoder according to the return mechanism (1-3) of the friction wheel_FWhen the quadrature pulse signal of the photoelectric encoder is subdivided into four parts in consideration of the reading, theta is calculated from the feedback data of the friction wheel reverse feedback mechanism (1-3)_1FMaximum truncation error theta of_1ECan be calculated according to the formula (4);

from the equation (2), the truncation error θ_1EAt an angular velocity ω_1FError omega introduced in_1EIs composed of

Under the condition of low-speed rotation of the under-actuated joint, the device is usedThe feedback data of the gyroscopes (1 to 14) is calculated to obtain the angular velocity omega_1GAngular velocity as an under-actuated joint

Angular velocity omega obtained by a friction wheel reverse feedback mechanism (1-3) when an under-actuated joint rotates at high speed_1FAt angular velocity of under-actuated joints

At omega_1GAnd ω_1FThe threshold value selected between is determined to be 20 omega_1ETo ensure

The relative error of (A) is not more than +/-5%;

at high speed of rotation of the under-actuated joint, in omega_1GAnd ω_1FThe absolute value of the deviation between the two friction wheels judges whether the friction wheels of the friction wheel reverse feedback mechanism (1-3) slip or not, and when the absolute value of the deviation is | omega |, the absolute value of the deviation is_1G-ω_1F|>When epsilon is considered to slip (epsilon is a threshold value of angular velocity deviation), theta should be set₁＝θ_1GAnd updates θ of the current (nth) sampling period by equation (6)_1F ⁽ⁿ⁾To eliminate the effect of slippage;

when | ω_1G-ω_1FWhen | ≦ epsilon, the friction wheel (1-3-2) is considered not to slip, and at the moment, theta is enabled₁＝θ_1G(ii) a Feedback of motion state of under-actuated joint by combining the above rules

It should be determined as in equation (7):

7. the control method for the truss climbing robot as claimed in claim 6, wherein the implementation process of the method further comprises:

the control process of swinging the grab bar started by the natural suspension state is as follows:

step one, modeling an inverse kinematics of a truss climbing robot: defining an active joint angle vector of the robot as theta_S＝[θ₂,θ₃,θ₄]^T，x＝[x_A,y_A,θ_A]^TIs the pose vector of the rider, where θ₂、θ₃、θ₄A wrist joint corner, an elbow joint corner, a wrist joint corner, x, adjacent to the supporting hand, respectively_A、y_A、θ_AThe x-axis coordinate and the y-axis coordinate of the central point of the free hand and the attitude angle of the free hand are respectively, the first paw (1) is used as a supporting hand, and the second paw (7) is used as the free hand; the inverse kinematics equation of the robot is shown in equation (8):

in the formula (8) < i >₁Is the distance from the axis of the truss rod (1-18) to the axis of the wrist joint adjacent to the supporting hand,/₂Is the distance from the axis of the wrist joint adjacent to the supporting hand to the axis of the elbow joint (4) |₃The distance between the elbow joint (4) axis and the wrist joint axis adjacent to the free hand, l₄Is the distance from the wrist joint axis adjacent to the free hand to the center of the free hand, a₁Is the distance from the axis of the first wrist joint (2) to the axis of the second wrist joint (6), a₂Is the distance from the wrist axis adjacent to the free hand to the truss rods (1-18), a₃The distance from the axis of the wrist joint adjacent to the supporting hand to the center of the free hand; a is₁、a₂、a₃Calculated according to the formulas (9), (10) and (11) respectively,

considering the need to align the gripper openings with the target truss bar when grabbing the bar with the hands, a constraint condition is added as shown in formula (12), where x^d、y^dRespectively the x-axis position coordinate and the y-axis position coordinate of the target truss rod, and the constraint condition determines the attitude angle theta when the center of the rider is not coincident with the center of the target truss rod_A；

θ_A＝π/2+arctan[(y^d-y_A)/(x^d-x_A)] (12)

When the calculated center of the free hand coincides with the center of the target truss rod, the constraint condition shown in equation (12) is invalid, and then the wrist joint angle adjacent to the free hand is set to a fixed value, namely theta, in order to make the robot avoid the singular configuration₄＝θ₀(θ₀>0) At this time, the attitude angle θ of the rider's hand_ACalculated according to equation (13):

in the formula (13), a₄Calculating the distance from the central point of the free hand to the axis of the elbow joint (4) according to the formula (14); the comprehensive formulas (8), (12) and (13) can be used for determining the position coordinates x of the free hand of the double-arm robot_A、y_AAnd under-actuated joint angle theta₁Determining a joint angle θ of an active joint₂、θ₃、θ₄；

Step two, self-starting stage control: firstly, a supporting hand of the truss climbing robot is held on a truss rod (1-18), other joints of the robot are straightened, and the whole robot keeps a suspension static state; then powering on a robot control system, carrying out self-checking on communication states of a friction wheel backward rotation feedback mechanism (1-3), a gyroscope (1-14) and each servo control/driver, enabling an elbow joint (4) of the robot to move according to a track shown by a formula (15), keeping a first wrist joint (2) and a second wrist joint (6) still, and supporting a target position x of manual opening and closing₁ ^dSet to the release grip position x_sTarget position x for opening and closing of rider₂ ^dSet to the open position x₀；

Theta in the formula (15)₃ ^dIs the elbow joint target angle; a is the motion amplitude of the elbow joint of the robot; t is t₁The system time obtained by timing from the self-starting stage; t is₁Is the movement time of the self-starting phase; the moment of finishing the self-starting stage is selected from the zero crossing point of the phase of the swing motion of the under-actuated joint, and the judgment condition is shown as the formula (16):

t₁≥T₁&β＝0 (16)

in the formula (16), beta is a phase angle of the swing motion of the under-actuated joint, beta is defined as an included angle between a connecting line from any point on a swing phase curve of the under-actuated joint to a central point of the phase curve and a longitudinal axis of a phase space, and as the central point of the phase curve possibly deviates from a theoretical (0,0) point and drifts along with time, an included angle between a tangent line of the phase curve and a transverse axis of the phase space is adopted

The approximation is carried out on the obtained data,

according to angular acceleration of under-actuated joints

And angular velocity

Calculated according to equation (17):

step three, controlling the excitation stage: starting to control the vibration exciting stage after the starting stage is finished, keeping the first wrist joint (2), the second wrist joint (6), the first paw (1) and the second paw (7) of the robot still, and moving the elbow joint (4) according to the target position given by the formula (18), namely, rotating angle theta of the elbow joint (4)₃Angle of rotation theta of under-actuated joint₁The phase difference of (a) is always kept at 90 degrees;

the judgment condition for the end of the excitation phase is as shown in equation (19), and is selected to be theta₁Maximum value of (max) ([ theta ])₁) To a theta₁ ^fWhen the swing phase angle beta of the under-actuated joint is 180 degrees, the speed of the elbow joint (4) of the robot is zero, and the impact when the follow-up motion is started can be reduced;

the safety factor a in the judgment condition (19) is more than 1, and the function of the safety factor a is to consider and compensate energy loss possibly generated in subsequent movement in the excitation stage in advance, so that the underactuated joint angle can still reach the target angle theta required by the gripping movement when the robot starts to grip the target truss rod₁ ^f，θ₁ ^fA first wrist joint (2) and a second wrist joint (7) of the robot are both straightened and a second claw(7) The angle of the under-actuated joint when grasping the target truss rod is calculated according to equation (20):

step four, control of an adjusting stage: at the moment when the tuning phase begins to end, the robot is tuned from the configuration at the end of the excitation phase to the configuration suitable for gripping the target truss rod in the tuning phase, and the specific tuning motion trajectory is as shown in formula (21), that is: at T₂Turning the angle theta of the elbow joint (4) within time₃Smooth transition from backward curved-A position to forward curved theta₃ ^fPosition, theta₃ ^fAs target rotation angles of the elbow joint (4) and the wrist joint adjacent to the free hand in the swing stage, and simultaneously rotating the wrist joint adjacent to the free hand by a rotation angle theta₄Transition from 0 to theta₄ ^f，θ₄ ^fAs a target corner for the adjustment phase; in the adjusting stage, a first paw (1), a wrist joint adjacent to the supporting hand and a second paw (7) of the robot are kept still;

theta in the formula (21)₃ ^f、θ₄ ^fTo be theta₁ ^fAnd target truss rod position coordinates (x)^d,y^d) Substituting the robot joint angle into an inverse kinematics equation (8); b₁Is the transition coefficient, calculated as equation (22):

t in formula (22)₂The system time is counted from the adjustment phase; the time when the adjusting phase is finished is selected to be 90-beta of swing phase angle beta of the under-actuated joint₀At a time of (b), wherein₀Is a constant greater than 0(ii) a Corresponding to the time when the under-actuated joint reaches the highest swing angle and the speed is 0, finishing the adjusting stage before the phase angle of the under-actuated joint reaches 90 degrees, and modifying the target position x for opening and closing the supporting hand at the finishing stage₁ ^dSet it to a grip position x_t；

Step five, controlling in a large damping stage: immediately after the adjustment stage, the damping stage is shifted to a large damping stage, and the target positions theta of the wrist joint adjacent to the supporting hand, the elbow joint (4), and the wrist joint adjacent to the free hand are set to be equal₂ ^f、θ₃ ^f、θ₄ ^fIs represented by equation (23):

in the formula (23) < theta >₂ ^*、θ₃ ^*、θ₄ ^*Is theta to be determined by equation (7)₁And target truss rod position coordinates (x)^d,y^d) Substituting the robot joint angle calculated in the inverse kinematics equation (8); b₂To smooth the transition coefficients for robot motion, the following is calculated as equation (24):

t in formula (24)₃The system time is counted from the beginning of the large damping stage; t is₃A grip transition time; when t is₃>T₃When the user wants to open or close the game hand, the game hand is closed to open or close the target position x₂ ^dFrom an open position x₀Switching to the closed position x_sCompleting the grasping of the target truss rod; and if the motion of the grabbing rod fails, the under-actuated joint recovers free rotation and is controlled again from the excitation stage until the target rod is successfully grabbed.

8. The control method for the truss climbing robot according to claim 7, wherein the implementation process of the method further comprises the following steps:

the truss climbing robot continues to perform a continuous moving control process after finishing primary target truss rod gripping:

step one, the transfer of joint variables: the initial state of the continuous movement is a state that the first paw (1) and the second paw (7) of the robot grasp the truss rods, the supporting hand positioned at the rear side in the moving direction of the robot loosens the truss rods and becomes a free hand in the subsequent continuous movement, and the free hand at the front side becomes the supporting hand in the subsequent continuous movement and forms an under-actuated joint with the grasped truss rods; therefore, the joint variables are required to be transcribed, namely the corner of the first wrist joint (1) is exchanged with the corner of the second wrist joint (7), the opening and closing distance of the first paw (1) is exchanged with the opening and closing distance of the second paw (7), and the corner of the elbow joint (4) is not changed; after the variable is transcribed, the following variables are always: x is the number of₁Indicating the opening and closing distance, x, of the supporting hand₂Indicating the opening and closing distance, theta, of the rider's hand₁Representing the angle of rotation theta of an under-actuated joint₂Representing the angle of rotation of the wrist joint, theta, adjacent to the supporting hand₃Indicates the angle of rotation, theta, of the elbow joint (4)₄Representing the wrist joint rotation angle adjacent to the free hand;

step two, controlling the configuration adjusting stage: angle of rotation theta in this stage₂、θ₄The two corresponding wrist joints move according to the planned track, and the position of the elbow joint rotation angle is servo to a given value theta₃ ^dAccording to theta₂、θ₄Kinematically determining, and adjusting the configuration of the robot by using the track shown in the formula (25); the open and close positions of the supporting hand and the free hand are kept in a loose holding state, i.e. x₁ ^d＝x_sAnd x₂ ^d＝x_s；

In the formula (25), θ₂ ^h、θ₄ ^hTheta at initial states of the configuration adjustment stages respectively₂、θ₄A value of (d); theta₂ ^pAnd theta₄ ^pRespectively representing theta in the target configuration at this stage₂、θ₄Value of (a), theta₂ ^p＝0，θ₄ ^pCalculated according to equation (26); b₃For the configuration adjustment of the transition coefficients used in the movement, the calculation is performed according to equation (27):

t in formula (28)₄System time timed from the start of the configuration adjustment phase; t is₄Adjusting the time of the movement for the configuration; the moment at which the adjustment phase ends is chosen at t₄＝T₄I.e. the moment at which the configuration adjustment movement is completed;

step three, rod loosening stage control: this phase begins immediately after the configuration adjustment phase is over, theta₂ ^d、θ₃ ^d、θ₄ ^d、x₁ ^dKeeping the position unchanged and releasing the rider to the open position (x)₂ ^d＝x₀) (ii) a The discrimination condition for the end of the rod releasing stage is shown in equation (28):

in the formula (28)

To identify the minimum speed of the under-actuated joint, t, at which the release of the rod is completed₅The system time is counted from the rod loosening stage; t is₅Is the minimum rod loosening time;

step four, controlling in a swing stage: the stage starts immediately after the pole loosening stage, and the opening and closing freedom degrees of the supporting hand and the free hand and the wrist joint adjacent to the supporting hand are supported in the swinging stageThe elbow joint (4) and the wrist joint adjacent to the free hand move from the position of rod loosening to the position ready for grasping within one swinging time of the under-actuated joint from back to front; the rotation angle of the wrist joint adjacent to the free hand at the end of the rod loosening stage is the terminal theta of the trajectory planning in the formula (25)₄ ^pAnd the rotation angle of the elbow joint is recorded as theta₃ ^p(ii) a The target rotation angles of the elbow joint (4) and the wrist joint adjacent to the free hand in the swinging stage are the same as the target rotation angles in the adjusting stage and are respectively theta₃ ^f、θ₄ ^f(ii) a The motion tracks of the elbow joint (4) and the wrist joint adjacent to the free hand are shown as the formula (29):

b in formula (29)₁Is the excessive coefficient of the swing motion, and is calculated according to the formula (30):

in the formula (30), t₅The system time is counted from the swing stage; t is₅The time of the swinging motion is determined according to the average time of swinging the under-actuated joint from back to front once in a plurality of experiments; the end time of the swing phase is selected in the same way as the end time of the adjustment phase in claim 7, and the end time of the swing phase is selected from the under-actuated joint swing phase angle β of 90 ° - β₀The time of day; modifying the target opening and closing position x of the supporting hand at the end of the swinging stage₁ ^dSet it to a grip position x_t；

Step five, controlling in a large damping stage: the large damping stage during continuous movement is completely the same as the large damping stage for grabbing the rod starting from the natural suspension state, please refer to step five in the control process of swinging the grabbing rod starting from the natural suspension state; and after the grabbing rod is finished, if the next target truss rod needs to be grabbed forwards continuously, restarting from the first step, and repeating the steps to realize continuous movement in the truss structure.

Images