Summary of the invention
The first technical problem to be solved by the present invention is to need to provide a kind of pose for survey engineering mechanical armMethod and system.
In order to solve the above-mentioned technical problem, embodiments herein provides firstly a kind of for survey engineering mechanical armThe method of pose, this method comprises: at least three targets, are mounted on the end of the engineering machinery arm by target installation steps,This at least three target and the set point of the central axes of the mechanical arm cantilever crane is set to constitute predetermined location relationship；Parameters acquiring procedure,Obtain the parameters of structural dimension of location information and the target of at least three target under mechanical arm carrier coordinate systemWith installation dimension parameter；Mechanical arm tail end pose calculates step, using the predetermined location relationship and acquisition location information andParameters of structural dimension and installation dimension parameter, the mechanical arm tail end is calculated currently includes under mechanical arm carrier coordinate systemThe posture information of position coordinates and posture.
According to one embodiment of present invention, it is calculated in step in the mechanical arm tail end pose, includes the following steps: appearanceState calculates step, determines first attitude matrix and the target of the target coordinate system relative to end arm support joint coordinate systemSecond attitude matrix of the coordinate system relative to mechanical arm carrier coordinate system is determined according to the first attitude matrix and the second attitude matrixAttitude matrix of the end arm support joint coordinate system relative to mechanical arm carrier coordinate system, to obtain the posture of the mechanical arm tail endInformation.
According to one embodiment of present invention, it is calculated in step in the mechanical arm tail end pose, further includes following steps:Position calculates step, according to the parameters of structural dimension of target and installation dimension parameter, obtains setting on mechanical arm central axes respectivelyThe coordinate of fixed point and the distance and the set point of end arm support articulation center point under mechanical arm carrier coordinate system, and then obtainCoordinate of the end arm support articulation center point under mechanical arm carrier coordinate system.
According to one embodiment of present invention, in Attitude Calculation step, according to three targets in mechanical arm carrier coordinateThe positional relationship between coordinate and target coordinate system and three targets under system can acquire first attitude matrix；According to obtainingThe setting of the central axes of the location information and this at least three target and the mechanical arm cantilever crane of at least three target takenPoint constitutes predetermined location relationship and determines second attitude matrix.
According to one embodiment of present invention, when installing three targets, the predetermined location relationship be three targets withCentered on the central axes of mechanical arm cantilever crane, line differs set angle each other between adjacent two target and the set point on central axesDegree.
According to another aspect of the present invention, a kind of system of pose for survey engineering mechanical arm is additionally provided, this isSystem includes: at least three targets, is installed in the end of the engineering machinery arm, makes this at least three target and the mechanical armThe set point of the central axes of cantilever crane constitutes predetermined location relationship；Parameter obtaining device obtains at least three target in machineThe parameters of structural dimension and installation dimension parameter of location information and the target under tool arm carrier coordinate system；Mechanical arm tail endPose computing device is joined using the predetermined location relationship and the location information and parameters of structural dimension and installation dimension of acquisitionPose letter including position coordinates and posture of the mechanical arm tail end currently under mechanical arm carrier coordinate system is calculated in numberBreath.
According to one embodiment of present invention, the mechanical arm tail end pose computing device, further comprises such as lower dieBlock: Attitude Calculation module determines first attitude matrix of the target coordinate system relative to end arm support joint coordinate system, Yi JisuoSecond attitude matrix of the target coordinate system relative to mechanical arm carrier coordinate system is stated, according to the first attitude matrix and the second posture squareBattle array determines attitude matrix of the end arm support joint coordinate system relative to mechanical arm carrier coordinate system, to obtain the mechanical arm tail endPosture information.
According to one embodiment of present invention, the mechanical arm tail end pose computing device, further includes following module: positionComputing module is set, according to the parameters of structural dimension of target and installation dimension parameter, obtains setting on mechanical arm central axes respectivelyThe coordinate of fixed point and the distance and the set point of end arm support articulation center point under mechanical arm carrier coordinate system, and then obtainCoordinate of the end arm support articulation center point under mechanical arm carrier coordinate system.
According to one embodiment of present invention, the Attitude Calculation module, further performs the following operations: according to threePositional relationship of the target between the coordinate and target coordinate system and three targets under mechanical arm carrier coordinate system acquires describedFirst attitude matrix；According to the location information of at least three target of acquisition and this at least three target and the mechanical armThe set point of the central axes of cantilever crane constitutes predetermined location relationship and determines second attitude matrix.
Compared with prior art, one or more embodiments in above scheme can have following advantage or beneficial to effectFruit:
Method of the invention constructs a georeferencing object using at least three target balls being mounted on end arm support, whenWhen the structure size and installation site of target ball determine, there is determining relative pose to close for the georeferencing object and end arm supportSystem.And the georeferencing object being made of target ball can be sat in the pose of mechanical arm carrier coordinate system by measuring the position of each target ballMark is calculated and is acquired.Therefore, further according to the position orientation relation of georeferencing object and end arm support, end arm support can be acquired in machineryPose under arm carrier coordinate system.It is more accurate, quick by the pose of above method calculating machine arm end arm support.
Other features and advantages of the present invention will be illustrated in the following description, also, partly becomes from specificationIt obtains it is clear that being understood by implementing technical solution of the present invention.The objectives and other advantages of the invention can bySpecifically noted structure and/or process are achieved and obtained in specification, claims and attached drawing.
Embodiment one
Firstly, introducing the measuring tool used in following this example: a laser tracker (example of coordinate information collectorSon), foldable target ball mechanism (example of target, specific structure can as shown in Figure 4).The foldable target ball mechanism can lead toThe driving for crossing motor is unfolded and is closed, and to guarantee in measurement process, is prevented as caused by the movement of mechanical armThe case where target ball is blocked guarantees being normally carried out for measurement.Structure in relation to the target ball mechanism, herein without limitation, as long as energyEnough has the target ball of above-mentioned function.
As shown in figure 4, the meaning of each appended drawing reference is as follows in the accompanying drawings: the meaning of each appended drawing reference is as follows in the accompanying drawings:1, pedestal, 2, folding target ball, 3, protection board, 10, target ball support deck, 11, protection board support deck, 12, intermediate transmissionFrame, 13, rack gear, 14, first sliding groove, 15, second sliding slot, the 16, first sliding pin, the 17, second sliding pin, 18, motor, 19, silkThick stick, 21, flip-arm, 22, support arm, 23, target ball ontology, 31, rotating shaft, 32, gear.
The structure of the target ball mechanism is described below.
As shown in figure 4, present embodiments providing a kind of target ball mechanism, comprising: pedestal 1, be arranged on the pedestal 1 canCollapsible target ball 2, the folding target ball 2 are constructed to be permeable to fold or stretch on the pedestal 1.
Preferably, the pedestal 1 is provided with folding protection board 3, and the folding target ball 2 is in folded stateWhen, the protection board 3 is in folded state and is covered on the top of the folding target ball 2.Preferably, the pedestal 1 wrapsThe target ball support deck 10 for connecting the folding target ball 2 is included, and the protection board of the connection protection board 3 supports deck11；Moveable intermediate transmission frame 12 is provided between the target ball support deck 10 and protection board support deck 11；ItsIn, the intermediate transmission frame 12 is able to drive the folding target ball 2 and the protection board 3 while folding or stretching when mobileExhibition.
In one example, first sliding groove 14, one end setting of intermediate transmission frame 12 are provided on target ball support deck 10There is the first sliding pin 16 matched with the first sliding groove 14；Second sliding slot 15, institute are provided on protection board support deck 11The other end for stating intermediate transmission frame 12 is provided with the second sliding pin 17 matched with second sliding slot 15.
Further, folding target ball 2 includes the flip-arm 21 being hinged on target ball support deck 10, and is hinged on instituteThe support arm 22 on intermediate transmission frame 12 is stated, and the upper end of the support arm 22 is provided with target ball ontology 23；Wherein, flip-arm21 upper end and the middle part of support arm 22 are hinged.
In one example, the angle of certain angle, the angle are set between target ball ontology 23 and the support arm 22Guarantee that the target ball ontology 2 and the flip-arm 21 are in parastate when the folding target ball 2 is in folded state.
Preferably, the bottom of protection board 3 supports deck 11 hinged by rotating shaft 31 and the protection board, the rotating shaftIt is arranged on 31 and with the co-rotational gear 32 of the rotating shaft 31；It is provided on the intermediate transmission frame 12 and the gear 32The rack gear 13 matched.
In one example, being provided on rotating shaft 31 makes the protection board 3 in the case where not stressing in stretch-likeThe torsional spring of state, and the protection board 3 is in extended state, the rack gear 13 with the intermediate transmission frame 12 be moved to it is describedThe position that gear 32 is detached from.
Preferably, it is provided with power device between the protection board support deck 11 and the intermediate transmission frame 12, it is describedPower device is constructed to be permeable to drive the intermediate transmission frame 12 mobile to drive the folding target ball 2 and the protectionPlate 3 folds or stretching, extension.
Further, power device includes lead screw 19, and one end of the lead screw 19 is rotatably connected on the protection board support cardOn seat (11), the other end is connected with the motor 18 for driving lead screw 19 to rotate；Wherein, it is provided with screw hole on intermediate transmission frame 12, instituteLead screw 19 is stated across the screw hole, and the screw thread of the lead screw 19 is matched with the screw thread of the screw hole.
Next, illustrating how the pose measurement of progress engineering machinery arm with reference to Fig. 1, Fig. 1 is the embodiment of the present application oneThe pose for survey engineering mechanical arm method flow diagram.
In step S110 (target installation steps), at least three targets are mounted on to the end of the engineering machinery arm,The set point on the central axes of this at least three target and the mechanical arm cantilever crane is set to constitute predetermined location relationship.
In step S120 (parameters acquiring procedure), at least three target is obtained under mechanical arm carrier coordinate systemThe parameters of structural dimension and installation dimension parameter of location information and the target.
In step S130 (mechanical arm tail end pose calculates step), the position of the predetermined location relationship and acquisition is utilizedThe mechanical arm tail end is calculated currently under mechanical arm carrier coordinate system in information and parameters of structural dimension and installation dimension parameterThe posture information including position coordinates and posture.
Below to illustrate above-mentioned each step for mechanical arm tail end installs three foldable target ball mechanisms.
In step s 110, by three target ball mechanisms centered on the central axes of mechanical arm cantilever crane, so that adjacent two target ball(face refers to the face that the set point on adjacent two target ball and central axes is constituted, centered on the set point in this for axial plane in mechanismPoint A ', such as plane P1, P2 and P3 in Fig. 2) between differ set angle each other, such as 120 ° are symmetrically installed, as shown in Fig. 2,B, C, D are respectively three target ball mechanisms in figure, and A ' is the central point on cantilever crane central axes.
Start to get parms in the step s 120: firstly, industrial personal computer issues calibration instruction, control motor makes three target ball exhibitionsIt opens, laser tracker starting.Then, the target ball coordinate information of end three is acquired using laser tracker, and anti-by can busIt is fed in industrial personal computer.On the other hand, the parameters of structural dimension and installation dimension parameter of the target also to be obtained.
Then, in step s 130, industrial personal computer finds out mechanical arm end using the location information of at least three target balls of feedbackEnd position and posture.An example is set forth below to illustrate how the position and attitude for measuring mechanical arm by three target balls, specific packetIt includes attitude measurement and position measures two parts.
(1) attitude measurement
Determine first attitude matrix and the target coordinate system phase of the target coordinate system relative to end arm support coordinate systemFor the second attitude matrix of mechanical arm carrier coordinate system, end arm support is determined according to the first attitude matrix and the second attitude matrixAttitude matrix of the coordinate system relative to mechanical arm carrier coordinate system, to obtain the posture information of the mechanical arm tail end.Specific sideFormula is as described below.
Firstly, target ball is installed as shown in Figure 2 relative to robot central point A '.Just due to 3 points of target ball center B, C, DA triangle is constituted well, therefore when the position B, C, D determines, unique point A is certainly existed on mechanical arm central axes, so thatIt is mutually perpendicular between any two.
The pose for forming georeferencing object by three target ball constructions for ease of calculation, using A as origin,Direction is respectively x, y, z axis direction to establish target coordinate system A-xAyAzA, while cantilever crane joint seat is established on cantilever crane EA 'Mark system E-xEyEzE, E is the articulation center point of end arm support, xEAxis along cantilever crane central axes, withIn the same direction, zEThe vertical cantilever crane of axis toOn, withDirection is identical；It is as shown in Figure 3 to establish mechanical arm carrier coordinate system O-xyz.According to target ball design feature it is found thatDuring target ball expansion and withdrawal, relative to cantilever crane central point A ', only distance changes target ball center B, C, D, and direction is constant.Therefore no matter target ball moves to any position, coordinate system A-xAyAzAWith end arm support coordinate system E-xEyEzEThere is determining appearancesState matrix(the first attitude matrix).
It willProject to xAAyAPlane obtainsWith xAThe angle of axis is α=45 °, withAngle is β=35.26 °, the target coordinate system A-x that can be asked is defined according to Eulerian anglesAyAzAWith end arm support coordinate system E-xEyEzEThere is trueFixed attitude matrix
Therefore a demand obtains target coordinate system A-xAyAzAAttitude matrix relative to mechanical arm carrier coordinate system?Acquire attitude matrix of the end arm support relative to mechanical arm carrier coordinate system
According to the location information of at least three target of acquisition and this at least three target and the mechanical arm cantilever craneCentral axes set point constitute predetermined location relationship determine second attitude matrix.It specifically, can using laser trackerThe coordinate of target ball B, C, D under mechanical arm carrier coordinate system mechanical arm carrier coordinate system is respectively under certain position surveyedThen A ' is carried in mechanical arm carrier coordinate system mechanical armCoordinate under body coordinate system isJust due to A-BCDConstitute one well with the special triangular pyramid on the vertex A: three rib length are identical and are mutually perpendicular to, therefore in known B, C, D situationUnder, the coordinate that can acquire A point is calculated using geometry teaching.A point is under mechanical arm carrier coordinate system mechanical arm carrier coordinate systemCoordinateFollowing relationship should be met:
It is defined according to attitude matrix it is found that attitude matrixFor target coordinate system xA、yA、zAUnit vector on axis is in machineProjection under tool arm carrier coordinate system x, y, z axis.X can be acquired by A, B, C, D point coordinateA、yA、zAUnit vector on axis:
Then target coordinate system A-xAyAzAAttitude matrix relative to mechanical arm carrier coordinate system(the second attitude matrix)Are as follows:
Then attitude matrix of the end arm support coordinate system relative to mechanical arm carrier coordinate systemAre as follows:
(2) position measures
According to the parameters of structural dimension of target and installation dimension parameter, obtain respectively set point on mechanical arm central axes andThe distance of end arm support articulation center pointAnd coordinate of the set point under mechanical arm carrier coordinate systemAnd then it obtainsTake coordinate of the end arm support articulation center point under mechanical arm carrier coordinate system.
Fig. 5 is the kinematic sketch of target ball mechanism deploying, and figure central axes a is the central axes of end arm support, and revolute pair G is overturningRotating shaft between arm 21 and target ball bearing cartridge 10；Rotating shaft of the revolute pair F between support arm 22 and flip-arm 21；B isTarget ball measuring center point；Prismatic pair H corresponds to active pin shaft 16.
As shown in fig. 6, confirmable target ball structure size and installation dimension parameter have: the length d of connecting rod GFGF；Between BHDistance dBH；And the horizontal distance d under original state between G and HGHx；Displacement sensor, measurement are installed at linear joint HThe displacement Δ d (being detected using displacement sensor) of active pin shaft 16；The distance between EG ' is d1；Linear joint H is away from cantilever craneAxial line distance is dA′I；Revolute pair G is d away from cantilever crane central axes distanceGG′.According to above-mentioned parameter, following size can be further acquired:
The real standard distance of G and H:
d2=dGHx+Δd
Target ball can be acquired away from cantilever crane central axes distance according to the coordinate of B point and at A 'Therefore the horizontal distance of H and J:
The distance between two o'clock can be acquired according to the coordinate of A point and at A 'AndAre as follows:
Due toWithCollinearly, therefore:
It can then acquireCoordinate of the as E point under mechanical arm carrier coordinate system:
Method through the embodiment of the present invention is capable of position and the posture of real-time and accurate measurement mechanical arm, less important work personnelFor controlling the movement of the mechanical arm, preferably fulfil assignment task.