CN104786226A - Posture and moving track positioning system and method of robot grabbing online workpiece - Google Patents

Abstract

The invention discloses a posture and moving track positioning system and method of robot grabbing an online workpiece. The device comprises a camera support, an industrial camera, the tested workpiece, a workpiece conveying belt, a tail end executer, a robot system, a workbench, an image processor and the like. The method comprises the steps that a depth positioning model of a plane point in an absolute coordinate system in the vertical direction, a depth positioning model of a space point in the absolute coordinate system in the vertical direction, an X-axis positioning model of the plane point in the absolute coordinate system in the horizontal direction and an X-axis positioning model of the space point in the absolute coordinate system in the horizontal direction are established, three-dimensional coordinates of imaging points in all the cameras can be solved through combination of the priori knowledge of the workpiece, accordingly, and a robot can be guided to adopt the accurate tail end executer posture and movement track to intelligently grab the workpiece accurately. The posture and moving track positioning system and method have the advantages that the grabbing precision is high, the structure is simple, and cost is low.

Description

Capture the robot pose of online workpiece and movement locus navigation system and method

Technical field

Motion planning and robot control of the present invention and vision positioning field, particularly a kind of capture online workpiece robot pose and movement locus navigation system and method.

Background technology

At present, industrial robot is applied widely in each industrial circle, and it can replace manually making and repeatably moves accurately.Robot will complete the intelligent grabbing of 3 D workpiece, must along particular track, adopt corresponding pose and movement locus to move.At present in the crawl of static three-dimensional workpiece, what industrial robot captured pose and movement locus is positioned with on-line teaching, off-line programing; In the online crawl in real time of Moving Workpieces, there is the guidance system that monocular vision guidance system, binocular vision guidance system and structured light are combined with vision at present.But, there is many-sided deficiency in existing industrial robot three-dimensional pose and movement locus location technology:

1) on-line teaching does not need the tool coordinates system of demarcating robot, the pose of robot, movement locus and clamping parameter rely on the experience range estimation of teaching engineering staff to obtain, system accuracy is lower, online programming is loaded down with trivial details, take the production time, be difficult to meet the current flexible production demand of multi items conversion fast, particularly can not meet the demand of the uncertain online conveying workpieces of automatic capturing pose.

2) must demarcate robot to obtain real robot location's spatial model.Conventional calibration facility has three coordinate measuring machine, joint arm measuring machine and laser tracker, but for restricted to the Robot calibration in operative scenario, location position error is still larger.

3) single camera vision system is generally used for the information extraction on two dimensional surface, cannot obtain the information of workpiece on Z axis (depth direction), and what pass to robot only has position in plane and angular metric.Camera optical axis must keep exact vertical with workpiece planarization, and depth location can only lean on empirical value people for presetting fixed value.

4) structured light combines with one camera and obtains the mode of workpiece information, need carry out graphics decoding computing.For ensureing calculation accuracy, camera and workpiece relative position must be kept to fix, be difficult to the crawl being applicable to Moving Workpieces, and relate to three-dimensional point cloud calculating, computing complicated and time consumption.

5) Binocular Stereo Vision System, utilize two cameras be fixed angle from different viewing angles workpiece image, calculate the three-dimensional coordinate of workpiece.Will through the process such as binocular vision calibration, extraction of depth information, computing more complicated, consuming time longer, cost is higher, and robustness is poor.

Therefore, for the 3 D workpiece that industrial robot automatic capturing moves online, there are development workpiece pose and movement locus to identify the demand of new method online fast, to improve above-mentioned shortcoming with not enough.

Summary of the invention

Main purpose of the present invention is that the shortcoming overcoming prior art is with not enough, a kind of the robot pose and the movement locus navigation system that capture online workpiece are provided, this system based on single camera vision system, be applicable to utilize industrial robot intelligence, rapidly capture known form on the modal production pipeline in manufacturing shop and the uncertain middle-size and small-size workpiece of pose (can difformity, specification, material various workpieces mixing).

Another object of the present invention is to provide a kind of based on the robot pose of the online workpiece of above-mentioned crawl and the localization method of movement locus navigation system.The method based on single camera vision system, but can obtain the three dimensional local information of workpiece, and calculates simple, and speed is fast.

Object of the present invention is realized by following technical scheme: the robot pose and the movement locus navigation system that capture online workpiece, comprise the camera for captured in real-time workpiece, the robot system for grabbing workpiece, obtain for image captured by camera image processor, the workbench that robot system captures coordinate, described camera is fixed on a camera support, and camera support and robot system are arranged at the both sides of workbench respectively; Described image processor is connected with the robot controller in robot system with camera respectively; Described workbench is provided with workpiece transfer band, and workpiece moves on workpiece transfer band; The end of described robot system is provided with for clamping and the end effector of rotational workpieces; The optical axis of described camera is vertical with the direction of workpiece motion s, and camera is for obtaining the shaft side figure picture of workpiece.The present invention adopts one camera obtain the workpiece information of uncertain pose in conveying motion and calculate its three-dimensional coordinate, and guided robot adopts correct end effector pose and movement locus accurately to capture workpiece intelligently.

Preferably, be provided with at the two ends of described workpiece transfer band the photoelectric sensor whether entering viewing field of camera for detecting workpiece, this photoelectric sensor is connected with the external trigger head of camera.Thus camera can be triggered by sensor signal and take pictures.

Based on the robot pose of the online workpiece of above-mentioned crawl and a localization method for movement locus navigation system, comprise step:

The installation site of S1, adjustment camera and setting angle, after making workpiece enter viewing field of camera, phase function obtains the shaft side figure picture of workpiece;

S2, scaling board to be positioned on workpiece transfer band, and to be positioned at the field of view center of camera, geometric calibration is carried out to camera-workpiece transfer band plane-end effector;

" the conversion Mathematical Modeling in v in workpiece features image coordinate and absolute coordinate system XYZ between workpiece three-dimensional pose coordinate of S3, according to the installing space position of camera and known workpiece shapes size, setting up frame coordinate system uo;

S4, according to the locus of robot in absolute coordinate system, set up workpiece in absolute coordinate system XYZ three-dimensional pose coordinate with at robot coordinate system X ₁y ₁z ₁conversion Mathematical Modeling between middle coordinate;

S5, workpiece conveyor carry workpiece translational motion motion with speed V, when detect workpiece enter viewing field of camera after camera obtain the shaft side figure picture of workpiece, extract the image characteristic point of workpiece in this shaft side figure picture;

S6, the coordinate of image characteristic point according to workpiece, the Mathematical Modeling that the calibration value in integrating step S2 and step S3, S4 set up, obtains workpiece when robot captures at robot coordinate system X ₁y ₁z ₁in three-dimensional pose and space coordinates, these data are transferred to robot controller, so control end effector capture online workpiece.

Further, described localization method, also comprises step:

The position of S7, robot controller Real-time Collection current end actuator, then the deviation of this physical location and target location is calculated, feed back a motion control signal according to this deviation and control end effector motion, form a close loop control circuit, until the accurate grabbing workpiece of end effector.

Concrete, in described step S3, set up frame coordinate system uo " method of the conversion Mathematical Modeling in v in workpiece features image coordinate and absolute coordinate system XYZ between workpiece three-dimensional pose coordinate is:

Set up the spatial point X axis location model in the horizontal direction in planar point in the planar point depth localization model in vertical direction in absolute coordinate system, the spatial point depth localization model in vertical direction in absolute coordinate system, absolute coordinate system X axis location model in the horizontal direction and absolute coordinate system, again in conjunction with the priori of workpiece, thus the three-dimensional coordinate of the point of imaging in all cameras is obtained, the computing formula of above-mentioned each model is as follows:

(3-1) set up coordinate system: absolute coordinate system XYZ with the optical axis center of camera lens at the upright projection M of table plane for initial point, to cross the vertical plane of camera lens optical axis for reference, perpendicular to the direction X-axis of this plane, direction perpendicular to X-axis is Y-direction, coordinate is expressed as (X, Y), in units of length; Photo coordinate system xo'y initial point is at the center of CCD imaging plane, and coordinate is expressed as (x, y); " v initial point is in the upper left corner of the plane of delineation, and coordinate is expressed as (u, v), and in units of pixel, o' is at coordinate system uo, and " coordinate under v is (u for frame coordinate system uo ₀, v ₀); Described planar point is the point in absolute coordinate system XYZ in XMY plane below;

(3-2) the planar point depth localization model in vertical direction in absolute coordinate system is set up: establish the planar point P (0, Y, 0) in absolute coordinate system, then this point to the distance of initial point M is:

$\mathrm{MP}=Y=\frac{h}{\tan (\mathrm{\α}+\arctan \frac{v_0-v}{f_y})}$

Wherein, α, h are the outer parameter by demarcating the camera obtained, f _y=f/d _y, d _ybe the spacing of pixel, f is the intrinsic parameter by demarcating the camera obtained; V is that planar point P is at the frame coordinate system uo " ordinate in v;

(3-3) the spatial point depth localization model in vertical direction in absolute coordinate system is set up: establish spatial point P ₁the P' point of imaging plane is all corresponded to, spatial point P with planar point P ₁the actual physics height P of XMY plane in absolute coordinate system ₁t=h ₁, known, the actual physics height OM=h (namely this height is one of them the outer parameter above by demarcating the camera obtained) of XMY plane, then P in the some O to absolute coordinate system of industrial camera optical center ₁distance between subpoint T in XMY plane and initial point M is:

$\mathrm{MT}=\frac{h}{\tan (\mathrm{\α}+\arctan \frac{v_0-v}{f_y})}\×(1-\frac{h_1}{h});$

Wherein, v is that spatial point P1 is at the frame coordinate system uo " ordinate in v;

(3-4) the planar point X axis location model in the horizontal direction in absolute coordinate system is set up: establish a planar point Q, the coordinate of its X axis in XMY plane is:

$\mathrm{PQ}=\frac{(u-u_0)\×h}{\sqrt{{f_y}^2+{(v-v_0)}^2}\×\sin [\mathrm{\α}+\arctan (\frac{v_0-v}{f}\×d_y)]}$

Wherein, u, v are that planar point Q is at the frame coordinate system uo " coordinate in v;

(3-5) the spatial point X axis location model in the horizontal direction in absolute coordinate system is set up: establish spatial point Q ₁the Q' point in imaging plane is all corresponded to, spatial point Q with planar point Q ₁the actual physics height Q of XMY plane in absolute coordinate system ₁n=h ₁, known, the actual physics height OM=h of XMY plane, then spatial point Q in the optical axis center point O to absolute coordinate system of camera lens ₁at the upright projection point N (X of XMY plane _e, Y _e) coordinate computing formula as follows:

$X_e=\frac{h}{\tan (\mathrm{\α}+\arctan \frac{u_0-u}{\mathrm{fx}})}\×(1-\frac{h1}{h});$

$Y_e=\frac{h}{\tan (\mathrm{\α}+\arctan \frac{v_0-v}{f_y})}\×(1-\frac{h_1}{h});$

Wherein, u, v are spatial point Q ₁at the frame coordinate system uo " coordinate in v; f _x=f/d _x, d _xbe the spacing of pixel, f is the intrinsic parameter by demarcating the camera obtained.

Concrete, in described step S4, set up workpiece in absolute coordinate system XYZ three-dimensional pose coordinate with at robot coordinate system X ₁y ₁z ₁the method of the conversion Mathematical Modeling between middle coordinate is:

Absolute coordinate system XYZ and robot coordinate system X ₁y ₁z ₁z-direction consistent, by translation and rotate the space transforming realized between two coordinate systems around Z axis, wherein changing Mathematical Modeling is: set absolute coordinate system XYZ into coordinate system A, robot coordinate system X ₁y ₁z ₁for coordinate system B, then:

B＝Trans(ΔX,ΔY,ΔZ)Rot(Z,θ)A

Wherein Trans (Δ X, Δ Y, the Δ Z) homogeneous transformation that is translation, its Mathematical Modeling is:

$\mathrm{Trans}(\mathrm{\ΔX},\mathrm{\ΔY},\mathrm{\ΔZ})=\left[\begin{array}{cccc}1& 0& 0& \mathrm{\ΔX}\\ 0& 1& 0& \mathrm{\ΔY}\\ 0& 0& 1& \mathrm{\ΔZ}\\ 0& 0& 0& 1\end{array}\right]$

In formula, element Δ X, Δ Y, Δ Z represents the amount of movement along respective reference axis X, Y, Z respectively;

Wherein Rot (Z, θ) carries out around Z axis the rotation operator that rotates, and Mathematical Modeling is:

$\mathrm{Rot}(Z,\mathrm{\θ})=\left[\begin{array}{cccc}\cos \mathrm{\θ}& -\sin \mathrm{\θ}& 0& 0\\ \sin \mathrm{\θ}& \cos \mathrm{\θ}& 0& 0\\ 0& 0& 1& 0\\ 0& 0& 0& 1\end{array}\right]$

In formula, element θ is the angle rotated around Z axis, and above-mentioned parameter Δ X, Δ Y, Δ Z, θ solve actual value by demarcation.

Compared with prior art, tool has the following advantages and beneficial effect in the present invention:

(1) make robot automtion automatic operation, capture precision high.The present invention adopts one camera obtain the workpiece information of uncertain pose in conveying motion and calculate its three-dimensional coordinate, and guided robot adopts correct end effector pose and movement locus accurately to capture workpiece intelligently.

(2) plurality of target mixing can identify, adapt to multi items Flexible Production demand.The present invention designs monocular vision and obtains workpiece spindle side image, finds out its feature according to different known form objects, can be adapted to difformity, specification, the workpiece identification of various workpieces blending transportation occasion of material and robot and capture and guide.

(3) wide adaptability, application conditions is wide in range.The present invention without the specific restriction (but being good with middle-size and small-size object) of material, shape, size, also has larger adaptability to workpiece with the speed of conveyer belt and position deviation size on a moving belt to identification objective workpiece.

(4) structure is simple, with low cost.The present invention adopts single general camera, adapted ordinary PC, forms Intelligent Recognition and grasping system with regular industrial robot, without the need to other additional devices and software platform.

(5) simple to operate, automaticity is high, nuisanceless.Hardware and software in the present invention is all quite simple, and do not produce any pollution, user easily grasps, and can be used for the full-automatic continued operation of guided robot, and energy generating run database automatically, preserve operating result, run into fault capable of automatic alarm.

Accompanying drawing explanation

Fig. 1 is present system hardware architecture schematic diagram.

Fig. 2 is industrial robot grasping system workflow diagram of the present invention.

Fig. 3-1 is planar point of the present invention depth localization modular concept figure in vertical direction.

Fig. 3-2 is spatial point of the present invention depth localization modular concept figure in vertical direction.

Fig. 3-3 is planar point of the present invention X axis location model schematic diagrams in the horizontal direction.

Fig. 3-4 is spatial point of the present invention X axis location model schematic diagrams in the horizontal direction.

Fig. 4 (a) is the present embodiment measured workpiece is cubical schematic diagram.

The schematic diagram of Fig. 4 (b) to be the present embodiment measured workpiece be other rule bodies.

Detailed description of the invention

Below in conjunction with embodiment and accompanying drawing, the present invention is described in further detail, but embodiments of the present invention are not limited thereto.

Embodiment 1

Consult Fig. 1, the present embodiment captures the robot pose of online workpiece and movement locus navigation system comprises camera support 1, industrial camera 2, measured workpiece 3, workpiece transfer band 4, end effector 5, robot system 6, workbench 7, image processor 8, Worktable control device 9, displacement transducer 10 and robot controller 11.Industrial camera 2 is arranged on camera support 1, and camera optical axis and vertical line are that suitable tilt angled down is installed, can obtain workpiece on conveyer belt shaft side figure picture and initial time end effector of robot position.Camera and industrial robot are arranged on motion platform both sides respectively, ensure that robotic arm does not block camera fields of view.Photoelectric sensor is arranged on by conveyor belt side, whether enters viewing field of camera for detecting workpiece.Image processor is connected with the robot controller in robot system with camera respectively, obtains robot system capture coordinate for image captured by camera.End effector is used for clamping, rotates and place work piece.

See Fig. 2, this gives the method for work of said system, be summarized as follows:

(1) vision monitoring: carrying out in monitor procedure, whether real-time detection is current will obtain image, if obtain image, then first object is carried out being divided into cube (such as Fig. 4 (a)) Sum fanction body (such as Fig. 4 (b)) two class, then pretreatment is carried out, extract minutiae, line, face to the image gathered;

(2) information converting transmission: for the feature point, line, surface of said extracted, utilize functional relation to change, be converted into coordinates of targets and angle, then according to communications protocol format data, finally data are sent to industrial robot;

(3) industrial robot control procedure: after namely robot is subject to data, first resolve, then according to coordinates of targets and angle adjustment attitude orientation, can also be fed back operating process by position compensation algorithm simultaneously, finally realize accurately location and crawl.

Below the localization method of described system is specifically described.

The installation site of S1, adjustment camera and setting angle, after making workpiece enter viewing field of camera, phase function obtains the shaft side figure picture of workpiece.

S2, scaling board to be positioned on workpiece transfer band, and to be positioned at the field of view center of camera, geometric calibration is carried out to camera-workpiece transfer band plane-end effector, demarcate the intrinsic parameter f that obtains camera and outer parameter alpha, h, h ₁.

" the conversion Mathematical Modeling in v in workpiece features image coordinate and absolute coordinate system XYZ between workpiece three-dimensional pose coordinate of S3, according to the installing space position of camera and known workpiece shapes size, setting up frame coordinate system uo.

This Mathematical Modeling specifically comprises four models, is the spatial point X axis location model in the horizontal direction in planar point in the planar point depth localization model in vertical direction in absolute coordinate system, the spatial point depth localization model in vertical direction in absolute coordinate system, absolute coordinate system X axis location model in the horizontal direction and absolute coordinate system respectively.According to above-mentioned model, then in conjunction with the priori of workpiece, thus the three-dimensional coordinate of the point of imaging in all cameras is obtained.

The reasoning process of above-mentioned each model is as follows:

(3-1) set up coordinate system: absolute coordinate system XYZ with the optical axis center of camera lens at the upright projection M of table plane for initial point, to cross the vertical plane of camera lens optical axis for reference, perpendicular to the direction X-axis of this plane, direction perpendicular to X-axis is Y-direction, coordinate is expressed as (X, Y), in units of length; Photo coordinate system xo'y initial point is at the center of CCD imaging plane, and coordinate is expressed as (x, y); " v initial point is in the upper left corner of the plane of delineation, and coordinate is expressed as (u, v), and in units of pixel, o' is at coordinate system uo, and " coordinate under v is (u for frame coordinate system uo ₀, v ₀); Described planar point is the point in absolute coordinate system XYZ in XMY plane below.

(3-2) the planar point depth localization model in vertical direction in absolute coordinate system is set up.

See Fig. 3-1, if the planar point P in absolute coordinate system (0, Y, 0), then:

$\mathrm{\β}=\mathrm{\α}-\mathrm{\γ};\mathrm{\γ}=\arctan \frac{y}{f};Y=\frac{h}{\tan \mathrm{\β}}=\frac{h}{\tan (\mathrm{\α}-\arctan \frac{y}{f})}---\left(1.1\right)$

Because what obtain from image is not y value, but the number v of pixel, required reduction formula is: y=(v-v ₀) × d _y(wherein d _ythe spacing of pixel), f _y=f/d _y.Then as shown in figure 3-1, the distance of the planar point P in absolute coordinate system to initial point M is:

$\mathrm{MP}=Y=\frac{h}{\tan (\mathrm{\α}+\arctan \frac{v_0-v}{f_y})}---\left(1.2\right)$

Wherein, f _y=f/d _y, d _ybe the spacing of pixel, f is the intrinsic parameter by demarcating the camera obtained; V is that planar point P is at the frame coordinate system uo " ordinate in v.

(3-3) the spatial point depth localization model in vertical direction in absolute coordinate system is set up.

See Fig. 3-2, because a some P' on the two dimensional image of monocular vision often will correspond to the many points in three dimensions, if spatial point P ₁the P' point of imaging plane is all corresponded to, spatial point P with planar point P ₁the actual physics height P of XMY plane in absolute coordinate system ₁t=h ₁, known, the actual physics height OM=h of XMY plane in the some O to absolute coordinate system of industrial camera optical center, then:

$\tan \mathrm{\β}=\frac{P_{1}T}{\mathrm{TP}}=\frac{\mathrm{OM}}{\mathrm{MP}}---\left(1.3\right)$

$\mathrm{TP}=\frac{P_{1}T}{\mathrm{OM}}\×\mathrm{MP}=\frac{h_1}{h}\×Y---\left(1.4\right)$

So the distance between the subpoint T of P1 in XMY plane and initial point M is:

$\mathrm{MT}=\mathrm{MP}-\mathrm{TP}=Y-\frac{h_1}{h}\×Y=\frac{h}{\tan (\mathrm{\α}+\arctan \frac{v_0-v}{f_y})}\×(1-\frac{h_1}{h})---\left(1.5\right)$

Wherein, v is that spatial point P1 is at the frame coordinate system uo " ordinate in v.

(3-4) the planar point X axis location model in the horizontal direction in absolute coordinate system is set up.

See Fig. 3-3, if a planar point Q (X, Y), then:

$\mathrm{\β}=\mathrm{\α}-\mathrm{\γ};\mathrm{\γ}=\arctan \left(\frac{y}{f}\right)---\left(1.6\right)$

$\mathrm{OP}=\frac{\mathrm{OM}}{\sin \mathrm{\β}}=\frac{h}{\sin (\mathrm{\α}-\mathrm{\γ})}=\frac{h}{\sin (\mathrm{\α}-\arctan \frac{y}{f})}---\left(1.7\right)$

Change pixel coordinate into:

$\mathrm{OP}=\frac{h}{\sin [\mathrm{\α}-\arctan (\frac{{v-v}_0}{f}\×d_y)]}=\frac{h}{\sin [\mathrm{\α}+\arctan (\frac{v_0-v}{f}\×d_y)]}---\left(1.8\right)$

$P^{\′}O=\sqrt{P^{\′}{O}^{\′2}+O^{\′}{O}^2}=\sqrt{f^2+y^2}=\sqrt{f^2+{(v-v_0)}^2\×{d_y}^2}---\left(1.9\right)$

Obtain according to correspondence theorem:

$\frac{P^{\′}{O}^{\′}}{P^{\′}O}=\frac{\mathrm{PQ}}{\mathrm{PO}}---\left(1.10\right)$

Then

$\mathrm{PQ}=\frac{P^{\′}{Q}^{\′}}{P^{\′}O}\×\mathrm{PO}---\left(1.11\right)$

What obtain from image is not x value, but the number u of pixel, required reduction formula is: x=(u-u ₀) × d _x(wherein d _xthe spacing of pixel), d _xand d _yfor intrinsic parameters of the camera, represent Distance geometry y direction, the x direction distance between CCD two neighboring photosites respectively, be linear CCD camera due to what adopt, x direction and y direction are evenly equal, therefore d _x=d _y.

So the coordinate obtaining planar point Q X axis in XMY plane by formula (1.11) is:

Transverse axis coordinate is:

$\mathrm{PQ}=\frac{P^{\′}{Q}^{\′}}{P^{\′}O}\×\mathrm{PO}\frac{\left({u-u}_0\right)\×h}{\sqrt{{f_y}^2+{(v-v_0)}^2}\×\sin [\mathrm{\α}+\arctan (\frac{v_0-v}{f}\×d_y)]}---\left(1.12\right)$

Wherein, u, v are that planar point Q is at the frame coordinate system uo " coordinate in v.

(3-5) the spatial point X axis location model in the horizontal direction in absolute coordinate system is set up.

Because a some Q' on the two dimensional image of monocular vision often will correspond to three-dimensional many points, as shown in Figure 3-4, spatial point Q ₁the Q' point in imaging plane is all corresponded to, spatial point Q with planar point Q ₁the actual physics height Q of XMY plane in absolute coordinate system ₁n=h1, known, the actual physics height OM=h of XMY plane in the optical axis center point O to absolute coordinate system of camera lens.

In OMQ plane, obtained by similar

$\frac{Q_{1}N}{\mathrm{NQ}}=\frac{\mathrm{OM}}{\mathrm{MQ}}---\left(1.13\right)$

$\mathrm{QM}=\sqrt{{\mathrm{PM}}^2+{\mathrm{PQ}}^2}=\sqrt{X^2+Y^2}---\left(1.14\right)$

$\mathrm{NQ}=\frac{Q_{1}N\×\mathrm{QM}}{\mathrm{OM}}=\frac{h_1}{h}\×\sqrt{X^2+Y^2}---\left(1.15\right)$

$\mathrm{NM}=\mathrm{MQ}-\mathrm{NQ}=\sqrt{X^2+Y^2}\×(1-\frac{h_1}{h})---\left(1.16\right)$

Then spatial point Q1 is at the upright projection point N (X of XMY plane _e, Y _e), obtained by similar:

$\frac{\mathrm{EN}}{\mathrm{NM}}=\frac{\mathrm{PQ}}{\mathrm{MQ}}---\left(1.17\right)$

$X_e=\mathrm{EN}=\frac{\mathrm{PQ}\×\mathrm{NM}}{\mathrm{MQ}}=X\×(1-\frac{h_1}{h})=\frac{h}{\tan (\mathrm{\α}+\arctan \frac{u_0-u}{\mathrm{fx}})}\×(1-\frac{h1}{h})---\left(1.18\right)$

In like manner can obtain:

$Y_e=\mathrm{EN}=\frac{\mathrm{PM}\×\mathrm{NM}}{\mathrm{MQ}}=Y\×(1-\frac{h1}{h})=\frac{h}{\tan (\mathrm{\α}+\arctan \frac{v_0-v}{f_y})}\×(1-\frac{h_1}{h})---\left(1.19\right)$

Wherein, u, v are that spatial point Q1 is at the frame coordinate system uo " coordinate in v; f _x=f/d _x, d _xbe the spacing of pixel, f is the intrinsic parameter by demarcating the camera obtained.

S4, according to the locus of robot in absolute coordinate system, set up workpiece in absolute coordinate system XYZ three-dimensional pose coordinate with at robot coordinate system X ₁y ₁z ₁conversion Mathematical Modeling between middle coordinate.

Concrete grammar is:

Absolute coordinate system XYZ and robot coordinate system X ₁y ₁z ₁z-direction consistent, by translation and rotate the space transforming realized between two coordinate systems around Z axis, wherein changing Mathematical Modeling is: set absolute coordinate system XYZ into coordinate system A, robot coordinate system X ₁y ₁z ₁for coordinate system B, then:

B＝Trans(ΔX,ΔY,ΔZ)Rot(Z,θ)A

Wherein Trans (Δ X, Δ Y, the Δ Z) homogeneous transformation that is translation, its Mathematical Modeling is:

$\mathrm{Trans}(\mathrm{\ΔX},\mathrm{\ΔY},\mathrm{\ΔZ})=\left[\begin{array}{cccc}1& 0& 0& \mathrm{\ΔX}\\ 0& 1& 0& \mathrm{\ΔY}\\ 0& 0& 1& \mathrm{\ΔZ}\\ 0& 0& 0& 1\end{array}\right]$

In formula, element Δ X, Δ Y, Δ Z represents the amount of movement along respective reference axis X, Y, Z respectively;

Wherein Rot (Z, θ) carries out around Z axis the rotation operator that rotates, and Mathematical Modeling is:

$\mathrm{Rot}(Z,\mathrm{\θ})=\left[\begin{array}{cccc}\cos \mathrm{\θ}& -\sin \mathrm{\θ}& 0& 0\\ \sin \mathrm{\θ}& \cos \mathrm{\θ}& 0& 0\\ 0& 0& 1& 0\\ 0& 0& 0& 1\end{array}\right]$

In formula, element θ is the angle rotated around Z axis, and above-mentioned parameter Δ X, Δ Y, Δ Z, θ solve actual value by demarcation.

S5, workpiece conveyor carry workpiece translational motion motion with speed V, when detect workpiece enter viewing field of camera after camera obtain the shaft side figure picture of workpiece, extract the image characteristic point of workpiece in this shaft side figure picture.

Such as, A, B, C, D, A', B', C', D' in Fig. 4 (a), (b).

S6, by the image coordinate (u of impact point _i, v _i) and inside and outside parameter substitute into formula (1.12) and formula (1.2), obtain the coordinate position of the planar point in the world coordinate system corresponding to frame coordinate.Such as can obtain the three-dimensional coordinate of the coordinate system that cube C, D, C`, D` characteristic point is set up relative to the upright projection point of video camera.

By the image coordinate (u of impact point _i, v _i) and inside and outside parameter substitute into formula (1.18) and formula (1.19), try to achieve the coordinate position of object relative to video camera of certain altitude.Such as can obtain the three-dimensional coordinate of A, B, A`, B` characteristic point in space.

According to the Mathematical Modeling that the three-dimensional coordinate of each characteristic point in absolute coordinate system and step S4 are set up, obtain workpiece when robot captures at robot coordinate system X ₁y ₁z ₁in three-dimensional pose and space coordinates, these data are transferred to robot controller, so control end effector capture online workpiece.

S7, in order to further improve the precision of crawl, the position of robot controller also Real-time Collection current end actuator, then the deviation of this physical location and target location is calculated, feed back a motion control signal according to this deviation and control end effector motion, form a close loop control circuit, until the accurate grabbing workpiece of end effector.

Above-described embodiment is the present invention's preferably embodiment; but embodiments of the present invention are not restricted to the described embodiments; change, the modification done under other any does not deviate from Spirit Essence of the present invention and principle, substitute, combine, simplify; all should be the substitute mode of equivalence, be included within protection scope of the present invention.

Claims (6)

1. capture robot pose and the movement locus navigation system of online workpiece, it is characterized in that, comprise the camera for captured in real-time workpiece, the robot system for grabbing workpiece, obtain for image captured by camera image processor, the workbench that robot system captures coordinate, described camera is fixed on a camera support, and camera support and robot system are arranged at the both sides of workbench respectively; Described image processor is connected with the robot controller in robot system with camera respectively; Described workbench is provided with workpiece transfer band, and workpiece moves on workpiece transfer band; The end of described robot system is provided with for clamping and the end effector of rotational workpieces; The optical axis of described camera is vertical with the direction of workpiece motion s, and camera is for obtaining the shaft side figure picture of workpiece.

2. the robot pose of the online workpiece of crawl according to claim 1 and movement locus navigation system, it is characterized in that, be provided with at the two ends of described workpiece transfer band the photoelectric sensor whether entering viewing field of camera for detecting workpiece, this photoelectric sensor is connected with the external trigger head of camera.

3., based on the robot pose of the online workpiece of crawl described in any one of claim 1-2 and a localization method for movement locus navigation system, it is characterized in that, comprise step:

The installation site of S1, adjustment camera and setting angle, after making workpiece enter viewing field of camera, phase function obtains the shaft side figure picture of workpiece;

S2, scaling board to be positioned on workpiece transfer band, and to be positioned at the field of view center of camera, geometric calibration is carried out to camera-workpiece transfer band plane-end effector;

" the conversion Mathematical Modeling in v in workpiece features image coordinate and absolute coordinate system XYZ between workpiece three-dimensional pose coordinate of S3, according to the installing space position of camera and known workpiece shapes size, setting up frame coordinate system uo;

S4, according to the locus of robot in absolute coordinate system, set up workpiece in absolute coordinate system XYZ three-dimensional pose coordinate with at robot coordinate system X ₁y ₁z ₁conversion Mathematical Modeling between middle coordinate;

S5, workpiece conveyor carry workpiece translational motion motion with speed V, when detect workpiece enter viewing field of camera after camera obtain the shaft side figure picture of workpiece, extract the image characteristic point of workpiece in this shaft side figure picture;

S6, the coordinate of image characteristic point according to workpiece, the Mathematical Modeling that the calibration value in integrating step S2 and step S3, S4 set up, obtains workpiece when robot captures at robot coordinate system X ₁y ₁z ₁in three-dimensional pose and space coordinates, these data are transferred to robot controller, so control end effector capture online workpiece.

4. localization method according to claim 3, is characterized in that, also comprises step:

The position of S7, robot controller Real-time Collection current end actuator, then the deviation of this physical location and target location is calculated, feed back a motion control signal according to this deviation and control end effector motion, form a close loop control circuit, until the accurate grabbing workpiece of end effector.

5. localization method according to claim 3, is characterized in that, in described step S3, sets up frame coordinate system uo " method of the conversion Mathematical Modeling in v in workpiece features image coordinate and absolute coordinate system XYZ between workpiece three-dimensional pose coordinate is:

Set up the spatial point X axis location model in the horizontal direction in planar point in the planar point depth localization model in vertical direction in absolute coordinate system, the spatial point depth localization model in vertical direction in absolute coordinate system, absolute coordinate system X axis location model in the horizontal direction and absolute coordinate system, again in conjunction with the priori of workpiece, thus the three-dimensional coordinate of the point of imaging in all cameras is obtained, the computing formula of above-mentioned each model is as follows:

(3-1) set up coordinate system: absolute coordinate system XYZ with the optical axis center of camera lens at the upright projection M of table plane for initial point, to cross the vertical plane of camera lens optical axis for reference, perpendicular to the direction X-axis of this plane, direction perpendicular to X-axis is Y-direction, coordinate is expressed as (X, Y), in units of length; Photo coordinate system xo'y initial point is at the center of CCD imaging plane, and coordinate is expressed as (x, y); " v initial point is in the upper left corner of the plane of delineation, and coordinate is expressed as (u, v), and in units of pixel, o' is at coordinate system uo, and " coordinate under v is (u for frame coordinate system uo ₀, v ₀); Described planar point is the point in absolute coordinate system XYZ in XMY plane;

(3-2) the planar point depth localization model in vertical direction in absolute coordinate system is set up: establish the planar point P (0, Y, 0) in absolute coordinate system, then this point to the distance of initial point M is:

$\mathrm{MP}=Y=\frac{h}{\tan (\mathrm{\α}+\arctan \frac{v_0-v}{f_y})}$

Wherein, α, h are the outer parameter by demarcating the camera obtained, f _y=f/d _y, d _ybe the spacing of pixel, f is the intrinsic parameter by demarcating the camera obtained; V is that planar point P is at the frame coordinate system uo " ordinate in v;

(3-3) the spatial point depth localization model in vertical direction in absolute coordinate system is set up: establish spatial point P1 and planar point P all to correspond to the P' point of imaging plane, the actual physics height P1T=h of XMY plane in spatial point P1 to absolute coordinate system ₁, known, the actual physics height OM=h of XMY plane in the some O to absolute coordinate system of industrial camera optical center, then the distance between the subpoint T of P1 in XMY plane and initial point M is:

$\mathrm{MT}=\frac{h}{\tan (\mathrm{\α}+\arctan \frac{v_0-v}{f_y})}\×(1-\frac{h_1}{h});$

Wherein, v is that spatial point P1 is at the frame coordinate system uo " ordinate in v;

(3-4) the planar point X axis location model in the horizontal direction in absolute coordinate system is set up: establish a planar point Q, the coordinate of its X axis in XMY plane is:

$\mathrm{PQ}=\frac{(u-u_0)\×h}{\sqrt{{f_y}^2+{(v-v_0)}^2}\×\sin [\mathrm{\α}+\arctan (\frac{v_0-v}{f}\×d_y)]}$

Wherein, u, v are that planar point Q is at the frame coordinate system uo " coordinate in v;

(3-5) the spatial point X axis location model in the horizontal direction in absolute coordinate system is set up: establish spatial point Q1 and planar point Q all to correspond to Q' point in imaging plane, the actual physics height Q1N=h1 of XMY plane in spatial point Q1 to absolute coordinate system, known, the actual physics height OM=h of XMY plane in the optical axis center point O to absolute coordinate system of camera lens, then spatial point Q1 is at the upright projection point N (X of XMY plane _e, Y _e) coordinate computing formula as follows:

$X_e=\frac{h}{\tan (\mathrm{\α}+\arctan \frac{u_0-u}{\mathrm{fx}})}\×(1-\frac{h1}{h});$

$Y_e=\frac{h}{\tan (\mathrm{\α}+\arctan \frac{v_0-v}{f_y})}\×(1-\frac{h_1}{h});$

Wherein, u, v are that spatial point Q1 is at the frame coordinate system uo " coordinate in v; f _x=f/d _x, d _xbe the spacing of pixel, f is the intrinsic parameter by demarcating the camera obtained.

6. localization method according to claim 3, is characterized in that, in described step S4, set up workpiece in absolute coordinate system XYZ three-dimensional pose coordinate with at robot coordinate system X ₁y ₁z ₁the method of the conversion Mathematical Modeling between middle coordinate is:

Absolute coordinate system XYZ and robot coordinate system X ₁y ₁z ₁z-direction consistent, by translation and rotate the space transforming realized between two coordinate systems around Z axis, wherein changing Mathematical Modeling is: set absolute coordinate system XYZ into coordinate system A, robot coordinate system X ₁y ₁z ₁for coordinate system B, then:

B＝Trans(ΔX,ΔY,ΔZ)Rot(Z,θ)A；

Wherein Trans (Δ X, Δ Y, the Δ Z) homogeneous transformation that is translation, its Mathematical Modeling is:

$\mathrm{Trans}(\mathrm{\ΔX},\mathrm{\ΔY},\mathrm{\ΔZ})=\left[\begin{array}{cccc}1& 0& 0& \mathrm{\ΔX}\\ 0& 1& 0& \mathrm{\ΔY}\\ 0& 0& 1& \mathrm{\ΔZ}\\ 0& 0& 0& 1\end{array}\right]$

In formula, element Δ X, Δ Y, Δ Z represents the amount of movement along respective reference axis X, Y, Z respectively;

Wherein Rot (Z, θ) carries out around Z axis the rotation operator that rotates, and Mathematical Modeling is:

$\mathrm{Rot}(Z,\mathrm{\θ})=\left[\begin{array}{cccc}\cos \mathrm{\θ}& -\sin \mathrm{\θ}& 0& 0\\ \sin \mathrm{\θ}& \cos \mathrm{\θ}& 0& 0\\ 0& 0& 1& 0\\ 0& 0& 0& 1\end{array}\right]$

In formula, element θ is the angle rotated around Z axis, and above-mentioned parameter Δ X, Δ Y, Δ Z, θ solve actual value by demarcation.