CN109048961A - It can swing and grasp the climbing truss robot and its control method of remote truss rod - Google Patents

Abstract

It can swing and grasp the climbing truss robot and its control method of remote truss rod, it is related to a kind of robot and its control method.The present invention be solves the problems, such as existing truss climbing robot can not save grip farther out truss rod with continuous moving can not be carried out in truss structure.Friction material is increased on the inside of the gripper of robot to provide bigger damping torque；Gyro sensor is increased on gripper can eliminate the influence that friction pulley skids when high speed swings；And give the feedback data for comprehensively considering friction pulley retreat feedback mechanism and gyroscope, the method for generating unactuated joint motion state feedback.Propose the vibrator control method based on phase difference, and it proposes and a kind of bar control method is grabbed by swinging of starting of natural overhang, and a kind of continuous moving control method, bar experiment and continuous moving emulation and experiment are grabbed in swinging for start from natural overhang, demonstrate the validity of proposed method.The present invention is applied to climbing truss robot field.

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 kind of can swing grasps the climbing truss robot of remote truss rod, which is characterized in that the climbing truss machine People includes the first 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 rotated by the one end of the first wrist joint (2) and first connecting rod (3) to be connected It connects, the other end of first connecting rod (3) is rotatablely connected by the one end of elbow joint (4) and the second connecting rod (5), the second gripper (7) it is rotatablely connected by the other end of the second wrist joint (6) and the second connecting rod (5)；First gripper (1) and the second gripper (7) Structure it is identical；

First gripper (1) retreats feedback mechanism (1-3), pawl sliding rail (1-4), ball screw by fixed half pawl (1-1), friction pulley (1-5), gripper rack (1-6), connecting flange (1-7), servomotor encoder (1-8), DC servo-motor (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), nut (1-19) composition, are moved；

Servomotor encoder (1-8), DC servo-motor (1-9), servomotor speed reducer (1-10) three are successively It is installed together, driving gear (1-11) is fixed on the output shaft of servomotor speed reducer (1-10), driven gear (1- 12) shaft end of ball screw (1-5) is engaged and is fixed on driving gear (1-11), and ball screw (1-5) is by bearing in gripper It is positioned in rack (1-6), the nut (1-19) being set on ball screw (1-5) passes through pawl sliding block (1-13) and dynamic half pawl (1- 15) it is connected, corresponds on the lateral surface of dynamic half pawl (1-15) and fixed half pawl (1-1) and moved back equipped with gyroscope (1-14) and friction pulley Turn feedback mechanism (1-3), the friction pulley (1-3-2) that friction pulley retreats feedback mechanism (1-3) is pressed by the spring (1-3-1) of two sides Tightly on truss rod (1-18), fixed half pawl (1-1), pawl sliding rail (1-4) and servomotor speed reducer (1-10) are each attached to In gripper rack (1-6), connecting flange (1-7) is additionally provided in gripper rack (1-6), it can be by gripper by connecting flange (1-7) It is connect with the end flange of the first wrist joint (2) or the second wrist joint (6).

2. climbing truss robot according to claim 1, it is characterised in that: first gripper (1) further includes determining pawl Bushing (1-2), pawl bushing (1-16) are corresponded in the rectangular channel on the inside of dynamic half pawl (1-15) and calmly half pawl (1-1) and are equipped with Pawl bushing (1-16) and determine pawl bushing (1-2).

3. climbing truss robot according to claim 2, it is characterised in that: first gripper (1) further includes friction Material layer (1-17), pawl bushing (1-16) and determines to post friction material layer (1-17) in half slot on the inside of pawl bushing (1-2).

4. climbing truss robot according to claim 1,2 or 3, it is characterised in that: the climbing truss robot is also Including by host computer and the first gripper (1), the first wrist joint (2), elbow joint (4), the second wrist joint (6), the second gripper (7) SERVO CONTROL/driver composition control system, the DC servo of five SERVO CONTROL/drivers and the first gripper (1) are electronic Machine, the DC servo-motor of the first wrist joint (2), the DC servo-motor of elbow joint (4), the second wrist joint (6) it is straight Flow servomotor, the DC servo-motor of the second gripper (7) corresponds；

The host node servo that host computer passes through DC servo-motor (1-9) in USB interface and driving and control the first gripper (1) Control/driver communication, host node SERVO CONTROL/driver are logical by CAN bus and four additional SERVO CONTROL/driver News；Friction pulley retreat feedback mechanism (1-3) and gyroscope (1-14) are communicated with host computer by serial ports, and host computer will contain The control instruction of main diarthrodial target position is sent to host node SERVO CONTROL/driver, host node servo by USB interface The control instruction received is sent to corresponding SERVO CONTROL/driver by CAN bus by control/driver.

5. climbing truss robot according to claim 4, which is characterized in that in the control system, host computer can It installs one piece of CAN-PCI board additional on PCI slot, is directly communicated with all SERVO CONTROL/drivers by this board； 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；All SERVO CONTROL/drivers are controlled after instruction by CAN Synchronization signal inside bus synchronizes, and carries out position or Torque Control to respective DC servo motor.

6. the control method of a kind of claim 1,2,3,4 or the 5 climbing truss robots, which is characterized in that the method The realization process includes:

The process that unactuated joint motion state generates: according to friction pulley retreat feedback mechanism (1-3) obtained by serial ports and top The feedback data of spiral shell instrument (1-14), host computer generate the motion state feedback of unactuated jointThe step of it is as follows:

Step 1: according to fixed sampling period T_SBy host computer serial ports to friction pulley retreat feedback mechanism (1-3) and gyroscope (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) obtains；

Step 2: only considering that the feedback data θ of friction pulley retreat feedback mechanism (1-3) calculates unactuated joint rotational angle theta_1FWith angle speed Spend ω_1F；Use I_FIndicate the reduction ratio of friction pulley retreat feedback mechanism (1-3), then the θ 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 feedback The unactuated joint angular velocity omega that the feedback data of 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；Due to Gyroscope is installed on the robot hand to form unactuated joint, therefore 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 the corner for climbing unactuated joint under original state when truss robot powers on, this original state is 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 (1-3)_1F、ω_1F, and by gyro The counted θ of feedback data of instrument (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, to the orthogonal pulses signal of this photoelectric encoder when considering to read Four subdivisions are carried out, then by the counted θ of feedback data of friction pulley retreat feedback mechanism (1-3)_1FThe middle maximum that may have is cut Disconnected error theta_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

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 (1-14)_1GMake For the angular speed of unactuated jointIt is obtained in unactuated joint high-speed rotation by friction pulley retreat feedback mechanism (1-3) Angular velocity omega_1F, in the angular speed of unactuated jointIn ω_1GAnd ω_1FBetween the threshold values that selects be determined as 20 ω_1E, to guarantee Relative error be no more than ± 5%；

In unactuated joint high-speed rotation, with ω_1GAnd ω_1FBetween partially absolute value of the difference judge friction pulley retreat feedback mechanism Whether the friction pulley of (1-3) skids, when | ω_1G-ω_1FThink that (ε is angular speed deviation for friction pulley (1-3-2) 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 (1-3-2) is non-slip when |≤ε, this season θ₁=θ_1G；In summary regular, drive lacking The motion state in joint is fed backIt should be determined by formula (7):

7. the control method of climbing truss robot according to claim 6, which is characterized in that the realization of the method Journey 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 θ₂、θ₃、θ₄Respectively with support palmistry neighbour wrist joint corner, Elbow joint corner, the wrist joint corner with trip palmistry neighbour, x_A、y_A、θ_ARespectively swim the x-axis coordinate of hand central point, y-axis coordinate and The attitude angle of hand is swum, is support hand with the first gripper (1), the second gripper (7) is trip hand；The inverse kinematics equation such as formula of robot (8) shown in:

L in formula (8)₁For truss rod (1-18) axis to support palmistry neighbour wrist joint axis at a distance from, l₂For with support hand Distance of the adjacent carpal axis to elbow joint (4) axis, l₃For elbow joint (4) axis to the wrist joint with trip palmistry neighbour The distance of axis, l₄For with trip palmistry neighbour wrist joint axis to swim hand center at a distance from, a₁It is arrived for the first wrist joint (2) axis The distance of the second wrist joint (6) axis, a₂For with trip palmistry neighbour wrist joint axis to truss rod (1-18) at a distance from, a₃For with Support the wrist joint axis of 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, add the constraint condition as shown in formula (12), Middle 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 purlin Attitude angle θ when hack lever 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 shown in formula (12) fails, at this moment to make Robot avoids unusual configuration, will be set as fixed value, i.e. θ with the wrist joint angle of trip palmistry neighbour₄=θ₀(θ₀> 0) hand, is swum at this time Attitude angle θ_AIt is calculated by formula (13):

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

Step 2: self-starting stage control: enabling the support hand of climbing truss robot be grasped on truss rod (1-18) first, machine Other joints of device people are stretched, and entire robot keeps static state of dangling；Then robot control system is powered on, right The communication state that friction pulley retreats feedback mechanism (1-3), gyroscope (1-14) and each SERVO CONTROL/driver carries out self-test Afterwards, 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) it remains stationary, 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 opening Position x₀；

θ in formula (15)₃ ^dIt is elbow joint target angle；A is the motion amplitude of Robot elbow joint；t₁For since the self-starting stage Carry out the system time that timing obtains；T₁For the run duration in self-starting stage；The self-starting stage is selected in deficient drive at the time of end The phase zero crossing of movable 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 any one on unactuated joint swing phase curve Point is to the line of phase center of curve point and the angle of the phase space longitudinal axis, due on the possible deviation theory of the central point of phase curve (0,0) it puts and drifts about at any time, using the angle of phase curve near tangent and phase space horizontal axisApproximation is carried out to it,According to deficient Drive diarthrodial angular accelerationAnd angular speedIt is calculated by formula (17):

Step 3: vibrator stage control: starting to carry out the control in vibrator stage, the first wrist of robot after the self-starting stage Joint (2), the second wrist joint (6), the first gripper (1), the second gripper (7) remain stationary, and elbow joint (4) is provided by formula (18) Target position moved, i.e. elbow joint (4) 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 drive lacking At the time of swaying phase angle beta=180 ° in joint, the speed of Robot elbow joint (4) is zero at this time, can reduce and start to carry out Impact when subsequent motion；

A is the safety factor greater than 1 in criterion (19), and effect is to consider in compensation subsequent motion in the vibrator stage in advance Issuable energy loss makes robot remain to reach grasping movement in unactuated joint angle when starting to grasp target truss rod Required target angle θ₁ ^f, θ₁ ^fThe first wrist joint (2), the second wrist joint (7) for robot stretch and the second gripper (7) The angle of unactuated joint when grasping target 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, and the adjusting stage, inner machine people was by encouraging For formation adjusting at the end of vibration to the configuration for being suitable for grasping target truss rod, the specific motion profile such as formula (21) that adjusts is shown, That is: in T₂By the rotational angle theta of elbow joint (4) 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 (1) of robot, with Wrist joint, the second gripper (7) 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) and calculate 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 closed in drive lacking at the time of adjusting stage terminates Save swaying phase angle beta=90 °-β₀At the time of, wherein β₀For the constant greater than 0；The corresponding unactuated joint in β=90 ° reaches highest pendulum At the time of angle and speed are 0, terminate the adjusting stage before unactuated joint phase angle reaches 90 °, and modification branch at the end Support the target position x of hand folding₁ ^d, it is set to gripping position x_t；

Step 5: big damping stage control: immediately entering big damping stage after the adjusting stage, closed with the wrist of support palmistry neighbour Section, 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 inverse movement Learn counted joint of robot angle in equation (8)；b₂For the sporting transfer coefficient of smooth robot, calculated by formula (24):

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, enable trip Its target is opened and closed position x by hand closure₂ ^dBy open position x₀It is switched to closed position x_s, complete the grasping to target truss rod； If grabbing bar movement failure, so that unactuated joint is affranchised and turn round and re-start control, Zhi Daocheng since the vibrator stage Function grasps target stem.

8. the control method of climbing truss robot according to claim 7, which is characterized in that the realization of the method Journey further include:

The climbing truss robot continues the control process of continuous moving after completing a target truss rod and grasping:

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, i.e. the corner of the first wrist joint (1) and the second wrist joint (7) The folding distance of corner exchange, the first gripper (1) is exchanged with the folding of the second gripper (7) distance, and the corner of elbow joint (4) is not Become；Make always after variable transcription: x₁Indicate the folding distance of support hand, x₂Indicate the folding distance of trip hand, θ₁Indicate that drive lacking closes The rotational angle theta of section₂Indicate the wrist joint corner with support palmistry neighbour, θ₃Indicate the corner of elbow joint (4), θ₄Indicate adjacent with trip palmistry Wrist joint corner；

Step 2: formation adjusting stage control: rotational angle theta in this stage₂、θ₄Corresponding two wrist joints are transported by planned trajectory It is dynamic, the position servo given value θ of elbow joint corner₃ ^dAccording to θ₂、θ₄It is determined by kinematics, using track shown in formula (25) to machine Device people 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 θ₄ ^pRespectively indicate this rank θ in section object shape₂、θ₄Value, θ₂ ^p=0, θ₄ ^pIt is calculated by formula (26)；b₃For formation adjusting movement used in transfer coefficient, It 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；Adjust rank Selection is in t at the time of section 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 keeps not Become, trip hand is enabled to 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 system of the timing since the loose bar stage 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 hand that swings, trip The folding freedom degree of hand and with support palmistry neighbour wrist joint remain stationary, elbow joint (4) and with trip palmistry neighbour wrist pass Section 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；Pine It with the wrist joint corner of trip palmistry neighbour is the terminal θ of trajectory planning in formula (25) at the end of the bar stage₄ ^p, and by the corner of elbow joint It is denoted as θ₃ ^p；Swing in the stage elbow joint (4) and with the adjusting stage in the carpal target rotation angle of trip palmistry neighbour and claim 7 Target rotation angle it is identical, respectively θ₃ ^f、θ₄ ^f；Then elbow joint (4) and the carpal motion profile such as formula (29) with trip palmistry neighbour It is shown:

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 in many experiments The average time that unactuated joint swings primary from the front to the back determines；Swing finish time in stage selection and claim 7 in The selection mode of finish time adjusting stage is identical, is selected in unactuated joint swaying phase angle beta=90 °-β₀At the time of；It swings The target of modification support hand opens and closes position x at the end of stage₁ ^d, it is set to gripping position x_t；

Step 5: big damping stage control: big damping stage when continuous moving and being actuated for grabbing bar from natural overhang Big damping stage it is identical, refer to the step five in the control process for grabbing bar that swings started by natural overhang； After grabbing bar, if need to continue to grasp next target truss rod forward, restarts from step 1, loop back and forth like this then The continuous moving in truss structure can be achieved.