CN106381899A - Position closed-loop control device and method for electric transmission excavator - Google Patents

Abstract

The invention relates to the technical field of electric transmission control intelligent program control, in particular to a position closed-loop control device and method for an electric transmission excavator. The device includes a left electric crossed handle (15), a right electric crossed handle (16), an instruction distributor (17), a bucket angular displacement sensor (18), a bucket rod angular displacement sensor (19), a movable arm angular displacement sensor (20), a controller (21), a bucket rod pilot electromagnetic valve (22), a bucket rod pilot proportional electromagnetic valve (23) and a movable arm pilot proportional electromagnetic valve (24). According to the position closed-loop control device and method for the electric transmission excavator, the instruction distributor is designed at the front end of the controller of a traditional electric transmission operation excavator, a space Descartes coordinate instruction of a rotating shaft of a bucket is mapped into a deflection angle instruction of a movable arm and a bucket rod, the excavator can be more visually operated, and the purposes of simplifying operation and relieving burdens of a driver are achieved.

Description

A kind of fax excavator position-force control apparatus and method

Technical field

Patent of the present invention is related to the intelligent program control technology field of Flying by wire, specially a kind of fax excavator position Closed-loop control device and method.

Background technology

Fig. 1 show six equipments of conventional excavator.Fig. 2 show the operating mechanism of conventional excavator.Drive Member is controlled to six equipments respectively by operating two capstan handles and two pedals.A left side for left capstan handle 11 Right bank is used for controlling revolution 4, the tilting forward and back for controlling dipper 2, the lateral tilting of right capstan handle 12 of left capstan handle 11 Tiltedly be used for controlling bucket 1, right capstan handle 12 tilt forward and back for controlling swing arm 3, left lateral walks pedal 13 and right lateral walks pedal 14 tilt forward and back be respectively intended to control left lateral walk 5 and right lateral walk 6.Excavator operation handle and pedal are to each equipment at present Control be all based on the control of speed, that is, the angle of inclination of operator capstan handle or pedal is bigger, accordingly controlled The movement velocity of device is faster.

A freedom of motion due to an operating mechanism correspond to the motion of a particular job device, if therefore When excavator needs to carry out a complicated action, driver needs to operate the multiple directions of a stick, or multiple behaviour The multiple directions of vertical pole just can complete.For example carrying out " putting down " (bucket bucket tooth is maintained at sustained height and moves back and forth) manipulation When, three equipments (swing arm, dipper, bucket) need mutually coordinated to move, and correspond to driver to two sticks three The hormany operating of individual freedom of motion.This considerably increases the manipulation burden to driver.

Traditional excavator maneuverability pattern is one of left capstan handle manipulation direction controlling dipper, right capstan handle two Direction of operating controls bucket and swing arm respectively, but these three equipments of bucket, dipper and swing arm are actually flat at one In-plane moving.Such maneuverability pattern is that the three-dimensional motion of two sticks is mapped in the plane motion of equipment Go.This maneuverability pattern is not very directly perceived, and during composite move, driver is difficult to complete several coordinations.This maneuverability pattern Very high request is proposed to the manipulation skill of driver.

In sum, the almost all of action of excavator is all the compound work needing several equipments, and traditional Maneuverability pattern is not directly perceived, and operating difficulty is big, and this is accomplished by a kind of new control program to simplify traditional maneuverability pattern.

Content of the invention

Goal of the invention：A kind of fax excavator position-force control apparatus and method are provided, simplify traditional maneuverability pattern.

Technical scheme：

A kind of fax excavator position-force control device, including：

Left electricity capstan handle (15), right electricity capstan handle (16), directive distributor (17), bucket angular displacement sensor (18), dipper angular displacement sensor (19), swing arm angular displacement sensor (20), controller (21), bucket pilot solenoid valve (22), Dipper guide proportion electromagnetic valve (23), swing arm guide proportion electromagnetic valve (24)；

Left electricity capstan handle (15), is connected with directive distributor (17), by before and after movable generation to control instruction Control the twisted point O of bucket₃Sagittal displacement in the horizontal direction, and described will be sent to instruction point to control instruction signal in front and back Orchestration (17)；

Right electricity capstan handle (16), is connected with directive distributor (17), generates upward and downward control instruction by movable Control the twisted point O of bucket₃Under in the vertical direction to displacement, and by described upward and downward control instruction signal be sent to instruction point Orchestration (17)；Outer by left and right moving control bucket is raised and is reclaimed, and by described bucket outer raise and reclaim signal be sent to Directive distributor (17)；

Directive distributor (17), is connected with controller (21), according to described in front and back to control instruction signal and upward and downward control Command signal processed passes through calculating and resolves into the control instruction of dipper and the control instruction of swing arm, according to the control instruction of described dipper With the control instruction of swing arm, and the outer control instruction raised and reclaim signal generation bucket of bucket, the control of described bucket The control instruction of instruction, the control instruction of dipper and swing arm is sent to controller (21)；

Controller (21), respectively with bucket angular displacement sensor (18), dipper angular displacement sensor (19), swing arm angular displacement Sensor (20) connects and gathers bucket angular displacement sensor (18), dipper angular displacement sensor (19), swing arm angle displacement transducer The feedback signal of device (20), the control instruction of described bucket is made the difference computing with bucket angular displacement sensor (18) feedback signal, Obtain the valve control instruction signal of bucket through instruction molding, be sent to bucket pilot solenoid valve (22)；Control by described dipper Instruction makes the difference computing with dipper angular displacement sensor (19) feedback signal, obtains the valve control instruction letter of dipper through instruction molding Number, it is sent to dipper guide proportion electromagnetic valve (23)；By the control instruction of described swing arm and swing arm angular displacement sensor (20) Feedback signal makes the difference computing, obtains the valve control instruction signal of swing arm through instruction molding, is sent to swing arm guide proportion electromagnetic valve (24)；

Bucket guide proportion electromagnetic valve (22), the valve control instruction signal according to bucket controls the motion of bucket actuator, enters And control bucket to work；

Dipper guide proportion electromagnetic valve (23), the valve control instruction signal according to dipper controls the motion of dipper actuator, enters And control dipper to work；

Swing arm guide proportion electromagnetic valve (24), the valve control instruction signal according to swing arm controls the motion of swing arm actuator, enters And control swing arm to work.

A kind of fax excavator position-force control method, including：

Step 1, record left lever displacement P_ARMWith right lever displacement P_BOOM, work as P_ARMOr P_BOOMLever displacement goes out extremely Qu Shi, records the deflection angle θ of this moment swing arm_2fDeflection angle θ with dipper_3fDeflection angle θ with bucket_4f, according to public affairs Formula (1) inscribes bucket twisted point O when calculating this₃Coordinate (x₃₀、z₃₀), inscribe bucket bucket tooth and ground when this is calculated according to formula (3) Face angle theta_d0；Described formula (1) is：

$\begin{array}{c}{x}_{30}=a_2{\mathrm{cos\θ}}_{2f}+a_3\sin (\frac{\mathrm{\π}}{2}+{\mathrm{\θ}}_{2f}-{\mathrm{\θ}}_{3f})\\ z_{30}=a_2{\mathrm{sin\θ}}_{2f}-a_3\cos (\frac{\mathrm{\π}}{2}+{\mathrm{\θ}}_{2f}-{\mathrm{\θ}}_{3f})\end{array}---\left(1\right)$

Wherein, a₂For boom length, a₃For bucket arm length；

Described formula (3) is

θ_d0=π+θ_2f-θ_3f-θ_4f(3)；

Step 2, by P_ARMAnd P_BOOMThe output result being obtained by instructing molding is P_{ARM 1}And P_{BOOM 1}, according to formula

$x_{3i}=x_{30}+{\∫}_0^t{P}_{ARM1}\·dt$

$z_{3i}=z_{30}+{\∫}_0^t{P}_{BOOM1}\·dt$

Obtain bucket twisted point O₃Theoretical coordinate (x_3i、z_3i)；

Step 3, control theory value θ according to formula (2) reverse now swing arm and dipper_2fi、θ_3fi；Described formula (2) is：

θ_2fi=alpha+beta

${\mathrm{\θ}}_{3fi}=\mathrm{\π}-\arccos \left(\frac{a_2^2+a_3^2-a_6^2}{2a_2{a}_3}\right)---\left(2\right)$

Wherein,

Step 4, by deflection angular measurement value of feedback θ of swing arm₂Control theory value θ with swing arm_2fiMake the difference, obtain error delta θ_2f, formula is Δ θ_2f=θ_2fi-θ₂；Deflection angular measurement value of feedback θ by dipper₃Control theory value θ with dipper_3fiMake the difference, obtain To error delta θ_3f, formula is Δ θ_3f=θ_3fi-θ₃；

Step 5, as Δ θ_2f>When 0, produce and lift arm instruction, Δ θ_2f<When 0, produce fall swing arm instruction；As Δ θ_3f>When 0, Produce and raise instruction, Δ θ outside dipper_3f<When 0, produce dipper recovery command；

Step 6, the deflection angle feedback signal theta according to swing arm₂Deflection angle feedback signal theta with dipper₃And θ_d0, by formula θ_4fi=π+θ₂-θ₃-θ_d0Calculate the theoretical deflection angle theta of bucket_4fi；

Step 7, by deflection angular measurement value of feedback θ of bucket₄Control theory value θ with bucket_4fiMake the difference, obtain error delta θ_4f, formula is Δ θ_4f=θ_4fi-θ₄；

Step 8, as Δ θ_4f>When 0, produce and reclaim bucket instruction, Δ θ_4f<When 0, produce outer bucket of raising and instruct.

Beneficial effect

The present invention simplifies traditional maneuverability pattern, alleviates driver's burden.

Brief description

Fig. 1 is digger operating device schematic diagram.

Fig. 2 is excavator operating mechanism schematic diagram.

Fig. 3 is excavator new control system schematic diagram.

Fig. 4 is new control system operation principle schematic diagram.

Fig. 5 goes out dead band moment swing arm, dipper deflection angle recording principle implementation method for stick.

Fig. 6 is swing arm, the mapping algorithm of dipper deflection angle to bucket twisted point cartesian coordinate is realized.

Fig. 7 is that bucket twisted point theoretical coordinate algorithm is realized.

Fig. 8 is swing arm, dipper deflection angle instructs calculation method.

Specific embodiment

A kind of fax excavator position-force control device, as shown in figure 3, include：

Left electricity capstan handle (15), right electricity capstan handle (16), directive distributor (17), bucket angular displacement sensor (18), dipper angular displacement sensor (19), swing arm angular displacement sensor (20), controller (21), bucket pilot solenoid valve (22), Dipper guide proportion electromagnetic valve (23), swing arm guide proportion electromagnetic valve (24)；

Left electricity capstan handle (15), is connected with directive distributor (17), by before and after movable generation to control instruction Control the twisted point O of bucket₃Sagittal displacement in the horizontal direction, and described will be sent to instruction point to control instruction signal in front and back Orchestration (17)；

Right electricity capstan handle (16), is connected with directive distributor (17), generates upward and downward control instruction by movable Control the twisted point O of bucket₃Under in the vertical direction to displacement, and by described upward and downward control instruction signal be sent to instruction point Orchestration (17)；Outer by left and right moving control bucket is raised and is reclaimed, and by described bucket outer raise and reclaim signal be sent to Directive distributor (17)；

Directive distributor (17), is connected with controller (21), according to described in front and back to control instruction signal and upward and downward control Command signal processed passes through calculating and resolves into the control instruction of dipper and the control instruction of swing arm, according to the control instruction of described dipper With the control instruction of swing arm, and the outer control instruction raised and reclaim signal generation bucket of bucket, the control of described bucket The control instruction of instruction, the control instruction of dipper and swing arm is sent to controller (21)；

Controller (21), respectively with bucket angular displacement sensor (18), dipper angular displacement sensor (19), swing arm angular displacement Sensor (20) connects and gathers bucket angular displacement sensor (18), dipper angular displacement sensor (19), swing arm angle displacement transducer The feedback signal of device (20), the control instruction of described bucket is made the difference computing with bucket angular displacement sensor (18) feedback signal, Obtain the valve control instruction signal of bucket through instruction molding, be sent to bucket pilot solenoid valve (22)；Control by described dipper Instruction makes the difference computing with dipper angular displacement sensor (19) feedback signal, obtains the valve control instruction letter of dipper through instruction molding Number, it is sent to dipper guide proportion electromagnetic valve (23)；By the control instruction of described swing arm and swing arm angular displacement sensor (20) Feedback signal makes the difference computing, obtains the valve control instruction signal of swing arm through instruction molding, is sent to swing arm guide proportion electromagnetic valve (24)；

Bucket guide proportion electromagnetic valve (22), the valve control instruction signal according to bucket controls the motion of bucket actuator, enters And control bucket to work；

Dipper guide proportion electromagnetic valve (23), the valve control instruction signal according to dipper controls the motion of dipper actuator, enters And control dipper to work；

Swing arm guide proportion electromagnetic valve (24), the valve control instruction signal according to swing arm controls the motion of swing arm actuator, enters And control swing arm to work.

A kind of fax excavator position-force control method, as shown in figure 4, include：

Step 1, record left lever displacement P_ARMWith right lever displacement P_BOOM, work as P_ARMOr P_BOOMLever displacement goes out extremely Qu Shi, records the deflection angle θ of this moment swing arm_2fDeflection angle θ with dipper_3f(record implementation method as shown in Figure 5) and The deflection angle θ of bucket_4f, when this is calculated according to formula (1), inscribe bucket twisted point O₃Coordinate (x₃₀、z₃₀), according to formula (3) Bucket bucket tooth and ground angle theta is inscribed when calculating this_d0；Described formula (1) is：

Wherein, a₂For boom length, a₃For bucket arm length；

The implementation method of formula (1) is as shown in Figure 6；

Described formula (3) is

θ_d0=π+θ_2f-θ_3f-θ_4f(3)；

Step 2, by P_ARMAnd P_BOOMThe output result being obtained by instructing molding is P_{ARM 1}And P_{BOOM 1}, according to formula

$x_{3i}=x_{30}+{\&Integral;}_0^t{P}_{ARM1}\&CenterDot;dt$

$z_{3i}=z_{30}+{\&Integral;}_0^t{P}_{BOOM1}\&CenterDot;dt$

Obtain bucket twisted point O₃Theoretical coordinate (x_3i、z_3i), implementation method is as shown in Figure 7；

Step 3, control theory value θ according to formula (2) reverse now swing arm and dipper_2fi、θ_3fi；Described formula (2) is：

θ_2fi=alpha+beta

${\mathrm{\&theta;}}_{3fi}=\mathrm{\&pi;}-\arccos \left(\frac{a_2^2+a_3^2-a_6^2}{2a_2{a}_3}\right)---\left(2\right)$

Wherein,

The implementation method of formula (2) is as shown in Figure 8；

Step 4, by deflection angular measurement value of feedback θ of swing arm₂Control theory value θ with swing arm_2fiMake the difference, obtain error delta θ_2f, formula is Δ θ_2f=θ_2fi-θ₂；Deflection angular measurement value of feedback θ by dipper₃Control theory value θ with dipper_3fiMake the difference, obtain To error delta θ_3f, formula is Δ θ_3f=θ_3fi-θ₃；

Step 5, as Δ θ_2f>When 0, produce and lift arm instruction, Δ θ_2f<When 0, produce fall swing arm instruction；As Δ θ_3f>When 0, Produce and raise instruction, Δ θ outside dipper_3f<When 0, produce dipper recovery command；

Step 6, the deflection angle feedback signal theta according to swing arm₂Deflection angle feedback signal theta with dipper₃And θ_d0, by formula θ_4fi=π+θ₂-θ₃-θ_d0Calculate the theoretical deflection angle theta of bucket_4fi；

Step 7, by deflection angular measurement value of feedback θ of bucket₄Control theory value θ with bucket_4fiMake the difference, obtain error delta θ_4f, formula is Δ θ_4f=θ_4fi-θ₄；

Step 8, as Δ θ_4f>When 0, produce and reclaim bucket instruction, Δ θ_4f<When 0, produce outer bucket of raising and instruct.

Claims (2)

1. a kind of fax excavator position-force control device is it is characterised in that include：

Left electricity capstan handle (15), right electricity capstan handle (16), directive distributor (17), bucket angular displacement sensor (18), bucket Bar angular displacement sensor (19), swing arm angular displacement sensor (20), controller (21), bucket pilot solenoid valve (22), dipper are first Lead proportional solenoid (23), swing arm guide proportion electromagnetic valve (24)；

Left electricity capstan handle (15), is connected with directive distributor (17), by before and after movable generation to control instruction control The twisted point O of bucket₃Sagittal displacement in the horizontal direction, and described will be sent to directive distributor to control instruction signal in front and back (17)；

Right electricity capstan handle (16), is connected with directive distributor (17), is controlled by the movable upward and downward control instruction that generates The twisted point O of bucket₃Under in the vertical direction to displacement, and described upward and downward control instruction signal is sent to directive distributor (17)；Outer by left and right moving control bucket is raised and is reclaimed, and by described bucket outer raise and reclaim signal be sent to instruction Allotter (17)；

Directive distributor (17), is connected with controller (21), is referred to according to described control to control instruction signal and upward and downward in front and back Signal is made to pass through to calculate the control instruction of the control instruction resolving into dipper and swing arm, the control instruction according to described dipper and dynamic The control instruction of arm, and the outer control instruction raised and reclaim signal generation bucket of bucket, the control instruction of described bucket, The control instruction of the control instruction of dipper and swing arm is sent to controller (21)；

Controller (21), respectively with bucket angular displacement sensor (18), dipper angular displacement sensor (19), swing arm angle displacement transducer Device (20) connects and gathers bucket angular displacement sensor (18), dipper angular displacement sensor (19), swing arm angular displacement sensor (20) feedback signal, the control instruction of described bucket is made the difference computing, warp with bucket angular displacement sensor (18) feedback signal Instruction molding obtains the valve control instruction signal of bucket, is sent to bucket pilot solenoid valve (22)；The control of described dipper is referred to Order makes the difference computing with dipper angular displacement sensor (19) feedback signal, obtains the valve control instruction signal of dipper through instruction molding, It is sent to dipper guide proportion electromagnetic valve (23)；Feedback by the control instruction of described swing arm and swing arm angular displacement sensor (20) Signal does difference operation, obtains the valve control instruction signal of swing arm through instruction molding, is sent to swing arm guide proportion electromagnetic valve (24)；

Bucket guide proportion electromagnetic valve (22), the valve control instruction signal according to bucket controls the motion of bucket actuator, and then controls Bucket work processed；

Dipper guide proportion electromagnetic valve (23), the valve control instruction signal according to dipper controls the motion of dipper actuator, and then controls Dipper work processed；

Swing arm guide proportion electromagnetic valve (24), the valve control instruction signal according to swing arm controls the motion of swing arm actuator, and then controls Brake arm works.

2. a kind of fax excavator position-force control method is it is characterised in that include：

Step 1, record left lever displacement P_ARMWith right lever displacement P_BOOM, work as P_ARMOr P_BOOMWhen lever displacement goes out dead band, Record the deflection angle θ of this moment swing arm_2fDeflection angle θ with dipper_3fDeflection angle θ with bucket_4f, according to formula (1) Bucket twisted point O is inscribed when calculating this₃Coordinate (x₃₀、z₃₀), inscribe bucket bucket tooth and ground angle when this is calculated according to formula (3) θ_d0；Described formula (1) is：

$\begin{array}{c}{x}_{30}=a_2{\mathrm{cos\&theta;}}_{2f}+a_3\sin (\frac{\mathrm{\&pi;}}{2}+{\mathrm{\&theta;}}_{2f}-{\mathrm{\&theta;}}_{3f})\\ z_{30}=a_2{\mathrm{sin\&theta;}}_{2f}-a_3\cos (\frac{\mathrm{\&pi;}}{2}+{\mathrm{\&theta;}}_{2f}-{\mathrm{\&theta;}}_{3f})\end{array}---\left(1\right)$

Wherein, a₂For boom length, a₃For bucket arm length；

Described formula (3) is

θ_d0=π+θ_2f-θ_3f-θ_4f(3)；

Step 2, by P_ARMAnd P_BOOMThe output result being obtained by instructing molding is P_{ARM 1}And P_{BOOM 1}, according to formula

$x_{3i}=x_{30}+{\&Integral;}_0^t{P}_{ARM1}\&CenterDot;dt$

$z_{3i}=z_{30}+{\&Integral;}_0^t{P}_{BOOM1}\&CenterDot;dt$

Obtain bucket twisted point O₃Theoretical coordinate (x_3i、z_3i)；

Step 3, control theory value θ according to formula (2) reverse now swing arm and dipper_2fi、θ_3fi；Described formula (2) is：

$\begin{array}{c}{\mathrm{\&theta;}}_{2fi}=\mathrm{\&alpha;}+\mathrm{\&beta;}\\ {\mathrm{\&theta;}}_{3fi}=\mathrm{\&pi;}-\arccos \left(\frac{a_2^2+a_3^2-a_6^2}{2a_2{a}_3}\right)\end{array}---\left(2\right)$

Wherein,

Step 4, by deflection angular measurement value of feedback θ of swing arm₂Control theory value θ with swing arm_2fiMake the difference, obtain error delta θ_2f, public Formula is Δ θ_2f=θ_2fi-θ₂；Deflection angular measurement value of feedback θ by dipper₃Control theory value θ with dipper_3fiMake the difference, obtain error Δθ_3f, formula is Δ θ_3f=θ_3fi-θ₃；

Step 5, as Δ θ_2f>When 0, produce and lift arm instruction, Δ θ_2f<When 0, produce fall swing arm instruction；As Δ θ_3f>When 0, produce Instruction, Δ θ is raised outside dipper_3f<When 0, produce dipper recovery command；

Step 6, the deflection angle feedback signal theta according to swing arm₂Deflection angle feedback signal theta with dipper₃And θ_d0, by formula θ_4fi= π+θ₂-θ₃-θ_d0Calculate the theoretical deflection angle theta of bucket_4fi；

Step 7, by deflection angular measurement value of feedback θ of bucket₄Control theory value θ with bucket_4fiMake the difference, obtain error delta θ_4f, public Formula is Δ θ_4f=θ_4fi-θ₄；

Step 8, as Δ θ_4f>When 0, produce and reclaim bucket instruction, Δ θ_4f<When 0, produce outer bucket of raising and instruct.