CN109917793A - A kind of duct piece assembling machine obstacle-avoiding route planning method and device - Google Patents
A kind of duct piece assembling machine obstacle-avoiding route planning method and device Download PDFInfo
- Publication number
- CN109917793A CN109917793A CN201910299712.0A CN201910299712A CN109917793A CN 109917793 A CN109917793 A CN 109917793A CN 201910299712 A CN201910299712 A CN 201910299712A CN 109917793 A CN109917793 A CN 109917793A
- Authority
- CN
- China
- Prior art keywords
- model
- assembling machine
- barrier
- node
- end effector
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Granted
Links
- 238000000034 method Methods 0.000 title claims abstract description 56
- 230000004888 barrier function Effects 0.000 claims abstract description 179
- 239000012636 effector Substances 0.000 claims abstract description 88
- 230000033001 locomotion Effects 0.000 claims abstract description 25
- 238000004321 preservation Methods 0.000 claims description 3
- 230000001737 promoting effect Effects 0.000 claims description 3
- 238000009434 installation Methods 0.000 claims 2
- 238000000605 extraction Methods 0.000 abstract description 5
- 238000010276 construction Methods 0.000 description 7
- 238000010586 diagram Methods 0.000 description 5
- 230000008569 process Effects 0.000 description 3
- 238000005516 engineering process Methods 0.000 description 2
- 238000005034 decoration Methods 0.000 description 1
- 238000005553 drilling Methods 0.000 description 1
- 238000012986 modification Methods 0.000 description 1
- 230000004048 modification Effects 0.000 description 1
- 230000008439 repair process Effects 0.000 description 1
- 238000012706 support-vector machine Methods 0.000 description 1
Classifications
-
- Y—GENERAL TAGGING OF NEW TECHNOLOGICAL DEVELOPMENTS; GENERAL TAGGING OF CROSS-SECTIONAL TECHNOLOGIES SPANNING OVER SEVERAL SECTIONS OF THE IPC; TECHNICAL SUBJECTS COVERED BY FORMER USPC CROSS-REFERENCE ART COLLECTIONS [XRACs] AND DIGESTS
- Y02—TECHNOLOGIES OR APPLICATIONS FOR MITIGATION OR ADAPTATION AGAINST CLIMATE CHANGE
- Y02P—CLIMATE CHANGE MITIGATION TECHNOLOGIES IN THE PRODUCTION OR PROCESSING OF GOODS
- Y02P90/00—Enabling technologies with a potential contribution to greenhouse gas [GHG] emissions mitigation
- Y02P90/30—Computing systems specially adapted for manufacturing
Landscapes
- Manipulator (AREA)
Abstract
The invention discloses a kind of duct piece assembling machine obstacle-avoiding route planning method and device, method includes: realtime graphic when obtaining assembling machine movement；If judging that there are barriers in the movement routine of assembling machine according to image, then according to the barrier feature of extraction, with can be by the complete envelope of barrier and the smallest sphere of radius is configured to barrier model, and according to the kinematics model of assembling machine, with can be by the complete envelope of the joint link lever of assembling machine and the smallest cylindrical body of radius is configured to joint link lever model, with can by the yoke of assembling machine to the partially complete envelope of end effector and the smallest cuboid of volume be configured to yoke to end effector part model；The path that assembling machine is moved to by current location target position is cooked up according to each model, so that assembling machine will not collide when mobile according to path with barrier.Avoidance path is cooked up automatically when duct piece assembling machine encounters obstacle to realize.
Description
Technical field
The present invention relates to robot motion planning technical fields, more particularly to a kind of duct piece assembling machine obstacle-avoiding route planning
Method and device.
Background technique
Duct piece assembling machine is one of important composition component of shield driving equipment, and major function is to install prefabricated pipe section
To the inner surface of tunnel just excavated, supporting is carried out to tunnel, it is that shield driving equipment is stablized that duct piece assembling machine, which keeps working normally,
The premise of construction.Currently, the duct piece assembling machine used both at home and abroad is by operator by remote control mode, control assembling machine is completed
Pipe sheet assembling work, but this mode depends on the technical level and skilled operation degree of operator, has positioning accuracy
Disadvantage low, speed of application is slow causes construction totle drilling cost relatively high.
With the development of Industrial Robot Technology, intelligentized duct piece assembling machine becomes a kind of development trend, can either subtract
Few manual operation, and can be improved positioning accuracy and speed of application.But in the intelligent development process of duct piece assembling machine,
In narrow space at the construction field (site), obstacle can be avoided by how realizing that duct piece assembling machine is cooked up automatically when encountering barrier
The suitable path of object is a technological difficulties for needing to solve.
Summary of the invention
In view of this, being realized the object of the present invention is to provide a kind of duct piece assembling machine obstacle-avoiding route planning method and device
Cook up avoidance path automatically when duct piece assembling machine encounters barrier.
To achieve the above object, the invention provides the following technical scheme:
A kind of duct piece assembling machine obstacle-avoiding route planning method, comprising:
Obtain realtime graphic when assembling machine movement；
If being judged in the movement routine of the assembling machine according to described image there are barrier, according to from described image
The barrier feature of extraction, with can be by the complete envelope of barrier and the smallest sphere of radius is configured to barrier model, and root
According to the kinematics model of the assembling machine, with can be by the complete envelope of the joint link lever of the assembling machine and the smallest cylinder of radius
Body is configured to joint link lever model, with can by the yoke of the assembling machine to the partially complete envelope of end effector and volume most
Small cuboid is configured to yoke to the model of end effector part；
According to the barrier model, each joint link lever model of the assembling machine and yoke to end effector part
Model, the path that the assembling machine is moved to target position by current location is cooked up, so that the assembling machine is according to institute
It will not collide with the barrier when stating path movement.
Preferably, building barrier model includes:
According to the barrier feature extracted from described image, three points of barrier are determined, wherein being some barrier
The point of the maximum absolute value of upper x coordinate value, on the other hand on barrier the maximum absolute value of y-coordinate value point, on the other hand hinder
Hinder the point of the maximum absolute value of z coordinate value on object；
Being calculated according to three points determined can be by the complete envelope of barrier and the smallest sphere of radius, by what is obtained
Sphere is configured to barrier model.
Preferably, the assembling machine includes walking joint, rotary joint and promotes joint；
According to the kinematics model of the assembling machine, with can by the complete envelope of the connecting rod in joint of walking and radius it is the smallest
Cylindrical body is configured to walking joint link lever model, with can be by the complete envelope of the connecting rod of rotary joint and the smallest cylindrical body of radius
It is configured to rotary joint connecting rod model, with can be by the complete envelope of the connecting rod for promoting joint and the smallest cylindrical body of radius is configured to
Promote joint link lever model.
Preferably, the model of building yoke to end effector part includes:
According to the run-length data of the two of acquisition lift cylinders, determine yoke and two lift cylinders tie point and
The location point of end effector calculates energy according to three points determined and according to the kinematics model of the assembling machine
It is enough by yoke to the partially complete envelope of end effector and side with the reference axis of basis coordinates system the smallest cuboid of parallel and volume,
Obtained cuboid is configured to yoke to the model of end effector part, the basis coordinates system is in building barrier mould
Type, joint link lever model and yoke to end effector part model when the reference frame established.
Preferably, further includes: if the assembling machine has grabbed section of jurisdiction, according to the section of jurisdiction threedimensional model pre-established and tie
The location point of end effector is closed, with can be by the complete envelope in section of jurisdiction and the smallest cuboid of volume is configured to section of jurisdiction model；
The assembling machine, which is cooked up, by the path that current location is moved to target position includes:
According to the barrier model, each joint link lever model of the assembling machine, yoke to end effector part
Model and the section of jurisdiction model, cook up the path that the assembling machine is moved to target position by current location, so that institute
Stating assembling machine will not collide the section of jurisdiction of the assembling machine and crawl according to the path when mobile with the barrier.
Preferably, building section of jurisdiction model includes:
According to the section of jurisdiction threedimensional model pre-established, the location point based on end effector determines several tops of section of jurisdiction
The minimum point of point and section of jurisdiction cambered surface, being calculated according to obtained each point can be by the complete envelope in section of jurisdiction and side and basis coordinates system
Reference axis is parallel and the smallest cuboid of volume, obtained cuboid is configured to section of jurisdiction model, the basis coordinates system be
The reference coordinate established when constructing the model of barrier model, joint link lever model and yoke to end effector part
System.
Preferably, plan that the assembling machine includes: by the path that current location is moved to target position
The first tree is established by tree root of the current location of end effector, with end effector target position to be reached
The second tree is established for tree root, operation is iterated with preset step-length, in which:
When the number of iterations is odd number, with one node of preset step-length random growth in the first tree, and from the first tree
The node nearest with the nodal distance that just grows is found out, along the node found out to the direction of the node just grown, according to presetting
Step-length obtains the first new node between the node found out and the node just grown, if the first new node meets impact conditions,
Regrow node in the first tree；
If the first new node is unsatisfactory for impact conditions, save the position of the first new node, and find out in the second tree with
First new node is apart from nearest node, along the node found out to the direction of the first new node, according to preset step-length from finding out
Node and the first new node between obtain the second new node, if the second new node meets impact conditions, in the second tree weight
Newborn long node saves the position of the second new node, is then changed next time if the second new node is unsatisfactory for impact conditions
For operation；
When the number of iterations is even number, with one node of preset step-length random growth in the second tree, and from the second tree
The node nearest with the nodal distance that just grows is found out, along the node found out to the direction of the node just grown, according to presetting
Step-length obtains third new node between the node found out and the node just grown, if third new node meets impact conditions,
Regrow node in the second tree；
If third new node is unsatisfactory for impact conditions, save the position of third new node, and find out in the first tree with
Third new node is apart from nearest node, along the node found out to the direction of third new node, according to preset step-length from finding out
Node and third new node between obtain the 4th new node, if the 4th new node meets impact conditions, in the first tree weight
Newborn long node saves the position of the 4th new node, is then changed next time if the 4th new node is unsatisfactory for impact conditions
For operation；
Each node location of preservation is arranged to form path according to the sequence of end effector current location to target position,
As the path cooked up, node meets impact conditions and refers to that the end effector of the assembling machine is located at node location when institute
It states assembling machine to collide with barrier, node is unsatisfactory for impact conditions and refers to that the end effector of the assembling machine is located at the section
The assembling machine does not collide with barrier when point position.
Preferably, the method for judging whether the assembling machine collides with barrier includes:
Judge whether the distance of axis of the centre of sphere of the barrier model to joint link lever model is greater than the barrier
The radius of model and the sum of the radius of joint link lever model, if not, it is determined that the assembling machine and barrier collide；
If so, judging the centre of sphere of the barrier model at a distance from the model axis of yoke to end effector part
Whether be greater than the barrier model radius and yoke to end effector part model outer surface to its axis maximum
Sum of the distance, if not, it is determined that the assembling machine and barrier collide；
If so, judge the barrier model the centre of sphere and yoke to the model of end effector part each vertex away from
From whether the radius of the barrier model is both greater than, if so, determining that the assembling machine does not collide with barrier, if it is not, then
Determine that the assembling machine and barrier collide.
Preferably, the method for judging whether the section of jurisdiction of the assembling machine and crawl collides with barrier includes:
Judge whether the distance of axis of the centre of sphere of the barrier model to joint link lever model is greater than the barrier
The radius of model and the sum of the radius of joint link lever model, if not, it is determined that the assembling machine and barrier collide；
If so, judging the centre of sphere of the barrier model at a distance from the model axis of yoke to end effector part
Whether be greater than the barrier model radius and yoke to end effector part model outer surface to its axis maximum
Sum of the distance, if not, it is determined that the assembling machine and barrier collide；
If so, judging at a distance from the centre of sphere of the barrier model and each vertex of model of yoke to end effector part
Whether the radius of the barrier model is both greater than, if not, it is determined that the assembling machine and barrier collide；
If so, judging whether the centre of sphere of the barrier model is greater than the barrier to the distance of the section of jurisdiction model axis
Hinder the sum of the maximum distance of the radius of object model with section of jurisdiction model outer surface to its axis, if not, it is determined that the assembly
Machine and barrier collide；
If so, judging whether the centre of sphere of the barrier model is both greater than described to the distance on each vertex of section of jurisdiction model
The radius of barrier model, if so, determining that the assembling machine does not collide with barrier；If not, it is determined that the assembling machine with
Barrier collision.
A kind of duct piece assembling machine obstacle-avoiding route planning device, for executing above-described duct piece assembling machine avoidance path rule
The method of drawing.
As shown from the above technical solution, duct piece assembling machine obstacle-avoiding route planning method and device provided by the present invention, it is first
Realtime graphic when assembling machine movement is first obtained, if being judged in the movement routine of assembling machine according to image there are barrier,
Assembling machine is constructed according to the barrier feature construction barrier model from image zooming-out, and according to the kinematics model of assembling machine
The yoke of joint link lever model and assembling machine to end effector part model, then according to barrier model, assembling machine
Joint link lever model and the model of yoke to end effector part of assembling machine cook up assembling machine and moved by current location
The path to target position is moved, i.e., will not be collided with barrier when assembling machine is mobile according to the path.
Duct piece assembling machine obstacle-avoiding route planning method and device disclosed by the invention, by obtaining figure when assembling machine movement
Picture constructs each partial model of assembling machine according to assembling machine kinematics model, further according to each mould according to picture construction barrier model
Type and cook up avoidance path, realize and cook up avoidance path automatically when duct piece assembling machine encounters barrier.
Detailed description of the invention
In order to more clearly explain the embodiment of the invention or the technical proposal in the existing technology, to embodiment or will show below
There is attached drawing needed in technical description to be briefly described, it should be apparent that, the accompanying drawings in the following description is only this
Some embodiments of invention for those of ordinary skill in the art without creative efforts, can be with
It obtains other drawings based on these drawings.
Fig. 1 is a kind of flow chart of duct piece assembling machine obstacle-avoiding route planning method provided in an embodiment of the present invention；
Fig. 2 is a kind of flow chart for duct piece assembling machine obstacle-avoiding route planning method that further embodiment of this invention provides；
Fig. 3 be in the embodiment of the present invention yoke established to signal of the model from side of end effector part
Figure；
Fig. 4 is the schematic diagram of the section of jurisdiction model established from side in the embodiment of the present invention；
Fig. 5 judge what whether assembling machine collided with barrier when being the non-grasping pipe piece of assembling machine in the embodiment of the present invention
Method flow diagram；
Fig. 6 is to have judged whether assembling machine and section of jurisdiction are sent out with barrier when assembling machine has grabbed section of jurisdiction in the embodiment of the present invention
The method flow diagram of raw collision.
Specific embodiment
Technical solution in order to enable those skilled in the art to better understand the present invention, below in conjunction with of the invention real
The attached drawing in example is applied, technical scheme in the embodiment of the invention is clearly and completely described, it is clear that described implementation
Example is only a part of the embodiment of the present invention, instead of all the embodiments.Based on the embodiments of the present invention, this field is common
Technical staff's every other embodiment obtained without making creative work, all should belong to protection of the present invention
Range.
Referring to FIG. 1, Fig. 1 is a kind of process of duct piece assembling machine obstacle-avoiding route planning method provided in an embodiment of the present invention
Figure, as seen from the figure, the described method comprises the following steps:
S10: realtime graphic when assembling machine movement is obtained.
In assembling machine operation, image when assembling machine movement, the i.e. image of assembling machine external environment are obtained.
S11: if being judged in the movement routine of the assembling machine according to described image there are barrier, according to from described
The barrier feature of image zooming-out, with can be by the complete envelope of barrier and the smallest sphere of radius is configured to barrier model,
And according to the kinematics model of the assembling machine, with can by the complete envelope of the joint link lever of the assembling machine and radius it is the smallest
Cylindrical body is configured to joint link lever model, with can be by the yoke of the assembling machine to the partially complete envelope of end effector and body
The smallest cuboid of product is configured to yoke to the model of end effector part.
According to the image of shooting, first determine whether that whether there are obstacles in the movement routine of assembling machine, if the shifting of assembling machine
There are barriers on dynamic path, then according to barrier model is established from the barrier feature of image zooming-out, are moved according to assembling machine
It learns model and model is established to each component of assembling machine.The kinematics model of assembling machine is pre-established according to the structure of assembling machine
's.
S12: according to the barrier model, each joint link lever model of the assembling machine and yoke to end effector
Partial model cooks up the path that the assembling machine is moved to target position by current location, so that the assembling machine is pressed
It will not collide with the barrier when mobile according to the path.
The present embodiment duct piece assembling machine obstacle-avoiding route planning method it can be seen from above content, by obtaining assembling machine
Image when movement constructs each partial model of assembling machine according to assembling machine kinematics model according to picture construction barrier model,
And then avoidance path is cooked up according to model, it realizes and cooks up avoidance path automatically when duct piece assembling machine encounters obstacle.
This duct piece assembling machine obstacle-avoiding route planning method is described in detail With reference to embodiment.
Referring to FIG. 2, the present embodiment duct piece assembling machine obstacle-avoiding route planning method specifically includes the following steps:
S20: realtime graphic when assembling machine movement is obtained.
In the specific implementation, photographic device can be set on assembling machine, assembling machine movement is shot by photographic device
Realtime graphic.3D image shot by camera is used in the present embodiment.
S21: judge that whether there are obstacles in the movement routine of the assembling machine according to described image.If it is not, then carrying out
Step S20；If so, carrying out step S22.
For the image that shooting obtains, characteristics of image can be extracted, the shifting of assembling machine is judged according to the characteristics of image of extraction
Whether there are obstacles on dynamic path.In the present embodiment specific limit is not done to method used in the characteristics of image for extracting image
It is fixed, it can accordingly select in practical applications.
According to the characteristics of image of extraction judge in the movement routine of assembling machine that whether there are obstacles can by the following method,
It specifically includes: the characteristics of image of extraction is input in preparatory trained support vector machine classifier, it is defeated by the classifier
The result that whether there are obstacles in the movement routine of the assembling machine out.
S22: according to the barrier feature extracted from described image, with can by the complete envelope of barrier and radius it is the smallest
Sphere is configured to barrier model, and according to the kinematics model of the assembling machine, can connect the joint of the assembling machine
The complete envelope of the bar and the smallest cylindrical body of radius is configured to joint link lever model, with can be by the yoke of the assembling machine to end
The partially complete envelope of actuator and the smallest cuboid of volume are configured to yoke to the model of end effector part.
This step establishes simplified model to each component and barrier of assembling machine.Basis coordinates system is initially set up, for barrier
Hindering object to establish barrier model can specifically include by the following method: according to the barrier feature from image zooming-out, determine to hinder
Hinder three points of object, wherein be some the point of the maximum absolute value of x coordinate value on barrier, on the other hand y-coordinate on barrier
The point of the maximum absolute value of value, on the other hand on barrier the maximum absolute value of z coordinate value point, further according to determining
Three points, which calculate, to be configured to barrier mould for obtained sphere for the complete envelope of barrier and the smallest sphere of radius
Type.
Assembling machine includes walking joint, rotary joint and promotes joint, for each joint link lever of assembling machine, according to assembly
The kinematics model of machine, with can by the complete envelope of the connecting rod in joint of walking and the smallest cylindrical body of radius be configured to walking joint
Connecting rod model, with can be by the complete envelope of the connecting rod of rotary joint and the smallest cylindrical body of radius is configured to rotary joint connecting rod mould
Type, with can by the complete envelope of the connecting rod for promoting joint and the smallest cylindrical body of radius be configured to promoted joint link lever model.
For assembling machine yoke to end effector part, with can be by the yoke of assembling machine to end effector part
Complete envelope and the smallest cuboid of volume are configured to yoke to the model of end effector part, in the specific implementation, preferably
Surrounding BOX Model using AABB is the yoke of assembling machine to end effector part building simplified model, i.e., with can be by assembling machine
Yoke is to the partially complete envelope of end effector, and each side of cuboid is parallel with the reference axis of basis coordinates system and volume is the smallest
Cuboid is configured to yoke to the model of end effector part.Specifically, establishing model to end effector part to yoke
Method include: that the tie point of yoke Yu two lift cylinders is determined according to the run-length data of two lift cylinders of acquisition
And the location point of end effector calculates energy according to three points determined and according to the kinematics model of assembling machine
It is enough by yoke to the partially complete envelope of end effector and side with basis coordinates system reference axis the smallest cuboid of parallel and volume, general
Obtained cuboid is configured to yoke to the model of end effector part, the basis coordinates system be building barrier model,
Joint link lever model and yoke to end effector part model when the reference frame established.The stroke of lift cylinder
Data are the distance of stretch out and draw back data of the lift cylinder in assembling machine movement, are equipped with stroke sensing in the lift cylinder of assembling machine
Device, stroke sensor can measure the distance of stretch out and draw back data of oil cylinder in real time.It specifically refers to shown in Fig. 3, Fig. 3 is gripping for foundation
Frame is to schematic diagram of the model from side of end effector part, and wherein P1 indicates the company of yoke and first lift cylinder
Contact, P2 indicate that the tie point of yoke and second lift cylinder, P3 indicate the location point of end effector.
Further, if assembling machine has grabbed section of jurisdiction, then section of jurisdiction be equally as assembling machine ontology with barrier not
The part that can be collided.Simplified model so is established to section of jurisdiction accordingly, simultaneously with specific reference to the section of jurisdiction threedimensional model pre-established
In conjunction with the location point of end effector, with can be by the complete envelope in section of jurisdiction and the smallest cuboid of volume is configured to section of jurisdiction model.
Specifically include following procedure: according to the section of jurisdiction threedimensional model pre-established, the location point based on end effector determines section of jurisdiction
Several vertex and section of jurisdiction cambered surface minimum point, according to obtained each point calculate can by the complete envelope in section of jurisdiction, and
Cuboid side is parallel with the reference axis of basis coordinates system and the smallest cuboid of volume is configured to section of jurisdiction model.Specifically refer to Fig. 4
Shown, Fig. 4 is schematic diagram of the section of jurisdiction model of foundation from side.
S23: according to the barrier model, each joint link lever model of the assembling machine and yoke to end effector
Partial model cooks up the path that the assembling machine is moved to target position by current location, so that the assembling machine is pressed
It will not collide with the barrier when mobile according to the path.
It plans that the assembling machine specifically includes following procedure by the path that current location is moved to target position: being held with end
The current location of row device is that tree root establishes the first tree, establishes second using end effector target position to be reached as tree root
Tree, is iterated operation with preset step-length, in which:
When the number of iterations is odd number, with one node of preset step-length random growth in the first tree, and from the first tree
The node nearest with the nodal distance that just grows is found out, along the node found out to the direction of the node just grown, according to presetting
Step-length obtains the first new node between the node found out and the node just grown, if the first new node meets impact conditions,
Regrow node in the first tree；
If the first new node is unsatisfactory for impact conditions, save the position of the first new node, and find out in the second tree with
First new node is apart from nearest node, along the node found out to the direction of the first new node, according to preset step-length from finding out
Node and the first new node between obtain the second new node, if the second new node meets impact conditions, in the second tree weight
Newborn long node saves the position of the second new node, is then changed next time if the second new node is unsatisfactory for impact conditions
For operation；
When the number of iterations is even number, with one node of preset step-length random growth in the second tree, and from the second tree
The node nearest with the nodal distance that just grows is found out, along the node found out to the direction of the node just grown, according to presetting
Step-length obtains third new node between the node found out and the node just grown, if third new node meets impact conditions,
Regrow node in the second tree；
If third new node is unsatisfactory for impact conditions, save the position of third new node, and find out in the first tree with
Third new node is apart from nearest node, along the node found out to the direction of third new node, according to preset step-length from finding out
Node and third new node between obtain the 4th new node, if the 4th new node meets impact conditions, in the first tree weight
Newborn long node saves the position of the 4th new node, is then changed next time if the 4th new node is unsatisfactory for impact conditions
For operation；
Each node location of preservation is arranged to form path according to the sequence of end effector current location to target position,
As the path cooked up, node meets impact conditions and refers to that the end effector of the assembling machine is located at node location when institute
It states assembling machine to collide with barrier, node is unsatisfactory for impact conditions and refers to that the end effector of the assembling machine is located at the section
The assembling machine does not collide with barrier when point position.
Further specifically, if assembling machine does not have grasping pipe piece, then referring to FIG. 5, judging that assembling machine and barrier are sent out
The method of raw collision includes following procedure:
S30: judge whether the distance of axis of the centre of sphere of the barrier model to joint link lever model is greater than the barrier
Hinder the radius of object model and the sum of the radius of joint link lever model.If not, it is determined that the assembling machine and barrier collide；If
It is then to carry out step S31.
Wherein, the axis of joint link lever model refers to the central axis for the cylinder model that corresponding joint connecting rod is established.
S31: judge the centre of sphere of the barrier model is at a distance from the model axis of yoke to end effector part
The no radius greater than the barrier model and yoke to end effector part model outer surface to its axis maximum away from
The sum of from, if not, it is determined that the assembling machine and barrier collide；If so, carrying out step S32.
Wherein, yoke to end effector part model axis refer to corresponding yoke to end effector part establish
Rectangular body Model central axis.
S32: judge at a distance from the centre of sphere of the barrier model and each vertex of model of yoke to end effector part
Whether the radius of the barrier model is both greater than, if so, determining that the assembling machine does not collide with barrier；If it is not, then really
The fixed assembling machine and barrier collide.
Further, if assembling machine has grabbed section of jurisdiction, then section of jurisdiction is considered as a part of assembling machine, it accordingly will basis
Barrier model, each joint link lever model of assembling machine, yoke to end effector part model and section of jurisdiction model, planning
Assembling machine is moved to the path of target position by current location so that when assembling machine is mobile according to the path assembling machine and
Section of jurisdiction will not collide with barrier.In this case the method for planning the avoidance path of assembling machine, equally can be used
The method process that planning assembling machine is moved to the path of target position by current location is stated, but should be noted that in this situation
Under, node meets impact conditions and refers to that the end effector of the assembling machine is located at the assembling machine or pipe when the node location
At least one in piece collides with barrier, and node is unsatisfactory for impact conditions and refers to that the end effector of the assembling machine is located at
The assembling machine and section of jurisdiction all do not collide with barrier when the node location.
In such cases, referring to FIG. 6, judging whether assembling machine and section of jurisdiction are sent out with barrier during planning path
The method of raw collision includes following procedure:
S40: judge whether the distance of axis of the centre of sphere of the barrier model to joint link lever model is greater than the barrier
Hinder the radius of object model and the sum of the radius of joint link lever model.If not, it is determined that the assembling machine and barrier collide；If
It is then to carry out step S41.
S41: judge the centre of sphere of the barrier model is at a distance from the model axis of yoke to end effector part
The no radius greater than the barrier model and yoke to end effector part model outer surface to its axis maximum away from
The sum of from.If not, it is determined that the assembling machine and barrier collide；If so, carrying out step S42:
S42: judge at a distance from the centre of sphere of the barrier model and each vertex of model of yoke to end effector part
Whether the radius of the barrier model is both greater than, if it is not, the centre of sphere of the i.e. described barrier model and yoke to end effector
At least one distance is less than the radius of the barrier model in the distance on each vertex of partial model, it is determined that the assembling machine
It is collided with barrier；If so, each vertex of model of the centre of sphere of the i.e. described barrier model and yoke to end effector part
The radius of the both greater than described barrier model of distance, then carry out step S43.
S43: judge whether the centre of sphere of the barrier model is greater than the obstacle to the distance of the section of jurisdiction model axis
The sum of the maximum distance of the radius of object model and section of jurisdiction model outer surface to its axis, if not, it is determined that the assembling machine
It is collided with barrier；If so, carrying out step S44.
Wherein, the axis of section of jurisdiction model refers to the central axis for the rectangular body Model that corresponding section of jurisdiction is established.
S44: judge whether the centre of sphere of the barrier model is both greater than described to the distance on each vertex of section of jurisdiction model
The radius of barrier model, if so, determining that the assembling machine does not collide with barrier；If not, it is determined that the assembling machine with
Barrier collision.
The present embodiment duct piece assembling machine obstacle-avoiding route planning method, for the design feature of assembling machine mixed in series and parallel,
Simplified model is established to each structure of assembling machine using cylindrical body enveloping method and AABB bounding box enveloping method, using sphere envelope
Method establishes simplified model to barrier, and the avoidance path of assembling machine is cooked up according to simplified model.
Correspondingly, the embodiment of the present invention also provides a kind of duct piece assembling machine obstacle-avoiding route planning device, more than executing
The duct piece assembling machine obstacle-avoiding route planning method.
It is real-time when duct piece assembling machine obstacle-avoiding route planning device provided by the present embodiment, first acquisition assembling machine movement
Image, it is special according to the barrier from image zooming-out if judging that there are barriers in the movement routine of assembling machine according to image
Sign building barrier model, and construct according to the kinematics model of assembling machine the joint link lever model and assembling machine of assembling machine
Then yoke is arrived to the model of end effector part according to barrier model, the joint link lever model of assembling machine and yoke
The model of end effector part cooks up the path that assembling machine is moved to target position by current location, i.e., assembling machine is according to this
Path will not collide when mobile with barrier.
Duct piece assembling machine obstacle-avoiding route planning device disclosed in the present embodiment, by obtaining image when assembling machine movement,
According to picture construction barrier model, each partial model of assembling machine is constructed according to assembling machine kinematics model, further according to each
Model and cook up avoidance path, realize and cook up avoidance path automatically when duct piece assembling machine encounters obstacle.
Detailed Jie has been carried out to a kind of duct piece assembling machine obstacle-avoiding route planning method provided by the present invention and device above
It continues.Used herein a specific example illustrates the principle and implementation of the invention, and the explanation of above embodiments is only
It is to be used to help understand method and its core concept of the invention.It should be pointed out that for those skilled in the art
For, it without departing from the principle of the present invention, can be with several improvements and modifications are made to the present invention, these improve and repair
Decorations are also fallen within the protection scope of the claims of the present invention.
Claims (10)
1. a kind of duct piece assembling machine obstacle-avoiding route planning method characterized by comprising
Obtain realtime graphic when assembling machine movement；
If judging that there are barriers in the movement routine of the assembling machine, extract according to from described image according to described image
Barrier feature, with can be by the complete envelope of barrier and the smallest sphere of radius is configured to barrier model, and according to institute
The kinematics model of assembling machine is stated, with can be by the complete envelope of the joint link lever of the assembling machine and the smallest cylindrical body structure of radius
Build as joint link lever model, with can by the yoke of the assembling machine to the partially complete envelope of end effector and volume it is the smallest
Cuboid is configured to yoke to the model of end effector part；
According to the mould of the barrier model, each joint link lever model of the assembling machine and yoke to end effector part
Type cooks up the path that the assembling machine is moved to target position by current location, so that the assembling machine is according to the road
Diameter will not collide when mobile with the barrier.
2. duct piece assembling machine obstacle-avoiding route planning method according to claim 1, which is characterized in that building barrier model
Include:
According to the barrier feature extracted from described image, three points of barrier are determined, wherein being a little that x is sat on barrier
The point of the maximum absolute value of scale value, on the other hand on barrier the maximum absolute value of y-coordinate value point, on the other hand on barrier
The point of the maximum absolute value of z coordinate value；
Being calculated according to three points determined can be by the complete envelope of barrier and the smallest sphere of radius, the sphere that will be obtained
It is configured to barrier model.
3. duct piece assembling machine obstacle-avoiding route planning method according to claim 1, which is characterized in that the assembling machine includes
Walking joint, rotary joint and promotion joint；
According to the kinematics model of the assembling machine, with can be by the complete envelope of the connecting rod in joint of walking and the smallest cylinder of radius
Body is configured to walking joint link lever model, with can be by the complete envelope of the connecting rod of rotary joint and the smallest cylindrical body of radius constructs
For rotary joint connecting rod model, with can be by the complete envelope of the connecting rod for promoting joint and the smallest cylindrical body of radius is configured to be promoted
Joint link lever model.
4. duct piece assembling machine obstacle-avoiding route planning method according to claim 1, which is characterized in that building yoke to end
The model of actuator part includes:
According to the run-length data of the two of acquisition lift cylinders, tie point and the end of yoke and two lift cylinders are determined
The location point of actuator, according to three points determined and according to the kinematics model of the assembling machine, calculating can be incited somebody to action
Yoke to the partially complete envelope of end effector and side with the reference axis of basis coordinates system the smallest cuboid of parallel and volume, will
To cuboid be configured to yoke to end effector part model, the basis coordinates system be building barrier model, close
The reference frame established when saving the model of connecting rod model and yoke to end effector part.
5. duct piece assembling machine obstacle-avoiding route planning method according to claim 1, which is characterized in that further include: if described
Assembling machine has grabbed section of jurisdiction, then according to the section of jurisdiction threedimensional model that pre-establishes and combines the location point of end effector, with can
By the complete envelope in section of jurisdiction and the smallest cuboid of volume is configured to section of jurisdiction model；
The assembling machine, which is cooked up, by the path that current location is moved to target position includes:
According to the barrier model, each joint link lever model of the assembling machine, yoke to end effector part model
And the section of jurisdiction model, the path that the assembling machine is moved to target position by current location is cooked up, so that the spelling
Installation will not collide the section of jurisdiction of the assembling machine and crawl according to the path when mobile with the barrier.
6. duct piece assembling machine obstacle-avoiding route planning method according to claim 5, which is characterized in that building section of jurisdiction model packet
It includes:
According to the section of jurisdiction threedimensional model pre-established, the location point based on end effector, determine several vertex of section of jurisdiction with
And the minimum point of section of jurisdiction cambered surface, being calculated according to obtained each point can be by the complete envelope in section of jurisdiction and the seat on side and basis coordinates system
Parameter is parallel and the smallest cuboid of volume, obtained cuboid is configured to section of jurisdiction model, the basis coordinates system is to construct
Barrier model, joint link lever model and yoke to end effector part model when the reference frame established.
7. duct piece assembling machine obstacle-avoiding route planning method according to claim 1-6, which is characterized in that planning institute
Assembling machine, which is stated, by the path that current location is moved to target position includes:
The first tree is established by tree root of the current location of end effector, is tree with end effector target position to be reached
Root establishes the second tree, is iterated operation with preset step-length, in which:
When the number of iterations is odd number, with one node of preset step-length random growth in the first tree, and found out from the first tree
The nearest node with the nodal distance that just grows, along the node found out to the direction of the node just grown, according to preset step-length
The first new node is obtained between the node found out and the node just grown, if the first new node meets impact conditions,
Regrow node in one tree；
If the first new node is unsatisfactory for impact conditions, the position of the first new node is saved, and is found out and first in the second tree
New node is apart from nearest node, along the node found out to the direction of the first new node, according to preset step-length from the section found out
The second new node is obtained between point and the first new node, if the second new node meets impact conditions, is given birth to again in the second tree
Long node saves the position of the second new node if the second new node is unsatisfactory for impact conditions, then carries out next iteration fortune
It calculates；
When the number of iterations is even number, with one node of preset step-length random growth in the second tree, and found out from the second tree
The nearest node with the nodal distance that just grows, along the node found out to the direction of the node just grown, according to preset step-length
Third new node is obtained between the node found out and the node just grown, if third new node meets impact conditions,
Regrow node in two trees；
If third new node is unsatisfactory for impact conditions, the position of third new node is saved, and is found out in the first tree and third
New node is apart from nearest node, along the node found out to the direction of third new node, according to preset step-length from the section found out
The 4th new node is obtained between point and third new node, if the 4th new node meets impact conditions, is given birth to again in the first tree
Long node saves the position of the 4th new node if the 4th new node is unsatisfactory for impact conditions, then carries out next iteration fortune
It calculates；
Each node location of preservation is arranged to form path according to the sequence of end effector current location to target position, as
The path cooked up, node meet impact conditions and refer to that the end effector of the assembling machine is located at the spelling when node location
Installation collides with barrier, and node is unsatisfactory for impact conditions and refers to that the end effector of the assembling machine is located at the node position
The assembling machine does not collide with barrier when setting.
8. duct piece assembling machine obstacle-avoiding route planning method according to claim 1, which is characterized in that judge the assembling machine
The method whether to collide with barrier includes:
Judge whether the distance of axis of the centre of sphere of the barrier model to joint link lever model is greater than the barrier model
Radius and joint link lever model the sum of radius, if not, it is determined that the assembling machine and barrier collide；
If so, judge the centre of sphere of the barrier model at a distance from the model axis of yoke to end effector part whether
Greater than the barrier model radius and yoke to end effector part model outer surface to its axis maximum distance
The sum of, if not, it is determined that the assembling machine and barrier collide；
If so, judge be at a distance from the centre of sphere of the barrier model and each vertex of model of yoke to end effector part
The radius of the no both greater than barrier model, if so, determining that the assembling machine does not collide with barrier, if not, it is determined that
The assembling machine and barrier collide.
9. duct piece assembling machine obstacle-avoiding route planning method according to claim 5, which is characterized in that judge the assembling machine
And the method whether section of jurisdiction of crawl collides with barrier includes:
Judge whether the distance of axis of the centre of sphere of the barrier model to joint link lever model is greater than the barrier model
Radius and joint link lever model the sum of radius, if not, it is determined that the assembling machine and barrier collide；
If so, judge the centre of sphere of the barrier model at a distance from the model axis of yoke to end effector part whether
Greater than the barrier model radius and yoke to end effector part model outer surface to its axis maximum distance
The sum of, if not, it is determined that the assembling machine and barrier collide；
If so, judge at a distance from the centre of sphere of the barrier model and each vertex of model of yoke to end effector part whether
The radius of the both greater than described barrier model, if not, it is determined that the assembling machine and barrier collide；
If so, judging whether the centre of sphere of the barrier model is greater than the barrier to the distance of the section of jurisdiction model axis
The sum of the maximum distance of its axis is arrived in the radius of model and section of jurisdiction model outer surface, if not, it is determined that the assembling machine and
Barrier collision；
If so, judging whether the centre of sphere of the barrier model is both greater than the obstacle to the distance on each vertex of section of jurisdiction model
The radius of object model, if so, determining that the assembling machine does not collide with barrier；If not, it is determined that the assembling machine and obstacle
Object collision.
10. a kind of duct piece assembling machine obstacle-avoiding route planning device, which is characterized in that require any one of 1-9 institute for perform claim
The duct piece assembling machine obstacle-avoiding route planning method stated.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201910299712.0A CN109917793B (en) | 2019-04-15 | 2019-04-15 | Obstacle avoidance path planning method and device for pipe piece assembling machine |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201910299712.0A CN109917793B (en) | 2019-04-15 | 2019-04-15 | Obstacle avoidance path planning method and device for pipe piece assembling machine |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN109917793A true CN109917793A (en) | 2019-06-21 |
| CN109917793B CN109917793B (en) | 2022-07-08 |
Family
ID=66969845
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN201910299712.0A Active CN109917793B (en) | 2019-04-15 | 2019-04-15 | Obstacle avoidance path planning method and device for pipe piece assembling machine |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN109917793B (en) |
Cited By (4)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN111070211A (en) * | 2020-01-06 | 2020-04-28 | 长沙中联恒通机械有限公司 | Vehicle-mounted manipulator one-key grabbing and replacing control method, device and system |
| CN111168683A (en) * | 2020-01-20 | 2020-05-19 | 吉利汽车研究院（宁波）有限公司 | Robot path interference detection method, device, medium and equipment |
| CN112528429A (en) * | 2020-11-27 | 2021-03-19 | 中铁磁浮科技（成都）有限公司 | Bent pipe coordinate configuration method under interference condition |
| CN114117796A (en) * | 2021-11-29 | 2022-03-01 | 山东交通学院 | Segment erector kinematic modeling method with passive joints |
Citations (10)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| JP2001207779A (en) * | 2000-01-24 | 2001-08-03 | Mitsubishi Heavy Ind Ltd | Obstacle detecting device for boring tunnel |
| CN102589443A (en) * | 2012-01-31 | 2012-07-18 | 华中科技大学 | System and method for intelligently detecting duct piece splicing quality based on image identification |
| CN104018850A (en) * | 2014-06-09 | 2014-09-03 | 吉林大学 | Binocular-vision-based segment positioning and capturing system of segment erector |
| CN107160402A (en) * | 2017-06-27 | 2017-09-15 | 华北电力大学（保定） | A kind of mechanical arm mobile route planning system and method |
| CN107449383A (en) * | 2017-09-22 | 2017-12-08 | 中国铁建重工集团有限公司 | A kind of section of jurisdiction automatic identification grabbing device and method |
| CN108104834A (en) * | 2018-01-05 | 2018-06-01 | 杭州图溪科技有限公司 | Shield duct piece automatic assembling method and system |
| CN108241373A (en) * | 2017-12-29 | 2018-07-03 | 深圳市越疆科技有限公司 | Barrier-avoiding method and intelligent robot |
| CN108247637A (en) * | 2018-01-24 | 2018-07-06 | 中南大学 | A kind of industrial machine human arm vision anticollision control method |
| CN108908331A (en) * | 2018-07-13 | 2018-11-30 | 哈尔滨工业大学（深圳） | The barrier-avoiding method and system, computer storage medium of super redundancy flexible robot |
| CN109571466A (en) * | 2018-11-22 | 2019-04-05 | 浙江大学 | A kind of seven freedom redundant mechanical arm dynamic obstacle avoidance paths planning method based on quick random search tree |
-
2019
- 2019-04-15 CN CN201910299712.0A patent/CN109917793B/en active Active
Patent Citations (10)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| JP2001207779A (en) * | 2000-01-24 | 2001-08-03 | Mitsubishi Heavy Ind Ltd | Obstacle detecting device for boring tunnel |
| CN102589443A (en) * | 2012-01-31 | 2012-07-18 | 华中科技大学 | System and method for intelligently detecting duct piece splicing quality based on image identification |
| CN104018850A (en) * | 2014-06-09 | 2014-09-03 | 吉林大学 | Binocular-vision-based segment positioning and capturing system of segment erector |
| CN107160402A (en) * | 2017-06-27 | 2017-09-15 | 华北电力大学（保定） | A kind of mechanical arm mobile route planning system and method |
| CN107449383A (en) * | 2017-09-22 | 2017-12-08 | 中国铁建重工集团有限公司 | A kind of section of jurisdiction automatic identification grabbing device and method |
| CN108241373A (en) * | 2017-12-29 | 2018-07-03 | 深圳市越疆科技有限公司 | Barrier-avoiding method and intelligent robot |
| CN108104834A (en) * | 2018-01-05 | 2018-06-01 | 杭州图溪科技有限公司 | Shield duct piece automatic assembling method and system |
| CN108247637A (en) * | 2018-01-24 | 2018-07-06 | 中南大学 | A kind of industrial machine human arm vision anticollision control method |
| CN108908331A (en) * | 2018-07-13 | 2018-11-30 | 哈尔滨工业大学（深圳） | The barrier-avoiding method and system, computer storage medium of super redundancy flexible robot |
| CN109571466A (en) * | 2018-11-22 | 2019-04-05 | 浙江大学 | A kind of seven freedom redundant mechanical arm dynamic obstacle avoidance paths planning method based on quick random search tree |
Non-Patent Citations (1)
| Title |
|---|
| 宗成星: "空间多自由度串联机械臂的避障运动规划方法研究", 《中国优秀硕士学位论文全文数据库 信息科技辑》 * |
Cited By (6)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN111070211A (en) * | 2020-01-06 | 2020-04-28 | 长沙中联恒通机械有限公司 | Vehicle-mounted manipulator one-key grabbing and replacing control method, device and system |
| CN111070211B (en) * | 2020-01-06 | 2021-06-22 | 中联恒通机械有限公司 | Vehicle-mounted manipulator one-key grabbing and replacing control method, device and system |
| CN111168683A (en) * | 2020-01-20 | 2020-05-19 | 吉利汽车研究院（宁波）有限公司 | Robot path interference detection method, device, medium and equipment |
| CN112528429A (en) * | 2020-11-27 | 2021-03-19 | 中铁磁浮科技（成都）有限公司 | Bent pipe coordinate configuration method under interference condition |
| CN114117796A (en) * | 2021-11-29 | 2022-03-01 | 山东交通学院 | Segment erector kinematic modeling method with passive joints |
| CN114117796B (en) * | 2021-11-29 | 2024-07-12 | 山东交通学院 | Segment erector kinematics modeling method with passive joint |
Also Published As
| Publication number | Publication date |
|---|---|
| CN109917793B (en) | 2022-07-08 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN109917793A (en) | A kind of duct piece assembling machine obstacle-avoiding route planning method and device | |
| CN107214701B (en) | A kind of livewire work mechanical arm automatic obstacle avoiding paths planning method based on movement primitive library | |
| CN108827309B (en) | Robot path planning method and dust collector with same | |
| CN110228069A (en) | A kind of online avoidance motion planning method of mechanical arm | |
| CN110986953B (en) | Path planning method, robot and computer readable storage medium | |
| CN108356819A (en) | Based on the industrial machinery arm Collision Free Path Planning for improving A* algorithms | |
| Luo et al. | Collision‐free path‐planning for six‐dof serial harvesting robot based on energy optimal and artificial potential field | |
| CN113062601B (en) | Q learning-based concrete distributing robot trajectory planning method | |
| CN109343345A (en) | Mechanical arm polynomial interopolation method for planning track based on QPSO algorithm | |
| CN109871021A (en) | A kind of robot navigation method based on particle swarm optimization algorithm | |
| CN113211447B (en) | Mechanical arm real-time perception planning method and system based on bidirectional RRT algorithm | |
| CN111552300A (en) | Crop picking system based on instance segmentation and path planning | |
| CN113799141B (en) | Six-degree-of-freedom mechanical arm obstacle avoidance path planning method | |
| CN114523477A (en) | Joint pose calibration method, system and storage medium | |
| Chen et al. | Path planning of the fruit tree pruning manipulator based on improved RRT-Connect algorithm | |
| CN109778939A (en) | Excavator arm intelligent control system and method capable of automatically planning track | |
| CN107160402A (en) | A kind of mechanical arm mobile route planning system and method | |
| CN113400303A (en) | Six-axis robot fruit and vegetable picking path planning method based on RRT (recursive least squares) algorithm | |
| CN116477505A (en) | Tower crane real-time path planning system and method based on deep learning | |
| US11906977B2 (en) | Path planning method | |
| CN110399679B (en) | Foundation position optimization design method | |
| CN207027520U (en) | A kind of mechanical arm mobile route planning system | |
| CN113146637A (en) | Robot Cartesian space motion planning method | |
| Jisen | Research on Six Degree of Freedom Robot Trajectory Planning Method Based on Improved Ant Colony Algorithm | |
| Chen et al. | Cooperative planning of dual arm robot based on Improved Particle Swarm Optimization |
Legal Events
| Date | Code | Title | Description |
|---|---|---|---|
| PB01 | Publication | ||
| PB01 | Publication | ||
| SE01 | Entry into force of request for substantive examination | ||
| SE01 | Entry into force of request for substantive examination | ||
| CB02 | Change of applicant information | ||
| CB02 | Change of applicant information |
Address after: 410100 No. 88 East 7 Line, Changsha Economic and Technological Development Zone, Changsha City, Hunan Province Applicant after: China Railway Construction Heavy Industry Co.,Ltd. Address before: 410100 No. 88 East 7 Line, Changsha Economic and Technological Development Zone, Changsha City, Hunan Province Applicant before: China Railway Construction Heavy Industry Co.,Ltd. |
|
| GR01 | Patent grant | ||
| GR01 | Patent grant |