CN105234942A - Control system and control method for heavy-weight hydraulic small boom - Google Patents

Abstract

The invention discloses a control system and a control method for a heavy-weight hydraulic small boom. According to the control system, a remote controller is communicated with an industrial personal computer through an optical fibre transmitting-receiving module; the industrial personal computer is connected with a signal processing module; an analogue quantity output by the signal processing module is connected with a plurality of hydraulic servo valves through a servo amplifier; an angle sensor is arranged on the small boom, and connected with the signal processing module through a sensor bundling system; instructions of the remote controller are sent to the industrial personal computer through the optical fibre transmitting-receiving module; and the industrial personal computer sends a control instruction and a switch driving instruction to the small boom through the signal processing module. The self-designed control system for the heavy-weight hydraulic small boom, which is disclosed by the invention, replaces a human arm to finish a task of live replacement for cross arms and transformer overloads of a 10kV power distribution station through experimental verification.

Description

The control system of the little arm of large prudent hydraulic pressure and control method thereof

Technical field

The present invention relates to a kind of Robotics, the control system of the little arm of the large prudent hydraulic pressure of especially a kind of robot for high-voltage hot-line work and control method thereof.

Background technology

In order to the gentle security of the Automated water improving livewire work, the labour intensity and the strong-electromagnetic field that alleviate operating personnel threaten the person of operating personnel, from the eighties, many countries have all successively carried out the research of hot line robot, as the countries such as Japan, Spain, the U.S., Canada, France have successively carried out the research to hot line robot.Within 2002, China has also carried out the development of robot for high-voltage hot-line work commercialization model machine.

Hot line robot is through years of researches and practice, complete the development of laboratory prototype, by a large amount of field trials, demonstrate the demand of robot in live working of distribution network and value, also demonstrate the feasibility of research approach, the reasonability of design simultaneously.But robot manipulating task content is comparatively single, mostly adopt double mechanical arms master-slave control method, can not contain whole live working of distribution network project, robot can not carry out operation under complicated line environment, therefore with the production physical presence gap of live working of distribution network.Therefore need development prudent greatly/from the little arm of anharmonic ratio, be that robot increases auxiliary large prudent little arm, thus complete and change the heavily loaded job task such as cross-arm, transformer.

The research of large prudent hydraulic pressure little arm is also one of focus of robot research always, at present in the world common industrial robot load ratio at below 1:10.By the constraint of line environment and aerial lift device with insulated arm, require that top workbench overall weight can not be excessive, general at 500 ~ 600Kg at present.According to research experience, within auxiliary little arm should be limited in 100Kg, therefore require that little arm should reach the top load ratio of about 3:1, and the system that ensures has enough rigidity, to ensure end-of-pipe control precision and stability.Existing little boom structure is simple, and control mode mostly adopts hydraulic valve Non-follow control, without position sensor, accurately controls so can not carry out position.

Summary of the invention

For solving the deficiency that prior art exists, the invention discloses control system and the control method thereof of the little arm of large prudent hydraulic pressure, the arm that this control system adopts principal and subordinate to add autonomous control mode replacement people completes 10kV distribution line and changes cross-arm, changes the heavily loaded job tasks such as transformer.

For achieving the above object, concrete scheme of the present invention is as follows:

The control system of the little arm of large prudent hydraulic pressure, comprise: remote controller is communicated with industrial computer by optical fiber receiver-transmitter module, industrial computer connection signal processing module, the analog quantity that signal processing module exports is connected with multiple hydraulic efficiency servo-valve by servo amplifier, the digital quantity that signal processing module exports is sent to multiple switch valve, the instruction of remote controller is handed down to industrial computer by optical fiber receiver-transmitter module, and industrial computer issues control instruction, switch drive instruction to little arm by signal processing module.

Described hydraulic efficiency servo-valve is connected with the first waist gyroaxis of corresponding little arm mechanical arm, large arm pitch axis, forearm pitch axis, the second waist gyroaxis and paw folding axle respectively.First waist gyroaxis, large arm pitch axis, forearm pitch axis, the second waist gyroaxis and paw folding axle are connected with corresponding simulation potentiometer respectively, and simulation potentiometer is all by sensor compaction system connection signal processing module.

Described remote controller comprises: analogue collection module, digital data acquisition module are connected with microprocessor respectively by magnetic lotus root isolation module I, magnetic lotus root isolation module II, microprocessor is connected successively with magnetic lotus root isolation module III, serial port drive circuit, optical fiber sending module, reset circuit, jtag circuit, crystal oscillating circuit are connected with microprocessor respectively, analogue collection module gathers the data of main hand potentiometer, the data of the locking button of digital data acquisition module acquires, Self-resetting button, and after microprocessor processes, through optical fiber receiver-transmitter module, send to industrial computer.

Described remote controller simulates main hand with 5DOF, for controlling movement velocity, the position in each joint of the little arm of 5DOF; Remote controller is arranged 11 buttons, be respectively used to hydraulic control switch, master/slave coordinate system switch key, paw locking, little arm time-out, a key reduces, X-axis positive direction world coordinate system moves, X-axis negative direction world coordinate system moves, Y-axis positive direction world coordinate system moves, Y-axis negative direction world coordinate system moves, Z axis positive direction world coordinate system moves, Z axis negative direction world coordinate system moves; Remote controller is also arranged 7 groups of status indicator lamps, i.e. hydraulic selector, paw locking, little arm time-out, a key reduction, single axial movement, the instruction of world coordinate system motion state, security alarm instruction.

Described analogue collection module gathers 5DOF and simulates main hand signal, voltage range 0 ~ 10V, the switching signal of digital data acquisition module acquires 11 buttons, high level 5V, low level 0V, and digital output module output switching signal controls 7 groups of status indicator lamps.

Described magnetic lotus root isolation module I, magnetic lotus root isolation module II and magnetic lotus root isolation module III are four-way digital isolator, two ends operating voltage 2.7V ~ 5.5V, support low voltage operating and can realize level conversion.

Described signal processing module exports 5 road analog signalses, current range 0 ~ 20ma; Export 3 way switch amount signals.

Described little arm is 5DOF hydraulic press mechanical arm; Angular transducer is located on little arm, the angular dimension of each axle of angular transducer measuring machine mechanical arm, mechanical arm frame for movement comprises pedestal, pedestal is provided with large arm, large arm can realize waist revolution, large arm pitching, and upper end is shoulder joint, and forearm is arranged in shoulder joint; forearm can realize wrist revolution, and the front end of forearm is connected with paw.

Main and slave terminal two parts are divided into during the main hand control Systematical control of the little arm of large prudent hydraulic pressure, main side sends order to from end, main side is fed back to from end, operate main hands movement, the little arm of industrial computer Real-time Collection is simulated the positional information of potentiometer and positional information is sent to main hand controls, and industrial computer is by following the tracks of the motion of mechanical arm to the position of main hand.

The invention also discloses the control method of the little arm of large prudent hydraulic pressure, comprise the steps:

1) at joint shaft place, a Cartesian coordinates (x is set up to each rod member _i, y _i, z _i), i is all positive integers between 1 to 5, and 5 is number of degrees of freedom, adds base coordinate system (x ₀, y ₀, z ₀);

2) for the member coordinates of each joint sets up 4 × 4 homogeneous transformation matrix, the relation of the coordinate system of the previous rod member of expression and previous rod member and current rod member;

3) adopt the timing interpolation algorithm of " calculation limit, limit is walked ", calculate position and the attitude of interpolated point; The joint position that " calculation limit, limit is walked " obtains after referring to and each interpolated point being carried out inverse kinematics conversion need not store, and directly again by these joint position setting in motions;

4) calculate the inverse kinematic of each axle, inverse kinematic refers to that known terminal position and attitude ask the angle in each joint, draws the movement angle of each axle in interpolation cycle.

In described step 1) in determine and set up each coordinate system to adopt three rules below: the motion of each joint i is around z _iaxle moves; x _ithe vertical z of axle _i-1axle also points to and leaves z _i-1the direction of axle; y _iaxle must ask foundation by right-handed coordinate system.

Beneficial effect of the present invention:

1. the little arm control system of the large prudent hydraulic pressure of designed, designed of the present invention is through experimental verification, replaces the arm of people to complete 10kV distribution charged for replacement cross-arm, transformer heavy duty task.

2. little arm control system can realize a key restoring function, and little arm can be made to restPose very soon.

3. control system can realize principal and subordinate and autokinetic movement dual-use function, powerful, easy to operate.

4. master controller adopts main industrial computer, and main industrial computer realizes the positive and negative solution of kinematics, interpolation algorithm, and can realize complicated motion control, processing speed is fast.

5. control system is with security alarm function, automatically stops and reporting to the police when exceeding rated load.

6. remote controller and mechanical arm control section, carry out electrical isolation by optical fiber, ensure that the safety of personnel.

Accompanying drawing explanation

Fig. 1 is structural principle general diagram of the present invention;

Fig. 2 is remote controller schematic diagram of the present invention;

Fig. 3 is mechanical arm structure chart of the present invention;

Fig. 4 is the present invention one key reduction principle figure;

Fig. 5 is the main hand control flow chart of the present invention;

Wherein, 1, remote controller; 2, optical fiber sending module; 3, optic fiber transceiver module; 4, industrial 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 data acquisition 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 JointTestActionGroup, i.e. combined testing action group.At present, JTAG has become a kind of international standard test protocol, is mainly used in the inside examination of all kinds of chip.

Detailed description of the invention:

Below in conjunction with accompanying drawing, the present invention is described in detail:

See Fig. 1, the little arm control system of a kind of large prudent hydraulic pressure, comprises remote controller 1, optical fiber sending module 2, optic fiber transceiver module 3, industrial computer 4, signal processing module 5, simulation potentiometer 6, sensor compaction system 7, servo valve 8, switch valve 9.

The output of remote controller 1 control panel connects the input of data acquisition board, and the output of data acquisition board connects optical fiber sending module 2; The output of optic fiber transceiver module 3 connects the input of industrial computer 4 by serial ports, the output of industrial computer 4 connects the input of signal processing module 5 by serial ports, and the output of signal processing module 5 connects servo valve 8 and switch valve 9; Simulation potentiometer 6 connects the input of industrial computer 4 by sensor compaction system 7.The instruction of described remote controller 1, is handed down to industrial computer 4 by optical fiber sending module 2 and optic fiber transceiver module 3, and industrial computer 4 passes through the instruction of signal processing module 5 downloading speed, switch drive instruction to hydraulic system.

See Fig. 2, described remote controller 1 collection plate comprises analogue collection module 10, digital data acquisition module 11, magnetic lotus root isolation module I12, 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 gathers the data of 7 tunnel simulation rocking bars, digital data acquisition module 11 gathers the data of locking button, Self-resetting button, and after microprocessor 17 processes, through serial port drive circuit 18, wireless sending module 2, send to wireless receiving module 3.

See Fig. 3, described mechanical arm is 5DOF, and frame for movement comprises pedestal, pedestal is provided with large arm, and large arm can realize waist revolution, large arm pitching, and upper end is shoulder joint; forearm is arranged in shoulder joint, and forearm can realize wrist revolution, and the front end of forearm is connected with paw.

See Fig. 4, when the little arm of large prudent hydraulic pressure is in automatic motion mode, perform, until press the pause button on control panel according to the motion control program automatic cycle write in advance.

Reset operation routine, whether SR is pressed, if not, routine terminates, if pressed, continue to judge whether Start button is pressed, if not, wait for, if pressed, judge whether reset symbol is zero, if be zero, then start reseting movement, detect reseting mark switch, complete until reset, terminate, if reset symbol is non-vanishing, then start zero motion, put in place detection routine, until it is complete to reset.

See Fig. 5, the main hand control system of the little arm of large prudent hydraulic pressure is divided into main and slave terminal two parts.Main side sends order to from end, main side is fed back to from end, operator operates main hands movement, positional information is also sent to main hand controls by the positional information of little arm controller Real-time Collection little arm potentiometer, mechanical arm controller is by following the tracks of the motion of mechanical arm to the position of main hand, this positional information is sent to handheld terminal, for display by main hand controls more simultaneously.Main hand controls is a part for little arm control system, and main hand control system is the control system of remote controller.

Initialization controller, main hand/coordinate system switching key, then judge whether main hand control, detect and the detection of main hand position if start mechanical arm place value, when mechanical arm place value detects, send place value information to main hand controls, then main hand, mechanical arm place value information are subtracted each other, then judge parameter B>e>0, if met, then main hand moves to positive direction, if-B<e<0, then main hand is to motion in the other direction.

In the application, microprocessor adopts 32 TMS320F2812DSP chips, and dominant frequency can reach 150MHz.

Magnetic lotus root isolation module selects the ADuM1400/1/2 four-way digital isolator of ADI company.Two ends operating voltage 2.7V ~ 5.5V, supports low voltage operating and can realize level conversion.

Described analogue collection module gathers 5DOF and simulates main hand signal, voltage range 0 ~ 10V, the switching signal of digital data acquisition module acquires 11 buttons, high level 5V, low level 0V, and digital output module output switching signal controls 7 groups of status indicator lamps.

5DOF is simulated main hand and is possessed the functions such as simple joint control, simple joint position feedback and security monitoring.

Industrial computer adopts the small-sized industrial computer of ARK-5260, and dominant frequency 1.66GHz, supports 4 tunnel 485 interfaces, carry 2 road pci expansion slots, 12-24V DC power supply.

Signal processing module exports 5 road analog signalses, current range 0 ~ 20ma; Export 1 way switch amount signal, drive current 10A, 12V; Input 5 road analog signalses, voltage range 0 ~ 10V.

Angular transducer adopts WX13-12-15K individual pen precision potentiator, tolerance 0.05, nominal resistance 0 ~ 15k Ω.

Ship heavy industry CSDY1 jet pipe servo valve during hydraulic efficiency servo-valve adopts, supply current 0 ~ 20ma.

Fiber optic telecommunications module comprises optical fiber sending module 2, optic fiber transceiver module 3, for the communication between remote controller and industrial computer, adopt technical grade RS485 bus fibre repeater, fiber type: single mode, multimode are optional, light opening connector: standard configuration ST, optical fiber adopts single-mode fiber, wavelength 1310nm, transmission range 0 ~ 20Km.

The invention also discloses the autonomous control method of a kind of large prudent little arm, comprise the steps:

1) at joint shaft place, a Cartesian coordinates (x is set up to each rod member _i, y _i, z _i), i is all positive integers between 1 to 5, and 5 is number of degrees of freedom, adds base coordinate system (x ₀, y ₀, z ₀) (position on support and direction can be optional, as long as z ₀axle is along the first articulating shaft);

2) for the member coordinates of each joint sets up 4 × 4 homogeneous transformation matrix, the relation with previous rod member (the previous rod member of current rod member) coordinate system is represented;

3) adopt the timing interpolation algorithm of " calculation limit, limit is walked ", calculate position and the attitude of interpolated point; The joint position that " calculation limit, limit is walked " obtains after referring to and each interpolated point being carried out inverse kinematics conversion need not store, and directly again by these joint position setting in motions;

4) adopt equation to calculate the inverse kinematic (inverse kinematic refers to that known terminal position and attitude ask the angle in each joint) of each axle, draw the movement angle of each axle in interpolation cycle;

In described step 1) to determine and set up each coordinate system to adopt three rules below: the motion of each joint i (i is all positive integers between 1 to 5, and 5 is number of degrees of freedom) is all around z _iaxle moves; x _ithe vertical z of axle _i-1axle also points to and leaves z _i-1the direction of axle; y _iaxle must ask foundation by right-handed coordinate system.

This invention space line interpolation can be divided into the following steps to complete:

The initial point P of machine entered people motion ₀(x ₀, y ₀, z ₀) and terminal P _f(x _f, y _f, z _f) (f is the abbreviation of final) movement velocity P _v, Acceleration and deceleration time T _awith interpolation cycle T _c, running time T;

The determination of basic parameter and the method for solving of interpolated point.Because robot space line motion through acceleration and deceleration and uniform motion section, therefore before carrying out moving interpolation, need should determine P _vwhether meet acceleration and deceleration requirement.Method is as follows:

By P ₀(x ₀, y ₀, z ₀) and P _f(x _f, y _f, z _f) obtain actual motion distance P _d=| P ₀p _f|; By P _vand T _aaccelerating and decelerating part required separation distance can be calculated if C _d>=P _d, then actual motion speed otherwise C _v=P _v; By time T _awith interpolation time T _cdraw and accelerate step number S _a.By P ₀(x ₀, y ₀, z ₀) and P _f(x _f, y _f, z _f), space the parametric equation of the straight line can be obtained

$\left\{\begin{array}{c}x=k(x_f-x_0)+x_0\\ y=k(y_f-y_0)+y_0\\ z=k(z_f-z_0)+z_0\end{array}\right.$ Wherein k is scale factor (0≤k≤1) (1)

Therefore by formula (1), each interpolated point P can be obtained _i(x _i, y _i, z _i) (i is the step number of each interpolated point, 0 with between all positive integers) to P ₀distance be

$C_{Sd\left(i\right)}=\left|P_i{P}_0\right|=\sqrt{{(x_i-x_0)}^2+{(y_i-y_0)}^2+{(z_i-z_0)}^2}={\mathrm{kP}}_d$ (C _{sd (i)}represent P _i(x _i, y _i, z _i) to P ₀distance, P _d=| P ₀p _f|) (2)

The n-th interpolation section move distance is made to be S _{d (n)}(n=1 ..., i) (n be 1 to all positive integers of i), (i is the step number of each interpolated point, 0 with between all positive integers) can invocation point P _ito P ₀distance $C_{Sd\left(i\right)}=\underset{n=1}{\overset{i}{\&Sigma;}}{S}_{d\left(n\right)}=C_{Sd(i-1)}+S_{d\left(i\right)}(C_{Sd(i-1)}$ Represent P _i-1(x _i-1, y _i-1, z _i-1) to P ₀distance, S _{d (i)}the i-th interpolation section move distance), therefore it is as follows to obtain the computing formula of each interpolated point scale factor k by formula (1) and (2):

$k=\frac{C_{Sd\left(i\right)}}{P_d}=\frac{\underset{n=1}{\overset{i}{\&Sigma;}}{S}_{d\left(n\right)}}{P_d}=\frac{C_{Sd(i-1)}+S_{d\left(i\right)}}{P_d}$ Wherein k is scale factor (0≤k≤1) (3)

Just can obtain k by formula (3), and obtain interpolated point rectangular co-ordinate.Therefore space line interpolation algorithm key is to determine each interpolation section move distance S _{d (i)}.Introduce each section of motion below and ask for S _{d (i)}method:

Accelerated motion section.Because the robot accelerating sections designed herein is uniformly accelerated motion, therefore by actual motion speed C _vwith Acceleration and deceleration time T _atry to achieve acceleration (unit is m/s^2), therefore the speed S of upper i-th interpolated point of acceleration section _{cv (i)}=iT _ca, can obtain

$S_{d\left(i\right)}=\frac{1}{2}(S_{cv\left(i\right)}+S_{cv(i-1)})\&CenterDot;T_c=\frac{1}{2}(2i-1){{\mathrm{aT}}_c}^2$ (S _{cv (i-1)}represent the speed of the i-th-1 interpolated point) (4)

Uniform motion section.Can because the robot designed herein requires to have to pass through braking section, and interpolation operation be " calculation limit, limit is walked ", therefore carry out before uniform motion section starts, must calculating remaining distance and meeting system slowdown requirement at every turn.At the uniform velocity section each interpolation section move distance S _{d (i)}=C _vt _c

Retarded motion section.Owing to asking for acceleration step number S _ashi Jinhang rounds calculating, therefore simply can not plan accelerating sections after the negate of the section of will speed up acceleration, can introduce error like this, therefore braking section acceleration should recalculate.After i-1 interpolated point above, remaining distance L can be obtained _{d (i)}=P _d-C _{sd (i-1)}, therefore can obtain braking section acceleration the then speed S of deceleration Duan Shang m interpolated point _{cv (m)}=C _v+ mT _ca, can obtain

$S_{d\left(m\right)}=\frac{1}{2}(S_{cv\left(m\right)}+S_{cv(m-1)})\&CenterDot;T_c=\frac{1}{2}\&lsqb;2C_v+a\&CenterDot;(2m-1)\&CenterDot;T_c\&rsqb;---\left(5\right)$

This invention adopt equation carry out inverse kinematic (inverse kinematic refers to that known terminal position and attitude ask the angle in each joint:

$\begin{array}{c}{p}_x\\ p_y\\ p_z\end{array}$ ------represents the position of mechanical arm tail end in world coordinate system;

$\begin{array}{ccc}{n}_x& o_x& a_x\\ n_y& o_y& a_y\\ n_z& o_z& a_z\end{array}$ ------represents the attitude of mechanical arm tail end in world coordinate system;

θ ₁..., θ ₆------represents the angle that each axle moves;

A _i∈ R ^{4 × 4}(i=1,2 ..., 6) and------be transition matrix on each connecting rod according to D-H establishment of coordinate system between coordinate system.

S _i-----represents sin θ _i;

C _i------represents cos θ _i;

S _ij------represents sin (θ _i+ θ _j);

C _ij-------represent cos (θ _i+ θ _j)

$A_1=\left[\begin{array}{cccc}cos\left({\mathrm{\&theta;}}_1\right)& -sin\left({\mathrm{\&theta;}}_1\right)& 0& 0\\ sin\left({\mathrm{\&theta;}}_1\right)& \cos \left({\mathrm{\&theta;}}_1\right)& 0& 0\\ 0& 0& 1& 0\\ 0& 0& 0& 1\end{array}\right]$ The homogeneous transform matrix of denotation coordination system 1 and coordinate system 0

$A_2=\left[\begin{array}{cccc}cos\left({\mathrm{\&theta;}}_2\right)& -sin\left({\mathrm{\&theta;}}_2\right)& 0& 0\\ 0& 0& 1& 0\\ -sin\left({\mathrm{\&theta;}}_2\right)& -\cos \left({\mathrm{\&theta;}}_2\right)& 0& 0\\ 0& 0& 0& 1\end{array}\right]$ The homogeneous transform matrix of denotation coordination system 2 and coordinate system 1

$A_3=\left[\begin{array}{cccc}cos\left({\mathrm{\&theta;}}_3\right)& -sin\left({\mathrm{\&theta;}}_3\right)& 0& a_2\\ sin\left({\mathrm{\&theta;}}_3\right)& \cos \left({\mathrm{\&theta;}}_3\right)& 0& 0\\ 0& 0& 1& 0\\ 0& 0& 0& 1\end{array}\right]$ The homogeneous transform matrix of denotation coordination system 3 and coordinate system 2

$A_4=\left[\begin{array}{cccc}\cos \left({\mathrm{\&theta;}}_4\right)& -\sin \left({\mathrm{\&theta;}}_4\right)& 0& 0\\ 0& 0& 1& d_4\\ -\sin \left({\mathrm{\&theta;}}_4\right)& -\cos \left({\mathrm{\&theta;}}_4\right)& 0& 0\\ 0& 0& 0& 1\end{array}\right]$ The homogeneous transform matrix of denotation coordination system 4 and coordinate system 3

$A_5=\left[\begin{array}{cccc}cos\left({\mathrm{\&theta;}}_5\right)& -sin\left({\mathrm{\&theta;}}_5\right)& 0& 0\\ 0& 0& -1& 0\\ sin\left({\mathrm{\&theta;}}_5\right)& \cos \left({\mathrm{\&theta;}}_5\right)& 0& 0\\ 0& 0& 0& 1\end{array}\right]$ The homogeneous transform matrix of denotation coordination system 5 and coordinate system 4

$A_6=\left[\begin{array}{cccc}cos\left({\mathrm{\&theta;}}_6\right)& -sin\left({\mathrm{\&theta;}}_6\right)& 0& 0\\ 0& 0& 1& 0\\ -sin\left({\mathrm{\&theta;}}_6\right)& -\cos \left({\mathrm{\&theta;}}_6\right)& 0& 0\\ 0& 0& 0& 1\end{array}\right]$ The homogeneous transform matrix of denotation coordination system 6 and coordinate system 5

${T_0}_6({\mathrm{\&theta;}}_1,...,{\mathrm{\&theta;}}_6)=\left[\begin{array}{cccc}{n}_x& o_x& a_x& p_x\\ n_y& o_y& a_y& p_y\\ n_z& o_z& a_z& p_z\\ 0& 0& 0& 1\end{array}\right]$ Which provide a determination manipulator end relative to the position of support coordinate system and attitude.

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 O _i(i is positive integer i=0,1 ..., 6) be then the D-H coordinate system set up on motion arm; a ₂, d ₄∈ R represents the length of mechanical arm respective link respectively.Can by motion arm ending coordinates system O ₆at base coordinate system O ₀under pose write as following expression: ⁰t ₆=A ₁a ₂a ₃a ₄a ₅a ₆.When solving the equation of motion, from ⁰t ₆start to solve joint position.Make ⁰t ₆each element of character expression equal ⁰t ₆general type, and determine θ accordingly ₁.Once try to achieve θ ₁afterwards, can by A ₁ ^-1premultiplication ⁰t ₆general type,

A ^-1 ₁ ⁰T ₆＝ ¹T ₆(7)

This formula can be used to solve other each joint variables.Constantly use the inverse matrix premultiplication (7) of A, following another four matrix equation can be obtained:

A ₂ ^-1A ₁ ^-10T ₆＝ ²T ₆(8)

A ₃ ^-1A ₂ ^-1A ₁ ^-10T ₆＝ ³T ₆(9)

A ₄ ^-1A ₃ ^-1A ₂ ^-1A ₁ ^-10T ₆＝ ⁴T ₆(10)

A ₅ ^-1A ₄ ^-1A ₃ ^-1A ₂ ^-1A ₁ ^-10T ₆＝ ⁵T ₆(11)

The levoform of each equation of above formula is ⁰t ₆with the function of front (i-1) individual joint variable, the position in each joint can be determined with these equations:

θ ₁＝atan2(p _y,p _x)(-3.1415≤θ ₁≤3.1415)(12)

$k=\frac{{p_x}^2+{p_y}^2+{p_z}^2-{a_2}^2-{d_4}^2}{2a_2}$

${\mathrm{\&theta;}}_3=-atan2(k,\&PlusMinus;\sqrt{{d_4}^2-k^2})(-3.9268\&le;{\mathrm{\&theta;}}_1\&le;0.7853)---\left(13\right)$

Attention: in formula, positive and negative number corresponding θ ₃two kinds may separate.

$\begin{array}{cc}{s}_{23}=\frac{-a_2{c}_3{p}_z+(c_1{p}_x+s_1{p}_y)(a_2{s}_3-d_4)}{{p_z}^2+{(c_1{p}_x+s_1{p}_y)}^2}& c_{23}=\frac{(-d_4+a_2{s}_3)p_z+(c_1{p}_x+s_1{p}_y)a_2{c}_3}{{p_z}^2+{(c_1{p}_x+s_1{p}_y)}^2}\end{array}$

θ ₂＝atan2(s ₂₃,c ₂₃)-θ ₃(-1.5707≤θ ₁≤1.5707)(14)

θ ₄＝atan2(-a _xs ₁+a _yc ₁,-a _xc ₁c ₂₃-a _ys ₁c ₂₃+a _zs ₂₃)(-3.1415≤θ ₁≤3.1415)(15)

Attention: work as s ₅when=0, mechanical arm is in Singularities.Now, joint shaft 4 and 6 overlaps, and can only solve θ ₄with θ ₆and or poor.Whether Singularities all can be judged close to zero by the Two Variables of atan2 in formula (15).

θ ₅＝atan2(-a _x(c ₁c ₂₃c ₄+s ₁s ₄)-a _y(s ₁c ₂₃c ₄-c ₁s ₄)+a _zs ₂₃c ₄,-a _xc ₁s ₂₃-a _ys ₂₃s ₁-a _zc ₂₃)

(-3.9268≤θ ₁≤0.7853)(16)

k ₁＝-n _x(c ₁c ₂₃s ₄-s ₁c ₄)-n _y(s ₁c ₂₃s ₄+c ₁c ₄)+n _zs ₂₃s ₄

k ₂＝n _x((c ₁c ₂₃c ₄+s ₁s ₄)c ₅-c ₁s ₂₃s ₅)+n _y((s ₁c ₂₃c ₄-c ₁s ₄)c ₅-s ₁s ₂₃s ₅)-n _z(s ₂₃c ₄c ₅+c ₂₃s ₅)

θ ₆＝atan2(k ₁,k ₂)(-3.1415≤θ ₁≤3.1415)(17)

By reference to the accompanying drawings the specific embodiment of the present invention is described although above-mentioned; but not limiting the scope of the invention; one of ordinary skill in the art should be understood that; on the basis of technical scheme of the present invention, those skilled in the art do not need to pay various amendment or distortion that creative work can make still within protection scope of the present invention.

Claims (10)

1. the control system of the little arm of large prudent hydraulic pressure, it is characterized in that, comprise: remote controller is communicated with industrial computer by optical fiber receiver-transmitter module, industrial computer connection signal processing module, the analog quantity that signal processing module exports is connected with multiple hydraulic efficiency servo-valve by servo amplifier, the digital quantity that signal processing module exports is sent to multiple switch valve, the instruction of remote controller is handed down to industrial computer by optical fiber receiver-transmitter module, and industrial computer issues control instruction, switch drive instruction to little arm by signal processing module.

2. the control system of the little arm of large prudent hydraulic pressure as claimed in claim 1, it is characterized in that, described hydraulic efficiency servo-valve is connected with the first waist gyroaxis of corresponding little arm mechanical arm, large arm pitch axis, forearm pitch axis, the second waist gyroaxis and paw folding axle respectively.First waist gyroaxis, large arm pitch axis, forearm pitch axis, the second waist gyroaxis and paw folding axle are connected with corresponding simulation potentiometer respectively, and simulation potentiometer is all by sensor compaction system connection signal processing module.

3. the control system of the little arm of large prudent hydraulic pressure as claimed in claim 1, it is characterized in that, described remote controller comprises: analogue collection module, digital data acquisition module is respectively by magnetic lotus root isolation module I, magnetic lotus root isolation module II is connected with microprocessor, microprocessor and magnetic lotus root isolation module III, serial port drive circuit, optical fiber sending module connects successively, reset circuit, jtag circuit, crystal oscillating circuit is connected with microprocessor respectively, analogue collection module gathers the data of main hand potentiometer, the locking button of digital data acquisition module acquires, the data of Self-resetting button, and after microprocessor processes, through optical fiber receiver-transmitter module, send to industrial computer.

4. the control system of the little arm of large prudent hydraulic pressure as claimed in claim 1, it is characterized in that, described remote controller simulates main hand with 5DOF, for controlling movement velocity, the position in each joint of the little arm of 5DOF; Remote controller is arranged 11 buttons, be respectively used to hydraulic control switch, master/slave coordinate system switch key, paw locking, little arm time-out, a key reduces, X-axis positive direction world coordinate system moves, X-axis negative direction world coordinate system moves, Y-axis positive direction world coordinate system moves, Y-axis negative direction world coordinate system moves, Z axis positive direction world coordinate system moves, Z axis negative direction world coordinate system moves; Remote controller is also arranged 7 groups of status indicator lamps, i.e. hydraulic selector, paw locking, little arm time-out, a key reduction, single axial movement, the instruction of world coordinate system motion state, security alarm instruction.

5. the control system of the little arm of large prudent hydraulic pressure as claimed in claim 1, it is characterized in that, described analogue collection module gathers 5DOF and simulates main hand signal, voltage range 0 ~ 10V, the switching signal of digital data acquisition module acquires 11 buttons, high level 5V, low level 0V, digital output module output switching signal controls 7 groups of status indicator lamps.

6. the control system of the little arm of large prudent hydraulic pressure as claimed in claim 1, it is characterized in that, described magnetic lotus root isolation module I, magnetic lotus root isolation module II and magnetic lotus root isolation module III are four-way digital isolator, two ends operating voltage 2.7V ~ 5.5V, supports low voltage operating and can realize level conversion;

Described signal processing module exports 5 road analog signalses, current range 0 ~ 20ma; Export 1 way switch amount signal.

7. the control system of the little arm of large prudent hydraulic pressure as claimed in claim 1, is characterized in that, described little arm is 5DOF hydraulic press mechanical arm; Angular transducer is located on little arm, the angular dimension of each axle of angular transducer measuring machine mechanical arm, mechanical arm frame for movement comprises pedestal, pedestal is provided with large arm, large arm can realize waist revolution, large arm pitching, and upper end is shoulder joint, and forearm is arranged in shoulder joint; forearm can realize wrist revolution, and the front end of forearm is connected with paw.

8. the control system of the little arm of large prudent hydraulic pressure as claimed in claim 1, it is characterized in that, main and slave terminal two parts are divided into during the main hand control Systematical control of the little arm of large prudent hydraulic pressure, main side sends order to from end, main side is fed back to from end, operate main hands movement, positional information is also sent to main hand controls by the positional information of little arm controller Real-time Collection little arm potentiometer, mechanical arm controller is by following the tracks of the motion of mechanical arm to the position of main hand, this positional information is sent to handheld terminal, for display by main hand controls more simultaneously.

9. the control method of the little arm of large prudent hydraulic pressure, is characterized in that, comprise the steps:

1) at joint shaft place, a Cartesian coordinates (x is set up to each rod member _i, y _i, z _i), i is all positive integers between 1 to 5, and 5 is number of degrees of freedom, adds base coordinate system (x ₀, y ₀, z ₀);

2) for the member coordinates of each joint sets up 4 × 4 homogeneous transformation matrix, the relation of the coordinate system of the previous rod member of expression and previous rod member and current rod member;

3) adopt the timing interpolation algorithm of " calculation limit, limit is walked ", calculate position and the attitude of interpolated point; The joint position that " calculation limit, limit is walked " obtains after referring to and each interpolated point being carried out inverse kinematics conversion need not store, and directly again by these joint position setting in motions;

4) calculate the inverse kinematic of each axle, inverse kinematic refers to that known terminal position and attitude ask the angle in each joint, draws the movement angle of each axle in interpolation cycle.

10. the control method of the little arm of large prudent hydraulic pressure as claimed in claim 9, is characterized in that, in described step 1) in determine and set up each coordinate system to adopt three rules below: the motion of each joint i is around z _iaxle moves; x _ithe vertical z of axle _i-1axle also points to and leaves z _i-1the direction of axle; y _iaxle must ask foundation by right-handed coordinate system.