CN103792956A - 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, substantially take the single jib of single remote-control handle control or turntable.In general, operator is transported to concrete the random position of building from pump truck hopper, then by workman, concrete is evenly spread out, a large amount of manpower and materials are saved although compare artificial transportation concrete, but exist onerous toil, pumper operation person should be noted pumping vehicle arm rack attitude on the one hand, considers the correctness of operation, and maloperation easily causes jib clobber; If action jib amplitude is excessive, also can bring injury to the person of assisting under arm support tail end flexible pipe.Workman's spread concrete is also pretty troublesome on the other hand.Under this background, pumper operation person wishes a kind of shirtsleeve operation mode and realizes 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 three-dimensional definite mode can be as follows: in the space that can reach at pumping vehicle arm rack, using pump truck turntable and hinged place, Section 1 jib top as true origin, take the hopper direction of vehicle body as x positive axis, take the direction perpendicular to vehicle body as y axle positive axis, according to the cartesian coordinate system right-hand rule, can determine z axle; H represents intersection point.Certainly, according to actual needs can be take other point as true origin, the direction of determining other is x positive axis, and corresponding definite y axle and z axle.

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

$\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, the D coordinates value that X, Y, Z are arm support tail end point, the joint number that n is jib, L _ibe the length of i joint jib, α ₀for the turntable anglec of rotation, α _i(i>0) represent that i saves the angle of jib and surface level.

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

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 the movement of automatic control puma arm 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 the puma arm with n joint jib, wherein said n is more than or equal to 2, and described puma arm spatial movement control method comprises:

Receive described puma arm each angle saving between jib and surface level in the time of current location of each angular transducer sensing and transmission;

According to described puma arm arm support tail end coordinate in jib space coordinates when the current location, determine first coordinate of described arm support tail end in jib plane coordinate system and the second coordinate in jib projected coordinate system;

In the coordinate system of the telepilot for controlling the motion of described puma arm and described jib space coordinates not at once, adjust described jib projected coordinate system and the second coordinate according to the coordinate system of described telepilot, or adjust the coordinate system of telepilot according to described jib projected coordinate system, so that the movement angle of the arm support tail end of described puma arm in the coordinate system of described telepilot is consistent with the movement angle in described jib projected coordinate system, and the movement instruction and described the second coordinate that send according to described telepilot, determine described puma arm arm support tail end three-dimensional in described jib projected coordinate system in the time of target location, and calculate the turntable anglec of rotation of described puma arm according to described the second coordinate and described three-dimensional, or, at once, according to described movement instruction and described the second coordinate, determine described puma arm arm support tail end three-dimensional in described jib projected coordinate system in the time of target location in the coordinate system of described telepilot and described jib space coordinates, and calculate the turntable anglec of rotation of described puma arm according to described the second coordinate and described three-dimensional,

Puma arm coordinate change amount jib plane coordinate system when current location moves to target location described in the length computation that respectively saves jib according to the angle between described the first coordinate, described each joint jib and surface level, described puma arm; And determine constraint condition according to described coordinate change amount, then according to default optimized algorithm, determine angle between the adjacent two joint jibs of described puma arm at described puma arm the differential seat angle during 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 respectively the motion of described puma arm turntable and jib.

Further, the coordinate system method not corresponding with described jib space coordinates of determining described telepilot comprises: receive that described telepilot sends for after the coordinate system that characterizes described telepilot and the not corresponding Relatively orientation signal of described jib space coordinates, 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 the telepilot for controlling described puma arm motion " comprising: according to the coordinate system of telepilot described in described Relatively orientation signal acquisition and the angle of preset reference direction, again according to the angle of the coordinate system of described telepilot and preset reference direction, adjust described jib projected coordinate system and the second coordinate, or adjust the coordinate system of telepilot.

Further, described optimized algorithm is so that the quadratic sum minimum of the angle of adjacent two joints between jibs of described puma arm differential seat angle during in current location and target location at described puma arm is optimization aim, and described optimization aim is expressed as:

wherein, COEFFICIENT K _ichange and determine according to the handle aperture of described puma arm i joint jib Motor ability and described telepilot; Δ θ in the time of i>1 _ibe i-1 joint jib with i save angle between jib at described puma arm the differential seat angle during in current location and target location; Δ θ ₁for the variable quantity of angle between Section 1 jib and surface level when current location and the target location.

Further, after the step of described " angle of adjacent two joints between jibs of determining described puma arm be the differential seat angle during in current location and target location at described puma arm ", also comprise:

Angle between each joint jib when angle is determined described puma arm in target location between each joint jib and surface level when the current location according to described differential seat angle and described puma arm;

And respectively save maximal value and the minimum value of angle between jib according to default described puma arm, judge described puma arm angle whether in the scope between described minimum value and the described maximal value of correspondence between each joint jib in the time of target location;

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

Further, step at described " determine constraint condition according to described coordinate change amount; again according to default optimized algorithm, the angle of adjacent two joints between jibs of determining described puma arm be the differential seat angle during in current location and target location at described puma arm " comprises before:

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

And calculate described puma arm in the time of target location between each joint jib angle be corresponding described minimum value or described peaked jib quantity;

In the time that described jib quantity is more than or equal to n-1, directly stop described puma arm and respectively save the action of jib;

In the time that described jib quantity is less than n-1, carry out the step of " determine constraint condition according to described coordinate change amount; again according to default optimized algorithm, the angle of adjacent two joints between jibs of determining described puma arm be the differential seat angle during in current location and target location at described puma arm ".

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

According to finite element theory, the n that calculates described puma arm saves the amount of elastic deformation f of arm under current load _n; And according to described amount of elastic deformation f _nand the length of described n joint arm, the coordinate of the arm support tail end of asking for described puma arm under the local coordinate system of described n joint 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 the alternate position spike under the local coordinate system of same joint arm; Again according to described alternate position spike, described puma arm arm support tail end coordinate in jib space coordinates when the current location is carried out to elastic deformation compensating operation.

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

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

On the other hand, a kind of puma arm spatial movement control device is provided, be applied to the puma arm with n joint jib, wherein said n is more than or equal to 2, described puma arm spatial movement control device comprises: receiving element, for the described puma arm that receives each angular transducer sensing and the transmission angle between each joint jib and surface level when the current location, coordinate transformation unit, at the coordinate of jib space coordinates, determining first coordinate of described arm support tail end in jib plane coordinate system and the second coordinate in jib projected coordinate system according to described puma arm arm support tail end when the current location, anglec of rotation determining unit, for the coordinate system of the telepilot for controlling the motion of described puma arm and described jib space coordinates not at once, adjust described jib projected coordinate system and the second coordinate according to the coordinate system of the telepilot for controlling described puma arm motion, or, adjust the coordinate system of telepilot according to described jib projected coordinate system, so that the movement angle of the arm support tail end of described puma arm in the coordinate system of described telepilot is consistent with the movement angle in described jib projected coordinate system, and the movement instruction and described the second coordinate that send according to described telepilot, determine described puma arm arm support tail end three-dimensional in described jib projected coordinate system in the time of described target location, and calculate the turntable anglec of rotation of described puma arm according to described the second coordinate and described three-dimensional, or, at once, according to described movement instruction and described the second coordinate, determine described puma arm arm support tail end three-dimensional in described jib projected coordinate system in the time of described target location in the coordinate system of described telepilot and described jib space coordinates, and calculate the turntable anglec of rotation of described puma arm according to described the second coordinate and described three-dimensional, the poor determining unit of angle, while moving to target location for puma arm described in the length computation that respectively saves jib according to the angle between described the first coordinate, described each joint jib and surface level, described puma arm from current location in the coordinate change amount of jib plane coordinate system, and determine constraint condition according to described coordinate change amount, then according to default optimized algorithm, determine angle between the adjacent two joint jibs of described puma arm at described puma arm the differential seat angle during in current location and target location, motion control unit, for according to the described turntable anglec of rotation and jib angle differential seat angle, controls respectively the motion of described puma arm turntable and jib.

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

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

Accompanying drawing explanation

The accompanying drawing that forms 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 is used 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;

In the puma arm spatial movement control method that Fig. 4 provides for Fig. 2, coordinate system is adjusted schematic diagram;

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

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 operation of the coordinate conversion in puma arm amount of elastic deformation computation process in Fig. 7;

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, in the situation that not conflicting, the feature in embodiment and embodiment in the present invention can combine mutually.Describe below with reference to the accompanying drawings and in conjunction with the embodiments the present invention in detail.

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 the puma arm with n joint 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: the puma arm each angle saving between jib and surface level in the time of current location that receives each angular transducer sensing and transmission; Wherein, each angular transducer (can also be oil cylinder displacement transducer etc. certainly) can be arranged on the root of the each jib of puma arm.Be understandable that, detect angle between each joint jib and surface level and the angle that detects between each joint jib and vertical plane is equal to because two angles are remaining mutually, two angles and be 90 degree.

Step 12: according to puma arm arm support tail end coordinate in jib space coordinates when the current location, determine 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 coordinates, 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, this jib projection coordinate is jib projected coordinate system [O; X, z] in coordinate, puma arm only has and rotates not rectilinear movement at this jib projection plane; Setting up after above-mentioned the first coordinate and the second coordinate, in jib plane and jib projection plane, the motion of puma arm is carried out to coordinated planning, refer to following description.

Step 13: in the coordinate system of the telepilot for controlling puma arm motion and jib space coordinates not at once, adjust jib projected coordinate system and the second coordinate according to the coordinate system of the telepilot for controlling puma arm motion, or adjust the coordinate system of telepilot according to described jib projected coordinate system, so that the movement angle of the arm support tail end of puma arm in the coordinate system of telepilot is consistent with the movement angle in jib projected coordinate system; Specifically can comprise:

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

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

As shown in Figure 4, under telepilot intelligent mode, operate in jib process, if telepilot (not shown) is the coordinate system of setting up with x1 axle and y1 axle in a point and its coordinate, now, telepilot coordinate system is corresponding with jib space coordinates, with just in time consistent (as shown in Fig. 4 dotted line coordinate system (coordinate system of setting up with x1 ' axle and z1 ' axle)) of jib projected coordinate system, without 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 the coordinate system of setting up with x2 axle and y2 axle in b point and its coordinate, now, telepilot coordinate system is not corresponding with jib space coordinates, switch in first remote controller, again universal handle is moved (as pulled to pump truck headstock direction toward default reference direction, be in Fig. 4 shown in unidirectional arrow P), the motion of universal handle represents different implications under two states of switch, as switch is in the present embodiment pressed, the movable information (being the Relatively orientation signal that telepilot sends) of universal handle represents that telepilot coordinate system is not corresponding with jib space coordinates, and the angle information that the movable information of this universal handle has comprised 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 can learn the actual conditions of telepilot its coordinate system in the time of b point, calculate angle poor of telepilot coordinate system and jib projected coordinate system, thereby can adjust the coordinate system of jib projected coordinate system (take the coordinate system of x1 ' axle and the foundation of z1 ' axle) as setting up with x2 ' axle and z2 ' axle, make it consistent with the coordinate system of setting up with x2 axle and y2 axle, realize compensation, 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 the universal handle movement instruction that directly representative is moved for controlling puma arm), 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 when concrete operations, and 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 to adjust telepilot coordinate system, make the coordinate system of telepilot consistent with jib projected coordinate system, send again after by coordinate conversion by the movement instruction of telepilot, or receive and first carry out corresponding conversion after the movement instruction of telepilot and control again.

Step 14: determining that the coordinate system of telepilot and jib space coordinates are not at once, the second coordinate after the movement instruction sending according to telepilot and adjustment, determine that puma arm arm support tail end three-dimensional in jib projected coordinate system in the time of target location (is appreciated that, in the time determining three-dimensional, if the second coordinate is adjusted in preceding step, what need employing is the second coordinate after adjusting, if adjust, is still unadjusted the second coordinate); Or, in the coordinate system of telepilot and jib space coordinates at once according to movement instruction and unadjusted the second coordinate, determine puma arm arm support tail end three-dimensional in jib projected coordinate system in the time of target location;

As shown in Figure 5, suppose in a planning horizon that (coordinate system in this figure not at once, is the jib space coordinates after adjusting in the coordinate system of telepilot and jib space coordinates; In the coordinate system of telepilot and jib space coordinates at once, 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), B point coordinate value (x from A point (correspondence position of arm support tail end current location jib projected coordinate system) _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+{\mathrm{RC}}_x\\ z_{n+1}=z_n+{\mathrm{RC}}_z\end{array}\right.---\left(2\right)$

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

Step 15: determining that the coordinate system of telepilot and jib space coordinates are not at once, the turntable anglec of rotation that calculates puma arm according to the second coordinate after adjusting and three-dimensional (is appreciated that, in the time calculating turntable angle, if the second coordinate is adjusted in preceding step, what need employing is the second coordinate after adjusting, if adjust, be still unadjusted the second coordinate); Or, determining that the coordinate system of telepilot and jib space coordinates are at once, calculate the turntable anglec of rotation of puma arm according to unadjusted the second coordinate and the 5th coordinate;

Continue referring to Fig. 5 (coordinate system in this figure at the coordinate system of telepilot with jib space coordinates not at once, be to adjust jib space coordinates afterwards; To at once, be unadjusted jib space coordinates in the coordinate system of telepilot and jib space coordinates), specifically can calculate the turntable anglec of rotation by following manner

Wherein,

be respectively the vector that arm support tail end forms 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 coordinate system of the telepilot of described motion and described jib space coordinates not at once, if adopt the mode of adjusting telepilot coordinate system, when concrete operations, the movement instruction that can directly send according to described telepilot, difference between the coordinate system of the telepilot of described the second coordinate and described motion and described jib space coordinates, determine described puma arm arm support tail end three-dimensional in described jib projected coordinate system in the time of target location, and calculate the turntable anglec of rotation of described puma arm according to described the second coordinate and described three-dimensional,

Step 16: according to the first coordinate, the puma arm that the measures inclination angle value between each jib and surface level, length computation puma arm that puma arm respectively the saves jib coordinate change amount jib plane coordinate system when current location moves to target location when the current location;

Motion for arm support tail end in jib plane, as shown in Figure 6, wherein angle θ _jand θ _j' (j can at value natural number between 1 to n) represent respectively that arm support tail end j-1 joint jib in the time of A ' and saves the angle between jib with j during at j>1 at B '; In the time of j=1, represent respectively arm support tail end angle between Section 1 jib and surface level in the time of A ' and at B '; 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), the A ' here with the equal corresponding arm support tail end of A point in Fig. 4 current location at three bit spaces, B ' with the equal corresponding arm support tail end of B point in Fig. 4 target location at three bit spaces, for n joint boom system, 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{\Δ\θ}}_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{\Δ\θ}}_j)-(i-1)\·\mathrm{\π})\end{array}\right.---\left(\text{5}\right)$

Wherein, Δ θ _j(when j>1) be j-1 joint jib with j save angle between jib at B ' some the variable quantity during with A ' some; Δ θ ₁for angle between Section 1 jib and surface level at B ' some the variable quantity during with A ' some; L _ibe the length of i joint jib, n is the joint number of jib; Jib angle theta _jwith jib inclination alpha _jcorresponding relation be:

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

Wherein, jib inclination alpha _jbe the angle between j joint jib and surface level, can be measured by the angular transducer that is arranged on j 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\·(\cos (\underset{j=1}{\overset{i}{\mathrm{\Σ}}}({\mathrm{\θ}}_j+{\mathrm{\Δ\θ}}_j)-(i-1)\·\mathrm{\π})-\cos (\underset{j=1}{\overset{i}{\mathrm{\Σ}}}{\mathrm{\θ}}_j-(i-1)\·\mathrm{\π}))\\ \mathrm{\Δy}=\underset{i=1}{\overset{n}{\mathrm{\Σ}}}{L}_i\·(\sin (\underset{j=1}{\overset{i}{\mathrm{\Σ}}}({\mathrm{\θ}}_j+{\mathrm{\Δ\θ}}_j)-(i-1)\·\mathrm{\π})-\sin (\underset{j=1}{\overset{i}{\mathrm{\Σ}}}{\mathrm{\θ}}_j-(i-1)\·\mathrm{\π}))\end{array}\right.---\left(7\right)$

Wherein, Δ x, Δ y are the coordinate change amount of arm support tail end jib plane coordinate system while moving at B ' from A ' some.

Because the planning of the present embodiment locus is to carry 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{\Σ}}}{\mathrm{\α}}_i\·{\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}{\mathrm{\α}}_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: determine constraint condition with coordinate change amount; Particularly, in the ideal situation in the motion process of arm support tail end, A ' and B ' 's ordinate 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\·(\cos (\underset{j=1}{\overset{i}{\mathrm{\Σ}}}({\mathrm{\θ}}_j+{\mathrm{\Δ\θ}}_j)-(i-1)\·\mathrm{\π})-\cos (\underset{j=1}{\overset{i}{\mathrm{\Σ}}}{\mathrm{\θ}}_j-(i-1)\·\mathrm{\π}))=\left|\stackrel{\→}{\mathrm{OB}}\right|-|\stackrel{\→}{\mathrm{OA}|}\\ \mathrm{\Δy}=\underset{i=1}{\overset{n}{\mathrm{\Σ}}}{L}_i\·(\sin (\underset{j=1}{\overset{i}{\mathrm{\Σ}}}({\mathrm{\θ}}_j+{\mathrm{\Δ\θ}}_j)-(i-1)\·\mathrm{\π})-\sin (\underset{j=1}{\overset{i}{\mathrm{\Σ}}}{\mathrm{\θ}}_j-(i-1)\·\mathrm{\π}))=0\end{array}\right.---\left(10\right)$

Wherein,

be respectively the vector that arm support tail end forms in the true origin of current location and target location and jib projected coordinate system, θ _j(when j>1) is the angle between j-1 joint jib and j joint jib; θ ₁for the angle of Section 1 jib and surface level, L _jbe the length of j 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{\Σ}}}{\mathrm{\α}}_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{\Δ\θ}}_i\end{array}\right.---\left(11\right)$

Wherein, △ y equals height tolerance.

Step 18: according to default optimized algorithm, so that the angle of adjacent two joints between jibs of puma arm is at puma arm, the differential seat angle quadratic sum minimum during in current location and target location is optimization aim, determine adjacent two of puma arm save angle between jibs at puma arm the differential seat angle during in current location and target location;

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, Δ θ in the time of i>1 _ibe i-1 joint jib with i save angle between jib at puma arm the differential seat angle during in current location and target location; Δ θ ₁for the variable quantity of angle between Section 1 jib and surface level when current location and the target location; COEFFICIENT K _ichange and determine according to the handle aperture of puma arm i joint jib Motor ability and telepilot, be i.e. K _ifor changing relevant time-varying coefficient to jib Motor ability and the universal handle aperture of telepilot, wherein the implication of jib Motor ability is as follows: if the angle between i joint jib and i-1 joint jib need to meet between 10 ° to 170 °, if when between current i joint jib and i-1 joint jib, angle is between 20 ° to 160 °, can think that the Motor ability of i joint jib is larger, now can be by K _ibe set to smaller value and substantially do not change, between 10 ° to 20 ° or between 160 ° to 170 ° time, can think that the Motor ability of i joint jib is less, now can be by K _ibeing set to higher value (is corresponding Δ θ _iless, realize the intelligent buffer of jib extreme angles); In addition, the universal handle aperture of telepilot changes that the direction of motion that greatly is conventionally embodied in jib changes, startup and the turning etc. of jib be while moving, when oil cylinder state sharply changes, and now can be by K _ibe set to higher value; Above-mentioned COEFFICIENT K _ican change and determine according to the handle aperture of puma arm i joint jib Motor ability and telepilot simultaneously, when concrete operations, can be according to practical operation experience, when the handle aperture variation of different jib Motor abilities 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 of every joint jib is distributed and limit), the Δ θ of the middle correspondence of formula (12) _iwill correspondence reduce or increase, thereby changed the speed of jib from planning, thereby realizing stopping gently and starting of arm support oil cylinder.

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

Step 19: according to the turntable anglec of rotation and jib angle differential seat angle, control the motion of turntable and jib, by solving

and Δ θ _i, the movement locus of jib is just definite; Specifically can comprise:

First the turntable anglec of rotation and jib angle differential seat angle are carried out to smothing filtering and calculate the speed of the motor for driving turntable motion and for driving the speed of oil cylinder of puma arm motion; Certainly according to actual needs, can set in advance maximal rate and minimum speed so that the speed of motor and oil cylinder is limited;

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

According to the corresponding relation between default electromagnetic valve current and motor flow (can measure and obtain in advance according to test), determine the current value of the corresponding solenoid valve of motor according to the real-time traffic of motor again; And according to default electromagnetic valve current and the corresponding relation of Flow of Cylinder (can measure and obtain in advance according to test), determine the current value of the corresponding solenoid valve of oil cylinder according to the real-time traffic of oil cylinder, and then realize the pool control to puma arm and turntable motion.

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

In the present embodiment by stereoscopic three-dimensional coordinate being split into two planar coordinates, the angle of adjacent two joints between jibs that solves puma arm be differential seat angle and the turntable anglec of rotation during in current location and target location at puma arm, and be converted into the drive current of jib and turntable solenoid valve (banked direction control valves), the motion when spatial movement of overall planning intelligent arm support in jib plane and the motion of turntable, realize the space line walking of jib, be convenient to routine processes; Locus planning is carried out next step planning, jib (turntable) to-and-fro movement of avoiding longterm planning point to occur based on current location; Simultaneously take cylinder action rate of change minimum as objective optimization function, the planning operation speed of oil cylinder continuously, steadily, by automatic control technology, when arm support tail end operation shake less, improved cloth precision, greatly reduce jib operation easier; Preferably, by pump truck and telepilot relative direction location, 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, has reduced 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 (explaining of Fig. 2-Fig. 6 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 operation of the coordinate conversion in puma arm amount of elastic deformation computation process in Fig. 7; Fig. 9 is the principle schematic of carrying out elastic deformation compensating operation in Fig. 7 according to alternate position spike; As shown in Fig. 7-Fig. 9;

Step 701: the relative direction location of pump truck and telepilot, can explaining referring to step 13 in Fig. 2;

Step 702: receive puma arm each angle saving between jib and surface level in the time of current location of each angular transducer sensing and transmission, basically identical with the effect of step 11 in Fig. 2; When concrete operations, can carry out the disposal of gentle filter to above-mentioned each angle;

Step 703: according to finite element theory, the amount of elastic deformation f of the n joint arm of computational intelligence arm under current load _n; And according to amount of elastic deformation f _nand the length of n joint arm, the coordinate of the arm support tail end of asking for puma arm under the local coordinate system of n 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 the alternate position spike under the local coordinate system of same joint arm;

This step is mainly to consider that jib is the Mechanism Combination of one group of elongated flexible element, in the time of work, be equivalent to cantilever beam structure, and interact or coupling with rigid motion again in 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, can cause jib elastic deformation, in the control procedure of intelligent arm support, if elastic deformation is not corrected, can affect cloth accuracy; The concrete operations of step 703 can be explained as follows:

Learning of the variation relation of the every joint jib distortion of process in-depth analysis puma arm and load, jib average deformation amount is probably 0.67%, can think that jib meets small deformation theory.This jib small deformation supposition is:

1) jib distortion and load are linear;

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

3) can combine by superposition principle the problem on deformation of jib.

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

First, because pump displacement can affect load, and load can affect the size of amount of elastic deformation, when concrete operations, can obtain amount of elastic deformation f in conjunction with current pump displacement and 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 amount of elastic deformation f of the n joint arm of computational intelligence arm under current pump displacement _n; When concrete operations, can utilize existing large-scale application finite element program NASTRAN, analyze the jib deflection of every joint jib in the distributed load of being conducted oneself with dignity, 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 the length of n joint arm, the coordinate of the arm support tail end of asking for puma arm under the local coordinate system of n 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; When concrete operations, as shown in Figure 8, if by abstract 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, thereby the accurate control of intelligent arm support terminal position is caused to very big difficulty; In order to correct the elastic deformation of flexible jib, proceed as follows:

Aa: can determine that the arm support tail end of puma arm is at the local coordinate system [O of n joint arm; 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 n joint arm; This local coordinate system [O; x _n, y _n] can n the root of joint arm be initial point, take with the tangent line of n joint arm as abscissa axis, and corresponding definite axis of ordinates, definite 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 n joint arm roots and n-1 joint arm roots and horizontal direction, save the coordinate (x' under the local coordinate system of arm at n-1 by rotation of coordinate and translation calculation arm support tail end _n-1, y' _n-1); Detailed process is as follows:

By two angular transducers, two inclination angle values of corresponding sensing (angle of every joint arm roots and the surface level) θ respectively that is arranged on n arm and n-1 joint arm roots _nand θ _n-1, under each local coordinate system, the coordinate of arm support tail end can calculate by rotation of coordinate and translation: the formula (13) of stating specific as follows,

$\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, the joint number that n is jib, L _nbe the length of n joint jib, θ _nbe the angle of n joint jib and surface level, f _n-1the amount of elastic deformation drawing for FEM (finite element) calculation; The coordinate of arm support tail end under the local coordinate system of each joint jib is its position under the local coordinate system of this jib; When concrete operations, can be 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 ₁'), all unify to compare under the local coordinate system of Section 1 arm by the position of each moment arm support tail end.

Again: calculate arm support tail end current time and on be engraved in for the moment the alternate position spike under the local coordinate system of same joint arm, i.e. jib straightening amount; When concrete operations, can calculate arm support tail end current time and on be engraved in for the moment the 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 moving right, the movement locus of expecting is A->B->C->D ... but in actual operational process, be subject to that jib is elastically-deformable to be affected, at arm support tail end from A(starting point) move in B point (coordinate is (x, y)) process, in fact gone to E point (coordinate is (x+Dx, y+Dy)); Now, through the elastically-deformable calculating of above steps, draw on the basis of original walking amount, arm support tail end needs the stroke of walk-Dx of multirow, be that alternate position spike is (Dx, Dy), so just can guarantee that arm support tail end maintains sustained height substantially, be convenient to realize the elastically-deformable rectification of intelligent arm support;

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

Step 704: judge whether alternate position spike (can only 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, in the time being greater than predetermined threshold value, execution step 705; Otherwise execution step 706;

Step 705: carry out elastic deformation compensating operation according to alternate position spike, when concrete operations, can be according to the difference Dy compensation puma arm of the difference Dx of the horizontal direction of alternate position spike and vertical direction at level and vertical direction velocity amplitude so that arm support tail end is not maintaining sustained height in the same time substantially, realize the compensation of displacement by the control of speed in the horizontal direction and on vertical direction;

Calculate jib elastic deformation by finite element method and save a large amount of manpower, material resources and financial resources, and associated with pump displacement, the accuracy of assurance 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 be referring to explaining in step 13 in Fig. 2 and 14;

Step 707: determine the target location of arm support tail end, specifically can be referring to explaining in step 16-18 in Fig. 2;

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

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

Next, it is corresponding minimum value or peaked jib quantity that computational intelligence arm respectively saves angle between jib in the time of target location;

Step 709, in the time that jib quantity is more than or equal to n-1, directly stops puma arm and respectively saves the action of jib;

Step 710: in the time that jib quantity is less than n-1, solve optimal angle changing value, refer to explaining in step 18 in Fig. 2;

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

First angle between each joint jib when, angle is determined puma arm in target location between each joint jib when the current location according to differential seat angle and puma arm;

Secondly, respectively save maximal value and the minimum value of angle between jib according to default puma arm, judge that puma arm respectively saves between jib angle whether in corresponding minimum value and peaked scope in the time of target location; If so, be judged to be rationally, otherwise, execution step 712:

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

By angle between jib in the corresponding minimum value COEFFICIENT K corresponding with peaked extraneous jib _ibe defined as zero, and redefine in puma arm other joint arms be adjacent angle between jib at puma arm the differential seat angle during in current location and target location.

Step 713: to Δ θ _iand

smothing filtering, and calculate oil cylinder or motor speed; Can explaining referring to step 19 in Fig. 2.

Step 714: the flow that is converted into oil cylinder or motor; Can explaining referring to step 19 in Fig. 2.

Step 715: according to default electric current and flow proportional characteristic, be converted into corresponding electromagnetic valve current; Can explaining referring to step 19 in Fig. 2.

In the present embodiment by stereoscopic three-dimensional coordinate being split into two planar coordinates, the angle of adjacent two joints between jibs that solves puma arm be differential seat angle and the turntable anglec of rotation during in current location and target location at puma arm, and be converted into the drive current of jib and turntable solenoid valve (banked direction control valves), the motion when spatial movement of overall planning intelligent arm support in jib plane and the motion of turntable, realize the space line walking of jib, be convenient to routine processes; Preferably, can realize automatic decision and avoid jib and interfere operating mode; Pump truck and telepilot relative direction location, 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, has reduced 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 the puma arm with n joint arm, and wherein n is more than or equal to explaining of 2, Fig. 2-Fig. 9 and can adapts to use the present embodiment, as shown in figure 10, puma arm spatial movement control device comprises:

Receiving element 60, for the puma arm that receives each angular transducer sensing and the transmission angle between each joint jib and surface level when the current location;

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

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

The poor determining unit 63 of angle, when according to the first coordinate, the puma arm that measures, the inclination angle value (can directly send to the poor determining unit of angle by the sensor of each jib root) between each jib and surface level, length computation puma arm that puma arm respectively saves jib move to target location from current location when the current location in the coordinate change amount of jib plane coordinate system; And determine constraint condition according to ordinate change amount, then according to default optimized algorithm, determine angle between the adjacent two joint jibs of puma arm at puma arm the differential seat angle during in current location and target location;

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

Particularly, anglec of rotation determining unit 62 comprises:

The anglec of rotation is determined subelement (this figure is not shown), for receive that telepilot sends for characterizing after the Relatively orientation signal that the coordinate system of telepilot is not corresponding with jib space coordinates, the coordinate system of determining telepilot is not corresponding with jib space coordinates, and according to the angle of the coordinate system of Relatively orientation signal acquisition telepilot and preset reference direction, again according to the angle of the coordinate system of telepilot and preset reference direction, adjust jib projected coordinate system and the second coordinate, or adjust the coordinate system of telepilot.

In the present embodiment by utilizing universal handle and the 1 arm handle of telepilot, telepilot pattern is switched under automatic mode, based on the real-time angle of jib and turntable, the movement instruction sending according to telepilot, by current jib attitude, by a series of computings and specific position processing, draw the attitude in next moment of jib and judge that whether this attitude is reasonable, the inverse operation changing through attitude variation-oil cylinder length (turntable angle) variation-oil cylinder (motor) flow conversion-electromagnetic valve current, drive whole jib, it is moved according to remote command, 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, can make site operation time, jib operation easier reduces, pumping precision improves, thereby realize simple, operate easily jib, make operator concrete pump can accurately be delivered to target location.

It should be noted that, various embodiments of the present invention are take the control of pump truck puma arm as example explains, and 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, in order to limit the present invention, within the spirit and principles in the present invention not all, any modification of doing, be equal to replacement, improvement etc., within all should being included in protection scope of the present invention.

Claims (10)

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

Receive described puma arm each angle saving between jib and surface level in the time of current location of each angular transducer sensing and transmission;

According to described puma arm arm support tail end coordinate in jib space coordinates when the current location, determine first coordinate of described arm support tail end in jib plane coordinate system and the second coordinate in jib projected coordinate system;

In the coordinate system of the telepilot for controlling the motion of described puma arm and described jib space coordinates not at once, adjust described jib projected coordinate system and the second coordinate according to the coordinate system of described telepilot, or adjust the coordinate system of telepilot according to described jib projected coordinate system, so that the movement angle of described arm support tail end in the coordinate system of described telepilot is consistent with the movement angle in described jib projected coordinate system, and the movement instruction and described the second coordinate that send according to described telepilot, determine described puma arm arm support tail end three-dimensional in described jib projected coordinate system in the time of target location, and calculate the turntable anglec of rotation of described puma arm according to described the second coordinate and described three-dimensional, or, at once, according to described movement instruction and described the second coordinate, determine described puma arm arm support tail end three-dimensional in described jib projected coordinate system in the time of target location in the coordinate system of described telepilot and described jib space coordinates, and calculate the turntable anglec of rotation of described puma arm according to described the second coordinate and described three-dimensional,

Puma arm coordinate change amount jib plane coordinate system when current location moves to target location described in the length computation that respectively saves jib according to the angle between described the first coordinate, described each joint jib and surface level, described puma arm; And determine constraint condition according to described coordinate change amount, then according to default optimized algorithm, determine angle between the adjacent two joint jibs of described puma arm at described puma arm the differential seat angle during 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 respectively the motion of described puma arm turntable and jib.

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

Receive that described telepilot sends for characterizing after the Relatively orientation signal that the coordinate system of described telepilot is not corresponding with described jib space coordinates, 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, is characterized in that, the step of described " adjusting described jib projected coordinate system and the second coordinate according to the coordinate system of the telepilot for controlling described puma arm motion " comprising:

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

4. according to puma arm spatial movement control method described in any one in claim 1-3, it is characterized in that, described optimized algorithm is so that the quadratic sum minimum of the angle of adjacent two joints between jibs of described puma arm differential seat angle during in current location and target location at described puma arm is 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 _ichange and determine according to the handle aperture of described puma arm i joint jib Motor ability and described telepilot; Δ θ in the time of i>1 _ibe i-1 joint jib with i save angle between jib at described puma arm the differential seat angle during in current location and target location; Δ θ ₁for the variable quantity of angle between Section 1 jib and surface level when current location and the target location.

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

Angle between each joint jib when angle is determined described puma arm in target location between each joint jib and surface level when the current location according to described differential seat angle and described puma arm;

And respectively save maximal value and the minimum value of angle between jib according to default described puma arm, judge described puma arm angle whether in the scope between described minimum value and the described maximal value of correspondence between each joint jib in the time of target location;

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

6. state puma arm spatial movement control method according to claim 5, it is characterized in that, step at described " determine constraint condition according to described coordinate change amount; again according to default optimized algorithm, the angle of adjacent two joints between jibs of determining described puma arm be the differential seat angle during in current location and target location at described puma arm " comprises before:

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

And calculate described puma arm in the time of target location between each joint jib angle be corresponding described minimum value or described peaked jib quantity;

In the time that described jib quantity is more than or equal to n-1, directly stop described puma arm and respectively save the action of jib;

In the time that described jib quantity is less than n-1, carry out the step of " determine constraint condition according to described coordinate change amount; again according to default optimized algorithm, the angle of adjacent two joints between jibs of determining described puma arm be the differential seat angle during in current location and target location at described puma arm ".

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

According to finite element theory, the n that calculates described puma arm saves the amount of elastic deformation f of arm under current load _n; And according to described amount of elastic deformation f _nand the length of described n joint arm, the coordinate of the arm support tail end of asking for described puma arm under the local coordinate system of described n joint 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 the alternate position spike under the local coordinate system of same joint arm; Again according to described alternate position spike, described puma arm arm support tail end coordinate in jib space coordinates when the current location is carried out to elastic deformation compensating operation.

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

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

9. a puma arm spatial movement control device, is applied to the puma arm with n joint jib, and 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), for the described puma arm that receives each angular transducer sensing and the transmission angle between each joint jib and surface level when the current location;

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

Anglec of rotation determining unit (62), for the coordinate system of the telepilot for controlling the motion of described puma arm and described jib space coordinates not at once, adjust described jib projected coordinate system and the second coordinate according to the coordinate system of the telepilot for controlling described puma arm motion, or adjust the coordinate system of telepilot according to described jib projected coordinate system, so that the movement angle of the arm support tail end of described puma arm in the coordinate system of described telepilot is consistent with the movement angle in described jib projected coordinate system, and the movement instruction and described the second coordinate that send according to described telepilot, determine described puma arm arm support tail end three-dimensional in described jib projected coordinate system in the time of described target location, and calculate the turntable anglec of rotation of described puma arm according to described the second coordinate and described three-dimensional, or, at once, according to described movement instruction and described the second coordinate, determine described puma arm arm support tail end three-dimensional in described jib projected coordinate system in the time of described target location in the coordinate system of described telepilot and described jib space coordinates, and calculate the turntable anglec of rotation of described puma arm according to described the second coordinate and described three-dimensional,

The poor determining unit of angle (63), while moving to target location for puma arm described in the length computation that respectively saves jib according to the angle between described the first coordinate, described each joint jib and surface level, described puma arm from current location in the coordinate change amount of jib plane coordinate system; And determine constraint condition according to described coordinate change amount, then according to default optimized algorithm, determine angle between the adjacent two joint jibs of described puma arm at described puma arm the differential seat angle during in current location and target location;

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

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

The anglec of rotation is determined subelement, for receive that described telepilot sends for characterizing after the Relatively orientation signal that the coordinate system of described telepilot is not corresponding with described jib space coordinates, the coordinate system of determining described telepilot is not corresponding with described jib space coordinates, and according to the coordinate system of telepilot described in described Relatively orientation signal acquisition and the angle of preset reference direction, again according to the angle of the coordinate system of described telepilot and preset reference direction, adjust described jib projected coordinate system and the second coordinate, or adjust the coordinate system of telepilot.

Images