CN105234942B - The control system and its control method of the big prudent small arm of hydraulic pressure - Google Patents
The control system and its control method of the big prudent small arm of hydraulic pressure Download PDFInfo
- Publication number
- CN105234942B CN105234942B CN201510735630.8A CN201510735630A CN105234942B CN 105234942 B CN105234942 B CN 105234942B CN 201510735630 A CN201510735630 A CN 201510735630A CN 105234942 B CN105234942 B CN 105234942B
- Authority
- CN
- China
- Prior art keywords
- arm
- axis
- small arm
- module
- movement
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Active
Links
- 238000000034 method Methods 0.000 title claims abstract description 17
- 238000012545 processing Methods 0.000 claims abstract description 23
- 239000013307 optical fiber Substances 0.000 claims abstract description 19
- 230000033001 locomotion Effects 0.000 claims description 55
- 235000006508 Nelumbo nucifera Nutrition 0.000 claims description 19
- 240000002853 Nelumbo nucifera Species 0.000 claims description 19
- 238000002955 isolation Methods 0.000 claims description 19
- 210000000245 forearm Anatomy 0.000 claims description 12
- 238000004088 simulation Methods 0.000 claims description 8
- 239000011159 matrix material Substances 0.000 claims description 6
- NJPPVKZQTLUDBO-UHFFFAOYSA-N novaluron Chemical compound C1=C(Cl)C(OC(F)(F)C(OC(F)(F)F)F)=CC=C1NC(=O)NC(=O)C1=C(F)C=CC=C1F NJPPVKZQTLUDBO-UHFFFAOYSA-N 0.000 claims description 6
- 238000003825 pressing Methods 0.000 claims description 6
- 210000000323 shoulder joint Anatomy 0.000 claims description 6
- 230000009466 transformation Effects 0.000 claims description 6
- 238000006243 chemical reaction Methods 0.000 claims description 4
- 239000013078 crystal Substances 0.000 claims description 4
- 238000001514 detection method Methods 0.000 claims description 4
- 230000009467 reduction Effects 0.000 claims description 4
- 210000004247 hand Anatomy 0.000 claims description 3
- 210000000707 wrist Anatomy 0.000 claims description 3
- 238000004891 communication Methods 0.000 claims description 2
- 230000008569 process Effects 0.000 claims description 2
- 238000005056 compaction Methods 0.000 abstract description 3
- 238000002474 experimental method Methods 0.000 abstract description 2
- 230000001133 acceleration Effects 0.000 description 9
- 239000000835 fiber Substances 0.000 description 7
- 238000011160 research Methods 0.000 description 7
- 229920000535 Tan II Polymers 0.000 description 5
- 238000012360 testing method Methods 0.000 description 4
- 230000009471 action Effects 0.000 description 2
- 238000011161 development Methods 0.000 description 2
- 238000010586 diagram Methods 0.000 description 2
- 238000005516 engineering process Methods 0.000 description 2
- KDLHZDBZIXYQEI-UHFFFAOYSA-N Palladium on carbon Substances [Pd] KDLHZDBZIXYQEI-UHFFFAOYSA-N 0.000 description 1
- 238000013459 approach Methods 0.000 description 1
- 230000009286 beneficial effect Effects 0.000 description 1
- 230000005540 biological transmission Effects 0.000 description 1
- 238000013461 design Methods 0.000 description 1
- 230000009977 dual effect Effects 0.000 description 1
- 238000012986 modification Methods 0.000 description 1
- 230000004048 modification Effects 0.000 description 1
- 238000012544 monitoring process Methods 0.000 description 1
- 230000003287 optical effect Effects 0.000 description 1
- XLYOFNOQVPJJNP-UHFFFAOYSA-N water Substances O XLYOFNOQVPJJNP-UHFFFAOYSA-N 0.000 description 1
Classifications
-
- B—PERFORMING OPERATIONS; TRANSPORTING
- B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
- B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
- B25J9/00—Programme-controlled manipulators
- B25J9/16—Programme controls
- B25J9/20—Programme controls fluidic
-
- B—PERFORMING OPERATIONS; TRANSPORTING
- B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
- B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
- B25J9/00—Programme-controlled manipulators
- B25J9/16—Programme controls
- B25J9/1602—Programme controls characterised by the control system, structure, architecture
-
- B—PERFORMING OPERATIONS; TRANSPORTING
- B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
- B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
- B25J9/00—Programme-controlled manipulators
- B25J9/16—Programme controls
- B25J9/1602—Programme controls characterised by the control system, structure, architecture
- B25J9/1605—Simulation of manipulator lay-out, design, modelling of manipulator
-
- B—PERFORMING OPERATIONS; TRANSPORTING
- B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
- B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
- B25J9/00—Programme-controlled manipulators
- B25J9/16—Programme controls
- B25J9/1602—Programme controls characterised by the control system, structure, architecture
- B25J9/161—Hardware, e.g. neural networks, fuzzy logic, interfaces, processor
-
- B—PERFORMING OPERATIONS; TRANSPORTING
- B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
- B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
- B25J9/00—Programme-controlled manipulators
- B25J9/16—Programme controls
- B25J9/1628—Programme controls characterised by the control loop
Landscapes
- Engineering & Computer Science (AREA)
- Robotics (AREA)
- Mechanical Engineering (AREA)
- Automation & Control Theory (AREA)
- Physics & Mathematics (AREA)
- Artificial Intelligence (AREA)
- Evolutionary Computation (AREA)
- Fuzzy Systems (AREA)
- Mathematical Physics (AREA)
- Software Systems (AREA)
- Manipulator (AREA)
Abstract
The invention discloses the control systems and its control method of the big prudent small arm of hydraulic pressure, remote controler is communicated by optical fiber receiver-transmitter module with industrial personal computer, industrial personal computer connection signal processing module, the analog quantity of signal processing module output is connect by servo amplifier with multiple hydraulic efficiency servo-valves, angular transducer is set on small arm, and pass through sensor compaction system connection signal processing module, the instruction of remote controler is handed down to industrial personal computer by optical fiber receiver-transmitter module, and industrial personal computer issues control instruction by signal processing module, switch drive is instructed to small arm.The big prudent small arm control system of hydraulic pressure of designed, designed of the present invention experiments verify that, complete 10kV distribution charged for replacement cross-arm, transformer heavy duty task instead of the arm of people.
Description
Technical field
The present invention relates to a kind of robot technology, especially a kind of robot for high-voltage hot-line work small arm of hydraulic pressure prudent greatly
Control system and its control method.
Background technology
The gentle safety of Automated water in order to improve livewire work, mitigates the labor intensity and strong-electromagnetic field of operating personnel
The person of operating personnel is threatened, many countries have all successively carried out the research of hot line robot from the eighties, such as day
The country such as sheet, Spain, the U.S., Canada, France has successively carried out the research to hot line robot.China in 2002
The development of robot for high-voltage hot-line work commercialization model machine is carried out.
Hot line robot completes the development of laboratory prototype, by a large amount of by years of researches and practice
Field test demonstrates demand and value of the robot in live working of distribution network, at the same also demonstrate research approach can
Row, the reasonability of design.But robot manipulating task content is more single, is mostly to use double mechanical arms master-slave control method, no
Whole live working of distribution network projects can be covered, robot cannot carry out operation, therefore and power distribution network under complicated line environment
There are gaps for the produce reality of livewire work.Therefore it needs to develop big prudent/dead weight than small arm, it is big to increase auxiliary for robot
Prudent small arm, to complete to replace the heavily loaded job task such as cross-arm, transformer.
The big small arm research of prudent hydraulic pressure is also one of the hot spot of robot research, at present common industry in the world always
Robot load ratio is 1：10 or less.It is constrained by line environment and aerial lift device with insulated arm, it is desirable that table-top overall weight is not
Can be excessive, at present generally in 500~600Kg.According to research experience, assist small arm that should be limited within 100Kg, therefore it is required that
Small arm should reach 3:1 or so top load ratio, and ensure system have enough rigidity, with ensure end-of-pipe control precision and
Stability.Existing small boom structure is simple, and control mode is mostly manually controlled using hydraulic valve, without position sensor,
It is accurately controlled so position cannot be carried out.
Invention content
To solve the shortcomings of the prior art, the invention discloses the control system of the big prudent small arm of hydraulic pressure and its controls
Method processed, the control system using principal and subordinate add autonomous control mode replace people arm complete 10kV distribution lines replace cross-arm,
Replace the heavily loaded job task such as transformer.
To achieve the above object, concrete scheme of the invention is as follows：
The control system of the big prudent small arm of hydraulic pressure, including：Remote controler is communicated by optical fiber receiver-transmitter module with industrial personal computer, work
The analog quantity of control machine connection signal processing module, signal processing module output is connected by servo amplifier and multiple hydraulic efficiency servo-valves
It connects, the digital quantity of signal processing module output is sent to multiple switch valve, and the instruction of remote controler is issued by optical fiber receiver-transmitter module
To industrial personal computer, industrial personal computer issues control instruction by signal processing module, switch drive is instructed to small arm.
The hydraulic efficiency servo-valve respectively with the first waist rotating shaft of corresponding small arm mechanical arm, large arm pitch axis, small
Arm pitch axis, the second waist rotating shaft and paw folding axis are connected.First waist rotating shaft, large arm pitch axis, forearm pitch axis,
Second waist rotating shaft and paw folding axis are connected with corresponding simulation potentiometer respectively, and simulation potentiometer passes through sensor collection
Beam system connection signal processing module.
The remote controler includes：Analogue collection module, digital data acquisition module pass through magnetic lotus root isolation module I, magnetic respectively
Lotus root isolation module II is connect with microprocessor, and microprocessor sends mould with magnetic lotus root isolation module III, serial port drive circuit, optical fiber
Block is sequentially connected, and reset circuit, jtag circuit, crystal oscillating circuit are connect with microprocessor respectively, analogue collection module acquisition master
The data of hand potentiometer, digital data acquisition module acquires the data of locking button, Self-resetting button, and passes through microprocessor processes
Afterwards, by optical fiber receiver-transmitter module, it is sent to industrial personal computer.
The remote controler simulates main hand with 5DOF, the movement speed for controlling each joint of the small arm of 5DOF
Degree, position；On remote controler be arranged 11 buttons, be respectively used to control hydraulic selector, master/slave coordinate system switch key, paw locking,
Small arm pause, key reduction, the movement of X-axis positive direction world coordinate system, the movement of X-axis negative direction world coordinate system, Y-axis positive direction
World coordinate system movement, the movement of Y-axis negative direction world coordinate system, the movement of Z axis positive direction world coordinate system, the Z axis negative direction world
Coordinate system moves；7 groups of status indicator lamps, i.e. hydraulic selector, paw locking, the pause of small arm, a key are also set up on remote controler also
Original, single axial movement, the instruction of world coordinate system motion state, security alarm instruction.
The analogue collection module acquisition 5DOF simulates main hand signal, 0~10V of voltage range, digital data acquisition mould
Block acquires the switching signal of 11 buttons, high level 5V, low level 0V, and digital output module output switching signal controls 7 groups
Status indicator lamp.
The magnetic lotus root isolation module I, magnetic lotus root isolation module II are that four-way number is isolated with magnetic lotus root isolation module III
Device, both ends operating voltage 2.7V~5.5V support low voltage operating and can realize level conversion.
The signal processing module exports 5 road analog signals, 0~20ma of current range；Export 3 way switch amount signals.
The small arm is 5DOF hydraulic manipulator；Angular transducer is set on small arm, angular transducer measuring machine
The angular dimension of each axis of tool arm, mechanical arm mechanical structure include pedestal, large arm are equipped on pedestal, waist may be implemented in large arm
Revolution, large arm pitching, upper end are shoulder joint, and forearm is mounted in shoulder joint, and wrist revolution, the front end of forearm may be implemented in forearm
It is connect with paw.
The big prudent small arm of hydraulic pressure main hand control system is divided into main and slave terminal two parts when controlling, and main side is sent commands to
Main hands movement is operated from end from end feedback information to main side, industrial personal computer acquires the location information of small arm simulation potentiometer in real time
And location information is sent to main hand controls, industrial personal computer completes the movement of mechanical arm by the position tracking to main hand.
The invention also discloses the control methods of the big prudent small arm of hydraulic pressure, include the following steps：
1) a Cartesian coordinates (x is established at joint shaft to each rod piecei,yi,zi), i is all between 1 to 5
Positive integer, 5 be number of degrees of freedom, adds base coordinate system (x0,y0,z0)；
2) it is that the member coordinates of each joint establish 4 × 4 homogeneous transformation matrix, indicates and previous rod piece, that is, current
The relationship of the coordinate system of the previous rod piece of rod piece；
3) the timing interpolation algorithm for using " being walked when calculating ", calculates position and the posture of interpolated point；" being walked when calculating " refers to
Each interpolated point is subjected to the joint position obtained after inverse kinematics transformation and does not have to storage, and is directly started again by these joint positions
Movement；
4) inverse kinematic of each axis is calculated, inverse kinematic refers to that known terminal position and posture ask each joint
Angle obtains the movement angle of each axis in interpolation cycle.
In the step 1) in determine and establish each coordinate system use following three rule：The movement of each joint i
Around ziAxis moves；xiThe vertical z of axisi-1Axis is simultaneously directed away from zi-1The direction of axis；yiAxis must ask foundation by right-handed coordinate system.
Beneficial effects of the present invention：
1. the big prudent small arm control system of hydraulic pressure of designed, designed of the present invention experiments verify that, instead of people arm complete
10kV distribution charged for replacement cross-arm, transformer heavy duty task.
2. a key restoring function may be implemented in small arm control system, small arm can be made to restPose quickly.
3. principal and subordinate and autokinetic movement dual function may be implemented in control system, powerful, easy to operate.
4. master controller uses main industrial personal computer, main industrial personal computer to realize the positive and negative solution of kinematics, interpolation algorithm, complexity may be implemented
Motion control, processing speed is fast.
5. control system carries security alarm function, it is automatically stopped and alarms when more than rated load.
6. remote controler and mechanical arm control section carry out electrical isolation by optical fiber, ensure that the safety of personnel.
Description of the drawings
Fig. 1 is structure of the invention principle general diagram；
Fig. 2 is remote controler schematic diagram of the present invention；
Fig. 3 is mechanical arm structure chart of the present invention；
Fig. 4 is a key reduction principle figure of the invention；
Fig. 5 is the main hand control flow chart of the present invention；
Wherein, 1, remote controler；2, optical fiber sending module；3, optic fiber transceiver module；4, industrial personal computer；5, signal processing module；
6, potentiometer is simulated；7, sensor compaction system；8, servo valve；9, switch valve；10, analogue collection module；11, digital quantity is adopted
Collect module；12, magnetic lotus root isolation module I；13, magnetic lotus root isolation module II；14, magnetic lotus root isolation module III；15, reset circuit；16、
Jtag circuit；17, microprocessor；18, serial port drive circuit；19, crystal oscillating circuit.
The full name of JTAG is Joint Test Action Group, i.e. combined testing action group.Currently, JTAG has become
For a kind of international standard test protocol, it is mainly used for the inside examination of all kinds of chips.
Specific implementation mode：
The present invention is described in detail below in conjunction with the accompanying drawings：
Referring to Fig. 1, a kind of big prudent small arm control system of hydraulic pressure, including remote controler 1, optical fiber sending module 2, optical fiber connect
Receive module 3, industrial personal computer 4, signal processing module 5, simulation potentiometer 6, sensor compaction system 7, servo valve 8, switch valve 9.
The output of 1 control panel of remote controler connects the input of data acquisition board, and the output of data acquisition board connects optical fiber and sends mould
Block 2；The output of optic fiber transceiver module 3 connects the input of industrial personal computer 4 by serial ports, and the output of industrial personal computer 4 is connect by serial ports at signal
The input of module 5 is managed, the output of signal processing module 5 connects servo valve 8 and switch valve 9；Simulation potentiometer 6 passes through sensor boundling
System 7 connects the input of industrial personal computer 4.The instruction of the remote controler 1 is handed down to by optical fiber sending module 2 and optic fiber transceiver module 3
Industrial personal computer 4, industrial personal computer 4 is instructed by 5 downloading speed of signal processing module, switch drive is instructed to hydraulic system.
Referring to Fig. 2,1 collection plate of the remote controler include analogue collection module 10, digital data acquisition module 11, magnetic lotus root every
From module I 12, magnetic lotus root isolation module II13, magnetic lotus root isolation module III14, reset circuit 15, jtag circuit 16, microprocessor
17, serial port drive circuit 18, crystal oscillating circuit 19.Analogue collection module 10 acquires the data of 7 tunnels simulation rocking bar, digital data acquisition
Module 11 acquires the data of locking button, Self-resetting button, and after the processing of microprocessor 17, by serial port drive circuit
18, wireless sending module 2 are sent to wireless receiving module 3.
Referring to Fig. 3, the mechanical arm is 5DOF, and mechanical structure includes pedestal, large arm is equipped on pedestal, large arm can
To realize that waist revolution, large arm pitching, upper end are shoulder joint, forearm is mounted in shoulder joint, and wrist revolution may be implemented in forearm,
The front end of forearm is connect with paw.
Referring to Fig. 4, when the big prudent small arm of hydraulic pressure is in automatic motion mode, according to the motion control journey write in advance
Sequence automatic cycle executes, until pressing the pause button on control panel.
Reset procedure routine, whether reset button press, if without if routine terminate, continue to judge if pressing
Whether Start buttons are pressed, and if being waited for without if, judging to reset whether symbol is zero if pressing, if it is zero, open
Dynamic resetting movement, detection reseting mark switch, finishes until resetting, terminates, and is not zero if resetting symbol, starts zero fortune
It is dynamic, routine is detected in place, is finished until resetting.
Referring to Fig. 5, the big prudent main hand control system of the small arm of hydraulic pressure is divided into main and slave terminal two parts.Main side sends order
To from end, from end feedback information to main side, operator operates main hands movement, and small arm controller acquires small arm potentiometer in real time
Location information and location information is sent to main hand controls, mechanical arm controller is completed by the position tracking to main hand
The movement of mechanical arm, while the location information is sent to handheld terminal by main hand controls again, for display.Main hand controls is small
A part for arm control system, main hand control system are the control systems of remote controler.
Then initialization controller, main hand/coordinate system switching key judge whether main hand control, mechanical if it is starting
Arm place value detect and main hand position detection, mechanical arm place value detect when to main hand controls send place value information, then by main hand,
Mechanical arm place value information is subtracted each other, and then judges parameter B>e>0, if it is satisfied, then main hand is moved to positive direction, if-B<e<0,
Then main hand is moved to negative direction.
In the application, microprocessor uses 32 TMS320F2812DSP chips, and dominant frequency is up to 150MHz.
Magnetic lotus root isolation module selects the ADuM1400/1/2 four-way digital isolators of ADI companies.Both ends operating voltage
2.7V~5.5V supports low voltage operating and can realize level conversion.
The analogue collection module acquisition 5DOF simulates main hand signal, 0~10V of voltage range, digital data acquisition mould
Block acquires the switching signal of 11 buttons, high level 5V, low level 0V, and digital output module output switching signal controls 7 groups
Status indicator lamp.
5DOF simulates main hand and has the functions such as simple joint control, simple joint position feedback and security monitoring.
Industrial personal computer uses the small-sized industrial personal computers of ARK-5260, dominant frequency 1.66GHz to support 4 tunnel, 485 interface, carries 2 road PCI and expands
Open up slot, the power supply of 12-24V DC power supplies.
Signal processing module exports 5 road analog signals, 0~20ma of current range；1 way switch amount signal is exported, is driven
Electric current 10A, 12V；Input 5 road analog signals, 0~10V of voltage range.
Angular transducer uses WX13-12-15K individual pen precision potentiators, tolerance 0.05,0~15k of nominal resistance
Ω。
Ship heavy industry CSDY1 jet pipe servo valves during hydraulic efficiency servo-valve uses, 0~20ma of supply current.
Fiber optic telecommunications module includes optical fiber sending module 2, optic fiber transceiver module 3, between remote controler and industrial personal computer
Communication, using technical grade RS485 bus fibre repeaters, fiber type：Single mode, multimode are optional, optical port connector：Standard configuration ST,
Optical fiber uses single mode optical fiber, wavelength 1310nm, 0~20Km of transmission range.
The invention also discloses a kind of big prudent small arm autonomous control methods, include the following steps：
1) a Cartesian coordinates (x is established at joint shaft to each rod piecei,yi,zi), i is all between 1 to 5
Positive integer, 5 be number of degrees of freedom, adds base coordinate system (x0,y0,z0) (position and direction on engine base can be optional, only
Want z0Axis is along the first articulating shaft)；
2) 4 × 4 homogeneous transformation matrix are established for the member coordinates of each joint, indicates (current with previous rod piece
The previous rod piece of rod piece) coordinate system relationship；
3) the timing interpolation algorithm for using " being walked when calculating ", calculates position and the posture of interpolated point；" being walked when calculating " refers to
Each interpolated point is subjected to the joint position obtained after inverse kinematics transformation and does not have to storage, and is directly started again by these joint positions
Movement；
4) using equation to calculate the inverse kinematic of each axis, (inverse kinematic refers to that known terminal position and posture are asked
The angle in each joint), obtain the movement angle of each axis in interpolation cycle；
1) each coordinate system, which is determined and establish, in the step uses following three rule：Each joint i (i be 1 to 5 it
Between all positive integers, 5 be number of degrees of freedom) movement all around ziAxis moves；xiThe vertical z of axisi-1Axis is simultaneously directed away from zi-1Axis
Direction；yiAxis must ask foundation by right-handed coordinate system.
The invention space line interpolation can be divided into the following steps completion:
Input the initial point P of robot motion0(x0,y0,z0) and terminal Pf(xf,yf,zf) (f is the abbreviation of final) movement
Speed Pv, Acceleration and deceleration time TaWith interpolation cycle Tc, running time T；
The determination of basic parameter and the method for solving of interpolated point.Due to the movement of robot space line need to pass through acceleration and deceleration and
Uniform motion section, therefore before carrying out moving interpolation, should determine PvWhether acceleration and deceleration requirement is met.Method is as follows：
By P0(x0,y0,z0) and Pf(xf,yf,zf) obtain actual motion distance Pd=| P0Pf|；By PvAnd TaIt can calculate and add
Braking section required distanceIf Cd≥Pd, then actual motion speedOtherwise Cv=Pv；By time TaWith
Interpolation time TcIt obtains and accelerates step number Sa.By P0(x0,y0,z0) and Pf(xf,yf,zf), space the parametric equation of the straight line can be obtained
Therefore by formula (1), each interpolated point P can be obtainedi(xi,yi,zi) (i be each interpolated point step number, 0 withBetween institute
Have positive integer) arrive P0Distance be
K can be found out by formula (3), and obtain interpolated point rectangular co-ordinate.Therefore space line interpolation algorithm key is really
Fixed each interpolation section move distance Sd(i).Each section of movement is described below and seeks Sd(i)Method：
Accelerate section.Since the robot accelerating sections designed herein is uniformly accelerated motion, therefore by actual motion speed Cv
With Acceleration and deceleration time TaAcquire acceleration(unit is m/s^2), thus in acceleration section i-th of interpolated point speed
Scv(i)=iTcA can be obtained
Uniform motion section.Since the robot requirement designed herein has to pass through braking section, and interpolation operation is " to calculate on side
While walking ", therefore before progress uniform motion section starts every time, it is necessary to can calculate remaining distance meet system slowdown requirement.At the uniform velocity section
Each interpolation section move distance Sd(i)=Cv·Tc
Retarded motion section.Due to seeking accelerating step number SaShi Jinhang roundings calculate, therefore cannot be simply by accelerating sections
Acceleration plans accelerating sections after negating, can introduce error in this way, therefore braking section acceleration should recalculate.By front i-1
After interpolated point, remaining distance L can be obtainedd(i)=Pd-CSd(i-1), therefore braking section acceleration can be obtainedThen deceleration segment
The speed S of upper m-th of interpolated pointcv(m)=Cv+mTcA, you can obtain
Using equation progress inverse kinematic, (inverse kinematic refers to that known terminal position and posture ask each for the invention
The angle in joint：
θ1..., θ6--- --- indicates the angle of each axis movement；
Ai∈R4×4(i=1,2 ..., 6) --- --- is the conversion between coordinate system on each connecting rod according to D-H establishment of coordinate system
Matrix.
si--- -- indicates sin θi；
ci--- --- indicates cos θi；
sij--- --- indicates sin (θi+θj)；
cij--- ----indicates cos (θi+θj)
The normal vector of n-hand
The sliding vector of s-hand
A-hand close to vector
The position vector (6) of p-hand
Coordinate system Oi(i is positive integer i=0,1 ..., 6) it is then the D-H coordinate systems established on motion arm；a2,d4
∈ R indicate the length of mechanical arm respective link respectively.It can be by motion arm ending coordinates system O6In base coordinate system O0Under pose
It is written as expression formula：0T6=A1A2A3A4A5A6.When solving the equation of motion, from0T6Start to solve joint position.Make0T6Symbol
The each element of expression formula is equal to0T6General type, and determine therefrom that θ1.Once acquiring θ1It later, can be by A1 -1Premultiplication0T6One
As form, obtain
A-1 1 0T6=1T6 (7)
This formula can be used to solve other each joint variables.The inverse matrix premultiplication (7) for constantly using A, can obtain following other four
Matrix equation：
A2 -1A1 -1 0T6=2T6 (8)
A3 -1A2 -1A1 -1 0T6=3T6 (9)
A4 -1A3 -1A2 -1A1 -1 0T6=4T6 (10)
A5 -1A4 -1A3 -1A2 -1A1 -1 0T6=5T6 (11)
The levoform of each equation of above formula is0T6With the function of preceding (i-1) a joint variable, each pass can be determined with these equations
The position of section：
θ1=a tan2 (py,px) (-3.1415≤θ1≤3.1415) (12)
Pay attention to：In formula, positive and negative number corresponding θ3Two kinds may solution.
θ2=a tan2 (s23,c23)-θ3(-1.5707≤θ1≤1.5707) (14)
θ4=a tan2 (- axs1+ayc1,-axc1c23-ays1c23+azs23)(-3.1415≤θ1≤3.1415) (15)
Pay attention to：Work as s5When=0, mechanical arm is in Singularities.At this point, joint shaft 4 and 6 overlaps, θ can only be solved4With θ6's
And/or it is poor.Whether Singularities can all be judged close to zero by two variables of atan2 in formula (15).
θ5=a tan2 (- ax(c1c23c4+s1s4)-ay(s1c23c4-c1s4)+azs23c4,-axc1s23-ays23s1-azc23)
(-3.9268≤θ1≤0.7853) (16)
k1=-nx(c1c23s4-s1c4)-ny(s1c23s4+c1c4)+nzs23s4
k2=nx((c1c23c4+s1s4)c5-c1s23s5)+ny((s1c23c4-c1s4)c5-s1s23s5)-nz(s23c4c5+c23s5)
θ6=a tan2 (k1,k2)(-3.1415≤θ1≤3.1415) (17)
Above-mentioned, although the foregoing specific embodiments of the present invention is described with reference to the accompanying drawings, not protects model to the present invention
The limitation enclosed, those skilled in the art should understand that, based on the technical solutions of the present invention, those skilled in the art are not
Need to make the creative labor the various modifications or changes that can be made still within protection scope of the present invention.
Claims (6)
1. the control system of the big prudent small arm of hydraulic pressure, characterized in that including：Remote controler passes through optical fiber receiver-transmitter module and industrial personal computer
The analog quantity of communication, industrial personal computer connection signal processing module, signal processing module output passes through servo amplifier and multiple hydraulic pressure
Servo valve connects, and the digital quantity of signal processing module output is sent to multiple switch valve, and the instruction of remote controler passes through optical fiber transceiving
Module is handed down to industrial personal computer, and industrial personal computer issues control instruction by signal processing module, switch drive is instructed to small arm；
The small arm is 5DOF hydraulic manipulator；Angular transducer is set on small arm, and angular transducer measures mechanical arm
The angular dimension of each axis, mechanical arm mechanical structure includes pedestal, and large arm is equipped on pedestal, and large arm may be implemented waist and return
Turn, large arm pitching, upper end is shoulder joint, and forearm is mounted in shoulder joint, and forearm may be implemented wrist revolution, the front end of forearm with
Paw connects；
The hydraulic efficiency servo-valve is bowed with the first waist rotating shaft, large arm pitch axis, the forearm of corresponding small arm mechanical arm respectively
Axis, the second waist rotating shaft and paw folding axis is faced upward to be connected；First waist rotating shaft, large arm pitch axis, forearm pitch axis, second
Waist rotating shaft and paw folding axis are connected with corresponding simulation potentiometer respectively, and simulation potentiometer passes through sensor boundling system
System connection signal processing module；
The remote controler simulates main hand, movement velocity, position for controlling each joint of the small arm of 5DOF with 5DOF
It sets；11 buttons are set on remote controler, is respectively used to control hydraulic selector, master/slave coordinate system switch key, paw locking, small hangs
Arm pause, key reduction, the movement of X-axis positive direction world coordinate system, the movement of X-axis negative direction world coordinate system, the Y-axis positive direction world
Coordinate system movement, the movement of Y-axis negative direction world coordinate system, the movement of Z axis positive direction world coordinate system, Z axis negative direction world coordinates
System's movement；7 groups of status indicator lamps, i.e. hydraulic selector, paw locking, the pause of small arm, key reduction, list are also set up on remote controler
Axis movement, the instruction of world coordinate system motion state, security alarm instruction；
When the big prudent small arm of hydraulic pressure is in automatic motion mode, held according to the motion control program automatic cycle write in advance
Row, until pressing the pause button on control panel；
Be divided into main and slave terminal two parts when the main hand control system control of the big prudent small arm of hydraulic pressure, main side send commands to from
End operates main hands movement, small arm controller acquires the location information of small arm potentiometer in real time from end feedback information to main side
And location information is sent to main hand controls, mechanical arm controller completes the fortune of mechanical arm by the position tracking to main hand
It is dynamic, while the location information is sent to handheld terminal by main hand controls again, for display；
When the control of main hand, then initialization controller, main hand/coordinate system switching key judges whether main hand control, if it is opening
Beginning mechanical arm place value detects and the detection of main hand position, sends place value information to main hand controls when mechanical arm place value detects, then
Main hand, mechanical arm position information are subtracted each other, then according to judging that parameter determines that main hand is moved to positive direction or main hand is to negative direction
Movement；
Reset procedure routine, whether reset button press, if without if routine terminate, continue to judge Start if pressing
Whether button is pressed, and if waited for without if, judges to reset whether symbol is zero if pressing, and if it is zero, starts multiple
Position movement, detection reseting mark switch, finishes until resetting, terminates, and is not zero if resetting symbol, starts zero movement,
Routine is detected in place, is finished until resetting；
The control system adds autonomous control mode that the arm of people is replaced to complete 10kV distribution lines replacement cross-arm, replaces using principal and subordinate
Transformer heavy duty job task.
2. the control system of the prudent small arm of hydraulic pressure as described in claim 1 big, characterized in that the remote controler includes：Mould
Analog quantity acquisition module, digital data acquisition module are connected by magnetic lotus root isolation module I, magnetic lotus root isolation module II and microprocessor respectively
It connects, microprocessor is sequentially connected with magnetic lotus root isolation module III, serial port drive circuit, optical fiber sending module, reset circuit, JTAG
Circuit, crystal oscillating circuit are connect with microprocessor respectively, and analogue collection module acquires the data of main hand potentiometer, digital data acquisition
Module acquires the data of locking button, Self-resetting button, and after microprocessor processes, by optical fiber receiver-transmitter module, sends
To industrial personal computer.
3. the control system of the big prudent small arm of hydraulic pressure as described in claim 1, characterized in that the analogue collection module
It acquiring 5DOF and simulates main hand signal, 0~10V of voltage range, digital data acquisition module acquires the switching signal of 11 buttons,
High level 5V, low level 0V, digital output module output switching signal control 7 groups of status indicator lamps.
4. the control system of the big prudent small arm of hydraulic pressure as claimed in claim 2, characterized in that the magnetic lotus root isolation module I,
Magnetic lotus root isolation module II and magnetic lotus root isolation module III is four-way digital isolator, both ends operating voltage 2.7V~5.5V, branch
It holds low voltage operating and can realize level conversion；
The signal processing module exports 5 road analog signals, 0~20ma of current range；Export 1 way switch amount signal.
5. the control method of the big prudent small arm of hydraulic pressure, characterized in that include the following steps：
1) a Cartesian coordinates (x is established at joint shaft to each rod piecei,yi,zi), i is all just whole between 1 to 5
Number, 5 be number of degrees of freedom, adds base coordinate system (x0,y0,z0)；
2) 4 × 4 homogeneous transformation matrix are established for the member coordinates of each joint, indicated and previous rod piece, that is, current rod piece
Previous rod piece coordinate system relationship；
3) the timing interpolation algorithm for using " being walked when calculating ", calculates position and the posture of interpolated point；" being walked when calculating " refers to will be each
Interpolated point carries out the joint position obtained after inverse kinematics transformation and does not have to storage, and directly starts to transport by these joint positions again
It is dynamic；
4) inverse kinematic of each axis is calculated, inverse kinematic refers to the angle that known terminal position and posture ask each joint
Degree, obtains the movement angle of each axis in interpolation cycle.
6. the control method of the prudent small arm of hydraulic pressure as claimed in claim 5 big, characterized in that in the step 1) in determine
Following three rule is used with each coordinate system is established：The movement of each joint i is around ziAxis moves；xiThe vertical z of axisi-1Axis is simultaneously
It is directed away from zi-1The direction of axis；yiAxis must ask foundation by right-handed coordinate system.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201510735630.8A CN105234942B (en) | 2015-11-02 | 2015-11-02 | The control system and its control method of the big prudent small arm of hydraulic pressure |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201510735630.8A CN105234942B (en) | 2015-11-02 | 2015-11-02 | The control system and its control method of the big prudent small arm of hydraulic pressure |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN105234942A CN105234942A (en) | 2016-01-13 |
| CN105234942B true CN105234942B (en) | 2018-08-10 |
Family
ID=55032887
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN201510735630.8A Active CN105234942B (en) | 2015-11-02 | 2015-11-02 | The control system and its control method of the big prudent small arm of hydraulic pressure |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN105234942B (en) |
Families Citing this family (6)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN106476012B (en) * | 2016-10-18 | 2019-04-02 | 国网山东省电力公司电力科学研究院 | A kind of control method of auxiliary mechanical arm control system |
| CN106976085B (en) * | 2017-04-10 | 2023-03-31 | 深圳供电局有限公司 | Control device of live working robot of transformer substation |
| CN108758272B (en) * | 2018-06-08 | 2020-07-03 | 国网智能科技股份有限公司 | Insulation lifting arm system and method for live-line maintenance operation of transformer substation |
| CN108436884B (en) * | 2018-06-08 | 2020-09-25 | 国网智能科技股份有限公司 | One-key restoration control method and system for live-line maintenance robot |
| CN112343878B (en) * | 2020-10-15 | 2023-01-13 | 南京晨光集团有限责任公司 | Hydraulic driving system for linear motion of mechanical arm and intelligent control method thereof |
| CN115149939B (en) * | 2022-09-01 | 2023-01-06 | 清华大学 | Discrete high-voltage electronic device and method |
Citations (8)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| US6385509B2 (en) * | 1998-04-16 | 2002-05-07 | California Institute Of Technology | Tool actuation and force feedback on robot-assisted microsurgery system |
| CN101913149A (en) * | 2010-07-23 | 2010-12-15 | 山东电力研究院 | Embedded light mechanical arm controller and control method thereof |
| CN102615638A (en) * | 2012-04-01 | 2012-08-01 | 山东鲁能智能技术有限公司 | Master-slave hydraulic mechanical arm system of high-voltage hot-line operation robot |
| CN103056866A (en) * | 2013-01-29 | 2013-04-24 | 山东电力集团公司电力科学研究院 | Live-working robot of transformer substation |
| CN103056883A (en) * | 2013-01-11 | 2013-04-24 | 山东电力集团公司电力科学研究院 | Double-manipulator coordination control system and double-manipulator coordination control method |
| CN203077287U (en) * | 2013-01-29 | 2013-07-24 | 山东电力集团公司电力科学研究院 | Master-slave mode hydraulic pressure feedback mechanical arm controlling system of charged repair robot |
| CN103753534A (en) * | 2013-12-13 | 2014-04-30 | 国家电网公司 | Mobile robot controller and control method thereof |
| CN204248894U (en) * | 2014-09-10 | 2015-04-08 | 国家电网公司 | Transformer station's living water washing is auxiliary rushes robot control system |
-
2015
- 2015-11-02 CN CN201510735630.8A patent/CN105234942B/en active Active
Patent Citations (8)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| US6385509B2 (en) * | 1998-04-16 | 2002-05-07 | California Institute Of Technology | Tool actuation and force feedback on robot-assisted microsurgery system |
| CN101913149A (en) * | 2010-07-23 | 2010-12-15 | 山东电力研究院 | Embedded light mechanical arm controller and control method thereof |
| CN102615638A (en) * | 2012-04-01 | 2012-08-01 | 山东鲁能智能技术有限公司 | Master-slave hydraulic mechanical arm system of high-voltage hot-line operation robot |
| CN103056883A (en) * | 2013-01-11 | 2013-04-24 | 山东电力集团公司电力科学研究院 | Double-manipulator coordination control system and double-manipulator coordination control method |
| CN103056866A (en) * | 2013-01-29 | 2013-04-24 | 山东电力集团公司电力科学研究院 | Live-working robot of transformer substation |
| CN203077287U (en) * | 2013-01-29 | 2013-07-24 | 山东电力集团公司电力科学研究院 | Master-slave mode hydraulic pressure feedback mechanical arm controlling system of charged repair robot |
| CN103753534A (en) * | 2013-12-13 | 2014-04-30 | 国家电网公司 | Mobile robot controller and control method thereof |
| CN204248894U (en) * | 2014-09-10 | 2015-04-08 | 国家电网公司 | Transformer station's living water washing is auxiliary rushes robot control system |
Also Published As
| Publication number | Publication date |
|---|---|
| CN105234942A (en) | 2016-01-13 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN105234942B (en) | The control system and its control method of the big prudent small arm of hydraulic pressure | |
| CN107214701B (en) | A kind of livewire work mechanical arm automatic obstacle avoiding paths planning method based on movement primitive library | |
| CN106493708B (en) | A kind of hot line robot control system based on double mechanical arms and sub-arm | |
| CN107901039B (en) | Python-based desktop-level robot offline programming simulation system | |
| CN107943283B (en) | Mechanical arm pose control system based on gesture recognition | |
| WO2018219194A1 (en) | Cyber arm-based teleoperation system for space station robot | |
| CN102310407A (en) | Bionic control method and control system of mechanical arm | |
| CN106429851B (en) | Overhead crane automatic control system based on remote tele-operation | |
| CN108214445A (en) | A kind of principal and subordinate's isomery remote operating control system based on ROS | |
| CN203077287U (en) | Master-slave mode hydraulic pressure feedback mechanical arm controlling system of charged repair robot | |
| CN104407519B (en) | Semi-physical simulation system of AC-DC-AC metallurgy rolling mill transmission system | |
| CN104267606B (en) | A kind of power-positioning control system hardware-in-the-loop test emulator and method of work thereof | |
| CN107885916A (en) | A kind of drill jumbo drill boom Analytical Methods of Kinematics based on CFDH methods | |
| CN104932405A (en) | Method for auxiliary test of PLC control program by employing equipment virtual prototype | |
| CN109079787A (en) | The automatic hand and eye calibrating method of non-rigid robot neural network based | |
| CN108582031A (en) | A kind of hot line robot branch based on force feedback master ＆ slave control connects gage lap method | |
| CN102393754B (en) | Arm support action control method and system, arm support tail end linear displacement control method and system, and concrete pump trucks | |
| CN106476012B (en) | A kind of control method of auxiliary mechanical arm control system | |
| CN104842356A (en) | Multi-palletizing robot teaching method based on distributed computing and machine vision | |
| CN108582119A (en) | A kind of hot line robot force feedback master-slave control method and system | |
| CN108462108A (en) | A kind of hot line robot strain insulator replacing options based on force feedback master ＆ slave control | |
| CN102360228B (en) | Cantilever crane action control system and concrete pump truck | |
| CN102681521A (en) | Four-transmission-point-based papermaking machine transmission experimental apparatus | |
| CN106965147A (en) | A kind of hot line robot isolation switch detection method | |
| CN109079736A (en) | A kind of control method and system of the mobile robot platform based on ROS |
Legal Events
| Date | Code | Title | Description |
|---|---|---|---|
| C06 | Publication | ||
| PB01 | Publication | ||
| C10 | Entry into substantive examination | ||
| SE01 | Entry into force of request for substantive examination | ||
| GR01 | Patent grant | ||
| GR01 | Patent grant | ||
| CP01 | Change in the name or title of a patent holder |
Address after: Wang Yue Central Road Ji'nan City, Shandong province 250002 City No. 2000 Co-patentee after: National Network Intelligent Technology Co., Ltd. Patentee after: Electric Power Research Institute of State Grid Shandong Electric Power Company Co-patentee after: State Grid Corporation of China Address before: Wang Yue Central Road Ji'nan City, Shandong province 250002 City No. 2000 Co-patentee before: Shandong Luneng Intelligent Technology Co., Ltd. Patentee before: Electric Power Research Institute of State Grid Shandong Electric Power Company Co-patentee before: State Grid Corporation of China |
|
| CP01 | Change in the name or title of a patent holder | ||
| TR01 | Transfer of patent right |
Effective date of registration: 20201027 Address after: 250101 Electric Power Intelligent Robot Production Project 101 in Jinan City, Shandong Province, South of Feiyue Avenue and East of No. 26 Road (ICT Industrial Park) Patentee after: National Network Intelligent Technology Co.,Ltd. Address before: Wang Yue Central Road Ji'nan City, Shandong province 250002 City No. 2000 Patentee before: ELECTRIC POWER RESEARCH INSTITUTE OF STATE GRID SHANDONG ELECTRIC POWER Co. Patentee before: National Network Intelligent Technology Co.,Ltd. Patentee before: STATE GRID CORPORATION OF CHINA |
|
| TR01 | Transfer of patent right |