CN109048961A - It can swing and grasp the climbing truss robot and its control method of remote truss rod - 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 aims to solve the problems that the existing truss climbing robot cannot reliably grasp the far truss rods and cannot move continuously in the truss structure. The friction material is added to the inner side of the robot's claws to provide greater damping torque; the gyro sensor is added to the claws to eliminate the effect of friction wheel slippage during high-speed swings; The feedback data of the instrument is used to generate the method of the motion state feedback of the underactuated joint. A vibration control method based on phase difference is proposed, and a swing grab bar control method started from the natural hanging state, and a continuous movement control method are carried out. The swing grab bar experiment started from the natural hanging state and the continuous movement Simulation and experiments verify the effectiveness of the proposed method. The invention is applied to the field of climbing truss robots.

Description

It can swing and grasp the climbing truss robot and its control method of remote truss rod

Technical field

The present invention relates to a kind of climbing truss robot and its control methods.

Background technique

Climbing truss robot is the mobile robot in a kind of typical Discontinuous transmission, and application environment not only includes Rigid truss structure in the buildings such as bridge, building, it may also be used for flexible truss (net made of knot is taken as hawser, chain etc. Network) structure, or even can also be applied in the unstructured complex environments such as the trees branch that nature is grown.Climb truss robot Itself does not include unactuated joint, but is affected by gravity, can phase between the gripper of the robot and the truss rod of circular cross-section To rotation, become a unactuated joint, therefore constitutes a under-actuated systems.Climb the move mode of truss robot It is bionical in primate (ape and monkey) swinging between branch and grasp branch movement, using a gripper hold truss rod as prop up Hand is supportted, another travelling gripper is flapped toward and grasps under the movement effect that swings that robot unactuated joint and active joint are coupled Another truss rod realizes rapidly, continuously moving in discrete truss structure repeatedly.

Quick professor male in Japan Nagoya university Feitian was had developed in 1996 with a main diarthrodial bionical monkey of two bars Child robot has successfully imitated the move mode for swinging branch, but since the bionical monkey robot developed does not have wrist joint, The robot is insufficient to the active regulating power of travelling gripper position in moving process, and its hook-type gripper is without any driving energy Power causes the movement uncertainty for supporting unactuated joint at hand to be difficult to eliminate, therefore the robot is it is difficult to ensure that grab the stabilization of bar Property, it swings and often grabs bar in the mobile experiment of branch and fail and need the vibrator that swings again.

Publication date is 20 days, Publication No. CN101434268 May in 2009, Patent No. ZL200810209775.4's Patent of invention provides a kind of ground moving and space truss climbing dual-purpose double-arm hand mobile robot.Robot described in the text With 10 freedom degrees, two First look sensors being symmetrically disposed on the platen of middle part and it is respectively arranged at right-hand man Two the second visual sensors on pawl, and grasping mechanism and wheeled locomotion mechanism have been separately designed on the gripper of left and right, so that Robot described in the text can not only flexibly move in the truss rod space with different section shapes and sizes, moreover it is possible to compared with Small energy consumption moves on the ground.But swinging when grasping circular cross-section truss rod is not provided in text and grabs bar control method.

Publication date is 2 days, Publication No. CN103332233A October in 2013, Patent No. ZL201310288965.0's Patent of invention provides a kind of Three Degree Of Freedom climbing truss robot and its big damping underactuated control method.Machine described in the text On device manpower pawl there is friction pulley to retreat feedback mechanism, turning for the unactuated joint formed when grasping circular truss bar can be measured Angle be used to swing and grab other master's diarthrodial target trajectories when bar in this corner control method described in the text and calculate, So that the robot is completed swinging in the truss structure that circular cross-section truss rod forms and grabs bar movement.But it is described Three Degree Of Freedom climbing truss robot and its big damping underactuated control method also rest on the theoretical and simulation study stage, do not have The test and validation under experimental situation is carried out, and does not provide the corresponding control method of continuous moving in control method.

In addition, other scholars are in the periodical delivered, also encourage primate bio-robot in meeting paper both at home and abroad It shakes and grabs bar movement and conduct in-depth research, but existing robot and its control method have the following problems: using single Vision or inertial measuring unit the motion state of unactuated joint is fed back, do not consider that single-sensor can not be in reality The problem of all complex working conditions are adapted in environment；Majority does not consider the movement uncertainties of unactuated joint to grabbing bar reliability Influence, can there is a situation where grab bar unsuccessfully to need to carry out multiple vibrator in experimentation；Unactuated joint is not considered when vibrator The influence of damping keeps vibrator speed relatively slow or is unable to reach biggish hunting range.

To sum up, existing patent, academic paper the problem of with can not be summarized as completely: existing climbing truss robot It is not perfect that gripper motion state feeds back measurement method, and the design of most of grippers is unable to satisfy save grip difference size, cuts The requirement of the truss rod of face shape；Existing climbing truss robot and its control method shortage swing in terms of grabbing bar motion credibility Effective experimental data, therefore the validity of designed robot and the control method provided cannot still determine completely；It is existing The vibrator control method that drive lacking climbs truss robot is big by unactuated joint damping effect, badly (freely turns round in operating condition When damp it is larger) in the case where the amplitude of oscillation rise slower, and be likely to occur the lower problem of the final amplitude of oscillation.

Summary of the invention

The present invention be solve existing truss climbing robot can not save grip truss rod and can not be in truss structure farther out The problem of interior progress continuous moving, so provide it is a kind of can swing grasp remote truss rod climbing truss robot and its Control method.

The purpose of the present invention is in the big damping drive lacking climbing truss robot of Three Degree Of Freedom and its control method (publication number CN103332233A, patent No. ZL201310288965.0) on the basis of, provide have between truss rod bigger frictional damping and The folding gripper of more accurate displacement state feedback, provide allow the robot to overcome unactuated joint friction and swing to The vibrator control of the amplitude of oscillation up to needed for grasping remote (distance in robot mechanism spread length 50% or more) target truss rod Method, and provide and be capable of swinging for save grip distant object truss rod and grab bar big damping underactuated control side stage by stage Method, to solve existing climbing truss robot and its swing to grab bar motion control method and cannot achieve remote circular cross-section purlin Hack lever grasps or grasps the not high problem of success rate.

The technical solution adopted by the present invention to solve the above technical problem is:

A kind of can swing grasps the climbing truss robot of remote truss rod, and the climbing truss robot includes first Gripper, the first wrist joint, first connecting rod, elbow joint, the second connecting rod, the second wrist joint and the second gripper；First gripper is logical The one end for crossing the first wrist joint and first connecting rod is rotatablely connected, and the other end of first connecting rod is connect by elbow joint with second One end of bar is rotatablely connected, and the second gripper is rotatablely connected by the second wrist joint and the other end of the second connecting rod；First gripper It is identical with the structure of the second gripper；First gripper retreats feedback mechanism, pawl sliding rail, ball screw, hand by fixed half pawl, friction pulley Pawl rack, connecting flange, servomotor encoder, DC servo-motor, servomotor speed reducer, driving gear, from Moving gear, pawl sliding block, gyroscope, dynamic half pawl, nut composition；Servomotor encoder, DC servo-motor, servo electricity Motivation speed reducer three is sequentially arranged at together, and driving gear is fixed on the output shaft of servomotor speed reducer, driven tooth Wheel engages with driving gear and is fixed on the shaft end of ball screw, and ball screw is positioned in gripper rack by bearing, ball wire The nut being set on bar is connected by pawl sliding block and dynamic half pawl, corresponds on the lateral surface of dynamic half pawl and fixed half pawl and top is housed Spiral shell instrument and friction pulley retreat feedback mechanism, and the friction pulley that friction pulley retreats feedback mechanism passes through the spring compression of two sides in truss rod On, fixed half pawl, pawl sliding rail and servomotor speed reducer are each attached in gripper rack, are additionally provided with connection method in gripper rack Gripper can be connect by orchid by connecting flange with the first wrist joint or the second carpal end flange.

Further, first gripper further includes determining pawl bushing, pawl bushing, moves the rectangle on the inside of half pawl and fixed half pawl It corresponds equipped with pawl bushing in slot and determines pawl bushing.

Further, first gripper further includes friction material layer, pawl bushing and determines the half slot on the inside of pawl bushing In post friction material layer.

When the truss rod to be grasped is the truss rod of circular cross-section, designed folding gripper can determine half pawl and move It is respectively charged into half pawl and determines pawl bushing and pawl bushing, and can selected according to the material and surface roughness of truss rod different Friction material；There is the truss rod of 90 ° of corner angle for such as angle steel, square steel, channel steel, I-steel prismatic shapes, it is fixed to remove Pawl bushing and pawl bushing are grasped using the rectangular channel on the inside of fixed half pawl and dynamic half pawl；Square on the inside of fixed half pawl, dynamic half pawl Connected in star and pawl bushing, the semi-circular recesses on the inside of pawl bushing are determined by Seriation Design, can so grasp various sizes of Truss rod.

Further, the climbing truss robot further includes being closed by host computer and the first gripper, the first wrist joint, elbow SERVO CONTROL/driver composition control system of section, the second wrist joint, the second gripper, five SERVO CONTROL/drivers and the The DC servo-motor of one gripper, the first carpal DC servo-motor, the DC servo-motor of elbow joint, second The DC servo-motor one-to-one correspondence of carpal DC servo-motor, the second gripper；Host computer is by USB interface and drives The host node SERVO CONTROL of DC servo-motor/driver communication, host node SERVO CONTROL/drive in dynamic and the first gripper of control Dynamic device is communicated by CAN bus and four additional SERVO CONTROL/driver；Friction pulley retreat feedback mechanism and gyroscope with it is upper Machine is communicated by serial ports, and the control instruction containing main diarthrodial target position is sent to by host computer by USB interface The control instruction received is sent to by host node SERVO CONTROL/driver, host node SERVO CONTROL/driver by CAN bus Corresponding SERVO CONTROL/driver.

In the control system, host computer can install one piece of CAN-PCI board additional on PCI slot, straight by this board It connects and is communicated with all SERVO CONTROL/drivers；In the case where host computer uses CAN-PCI board, host computer is by watching It takes control/driver node number and successively sends movement instruction, directly carry out data exchange with each SERVO CONTROL/driver；Institute It is synchronized after thering is SERVO CONTROL/driver to be controlled instruction by the synchronization signal inside CAN bus, and to respective direct current Servo motor carries out position or Torque Control.

A kind of control method of above-mentioned climbing truss robot, the method the realization process includes:

The process that unactuated joint motion state generates: according to the friction pulley retreat feedback mechanism and gyro obtained by serial ports The feedback data of instrument, host computer generate the motion state feedback of unactuated jointThe step of it is as follows:

Step 1: according to fixed sampling period T_SFeedback mechanism and gyroscope are retreated to friction pulley by host computer serial ports Reading data is carried out, and is converted to decimal number for binary number is obtained, is compiled by the photoelectricity that friction pulley retreat feedback mechanism obtains Code device corner is θ, is ω by the gripper rotational angular velocity that gyroscope obtains；

Step 2: only considering that the feedback data θ of friction pulley retreat feedback mechanism calculates unactuated joint rotational angle theta_1FWith angle speed Spend ω_1F；Use I_FIndicate the reduction ratio of friction pulley retreat feedback mechanism, the then θ calculated according to rotational angle theta_1FAs shown in formula (1)；

θ_1F=θ/I_F (1)

With θ_1F ⁽ⁿ⁾Indicate n-th of sampling period counted θ_1F, then within n-th of sampling period, only consider friction pulley retreat The unactuated joint angular velocity omega that the feedback data of feedback mechanism acquires_1F ⁽ⁿ⁾It can be calculated by formula (2)；

Step 3: only considering that the feedback data ω of gyroscope calculates unactuated joint rotational angle theta_1GAnd angular velocity omega_1G；Due to Gyroscope is installed on the robot hand to form unactuated joint, therefore gyroscope directly measures the revolving speed of unactuated joint, That is ω_1G=ω；With ω_1G ⁽ⁿ⁾Indicate the ω that n-th of sampling period obtain_1G, then when only considering the feedback data data of gyroscope, The unactuated joint rotational angle theta in n-th of sampling period_1G ⁽ⁿ⁾It can be calculated by formula (3)；

θ in formula (3)_1G ⁽⁰⁾For the corner for climbing unactuated joint under original state when truss robot powers on, this initial shape State is that robot grasps truss rod on the other hand, and main driving joint, which is stretched, naturally droops stationary state, i.e. θ_1G ⁽⁰⁾=0 °；

Step 4: comprehensively considering by the counted θ of feedback data of friction pulley retreat feedback mechanism_1F、ω_1F, and by gyro The counted θ of the feedback data of instrument_1G、ω_1G, determine the motion state feedback of unactuated jointIt is anti-according to friction pulley retreat Present the photoelectric encoder line number N of mechanism_F, consider to have carried out four carefully to the orthogonal pulses signal of this photoelectric encoder when reading Point, then by the counted θ of feedback data of friction pulley retreat feedback mechanism_1FThe middle maximum truncated error θ that may have_1EIt can be by formula (4) it calculates；

By formula (2) it is found that truncated error θ_1EIn angular velocity omega_1FThe error ω of middle introducing_1EFor

It should be used under conditions of unactuated joint slowly runs and angular velocity omega is calculated to obtain by the feedback data of gyroscope_1GAs The angular speed of unactuated jointThe angular speed obtained in unactuated joint high-speed rotation by friction pulley retreat feedback mechanism ω_1F, in the angular speed of unactuated jointIn ω_1GAnd ω_1FBetween the threshold values that selects be determined as 20 ω_1E, to guaranteeIt is opposite Error is no more than ± 5%；

In unactuated joint high-speed rotation, with ω_1GAnd ω_1FBetween partially absolute value of the difference judge friction pulley retreat feed back Whether the friction pulley of mechanism skids, when | ω_1G-ω_1FThink that friction pulley skids (threshold value that ε is angular speed deviation) when | > ε, It should make θ₁=θ_1G, and by the θ in formula (6) update current (n-th) sampling period_1F ⁽ⁿ⁾To eliminate the influence skidded；

When | ω_1G-ω_1FThink that friction pulley is non-slip when |≤ε, this season θ₁=θ_1G；In summary regular, drive lacking closes The motion state of section is fed backIt should be determined by formula (7):

Further, the realization process of the method further include:

The control process of bar is grabbed by swinging of starting of natural overhang:

Step 1: the Inverse Kinematics of climbing truss robot: the active joint angle vector for defining robot is θ_S= [θ₂,θ₃,θ₄]^T, x=[x_A,y_A,θ_A]^TFor the pose vector for swimming hand, wherein θ₂、θ₃、θ₄Turn respectively with the wrist joint of support palmistry neighbour Angle, elbow joint corner, the wrist joint corner with trip palmistry neighbour, x_A、y_A、θ_ARespectively swim x-axis coordinate, the y-axis coordinate of hand central point It is support hand with the first gripper with the attitude angle of trip hand, the second gripper is trip hand；The inverse kinematics equation such as formula (8) of robot It is shown:

L in formula (8)₁For truss rod axis to support palmistry neighbour wrist joint axis at a distance from, l₂For with support palmistry Distance of the adjacent carpal axis to elbow joint axis, l₃For elbow joint axis to trip palmistry neighbour wrist joint axis away from From l₄For with trip palmistry neighbour wrist joint axis to swim hand center at a distance from, a₁For the first wrist joint axis to the second wrist joint axis The distance of line, a₂For with trip palmistry neighbour wrist joint axis to truss rod at a distance from, a₃For the wrist joint axis with support palmistry neighbour To the distance at trip hand center；a₁、a₂、a₃It is calculated respectively by formula (9), (10), (11),

Need to be open gripper when grabbing bar in view of trip hand alignment target truss rod, and addition constrains item as shown in formula (12) Part, wherein x^d、y^dThe respectively x-axis of target truss rod, y-axis position coordinates, this constraint condition determined trip hand center not with mesh Mark attitude angle θ when truss rod center is overlapped_A；

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

When the center for calculating trip hand is overlapped with target truss rod center, constraint condition failure shown in formula (12), at this moment To make robot avoid unusual configuration, fixed value, i.e. θ will be set as with the wrist joint angle of trip palmistry neighbour₄=θ₀(θ₀> 0), at this time Swim the attitude angle θ of hand_AIt is calculated by formula (13):

In formula (13), a₄For the distance of trip hand central point to elbow joint axis, calculated by formula (14)；Composite type (8), (12), (13), can be according to both arms hand robot travel hand position coordinate x_A、y_AWith unactuated joint angle θ₁Determine main diarthrodial pass Save angle θ₂、θ₃、θ₄；

Step 2: self-starting stage control: enabling the support hand of climbing truss robot be grasped on truss rod first, machine Other joints of people are stretched, and entire robot keeps static state of dangling；Then robot control system is powered on, to rubbing After the communication state progress self-test for wiping wheel retreat feedback mechanism, gyroscope and each SERVO CONTROL/driver, the elbow of robot is enabled Joint is moved by track shown in formula (15), while the first wrist joint, the second wrist joint remain stationary, the mesh of support hand folding Cursor position x₁ ^dIt is set as pine and holds position x_s, the target position x of trip hand folding₂ ^dIt is set as deployed position x₀；

θ in formula (15)₃ ^dIt is elbow joint target angle；A is the motion amplitude of Robot elbow joint；t₁For from the self-starting stage Start to carry out the system time that timing obtains；T₁For the run duration in self-starting stage；The self-starting stage is selected at the time of end The phase zero crossing of unactuated joint pendulum motion, shown in criterion such as formula (16):

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

β is that unactuated joint swings the phase angle of movement in formula (16), and β is defined as unactuated joint swing phase curve and takes up an official post Meaning is a little to the angle of the line of phase center of curve point and the phase space longitudinal axis, due on the possible deviation theory of the central point of phase curve (0,0) put and drift about at any time, using the angle of phase curve near tangent and phase space horizontal axisApproximation is carried out to it,According to The angular acceleration of unactuated jointAnd angular speedIt is calculated by formula (17):

Step 3: vibrator stage control: start to carry out the control in vibrator stage after the self-starting stage, the of robot One wrist joint, the second wrist joint, the first gripper, the second gripper remain stationary, the target position that elbow joint is provided by formula (18) It is moved, i.e. elbow joint rotational angle theta₃With unactuated joint rotational angle theta₁Phase difference remain at 90 °；

Shown in the criterion that the vibrator stage terminates such as formula (19), select in θ₁Maximum value max (θ₁) reach a θ₁ ^f, and At the time of swaying phase angle beta=180 ° of unactuated joint, the speed of Robot elbow joint is zero at this time, can reduce start into Impact when row subsequent motion；

A is the safety factor greater than 1 in criterion (19), and effect is the reforwarding after the vibrator stage considers compensation in advance Issuable energy loss in dynamic makes robot remain to reach grasping in unactuated joint angle when starting to grasp target truss rod Target angle θ needed for movement₁ ^f, θ₁ ^fThe first wrist joint, the second wrist joint for robot stretch and the second gripper grasps mesh The angle of unactuated joint when marking truss rod, is calculated by formula (20):

Step 4: the adjusting stage controls: the adjusting stage starts from the moment terminated in the vibrator stage, adjusting stage inner machine people It is specific to adjust motion profile such as formula (21) institute by the formation adjusting at the end of vibrator to the configuration for being suitable for grasping target truss rod Show, it may be assumed that in T₂By the rotational angle theta of elbow joint in time₃It is smoothly transitted by recurvate-location A to prone θ₃ ^fPosition, Simultaneously by the wrist joint rotational angle theta with trip palmistry neighbour₄θ is transitted to by 0₄ ^f；Within the adjusting stage, the first gripper of robot and branch Wrist joint, the second gripper of support palmistry neighbour remains stationary；

θ in formula (21)₃ ^f、θ₄ ^fFor by θ₁ ^fWith target truss rod position coordinates (x^d,y^d) substitute into inverse kinematics equation (8) Counted joint of robot angle；b₁It is excessive coefficient, is calculated by formula (22):

T in formula (22)₂For the system time of the timing since the adjusting stage；Theoretically, it is answered at the time of the adjusting stage terminates Select at the time of it is 0 that unactuated joint, which reaches highest pivot angle and speed, i.e., β=90 ° at the time of, but in view of support hand is from pine It holds position and moves to gripping position and need the time, need to be selected in unactuated joint at the time of terminating the adjusting stage and reach highest pivot angle Before, excessive to prevent from retreating during support holding is tight；Therefore β=90 °-β is selected at the time of terminating the adjusting stage₀When It carves, wherein β₀For the constant greater than 0, the adjusting stage is terminated in advance at β < 90 ° in this way, and the hand folding of modification support at the end Target position x₁ ^d, it is set to gripping position x_t, hold support tight；

Step 5: big damping stage control: big damping stage is immediately entered after the adjusting stage, with support palmistry neighbour's Wrist joint, elbow joint, the carpal target position θ with trip palmistry neighbour₂ ^f、θ₃ ^f、θ₄ ^fCalculation formula such as formula (23) shown in:

θ in formula (23)₂ ^*、θ₃ ^*、θ₄ ^*For the θ that will be determined by formula (7)₁With target truss rod position coordinates (x^d,y^d) substitute into it is inverse Counted joint of robot angle in kinematical equation (8)；b₂For the sporting transfer coefficient of smooth robot, based on formula (24) It calculates:

T in formula (24)₃For the system time of the timing since big damping stage；T₃To grasp transit time；Work as t₃>T₃When, Trip hand closure is enabled, its target is opened and closed into position x₂ ^dBy open position x₀It is switched to (pine is held) closed position x_s, complete to target purlin The grasping of hack lever；If grabbing bar movement failure, so that unactuated joint is affranchised and turn round and re-started since the vibrator stage Control, until successfully grasping target stem.

Further, the realization process of the method further include: the climbing truss robot completes a target purlin Hack lever continues the control process of continuous moving after grasping:

Step 1: the transcription of joint variable: the original state of continuous moving is that the first gripper of robot, the second gripper are equal The state of truss rod is hold, the support hand on rear side of robot moving direction will unclamp truss in continuous moving later Bar simultaneously becomes swimming hand, and the trip hand of front side will become supporting hand in continuous moving later and be formed with the truss rod grasped deficient Drive joint；Therefore needing to carry out the transcription of joint variable, i.e., the first carpal corner is exchanged with the second carpal corner, The folding distance of first gripper is exchanged with the folding of the second gripper distance, and the corner of elbow joint is constant；Make always after variable transcription: x₁Indicate the folding distance of support hand, x₂Indicate the folding distance of trip hand, θ₁Indicate the corner of unactuated joint (by being installed on branch Gyroscope and friction pulley the retreat feedback mechanism supportted on hand measures), θ₂Indicate the wrist joint corner with support palmistry neighbour, θ₃It indicates The corner of elbow joint, θ₄Indicate the wrist joint corner with trip palmistry neighbour；

Step 2: formation adjusting stage control: rotational angle theta in this stage₂、θ₄Corresponding two wrist joints are by planning rail Mark movement, the position servo given value θ of elbow joint corner₃ ^dAccording to θ₂、θ₄It is determined by kinematics, uses track shown in formula (25) Robot configuration is adjusted；Support hand and the folding position for swimming hand are maintained at pine and hold state, i.e. x₁ ^d=x_sAnd x₂ ^d=x_s；

In formula (25), θ₂ ^h、θ₄ ^hRespectively θ under the original state in formation adjusting stage₂、θ₄Value；θ₂ ^pAnd θ₄ ^pIt respectively indicates θ in this phase targets configuration₂、θ₄Value, θ₂ ^p=0, θ₄ ^pIt is calculated by formula (26)；b₃For transition system used in formation adjusting movement Number is calculated by formula (27):

T in formula (28)₄For the system time of the timing since the formation adjusting stage；T₄For the time of formation adjusting movement；It adjusts Selection is in t at the time of the whole stage terminates₄=T₄At the time of, i.e., at the time of formation adjusting movement is completed；

Step 3: loose bar stage control: this stage immediately begins to after the configuration adjusting stage, θ₂ ^d、θ₃ ^d、θ₄ ^d、x₁ ^dIt protects Hold it is constant, enable trip hand unclamp to deployed position (x₂ ^d=x₀)；Shown in the criterion such as formula (28) that the loose bar stage terminates:

In formula (28)For the unactuated joint minimum speed for assert loose bar completion, t₅For the timing since the loose bar stage System time；T₅For the minimum loose bar time；

Step 4: the stage control that swings: this stage gets started after the loose bar stage, in the stage inner support that swings Hand, swim hand folding freedom degree and with support palmistry neighbour wrist joint remain stationary, elbow joint and with swim palmistry neighbour wrist Joint will move to the position for preparing to grasp in position when once swinging in the time by loose bar of unactuated joint from the front to the back； It with the wrist joint corner of trip palmistry neighbour is the terminal θ of trajectory planning in formula (25) at the end of the loose bar stage₄ ^p, and elbow joint is turned Angle is denoted as θ₃ ^p；Swing elbow joint and carpal target rotation angle and the target rotation angle phase of adjusting stage with trip palmistry neighbour in the stage Together, respectively θ₃ ^f、θ₄ ^f；Then elbow joint and shown with the carpal motion profile such as formula (29) of trip palmistry neighbour:

B in formula (29)₁It is the excessive coefficient of movement of swinging, is calculated by formula (30):

In formula (30), t₅For the system time of the timing since the stage of swinging；T₅Swing time of movement, according to multiple reality It is determining to test the average time that middle unactuated joint swings primary from the front to the back；Swing finish time in stage selection and the adjusting stage The selection mode of finish time is identical, is selected in unactuated joint swaying phase angle beta=90 °-β₀At the time of；The stage of swinging terminates Shi Xiugai supports the target of hand to open and close position x₁ ^d, it is set to gripping position x_t；

Step 5: big damping stage control: big damping stage when continuous moving is actuated for from natural overhang The big damping stage for grabbing bar is identical, refers to the step in the control process for grabbing bar that swings started by natural overhang Five；After grabbing bar, if need to continue to grasp next target truss rod forward, restarts from step 1, so recycle past Continuous moving in multiple then achievable truss structure.

The beneficial effects of the present invention are:

Present invention improves over climbing truss robot gripper design, thus design and have developed can save grip it is remote The climbing truss robot and its control system of truss rod (adjacent truss span is greater than 50% that robots arm opens up)；This hair The bright unactuated joint motion state for proposing the feedback data for comprehensively considering friction pulley retreat feedback mechanism and gyroscope generates Method, and by based on phase difference vibrator control method and big damping drive lacking grab bar control method, proposing one kind can It grabs bar method by grasping swinging for remote truss rod and a kind of carries out continuous moving in truss structure in a manner of swinging Motion control method.

Friction material is mounted on the inside of the gripper of designed truss climbing robot, and robot can be made to stretch to farther out Target when obtain enough dampings；Due to being equipped with friction pulley retreat feedback mechanism and gyroscope on gripper simultaneously, eliminate The influence that friction pulley skids to the motion state detection of unactuated joint feeds back the motion state of unactuated joint more quasi- Really；The vibrator control method proposed can play whole driving capabilities of robot, carry out with existing vibrator control method Comparison, has the advantages that vibrator speed is fast, amplitude is easily controllable and small by unactuated joint frictional influence；In robot because of gravity Effect and in the case where constantly retreating, proposed start from natural overhang swing and grab bar control method and remain to long distance It realizes to stablize from truss rod and grasp, experiment success rate is up to 100%；Through emulation and experimental verification, the continuous moving control proposed Method processed can complete the continuous moving motion control task of climbing truss robot, and the continuous moving Success in Experiment of half period Rate is 100%, has practical value.

To sum up, the climbing truss robot and its control system and proposed by the present invention swing that the present invention designs grab bar control Method moves towards practical for climbing truss robot and has pushed ahead major step with technical aspect theoretical.

Publication date is 2 days, Publication No. CN103332233A October in 2013, Patent No. ZL201310288965.0's Patent of invention provides a kind of Three Degree Of Freedom climbing truss robot and its big damping underactuated control method.The present invention and its not Be with place: the folding gripper of robot is added between the gyroscope and increase and truss rod for measuring angular velocity of rotation and rubs The friction material of power；The damping of unactuated joint under free turn state is considered in control method of the present invention to the shadow in vibrator stage It rings, proposes the vibrator control method based on the phase difference that swings；The present invention not only gives the vibrator since natural overhang Later grasp swinging for target truss rod grab bar control method, give completion once swing grab bar after connect in truss structure Continue mobile control method；The present invention completes swinging for truss climbing robot under experimental conditions and grabs bar exercise testing, The continuous moving emulation that the multicycle is completed under simulated conditions, is easy to carry out technical transform and forms product.

The present invention increases friction material on the inside of the gripper of robot to provide bigger damping torque；Increase on gripper Gyro sensor can eliminate the influence that friction pulley skids when high speed swings；And it gives and comprehensively considers friction pulley retreat feedback The feedback data of mechanism and gyroscope, the method for generating unactuated joint motion state feedback.Propose encouraging based on phase difference It shakes control method, and proposes and a kind of bar control method and a kind of continuous moving are grabbed by swinging of starting of natural overhang Control method, swinging for start from natural overhang grab bar experiment and continuous moving emulation and experiment, demonstrate institute The validity of proposition method.The present invention is applied to climbing truss robot field.

Detailed description of the invention

Fig. 1 is that the present invention improves and the object of control (the Three Degree Of Freedom climbing purlin of Patent No. ZL201310288965.0 Frame robot) photo；Fig. 2 is the improved paw mechanism schematic diagram of the present invention；Fig. 3 is the climbing truss machine designed in the present invention The folding gripper of device people grasps the schematic diagram when truss rod of different cross section shape；Fig. 4 is controlled device of the present invention (climbing truss Robot) mechanism parameter and physical parameter definition scheme；Fig. 5 is the climbing truss robot control system hardware that the present invention designs Block diagram；Fig. 6, which is the present invention, to be grabbed bar motion stage to swinging of being started by natural overhang and divides and each course movement schematic diagram； Fig. 7 is to have continued the divided stages of continuous moving and each rank truss rod after the present invention has grasped target truss rod to robot Duan Yundong schematic diagram；Fig. 8 is the phase space schematic diagram that unactuated joint swings；Fig. 9 be the present invention design by natural catenary Rod controller functional block diagram is grabbed in swinging for state starting；Figure 10 is the continuous moving controller principle block diagram that the present invention designs；Figure 11 It is that (energy is pumped into method and by the controlling party proposed by the present invention based on phase difference the obtained two kinds of control methods of vibrator control emulation Method) unactuated joint amplitude curve figure；Figure 12 is swing with the improved truss climbing robot of the present invention grabbing bar reality Experiment scene photo when testing；Figure 13 is the joint of robot adjusted the distance when being grasped for the target truss rod of 0.4m and 1.0m Angular curve figure, in figure, the joint angular curve that (a) is target stem distance when being 400mm, when (b) to be target stem distance be 1000mm Joint angular curve；Figure 14 is the experiment video recording screenshot adjusted the distance when being grasped for the target truss rod of 0.4m and 1.0m；Figure 15 be swing from start completion under natural overhang grab bar after complete a cycle continuous moving simulation video screenshot； Figure 16 is that the target truss rod for 0.5m and 0.8m of adjusting the distance is grasped that (original state has held the purlin that distance is 1.0m respectively Hack lever) half period continuous moving experiment video recording screenshot；Figure 17 is the joint of robot angular curve of two groups of experiments in Figure 16.

Specific embodiment

Specific embodiment 1: as shown in figures 1-4, improved climbing truss robot includes first in present embodiment Gripper 1, the first wrist joint 2, first connecting rod 3, elbow joint 4, the second connecting rod 5, the second wrist joint 6 and the second gripper 7, first Gripper 1 is rotatablely connected by one end of the first wrist joint 2 and first connecting rod 3, and the other end of first connecting rod 3 passes through elbow joint 4 and second one end of connecting rod 5 be rotatablely connected, the other end turn that the second gripper 7 passes through the second wrist joint 6 and the second connecting rod 5 Dynamic connection；First gripper 1 and the second gripper 7 are the same part, the first wrist joint 2 and the second wrist joint 6 for the same part, first Connecting rod 3 and the second connecting rod 5 are Same Part.First wrist joint 2, elbow joint 4, the second wrist joint 6 are by DC servo electricity Motivation, Synchronous Belt Drives and harmonic drive composition, DC servo-motor is via Synchronous Belt Drives harmony Wave gear drive drives articulation.

First gripper 1 or the second gripper 7 by fixed half pawl 1-1, determine pawl bushing 1-2, friction pulley retreat feedback mechanism 1-3, move Pawl sliding rail 1-4, ball screw 1-5, gripper rack 1-6, connecting flange 1-7, servomotor encoder 1-8, DC servo electricity Motivation 1-9, servomotor speed reducer 1-10, driving gear 1-11, driven gear 1-12, pawl sliding block 1-13, gyroscope 1- 14, half pawl 1-15, pawl bushing 1-16, friction material 1-17, nut 1-19 composition are moved.Servomotor encoder 1-8, direct current Servomotor 1-9, servomotor speed reducer 1-10 three are sequentially arranged at together, and driving gear 1-11 is fixed on servo electricity On the output shaft of motivation speed reducer 1-10, driven gear 1-12 is engaged with driving gear 1-11 and is fixed on ball screw 1-5's Shaft end, ball screw 1-5 are positioned on gripper rack 1-6 by bearing, and the nut 1-19 being set on ball screw 1-5 passes through pawl Sliding block 1-13 and dynamic half pawl 1-15 are connected, and are respectively provided with pawl bushing in the rectangular channel on the inside of dynamic half pawl 1-15 and fixed half pawl 1-1 1-16 and determine pawl bushing 1-2, be respectively provided with gyroscope 1-14 on dynamic half pawl 1-15 and the calmly lateral surface of half pawl 1-1 and friction pulley moves back Turn feedback mechanism 1-3, the friction pulley 1-3-2 of friction pulley retreat feedback mechanism 1-3 is pressed in truss by the spring 1-3-1 of two sides On bar 1-18, pawl bushing 1-16 and determine to post friction material 1-17 in half slot on the inside of pawl bushing 1-2, fixed half pawl 1-1, move Pawl sliding rail 1-4 and servomotor speed reducer 1-10 are each attached on gripper rack 1-6, are additionally provided with connection on gripper rack 1-6 Gripper can be connect by flange 1-7 by connecting flange 1-7 with the end flange of the first wrist joint 2 or the second wrist joint 6.Gripper DC servo-motor 1-9 via driving gear 1-11 → driven gear 1-12 → ball screw 1-5 → nut 1-19 → dynamic Pawl sliding block 1-13 drives dynamic half pawl 1-15 to carry out the closing motion towards fixed half pawl 1-1 and the opening movement away from fixed half pawl 1-1.

Specific embodiment 2: as shown in Figure 2,3, as the truss rod 1-18 that the truss rod to be grasped is circular cross-section When, the folding gripper of the robot can be respectively charged into fixed half pawl 1-1 and dynamic half pawl 1-15 determines pawl bushing 1-2 and pawl lining 1-16 is covered, and different friction material 1-17 can be selected according to the material and surface roughness of truss rod 1-18, is such as grasped The biggish rubber material of coefficient of friction can be used in hard slippery surface, and grasping hard rough surface can be used the preferable silicon of wearability Glue material, grasping the soft out-of-flatness surfaces such as hawser can be used the preferable asbestos layer of mosaic.For such as angle steel 1-20, square steel 1- 21, channel steel 1-22, I-steel 1-23 prismatic shapes have the truss rod of 90 ° of corner angle, can remove and determine pawl bushing 1-2 and pawl lining 1-16 is covered, is grasped using the rectangular channel on the inside of fixed half pawl 1-1 and dynamic half pawl 1-15.On the inside of fixed half pawl 1-1, dynamic half pawl 1-15 Rectangular recess and determine semi-circular recesses on the inside of pawl bushing 1-2, pawl bushing 1-16 by Seriation Design, can so grasp Various sizes of truss rod.

Specific embodiment 3: as shown in figure 5, the control system of the climbing truss robot is by host computer and first-hand Pawl 1, the first wrist joint 2, elbow joint 4, the second wrist joint 6, the second gripper 7 servomotor control/driver composition, it is upper Host node SERVO CONTROL/driver that machine passes through DC servo-motor 1-9 in USB interface and driving and the first gripper 1 of control Communication, host node SERVO CONTROL/driver are communicated by CAN bus and other SERVO CONTROLs/driver, and each SERVO CONTROL/ Driver corresponds to a DC servo-motor.Host computer can also install one piece of CAN-PCI board additional on PCI slot, pass through This board is directly communicated with all SERVO CONTROL/drivers.Friction pulley on robot hand retreats feedback mechanism 1-3 The rotational angle theta of its interior photoelectric encoder axis is fed back, gyroscope 1-14 feeds back the angular velocity of rotation ω of itself, and friction pulley retreats feedback machine Structure 1-3 and gyroscope 1-14 is communicated with host computer by serial ports, and host computer generates the movement of unactuated joint according to θ and ω State feedbackAnd according toMain diarthrodial target position is calculated with the diarthrodial corner feedback of other masters.On Control instruction containing main diarthrodial target position is sent to host node SERVO CONTROL/driver by USB interface by position machine, The control instruction received is sent to corresponding SERVO CONTROL/driver by CAN bus by host node SERVO CONTROL/driver； In the case where host computer uses CAN-PCI board, host computer successively sends movement by SERVO CONTROL/driver node number and refers to It enables, directly carries out data exchange with each SERVO CONTROL/driver, required communication time ratio relies on USB interface in such cases When it is less, can be realized that the identical Shaft and NC Machining Test period is shorter or control period identical more on line real time controls of the number of axle.Institute It is synchronized after thering is SERVO CONTROL/driver to be controlled instruction by the synchronization signal inside CAN bus, and to respective direct current Servo motor carries out position or Torque Control.

Specific embodiment 4: according to the anti-of the friction pulley retreat feedback mechanism 1-3 and gyroscope 1-14 obtained by serial ports Data are presented, host computer generates the motion state feedback of unactuated jointThe step of it is as follows:

Step 1: according to fixed sampling period T_SFeedback mechanism 1-3 and top are retreated to friction pulley by host computer serial ports Spiral shell instrument 1-14 carries out reading data, and is converted to decimal number for binary number is obtained, and is obtained by friction pulley retreat feedback mechanism 1-3 The photoelectric encoder corner arrived is θ, is ω by the gripper rotational angular velocity that gyroscope 1-14 is obtained.

Step 2: only considering that the feedback data θ of friction pulley retreat feedback mechanism 1-3 calculates unactuated joint rotational angle theta_1FThe angle and Speed omega_1F.Use I_FIndicate the reduction ratio of friction pulley retreat feedback mechanism 1-3, the then θ calculated according to rotational angle theta_1FAs shown in formula (1).

θ_1F=θ/I_F (1)

With θ_1F ⁽ⁿ⁾Indicate n-th of sampling period counted θ_1F, then within n-th of sampling period, only consider friction pulley retreat The unactuated joint angular velocity omega that the feedback data of feedback mechanism 1-3 acquires_1F ⁽ⁿ⁾It can be calculated by formula (2).

Step 3: only considering that the feedback data ω of gyroscope 1-14 calculates unactuated joint rotational angle theta_1GAnd angular velocity omega_1G。 Since gyroscope is installed on the robot hand to form unactuated joint, gyroscope 1-14 directly measures unactuated joint Revolving speed, i.e. ω_1G=ω.With ω_1G ⁽ⁿ⁾Indicate the ω that n-th of sampling period obtain_1G, then only consider the feedback of gyroscope 1-14 When Data Data, the unactuated joint rotational angle theta in n-th of sampling period_1G ⁽ⁿ⁾It can be calculated by formula (3).

θ in formula (3)_1G ⁽⁰⁾For climbing truss robot power on when original state under unactuated joint corner, in the present invention In this original state be robot on the other hand grasp truss rod, main driving joint, which is stretched, naturally droops stationary state, i.e., θ_1G ⁽⁰⁾=0 °.

Step 4: comprehensively considering by the counted θ of feedback data of friction pulley retreat feedback mechanism 1-3_1F、ω_1F, Yi Jiyou The counted θ of the feedback data of gyroscope 1-14_1G、ω_1G, determine the motion state feedback of unactuated jointAccording to friction pulley Retreat the photoelectric encoder line number N of feedback mechanism 1-3_F, consider reading when to the orthogonal pulses signal of this photoelectric encoder into Gone four subdivision, then by friction pulley retreat feedback mechanism 1-3 the counted θ of feedback data_1FThe middle maximum truncation that may have misses Poor θ_1EIt can be calculated by formula (4).

By formula (2) it is found that truncated error θ_1EIn angular velocity omega_1FThe error ω of middle introducing_1EFor

Due to the sampling period T of sensor_SVery short (generally several milliseconds to more than ten milliseconds), therefore angle is calculated by formula (5) Speed omega_1FIn truncated error ω_1EIt can not ignore, should use under conditions of unactuated joint slowly runs by gyroscope 1- 14 feedback data calculates to obtain angular velocity omega_1GAngular speed as unactuated jointBut the machine of friction pulley retreat feedback mechanism 1-3 Tool ontology has certain filter action, can filter because truss vibration can be the same as the high frequency incoming by gyroscope 1-14 feedback Disturbing signal, therefore the angle speed obtained by friction pulley retreat feedback mechanism 1-3 should be also used in unactuated joint high-speed rotation Spend ω_1F, in the angular speed of unactuated jointIn ω_1GAnd ω_1FBetween the threshold values that selects be determined as 20 ω_1E, so can guarantee Relative error be no more than ± 5%.

In addition, in unactuated joint high-speed rotation, with ω_1GAnd ω_1FBetween partially absolute value of the difference judge friction pulley retreat Whether the friction pulley of feedback mechanism 1-3 skids, when | ω_1G-ω_1FThink that (ε is angular speed deviation for friction pulley skidding when | > ε Threshold value), θ should be made₁=θ_1G, and by the θ in formula (6) update current (n-th) sampling period_1F ⁽ⁿ⁾To eliminate the influence skidded.

When | ω_1G-ω_1FThink that friction pulley is non-slip when |≤ε, this season θ₁=θ_1G.In summary regular, drive lacking closes The motion state of section is fed backIt should be determined by formula (7).

Specific embodiment 5: the climbing truss robot is swung by what natural overhang started as shown in Fig. 6,9 Grabbing bar control method, steps are as follows:

Step 1: the Inverse Kinematics of climbing truss robot.The active joint angle vector for defining robot is θ_S= [θ₂,θ₃,θ₄]^T, x=[x_A,y_A,θ_A]^TFor the pose vector for swimming hand, wherein θ₂、θ₃、θ₄Turn respectively with the wrist joint of support palmistry neighbour Angle, elbow joint corner, the wrist joint corner with trip palmistry neighbour, x_A、y_A、θ_ARespectively swim x-axis coordinate, the y-axis coordinate of hand central point It with the first gripper 1 is support hand in present embodiment, the second gripper 7 is trip hand with the attitude angle of trip hand.The inverse movement of robot It learns shown in equation such as formula (8).

L in formula (8)₁For truss rod (1-18) axis to support palmistry neighbour wrist joint axis at a distance from, l₂For with branch Support distance of the carpal axis of palmistry neighbour to elbow joint (4) axis, l₃For elbow joint (4) axis to the wrist with trip palmistry neighbour The distance of joints axes, l₄For with trip palmistry neighbour wrist joint axis to swim hand center at a distance from, a₁For 2 axis of the first wrist joint To the distance of 6 axis of the second wrist joint, a₂For with trip palmistry neighbour wrist joint axis to truss rod 1-18 at a distance from, a₃For with branch The wrist joint axis of palmistry neighbour is supportted to the distance at trip hand center.a₁、a₂、a₃It is calculated respectively by formula (9), (10), (11).

Need to be open gripper when grabbing bar in view of trip hand alignment target truss rod, and addition constrains item as shown in formula (12) Part, wherein x^d、y^dThe respectively x-axis of target truss rod, y-axis position coordinates, this constraint condition determined trip hand center not with target Attitude angle θ when truss rod center is overlapped_A。

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

When calculating trip hand center and being overlapped with target truss rod center, constraint condition shown in formula (12) fails, at this moment for So that robot is avoided unusual configuration, fixed value, i.e. θ will be set as with the wrist joint angle of trip palmistry neighbour₄=θ₀(θ₀> 0) it, swims at this time Hand attitude angle θ_AIt is calculated by formula (13).

In formula (13), a₄For the distance of trip hand central point to 4 axis of elbow joint, calculated by formula (14).Composite type (8), (12), (13), can be according to both arms hand robot travel hand position coordinate x_A、y_AWith unactuated joint angle θ₁Determine main diarthrodial pass Save angle θ₂、θ₃、θ₄。

Step 2: self-starting stage control.The support hand of climbing truss robot is enabled to be grasped on truss rod 1-18 first, Other joints of robot are stretched, and entire robot keeps static state of dangling.Then robot control system is powered on, After carrying out self-test to the communication state of friction pulley retreat feedback mechanism 1-3, gyroscope 1-14 and each SERVO CONTROL/driver, The elbow joint 4 of robot is enabled to be moved by track shown in formula (15), while the first wrist joint 2, the second wrist joint 6 are kept not It is dynamic, the target position x of support hand folding₁ ^dIt is set as pine and holds position x_s, the target position x of trip hand folding₂ ^dIt is set as deployed position x₀。

θ in formula (15)₃ ^dIt is elbow joint target angle；A is the motion amplitude of Robot elbow joint；t₁For from the self-starting stage Start to carry out the system time that timing obtains；T₁For the run duration in self-starting stage.The self-starting stage is selected at the time of end The phase zero crossing of unactuated joint pendulum motion, shown in criterion such as formula (16).

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

β is that unactuated joint swings the phase angle of movement in formula (16), as shown in figure 8, β is defined as unactuated joint swing Any point is to the line of phase center of curve point and the angle of the phase space longitudinal axis on phase curve, since the central point of phase curve may (0,0) on deviation theory puts and drifts about at any time, and the present invention uses the angle of phase curve near tangent and phase space horizontal axisIt is right It carries out approximation,According to the angular acceleration of unactuated jointAnd angular speedIt is calculated by formula (17).

Step 3: vibrator stage control.Start to carry out the control in vibrator stage after the self-starting stage, the of robot One wrist joint 2, the second wrist joint 6, the first gripper 1, the second gripper 7 remain stationary, the target that elbow joint 4 is provided by formula (18) Position is moved, i.e. 4 rotational angle theta of elbow joint₃With unactuated joint rotational angle theta₁Phase difference remain at 90 °.

Shown in the criterion that the vibrator stage terminates such as formula (19), select in θ₁Maximum value max (θ₁) reach a θ₁ ^f, and At the time of swaying phase angle beta=180 ° of unactuated joint, the speed of Robot elbow joint 4 is zero at this time, can reduce and start Carry out impact when subsequent motion.

A is the safety factor greater than 1 in criterion (19), and effect is the reforwarding after the vibrator stage considers compensation in advance Issuable energy loss in dynamic makes robot remain to reach grasping in unactuated joint angle when starting to grasp target truss rod Target angle θ needed for movement₁ ^f, θ₁ ^fThe first wrist joint 2, the second wrist joint 7 for robot stretch and the second gripper 7 is grabbed The angle of unactuated joint when holding target truss rod is calculated by formula (20).

Step 4: the adjusting stage controls.Adjusting stage starts from the moment terminated in the vibrator stage, adjusting stage inner machine people It is specific to adjust motion profile such as formula (21) institute by the formation adjusting at the end of vibrator to the configuration for being suitable for grasping target truss rod Show, it may be assumed that in T₂By the rotational angle theta of elbow joint 4 in time₃It is smoothly transitted by recurvate-location A to prone θ₃ ^fPosition Set, at the same by with trip palmistry neighbour wrist joint rotational angle theta₄θ is transitted to by 0₄ ^f.Within the adjusting stage, the first gripper 1 of robot, It is remained stationary with wrist joint, the second gripper 7 of support palmistry neighbour.

θ in formula (21)₃ ^f、θ₄ ^fFor by θ₁ ^fWith target truss rod position coordinates (x^d,y^d) substitute into inverse kinematics equation (8) Counted joint of robot angle；b₁It is excessive coefficient, is calculated by formula (22).

T in formula (22)₂For the system time of the timing since the adjusting stage.Selection is owing to drive at the time of adjusting stage terminates Movable joint swaying phase angle beta=90 °-β₀At the time of, wherein β₀For the constant greater than 0.The corresponding unactuated joint in β=90 ° reaches most At the time of high pivot angle and speed are 0 (theoretically the best time that tight truss rod 1-18 is held in support), but in view of support hand from Pine holds position x_sMove to gripping position x_tThe time is needed, i.e., the small resistance that unactuated joint cannot be held from pine at once under actual condition The switching of Buddhist nun's state reaches big damping state, and damping has the process of a rising, therefore the present invention is proposed in unactuated joint phase Parallactic angle terminates the adjusting stage before reaching 90 °, and the target position x of the hand folding of modification support at the end₁ ^d, it is set to hold Position x_t。

Step 5: big damping stage control.Big damping stage is immediately entered after adjusting stage, with support palmistry neighbour's Wrist joint, elbow joint 4, the carpal target position θ with trip palmistry neighbour₂ ^f、θ₃ ^f、θ₄ ^fCalculation formula such as formula (23) shown in.

θ in formula (23)₂ ^*、θ₃ ^*、θ₄ ^*For the θ that will be determined by formula (7)₁With target truss rod position coordinates (x^d,y^d) substitute into it is inverse Counted joint of robot angle in kinematical equation (8)；b₂For the sporting transfer coefficient of smooth robot, based on formula (24) It calculates.

T in formula (24)₃For the system time of the timing since big damping stage；T₃To grasp transit time.Work as t₃>T₃When, Trip hand closure is enabled, its target is opened and closed into position x₂ ^dBy open position x₀It is switched to (pine is held) closed position x_s, complete to target purlin The grasping of hack lever.If grabbing bar movement failure, so that unactuated joint is affranchised and turn round and re-started since the vibrator stage Control, until successfully grasping target stem.

It grabs bar control method according to swinging for above-mentioned climbing truss robot and is controlled, the signal of robot kinematics As indicated with 6, the support hand pine of robot holds truss rod 1-18 to figure under original state, and keeps the stationary state naturally drooped；? Robot is set to begin to deviate from the original state that it is naturally drooped by the movement of elbow joint 4 in the self-starting stage, in unactuated joint Swing phase parallactic angle be switched to the vibrator stage when reaching 0 °；By the movement of elbow joint 4 to system input energy in the vibrator stage, The amplitude of oscillation of unactuated joint is switched to the adjusting stage after reaching requirement；Adjusting stage inner machine people rapidly from vibrator movement at the end of Configuration be transitioned into preparation grasp target truss rod configuration, and start close compact schemes hand；Robot after into big damping stage According to target the position computer device people of truss rod three is main drives diarthrodial target position, where trip hand reaches target truss rod Position after close trip hand, complete to grasp.

Specific embodiment 6: the climbing truss robot is grabbed in target truss rod of completion as shown in Fig. 7,10 Continuing the control method of continuous moving after holding, steps are as follows:

Step 1: the transcription of joint variable.The original state of continuous moving is the first gripper 1 of robot, the second gripper 7 The state of truss rod is hold, the support hand on rear side of robot moving direction will unclamp purlin in continuous moving later Hack lever simultaneously becomes swimming hand, and the trip hand of front side will become supporting hand in continuous moving later and be formed with the truss rod grasped Unactuated joint.Therefore need to carry out the transcription of joint variable, the i.e. corner of the corner of the first wrist joint 1 and the second wrist joint 7 Exchange, the folding distance of the first gripper 1 are exchanged with the folding of the second gripper 7 distance, and the corner of elbow joint 4 is constant.Variable transcription Make always afterwards: x₁Indicate the folding distance of support hand, x₂Indicate the folding distance of trip hand, θ₁Indicate unactuated joint corner (by It is installed on support gyroscope 1-14 on hand and friction pulley retreat feedback mechanism 1-3 is measured), θ₂Indicate the wrist with support palmistry neighbour Joint rotation angle, θ₃Indicate the corner of elbow joint 4, θ₄Indicate the wrist joint corner with trip palmistry neighbour.

Step 2: formation adjusting stage control.The purpose in this stage is the configuration at the end of making robot grab bar from last time It is adjusted to the configuration for being suitble to unclamp trip hand and swing forward again, this object shape should have the feature that and unactuated joint Adjacent wrist joint is stretched to facilitate swing next time, and the vertical posture (θ of You Shoucheng_A=-180 °) to facilitate machine after gripper release Device people can the smooth bottom.Climbing truss robot is formd with two truss rods grasped when the formation adjusting stage starts singly closes 5 linkage of ring, this mechanism is with 2 freedom degrees but robot has 3 main driving joints, therefore corner in the formation adjusting stage θ₂、θ₄Corresponding two wrist joints are moved by planned trajectory, the position servo given value θ of elbow joint corner₃ ^dAccording to θ₂、θ₄ It determines by kinematics, robot configuration is adjusted using track shown in formula (25).It supports hand and swims the folding position of hand It is maintained at pine and holds state, i.e. x₁ ^d=x_sAnd x₂ ^d=x_s。

In formula (25), θ₂ ^h、θ₄ ^hRespectively θ under the original state in formation adjusting stage₂、θ₄Value；θ₂ ^pAnd θ₄ ^pIt respectively indicates θ in this phase targets configuration₂、θ₄Value, θ₂ ^p=0, θ₄ ^pIt is calculated by formula (26)；b₃For transition system used in formation adjusting movement Number is calculated by formula (27).

T in formula (28)₄For the system time of the timing since the formation adjusting stage；T₄For the time of formation adjusting movement.It adjusts Selection is in t at the time of the whole stage terminates₄=T₄At the time of, i.e., at the time of formation adjusting movement is completed.

Step 3: loose bar stage control.This stage immediately begins to after the configuration adjusting stage, θ₂ ^d、θ₃ ^d、θ₄ ^d、x₁ ^dIt protects Hold it is constant, enable trip hand unclamp to deployed position (x₂ ^d=x₀).The differentiation that loose bar is completed can be by judging unactuated joint speed It is no to reach a certain threshold value to carry out, and the movement of next stage is opened too early when swimming hand and not yet completely disengaging truss rod in order to prevent It is dynamic, a time delay condition is added, therefore the criterion such as formula (28) that the loose bar stage terminates is shown.

In formula (28)For the unactuated joint minimum speed for assert loose bar completion, t₅For the timing since the loose bar stage System time；T₅For the minimum loose bar time.

Step 4: the stage control that swings.This stage gets started after the loose bar stage, in the stage inner support that swings Hand, swim hand folding freedom degree and with support palmistry neighbour wrist joint remain stationary, elbow joint 4 and with swim palmistry neighbour wrist Joint will move to the position for preparing to grasp in position when once swinging in the time by loose bar of unactuated joint from the front to the back. Since the active joint of robot in the loose bar stage remains stationary, the diarthrodial position of robot master at the end of the loose bar stage It is also maintained at the position at the end of the formation adjusting stage, is at this time trajectory planning in formula (25) with the wrist joint corner of trip palmistry neighbour Terminal θ₄ ^p, and the corner of elbow joint is denoted as θ₃ ^p.Swing elbow joint 4 and the carpal target with trip palmistry neighbour in the stage Corner is identical as the target rotation angle of adjusting stage in specific embodiment five, respectively θ₃ ^f、θ₄ ^f.Then elbow joint 4 and with trip palmistry Shown in adjacent carpal motion profile such as formula (29).

B in formula (29)₁It is the excessive coefficient of movement of swinging, is calculated by formula (30).

In formula (30), t₅For the system time of the timing since the stage of swinging；T₅Swing time of movement, according to multiple reality It is determining to test the average time that middle unactuated joint swings primary from the front to the back.Swing the selection and specific implementation of finish time in stage The selection mode of finish time adjusting stage is identical in mode five, is selected in unactuated joint swaying phase angle beta=90 °-β₀'s Moment holds position x to concede support hand in advance from pine_sMove to gripping position x_tNeed the time.It swings and modifies branch at the end of the stage The target for supportting hand opens and closes position x₁ ^d, it is set to gripping position x_t。

Step 5: big damping stage control.Big damping stage when continuous moving is actuated for from natural overhang The big damping stage for grabbing bar is identical, refers to the step five in specific embodiment five.After grabbing bar, if needing to continue Next target truss rod is grasped forward, then is restarted from step 1, and the company that then can be achieved in truss structure is looped back and forth like this Continuous movement.

Specific embodiment 7: emulation testing has been carried out to the above-mentioned vibrator stage control grabbed in bar control method, And the vibrator simulation result for direct applied energy being pumped into method compares.Simulated environment by Matlab/Simulink software and Adams software joint is established, and wherein Matlab/Simulink software is responsible for the control of robot, and Adams software is responsible for robot Solid modelling and Mechanics Simulation.To five kinds of situations point of unactuated joint damped coefficient c=0,29,58,87,106 (Nms/ °) Vibrator control emulation has not been carried out, is pumped into method point using by the control method proposed by the present invention based on phase difference and applied energy The unactuated joint amplitude curve not obtained is as shown in figure 11.

In Figure 11, no matter how damped coefficient c changes, and can obtain height always by vibrator control method proposed by the present invention The unactuated joint amplitude curve of method is pumped into energy；All amplitude curves obtained by vibrator control method proposed by the present invention All show the feature that speedup is successively decreased；In the case where unactuated joint has friction (c > 0), by vibrator control proposed by the present invention The unactuated joint amplitude curve that method processed obtains is smaller relative to the fall of no friction condition (c=0).Above-mentioned phenomenon is said It is bright: in -90 ° < θ of emulation testing₁Within the scope of < 90 °, the vibrator control method proposed by the present invention based on phase difference has faster Vibrator speed, the amplitude of unactuated joint is easier to control, and can preferably overcome and be brought by unactuated joint friction Influence.

Specific embodiment 8: as shown in figure 12, grabbing bar control using the climbing truss robot and described swinging Method has carried out the target truss rod of different distance to grab bar experiment, and the range of target truss rod distance is for 0.4m~1m (machine The 28.5%~69.4% of device robot mechanism spread length), a target stem distance (totally 7 is selected every 0.1m within this range Different target bar distance) respectively carry out 5 repetition tests.It is the target truss rod adjusted the distance respectively as 0.4m and 1m as shown in figure 13 The joint of robot curve movement of each one group of grasp experiments, Figure 14 give the video recording screenshot that should be tested twice in Figure 13.Institute As soon as 35 groups of experiments having completely can swing to grab in the bar period and be succeeded, that is, it is not required to carry out the additional vibrator stage Target truss rod can be grasped with 100% success rate, it should be noted that adjust the distance is more than for 0.8m, 0.9m, 1.0m tri- The target truss rod of robot mechanism spread length 50%, the climbing truss robot and its swings and grabs bar control method and remain to Reach 100% success rate.

Specific embodiment 9: to verify the validity of proposed continuous moving motion control method, in Adams software The continuous moving simulated environment with 4 truss rods is inside established, climbs the virtual prototype of truss robot in the initial state Naturally it overhangs below the first truss rod, the spacing of every truss rod is 0.6m.As shown in figure 15, by (a), (b) ..., (l) Sequentially, climbing truss robot grabs bar control method by swinging of starting of natural overhang using what is proposed in the present invention first Make to swim hand the second truss rod of grasping, successively unclamps the first truss rod by the continuous moving control method proposed later and grasp third Truss rod unclamps the second truss rod the 4th truss rod of grasping, completes a complete continuous moving period, last robot unclamps Third truss rod simultaneously replys nature overhang.Entire motion process time-consuming 90s, completes and once grabs from what overhang started Bar and twice continuous moving grab bar, and robot moves 1.8m altogether.From above-mentioned simulation process: the slave overhang proposed Swinging for starting is grabbed bar method and continuous moving method and can be smoothly combined together, and can complete climbing truss robot Mobile task in truss structure.

As shown in figure 16, it is carried out continuously after having grasped truss rod to the climbing truss robot both hands proposed in the present invention Mobile control method has carried out experimental verification, due to the robot of exchange support hand and You Shouhou every half of continuous moving period Move it is all the same, the present invention in only carried out the half period continuous moving experiment.The original state of experiment be robot grasp away from From the truss rod for 1.0m and have been carried out formation adjusting movement after state, therefore test in robot directly from loose bar rank Section setting in motion, the target truss rod distance to be grasped is respectively 0.5m, 0.6m, 0.7m, 0.8m, and each distance is repeated three Secondary experiment gives the continuous moving experiment video recording adjusted the distance and grab bar for 0.5m and 0.8m target truss rod in Figure 16 and cuts Figure, Figure 17 is the joint angular curve that this is tested twice.All 12 continuous moving experiments, which only need to once swing and switch, hinders greatly Buddhist nun, which just completes, grabs bar, i.e., success rate is 100%, the continuous moving control for illustrating designed climbing truss robot and being proposed Method processed, which has, to be fulfiled assignment the ability of task in actual working environment with high-reliability.

All parameters or the meaning of variable are referring to table one in the present invention.

One parameter of table, variable-definition table

Claims (8)

1. A climbing truss robot capable of swinging and grasping a long-distance truss bar, characterized in that, said climbing truss robot includes a first claw (1), a first wrist joint (2), a first connecting rod ( 3), the elbow joint (4), the second connecting rod (5), the second wrist joint (6) and the second pawl (7); the first paw (1) connects with the first wrist joint (2) One end of a connecting rod (3) is rotationally connected, the other end of the first connecting rod (3) is rotationally connected with one end of the second connecting rod (5) through the elbow joint (4), and the second claw (7) is connected through the second The wrist joint (6) is rotationally connected with the other end of the second connecting rod (5); the first claw (1) and the second claw (7) have the same structure; The first gripper (1) consists of a fixed half gripper (1-1), a friction wheel retreat feedback mechanism (1-3), a movable gripper slide rail (1-4), a ball screw (1-5), and a gripper Frame (1-6), connecting flange (1-7), servo motor encoder (1-8), DC servo motor (1-9), servo motor reducer (1-10), driving gear (1 -11), driven gear (1-12), movable claw slider (1-13), gyroscope (1-14), movable half claw (1-15), and nut (1-19); The servo motor encoder (1-8), the DC servo motor (1-9), and the servo motor reducer (1-10) are installed together sequentially, and the driving gear (1-11) is fixed on the servo motor reducer (1 -10) on the output shaft, the driven gear (1-12) meshes with the driving gear (1-11) and is fixed on the shaft end of the ball screw (1-5), and the ball screw (1-5) is driven by the bearing Position on the claw frame (1-6), and the nut (1-19) set on the ball screw (1-5) is fixed by the movable claw slider (1-13) and the movable half claw (1-15). Even, the outer surfaces of the moving half-claw (1-15) and the fixed half-claw (1-1) are equipped with a gyroscope (1-14) and a friction wheel retreat feedback mechanism (1-3), and the friction wheel The friction wheel (1-3-2) of the reverse rotation feedback mechanism (1-3) is pressed on the truss rod (1-18) by the springs (1-3-1) on both sides, and the fixed half claw (1-1 ), the movable claw slide rail (1-4) and the servo motor reducer (1-10) are all fixed on the claw frame (1-6), and the claw frame (1-6) is also provided with a connecting method Flange (1-7), through connecting flange (1-7), hand claw can be connected with the end flange of first wrist joint (2) or second wrist joint (6). 2. The climbing truss robot according to claim 1, characterized in that: the first gripper (1) also includes a fixed claw bushing (1-2), a movable claw bushing (1-16), and a movable claw bushing (1-16). The rectangular grooves inside the half claws (1-15) and the fixed claws (1-1) are equipped with moving claw bushings (1-16) and fixed claw bushings (1-2) one by one. 3. The climbing truss robot according to claim 2, characterized in that: the first gripper (1) also includes a friction material layer (1-17), a movable claw bushing (1-16) and a fixed claw A friction material layer (1-17) is pasted in the semicircular groove inside the bushing (1-2). 4. The climbing truss robot according to claim 1, 2 or 3, characterized in that: the climbing truss robot also includes a host computer and the first claw (1), the first wrist joint (2), A control system composed of elbow joint (4), second wrist joint (6), and servo controllers/drivers of the second claw (7), five servo controllers/drivers and DC servo motors of the first claw (1), The DC servo motor of the first wrist joint (2), the DC servo motor of the elbow joint (4), the DC servo motor of the second wrist joint (6), and the DC servo motor of the second claw (7) correspond one-to-one; The upper computer communicates with the master node servo control/driver that drives and controls the DC servo motor (1-9) in the first gripper (1) through the USB interface, and the master node servo control/driver communicates with the other four servo control/drivers through the CAN bus. Driver communication; the friction wheel retreat feedback mechanism (1-3) and gyroscope (1-14) communicate with the host computer through the serial port, and the host computer sends the control command containing the target position of the active joint to the master node servo through the USB interface The control/driver, the master node servo control/driver sends the received control command to the corresponding servo control/driver through the CAN bus. 5. climbing truss robot according to claim 4, is characterized in that, in described control system, upper computer can install a CAN-PCI board card additionally on PCI slot, by this board card directly and all Servo control/drivers communicate; when the host computer uses the CAN-PCI board, the host computer sends motion commands in sequence according to the node numbers of the servo controls/drivers, and directly exchanges data with each servo control/driver; all servo controls/drivers After receiving the control command, the drive synchronizes according to the synchronization signal inside the CAN bus, and controls the position or torque of the respective DC servo motors. 6. A control method for a climbing truss robot according to claim 1, 2, 3, 4 or 5, wherein the implementation of the method comprises: The process of generating the motion state of the underactuated joint: According to the feedback data of the friction wheel retreat feedback mechanism (1-3) and the gyroscope (1-14) obtained through the serial port, the upper computer generates the motion state feedback of the underactuated joint The steps are as follows: Step 1. According to the fixed sampling period T _S , read the data of the friction wheel retreat feedback mechanism (1-3) and the gyroscope (1-14) through the serial port of the host computer, and convert the obtained binary number into a decimal number. The angle of rotation of the photoelectric encoder obtained by the friction wheel retreat feedback mechanism (1-3) is θ, and the rotational angular velocity of the claw obtained by the gyroscope (1-14) is ω; Step 2, only consider the feedback data θ of the friction wheel retreat feedback mechanism (1-3) to calculate the underactuated joint rotation angle θ _1F and angular velocity ω _1F ; represent the reduction ratio of the friction wheel retreat feedback mechanism (1-3) with I _F , then θ _1F calculated according to the rotation angle θ is shown in formula (1); θ _1F ＝θ/I _F (1) Let θ _1F ⁽ⁿ⁾ represent the θ _1F calculated in the nth sampling period, then in the nth sampling period, only consider the underactuated joint angular velocity ω obtained from the feedback data of the friction wheel retreat feedback mechanism (1-3) _1F ⁽ⁿ⁾ can be calculated according to formula (2); Step 3, only consider the feedback data ω of the gyroscope (1-14) to calculate the underactuated joint rotation angle θ _1G and angular velocity ω _1G ; since the gyroscope is installed on the robot claw forming the underactuated joint, the gyroscope (1-14 ) directly measure the rotational speed of the underactuated joint, that is, ω _1G = ω; use ω _1G ⁽ⁿ⁾ to represent the ω _1G obtained in the nth sampling period, when only the feedback data of the gyroscope (1-14) is considered, the nth The underactuated joint rotation angle θ _1G ⁽ⁿ⁾ of a sampling period can be calculated according to formula (3); In formula (3), θ _1G ⁽⁰⁾ is the rotation angle of the underactuated joints in the initial state when the climbing truss robot is powered on. This initial state is the state in which the robot grasps the truss rod with one hand, and the main drive joints are stretched and drooped naturally. , namely θ _1G ⁽⁰⁾ ＝0°; Step 4: Comprehensively consider θ _1F and ω _1F calculated from the feedback data of the friction wheel retreat feedback mechanism (1-3), and θ _1G and ω _1G calculated from the feedback data of the gyroscope (1-14), determine the Motion status feedback of drive joints According to the line number _NF of the photoelectric encoder of the friction wheel retrograde feedback mechanism (1-3), the quadrature pulse signal of the photoelectric encoder has been subdivided when considering the reading, then the friction wheel retrograde feedback mechanism ( 1-3) The maximum truncation error θ _1E that may have in the θ _1F calculated by the feedback data can be calculated by formula (4); It can _be known from formula (2) that the error ω 1E introduced by the truncation error θ _1E in the angular velocity ω _1F is Under the condition of low-speed rotation of the underactuated joint, the angular velocity ω _1G calculated from the feedback data of the gyroscope (1-14) should be used as the angular velocity of the underactuated joint The angular velocity ω _1F obtained by the friction wheel retreat feedback mechanism (1-3) when the underactuated joint rotates at high speed, the angular velocity of the underactuated joint The threshold selected between ω _1G and ω _1F is determined to be 20ω _1E to ensure The relative error is not more than ±5%; When the underactuated joint rotates at high speed, the absolute value of the deviation between ω _1G and ω _1F is used to judge whether the friction wheel slips in the friction wheel retreat feedback mechanism (1-3), when |ω _1G -ω _1F |&gt;ε When it is considered that the friction wheel (1-3-2) is slipping (ε is the threshold of angular velocity deviation), θ ₁ = θ _1G should be set, and θ _1F ⁽ⁿ ) of the current (nth) sampling period should be updated according to formula (6) to eliminate the effects of slipping; When |ω _1G -ω _1F |≤ε, it is considered that the friction wheel (1-3-2) does not slip, and at this time θ ₁ = θ _1G ; based on the above rules, the motion state feedback of the underactuated joint It should be determined according to formula (7): 7. the control method of climbing truss robot according to claim 6, is characterized in that, the realization process of described method also comprises: The control process of the swing grab bar activated by the natural hanging state: Step 1. Inverse kinematics modeling of the climbing truss robot: define the active joint angle vector of the robot as θ _S =[θ ₂ ,θ ₃ ,θ ₄ ] ^T , x=[x _A ,y _A ,θ _A ] ^T is the pose vector of the free hand, where θ ₂ , θ ₃ , and θ ₄ are respectively the wrist joint rotation angle adjacent to the supporting hand, the elbow joint rotation angle, and the wrist joint rotation angle adjacent to the free hand, x _A , y _A , and θ _A are respectively the x-axis coordinates and y-axis coordinates of the center point of the wandering hand and the attitude angle of the wandering hand, with the first paw (1) as the supporting hand and the second paw (7) as the wandering hand; the inverse kinematics equation of the robot As shown in formula (8): In formula (8), l ₁ is the distance from the axis of the truss rod (1-18) to the axis of the wrist joint adjacent to the supporting hand, and l ₂ is the axis of the wrist joint adjacent to the supporting hand to the axis of the elbow joint (4) l ₃ is the distance from the axis of the elbow joint (4) to the axis of the wrist joint adjacent to the player, l ₄ is the distance from the axis of the wrist joint adjacent to the player to the center of the player, a ₁ is the first wrist The distance from the axis of the joint (2) to the axis of the second wrist joint (6), a ₂ is the distance from the axis of the wrist joint adjacent to the free hand to the truss bar (1-18), and a ₃ is the distance from the axis of the wrist adjacent to the supporting hand The distance from the joint axis to the center of the hand; a ₁ , a ₂ , and a ₃ are calculated according to formulas (9), (10), and (11), Considering that the opening of the claw needs to be aligned with the target truss bar when grabbing the bar freely, the constraint conditions shown in formula (12) are added, where x ^d and y ^d are the x-axis and y-axis position coordinates of the target truss bar respectively, This constraint condition determines the attitude angle θ _A when the center of the free hand does not coincide with the center of the target truss bar; θ _A ＝π/2+arctan[(y ^d -y _A )/(x ^d -x _A )] (12) When the center of the calculated free hand coincides with the center of the target truss bar, the constraints shown in Equation (12) are invalid. At this time, in order to make the robot avoid the singular configuration, the angle of the wrist joint adjacent to the free hand is set to be fixed value, that is, θ ₄ = θ ₀ (θ ₀ &gt; 0), at this time, the attitude angle θ _A of the swimmer is calculated according to formula (13): In formula (13), a ₄ is the distance from the center point of the player’s hand to the axis of the elbow joint (4), which is calculated according to formula (14); comprehensive formulas (8), (12), and (13) can be calculated according to the dual-arm hand robot The free hand position coordinates x _A , y _A and the underactuated joint angle θ ₁ determine the joint angles θ ₂ , θ ₃ , θ ₄ of the active joints; Step 2. Self-starting stage control: first, make the supporting hand of the climbing truss robot grasp on the truss rod (1-18), the other joints of the robot are straightened, and the whole robot remains in a suspended state; Electricity, after self-inspection is carried out to the communication status of friction wheel retreating feedback mechanism (1-3), gyroscope (1-14) and each servo control/driver, make the elbow joint (4) of robot according to formula (15) The trajectory shown is moving, while the first wrist joint (2) and the second wrist joint (6) remain motionless, the target position x ₁ ^d of the supporting hand opening and closing is set as the loose grip position x _s , and the target position of the free hand opening and closing Position x ₂ ^d is set to open position x ₀ ; In formula (15), θ ₃ ^d is the target angle of the elbow joint; A is the range of motion of the elbow joint of the robot; t ₁ is the system time obtained from timing from the self-starting stage; T ₁ is the movement time of the self-starting stage; The moment of the end of the phase is selected at the zero-crossing point of the phase of the swing motion of the underactuated joint, and the judgment conditions are shown in formula (16): t ₁ ≥T ₁ &β=0 (16) In formula (16), β is the phase angle of the swing motion of the underactuated joint, and β is defined as the angle between the line connecting any point on the swing phase curve of the underactuated joint to the center point of the phase curve and the vertical axis of the phase space. Since the center of the phase curve The point may deviate from the theoretical (0,0) point and drift over time, using the angle between the tangent of the phase curve and the horizontal axis of the phase space to approximate it, According to the angular acceleration of the underactuated joint and angular velocity Calculate according to formula (17): Step 3, control of the vibration phase: the control of the vibration phase begins after the start-up phase, the first wrist joint (2), the second wrist joint (6), the first claw (1), and the second claw ( 7) keep still, the elbow joint (4) moves according to the target position given by formula (18), that is, the phase difference between the elbow joint (4) rotation angle θ ₃ and the underactuated joint rotation angle θ ₁ is always kept at 90°; The judging condition for the end of the excitation phase is shown in formula (19). Select the moment when the maximum value max(θ ₁ ) of θ ₁ reaches aθ ₁ ^f and the swing phase angle β of the underactuated joint is 180°. At this time, the robot elbow The velocity of the joint (4) is zero, which can reduce the impact when starting the follow-up movement; In the discriminant condition (19), a is an insurance factor greater than 1. Its function is to consider and compensate the energy loss that may occur in the subsequent motion in advance during the excitation stage, so that the underactuated joint angle of the robot can still reach the grasping point when the robot starts to grasp the target truss rod. The target angle θ ₁ ^f required for the grip movement, θ ₁ ^f is the time when the first wrist joint (2) and the second wrist joint (7) of the robot are straightened and the second claw (7) grasps the target truss rod The angle of the driving joint is calculated according to formula (20): Step 4. Adjustment phase control: The adjustment phase starts at the moment when the excitation phase ends, and the robot is adjusted from the configuration at the end of the excitation to a configuration suitable for grasping the target truss rod in the adjustment phase. The specific adjustment trajectory is as follows: (21 ), that is, within T ₂ time, the angle θ ₃ of the elbow joint (4) is smoothly transitioned from the backward-bending -A position to the forward-bending θ ₃ ^f position, and at the same time, the wrist adjacent to the free hand The joint rotation angle θ ₄ transitions from 0 to θ ₄ ^f ; during the adjustment phase, the robot's first claw (1), the wrist joint adjacent to the supporting hand, and the second claw (7) remain motionless; In formula (21), θ ₃ ^f and θ ₄ ^f are the robot joint angles calculated by substituting θ ₁ ^f and target truss rod position coordinates (x ^d , y ^d ) into inverse kinematics equation (8); b ₁ is the transition coefficient , calculated according to formula (22): In formula (22), _t2 is the system time counted from the adjustment stage; the moment at the end of the adjustment stage is selected at the moment when the underactuated joint swing phase angle β=90° _-β0 , where _β0 is a constant greater than 0; β =90° corresponds to the moment when the underactuated joint reaches the highest swing angle and the speed is 0, the adjustment phase ends before the phase angle of the underactuated joint reaches 90°, and at the end, modify the target position x ₁ ^d of the opening and closing of the supporting hand, and set it to Set as the grip position x _t ; Step 5. Large damping stage control: immediately enter the large damping stage after the adjustment stage, 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 θ ₂ ^f , θ ₃ The calculation formulas of ^f and θ ₄ ^f are shown in formula (23): In formula (23), θ ₂ ^* , θ ₃ ^* and θ ₄ ^* are calculated by substituting θ ₁ determined by formula (7) and target truss bar position coordinates (x ^d , y ^d ) into inverse kinematics equation (8). Robot joint angle; b ₂ is the transition coefficient for smooth robot motion, calculated according to formula (24): In formula (24), t ₃ is the system time counted from the stage of large damping; T ₃ is the gripping transition time; when t ₃ &gt; T ₃ , the hand is closed, and its target opening and closing position x ₂ ^d The open position x ₀ is switched to the closed position x _s to complete the grasping of the target truss bar; if the movement of the grasping bar fails, the underactuated joint is restored to free rotation and the control is resumed from the excitation stage until the target bar is successfully grasped. 8. the control method of climbing truss robot according to claim 7, is characterized in that, the realization process of described method also comprises: The climbing truss robot continues the control process of continuous movement after completing a target truss rod grasp: Step 1. Transcription of joint variables: the initial state of continuous movement is the state where the robot’s first gripper (1) and second gripper (7) are both holding the truss rod, and the supporting arm located on the rear side of the robot’s moving direction In subsequent successive movements the loose hand will loosen the truss rod and become a free hand, the front free hand will then become the support hand and form an underactuated joint with the grasped truss rod in the subsequent continuous movement; hence the need for articulation The transliteration of variables, that is, the rotation angle of the first wrist joint (1) is exchanged with the rotation angle of the second wrist joint (7), and the opening and closing distance of the first claw (1) and the opening and closing distance of the second claw (7) In exchange, the rotation angle of the elbow joint (4) remains unchanged; after the variable is transcribed, x ₁ represents the opening and closing distance of the supporting hand, x ₂ represents the opening and closing distance of the free hand, θ ₁ represents the rotation angle of the underactuated joint, θ ₂ represents The rotation angle of the wrist joint adjacent to the supporting hand, θ ₃ represents the rotation angle of the elbow joint (4), and θ ₄ represents the rotation angle of the wrist joint adjacent to the free hand; Step 2. Configuration adjustment stage control: In this stage, the two wrist joints corresponding to the rotation angles θ ₂ and θ ₄ move according to the planned trajectory, and the position servo given value θ ₃ ^d of the rotation angle of the elbow joint moves according to θ ₂ and θ ₄ Determined scientifically, use the trajectory shown in formula (25) to adjust the configuration of the robot; the opening and closing positions of the supporting hand and the free hand are kept in the loose grip state, that is, x ₁ ^d =x _s and x ₂ ^d =x _s ; In formula (25), θ ₂ ^h and θ ₄ ^h are the values of θ ₂ and θ ₄ in the initial state of the configuration adjustment stage; θ ₂ ^p and θ ₄ ^p respectively represent the values of θ ₂ and θ The value of ₄ , θ ₂ ^p = 0, θ ₄ ^p is calculated according to formula (26); b ₃ is the transition coefficient used in configuration adjustment exercise, calculated according to formula (27): In formula (28), _t4 is the system time counted from the configuration adjustment stage; _T4 is the time of the configuration adjustment movement; the moment of the end of the adjustment stage is selected at the moment of _t4 = _T4 , that is, the configuration adjustment movement is completed moment; Step 3. Control of lever loosening stage: this stage starts immediately after the configuration adjustment stage ends, θ ₂ ^d , θ ₃ ^d , θ ₄ ^d , x ₁ ^d remain unchanged, so that the free hand is released to the open position (x ₂ ^d ＝x ₀ ); the discriminant condition for the end of the loose rod stage is shown in formula (28): In formula (28) In order to determine the minimum speed of the underactuated joint after the release of the rod, t ₅ is the system time counted from the release stage; T ₅ is the minimum release time of the rod; Step 4. Swing stage control: This stage starts immediately after the release stage. During the swing stage, the opening and closing degrees of freedom of the supporting hand and the free hand, as well as the wrist joints adjacent to the supporting hand, remain stationary. The elbow joint (4 ) and the wrist joint adjacent to the free hand will move from the position when the bar is released to the position ready to grasp within one swing time of the underactuated joint from back to front; The joint rotation angle is the end point θ ₄ ^p of the trajectory planning in formula (25), and the elbow joint rotation angle is recorded as θ ₃ ^p ; the target rotation angle of the elbow joint (4) and the wrist joint adjacent to the free hand in the swing phase is related to the right The target rotation angles in the adjustment stage in Requirement 7 are the same, which are θ ₃ ^f and θ ₄ ^f respectively; then the motion trajectories of the elbow joint (4) and the wrist joint adjacent to the free hand are shown in formula (29): In the formula (29), _b1 is the transition coefficient of the swing motion, calculated according to the formula (30): In formula (30), _t5 is the system time counted from the swing phase; the time of _T5 swing motion is determined according to the average time of the underactuated joint swinging from back to front in many experiments; the selection of the end time of the swing phase It is the same as the selection method of the end time of the adjustment phase in claim 7, all selected at the time when the underactuated joint swing phase angle β=90°-β ₀ ; at the end of the swing phase, the target opening and closing position x ₁ ^d of the support hand is modified, and the which is set to the grip position x _t ; Step 5. Large damping stage control: The large damping stage of continuous movement is exactly the same as the large damping stage of grabbing the bar starting from the natural hanging state. Please refer to step 5 in the control process of the swing grabbing bar started from the natural hanging state; After the grab bar is finished, if it is necessary to continue to grab the next target truss bar forward, start again from step 1, so that the continuous movement in the truss structure can be realized by repeating this cycle.