CN106041941B - Trajectory planning method and device for mechanical arm - Google Patents
Trajectory planning method and device for mechanical arm Download PDFInfo
- Publication number
- CN106041941B CN106041941B CN201610452034.3A CN201610452034A CN106041941B CN 106041941 B CN106041941 B CN 106041941B CN 201610452034 A CN201610452034 A CN 201610452034A CN 106041941 B CN106041941 B CN 106041941B
- Authority
- CN
- China
- Prior art keywords
- mechanical arm
- intersection point
- ray
- joint
- coordinate
- 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.)
- Active
Links
- 238000000034 method Methods 0.000 title claims abstract description 32
- 230000000694 effects Effects 0.000 claims description 15
- 238000004422 calculation algorithm Methods 0.000 claims description 10
- 238000004364 calculation method Methods 0.000 claims description 5
- 238000005457 optimization Methods 0.000 claims 1
- 238000010586 diagram Methods 0.000 description 4
- 238000005516 engineering process Methods 0.000 description 2
- 238000004519 manufacturing process Methods 0.000 description 2
- 238000004590 computer program Methods 0.000 description 1
- 230000006870 function Effects 0.000 description 1
- 238000012544 monitoring process Methods 0.000 description 1
- 238000002360 preparation method Methods 0.000 description 1
- 239000004065 semiconductor Substances 0.000 description 1
- 238000003860 storage Methods 0.000 description 1
- 230000001225 therapeutic effect Effects 0.000 description 1
Classifications
-
- B—PERFORMING OPERATIONS; TRANSPORTING
- B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
- B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
- B25J9/00—Programme-controlled manipulators
- B25J9/16—Programme controls
- B25J9/1656—Programme controls characterised by programming, planning systems for manipulators
- B25J9/1664—Programme controls characterised by programming, planning systems for manipulators characterised by motion, path, trajectory planning
-
- B—PERFORMING OPERATIONS; TRANSPORTING
- B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
- B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
- B25J9/00—Programme-controlled manipulators
- B25J9/16—Programme controls
- B25J9/1602—Programme controls characterised by the control system, structure, architecture
- B25J9/161—Hardware, e.g. neural networks, fuzzy logic, interfaces, processor
Landscapes
- Engineering & Computer Science (AREA)
- Automation & Control Theory (AREA)
- Robotics (AREA)
- Mechanical Engineering (AREA)
- Physics & Mathematics (AREA)
- Artificial Intelligence (AREA)
- Evolutionary Computation (AREA)
- Fuzzy Systems (AREA)
- Mathematical Physics (AREA)
- Software Systems (AREA)
- Manipulator (AREA)
Abstract
The invention discloses a track planning method of a mechanical arm, which comprises the following steps: generating a working area of the mechanical arm based on joint parameters of the mechanical arm to be worked; generating a ray according to the coordinates of a pre-input starting point and the moving direction, calculating the intersection point of the ray and the boundary of the working area, and generating an intersection point set; calculating the distance between each intersection point in the intersection point set and the starting point, acquiring the coordinates of the intersection point corresponding to the minimum distance, and marking the intersection point as a terminal point; and planning the motion trail of the mechanical arm based on the coordinates of the starting point and the coordinates of the end point. The invention also discloses a device for planning the track of the mechanical arm, and the invention determines the boundary of the working area of the mechanical arm based on a geometric method, so that the position of a terminal point can be known in advance before the robot moves, and the robot can conveniently plan the motion track.
Description
Technical field
The present invention relates to the method for planning track and dress in manipulator motion trajectory planning field, more particularly to a kind of mechanical arm
Put.
Background technology
, it is necessary to plan the track of mechanical arm in the course of work of mechanical arm, in general, this trajectory planning
All it is to be realized by predetermined velocity planning algorithm, for example, S types (Double S) velocity planning algorithm can be used to complete
The trajectory planning of mechanical arm.
Some velocity planning algorithms are when carrying out trajectory planning, it is necessary to receive the position for the beginning and end that user provides in advance
Put, then program can generate a series of interpolated point between two points to describe the running orbit of mechanical arm.Such as in teaching work(
In energy, user is sent by handheld device and instructed so that mechanical arm moves along a straight line, directly along some direction always from starting point
To the border (i.e. terminal) for reaching working region.
Wherein, start position is specified by user, thus can simply be obtained very much, but the position of terminal (works
The border in region) it is, it is necessary to by monitoring or calculating acquisition by the structures shape of mechanical arm.Traditional method is by continuous
Whether monitor Current mechanical arm has been to reach the limit of position to realize, but this method is needed to position all on path
Put and a little detected, efficiency is low, and computationally intensive.
The content of the invention
In view of the above-mentioned problems, it is an object of the invention to provide a kind of method for planning track of mechanical arm and device, transporting
Can knows the position of terminal in advance before dynamic, facilitates robot to carry out the planning of movement locus.
The invention provides a kind of method for planning track of mechanical arm, comprise the following steps：
Based on the joint parameter for the mechanical arm for treating work, the working region of the mechanical arm is generated；
One ray is generated according to the coordinate of the starting point pre-entered and moving direction, and calculates the ray and the work
Make the intersection point on the border in region, generate intersection point set；
The distance of each intersection point and the starting point in the intersection point set is calculated, obtains intersection point corresponding with minimum range
Coordinate, and the intersection point is labeled as terminal；
The coordinate of coordinate and the terminal based on the starting point, is planned the movement locus of the mechanical arm,
To control the mechanical arm to be moved according to the movement locus of planning.
Preferably, the mechanical arm is SCARA type mechanical arms.
Preferably, the joint parameter includes the brachium between joint type, joint and the scope of activities in joint；It is then described
Based on the joint parameter for the mechanical arm for treating work, the working region of the mechanical arm is generated, is specifically included：
It is each based on the generation of DH establishment of coordinate system according to the relative position relation between the joint type of mechanical arm and joint
The coordinate system in joint；
According to the brachium between the coordinate system in each joint, the scope of activities in each joint and joint, the machinery is generated
The working region of arm.
Preferably, the coordinate for the starting point that the basis pre-enters and moving direction generate a ray, and described in calculating
Ray and the intersection point on the border for stating working region, generate intersection point set, are specially：
The working region is decomposed at least two sections of camber lines, obtains the center of circle, radius and the center of circle angular region of every section of camber line,
And generate the equation of circle corresponding with every section of camber line；
One ray is generated according to the coordinate of the starting point pre-entered and moving direction, calculates the ray and each circle
Intersection point；
Radian of the intersection point on the circle where the intersection point is calculated, and is located at the central angle in the radian
In the range of when, the intersection point is stored in default set, generates intersection point set.
Preferably, the coordinate for the starting point that the basis pre-enters and moving direction generate a ray, are penetrated described in calculating
Line is specially with each round intersection point：
One ray is generated according to the coordinate of the starting point pre-entered and moving direction, by the ray equation and each circle
Equation carry out simultaneous, the ray and each round intersection point are calculated based on Parametric Equation Method.
Preferably, the coordinate for the starting point that the basis pre-enters and moving direction generate a ray, are penetrated described in calculating
Line is specially with each round intersection point：
One ray is generated according to the coordinate of the starting point pre-entered and moving direction, based on ray and circle test for intersection
Optimized algorithm calculates the ray and the intersection point each justified.
Present invention also offers a kind of trajectory planning device of mechanical arm, including：
Working region generation unit, for the joint parameter based on the mechanical arm for treating work, generate the work of the mechanical arm
Make region；
Intersection point set generation unit, a ray is generated for the coordinate according to the starting point pre-entered and moving direction,
And the ray and the intersection point on the border of the working region are calculated, generate intersection point set；
Terminal indexing unit, for calculating the distance of each intersection point and the starting point in the intersection point set, obtain with
The coordinate of intersection point corresponding to minimum range, and the intersection point is labeled as terminal；
Motion trajectory unit, for the coordinate based on the starting point and the coordinate of the terminal, to the machinery
The movement locus of arm is planned, to control the mechanical arm to be moved according to the movement locus of planning.
Preferably, the mechanical arm is SCARA type mechanical arms.
Preferably, the joint parameter includes the brachium between joint type, joint and the scope of activities in joint；It is then described
Working region generation unit specifically includes：
Coordinate system generation module, for the relative position relation between the joint type according to mechanical arm and joint, it is based on
DH establishment of coordinate system generates the coordinate system in each joint；
Working region generation module, for the coordinate system according to each joint, the scope of activities in each joint and joint it
Between brachium, generate the working region of the mechanical arm.
Preferably, the intersection point set generation unit specifically includes：
Working region decomposing module, for the working region to be decomposed into at least two sections of camber lines, every section of camber line of acquisition
The center of circle, radius and center of circle angular region, and generate the equation of circle corresponding with every section of camber line；
Intersection point calculation module, a ray is generated for the coordinate according to the starting point pre-entered and moving direction, is calculated
The ray and the intersection point each justified；
Judge module, for calculating radian of the intersection point on the circle where the intersection point, and in the radian
When in the range of the central angle, determine that the intersection point is located on the camber line, the intersection point is stored in default set, it is raw
Conclusion of the business point set.
The motion trail planning method and device of mechanical arm provided in an embodiment of the present invention, by generating the mechanical arm
Working region, then calculate by the coordinate of starting point pre-entered and the friendship on the border of the ray of moving direction generation and working region
Point, and the coordinate for the intersection point that there is minimum range with starting point is obtained, terminal corresponding with the starting point and moving direction is obtained, then
The planning of movement locus is carried out according to the coordinate of the starting point and the terminal.The present invention determines machine based on the method for geometry
The border of tool arm working region therefore robot can before motion know the position of terminal in advance, facilitate robot to be transported
The planning of dynamic rail mark.
Brief description of the drawings
In order to illustrate more clearly of technical scheme, the required accompanying drawing used in embodiment will be made below
Simply introduce, it should be apparent that, drawings in the following description are only some embodiments of the present invention, general for this area
For logical technical staff, on the premise of not paying creative work, other accompanying drawings can also be obtained according to these accompanying drawings.
Fig. 1 is the schematic flow sheet of the method for planning track of mechanical arm provided in an embodiment of the present invention.
Fig. 2 is the structural representation of SCARA type mechanical arms.
Fig. 3 is schematic diagram of the SCARA types mechanical arm shown in Fig. 2 in DH coordinate systems.
Fig. 4 is the working region schematic diagram of the SCARA type mechanical arms shown in Fig. 2.
Fig. 5 is the top view of the working region shown in Fig. 4.
Fig. 6 is the connection diagram of ray and working region.
Fig. 7 be it is provided in an embodiment of the present invention by ray with circle test for intersection optimized algorithm come calculate ray with circle
The schematic flow sheet of intersection point.
Fig. 8 (a) to (c) is the schematic diagram of the optimized algorithm shown in Fig. 7.
Fig. 9 is the structural representation of the trajectory planning device of mechanical arm provided in an embodiment of the present invention.
Figure 10 is the structural representation of Fig. 9 working region generation unit.
Figure 11 is the structural representation of Fig. 9 intersection point set generation unit.
Embodiment
Below in conjunction with the accompanying drawing in the embodiment of the present invention, the technical scheme in the embodiment of the present invention is carried out clear, complete
Site preparation describes, it is clear that described embodiment is only part of the embodiment of the present invention, rather than whole embodiments.It is based on
Embodiment in the present invention, those of ordinary skill in the art are obtained every other under the premise of creative work is not made
Embodiment, belong to the scope of protection of the invention.
Referring to Fig. 1, the embodiment of the present invention provides a kind of method for planning track of mechanical arm, the track rule of the mechanical arm
The method of drawing can be performed by the trajectory planning device of mechanical arm, and comprise at least following steps：
S101, based on the joint parameter for the mechanical arm for treating work, generate the working region of the mechanical arm.
In embodiments of the present invention, mechanical arm is automated machine device most widely used in robotic technology field,
It is widely used in the fields such as industrial manufacture, therapeutic treatment, entertainment service, military affairs, semiconductor manufacturing and space probation.Although
Their form is had nothing in common with each other, but they have one it is common the characteristics of, can exactly receive instruction, and according to instruction accurate
Navigate to the certain point of three-dimensional (or two dimension) spatially and carry out operation.
In general, a mechanical arm includes several joints, and each joint has joint parameter, the joint parameter
It may include joint type, brachium (length of the linking arm in two joints of connection), scope of activities etc..Wherein, the joint type
It may include cradle head and arthrodia, cradle head can control the rotation of linking arm, and arthrodia can realize linking arm
Vertical sliding motion.Pass through the combination in different joints, you can control the working region of the mechanical arm.Specifically, can be by walking as follows
It is rapid to determine the working region of the mechanical arm：
S1011, according to the relative position relation between the joint type of mechanical arm and joint, given birth to based on DH establishment of coordinate system
Into the coordinate system in each joint.
As shown in Fig. 2 with SCARA (Selective Compliant Articulated Robot for Assembly)
Exemplified by type mechanical arm, it has 4 joints J1, J2, J3, a J4, wherein J1, J2, and J4 is cradle head, and J1 and J2, J2 and J4
It is attached by corresponding linking arm, and J3 is arthrodia.However, it is to be understood that it is also possible to apply the invention to it
The mechanical arm of his type, will not be described here.
It is determined that, it is necessary to coordinate system be established for each joint, as shown in figure 3, DH can be based on behind above-mentioned joint
(Denavit-Hartenberg) coordinate system establishes the coordinate system in each joint, wherein cradle head Ji rotary shaft with each
Zi axles (cradle head Ji linking arm around zi axles rotate) it is parallel to each other, the brachium of linking arm is α i, { i │ i ∈ { 1,2,4 } }；
Arthrodia J3 axle (z3 axles) is parallel with J4 axle (z4 axles).After zi axles are determined, second can be pointed to first joint
The direction in individual joint is as xi directions, and yi direction can be determined based on the right-hand rule, in this way, generating each joint
Coordinate system.
S1012, according to the brachium between the coordinate system in each joint, the scope of activities in each joint and joint, generate institute
State the working region of mechanical arm.
In embodiments of the present invention, after the coordinate system in each joint is generated, according to the scope of activities in each joint (for
Cradle head, its scope of activities are the rotation angular region that its linking arm rotates, and for arthrodia, its scope of activities connects for it
The up and down motion scope of arm), you can the working region (as shown in Figure 4) of the mechanical arm is generated, now, need to only be carried out in plane
Projection can generate top view as shown in Figure 5.
S102, a ray is generated according to the coordinate of the starting point pre-entered and moving direction, and calculate the ray with
The intersection point on the border of the working region, generate intersection point set.
Specifically, when carrying out teaching using mechanical arm, the most frequently used function is that user sends instruction by handheld device,
So that the mechanical arm is from default starting point, and moves linearly along some direction always, until reaching working region
Border (terminal).Wherein, the mechanical arm need to know that terminal could carry out the planning of movement locus in advance in movement, in this hair
In bright embodiment, in order to obtain the terminal, specifically, it may include：
S1021, the working region is decomposed at least two sections of camber lines, obtains the center of circle, radius and the center of circle of every section of camber line
Angular region, and generate the equation of circle corresponding with every section of camber line.
From figure 5 it can be seen that the working region of the mechanical arm be byFour sections of arcs
Line is surrounded.And this four sections of camber lines are located at ⊙ O respectively1, ⊙ O2, ⊙ O3, ⊙ O4On four circles.Center of circle O1, O4With J1Overlapping of axles；O2
And O3Respectively J1Axle turns to both positive and negative polarity and prescribed a time limit J2The position of axle.WithIt is axle J respectivelyiPositive/negative direction maximum
Scope of activities,{ji|-π≤ji≤π}。
The detail parameters of this four sections of camber lines can refer to shown in table 1：
Table 1
Wherein：
S1022, a ray is generated according to the coordinate of the starting point pre-entered and moving direction, calculate the ray with it is every
The intersection point of individual circle.
In embodiments of the present invention, it is assumed that starting point I, direction vector n, | | n | | it is direction vector n unit vector.
Then now ray is represented by R (u)=I+u | | n | |.
If starting point I is located in working region, ray R (u) must be with certain circle ⊙ OiThere is intersection point
Wherein, when calculating intersection point, can by the way that ray R (u) equation is carried out into simultaneous with the equation of each circle successively,
Solved and obtained using Parametric Equation Method.
S1023, radian of the intersection point on the circle where the intersection point is calculated, and in the radian positioned at described
When in the range of central angle, determine that the intersection point is located on the camber line, the intersection point is stored in default set, generates intersection point collection
Close.
In embodiments of the present invention, in the above-mentioned intersection point tried to achieve, although some intersect with circle, it is not located at camber line
On, thus need to remove.Specifically, as shown in fig. 6, ray IP respectively with ⊙ O1Intersect at point N；With ⊙ O2Intersect at point K and point
M；With ⊙ O4Intersect at point J and point L；With ⊙ O3There is no intersection point.Wherein, point K, although N on circle, not surround it is described
On the camber line of working region, so after intersection point is obtained, the radian θ of the intersection point is also checkediWhether meet
Specifically, it is assumed that point s (x, y) is ⊙ OiOn a point, then point s is relative to ⊙ OiRadian θiFor：
It is assured which intersection point is on camber line by above-mentioned formula, which does not exist.Now, those are located at camber line
On intersection point be stored in default set ξ={ κiIn, generate intersection point set.
It should be noted that in other embodiments of the invention, the working region is not necessarily by round camber line
Composition.For example, the working region may be made up of the camber line of ellipse or be mixed by the camber line of different types of geometry
Composition, now, similarly, can obtain the equation and angular range of geometry corresponding to these camber lines, then pass through above-mentioned method
Generation intersection point set can be calculated, these technical schemes will not be described here within protection scope of the present invention.
S103, the distance of each intersection point and the starting point in the intersection point set is calculated, obtained corresponding with minimum range
Intersection point coordinate, and the intersection point is labeled as terminal.
In embodiments of the present invention, the distance of each intersection point and the starting point I can be calculated by Euler's formula, then
The each distance being calculated is compared, obtains the coordinate of intersection point corresponding with minimum range, and the intersection point is marked
For terminal, now, that is, the border of required working region is obtained.
S104, the coordinate of coordinate and the terminal based on the starting point, the movement locus of the mechanical arm is carried out
Planning, to control the mechanical arm to be moved according to the movement locus of planning.
In embodiments of the present invention, after the coordinate of terminal corresponding with the starting point and the moving direction is obtained, i.e.,
The movement locus of the mechanical arm can be planned according to the coordinate of the starting point and the coordinate of the terminal.
For example, by taking typical S types velocity planning algorithm as an example, it is receiving the coordinate of the starting point, the terminal
After coordinate, you can planning obtains seven sections of motion processes, and is realized by this seven sections of motion processes by the mechanical arm from described
Starting point (initial velocity zero) is steadily quickly moved to the terminal (end speed is also zero).
In summary, the motion trail planning method of mechanical arm provided in an embodiment of the present invention, by generating the machinery
The working region of arm, then calculate the border of the ray generated by the coordinate and moving direction of the starting point pre-entered and working region
Intersection point, and obtain with starting point have minimum range intersection point coordinate, obtain it is corresponding with the starting point and moving direction end
Point, the planning of movement locus is carried out further according to the coordinate of the starting point and the terminal.The present invention based on the method for geometry come
Determine the border of mechanical arm working region therefore robot can before motion knows the position of terminal in advance, facilitate robot
Carry out the planning of movement locus.
For the ease of the understanding of the present invention, some currently preferred embodiments of the present invention will be further described below.
Preferably, for step S1022, after the ray is generated, can be found intersection using traditional Parametric Equation Method,
It can also useThe ray is calculated based on the optimized algorithm of ray and circle test for intersection Deng proposition
With the intersection point of each circle.
Specifically, as shown in fig. 7, setting the parametric equation of ray as R (u)=I+u | | n | |, wherein | | n | | it is unit length
Degree.As shown in Fig. 8 (a), calculate first from starting point I to center of circle OiVectorVectorLengthAnd vectorAlong | | n | | the projection in directionIfAnd l<0, then illustrate that starting point is located at ⊙
OiOutside, and ray direction along away from ⊙ OiDirection extends, therefore the ray and ⊙ OiNon-intersect (shown in such as Fig. 8 (b)), this
When complete for the first time exclude test.Otherwise, center of circle O is calculated using Pythagorean theoremiSquare of distance between projection：m2=a2-
l2IfIt then can be determined that ray and circle OiIt is certain non-intersect, complete second and exclude test.If ray and ⊙ Oi
By excluding to test twice, then it is determined that they are certain intersecting.Next, calculate ray and ⊙ OiIntersection point：First, count
Calculate distanceThen judge whether starting point I is located at ⊙ OiIt is interior, ifThen illustrate that starting point I is located at ⊙ Oi
Outside, now, the ray and circle OiThere are two intersection points, be respectively And
(shown in such as Fig. 8 (a)).IfThen illustrate that starting point I is located at circle OiIt is interior, now, the ray and circle OiThere is an intersection point,
For I+ (l+q) | | n | | (shown in such as Fig. 8 (c)).
In this preferred scheme, before intersection point of the ray with circle is calculated, first pass through test twice judge ray with it is round whether
There is intersection point, when calculating, only calculate has the circle of intersection point with ray, without being counted to those with circle that ray does not have intersection point
Calculate, in this way, reducing amount of calculation, improve computational efficiency.
Also referring to Fig. 9, the embodiment of the present invention also provides a kind of trajectory planning device 100 of mechanical arm, and it includes：
Working region generation unit 10, for the joint parameter based on the mechanical arm for treating work, generate the mechanical arm
Working region.
Intersection point set generation unit 20, generate one for the coordinate according to the starting point pre-entered and moving direction and penetrate
Line, and the ray and the intersection point on the border of the working region are calculated, generate intersection point set；
Terminal indexing unit 30, for calculating the distance of each intersection point and the starting point in the intersection point set, obtain
The coordinate of intersection point corresponding with minimum range, and the intersection point is labeled as terminal；
Motion trajectory unit 40, for the coordinate based on the starting point and the coordinate of the terminal, to the machine
The movement locus of tool arm is planned, to control the mechanical arm to be moved according to the movement locus of planning.
Wherein it is preferred to the mechanical arm is SCARA type mechanical arms.
Wherein it is preferred to also referring to Figure 10, the working region generation unit 10 specifically includes：
Coordinate system generation module 11, for the relative position relation between the joint type according to mechanical arm and joint, base
The coordinate system in each joint is generated in DH establishment of coordinate system.
Working region generation module 12, for the coordinate system according to each joint, the scope of activities in each joint and joint
Between brachium, generate the working region of the mechanical arm.
Wherein it is preferred to also referring to Figure 11, the intersection point set generation unit 20 specifically includes：
Working region decomposing module 21, for the working region to be decomposed into at least two sections of camber lines, obtain every section of camber line
The center of circle, radius and center of circle angular region, and generate it is corresponding with every section of camber line justify equation.
Intersection point calculation module 22, a ray, meter are generated for the coordinate according to the starting point pre-entered and moving direction
Calculate the ray and the intersection point each justified.
Wherein, the intersection point calculation module 22, can be directly by institute when the ray and each round intersection point is calculated
State ray to be calculated with each circle progress simultaneous, the optimized algorithm that may be based on ray and circle test for intersection calculates the ray
With the intersection point of each circle, the present invention is not specifically limited.
Judge module 23, for calculating radian of the intersection point on the circle where the intersection point, and in the radian
When angle is located in the range of the central angle, determine that the intersection point is located on the camber line, the intersection point be stored in default set,
Generate intersection point set.
In summary, the Motion trajectory device 100 of mechanical arm provided in an embodiment of the present invention, by the working region
Generation unit 10 generates the working region of the mechanical arm, then the starting point by pre-entering is calculated by intersection point set generation unit 20
Coordinate and moving direction generation ray and working region border intersection point, and by the terminal indexing unit 30 obtain with
Starting point has the coordinate of the intersection point of minimum range, terminal corresponding with the starting point and moving direction is obtained, finally by motion rail
Mark planning unit 40 carries out the planning of movement locus according to the coordinate of the starting point and the terminal.The present invention is based on geometry
Method determines that therefore robot can before motion knows the position of terminal in advance on the border of mechanical arm working region, convenient
Robot carries out the planning of movement locus.
The above disclosed power for being only a kind of preferred embodiment of the present invention, the present invention can not being limited with this certainly
Sharp scope, one of ordinary skill in the art will appreciate that realizing all or part of flow of above-described embodiment, and weighed according to the present invention
Profit requires made equivalent variations, still falls within and invents covered scope.
One of ordinary skill in the art will appreciate that realize all or part of flow in above-described embodiment method, being can be with
The hardware of correlation is instructed to complete by computer program, described program can be stored in a computer read/write memory medium
In, the program is upon execution, it may include such as the flow of the embodiment of above-mentioned each method.Wherein, described storage medium can be magnetic
Dish, CD, read-only memory (Read-Only Memory, ROM) or random access memory (Random Access
Memory, RAM) etc..
Claims (10)
1. a kind of method for planning track of mechanical arm, it is characterised in that comprise the following steps：
Based on the joint parameter for the mechanical arm for treating work, the working region of the mechanical arm is generated；
One ray is generated according to the coordinate of the starting point pre-entered and moving direction, and calculates the ray and the workspace
The intersection point on the border in domain, generate intersection point set；
The distance of each intersection point and the starting point in the intersection point set is calculated, obtains the seat of intersection point corresponding with minimum range
Mark, and the intersection point is labeled as terminal；
The coordinate of coordinate and the terminal based on the starting point, is planned the movement locus of the mechanical arm, with control
The mechanical arm is made to be moved according to the movement locus of planning.
2. the method for planning track of mechanical arm according to claim 1, it is characterised in that the mechanical arm is SCARA types
Mechanical arm.
3. the method for planning track of mechanical arm according to claim 1, it is characterised in that the joint parameter includes joint
The scope of activities of brachium and joint between type, joint；
The then joint parameter based on the mechanical arm for treating work, generates the working region of the mechanical arm, specifically includes：
According to the relative position relation between the joint type of mechanical arm and joint, each joint is generated based on DH establishment of coordinate system
Coordinate system；
According to the brachium between the coordinate system in each joint, the scope of activities in each joint and joint, the mechanical arm is generated
Working region.
4. the method for planning track of mechanical arm according to claim 1, it is characterised in that what the basis pre-entered rises
The coordinate and moving direction of point generate a ray, and calculate the intersection point of the ray and the border for stating working region, raw
Conclusion of the business point set, it is specially：
The working region is decomposed at least two sections of camber lines, obtains the center of circle, radius and the center of circle angular region of every section of camber line, and it is raw
Into the equation of circle corresponding with every section of camber line；
One ray is generated according to the coordinate of the starting point pre-entered and moving direction, calculates the ray and the friendship each justified
Point；
Radian of the intersection point on the circle where the intersection point is calculated, and is located at the center of circle angular region in the radian
When interior, the intersection point is stored in default intersection point set, generates intersection point set.
5. the method for planning track of mechanical arm according to claim 4, it is characterised in that what the basis pre-entered rises
The coordinate and moving direction of point generate a ray, calculate the ray and are specially with the intersection point each justified：
One ray is generated according to the coordinate of the starting point pre-entered and moving direction, and generates ray corresponding with the ray
Equation, by the ray equation and each round equation progress simultaneous, the ray and each circle are calculated based on Parametric Equation Method
Intersection point.
6. the method for planning track of mechanical arm according to claim 4, it is characterised in that what the basis pre-entered rises
The coordinate and moving direction of point generate a ray, calculate the ray and are specially with the intersection point each justified：
One ray is generated according to the coordinate of the starting point pre-entered and moving direction, the optimization based on ray with circle test for intersection
Algorithm calculates the ray and the intersection point each justified.
A kind of 7. trajectory planning device of mechanical arm, it is characterised in that including：
Working region generation unit, for the joint parameter based on the mechanical arm for treating work, generate the workspace of the mechanical arm
Domain；
Intersection point set generation unit, a ray is generated for the coordinate according to the starting point pre-entered and moving direction, and counted
The ray and the intersection point on the border of the working region are calculated, generates intersection point set；
Terminal indexing unit, for calculating the distance of each intersection point and the starting point in the intersection point set, obtain and minimum
The coordinate of intersection point corresponding to distance, and the intersection point is labeled as terminal；
Motion trajectory unit, for the coordinate based on the starting point and the coordinate of the terminal, to the mechanical arm
Movement locus is planned, to control the mechanical arm to be moved according to the movement locus of planning.
8. the trajectory planning device of mechanical arm according to claim 7, it is characterised in that the mechanical arm is SCARA types
Mechanical arm.
9. the trajectory planning device of mechanical arm according to claim 7, it is characterised in that
The joint parameter includes the brachium between joint type, joint and the scope of activities in joint；
The working region generation unit specifically includes：
Coordinate system generation module, for the relative position relation between the joint type according to mechanical arm and joint, sat based on DH
Mark system establishes the coordinate system for generating each joint；
Working region generation module, between the coordinate system according to each joint, the scope of activities in each joint and joint
Brachium, generate the working region of the mechanical arm.
10. the trajectory planning device of mechanical arm according to claim 7, it is characterised in that the intersection point set generation is single
Member specifically includes：
Working region decomposing module, for the working region to be decomposed into at least two sections of camber lines, obtain every section of camber line the center of circle,
Radius and center of circle angular region, and generate the equation of circle corresponding with every section of camber line；
Intersection point calculation module, for generating a ray according to the coordinate of starting point and moving direction that pre-enter, described in calculating
Ray and the intersection point each justified；
Judge module, it is located at for calculating radian of the intersection point on the circle where the intersection point, and in the radian
When in the range of the central angle, determine that the intersection point is located on the camber line, the intersection point is stored in default set, generation is handed over
Point set.
Priority Applications (2)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201610452034.3A CN106041941B (en) | 2016-06-20 | 2016-06-20 | Trajectory planning method and device for mechanical arm |
| PCT/CN2016/113194 WO2017219640A1 (en) | 2016-06-20 | 2016-12-29 | Trajectory planning method and device for mechanical arm |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201610452034.3A CN106041941B (en) | 2016-06-20 | 2016-06-20 | Trajectory planning method and device for mechanical arm |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN106041941A CN106041941A (en) | 2016-10-26 |
| CN106041941B true CN106041941B (en) | 2018-04-06 |
Family
ID=57168726
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN201610452034.3A Active CN106041941B (en) | 2016-06-20 | 2016-06-20 | Trajectory planning method and device for mechanical arm |
Country Status (2)
| Country | Link |
|---|---|
| CN (1) | CN106041941B (en) |
| WO (1) | WO2017219640A1 (en) |
Families Citing this family (24)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN106041941B (en) * | 2016-06-20 | 2018-04-06 | 广州视源电子科技股份有限公司 | Trajectory planning method and device for mechanical arm |
| CN106346478A (en) * | 2016-11-09 | 2017-01-25 | 广州视源电子科技股份有限公司 | control method and device of mechanical arm |
| CN106985140B (en) * | 2017-04-19 | 2019-05-07 | 广州视源电子科技股份有限公司 | Robot point-to-point motion control method and system |
| CN107457780B (en) * | 2017-06-13 | 2020-03-17 | 广州视源电子科技股份有限公司 | Method and device for controlling mechanical arm movement, storage medium and terminal equipment |
| CN108081270B (en) * | 2017-12-15 | 2020-05-19 | 中国兵器装备集团自动化研究所 | Dangerous goods processing system and control method |
| CN110456729B (en) * | 2018-05-07 | 2021-09-28 | 苏州睿牛机器人技术有限公司 | Trajectory tracking control method and trajectory tracking system |
| CN109129470B (en) * | 2018-08-02 | 2021-07-09 | 深圳市智能机器人研究院 | Planning method and system for linear path of robot |
| CN109571478B (en) * | 2018-12-17 | 2021-07-27 | 浙江大学昆山创新中心 | Tracking control method for tail end of serially-connected multi-degree-of-freedom mechanical arm |
| CN109285173A (en) * | 2018-12-24 | 2019-01-29 | 常州节卡智能装备有限公司 | A kind of safety protecting method, device and computer equipment |
| CN111037551B (en) * | 2019-12-05 | 2022-10-11 | 上海新时达机器人有限公司 | Method and device for acquiring robot test space and robot system |
| CN111400891B (en) * | 2020-03-11 | 2023-08-11 | 中煤航测遥感集团有限公司 | Method, device, equipment and storage medium for obtaining deviation degree of pipeline inspection point |
| CN113399200B (en) * | 2020-03-17 | 2022-08-30 | 深圳市腾盛精密装备股份有限公司 | Dispensing method and five-axis linkage dispensing machine |
| CN114687538B (en) * | 2020-12-29 | 2023-08-15 | 广东博智林机器人有限公司 | Working method, device, equipment and medium of floor paint coating equipment |
| CN112998863B (en) * | 2021-03-12 | 2022-05-06 | 杭州柳叶刀机器人有限公司 | Robot safety boundary interaction device, electronic apparatus, and storage medium |
| CN113182701A (en) * | 2021-03-29 | 2021-07-30 | 大族激光科技产业集团股份有限公司 | Laser processing method, apparatus, device and storage medium |
| CN113989220A (en) * | 2021-10-25 | 2022-01-28 | 无锡闻泰信息技术有限公司 | Method, device and equipment for calculating motion trail of product and storage medium |
| CN114700960B (en) * | 2022-03-02 | 2023-09-08 | 西北工业大学 | Order-optimized remote control behavior contour planning method |
| CN115153855B (en) * | 2022-07-29 | 2023-05-05 | 中欧智薇(上海)机器人有限公司 | Positioning and aligning method and device for micro mechanical arm and electronic equipment |
| CN116252288A (en) * | 2023-03-09 | 2023-06-13 | 南京智欧智能技术研究院有限公司 | Multi-degree-of-freedom plane line control robot and line control method |
| CN116160141B (en) * | 2023-03-24 | 2024-01-30 | 中国科学院西安光学精密机械研究所 | Part laser processing interference checking method and part laser processing method |
| CN116901074A (en) * | 2023-08-03 | 2023-10-20 | 南京云创大数据科技股份有限公司 | Mechanical arm track planning method and device, storage medium and electronic equipment |
| CN116901073B (en) * | 2023-08-03 | 2024-07-05 | 南京云创大数据科技股份有限公司 | Multi-mechanical-arm collaborative track planning method and device, electronic equipment and storage medium |
| CN117454060B (en) * | 2023-12-22 | 2024-03-19 | 季华实验室 | Linear motion device position measuring method and device, electronic equipment and storage medium |
| CN117984334B (en) * | 2024-04-03 | 2024-05-28 | 泓浒(苏州)半导体科技有限公司 | Self-adaptive wafer mechanical arm moment adjusting system and method |
Citations (4)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| US4864205A (en) * | 1987-10-09 | 1989-09-05 | Hewlett-Packard Company | Method for coordinated control of motion devices |
| US7146242B2 (en) * | 2003-09-30 | 2006-12-05 | Rockwell Automation Technologies, Inc. | Method and system for generating multi-dimensional motion profiles |
| CN103227133A (en) * | 2012-01-31 | 2013-07-31 | 株式会社安川电机 | Transfer system |
| CN105415375A (en) * | 2016-01-02 | 2016-03-23 | 韩少卿 | Off-line programming device |
Family Cites Families (12)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| JP3339840B2 (en) * | 1999-09-28 | 2002-10-28 | タツモ株式会社 | Horizontal articulated industrial robot and control method thereof |
| WO2004095520A2 (en) * | 2003-04-22 | 2004-11-04 | Berkeley Process Control, Inc. | System of path planning for robotic manipulators based on maximum acceleration and finite jerk constraints |
| JP4736607B2 (en) * | 2005-08-05 | 2011-07-27 | 株式会社デンソーウェーブ | Robot controller |
| DE102009023307A1 (en) * | 2009-05-29 | 2010-12-02 | Kuka Roboter Gmbh | Method and device for controlling a manipulator |
| DE102012015437A1 (en) * | 2012-08-02 | 2014-05-15 | Kuka Roboter Gmbh | Method and programming means for modifying a robot path |
| US9352465B2 (en) * | 2013-11-12 | 2016-05-31 | Canon Kabushiki Kaisha | Control method for robot apparatus and robot apparatus |
| EP2898996A1 (en) * | 2014-01-23 | 2015-07-29 | Plum Sp. z o.o. | Method of controlling a robotic system and a robotic system controller for implementing this method |
| DE102014011012A1 (en) * | 2014-07-24 | 2016-01-28 | Kuka Roboter Gmbh | Method and means for designing and / or operating a robot |
| CN104457566A (en) * | 2014-11-10 | 2015-03-25 | 西北工业大学 | Spatial positioning method not needing teaching robot system |
| JP6137155B2 (en) * | 2014-12-09 | 2017-05-31 | トヨタ自動車株式会社 | Interference avoidance method, control device, and program |
| CN105922265B (en) * | 2016-06-20 | 2018-08-24 | 广州视源电子科技股份有限公司 | Motion trail planning method and device for mechanical arm and robot |
| CN106041941B (en) * | 2016-06-20 | 2018-04-06 | 广州视源电子科技股份有限公司 | Trajectory planning method and device for mechanical arm |
-
2016
- 2016-06-20 CN CN201610452034.3A patent/CN106041941B/en active Active
- 2016-12-29 WO PCT/CN2016/113194 patent/WO2017219640A1/en active Application Filing
Patent Citations (4)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| US4864205A (en) * | 1987-10-09 | 1989-09-05 | Hewlett-Packard Company | Method for coordinated control of motion devices |
| US7146242B2 (en) * | 2003-09-30 | 2006-12-05 | Rockwell Automation Technologies, Inc. | Method and system for generating multi-dimensional motion profiles |
| CN103227133A (en) * | 2012-01-31 | 2013-07-31 | 株式会社安川电机 | Transfer system |
| CN105415375A (en) * | 2016-01-02 | 2016-03-23 | 韩少卿 | Off-line programming device |
Also Published As
| Publication number | Publication date |
|---|---|
| WO2017219640A1 (en) | 2017-12-28 |
| CN106041941A (en) | 2016-10-26 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN106041941B (en) | Trajectory planning method and device for mechanical arm | |
| CN105922265B (en) | Motion trail planning method and device for mechanical arm and robot | |
| CN107995885A (en) | A kind of coordinate system scaling method, system and device | |
| CN103063213B (en) | The scaling method of a kind of welding robot and positioner position orientation relation | |
| Gong et al. | Analytical inverse kinematics and self-motion application for 7-DOF redundant manipulator | |
| CN109278048B (en) | Welding path planning method of five-axis welding robot and five-axis welding robot | |
| US20130013110A1 (en) | Method of finding feasible joint trajectories for an n-dof robot with rotation in-variant process (n>5) | |
| EP2660013A1 (en) | Control method for seven-shaft multi-joint robot, control program, and robot control device | |
| CN105598975B (en) | A kind of method for determining industrial robot motion track | |
| CN107791248A (en) | Control method based on the six degree of freedom serial manipulator for being unsatisfactory for pipper criterions | |
| CN109015652A (en) | A kind of control method of robot and the positioner coordinated movement of various economic factors | |
| CN104526117A (en) | Movable arc-welding robot and control system thereof | |
| CN108153707A (en) | A kind of arc welding robot straight line pendulum soldering method based on spatial alternation principle | |
| Bhatt et al. | Optimizing multi-robot placements for wire arc additive manufacturing | |
| CN110515346A (en) | A kind of industrial robot milling is complex-curved without cutter path interpolating method excessively | |
| Li et al. | Inverse kinematics study for intelligent agriculture robot development via differential evolution algorithm | |
| Abderrahmane et al. | Study and validation of singularities for a Fanuc LR Mate 200iC robot | |
| Manjunath et al. | Design & simulation of the workspace for a stationary robot system | |
| Chiddarwar et al. | Offline decoupled path planning approach for effective coordination of multiple robots | |
| Kong et al. | Collision detection algorithm for dual-robot system | |
| Lee et al. | Computer Vision-Based Robotic Welding for Construction of Nuclear Power Plants | |
| Sagar et al. | Path planning of a material handling agent with novel locomotion | |
| JP2008055518A (en) | Control method of welding robot | |
| CN116533248B (en) | Snake-shaped robot movement control method and device based on in-situ steering LOS angle guidance | |
| Chen et al. | Kinematics analysis and application of 5-DOF manipulator with special joint |
Legal Events
| Date | Code | Title | Description |
|---|---|---|---|
| C06 | Publication | ||
| PB01 | Publication | ||
| C10 | Entry into substantive examination | ||
| SE01 | Entry into force of request for substantive examination | ||
| GR01 | Patent grant | ||
| GR01 | Patent grant |