Summary of the invention
Embodiments provide a kind of multi-joint class machinery control method of jib, equipment, system and engineering machinery, the end for solving the multi-joint jib occurred when controlling multi-joint jib in prior art accurately can not arrive the low problem of operating efficiency that target location causes.
A control method for multi-joint class machinery jib, comprising:
Utilize the method for analog mechanical arm support tail end movement of setting, the trajectory path of mechanical arm support tail end movement be divided into into N strip trajectory path, wherein, N be not less than 1 positive integer;
For each strip trajectory path of other except the Article 1 sub-trajectory path obtained, perform following operation respectively:
According to the displacement of targets distance of mechanical jib motion on a upper strip trajectory path, actual displacement distance, displacement line speed and displacement line acceleration, calculate displacement line speed, the displacement line acceleration of mechanical jib motion in current sub-trajectory path, and Current mechanical arm support tail end can the length value of movement within the setting-up time cycle;
The current location information in each joint of collection machinery jib, utilize the computation of inverse-kinematics method of setting, calculate the angle information of the current needs in each joint of the mechanical arm frame movement when the sub-goal positional information that Current mechanical arm support tail end can arrive, wherein, the sub-goal positional information that described Current mechanical arm support tail end can arrive can the length value of movement determine according to the Current mechanical arm support tail end determined;
The angle information in each joint obtained is converted into control instruction, and electro-hydraulic proportional valve corresponding to each joint of controller mechanical arm frame moves.
A control appliance for multi-joint class machinery jib, comprising:
Divide module, for utilizing the method for the analog mechanical arm support tail end movement of setting, the trajectory path of mechanical arm support tail end movement is divided into into N strip trajectory path, wherein, N be not less than 1 positive integer;
Control module, for for each strip trajectory path of other except the Article 1 sub-trajectory path obtained, performs following operation respectively:
According to the displacement of targets distance of mechanical jib motion on a upper strip trajectory path, actual displacement distance, displacement line speed and displacement line acceleration, calculate displacement line speed, the displacement line acceleration of mechanical jib motion in current sub-trajectory path, and Current mechanical arm support tail end can the length value of movement within the setting-up time cycle;
The current location information in each joint of collection machinery jib, utilize the computation of inverse-kinematics method of setting, calculate the angle information of the current needs in each joint of the mechanical arm frame movement when the sub-goal positional information that Current mechanical arm support tail end can arrive, wherein, the sub-goal positional information that described Current mechanical arm support tail end can arrive can the length value of movement determine according to the Current mechanical arm support tail end determined;
The angle information in each joint obtained is converted into control instruction, and electro-hydraulic proportional valve corresponding to each joint of controller mechanical arm frame moves.
A control system for multi-joint class machinery jib, comprising:
Collecting device, for the positional information in collection machinery jib each joint current;
Control appliance, for utilizing the method for the analog mechanical arm support tail end movement of setting, is divided into into N strip trajectory path by the trajectory path of mechanical arm support tail end movement, wherein, N be not less than 1 positive integer;
For each strip trajectory path of other except the Article 1 sub-trajectory path obtained, perform following operation respectively: according to the displacement of targets distance of mechanical jib motion on a upper strip trajectory path, actual displacement distance, displacement line speed and displacement line acceleration, calculate displacement line speed, the displacement line acceleration of mechanical jib motion in current sub-trajectory path, and Current mechanical arm support tail end can the length value of movement within the setting-up time cycle;
Utilize the computation of inverse-kinematics method of setting, calculate the angle information of the current needs in each joint of the mechanical arm frame movement when the sub-goal positional information that Current mechanical arm support tail end can arrive, the angle information in each joint obtained is converted into control instruction, triggers electro-hydraulic proportional valve;
Electro-hydraulic proportional valve, for the control instruction of receiving control apparatus, and each joint motions of controller mechanical arm frame.
There is an engineering machinery for multi-joint class machinery jib, comprise the control system of above-mentioned multi-joint class machinery jib.
Beneficial effect of the present invention is as follows:
The embodiment of the present invention is by dividing the trajectory path of mechanical arm support tail end movement, obtain many strips trajectory path, for each strip trajectory path of other except the Article 1 sub-trajectory path obtained, perform following operation respectively: according to the displacement of targets distance of mechanical jib motion on a upper strip trajectory path, actual displacement distance, displacement line speed and displacement line acceleration, calculate the displacement line speed of mechanical jib motion in current sub-trajectory path, displacement line acceleration, and Current mechanical arm support tail end can the length value of movement within the setting-up time cycle, and the current location information in each joint of collection machinery jib, utilize the computation of inverse-kinematics method of setting, calculate the angle information of the current needs in each joint of the mechanical arm frame movement when the sub-goal positional information that Current mechanical arm support tail end can arrive, the angle information in each joint obtained is converted into control instruction, electro-hydraulic proportional valve corresponding to each joint of controller mechanical arm frame moves, compared with prior art, the trajectory path of arm support tail end is carried out discrete programming, and determine the angle information of other each joint movements according to inverse kinematics principle for the sub-trajectory path after planning, adequately achieve the flexibility of arm support tail end motion and each joint motions, ensure arm support tail end traveling comfort, the speed of reducing is undergone mutation the impact to mechanical jib brought, improve the control accuracy to multi-joint mechanical arm frame end.
Detailed description of the invention
In order to realize object of the present invention, embodiments provide the control method of a kind of multi-joint class machinery jib, equipment, system and engineering machinery, by the trajectory path of mechanical arm support tail end movement is divided, obtain many strips trajectory path, for each strip trajectory path of other except the Article 1 sub-trajectory path obtained, perform following operation respectively: according to the displacement of targets distance of mechanical jib motion on a upper strip trajectory path, actual displacement distance, displacement line speed and displacement line acceleration, calculate the displacement line speed of mechanical jib motion in current sub-trajectory path, displacement line acceleration, and Current mechanical arm support tail end can the length value of movement within the setting-up time cycle, and the current location information in each joint of collection machinery jib, utilize the computation of inverse-kinematics method of setting, calculate the angle information of the current needs in each joint of the mechanical arm frame movement when the sub-goal positional information that Current mechanical arm support tail end can arrive, the angle information in each joint obtained is converted into control instruction, electro-hydraulic proportional valve corresponding to each joint of controller mechanical arm frame moves.
Compared with prior art, the trajectory path of arm support tail end is carried out discrete programming, and determine the angle information of other each joint movements according to inverse kinematics principle for the sub-trajectory path after planning, adequately achieve the flexibility of arm support tail end motion and each joint motions, ensure arm support tail end traveling comfort, the speed of reducing is undergone mutation the impact to mechanical jib brought, and improves the control accuracy to multi-joint mechanical arm frame end.
Below in conjunction with Figure of description, each embodiment of the present invention is described in detail.
Embodiment one:
As shown in Figure 2, be the flow chart of the control method of a kind of multi-joint class machinery jib of the embodiment of the present invention one, described method comprises:
Step 101: the method utilizing the analog mechanical arm support tail end movement of setting, is divided into into N strip trajectory path by the trajectory path of mechanical arm support tail end movement.
Wherein, N be not less than 1 positive integer.
In a step 101, before the trajectory path of mechanical arm support tail end movement carries out discrete operations, described method also comprises: the trajectory path determining mechanical arm support tail end movement.
Particularly, determine that the method for the trajectory path of mechanical arm support tail end movement includes but not limited to: the positional information according to the current place of mechanical arm support tail end is determined with the target location moved to.
After the trajectory path determining mechanical arm support tail end movement, can carry out analogue enlargement in the following manner to the trajectory path of the mechanical arm support tail end movement determined, wherein, the method for the analog mechanical arm support tail end movement of described setting specifically includes but not limited to:
First kind of way: crawl triggering mode.Spatialization is carried out in the position at mechanical arm support tail end place, determine the X-axis at mechanical arm support tail end place, Y-axis and Z axis, respectively from the positive and negative direction of X-axis, the positive and negative direction of Y-axis and the positive and negative direction of Z axis provide arbitrary signal, mechanical arm support tail end is moved towards either direction with the track of straight line, until this blackout, the trajectory path of this mechanical arm support tail end can be obtained.
The second way: read skimulated motion trail file.Routing information in scene to be moved for mechanical arm support tail end is processed, and adopt curve-fitting method that the routing information after process is changed into the movement locus file of mechanical arm support tail end, using the movement locus file that the obtains trajectory path as mechanical arm support tail end movement.
The third mode: sampling generates the trajectory path of movement.Point trigger-type is adopted to obtain the desired motion track of mechanical arm support tail end, utilize existing software equipment in the process obtaining desired motion track, gather the positional information of mechanical arm support tail end on fixed time period, will the motion track information of positional information as mechanical arm support tail end be collected.
4th kind of mode: automatic orbit key point mode.By mechanical arm support tail end manually to original position, open software controlled device, select shape and the parameter of crucial line segment, again mechanical arm support tail end is moved in the key point of expectation, determine to record the routing information from original position to this key point, move in circles, the multiple routing informations obtained are integrated and obtains the trajectory path of mechanical arm support tail end from original position to final position.
More preferably, some key points are calibrated at the trajectory path determined, and by mechanical arm support tail end from a key point to the simulated operation of another key point, judge the shape in the sub-trajectory path between two key points, and determine the origin coordinates information in this sub-trajectory path and stop coordinate information, the traveling time of calculating machine arm support tail end on each strip trajectory path.
When the shape of group trajectory path is straight line, utilize discrete mode, according to origin coordinates information and termination coordinate information, portable cord speed, the linear acceleration of this space line, often set the sub-trajectory line segment of movement in moving period of unit, calculated the coordinate value of the sub-trajectory straightway impact point often setting movement in moving period of unit by coordinate.
Such as: the sub-trajectory path from A1 point to A2 point is straight line, so determine the co-ordinate position information of A1 point and the co-ordinate position information of A2 point, and the mode adopting mathematics discrete, calculate the sub-trajectory line segment of the sub-trajectory path movement in moving period of setting unit from A1 to A2: A1 to A11, A11 to A12 etc., and calculated the coordinate information of the sub-trajectory straightway impact point often setting movement in moving period of unit by coordinate, i.e. the coordinate information of A11, the coordinate information etc. of A12.
When the shape of group trajectory path is circular arc, utilize discrete mode, according to origin coordinates information and the termination coordinate information of this space circular arc, obtain centre coordinate information, the normal direction information of space circular arc, space circular arc radius information, utilize portable cord speed, linear acceleration, calculate the sub-trajectory arc section often setting movement in moving period of unit, calculated the coordinate information of the sub-trajectory circular arc line segment impact point often setting movement in moving period of unit by Coordinate Conversion.
Such as: the sub-trajectory path from A1 point to A2 point is circular arc, so determine the co-ordinate position information of A1 point and the co-ordinate position information of A2 point, and obtain the centre coordinate information of circular arc A1A2, the normal direction information of space circular arc, space circular arc radius information, adopt the mode that mathematics is discrete, utilize portable cord speed, linear acceleration, calculate the sub-trajectory arc section of the sub-trajectory path movement in moving period of setting unit from A1 to A2: A1 to A11, A11 to A12 etc., and the coordinate information of the sub-trajectory arc section impact point often setting movement in moving period of unit is calculated by coordinate, the i.e. coordinate information of A11, the coordinate information etc. of A12.
More preferably, after discrete processes is carried out to each strip trajectory path, the continuous print sub-trajectory line segment obtained after discrete or sub-trajectory circular arc are stored in track queue as a strip trajectory path information.
It should be noted that, for the trajectory path of the mechanical arm support tail end movement determined, also antithetical phrase trajectory path can be utilized to carry out discrete mode, obtain for the trajectory path line segment and/or trajectory path circular arc with trajectory path, like this by the trajectory path line segment determined and/or trajectory path circular arc, improve the precision of controller tool arm support tail end movement.
Step 102: for each strip trajectory path of other except the Article 1 sub-trajectory path obtained, circulation performs following operation:
Step 1021: according to the displacement of targets distance of mechanical jib motion on a upper strip trajectory path, actual displacement distance, displacement line speed and displacement line acceleration, calculate displacement line speed, the displacement line acceleration of mechanical jib motion in current sub-trajectory path, and Current mechanical arm support tail end can the length value of movement within the setting-up time cycle.
Particularly, in step 1021, obtain displacement line speed, the displacement line acceleration of mechanical jib motion in current sub-trajectory path in the following manner, and Current mechanical arm support tail end can the length value of movement within the setting-up time cycle:
Wherein:
l
targetfor displacement of targets distance, L
progressfor the range difference between actual displacement distance and displacement of targets distance, V
a upper linear velocityfor mechanical jib motion is in a upper strip trajectory path top offset linear velocity, V
linear velocityfor mechanical jib motion is in current sub-trajectory path top offset linear velocity, a
maxfor the peak acceleration of mechanical arm support tail end movement, t is mechanical jib end movement traveling time on current sub-trajectory path, and Δ s is the length value that Current mechanical arm support tail end needs movement.
More preferably, obtaining the displacement line speed of mechanical jib end movement in current sub-trajectory path, after displacement line acceleration, the maximum line velocity of the displacement line speed obtained and setting is compared, and the max line acceleration of the displacement line acceleration obtained and setting is compared, when the little linear velocity of the displacement obtained is greater than the maximum line velocity of setting, and/or the displacement line acceleration obtained is when being greater than the max line acceleration of setting, maximum linear velocity and linear acceleration is utilized to recalculate the mechanical jib displacement line speed of motion in current sub-trajectory path and displacement line acceleration.
More preferably, can the length value determination Current mechanical arm support tail end of the movement sub-goal positional information that can arrive according to the Current mechanical arm support tail end calculated, specifically comprise:
First, after the operation execution of mechanical arm support tail end on a upper strip trajectory path terminates, the positional information at the place of Current mechanical arm support tail end is gathered;
Secondly, according to described positional information and calculate Current mechanical arm support tail end can the length value of movement, obtain the mechanical jib sub-goal positional information that movement can arrive on current sub-trajectory path.
Step 1022: the current location information in each joint of collection machinery jib, utilize the computation of inverse-kinematics method of setting, calculate the angle information of each joint of mechanical arm frame current needs movement during the sub-goal positional information that can arrive at Current mechanical arm support tail end.
In step 1022, the angle information of the current needs in each joint of mechanical arm frame movement when profit determines the sub-goal positional information that can arrive at Current mechanical arm support tail end with the following methods:
First, according to the current location information in mechanical each joint of jib collected, utilize Jacobian matrix algorithm, obtain the matrix about each joint position information.
Particularly, concerning one degree of freedom is the mechanical jib of n, analyze from kinematics angle, can obtain its kinematical equation is X=JQ, and wherein: X is the position coordinates of arm support tail end at cartesian space, Q is the angle/motion vector in each joint;
And Jacobian matrix is 6*n rank matrixes, the joint variable in joint variable space can be converted to the joint variable of cartesian space by it;
Then to the differentiate of kinematical equation differential, and resolve and obtain inverse kinetics solution
wherein: J
+for pseudo inverse matrix, be n*6 matrix, and
the gradient vector of optimization object function H (θ),
Eventually through calculating the optimal motion velocity amplitude that can obtain each joint of mechanical arm frame.
Secondly, utilize Arithmetic of inverse kinematics, calculate the angle information of each joint of mechanical arm frame current needs movement during the sub-goal positional information that can arrive at Current mechanical arm support tail end.
More preferably, for each strip trajectory path, when determining the angle information for each joint of mechanical jib current needs movement in this sub-trajectory path, can revise by the current angle information of movement that needs each joint of mechanical jib determined, like this, the control instruction determined by the angle information of revised each joint of mechanical arm frame current needs movement, will improve the precision of mechanical arm support tail end arrival target location.
After the angle information calculating the current needs movement of each joint of mechanical jib, before the angle information in each joint obtained is converted into control instruction, described method also comprises:
For each joint, perform following operation:
First, three smooth track function # (t)=a are utilized
0+ a
1t+a
2t
2+ a
3t
3, calculate the curve meeting the movement locus in this joint, wherein, a
0, a
1, a
2and a
3represent the coefficient of three smooth track functions, t represents the traveling time that mechanical jib is current.
Particularly, the smooth track function in each joint need meet following constraints when calculating:
The i.e. position coordinates constraints of the geometric locus two-end-point in this joint and movement velocity constraints,
Wherein, θ
0for the moveable maximal angle value in this joint, θ
ffor the moveable minimum angles value in this joint, V
0for the moveable maximum speed value in this joint, V
ffor the moveable minimum speed value in this joint.
According to constraints, solve and obtain each coefficient in three smooth track functions:
Secondly, the path curves in this joint calculated is converted to the angle information that this joint needs movement.
More preferably, determine that this joint needs the angle information of movement in current track path according to each coefficient obtained.
Step 1023: the angle information in each joint obtained is converted into control instruction, electro-hydraulic proportional valve corresponding to each joint of controller mechanical arm frame moves.
More preferably, in a strip trajectory path, after mobile end, when starting to move in next strip trajectory path, following operation is performed at mechanical arm support tail end:
The first step: judge whether the difference of mechanical arm support tail end actual displacement Distance geometry displacement of targets distance in a upper strip trajectory path is greater than setting numerical value.
Second step: when the difference of mechanical arm support tail end actual displacement Distance geometry displacement of targets distance in a upper strip trajectory path is greater than setting numerical value, then calculate the range difference between actual displacement distance and displacement of targets distance in a upper strip trajectory path, and according to the range difference calculated, mechanical arm support tail end in upper strip trajectory path centerline velocities and a linear acceleration, update stored in the length value in the current sub-trajectory path in track queue, and operate for the current sub-trajectory path after renewal in the updated.
When the difference of mechanical arm support tail end actual displacement Distance geometry displacement of targets distance in a upper strip trajectory path is not more than setting numerical value, triggers and operate for the current sub-trajectory path in track queue.
It should be noted that, when mechanical arm support tail end is started working, for the Article 1 sub-trajectory path of this trajectory path mode of operation from except the mode of operation in Article 1 sub-trajectory path different, be specially:
Described for Article 1 sub-trajectory path, perform following operation:
First: the first sub-goal positional information determining Article 1 sub-trajectory path.
Second: the current location information in each joint of collection machinery jib, and according to the setting initial plant degree of mechanical arm support tail end movement and linear acceleration, calculate the angle information of each joint current needs movement when mechanical arm support tail end arrives position corresponding to the first sub-goal positional information.
3rd: the angle information in each joint obtained is converted into control instruction, electro-hydraulic proportional valve corresponding to each joint of controller mechanical arm frame moves.
By the scheme of the embodiment of the present invention one, utilize the algorithm of setting, the trajectory path of mechanical arm support tail end movement is carried out discrete processes, obtain many strips trajectory path, for each strip trajectory path, perform following operation: according to the displacement of targets distance of mechanical jib motion on a upper strip trajectory path, actual displacement distance, displacement line speed and displacement line acceleration, calculate the displacement line speed of mechanical jib motion in current sub-trajectory path, displacement line acceleration, and Current mechanical arm support tail end can the length value of movement within the setting-up time cycle, and the current location information in each joint of collection machinery jib, utilize the computation of inverse-kinematics method of setting, calculate the angle information of the current needs in each joint of the mechanical arm frame movement when the sub-goal positional information that Current mechanical arm support tail end can arrive, the angle information in each joint obtained is converted into control instruction, electro-hydraulic proportional valve corresponding to each joint of controller mechanical arm frame moves, compared with prior art, the trajectory path of arm support tail end is carried out discrete programming, and determine the angle information of other each joint movements according to inverse kinematics principle for the sub-trajectory path after planning, adequately achieve the flexibility of arm support tail end motion and each joint motions, ensure arm support tail end traveling comfort, the speed of reducing is undergone mutation the impact to mechanical jib brought, improve the control accuracy to multi-joint mechanical arm frame end.
Embodiment two:
As shown in Figure 3, be the structural representation of the control appliance of a kind of multi-joint class machinery jib of the embodiment of the present invention two, described equipment comprises: discrete processes module 11 and control module 12, wherein:
Discrete processes module 11, for utilizing the method for the analog mechanical arm support tail end movement of setting, is divided into into N strip trajectory path by the trajectory path of mechanical arm support tail end movement, wherein, N be not less than 1 positive integer;
Control module 12, for for each strip trajectory path of other except the Article 1 sub-trajectory path obtained, performs following operation respectively:
According to the displacement of targets distance of mechanical jib motion on a upper strip trajectory path, actual displacement distance, displacement line speed and displacement line acceleration, calculate displacement line speed, the displacement line acceleration of mechanical jib motion in current sub-trajectory path, and Current mechanical arm support tail end can the length value of movement within the setting-up time cycle;
The current location information in each joint of collection machinery jib, utilize the computation of inverse-kinematics method of setting, calculate the angle information of the current needs in each joint of the mechanical arm frame movement when the sub-goal positional information that Current mechanical arm support tail end can arrive, wherein, the sub-goal positional information that described Current mechanical arm support tail end can arrive can the length value of movement determine according to the Current mechanical arm support tail end determined;
The angle information in each joint obtained is converted into control instruction, and electro-hydraulic proportional valve corresponding to each joint of controller mechanical arm frame moves.
More preferably, the trajectory path of described mechanical arm support tail end movement determines with the target position information moved to according to the positional information at the current place of mechanical jib.
Particularly, described discrete processes module 11, specifically comprises:
Divide submodule 21, for described trajectory path is divided into N strip trajectory path, and determines the origin coordinates information in this sub-trajectory path and stop coordinate information, the traveling time of calculating machine arm support tail end on each strip trajectory path;
Judge submodule 22, for judging that M strip trajectory path is straight line or circular arc;
Calculating sub module 23, if be straight line for judged result, then utilize discrete mode, according to origin coordinates information and termination coordinate information, portable cord speed, the linear acceleration of this space line, often set the sub-trajectory line segment of movement in moving period of unit, calculated the coordinate value of the sub-trajectory straightway impact point often setting movement in moving period of unit by coordinate;
If judged result is circular arc, then utilize discrete mode, according to origin coordinates information and the termination coordinate information of this space circular arc, obtain centre coordinate information, the normal direction information of space circular arc, space circular arc radius information, utilize portable cord speed, linear acceleration, calculate the sub-trajectory arc section often setting movement in moving period of unit, calculated the coordinate information of the sub-trajectory circular arc line segment impact point often setting movement in moving period of unit by Coordinate Conversion;
Sub module stored 24, for being stored in the continuous print sub-trajectory line segment obtained after discrete or sub-trajectory circular arc in track queue as a strip trajectory path information.
Particularly, described control module 12, specifically for determining the first sub-goal positional information in Article 1 sub-trajectory path, the current location information in each joint of collection machinery jib, and according to setting initial plant degree and the linear acceleration of mechanical arm support tail end movement, calculate the angle information of the current needs in each joint movement when mechanical arm support tail end arrives position corresponding to the first sub-goal positional information, and the angle information in each joint obtained is converted into control instruction, electro-hydraulic proportional valve corresponding to each joint of controller mechanical arm frame moves.
Described control module 12, specifically for calculating the displacement line speed of mechanical jib end movement in current sub-trajectory path, displacement line acceleration in the following manner, and Current mechanical arm support tail end needs the length value of movement within the setting-up time cycle:
Wherein:
l
targetfor displacement of targets distance, L
progressfor the range difference between actual displacement distance and displacement of targets distance, V
a upper linear velocityfor mechanical jib motion is in a upper strip trajectory path top offset linear velocity, V
linear velocityfor mechanical jib motion is in current sub-trajectory path top offset linear velocity, a
maxfor the peak acceleration of mechanical arm support tail end movement, t is mechanical jib motion moving period on a upper strip trajectory path, and Δ s is the length value that Current mechanical arm support tail end needs movement.
Described control module 12, after terminating when the operation execution of mechanical arm support tail end on a upper strip trajectory path, gather the positional information at the place of Current mechanical arm support tail end, and according to described positional information and calculate Current mechanical arm support tail end can the length value of movement, obtain the mechanical jib sub-goal positional information that movement can arrive on current sub-trajectory path.
Described control module 12, specifically for the current location information according to mechanical each joint of jib collected, utilize Jacobian matrix algorithm, obtain the matrix about each joint position information, and utilize Arithmetic of inverse kinematics, calculate the angle information of each joint of mechanical arm frame current needs movement during the sub-goal positional information that can arrive at Current mechanical arm support tail end.
More preferably, described equipment also comprises: interpolation module 13, wherein:
Interpolation module 13, for for each joint, performs following operation:
Utilize three smooth track function # (t)=a
0+ a
1t+a
2t
2+ a
3t
3, calculate the curve meeting the movement locus in this joint, wherein, a
0, a
1, a
2and a
3represent the coefficient of three smooth track functions, t represents the traveling time that mechanical jib is current, the path curves in this joint calculated is converted to the angle information that this joint needs movement.
More preferably, described equipment also comprises: more new module 14, wherein:
More new module 14, for on a strip trajectory path operation after execution terminates, before starting for current sub-trajectory path operations execution, judge whether the difference of mechanical arm support tail end actual displacement Distance geometry displacement of targets distance in a upper strip trajectory path is greater than setting numerical value;
When the difference of mechanical arm support tail end actual displacement Distance geometry displacement of targets distance in a upper strip trajectory path is greater than setting numerical value, then calculate the range difference between actual displacement distance and displacement of targets distance in a upper strip trajectory path, and according to the range difference calculated, mechanical arm support tail end in upper strip trajectory path centerline velocities and a linear acceleration, update stored in the length value in the current sub-trajectory path in track queue;
When the difference of mechanical arm support tail end actual displacement Distance geometry displacement of targets distance in a upper strip trajectory path is not more than setting numerical value, trigger control module.
Embodiment three:
As shown in Figure 4, be the structural representation of the control system of a kind of multi-joint class machinery jib of the embodiment of the present invention three, described system comprises: collecting device 31, control appliance 32 and electro-hydraulic proportional valve 33, wherein:
Collecting device 31, for the positional information in collection machinery jib each joint current.
Control appliance 32, for utilizing the method for the analog mechanical arm support tail end movement of setting, is divided into into N strip trajectory path by the trajectory path of mechanical arm support tail end movement, wherein, N be not less than 1 positive integer;
For each strip trajectory path of other except the Article 1 sub-trajectory path obtained, perform following operation respectively: according to the displacement of targets distance of mechanical jib motion on a upper strip trajectory path, actual displacement distance, displacement line speed and displacement line acceleration, calculate displacement line speed, the displacement line acceleration of mechanical jib motion in current sub-trajectory path, and Current mechanical arm support tail end can the length value of movement within the setting-up time cycle;
Utilize the computation of inverse-kinematics method of setting, calculate the angle information of the current needs in each joint of the mechanical arm frame movement when the sub-goal positional information that Current mechanical arm support tail end can arrive, the angle information in each joint obtained is converted into control instruction, triggers electro-hydraulic proportional valve;
Electro-hydraulic proportional valve 33, for the control instruction of receiving control apparatus, and each joint motions of controller mechanical arm frame.
Particularly, control instruction is received by CAN.
More preferably, described collecting device 31 can be displacement transducer 41 and rotary coding sensor 42.
Described system also comprises: I/O analog input and output card 34, wherein:
I/O analog input and output card 34, the angle value for collecting device being collected mechanical each joint of jib is converted into the discernible signal message of control appliance, exports control appliance to.
Described system also comprises: touch display screen 35, wherein,
Touch display screen, for carrying out man-machine information interaction by VGA serial ports, comprises the display of the input of controling parameters, the output of state parameter and jib threedimensional model.
Described system also comprises: wireless remote controlled receiver 36, wherein:
Wireless remote controlled receiver 36, for receiving jib end-of-pipe control signal by CAN.
It should be noted that, described control appliance can be RT-Linux built-in industrial control machine.
The present invention also provides a kind of engineering machinery with multi-joint class machinery jib, comprises the control system of above-mentioned multi-joint class machinery jib.
Obviously, those skilled in the art can carry out various change and modification to the present invention and not depart from the spirit and scope of the present invention.Like this, if these amendments of the present invention and modification belong within the scope of the claims in the present invention and equivalent technologies thereof, then the present invention is also intended to comprise these change and modification.