A kind of robot hydraulic-driven cradle head closed loop control method
It is on November 22nd, 2016 that the application, which is for the applying date, and application No. is CN201611039763.2, denominations of invention
The division proposed for the invention of " a kind of robot hydraulic-driven cradle head closed-loop control system ".
Technical field
The invention belongs to technical field of robot control, in particular it relates to which a kind of robot hydraulic-driven rotates
Joint closed loop control method.
Background technique
Cradle head is most widely used a kind of joint in current industrial robot field.General cradle head mostly uses
Motor direct-drive, and it is less by the cradle head of hydraulic-driven.Since hydraulic drive is compressible etc. in the presence of leakage, transmission fluid
Problem is difficult to accurate control so that moving to it.Moreover, being easy to produce impact when load changes.Conventional method
Proportional control solenoid valve is mostly used to control flow, but proportional control solenoid valve is expensive, and high-end product is mostly
Foreign country is monopolized, and causes to control higher cost.Therefore need that a kind of low in cost and position control of control is accurate, movement is flat at present
The preferable control system of stability and its method.
Number of patent application is that 201310567608.8 Chinese invention patents disclose a kind of closing based on hydraulic control mode
Ring Force control system and control method obtain power closed loop control parameters according to the controller module and execute the voice and believe
Number, and to setting hydraulic cylinder output stepping-in amount according to the control parameter and judging whether to power output control, if so, described
DSP module samples the drag force of the feedback measuring cell detection, and calculates the size of the drag force as power closed-loop control
The force feedback value of method, the DSP module further according to the voice module obtain power closed loop control parameters and the force feedback value into
Row increment PI calculates, and using the value after calculating as the input value of hydraulic control component.The closed loop power control that the invention can provide
System and control method can provide safely and reliably drag force in fracture of lower arm operation for patient, but the invention is defeated in power
Operation stability is poor in control out.
Number of patent application discloses a kind of revolute joint driving device, including driving machine for 201410119355.2
Structure and execution output mechanism;Driving mechanism includes servo motor and hydraulic pump, and executing output mechanism includes two groups disposed in parallel
The piston space of hydraulic cylinder, two groups of hydraulic cylinders is connected with swing rod, and at both ends, the drive draft link of piston can be put with its centre of gyration
It is dynamic;Robot components are connected at the centre of gyration of swing rod and rotate with the swing of swing rod;Driving device further includes detection mould
Block, processing module and electric machine controller.By using servo motor driving hydraulic actuating mechanism output, output power can have
Effect solves the demand that rotation operation robot needs high-power, cramped construction cradle head.It is driven by using unilateral bearing,
Realize the unidirectional turnover movement of the intermittence of transmission shaft.By using the mating reaction of transmitter and amplifier, effectively adjustment machine
The amount of spin of people's component, but invention operation stability in power output control is poor.
Summary of the invention
In order to overcome the above problems of the prior art, the present invention provides a kind of robot hydraulic-driven cradle head
Closed loop control method, which is characterized in that robot hydraulic-driven cradle head closed-loop control system includes driving mechanism, execution machine
Structure and control mechanism, power module are system power supply, wherein driving mechanism includes direct current generator, direct current generator and gear pump phase
Even, executing agency includes hydraulic rotation joint, and control mechanism includes host computer, and control system further includes motor driver, signal
Capture card, three-position four-way electromagnetic directional valve, host computer are connected by USB interface with slave computer, gear pump and 3-position 4-way electromagnetism
Reversal valve be connected, the oil circuit of three-position four-way electromagnetic directional valve is connected with hydraulic rotation joint, three-position four-way electromagnetic directional valve into
Pressure transmitter is housed, pressure transmitter is connected with data acquisition card, and hydraulic rotation is set on joint on oil pipe line and oil outlet pipe
Have potentiometric sensor, potentiometric sensor is connected with data acquisition card, and data acquisition card is further connected with host computer, host computer with
It is connected between motor driver by CAN bus communication module;
The motor is connected by shaft coupling with gear pump, the output circuit and 3-position 4-way electromagnetic switch of the gear pump
Valve is connected, and the pressure transmitter is connected to the oil inlet pipeline and oil outlet pipeline of solenoid directional control valve, respectively oil inlet pressure
Power transmitter, oil outlet pressure oscillation device respectively will pipes for measuring the pressure value of hydraulic system oil inlet pipe and oil outlet pipe
The pressure value of road binders liquid is converted into analog electrical signal and is sent to data acquisition card;Inlet pressure transmitter and oil inlet overflow
Valve is both connected on oil inlet pipeline, and oil inlet overflow valve provides overload protection for oil inlet pipeline;Oil outlet pressure transmitter
It is all connected on oil outlet pipeline with oil outlet overflow valve, oil outlet overflow valve provides certain back pressure for oil outlet pipeline;
Step 1: starting direct current generator, direct current generator are rotated according to default PWM duty cycle D;
Step 2: the current location and target position to be reached that host computer is returned according to potentiometric sensor judge hydraulic
Cradle head rotation direction;
Step 3: host computer sends the corresponding command to slave computer, and the valve body for controlling solenoid directional control valve is moved to corresponding position,
Oil inlet, oil outlet are opened, and hydraulic rotation joint starts turning;
Step 4: host computer calculates pressure according to the pressure value that inlet pressure transmitter and oil outlet pressure transmitter return
Power difference △ P adjusts PWM duty cycle D;
Step 5: DC motor driver realizes the acceleration or deceleration of direct current generator according to newest duty ratio D;
Step 6: the hydraulic rotation joint position information real-time judge hydraulic rotation that host computer is returned according to potentiometric sensor
Whether joint reaches target position, if reaching, thens follow the steps 7, no to then follow the steps 4;
Step 7: host computer gives slave computer to send instruction, and control electromagnetic switch valve body is moved to Median Function, oil inlet,
Oil outlet is locked, and hydraulic rotation joint position locking a, subtask terminates;
Step 8: direct current generator keeps rotation, and oil inlet overflow valve overflow waits next subtask;
Wherein, after the host computer receives oil inlet and oil outlet pressure value, calculate pressure difference △ P=Pi-Po, Pi be into
Hydraulic fluid port pressure value, Po are oil outlet pressure value;According to pressure difference △ P, judge whether load changes, and according to the change of load
Change the value adjusted in real time for controlling the duty ratio D of the PWM of motor speed, host computer leads to the value of duty ratio D by CAN bus
Letter module is sent to motor driver, and motor driver controls DC motor speed according to obtained PWM value.
Further, USB module connection is turned by RS232 between the data acquisition card and host computer, RS232 turns USB
Module one end is connected with data acquisition card, and the other end is connected with the USB interface of host computer, data acquisition card by pressure transmitter with
And the pressure value and hydraulic rotation joint position of potentiometric sensor transmission are sent to host computer.
Further, the solenoid directional control valve is used to after hydraulic rotation joint reaches target position convert to middle-position machine
Locking for energy, realization oil inlet and oil outlet, guarantees the locking of position, when hydraulic rotation joint needs to change rotation direction,
It is diarthrodial into and out of hydraulic fluid port that solenoid directional control valve changes hydraulic rotation by the movement of valve position.
Further, the potentiometric sensor is for detecting the diarthrodial position of hydraulic rotation and sending to data acquisition card.
Beneficial effect
The present invention provides a kind of robot hydraulic-driven cradle head closed loop control method, by oil inlet, oil outlet
The detection of pressure value simultaneously accordingly adjusts PWM duty cycle D, to hydraulic rotation articulation angle, oil inlet and oil outlet pressure
Real-time detection carries out timely adjustment to motor speed, to form closed feedback loop, realizes that hydraulic rotation joint becomes load
That changes is adaptively adjusted in time, guarantees the stationarity of rotation process, by the real-time detection to location information, realizes movement control
The accuracy of system.
Detailed description of the invention
Fig. 1 is robot hydraulic-driven cradle head closed-loop control system annexation figure;
Fig. 2 is robot hydraulic-driven cradle head closed loop control method flow diagram.
Specific embodiment
In order to be best understood from technical solution of the present invention and advantage, below by way of specific embodiment, and in conjunction with attached drawing pair
The present invention is described further.In addition, direct current generator and motor described in description of the invention are identical concept, direct current generator
Driver and motor driver are identical concept, and three-position four-way electromagnetic directional valve and solenoid directional control valve are identical concept.
Embodiment 1.1
As shown in Figure 1, being a kind of robot hydraulic-driven cradle head closed-loop control system provided by the invention, including drive
Motivation structure, executing agency and control mechanism, driving mechanism include direct current generator 5 and gear pump 6, direct current generator 5 and 6 phase of gear pump
Even, executing agency includes hydraulic rotation joint 12, and control mechanism includes host computer 1, and host computer 1 calculates pressure transmitter and detects
Inlet pressure value and oil outlet pressure value difference, when pressure difference increase when, illustrate load increase, host computer increase PWM
Duty ratio D, motor accelerate to adapt to the increase of load;When pressure difference reduces, illustrate that load reduces, host computer reduces PWM and accounts for
Sky ratio D, decelerating through motor is to adapt to the reduction loaded, the hydraulic rotation joint current location that host computer detects potentiometric sensor
Compared with the real-time perfoming of target position, when the two is consistent, PC control solenoid directional control valve goes to Median Function, to realize
The locking of position.
The system further includes slave computer 2, CAN bus communication module 3, motor driver 4, data acquisition card 7, potentiometric sensors
Device 10, three-position four-way electromagnetic directional valve 11, oil inlet overflow valve, oil outlet overflow valve 14.
Pressure transmitter includes inlet pressure transmitter 8 and oil outlet pressure transmitter 9, inlet pressure transmitter 8
It is connected to oil inlet pipe, oil outlet pressure transmitter 9 is connected to oil outlet pipe, measures the pressure value into and out of oil pipe line respectively;Into
Hydraulic fluid port pressure transmitter 8, oil outlet pressure transmitter 9 are connected to data acquisition card 7, and the pressure value of pipeline oil is converted to mould
Quasi- electric signal (current value) is simultaneously sent to data acquisition card 7.
Potentiometric sensor 10 is connected to hydraulic rotation joint 12, for measuring the diarthrodial current operation angle of hydraulic rotation,
The diarthrodial angular displacement of hydraulic rotation is converted into corresponding voltage signal.
Data acquisition card 7 is connected to host computer 1, for acquiring inlet pressure transmitter 8, oil outlet pressure transmitter 9
The signal measured with potentiometric sensor 10, and send the signal in host computer 1.
Control method as shown in Fig. 2, host computer 1 by data acquisition card 7 obtain into and out of oil pipe line pressure value signal and
The position signal in hydraulic rotation joint 12 calculates pressure difference value △ P according to import and export loine pressure value signal, according to pressure difference
△ P judges whether load changes, and adjusts the duty for controlling the PWM of 9 revolving speed of motor in real time according to the variation of load
Value than D;The value of duty ratio D is sent to motor driver 4, motor driver by CAN bus communication module 3 by host computer 1
5 revolving speed of direct current generator is controlled according to obtained PWM value.
Direct current generator 5 is connect with gear pump 6, the performance curve of conjunction gear wheel pump 6, establishes the output flow mould of gear pump 6
Type is that (each letter of above formula represents Q (ml)=q*n*v/60: Q: flow;Q: the average every turn of tidal stream amount (ml/ turns) of gear pump and gear
Structure it is related;N: gear revolution speed, (rev/min);V: volumetric efficiency).The revolving speed of direct current generator 5 determines gear pump 6
Output flow.When the load of hydraulic system is constant, the output flow of gear pump 6 determines the diarthrodial rotation speed of hydraulic rotation
Degree.When load changes, the diarthrodial movement speed of hydraulic rotation is affected, and correspondingly changes 6 output flow of gear pump
This influence can be offset, so as to improve the stationarity of hydraulic rotation articulation.
Specifically, the relationship of duty ratio D in PWM and pressure difference △ P is simplified are as follows: D=κ △ P (κ is proportionality coefficient), when
Pressure difference △ P increases, and illustrates that load increases, motor 5 need to be made to accelerate, host computer 1 increases accordingly duty ratio D, and is sent to
To motor driver 4,4 driving motor 5 of motor driver accelerates, to adapt to the instantaneous increase of load;Vice versa.
Host computer 1 obtains the diarthrodial rotational angle of hydraulic rotation by data acquisition card 7.1 real-time judge of host computer is current
Whether rotational angle reaches target rotation angle, such as reaches designated position, then machine 1 gives 2 signal of slave computer, and slave computer 2 controls corresponding electricity
The on-off of magnetic relay converts the valve body of solenoid directional control valve 11 to Median Function, realizes the locked of oil inlet and oil outlet, this
When hydraulic rotation joint locked in current location, primary rotation task terminates.When task start next time, host computer according to appoint
Business first judges the diarthrodial rotation direction of hydraulic rotation, and correspondingly controls solenoid directional control valve according to above-mentioned steps according to rotation direction
11。
Specifically, the calculation formula of pressure difference is △ P=Pi-Po (Pi: inlet pressure, Po: oil outlet pressure);Letter
USB module connection is turned by RS232 number between capture card 7 and host computer 1, RS232 turns USB module one end and data acquisition card 7
It is connected, the other end is connected with the USB interface of host computer 1.RS232, which turns USB module, may be implemented the serial interface of data acquisition card 7
With the conversion of the USB interface of host computer 1, to realize the communication between data acquisition card and host computer.
It is connected between host computer 1 and DC motor driver 4 by CAN bus communication module 3, host computer 1 and CAN bus
Communication module 3 is connected by USB connection, CAN bus communication module 3 with motor driver 4.
Host computer 1 is connected by USB with slave computer 2, and slave computer 2 controls electromagnetic relay, gives electromagnetic relay switch amount,
Electromagnetic relay controls the power on/off of the both ends electromagnet of solenoid directional control valve 11 to realize the commutation of solenoid directional control valve 11.
Robot hydraulic-driven cradle head closed-loop control system of the invention further includes power module, respectively host computer
1 (220V), slave computer 2 (5V), data acquisition card 7 (5V), motor driver 4 (24V), solenoid directional control valve 11 (24V) etc. provide
Power supply necessary to working normally.
Robot hydraulic-driven cradle head closed loop control method specifically includes the following steps:
Step 1: starting direct current generator 4, direct current generator 4 are rotated according to default PWM duty cycle D0;
Step 2: the current location and target position to be reached that host computer 1 is returned according to potentiometric sensor 10, judgement
Hydraulic rotation articulation direction;
Step 3: host computer 1 sends the corresponding command to slave computer 2, and the valve body of control solenoid directional control valve 11 is moved to corresponding positions
It sets, is opened into and out of hydraulic fluid port, hydraulic rotation joint starts turning;
Step 4: host computer 1 calculates pressure according to the pressure value that inlet pressure transmitter and oil outlet pressure transmitter return
Power difference △ P adjusts PWM duty cycle D according to aforementioned rule;
Step 5: DC motor driver 4 realizes the acceleration or deceleration of direct current generator 5 according to newest duty ratio D;
Step 6: whether the hydraulic rotation joint position information real-time judge that host computer 1 is returned according to potentiometric sensor 10 arrives
Up to target position, if reaching, 7 are thened follow the steps, it is no to then follow the steps 4;
Step 7: host computer 1 sends instruction to slave computer 2, and control 11 valve body of solenoid directional control valve is moved to Median Function, into,
Oil outlet is locked, and hydraulic rotation joint position locking a, subtask terminates;
Step 8: direct current generator 5 keeps rotation, and oil inlet overflow valve overflow waits next subtask.
It should be noted that the above embodiments are only used to illustrate the technical solution of the present invention., rather than its limitations;Although
Present invention has been described in detail with reference to the aforementioned embodiments, those skilled in the art should understand that: it is still
It is possible to modify the technical solutions described in the foregoing embodiments, or some or all of the technical features is carried out
Equivalent replacement;And these are modified or replaceed, it does not separate the essence of the corresponding technical solution various embodiments of the present invention technical side
The range of case.