High-tension line repairing mechanical arm mobile route planning system and method
Technical field
The present invention relates to mobile route planning field, particularly relate to a kind of high-tension line repairing mechanical arm mobile route planning system and method.
Background technology
At present, the technology such as artificial intelligence, machine vision develops rapidly, and robot is the machine that a kind of intelligence degree is higher, and they can utilize video camera to obtain visual information, make it have logical thinking ability by CPU operational data.Utilize binocular vision system can simulate the mechanism of mankind's eyes observation three-dimensional world, but the binocular vision system of machine composition is difficult to simulate the sensitivity characteristic of human eye to motor message.
An impact point in three-dimensional world, will be mapped to the pixel of two diverse locations in the left and right vision of biocular systems.Utilize the two width two dimensional images that left and right vision in biocular systems obtains, can reconstruct the form of object in actual three-dimensional world simultaneously occurred in left and right vision, this is the basis that robot can utilize binocular vision system.We often utilize binocular vision system carry out moving of guided robot or carry out a certain operation, and robot does not have the information processing function of mankind's prosperity after all, and its any one operation is all determined by associated a series of data operation results.
Existing data operating method is that each calculating in the binocular vision visual field is put relatively and the side-play amount of regulation movement locus, but this method will bring a large amount of useless calculating, and overall calculation amount is very huge.And, different positions will be presented in the image that the motion of mechanical arm obtains in left and right vision, point in the visual pattern of left and right is according to Epipolar geometry relation one to one, and therefore the movement locus reaction of mechanical arm will be a complicated two-dimensional optimal value search problem in left and right visual pattern.
With the robot that high-tension line repairing designs for application background, when using robot to carry out plant maintenance operation, often there are some barriers in the front end of robot.Now, the barrier problem of keeping away when mechanical arm moves will be an important problem of path planning.The mechanical arm of robot has multiple turning joint in addition, and the Angulation changes in each joint will change position and the move mode of whole mechanical arm, and therefore, the state modulator of each turning joint of mechanical arm is also an important problem of path planning.
Summary of the invention
Object of the present invention is exactly to solve the problem, provide a kind of high-tension line repairing mechanical arm mobile route planning system and method, this system and method can effectively controller mechanical arm move along the demarcation path in image, solve control problem and the robot obstacle-avoiding problem of each facet joint complex of mechanical arm, solved a relevant difficult problem for path planning by manual path's planning in conjunction with genetic algorithm.
To achieve these goals, the present invention adopts following technical scheme:
A mobile route planning system for high-tension line repairing mechanical arm, comprising:
Binocular vision system: for obtaining the image of repairing mechanical arm front end and image being sent to three-dimensional reconstruction system;
Three-dimensional reconstruction system: the view data obtained for utilizing binocular vision system, the three-dimensional coordinate of reconstructed object image;
Path planning system: for setting the path of mechanical arm movement, and the optimal control parameter of computing machine mechanical arm each joint move angle;
Kinetic control system: the mechanical arm for the path clustering robot planned according to path planning system moves;
Described binocular vision system is connected with three-dimensional reconstruction system, and path planning system is connected respectively with three-dimensional reconstruction system and kinetic control system.
Described binocular vision system comprises: optical axis intersection binocular camera and image pick-up card;
Optical axis intersects binocular camera for obtaining the image of mechanical arm front end to simulate the binocular vision function of human eye, and the simulating signal that image pick-up card is used for binocular camera exports converts digital signal to, and described image pick-up card is at least two passages.
A method for the mobile route planning system of high-tension line repairing mechanical arm, comprises the steps:
Step 1: the attitude of adjustment binocular camera and position, demarcates the left and right cameras of binocular vision system respectively;
Step 2: binocular vision system obtains binocular vision image, and corrects binocular image;
Step 3: according to the binocular vision image obtained, in path planning system, preset mechanical arm should the path of movement, and the mobile route of described mechanical arm is a series of straight-line segments be made up of image coordinate value, is referred to as path sequence;
Step 4: the position of tracing machine mechanical arm gripping target in binocular vision system, respectively according to the coordinate (x of gripping target in binocular vision system
1, y
1), (x
2, y
2), the actual three-dimensional coordinate (X of the gripping target of reconstructed object position
w, Y
w, Z
w);
Step 5: the optimal control parameter utilizing genetic algorithm computing machine mechanical arm each joint move angle, and computing machine mechanical arm move after the theory three-dimensional coordinate (X of gripping target
c, Y
c, Z
c), make the movement of mechanical arm continuous close to path termination while move along path planning;
Step 6: the actual three-dimensional coordinate (X calculating gripping target location
w, Y
w, Z
w) with use the gripping goal theory three-dimensional coordinate (X that calculates of genetic algorithm
c, Y
c, Z
c) between error;
If described error is in the allowed band of setting, then mechanical arm moves effectively; If described error exceeds the allowed band of setting, then according to the distance between above-mentioned Two coordinate, controller mechanical arm moves to the mobile route direction close to setting;
Step 7: repeat step 5-6, until mechanical arm moves to the final position of demarcation.
Actual three-dimensional coordinate (the X of gripping target location is rebuild in described step 4
w, Y
w, Z
w) concrete grammar be:
Suppose that imaging plane and the ground of binocular camera keep level, then the three-dimensional coordinate rebuilding gripping target location is:
Wherein, l is the half of two camera distances, δ
afor camera horizon visual angle, W is camera horizon resolution, δ
bfor video camera vertical angle of view, H is video camera vertical resolution,
be the angle of two camera light axis, δ
1, δ
2represent reconstructed object position and the line of photocentre and the angle of optical axis.
The concrete grammar of described step 5 is:
1) determine the funtcional relationship that each turning joint of mechanical arm and its three-dimensional coordinate change, postulated mechanism mechanical arm has n turning joint, then three-dimensional coordinate (the X of mechanical arm gripping target
c, Y
c, Z
c) with the angle (θ in each joint
1, θ
2, θ
3..., θ
n) relation can be expressed as:
X
c=x(θ
1,θ
2,θ
3,…,θ
n)
Y
c=y(θ
1,θ
2,θ
3,…,θ
n)
Z
c=z(θ
1,θ
2,θ
3,…,θ
n)
The step-length arranging joint angles change is Δ θ, then the three-dimensional coordinate of mechanical arm and the relation of joint angles can be expressed as further:
X
c=x(θ
1+k
1Δθ,θ
2+k
2Δθ,θ
3+k
3Δθ,…,θ
n+k
nΔθ)
Y
c=y(θ
1+k
1Δθ,θ
2+k
2Δθ,θ
3+k
3Δθ,…,θ
n+k
nΔθ)
Z
c=z(θ
1+k
1Δθ,θ
2+k
2Δθ,θ
3+k
3Δθ,…,θ
n+k
nΔθ)
Wherein k
nget-1,0, any one value in 1; Respectively to k
ncarry out binary coding, that is: 00 represent k
n=0,01 or 10 represent k
n=1,11 represent k
n=-1;
2) mechanical arm has n turning joint, therefore a setting chromosomal length is 2n; The N number of such chromosome of stochastic generation, material is thus formed Population Size is N, and length is the random starting values of 2n;
3) suppose that the three-dimensional coordinate of the target adjusting mechanical arm gripping after parameter is for (X, Y, Z), (X, Y, Z) is mapped in the image coordinate system of path selection (x '
l, y '
l), transformational relation is:
Wherein, matrix M is carry out timing signal by a positive friendly calibration algorithm to the left and right cameras of binocular vision system to calculate, and s is homogeneous coordinates item;
4) calculate respectively (x '
l, y '
l) with the bee-line d of mechanical arm mobile route preset
1, and (x '
l, y '
l) with the distance d of terminal
2, at maintenance d
2under the condition do not increased, genetic algorithm search is utilized to make d
1the k reduced
noptimum combination value.
Beneficial effect of the present invention:
1, the present invention is by calculating the optimal control parameter of each joint move angle, and the move angle reducing each joint, on the impact of mechanical arm mobile route, makes mechanical arm accurately to move along path planning.
2, the path sequence of the present invention's setting effectively can solve mechanical arm and keep away barrier problem in moving process, makes the movement of mechanical arm more accurately and reliably.
3, the present invention is based on image information and carry out computing, informative, computing method are simple, and cost is low, are easy to promote.
4, the genetic algorithm that the present invention adopts has search capability random fast, and process is simple, has extensibility, is easily combined with other algorithms, facilitates further improvement of the present invention.
Accompanying drawing explanation
Fig. 1 is that the present invention intersects binocular vision model one;
Fig. 2 is that the present invention intersects binocular vision model two;
Fig. 3 is path sequence schematic diagram of the present invention;
Fig. 4 is the inventive method process flow diagram.
Embodiment
Below in conjunction with accompanying drawing and embodiment, the invention will be further described.
Utilize genetic algorithm to solve a system for high-tension line repairing mechanical arm mobile route planning problem, consist of the following components:
Binocular vision system: binocular vision system comprises binocular camera, image pick-up card.Binocular vision system is made up of two high-definition cameras, the binocular vision function of simulation human eye.Image pick-up card should have the real time image collection function of more than 2 passages.The main task of binocular vision system gathers binocular data image, changes into the digital signal that can utilize computer disposal via image pick-up card.
Three-dimensional reconstruction system: the binocular image data that three-dimensional reconstruction system mainly utilizes binocular vision system to obtain, according to binocular vision system model, rebuild the three-dimensional coordinate of target in binocular image, namely according to the image coordinate of target in binocular image, the world coordinates of reconstructed object.
Path planning system: utilize mouse, draws the path of mechanical arm movement, utilizes the optimal control parameter of genetic algorithm computing machine mechanical arm in a visual pattern of binocular vision.
Above-described binocular vision system adopts intersection binocular, and namely the optical axis of two video cameras intersects on the perpendicular bisector of two video camera photocentre lines, and the plane that two crossing optical axises are formed is parallel with the reference planes of world coordinate system, as shown in Figure 1.Utilize and intersect binocular and can expand the effective field of view of binocular vision system, namely the moving range that simultaneously appears in binocular image of same target is larger.
What human assistance regulation mechanical arm mobile route can solve mechanical arm keeps away barrier problem; Utilize the optimal control parameter of Genetic algorithm searching mechanical arm, mechanical arm is moved along artificial prescribed path, solves the control problem of each facet joint complex of mechanical arm.
A kind of method utilizing genetic algorithm to solve high-tension line repairing mechanical arm mobile route planning problem as shown in Figure 4, is realized by following steps:
Step one: correct binocular vision system, the attitude of artificial adjustment video camera and position, make binocular vision system meet the condition shown in Fig. 1, utilize Zhang Zhengyou calibration algorithm to demarcate two video cameras.
Step 2: obtain binocular image, image fault is corrected.The correcting image of an optional vision, utilizing mouse to draw a mechanical arm should the path of movement in this vision, for convenience of describing, selects left visual pattern to demarcate mobile route here.The mobile route of the mechanical arm chosen in the picture is made up of a series of straight-line segment, is namely made up of a series of image coordinate value, is referred to as path sequence here, requires that number of straight segments is few as much as possible, so just can artificial avoiding obstacles.Described path sequence as shown in Figure 3.
Step 3: the target of tracing machine mechanical arm gripping in binocular image, according to the image coordinate (x of target in binocular image
r, y
r), (x
l, y
l) utilize the actual three-dimensional coordinate (X of three-dimensional reconstruction coordinate formula calculated target positions
w, Y
w, Z
w).
Step 4: according to the mobile controling parameters of mechanical arm, utilize the optimal control parameter of genetic algorithm computing machine mechanical arm, and computing machine mechanical arm move after the theory three-dimensional coordinate (X of gripping target
c, Y
c, Z
c), make the movement of mechanical arm continuous close to path termination while move along path planning.
Step 5: the real world coordinate (X calculating the target location obtained by binocular image
w, Y
w, Z
w) and use error between the world coordinates that obtains of genetic algorithm; According to the error range of the permission of setting in advance, if error is in allowed band, then mechanical arm moves effectively; If error exceeds allowed band, then utilize kinetic control system to correct, make moving back on correct path of mechanical arm.
Rebuild the actual three-dimensional coordinate (X of gripping target location
w, Y
w, Z
w) concrete grammar be:
Suppose that imaging plane and the ground of binocular camera keep level, as shown in Figure 2, then the three-dimensional coordinate rebuilding gripping target location is concrete model:
Wherein, l is the half of two camera distances, δ
afor camera horizon visual angle, W is camera horizon resolution, δ
bfor video camera vertical angle of view, H is video camera vertical resolution,
be the angle of two camera light axis, δ
1, δ
2represent reconstructed object position and the line of photocentre and the angle of optical axis.
Step 4 mentioned above realizes in the following manner:
Determine the funtcional relationship that mechanical arm turning joint and target location three-dimensional coordinate change, postulated mechanism mechanical arm has n turning joint, then three-dimensional coordinate (the X of the target location of mechanical arm gripping
c, Y
c, Z
c) with the angle (θ in each joint
1, θ
2, θ
3..., θ
n) relation can be expressed as:
X
c=x(θ
1,θ
2,θ
3,…,θ
n)
Y
c=y(θ
1,θ
2,θ
3,…,θ
n)
Z
c=z(θ
1,θ
2,θ
3,…,θ
n)
The step-length arranging joint angles change is Δ θ, then the three-dimensional coordinate of the target location of mechanical arm gripping and the relation of joint angles can be expressed as further:
X
c=x(θ
1+k
1Δθ,θ
2+k
2Δθ,θ
3+k
3Δθ,…,θ
n+k
nΔθ)
Y
c=y(θ
1+k
1Δθ,θ
2+k
2Δθ,θ
3+k
3Δθ,…,θ
n+k
nΔθ)
Z
c=z(θ
1+k
1Δθ,θ
2+k
2Δθ,θ
3+k
3Δθ,…,θ
n+k
nΔθ)
Wherein can make the k that d1 reduces
noptimum combination value can be taken as-1,0, any one value in 1.
To k
ncarry out binary coding, every two one group, represent k
nvalue decimal value, as: 00 represents k
n=0,01,10 represents k
n=1,11 represent k
n=-1.Mechanical arm has n turning joint, therefore a chromosomal length is 2n.The N number of such chromosome of stochastic generation, material is thus formed Population Size is N, and length is the random starting values of 2n.
Suppose that the calculating world coordinates of the target adjusting mechanical arm gripping after parameter is for (X, Y, Z), (X, Y, Z) is mapped in the image coordinate system of path selection (x '
l, y '
l), transformational relation is:
Matrix M can use Zhang Zhengyou standardization to calculate at computing machine timing signal herein.
Calculate (x '
l, y '
l) and the artificial bee-line d1 marking path, and (x '
l, y '
l) with the distance d2 of terminal, the k utilizing genetic algorithm search can make d1 under the condition keeping d2 not increase to reduce
noptimum combination value.
Some explanations about genetic algorithm:
The fitness function of genetic algorithm:
The present invention adopts the random method of weighting to construct fitness function, i.e. F=w
1d
1+ w
2d
2.Wherein, w
1+ w
2=1, w
1for produce at random 0 to 1 between random number.
Select operation: with the population that certain probability selection individuality composition is new in the old population of previous step.Wherein, individual fitness value is higher, larger by the probability selected, and individual selected probability is expressed as:
Wherein, F
ibe the fitness function value of i-th population, N is population number.
Interlace operation: Stochastic choice two individualities in population, utilize the mode of chiasma, the excellent genes of parent is passed to filial generation, a kth chromosome and i-th interlace operation of chromosome in jth position.Individual employing binary coding, intercepts two chromosomes in jth position during intersection, exchanges the afterbody gene order that two chromosomes intercept.
Mutation operation: mutation operation can keep the diversity of population.In population, select body one by one at random, select a point in individuality to carry out mutation operation.Mutation operation is carried out to the gene of i-th in kth individuality.Because individuality adopts binary coding mode, adopt the mode of roulette, select i-th gene to carry out 0/1 conversion with certain mutation probability.
Adopt above-mentioned genetic algorithm calculate send as an envoy to d1 reduce k
noptimum combination value.According to the optimal control parameter calculating mechanical arm, the theory three-dimensional coordinate of the target location of computing machine mechanical arm gripping.
By reference to the accompanying drawings the specific embodiment of the present invention is described although above-mentioned; but not limiting the scope of the invention; one of ordinary skill in the art should be understood that; on the basis of technical scheme of the present invention, those skilled in the art do not need to pay various amendment or distortion that creative work can make still within protection scope of the present invention.