CN103792956B - Method and device controlling spatial motion of intelligent arm - Google Patents

Abstract

The invention discloses a method and device for controlling the spatial motion of an intelligent arm. The method comprises the steps of determining a first coordinate of the tail end of an arm rest in an arm rest plane coordinate system and a second coordinate of the tail end of the arm rest in an arm rest projection coordinate system; when a coordinate system of a remote controller does not correspond to an arm rest spatial coordinate system, adjusting the arm rest projection coordinate system and the second coordinate or adjusting the coordinate system of the remote controller, determining a third coordinate of the intelligent arm, and conducting calculation to obtain the rotating angle of a rotating table; or when the coordinate system of the remote controller corresponds to the arm rest spatial coordinate system, conducting calculation to obtain the rotating angle of the rotating table according to the second coordinate and the third coordinate; calculating the coordinate variation of the intelligent arm; determining the angle difference of included angles between two adjacent segments of the arm rest of the intelligent arm according to an optimization algorithm; respectively controlling the motion of the rotating table and the arm rest of the intelligent arm according to the rotating angle of the rotating table and the angle difference. According to the method for controlling the spatial motion of the intelligent arm, the movement of the intelligent arm in the space can be conveniently and automatically controlled, the use performance of the intelligent arm rest is improved, and the operation difficulty of the arm rest can be reduced.

Description

Puma arm spatial movement control method and device

Technical field

The present invention relates to puma arm control technology field, particularly puma arm spatial movement control method and device.

Background technology

At present, for the control of concrete pump truck arm, single remote-control handle is substantially taked to control single jib or turntable.In general, concrete is transported to the random position of building by operator from pump truck hopper, then by workman, concrete is evenly spread out, although compare hand haulage's concrete to save a large amount of manpower and materials, but there is onerous toil, pumper operation person should be noted pumping vehicle arm rack attitude on the one hand, and consider the correctness of operation, maloperation easily causes jib clobber; If action jib amplitude is excessive, also injury can be brought to the person of assisting under arm support tail end flexible pipe.Workman's spread concrete is also pretty troublesome on the other hand.In this context, pumper operation person wishes that a kind of shirtsleeve operation mode is to realize concrete typical condition uniform distribution, and pump truck fabricator also wishes to produce more high performance concrete mixer, for it expands market share, thus, intelligent arm support technology is arisen at the historic moment.

Concrete pump truck arm (as puma arm) can be as shown in Figure 1 at three-dimensional coordinate (X Y Z), this is three-dimensional determines that mode can be as follows: in the space that pumping vehicle arm rack can reach, using pump truck turntable and hinged place, Section 1 jib top as true origin, with the hopper direction of vehicle body for x positive axis, with the direction perpendicular to vehicle body for y-axis positive axis, according to the cartesian coordinate system right-hand rule, z-axis can be determined; H represents intersection point.Certainly, with other point for true origin, can determine that other direction is x positive axis according to actual needs, and correspondingly determine y-axis and z-axis.

For the acquisition of concrete pump truck arm end spaces position at the projection h of surface level, adopt three-dimensional coordinate account form, that is:

$\left\{\begin{array}{c}X=\cos \left({\mathrm{\α}}_0\right)\·\underset{i=1}{\overset{n}{\mathrm{\Σ}}}{L}_i\·\cos {\mathrm{\α}}_i\\ Y=\underset{i=1}{\overset{n}{\mathrm{\Σ}}}{L}_i\·\sin {\mathrm{\α}}_i\\ Z=\sin \left({\mathrm{\α}}_0\right)\·\underset{i=1}{\overset{n}{\mathrm{\Σ}}}{L}_i\·\sin {\mathrm{\α}}_i\end{array}\right.---\left(1\right)$

Wherein, X, Y, Z are the D coordinates value of arm support tail end point, and n is the joint number of jib, L _ibe the length of the i-th joint jib, α ₀for the turntable anglec of rotation, α _i(i>0) angle of the i-th joint jib and surface level is represented.

Because the jib spatial movement planning carried out on the three-dimensional coordinate computing method basis such as shown in formula (1) is very complicated, all the spatial movement planning of jib is avoided mentioning in a lot of intelligent arm support Patents, directly the motion in jib space is reduced to linearly operating, does not namely consider the automatic control that turntable moves.If manual operation pump truck turntable coordinates the motion of jib, be difficult to again walk out more straight track, cause the actual usability of intelligent arm support greatly to reduce thus.

Summary of the invention

In view of this, the present invention proposes a kind of puma arm spatial movement control method and device, to realize automatically controlling puma arm movement in space, improves the usability of intelligent arm support, and can reduce jib operation easier.

On the one hand, provide a kind of puma arm spatial movement control method, be applied to and have the puma arm that n saves jib, wherein said n is more than or equal to 2, and described puma arm spatial movement control method comprises:

Receive each angular transducer sensing and the described puma arm that sends and respectively save angle between jib and surface level when being in current location;

When being in current location according to described puma arm, the coordinate of arm support tail end in jib space coordinates, determines first coordinate of described arm support tail end in jib plane coordinate system and the second coordinate in jib projected coordinate system;

The telepilot for controlling the motion of described puma arm coordinate system and described jib space coordinates not to time corresponding described jib projected coordinate system and the second coordinate is adjusted according to the coordinate system of described telepilot, or according to the coordinate system of described jib projected coordinate system adjustment telepilot, to make the movement angle of the arm support tail end of described puma arm in the coordinate system of described telepilot consistent with the movement angle in described jib projected coordinate system, and the movement instruction sent according to described telepilot and described second coordinate, the three-dimensional of arm support tail end in described jib projected coordinate system when determining that described puma arm is in target location, and the turntable anglec of rotation of described puma arm is calculated according to described second coordinate and described three-dimensional, or, described telepilot coordinate system and described jib space coordinates to time corresponding according to described movement instruction and described second coordinate, the three-dimensional of arm support tail end in described jib projected coordinate system when determining that described puma arm is in target location, and the turntable anglec of rotation of described puma arm is calculated according to described second coordinate and described three-dimensional,

Coordinate knots modification when puma arm moves to target location from current location described in the length computation respectively saving jib according to described first coordinate, the angle between described each joint jib and surface level, described puma arm jib plane coordinate system; And according to described coordinate knots modification determination constraint condition, then according to preset optimized algorithm, determine described puma arm adjacent two joint jibs between the differential seat angle of angle when described puma arm is in current location and target location;

According to the differential seat angle of angle between the described turntable anglec of rotation and described adjacent two joint jibs, control the motion of described puma arm turntable and jib respectively.

Further, determine that the method that the coordinate system of described telepilot is not corresponding with described jib space coordinates comprises: after receiving the Relatively orientation signal not corresponding with described jib space coordinates for the coordinate system characterizing described telepilot that described telepilot sends, determine that the coordinate system of described telepilot is not corresponding with described jib space coordinates.

Further, the step of described " adjusting described jib projected coordinate system and the second coordinate according to the coordinate system of described telepilot " comprising: the coordinate system of telepilot and the angle in preset reference direction according to described Relatively orientation signal acquisition, again according to the coordinate system of described telepilot and the angle in preset reference direction, adjust described jib projected coordinate system and the second coordinate, or the coordinate system of adjustment telepilot.

Further, described optimized algorithm is to make the quadratic sum of the differential seat angle of angle when described puma arm is in current location and target location between the adjacent two joint jibs of described puma arm minimum for optimization aim, and described optimization aim is expressed as: wherein, COEFFICIENT K _ithe handle aperture change saving jib Motor ability and described telepilot according to described puma arm i-th is determined; The Δ θ when i>1 _ibe the differential seat angle of angle when described puma arm is in current location and target location that the i-th-1 joint jib and i-th saves between jib; Δ θ ₁for the variable quantity of angle between Section 1 jib and surface level when current location and target location.

Further, described " determine described puma arm adjacent two joint jibs between the differential seat angle of angle when described puma arm is in current location and target location " step after also comprise:

Respectively save when angle determines described puma arm in target location between jib and surface level when current location according to described differential seat angle and described puma arm and respectively save angle between jib;

And respectively save maximal value and the minimum value of angle between jib according to the described puma arm preset, judge that described puma arm respectively to save between jib angle whether in the scope between the described minimum value and described maximal value of correspondence when target location;

Angle between jib is in the COEFFICIENT K that corresponding described minimum value is corresponding with the extraneous jib between described maximal value _ibe defined as zero, and redefine in described puma arm other joint arms and be adjacent the differential seat angle of angle when described puma arm is in current location and target location between jib.

Further, described " according to described coordinate knots modification determination constraint condition; again according to preset optimized algorithm, determine described puma arm adjacent two joint jibs between the differential seat angle of angle when described puma arm is in current location and target location " step before comprise:

Judge that described puma arm respectively saves when target location whether angle between jib is corresponding described minimum value or described maximal value;

And calculating described puma arm, respectively save angle between jib when target location be corresponding described minimum value or the jib quantity of described maximal value;

When described jib quantity is more than or equal to n-1, described puma arm is directly stopped respectively saving the action of jib;

When described jib quantity is less than n-1, perform " according to described coordinate knots modification determination constraint condition; again according to preset optimized algorithm, determine described puma arm adjacent two joint jibs between the differential seat angle of angle when described puma arm is in current location and target location " step.

Further, when determining that described puma arm is in current location, the seat calibration method of arm support tail end in jib space coordinates comprises:

According to finite element theory, calculate the n-th amount of elastic deformation f of joint arm under current load of described puma arm _n; And according to described amount of elastic deformation f _nand the length of described n-th joint arm, ask for the coordinate of arm support tail end under the local coordinate system of described n-th joint arm of described puma arm; And according to the angle between described each joint jib and surface level, realize the coordinate conversion of described arm support tail end under the local coordinate system of each joint arm by rotation of coordinate and translation; And calculate described arm support tail end current time and on be engraved in for the moment alternate position spike under the local coordinate system of same joint arm; Again according to described alternate position spike, when being in current location to described puma arm, the coordinate of arm support tail end in jib space coordinates carries out elastic deformation compensating operation.

Further, comprised before the step of described " according to described alternate position spike, when being in current location to described puma arm, the coordinate of arm support tail end in jib space coordinates carries out elastic deformation compensating operation ":

Judge whether described alternate position spike is greater than predetermined threshold value, when being greater than described predetermined threshold value, when being in current location to described puma arm, the coordinate of arm support tail end in jib space coordinates carries out elastic deformation compensating operation.

On the other hand, a kind of puma arm spatial movement control device is provided, be applied to and there is the puma arm that n saves jib, wherein said n is more than or equal to 2, described puma arm spatial movement control device comprises: receiving element, respectively saves the angle between jib and surface level when the described puma arm for receiving each angular transducer sensing and transmission is in current location, coordinate transformation unit, during for being in current location according to described puma arm, the coordinate of arm support tail end in jib space coordinates, determines first coordinate of described arm support tail end in jib plane coordinate system and the second coordinate in jib projected coordinate system, anglec of rotation determining unit, for the telepilot for controlling the motion of described puma arm coordinate system and described jib space coordinates not to time corresponding coordinate system according to the telepilot for controlling the motion of described puma arm adjusts described jib projected coordinate system and the second coordinate, or, according to the coordinate system of described jib projected coordinate system adjustment telepilot, to make the movement angle of the arm support tail end of described puma arm in the coordinate system of described telepilot consistent with the movement angle in described jib projected coordinate system, and the movement instruction sent according to described telepilot and described second coordinate, the three-dimensional of arm support tail end in described jib projected coordinate system when determining that described puma arm is in described target location, and the turntable anglec of rotation of described puma arm is calculated according to described second coordinate and described three-dimensional, or, described telepilot coordinate system and described jib space coordinates to time corresponding according to described movement instruction and described second coordinate, the three-dimensional of arm support tail end in described jib projected coordinate system when determining that described puma arm is in described target location, and the turntable anglec of rotation of described puma arm is calculated according to described second coordinate and described three-dimensional, angle difference determining unit, for respectively save jib according to described first coordinate, the angle between described each joint jib and surface level, described puma arm length computation described in puma arm move to target location from current location time coordinate knots modification jib plane coordinate system, and according to described coordinate knots modification determination constraint condition, then according to preset optimized algorithm, determine described puma arm adjacent two joint jibs between the differential seat angle of angle when described puma arm is in current location and target location, motion control unit, for according to the described turntable anglec of rotation and cantilever crane angle differential seat angle, controls the motion of described puma arm turntable and jib respectively.

Further, described anglec of rotation determining unit comprises: anglec of rotation determination subelement, for after the Relatively orientation signal not corresponding with described jib space coordinates for the coordinate system characterizing described telepilot receiving the transmission of described telepilot, determine that the coordinate system of described telepilot is not corresponding with described jib space coordinates, and according to described Relatively orientation signal acquisition the coordinate system of telepilot and the angle in preset reference direction, again according to the coordinate system of described telepilot and the angle in preset reference direction, adjust described jib projected coordinate system and the second coordinate, or the coordinate system of adjustment telepilot.

Puma arm spatial movement control method of the present invention and device are by splitting into two planar two dimensional coordinate by stereoscopic three-dimensional coordinate, the differential seat angle of angle when puma arm is in current location and target location between the adjacent two joint jibs solving puma arm and the turntable anglec of rotation, and be converted into the drive current of jib and turntable solenoid valve (banked direction control valves), motion during overall planning intelligent arm support spatial movement in jib plane and the motion of turntable, achieve the space line motion of arm support tail end, be convenient to automatically control puma arm movement in space, improve the usability of intelligent arm support, simultaneously telepilot coordinate system and jib space coordinates not to the time in of corresponding according to coordinate system adjustment jib projected coordinate system and second coordinate of telepilot, or the coordinate system of adjustment telepilot, operator can operate jib at arbitrary orientation, in addition, direction of operating on telepilot is consistent with the direction of action of jib, reduces jib operation easier.

Accompanying drawing explanation

The accompanying drawing forming a part of the present invention is used to provide a further understanding of the present invention, and schematic description and description of the present invention, for explaining the present invention, does not form inappropriate limitation of the present invention.In the accompanying drawings:

Fig. 1 is that existing puma arm is at three-dimensional coordinate schematic diagram;

The puma arm spatial movement control method process flow diagram that Fig. 2 provides for the embodiment of the present invention one;

Coordinate system decomposing schematic representation in the puma arm spatial movement control method that Fig. 3 provides for Fig. 2;

Coordinate system adjustment schematic diagram in the puma arm spatial movement control method that Fig. 4 provides for Fig. 2;

The puma arm spatial movement control method intermediate station anglec of rotation that Fig. 5 provides for the embodiment of the present invention solves schematic diagram;

In the puma arm spatial movement control method that Fig. 6 provides for the embodiment of the present invention, between adjacent jib, angle solves schematic diagram in the difference of diverse location;

The puma arm spatial movement control method process flow diagram that Fig. 7 provides for the embodiment of the present invention two;

Fig. 8 is the principle schematic of the coordinate conversion operation in Fig. 7 in puma arm amount of elastic deformation computation process;

Fig. 9 is the principle schematic of carrying out elastic deformation compensating operation in Fig. 7 according to alternate position spike;

The structured flowchart of the puma arm spatial movement control device that Figure 10 provides for the embodiment of the present invention.

Embodiment

It should be noted that, when not conflicting, the embodiment in the present invention and the feature in embodiment can combine mutually.Below with reference to the accompanying drawings and describe the present invention in detail in conjunction with the embodiments.

The process flow diagram of the puma arm spatial movement control method that Fig. 2 provides for the embodiment of the present invention, this puma arm spatial movement control method is applied to has the puma arm that n saves arm, and wherein n is more than or equal to 2; As shown in Figure 2, this puma arm spatial movement control method comprises:

Step 11: receive each angular transducer sensing and the puma arm that sends and respectively save angle between jib and surface level when being in current location; Wherein, each angular transducer (can also be oil cylinder displacement transducer etc. certainly) can be arranged on the root of each jib of puma arm.Be understandable that, detect each joint jib and the angle between surface level and detect each joint jib and be equal to the angle between vertical plane because two angles are remaining mutually, namely two angles and be 90 degree.

Step 12: the coordinate of arm support tail end in jib space coordinates when being in current location according to puma arm, determines first coordinate of arm support tail end in jib plane coordinate system and the second coordinate in jib projected coordinate system;

Specifically as shown in Figure 3, stereoscopic three-dimensional coordinate is split into two planar two dimensional coordinate, be respectively jib planimetric coordinates and jib projection coordinate, wherein, this jib planimetric coordinates is jib plane coordinate system [O; X, y] in coordinate, puma arm only has rectilinear movement not rotate in this jib plane, and this jib projection coordinate is jib projected coordinate system [O; X, z] in coordinate, puma arm only has at this jib projection plane and rotates not rectilinear movement; After setting up above-mentioned first coordinate and the second coordinate, in jib plane and jib projection plane, coordinated planning is carried out to the motion of puma arm, refer to following description.

Step 13: the telepilot for controlling puma arm motion coordinate system and jib space coordinates not to the time in of corresponding according to coordinate system adjustment jib projected coordinate system and second coordinate of the telepilot for controlling puma arm motion, or according to the coordinate system of described jib projected coordinate system adjustment telepilot, to make the movement angle of the arm support tail end of puma arm in the coordinate system of telepilot consistent with the movement angle in jib projected coordinate system; Specifically can comprise:

First, after the Relatively orientation signal not corresponding with jib space coordinates for the coordinate system characterizing telepilot receiving telepilot transmission, determine that the coordinate system of telepilot is not corresponding with jib space coordinates;

Secondly, according to the coordinate system of Relatively orientation signal acquisition telepilot and the angle in preset reference direction, then according to the coordinate system of telepilot and the angle in preset reference direction, adjustment jib projected coordinate system and the second coordinate, or the coordinate system of adjustment telepilot;

As shown in Figure 4, operate in jib process under telepilot intelligent mode, if telepilot (not shown) is in a point and its coordinate is the coordinate system set up with x1 axle and y1 axle, now, telepilot coordinate system is corresponding with jib space coordinates, namely with jib projected coordinate system just in time consistent (as Suo Shi Fig. 4 dotted line coordinate system (coordinate system set up with x1 ' axle and z1 ' axle)), without the need to adjusting jib projected coordinate system, now pull universal handle along OM direction, arm support tail end can be mobile along AB direction (it is consistent with OM direction), if telepilot is in b point and its coordinate is the coordinate system set up with x2 axle and y2 axle, now, telepilot coordinate system is not corresponding with jib space coordinates, a switch then in first remote controller, again universal handle is moved (as pulled to pump truck headstock direction toward the reference direction preset, namely in Fig. 4, unidirectional arrow P is shown), the motion of universal handle represents different implications under two states of switch, as switch is pressed in the present embodiment, it is not corresponding with jib space coordinates that the movable information of universal handle (i.e. telepilot send Relatively orientation signal) then represents telepilot coordinate system, and the movable information of this universal handle contains the angle information of itself and remote control coordinate axis, as the angle between unidirectional arrow P and coordinate axis y2, and unidirectional arrow P is default reference direction, therefore the actual conditions of its coordinate system when can learn that telepilot is in b point, calculate the difference of the angle of telepilot coordinate system and jib projected coordinate system, thus jib projected coordinate system (coordinate system set up with x1 ' axle and the z1 ' axle) coordinate system for setting up with x2 ' axle and z2 ' axle can be adjusted, make it consistent with the coordinate system set up with x2 axle and y2 axle, realize compensating, namely after universal handle, Relatively orientation between telepilot and pump truck completes, jib projection coordinate is the coordinate system that x2 ' axle and z2 ' axle are set up, now pull universal handle along ON direction and (now do not coordinate switch motion, the operation of universal handle directly represents the movement instruction for controlling puma arm action), jib moves along AB direction,

It should be noted that, the implication of above-mentioned universal handle motion under switch different conditions is not all a kind of mode, can use handle (as 1 arm handle) alternative switch during concrete operations, namely the implication of universal handle motion under handle different conditions can be different.Certainly, also can be benchmark with jib projection coordinate, adopt similar method adjustment telepilot coordinate system, make the coordinate system of telepilot consistent with jib projected coordinate system, by the movement instruction of telepilot by sending again after coordinate conversion, or first carry out corresponding conversion after the movement instruction receiving telepilot and control again.

Step 14: determine the coordinate system of telepilot and jib space coordinates not to time corresponding the movement instruction sent according to telepilot and the second coordinate after adjusting, when determining that puma arm is in target location, the three-dimensional of arm support tail end in jib projected coordinate system (is appreciated that, when determining three-dimensional, if adjust the second coordinate in preceding step, then need adopt be adjustment after the second coordinate, if adjust, be then still unadjusted second coordinate); Or, the coordinate system of telepilot with jib space coordinates to movement instruction time corresponding and unadjusted second coordinate, the three-dimensional of arm support tail end in jib projected coordinate system when determining that puma arm is in target location;

As shown in Figure 5, to suppose in a planning horizon (coordinate system in this figure telepilot coordinate system and jib space coordinates not to time corresponding be adjust after jib space coordinates; Telepilot coordinate system and jib space coordinates to the time in of corresponding for unadjusted jib space coordinates) wish that arm support tail end point moves to B point (correspondence position of arm support tail end point target position in jib projected coordinate system) from A point (correspondence position of arm support tail end current location jib projected coordinate system), B point coordinate value (x _n+1, z _n+1) be at A point coordinate value (x _n, z _n) basis on, add that the movement instruction of pump truck telepilot obtains, that is:

$\left\{\begin{array}{c}{x}_{n+1}=x_n+R{C}_x\\ z_{n+1}=z_n+R{C}_z\end{array}\right.---\left(2\right)$

Wherein RC _x, RC _zrepresent the parsing input quantity of the universal handle of pump truck telepilot in x, z direction respectively.

Step 15: determine the coordinate system of telepilot and jib space coordinates not to time corresponding the turntable anglec of rotation calculating puma arm according to the second coordinate after adjustment and three-dimensional (is appreciated that, when calculating turntable angle, if adjust the second coordinate in preceding step, then need adopt be adjustment after the second coordinate, if adjust, be then still unadjusted second coordinate); Or, determining that the coordinate system of telepilot and jib space coordinates are to the turntable anglec of rotation calculating puma arm time corresponding according to unadjusted second coordinate and Five Axis;

Continue see Fig. 5 (coordinate system in this figure telepilot coordinate system and jib space coordinates not to time corresponding be adjust after jib space coordinates; The coordinate system of telepilot with jib space coordinates to time corresponding being unadjusted jib space coordinates), specifically can calculate the turntable anglec of rotation by following manner

Wherein, be respectively the vector that arm support tail end is formed in the true origin of A point and B point and jib projected coordinate system, × be vector cross product sign of operation, for the length of corresponding vector, it is multiplying; Be understandable that, can also basis by arc cosine (acos), arc tangent (atan), arc cotangent (acot) the function calculating turntable anglec of rotation of trigonometric function.

Be understandable that, step 13-15 for the telepilot of described motion coordinate system and described jib space coordinates not to time corresponding according to the mode of adjustment telepilot coordinate system, during concrete operations, can directly according to the movement instruction that described telepilot sends, difference between the coordinate system of the telepilot of described second coordinate and described motion and described jib space coordinates, the three-dimensional of arm support tail end in described jib projected coordinate system when determining that described puma arm is in target location, and the turntable anglec of rotation of described puma arm is calculated according to described second coordinate and described three-dimensional,

Step 16: according to the first coordinate, measure inclination value when the puma arm that obtains is in current location between each jib and surface level, length computation puma arm that puma arm respectively saves jib coordinate knots modification when moving to target location from current location jib plane coordinate system;

For the motion of arm support tail end in jib plane, as shown in Figure 6, wherein angle θ _jand θ _j' (j can between 1 to n value natural number) represent respectively that when j>1 arm support tail end jth-1 when A ' and B ' saves jib and jth and saves angle between jib; The angle of arm support tail end when A ' and B ' between Section 1 jib and surface level is represented respectively when j=1; Suppose in a planning horizon, wish that arm support tail end point moves to B ' point (correspondence position of arm support tail end target location in jib plane) from A ' point (correspondence position of arm support tail end current location in jib plane), here A ' with the current location of the equal corresponding arm support tail end of A point in Fig. 4 at three bit spaces, B ' with the equal corresponding arm support tail end of B point in Fig. 4 in the target location of three bit spaces, save boom system for n, the coordinate of A ' represents can be as follows:

$\left\{\begin{array}{c}{x}_n=\underset{i=1}{\overset{n}{\mathrm{\Σ}}}{L}_i\·\cos (\underset{j=1}{\overset{i}{\mathrm{\Σ}}}{\mathrm{\θ}}_j-(i-1)\·\mathrm{\π})\\ y_n=\underset{i=1}{\overset{n}{\mathrm{\Σ}}}{L}_i\·\sin (\underset{j=1}{\overset{i}{\mathrm{\Σ}}}{\mathrm{\θ}}_j-(i-1)\·\mathrm{\π})\end{array}\right.---\left(4\right)$

The coordinate of B ' represents can be as follows:

$\left\{\begin{array}{c}{x}_{n+1}=\underset{i=1}{\overset{n}{\mathrm{\Σ}}}{L}_i\·\cos (\underset{j=1}{\overset{i}{\mathrm{\Σ}}}({\mathrm{\θ}}_j+\mathrm{\Δ}{\mathrm{\θ}}_j)-(i-1)\·\mathrm{\π})\\ y_{n+1}=\underset{i=1}{\overset{n}{\mathrm{\Σ}}}{L}_i\·\sin (\underset{j=1}{\overset{i}{\mathrm{\Σ}}}({\mathrm{\θ}}_j+\mathrm{\Δ}{\mathrm{\θ}}_j)-(i-1)\·\mathrm{\π})\end{array}\right.---\left(5\right)$

Wherein, Δ θ _j(during j>1) for jth-1 save jib and jth to save between jib angle B ' with variable quantity when A '; Δ θ ₁for angle between Section 1 jib and surface level B ' with variable quantity when A '; L _ibe the length of the i-th joint jib, n is the joint number of jib; Cantilever crane angle θ _jwith jib angle α _jcorresponding relation be:

${\mathrm{\α}}_j=\underset{i=1}{\overset{j}{\mathrm{\Σ}}}{\mathrm{\θ}}_i-(j-1)\·\mathrm{\π}---\left(6\right)$

Wherein, jib angle α _jfor the angle between jth joint jib and surface level, can be obtained by the angular transducer measurement being arranged on jth joint jib root;

Deduct formula (4) by formula (5), can obtain:

$\left\{\begin{array}{c}\mathrm{\Δx}=\underset{i=1}{\overset{n}{\mathrm{\Σ}}}{L}_i\·\left(\cos (\underset{j=1}{\overset{i}{\mathrm{\Σ}}}({\mathrm{\θ}}_j+\mathrm{\Δ}{\mathrm{\θ}}_j)-(i-1)\·\mathrm{\π})\text{-cos}(\underset{j=1}{\overset{i}{\mathrm{\Σ}}}{\mathrm{\θ}}_j-(i-1)\·\mathrm{\π})\right)\\ \mathrm{\Δy}=\underset{i=1}{\overset{n}{\mathrm{\Σ}}}{L}_i\·\left(\sin (\underset{j=1}{\overset{i}{\mathrm{\Σ}}}({\mathrm{\θ}}_j+\mathrm{\Δ}{\mathrm{\θ}}_j)-(i-1)\·\mathrm{\π})\text{-sin}(\underset{j=1}{\overset{i}{\mathrm{\Σ}}}{\mathrm{\θ}}_j-(i-1)\·\mathrm{\π})\right)\end{array}\right.---\left(7\right)$

Wherein, the coordinate knots modification of Δ x, Δ y jib plane coordinate system when to be arm support tail end move at B ' from A ' some.

Because the planning of the present embodiment locus carries out next step planning based on current location, the distance between current location and target location is very little, and we can carry out nonlinear equation linearization process to formula (7), that is:

f(x+Δx)＝f(x)+f(x)′·Δx (8)

According to formula (8), formula (7) can be reduced to:

$\left\{\begin{array}{c}\mathrm{\Δx}=\underset{i=1}{\overset{n}{\mathrm{\Σ}}}{a}_i\·\mathrm{\Δ}{\mathrm{\θ}}_i\\ \mathrm{\Δy}=\underset{i=1}{\overset{n}{\mathrm{\Σ}}}{b}_i\·\mathrm{\Δ}{\mathrm{\θ}}_i\end{array}\right.---\left(9\right)$

Wherein, $\left\{\begin{array}{c}{a}_k=-\underset{i=k}{\overset{n}{\mathrm{\Σ}}}{L}_i\·\sin (\underset{j=1}{\overset{i}{\mathrm{\Σ}}}{\mathrm{\θ}}_j-(i-1)\·\mathrm{\π})\\ b_k=\underset{i=k}{\overset{n}{\mathrm{\Σ}}}{L}_i\·\cos (\underset{j=1}{\overset{i}{\mathrm{\Σ}}}{\mathrm{\θ}}_j-(i-1)\·\mathrm{\π})\end{array}\right.$

Step 17: with coordinate knots modification determination constraint condition; Particularly, in the ideal situation in the motion process of arm support tail end, the ordinate of A ' and B ' remains unchanged; A ' is in Fig. 4 with the difference of the horizontal ordinate of B ' simultaneously therefore, can be expressed as according to formula (7) constraint condition:

$\left\{\begin{array}{c}\mathrm{\Δx}=\underset{i=1}{\overset{n}{\mathrm{\Σ}}}{L}_i\·\left(\cos (\underset{j=1}{\overset{i}{\mathrm{\Σ}}}({\mathrm{\θ}}_j+\mathrm{\Δ}{\mathrm{\θ}}_j)-(i-1)\·\mathrm{\π})\text{-cos}(\underset{j=1}{\overset{i}{\mathrm{\Σ}}}{\mathrm{\θ}}_j-(i-1)\·\mathrm{\π})\right)=\left|\stackrel{\→}{\mathrm{OB}}\right|-\left|\stackrel{\→}{\mathrm{OA}}\right|\\ \mathrm{\Δy}=\underset{i=1}{\overset{n}{\mathrm{\Σ}}}{L}_i\·\left(\sin (\underset{j=1}{\overset{i}{\mathrm{\Σ}}}({\mathrm{\θ}}_j+\mathrm{\Δ}{\mathrm{\θ}}_j)-(i-1)\·\mathrm{\π})\text{-sin}(\underset{j=1}{\overset{i}{\mathrm{\Σ}}}{\mathrm{\θ}}_j-(i-1)\·\mathrm{\π})\right)=0\end{array}\right.---\left(10\right)$

Wherein, be respectively the vector that arm support tail end is formed in the true origin of current location and target location and jib projected coordinate system, θ _j(during j>1) saves angle between jib for jth-1 saves jib and jth; θ ₁for the angle of Section 1 jib and surface level, L _ibe the length of the i-th joint jib;

Correspondingly, can be expressed as according to formula (9) constraint condition:

$\left\{\begin{array}{c}\mathrm{\Δx}=\underset{i=1}{\overset{n}{\mathrm{\Σ}}}{a}_i\·\mathrm{\Δ}{\mathrm{\θ}}_i=\left|\stackrel{\→}{\mathrm{OB}}\right|-\left|\stackrel{\→}{\mathrm{OA}}\right|\\ \mathrm{\Δy}=\underset{i=1}{\overset{n}{\mathrm{\Σ}}}{b}_i\·\mathrm{\Δ}{\mathrm{\θ}}_i\end{array}\right.---\left(11\right)$

Wherein, △ y equals height tolerance.

Step 18: according to the optimized algorithm preset, to make the differential seat angle quadratic sums of angle when puma arm is in current location and target location between the adjacent two joint jibs of puma arm minimum for optimization aim, determine that adjacent two of puma arm save the differential seat angle of angle when puma arm is in current location and target location between jibs;

Optimization aim can be expressed as:

$\min \left(\underset{i=1}{\overset{n}{\mathrm{\Σ}}}{(K_i\·\mathrm{\Δ}{\mathrm{\θ}}_i)}^2\right)---\left(12\right)$

Wherein, the Δ θ when i>1 _ibe the differential seat angle of angle when puma arm is in current location and target location that the i-th-1 joint jib and i-th saves between jib; Δ θ ₁for the variable quantity of angle between Section 1 jib and surface level when current location and target location; COEFFICIENT K _ithe handle aperture change saving jib Motor ability and telepilot according to puma arm i-th is determined, i.e. K _ifor changing relevant time-varying coefficient to jib Motor ability and telepilot universal handle aperture, wherein the implication of jib Motor ability is as follows: if the i-th joint jib and i-th-1 saves angle demand fulfillment between jib between 10 ° to 170 °, if when current i-th joint jib and i-th-1 saves that between jib, angle is between 20 ° to 160 °, can think that the Motor ability of the i-th joint jib is comparatively large, now can by K _ibe set to smaller value and substantially do not change, time between 10 ° to 20 ° or between 160 ° to 170 °, can think that the Motor ability of the i-th joint jib is less, now can by K _ibe set to higher value (namely corresponding Δ θ _iless, realize the intelligent buffer of jib extreme angles); In addition, during the actions such as the universal handle aperture of telepilot changes greatly that the direction of motion being usually embodied in jib changes, the startup of jib and turning, when namely oil cylinder state sharply changes, now can by K _ibe set to higher value; Above-mentioned COEFFICIENT K _ithe handle aperture change that simultaneously can save jib Motor ability and telepilot according to puma arm i-th is determined, during concrete operations, can according to practical operation experience, and when the handle aperture change of different jib Motor ability and telepilot is set, coefficient of correspondence K _ioccurrence; In a word, by increasing or reduce the K of jib _ivalue (its for time the weight that becomes, the Motor ability often saving jib is distributed and limit), then the Δ θ of the middle correspondence of formula (12) _iwill correspondingly reduce or increase, thus change the speed of jib from planning, thus realize mild stopping and the startup of arm support oil cylinder.

It should be noted that, except minimum with differential seat angle be except optimization aim, can also using arm support oil cylinder fluctuations in discharge minimum or jib change joint number minimum etc. as optimization aim, all determine according to control overflow.

Step 19: according to the turntable anglec of rotation and cantilever crane angle differential seat angle, controls the motion of turntable and jib, namely by solving and Δ θ _i, the movement locus of jib is just determined; Specifically can comprise:

First the speed of the motor for driving turntable to move and the speed for the oil cylinder that drives puma arm to move calculates to the turntable anglec of rotation and the smoothing filtering of cantilever crane angle differential seat angle; Certainly according to actual needs, maximal rate and minimum speed can be pre-set to limit the speed of motor and oil cylinder;

Then the motor speed determined according to the turntable anglec of rotation controls the real-time traffic of motor, and the real-time traffic of the oil cylinder speeds control oil cylinder determined according to differential seat angle.

Again according to the corresponding relation (acquisition can be measured in advance according to testing) between the electromagnetic valve current preset and motor flow, according to the current value of the corresponding solenoid valve of the real-time traffic determination motor of motor; And according to the electromagnetic valve current preset and the corresponding relation (acquisition can be measured in advance according to testing) of Flow of Cylinder, according to the current value of the corresponding solenoid valve of the real-time traffic determination oil cylinder of oil cylinder, and then realize controlling the pool of puma arm and turntable motion.

It should be noted that, step 13 is to 15 for calculating the turntable anglec of rotation, and itself and the step 16 for calculating the differential seat angle of jib when diverse location do not have precedence relation between step 18.

By stereoscopic three-dimensional coordinate is split into two planar two dimensional coordinate in the present embodiment, the differential seat angle of angle when puma arm is in current location and target location between the adjacent two joint jibs solving puma arm and the turntable anglec of rotation, and be converted into the drive current of jib and turntable solenoid valve (banked direction control valves), motion during overall planning intelligent arm support spatial movement in jib plane and the motion of turntable, achieve the space line walking of jib, be convenient to routine processes; Locus planning carries out next step planning based on current location, avoids jib (turntable) to-and-fro movement that longterm planning point may occur; Simultaneously minimum with cylinder action rate of change is objective optimization function, and continuously, steadily, by automatic control technology, when arm support tail end runs, shake is less, improves cloth precision, greatly reduces jib operation easier for the planning operation speed of oil cylinder; Preferably, located by pump truck and telepilot relative direction, operator can operate jib at arbitrary orientation; By Rotating Transition of Coordinate, the direction of operating of universal handle is consistent with the direction of action of jib, reduces jib operation easier, makes by operating universal handle, easily realizes typical condition (rectangle, triangle) cloth.

The puma arm spatial movement control method process flow diagram (explanation of Fig. 2-Fig. 6 illustrates and goes for the present embodiment) that Fig. 7 provides for the embodiment of the present invention two; Fig. 8 is the principle schematic of the coordinate conversion operation in Fig. 7 in puma arm amount of elastic deformation computation process; Fig. 9 is the principle schematic of carrying out elastic deformation compensating operation in Fig. 7 according to alternate position spike; As shown in figs. 7 to 9;

Step 701: the relative direction of pump truck and telepilot is located, can see the explanation explanation of step 13 in Fig. 2;

Step 702: receive each angular transducer sensing and the puma arm that sends and respectively save angle between jib and surface level when being in current location, basically identical with the effect of step 11 in Fig. 2; During concrete operations, can to the smoothing filtering process of above-mentioned each angle;

Step 703: according to finite element theory, the n-th amount of elastic deformation f of joint arm under current load of computational intelligence arm _n; And according to amount of elastic deformation f _nand n-th length of joint arm, the coordinate of the arm support tail end asking for puma arm under the local coordinate system of the n-th joint arm; And according to the angle between each joint jib and surface level, realize the coordinate conversion of arm support tail end under the local coordinate system of each joint arm by rotation of coordinate and translation; And calculate arm support tail end current time and on be engraved in for the moment alternate position spike under the local coordinate system of same joint arm;

This step mainly considers that jib is the Mechanism Combination of one group of elongated flexible element, operationally be equivalent to cantilever beam structure, and interact with rigid motion or be coupled while jib distortion, so jib is in fact a flexible body, in the actual cloth process of pump truck, jib is conducted oneself with dignity and concrete belt carries and affects, jib elastic deformation can be caused, in the control procedure of intelligent arm support, if do not corrected elastic deformation, then can affect cloth accuracy; The concrete operations of step 703 can be explained as follows:

Often save learning of the variation relation of jib distortion and load through analysing in depth puma arm, jib average deformation amount is probably 0.67%, can think that jib meets small deformation theory.The supposition of this jib small deformation is:

1) jib distortion and load linear;

2) only consider along the distortion (main transformer shape) on force direction;

3) problem on deformation of jib can be combined by superposition principle.

Because jib meets above-mentioned small deformation theory, its elastic deformation can be fitted and be obtained with the following methods;

First, because pump displacement can affect load, and load can the size of influence elastane deformation quantity, can obtain amount of elastic deformation f during concrete operations in conjunction with current pump displacement based on finite element theory _n, concrete as:

According to finite element theory, according to from heavy load, jib moment of flexure, vertical load and transverse load, the n-th amount of elastic deformation f of joint arm under current pump displacement of computational intelligence arm _n; During concrete operations, existing large-scale application finite element program NASTRAN can be utilized, analyze often joint jib and be subject to the jib deflection in deadweight distributed load, Equivalent Moment, equivalent vertical load, transverse load situation; Operational applications for existing large-scale application finite element program NASTRAN is prior art, does not hereby repeat;

Secondly, according to amount of elastic deformation f _nand n-th length of joint arm, the coordinate of the arm support tail end asking for puma arm under the local coordinate system of the n-th joint arm; And realize the coordinate conversion of arm support tail end under the local coordinate system of each joint arm by rotation of coordinate and translation; During concrete operations, as shown in Figure 8, if by abstract for jib be multiple stiffness system, the position of arm support tail end is A, however jib actual be the Mechanism Combination of flexible piece, there is elastic deformation in flexible jib, actual arm support tail end position is B.Obviously, along with the increase of jib joint number, this deviation can be increasing, thus cause extreme difficulties to the accurate control of intelligent arm support terminal position; In order to correct the elastic deformation of flexible jib, proceed as follows:

Aa: the local coordinate system [O of the arm support tail end of puma arm at the n-th joint arm can be determined; x _n, y _n] under coordinate be (x' _n, y' _n), wherein, x' _n=L _n, y' _n=-f _n; This L _nbe the length of the n-th joint arm; This local coordinate system [O; x _n, y _n] can the root of the n-th joint arm be initial point, to save the tangent line of arm for abscissa axis with n-th, and correspondingly determine axis of ordinates, the determination mode of this local coordinate system goes for each joint arm;

Bb, according to above-mentioned coordinate (x' _n, y' _n) and the angle of the n-th joint arm roots and the (n-1)th joint arm roots and horizontal direction, save the coordinate (x' under the local coordinate system of arm by rotation of coordinate and translation calculation arm support tail end (n-1)th _n-1, y' _n-1); Detailed process is as follows:

By two angular transducers corresponding sensing two inclination value (often saving the angle of arm roots and the surface level) θ being respectively arranged on the n-th arm and the (n-1)th joint arm roots _nand θ _n-1, under each local coordinate system, the coordinate of arm support tail end calculates by rotation of coordinate and translation: specific as followsly state formula (13),

$\left[\begin{array}{cc}{x}_{n-1}^{\′}& y_{n-1}^{\′}\end{array}\right]=\left[\begin{array}{cc}{x}_n^{\′}& y_n^{\′}\end{array}\right]\×\left[\begin{array}{cc}\cos ({\mathrm{\θ}}_n-{\mathrm{\θ}}_{n-1})& \sin ({\mathrm{\θ}}_n-{\mathrm{\θ}}_{n-1})\\ -\sin ({\mathrm{\θ}}_n-{\mathrm{\θ}}_{n-1})& \cos ({\mathrm{\θ}}_n-{\mathrm{\θ}}_{n-1})\end{array}\right]+\left[\begin{array}{cc}{L}_{n-1}& -f_{n-1}\end{array}\right]---\left(13\right)$

Wherein, (x' _n, y' _n) for arm support tail end point is at [O; x _n, y _n] coordinate figure under coordinate system, n is the joint number of jib, L _nbe the length of the n-th joint jib, θ _nbe the angle of the n-th joint jib and surface level, f _n-1for the amount of elastic deformation that FEM (finite element) calculation draws; The coordinate of arm support tail end under the local coordinate system of each joint jib is the position under its local coordinate system at this jib; During concrete operations, can by the operation of above-mentioned coordinate conversion, calculate the coordinate of arm support tail end under the local coordinate system of Section 1 arm of puma arm is (x' always ₁, y' ₁), the position by each moment arm support tail end is all unified to be compared under the local coordinate system of Section 1 arm.

Again: calculate arm support tail end current time and on be engraved in for the moment alternate position spike under the local coordinate system of same joint arm, i.e. jib straightening amount; During concrete operations, can calculate arm support tail end current time and on be engraved in for the moment alternate position spike under the local coordinate system of Section 1 arm;

As shown in Figure 9, arm support tail end is in the process moved right, the movement locus expected is A->B->C->D ... but in the operational process of reality, affect by jib is elastically-deformable, move in B point (coordinate is (x, y)) process at arm support tail end from A (starting point), in fact E point (coordinate is (x+Dx, y+Dy)) has been gone to; Now, through the elastically-deformable calculating of above steps, draw on the basis of measuring in original walking, arm support tail end needs multirow to walk the stroke of-Dx, namely alternate position spike is (-Dx, Dy), so just can ensure that arm support tail end maintains sustained height substantially, be convenient to realize the elastically-deformable rectification of intelligent arm support;

Then: upgrade the variate-value for characterizing above-mentioned alternate position spike, as being updated to (-Dx, Dy).

Step 704: judge whether alternate position spike (only can consider the deviation in short transverse, now predetermined threshold value corresponds to the predetermined threshold value in short transverse) is greater than predetermined threshold value, when being greater than predetermined threshold value, performs step 705; Otherwise perform step 706;

Step 705: carry out elastic deformation compensating operation according to alternate position spike, during concrete operations, in the same time substantially can not maintain sustained height in level and vertical direction velocity amplitude to make arm support tail end according to the difference Dy compensation puma arm of the difference Dx of the horizontal direction of alternate position spike and vertical direction, namely pass through in the horizontal direction and the compensation of the control realization displacement of speed on vertical direction;

Save a large amount of manpower, material resources and financial resources by the elastic deformation of Finite element arithmetic jib, and associate with pump displacement, ensure the accuracy of deformation calculation; And by the elastically-deformable rectification of jib, improve jib running precision.

Step 706: receive the walking order that telepilot sends, specifically can see the explanation explanation in step 13 in Fig. 2 and 14;

Step 707: the target location determining arm support tail end, specifically can see the explanation explanation in step 16-18 in Fig. 2;

Step 708: judge whether to need Optimization Solution; Specifically comprise:

First judge that puma arm respectively saves when target location whether angle between jib is corresponding minimum value or maximal value;

Secondly, computational intelligence arm respectively saves angle between jib when target location is corresponding minimum value or the jib quantity of maximal value;

Step 709, when jib quantity is more than or equal to n-1, directly stops puma arm respectively saving the action of jib;

Step 710: when jib quantity is less than n-1, solve optimal angle changing value, refers to the explanation explanation in Fig. 2 in step 18;

Step 711: judge that whether new angle value is reasonable; Specifically comprise:

First, respectively save when angle determination puma arm is in target location between jib when current location according to differential seat angle and puma arm and respectively save angle between jib;

Secondly, respectively save maximal value and the minimum value of angle between jib according to the puma arm preset, judge that puma arm respectively to save between jib angle whether in the minimum value of correspondence and the scope of maximal value when target location; If so, be then judged to be rationally, otherwise, perform step 712:

Step 712: lock corresponding jib, and return step 710 (namely recalculate other jibs and be adjacent differential seat angle between jib); Specific as follows:

Angle between jib is in the COEFFICIENT K that corresponding minimum value is corresponding with the extraneous jib of maximal value _ibe defined as zero, and redefine in puma arm other joint arms and be adjacent the differential seat angle of angle when puma arm is in current location and target location between jib.

Step 713: to Δ θ _iand smothing filtering, and calculate oil cylinder or motor speed; Can see the explanation explanation of step 19 in Fig. 2.

Step 714: the flow being converted into oil cylinder or motor; Can see the explanation explanation of step 19 in Fig. 2.

Step 715: according to the electric current preset and flow proportional characteristic, be converted into corresponding electromagnetic valve current; Can see the explanation explanation of step 19 in Fig. 2.

By stereoscopic three-dimensional coordinate is split into two planar two dimensional coordinate in the present embodiment, the differential seat angle of angle when puma arm is in current location and target location between the adjacent two joint jibs solving puma arm and the turntable anglec of rotation, and be converted into the drive current of jib and turntable solenoid valve (banked direction control valves), motion during overall planning intelligent arm support spatial movement in jib plane and the motion of turntable, achieve the space line walking of jib, be convenient to routine processes; Preferably, can automatic decision be realized and avoid jib interference operating mode; Pump truck and telepilot relative direction are located, and operator can operate jib at arbitrary orientation; By Rotating Transition of Coordinate, the direction of operating of universal handle is consistent with the direction of action of jib, reduces jib operation easier.

The structural representation of the puma arm spatial movement control device that Figure 10 provides for the embodiment of the present invention, it is applied to has n and saves the puma arm of arm, and the explanation that wherein n is more than or equal to 2, Fig. 2-Fig. 9 illustrates and can adapt to the present embodiment, as shown in Figure 10, puma arm spatial movement control device comprises:

Receiving element 60, respectively saves the angle between jib and surface level when the puma arm for receiving each angular transducer sensing and transmission is in current location;

Coordinate transformation unit 61, during for being in current location according to puma arm, the coordinate of arm support tail end in jib space coordinates, determines first coordinate of arm support tail end in jib plane coordinate system and the second coordinate in jib projected coordinate system;

Anglec of rotation determining unit 62, for the telepilot for controlling puma arm motion coordinate system and jib space coordinates not to the time in of corresponding according to coordinate system adjustment jib projected coordinate system and second coordinate of the telepilot for controlling puma arm motion, or according to the coordinate system of described jib projected coordinate system adjustment telepilot, to make the movement angle of the arm support tail end of puma arm in the coordinate system of telepilot consistent with the movement angle in jib projected coordinate system, and according to telepilot send movement instruction and the second coordinate, the three-dimensional of arm support tail end in jib projected coordinate system when determining that puma arm is in target location, and the turntable anglec of rotation of puma arm is calculated according to the second coordinate and three-dimensional, or, telepilot coordinate system and jib space coordinates to the time in of corresponding according to movement instruction (can directly send to this anglec of rotation determining unit by pump truck telepilot) and the second coordinate, the three-dimensional of arm support tail end in jib projected coordinate system when determining that puma arm is in target location, and the turntable anglec of rotation of puma arm is calculated according to the second coordinate and three-dimensional,

Angle difference determining unit 63, for according to the first coordinate, measure inclination value (can directly send to angle difference determining unit by the sensor of each jib root) when the puma arm that obtains is in current location between each jib and surface level, length computation puma arm that puma arm respectively saves jib move to target location from current location time coordinate knots modification jib plane coordinate system; And according to ordinate knots modification determination constraint condition, then according to preset optimized algorithm, determine puma arm adjacent two joint jibs between the differential seat angle of angle when puma arm is in current location and target location;

Motion control unit 64, for according to the turntable anglec of rotation and cantilever crane angle differential seat angle, controls the motion of turntable and jib respectively.

Particularly, anglec of rotation determining unit 62 comprises:

Anglec of rotation determination subelement (this figure is not shown), for after the Relatively orientation signal not corresponding with jib space coordinates for the coordinate system characterizing telepilot receiving telepilot transmission, determine that the coordinate system of telepilot is not corresponding with jib space coordinates, and according to the coordinate system of Relatively orientation signal acquisition telepilot and the angle in preset reference direction, again according to the coordinate system of telepilot and the angle in preset reference direction, adjustment jib projected coordinate system and the second coordinate, or the coordinate system of adjustment telepilot.

By utilizing telepilot universal handle and 1 arm handle in the present embodiment, under telepilot pattern is switched to automatic mode, based on the real-time angular of jib and turntable, according to the movement instruction that telepilot sends, by current jib attitude, by a series of computing and specific position process, draw the attitude of jib subsequent time and judge that whether this attitude is reasonable, through the inverse operation of attitudes vibration-length of oil cylinder (turntable angle) change-oil cylinder (motor) flow conversion-electromagnetic valve current change, drive whole jib, make it according to remote command action, realize the horizontal direction of arm support tail end, vertical direction (or compound) is moved stably, also consider emergency protection simultaneously, safeguard protection, the restrictions such as limiting condition, in order to avoid cause unnecessary loss, when can make site operation, jib operation easier reduces, pumping precision improves, thus realize simple, operate jib easily, make operator concrete pump can accurately be delivered to target location.

It should be noted that, various embodiments of the present invention explain for the control of pump truck puma arm, are appreciated that the control method of puma arm of the present invention and device go for the control of the puma arm of Other Engineering machinery, are not limited to pump truck.

The foregoing is only preferred embodiment of the present invention, not in order to limit the present invention, within the spirit and principles in the present invention all, any amendment done, equivalent replacement, improvement etc., all should be included within protection scope of the present invention.

Claims (10)

1. a puma arm spatial movement control method, be applied to and have the puma arm that n saves jib, wherein said n is more than or equal to 2, it is characterized in that, described puma arm spatial movement control method comprises:

Receive each angular transducer sensing and the described puma arm that sends and respectively save angle between jib and surface level when being in current location;

When being in current location according to described puma arm, the coordinate of arm support tail end in jib space coordinates, determines first coordinate of described arm support tail end in jib plane coordinate system and the second coordinate in jib projected coordinate system;

The telepilot for controlling the motion of described puma arm coordinate system and described jib space coordinates not to time corresponding described jib projected coordinate system and the second coordinate is adjusted according to the coordinate system of described telepilot, or according to the coordinate system of described jib projected coordinate system adjustment telepilot, to make the movement angle of described arm support tail end in the coordinate system of described telepilot consistent with the movement angle in described jib projected coordinate system, and the movement instruction sent according to described telepilot and described second coordinate, the three-dimensional of arm support tail end in described jib projected coordinate system when determining that described puma arm is in target location, and the turntable anglec of rotation of described puma arm is calculated according to described second coordinate and described three-dimensional, or, described telepilot coordinate system and described jib space coordinates to time corresponding according to described movement instruction and described second coordinate, the three-dimensional of arm support tail end in described jib projected coordinate system when determining that described puma arm is in target location, and the turntable anglec of rotation of described puma arm is calculated according to described second coordinate and described three-dimensional,

Coordinate knots modification when puma arm moves to target location from current location described in the length computation respectively saving jib according to described first coordinate, the angle between described each joint jib and surface level, described puma arm jib plane coordinate system; And according to described coordinate knots modification determination constraint condition, then according to preset optimized algorithm, determine described puma arm adjacent two joint jibs between the differential seat angle of angle when described puma arm is in current location and target location;

According to the differential seat angle of angle between the described turntable anglec of rotation and described adjacent two joint jibs, control the motion of described puma arm turntable and jib respectively.

2. puma arm spatial movement control method according to claim 1, is characterized in that, determine that the method that the coordinate system of described telepilot is not corresponding with described jib space coordinates comprises:

After the Relatively orientation signal not corresponding with described jib space coordinates for the coordinate system characterizing described telepilot receiving the transmission of described telepilot, determine that the coordinate system of described telepilot is not corresponding with described jib space coordinates.

3. puma arm spatial movement control method according to claim 2, it is characterized in that, the step of described " adjusting described jib projected coordinate system and the second coordinate according to the coordinate system of described telepilot " comprising:

The coordinate system of telepilot and the angle in preset reference direction according to described Relatively orientation signal acquisition, again according to the coordinate system of described telepilot and the angle in preset reference direction, adjust described jib projected coordinate system and the second coordinate, or the coordinate system of adjustment telepilot.

4. puma arm spatial movement control method according to any one of claim 1-3, it is characterized in that, described optimized algorithm is to make the quadratic sum of the differential seat angle of angle when described puma arm is in current location and target location between the adjacent two joint jibs of described puma arm minimum for optimization aim, and described optimization aim is expressed as:

$\min \left(\underset{i=1}{\overset{n}{\mathrm{\Σ}}}{(K_i\·{\mathrm{\Δ\θ}}_i)}^2\right)$

Wherein, COEFFICIENT K _ithe handle aperture change saving jib Motor ability and described telepilot according to described puma arm i-th is determined; The Δ θ when i>1 _ibe the differential seat angle of angle when described puma arm is in current location and target location that the i-th-1 joint jib and i-th saves between jib; Δ θ ₁for the variable quantity of angle between Section 1 jib and surface level when current location and target location.

5. puma arm spatial movement control method according to claim 4, it is characterized in that, described " determine described puma arm adjacent two joint jibs between the differential seat angle of angle when described puma arm is in current location and target location " step after also comprise:

Respectively save when angle determines described puma arm in target location between jib and surface level when current location according to described differential seat angle and described puma arm and respectively save angle between jib;

And respectively save maximal value and the minimum value of angle between jib according to the described puma arm preset, judge that described puma arm respectively to save between jib angle whether in the scope between the described minimum value and described maximal value of correspondence when target location;

Angle between jib is in the COEFFICIENT K that corresponding described minimum value is corresponding with the extraneous jib between described maximal value _ibe defined as zero, and redefine in described puma arm other joint arms and be adjacent the differential seat angle of angle when described puma arm is in current location and target location between jib.

6. state puma arm spatial movement control method according to claim 5, it is characterized in that, described " according to described coordinate knots modification determination constraint condition; again according to preset optimized algorithm, determine described puma arm adjacent two joint jibs between the differential seat angle of angle when described puma arm is in current location and target location " step before comprise:

Judge that described puma arm respectively saves when target location whether angle between jib is corresponding described minimum value or described maximal value;

And calculating described puma arm, respectively save angle between jib when target location be corresponding described minimum value or the jib quantity of described maximal value;

When described jib quantity is more than or equal to n-1, described puma arm is directly stopped respectively saving the action of jib;

When described jib quantity is less than n-1, perform " according to described coordinate knots modification determination constraint condition; again according to preset optimized algorithm, determine described puma arm adjacent two joint jibs between the differential seat angle of angle when described puma arm is in current location and target location " step.

7. puma arm spatial movement control method according to any one of claim 1-3, is characterized in that, when determining that described puma arm is in current location, the seat calibration method of arm support tail end in jib space coordinates comprises:

According to finite element theory, calculate the n-th amount of elastic deformation f of joint arm under current load of described puma arm _n; And according to described amount of elastic deformation f _nand the length of described n-th joint arm, ask for the coordinate of arm support tail end under the local coordinate system of described n-th joint arm of described puma arm; And according to the angle between described each joint jib and surface level, realize the coordinate conversion of described arm support tail end under the local coordinate system of each joint arm by rotation of coordinate and translation; And calculate described arm support tail end current time and on be engraved in for the moment alternate position spike under the local coordinate system of same joint arm; Again according to described alternate position spike, when being in current location to described puma arm, the coordinate of arm support tail end in jib space coordinates carries out elastic deformation compensating operation.

8. puma arm spatial movement control method according to claim 7, it is characterized in that, comprised before the step of described " according to described alternate position spike, when being in current location to described puma arm, the coordinate of arm support tail end in jib space coordinates carries out elastic deformation compensating operation ":

Judge whether described alternate position spike is greater than predetermined threshold value, when being greater than described predetermined threshold value, when being in current location to described puma arm, the coordinate of arm support tail end in jib space coordinates carries out elastic deformation compensating operation.

9. a puma arm spatial movement control device, be applied to and have the puma arm that n saves jib, wherein said n is more than or equal to 2, it is characterized in that, described puma arm spatial movement control device comprises:

Receiving element (60), respectively saves the angle between jib and surface level when the described puma arm for receiving each angular transducer sensing and transmission is in current location;

Coordinate transformation unit (61), during for being in current location according to described puma arm, the coordinate of arm support tail end in jib space coordinates, determines first coordinate of described arm support tail end in jib plane coordinate system and the second coordinate in jib projected coordinate system;

Anglec of rotation determining unit (62), for the telepilot for controlling the motion of described puma arm coordinate system and described jib space coordinates not to time corresponding coordinate system according to the telepilot for controlling the motion of described puma arm adjusts described jib projected coordinate system and the second coordinate, or according to the coordinate system of described jib projected coordinate system adjustment telepilot, to make the movement angle of the arm support tail end of described puma arm in the coordinate system of described telepilot consistent with the movement angle in described jib projected coordinate system, and the movement instruction sent according to described telepilot and described second coordinate, the three-dimensional of arm support tail end in described jib projected coordinate system when determining that described puma arm is in described target location, and the turntable anglec of rotation of described puma arm is calculated according to described second coordinate and described three-dimensional, or, described telepilot coordinate system and described jib space coordinates to time corresponding according to described movement instruction and described second coordinate, the three-dimensional of arm support tail end in described jib projected coordinate system when determining that described puma arm is in described target location, and the turntable anglec of rotation of described puma arm is calculated according to described second coordinate and described three-dimensional,

Angle difference determining unit (63), for respectively save jib according to described first coordinate, the angle between described each joint jib and surface level, described puma arm length computation described in puma arm move to target location from current location time coordinate knots modification jib plane coordinate system; And according to described coordinate knots modification determination constraint condition, then according to preset optimized algorithm, determine described puma arm adjacent two joint jibs between the differential seat angle of angle when described puma arm is in current location and target location;

Motion control unit (64), for the differential seat angle according to angle between the described turntable anglec of rotation and jib, controls the motion of described puma arm turntable and jib respectively.

10. puma arm spatial movement control device according to claim 9, it is characterized in that, described anglec of rotation determining unit (62) comprising:

Anglec of rotation determination subelement, for after the Relatively orientation signal not corresponding with described jib space coordinates for the coordinate system characterizing described telepilot receiving the transmission of described telepilot, determine that the coordinate system of described telepilot is not corresponding with described jib space coordinates, and according to described Relatively orientation signal acquisition the coordinate system of telepilot and the angle in preset reference direction, again according to the coordinate system of described telepilot and the angle in preset reference direction, adjust described jib projected coordinate system and the second coordinate, or the coordinate system of adjustment telepilot.