CN103862330B - Based on the bend pipe magnetic grinding automatic navigation method of machine vision - Google Patents

Abstract

The present invention relates to a kind of bend pipe magnetic based on machine vision grinding automatic navigation method.Its method comprises: demarcate CCD camera parameter and obtain Robot Hand-eye relational matrix; CCD camera carries out three-dimensional image acquisition to the striation of outer surface of bent pipe, obtains the semicircular data point set of camera light plane space; Carry out being with the least square fitting of Radius Constraint to data point set, try to achieve the central coordinate of circle under camera light plane coordinate system and central coordinate of circle under being converted to basis coordinates system; B-spline curves matching is carried out to a series of centre point, obtains bend pipe orbit of shaft center; The magnetic grinding and polishing device of control carries out pose adjustment according to orbit of shaft center, completes the magnetic Polishing machining of bend pipe.Real-time of the present invention is good, accuracy is high, can complete the processing of complicated bend pipe, significantly shorten the process-cycle, raises the efficiency and machining accuracy.

Description

Based on the bend pipe magnetic grinding automatic navigation method of machine vision

Technical field

The present invention relates to the technology such as computer vision, image procossing, B-spline curves matching, robot controlling, be specially and adopt computer vision methods to carry out self-navigation to free elbow internal wall magnetic grinding.

Background technology

In recent years, the high-accuracy pipe fitting occurred in the field such as mechanical carrier, Aero-Space, due to conveying is highly purified gas or liquid, very high to the roughness requirements of inner surface of pipe fitting.The high glossy of inside pipe wall, can ensure on the one hand to carry unobstructed, avoid occurring that turbulent flow makes tube fluid pressure uniform; Be avoid producing due to blemish polluting and corrosion on the other hand, improve service life.Due to the restriction of environment for use, a lot of pipe fitting is elongated bend pipe, and it is complex-shaped, and curvature is changeable.When grinding elbow internal wall, need move along the axis line track of bend pipe.This just requires in process, determines the machining locus of bend pipe.Current existence two kinds of situations, a kind of situation is, the center curve of known bend pipe, add that to preset axle center curve man-hour be that machining locus is processed, but the machining locus of setting and actual bend pipe center curve difference are very large, need to carry out self-navigation, carrying out smoothly of guarantee processing.At all another kind of situation can not determine the situation of bend pipe axle center curve, relies on self-navigation just can complete the processing of whole bend pipe completely.Self-navigation requires certain real-time, can identify rapidly the case of bending of bend pipe, processing unit (plant) is adjusted, and ensures to carry out magnetic Polishing machining with correct machining posture to bend pipe.

Summary of the invention

In order to overcome the problems referred to above, realize the self-navigation of bend pipe processing, the object of this invention is to provide a kind of tracking accurately, real-time is good, the automatic navigation method that working (machining) efficiency is high.

The technical scheme that the present invention is adopted for achieving the above object is: based on the bend pipe magnetic grinding automatic navigation method of machine vision, comprise the following steps:

1) Robotic Hand-Eye Calibration is carried out to camera; Control arm motion makes one end of bend pipe vertically penetrate the bearing centre of magnetic lapping device, and fixed bending tube two ends; By the outer surface of the linear light vertical irradiation of light source at bend pipe, form semicircular striation;

2) camera absorbs this optical strip image, and obtains the three-dimensional point set relative to camera coordinates system Oc of this optical strip image; The approximate center of circle and the radius that least square fitting obtains this semicircle striation is carried out to three-dimensional point set, then this approximate center of circle is optimized, obtain this semicircle striation center of circle at camera coordinates system O _cin coordinate p; Coordinate p is converted to this center of circle at basis coordinates system of robot O _bin coordinate C;

3) using first editing objective point that coordinate C moves as robot arm, control arm motion makes the bearing centre of magnetic lapping device with this impact point for starting point makes initial straight-line motion; Repeat step 2 simultaneously) obtain a series of central coordinate of circle C _i;

4) by central coordinate of circle C _icarry out the orbit of shaft center obtaining bend pipe based on the matching of B-spline curves; This orbit of shaft center is position required for processing of robots and attitude as editing objective track, realizes robot carries out magnetic grinding self-navigation to free bend pipe.

Described to three-dimensional point set carry out least square fitting obtain this semicircle striation the approximate center of circle (m, n, l) and and radius r be $\begin{array}{c}m=\frac{2(\mathrm{bde}-a)}{1+e^2+d^2},n=\frac{-b/2-\mathrm{dem}}{1+e^2},l=\mathrm{dm}+\mathrm{em}+f\\ r=\sqrt{m^2+n^2+f^2-2\mathrm{lf}+l^2-c}\end{array}$

Wherein a, B, C, D are the coefficient in line-structured light plane equation Ax+By+Cz+D=0, and wherein, line-structured light plane equation is determined by the three-dimensional point of the demarcation in camera intrinsic parameter and line-structured light plane.

$a=\frac{\mathrm{HD}-\mathrm{EG}}{\mathrm{GG}-D^2},$ $b=\frac{\mathrm{HC}-\mathrm{ED}}{D^2-\mathrm{CG}},$ $c=\frac{\mathrm{\Σ}(x_i^2+y_i^2)+\mathrm{a\Σ}{x}_i+\mathrm{b\Σ}{y}_i}{N},$ $G=\mathrm{N\Σ}{y}_i^2-\mathrm{\Σ}{y}_i\mathrm{\Σ}{y}_i,$

$E=N(1+d^2)\mathrm{\Σ}{x}_i^3+(1+e^2)\mathrm{\Σ}{x}_i\mathrm{\Σ}{y}_i^2+\mathrm{Nh\Σ}{x}_i^2\mathrm{\Σ}{y}_i-(1+d^2)\mathrm{\Σ}{x}_i^2\mathrm{\Σ}{x}_i$

$-(1+e^2)\mathrm{\Σ}{y}_i^2\mathrm{\Σ}{x}_i-\mathrm{h\Σ}{x}_i{y}_i\mathrm{\Σ}{x}_i$

$E=N(1+d^2)\mathrm{\Σ}{x}_i^2\mathrm{\Σ}{y}_i+N(1+e^2)\mathrm{\Σ}{y}_i^3+\mathrm{Nh\Σ}{x}_i\mathrm{\Σ}{y}_i^2-(1+d^2)\mathrm{\Σ}{x}_i^2\mathrm{\Σ}{y}_i$

$-(1+e^2)\mathrm{\Σ}{y}_i^2\mathrm{\Σ}{y}_i-\mathrm{h\Σ}{x}_i{y}_i\mathrm{\Σ}{y}_i$

(x _i, y _i, z _i) (i=1,2 ..., N) and be three-dimensional point set, N is number a little.

Described being optimized this approximate center of circle adopts LMF algorithm, is specially the center of circle p (p after by approximate center of circle c (x, y, z), optimization ₁, p ₂, p ₃) and variance res bring into LMF algorithmic formula namely obtain optimization after the center of circle;

LMF algorithmic formula be [p, ssq, CNT]=LMFsolve (res, c, ' Display' ,-1), wherein, ssq represents residual sum, and CNT represents iterations, and-1 represents controling parameters.

Described by central coordinate of circle C _ithe matching carried out based on B-spline curves comprises the following steps:

4-1) from sequence of points center of circle C _iin extract discrete data point in order, determine the knot vector of each data point by the method for accumulation Chord Length Parameterization; Define establishment equation group according to the discrete data point extracted and knot vector by B-spline curves, obtain curve controlled summit; By the knot vector on accumulation Chord Length Parameterization method determination curve controlled summit, according to obtaining control vertex and knot vector determines B-spline curves;

4-2) B-spline curves of gained are divided equally by arc length, again get a little as discrete data point in the same ratio position of every section of arc; Re-executing step 4-1) SPL that obtains is the orbit of shaft center of bend pipe.

The described equation group defining establishment by B-spline curves is

$p\left(u_i\right)=\underset{j=0}{\overset{n}{\mathrm{\Σ}}}{d}_j{N}_{j,k}\left(u_i\right)=\underset{j=i-k}{\overset{i}{\mathrm{\Σ}}}{d}_{j,k}\left(u_i\right)=q_{i-k}$

d _n-k+1+i＝d _i(i＝0,1,…,k-1)

Wherein, for knot vector, summit d _j(j=i-k, i-k+1 ... i), N _i,k(x) (i=0,1 ..., n) be B-spline basic function, n is control vertex number, qi (i=0,1 ..., m) be one group of data point that k B-spline curves pass through.

The present invention has following beneficial effect and advantage:

1. utilize the present invention to follow the tracks of free bend pipe orbit of shaft center efficiently, real-time is good, eliminates the step that track presets, shortens the process-cycle.

2. the present invention adopts CCD camera to obtain space three-dimensional image by the method for computer vision, determine bend pipe surface information, utilize the least square method determination bend pipe home position of band Radius Constraint, then with B-spline curves curved surface adaptive fitting algorithm, a series of bend pipe centre point is processed again, determine bend pipe orbit of shaft center, and secondary gets the matching again a little carrying out orbit of shaft center, make machining locus fairing more, be conducive to the level and smooth working motion of robot.

Accompanying drawing explanation

Fig. 1 is free bend pipe magnetic Polishing machining system architecture schematic diagram of the present invention;

Fig. 2 is bend pipe magnetic lapping device structural representation of the present invention;

Fig. 3 bend pipe space circle of the present invention center of circle and radius acquiring method flow chart;

Fig. 4 is the axle center path approximating method flow chart of bend pipe of the present invention;

Fig. 5 is B-spline curves fitting effect comparison diagram of the present invention.

Detailed description of the invention

Below in conjunction with drawings and Examples, the present invention is described in further detail.

Technical scheme of the present invention adopts computer vision to free bend pipe real-time tracking, to reach elbow internal wall magnetic grinding.

First require to vision camera itself do strict demarcate and hand and eye calibrating to determine Robot Hand-eye relational matrix;

The line-structured light of light source is radiated on the transversal outer surface of bend pipe, and form semicircular striation, CCD camera absorbs this image, and three-dimensional reconstruction, obtain the three-dimensional point set of bend pipe half transversal outer surface, and this three-dimensional point set is the local coordinate system relative to camera;

Half transversal outer surface three-dimensional data point set is carried out to least square fitting and the optimization of Radius Constraint, obtain bend pipe cross-section center coordinate position under local coordinate system;

By Robot Hand-eye relational matrix, under the central coordinate of circle under camera local coordinate system is transformed to robot end's coordinate system, then under transforming to robot polar coordinate system, obtain the coordinate of central coordinate of circle under basis coordinates system of robot;

B-spline curves matching fairing is carried out to the center of circle under the basis coordinates system of a series of robot obtained, determines bend pipe orbit of shaft center.

Utilize the present invention can carry out real-time track extraction to bending axis efficiently, realize controlling in real time the track of robot and attitude, there is stronger practical value.

The step of the free elbow internal wall magnetic grinding automatic navigation method based on computer vision of the present invention is as follows:

First, camera calibration and hand and eye calibrating is carried out to determine Robot Hand-eye relational matrix; The line-structured light of light source is incident upon outer surface of bent pipe, and CCD camera carries out three-dimensional image acquisition to striation; To the image zooming-out light stripe centric line gathered and three-dimensional reconstruction, obtain the semicircle data point set of pipe half-section under camera coordinates system; Double circular data point set carries out least square fitting and the optimization of Radius Constraint, obtains the central coordinate of circle under camera coordinates system; By Robot Hand-eye relational matrix, under this central coordinate of circle is transformed to robot end's coordinate system, then under transforming to basis coordinates system of robot, obtain the coordinate of the center of circle under basis coordinates system of robot; B-spline curves matching is carried out to the center of circle under the basis coordinates system of a series of robot obtained, determines bend pipe orbit of shaft center.

Carry out elaborating of key technology below:

1. the structure of autopilot and principle

As shown in Figure 1, magnetic lapping device is fixed on articulated arm robots's end to the schematic diagram of autopilot structure, and vision sensor is connected with end effector of robot (i.e. magnetic lapping device) maintaining rigidness, and vision sensor is made up of light source and CCD camera.

As shown in Figure 3, need before system cloud gray model to demarcate camera, obtain the relevant parameter of camera; Also to carry out hand and eye calibrating to determine Robot Hand-eye relational matrix.Light source projects striation is in the outer cross section surface of pipe, and CCD camera carries out three-dimensional image acquisition to striation, extract optical losses and carry out three-dimensional reconstruction, obtain managing the semicircular data point set of outer cross section surface relative under camera coordinates system, the LMF method of the method for least square and band Radius Constraint is utilized to process to data point set, determine the home position coordinate under camera coordinates system, to the matrixing of central coordinate of circle through trick relational matrix and robot end's coordinate system and basis coordinates system of robot, obtain the central coordinate of circle of centre point under basis coordinates system, B-spline curves curved surface adaptive fitting algorithm is utilized to carry out B-spline curves matching to the center of circle point set under gained basis coordinates system, determine bend pipe orbit of shaft center, robot controlling end effector carries out inner polishing operation along required track to free bend pipe.

As shown in Figure 2, the magnetic lapping device that the present invention adopts is the magnetic grinding for free elbow internal wall, concrete structure is as follows: the support 1 be fixedly connected with robot arm 10 end is inlaid with bearing 2 by flange, the uniform magnet 3 in one end of bearing 2, the other end is connected with the belt pulley 11 be arranged on support 1 by conveyer belt 4, and belt pulley 11 is fixed on support by a bearing; The turning cylinder of belt pulley 11 is connected with motor 6 main shaft by flexible axle 5; Support 1 side is also installed with aluminium box 7, and aluminium box 7 internal fixtion has light source 8 and camera 9.The optical axis of light source 8 is perpendicular to the axial line of bearing 2.

Before work starts, Robotic Hand-Eye Calibration is carried out to camera 9; Control arm motion makes one end of bend pipe 9 vertically penetrate bearing 2 center, and fixed bending tube 9 two ends, bend pipe 9 external diameter is less than bearing 2 internal diameter, the gap within making bend pipe 9 and bearing 2 inner hole wall leave 3mm; During work, make the outer surface of linear light vertical irradiation at bend pipe 10 of light source 4, form semicircular striation; While gathering optical strip image data, drive motors band dynamic bearing 2 rotates with certain speed (800 revs/min), and the magnet on bearing 2 also rotates thereupon; Iron powder in bend pipe 9 and abrasive material with magnetic rotation, carry out magnetic grinding to bend pipe inside under the effect of magnetic force simultaneously; Control motion makes bearing 2 endoporus center along the movement in real time of bend pipe orbit of shaft center, can complete the attrition process of the self-navigation to whole elbow internal wall.。

2. the acquiring method of central coordinate of circle

2.1 utilize least square method to ask for the space circle center of circle and radius

Because the three-dimensional point data of the transversal outer surface of bend pipe obtained are half circular arc, the matching time error carrying out justifying is comparatively large, therefore can utilize known conditions: the radius of pipe is known, if the radius of pipe is R; The spatial image obtained for camera processes, and obtains space circular arc data (x _i, y _i, z _i) (i=1,2 ..., N), then the desirable center of circle (m, n, l) and radius R are Least Square Method value equation group:

${(x_i-\hat{m})}^2+{(y_i-\hat{n})}^2+{(z_i-\hat{l})}^2=\widehat{R^2}---\left(1\right)$

(i＝1,2…,N)

Least square solution.

For the striation circular arc data (x that camera obtains _i, y _i, z _i) (i=1,2 ..., N), they are distributed on the line-structured light plane Ax+By+Cz+D=0 of camera, and A, B, C, D are equation coefficient, and the zi value of such circular arc data can by optic plane equations by x _iand y _irepresent, that is:

z＝dx+ey+f(2)

Wherein: can be transformed on two-dimensional space asking ternary least square method, i.e. (x _i-m) ²+ (y _i-n) ²+ (z _i-l) ²=R ²transfer to:

(x _i-m) ²+(y _i-n) ²+(dx _i+ey _i+f _i-l) ²＝R ²(3)

Formula (3) can turn to:

(1+d ²)x ²+(1+e ²)y ²+ax+by+c+hxy＝0(4)

Wherein: a=2 (fd-ld-m), b=2 (fe-le-n), h=2de, c=m ²+ n ²+ (f-l) ²-R ².

If sample set (x _i, y _i, z _i) (i=1,2 ..., N) to the distance in the center of circle be d _i:

${(x_i-m)}^2+{(y_i-n)}^2+{(z_i-l)}^2={d_i}^2---\left(5\right)$

Point (x _i, y _i, z _i) (i=1,2 ..., N) to rounded edge distance square with the difference of two squares of radius be:

${\mathrm{\δ}}_i=d_i^2-R^2=(1+d^2){x_i}^2+(1+e^2){y_i}^2+{\mathrm{ax}}_i+{\mathrm{by}}_i+c+{\mathrm{hx}}_i{y}_i---\left(6\right)$

Q (a, b, c) is made to be δ _iquadratic sum:

$Q(a,b,c)=\mathrm{\Σ}{\mathrm{\δ}}_i^2=\mathrm{\Σ}{[(1+d^2){x_i}^2+(1+e^2){y_i}^2+{\mathrm{ax}}_i+{\mathrm{by}}_i+c+{\mathrm{hx}}_i{y}_i]}^2---\left(7\right)$

Ask parameter a, b, c make the value of Q (a, b, c) minimum:

$\frac{\∂Q(a,b,c)}{\∂a}=\mathrm{\Σ}2[(1+d^2)x_i^2+(1+e^2)y_i^2+{\mathrm{ax}}_i+{\mathrm{by}}_i+c+{\mathrm{hx}}_i{y}_i]x_i---\left(8\right)$

$\frac{\∂Q(a,b,c)}{\∂b}=\mathrm{\Σ}2[(1+d^2)x_i^2+(1+e^2)y_i^2+{\mathrm{ax}}_i+{\mathrm{by}}_i+c+{\mathrm{hx}}_i{y}_i]y_i---\left(9\right)$

$\frac{\∂Q(a,b,c)}{\∂c}=\mathrm{\Σ}2[(1+d^2)x_i^2+(1+e^2)y_i^2+{\mathrm{ax}}_i+{\mathrm{by}}_i+c+{\mathrm{hx}}_i{y}_i]---\left(10\right)$

Make (8) * N-(10) * ∑ x _i, (9) * N-(10) * ∑ y _i:

Ca+Db+E＝0(11)

Da+Gb+H＝0

Wherein: $C=\mathrm{N\Σ}{x}_i^2-\mathrm{\Σ}{x}_i*\mathrm{\Σ}{x}_i,$ D＝N∑x _iy _i-∑x _i*∑y _i， $G=N{y}_i^2-\mathrm{\Σ}{y}_i\mathrm{\Σ}{y}_i$

$E=N(1+d^2)\mathrm{\Σ}{x}_i^3+(1+e^2)\mathrm{\Σ}{x}_i\mathrm{\Σ}{y}_i^2+\mathrm{Nh\Σ}{x}_i^2\mathrm{\Σ}{y}_i-(1+d^2)\mathrm{\Σ}{x}_i^2\mathrm{\Σ}{x}_i$

$-(1+e^2)\mathrm{\Σ}{y}_i^2\mathrm{\Σ}{x}_i-\mathrm{h\Σ}{x}_i{y}_i\mathrm{\Σ}{x}_i$

$E=N(1+d^2)\mathrm{\Σ}{x}_i^2\mathrm{\Σ}{y}_i+N(1+e^2)\mathrm{\Σ}{y}_i^3+\mathrm{Nh\Σ}{x}_i\mathrm{\Σ}{y}_i^2-(1+d^2)\mathrm{\Σ}{x}_i^2\mathrm{\Σ}{y}_i$

$-(1+e^2)\mathrm{\Σ}{y}_i^2\mathrm{\Σ}{y}_i-\mathrm{h\Σ}{x}_i{y}_i\mathrm{\Σ}{y}_i$

Can be solved by formula (11):

$a=\frac{\mathrm{HD}-\mathrm{EG}}{\mathrm{GG}-D^2},$ $b=\frac{\mathrm{HC}-\mathrm{ED}}{D^2-\mathrm{CG}},$ $c=\frac{\mathrm{\Σ}(x_i^2+y_i^2)+\mathrm{a\Σ}{x}_i+\mathrm{b\Σ}{y}_i}{N}---\left(12\right)$

Radius and the center of circle are:

$m=\frac{2(\mathrm{bde}-a)}{1+e^2+d^2},n=\frac{-b/2-\mathrm{dem}}{1+e^2},l=\mathrm{dm}+\mathrm{en}+f$

$r=\sqrt{m^2+n^2+f^2-2\mathrm{lf}+l^2-c}$

2.2 utilize LMF method to be optimized the center of circle of trying to achieve and radius

Utilize least square method to obtain the center of circle and the radius of space circle, and the real radius of bend pipe can be obtained by actual measurement, if the radial misalignment of the radius obtained and actual measurement is comparatively large, the center of circle calculated also can be forbidden.In order to address this problem, by about beam radius, LMF algorithm is adopted to solve the optimization problem of the center of circle, the space degree of accuracy.

If the round heart of known spatial is c (x, y, z), about beam radius is r, space circular arc sample set (x _i, y _i, z _i) (i=1,2 ... N), the center of circle after optimization is p (p ₁, p ₂, p ₃), so the variance of least square is:

res＝∑[(x _i-p ₁) ²+(y _i-p ₂) ²+(z _i-p ₃) ²-r ²](14)

The expression formula of LMF algorithm is:

[Xf,ssq,CNT]＝LMFsolve(FUN,Xo,Options)(15)

Wherein Xf represents final approximate solution, and ssq represents residual sum, and CNT represents iterations, and FUN is function name, and Xo represents n and ties up initial solution, and Options represents alternative controling parameters.

By known center of circle c (x, y, z), the unknown center of circle p (p after optimization ₁, p ₂, p ₃) and variance res bring formula (15) into:

[p,ssq,CNT]＝LMFsolve(res,c,'Display',-1)(16)

Center of circle p (the p closer to actual conditions after band Radius Constraint is optimized is calculated by LMF algorithm ₁, p ₂, p ₃).

The coordinate transform in 2.3 centers of circle

By being with the center of circle after Radius Constraint least square fitting to be p (p ₁, p ₂, p ₃), this center of circle is the central coordinate of circle of striation under camera coordinates system, and in practical application, carries out subsequent treatment under the camera light plane center of circle being transformed into basis coordinates system, therefore needs the aiming plane center of circle to carry out coordinate transform.

In free bend pipe system of processing, CCD camera is (pxCE, pyCE, pzCE) relative to the coordinate of robot end, T _ce3 × 3 matrixes that middle the first row first row starts are the attitude rotational transformation matrix (CE be cameratorobotend) of camera to robot end.Trick relational matrix and the camera transformation matrix relative to end is obtained by demarcating camera:

$T_{\mathrm{ce}}=\left[\begin{array}{cccc}\mathrm{nxCE}& \mathrm{oxCE}& \mathrm{axCE}& \mathrm{pxCE}\\ \mathrm{nyCE}& \mathrm{oyCE}& \mathrm{ayCE}& \mathrm{pyCE}\\ \mathrm{nzCE}& \mathrm{ozCE}& \mathrm{azCE}& \mathrm{pzCE}\\ 0& 0& 0& 0\end{array}\right]---\left(17\right)$

Robot end's coordinate system relative to the transformation matrix of basis coordinates system of robot is:

$T_{\mathrm{eb}}=\left[\begin{array}{cccc}\mathrm{nxEB}& \mathrm{oxEB}& \mathrm{axEB}& \mathrm{pxEB}\\ \mathrm{nyEB}& \mathrm{oyEB}& \mathrm{ayEB}& \mathrm{pyEB}\\ \mathrm{nzEB}& \mathrm{ozEB}& \mathrm{azEB}& \mathrm{pzEB}\\ 0& 0& 0& 1\end{array}\right]---\left(18\right)$

Wherein, (pxEB, pyEB, pzEB) for robot end is relative to the coordinate under robot base mark system, T _eb3 × 3 matrixes that middle the first row first row starts are the attitude rotational transformation matrix (EB be endtobase) of robot end to robot base mark system.

Then camera to the transformation matrix of basis coordinates system of robot is:

$T_{\mathrm{cb}}=T_{\mathrm{eb}}*T_{\mathrm{ce}}=\left[\begin{array}{cccc}\mathrm{nxCB}& \mathrm{oxCB}& \mathrm{axCB}& \mathrm{pxCB}\\ \mathrm{nyCB}& \mathrm{oyCB}& \mathrm{ayCB}& \mathrm{pyCB}\\ \mathrm{nzCB}& \mathrm{ozCB}& \mathrm{azCB}& \mathrm{pzCB}\\ 0& 0& 0& 1\end{array}\right]---\left(19\right)$

Wherein, (pxCB, pyCB, pzCB) for camera is relative to the coordinate of basis coordinates system of robot, T _cb3 × 3 matrixes that middle the first row first row starts are the attitude rotational transformation matrix (CB be cameratobase) of camera to robot base mark system.

Robot end is (pxEB, pyEB, pzEB) relative to the coordinate of robot, obtains the coordinate (px, py, pz) of the camera light plane center of circle under basis coordinates system of robot thus:

$\left[\begin{array}{c}\mathrm{px}\\ \mathrm{py}\\ \mathrm{pz}\\ 1\end{array}\right]=\left[\begin{array}{cccc}\mathrm{nxCB}& \mathrm{oxCB}& \mathrm{axCB}& \mathrm{pxCB}\\ \mathrm{nyCB}& \mathrm{oyCB}& \mathrm{ayCB}& \mathrm{pyCB}\\ \mathrm{nzCB}& \mathrm{ozCB}& \mathrm{azCB}& \mathrm{pzCB}\\ 0& 0& 0& 1\end{array}\right]\left[\begin{array}{c}{p}_1\\ p_2\\ p_3\\ 1\end{array}\right]---\left(20\right)$

The algorithm flow that the space circle center of circle and radius are determined as shown in Figure 3.

3. the B-spline curves fit procedure of bend pipe orbit of shaft center

The B-spline curves initial fitting of 3.1 bend pipe orbit of shaft center

B-spline interpolation curve is expressed as:

In formula (21), k represents the power of B-spline, and t is node, and subscript i is the sequence number of B-spline difference curve.

From formula (21), for determining i-th k B-spline N _i,kx () need use t _i, t _i+1..., t _i+k+1k+2 node altogether, n+1 control vertex d in equation (22) _i(i=0,1 ..., n+1 k B-spline basic function N n) will be used _i,k(x) (i=0,1 ..., n).B-spline curves are defined in interval x ∈ [t _i, t _i+1] on curved section, omit the item that wherein basic function gets null value, can be expressed as:

$P\left(x\right)=\underset{j=i=k}{\overset{i}{\mathrm{\Σ}}}{d}_j{N}_{j,k}\left(x\right),x\∈[t_i,t_{i+1}]---\left(22\right)$

Namely on k B-spline curves, domain of definition intrinsic parameter is x ∈ [t _i, t _i+1] 1 p (x) at the most with k+1 summit d _j(j=i-k, i-k+1 ... i) relevant, have nothing to do with other summit.

Different according to the distribution situation of the knot vector interior joint of B-spline curves, B-spline curves can be divided into following 4 types: Uniform B-Spline Curve, Quasi uniform B-spline, segmentation bezier curve, general non-uniform B-spline curve.

The B-spline curves curved surface adaptive fitting algorithm that this axis line track generation method adopts proposes on the basis of Quasi uniform B-spline, can meet the requirement of different fitting precision.

Corresponding pose adjustment is made for making the orbit of shaft center change of end effector of robot according to bend pipe in traveling process, need B-spline curves curved surface adaptive fitting algorithm be utilized to carry out curve fitting to a series of center of circle C obtained, as shown in Figure 4, its step comprises specific algorithm flow process:

1) from the point sequence of the centre point C tried to achieve, discrete data point is extracted in order; 2) to the method pressing specification accumulation Chord Length Parameterization of fetching data determine the parameter value of each discrete data point; 3) defined by B-spline interpolation curve and set up equation group, inverse curve controlled summit; By the method for specification accumulation Chord Length Parameterization, knot vector is tried to achieve to control vertex; 4) B-spline curves are determined by required control vertex and knot vector.

Specific implementation step is:

1) required B-spline interpolation curve will by n+1 control point d _i(i=0,1 ..., n) with knot vector U=[u ₀, u ₁... u _n+k-1] definition.Wherein, n=m+k-1, namely control vertex number has more k-1 than data point number, total m+k control point;

2) one group of data point q that k B-spline curves pass through first is extracted _i(i=0,1 ..., m), these data points obtain from the point sequence of the centre point C asked for;

3) the first and last end points of B-spline interpolation curve is consistent with first and last data point respectively, the connection segment point of curve successively with the node one_to_one corresponding in the B-spline matched curve domain of definition.By end-points interpolation condition, the clamped condition of k+1 multiple knot end points should be got, get again the specification domain of definition, so have

u ₀＝u ₁＝…＝u _k＝0,u _n+1＝u _n+2＝…＝u _n+k+1；

4) method of data point by specification accumulation Chord Length Parameterization is obtained namely from u _kthe domain of definition internal segment point value risen equals successively the data point parameter value risen.Now can provide by interpolation condition the system of linear equations formed with n+1 control point m+1 the linear equation that be unknown vector:

$p\left(u_i\right)=\underset{j=0}{\overset{n}{\mathrm{\Σ}}}{d}_j{N}_{j,k}\left(u_i\right)=\underset{j=i-k}{\overset{i}{\mathrm{\Σ}}}{d}_{j,k}\left(u_i\right)=q_{i-k}---\left(23\right)$

In formula, $u\∈[u_t,u_{t+1}]\⋐[u_k,u_{n+1}],t=k,k+1,...,n.$

5) for open curve, equation number is less than unknown number of vertex, must supplement k-1 the additional equation provided by suitable boundary condition, could simultaneous solution.For C ^k-1continuous print B-spline closed curve, domain of definition interior nodes can be determined equally, but the domain of definition is total to the clamped condition that 2k node just should not get into multiple knot end points outward, and should be got into by nodal values according to the method for structure B-spline curves

u ₀＝u _n-k+1-1,u ₁＝u _n-k+2-1,…,u _n-k＝u _n-1;u _n+2＝1+u _k+1,u _n+3＝1+u _k+2,…,u _n+k+1＝1+u _2k

Because first and last data point is mutually overlapping, q ₀=q _m, equation reduces one.Because the first and last k strong point generated is consistent, i.e. d _n-k+1+i=d _i(i=0,1 ..., k-1), so only remaining m equation;

6) form equation group by above-mentioned two gained equation simultaneous, solve curve controlled summit d _i(i=0,1 ..., knot vector U=[u n) and to control vertex utilizing specification accumulation Chord Length Parameterization method to obtain ₀, u ₁..., u _n+k+1] jointly determine B-spline curves.When actual configuration B-spline interpolation curve, adopt C ²continuous print B-spline Curve is as interpolation curve.

The quadratic fit of 3.2B SPL

The B-spline curves tentatively asked for are consistent with already present curve.But due to measured deviation, if can there is noise and swing when data point is too much, the B-spline curves that inverse operation process obtains can depart from some some place and ideal curve.Disturbance will cause machining locus level and smooth not frequently, for improving this situation, propose and carrying out to obtaining B-spline curves the method that a quadratic fit is got in segmentation.Concrete steps are:

1) B-spline curves after matching are divided equally by arc length, again get a little, as the data point asking for B-spline curves in the special ratios position (such as position of halving) of each curved section;

2) then again by B-spline curves fitting algorithm, quadratic fit is carried out to a series of data points obtained, again obtain B-spline curves.

Experiment proves, this curve is closer to ideal curve movement locus, and amplitude of fluctuation and frequency all significantly reduce, and is more suitable for the smooth motion processing of robot.Curve effect contrast figure as shown in Figure 5.

Claims (5)

1., based on the bend pipe magnetic grinding automatic navigation method of machine vision, it is characterized in that comprising the following steps:

1) Robotic Hand-Eye Calibration is carried out to camera; Control arm motion makes one end of bend pipe vertically penetrate the bearing centre of magnetic lapping device, and fixed bending tube two ends; By the outer surface of the linear light vertical irradiation of light source at bend pipe, form semicircular striation;

2) camera absorbs this optical strip image, and obtain this optical strip image relative to camera coordinates system O _cthree-dimensional point set; The approximate center of circle and the radius that least square fitting obtains this semicircle striation is carried out to three-dimensional point set, then this approximate center of circle is optimized, obtain this semicircle striation center of circle at camera coordinates system O _cin coordinate p; Coordinate p is converted to this center of circle at basis coordinates system of robot O _bin coordinate C;

3) using first editing objective point that coordinate C moves as robot arm, control arm motion makes the bearing centre of magnetic lapping device with this impact point for starting point makes initial straight-line motion; Repeat step 2 simultaneously) obtain a series of central coordinate of circle C _i;

4) by central coordinate of circle C _icarry out the orbit of shaft center obtaining bend pipe based on the matching of B-spline curves; This orbit of shaft center is position required for processing of robots and attitude as editing objective track, realizes robot carries out magnetic grinding self-navigation to free bend pipe.

2. the bend pipe magnetic based on machine vision according to claim 1 grinding automatic navigation method, is characterized in that: described to three-dimensional point set carry out least square fitting obtain this semicircle striation the approximate center of circle (m, n, l) and and radius r be $\begin{array}{c}m=\frac{2(bde-a)}{1+e^2+d^2},n=\frac{-b/2-dem}{1+e^2},l=dm+en+f\\ r=\sqrt{m^2+n^2+f^2-2lf+l^2-c}\end{array}$

Wherein a, B, C, D are the coefficient in line-structured light plane equation Ax+By+Cz+D=0, and wherein, line-structured light plane equation is determined by the three-dimensional point of the demarcation in camera intrinsic parameter and line-structured light plane;

$a=\frac{HD-EG}{CG-D^2},b=\frac{HD-EG}{D^2-CG},c=\frac{\mathrm{\Σ}\left(x_i^2+y_i^2\right)+{\mathrm{a\Σx}}_i+{\mathrm{b\Σy}}_i}{N},G={\mathrm{N\Σy}}_i^2-{\mathrm{\Σy}}_i{\mathrm{\Σy}}_i,$

$\begin{array}{c}E=N(1+d^2){\mathrm{\Σx}}_i^3+(1+e^2){\mathrm{\Σx}}_i{\mathrm{\Σy}}_i^2+{\mathrm{Nh\Σx}}_i^2{\mathrm{\Σy}}_i-(1+d^2){\mathrm{\Σx}}_i^2{\mathrm{\Σx}}_i\\ -(1+e^2){\mathrm{\Σy}}_i^2{\mathrm{\Σx}}_i-{\mathrm{h\Σx}}_i{y}_i{\mathrm{\Σx}}_i\end{array}$

$\begin{array}{c}H=N(1+d^2){\mathrm{\Σx}}_i^2{\mathrm{\Σy}}_i+N(1+e^2){\mathrm{\Σy}}_i^3+{\mathrm{Nh\Σx}}_i{\mathrm{\Σy}}_i^2-(1+d^2){\mathrm{\Σx}}_i^2{\mathrm{\Σy}}_i\\ -(1+e^2){\mathrm{\Σy}}_i^2{\mathrm{\Σy}}_i-{\mathrm{h\Σx}}_i{y}_i{\mathrm{\Σy}}_i\end{array},$

(x _i, y _i, z _i) (i=1,2 ..., N) and be three-dimensional point set, N is number a little, h=2de.

3. the grinding of the bend pipe magnetic based on machine vision automatic navigation method according to claim 1, it is characterized in that: described being optimized this approximate center of circle adopts LMF algorithm, be specially the center of circle p (p after being similar to center of circle c (x, y, z), optimization ₁, p ₂, p ₃) and variance res bring into LMF algorithmic formula namely obtain optimization after the center of circle;

LMF algorithmic formula be [p, ssq, CNT]=LMFsolve (res, c, ' Display' ,-1), wherein, ssq represents residual sum, and CNT represents iterations, and-1 represents controling parameters.

4. the grinding of the bend pipe magnetic based on machine vision automatic navigation method according to claim 1, is characterized in that: described by central coordinate of circle C _ithe matching carried out based on B-spline curves comprises the following steps:

4-1) from sequence of points center of circle C _iin extract discrete data point in order, determine the knot vector of each data point by the method for accumulation Chord Length Parameterization; Define establishment equation group according to the discrete data point extracted and knot vector by B-spline curves, obtain curve controlled summit; By the knot vector on accumulation Chord Length Parameterization method determination curve controlled summit, according to obtaining control vertex and knot vector determines B-spline curves;

4-2) B-spline curves of gained are divided equally by arc length, again get a little as discrete data point in the same ratio position of every section of arc; Re-executing step 4-1) SPL that obtains is the orbit of shaft center of bend pipe.

5. the bend pipe magnetic based on machine vision according to claim 4 grinding automatic navigation method, is characterized in that: the described equation group defining establishment by B-spline curves is

$p\left(u_i\right)=\underset{j=0}{\overset{n}{\Σ}}{d}_j{N}_{j,k}\left(u_i\right)=\underset{j=i-k}{\overset{i}{\Σ}}{d}_{j,k}\left(u_i\right)=q_{i-k}$

d _n-k+1+i＝d _i(i＝0,1,…,k-1)

Wherein, $u\∈\[u_t,u_{t+1}\]\⋐\[u_k,u_{n+1}\],t=k,k+1,...,n$ For knot vector, summit d _j(j=i-k, i-k+1 ... i), N _i,k(x) (i=0,1 ..., n) be B-spline basic function, n is control vertex number, q _i(i=0,1 ..., m) be one group of data point that k B-spline curves pass through.